一种基于间接卡尔曼滤波的移动平台位姿估计方法

摘要

本发明公开了一种基于间接卡尔曼滤波的移动平台位姿估计方法，该方法包括以下步骤：将所述捷联惯导系统的推算值X_I和所述全球定位系统的观测数据为X_N二者的差值作为所述间接卡尔曼滤波器的观测输入，间接卡尔曼滤波器内部构造预测方程，滤波器输出为误差的估计值此输出中的部分参数反馈用于校正所述捷联惯导系统以及全球定位系统观测计算中的参数，以修正二者的误差计算，X_I与中坐标与姿态等参数的和作为最终系统各项数值的估计值。同传统的采用直接卡尔曼滤波算法的方法相比,本发明不但能够给出更为准确的位姿估计结果,且有效提高机器人的工作效率。

主权项

一种基于间接卡尔曼滤波的移动平台位姿估计方法，其特征在于，该方法包括以下步骤：步骤1：构建基于间接卡尔曼滤波的移动平台，该移动平台包括捷联惯导系统、全球定位系统和间接卡尔曼滤波器；步骤2：根据捷联惯导系统中的陀螺仪的角速度测量值[ω_x ω_y ω_z]，通过递推计算旋转矩阵并根据捷联惯导系统中的里程计的测量值计算速度测量值在导航系统中的投影步骤3：采用间接卡尔曼滤波器构建系统的状态预测方程和观测方程，并利用状态预测方程计算状态的预测值；步骤4：根据捷联惯导系统中的陀螺仪和里程计中计算得到的数据推算x_m、y_m、z_m，得到捷联惯导系统的推算值X_I；步骤5：将全球定位系统中的观测数据X_N和步骤4中的捷联惯导系统中的推算值X_I做差运算，其差值作为间接卡尔曼滤波器输入的观测值，并计算增益矩阵得到误差的估计值步骤6：将误差估计值作为反馈，用于修正捷联惯导系统的递推值和全球定位系统的观测数据，并将捷联惯导系统的推算值X_I和误差估计值结合得到导航系统所需的移动平台的位置和姿态；步骤7：返回步骤1，循环计算，直到达到导航系统设定的停止条件为止。