一种基于平方根容积卡尔曼滤波的多机器人协同定位算法

摘要

本发明公开了一种基于平方根容积卡尔曼滤波(Square Root Cubature Kalman Filtering，SR‑CKF)的多机器人协同定位算法，属于机器人协同定位领域。整个算法分为两步：预测和更新。首先建立机器人运动方程和观测方程，利用相对方位作为测量值，进一步得到多机器人协同定位的动态模型。预测阶段包括：计算容积点集；通过状态方程传播容积点；机器人位姿状态预估计和平方根因子预测。更新阶段包括：计算容积点集；计算卡尔曼增益；计算位姿信息和平方根因子；更新状态向量、协方差阵和位姿信息。本发明实例在更新过程中直接传递目标状态均值和协方差矩阵的平方根因子，降低了计算的复杂度，确保了协方差矩阵的对称性和半正定性，改进了数值精度和稳定性。

主权项

一种基于平方根容积卡尔曼滤波(Square Root Cubature Kalman Filtering，SR‑CKF)的多机器人协同定位算法，其特征在于，包括：预测阶段包括：计算容积点集；通过状态方程传播容积点；机器人位姿状态预估计和平方根因子预测；更新阶段包括：计算容积点集；计算卡尔曼增益；计算位姿信息和平方根因子；更新状态向量、协方差阵和位姿信息。