CN106153069A - 自主导航系统中的姿态修正装置和方法 - Google Patents

Abstract

本发明提出了一种自主导航系统中的姿态修正装置和方法，该装置包括：零速检测标志位后处理单元，对零速检测标志位进行后处理，以去除零速检测标志位中的高频误差，从而获得经后处理的零速区间和零速检测标志位；过程中姿态修正单元，用于利用经后处理获得的零速区间，对非零速区间的起始时刻的姿态信息进行修正。本发明不仅能够实时地去除零速检测中的高频误差，而且在非零速期间的起始时刻，利用加速度计的测量数据和磁力计的测量数据对姿态信息重新修正，进一步消除系统姿态误差，从而提高了系统精度。

Description

自主导航系统中的姿态修正装置和方法

技术领域

本发明涉及导航定位技术领域，具体涉及一种姿态修正装置和方法，其能够基于微机电系统(Micro Electro Mechanical System，简写为MEMS)惯性测量技术，对用于行人自主导航的捷联惯导系统的姿态进行修正。

背景技术

惯导系统(INS)主要分为平台式惯导系统和捷联式惯导系统两大类。惯导系统是一种不依赖于任何外部信息、也不向外部辐射能量的自主式导航系统，具有隐蔽性好，可在空中、地面、水下等各种复杂环境下工作的特点。捷联惯导系统(SINS)是在平台式惯导系统基础上发展而来的，它是一种无框架系统，由三个速率陀螺、三个线加速度计和微型计算机组成。平台式惯导系统和捷联式惯导系统的主要区别是：前者是有实体的物理平台，陀螺仪和加速度计置于稳定的平台上，该平台跟踪导航坐标系，以实现速度和位置解算，姿态数据直接取自于平台的环架；而后者的陀螺仪和加速度计直接固连在载体上作为测量基准，它不再采用机电平台，惯性平台的功能由计算机完成，即在计算机内建立一个数学平台取代机电平台的功能，其飞行器姿态数据通过计算机计算得到，故有时也称其为″数学平台″，这是捷联惯导系统与平台式惯导系统的重要区别。

在基于捷联惯导的行人导航系统中，姿态参数是行人导航的重要组成部分。捷联惯导系统通过提取MEMS陀螺仪、MEMS加速度计、MEMS磁力计的测量值实时进行姿态矩阵更新，从而获得载体的姿态信息。MEMS传感器是采用微电子和微机械加工技术制造出来的新型传感器。与传统的传感器相比，它具有体积小、重量轻、成本低、功耗低、可靠性高、适于批量化生产、易于集成和实现智能化的特点。同时，微米量级的特征尺寸使得它可以完成某些传统机械传感器所不能实现的功能。

捷联惯导系统采用了误差控制技术，其误差控制技术包括：测量前系统的初始对准；测量过程中实时的误差控制(例如：利用卡尔曼滤波进行零速修正)。测量过程中实时的误差控制对捷联惯导系统达到较高的定位精度起到很大的作用。在实时的误差控制中，捷联惯导系统主要采用两种姿态修正，即零速修正和测量点的位置修正。其中最经常采用的是零速修正，而测量点的位置修正只是在行进路线中个别的点进行。捷联惯导系统中姿态误差主要由陀螺仪的漂移所带来的积累误差而产生，往往利用零速修正进行实时的误差控制。

所谓零速修正，是使载体在运行中周期性的停止不动，此时载体的速度为零。但由于陀螺仪的漂移而产生姿态误差以及加速度计本身的误差，因此在载体不动时惯导系统总是存在一定的速度输出值，这就是零速误差。根据零速的大小，采用卡尔曼滤波对误差进行估计，并推算出由此造成的三轴位置、三轴姿态误差，然后将上述误差值输入至系统中进行补偿。这样就可以大大限制系统中姿态误差随时间而增长的特性，达到高精度的确定载体三轴位置的目的。

在每次基于卡尔曼滤波的零速误差修正之后，总会留下卡尔曼控制器不能完全消除的小量的剩余的姿态误差增量，这个姿态误差增量与上次零速修正间隔的时间成正比。在非零速期间，对存在姿态误差增量的姿态矩阵进行迭代更新，则姿态矩阵的误差进一步增大，同时在非零速期没有办法进行误差控制，因此会引起非零速期定位精度恶化现象。

因此，需要一种姿态修正装置和方法，其能够克服现有技术的上述不足，在非零速期间的起始时刻，利用加速度计的测量数据和磁力计的测量数据对姿态参数重新对准，从而进一步消除系统姿态误差。

发明内容

为解决上述问题，本发明提出了一种自主导航系统中的姿态修正装置，包括：

惯性测量单元，用于输出MEMS三轴陀螺仪、MEMS三轴加速度计和MEMS三轴磁力计的测量数据；

初始姿态信息获取单元，用于根据所述MEMS三轴加速度计和所述MEMS三轴磁力计的测量数据，计算初始姿态信息；

姿态信息更新单元，用于根据所述MEMS三轴陀螺仪、MEMS三轴加速度计和MEMS三轴磁力计的测量数据，更新姿态信息；

导航状态向量估计单元，用于根据所述更新的姿态信息以及所述MEMS三轴加速度计的测量数据，估计自主导航系统的导航状态向量，所述导航状态向量包括位置向量、速度向量和姿态向量；

零速检测单元，用于根据所述MEMS三轴陀螺仪和所述MEMS三轴加速度计的测量数据，确定自主导航系统的零速区间并获得零速检测标志位；

零速检测标志位后处理单元，用于对所述零速检测标志位进行后处理，以去除零速检测标志位中的高频误差，从而获得经后处理的零速区间和零速检测标志位；

导航状态向量修正单元，用于利用所述经后处理的零速区间，使用卡尔曼滤波的零速误差校正器，修正所述导航状态向量；

过程中姿态修正单元，用于利用经后处理的零速区间，对非零速区间的起始时刻的姿态信息进行修正。

优选地，所述零速检测标志位后处理单元包括：

滑动窗口更新标志位设置单元，用于根据当前时刻的零速检测标志位以及前一时刻的零速检测标志位，设置当前时刻的滑动窗口更新标志位，其中，所述滑动窗口更新标志位指示是否对滑动窗口内的对应数值进行更新；以及

新标志位设置单元，用于根据当前时刻的滑动窗口更新标志位和当前时刻的零速检测标志位，设置当前时刻的新零速检测标志位。

优选地，如果所述滑动窗口更新标志位指示对滑动窗口进行更新，则所述新标志位设置单元通过根据当前时刻的零速检测标志位来更新滑动窗口内的对应数值并在整个滑动窗口更新完毕之后进行平滑滤波，并对滤波结果进行非线性处理，设置所述当前时刻的新零速检测标志位。

优选地，所述非线性处理的阈值范围为[0，1]。

优选地，所述过程中姿态修正单元包括：

静态修正姿态信息计算单元，用于利用经后处理的零速区间内MEMS三轴加速度计和MEMS三轴磁力计的测量数据，计算用于静态修正的姿态信息；

静态修正条件判定单元，用于根据经后处理的零速区间内MEMS三轴加速度计的测量数据，通过判定标准来判定所述用于静态修正的姿态信息是否能够为后续非零速区间的起始时刻提供姿态信息的修正；以及

姿态修正单元，用于在判定所述用于静态修正的姿态信息能够为后续非零速区间的起始时刻提供姿态信息的修正时，在后续非零速区间的起始时刻进行姿态信息修正。

优选地，所述静态修正姿态信息计算单元包括：

累加标志位设置单元，用于根据当前时刻的新零速检测标志位和前一时刻的新零速检测标志位，设置当前时刻的累加标志位；

静态姿态信息计算单元，用于根据所述累加标志位，计算静态三轴MEMS加速度计值和静态三轴MEMS磁力计值，从而计算用于静态修正的姿态信息。

优选地，所述判定标准定义为：

abs(sqrt(abs(f_u)*abs(f_u)+abs(f_v)*abs(f_v)+abs(abs(f w)*abs(f_w)))-g)＜th，

其中，f_u、f_v、f_w分别为零速区间内MEMS三轴加速度计的测量数据的均值，g为当地重力加速度，th为阈值。

优选地，所述阈值的范围为[0.0001m/s²，0.1m/s²]。

优选地，所述姿态修正单元通过将所述用于静态修正的姿态信息与更新的姿态信息进行线性组合运算，来实现所述姿态信息修正。

本发明还提出了一种自主导航系统中的姿态修正方法，包括：

惯性测量步骤，用于输出MEMS三轴陀螺仪、MEMS三轴加速度计和MEMS三轴磁力计的测量数据；

初始姿态信息获取步骤，用于根据所述MEMS三轴加速度计和所述MEMS三轴磁力计的测量数据，计算初始姿态信息；

姿态信息更新步骤，用于根据所述MEMS三轴陀螺仪、MEMS三轴加速度计和MEMS三轴磁力计的测量数据，更新姿态信息；

导航状态向量估计步骤，用于根据所述更新的姿态信息以及所述MEMS三轴加速度计的测量数据，估计自主导航系统的导航状态向量，所述导航状态向量包括位置向量、速度向量和姿态向量；

零速检测步骤，用于根据所述MEMS三轴陀螺仪和所述MEMS三轴加速度计的测量数据，确定自主导航系统的零速区间并获得零速检测标志位；

零速检测标志位后处理步骤，用于对所述零速检测标志位进行后处理，以去除零速检测标志位中的高频误差，从而获得经后处理的零速区间和零速检测标志位；

导航状态向量修正步骤，用于利用所述经后处理的零速区间，使用卡尔曼滤波的零速误差校正器，修正所述导航状态向量；

过程中姿态修正步骤，用于利用经后处理的零速区间，对非零速区间的起始时刻的姿态信息进行修正。

优选地，所述零速检测标志位后处理步骤包括：

滑动窗口更新标志位设置步骤，用于根据当前时刻的零速检测标志位以及前一时刻的零速检测标志位，设置当前时刻的滑动窗口更新标志位，其中，所述滑动窗口更新标志位指示是否对滑动窗口内的对应数值进行更新；以及

新标志位设置步骤，用于根据当前时刻的滑动窗口更新标志位和当前时刻的零速检测标志位，设置当前时刻的新零速检测标志位。

优选地，如果所述滑动窗口更新标志位指示对滑动窗口进行更新，则所述新标志位设置步骤通过根据当前时刻的零速检测标志位来更新滑动窗口内的对应数值并在整个滑动窗口更新完毕之后进行平滑滤波，并对滤波结果进行非线性处理，设置所述当前时刻的新零速检测标志位。

优选地，所述非线性处理的阈值范围为[0，1]。

优选地，所述过程中姿态修正步骤包括：

静态修正姿态信息计算步骤，用于利用经后处理的零速区间内MEMS三轴加速度计和MEMS三轴磁力计的测量数据，计算用于静态修正的姿态信息；

静态修正条件判定步骤，用于根据经后处理的零速区间内MEMS三轴加速度计的测量数据，通过判定标准来判定所述用于静态修正的姿态信息是否能够为后续非零速区间的起始时刻提供姿态信息的修正；以及

姿态修正步骤，用于在判定所述用于静态修正的姿态信息能够为后续非零速区间的起始时刻提供姿态信息的修正时，在后续非零速区间的起始时刻进行姿态信息修正。

优选地，所述静态修正姿态信息计算步骤包括：

累加标志位设置步骤，用于根据当前时刻的新零速检测标志位和前一时刻的新零速检测标志位，设置当前时刻的累加标志位；

静态姿态信息计算步骤，用于根据所述累加标志位，计算静态MEMS三轴加速度计值和静态MEMS三轴磁力计值，从而计算用于静态修正的姿态信息。

优选地，所述判定标准定义为：

abs(sqrt(abs(f_u)*abs(f_u)+abs(f_v)*abs(f_v)+abs(abs(f_w)*abs(f_w)))-g)＜th，

其中，f_u、f_v、f_w分别为零速区间内MEMS三轴加速度计的测量数据的均值，g为当地重力加速度，th为阈值。

优选地，所述阈值的范围为[0.0001m/s²，0.1m/s²]。

优选地，所述姿态修正步骤通过将所述用于静态修正的姿态信息与更新的姿态信息进行线性组合运算，来实现所述姿态信息修正。

在本发明的自主导航系统中的姿态修正装置和方法中，首先，通过零速检测标志位后处理解决了零速区间的判断结果有时不准确、不连续的问题。图1示出了零速检测标志位与零速检测后处理的标志位的比较图，从图中可以清楚地看出，在静止期零速检测标志位中存在高频误差，而通过本发明实施例的零速检测后处理，即，一系列逻辑判断、滤波处理、非线性处理，实时地去除零速检测中的高频误差，使其成为慢变信号。另外，在非零速期间的起始时刻，利用加速度计的测量数据和磁力计的测量数据对姿态信息重新修正，从而进一步消除系统姿态误差。本发明实施例的采用非零速期起始时刻的姿态修正，进一步修正了姿态参数，有效地提高轨迹解算精度。进而进一步解决了因姿态误差累积过大导致轨迹解算误差发散的问题。

附图说明

通过下面结合附图说明本发明的优选实施例，将使本发明的上述及其它目的、特征和优点更加清楚，其中：

图1示出了零速检测标志位与零速检测后处理的标志位的比较图；

图2示出了根据本发明示例实施例的自主导航系统中的姿态修正装置的框图；

图3示出了根据本发明示例实施例的自主导航系统中的姿态修正方法的流程图；

图4示出了图3所示姿态修正方法中零速检测标志位后处理步骤的具体流程图；

图5示出了图3所示姿态修正方法中静态修正姿态信息计算步骤的具体流程图。

具体实施方式

以下参照附图，对本发明的示例实施例进行详细描述，本发明不限于下述示例实施例。为了清楚描述本发明的基本思想，附图中仅示出了与本发明的技术方案密切相关的部件、功能或步骤，并且以下描述中省略了对已知技术、功能、部件或步骤的具体描述。

图2示出了根据本发明示例实施例的自主导航系统中的姿态修正装置的框图。姿态修正装置20包括：惯性测量单元201、初始姿态信息获取单元202、姿态信息更新单元203、导航状态向量估计单元204、零速检测单元205、零速检测标志位后处理单元206、导航状态向量修正单元207、过程中姿态修正单元208。

惯性测量单元201输出MEMS三轴陀螺仪、MEMS三轴加速度计和MEMS三轴磁力计的测量数据。例如，可以将惯性测量单元201固定在行人的脚上，手持PDA实时接收并存储行人运动时系统输出的测量信息，即系统上配置的MEMS三轴陀螺仪、MEMS三轴加速度计和MEMS三轴磁力计的测量数据。

初始姿态信息获取单元202根据MEMS三轴加速度计和MEMS三轴磁力计的测量数据，计算初始载体姿态信息。具体地，利用加速度计的测量数据来计算系统初始横滚角和俯仰角，结合根据加速度计计算出的横滚角/俯仰角，利用磁力计的测量数据计算初始航向角。

姿态信息更新单元203根据MEMS三轴陀螺仪、MEMS三轴加速度计和MEMS三轴磁力计的测量数据，更新姿态信息。在捷联惯导系统中，更新姿态信息的方法主要是四元数法，即求解四元数的微分方程。在本发明实施例中，角增量对应的采样时间间隔是相同的，因此采用四元数微分方程毕卡求解法的定时采样增量法。在该方法中，根据采样周期内角度变化的向量OMEGA(如以下公式)和前一时刻的四元数quat(k-1)，迭代更新得到当前采样时刻的四元数quat(k)。

$\begin{array}{c}\mathrm{OMEGA}=\underset{t\_l-1}{\overset{t\_k}{\∫}}\left[\begin{array}{c}0,-\mathrm{wx},-\mathrm{wy},-\mathrm{wz}\\ \mathrm{wx},0,\mathrm{wz},-\mathrm{wy}\\ \mathrm{wy},-\mathrm{wz},0,\mathrm{wx}\\ \mathrm{wz},\mathrm{wy},-\mathrm{wx},0\end{array}\right]\mathrm{dt}\\ \≈\left[\begin{array}{c}0,-\mathrm{\Δ\θx},-\mathrm{\Δ\θy},-\mathrm{\Δ\θz}\\ \mathrm{\Δ\θx},0,\mathrm{\Δ\θz},-\mathrm{\Δ\θy}\\ \mathrm{\Δ\θy},-\mathrm{\Δ\θz},0,\mathrm{\Δ\θx}\\ \mathrm{\Δ\θz},\mathrm{\Δ\θy},-\mathrm{\Δ\θz},0\end{array}\right]\end{array}$

其中wx,wy,wz为陀螺仪三轴测量值，Δθx,Δθy,Δθz为陀螺仪三轴在[t_k-1，t_k]采样时间间隔内的角增量，Δθ²＝Δθx²+Δθy²+Δθz²。

四元数迭代更新的公式如下

$\mathrm{quat}\left(k\right)=\{I*\cos \left(\frac{\mathrm{\Δ\θ}}{2}\right)+\mathrm{OMEGA}*\frac{\sin \frac{\mathrm{\Δ\θ}}{2}}{\mathrm{\Δ\θ}}\}*\mathrm{quat}(k-1)$

这样，在非零速期的起始时刻，其前一时刻的四元数quat(k-1)包含有姿态误差增量，在非零速期进行四元数迭代更新时，姿态误差增量会随时间积累到quat(k)。

导航状态向量估计单元204根据姿态信息更新单元203更新的姿态信息以及MEMS三轴加速度计的测量数据，估计自主导航系统的导航状态向量，导航状态向量包括位置向量、速度向量和姿态向量；

零速检测单元205根据MEMS三轴陀螺仪和MEMS三轴加速度计的测量数据，确定自主导航系统的零速区间并获得零速检测标志位。零速区间是指载体在运行中停止不动的一段时间，并在运行中周期性地出现，在该区间内载体的速度为零。零速检测标志位的设置用来指示零速区间或非零速区间。但由于陀螺仪的漂移而产生姿态误差以及加速度计本身的误差，因此在零速区间内惯导系统总是存在一定的速度输出值，这就是零速误差。需要对零速误差进行估计，并推算出由此造成的三轴位置、三轴姿态误差，然后将上述误差值输入至系统中进行补偿。因此对零速区间的判断的准确性对于零速误差估计有显著地影响。

为了解决零速区间判断有时不准确不连续的问题，即零速检测标志位中的高频误差，本发明的示例实施例提出了在惯导系统中增加了零速检测标志位后处理单元206，用于对零速检测标志位进行后处理，以去除零速检测标志位中的高频误差，从而获得经后处理的零速区间和新零速检测标志位。这种后处理涉及一系列逻辑判断、滤波处理、非线性处理，从而能够实时地去除零速检测中的高频误差，使其成为慢变信号。零速检测标志位后处理单元206中包括两个子单元，滑动窗口更新标志位设置单元2061和新标志位设置单元2062。下面对这两个子单元进行描述。

滑动窗口更新标志位设置单元2061根据当前时刻的零速检测标志位以及前一时刻的零速检测标志位，设置当前时刻的滑动窗口更新标志位。这里，滑动窗口更新标志位指示是否对滑动窗口内的对应数值进行更新。具体地，根据当前时刻的零速检测标志位以及当前时刻的零速检测标志位与前一时刻的零速检测标志位的差值，进行一系列逻辑判断，即上升沿判断、下降沿判断和零速区间判断，例如，参见图1中零速检测结果(标志位)曲线。根据以上判断结果来设置当前时刻的滑动窗口更新标志位。

新标志位设置单元2062根据当前时刻的滑动窗口更新标志位和当前时刻的零速检测标志位，设置当前时刻的新零速检测标志位。具体地，如果当前时刻的滑动窗口更新标志位指示对滑动窗口进行更新，则根据当前时刻的零速检测标志位来更新滑动窗口内的对应数值并在整个滑动窗口更新完毕之后进行平滑滤波，并对滤波结果进行非线性处理，从而设置当前时刻的新零速检测标志位。

在本示例实施例中，非线性处理的阈值范围可以设置为[0，1]。然而非线性处理的阈值范围不限于此，而是可以根据实际系统的需要来设置。

导航状态向量修正单元207利用经零速检测标志位后处理而获得的零速区间，使用卡尔曼滤波的零速误差校正器，采用输出校正的方式对导航状态向量进行修正。然而在每次基于卡尔曼滤波的零速误差修正之后，总会留下卡尔曼控制器不能完全消除的小量的剩余的姿态误差增量，这个姿态误差增量与上次零速修正间隔的时间成正比。在非零速期间，对存在姿态误差增量的姿态矩阵进行迭代更新，则姿态矩阵的误差进一步增大，同时在非零速期没有办法进行误差控制，因此会引起非零速期定位精度恶化现象。有鉴于此，本示例实施例提出了在系统中增加过程中姿态修正单元208来对运动过程中满足条件的多个非零速区间的起始时刻的姿态信息进行修正，从而进一步消除系统姿态误差。过程中姿态修正单元208包括三个子单元，即静态修正姿态信息计算单元2081、静态修正条件判定单元2082和姿态修正单元2083。以下对这三个子单元进行详细描述。

静态修正姿态信息计算单元2081利用经后处理的零速区间内MEMS三轴加速度计和MEMS三轴磁力计的测量数据，计算用于静态修正的姿态信息。静态修正姿态信息计算单元2081还包括累加标志位设置单元和静态姿态信息计算单元(图中未示出)。累加标志位设置单元用于根据当前时刻的新零速检测标志位和前一时刻的新零速检测标志位，设置当前时刻的累加标志位。累加标志位指示是否进行累加。具体地，首先根据当前时刻的新零速检测标志位与前一时刻的新零速检测标志位的差值，进行一系列逻辑判断，即上升沿判断、下降沿判断，例如，参见图1中零速检测结果(标志位)曲线。根据以上判断结果和前一时刻的累加标志位来设置当前时刻的累加标志位。

静态姿态信息计算单元根据当前时刻的累加标志位，计算静态三轴MEMS加速度计值和静态三轴MEMS磁力计值，从而计算用于静态修正的姿态信息。具体地，如果当前时刻的累加标志位指示进行累加，则分别对当前时刻的三轴加速度计的测量数据和当前时刻的三轴磁力计的测量数据进行累加，计算静态三轴加速度计值和静态三轴磁力计值，从而计算静态姿态信息(横滚角/俯仰角/航向角)。

静态修正条件判定单元2082根据经后处理的零速区间内MEMS三轴加速度计的测量数据，通过判定标准来判定用于静态修正的姿态信息是否能够为后续非零速区间的起始时刻提供姿态信息的修正。具体地，根据基本原理“惯性测量单元完全静止时，三轴加速度计的测量值的平方和的开方等于当前重力加速度值”，定义如下判定标准：

abs(sqrt(abs(f_u)*abs(f_u)+abs(f_v)*abs(f_v)+abs(abs(f_w)*abs(f_w)))-g)＜th，

其中，f_u、f_v、f_w分别为零速区间内MEMS三轴加速度计的测量数据的均值，g为当地重力加速度，th为阈值。优选地，该阈值th的范围为[0.0001m/s²，0.1m/s²]。然而阈值th不限于此，而是可以根据实际系统的需要来设置。

姿态修正单元2083在判定用于静态修正的姿态信息能够为后续非零速区间的起始时刻提供姿态信息的修正时，在后续非零速区间的起始时刻进行姿态信息修正。即，通过将用于静态修正的姿态信息与更新的姿态信息进行线性组合运算，来实现姿态信息修正。

因此，根据本发明示例实施例的姿态修正装置不仅通过实时地去除零速检测中的高频误差而解决了零速区间判断有时不准确、不连续的问题，而且，在非零速期间的起始时刻，利用加速度计的测量数据和磁力计的测量数据对姿态信息重新修正，进一步消除系统姿态误差，从而提高了系统精度。

图3示出了根据本发明示例实施例的自主导航系统中的姿态修正方法的流程图。在①惯性测量步骤，输出MEMS三轴陀螺仪、MEMS三轴加速度计和MEMS三轴磁力计的测量数据，即通过惯性测量单元输出测量信息。在②初始姿态信息获取步骤，根据MEMS三轴加速度计和MEMS三轴磁力计的测量数据，计算初始姿态信息。在③姿态信息更新步骤，根据MEMS三轴陀螺仪、MEMS三轴加速度计和MEMS三轴磁力计的测量数据，更新姿态信息。在④导航状态向量估计步骤，根据更新的姿态信息以及MEMS三轴加速度计的测量数据，估计自主导航系统的导航状态向量。导航状态向量包括位置向量、速度向量和姿态向量。在⑤零速检测步骤，根据MEMS三轴陀螺仪和MEMS三轴加速度计的测量数据，确定自主导航系统的零速区间并获得零速检测标志位，即进行零速检测。在⑥零速检测标志位后处理步骤，对零速检测标志位进行后处理，以去除零速检测标志位中的高频误差，从而获得经后处理的零速区间和零速检测标志位。在⑦静态修正姿态信息计算步骤，用于利用经后处理的零速区间内MEMS三轴加速度计和MEMS三轴磁力计的测量数据，计算用于静态修正的姿态信息(姿态角)。在⑧导航状态向量修正步骤，利用经后处理的零速区间，使用卡尔曼滤波的零速误差校正器，修正导航状态向量。在⑨静态修正条件判定步骤，根据经后处理的零速区间内MEMS三轴加速度计的测量数据，通过判定标准来判定用于静态修正的姿态信息是否能够为后续非零速区间的起始时刻提供姿态信息的修正。在⑩姿态修正步骤，在判定用于静态修正的姿态信息能够为后续非零速区间的起始时刻提供姿态信息的修正时，在后续非零速区间的起始时刻进行姿态信息(姿态角)修正。其中步骤⑦和步骤⑧在零速区间进行，步骤⑨和步骤⑩在非零速区间的起始时刻进行。另外，步骤⑦、⑨和⑩可以结合起来称作过程中姿态修正步骤，用于对非零速区间的起始时刻的姿态信息进行修正。

下面结合图4，详细描述零速检测标志位后处理步骤，包括滑动窗口更新标志位设置步骤和新标志位设置步骤。滑动窗口更新标志位设置步骤，根据当前时刻的零速检测标志位(例如，k时刻的zupt_flag)以及前一时刻的零速检测标志位(例如，k-1时刻的zupt_flag)，设置当前时刻的滑动窗口更新标志位(例如，k时刻的zupt_winbuf_flag)。这里，通过设置一个延迟功能来获得k-1时刻的zupt_flag。滑动窗口更新标志位指示是否对滑动窗口(zupt_winbuffer)内的对应数值进行更新。具体地，根据k时刻的zupt_flag以及k时刻的zupt_flag与k-1时刻的zupt_flag的差值，进行一系列逻辑判断，即上升沿判断、下降沿判断和零速区间判断。例如，参见图1中零速检测结果(标志位)曲线，当该差值为+1时，可以判断为上升沿，当该差值为-1时，可以判断为下降沿，并且当k时刻的zupt_flag为1时，可以判断为零速区间。根据以上判断结果来设置k时刻的zupt_winbuf_flag，zupt_winbuf_flag指示是否对滑动窗口(zupt_winbuffer)进行更新。例如，如果判断结果是上升沿、下降沿或零速区间，则需要对zupt_winbuffer进行更新。然后新标志位设置步骤，根据k时刻的zupt_winbuf_flag和k时刻的zupt_flag，设置当前时刻的新零速检测标志位(例如，k时刻的zupt_flag_new)。具体地，如果k时刻的zupt_winbuf_flag指示对滑动窗口进行更新，则根据k时刻的zupt_flag来更新滑动窗口内的对应数值并在整个滑动窗口更新完毕之后进行平滑滤波，并对滤波结果进行非线性处理，从而设置k时刻的zupt_flag_new。

下面结合图5对静态修正姿态信息(姿态角)计算步骤进行描述，包括累加标志位设置(计算)步骤和静态姿态信息(姿态角)计算步骤。累加标志位设置步骤，根据当前时刻的新零速检测标志位(例如，k时刻的zupt_flag_new)和前一时刻的新零速检测标志位(例如，k-1时刻的zupt_flag_new)，设置当前时刻的累加标志位(例如，k时刻的acc_flag)。这里，通过设置一个延迟功能来获得k-1时刻的zupt_flag_new。累加标志位指示是否进行累加。具体地，根据k时刻的zupt_flag_new与k-1时刻的zupt_flag_new的差值，进行一系列逻辑判断，即上升沿判断、下降沿判断。例如，参见图1中零速检测结果(标志位)曲线，当该差值为+1时，可以判断为上升沿，当该差值为-1时，可以判断为下降沿。根据以上判断结果和k-1时刻的acc_flag来设置k时刻的acc_flag，acc_flag指示是否进行累加。例如，如果判断结果是上升沿、下降沿或k-1时刻的acc_flag为1，则需要进行累加。

然后静态姿态信息计算步骤根据k时刻的acc_flag，计算静态三轴MEMS加速度计值和静态三轴MEMS磁力计值，从而计算用于静态修正的姿态信息。具体地，如果k时刻的acc_flag指示进行累加，则分别对k时刻的三轴加速度计的测量数据和k时刻的三轴磁力计的测量数据进行累加，计算静态三轴加速度计值和静态三轴磁力计值，从而计算静态姿态信息(姿态角，例如横滚角、俯仰角、航向角)。

以上描述了根据本发明示例实施例的自主导航系统中的姿态修正装置和方法。在以上的描述中，仅以示例的方式，示出了本发明的优选实施例，但并不意味着本发明局限于上述步骤和单元结构。在可能的情形下，可以根据需要对步骤和单元进行调整、取舍和组合。此外，某些步骤和单元并非实施本发明的总体发明思想所必需的元素。因此，本发明所必需的技术特征仅受限于能够实现本发明的总体发明思想的最低要求，而不受以上具体实例的限制。

至此已经结合优选实施例对本发明进行了描述。应该理解，本领域技术人员在不脱离本发明的精神和范围的情况下，可以进行各种其它的改变、替换和添加。因此，本发明的范围不局限于上述特定实施例，而应由所附权利要求所限定。

Claims (18)

1.一种自主导航系统中的姿态修正装置，包括：

惯性测量单元，用于输出MEMS三轴陀螺仪、MEMS三轴加速度计和MEMS三轴磁力计的测量数据；

初始姿态信息获取单元，用于根据所述MEMS三轴加速度计和所述MEMS三轴磁力计的测量数据，计算初始姿态信息；

姿态信息更新单元，用于根据所述MEMS三轴陀螺仪、MEMS三轴加速度计和MEMS三轴磁力计的测量数据，更新姿态信息；

导航状态向量估计单元，用于根据更新的姿态信息以及所述MEMS三轴加速度计的测量数据，估计自主导航系统的导航状态向量，所述导航状态向量包括位置向量、速度向量和姿态向量；

零速检测单元，用于根据所述MEMS三轴陀螺仪和所述MEMS三轴加速度计的测量数据，确定自主导航系统的零速区间并获得零速检测标志位；

零速检测标志位后处理单元，用于对所述零速检测标志位进行后处理，以去除零速检测标志位中的高频误差，从而获得经后处理的零速区间和零速检测标志位；

导航状态向量修正单元，用于利用所述经后处理的零速区间，使用卡尔曼滤波的零速误差校正器，修正所述导航状态向量；

过程中姿态修正单元，用于利用所述经后处理的零速区间，对非零速区间的起始时刻的姿态信息进行修正。

2.根据权利要求1所述的装置，其中，所述零速检测标志位后处理单元包括：

滑动窗口更新标志位设置单元，用于根据当前时刻的零速检测标志位以及前一时刻的零速检测标志位，设置当前时刻的滑动窗口更新标志位，其中，所述滑动窗口更新标志位指示是否对滑动窗口内的对应数值进行更新；以及

新标志位设置单元，用于根据当前时刻的滑动窗口更新标志位和当前时刻的零速检测标志位，设置当前时刻的新零速检测标志位。

3.根据权利要求2所述的装置，其中，如果所述滑动窗口更新标志位指示对滑动窗口进行更新，则所述新标志位设置单元通过根据当前时刻的零速检测标志位来更新滑动窗口内的对应数值并在整个滑动窗口更新完毕之后进行平滑滤波，并对滤波结果进行非线性处理，设置所述当前时刻的新零速检测标志位。

4.根据权利要求3所述的装置，其中，所述非线性处理的阈值范围为[0，1]。

5.根据权利要求2所述的装置，其中，所述过程中姿态修正单元包括：

静态修正姿态信息计算单元，用于利用经后处理的零速区间内MEMS三轴加速度计和MEMS三轴磁力计的测量数据，计算用于静态修正的姿态信息；

静态修正条件判定单元，用于根据经后处理的零速区间内MEMS三轴加速度计的测量数据，通过判定标准来判定所述用于静态修正的姿态信息是否能够为后续非零速区间的起始时刻提供姿态信息的修正；以及

姿态修正单元，用于在判定所述用于静态修正的姿态信息能够为后续非零速区间的起始时刻提供姿态信息的修正时，在后续非零速区间的起始时刻进行姿态信息修正。

6.根据权利要求5所述的装置，其中，所述静态修正姿态信息计算单元包括：

累加标志位设置单元，用于根据当前时刻的新零速检测标志位和前一时刻的新零速检测标志位，设置当前时刻的累加标志位，所述累加标志位指示是否进行累加；

静态姿态信息计算单元，用于根据所述累加标志位，计算静态MEMS三轴加速度计值和静态MEMS三轴磁力计值，从而计算用于静态修正的姿态信息。

7.根据权利要求5所述的装置，其中，所述判定标准定义为：

abs(sqrt(abs(f_u)*abs(f_u)+abs(f_v)*abs(f_v)+abs(abs(f_w)*abs(f_w)))-g)＜th，

其中，f_u、f_v、f_w分别为零速区间内MEMS三轴加速度计的测量数据的均值，g为当地重力加速度，th为阈值。

8.根据权利要求7所述的装置，其中，所述阈值的范围为[0.0001m/s²，0.1m/s²]。

9.根据权利要求5所述的装置，其中，所述姿态修正单元通过将所述用于静态修正的姿态信息与更新的姿态信息进行线性组合运算，来实现所述姿态信息修正。

10.一种自主导航系统中的姿态修正方法，包括：

惯性测量步骤，用于输出MEMS三轴陀螺仪、MEMS三轴加速度计和MEMS三轴磁力计的测量数据；

初始姿态信息获取步骤，用于根据所述MEMS三轴加速度计和所述MEMS三轴磁力计的测量数据，计算初始姿态信息；

姿态信息更新步骤，用于根据所述MEMS三轴陀螺仪、MEMS三轴加速度计和MEMS三轴磁力计的测量数据，更新姿态信息；

导航状态向量估计步骤，用于根据更新的姿态信息以及所述MEMS三轴加速度计的测量数据，估计自主导航系统的导航状态向量，所述导航状态向量包括位置向量、速度向量和姿态向量；

零速检测步骤，用于根据所述MEMS三轴陀螺仪和所述MEMS三轴加速度计的测量数据，确定自主导航系统的零速区间并获得零速检测标志位；

零速检测标志位后处理步骤，用于对所述零速检测标志位进行后处理，以去除零速检测标志位中的高频误差，从而获得经后处理的零速区间和零速检测标志位；

导航状态向量修正步骤，用于利用所述经后处理的零速区间，使用卡尔曼滤波的零速误差校正器，修正所述导航状态向量；

过程中姿态修正步骤，用于利用经后处理的零速区间，对非零速区间的起始时刻的姿态信息进行修正。

11.根据权利要求10所述的方法，其中，所述零速检测标志位后处理步骤包括：

滑动窗口更新标志位设置步骤，用于根据当前时刻的零速检测标志位以及前一时刻的零速检测标志位，设置当前时刻的滑动窗口更新标志位，其中，所述滑动窗口更新标志位指示是否对滑动窗口内的对应数值进行更新；以及

新标志位设置步骤，用于根据当前时刻的滑动窗口更新标志位和当前时刻的零速检测标志位，设置当前时刻的新零速检测标志位。

12.根据权利要求11所述的方法，其中，如果所述滑动窗口更新标志位指示对滑动窗口进行更新，则所述新标志位设置步骤通过根据当前时刻的零速检测标志位来更新滑动窗口内的对应数值并在整个滑动窗口更新完毕之后进行平滑滤波，并对滤波结果进行非线性处理，设置所述当前时刻的新零速检测标志位。

13.根据权利要求12所述的方法，其中，所述非线性处理的阈值范围为[0，1]。

14.根据权利要求11所述的方法，其中，所述过程中姿态修正步骤包括：

静态修正姿态信息计算步骤，用于利用经后处理的零速区间内MEMS三轴加速度计和MEMS三轴磁力计的测量数据，计算用于静态修正的姿态信息；

静态修正条件判定步骤，用于根据经后处理的零速区间内MEMS三轴加速度计的测量数据，通过判定标准来判定所述用于静态修正的姿态信息是否能够为后续非零速区间的起始时刻提供姿态信息的修正；以及

姿态修正步骤，用于在判定所述用于静态修正的姿态信息能够为后续非零速区间的起始时刻提供姿态信息的修正时，在后续非零速区间的起始时刻进行姿态信息修正。

15.根据权利要求14所述的方法，其中，所述静态修正姿态信息计算步骤包括：

累加标志位设置步骤，用于当前时刻的新零速检测标志位和前一时刻的新零速检测标志位，设置当前时刻的累加标志位；

静态姿态信息计算步骤，用于根据所述累加标志位，计算静态MEMS三轴加速度计值和静态MEMS三轴磁力计值，从而计算用于静态修正的姿态信息。

16.根据权利要求14所述的方法，其中，所述判定标准定义为：

abs(sqrt(abs(f_u)*abs(f_u)+abs(f_v)*abs(f_v)+abs(abs(f_w)*abs(f_w)))-g)＜th，

其中，f_u、f_v、f_w分别为零速区间内MEMS三轴加速度计的测量数据的均值，g为当地重力加速度，th为阈值。

17.根据权利要求16所述的方法，其中，所述阈值的范围为[0.0001m/s²，0.1m/s²]。

18.根据权利要求14所述的方法，其中，所述姿态修正步骤通过将所述用于静态修正的姿态信息与更新的姿态信息进行线性组合运算，来实现所述姿态信息修正。