法律状态公告日
法律状态信息
法律状态
2022-06-07
公开
发明专利申请公布
技术领域
本发明涉及室内定位导航技术领域，特别涉及了一种基于UWB和IMU 的非视距误差补偿的室内定位方法及系统。
背景技术
室内物流系统要精准把控AGV(Automated Guided Vehicle)小车的位置，如何缓解非视距环境对实际室内仓储环境定位信息的影响，是保证智慧物流稳定运行的关键。目前，针对仓储室内AGV小车目标的定位技术，有红外射线、激光导航定位、超声波引导、视觉传感、磁钉定位等。这些技术有较好的性能，但受复杂室内环境因素影响较大，且不能很好地适应复杂环境下对定位感知系统的要求，存在定位精度不高、适应力弱、成本高等缺点。与传统定位技术相比，超宽带(UWB)技术以其多径分辨率高、穿透力强、低功耗且易于集成的优势成为目前最广泛的无线室内定位技术。
惯性导航测量单元(IMU)通过对目标载体的加速度自动进行积分运算，获得瞬时速度和瞬时位置信息。惯性导航技术作为自主导航系统，不依赖于外界环境，不易受到干扰。
因为在实际仓储室内环境下，存在较多货柜物和零部件货架等遮挡物，对UWB产生非视距误差，会导致定位误差较大；IMU定位技术的位置信息是通过积分得到的，误差随着时间积累而增加，难以长时间单独提供定位任务。目前主流的方法是将两者融合定位，实现双传感器优势互补，缓解非视距影响造成的误差，实现对AGV小车目标的精确定位。
如中国专利局2019年10月25日公开了一种名称为一种基于IMU和UWB融合的室内定位导航系统的发明，其公开号为CN110844494A，该发明基于IMU和UWB融合的室内定位导航系统包括IMU传感器、IMU位置计算单元、UWB传感器、UWB位置计算单元、融合位置计算单元，采用融合定位算法，将IMU和UWB相互结合，IMU得到的数据作为卡尔曼滤波的先验信息，UWB得到的数据作为卡尔曼滤波的观测信息，利用各自表现出的优点，可以有效地提高系统的定位导航精度，利用少量的观测站即可实现目标的高精度室内定位导航，实现其在高精度室内定位导航场景需求中的应用。
但这种定位方法会使得系统实时性较差、定位信息反馈速度慢。且在识别出AGV小车处于非视距情形时，会因UWB定位数据误差较大而舍弃，仅采用IMU解算的位置数据作为定位信息，这样会造成在非视距情形时丢失 UWB的定位数据，从而不能和单一UWB传感器在非视距情形下的定位数据进行对比。因此，这种方法将无法作为可靠的非视距误差缓解的方法。
发明内容
本发明的目的是克服现有技术中利用UWB和IMU进行室内定位时存在非视距误差的问题，提供了一种基于UWB和IMU的非视距误差补偿的室内定位方法及系统，全面利用传感器所获取的数据信息，根据不同情形的视距情况采取不同方法对待测目标因非视距产生的定位误差实施补偿，从而缓解非视距对误差带来的影响，提高定位精度，达到在实际情况下灵活处理、优势凸显的作用。
为了实现上述目的，本发明采用以下技术方案：一种基于双TOF相机的 AGV高位堆叠方法，其特征在于，它包括下列步骤：
S1：将空旷的室内仓储环境划分为M个网格，在每个网格中进行数据信息的采集；
S2：在实际的室内仓储环境中，对AGV小车任一物流路径每一时刻进行数据信息采集；
S3：判断空旷的室内仓储环境和实际的室内仓储环境的一致性；
S4：根据判断结果，分情况计算k时刻待测目标的最终位置pos
本发明通过对空旷室内仓储环境下进行网格划分，得到的离散化数据信息和实际室内仓储环境在每时刻进行数据标定，根据权衡数据信息一致性程度的变量值以及表征不同信息的向量值得到不同情形下的视距情况，并根据视距情况考虑将IMU融入定位。全面地利用了传感器所获取的数据信息，根据不同情形的视距情况采取不同方法对待测目标进行定位，从而缓解了非视距对误差带来的影响，提高定位精度，达到了在实际情况下灵活处理、优势凸显的作用。
作为优选，所述的步骤S1具体步骤为：
S1.1：根据实际的空旷的室内仓储环境，在满足定位精度的条件下将其划分为M个网格；
S1.2：将带有定位标签的AGV小车在每个网格中进行数据信息的采集和存储；
S1.2.1：每个网格采集的数据信息表示为一组，一组数据信息包含每个基站的测距值、每个基站接收的信号强度值和UWB定位解算的坐标值；
S1.2.2：定义第r组网格接收的信号强度信息基站的测距值分别为向量p′
p′
d′
式中，n表示基站个数；
S1.2.3：在M组网格中，分别设接收的信号强度集为P'
P'
D′
Pos′
P'
在空旷的室内仓储环境下，该环境属于在未移入货架、货柜等遮挡物的视距条件。
作为优选，所述的步骤S2，进一步包括：
每一时刻采集的数据信息包含每个基站的测距值、每个基站接收的信号强度值和UWB定位解算的坐标值；数据信息分别表示为：
p
d
其中，p
当移入货架、货柜等遮挡物后，此时为实际的室内环境。
作为优选，所述的步骤S3中，判断空旷的室内仓储环境和实际的室内仓储环境的一致性：
通过定义源信号强度变量
δ
α
定义一个新向量p′
获取对应的向量d′
定义一个新向量d′
获取对应的向量p′
定义一个新向量pos′
获取对应的向量p'
作为优选，所述的步骤S3中，根据一致性程度分为三种情形以区分实际仓储环境下的视距、遮挡情形：
A1：基站为完全视距情形，定义该网格位置pos′
A2：基站为不完全遮挡，定义该网格位置pos′
A3：基站为严重遮挡，输出此时刻基站带有非视距误差的测距值d
若视距、遮挡情形为情形一或情形二，采用平均值测算法进行目标位置的估计；若视距、遮挡情形为情形三，基于扩展卡尔曼滤波算法EKF将UWB 和IMU进行数据融合处理得到该时刻下目标的最终位置。
作为优选，所述的步骤S4中，根据判断结果计算k时刻待测目标的最终位置pos
S4.1：为A1和A2的情形：
计算网格位置坐标：
第k时刻根据Chan-Taylor定位算法解算的位置坐标为
其中，pos'
S4.2：为A3的情形：
S4.2.1：得到IMU得到的k时刻导航坐标系的位置：
B1：IMU获取AGV小车包括加速度与角速度在内的运动参数，根据IMU 定位解算算法二次积分得出AGV小车的位置和速度；
B2：通过坐标系变换求得旋转矩阵
其中：
B3：在采样间隔ΔT较短时，载体目标近似为匀速直线运动，用Δv
其中，
B4：设k-1时刻在导航坐标n系下的速度为
其中，
B5：设Δpos
其中，
B6：计算k-1时刻待测目标的实际位置：
pos
得到IMU得到的k时刻待测目标导航坐标系的位置：
S4.2.2：采用扩展卡尔曼滤波即EKF算法将IMU数据和UWB带有非视距误差的测距值进行融合滤波处理，得到k时刻待测目标的最终位置pos
载体坐标系上的加速度a
作为优选，所述的步骤S4.2.2，具体表示为：
C1：将IMU在导航坐标n系即UWB定位坐标系下k-1时刻加速度计获得的数据
X
Z
其中，F表示系统的状态转移矩阵，B为控制输入矩阵，G表示噪声驱动矩阵，w
C2：建立系统的观测方程：
状态方程：Z
C3：初始化状态均值U(0)＝E[X(0)]，状态协方差矩阵P(0)＝var[X(0)]，进行EKF迭代：
预测状态：
预测状态协方差矩阵：P
其中，预测状态的协方差矩阵P
计算卡尔曼滤波增益矩阵：
更新状态：
更新状态协方差矩阵：P
其中，I
以四个基站为例，建立状态方程和观测方程的具体过程为：
在实际情况下，当采样间隔时间为ΔT，建立系统状态方程为：
其中，pos
将C2中得到的方程组转换为矩阵形式，则系统的状态方程为：
其中系统的状态转移矩阵、控制输入矩阵和噪声驱动矩阵分别为：
因为基站的位置已知且固定，设基站的位置坐标为：(x
其中标签到每个基站测距值为：
进行线性化处理，对非线性函数h(·)进行一阶Taylor级数展开后可得到雅可比矩阵H
其中：
得到经过线性化后的系统的状态方程为：Z
其中，Δv
作为优选，每个基站接收的信号强度值由接收信号强度算法所获得，具体表现为：
其中p是基站接收标签的信号强度功率值；P
其中λ是当PRF(平均脉冲重复频率)为16MHz时为115.72，或者当PRF (平均脉冲重复频率)为64MHz时为121.74，本发明仓储环境下优选采用 16MHz的PRF，因此λ为115.72。
作为优选，UWB定位解算的坐标值由双曲线模型构造非线性方程组进而采用Chan-Taylor融合定位算法获得，具体表现为：
根据：
其中，R
由此得到：
G
其中：
进行第一次LS(最小二乘法)估计，得到
其中：
ψ＝4BQB
B＝diag(R
e＝diag(e
Q＝E[ee
式中，e
进行第二次LS(最小二乘法)估计，得到：
Z'
其中：
则第二次LS估计
其中：
对待测目标标签的位置估计：
以此位置估计作为Taylor算法的初值坐标值，以此位置进行Taylor展开，并忽略二阶及二阶以上的项，则可获得误差矢量的表达式：
φ＝h
其中：
(x
表达式的WLS(加权最小二乘法解)为：
其中，Q为测量值误差的协方差矩阵，则下一步递归初始值变更为：
x'＝x+Δx
y'＝y+Δy
按照以上步骤一直递归迭代计算，直到Δx,Δy满足预先设定的门限值ε， |Δx|+|Δy|＜ε，迭代结束，得到第k时刻的定位解算值pos
采用Chan-Taylor融合定位算法是利用Chan算法给Taylor级数法提供相对精度的初始位置，进而以此为基础进行Taylor级数展开，实现对目标标签的定位。Chan算法是一种非递归算法，它不需要初值，仅进行两次迭代就可求得最终结果，该算法是在当信号处于视距情形下传输时以及TDOA测量值较准确的前提下，定位精度较高；但当信号在非视距情形下传输及信道性能较差的情况下，定位精度较低。Taylor级数展开法是一种需初始估计位置的递归算法，通过不断递归来改进估计位置，逐步逼近真实值，该算法适用于各种信道环境，但计算量较大，又对初始估计位置要求较高。在初始估计位置与实际位置比较接近的情况下，得到的定位结果较准确。若初始位置的估计值偏差较大，直接影响该算法的定位精度。在仓储室内环境下存在许多遮挡物，Chan算法虽只仅限于信号在视距传输时定位精度较高，单对于确定标签的初始位置，有很大实际作用，尽管在信道性能处于并不优良情况，定位效果有所下降，但效果仍可以反应实际情况，可作为Taylor级数法的初始值。利用Chan算法给Taylor算法提供相对精确的初值，以此基础进行Taylor展开，优化两种算法的缺陷，对AGV小车在仓储环境下的定位有更好的效果。
一种基于UWB和IMU的非视距误差补偿的室内定位系统，其特征在于，包括：位于同一室内的若干个基站和设置在定位目标AGV小车上的定位标签和IMU模块，还包括上位机服务器，所述上位服务器包括所述上位机服务器包括对标签和基站上传的原始数据进行解析预处理的数据输入模块、算法处理模块、定位结果实时显示模块以及数据存储模块，所述上位机服务器包括对标签和基站上传的原始数据进行解析预处理的数据输入模块、算法处理模块、显示计算结果的定位结果实时显示模块以及数据存储模块，所述数据输入模块与算法处理模块连接，算法处理模块与显示结果计算模块连接。
数据输入模块包括数据接口和数据解析模块；定位结果实时显示模块包括可优化显示界面；算法处理模块包含Chan-Taylor融合定位算法、IMU定位解算算法以及非视距误差补偿算法；数据存储模块包括数据记录和数据存储模块。
因此，本发明具有如下有益效果：全面地利用了传感器所获取的数据信息，根据不同情形的视距情况采取不同方法对待测目标进行定位，从而缓解了非视距对误差带来的影响，提高定位精度，达到了在实际情况下灵活处理、优势凸显的作用。
附图说明
图1为本发明方法的具体操作流程图；
图2为本发明系统的结构示意图；
图3为本发明上位机服务器整体框架结构图；
图4为本发明UWB与IMU非视距误差缓解流程图；
图5为本发明完全视距情形的三点重合网格图；
图6为本发明不完全遮挡情形的两点重合网格图；
图7为本发明遮挡严重情形的三点分散网格图；
图中：1、上位机服务器；2、数据输入模块；3、算法处理模块；4、定位结果实时显示模块；5、数据存储模块；6、数据接口；7、数据解析模块； 8、可优化显示界面；9、Chan-Taylor融合定位算法；10、IMU定位解算算法； 11、非视距误差补偿算法；12、数据记录；13、数据存储模块。
具体实施方式
下面结合附图与具体实施方式对本发明作进一步详细描述：
如图1所示的实施例中，可以看到一种基于UWB和IMU的非视距误差补偿的室内定位方法，其操作流程为：步骤一，将空旷的室内仓储环境划分为M个网格，在每个网格中进行数据信息的采集；步骤二，在实际的室内仓储环境中，对AGV小车任一物流路径每一时刻进行数据信息采集；步骤三，判断空旷的室内仓储环境和实际的室内仓储环境的一致性；步骤四，根据判断结果对非视距误差进行补偿，计算k时刻待测目标的最终位置pos
本发明提供的基于UWB和IMU的AGV非视距误差补偿的室内定位方法及系统，通过对空旷室内仓储环境下进行网格划分，得到的离散化数据信息和实际室内仓储环境在每时刻进行数据标定，根据权衡数据信息一致性程度的变量值以及表征不同信息的向量值得到不同情形下的视距情况，并根据视距情况考虑将IMU融入定位。该方法更加全面地利用了传感器所获取的数据信息，根据不同情形的视距情况采取不同方法对待测目标进行定位，从而缓解了非视距对误差带来的影响，提高定位精度，达到了在实际情况下灵活处理、优势凸显的作用。
如图2所示的实施例中，可以看到一种基于UWB和IMU的非视距误差补偿的室内定位系统，包括：位于同一室内的4个基站(基站0、基站1、基站2和基站3)和一个设置在定位目标AGV小车上的定位标签和IMU模块。如图2实施例所示，存在3个直达路径和1个非直达路径，其中直达路径是指定位基站和定位标签的信号传输过程不存在遮挡物的视距情形，非直达路径是指定位基站和定位标签的信号传输过程存在遮挡物的非视距情形。
如图3所示的实施例中，为上位机服务器整体框架结构图，该上位机服务器1包括数据输入模块2、定位结果实时显示模块3、算法处理模块4以及数据存储模块5，所述数据输入模块包括数据接口6和数据解析模块7；定位结果实时显示模块包括可优化显示界面8；算法处理模块包含Chan-Taylor融合定位算法9、IMU定位解算算法10以及非视距误差补偿算法11；数据存储模块包括数据记录12和数据存储模块13。
以下详细介绍各模块功能的实现过程：
数据输入模块：该主要功能是对标签和基站上传的原始数据进行解析预处理，处理的数据包主要为时间间隔的心跳包和定位数据信息包，当心跳包接收到以后可以选择进行处理以显示基站的一些状态信息，也可直接丢弃。
算法处理模块：该核心包含基于TDOA的Chan-Taylor定位算法、IMU 定位解算算法等，该算法的输入值包含传入的测量值数据和环境相关的配置数据，测量值数据是指四个基站对标签的测距值、信号接收强度以及IMU测量的加速度数据、角速度数据等，环境配置是指当前基站在该仓储环境中自定义的坐标系当中的坐标位置，基站的唯一识别号与测量值数据中的基站号相匹配。算法处理模块决定了整个系统的性能。
定位结果实时显示模块：由算法模块输出的待定位标签当前位置的定位估计值，通过UDP的方式发送给显示模块。本实施例中，可以通过在Java 中调用Matlab的绘图接口实现二维显示，原理是通过将Matlab中的自定义函数输出Java的包文件，导入到当前Java项目中，同普通的Java函数方式一致，直接调用自定义的函数接口可达到Matlab显示的目的。但这并不是唯一的显示方法。该方式可直观的显示定位误差范围，对评估算法模块的性能有着关键性作用。
数据存储模块：在定位测试过程中，需记录中间过程每时刻的信息数据。记录数据有以下优点：方便分析误差产生的原因以给予相应的误差补偿；方便评估算法模块的稳定性和定位精度。整个数据记录可以控制数据信息的输出方式：控制台、文件、图形用户界面组件等，数据存储以压缩形式的txt 文件进行保存。
下面继续通过具体的例子，进一步说明本发明的技术方案和技术效果，以下实例是对本发明的解释而本发明并不局限于以下实例。
本实施例以同一室内包含4个基站(基站0、基站1、基站2和基站3) 为例。
第一步：将空旷的室内仓储环境划分为M个网格，在每个网格中进行数据信息的采集
结合图4的UWB与IMU非视距误差缓解流程图。首先，在空旷的室内仓储环境下，该环境属于在未移入货架、货柜等遮挡物的视距条件。根据该环境的实际情况以及满足定位精度的条件下划分M个网格，将带有定位标签的AGV小车在每个网格中进行数据信息的采集和存储，每个网格采集的数据信息表示为一组。其中，一组数据信息包含四个基站的测距值和四个基站接收的信号强度值和UWB定位解算的坐标值，接收信号强度值由接收信号强度算法所获得，位置定位坐标由算法处理模块根据双曲线模型构造非线性方程组进而采用Chan-Taylor融合定位算法获得。
定义第r组网格接收的四个信号强度信息和四个基站的测距值分别为向量p'
p'
d′
M组网格分别设接收的信号强度集为P'
P'
D'
Pos'
P'
第二步：在实际的室内仓储环境中，对AGV小车任一物流路径每一时刻进行数据信息采集
结合图4的UWB与IMU非视距误差缓解流程图。当移入货架、货柜等遮挡物后，此时为实际的室内环境。系统对AGV小车任一物流路径每一时刻进行数据信息采集，即相同时间间隔采集一组数据信息。其中，每一时刻采集的数据信息包含四个基站的测距值、四个基站接收的信号强度值和UWB 定位解算的坐标值；接收信号强度值由接收信号强度算法所获得，位置定位坐标由算法处理模块根据双曲线模型构造非线性方程组进而采用Chan-Taylor 融合定位算法获得。
数据信息分别表示为：
p
d
其中，p
第三步：判断空旷的室内仓储环境和实际的室内仓储环境的一致性
本发明分别定义源信号强度变量
本发明在实施中具体采用以下方式来表示：
δ
α
定义一个新向量p′
相应地，获取对应的向量d′
同理，定义一个新向量d′
相应地，获取对应的向量p′
同理，定义一个新向量pos′
相应地，获取对应的向量p'
结合图4的UWB与IMU非视距误差缓解流程图。通过上述三个变量获得的仓储环境与空旷环境的一致性程度，可分为如下三种情形以区分实际仓储环境下的视距、遮挡情形：
情形一：结合图5的三点重合网格图，三个网格位置重合，此时可认定四基站为完全视距情形，定义该网格位置pos'
情形二：结合图6的两点重合网格图，此时可认定四基站为不完全遮挡的情形，选取两个网格位置重合的数据信息，定义该网格位置pos'
情形三：结合图7的三点分散网格图，表示待测标签与至少两个基站传输过程中处于非视距，属于遮挡严重的情形，输出此时刻四个基站带有非视距误差的测距值d
第四步：根据判断结果对非视距误差进行补偿，计算k时刻待测目标的最终位置pos
所述的情形一和情形二，通过式
同时，第k时刻根据Chan-Taylor定位算法解算的位置坐标为
其中，pos′
所述的情形三，结合图4的UWB与IMU非视距误差缓解流程图，在实际室内仓储环境下，IMU固定在AGV小车上，可测得加速度与角速度等运动参数。
算法处理模块根据IMU定位解算算法二次积分得出AGV小车的位置和速度。通过坐标系变换求得旋转矩阵
其中
式中，
在采样间隔ΔT较短时，载体目标近似为匀速直线运动，因此用Δv
其中，
设k-1时刻在导航坐标n系下的速度为
其中，
进一步地，设Δpos
其中，
进一步地，pos
其中，
具体地，第三种情形中，当处于遮挡严重情形时，若单独用IMU定位时，随着时间的延长，有累计误差的存在，定位精确度会较低，因此采用EKF(扩展卡尔曼滤波)算法将IMU数据和UWB带有非视距误差的测距值进行融合滤波处理。
在实际应用中，将IMU在导航坐标n系即UWB定位坐标系下k-1时刻加速度计获得的数据
X
Z
其中，F表示系统的状态转移矩阵，B为控制输入矩阵，G表示噪声驱动矩阵，w
在实际情况下，当采样间隔时间为ΔT，建立系统状态方程为：
其中，pos
将上述方程组转换为矩阵形式，则系统的状态方程为：
其中系统的状态转移矩阵、控制输入矩阵和噪声驱动矩阵为：
因为四基站的位置已知且固定，设四个基站的位置坐标为(x
其中标签到四个基站测距值为：
上式为非线性的，因此需对其进行线性化处理。对非线性函数h(·)进行一阶Taylor级数展开后可得到雅可比矩阵H
其中：
此时可得到经过线性化后的系统的状态方程为：
Z
其中，Δv
1、预测状态：
2、预测状态协方差矩阵：
P
3、计算卡尔曼滤波增益矩阵：
4、更新状态：
5、更新状态协方差矩阵：
P
以上5个步骤为EKF一个计算周期，基于上述融合所得，在保证UWB 和IMU获取数据同步的情况下，当处于非视距的基站多于两个时的遮挡严重情形，此时系统基于EKF将UWB和IMU融合定位得到该k时刻待测目标的最终位置pos
实施例中，利用接收信号强度算法获取基站接收的信号强度值的具体方式为：
其中p是基站接收标签的信号强度功率值；P
UWB定位解算的坐标值由双曲线模型构造非线性方程组进而采用 Chan-Taylor融合定位算法获得：利用双曲线模型构造含有目标节点位置坐标信息的非线性方程组，采用Chan-Taylor融合定位算法是利用Chan算法给 Taylor级数法提供相对精度的初始位置，进而以此为基础进行Taylor级数展开，实现对目标标签的定位。Chan算法是一种非递归算法，它不需要初值，仅进行两次迭代就可求得最终结果，该算法是在当信号处于视距情形下传输时以及TDOA测量值较准确的前提下，定位精度较高；但当信号在非视距情形下传输及信道性能较差的情况下，定位精度较低。Taylor级数展开法是一种需初始估计位置的递归算法，通过不断递归来改进估计位置，逐步逼近真实值，该算法适用于各种信道环境，但计算量较大，又对初始估计位置要求较高。在初始估计位置与实际位置比较接近的情况下，得到的定位结果较准确。若初始位置的估计值偏差较大，直接影响该算法的定位精度。在仓储室内环境下存在许多遮挡物，Chan算法虽只仅限于信号在视距传输时定位精度较高，单对于确定标签的初始位置，有很大实际作用，尽管在信道性能处于并不优良情况，定位效果有所下降，但效果仍可以反应实际情况，可作为 Taylor级数法的初始值。
利用Chan算法给Taylor算法提供相对精确的初值，以此基础进行Taylor 展开，优化两种算法的缺陷，对AGV小车在仓储环境下的定位有更好的效果。具体为：
其中，R
其中，
G
其中：
进行第一次LS(最小二乘法)估计，得到
其中：
ψ＝4BQB
B＝diag(R
e＝diag(e
Q＝E[ee
其中e
Z'
其中：
得到第二次LS估计
其中：
其中，ψ′表示第二次LS的误差均值，
对待测目标标签的位置估计：
以此位置估计作为Taylor算法的初值坐标值，以此位置进行Taylor展开，并忽略二阶及二阶以上的项，则可获得误差矢量的表达式：
φ＝h
其中：
其中，(x
其中，Q为测量值误差的协方差矩阵，则下一步递归初始值变更为：
x'＝x+Δx
y'＝y+Δy
按照以上步骤一直递归迭代计算，直到Δx,Δy满足预先设定的门限值ε， |Δx|+|Δy|＜ε，迭代结束，得到第k时刻的定位解算值pos
本发明将UWB于IMU传感器进行结合，对定位目标进行非视距误差的缓解。在满足定位精度条件下，通过对空旷室内仓储环境下进行网格划分，对网格的定位数据信息和实际室内仓储环境下每个时刻数据信息进行标定，基于源信号强度变量、源测距值变量和源位置变量的不同指标评价实际仓储环境当前时刻获取信息与空旷环境对应网格的一致性程度。同时通过上述源信号变量、源测距值变量和源位置变量，根据这三个指标对应网格位置的重叠程度，分析得到完全视距、不完全视距、遮挡严重三种情形。在遮挡严重情形时将IMU与UWB对待测目标进行协同定位，并基于EKF(扩展卡尔曼滤波算法)将两者进行数据融合处理得到该时刻下目标的最终位置。而在完全视距和不完全视距时，则采用平均值测算法进行目标位置的估计。全面地利用了传感器所获取的数据信息，根据不同情形的视距情况采取不同方法对待测目标进行定位，从而缓解了非视距对误差带来的影响，提高定位精度，达到了在实际情况下灵活处理、优势凸显的作用。
本发明还可以通过UWB和其他传感器，如同步定位与建图(SLAM)对 AGV小车进行协同定位，也可达到定位中非视距误差缓解的效果。
以上所述的实施例只是本发明的一种较佳的方案，并非对本发明作任何形式上的限制，在不超出权利要求所记载的技术方案的前提下还有其它的变体及改型。
