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

摘要

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

主权项

基于互补重构技术的多传感器姿态融合方法，包括以下步骤：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)；在步骤1)计算载体的姿态信息时，使用了旋转矢量算法对载体姿态信息进行了迭代更新，步骤1)中的载体的姿态信息的计算包括以下步骤：(1.1)获取陀螺仪传感器的数据，并对其进行直流偏置修正处理；先预先采样若干次计算其均值作为三轴角速率的偏置值，再分别以三轴角速率的陀螺仪当前采样值减去该偏置值作为修正后的三轴角速率的输出值；(1.2)根据所述步骤(1.1)中获得的陀螺仪传感器的数据计算四元素更新矩阵M'[q(h)]；$M^{\prime }\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]$其中Φ(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)的分量；在所述步骤2)计算载体的姿态信息观测值时，使用观测传感器的信息进行载体姿态信息观测值的计算，并根据载体姿态信息观测值计算得到四元素的观测值步骤2)中的载体的姿态信息观测值的计算包括以下步骤：(2.1)获取加速度传感器的数据a_{x_org},a_{y_org},a_{z_org}，进行直流偏置修正；首先采样若干次计算其均值作为三轴加速度的偏置值，再分别以三轴加速度的当前采样值减去该偏置值作为修正后的三轴加速度输出值；(2.2)根据力学原理计算静态情况下的观测传感器包含的载体姿态信息，其中对上述载体姿态信息进行滤波，获得加速度传感器包含的载体姿态信息同时获取磁阻传感器数据，数据处理后得到载体航向角信息(2.3)根据所述步骤(2.2)中载体姿态信息的观测值计算载体姿态矩阵元素的观测值，具体计算公式如下：(2.4)根据所述步骤(2.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.$其中，的符号由式确定，的符号可任取；步骤3)根据多传感器的数据，构造并计算得出表征载体运动状态信息的互补重构参数α，α的值表示载体的运动状态，α→1则表明载体的运动比较剧烈可认为载体处在高速运动状态，α→0则表明载体运动不剧烈，可以认为载体趋于静止状态；步骤3)中的计算表征载体运动状态的互补重构参数α包含以下步骤：(3.1)数据获取单元获得陀螺仪传感器的数据ω_x,ω_y,ω_z，再获得加速度传感器的数据a_x,a_y，并进行数据处理，由于a_z在重力方向有一个常值，因此a_z的值不能作为判断载体运行状态的数据；(3.2)参数计算单元计算互补重构参数α＝max/(max+1)，x₁,x₂,x₃,...为各传感器数据；其中,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^{\prime }\left(t_k\right)=\alpha ·Q\left(t_k\right)+(1-\alpha )·\hat{Q}\left(t_k\right)$并对融合后的载体姿态四元素进行标准归一化处理。