CN104101345A - 基于互补重构技术的多传感器姿态融合方法 - Google Patents

Abstract

基于互补重构技术的多传感器姿态融合方法，主要包括：四元素迭代更新及载体姿态计算，多传感器载体姿态观测及四元素观测值计算；互补重构参数估计；互补重构四元素值计算等四个步骤。其主要优点：利用四元素观测值修正上一时刻四元素计算值，从而获得互补重构的四元素值，并利用此值进行当前时刻的四元素迭代更新，从机理上消除了算法的累积误差；同时将四元素更新算法和重构算法合二为一，提高了算法的执行效率。

Description

基于互补重构技术的多传感器姿态融合方法

技术领域

本发明涉及惯性导航领域，尤其是捷联惯性导航系统在利用单一惯性传感器(如陀螺仪传感器)数据进行载体姿态测量时，虽然短时间精度好，但是长期运行时的姿态测量结果由于积分作用产生累积误差，导致导航系统精度下降。本发明利用多传感器信息进行数据融合，可以用较低成本提高捷联惯性导航系统长时间精度。

背景技术

捷联惯性导航系统在计算时，需要把载体测得的加速度，角速度变化从载体坐标系转换到导航坐标系中，假设空间位置P在导航坐标系n的坐标表示为P(x_n,y_n,z_n)，在载体所在坐标系b的坐标表示为P(x_b,y_b,z_b)，则两者之间满足如下关系：

$\left[\begin{array}{c}{x}_n\\ y_n\\ z_n\end{array}\right]=C_b^n\·\left[\begin{array}{c}{x}_b\\ y_b\\ z_b\end{array}\right]---\left(1\right)$

其中 $C_b^n=\left(\begin{array}{ccc}{T}_{11}& T_{12}& T_{13}\\ T_{21}& T_{22}& T_{23}\\ T_{31}& T_{32}& T_{33}\end{array}\right),$ 称为姿态变换矩阵，或姿态矩阵。

姿态矩阵包含了载体当前的全部姿态信息。假设航向角为(以地理北向为七点，顺时针为正方向，逆时针为负方向，定义域为[-180°,+180°])，俯仰角θ(以载体横向水平轴为中心轴，绕中心轴转动，逆时针为正，顺时针为负，定义域[-90°,+90°])以及横滚角γ(以载体正向水平轴为中心轴转动，逆时针为正，顺时针为负，定义域[-90°,+90°])，则姿态矩阵中的各个元素可由下式给出：

一般的，为了更简便地描述从载体坐标系到导航坐标系的变换，采用了四元素这个数学工具。假设载体坐标系经过一次无中间过程的等效刚体旋转形成导航坐标系，其等效旋转矢量为如下所示：

$Q=q_0+q_1\·\stackrel{\→}{i}+q_2\·\stackrel{\→}{j}+q_3\·\stackrel{\→}{k}---\left(3\right)$

则旋转前载体的姿态矩阵与旋转矢量的四元素表达式之间的对应关系可由下式给出：

$\left\{\begin{array}{c}{T}_{11}={q_0}^2+{q_1}^2-{q_2}^2-{q_3}^2\\ T_{12}=2(q_1{q}_2-q_0{q}_3)\\ T_{13}=2(q_1{q}_3+q_0{q}_2)\\ T_{21}=2(q_1{q}_2+q_0{q}_3)\\ T_{22}={q_0}^2-{q_1}^2+{q_2}^2-{q_3}^2\\ T_{23}=2(q_2{q}_3-q_0{q}_1)\\ T_{31}=2(q_1{q}_3-q_0{q}_2)\\ T_{32}=2(q_2{q}_3+q_0{q}_1)\\ T_{33}={q_0}^2-{q_1}^2-{q_2}^2+{q_3}^2\end{array}\right.---\left(4\right)$

通过计算载体对应导航坐标系的旋转矢量的四元素来计算姿态矩阵，就是姿态矩阵的四元素更新法。

当前四元素更新计算主要利用迭代策略，假设t_k时刻的载体坐标系为b(k),导航坐标系为n(k)，t_k+1时刻的载体坐标系为b(k+1)，导航坐标系为n(k+1)。记从b(k)到b(k+1)的旋转四元素为q(h)，n(k)至b(k)的旋转四元素为Q(t_k)，n(k+1)至b(k+1)的旋转四元素为Q(t_k+1)。则Q(t_k)包含t_k时刻的载体的姿态信息，Q(t_k+1)包含t_k+1时刻的载体的姿态信息，q(h)则表示载体由t_k时刻经过一次旋转到达t_k+1时刻的姿态变化。

根据四元素自身的计算法则可得到如下的迭代更新表达式：

$Q\left(t_{k+1}\right)=Q\left(t_k\right)\⊗q\left(h\right)---\left(5\right)$

其中为四元素乘法运算。上述写成矩阵表达式有如下形式：

Q(t_k+1)＝M′[q(h)]·Q(t_k)     (6)

其中 $Q\left(t_{k+1}\right)=\left[\begin{array}{c}{q}_0\left(t_{k+1}\right)\\ q_1\left(t_{k+1}\right)\\ q_2\left(t_{k+1}\right)\\ q_3\left(t_{k+1}\right)\end{array}\right],$ $Q\left(t_k\right)=\left[\begin{array}{c}{q}_0\left(t_k\right)\\ q_1\left(t_k\right)\\ q_2\left(t_k\right)\\ q_3\left(t_k\right)\end{array}\right],$ $M^{\′}\left[q\left(h\right)\right]=\left[\begin{array}{cccc}{q}_0\left(h\right)& {-q}_1\left(h\right)& -q_2\left(h\right)& -q_3\left(h\right)\\ q_1\left(h\right)& q_0\left(h\right)& -q_3\left(h\right)& q_2\left(h\right)\\ q_2\left(h\right)& q_3\left(h\right)& q_0\left(h\right)& {-q}_1\left(h\right)\\ q_3\left(h\right)& {-q}_2\left(h\right)& q_1\left(h\right)& q_0\left(h\right)\end{array}\right].$

由于在计算的过程中，对q(h)有积分运算，因此会产生积分累积误差。积分累积误差会导致载体的推算定位与实际位置产生较大偏移，长时间导致导航精度下降以至于不可用。当前消除这一积分累积误差的主要手段均为通过辅助传感器直接对捷联惯性导航系统的输出结果进行修正，如互补滤波算法，利用磁感应器和里程计的修正法等。这些方法都不能对四元素的计算结果进行修正，长时间导航精度极其有限。

发明内容

本发明要克服现有技术的上述缺点，提出一种基于互补重构技术的多传感器姿态融合方法。

本发明的基本思路如原理如附图1所示；首先，它利用惯性传感器的数据进行载体姿态四元素的计算值的迭代更新，并进一步计算得到载体姿态信息；再收集观测传感器的数据求解得到载体姿态信息的观测值，并进一步计算姿态四元素的观测值；然后根据观测传感器和惯性传感器的数据计算互补重构参数；最后把载体姿态四元素的计算值和观测值融合起来，实现四元素的互补重构来修正惯性导航系统的积分累积误差和陀螺仪的漂移，提高惯性导航系统长时间精度。一种基于互补重构技术的多传感器姿态融合方法，其流程图如附图2所示，具体步骤如下所述：

1)、载体姿态信息计算；利用陀螺仪传感器的数据,根据旋转矢量算法计算四元素迭代更新矩阵M'[q(h)]，并根据下式：

Q(t_k)＝M′[q(h)]·Q′(t_k-1)     (7)

更新四元素值Q(t_k)，式中Q(t_k)为当前时刻四元素迭代值,M'[q(h)]为四元素更新矩阵,Q'(t_k-1)为上一时刻四元素重构值；并根据更新后的四元素计算值Q(t_k)计算得到载体姿态信息，分别是航向角俯仰角θ以及横滚角γ；

2)、四元素观测值计算；利用观测传感器数据，计算载体姿态信息的观测值，分别是航向角俯仰角以及横滚角并进一步计算姿态四元素的观测值

3)、互补参数计算；利用不同传感器的数据，判断和估计载体的运动状态，计算数据融合的互补重构参数α＝g(x₁,x₂,x₃,...)，其中x₁,x₂,x₃,...为各个传感器的数据；

4)、四元素互补重构；根据步骤3)中的数据融合的互补重构参数α，对步骤1)中得到的Q(t_k)和步骤2)中得到的进行数据融合，重构载体姿态四元素Q'(t_k)，其表达式为并对其进行标准化处理后用于下一次迭代更新，并返回步骤1)。

进一步，步骤1)所述利用陀螺仪传感器的数据进行姿态四元素的更新，并计算得到当前时刻的载体姿态信息，其包括以下步骤，如附图3所示：

(1.1)角速率数据获取与预处理；获取陀螺仪传感器的数据，并对其进行直流偏置修正处理，分别以三轴角速率的陀螺仪当前采样值减去预先计算的三轴角速率偏置值获得修正后的三轴角速率输出值。

(1.2)四元素更新矩阵计算；根据步骤(1.1)中获得的陀螺仪传感器的修正后的三轴角速率输出值计算四元素姿态更新矩阵M'[q(h)]，

$M^{\′}\left[q\left(h\right)\right]=\left[\begin{array}{cccc}{q}_0\left(h\right)& {-q}_1\left(h\right)& -q_2\left(h\right)& -q_3\left(h\right)\\ q_1\left(h\right)& q_0\left(h\right)& -q_3\left(h\right)& q_2\left(h\right)\\ q_2\left(h\right)& q_3\left(h\right)& q_0\left(h\right)& {-q}_1\left(h\right)\\ q_3\left(h\right)& {-q}_2\left(h\right)& q_1\left(h\right)& q_0\left(h\right)\end{array}\right]---\left(8\right)$

其中 $q_0\left(h\right)=\cos \frac{\mathrm{\Φ}}{2},q_1\left(h\right)=\frac{{\mathrm{\Φ}}_x\left(h\right)}{\mathrm{\Φ}}\sin \frac{\mathrm{\Φ}}{2},q_2\left(h\right)=\frac{{\mathrm{\Φ}}_y\left(h\right)}{\mathrm{\Φ}}\sin \frac{\mathrm{\Φ}}{2},q_3\left(h\right)=\frac{{\mathrm{\Φ}}_z\left(h\right)}{\mathrm{\Φ}}\sin \frac{\mathrm{\Φ}}{2}.$ Φ(h)为前一次采样时刻和当前采样时刻[t_k,t_k+1]时间段内的等效旋转矢量。Φ_x(h),Φ_y(h),Φ_z(h)为的旋转矢量三轴的分量。

(1.3)四元素迭代更新；根据步骤4)中互补重构后的四元素值Q'(t_k-1)，按照下式更新姿态四元素的值：

Q(t_k)＝M′[q(h)]·Q′(t_k-1)；

其中，Q'(t₀)为给定的模值为1的四元素初始值。

(1.4)姿态信息计算；根据更新后的四元素迭代值Q(t_k)，计算载体姿态信息，分别是航向角俯仰角θ以及横滚角γ，其主要计算公式如下：

其中，q₀，q₁，q₂，q₃为四元素迭代值的元素。

进一步，步骤2)中所述的计算载体的姿态观测信息包括以下步骤，如附图4所示：

(2.1)加速度数据获取与预处理；获取加速度传感器的数据a_{x_org},a_{y_org},a_{z_org}，进行直流偏置修正。先预先采样若干次计算其平均值作为三轴加速度的偏置值，然后分别以三轴加速度的当前采样值减去该三轴加速度偏置值作为修正后的三轴加速度输出值。

(2.2)载体姿态信息观测值计算；根据附图6所示力学原理计算静态情况下的观测传感器(加速度传感器)包含的载体姿态信息，其中；对上述载体姿态信息进行滤波，其主要滤波方法有均值滤波，一阶惯性滤波，中值滤波，kalman滤波等方法。优选的，我们采用获得kalman滤波方法对加速度传感器包含的载体姿态信息进行滤波得到获取磁阻传感器数据，数据处理后得到载体航向角信息

(2.3)姿态矩阵观测值计算；根据载体姿态信息的观测值计算载体姿态矩阵元素的观测值，具体计算公式如下：

(2.4)四元素观测值计算；根据载体姿态矩阵的观测值计算载体姿态四元素的观测值具体计算公式如下：

$\left\{\begin{array}{c}\left|{\hat{q}}_0\right|=\frac{1}{2}\sqrt{1+{\hat{T}}_{11}+{\hat{T}}_{22}+{\hat{T}}_{33}}\\ \left|{\hat{q}}_1\right|=\frac{1}{2}\sqrt{1+{\hat{T}}_{11}-{\hat{T}}_{22}-{\hat{T}}_{33}}\\ \left|{\hat{q}}_2\right|=\frac{1}{2}\sqrt{1-{\hat{T}}_{11}+{\hat{T}}_{22}-{\hat{T}}_{33}}\\ \left|{\hat{q}}_3\right|=\frac{1}{2}\sqrt{1-{\hat{T}}_{11}-{\hat{T}}_{22}+{\hat{T}}_{33}}\\ 4{\hat{q}}_0{\hat{q}}_1={\hat{T}}_{32}-{\hat{T}}_{23}\\ 4{\hat{q}}_0{\hat{q}}_2={\hat{T}}_{13}-{\hat{T}}_{31}\\ 4{\hat{q}}_0{\hat{q}}_3={\hat{T}}_{21}-{\hat{T}}_{12}\end{array}\right.---\left(11\right)$

其中，的符号由式 $\left\{\begin{array}{c}\mathrm{sign}\left({\hat{q}}_1\right)=\mathrm{sign}\left({\hat{q}}_0\right)\mathrm{sign}\left[({\hat{T}}_{32}-{\hat{T}}_{23})\right]\\ \mathrm{sign}\left({\hat{q}}_2\right)=\mathrm{sign}\left({\hat{q}}_0\right)\mathrm{sign}\left[({\hat{T}}_{13}-{\hat{T}}_{31})\right]\\ \mathrm{sign}\left({\hat{q}}_3\right)=\mathrm{sign}\left({\hat{q}}_0\right)\mathrm{sign}\left[({\hat{T}}_{21}-{\hat{T}}_{12})\right]\end{array}\right.$ 确定，的符号可任取。

进一步，步骤3)中所述的计算数据融合时的互补重构参数α包含以下步骤，如附图5所示：

(3.1)互补参数计算数据获取；收集陀螺仪传感器的数据ω_x,ω_y,ω_z，再收集加速度传感器的数据a_x,a_y，并进行数据处理，由于a_z在重力方向有一个常值，以此a_z的值不能作为判断载体运行状态的数据。

(3.2)互补参数计算；计算互补重构参数α＝g(x₁,x₂,x₃,...)，x₁,x₂,x₃,...为步骤(3.1)中收集的各传感器数据，优选的，其表达式如下所示：α＝max/(max+1)。其中,max是集合{K_ω·|ω_x|,K_ω·|ω_y|,K_ω·|ω_z|,K_a·|a_x|,K_a·|a_y|}的最大值，其中ω_{S_max},a_{S_max}分别为陀螺仪和加速度传感器静态测得的最大读数。

进一步，步骤4)中所述的四元素互补重构计算在对载体的姿态四元素计算值Q(t_k)和姿态四元素观测值进行融合时，考虑了载体运动状态信息，利用互补重构参数α融合计算值和观测值得到新的载体姿态四元素。优选的，其融合公式如下所示：

$Q^{\′}\left(t_k\right)=\mathrm{\α}\·Q\left(t_k\right)+(1-\mathrm{\α})\·\hat{Q}\left(t_k\right)---\left(12\right)$

并对融合后的载体姿态四元素进行标准归一化处理。

本发明所提出的基于互补重构技术的多传感器姿态融合方法，除了传统四元素姿态测量方法所具备的特点外，还具有如下几个特点：1)由于引入了多传感器的信息，使得传统姿态测量方法的积分累积误差得到及时修正，保证了惯性导航系统的长时间精确性；2)利用多传感器信息重新构造了载体姿态四元素的观测值，用于对载体的姿态四元素进行重构和归一化处理，使得载体姿态信息在静态时输出值为观测值；3)把四元素的更新算法和重构算法合二为一，提高了算法执行的效率。

本发明的优点是：在载体姿态可观测情况下，利用上一时刻四元素观测值修正上一时刻四元素迭代值，从而获得四元素重构值并利用此值进行当前时刻的四元素迭代更新，消除了算法的累积误差，保证惯性导航系统的长时间精确性；本发明较好解决了惯性导航系统在利用单一陀螺仪传感器进行姿态测量时的计算结果随时间漂移的问题，可以比较全面的利用不同传感器的特性，得到载体姿态四元素的计算值和观测值，根据不同传感器的数据判定载体运动状态，实现不同传感器表征的载体姿态四元素的互补重构，使得惯性导航系统在长时间运行时具有较高的导航精度。

附图说明

图1为本发明的原理图。

图2为本发明的总体流程图。

图3为本发明的步骤1的流程图。

图4为本发明的步骤2的流程图。

图5为本发明的步骤3的流程图。

图6为本发明公开的计算观测传感器(加速度传感器)包含的载体姿态信息示意图。

具体实施方式

下面参照附图，详细说明本发明的具体实施方式。

实施案例

一种基于互补重构技术的多传感器姿态融合方法，具体实施步骤如下所示：

1)、载体姿态信息计算；利用陀螺仪传感器的数据,根据旋转矢量算法计算四元素迭代更新矩阵M'[q(h)]，并根据下式：

Q(t_k)＝M′[q(h)]·Q′(t_k-1)     (13)

更新四元素值Q(t_k)，式中Q(t_k)为当前时刻四元素迭代值,M'[q(h)]为四元素更新矩阵,Q'(t_k-1)为上一时刻四元素重构值；并根据更新后的四元素计算值Q(t_k)计算得到载体姿态信息，分别是航向角俯仰角θ以及横滚角γ，具体实施步骤如下：

(1.1)角速率数据获取与预处理；获取陀螺仪传感器的数据，并对其进行直流偏置修正处理，处理过程如下所示；

ω_x＝ω_{x_org}-ω_{x_avg},ω_y＝ω_{y_org}-ω_{y_avg},ω_z＝ω_{z_org}-ω_{z_avg}    (14)

其中，ω_{x_avg},ω_{y_avg},ω_{z_avg}表示预先采样若干次并计算其均值作为三轴角速率偏置值；ω_{x_org},ω_{y_org},ω_{z_org}分别表示陀螺仪当前采样的三轴角速率值；ω_x,ω_y,ω_z为修正后的三轴角速率输出值。

(1.2)四元素更新矩阵计算；根据步骤(1.1)中获得的陀螺仪传感器的数据计算四元素姿态更新矩阵M'[q(h)]。

优选的，为了更好的解决高动态环境下的载体姿态更新的精度问题，本发明中的四元素迭代更新算法采用双子样旋转矢量算法求解姿态更新矩阵，求解过程如下：

$Q\left(t_k\right)=Q^{\′}\left(t_{k-1}\right)\⊗q\left(h\right)=\left[\begin{array}{cccc}{q}_0\left(h\right)& -q_1\left(h\right)& -q_2\left(h\right)& -q_3\left(h\right)\\ q_1\left(h\right)& q_0\left(h\right)& -q_3\left(h\right)& q_2\left(h\right)\\ q_2\left(h\right)& q_3\left(h\right)& q_0\left(h\right)& -q_1\left(h\right)\\ q_3\left(h\right)& -q_2\left(h\right)& q_1\left(h\right)& q_0\left(h\right)\end{array}\right]\·Q^{\′}\left(t_{k-1}\right)---\left(15\right)$

其中Q(t_k)为当前载体姿态四元素的值，Q'(t_k-1)为上一次载体所在位置的姿态四元素值(Q'(t₀)为给定的四元素初值)，q(h)为当前位置与上一次载体所处位置的旋转矢量，

$q\left(h\right)=\left[\begin{array}{c}{q}_0\left(h\right)\\ q_1\left(h\right)\\ q_2\left(h\right)\\ q_3\left(h\right)\end{array}\right]---\left(16\right)$

其中 $q_0\left(h\right)=\cos \frac{\mathrm{\Φ}}{2},q_1\left(h\right)=\frac{{\mathrm{\Φ}}_x\left(h\right)}{\mathrm{\Φ}}\sin \frac{\mathrm{\Φ}}{2},q_2\left(h\right)=\frac{{\mathrm{\Φ}}_y\left(h\right)}{\mathrm{\Φ}}\sin \frac{\mathrm{\Φ}}{2},q_3\left(h\right)=\frac{{\mathrm{\Φ}}_z\left(h\right)}{\mathrm{\Φ}}\sin \frac{\mathrm{\Φ}}{2}.$ Φ(h)为前一次采样时刻和当前采样时刻[t_k,t_k+1]时间段内的等效旋转矢量，而Φ_x(h),Φ_y(h),Φ_z(h)为Φ(h)的三轴分量， $\mathrm{\Φ}=\left|\mathrm{\Φ}\left(h\right)\right|=\sqrt{{\mathrm{\Φ}}_x^2\left(h\right)+{\mathrm{\Φ}}_y^2\left(h\right)+{\mathrm{\Φ}}_z^2\left(h\right)},$ $\mathrm{\Φ}\left(h\right)=\left[\begin{array}{c}{\mathrm{\Φ}}_x\left(h\right)\\ {\mathrm{\Φ}}_y\left(h\right)\\ {\mathrm{\Φ}}_z\left(h\right)\end{array}\right]={\mathrm{\Δ\θ}}_1+{\mathrm{\Δ\θ}}_2+\frac{2}{3}{\mathrm{\Δ\θ}}_1\×{\mathrm{\Δ\θ}}_2,$ h＝t_k+1-t_k为当前采样和上一次采样的时间间隔。Φ(h)表达式中，Δθ₁,Δθ₂分别为和时间段内的载体的三轴角增量组成的向量，由陀螺仪传感器获得。

(1.3)四元素迭代更新；根据步骤4)中互补重构后的四元素值Q'(t_k-1)，按照下式更新姿态四元素的值：

Q(t_k)＝M′[q(h)]·Q′(t_k-1)     (17)

其中，Q(t_k)为当前四元素迭代值，M'[q(h)]为步骤(1.2)中计算的四元素更新矩阵，Q'(t_k-1)为上一次重构的四元素值，特殊的，Q'(t₀)为给定的模值为1的四元素初始值。

(1.4)姿态信息计算；根据更新后的四元素迭代值Q(t_k)，按照下式计算载体姿态信息数值，分别是航向角俯仰角θ以及横滚角γ：

其中，q₀，q₁，q₂，q₃为四元素迭代值Q(t_k)的分量。

2)、载体姿态信息观测；获取观测传感器数据，计算载体姿态信息的观测值，分别是航向角俯仰角以及横滚角并根据载体姿态信息的观测值计算四元素的观测值，具体实施步骤如下：

(2.1)加速度数据获取与预处理单元401获取加速度传感器的数据a_{x_org},a_{y_org},a_{z_org}，进行直流偏置修正，具体处理方式如下所示；

a_x＝a_{x_org}-a_{x_avg},a_y＝a_{y_org}-a_{y_avg},a_z＝a_{z_org}-a_{z_avg}     (19)

其中，表示预先采样计算的三轴加速度计直流偏置值，a_{x_org},a_{y_org},a_{z_org}为三轴加速度计当前的采样值；a_x,a_y,a_z修正后的三轴加速度输出值。

(2.2)载体姿态信息观测值计算；根据附图6所示力学原理计算静态情况下的观测传感器(加速度传感器)包含的载体姿态信息，其中；对上述载体姿态信息进行滤波，其主要滤波方法有均值滤波，一阶惯性滤波，中值滤波，kalman滤波等方法；获得加速度传感器包含的载体姿态信息获取磁阻传感器数据，数据处理后得到载体航向角信息

优选的，计算得到加速度传感器表征的姿态信息θ',γ'后，以之作为观测值，以陀螺仪传感器的数据作为预测值，对θ',γ'进行Kalman滤波，得到滤波后的加速度传感器包含的姿态信息的观测值以此可以提高惯性导航系统的精度。

(2.3)姿态矩阵观测值计算；根据载体姿态信息的观测值计算载体姿态矩阵的观测值，具体计算公式如下所示：

(2.4)四元素观测值计算；根据姿态矩阵的观测值计算四元素的观测值，具体计算公式如下：

$\left\{\begin{array}{c}\left|{\hat{q}}_0\right|=\frac{1}{2}\sqrt{1+{\hat{T}}_{11}+{\hat{T}}_{22}+{\hat{T}}_{33}}\\ \left|{\hat{q}}_1\right|=\frac{1}{2}\sqrt{1+{\hat{T}}_{11}-{\hat{T}}_{22}-{\hat{T}}_{33}}\\ \left|{\hat{q}}_2\right|=\frac{1}{2}\sqrt{1-{\hat{T}}_{11}+{\hat{T}}_{22}-{\hat{T}}_{33}}\\ \left|{\hat{q}}_3\right|=\frac{1}{2}\sqrt{1-{\hat{T}}_{11}-{\hat{T}}_{22}+{\hat{T}}_{33}}\\ 4{\hat{q}}_0{\hat{q}}_1={\hat{T}}_{32}-{\hat{T}}_{23}\\ 4{\hat{q}}_0{\hat{q}}_2={\hat{T}}_{13}-{\hat{T}}_{31}\\ 4{\hat{q}}_0{\hat{q}}_3={\hat{T}}_{21}-{\hat{T}}_{12}\end{array}\right.---\left(21\right)$

其中，的符号由式 $\left\{\begin{array}{c}\mathrm{sign}\left({\hat{q}}_1\right)=\mathrm{sign}\left({\hat{q}}_0\right)\mathrm{sign}\left[({\hat{T}}_{32}-{\hat{T}}_{23})\right]\\ \mathrm{sign}\left({\hat{q}}_2\right)=\mathrm{sign}\left({\hat{q}}_0\right)\mathrm{sign}\left[({\hat{T}}_{13}-{\hat{T}}_{31})\right]\\ \mathrm{sign}\left({\hat{q}}_3\right)=\mathrm{sign}\left({\hat{q}}_0\right)\mathrm{sign}\left[({\hat{T}}_{21}-{\hat{T}}_{12})\right]\end{array}\right.$ 确定，的符号可任取。

优选的，根据发明内容中的步骤可以获得载体的姿态四元素的观测值。具体计算时，由于四元素观测值和计算值表征同一刚体的旋转，我们确定的符号为与步骤1)中计算得到的载体姿态四元素计算值的q₀值符号相同。

3)、互补参数计算；根据不同传感器的数据，判断和估计载体的运动状态，计算数据融合的互补重构参数α＝g(x₁,x₂,x₃,...)(其中x₁,x₂,x₃,...为各个传感器的数据)，具体计算步骤如下：

(3.1)互补参数计算数据获取；收集陀螺仪传感器的数据ω_x,ω_y,ω_z，再收集加速度传感器的数据a_x,a_y，并进行数据处理，由于a_z在重力方向有一个常值，以此a_z的值不能作为判断载体运行状态的数据。

优选的，对传感器的数据处理中，采用如下处理方法：对陀螺仪传感器数据取绝对值并乘以一个给定系数K_ω得到K_ω·|ω_x|,K_ω·|ω_y|,K_ω·|ω_z|，对加速度传感器数据取绝对值并乘以一个给定系数K_a，得到K_a·|a_x|,K_a·|a_y|，给定系数K_ω，K_a由如下表达式确定：

$K_{\mathrm{\ω}}=\frac{1}{{\mathrm{\ω}}_{S\_\max }},K_a=\frac{1}{a_{S\_\max }}---\left(22\right)$

其中ω_{S_max},a_{S_max}分别为陀螺仪和加速度传感器静态测得的最大读数。

(3.2)互补参数计算；计算互补重构参数α＝g(x₁,x₂,x₃,...)，x₁,x₂,x₃,...为各传感器数据。α的值表征载体的运动状态，α→1则表明载体的运动比较剧烈可认定为载体处在高速运动状态，α→0则表明载体运动不剧烈，可以认为载体趋于静止状态。

优选的，互补重构参数计算时须首先计算运动程度衡量参数max，max为如下集合{K_ω·|ω_x|,K_ω·|ω_y|,K_ω·|ω_z|,K_a·|a_x|,K_a·|a_y|}的最大值，其具体计算函数如下：

max＝max{K_ω·|ω_x|,K_ω·|ω_y|,K_ω·|ω_z|,K_a·|a_x|,K_a·|a_y|}     (23)

然后计算数据融合的互补重构参数α＝max/(max+1)。

4)、四元素互补重构；根据步骤3)中的数据融合的互补重构参数α，对步骤1)中得到的Q(t_k)和步骤2)中得到的进行数据融合，重构载体姿态四元素，其表达式为标准归一化处理后作为下一时刻姿态更新的载体姿态四元素。

优选的，对载体姿态四元素的计算值Q(t_k)和观测值进行融合时，采用重构算法实现，其系数α由不同传感器数据计算确定，表征载体的运动状态，具体融合的公式如下所示：

$Q^{\′}\left(t_k\right)=\mathrm{\α}\·Q\left(t_k\right)+(1-\mathrm{\α})\·\hat{Q}\left(t_k\right)---\left(24\right)$

对融合后的载体姿态四元素进行标准归一化处理得到Q(t_k)将作为下一次迭代运算的载体姿态四元素值。

本说明书实施例所述的内容仅仅是对发明构思的实现形式的列举，本发明的保护范围不应当被视为仅限于实施例所陈述的具体形式，本发明的保护范围也及于本领域技术人员根据本发明构思所能够想到的等同技术手段。

Claims (5)

1.基于互补重构技术的多传感器姿态融合方法，包括以下步骤：

1)、利用陀螺仪传感器的数据，根据旋转矢量算法计算四元素迭代更新矩阵M'[q(h)]，并根据下式：

Q(t_k)＝M′[q(h)]·Q′(t_k-1)

更新四元素值Q(t_k)，式中Q(t_k)为当前时刻四元素迭代值,M'[q(h)]为四元素更新矩阵,Q'(t_k-1)为上一时刻四元素重构值；根据更新后的四元素计算值Q(t_k)，计算得到载体姿态信息，分别是航向角俯仰角θ以及横滚角γ；

2)、利用观测传感器数据，计算载体姿态信息的观测值，分别是航向角俯仰角以及横滚角并进一步计算姿态四元素的观测值

3)、利用不同传感器的数据，判断和估计载体的运动状态，计算数据融合的互补重构参数α＝g(x₁,x₂,x₃,...)，其中x₁,x₂,x₃,...为各个传感器的数据；

4)、根据步骤3)中的数据融合的互补重构参数α，对步骤1)中得到的Q(t_k)和步骤2)中得到的进行数据融合，重构载体姿态四元素Q'(t_k)，其表达式为并对其进行标准化处理后用于下一次迭代更新，并返回步骤1)。

2.根据权利要求1所述的方法，其特征在于：在步骤1)所述计算载体的姿态信息时，使用了旋转矢量算法对载体姿态信息进行了迭代更新，步骤1)中的载体的姿态信息的计算包括以下步骤：

(1.1)获取陀螺仪传感器的数据，并对其进行直流偏置修正处理。先预先采样若干次计算其均值作为三轴角速率的偏置值，再分别以三轴角速率的陀螺仪当前采样值减去该偏置值作为修正后的三轴角速率的输出值；

(1.2)根据权利要求书2所述步骤1)中获得的陀螺仪传感器的数据计算四元素更新矩阵M'[q(h)]；

$M^{\′}\left[q\left(h\right)\right]=\left[\begin{array}{cccc}{q}_0\left(h\right)& {-q}_1\left(h\right)& -q_2\left(h\right)& -q_3\left(h\right)\\ q_1\left(h\right)& q_0\left(h\right)& -q_3\left(h\right)& q_2\left(h\right)\\ q_2\left(h\right)& q_3\left(h\right)& q_0\left(h\right)& {-q}_1\left(h\right)\\ q_3\left(h\right)& {-q}_2\left(h\right)& q_1\left(h\right)& q_0\left(h\right)\end{array}\right]$

其中 $q_0\left(h\right)=\cos \frac{\mathrm{\Φ}}{2},q_1\left(h\right)=\frac{{\mathrm{\Φ}}_x\left(h\right)}{\mathrm{\Φ}}\sin \frac{\mathrm{\Φ}}{2},q_2\left(h\right)=\frac{{\mathrm{\Φ}}_y\left(h\right)}{\mathrm{\Φ}}\sin \frac{\mathrm{\Φ}}{2},q_3\left(h\right)=\frac{{\mathrm{\Φ}}_z\left(h\right)}{\mathrm{\Φ}}\sin \frac{\mathrm{\Φ}}{2}.$ Φ(h)为前一次采样时刻和当前采样时刻[t_k,t_k+1]时间段内的等效旋转矢量，Φ_x(h),Φ_y(h),Φ_z(h)为的旋转矢量三轴的分量；

(1.3)根据步骤4)中互补重构后的四元素值Q'(t_k-1)，按照下式更新姿态四元素的值：

Q(t_k)＝M′[q(h)]·Q′(t_k-1)

其中，Q'(t₀)为给定的四元素初始值；

(1.4)根据更新后的四元素迭代值Q(t_k)，按照下式计算载体姿态信息，分别是航向角俯仰角θ以及横滚角γ：

其中，q₀，q₁，q₂，q₃为四元素Q(t_k)的分量。

3.根据权利要求1所述的方法，其特征在于：在所述步骤2)计算载体的姿态信息观测值时，使用观测传感器的信息进行载体姿态信息观测值的计算，并根据载体姿态信息观测值计算得到四元素的观测值步骤2)中的载体的姿态信息观测值的计算包括以下步骤：

(2.1)获取加速度传感器的数据a_{x_org},a_{y_org},a_{z_org}，进行直流偏置修正。首先采样若干次计算其均值作为三轴加速度的偏置值，再分别以三轴加速度的当前采样值减去该偏置值作为修正后的三轴加速度输出值；

(2.2)根据力学原理计算静态情况下的观测传感器(加速度传感器)包含的载体姿态信息，其中；对上述载体姿态信息进行滤波，获得加速度传感器包含的载体姿态信息同时获取磁阻传感器数据，数据处理后得到载体航向角信息

(2.3)根据权利要求书3所述步骤2)中载体姿态信息的观测值按计算载体姿态矩阵元素的观测值，具体计算公式如下：

(2.4)根据权利要求书3所述步骤3)中载体姿态矩阵的观测值计算载体姿态四元素的观测值的各个分量，具体计算公式如下：

$\left\{\begin{array}{c}\left|{\hat{q}}_0\right|=\frac{1}{2}\sqrt{1+{\hat{T}}_{11}+{\hat{T}}_{22}+{\hat{T}}_{33}}\\ \left|{\hat{q}}_1\right|=\frac{1}{2}\sqrt{1+{\hat{T}}_{11}-{\hat{T}}_{22}-{\hat{T}}_{33}}\\ \left|{\hat{q}}_2\right|=\frac{1}{2}\sqrt{1-{\hat{T}}_{11}+{\hat{T}}_{22}-{\hat{T}}_{33}}\\ \left|{\hat{q}}_3\right|=\frac{1}{2}\sqrt{1-{\hat{T}}_{11}-{\hat{T}}_{22}+{\hat{T}}_{33}}\\ 4{\hat{q}}_0{\hat{q}}_1={\hat{T}}_{32}-{\hat{T}}_{23}\\ 4{\hat{q}}_0{\hat{q}}_2={\hat{T}}_{13}-{\hat{T}}_{31}\\ 4{\hat{q}}_0{\hat{q}}_3={\hat{T}}_{21}-{\hat{T}}_{12}\end{array}\right.$

其中，的符号由式 $\left\{\begin{array}{c}\mathrm{sign}\left({\hat{q}}_1\right)=\mathrm{sign}\left({\hat{q}}_0\right)\mathrm{sign}\left[({\hat{T}}_{32}-{\hat{T}}_{23})\right]\\ \mathrm{sign}\left({\hat{q}}_2\right)=\mathrm{sign}\left({\hat{q}}_0\right)\mathrm{sign}\left[({\hat{T}}_{13}-{\hat{T}}_{31})\right]\\ \mathrm{sign}\left({\hat{q}}_3\right)=\mathrm{sign}\left({\hat{q}}_0\right)\mathrm{sign}\left[({\hat{T}}_{21}-{\hat{T}}_{12})\right]\end{array}\right.$ 确定，的符号可任取。

4.根据权利要求1所述的方法，其特征在于：步骤3)所述根据多传感器的数据，构造并计算得出表征载体运动状态信息的互补重构参数α，其意义在于，α的值表示载体的运动状态，α→1则表明载体的运动比较剧烈可认为载体处在高速运动状态，α→0则表明载体运动不剧烈，可以认为载体趋于静止状态；步骤3)中的计算表征载体运动状态的互补重构参数α包含以下步骤：

(3.1)获得陀螺仪传感器的数据ω_x,ω_y,ω_z，再获得加速度传感器的数据a_x,a_y，并进行数据处理，由于a_z在重力方向有一个常值，以此a_z的值不能作为判断载体运行状态的数据；

(3.2)计算互补重构参数α＝g(x₁,x₂,x₃,...)，x₁,x₂,x₃,...为各传感器数据。

优选的，计算互补重构参数数学表达式如下所示：α＝max/(max+1)；其中,max是集合{K_ω·|ω_x|,K_ω·|ω_y|,K_ω·|ω_z|,K_a·|a_x|,K_a·|a_y|}的最大值，其中ω_{S_max},a_{S_max}分别为陀螺仪和加速度传感器静态测得的最大读数。

5.根据权利要求1所述的方法，其特征在于：步骤4)所述的对载体的姿态四元素计算值Q(t_k)和姿态四元素观测值进行融合时，考虑了载体运动状态信息，利用互补重构参数α融合计算值和观测值得到新的载体姿态四元素，融合公式如下所示：

$Q^{\′}\left(t_k\right)=\mathrm{\α}\·Q\left(t_k\right)+(1-\mathrm{\α})\·\hat{Q}\left(t_k\right)$

并对融合后的载体姿态四元素进行标准归一化处理。