CN108318038A - 一种四元数高斯粒子滤波移动机器人姿态解算方法 - Google Patents

Abstract

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

Description

一种四元数高斯粒子滤波移动机器人姿态解算方法

技术领域

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

背景技术

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

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

发明内容

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

本发明具体过程如下：

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

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

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

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

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

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

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

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

ω＝ω°+b+η

其中，ω为陀螺仪实际测量值，ω°为真实值，b为陀螺仪漂移值，η为均值为0，方差为的高斯白噪声；

利用陀螺仪实际三轴测量值ω＝[ω_x ω_y ω_z]^T构建四元数微分方程：

对四元数微分方程进行离散化作为系统的状态模型：

Q_k+1＝Φ_k+1，k(ω_k)Q_k

Q_k+1表示离散化后k+1时刻的四元数，k取0到T-1间的正整数，T为仿真时间，Q_k表示离散化后k时刻的四元数，Φ_k+1，k(ω_k)为状态转移矩阵，具体形式如下：

其中，ω_k为k时刻的陀螺仪实际测量值， 为的反对称矩阵，Δt为采样时间间隔，I_3×3为3阶单位矩阵。

进一步地，步骤3中根据加速度计的输出与重力加速度之间的关系，采用四元数的形式来建立系统量测方程，即将姿态变换矩阵表示为四元数的形式，量测方程如下：

y_k+1＝A_k+1(Q)r+v

y_k+1为k+1时刻的加速度计测量值，r＝[0，0，g]^T为重力加速度矢量，v为均值为0，方差为的量测噪声，根据步骤1的姿态变换矩阵，采用四元数表示方式，A_k+1(Q)为：

进一步地，步骤4中根据步骤2，3建立的系统状态、量测模型，采用高斯粒子滤波算法，具体步骤如下：

Step1：初始化：对初始四元数Q₀进行高斯近似，得到初始四元数粒子集其服从N(Q₀；μ₀；Σ₀)高斯分布，μ₀为初始四元数均值，Σ₀为初始四元数方差，i＝1，…，Ns，Ns为大于1的正整数；

Step2：时间更新：采用步骤2中状态方程更新粒子Φ_k+1，k(ω_k)为k到k+1时刻的状态转移矩阵，ω_k为k时刻的陀螺仪测量值，获得k+1时刻的先验四元数粒子集

Step3：量测更新：采用先验四元数粒子集计算姿态变换矩阵利用步骤3中的量测方程计算出先验粒子集的似然概率：

ρ_v(·)表示加速度计量测噪声v的密度函数，四元数粒子后验权值更新如下：

Step4：采用特征值分解的方法，根据后验权值求取四元数粒子的加权平均值，即四元数最优估计值并求取四元数误差协方差

定义为四元数乘法，由及作高斯近似得到k+1时刻后验四元数粒子集其服从正态分布用于下一次的时间更新过程。

进一步地，步骤5中对步骤4-step4求取的四元数最优估计值进行归一化处理，通过姿态变换矩阵求出姿态角：

θ＝arcsin(2(q₃q₄+q₁q₂))

本发明具有如下优点：采用四元数微分方程建立状态模型，相对于欧拉角法和方向余弦法来说，可以全姿态工作，无奇点存在，同时计算量降低；滤波算法采用高斯粒子滤波，可以直接建立非线性量测方程，建模简单，相较于扩展卡尔曼滤波算法来说，高斯粒子滤波无需对量测模型进行线性化处理，减少了线性化带来的高阶截断误差，提高了姿态解算的精度；同时高斯粒子滤波不存在重采样过程，算法较为简单，降低了解算的计算量，提高了系统的实时性。

附图说明

图1四元数高斯粒子滤波流程图

图2原始误差及四元数高斯粒子滤波误差曲线

图3四元数高斯粒子滤波及扩展卡尔曼滤波误差曲线

图4四元数高斯粒子滤波及扩展卡尔曼滤波解算俯仰角、横滚角均方误差曲线

具体实施方式

下面详细描述本发明的实施例，本实施例是示例性的，仅用于解释本发明，而不能理解为对本发明的限制。参照说明书附图对本发明的一种四元数高斯粒子滤波移动机器人姿态解算方法作以下详细地说明：

如图1所示，为四元数高斯粒子滤波的算法流程。

首先选取移动机器人姿态角及相应的参考坐标系，确定姿态变换矩阵；然后由初始姿态角获得初始四元数，采用四元数微分方程建立系统状态模型；采用加速度计输出与重力加速度的关系建立系统量测模型；获得系统模型之后，采用高斯粒子滤波算法融合陀螺仪与加速度计数据得到最优估计四元数；最后由最优估计四元数解算出姿态角。

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

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

2)建立基于四元数的系统状态模型

在仿真中选取初始姿态角为：θ＝10°，γ＝10°，ψ＝0°，由初始姿态角计算出初始四元数Q₀：

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

ω＝ω°+b+η

其中，ω为陀螺仪实际测量值，ω°为真实值，b为陀螺仪漂移值，在本实施例中b＝[0.003 0.003 0.003]^T，η为均值为0，方差为的高斯白噪声，在本实施例中

利用陀螺仪实际三轴测量值ω＝[ω_x ω_y ω_z]^T构建四元数微分方程，作为系统的状态方程：

对四元数微分方程进行离散化：

Q_k+1＝Φ_k+1，k(ω_k)Q_k

Q_k+1表示离散化后k+1时刻的四元数，k取0到T-1间的正整数，T为仿真时间，在本实施例中，T取200s，Q_k表示离散化后k时刻的四元数，Φ_k+1，k(ω_k)为状态转移矩阵，具体形式如下：

其中，ω_k为k时刻的陀螺仪实际测量值， 为的反对称矩阵，Δt为采样时间间隔，I_3×3为3阶单位矩阵。

3)建立系统非线性量测模型

根据加速度计的输出与重力加速度之间的关系来建立系统量测方程，即将姿态变换矩阵表示为四元数的形式，量测方程如下：

y_k+1＝A_k+1(Q)r+v

y_k+1为k+1时刻的加速度计测量值，r＝[0，0，g]^T为重力加速度矢量，v为均值为0，方差为的量测噪声，根据步骤1的姿态变换矩阵，采用四元数表示方式，A_k+1(Q)为：

4)构建用于融合陀螺仪和加速度计数据的高斯粒子滤波器

本发明中数据融合算法采用高斯粒子滤波算法实现，下面给出用于融合陀螺仪和加速度计数据的高斯粒子滤波算法基本步骤：

Step1：初始化：对初始四元数Q₀进行高斯近似，得到初始四元数粒子集其服从N(Q₀；μ₀；Σ₀)高斯分布，μ₀为初始四元数均值，Σ₀为初始四元数方差，i＝1，…，Ns，Ns为大于1的正整数，本实施例中Ns选取为1000，即选取1000个粒子点数；

Step2：时间更新：采用步骤2中状态方程更新粒子Φ_k+1，k(ω_k)为k到k+1时刻的状态转移矩阵，ω_k为k时刻的陀螺仪测量值，获得k+1时刻的先验四元数粒子集

Step3：量测更新：采用先验四元数粒子集计算姿态变换矩阵利用步骤3中的量测方程计算出先验粒子集的似然概率：

ρ_v(·)表示加速度计量测噪声v的密度函数，四元数粒子后验权值更新如下：

Step4：采用特征值分解的方法，根据后验权值求取四元数粒子的加权平均值，即四元数最优估计值并求取四元数误差协方差

定义为四元数乘法，由及作高斯近似得到k+1时刻后验四元数粒子集其服从正态分布用于下一次的时间更新过程。

5)解算姿态角

在获得最优估计四元数后，对其进行归一化：

通过四元数姿态变换矩阵与欧拉角表示的姿态变换矩阵两者之间的关系求出姿态角：

θ＝arcsin(2(q₃q₄+q₁q₂))

综上所述，对该方法进行效果分析，如图2所示，红线表示的为未滤波的姿态角误差曲线，由于陀螺仪漂移的存在，红线逐渐发散，蓝线表示的是经过滤波后的姿态角误差曲线，滤波后的误差明显小于未滤波误差，陀螺仪的漂移及噪声被很好地滤除；如图3所示，由于避免了扩展卡尔曼滤波的线性化过程，四元数高斯粒子滤波的精度高于扩展卡尔曼滤波的精度；为了评估两者的估计性能，进行了50次仿真实验，如图4所示，为两者仿真50次的均方误差对比图，均方误差能够体现数据的变化程度，从图中可以看出，四元数高斯粒子滤波的均方误差小于扩展卡尔曼滤波，说明其估计精确度高于扩展卡尔曼滤波，具有良好的估计性能。同时该方法响应速度快，具有良好的稳定性和实时性，具有实际的应用价值。

Claims (3)

1.一种四元数高斯粒子滤波移动机器人姿态解算方法，其特征在于，具体包括以下步骤：

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

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

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

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

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

2.根据权利要求1所述的一种四元数高斯粒子滤波移动机器人姿态解算方法，其特征在于步骤3所述系统非线性量测方程是根据加速度计的输出与重力加速度之间的关系，采用四元数的形式来建立，量测方程如下：

y_k+1＝A_k+1(Q)r+v

y_k+1为k+1时刻的加速度计测量值，k取0到T-1间的正整数，T为仿真时间，r＝[0，0，g]^T为重力加速度矢量，v为均值为0，方差为的量测噪声，根据步骤1的姿态变换矩阵，采用四元数表示方式，A_k+1(Q)为：

q₁，q₂，q₃，q₄为四元数Q的值，即Q＝[q₁ q₂ q₃ q₄]^T，其中q₁为四元数标量部分，q₂，q₃，q₄为四元数矢量部分。

3.根据权利要求1所述的一种四元数高斯粒子滤波移动机器人姿态解算方法，其特征在于步骤4中所述的基于四元数高斯粒子滤波算法的移动机器人姿态角的求解方法如下：

Step1：初始化：对初始四元数Q₀进行高斯近似，得到初始四元数粒子集其服从N(Q₀；μ₀；Σ₀)高斯分布，μ₀为初始四元数均值，Σ₀为初始四元数方差，i＝1，…，Ns，Ns为大于1的正整数；

Step2：时间更新：采用步骤2中状态方程更新粒子Φ_k+1，k(ω_k)为k到k+1时刻的状态转移矩阵，ω_k为k时刻的陀螺仪测量值，获得k+1时刻的先验四元数粒子集

Step3：量测更新：采用先验四元数粒子集计算姿态变换矩阵利用步骤3中的量测方程计算出先验粒子集的似然概率：

ρ_v(·)表示加速度计量测噪声v的密度函数，四元数粒子后验权值更新如下：

Step4：采用特征值分解的方法，根据后验权值求取四元数粒子的加权平均值，即四元数最优估计值并求取四元数误差协方差

定义为四元数乘法，由及作高斯近似得到k+1时刻后验四元数粒子集其服从正态分布用于下一次的时间更新过程。