室内惯性导航系统/相机拓扑测量的因子图合作定位算法

Inertial Navigation System/Topology Measurement Integrated Algorithm with Factor Graph for Indoor Cooperative Localization

为解决合作定位算法中滤波结构不易扩展、鲁棒性差的问题,提出了一种适应于室内多用户的惯性导航系统/相机拓扑测量的因子图合作定位算法。利用目标检测识别算法,提出相机拓扑测量合作定位算法。通过构建拓扑测量、惯性导航系统因子函数,推导出基于因子图的可扩展参数优化模型。为进一步提高鲁棒性,引入综合考虑残差和检测分数的权值判断法,提出适应于相机拓扑测量的改进型开关约束算法。仿真和实测实验表明:拓扑测量观测精度的提升对位置和速度估计、算法收敛次数均有不同程度的改善;合作+改进型开关约束算法的定位精度较非合作+非鲁棒算法的提高了79.8%;改进型开关约束算法相比原开关约束算法具有较高的预测成功率,当测量遮挡时长比为0.4时,改进型开关约束算法将原开关约束算法对野值点的预测成功率由89.35%提高到了97.4%;与原开关约束算法相比,引入权值判断法的改进型开关约束算法剔除了同用户多边框的异常拓扑测量值,减小了计算开销,提高了合作定位精度和鲁棒性。

To address the deficiencies of standard filters and low robustness in cooperative localization system,a novel factor graph algorithm is proposed,which can be applied to indoor multiple users by fusing topology measurement provided by cameras with inertial navigation system.A topology measuring cooperative localization method is further presented utilizing the objects detection algorithm from the images.To derive a flexible optimization model of the navigation solution with incorporating asynchronous sensors capabilities,the topology measuring factors and inertial navigation system factors are created in solving nonlinear optimization problems.To further enhance the robustness,an improved switch constraint algorithm is developed by introducing weights decision approach considering both residuals and detection scores,and it better suits to the topology measurements.Simulations and experiments show that the rising accuracy of topology measurements improves position and velocity estimations and reduces iteration times of the navigation solutions.The positioning accuracy of non-cooperative+non-robust approach is 79.8% higher than that of cooperative+improved switching constraint approach.The improved switch constraint algorithm has a higher prediction success rate than that of the original switch constraint algorithm,and when the ratio of measuring occlusion duration is 0.4,the former improves the prediction success rate of the latter for outliers from89.35%to 97.4%.Compared with the switch constraint algorithm,the improved algorithm by introducing weights decision approach removes abnormal topology measurements of the same user with multiple detected rectangles,improves cooperative positioning accuracy and robustness time consumption in computation.