[发明专利]一种改进误差四元数卡尔曼滤波机器人姿态解算方法

说明书

本发明是一种改进误差四元数卡尔曼滤波机器人姿态解算方法，主要作用于移动机器人姿态解算系统中，使之获得准确的姿态角便于进行后续姿态控制。该方法采用陀螺仪加速度计获得姿态数据，构建系统误差四元数卡尔曼滤波状态方程及量测方程，并根据非重力加速度判定公式判断是否存在非重力加速度，并自适应补偿非重力加速度对姿态解算带来的影响，然后利用误差四元数卡尔曼滤波器，获得误差四元数更新值，计算得到真实的姿态四元数，根据四元数法进行姿态解算得到姿态角。本发明能够实现全姿态解算，而且能明显抑制噪声、漂移误差和非重力加速度对姿态解算的影响。

技术领域

本发明是一种改进误差四元数卡尔曼滤波机器人姿态解算方法，本发明属于数字滤波和多传感器数据融合技术领域。该发明适用于移动机器人的姿态解算，同时也适用于移动电动车。

背景技术

姿态解算的精度和速度将直接影响姿态控制算法的稳定性、可靠性和实现的难易程度，所以，姿态解算是移动或飞行控制实现的前提。移动机器人一般采用的低成本姿态传感器，由于传感器本身特性，加速度计噪声的影响很大，陀螺仪信号具有偏置误差，当惯性传感器长时间工作时，误差将随着时间累积导致系统无法正常工作。目前移动机器人姿态解算方法主要有卡尔曼滤波法、扩展卡尔曼滤波法以及粒子滤波法，扩展卡尔曼滤波法姿态解算方法实时性较好，但是线性化损失了部分二次项值，线性化误差较大。卡尔曼滤波法姿态解算方法建模简单，实时性较好，但是忽略了非线性因素，粒子滤波法存在粒子退化问题，实时性较差，且不够简便。本发明专利是针对上述问题的一种有效的移动机器人姿态解算方法。

发明内容

本发明主要解决的技术问题是针对上述背景技术的不足，提供了一种改进误差四元数卡尔曼滤波机器人姿态解算方法。本发明为实现上述发明目的采用如下技术方案。

一种改进误差四元数卡尔曼滤波机器人姿态解算方法，包括如下步骤：

步骤1：选取移动机器人相应的参考坐标系，确定姿态变换矩阵，用非线性指数模型，结合Levenberg-Marquardt迭代最小平方拟合方法建立陀螺仪漂移数学模型，利用陀螺仪输出数据和误差四元数间的关系建立误差四元数微分方程；

步骤2：根据误差四元数微分方程和陀螺仪漂移数学模型建立误差四元数卡尔曼滤波状态方程；

步骤3：根据加速度计输出数据与误差四元数之间的关系建立误差四元数卡尔曼滤波观测方程；

步骤4：根据加速度计输出数据判断移动机器人是否存在非重力加速度，并自适应更新移动机器人非重力加速度的协方差矩阵；

步骤5：利用自适应更新的非重力加速度协方差矩阵对量测噪声矩阵进行补偿，并利用误差四元数卡尔曼滤波器获得误差四元数更新值，计算得到真实的姿态四元数，根据四元数法进行姿态解算得到姿态角。

进一步地，步骤1中，选取东北天坐标系作为导航坐标系，导航系与载体系之间的变换关系可用如下姿态变换矩阵表示：

其中，θ为俯仰角，γ为横滚角，ψ为航向角。

陀螺仪输出信号与运动条件无关，但是陀螺仪信号具有偏置误差。因此陀螺仪测量模型为角速度、偏置误差、白噪声的总和：

ω_o＝ω+ε_g+n_g

其中，ω_o表示陀螺仪的输出，ω表示实际角速度，ε_g表示陀螺仪的漂移，n_g表示陀螺仪零均值白噪声。

用非线性指数模型，结合Levenberg-Marquardt迭代最小平方拟合方法建立陀螺仪漂移数学模型；