CN114598990A - 一种基于uwb和imu的非视距误差补偿的室内定位方法及系统 - Google Patents

Abstract

本发明公开了一种基于UWB和IMU的非视距误差补偿的室内定位方法，克服了现有技术中利用UWB和IMU进行室内定位时存在非视距误差的问题，包括下列步骤：将空旷的室内仓储环境划分为M个网格，在每个网格中进行数据信息的采集；在实际的室内仓储环境中，对AGV小车任一物流路径每一时刻进行数据信息采集；判断空旷的室内仓储环境和实际的室内仓储环境的一致性；根据判断结果，分情况计算k时刻待测目标的最终位置pos_k。还提供了一种基于UWB和IMU的非视距误差补偿的室内定位系统。缓解了非视距对误差带来的影响，提高定位精度，达到了在实际情况下灵活处理、优势凸显的作用。

Description

一种基于UWB和IMU的非视距误差补偿的室内定位方法及系统

技术领域

本发明涉及室内定位导航技术领域，特别涉及了一种基于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_k。

本发明通过对空旷室内仓储环境下进行网格划分，得到的离散化数据信息和实际室内仓储环境在每时刻进行数据标定，根据权衡数据信息一致性程度的变量值以及表征不同信息的向量值得到不同情形下的视距情况，并根据视距情况考虑将IMU融入定位。全面地利用了传感器所获取的数据信息，根据不同情形的视距情况采取不同方法对待测目标进行定位，从而缓解了非视距对误差带来的影响，提高定位精度，达到了在实际情况下灵活处理、优势凸显的作用。

作为优选，所述的步骤S1具体步骤为：

S1.1：根据实际的空旷的室内仓储环境，在满足定位精度的条件下将其划分为M个网格；

S1.2：将带有定位标签的AGV小车在每个网格中进行数据信息的采集和存储；

S1.2.1：每个网格采集的数据信息表示为一组，一组数据信息包含每个基站的测距值、每个基站接收的信号强度值和UWB定位解算的坐标值；

S1.2.2：定义第r组网格接收的信号强度信息基站的测距值分别为向量p′_r， d′_r以及坐标值pos'_r，其中{r|1≤r≤M，r∈N⁺}，可表示为：

p′_r＝[p′_1,r，p′_2,r，…，p′_n,r]^T

d′_r＝[d′_1,r，d′_2,r，…，d′_n,r]^T

式中，n表示基站个数；

S1.2.3：在M组网格中，分别设接收的信号强度集为P'_M，测距集为D'_M和坐标集为Pos'_M，分别表示为：

P'_M＝[p′₁ p′₂ ... p′_r ... p′_M]

D′_M＝[d′₁ d′₂ ... d′_r ... d′_M]

Pos′_M＝[pos′₁,pos′₂,...pos′_r,...pos′_M]^T

P'_M、D'_M、Pos'_M分别存储了M组网格标签的数据信息。

在空旷的室内仓储环境下，该环境属于在未移入货架、货柜等遮挡物的视距条件。

作为优选，所述的步骤S2，进一步包括：

每一时刻采集的数据信息包含每个基站的测距值、每个基站接收的信号强度值和UWB定位解算的坐标值；数据信息分别表示为：

p_k＝[p_1,k，p_2,k，…，p_n,k]^T

d_r＝[d_1,k，d_2,k，…，d_n,k]^T

其中，p_n,k、d_n,k分别表示每个基站在该k时刻获取的信号强度值和测距值的数据信息，p_k、d_k分别表示一个向量为k时刻各基站所获得的信号强度向量和与标签的距离向量，

为k时刻UWB定位解算获得的标签坐标值。

当移入货架、货柜等遮挡物后，此时为实际的室内环境。

作为优选，所述的步骤S3中，判断空旷的室内仓储环境和实际的室内仓储环境的一致性：

通过定义源信号强度变量

源测距变量δ_r、源位置变量α_r来评价实际仓储环境与空旷环境的标签UWB信息的一致性程度，其中：

δ_r＝||d_k-d′_r||

α_r＝||pos_k-pos′_r||；

定义一个新向量p′_s，其中{s|1≤s≤M，s∈N⁺}，满足：

获取对应的向量d′_s、pos′_s和三个变量值

δ_s、α_s；

定义一个新向量d′_l，{l|1≤l≤M，l∈N⁺}，满足：

获取对应的向量p′_l、pos′_l和三个变量值δ_l、

α_l；

定义一个新向量pos′_z，{z|1≤z≤M，z∈N⁺}，满足：

获取对应的向量p'_z、d'_z和三个变量值α_z、δ_z、

作为优选，所述的步骤S3中，根据一致性程度分为三种情形以区分实际仓储环境下的视距、遮挡情形：

A1：基站为完全视距情形，定义该网格位置pos′_v；

A2：基站为不完全遮挡，定义该网格位置pos′_v；

A3：基站为严重遮挡，输出此时刻基站带有非视距误差的测距值d_k。

若视距、遮挡情形为情形一或情形二，采用平均值测算法进行目标位置的估计；若视距、遮挡情形为情形三，基于扩展卡尔曼滤波算法EKF将UWB 和IMU进行数据融合处理得到该时刻下目标的最终位置。

作为优选，所述的步骤S4中，根据判断结果计算k时刻待测目标的最终位置pos_k：

S4.1：为A1和A2的情形：

计算网格位置坐标：

第k时刻根据Chan-Taylor定位算法解算的位置坐标为

通过对两个位置坐标求平均值可得到该k时刻待测目标最终的位置pos_k，具体为：

其中，pos'_v＝(pos′_v,x,pos′_v,y)，

为根据UWB进行 Chan-Taylor定位解算获得的位置坐标；

S4.2：为A3的情形：

S4.2.1：得到IMU得到的k时刻导航坐标系的位置：

B1：IMU获取AGV小车包括加速度与角速度在内的运动参数，根据IMU 定位解算算法二次积分得出AGV小车的位置和速度；

B2：通过坐标系变换求得旋转矩阵

将载体坐标系上的加速度a^b转换，获得导航坐标n系下的加速度aⁿ，具体为：

其中：

表示载体坐标b系下的水平、垂直方向的加速度；

B3：在采样间隔ΔT较短时，载体目标近似为匀速直线运动，用Δv_n表示导航坐标n系下系统的速度变化，可得到：

其中，

分别表示导航坐标系下系统在水平、垂直方向上的速度变化，

表示导航坐标n系下系统在水平、垂直方向的加速度；

B4：设k-1时刻在导航坐标n系下的速度为

k时刻速度

可表示为：

其中，

表示k时刻导航坐标n系下水平、垂直方向的速度；

表示k-1时刻导航坐标n系下水平、垂直方向的速度；

B5：设Δposⁿ为导航坐标n系下的位移变化，具体为：

其中，

分别表示导航坐标n系下在水平、垂直方向上的位移变化；

B6：计算k-1时刻待测目标的实际位置：

pos_k-1＝(pos_k-1,x,pos_k-1,y)

得到IMU得到的k时刻待测目标导航坐标系的位置：

S4.2.2：采用扩展卡尔曼滤波即EKF算法将IMU数据和UWB带有非视距误差的测距值进行融合滤波处理，得到k时刻待测目标的最终位置pos_k。

载体坐标系上的加速度a^b指的是小车实际运动中的加速度。当处于遮挡严重情形时，若单独用IMU定位时，随着时间的延长，有累计误差的存在，定位精确度会较低，因此采用EKF(扩展卡尔曼滤波)算法将IMU数据和 UWB带有非视距误差的测距值进行融合滤波处理。

作为优选，所述的步骤S4.2.2，具体表示为：

C1：将IMU在导航坐标n系即UWB定位坐标系下k-1时刻加速度计获得的数据

作为系统的输入，将UWB在k时刻得到的基站的测距值作为系统的观测向量Z_k＝[d_1,k d_2,k … d_n,k]^T，将k-1时刻目标 AGV小车的速度和位置作为系统的状态向量X_k-1＝[pos_k-1,x pos_k-1,y v_k-1,x v_k-1,y]^T，根据EKF原理，建立系统模型如下：

X_k＝FX_k-1+Bu_k-1+Gw_k-1

Z_k＝h[X_k]+v_k

其中，F表示系统的状态转移矩阵，B为控制输入矩阵，G表示噪声驱动矩阵，w_k-1＝[w_k-1,x w_k-1,y]^T表示均值为零，方差为

的过程噪声矩阵，h[X_k]＝[d_1,k d_2,k … d_n,k]^T表示系统在k时刻与测距值相关的非线性观测函数， v_k＝[v_1,k v_2,k… v_n,k]^T表示均值为零，方差为

的测距距离观测噪声矩阵；

C2：建立系统的观测方程：

状态方程：Z_k-Z_k-1＝ΔZ_k＝H_kX_k+Δv_k；

C3：初始化状态均值U(0)＝E[X(0)]，状态协方差矩阵P(0)＝var[X(0)]，进行EKF迭代：

预测状态：

预测状态协方差矩阵：P_k/k-1＝FP_k-1|k-1F^T+GQG^T

其中，预测状态的协方差矩阵P_k/k-1，通过状态转移矩阵F乘以前一时刻的误差协方差矩阵P_k-1/k-1再乘以状态转移矩阵F的转置即F^T，加上过程噪声驱动矩阵G乘以过程噪声矩阵Q再乘以过程噪声驱动矩阵G的转置即G^T。

计算卡尔曼滤波增益矩阵：

更新状态：

更新状态协方差矩阵：P_k|k＝[I_n-KH_k]P_k|k-1

其中，I_n为n×n矩阵，将以上5个步骤做为EKF一个计算周期，基于EKF 将UWB和IMU融合定位得到该k时刻待测目标的最终位置pos_k。

以四个基站为例，建立状态方程和观测方程的具体过程为：

在实际情况下，当采样间隔时间为ΔT，建立系统状态方程为：

其中，pos_k＝(pos_k,x,pos_k,y)，pos_k,x、pos_k,y分别表示k时刻水平、垂直方向位置坐标值，v_k,x、v_k,y分别表示k时刻水平、垂直方向的速度值；

将C2中得到的方程组转换为矩阵形式，则系统的状态方程为：

其中系统的状态转移矩阵、控制输入矩阵和噪声驱动矩阵分别为：

因为基站的位置已知且固定，设基站的位置坐标为：(x_i，y_i)，i＝1,2,…n，其中UWB在k时刻定位的标签位置为

系统的观测方程为：

其中标签到每个基站测距值为：

进行线性化处理，对非线性函数h(·)进行一阶Taylor级数展开后可得到雅可比矩阵H_k为：

其中：

得到经过线性化后的系统的状态方程为：Z_k-Z_k-1＝ΔZ_k＝H_kX_k+Δv_k；

其中，Δv_k＝v_k-v_k-1。

作为优选，每个基站接收的信号强度值由接收信号强度算法所获得，具体表现为：

其中p是基站接收标签的信号强度功率值；P_CIR为信道脉冲响应的信号强度功率值，N和λ为实验常数参数；N是指在寄存器中的前导码累加计数值，即PAC。

其中λ是当PRF(平均脉冲重复频率)为16MHz时为115.72，或者当PRF (平均脉冲重复频率)为64MHz时为121.74，本发明仓储环境下优选采用 16MHz的PRF，因此λ为115.72。

作为优选，UWB定位解算的坐标值由双曲线模型构造非线性方程组进而采用Chan-Taylor融合定位算法获得，具体表现为：

根据：

得到：

其中，R_i表示各基站到标签位置(x,y)的距离，(x_i,y_i)为基站i的坐标，(x,y) 为待测目标标签的坐标，参与定位算法的基站共n个，

当n＞2时，方程组未知数小于方程组方程个数，为超定方程组，且方程为非线性方程；

由此得到：

G_aZ_a＝h

其中：

进行第一次LS(最小二乘法)估计，得到

其中：

ψ＝4BQB

B＝diag(R₁,R₂,...,R_n)

e＝diag(e₁,e₂,...,e_n)

Q＝E[ee^T]

式中，e_i为R_i所对应的误差量；

进行第二次LS(最小二乘法)估计，得到：

Z'_aG'_a＝h'

其中：

则第二次LS估计

其中：

对待测目标标签的位置估计：

以此位置估计作为Taylor算法的初值坐标值，以此位置进行Taylor展开，并忽略二阶及二阶以上的项，则可获得误差矢量的表达式：

φ＝h_d-G_dδ_d

其中：

(x_i,y_i)表示基站的位置坐标，i＝1,2......n；R_i表示各基站到标签初始位置 (x,y)的距离，R_i,1表示待测标签到第i个基站的距离与标签到第1个基站的距离的差值；

表达式的WLS(加权最小二乘法解)为：

其中，Q为测量值误差的协方差矩阵，则下一步递归初始值变更为：

x'＝x+Δx

y'＝y+Δy

按照以上步骤一直递归迭代计算，直到Δx,Δy满足预先设定的门限值ε， |Δx|+|Δy|＜ε，迭代结束，得到第k时刻的定位解算值pos_k,uwb。

采用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_k。

本发明提供的基于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'_r，d′_r以及坐标值pos'_r，其中{r|1≤r≤M，r∈N⁺}，可表示为：

p'_r＝[p′_1,r,p'_2,r,p'_3,r,p'_4,r]^T

d′_r＝[d′_1,r,d'_2,r,d′_3,r,d'_4,r]^T

M组网格分别设接收的信号强度集为P'_M，测距集为D'_M和坐标集为Pos'_M，分别表示为：

P'_M＝[p′₁ p'₂ ... p'_r ... p'_M]

D'_M＝[d′₁ d'₂ ... d′_r ... d'_M]

Pos'_M＝[pos′₁,pos'₂,...pos'_r,...pos'_M]^T

P'_M、D'_M、Pos'_M分别存储了M组网格标签的数据信息。

第二步：在实际的室内仓储环境中，对AGV小车任一物流路径每一时刻进行数据信息采集

结合图4的UWB与IMU非视距误差缓解流程图。当移入货架、货柜等遮挡物后，此时为实际的室内环境。系统对AGV小车任一物流路径每一时刻进行数据信息采集，即相同时间间隔采集一组数据信息。其中，每一时刻采集的数据信息包含四个基站的测距值、四个基站接收的信号强度值和UWB 定位解算的坐标值；接收信号强度值由接收信号强度算法所获得，位置定位坐标由算法处理模块根据双曲线模型构造非线性方程组进而采用Chan-Taylor 融合定位算法获得。

数据信息分别表示为：

p_k＝[p_1,k,p_2,k,p_3,k,p_4,k]^T

d_k＝[d_1,k,d_2,k,d_3,k,d_4,k]^T

其中，p_j,k、d_j,k(j＝1,2,3,4)分别表示每个基站在该k时刻获取的信号强度值和测距值的数据信息，p_k、d_k分别表示一个向量为k时刻各基站所获得的信号强度向量和与标签的距离向量，

为k时刻UWB定位解算获得的标签坐标值。

第三步：判断空旷的室内仓储环境和实际的室内仓储环境的一致性

本发明分别定义源信号强度变量

源测距变量δ_r、源位置变量α_r来评价实际仓储环境与空旷环境的标签UWB信息的一致性程度。

本发明在实施中具体采用以下方式来表示：

δ_r＝||d_k-d′_r||

α_r＝||pos_k-pos′_r||。

定义一个新向量p′_s，其中{s|1≤s≤M，s∈N⁺}，满足：

相应地，获取对应的向量d′_s、pos'_s和三个变量值

δ_s、α_s。

同理，定义一个新向量d′_l，{l|1≤l≤M，l∈N⁺}，满足：

相应地，获取对应的向量p′_l、pos′_l和三个变量值δ_l、

α_l。

同理，定义一个新向量pos′_z，{z|1≤z≤M，z∈N⁺}，满足：

相应地，获取对应的向量p'_z、d'_z和三个变量值α_z、δ_z、

结合图4的UWB与IMU非视距误差缓解流程图。通过上述三个变量获得的仓储环境与空旷环境的一致性程度，可分为如下三种情形以区分实际仓储环境下的视距、遮挡情形：

情形一：结合图5的三点重合网格图，三个网格位置重合，此时可认定四基站为完全视距情形，定义该网格位置pos'_v。其中pos'_v与pos'_z最接近，通过 Chan-Taylor定位算法得到。

情形二：结合图6的两点重合网格图，此时可认定四基站为不完全遮挡的情形，选取两个网格位置重合的数据信息，定义该网格位置pos'_v。

情形三：结合图7的三点分散网格图，表示待测标签与至少两个基站传输过程中处于非视距，属于遮挡严重的情形，输出此时刻四个基站带有非视距误差的测距值d_k。

第四步：根据判断结果对非视距误差进行补偿，计算k时刻待测目标的最终位置pos_k

所述的情形一和情形二，通过式

获得网格位置坐标pos'_v。

同时，第k时刻根据Chan-Taylor定位算法解算的位置坐标为

通过对两个位置坐标求平均值可得到该k时刻待测目标最终的位置pos_k，具体为：

其中，pos′_v＝(pos′_v,x,pos′_v,y)，

根据UWB 进行Chan-Taylor定位解算获得的位置坐标。

所述的情形三，结合图4的UWB与IMU非视距误差缓解流程图，在实际室内仓储环境下，IMU固定在AGV小车上，可测得加速度与角速度等运动参数。

算法处理模块根据IMU定位解算算法二次积分得出AGV小车的位置和速度。通过坐标系变换求得旋转矩阵

将载体坐标系上的加速度a^b转换获得导航坐标n系下的加速度aⁿ，具体为：

其中

式中，

表示载体坐标b系下的水平、垂直方向的加速度。

在采样间隔ΔT较短时，载体目标近似为匀速直线运动，因此用Δv_n表示导航坐标n系下系统的速度变化，可得到

其中，

分别表示导航坐标系下系统在水平、垂直方向上的速度变化，

表示导航坐标n系下系统在水平、垂直方向的加速度。

设k-1时刻在导航坐标n系下的速度为

k时刻速度

可表示为：

其中，

表示k时刻导航坐标n系下水平、垂直方向的速度；

表示k-1时刻导航坐标n 系下水平、垂直方向的速度。

进一步地，设Δposⁿ为导航坐标n系下的位移变化，具体为：

其中，

分别表示导航坐标n系下在水平、垂直方向上的位移变化。

进一步地，pos_k-1表示k-1时刻的本发明算法所得的实际位置，其中，pos_k-1＝(pos_k-1,x,pos_k-1,y)、

具体为：

其中，

为IMU得到的k时刻导航坐标系的位置。

具体地，第三种情形中，当处于遮挡严重情形时，若单独用IMU定位时，随着时间的延长，有累计误差的存在，定位精确度会较低，因此采用EKF(扩展卡尔曼滤波)算法将IMU数据和UWB带有非视距误差的测距值进行融合滤波处理。

在实际应用中，将IMU在导航坐标n系即UWB定位坐标系下k-1时刻加速度计获得的数据

作为系统的输入，将UWB在k时刻得到的四个基站的测距值作为系统的观测向量Z_k＝[d_1,k d_2,k d_3,k d_4,k]^T，将k-1时刻目标AGV小车的速度和位置作为系统的状态向量 X_k-1＝[pos_k-1,x pos_k-1,y v_k-1,x v_k-1,y]^T，根据EKF原理，建立系统模型如下：

X_k＝FX_k-1+Bu_k-1+Gw_k-1

Z_k＝h[X_k]+v_k

其中，F表示系统的状态转移矩阵，B为控制输入矩阵，G表示噪声驱动矩阵，w_k-1＝[w_k-x1,w_k-y]^T表示均值为零，方差为

的过程噪声矩阵，h[X_k]＝[d_1,k d_2,k d_3,k d_4,k]^T表示系统在k时刻与测距值相关的非线性观测函数，v_k＝[v_1,k v_2,kv_3,k v_4,k]^T表示均值为零，方差为

的测距距离观测噪声矩阵。

在实际情况下，当采样间隔时间为ΔT，建立系统状态方程为：

其中，pos_k＝(pos_k,x,pos_k,y)，pos_k,x、pos_k,y分别表示k时刻水平、垂直方向位置坐标值，v_k,x、v_k,y分别表示k时刻水平、垂直方向的速度值。

将上述方程组转换为矩阵形式，则系统的状态方程为：

其中系统的状态转移矩阵、控制输入矩阵和噪声驱动矩阵为：

因为四基站的位置已知且固定，设四个基站的位置坐标为(x_i,y_i),i＝1,2,3,4，其中UWB在k时刻定位的标签位置为

系统的观测方程为：

其中标签到四个基站测距值为：

上式为非线性的，因此需对其进行线性化处理。对非线性函数h(·)进行一阶Taylor级数展开后可得到雅可比矩阵H_k为：

其中：

此时可得到经过线性化后的系统的状态方程为：

Z_k-Z_k-1＝ΔZ_k＝H_kX_k+Δv_k

其中，Δv_k＝v_k-v_k-1。在确定好系统的状态方程和观测方程后，根据EKF (扩展卡尔曼滤波)算法过程，首先初始化状态均值U(0)＝E[X(0)]，状态协方差矩阵P(0)＝var[X(0)]。I_n为n×n矩阵，EKF迭代过程由下述公式给出：

1、预测状态：

2、预测状态协方差矩阵：

P_k/k-1＝FP_k-1|k-1F^T+GQG^T

3、计算卡尔曼滤波增益矩阵：

4、更新状态：

5、更新状态协方差矩阵：

P_k|k＝[I_n-KH_k]P_k|k-1

以上5个步骤为EKF一个计算周期，基于上述融合所得，在保证UWB 和IMU获取数据同步的情况下，当处于非视距的基站多于两个时的遮挡严重情形，此时系统基于EKF将UWB和IMU融合定位得到该k时刻待测目标的最终位置pos_k。

实施例中，利用接收信号强度算法获取基站接收的信号强度值的具体方式为：

其中p是基站接收标签的信号强度功率值；P_CIR为信道脉冲响应的信号强度功率值，包含信道的路径、衰减等信息；N和λ为实验常数参数；N是指在寄存器中的PAC，即前导码累加计数值。其中λ当PRF(平均脉冲重复频率)为16MHz时为115.72，或者当PRF(平均脉冲重复频率)为64MHz时为121.74。本实施例仓储环境下采用的是16MHz的PRF，因此λ为115.72。

UWB定位解算的坐标值由双曲线模型构造非线性方程组进而采用 Chan-Taylor融合定位算法获得：利用双曲线模型构造含有目标节点位置坐标信息的非线性方程组，采用Chan-Taylor融合定位算法是利用Chan算法给 Taylor级数法提供相对精度的初始位置，进而以此为基础进行Taylor级数展开，实现对目标标签的定位。Chan算法是一种非递归算法，它不需要初值，仅进行两次迭代就可求得最终结果，该算法是在当信号处于视距情形下传输时以及TDOA测量值较准确的前提下，定位精度较高；但当信号在非视距情形下传输及信道性能较差的情况下，定位精度较低。Taylor级数展开法是一种需初始估计位置的递归算法，通过不断递归来改进估计位置，逐步逼近真实值，该算法适用于各种信道环境，但计算量较大，又对初始估计位置要求较高。在初始估计位置与实际位置比较接近的情况下，得到的定位结果较准确。若初始位置的估计值偏差较大，直接影响该算法的定位精度。在仓储室内环境下存在许多遮挡物，Chan算法虽只仅限于信号在视距传输时定位精度较高，单对于确定标签的初始位置，有很大实际作用，尽管在信道性能处于并不优良情况，定位效果有所下降，但效果仍可以反应实际情况，可作为 Taylor级数法的初始值。

利用Chan算法给Taylor算法提供相对精确的初值，以此基础进行Taylor 展开，优化两种算法的缺陷，对AGV小车在仓储环境下的定位有更好的效果。具体为：

其中，R_i表示各基站到标签位置(x,y)的距离，(x_i,y_i)为基站i的坐标，(x,y) 为待测目标标签的坐标，参与定位算法的基站共n个。通过上式可得到表达式：

其中，

当n＞2时，方程组未知数小于方程组方程个数，为超定方程组，且方程为非线性方程。根据上述表达式，可得到如下表达式：

G_aZ_a＝h

其中：

进行第一次LS(最小二乘法)估计，得到

其中：

ψ＝4BQB

B＝diag(R₁,R₂,...,R_n)

e＝diag(e₁,e₂,...,e_n)

Q＝E[ee^T]

其中e_i为R_i所对应的误差量。在第一次LS中，前提条件时假设Z_a矩阵中三个未知数x,y,K相互独立，第二次LS利用三个未知量的内在联系构造新方程组，并考虑第一次得到的

存在一定误差，得到表达式：

Z'_aG'_a＝h'

其中：

得到第二次LS估计

其中：

其中，ψ′表示第二次LS的误差均值，

表示第一次LS(最小二乘法)估计值

的协方差值，

表示第一次LS(最小二乘法)估计值的水平x、垂直y的坐标值。

对待测目标标签的位置估计：

以此位置估计作为Taylor算法的初值坐标值，以此位置进行Taylor展开，并忽略二阶及二阶以上的项，则可获得误差矢量的表达式：

φ＝h_d-G_dδ_d

其中：

其中，(x_i,y_i)表示基站的位置坐标，i＝1,2......n；R_i表示各基站到标签初始位置(x,y)的距离，R_i,1表示待测标签到第i个基站的距离与标签到第1个基站的距离的差值。则表达式的WLS(加权最小二乘法解)为：

其中，Q为测量值误差的协方差矩阵，则下一步递归初始值变更为：

x'＝x+Δx

y'＝y+Δy

按照以上步骤一直递归迭代计算，直到Δx,Δy满足预先设定的门限值ε， |Δx|+|Δy|＜ε，迭代结束，得到第k时刻的定位解算值pos_k,uwb。

本发明将UWB于IMU传感器进行结合，对定位目标进行非视距误差的缓解。在满足定位精度条件下，通过对空旷室内仓储环境下进行网格划分，对网格的定位数据信息和实际室内仓储环境下每个时刻数据信息进行标定，基于源信号强度变量、源测距值变量和源位置变量的不同指标评价实际仓储环境当前时刻获取信息与空旷环境对应网格的一致性程度。同时通过上述源信号变量、源测距值变量和源位置变量，根据这三个指标对应网格位置的重叠程度，分析得到完全视距、不完全视距、遮挡严重三种情形。在遮挡严重情形时将IMU与UWB对待测目标进行协同定位，并基于EKF(扩展卡尔曼滤波算法)将两者进行数据融合处理得到该时刻下目标的最终位置。而在完全视距和不完全视距时，则采用平均值测算法进行目标位置的估计。全面地利用了传感器所获取的数据信息，根据不同情形的视距情况采取不同方法对待测目标进行定位，从而缓解了非视距对误差带来的影响，提高定位精度，达到了在实际情况下灵活处理、优势凸显的作用。

本发明还可以通过UWB和其他传感器，如同步定位与建图(SLAM)对 AGV小车进行协同定位，也可达到定位中非视距误差缓解的效果。

以上所述的实施例只是本发明的一种较佳的方案，并非对本发明作任何形式上的限制，在不超出权利要求所记载的技术方案的前提下还有其它的变体及改型。

Claims (10)

1.一种基于UWB和IMU的非视距误差补偿的室内定位方法，其特征在于，它包括下列步骤：

S1：将空旷的室内仓储环境划分为M个网格，在每个网格中进行数据信息的采集；

S2：在实际的室内仓储环境中，对AGV小车任一物流路径每一时刻进行数据信息采集；

S3：判断空旷的室内仓储环境和实际的室内仓储环境的一致性；

S4：根据判断结果，分情况计算k时刻待测目标的最终位置pos_k。

2.根据权利要求1所述的一种基于UWB和IMU的非视距误差补偿的室内定位方法，其特征在于，所述的步骤S1具体步骤为：

S1.1：根据实际的空旷的室内仓储环境，在满足定位精度的条件下将其划分为M个网格；

S1.2：将带有定位标签的AGV小车在每个网格中进行数据信息的采集和存储；

S1.2.1：每个网格采集的数据信息表示为一组，一组数据信息包含每个基站的测距值、每个基站接收的信号强度值和UWB定位解算的坐标值；

S1.2.2：定义第r组网格接收的信号强度信息基站的测距值分别为向量p'_r，d'_r以及坐标值pos'_r，其中{r|1≤r≤M，r∈N⁺}，可表示为：

p′_r＝[p′_1,r，p′_2,r，…，p′_n,r]^T

d′_r＝[d′_1,r，d′_2,r，…，d′_n,r]^T

式中，n表示基站个数；

S1.2.3：在M组网格中，分别设接收的信号强度集为P′_M，测距集为D′_M和坐标集为Pos′_M，分别表示为：

P'_M＝[p'₁ p'₂...p'_r...p'_M]

D'_M＝[d'₁ d'₂...d'_r...d'_M]

Pos'_M＝[pos'₁,pos'₂,...pos'_r,...pos'_M]^T

P'_M、D'_M、Pos'_M分别存储了M组网格标签的数据信息。

3.根据权利要求1或2所述的一种基于UWB和IMU的非视距误差补偿的室内定位方法，其特征在于，所述的步骤S2，进一步包括：

每一时刻采集的数据信息包含每个基站的测距值、每个基站接收的信号强度值和UWB定位解算的坐标值；数据信息分别表示为：

p_k＝[p_1,k，p_2,k，…，p_n,k]^T

d_r＝[d_1,k，d_2,k，…，d_n,k]^T

其中，p_n,k、d_n,k分别表示每个基站在该k时刻获取的信号强度值和测距值的数据信息，p_k、d_k分别表示一个向量为k时刻各基站所获得的信号强度向量和与标签的距离向量，

为k时刻UWB定位解算获得的标签坐标值。

4.根据权利要求1所述的一种基于UWB和IMU的非视距误差补偿的室内定位方法，其特征在于，所述的步骤S3中，判断空旷的室内仓储环境和实际的室内仓储环境的一致性：

通过定义源信号强度变量

源测距变量δ_r、源位置变量α_r来评价实际仓储环境与空旷环境的标签UWB信息的一致性程度，其中：

δ_r＝||d_k-d′_r||

α_r＝||pos_k-pos′_r||；

定义一个新向量p′_s，其中{s|1≤s≤M，s∈N⁺}，满足：

获取对应的向量d'_s、pos'_s和三个变量值

δ_s、α_s；

定义一个新向量d'_l，{l|1≤l≤M，l∈N⁺}，满足：

获取对应的向量p'_l、pos'_l和三个变量值δ_l、

α_l；

定义一个新向量pos'_z，{z|1≤z≤M，z∈N⁺}，满足：

获取对应的向量p'_z、d'_z和三个变量值α_z、δ_z、

5.根据权利要求1或4所述的一种基于UWB和IMU的非视距误差补偿的室内定位方法，其特征在于，所述的步骤S3中，根据一致性程度分为三种情形以区分实际仓储环境下的视距、遮挡情形：

A1：基站为完全视距情形，定义该网格位置pos'_v；

A2：基站为不完全遮挡，定义该网格位置pos'_v；

A3：基站为严重遮挡，输出此时刻基站带有非视距误差的测距值d_k。

6.根据权利要求5所述的一种基于UWB和IMU的非视距误差补偿的室内定位方法，其特征在于，所述的步骤S4中，根据判断结果计算k时刻待测目标的最终位置pos_k：

S4.1：为A1和A2的情形：

计算网格位置坐标：

第k时刻根据Chan-Taylor定位算法解算的位置坐标为

通过对两个位置坐标求平均值可得到该k时刻待测目标最终的位置pos_k，具体为：

其中，pos'_v＝(pos'_v,x,pos'_v,y)，

为根据UWB进行Chan-Taylor定位解算获得的位置坐标；

S4.2：为A3的情形：

S4.2.1：得到IMU得到的k时刻导航坐标系的位置：

B1：IMU获取AGV小车包括加速度与角速度在内的运动参数，根据IMU定位解算算法二次积分得出AGV小车的位置和速度；

B2：通过坐标系变换求得旋转矩阵

将载体坐标系上的加速度a^b转换，获得导航坐标n系下的加速度aⁿ，具体为：

其中：

表示载体坐标b系下的水平、垂直方向的加速度；

B3：在采样间隔ΔT较短时，载体目标近似为匀速直线运动，用Δv_n表示导航坐标n系下系统的速度变化，可得到：

其中，

分别表示导航坐标系下系统在水平、垂直方向上的速度变化，

表示导航坐标n系下系统在水平、垂直方向的加速度；

B4：设k-1时刻在导航坐标n系下的速度为

k时刻速度

可表示为：

其中，

表示k时刻导航坐标n系下水平、垂直方向的速度；

表示k-1时刻导航坐标n系下水平、垂直方向的速度；

B5：设Δposⁿ为导航坐标n系下的位移变化，具体为：

其中，

分别表示导航坐标n系下在水平、垂直方向上的位移变化；

B6：计算k-1时刻待测目标的实际位置：

pos_k-1＝(pos_k-1,x,pos_k-1,y)

得到IMU得到的k时刻待测目标导航坐标系的位置：

S4.2.2：采用扩展卡尔曼滤波即EKF算法将IMU数据和UWB带有非视距误差的测距值进行融合滤波处理，得到k时刻待测目标的最终位置pos_k。

7.根据权利要求6所述的一种基于UWB和IMU的非视距误差补偿的室内定位方法，其特征在于，所述的步骤S4.2.2，具体表示为：

C1：将IMU在导航坐标n系即UWB定位坐标系下k-1时刻加速度计获得的数据

作为系统的输入，将UWB在k时刻得到的基站的测距值作为系统的观测向量Z_k＝[d_1,k d_2,k…d_n,k]^T，将k-1时刻目标AGV 小车的速度和位置作为系统的状态向量X_k-1＝[pos_k-1,x pos_k-1,y v_k-1,x v_k-1,y]^T，根据EKF原理，建立系统模型如下：

X_k＝FX_k-1+Bu_k-1+Gw_k-1

Z_k＝h[X_k]+v_k

其中，F表示系统的状态转移矩阵，B为控制输入矩阵，G表示噪声驱动矩阵，w_k-1＝[w_k-1,xw_k-1,y]^T表示均值为零，方差为

的过程噪声矩阵，h[X_k]＝[d_1,kd_2,k…d_n,k]^T表示系统在k时刻与测距值相关的非线性观测函数，v_k＝[v_1,k v_2,k…v_n,k]^T表示均值为零，方差为

的测距距离观测噪声矩阵；

C2：建立系统的观测方程：

状态方程：Z_k-Z_k-1＝ΔZ_k＝H_kX_k+Δv_k；

C3：初始化状态均值U(0)＝E[X(0)]，状态协方差矩阵P(0)＝var[X(0)]，进行EKF迭代：

预测状态：

预测状态协方差矩阵：P_k/k-1＝FP_k-1|k-1F^T+GQG^T

式中，P_k/k-1为预测状态协方差矩阵，GQG^T为预测噪声协方差矩阵；

计算卡尔曼滤波增益矩阵：

更新状态：

更新状态协方差矩阵：P_k|k＝[I_n-KH_k]P_k|k-1

其中，I_n为n×n矩阵，将以上5个步骤做为EKF一个计算周期，基于EKF将UWB和IMU融合定位得到该k时刻待测目标的最终位置pos_k。

8.根据权利要求2或3所述的一种基于UWB和IMU的非视距误差补偿的室内定位方法，其特征在于，每个基站接收的信号强度值由接收信号强度算法所获得，具体表现为：

其中p是基站接收标签的信号强度功率值；P_CIR为信道脉冲响应的信号强度功率值，N和λ为实验常数参数；N是指在寄存器中的前导码累加计数值，即PAC。

9.根据权利要求2或3所述的一种基于UWB和IMU的非视距误差补偿的室内定位方法，其特征在于，UWB定位解算的坐标值由双曲线模型构造非线性方程组进而采用Chan-Taylor融合定位算法获得，具体表现为：

根据：

得到：

其中，R_i表示各基站到标签位置(x,y)的距离，(x_i,y_i)为基站i的坐标，(x,y)为待测目标标签的坐标，参与定位算法的基站共n个，

当n＞2时，方程组未知数小于方程组方程个数，为超定方程组，且方程为非线性方程；由此得到：

G_aZ_a＝h

其中：

进行第一次LS(最小二乘法)估计，得到

其中：

ψ＝4BQB

B＝diag(R₁,R₂,...,R_n)

e＝diag(e₁,e₂,...,e_n)

Q＝E[ee^T]

式中，e_i为R_i所对应的误差量；

进行第二次LS(最小二乘法)估计，得到：

Z'_aG'_a＝h'

其中：

则第二次LS估计

其中：

对待测目标标签的位置估计：

以此位置估计作为Taylor算法的初值坐标值，以此位置进行Taylor展开，并忽略二阶及二阶以上的项，则可获得误差矢量的表达式：

φ＝h_d-G_dδ_d

其中

(x_i,y_i)表示基站的位置坐标，i＝1,2......n；R_i表示各基站到标签初始位置(x,y)的距离，R_i,1表示待测标签到第i个基站的距离与标签到第1个基站的距离的差值；

表达式的WLS(加权最小二乘法解)为：

其中，Q为测量值误差的协方差矩阵，则下一步递归初始值变更为：

x'＝x+Δx

y'＝y+Δy

按照以上步骤一直递归迭代计算，直到Δx,Δy满足预先设定的门限值ε，|Δx|+|Δy|＜ε，迭代结束，得到第k时刻的定位解算值pos_k,uwb。

10.一种基于UWB和IMU的非视距误差补偿的室内定位系统，应用于权利要求1-9中任意一项所述的一种基于UWB和IMU的非视距误差补偿的室内定位方法，其特征在于，包括：位于同一室内的若干个基站和设置在定位目标AGV小车上的定位标签和IMU模块，还包括上位机服务器(1)，所述上位机服务器包括对标签和基站上传的原始数据进行解析预处理的数据输入模块(2)、算法处理模块(3)、显示计算结果的定位结果实时显示模块(4)以及数据存储模块(5)，所述数据输入模块与算法处理模块连接，算法处理模块与显示结果计算模块连接。

Images