基于互补重构技术的多传感器姿态融合方法
【专利摘要】基于互补重构技术的多传感器姿态融合方法，主要包括：四元素迭代更新及载体姿态计算，多传感器载体姿态观测及四元素观测值计算；互补重构参数估计；互补重构四元素值计算等四个步骤。其主要优点：利用四元素观测值修正上一时刻四元素计算值，从而获得互补重构的四元素值，并利用此值进行当前时刻的四元素迭代更新，从机理上消除了算法的累积误差；同时将四元素更新算法和重构算法合二为一，提高了算法的执行效率。
【专利说明】基于互补重构技术的多传感器姿态融合方法
【技术领域】
[0001] 本发明涉及惯性导航领域，尤其是捷联惯性导航系统在利用单一惯性传感器（如 陀螺仪传感器）数据进行载体姿态测量时，虽然短时间精度好，但是长期运行时的姿态测 量结果由于积分作用产生累积误差，导致导航系统精度下降。本发明利用多传感器信息进 行数据融合，可以用较低成本提高捷联惯性导航系统长时间精度。
【背景技术】
[0002] 捷联惯性导航系统在计算时，需要把载体测得的加速度，角速度变化从载体坐标 系转换到导航坐标系中，假设空间位置P在导航坐标系η的坐标表示为P (xn，yn，zn)，在载体 所在坐标系b的坐标表示为P (xb，yb，zb)，则两者之间满足如下关系：
【权利要求】
1. 基于互补重构技术的多传感器姿态融合方法，包括以下步骤： 1) 、利用陀螺仪传感器的数据，根据旋转矢量算法计算四元素迭代更新矩阵M' [q(h)]， 并根据下式： Q(tk) = M' [q(h)] · Q' （tj^) 更新四元素值Q(tk)，式中Q(tk)为当前时刻四元素迭代值，M' [q(h)]为四元素更新矩 阵，Q'（tg)为上一时刻四元素重构值；根据更新后的四元素计算值Q(tk)，计算得到载体姿 态信息，分别是航向角俯仰角Θ以及横滚角Y ; 2) 、利用观测传感器数据，计算载体姿态信息的观测值，分别是航向角#，俯仰角|以 及横滚角^并进一步计算姿态四元素的观测值; 3) 、利用不同传感器的数据，判断和估计载体的运动状态，计算数据融合的互补重构参 数a = g (Χι, χ2, χ3,...)，其中Χι, χ2, χ3,...为各个传感器的数据； 4) 、根据步骤3)中的数据融合的互补重构参数α，对步骤1)中得到的Q(tk) 和步骤2)中得到的进行数据融合，重构载体姿态四元素 Q'（tk)，其表达式为 ))，并对其进行标准化处理后用于下一次迭代更新，并返回步骤1)。
2. 根据权利要求1所述的方法，其特征在于：在步骤1)所述计算载体的姿态信息时， 使用了旋转矢量算法对载体姿态信息进行了迭代更新，步骤1)中的载体的姿态信息的计 算包括以下步骤： (1. 1)获取陀螺仪传感器的数据，并对其进行直流偏置修正处理。先预先采样若干次计 算其均值作为三轴角速率的偏置值，再分别以三轴角速率的陀螺仪当前采样值减去该偏置 值作为修正后的三轴角速率的输出值； (1. 2)根据权利要求书2所述步骤1)中获得的陀螺仪传感器的数据计算四元素更新矩 阵 M，[q(h)];
Φ00为前一次采样时刻和当前采样时刻[tk，tk+1]时间段内的等效旋转矢量， Φχ〇ι)，Φ>)，Φζ〇ι)为的旋转矢量三轴的分量； (1.3) 根据步骤4)中互补重构后的四元素值Q'（tu)，按照下式更新姿态四元素的值： Q(tk) = M' [q(h)] · Q' （tj^) 其中，Q'为给定的四元素初始值； (1.4) 根据更新后的四元素迭代值Q(tk)，按照下式计算载体姿态信息，分别是航向角 炉，俯仰角Θ以及横滚角 Y :
其中，qQ，qi，q2, q3为四元素 Q(tk)的分量。
3.根据权利要求1所述的方法，其特征在于：在所述步骤2)计算载体的姿态信息观测 值时，使用观测传感器的信息进行载体姿态信息观测值的计算，并根据载体姿态信息观测 值计算得到四元素的观测值步骤2)中的载体的姿态信息观测值的计算包括以下步 骤： (2. 1)获取加速度传感器的数据ax_OTg，ay_OTg，a z_OTg，进行直流偏置修正。首先采样若干 次计算其均值作为三轴加速度的偏置值，再分别以三轴加速度的当前采样值减去该偏置值 作为修正后的三轴加速度输出值； (2.2) 根据力学原理计算静态情况下的观测传感器（加速度传感器）包含的载体姿 态信息，其中
：寸上述载体姿态信息进行滤波，获得加速度传 感器包含的载体姿态信息氣^同时获取磁阻传感器数据，数据处理后得到载体航向角信息 ΦI (2.3) 根据权利要求书3所述步骤2)中载体姿态信息的观测值忒么#，按计算载体姿 态矩阵元素的观测值，具体计算公式如下：
(2. 4)根据权利要求书3所述步骤3)中载体姿态矩阵的观测值计算载体姿态四元素的 观测值0? >的各个分量，具体计算公式如下：
4. 根据权利要求1所述的方法，其特征在于：步骤3)所述根据多传感器的数据，构造 并计算得出表征载体运动状态信息的互补重构参数α，其意义在于，α的值表示载体的运 动状态，α -1则表明载体的运动比较剧烈可认为载体处在高速运动状态，α -〇则表明 载体运动不剧烈，可以认为载体趋于静止状态；步骤3)中的计算表征载体运动状态的互补 重构参数α包含以下步骤： (3. 1)获得陀螺仪传感器的数据ωχ，coy，ωζ，再获得加速度传感器的数据^，\，并进 行数据处理，由于az在重力方向有一个常值，以此az的值不能作为判断载体运行状态的数 据； (3. 2)计算互补重构参数a = g(Xl, χ2, χ3, . . . )，Χι, χ2, χ3,...为各传感器数据。 优选的，计算互补重构参数数学表达式如下所示：ct = max/ (max+1);其中，max 是集合{Κω · I ωχ|，Κω · I ω」，Κω · I c〇z|，Ka· |ax|，Ka· |ay|}的最大值，其中
?s__，分别为陀螺仪和加速度传感器静态测得的最大读 数。
5. 根据权利要求1所述的方法，其特征在于：步骤4)所述的对载体的姿态四元素计算 值Q(tk)和姿态四元素观测值0(?)进行融合时，考虑了载体运动状态信息，利用互补重构 参数α融合计算值和观测值得到新的载体姿态四元素，融合公式如下所示：
并对融合后的载体姿态四元素进行标准归一化处理。
【文档编号】G01C21/20GK104101345SQ201410247629
【公开日】2014年10月15日 申请日期:2014年6月5日 优先权日:2014年6月5日
【发明者】董利达, 尹俊, 迟天阳, 管林波 申请人:杭州师范大学