基于分布式协同导航的行人三维惯性SLAM方法

针对仅依赖惯性传感器数据进行零速修正的行人航位推算方法导致室内行人协同导航定位累计误差较大的问题，提出了一种三维环境下的分布式行人协同惯性SLAM方法。在行人间初始相对位姿未知的情况下，通过蓝牙检测相遇，使用基于奇异值分解的最小二乘方法求解行人间位姿的刚性变换；然后构建基于三维占用栅格的惯性SLAM地图，通过优化联合后验概率分布获得行人位姿与地图的最优估计，减小系统的累计误差；同时构建了一个分布式协同SLAM框架，使得每个行人的定位系统都能够独立运行。实验结果表明，所提出的算法能有效提升三维惯性行人协同导航系统的定位精度，在大约2000?m2、总长2000?m的多楼层双人协同实验中，算法的平均定位误差为1.22?m，与传统零速修正方法相比，定位精度提升80%以上。