一种四元数LI-EKF的机器人姿态估计方法

本发明属于移动机器人领域，具体涉及一种四元数LI‑EKF的机器人姿态估计方法，包括：获取测量数据；构建姿态估计模型，定义乘法误差；将测量数据输入到姿态估计模型中，得到离散时间状态更新方程；根据离散时间状态更新方程计算雅可比矩阵；采用EM算法估计雅可比矩阵的协方差矩阵；根据协方差矩阵采用RTS平滑器和滞后一阶协方差平滑器获取平滑后的状态和协方差；建对数似然函数，求对数似然函数的最小值，并更新噪声协方差矩阵；采用LI‑EKF对当前时刻状态对机器人姿态进行估计；本发明采用四元数和左不变扩展卡尔曼滤波器对数据进行出来，从而更加精确对机器人的姿态。