基于抗差LM视觉惯性里程计与UWB混合高精度室内定位方法

摘要

本发明提供一种基于抗差LM视觉惯性里程计与UWB混合高精度室内定位方法：先在室内空旷区域部署一定数量的UWB基站；将UWB和相机设备安装组合，对待测区域进行定位，所述定位，利用三次样条曲线进行内插；采用改进的LM算法优化用于视觉VIO和UWB融合的模型，有效提高室内定位的精度。

说明书

技术领域

本发明涉及室内定位技术领域，尤其是一种基于抗差LM视觉惯性里程计与UWB混合高精度室内定位方法。

背景技术

视觉导航定位是通过视觉系统在相机运动过程中提取不同的图像，通过检测这些不同图像的变化，提取并且匹配相同的特征点，判断特征点的运动变化来估计相机的运动情况。视觉里程计(VO)由于在面对单纯的旋转时无法很好地跟踪，因此在实际应用中通常会与低廉的IMU等惯性传感器进行组合，在组合后视觉惯性里程计(VIO)能够很鲁棒的应用。VIO虽然在光照条件、图像质量良好的情况下具有很高的定位精度，但在没有其他信息的情况下由于缺少全局的位置信息参考，其实质是一种局部定位算法，存在累积误差的问题。因此，目前有许多研究通过设计全局的路标或者利用其他全局的信息予以补充，提高视觉定位在长距离范围的适用性。

VIO无法避免累积误差。UWB是一种全局观测的传感器，每一次观测都是独立的，因此误差并不会累积，但是导航型GNSS定位输出频率和定位精度较低，通常无法满足用户更高的需求。从理论上看，将一个局部精度很高但存在累积误差的VIO和一个局部精度无法保证到不存在累积误差的UWB相结合，可以互相弥补各自的不足。

由于视觉、IMU等传感器其无法获得全局信息，因此被定义为local sensors。而对于UWB、磁力计、UWB等能够感知全局信息的传感器，被定义为global sensors。对于localsensors，首先进行局部位姿估计，即传统意义上的VO或者VIO。得到局部的位姿估计之后，再与UWB等全局传感器进行对齐。对齐的方式是建立一个位姿图，每个UWB时刻建立一个位姿节点，连续的两个节点之间将局部位姿估计得到的相对位姿作为约束。每个节点还与UWB等全局位置建立约束。

由于视觉、IMU等传感器其无法获得全局信息，因此被定义为local sensors。而对于GNSS、磁力计、UWB等能够感知全局信息的传感器，被定义为global sensors。对于localsensors，首先进行局部位姿估计，即传统意义上的VO或者VIO。得到局部的位姿估计之后，再与GNSS等全局传感器进行对齐。对齐的方式是建立一个位姿图，每个GNSS时刻建立一个位姿节点，连续的两个节点之间将局部位姿估计得到的相对位姿作为约束。每个节点还与GNSS等全局位置建立约束。

由于在室内无法接收到GNSS信号，因此在室内就无法利用GNSS消除VIO的累积误差。室内结构复杂，UWB室内非视距的影响往往能达到几十米甚至几百米。

多传感器融合算法一般包括基于滤波和基于非线性的融合方式。基于滤波的算法有卡尔曼滤波(包括一些延伸的算法)、无迹卡尔曼滤波(UKF)及粒子滤波(PF)等，基于非线性优化的算法有最速下降法、高斯-牛顿及L-M算法等。很多文献表明，由于基于非线性优化的算法可以同时优化多个时间段的数据，因此基于非线性优化的算法要优于基于滤波的算法。

多传感器融合算法一般包括基于滤波和基于非线性的融合方式。基于滤波的算法有卡尔曼滤波(包括一些延伸的算法)、无迹卡尔曼滤波(UKF)及粒子滤波(PF)等，基于非线性优化的算法有最速下降法、高斯-牛顿及L-M算法等。很多文献表明，由于基于非线性优化的算法可以同时优化多个时间段的数据，因此基于非线性优化的算法要优于基于滤波的算法。本次主要研究基于非线性优化的融合算法。最速下降法无法确定收敛步长，步长较小会导致收敛较慢，步长较大会导致算法发散。高斯牛顿法采用二阶泰勒展开来近似，只有在展开点附近才会有比较好的近似效果，如果Hessian矩阵是非奇异矩阵，可能会导致算法发散。LM能够很好地克服上面的缺点，已经广泛应用于很多非线性优化实例中，但是LM还有自身的缺陷。尽管LM能够克服Hessian矩阵病态解的影响，由于LM算法采用最小二乘估计作为收敛条件从而不能抵御传感器数据中心粗差的干扰，特别是室内UWB定位时，由于室内非视觉现象比较严重，很难保证UWB定位的稳定性和可靠性。若直接使用LM算法进行优化，则会使得求解出的融合解偏离真实值。

发明内容

基于上述问题，本发明提供一种基于抗差LM视觉惯性里程计与UWB混合高精度室内定位方法：其应用抗差LM算法进行VIO和UWB融合的问题，抵消观测值粗差对模型解的影响，有效提高室内定位的精度。

包括以下步骤：

S1：首先在室内空旷区域部署一定数量的UWB基站；

S2：将UWB和相机设备安装组合，以UWB时间为基准，相机内安装有IMU，且已校准好外参和内参；

S3：UWB外参标定，利用全站仪标定UWB移动站和相机的安装外参；

S4：把组合好的设备安装在车上，对待测区域进行定位，所述定位步骤包括；

S4-1：根据视觉输出的频率以及相应的时间戳对UWB输出数据，利用三次样条曲线进行内插；

S4-2：依据VIO的两帧之间相对的位姿建立VIO函数方程，依据位置和速度信息建立UWB约束方程；然后利用扩展卡尔曼滤波初始化全局坐标，求得视觉世界坐标系到全局坐标系的旋转和平移矩阵p

进一步地，所述步骤S4-1包括已知的n个坐标点，x(t

iv.S

v.S(t

vi.S(t

则称S(t

根据公式(1)，计算参数a

S

根据二阶微分连续性，可得S′

进一步地，所述步骤S4-2包括，首先把融合后的地图坐标系第一帧与UWB坐标系对齐，利用坐标向量和速度向量确定融合约束方程为

其中r

χ＝[p

其中p

进一步地，所述步骤S4-2中视觉约束包括：

依据VIO的两帧之间相对的位姿建立方程(4)-(7)

视觉目标函数可写为

根据经验值选取权值初值

进一步地，所述步骤S4-2包括：依据位置和速度信息建立UWB约束方程(8)

S代表UWB坐标系标志；

进一步地，所述步骤S4-2包括：基于扩展卡尔曼滤波初始化全局坐标，建立方程(9)-(16)并计算视觉世界坐标系到全局坐标系的旋转和平移矩阵p

状态预测方程为：

状态约束方程为：

其中，ε和ζ分别为视觉惯性里程计和UWB的观测噪声。

当i＝1时，

p

q

V

设i是当前帧，根据p

依据旋转和平移矩阵p

进一步地，所述步骤S4-2每次优化完成后重新求取p

进一步地，所述步骤S4-2之后还包括有效性核验步骤S4-3

已知第i+1帧相对视觉世界坐标系下的平移量p

p′

UWB在i+1时刻的坐标为p

|p′

进一步地，，所述步骤S4之后还包括步骤S5,所述步骤S5利用抗差LM计算平移、旋转和速度矩阵，包括

已知LM(Levenberg-Marquard)非线性优化模型为公式(19)：

(H

采用拉格朗日乘子将上述问题转化为一个无约束问题

实际模型和近似模型变化量的比值：

ρ＝(f(x+Δx)-f(x))/(J(x)Δx), (21)

采用抗差估计来减弱粗差对估值的影响，得到LM算法的抗差解；把公式(7)和(8)中的权P换为等价权

采用IGGⅢ等价权函数计算

其中

利用抗差LM求出目标函数的等价权后，利用LM非线性优化算法解算出平移矩阵p

采用本发明的技术方案后，具有以下技术效果，

本申请首次提出改进的LM算法，并把改进的LM算法用于视觉VIO和UWB融合模型优化中，首先分别测试了视觉VIO和UWB定位的精度；有效提高室内定位的精度。通过实例数据分析，基线1(如图2(a)、(b)所示)中利用本文改进的抗差LM算法融合后相对于UWB和视觉VIO定位精度分别提高了29.0％和54.9％；基线2(如图3(a)、(b)所示)中利用本文改进的抗差LM算法融合后相对于UWB和视觉VIO定位精度分别提高了22.6％和243.6％。

通过测试UWB在短暂遮挡下组合系统定位性能，通过实验可以得出，即使UWB在遮挡情况下，组合系统定能定位性能基本不受影响，基线1和基线2的平均定位残差都在0.3m以内，通过在室内环境下搭建的高精度动态捕捉设备对组合定位方法进行实验验证，表明利用该方法不依赖回环即可消除视觉惯性里程计的累积误差，有效提高室内定位精度及可靠性，能保持定位的可靠性和连续性。

附图说明

图1是本次研究的多传感器融合流程图。

图2(a)是基线一抗差LM融合后定位轨迹。

图2(b)是基线一抗差LM融合后定位残差。

图3(a)是基线二抗差LM融合后定位轨迹。

图3(b)是基线二抗差LM融合后定位残差。

具体实施方式

下面将结合本发明实施例，对本发明实施例中的技术方案进行清楚，完整地描述，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。

实施例一：

S1、首先在室内区域部署一定数量的UWB基站(至少4个)，部署在室内空旷区域(防止非视距发生)；

S2、UWB和相机设备安装，为减少杠臂误差，把UWB移动站尽量与相机进行组合，二者都是以UWB时间为基准；相机选用小觅相机标准版(里面安装有IMU，且已校准好外参和内参)；

S3、UWB外参标定，利用全站仪标定UWB移动站和相机的安装外参。

S4、把组合好的设备安装在手推车上，在中电54所C7实验场地进行测试。

S4-1数据内插

多传感器的融合时间戳对齐是非常关键的一步。时间戳无法对齐或对齐错误最后可能得出一个错误的轨迹，很可能使优化算法失效。为了保证融合算法的鲁棒性，视觉传感器的时间戳与UWB的时间戳相差不超过10ms。

一般UWB数据的输出频率为1-10Hz，而视觉VIO数据一般可以达到10-20Hz，为了更方便进行后端融合，本文首先根据视觉输出的频率及相应的时间戳对UWB输出数据进行内插。由于两个UWB输出数据间隔一般只有3-4个视觉输出数据，考虑到内插平滑的精度，本文利用三次样条曲线内插，假设已知的n个坐标点(S(t

vii.S(t

viii.S(t

ix.S(t

则称S(t

S

其中a

S4-2多传感器约束方程的建立

为了实时进行UWB和VIO的融合，本文选择滑窗融合模式，窗口长度为10。UWB坐标系采用室内局部坐标系。为防止秩亏现象发生，首先把融合后的地图坐标系第一帧与UWB坐标系对齐。

由于UWB不能求出旋转向量，因此在优化的时候只能利用坐标向量和速度向量。总的融合约束方程可以写为：

其中r

χ＝[p

其中p

由于p

S4-2-1视觉约束

由于VIO的尺度漂移是不稳定的，因此直接把VIO的pose放在全局坐标系下建立观测方程去估计尺度是不够准确的，因此我们把VIO的两帧之间相对的位姿去建立观测方程。

视觉目标函数可写为

根据经验值选取权值初值

S4-2-2UWB约束

S4-2-3基于扩展卡尔曼滤波的全局坐标初始化

p

状态预测方程为：

状态更新方程为：

当i＝1时，

p

q

V

假设i是当前帧，根据p

由于VO随着时间误差会累积，因此为了更加准确地求取视觉世界坐标系到全局坐标系的转换，在每次优化完成后重新求取p

S4-3有效性检核

为增加定位的鲁棒性，减少粗差给融合定位带来影响，本研究使用下列方案检验数据的有效性。

已知第i+1帧相对视觉世界坐标系(视觉初始化时的参考坐标系)下的平移量p

p′

假设UWB在i+1时刻的坐标为p

|p′

S5改进的抗差LM非线性优化的融合策略

已知LM(Levenberg-Marquard)非线性优化模型为

(H

采用拉格朗日乘子将上述问题转化为一个无约束问题

因此，LM算法可以看作给变化量Δx添加一个信赖区域来限制Δx的大小，并认为在信赖区域里面近似是有效的，否则近似不准确。

确定信赖区域一个好的办法是通过比较近似模型和实际模型的差异来确定，如果差异小，我们就增大范围；如果差异太大，就缩小范围。考虑实际模型和近似模型变化量的比值：

ρ＝(f(x+Δx)-f(x))/(J(x)Δx), (21)

上面式子可以通过ρ的值来判断泰勒近似的好坏，其中分子是实际模型的变化量，分母是近似模型的变化量，当ρ接近1的时候表明近似模型较好，如果ρ较小，则实际模型的变化量小于近似模型的变化量，则认为近似模型较差，需要缩小近似范围；反之，当ρ较大时，说明实际模型变化量更大，需要放大近似范围。经典LM算法可以表示如下：

(1)给定初始迭代值x

(2)对于第k次迭代，求解优化问题。

(3)计算ρ。如果ρ＞0，则认为近似可行，令x

(4)判断算法是否收敛。如果不收敛，跳回步骤(2)，否则结束。

当μ

在经典LM算法中，从信赖域法角度分析，μ

(H

diag[H

尽管LM能够克服Hessian矩阵病态解的影响，由于LM算法采用最小二乘估计作为收敛条件从而不能抵御观测数据中心粗差的干扰，特别是室内UWB定位时，由于室内非视距比较严重，即使UWB原始观测数据能够通过section-2.3的有效性检核，仍旧很难保证UWB定位的稳定性和可靠性。若直接使用LM算法进行优化，则会使得求解出的融合解偏离真实值。

为了抵消观测值粗差对模型解的影响，可以采用抗差估计来减弱粗差对估值的影响，得到LM算法的抗差解。把公式(7)和(8)中的权P换为等价权

采用IGGⅢ等价权函数计算

其中

以上所述，仅为本发明较佳的具体实施方式，但本发明的保护范围并不局限于此，任何本领域的技术人员，可根据本发明的技术方案及发明构想同等替换或者改变，都应涵盖在本发明的保护范围之内。