一种基于配准优化的分布式协同定位方法及介质

本发明涉及一种基于配准优化的分布式协同定位方法及介质，该方法首先利用四元数姿态滤波算法和零速修正算法对各节点的姿态和速度进行精确解算，生成初步位置估计值。随后，通过共享各节点的初步位置估计和测距数据，构建以测距误差平方和最小化为目标的优化函数，并采用顺序最小二乘规划算法高效求解该非线性优化问题，从而获得各节点的修正平移量和旋转角度。与传统依赖于扩展卡尔曼滤波器、无迹卡尔曼滤波器及粒子滤波算法的方法相比，本发明的方法无需对系统进行精确建模，显著降低了对计算资源的需求，同时增强了对异常值和系统非线性特性的鲁棒性。