[发明专利]一种四元数高斯粒子滤波移动机器人姿态解算方法

说明书

本发明提出的一种四元数高斯粒子滤波移动机器人姿态解算方法，属于数字滤波和多传感器数据融合技术领域，主要作用于移动机器人姿态解算系统中，使之获得准确的姿态角便于进行后续姿态控制。该方法采用陀螺仪加速度计获得姿态数据，构建系统四元数状态方程及非线性量测方程，并采用高斯粒子滤波直接处理非线性的量测方程，获得最优四元数估计，根据四元数姿态变换矩阵实时解算出最优姿态角。本发明不仅能够实现全姿态解算，而且能明显抑制噪声和漂移误差，获得精确姿态角，同时降低粒子滤波的计算量，使算法满足系统实时性要求。

技术领域

本发明提供的一种四元数高斯粒子滤波移动机器人姿态解算方法，属于数字滤波和多传感器数据融合技术领域，本发明提供的方法适用于非线性姿态测量系统。

背景技术

姿态解算的精度和速度将直接影响姿态控制算法的稳定性、可靠性和实现的难易程度，所以，姿态解算是移动或飞行控制实现的前提。姿态解算方法主要有：方向余弦法、欧拉角法和四元数法。方向余弦法的微分方程组的维数太高，达到了9维，且存在非正交化误差；欧拉角法虽然可以直观的表示姿态角，但是在解算过程中存在奇点；而四元数法可以全姿态工作，且相对于方向余弦法，四元数的计算量明显降低。因此，四元数法成为目前惯导系统求解姿态的主要方法。

小型两轮机器人姿态的测量普遍采用低成本的MEMS捷联惯性测量单元，其主要由低成本陀螺仪、加速度传感器和电子罗盘组成。MEMS陀螺仪具有温度漂移特性，加速度传感器会受到机器人移动过程中机体振动的影响。因此，为了获得高可靠性、高精度的姿态数据需要融合多传感器的数据，滤除外部干扰。由于在滤波过程中观测方程存在非线性环节，传统的线性卡尔曼滤波技术难以处理，而扩展卡尔曼滤波需要对系统模型进行线性化才能处理，线性化的过程带来了高阶截断误差。理论上，粒子滤波可以直接处理非线性问题，但是，由于系统实时性的要求，大部分粒子滤波难以直接应用，因此，需要采用改进的粒子滤波算法。本发明专利是针对上述问题的一种有效的移动机器人姿态解算方法。

发明内容

本发明的目的在于针对上述存在的问题和不足，提出一种用于移动机器人姿态检测的四元数高斯粒子滤波方法，算法较为简单，能够直接处理非线性的观测方程，对噪声和漂移抑制明显，能得到高精准的姿态角。

本发明具体过程如下：

步骤1：选取移动机器人姿态角及相应的参考坐标系，确定姿态变换矩阵；

步骤2：选取四元数作为系统状态量，根据初始姿态角获得初始四元数，利用陀螺仪输出数据建立四元数微分方程作为姿态解算系统的线性状态方程；

步骤3：根据加速度计三轴输出数据与重力加速度之间的关系建立系统非线性观测方程；

步骤4：根据系统的状态方程、观测方程及四元数与姿态角之间的变换关系，利用高斯粒子滤波框架，建立四元数高斯粒子滤波算法流程，获取四元数的估计值；

步骤5：对四元数的估计值进行归一化处理，通过归一化四元数的值及姿态变换矩阵解算出滤波后的姿态角。

进一步地，步骤1中由于本发明只涉及俯仰角θ和横滚角γ，不考虑航向角ψ，因此移动机器人姿态数据采集方案采用陀螺仪和加速度计采集姿态数据，选取东北天坐标系作为导航坐标系，导航系与载体系之间的变换关系可用如下姿态变换矩阵表示：

进一步地，步骤2中定义四元数Q＝[q₁，q₂，q₃，q₄]^T，其中q₁为四元数标量部分，q₂，q₃，q₄为四元数矢量部分，初始四元数Q₀可由初始姿态角获得：

陀螺仪输出模型可表示为：