CN109764870B - 基于变换估计量建模方案的载体初始航向估算方法 - Google Patents

Abstract

本发明提供了基于变换估计量建模方案的载体初始航向估算方法，包括以下步骤:步骤(1)：数据获取与同步；步骤(2)：载体动态识别；步骤(3)：水平角初始化；步骤(4)：航向估计算法解算；步骤(5)：航向估算收敛评价，本发明的方法可以实现多源组合导航定位设备的动态航向初始化，达到多源组合导航定位设备任意位置配置均能完成航向初始化的效果，降低多源组合导航定位设备的安装要求，提高多源组合导航设备的场景适应性和用户体验。

Description

基于变换估计量建模方案的载体初始航向估算方法

技术领域

本发明涉及一种装备绝对测量传感器和惯性测量传感器的运载体初始航向动态估算方法，适用于利用绝对测量传感器和惯性测量传感器以及多源传感器组合导航算法进行导航定位的领域。

背景技术

以惯性导航为主导航定位系统的多源传感器组合导航定位方法需要提供初始的导航定位信息，包括初始位置，初始速度和初始姿态，其中初始位置和初始速度一般可以通过绝对测量传感器给出，主要是要确认初始姿态，初始姿态包括俯仰角、横滚角和航向角，是惯性传感器坐标轴相对导航坐标系的旋转角度，初始姿态的确认方法主要分动态初始化和静态初始化两类：

静态初始化方法以重力矢量和地球自转角速度作为参考基准，通过惯性传感器测量重力矢量和地球自转角速度以计算初始姿态，计算的方法主要有双矢量定姿算法和kalman滤波算法，但要实现静态初始化方法的前提条件是能够从惯性传感器的观测值中分辨出地球自转角速度分量，这对惯性传感器的性能提出了很高的要求，目前能够满足静态初始化要求的多是光学陀螺惯性传感器和转子陀螺惯性传感器，价格均非常昂贵，民用领域广泛使用的MEMS惯性传感器，价格低廉，但性能还无法满足静态初始化的要求；

动态初始化方法是以外部观测量提供的航向观测信息完成初始姿态中的航向角初始化，或者通过非线性算法实现航向角初始化，俯仰角和横滚角的初始化可用通过惯性传感器给出，外部信息可以是导航系下的位移矢量，导航系下的速度矢量，双天线航向，磁力计航向等信息，但通过这些信息进行初始化都对惯性传感器的安装提出了要求，即惯性传感器的坐标轴系需要与外部信息对应的参考轴系平行，否则会带来安装误差，安装误差的大小直接影响外部航向信息的精度，若安装误差较大(>5°)，则会导致多源组合导航定位算法模型不满足线性化的假设，导致滤波器发散，非线性滤波算法可以避免因安装误差较大导致的模型非线性问题，但非线性滤波算法的计算量非常大，对CPU的处理能力是很大的挑战，且非线性滤波并不能完全解决安装误差带来的非线性问题，当外部信息给出的航向信息误差非常大时，非线性滤波仍有可能会失败。

发明内容

为了解决上述不足的缺陷，本发明提供了基于变换估计量建模方案的载体初始航向估算方法，本发明的方法可以实现多源组合导航定位设备的动态航向初始化，达到多源组合导航定位设备任意位置配置均能完成航向初始化的效果，降低多源组合导航定位设备的安装要求，提高多源组合导航设备的场景适应性和用户体验。

本发明提供了基于变换估计量建模方案的载体初始航向估算方法，包括以下步骤:

步骤(1)：数据获取与同步；

步骤(2)：载体动态识别；

步骤(3)：水平角初始化；

步骤(4)：航向估计算法解算；

步骤(5)：航向估算收敛评价。

上述的方法，其中，所述步骤(1)中具体为：通过CPU同时获取惯性测量传感器和绝对测量传感器的观测数据，并通过CPU的晶振信号对观测数据进行标记，从而实现了基于CPU晶振的数据同步。

上述的方法，其中，所述步骤(2)中：通过惯性测量传感器和绝对测量传感器的观测数据对运载体的动静运动状态进行判断识别，当载体进入到运动状态后再开始执行航向估计算法。

上述的方法，其中，所述步骤(3)中：根据加速度的输出信息或者绝对测量传感器的观测信息对俯仰角和横滚角进行初始化。

上述的方法，其中，所述步骤(4)中：根据变换估计量的建模方法建立航向估计算法模型，通过变换估计量将航向角的估计转化为两个三角函数的估计，通过kalman滤波器对航向进行估计解算。

上述的方法，其中，所述步骤(5)中：kalman滤波器估算会逐渐收敛达到稳态，通过对滤波器的误差协方差矩阵进行监测，识别滤波器达到稳态后中断航向估计算法的解算，将此时估计的航向角作为多源组合导航定位设备导航定位解算的初始航向。

上述的方法，其中，所述绝对测量传感器，用来测量运载的三维位置或三维速度信息，作为kalman滤波器的量测输入；所述惯性测量传感器用于测量惯性测量传感器三个正交方向的线速度和角速度，以进行惯性捷联解算解算和构建航向角估计模型。

本发明提供了基于变换估计量建模方案的载体初始航向估算方法具有以下有益效果：本发明实施了时间同步环节，利用CPU的晶振对绝对测量传感器和惯性测量传感器的观测数据进行了时间戳标记，惯性测量传感器测量的是线加速度和角速度，需要对观测量进行进行积分才能获取角度增量、速度增量和位移增量，采样间隔的精度直接影响积分解算的精度，尤其是运载体角速度和加速度变化剧烈的高动态情况，同时，对绝对测量传感器观测量进行标记，可以实现相对观测值和绝对观测值在时间序列上的对应，利用CPU晶振进行时间戳标记，其时间精度在微秒级，可以满足绝大多数的应用场景；本发明实施了动静态判断环节，因为有些绝对测量传感器需要运载体处于运动状态才能通过绝对测量值提供航向信息，比如提供位置的传感器需要有位移才能通过位移量计算运载体的行进方向，提供速度的传感器需要速度大于一定阈值才能通过速度分量计算运载体的行进方向；本发明实施了水平角初始化环节，相对航向初始化，水平角的初始化容易的多，通过惯性测量传感器在运载体静止或稳态时输出的加速度信息即可计算俯仰角和横滚角，且精度较高，完成水平角的初始化则在航向估计算法建模时即可忽略水平角误差的影响，降低模型的复杂度；本发明基于kalman滤波器和变换估计量的航向估计模型实现运载体的航向估计，通过对惯性测量传感器的测量值进行捷联解算解算来跟踪运载体的动态变化，通过kalman滤波器的量测更新过程和绝对测量传感器的测量结果对捷联解算的解算结果进行修正，从而实现任意安装配置的航向角动态估计；本发明对kalman滤波器的收敛状态进行了识别，当判断滤波器达到稳态后，说明航向角的估计已经收敛，可以中断航向角估计算法，将此时的航向角估计值作为多源组合导航算法的航向初始值。

附图说明

通过阅读参照以下附图对非限制性实施例所作的详细描述，本发明及其特征、外形和优点将会变得更明显。在全部附图中相同的标记指示相同的部分。并未刻意按照比例绘制附图，重点在于示出本发明的主旨。

图1为本发明的硬件系统构成图。

图2为本发明的绝对传感器与惯性测量传感器数据同步原理图。

图3为本发明的航向估计算法数据处理流程图。

图4为本发明中提到的h系相对n系旋转

角示意图。

具体实施方式

在下文的描述中，给出了大量具体的细节以便提供对本发明更为彻底的理解。然而，对于本领域技术人员而言显而易见的是，本发明可以无需一个或多个这些细节而得以实施。在其他的例子中，为了避免与本发明发生混淆，对于本领域公知的一些技术特征未进行描述。

为了彻底理解本发明，将在下列的描述中提出详细的步骤以及详细的结构，以便阐释本发明的技术方案。本发明的较佳实施例详细描述如下，然而除了这些详细描述外，本发明还可以具有其他实施方式。

参照图1-图4所示，本发明提供的基于变换估计量建模方案的载体初始航向估算方法，包括以下步骤:

步骤(1)：数据获取与同步，具体为：通过CPU同时获取惯性测量传感器和绝对测量传感器的观测数据，并通过CPU的晶振信号对观测数据进行标记，从而实现了基于CPU晶振的数据同步；

步骤(2)：载体动态识别，具体为通过惯性测量传感器和绝对测量传感器的观测数据对运载体的动静运动状态进行判断识别，当载体进入到运动状态后再开始执行航向估计算法；

步骤(3)：水平角初始化，具体为根据加速度的输出信息或者绝对测量传感器的观测信息对俯仰角和横滚角进行初始化；

步骤(4)：航向估计算法解算，具体为根据变换估计量的建模方法建立航向估计算法模型，通过变换估计量将航向角的估计转化为两个三角函数的估计，通过kalman滤波器对航向进行估计解算；

步骤(5)：航向估算收敛评价，具体为kalman滤波器估算会逐渐收敛达到稳态，通过对滤波器的误差协方差矩阵进行监测，识别滤波器达到稳态后中断航向估计算法的解算，将此时估计的航向角作为多源组合导航定位设备导航定位解算的初始航向，其中绝对测量传感器，用来测量运载的三维位置或三维速度信息，作为kalman滤波器的量测输入；所述惯性测量传感器用于测量惯性测量传感器三个正交方向的线速度和角速度，以进行惯性捷联解算解算和构建航向角估计模型。

参照图1所示，本发明的系统构成包括：惯性测量传感器(IMU)，绝对测量传感器，CPU，在图1中，A为绝对测量传感器，用来测量运载的三维位置或三维速度信息，作为kalman滤波器的量测输入；B为惯性测量传感器，一般安装于载体的质心位置，用于测量惯性测量传感器三个正交方向的线速度和角速度，以进行惯性捷联解算解算和构建航向角估计模型；C为CPU处理器，主要实现绝对测量传感器数据、惯性测量传感器数据的时间同步，惯性捷联解算解算，和航向角kalman滤波估计算法的运行；D为运载体，绝对测量传感器和惯性测量传感器安装的物理媒介；

惯性测量传感器包括3轴加速度计和3轴陀螺仪，安装于运载体上的惯性测量传感器，用来测量运载体的线加速度和角速度，利用其测量的加速度和角速度进行惯性捷联解算解算和参与构建航向估计算法的运动模型，本发明通过所设计的航向估计算法模型估算运载体的航向，这是与其他航向初始化方法最大的不同；

绝对测量传感器用来测量运载体在导航系下的绝对空间信息，可以是三维速度，三维位置和三维姿态中的任意种信息，比如GNSS接收机可以提供位置和速度信息，里程计可以提供速度信息，磁力计可以提供速度信息等等，本发明所用的绝对测量传感器需要能够测量三维速度和三维位置的一种或两种，典型的传感器为GNSS接收机，利用绝对测量传感器的观测信息可以通过kalman滤波器对航向估计算法模型进行修正；

CPU是数据处理中心，负责惯性测量传感器和绝对测量传感器数据的获取和同步，负责航向估计算法的运行，本发明通过对变换估计量的方法，将航向角的估计转换成两个三角函数变量的估计，并忽略短时间内小量误差的影响，将非线性模型转换为线性模型，这与通过非线性滤波算法和非线性模型估计航向角的方法在算法模型上存在本质上的区别；由于对航向角进行了建模估计，所以本发明的方法对多源组合导航定位模块的安装没有限制，不需要限定模块的指向，这与需要限定模块安装方式的通过外部观测信息提供航向的方法存在实现方案上的根本区别。

在本发明中，数据获取与同步具体为通过CPU同时获取惯性测量传感器和绝对测量传感器的观测数据，并通过CPU的晶振信号对观测数据进行了标记，从而实现了基于CPU晶振的数据同步，见附图2；载体动态识别具体为通过惯性测量传感器和绝对测量传感器的观测数据对运载体的动静运动状态进行判断识别，当载体进入到运动状态后再开始执行本发明提出的航向估计算法；水平角初始化具体为根据加速度的输出信息或者绝对测量传感器的观测信息(比如磁力计、双天线定位定向接收机等)对俯仰角和横滚角进行初始化；航向估计算法解算具体为根据本发明提出的变换估计量的建模方法建立航向估计算法模型，通过变换估计量将航向角的估计转化为两个三角函数的估计，通过kalman滤波器对航向进行估计解算，见附图3和具体实施方式；航向估算收敛评价具体为通过kalman滤波器估算会逐渐收敛达到稳态，本发明通过对滤波器的误差协方差矩阵进行监测，识别滤波器达到稳态后中断航向估计算法的解算，将此时估计的航向角作为多源组合导航定位设备导航定位解算的初始航向。

以下提供一具体的实施例

实施例1，结合附图1、附图2、附图3，对本发明中的航向角估计算法和变换估计量模型设计做简要说明：

第一步：CPU晶振数据同步：

如附图1和附图2，待惯性测量传感器和绝对测量传感器通电完成初始化之后，将I惯性传感器和绝对测量传感器的观测数据接入到CPU处理器，通过CPU的晶振对惯性传感器观测值和绝对测量传感器观测值进行CPU系统时间标记，然后将带有时间戳的惯性传感器观测数据和绝对传测量感器观测数据传递给航向角估计算法解算模块。

第二步：静态判断与水平角初始化：

本发明分别在静态或准静态阶段完成水平角的初始化，在运动阶段完成航向角的估算，本发明通过静态加速度计测量的比力与重力矢量大小相同，方向相反的特性进行水平角的计算，综合利用绝对测量传感器的数据和惯性传感器的输出判断运载体的运动状态。

第三步：航向角滤波估算：

在此对本发明提出的通过变换估计量实现航向角估计的建模方法和航向角滤波估计算法进行详细说明：

坐标系定义：定义坐标系只为通过一个实例详细说明本发明的建模方法，其他坐标系定义方式不影响本发明提出的建模方法的推导和使用；

a.地心惯性坐标系(i系)：坐标原点位于地球质心；

b.地心地固坐标系(e系)：坐标原点与i系重合，相对i系以地球自转角速度旋转；

c.导航坐标系(n系)：选取当地水平坐标系(东北天)作为导航坐标系；

d.水平坐标系(h系)：h系相对n系在航向方向上旋转

角，如图4；

e.载体坐标系(b系)；与运载体固连的坐标系；

惯性捷联解算运动方程：

其中

为b系相对n系的方向余弦矩阵(DCM)，Vⁿ为载体在n系下的三维速度矢量，L,λ,h为载体的经纬高坐标，f^b为惯性传感器测量的比力，

为角速度

的反对称矩阵，gⁿ为当地的重力矢量，

为地球自转角速度在n系的投影，

为n系相对n系旋转的角速度在n系的投影，R_e为地球半径，本发明中假设地球为球体；

变换估计量航向角估计模型设计：

定义

为h系至n系的DCM，

为b系至h系的DCM，则有如下转换：

定义DCM的误差如下：

定义航向误差为

则有：

通过公式(4)即将对航向角

的估计变换为对

和

的估计，公式(2)(3)(4)(5)即本发明提出的变换估计量航向角估计算法模型设计的具体实现，也是本发明变换估计量航向角建模的核心思想。

假设俯仰角误差δθ和横滚角误差δγ为小量，记

则有：

将(5)，(6)带入(3)，推导可得：

忽略

则有

分别对(7)和(5)进行求导，则有：

结合公式(5)，则有如下误差方程：

对速度进行微分，有如下结果：

对经纬度进行微分，有如下结果：

变换估计量航向角估计kalman滤波器设计：

Kalman滤波器的标准过程如下：

①状态一步预测

②状态一步预测均方误差

③滤波增益

④状态估计

⑤状态估计均方误差

P_k＝(I-K_kH_k)P_k/k-1   (7-5)

选取滤波器状态空间如下：

根据公式(9)(10)(11)可以提取如下状态转移矩阵：

Φ＝I_6×6+F(t)×dt

以绝对测量传感器测量的位置，速度作为滤波器的观测，则有：

以上即为变换估计量航向角估计算法的滤波器模型和建模过程。

第四步：滤波器收敛判断：

本发明根据kalman滤波器的误差协方差矩阵中对角线元素的收敛情况判断滤波器是否达到稳态，具体方法为将误差协方差对角线元素放入一定长度的窗口中，对每一个对角线元素窗口的标准差进行监测，若每个元素在一定时间阶段内均小于设定的经验阈值，则认为滤波器达到稳态，中断滤波器，输出航向角的估计结果。在本发明中实施了时间同步环节，利用CPU的晶振对绝对测量传感器和惯性测量传感器的观测数据进行了时间戳标记，惯性测量传感器测量的是线加速度和角速度，需要对观测量进行进行积分才能获取角度增量、速度增量和位移增量，采样间隔的精度直接影响积分解算的精度，尤其是运载体角速度和加速度变化剧烈的高动态情况，同时，对绝对测量传感器观测量进行标记，可以实现相对观测值和绝对观测值在时间序列上的对应，利用CPU晶振进行时间戳标记，其时间精度在微秒级，可以满足绝大多数的应用场景；实施了动静态判断环节，因为有些绝对测量传感器需要运载体处于运动状态才能通过绝对测量值提供航向信息，比如提供位置的传感器需要有位移才能通过位移量计算运载体的行进方向，提供速度的传感器需要速度大于一定阈值才能通过速度分量计算运载体的行进方向；实施了水平角初始化环节，相对航向初始化，水平角的初始化容易的多，通过惯性测量传感器在运载体静止或稳态时输出的加速度信息即可计算俯仰角和横滚角，且精度较高，完成水平角的初始化则在航向估计算法建模时即可忽略水平角误差的影响，降低模型的复杂度；本发明基于kalman滤波器和变换估计量的航向估计模型实现运载体的航向估计，通过对惯性测量传感器的测量值进行捷联解算解算来跟踪运载体的动态变化，通过kalman滤波器的量测更新过程和绝对测量传感器的测量结果对捷联解算的解算结果进行修正，从而实现任意安装配置的航向角动态估计；本发明对kalman滤波器的收敛状态进行了识别，当判断滤波器达到稳态后，说明航向角的估计已经收敛，可以中断航向角估计算法，将此时的航向角估计值作为多源组合导航算法的航向初始值。

以上对本发明的较佳实施例进行了描述。需要理解的是，本发明并不局限于上述特定实施方式，其中未尽详细描述的设备和结构应该理解为用本领域中的普通方式予以实施；任何熟悉本领域的技术人员，在不脱离本发明技术方案范围情况下，都可利用上述揭示的方法和技术内容对本发明技术方案做出许多可能的变动和修饰，或修改为等同变化的等效实施例，这并不影响本发明的实质内容。因此，凡是未脱离本发明技术方案的内容，依据本发明的技术实质对以上实施例所做的任何简单修改、等同变化及修饰，均仍属于本发明技术方案保护的范围内。

Claims (1)

1.基于变换估计量建模方案的载体初始航向估算方法，其特征在于，包括以下步骤:步骤(1)：数据获取与同步；

步骤(2)：载体动态识别；

步骤(3)：水平角初始化；

步骤(4)：航向估计算法解算；

步骤(5)：航向估算收敛评价，所述步骤(1)中具体为：通过CPU同时获取惯性测量传感器和绝对测量传感器的观测数据，并通过CPU的晶振信号对观测数据进行标记，从而实现了基于CPU晶振的数据同步，所述步骤(2)中：通过惯性测量传感器和绝对测量传感器的观测数据对运载体的动静运动状态进行判断识别，当载体进入到运动状态后再开始执行航向估计算法，所述步骤(3)中：根据加速度的输出信息或者绝对测量传感器的观测信息对俯仰角和横滚角进行初始化，所述步骤(4)中：根据变换估计量的建模方法建立航向估计算法模型，通过变换估计量将航向角的估计转化为两个三角函数的估计，通过kalman滤波器对航向进行估计解算，其中变换估计量航向角估计模型设计：

定义

为h系至n系的DCM，

为b系至h系的DCM，则有如下转换：

定义DCM的误差如下：

定义航向误差为

则有：

通过公式(4)即将对航向角

的估计变换为对

和

的估计，所述步骤(5)中：kalman滤波器估算会逐渐收敛达到稳态，通过对滤波器的误差协方差矩阵进行监测，识别滤波器达到稳态后中断航向估计算法的解算，将此时估计的航向角作为多源组合导航定位设备导航定位解算的初始航向，所述绝对测量传感器，用来测量运载的三维位置或三维速度信息，作为kalman滤波器的量测输入；所述惯性测量传感器用于测量惯性测量传感器三个正交方向的线速度和角速度，以进行惯性捷联解算和构建航向角估计模型。

Images