[发明专利]一种基于间接卡尔曼滤波的移动平台位姿估计方法

摘要

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

主权项

1.一种基于间接卡尔曼滤波的移动平台位姿估计方法，其特征在于，该方法包括以下步骤：步骤1：构建基于间接卡尔曼滤波的移动平台，该移动平台包括捷联惯导系统、全球定位系统和间接卡尔曼滤波器；步骤2：根据捷联惯导系统中的陀螺仪的角速度测量值[ω_x ω_y ω_z]，通过递推计算旋转矩阵并根据捷联惯导系统中的里程计的测量值计算速度测量值在导航系统中的投影步骤2具体包括以下步骤：根据捷联惯导系统中的陀螺仪的角速度测量值[ω_x ω_y ω_z]，通过四元数递推计算得出旋转矩阵递推计算公式如下：初始旋转矩阵公式1中，T是数据周期，即k‑1时刻到k时刻之间的时间间隔；指角速度矢量的模，即矢量大小；进一步通过旋转矩阵计算出惯导递推的计算公式如下：公式2中，表示根据陀螺仪角速度计算的载体在导航坐标系中的三轴方位角度，因导航坐标系选取的是北东地坐标系，所以分别表示载体在北东地坐标系下的横滚角、俯仰角、航向角；R₁₁代表旋转矩阵中的第1行第1列的数据，R₁₂代表旋转矩阵中的第1行第2列的数据，R₁₃代表旋转矩阵中的第1行第3列的数据，R₂₃代表旋转矩阵中的第2行第3列的数据，R₃₃代表旋转矩阵中的第3行第3列的数据；并根据捷联惯导系统中的里程计测量值计算速度测量值在导航系中的投影计算公式如下：公式3中，分别为里程计测量值计算速度测量值在导航系北、东和地三个方向上的速度投影；步骤3：采用间接卡尔曼滤波器构建系统的状态预测方程和观测方程，并利用状态预测方程计算状态的预测值；步骤4：根据捷联惯导系统中的陀螺仪和里程计中计算得到的数据推算xm、ym、zm，得到捷联惯导系统的推算值XI；步骤5：将全球定位系统中的观测数据X_N和步骤4中的捷联惯导系统中的推算值X_I做差运算，其差值作为间接卡尔曼滤波器输入的观测值，并计算增益矩阵得到误差的估计值步骤6：将误差估计值作为反馈，用于修正捷联惯导系统的递推值和全球定位系统的观测数据，并将捷联惯导系统的推算值X_I和误差估计值结合得到导航系统所需的移动平台的位置和姿态；步骤7：返回步骤1，循环计算，直到达到导航系统设定的停止条件为止。