【技术实现步骤摘要】
本专利技术属于多源传感器融合导航，具体涉及一种传感器失效场景视觉惯性超宽带鲁棒融合导航方法及系统。
技术介绍
1、随着多源传感器融合导航理论和技术的不断发展完善，多源传感器融合导航技术的定位精度不断提高。现如今，国内外领先的多源传感器融合导航定位精度能达到分米级甚至厘米级，其精度能够满足多数实际中的应用需求。然而，多源传感器融合导航技术在实际中的应用还相对较少，原因在于系统的鲁棒性不强，由于环境等因素造成某个传感器短暂失效时，系统的定位精度可能骤降甚至无法工作，不利于系统的稳定持续导航，传感器失效问题对于自动驾驶，无人配送等实际应用场景是致命问题，这严重阻碍了多源传感器融合导航方法的技术落地和应用前景。因此，研究传感器失效场景下的多源传感器鲁棒融合导航技术，如何提高传感器失效场景下的鲁棒性是亟待解决的关键问题。
2、近年来，学者针对传感器失效和场景退化的多源传感器融合导航方法展开了研究。cao等提出了视觉/惯性/gnss的融合导航方法并针对gnss信号差的区域，考虑并处理了场景退化的情况以提高系统的鲁棒性；yin在视觉/惯性/gnss/轮速计的融合方法上考虑了视觉失效和gnss退化场景下的融合导航策略；徐爱功对室内退化场景下的uwb/lidar融合定位方法展开了研究；杨明基于位姿约束对激光slam的退化检测展开了研究；杨博针对黑暗场景下的视觉失效问题，开展了基于点线特征的视觉/惯性导航技术研究。总体来讲，目前国内外研究主要集中在退化场景下的多源传感器融合导航算法，对于传感器失效甚至无法工作的研究不够成熟。尤其是针对视
技术实现思路
1、本专利技术的目的在于解决传感器失效场景下视觉/惯性/超宽带鲁棒融合导航问题，提出一种传感器失效场景视觉惯性超宽带鲁棒融合导航方法及系统，能够有效提高传感器失效场景下视觉/惯性/超宽带融合导航系统的鲁棒性，保证传感器失效场景下系统仍能持续稳定的导航。
2、为了实现上述目的，本专利技术采用以下的技术方案：
3、一种传感器失效场景视觉惯性超宽带鲁棒融合导航方法，包含：
4、利用开源vir-slam算法进行多源传感器融合导航系统初始化，之后开始进行传感器失效检测；
5、如果多源传感器融合导航系统良好，则进行后端非线性优化；如果多源传感器融合导航系统失效，针对每种传感器失效的情况提出不同的鲁棒融合导航方案；
6、针对视觉传感器、惯性传感器和uwb传感器失效的情况，分别使用基于当前统计模型的卡尔曼滤波算法、基于线性插值的imu观测值预测方法和基于多项式拟合的uwb测距值预测方法预测观测值并构建残差项，并将构建的残差项重新融入到系统进行后端非线性优化。
7、根据本专利技术传感器失效场景视觉惯性超宽带鲁棒融合导航方法，进一步地，如果多源传感器融合导航系统良好，则进行后端非线性优化，非线性优化的约束函数为：
8、
9、其中，rp-hpχ表示边缘化的先验信息残差，表示相机的测量残差，表示imu的测量残差，表示uwb的测量残差；待优化变量为：
10、χ＝[s0,s1,…sn,x0,x1,…xh]
11、其中，si表示特征点逆深度，i∈[0,n]，xk的表达式如下：
12、
13、其中，h表示滑动窗口的长度，表示第k个关键帧时刻载体坐标系相对于世界坐标系的位置、姿态和速度，和表示加速度计和陀螺仪的零偏。
14、根据本专利技术传感器失效场景视觉惯性超宽带鲁棒融合导航方法，进一步地，传感器失效检测方法如下：
15、假设视觉传感器、惯性传感器和uwb传感器的最新采样时刻分别为tnew_cam，tnew_imu和tnew_uwb，判断三种传感器是否失效的最大时间间隔为tthre_cam，tthre_imu和tthre_uwb，当[tnew_cam,tnew_cam+tthre_cam]时间段没有新的视觉传感器数据进入，则认为视觉传感器失效；当[tnew_imu,tnew_imu+tthre_imu]时间段没有新的惯性传感器数据进入，则认为惯性传感器失效；当[tnew_uwb,tnew_uwb+tthre_uwb]时间段没有新的uwb传感器数据进入，则认为uwb传感器失效；其中tthre_cam，tthre_imu和tthre_uwb的计算公式如下：
16、
17、式中，tcam，timu，tuwb分别表示视觉传感器、惯性传感器和uwb传感器的采样频率，τcam,τimu,τuwb为常数。
18、根据本专利技术传感器失效场景视觉惯性超宽带鲁棒融合导航方法，进一步地，所述使用基于当前统计模型的卡尔曼滤波算法预测观测值包含：
19、视觉传感器失效前根据融合算法得到的多源传感器融合导航系统三维位置为已知量，假设k-1,k,k+1时刻系统的定位结果分别是pk-1,pk,pk+1，那么得到系统在k时刻的速度和加速度：
20、
21、根据当前统计模型原理，离散时刻系统的状态方程为：
22、
23、其中，
24、xk＝[pk vk ak]t
25、
26、
27、式中，表示加速度均值，vk表示零均值的高斯白噪声，xk表示k时刻系统的位置、速度和加速度，α表示系统的机动频率；过程噪声矩阵的计算公式为：
28、
29、式中，表示加速度的方差，q表示3×3的矩阵；
30、最后，基于当前统计模型的卡尔曼滤波算法，预测下一时刻系统的位置坐标，作为观测值。
31、根据本专利技术传感器失效场景视觉惯性超宽带鲁棒融合导航方法，进一步地，由于视觉传感器失效后没有图像特征信息，因此待优化变量不再包含特征点的逆深度，因此公式(1)非线性优化的约束函数改变为：
32、
33、其中表示当前统计模型预测残差项；待优化变量为：
34、
35、此时，使用当前统计模型预测残差项替代视觉测量残差项，系统其他残差模块不变，执行后端非线性优化模块。
36、根据本专利技术传感器失效场景视觉惯性超宽带鲁棒融合导航方法，进一步地，所述使用基于线性插值的imu观测值预测方法预测观测值并构建残差项，并将构建的残差项重新融入到系统进行后端非线性优化，包含：
37、假设惯性传感器失效前惯性传感器的最新帧和次新帧的采样时刻分别为timu_l和timu_n，对应的陀螺仪和加速度计的观测值分别为(ωimu_l,aimu_l)和(ωimu_n,aimu_n)；则根据线性插值原理，传感器失效后的第timu_j时刻的陀螺仪和加速度计的预测值分别为：
38、
39、将陀螺仪和加速度计的预测值作为imu观测值，根据imu预积分原理得到：
40、
41、其中，分别表示旋转预积分本文档来自技高网...
【技术保护点】
1.一种传感器失效场景视觉惯性超宽带鲁棒融合导航方法，其特征在于，包含：
2.根据权利要求1所述的传感器失效场景视觉惯性超宽带鲁棒融合导航方法，其特征在于，如果多源传感器融合导航系统良好，则进行后端非线性优化，非线性优化的约束函数为：
3.根据权利要求2所述的传感器失效场景视觉惯性超宽带鲁棒融合导航方法，其特征在于，传感器失效检测方法如下：
4.根据权利要求3所述的传感器失效场景视觉惯性超宽带鲁棒融合导航方法，其特征在于，所述使用基于当前统计模型的卡尔曼滤波算法预测观测值包含：
5.根据权利要求4所述的传感器失效场景视觉惯性超宽带鲁棒融合导航方法，其特征在于，由于视觉传感器失效后没有图像特征信息，因此待优化变量不再包含特征点的逆深度，因此公式(1)非线性优化的约束函数改变为：
6.根据权利要求2所述的传感器失效场景视觉惯性超宽带鲁棒融合导航方法，其特征在于，所述使用基于线性插值的IMU观测值预测方法预测观测值并构建残差项，并将构建的残差项重新融入到系统进行后端非线性优化，包含：
7.根据权利要求2所述的传感器
8.一种传感器失效场景视觉惯性超宽带鲁棒融合导航系统，其特征在于，用于实现如权利要求1-7任一项所述的传感器失效场景视觉惯性超宽带鲁棒融合导航方法，包含：
...【技术特征摘要】
1.一种传感器失效场景视觉惯性超宽带鲁棒融合导航方法，其特征在于，包含：
2.根据权利要求1所述的传感器失效场景视觉惯性超宽带鲁棒融合导航方法，其特征在于，如果多源传感器融合导航系统良好，则进行后端非线性优化，非线性优化的约束函数为：
3.根据权利要求2所述的传感器失效场景视觉惯性超宽带鲁棒融合导航方法，其特征在于，传感器失效检测方法如下：
4.根据权利要求3所述的传感器失效场景视觉惯性超宽带鲁棒融合导航方法，其特征在于，所述使用基于当前统计模型的卡尔曼滤波算法预测观测值包含：
5.根据权利要求4所述的传感器失效场景视觉惯性超宽带鲁棒融合导航方法，其特征在于，由于视觉传感器失效后没有图像特征信息，因此待优化变量不再...
【专利技术属性】
技术研发人员：罗豪龙，李广云，李建胜，王力，王安成，李雪强，杨子迪，郭雨岩，魏亚旭，
申请(专利权)人：中国人民解放军战略支援部队信息工程大学，
类型：发明
国别省市：
还没有人留言评论。发表了对其他浏览者有用的留言会获得科技券。