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

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

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