CN110595434B - 基于mems传感器的四元数融合姿态估计方法 - Google Patents

Abstract

本发明公开了一种基于MEMS传感器的四元数融合姿态估计方法，包括：将陀螺仪原始测量信号进行预处理，得到预处理后的高频陀螺输出；使用含遗忘因子的加权最小二乘算法对所述高频陀螺输出进行处理，得到去噪后的陀螺输出；基于获取的初始姿态四元数和所述去噪后的陀螺输出使用改进的最小二乘递推方法估计陀螺漂移；将估计得到的陀螺漂移输入最小二乘估计模型得到补偿后的姿态四元数；基于所述补偿后的姿态四元对陀螺仪的姿态进行更新。实现提高姿态估计精度的优点。

Description

基于MEMS传感器的四元数融合姿态估计方法

技术领域

本发明涉及姿态估计领域，具体地，涉及一种基于MEMS传感器的四 元数融合姿态估计方法。

背景技术

姿态估计作为其感知的重要环节，在无人机、无人车等智能机器中被 广泛使用。

如今消费级无人市场逐渐爆发，尤其是作为无人市场先驱的无人机系 列，由于MEMS(Microelectromechanical Systems)技术因为其独有的优 势可以低成本的集成更多传感器，从而有利于实现姿态的精确测量和控制， 并且MEMS传感器可测量的动态范围也在不断扩大。MEMS陀螺仪在无 人系列中最直观的应用是获取姿态，延续应用是对运载体的平稳控制与辅 助导航，其中陀螺仪是关键器件。但陀螺仪无法提供绝对的基准，因此， 如何从带有噪声的MEMS陀螺仪输出中获取更为准确且稳定的实时姿态 信息是目前研究的重点，所以研究如何对MEMS陀螺仪进行降噪更事关 姿态估计的精度。

姿态的表示方法是研究姿态估计的基础，常见的姿态解算方法有欧拉 角、方向余弦、旋转矢量、四元数微分方程。由于全局且非奇异的优点， 与可以避免大量三角计算的特性，计算量小的四元数表示方法应用广泛。 而基于MEMS的加速度计/陀螺仪的微惯性姿态测量单元(Micro Inertial Measurement Unit,MIMU)因为具有成本低、体积小、功耗低的优点，在姿 态估计领域应用较多。在长时间的工作段内，由于加速度计的精度高于陀 螺仪，且陀螺仪有累计误差，结合加速度计与陀螺仪的做法可以得到较高 精度的姿态信息，使用磁强计辅助的方式也可进行航向角的修正，但磁强 计对外界磁场较为敏感，比较容易受到干扰，对运载体的行驶环境有一定 的要求。因此，研究基于MEMS加速度计/陀螺仪的微惯性姿态测量单元 的四元数姿态更新方式具有重要的意义，不仅成本较低，可适用环境广泛， 而且其寿命长、集成度较高，所以在无人系列的感知层中，研究基于MEMS 技术的姿态测量具有重要的作用。

尤其是小型无人机的应用，因为体积小的限制，小型无人机在一定程 度上限制了激光陀螺和光纤陀螺等高精度姿态测量装置的应用，于是 MEMS传感器成为小型无人机姿态测量和控制的首选。由于单纯依靠提高 惯性仪表精度来改善测量精度不仅需要较高的技术水平，而且研发成本也 相应提升，所以在硬件基础保持不变的情况下，提升姿态精度的关键就在 于算法的选择和融合方式上。例如仿生无人机的应用，它作为一种小型且 有着重要应用的无人机系列，基于MEMS的传感器是对它进行稳定控制 和姿态测量最合适的种类。

发明内容

本发明的目的在于，针对上述问题，提出一种基于MEMS传感器的 四元数融合姿态估计方法，以实现提高姿态估计精度的优点。

为实现上述目的，本发明实施例采用的技术方案是：

一种基于MEMS传感器的四元数融合姿态估计方法，包括：

将陀螺仪原始测量信号进行预处理，得到预处理后的高频陀螺输出；

使用含遗忘因子的加权最小二乘算法对所述高频陀螺输出进行处理，得到 去噪后的陀螺输出；

基于获取的初始姿态四元数和所述去噪后的陀螺输出使用改进的最小二乘 递推方法估计陀螺漂移；

将估计得到的陀螺漂移输入最小二乘估计模型得到补偿后的姿态四元数；

基于所述补偿后的姿态四元对陀螺仪的姿态进行更新。

进一步的，所述将陀螺仪原始测量信号进行预处理的步骤之前，包括：

将陀螺仪原始测量信号表示为：

s(k)＝h(k)+ε·e(k),k＝0,1,…,n-1，

其中，s(k)为陀螺仪原始测量信号，h(k)为无偏信号，e(k)为噪声，ε为 噪声标准差。

进一步的，所述含遗忘因子的加权最小二乘算法，包括：

设定输入与输出服从的函数关系；

基于所述函数关系建立模型输出值和实际观测值的误差平方和方程，并在 所述误差平方和方程中引入遗忘因子λ；

基于引入遗忘因子λ的误差平方和方程获取最小二乘问题；

对所述最小二乘问题进行加权处理。

进一步的，所述含遗忘因子的加权最小二乘算法，包括：

设定系统输入与输出所服从的函数关系为y＝f(x,t_i)，其中y为系统输 出，t_i为系统输入，x∈Rⁿ，为待定参数或者待定向量；

记基于模型输出值和实际观测值的误差平方和为S，如

其中，m为实际观测次数，求得S最小时x 的值为一般的最小二乘问题，并引入遗忘因子λ到一般最小二乘估计准则 中，如式：

引入残差函数r_i(x)＝y_i-f(x,t_i),i＝1,2,…,m，记

于是记最小二乘问题为

将系统量测方程写为矩阵形式z＝Hx+v，对最小二乘估计准则进行加 权处理，如式：

其中，z为量测向量，作为无法得到待估向量x的真实值的一种间接 测量，选择了真实值x的各分量进行线性组合；H为量测矩阵，v为随机 量测噪声，W为加权正定矩阵，得到加权最小二乘估计为如式：

加权最小二乘估计残差如式：

其中测量噪声v满足均值为零，方差阵为R，即且满足W＝R^-1，则加 权最小二乘估计如式

进一步的，所述改进的最小二乘递推方法，包括：

建立最小二乘离散递推方程式：

其中，K为滤波增益，P为均方误差矩阵，I为单位阵，H为量测矩 阵，Z为量测向量，

为状态向量，k为自然数；

利用平方根滤波将最小二乘离散递推方程式的状态均方误差阵更新 为：

Δ为P的平方根。

进一步的，所述将估计得到的陀螺漂移输入最小二乘估计模型得到补偿后 的姿态四元数，包括使用四元数融合的动态更新姿态方式对对陀螺仪的姿态进 行更新，

所述四元数融合的动态更新姿态方式，包括：

设载体坐标系为b系，导航坐标系为n系，由b系到n系的坐标变换矩阵

称为姿态转换矩阵；

基于所述姿态转换矩阵表示陀螺输出的角速度信息，得到陀螺四元数；

基于所述姿态转换矩阵表示加速度计输出的加速度信息，得到加速度计四 元数；

将所述陀螺四元数与所述加速度计四元数做差，作为滤波的状态量，将所 述加速度计四元数作为观测量，则得到四元数最小二乘估计模型；

基于所述四元数最小二乘估计模型得到四元数的姿态补偿模型。

进一步的，所述四元数最小二乘估计模型为：

其中q_ω为陀螺四元数，q_a为加速度计四元数，T为角速度采样周期，Q_k为 上一时刻的四元数矩阵，ω₀为陀螺漂移向量，q_a0加速度计解算中的噪声四元数， k为自然数。

为状态估计向量，由四元数误差组成；z为观测向量，由加速度 计四元数组成。

进一步的，所述四元数的姿态补偿模型为：

其中Q_k为上一时刻的四元数矩阵，k为自然数，q_ω为当前时刻的陀螺姿态 四元数，q为补偿漂移的姿态四元数。

进一步的，所述加速度信息由加速度计测得的重力矢量得到。

本发明的技术方案具有以下有益效果：

1、本发明提出的一种基于MEMS传感器的四元数融合姿态估计方法，通 过改进的四元数姿态更新方法针对陀螺长时间工作时间段内漂移严重的问题， 使用了一种改进的最小二乘方法对陀螺输出进行了最优拟合估计，使得陀螺精 度在提高的同时输出也更为平滑，减少了一部分突变与奇异值。

2、本发明提出的一种基于MEMS传感器的四元数融合姿态估计方法，利 用四元数计算量小且包含所有姿态信息的特点，对量测值进行了不同程度的加 权，使得在滤波过程可以充分利用量测值所包含的信息而且可以减少一部分计 算量。

3、本发明提出的一种基于MEMS传感器的四元数融合姿态估计方法，在 四元数姿态更新方法，在滤波过程中引入了平方根滤波的思想，可以有效克服 迭代过程中的计算误差与数值截断误差。

4、本发明提出的一种基于MEMS传感器的四元数融合姿态估计方法，优 势在于能够长时间段内较高精度的工作，将加速度计测得的重力矢量与陀螺仪 测得的角速率信息分别解算得到的四元数进行融合滤波，有效提高了姿态估计 的精度。

下面通过附图和实施例，对本发明的技术方案做进一步的详细描述。

附图说明

图1为本发明实施例所述的基于MEMS传感器的四元数融合姿态估 计方法的流程图；

图2为本发明实施例所述的改进的最小二乘递推方法的框图；

图3为本发明实施例所述的姿态估计更新框图；

图4为本发明实施例所述的四元数融合框图；

图5为本发明实施例所述的实验效果图。

具体实施方式

以下结合附图对本发明的优选实施例进行说明，应当理解，此处所描 述的优选实施例仅用于说明和解释本发明，并不用于限定本发明。

如图1所示，一种基于MEMS传感器的四元数融合姿态估计方法， 包括：

S101：将陀螺仪原始测量信号进行预处理，得到预处理后的高频陀螺输出；

S102：使用含遗忘因子的加权最小二乘算法对所述高频陀螺输出进行处理， 得到去噪后的陀螺输出；

S103：基于获取的初始姿态四元数和所述去噪后的陀螺输出使用改进的最 小二乘递推方法估计陀螺漂移；

S104：将估计得到的陀螺漂移输入最小二乘估计模型得到补偿后的姿态四 元数；

S105：基于所述补偿后的姿态四元对陀螺仪的姿态进行更新。

进一步的，所述含遗忘因子的加权最小二乘算法，包括：

设定输入与输出服从的函数关系；

基于所述函数关系建立模型输出值和实际观测值的误差平方和方程，并在 所述误差平方和方程中引入遗忘因子λ；

基于引入遗忘因子λ的误差平方和方程获取最小二乘问题；

对所述最小二乘问题进行加权处理。

进一步的，所述将估计得到的陀螺漂移输入最小二乘估计模型得到补偿后 的姿态四元数，包括使用四元数融合的动态更新姿态方式对对陀螺仪的姿态进 行更新，

所述四元数融合的动态更新姿态方式，包括：

设载体坐标系为b系，导航坐标系为n系，由b系到n系的坐标变换矩阵C_bn 称为姿态转换矩阵；

基于所述姿态转换矩阵表示陀螺输出的角速度信息，得到陀螺四元数；

基于所述姿态转换矩阵表示加速度计输出的加速度信息，得到加速度计四 元数；

将所述陀螺四元数与所述加速度计四元数做差，作为滤波的状态量，将所 述加速度计四元数作为观测量，则得到四元数最小二乘估计模型；

基于所述四元数最小二乘估计模型得到四元数的姿态补偿模型。

在一个具体的应用场景中，

一种基于MEMS传感器的四元数融合姿态估计方法，首先将陀螺仪 原始测量信号表示如式(1)，

s(k)＝h(k)+ε·e(k),k＝0,1,…,n-1 (1)，

其中，s(k)为陀螺仪原始测量信号，h(k)为无偏信号，e(k)为噪声，ε为 噪声标准差。首先对其进行预处理，得到预处理后的高频陀螺输出。

预处理是为了使得数据曲线更为平滑，剔除掉明显不正常的数值，以 防在后续的数据处理中污染其它步骤。本实施所用为普通的数据平滑，但 不限于该方法。

将陀螺预处理后信号作为初始值，使用改进的最小二乘递推方法，使 得陀螺信号减少突变与奇异值，更加平滑。

含遗忘因子的加权最小二乘算法，包括：

步骤1：设定系统输入与输出所服从的函数关系为y＝f(x,t_i)，其中y为 系统输出，t_i为系统输入，x∈Rⁿ，为待定参数或者待定向量。系统为MEMS 系统。

步骤2：记基于模型输出值和实际观测值的误差平方和为S，如

其中，m为实际观测次数，求得S最小时x 的值为一般的最小二乘问题。在步骤一的基础上引入遗忘因子λ到一般最 小二乘估计准则中，如式(2)

引入残差函数r_i(x)＝y_i-f(x,t_i),i＝1,2,…,m，记

于是记最小二乘问题为

步骤3：将系统量测方程写为矩阵形式z＝Hx+v，对最小二乘估计准 则进行加权处理，如式(3)：

其中，z为量测向量，它作为无法得到待估向量x的真实值的一种间 接测量，选择了真实值x的各分量进行线性组合。H为量测矩阵，v为随 机量测噪声，W为加权正定矩阵，得到加权最小二乘估计为如式(4)：

加权最小二乘估计残差如式(5)：

其中测量噪声v满足均值为零，方差阵为R，即且满足W＝R^-1，则加 权最小二乘估计如式(6)：

也被称为马尔科夫估计。

含遗忘因子的加权最小二乘算法在实际求解的过程中，克服了普通最小 二乘递推估计准则使得所有偏差的平方和达到最小但是对于各个量测值 的使用上没有差异的劣势，能够充分地利用量测值信息。

含遗忘因子的加权最小二乘算法为马尔可夫估计。所谓马尔科夫估计， 就是从某个状态值

出发，随着时间的变化，随机地反复更新状态值，最 终使得状态值成为一个更接近目标分布的样本量，其优势是避免了原始采 样困难的问题，所以其均方误差最小，是最小二乘中的最优者。

改进的最小二乘递推方法，包括：

步骤1：最小二乘离散递推如式(7)：

其中，K为滤波增益，P为均方误差矩阵，I为单位阵，H为量测矩 阵，Z为量测向量，

为状态向量。

步骤2：引入平方根滤波思想，对式(7)的状态均方误差阵进行更新， 如式(8)：

Δ为P的平方根，平方根滤波的含义是由Δ_k的递推关系代替P的递推关系。

引入平方根滤波思想，对状态均方误差阵进行更新可以有效克服滤波 器计算发散的问题，具体表现为可以克服误差协方差矩阵失去非负定性和 对称性。

引入平方根滤波思想，对状态均方误差阵进行更新本质是以式(8) 中Δ_k的递推关系代替P_k的递推关系，可以有效克服计算误差与数值截断误 差。

引入平方根滤波思想，对状态均方误差阵进行初始输入为状态均方差 阵的平方根Δ₀，其更新公式如式(9)：

其中，

Δ₀为Δ_k中k取0时的值。

四元数融合的动态更新姿态方式，包括：

步骤1：设载体坐标系为b系，导航坐标系为n系，由b系到n系的 坐标变换矩阵

称为姿态转换矩阵，也被称为数学平台。定义四元数如式 (10)：

其中，uⁿ表示了旋转轴的方向，θ表示旋转轴转过的角度，即式(10) 所表示的四元数Q包含了这种等效旋转的全部信息。

步骤2：表示陀螺输出的角速度信息如式(11)：

ω＝[ω_x ω_y ω_z]^T (11)，

求解式(10)可得式(12)：

计算(12)式可得陀螺更新陀螺四元数q_ω＝[q₀,q₁,q₂,q₃]^T，带入式(14)式得 到姿态更新矩阵

简记式(13)为式(14)：

由四元数与欧拉角的转换关系可得姿态求解式(15)，

θ,γ分别为航向、 俯仰、横滚角，

步骤3：表示加速度计输出的加速度信息如式(16)：

a＝[a_x a_y a_z]^T (16)，

由重力矢量解算得到的四元数如式(17)，由g_b＝a得到当前加速度姿态 四元数q_a，

步骤4：将陀螺四元数与加速度计四元数做差，作为滤波的状态量， 将加速度计四元数作为观测量，则得到四元数最小二乘估计模型如式(18)：

其中q_ω为陀螺四元数，q_a为加速度计四元数，T为角速度采样周期，Q_k为 上一时刻的四元数矩阵，ω₀为陀螺漂移向量，q_a0加速度计解算中的噪声四元数， k为自然数。

为状态估计向量，由四元数误差组成；z为观测向量，由加速度 计四元数组成。

步骤5：四元数的姿态补偿模型如式(19)：

其中Q_k为上一时刻的四元数矩阵，k为自然数，q_ω为当前时刻的陀螺姿态 四元数，q为补偿漂移的姿态四元数。

加速度计四元数是由加速度计测得的重力矢量得到的。

四元数差值作为状态量估计值，由于在长时间的工作段内，加速度计 的精度是高于陀螺仪的，并且没有陀螺仪的累计误差，所以将四元数融合 可以有效提高滤波的精度。

四元数差值法，在仅使用陀螺仪与加速度计传感器并不增加额外部件 的情况下，摒弃直接使用加速度计求运载体角度的环节，先建立重力四元 数模型，在对陀螺仪降噪后，从而补偿陀螺仪的姿态四元数。

四元数最小二乘估计模型，由于当前时刻的真实运载体姿态难以获得， 所以利用式(18)得到陀螺仪漂移角速率值ω₀(k+1)，再根据陀螺仪的四元 数姿态方程(19)得到补偿后的姿态四元数。

如图2所示，本发明的一种基于MEMS传感器的四元数融合姿态估 计方法，对于陀螺输出分两部分处理；首先对其进行预处理，由于陀螺的 噪声在主要集中在高频段，所以主要先降低陀螺的高频段噪声，减少所包 含的突变和奇异值；其次使用具有遗忘因子的加权最小二乘方法对陀螺输 出进行降噪估计，使得陀螺仪输出精度较高且更为平滑。

本发明的一种基于MEMS传感器的四元数融合姿态估计方法，姿态 更新过程如图3。初始姿态四元数由陀螺仪输出解算得到，对陀螺输出进 行降噪后解算得到补偿漂移后的姿态四元数。使用最小二乘误差估计模型， 将加速度计四元数与陀螺仪四元数进行融合滤波，得到融合后的姿态四元 数，最后进行姿态更新。

如图4所示，本发明的基于MEMS传感器的四元数融合姿态估计方 法，利用加速度计与陀螺仪传感器分别获得运载体的加速度与角速度。由 于陀螺仪长时间工作存在误差累计导致精度下降，加速度计长时间的工作 精度比较高，所以将加速度计输出作为量测值，将加速度计输出解算得到 的四元数与陀螺仪输出解算得到的四元数作差，作为滤波的状态估计量， 使用改进的递推最小二乘方法进行滤波融合，更新姿态估计值。

如图5，为姿态角估计效果图，红色线为实验转台自带的航姿参考系 统输出的姿态值；方案一为传统互补滤波姿态更新；方案二为本发明提出 的基于MEMS的融合四元数姿态估计方法；可观察到方案二比起方案一， 所估计的姿态角收敛较快，并且能显著减小因陀螺漂移造成的姿态解算累 积误差，姿态估计精度能稳定提高40％左右。

最后应说明的是：以上所述仅为本发明的优选实施例而已，并不用于 限制本发明，尽管参照前述实施例对本发明进行了详细的说明，对于本领 域的技术人员来说，其依然可以对前述各实施例所记载的技术方案进行修 改，或者对其中部分技术特征进行等同替换。凡在本发明的精神和原则之 内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围 之内。

Claims (7)

1.一种基于MEMS传感器的四元数融合姿态估计方法，其特征在于，包括：

将陀螺仪原始测量信号进行预处理，得到预处理后的高频陀螺输出；

使用含遗忘因子的加权最小二乘算法对所述高频陀螺输出进行处理，得到去噪后的陀螺输出；

基于获取的初始姿态四元数和所述去噪后的陀螺输出使用改进的最小二乘递推方法估计陀螺漂移；

将估计得到的陀螺漂移输入最小二乘估计模型得到补偿后的姿态四元数；

基于所述补偿后的姿态四元对陀螺仪的姿态进行更新；

所述将估计得到的陀螺漂移输入最小二乘估计模型得到补偿后的姿态四元数，包括使用四元数融合的动态更新姿态方式对陀螺仪的姿态进行更新，

所述四元数融合的动态更新姿态方式，包括：

设载体坐标系为b系，导航坐标系为n系，由b系到n系的坐标变换矩阵

称为姿态转换矩阵；

基于所述姿态转换矩阵表示陀螺输出的角速度信息，得到陀螺四元数；

基于所述姿态转换矩阵表示加速度计输出的加速度信息，得到加速度计四元数；

将所述陀螺四元数与所述加速度计四元数做差，作为滤波的状态量，将所述加速度计四元数作为观测量，则得到四元数最小二乘估计模型；

基于所述四元数最小二乘估计模型得到四元数的姿态补偿模型；

所述四元数最小二乘估计模型为：

其中q_ω为陀螺四元数，q_a为加速度计四元数，T为角速度采样周期，Q_k为上一时刻的四元数矩阵，ω₀为陀螺漂移向量，q_a0加速度计解算中的噪声四元数， k为自然数；

为状态估计向量，由四元数误差组成；z为观测向量，由加速度计四元数组成。

2.根据权利要求1所述的基于MEMS传感器的四元数融合姿态估计方法，其特征在于，所述将陀螺仪原始测量信号进行预处理的步骤之前，包括：

将陀螺仪原始测量信号表示为：

s(k)＝h(k)+ε·e(k),k＝0,1,…,n-1，

其中，s(k)为陀螺仪原始测量信号，h(k)为无偏信号，e(k)为噪声，ε为噪声标准差。

3.根据权利要求1所述的基于MEMS传感器的四元数融合姿态估计方法，其特征在于，所述含遗忘因子的加权最小二乘算法，包括：

设定输入与输出服从的函数关系；

基于所述函数关系建立模型输出值和实际观测值的误差平方和方程，并在所述误差平方和方程中引入遗忘因子λ；

基于引入遗忘因子λ的误差平方和方程获取最小二乘问题；

对所述最小二乘问题进行加权处理。

4.根据权利要求3所述的基于MEMS传感器的四元数融合姿态估计方法，其特征在于，所述含遗忘因子的加权最小二乘算法，包括：

设定系统输入与输出所服从的函数关系为y＝f(x,t_i)，其中y为系统输出，t_i为系统输入，x∈Rⁿ，为待定参数或者待定向量；

记基于模型输出值和实际观测值的误差平方和为S，如

其中，m为实际观测次数，求得S最小时x的值为一般的最小二乘问题，并引入遗忘因子λ到一般最小二乘估计准则中，如式：

引入残差函数r_i(x)＝y_i-f(x,t_i),i＝1,2,…,m，记

于是记最小二乘问题为

将系统量测方程写为矩阵形式z＝Hx+v，对最小二乘估计准则进行加权处理，如式：

其中，z为量测向量，作为无法得到待估向量x的真实值的一种间接测量，选择了真实值x的各分量进行线性组合；H为量测矩阵，v为随机量测噪声，W为加权正定矩阵，得到加权最小二乘估计为如式：

加权最小二乘估计残差如式：

其中测量噪声v满足均值为零，方差阵为R，即且满足W＝R^-1，则加权最小二乘估计如式

5.根据权利要求1所述的基于MEMS传感器的四元数融合姿态估计方法，其特征在于，所述改进的最小二乘递推方法，包括：

建立最小二乘离散递推方程式：

其中，K为滤波增益，P为均方误差矩阵，I为单位阵，H为量测矩阵，Z为量测向量，

为状态向量，k为自然数；

利用平方根滤波将最小二乘离散递推方程式的状态均方误差阵更新为：

Δ为P的平方根。

6.根据权利要求1所述的基于MEMS传感器的四元数融合姿态估计方法，其特征在于，所述四元数的姿态补偿模型为：

其中Q_k为上一时刻的四元数矩阵，k为自然数，q_ω为当前时刻的陀螺姿态四元数，q为补偿漂移的姿态四元数。

7.根据权利要求1所述的基于MEMS传感器的四元数融合姿态估计方法，其特征在于，所述加速度信息由加速度计测得的重力矢量得到。

Images