CN112378396A - 基于抗差lm视觉惯性里程计与uwb混合高精度室内定位方法 - Google Patents

Abstract

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

Description

基于抗差LM视觉惯性里程计与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_i,CG和q_i,CG，根据旋转和平移矩阵p_i,CG和q_i,CG实现高精度室内定位。

进一步地，所述步骤S4-1包括已知的n个坐标点，x(t_i)，y(t_i)和z(t_i)为t_i时间戳对应的三维坐标值，S(t_i)是一个分段定义的公式，如果函数S(t_i)满足以下三个条件(下面仅以x(t_i)举例)：

iv.S_i(t_i)＝x(t_i),i＝(1,2,...,n)；

v.S(t_i)在每个区间上[t_i,t_i+1](1,2,...,n-1)，S(t_i)＝S_i(t_i)是一个三次多项式；

vi.S(t_i)在整个区间上[t₁,t_n]有连续的一阶及二阶导数；

则称S(t_i)为过n个点的三次样条函数；

根据公式(1)，计算参数a_i,b_i,c_i,d_i，即可得到每段曲线的具体表达式；

S_i(t)＝a_i+b_i(t-t_i)+c_i(t-t_i)²+d_i(t-t_i)³,i＝1,2,...,n-1 (1)

根据二阶微分连续性，可得S′_i(t_i+1)＝S′_i+1(t_i+1)和S″_i(t_i+1)＝S″_i+1(t_i+1)，其中，t_i为UWB时间戳，S′_i(t)和S"_i(t)分别是S_i(t)的一阶和二阶导数，公式可列出(n-1)个方程，结合边界约束条件S″_i(t_i)＝S″_i(t_i+n)＝0，首先根据UWB连续输出的相邻的4-5组数据求出三次样条方程系数a_i、b_i、c_i和d_i，然后根据视觉对应的时间戳内插出该时间阶段任意时刻的三维坐标值。

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

其中r_C和r_S分别是VIO和UWB测量残差；

和

分别是VIO的平移和旋转测量值；p_i,S是UWB测量值；χ是需要优化的参数；

χ＝[p_1,G,p_2,G,...,p_i,G,q_1,G,q_2,G,...,q_i,G,V_1,G,V_2,G,...,V_i,G,p_i,CG,q_i,CG] (3)

其中p_i,G和q_1,G是优化后的全局平移和旋转参数。

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

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

视觉目标函数可写为

根据经验值选取权值初值

和

C和G代表视觉世界和融合后全局坐标系标志；i是滑窗内数据索引(i＝1,2，…10)；

为相邻两帧间的三维旋转欧拉角变化量；

为相邻两帧间的三维平移变化量；

为相邻两帧间的三维速度变化量；p_i,C、V_i,C和q_i,C是视觉位置、速度和旋转向量；p_i,G、V_i,G和q_i,G为要优化的参数；

和

分别为视觉相邻两帧间的平移、速度和旋转变化量；

和

分别为全局坐标系下相邻两帧间的平移、速度和旋转变化量。

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

S代表UWB坐标系标志；

和

为UWB的定位和测速的精度因子；V_i,S可以根据位置差分求得。

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

状态预测方程为：

状态约束方程为：

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

当i＝1时，

p_1,G＝p_1,S (11)

q_1,G＝[1,0,0,0]^T (12)

V_1,G＝V_1,S (13)

设i是当前帧，根据p_i,C、q_i,C、p_i,G及q_i,G，可以求得视觉世界坐标系到全局坐标系的旋转和平移矩阵p_i,CG和q_i,CG；

依据旋转和平移矩阵p_i,CG和q_i,CG，计算当前位置在世界坐标系下的坐标，从而实现高精度室内定位。

进一步地，所述步骤S4-2每次优化完成后重新求取p_i,CG和q_i,CG。

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

已知第i+1帧相对视觉世界坐标系下的平移量p_i+1,C，又已知第i帧视觉世界坐标系向全局坐标系的旋转矩阵q_i,CG和平移矩阵p_i,CG，我们可以得到第i+1帧全局坐标系下的坐标估值为：

p′_i+1,G＝q_i,CG·p_i+1,C+p_i,CG， (17)

UWB在i+1时刻的坐标为p_i+1,S，方差为σ²，依据检核方程(18)进行有效性核验：

|p′_i+1,G-p_i+1,S|＜4·σ, (18)

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

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

(H_k+μ_kI)Δx_k＝g_k, (19)其中

和

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

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

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

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

再计算υ＝f(x_k)-f(x^*)，其中x^*为求得的最优解；

采用IGGⅢ等价权函数计算

令u＝υ/σ，其中υ为观测值的改正数，σ为改正数的中误差。计算

的模型为：

其中

是

的一个元素；d＝k₁-|u|/k₁-k₀，称为平滑因子，且0≤d≤1；k₀可取值1.0～1.2；k₁可取值2.0～4.0。

利用抗差LM求出目标函数的等价权后，利用LM非线性优化算法解算出平移矩阵p_i,CG、旋转矩阵q_i,CG和速度矩阵V_i,CG。

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

本申请首次提出改进的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_i),t_i)，···，(S(t_i+n),t_i+n)，其中S(t_i)为t_i时间戳对应的三维坐标值x(t_i)，y(t_i)和z(t_i)，我们可以列出样条曲线的微分式。如果函数S(t_i)满足以下三个条件(本文为了方便计算，把x_i,y_i,z_i根据t_i分开内插求解)：

vii.S(t_i)＝x(t_i),i＝(1,2,...,n)；

viii.S(t_i)在每个区间上[t_i,t_i+1](1,2,...,n-1)是一个三次多项式；

ix.S(t_i)在整个区间上[t₁,t_n]有连续的一阶及二阶导数；

则称S(t_i)为过n个点的三次样条函数。三次样条曲线内插是通过一系列形值点的一条光滑曲线，数学上通过求解三弯矩方程组得出曲线函数组的过程。实际计算时还需要引入边界条件才能完成计算。假设方程为：

S_i(t)＝a_i+b_i(t-t_i)+c_i(t-t_i)²+d_i(t-t_i)³,i＝1,2,...,n-1 (1)

其中a_i,b_i,c_i,d_i待定。根据二阶微分连续性，可得S′_i(t_i+1)＝S′_i+1(t_i+1)和S″_i(t_i+1)＝S″_i+1(t_i+1)。其中，t_i为UWB时间戳。S′_i(t)和S"_i(t)分别是S_i(t)的一阶和二阶导数。公式可列出(n-1)个方程，但是有(n+1)个参数，因此求解该方程组，还需添加其它约束。本文选择“自由边界”约束，即：S″_i(t_i)＝S″_i(t_i+n)＝0。首先根据UWB连续输出的相邻的4-5组数据求出三次样条方程系数a_i、b_i、c_i和d_i，关于y(t_i)和z(t_i)的内插同x(t_i)。解算出这些系数后，可根据视觉对应的时间戳内插出该时间阶段任意时刻的三维坐标值。

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

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

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

其中r_C和r_S分别是VIO和UWB测量残差；

和

分别是VIO的平移和旋转测量值；p_i,S是UWB测量值；χ是需要优化的参数。

χ＝[p_1,G,p_2,G,...,p_i,G,q_1,G,q_2,G,...,q_i,G,V_1,G,V_2,G,...,V_i,G,p_i,CG,q_i,CG] (3)

其中p_i,G和q_1,G是优化后的全局平移和旋转参数。

由于p_i,CG、q_i,CG和视觉VIO密切相关，而视觉VIO会随着时间有误差累积，因此p_i,CG和q_i,CG也会有误差累积。为了使优化后的位姿更加平滑，本次研究选择实时优化这两个向量。

S4-2-1视觉约束

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

视觉目标函数可写为

根据经验值选取权值初值

和

i是滑窗内数据索引(i＝1,2，…10)；p_i,C、V_i,C和q_i,C是视觉位置、速度和旋转向量；C、S和G代表视觉世界、UWB和融合后全局坐标系标志；p_i，C、p_i，s、q_i，C和q_i，s为后端输出数据；p_i，G和q_i，G为要优化的参数；

和

分别为视觉相邻两帧间的平移和旋转变化量。

S4-2-2UWB约束

和

为UWB的定位和测速的精度因子；V_i,S可以根据位置差分求得。因为UWB无法求出旋转角度，因此只利用位置和速度信息进行约束。

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

p_k,G和q_k,G为全局坐标系下的平移和旋转矩阵。由于UWB只能提供全局位置约束，因此若想求得当前坐标系相对全局坐标系下的旋转矩阵，至少需要提供四个历元的位置约束。假设滑窗宽度为10，首先利用扩展卡尔曼滤波初始化全局坐标。

状态预测方程为：

状态更新方程为：

当i＝1时，

p_1,G＝p_1,S (11)

q_1,G＝[1,0,0,0]^T (12)

V_1,G＝V_1,S (13)

假设i是当前帧，根据p_i,C、q_i,C、p_i,G及q_i,G，可以求得视觉世界坐标系到全局坐标系的旋转和平移矩阵p_i,CG和q_i,CG。

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

S4-3有效性检核

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

已知第i+1帧相对视觉世界坐标系(视觉初始化时的参考坐标系)下的平移量p_i+1,C，又已知第i帧视觉世界坐标系向全局坐标系的旋转矩阵q_i,CG和平移矩阵p_i,CG，我们可以得到第i+1帧全局坐标系下的坐标估值为：

p′_i+1,G＝q_i,CG·p_i+1,C+p_i,CG， (17)

假设UWB在i+1时刻的坐标为p_i+1,S，方差为σ²，我们可以建立检核方程为：

|p′_i+1,G-p_i+1,S|＜4·σ, (18)

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

已知LM(Levenberg-Marquard)非线性优化模型为^[23][24]：

(H_k+μ_kI)Δx_k＝g_k, (19)其中

和

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

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

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

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

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

(1)给定初始迭代值x₀(坐标向量初值可由UWB给出，旋转向量初值由视觉VIO提供)以及调节因子μ₀：μ₀＝max([h_ii])，其中h_ii是H₀的对角线元素。

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

(3)计算ρ。如果ρ＞0，则认为近似可行，令x_k+1＝x_k+Δx_k.若ρ＞4/5,则μ_k+1＝2μ_k.若ρ＞1/5,则μ_k+1＝0.5μ_k。

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

当μ_k较小时，说明H_k占主导地位，说明二次近似在该范围类是比较好的，LM方法更接近于高斯牛顿法；另一方面，当μ_k较大时，LM算法更接近一阶梯度下降算法，这说明二次近似不够好。LM算法的求解方式，可以避免线性方程组的矩阵非奇异和病态等问题，提供更稳定、更准确的解法。

在经典LM算法中，从信赖域法角度分析，μ_k可以看作是每次迭代约束区域的半径，其实是一个球形的区域，该约束认为只有在球内才是有效的。||H||与||J_f||成正相关，J_f为f的一阶雅克比矩阵，这就默认了各参数分量的约束是等权的。H近似等于Hessian矩阵，为了更加高效地约束Δx_k的变化，由于对公式进行变形如下：

(H_k+μ_k·diag[H_k])Δx_k＝g_k (22)

diag[H_k]指只保留H_k的对角线元素。带上矩阵diag[H_k]后约束区域就是一个椭球，可以约束参数中不同维度的变化。

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

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

再计算υ＝f(x_k)-f(x^*)，其中x^*为求得的最优解。

采用IGGⅢ等价权函数计算

令u＝υ/σ，其中υ为观测值的改正数，σ为改正数的中误差。计算

的模型为：

其中

是

的一个元素；d＝k₁-|u|/k₁-k₀，称为平滑因子，且0≤d≤1；k₀可取值1.0～1.2；k₁可取值2.0～4.0。

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

Claims (9)

1.一种基于抗差LM视觉惯性里程计与UWB混合高精度室内定位方法：其特征在于，包括以下步骤：

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

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

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

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

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

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

2.根据权利要求1所述的基于抗差LM视觉惯性里程计与UWB混合高精度室内定位方法：其特征在于，所述所述步骤S4-1包括

已知的n个坐标点，x(t_i)，y(t_i)和z(t_i)为t_i时间戳对应的三维坐标值，以x(t_i)进行说明，y(t_i)和z(t_i)参照y(t_i)和z(t_i)，S(t_i)是一个分段定义的公式，如果函数S(t_i)满足以下三个条件：

i.S_i(t_i)＝x(t_i),i＝(1,2,...,n)；

ii.S(t_i)在每个区间上[t_i,t_i+1](1,2,...,n-1)，S(t_i)＝S_i(t_i)是一个三次多项式；

iii.S(t_i)在整个区间上[t₁,t_n]有连续的一阶及二阶导数；

则称S(t_i)为过n个点的三次样条函数；

根据公式(1)，计算参数a_i,b_i,c_i,d_i，即可得到每段曲线的具体表达式；

S_i(t)＝a_i+b_i(t-t_i)+c_i(t-t_i)²+d_i(t-t_i)³,i＝1,2,...,n-1 (1)

根据二阶微分连续性，可得S′_i(t_i+1)＝S′_i+1(t_i+1)和S″_i(t_i+1)＝S″_i+1(t_i+1)，其中，t_i为UWB时间戳，S′_i(t)和S″_i(t)分别是S_i(t)的一阶和二阶导数，公式可列出(n-1)个方程，结合边界约束条件S″_i(t_i)＝S″_i(t_i+n)＝0，首先根据UWB连续输出的相邻的4-5组数据求出三次样条方程系数a_i、b_i、c_i和d_i，然后根据视觉对应的时间戳内插出该时间阶段任意时刻的三维坐标值。

3.根据权利要求1所述的基于抗差LM视觉惯性里程计与UWB混合高精度室内定位方法：其特征在于，所述步骤S4-2包括，首先把融合后的地图坐标系第一帧与UWB坐标系对齐，利用坐标向量和速度向量确定融合约束方程为

其中r_C和r_S分别是VIO和UWB测量残差；

和

分别是VIO的平移和旋转测量值；p_i,S是UWB测量值；χ是需要优化的参数；

χ＝[p_1,G,p_2,G,...,p_i,G,q_1,G,q_2,G,...,q_i,G,V_1,G,V_2,G,...,V_i,G,p_i,CG,q_i,CG] (3)

其中p_i,G和q_1,G是优化后的全局平移和旋转参数。

4.根据权利要求3所述的基于抗差LM视觉惯性里程计与UWB混合高精度室内定位方法：其特征在于，所述步骤S4-2中视觉约束包括：

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

视觉目标函数可写为

根据经验值选取权值初值

和

C和G代表视觉世界和融合后全局坐标系标志；i是滑窗内数据索引(i＝1,2，…10)；

为相邻两帧间的三维旋转欧拉角变化量；

为相邻两帧间的三维平移变化量；

为相邻两帧间的三维速度变化量；p_i,C、V_i,C和q_i,C是视觉位置、速度和旋转向量；p_i,G、V_i,G和q_i,G为要优化的参数；

和

分别为视觉相邻两帧间的平移、速度和旋转变化量；

和

分别为全局坐标系下相邻两帧间的平移、速度和旋转变化量。

5.根据权利要求4所述的基于抗差LM视觉惯性里程计与UWB混合高精度室内定位方法：其特征在于，所述步骤S4-2包括：依据位置和速度信息建立UWB约束方程(8)

S代表UWB坐标系标志；

和

为UWB的定位和测速的精度因子；V_i,S可以根据位置差分求得。

6.根据权利要求3所述的基于抗差LM视觉惯性里程计与UWB混合高精度室内定位方法：其特征在于，所述步骤S4-2包括：基于扩展卡尔曼滤波初始化全局坐标，建立方程(9)-(16)并计算视觉世界坐标系到全局坐标系的旋转和平移矩阵p_i,CG和q_i,CG，

状态预测方程为：

状态约束方程为：

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

当i＝1时，

p_1,G＝p_1,S (11)

q_1,G＝[1,0,0,0]^T (12)

V_1,G＝V_1,S (13)

设i是当前帧，根据p_i,C、q_i,C、p_i,G及q_i,G，可以求得视觉世界坐标系到全局坐标系的旋转和平移矩阵p_i,CG和q_i,CG；

依据旋转和平移矩阵p_i,CG和q_i,CG，计算当前位置在世界坐标系下的坐标，从而实现高精度室内定位。

7.根据权利要求6所述的基于抗差LM视觉惯性里程计与UWB混合高精度室内定位方法：其特征在于，所述步骤S4-2每次优化完成后重新求取p_i,CG和q_i,CG。

8.根据权利要求1所述的基于抗差LM视觉惯性里程计与UWB混合高精度室内定位方法：其特征在于，所述步骤S4-2之后还包括有效性核验步骤S4-3

已知第i+1帧相对视觉世界坐标系下的平移量p_i+1,C，又已知第i帧视觉世界坐标系向全局坐标系的旋转矩阵q_i,CG和平移矩阵p_i,CG，我们可以得到第i+1帧全局坐标系下的坐标估值为：

p′_i+1,G＝q_i,CG·p_i+1,C+p_i,CG， (17)

UWB在i+1时刻的坐标为p_i+1,S，方差为σ²，依据检核方程(18)进行有效性核验：

|p′_i+1,G-p_i+1,S|＜4·σ, (18)。

9.根据权利要求1所述的基于抗差LM视觉惯性里程计与UWB混合高精度室内定位方法：其特征在于，所述步骤S4之后还包括步骤S5,所述步骤S5利用抗差LM计算平移、旋转和速度矩阵，包括

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

(H_k+μ_kI)Δx_k＝g_k, (19)

其中

和

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

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

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

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

再计算υ＝f(x_k)-f(x^*)，其中x^*为求得的最优解；

采用IGGⅢ等价权函数计算

令u＝υ/σ，其中υ为观测值的改正数，σ为改正数的中误差。计算

的模型为：

其中

是

的一个元素；d＝k₁-|u|/k₁-k₀，称为平滑因子，且0≤d≤1；k₀可取值1.0～1.2；k₁可取值2.0～4.0。

利用抗差LM求出目标函数的等价权后，利用LM非线性优化算法解算出平移矩阵p_i,CG、旋转矩阵q_i,CG和速度矩阵V_i,CG。

Images