CN110388941A - 一种基于自适应滤波的车辆姿态对准方法 - Google Patents

Abstract

本发明涉及一种基于自适应滤波的车辆姿态对准方法，属于车辆姿态对准技术，解决了现有技术对准精度差、对准时间较长的问题。该方法包括如下步骤：获取车辆当前姿态改变量，包括当前三轴偏离原始三轴的角度增量，以及沿原始三轴的速度增量；对上述角度增量和速度增量分别进行误差补偿，实施粗对准，得到粗对准后车辆的姿态和速度；更新车辆姿态对准系统的实时状态，进而修正粗对准后车辆的姿态和速度；根据车辆姿态对准系统自适应滤波的滤波增益获得车辆状态估计，判断所述车辆状态估计与车辆当前状态是否相同；不同，则用所述车辆状态估计替代车辆上一时刻状态，再次更新车辆对准系统的实时状态，直到相同为止，完成车辆姿态精对准。

Description

一种基于自适应滤波的车辆姿态对准方法

技术领域

本发明涉及车辆姿态对准技术领域，尤其涉及一种基于自适应滤波的车辆姿态对准方法。

背景技术

在发动机怠速和乘客上下车时，车辆会发生晃动，现有技术一般需要较长的粗对准时间来计算载体的初始姿态，才能保证精对准过程获得的车辆当前姿态精度较高。

现有技术采用的姿态对准方法一般如下：对光纤陀螺和石英加速度计的原始数据进行补偿后，实时进行粗对准和精对准两个过程，这两个过程顺序进行。但该姿态对准方法获得的对准精度较差，且对准时间较长。

现有技术在精对准过程中一般采用卡尔曼滤波器，但不对车辆姿态对准系统的噪声方差阵进行实时更新，当预设的噪声方差阵与实际噪声方差差别较大时，车辆姿态对准系统的状态估计误差较大，滤波器收敛速度慢，从而导致姿态修正效果较差。

发明内容

鉴于上述的分析，本发明实施例旨在提供一种基于自适应滤波的车辆姿态对准方法，用以解决现有技术对准精度差、对准时间较长的问题。

一方面，本发明实施例提供了一种基于自适应滤波的车辆姿态对准方法，包括如下步骤：

获取车辆当前姿态改变量，包括当前三轴偏离原始三轴的角度增量，以及沿原始三轴的速度增量；

对上述角度增量和速度增量分别进行误差补偿，根据补偿后的等效旋转矢量和速度增量实施粗对准，得到粗对准后车辆的姿态和速度；

更新车辆姿态对准系统的实时状态，根据更新后获得的失准角、速度误差修正粗对准后车辆的姿态和速度；

根据车辆姿态对准系统自适应滤波的滤波增益获得车辆状态估计，判断所述车辆状态估计与车辆当前状态是否相同；不同，则用所述车辆状态估计替代车辆上一时刻状态，再次更新车辆姿态对准系统的实时状态，直到相同为止，完成车辆姿态精对准。

上述技术方案的有益效果如下：通过采集的当前姿态改变获得等效旋转矢量和速度增量，进而根据等效旋转矢量和速度增量实施粗对准，将粗对准结束的姿态结果作为初始姿态，根据对准原理迭代并进行自适应卡尔曼滤波调整滤波增益进行精对准，提高滤波器收敛速度，在保证对准时间的基础上提高了对准精度。

基于上述方法的进一步改进，所述获取车辆当前姿态改变量，进一步包括如下步骤：

通过三轴光纤陀螺，实时采集在载体坐标系内车辆当前三轴偏离原始三轴的角度增量；

通过三轴石英加速度计，实时采集在载体坐标系内车辆沿原始三轴的速度增量；

所述载体坐标系的中心设置于车辆重心位置，x轴正向为沿车辆横轴向右，y轴正向为沿车辆纵轴向前，z轴正向为沿车辆立轴向上。

上述进一步改进方案的有益效果是：利用光纤陀螺对角度增量进行采集，价格成本低，体积小、重量轻，加工工艺、采集方式简单，数据准确。

进一步，所述对角度增量进行误差补偿，进一步包括如下步骤：

连续获取等间隔时间的4组角度增量数据；

通过下面公式对上述角度增量进行4子样圆锥误差补偿，获得补偿后的等效旋转矢量φ(T)

式中，Δθ(i)为t_i时刻获得的角度增量，i＝1,…,4，T表示连续获取时间。

上述进一步改进方案的有益效果是：对解算时间内车辆在空间旋转变化引起的速度增量进行了补偿，扩展了车辆在对准过程中的工作适应性。

进一步，所述对速度增量进行误差补偿，进一步包括如下步骤：

连续获取等间隔时间的4组速度增量数据；

通过下面公式对上述速度增量进行4子样旋转误差补偿和划桨误差补偿，获得补偿后的速度增量ΔV(T)

式中，ΔV(i)为t_i时刻获得的速度增量，i＝1,…,4。

上述进一步改进方案的有益效果是：对解算时间内车辆存在周期性的角运动和线运动引起的速度增量进行了补偿，扩展了车辆在对准过程中的工作适应性。

进一步，所述根据补偿后的等效旋转矢量和速度增量实施粗对准，确定粗对准后车辆的姿态和速度，进一步包括如下步骤：

根据补偿后的等效旋转矢量φ(T)和速度增量ΔV(T)，通过惯性系对准方法初步获取车辆的姿态矩阵

将上述姿态矩阵作为初始姿态，结合补偿后的等效旋转矢量φ(T)和补偿后的速度增量ΔV(T)进行捷联解算，更新车辆的姿态矩阵速度v和位置p，直到捷联解算算法收敛为止；

对上述算法收敛获得的车辆的姿态矩阵进一步分析，获得粗对准后车辆的姿态，同时，将上述算法收敛获得的车辆的速度作为获得粗对准后车辆的速度，完成车辆粗对准。

上述进一步改进方案的有益效果是：粗对准过程采用惯性系对准方法，提高了车辆对准过程中抗角晃动干扰的能力。进行迭代捷联解算的是补偿后的等效旋转矢量和速度增量，有效降低了存储数据的大小。

进一步，所述通过惯性系对准方法初步获取车辆的姿态矩阵进一步包括如下步骤：

以车辆重心为中心，指向东为x轴正向，指向北为y轴正向，向上为z轴正向，建立导航坐标系；

通过下面公式获取初始时刻导航坐标系相对于当前时刻导航坐标系的转换矩阵

式中，ω_ie为地球自转角速率，t为采样时间，L为车辆所处地理纬度；

通过下面公式获取当前时刻载体坐标系相对于初始时刻载体坐标系的转换矩阵

式中，φ(T)×为向量φ(T)的反对称阵运算，I为单位阵；

通过下面公式获取初始时刻载体坐标系相对于初始时刻导航坐标系的转换矩阵

其中

式中，为当前解算时刻相对初始时刻导航坐标系速度，为上一解算时刻相对初始时刻导航坐标系速度，为当前解算时刻相对初始时刻载体坐标系速度，为上一解算时刻相对初始时刻载体坐标系速度，为下一解算时刻相对当前时刻导航坐标系速度，为上一解算时刻获得的车辆的姿态矩阵，初值为0；

通过下面公式获取初步获取车辆当前解算时刻的姿态矩阵

上述进一步改进方案的有益效果是：粗对准过程采用惯性系对准方法，提高了车辆对准过程中抗角晃动干扰的能力。

进一步，所述迭代进行捷联解算更新车辆的姿态矩阵速度v和位置p，进一步包括如下步骤：

将上述车辆当前解算时刻的姿态矩阵作为初始姿态通过下面公式更新车辆的姿态矩阵

其中

式中，t为采样时间，T为导航解算时间，为向量的反对称阵运算，v_N为当前时刻导航坐标系速度的北向分量，v_E为当前时刻导航坐标系速度的东向分量，R_e为地球椭圆长半轴，R_M为子午圈主曲率半径，R_N为卯酉圈主曲率半径，ω_ie为地球自转角速率，L和h分别为车辆所处地理纬度和地理高度，分别为t_m和t_m-1时刻的姿态矩阵，m＝2，…，e为地球椭圆偏心率；

将与差值的绝对值与预设阈值进行比较，如果所述差值的绝对值大于等于预设阈值，重复上述步骤，直到所述差值的绝对值小于预设阈值为止，完成车辆的姿态矩阵的更新；

设置粗对准的初始速度为0，通过下面公式更新车辆的当前时刻速度

式中，分别表示t_m、t_m-1时刻的导航坐标系速度，m＝2,…，gⁿ为重力加速度，表示t_m-1时刻的姿态矩阵，T为导航解算时间，ΔV(T)为补偿后的速度增量；

将与差值的绝对值与预设阈值进行比较，如果所述差值的绝对值大于等于预设阈值，重复上述步骤，直到所述差值的绝对值小于预设阈值为止，完成车辆的速度v更新；

通过GPS获得车辆初始位置坐标p₁，通过下面公式更新车辆的当前时刻位置坐标p_m

其中

R_Mh＝R_M+h

R_Nh＝R_N+h

式中，p_m、p_m-1分别表示t_m、t_m-1时刻的位置坐标，分别表示t_m、t_m-1时刻的导航坐标系速度，m＝2,…，L为车辆所处地理纬度，R_M为子午圈主曲率半径，R_N为卯酉圈主曲率半径，h为车辆所处地理高度；

将p_m与p_m-1差值的绝对值与预设阈值进行比较，如果所述差值的绝对值大于等于预设阈值，重复上述步骤，直到所述差值的绝对值小于预设阈值，完成车辆的位置p更新。

上述进一步改进方案的有益效果是：进行迭代捷联解算的是补偿后的等效旋转矢量和速度增量，与使用直接存储的角度增量和速度增量相比，节省存储空间。

进一步，所述更新车辆姿态对准系统的实时状态，根据更新后获得的失准角、速度误差修正粗对准后车辆的姿态和速度，进一步包括如下步骤：

建立车辆姿态对准系统的状态向量X，获得对应的状态均方误差矩阵P；

根据建立的状态向量X，获取车辆姿态对准系统上一时刻对应的状态向量X_k-1，进而根据状态均方误差矩阵P得出上一时刻对应的状态均方误差矩阵P_k-1，进而通过下面公式获取车辆姿态对准系统当前时刻对应的状态向量X_k，并得出当前时刻对应的状态均方误差矩阵P_k

X_k＝Φ_k/k-1X_k-1+Q_k

其中

Φ_k/k-1＝I+F(t_k-1)T

Q_k＝q(t_k-1)T

式中，()×为向量的反对称阵运算，为更新后车辆的姿态矩阵，T为导航解算时间，gⁿ为重力加速度，ω_db为陀螺随机常值，ω_eb为加速度计随机常值；

通过下面公式更新车辆姿态对准系统的状态向量X以及状态均方误差矩阵P，获得预测向量X_k/k-1以及预测矩阵P_k/k-1

X_k/k-1＝Φ_k/k-1X_k-1

式中，X_k/k-1为系统的状态一步预测向量，P_k/k-1为系统的状态均方误差一步预测矩阵；

根据上述预测矩阵X_k/k-1中的φ_E、φ_N、φ_U、δv_E、δv_N，通过下面公式进一步修正车辆的姿态矩阵速度v

v'＝v-aδv

式中，为修正后的姿态矩阵，为粗对准获得的姿态矩阵，v'为修正后的速度，v为粗对准获得的速度，a为修正系数，φ为向量[φ_E φ_N φ_U]^T，δv为向量[δv_E δv_N 0]^T。

上述进一步改进方案的有益效果是：通过对车辆姿态对准系统的状态向量X和状态均方误差矩阵P不断迭代，修正车辆的姿态矩阵速度，进而使得对准更加精确。

进一步，所述车辆姿态对准系统自适应滤波的滤波增益，通过如下步骤获得：

通过下面公式获取滤波增益对应的当前时刻的系统噪声方差R_k

其中

Z_k/k-1＝H X_k/k-1+V_k

式中，R_k-1为上一时刻的系统噪声方差，β_k为当前时刻的自适应滤波系数，β_k-1为上一时刻的自适应滤波系数，k＝2,…，β₀＝1，b为渐消因子，Z_k/k-1为量测预测误差矩阵，V_k为系统当前时刻的量测噪声向量，V_k-1为系统上一时刻的量测噪声向量，V₁＝[0.5,0.5,2.0]^T，T_H为系统的量测更新时间；

根据所述R_k，通过下面公式获取车辆姿态对准系统自适应滤波的滤波增益K_k

K_k＝P_k/k-1H^T(HP_k/k-1H^T+R_k)^-1。

上述进一步改进方案的有益效果是：通过自适应滤波不断修正系统噪声方差R_k，能够获得准确的滤波增益K_k，进而提高系统滤波效果。

进一步，所述根据车辆姿态对准系统自适应滤波的滤波增益获得车辆状态估计，进一步包括如下步骤：

通过下面公式获得车辆姿态校准系统的状态向量估计

式中，Z_k为系统当前时刻的量测向量，等于粗对准捷联解算获得的速度

通过下面公式获得上述状态向量估计的均方误差矩阵估计

判断所述估计与车辆当前状态X_k、P_k是否相同，如果二者有一个不同，用所述估计替代X_k-1、P_k-1，再次更新车辆姿态对准系统的实时状态，直到二者相同为止，完成车辆姿态精对准。

上述进一步改进方案的有益效果是：通过将状态向量估计与当前状态进行比较，以及将均方误差矩阵估计与均方误差矩阵进行比较，能够充分保证精对准方法的正确性，是获得的对准结果符合实际情况、精确、可靠。

本发明中，上述各技术方案之间还可以相互组合，以实现更多的优选组合方案。本发明的其他特征和优点将在随后的说明书中阐述，并且，部分优点可从说明书中变得显而易见，或者通过实施本发明而了解。本发明的目的和其他优点可通过说明书以及附图中所特别指出的内容中来实现和获得。

附图说明

附图仅用于示出具体实施例的目的，而并不认为是对本发明的限制，在整个附图中，相同的参考符号表示相同的部件。

图1为本发明实施例1方法步骤示意图。

具体实施方式

下面结合附图来具体描述本发明的优选实施例，其中，附图构成本申请一部分，并与本发明的实施例一起用于阐释本发明的原理，并非用于限定本发明的范围。

首先介绍本发明公式推导涉及的坐标系定义。

1)惯性坐标系(i系)：原点为地球中心，x_i、y_i轴在地球赤道平面内，x_i轴正向指向春分点(赤道面与黄道面的交线再与天球相交的交点之一)，z_i轴为地球自转轴，正向指向地球北极。

2)地球坐标系(e系)：原点为地球中心，x_e、y_e轴在地球赤道平面内，x_e轴正向指向本初子午线，z_e轴为地球自转轴，正向指向北极。地球坐标系与地球固联，相对于惯性坐标系以地球自转角速率运动。

3)导航坐标系(n系)：原点为载体(车辆)重心，x_n轴正向指东，y_n轴正向指北，z_n轴正向指天。

4)载体坐标系(b系)：原点为载体重心，x_b轴沿载体横轴向右，y_b轴沿载体纵轴向前，z_b轴沿载体立轴向上。

实施例1

本发明的一个具体实施例，公开了一种基于自适应滤波的车辆姿态对准方法，如图1所示，包括如下步骤：

S1.获取车辆当前姿态改变量，包括当前三轴偏离原始三轴的角度增量，以及沿原始三轴的速度增量；

S2.对上述角度增量和速度增量分别进行误差补偿，根据补偿后的等效旋转矢量和速度增量实施粗对准，得到粗对准后车辆的姿态和速度。

S3.更新车辆姿态对准系统的实时状态，根据更新后获得的失准角、速度误差修正粗对准后车辆的姿态和速度。具体地，获取车辆姿态对准系统上一时刻状态，根据车量姿态对准原理更新车辆姿态对准系统的实时状态，根据更新后获得的失准角、速度误差修正粗对准后车辆的姿态和速度。

S4.根据车辆姿态对准系统自适应滤波的滤波增益获得车辆状态估计，判断所述车辆状态估计与车辆当前状态是否相同；不同，则用所述车辆状态估计替代车辆上一时刻状态，再次更新车辆姿态对准系统的实时状态，直到相同为止，完成车辆姿态精对准。

实施时，步骤S2进行车辆姿态粗对准，步骤S3～S4进行车辆姿态精对准。

与现有技术相比，本实施例提供的方法通过采集的当前姿态改变获得等效旋转矢量和速度增量，进而根据等效旋转矢量和速度增量实施粗对准，将粗对准结束的姿态结果作为初始姿态，根据对准原理迭代并进行自适应卡尔曼滤波调整滤波增益进行精对准，能够有效提高滤波器收敛速度，在保证对准时间的基础上提高了对准精度。

实施例2

在实施例1的基础上进行优化，所述获取车辆当前姿态改变量，进一步包括如下步骤：

S11.通过三轴光纤陀螺，实时采集在所述载体坐标系内车辆当前三轴偏离原始三轴的角度增量；

S12.通过三轴石英加速度计，实时采集在所述载体坐标系内车辆沿原始三轴的速度增量。

上述步骤S11和S12中，所述载体坐标系的中心设置于车辆重心位置，x轴正向为沿车辆横轴向右，y轴正向为沿车辆纵轴向前，z轴正向为沿车辆立轴向上。

优选地，步骤S2中，所述对角度增量进行误差补偿，进一步包括如下步骤：

S21.连续获取等间隔时间的4组角度增量数据；

S22.通过下面公式对上述角度增量数据进行4子样圆锥误差补偿，获得补偿后的等效旋转矢量φ(T)

式中，Δθ(i)为t_i时刻获得的角度增量数据，i＝1,…,4，T表示连续获取时间。

优选地，步骤S2中，所述对速度增量进行误差补偿，进一步包括如下步骤：

S23.连续获取等间隔时间的4组速度增量数据；

S24.通过下面公式对上述速度增量数据进行4子样旋转误差补偿和划桨误差补偿，获得补偿后的速度增量ΔV(T)

式中，ΔV(i)为t_i时刻获得的速度增量数据，i＝1,…,4。

优选地，步骤S2中，所述根据补偿后的等效旋转矢量和速度增量实施粗对准，得到粗对准后车辆的姿态和速度，进一步包括如下步骤：

S21.根据补偿后的等效旋转矢量φ(T)和速度增量ΔV(T)，通过惯性系对准方法初步获取车辆的姿态矩阵

S22.将上述姿态矩阵作为初始姿态，结合补偿后的等效旋转矢量φ(T)和补偿后的速度增量ΔV(T)进行捷联解算，更新车辆的姿态矩阵速度v和位置p，直到捷联解算算法收敛为止；

S23.对上述捷联解算算法收敛后获得的车辆的姿态矩阵进行分析，获得粗对准后车辆的姿态，同时，将上述捷联解算算法收敛获得的车辆的速度作为获得粗对准后车辆的速度，完成车辆粗对准。

优选地，步骤S21中，所述通过惯性系对准方法初步获取车辆的姿态矩阵进一步包括如下步骤：

S211.以车辆重心为中心，指向东为x轴正向，指向北为y轴正向，指向上为z轴正向，建立导航坐标系。

S212.通过下面公式获取初始时刻导航坐标系相对于当前时刻导航坐标系的转换矩阵

式中，ω_ie为地球自转角速率，t为采样时间，L为车辆所处地理纬度。

S213.通过下面公式获取当前时刻载体坐标系相对于初始时刻载体坐标系的转换矩阵

式中，φ(T)×为向量φ(T)的反对称阵运算，I为单位阵。

S214.通过下面公式获取初始时刻载体坐标系相对于初始时刻导航坐标系的转换矩阵

其中

式中，为当前解算时刻相对初始时刻导航坐标系速度(即在导航坐标系下的车辆速度，本领域技术人员能够理解)，为上一解算时刻相对初始时刻导航坐标系速度，初值为0，为当前解算时刻相对初始时刻载体坐标系速度(即在载体坐标系下的车辆速度，本领域技术人员能够理解)，为上一解算时刻相对初始时刻载体坐标系速度，为下一解算时刻相对当前时刻导航坐标系速度，为上一解算时刻获得的车辆的姿态矩阵，第一次解算时此处ΔV(T)为补偿后的速度增量。

S215.根据上述通过下面公式获取车辆当前解算时刻的姿态矩阵

优选地，步骤S22中，所述进行捷联解算更新车辆的姿态矩阵速度v和位置p，进一步包括如下步骤：

S221.将上述车辆当前解算时刻的姿态矩阵作为初始姿态通过下面公式更新车辆的姿态矩阵

其中

式中，t为采样时间，T为导航解算时间，为向量的反对称阵运算，vN为当前时刻导航坐标系速度的北向分量，v_E为当前时刻导航坐标系速度的东向分量，R_e为地球椭圆长半轴，R_M为子午圈主曲率半径，R_N为卯酉圈主曲率半径，ω_ie为地球自转角速率，L和h分别为车辆所处地理纬度和地理高度，分别为t_m和t_m-1时刻的姿态矩阵，m＝2，…，e为地球椭圆偏心率。

具体地，e通过下面公式获得

式中，f为椭圆扁率，取值1/298.257。

S222.将与差值的绝对值与预设阈值进行比较，如果所述差值的绝对值大于等于预设阈值，重复上述步骤，直到所述差值的绝对值小于预设阈值为止，完成车辆的姿态矩阵的更新。

S223.设置粗对准的初始速度为0，通过下面公式更新车辆的当前时刻速度

式中，分别表示t_m、t_m-1时刻的导航坐标系速度，m＝2,…，gⁿ为重力加速度，表示t_m-1时刻的姿态矩阵，T为导航解算时间，ΔV(T)为补偿后的速度增量。

S224.将与差值的绝对值与预设阈值进行比较，如果所述差值的绝对值大于等于预设阈值，重复上述步骤，直到所述差值的绝对值小于预设阈值为止，完成车辆的速度v更新。

S225.通过GPS获得车辆初始位置坐标p₁，通过下面公式更新车辆的当前时刻位置坐标p_m

其中

R_Mh＝R_M+h

R_Nh＝R_N+h

式中，p_m、p_m-1分别表示t_m、t_m-1时刻的位置坐标，分别表示t_m、t_m-1时刻的导航坐标系速度，m＝2,…，L为车辆所处地理纬度，R_M为子午圈主曲率半径，R_N为卯酉圈主曲率半径，h为车辆所处地理高度。

S226.将p_m与p_m-1差值的绝对值与预设阈值进行比较，如果所述差值的绝对值大于等于预设阈值，重复上述步骤，直到所述差值的绝对值小于预设阈值，完成车辆的位置p更新。

优选地，步骤S23中，通过下面公式获得粗对准后车辆的姿态θ、γ、ψ

优选地，步骤S3中，所述更新车辆姿态对准系统的实时状态，根据更新后获得的失准角、速度误差修正粗对准后车辆的姿态和速度，进一步包括如下步骤：

S31.建立车辆姿态对准系统的状态向量X，获得对应的状态均方误差矩阵P。

S32.根据建立的状态向量X，获取车辆姿态对准系统上一时刻对应的状态向量X_k-1，得出上一时刻对应的状态均方误差矩阵P_k-1，进而通过下面公式获取车辆姿态对准系统当前时刻对应的状态向量X_k和当前时刻对应的状态均方误差矩阵P_k(P_k-1和P_k通过应的状态向量获得，参见上面公式，)

X_k＝Φ_k/k-1X_k-1+Q_k (15)

其中

Φ_k/k-1＝I+F(t_k-1)T

Q_k＝q(t_k-1)T

式中，()×为向量的反对称阵运算，为更新后车辆的姿态矩阵，T为导航解算时间，gⁿ为重力加速度，ω_db为陀螺随机常值，ω_eb为加速度计随机常值。

S33.通过下面公式更新车辆姿态对准系统的状态向量X以及状态均方误差矩阵P，获得预测向量X_k/k-1以及预测矩阵P_k/k-1

X_k/k-1＝Φ_k/k-1X_k-1

式中，X_k/k-1为系统的状态一步预测向量，P_k/k-1为系统的状态均方误差一步预测矩阵；

S34.根据上述预测矩阵X_k/k-1中的φ_E、φ_N、φ_U、δv_E、δv_N，通过下面公式进一步修正车辆的姿态矩阵速度v

v'＝v-aδv (17)

式中，为修正后的姿态矩阵，为粗对准获得的姿态矩阵，v'为修正后的速度，v为粗对准获得的速度，a为修正系数，φ为向量[φ_E φ_N φ_U]^T，δv为向量[δv_E δv_N 0]^T。

优选地，步骤S4中，所述车辆姿态对准系统自适应滤波的滤波增益，通过如下步骤获得：

S41.通过下面公式获取滤波增益对应的当前时刻的系统噪声方差R_k

其中

Z_k/k-1＝H X_k/k-1+V_k

式中，R_k-1为上一时刻的系统噪声方差，β_k为当前时刻的自适应滤波系数，β_k-1为上一时刻的自适应滤波系数，k＝2,…β₀＝1，b为渐消因子，Z_k/k-1为量测预测误差矩阵，V_k为系统当前时刻的量测噪声向量，V_k-1为系统上一时刻的量测噪声向量，V₁＝[0.5,0.5,2.0]^T，T_H为系统的量测更新时间；

S42.根据所述R_k，通过下面公式获取车辆姿态对准系统自适应滤波的滤波增益K_k

K_k＝P_k/k-1H^T(HP_k/k-1H^T+R_k)^-1 (19)

优选地，步骤S4中，所述根据车辆姿态对准系统自适应滤波的滤波增益获得车辆状态估计，进一步包括如下步骤：

S43.通过下面公式获得车辆姿态校准系统的状态向量估计

式中，Z_k为系统当前时刻的量测向量，等于粗对准捷联解算获得的速度

S44.通过下面公式获得上述状态向量估计的均方误差矩阵估计

S45.判断所述估计与车辆当前状态X_k、P_k是否相同，如果二者有一个不同，用所述估计替代X_k-1、P_k-1，再次更新车辆姿态对准系统的实时状态，直到二者相同为止，完成车辆姿态精对准。

与实施例1相比，本实施例提供的方法将惯性系对准方法获得的姿态结果作为初始姿态，利用4子样圆锥误差补偿获得的等效旋转矢量、4子样旋转误差补偿和划桨误差补偿获得的速度增量进行迭代对准。并且，精对准过程采用自适应卡尔曼滤波不断修正当前时刻的系统噪声方差R_k，使得最终对准结果能够精确、可靠、快速获得。

实施例3

本实施例还提供了一种实施例1或2对应的基于自适应滤波的车辆姿态对准系统，其包括三轴光纤陀螺、三轴石英加速度计、I/F信号转换器。

三轴光纤陀螺，用于实时采集车辆三轴偏离原始三轴的角度增量，将其转换成电信号，通过至I/F信号转换器传输至导航计算机。

三轴石英加速度计，用于实时测量车辆三轴上的速度增量，将其转换成电信号，通过I/F信号转换器传输至导航计算机。

I/F信号转换器，用于将上述电信号从模拟量转换成数字量。

导航计算机，用于根据接收到的数字电信号解算出车辆的所述角度增量和速度增量，对上述角度增量和速度增量分别进行误差补偿，根据补偿后的等效旋转矢量和速度增量实施粗对准，得到粗对准后车辆的姿态和速度；更新车辆姿态对准系统的实时状态，根据更新后获得的失准角、速度误差修正粗对准后车辆的姿态和速度；根据车辆姿态对准系统自适应滤波的滤波增益获得车辆状态估计，判断所述车辆状态估计与车辆当前状态是否相同；不同，则用所述车辆状态估计替代车辆上一时刻状态，再次更新车辆姿态对准系统的实时状态，直到相同为止，完成车辆姿态精对准。

本领域技术人员可以理解，实现上述实施例方法的全部或部分流程，可以通过计算机程序来指令相关的硬件来完成，所述的程序可存储于计算机可读存储介质中。其中，所述计算机可读存储介质为磁盘、光盘、只读存储记忆体或随机存储记忆体等。

以上所述，仅为本发明较佳的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，可轻易想到的变化或替换，都应涵盖在本发明的保护范围之内。

Claims (10)

1.一种基于自适应滤波的车辆姿态对准方法，其特征在于，包括如下步骤：

获取车辆当前姿态改变量，包括当前三轴偏离原始三轴的角度增量，以及沿原始三轴的速度增量；

对上述角度增量和速度增量分别进行误差补偿，根据补偿后的等效旋转矢量和速度增量实施粗对准，得到粗对准后车辆的姿态和速度；

更新车辆姿态对准系统的实时状态，根据更新后获得的失准角、速度误差修正粗对准后车辆的姿态和速度；

根据车辆姿态对准系统自适应滤波的滤波增益获得车辆状态估计，判断所述车辆状态估计与车辆当前状态是否相同；不同，则用所述车辆状态估计替代车辆上一时刻状态，再次更新车辆姿态对准系统的实时状态，直到相同为止，完成车辆姿态精对准。

2.根据权利要求1所述的基于自适应滤波的车辆姿态对准方法，其特征在于，所述获取车辆当前姿态改变量，进一步包括如下步骤：

通过三轴光纤陀螺，实时采集在所述载体坐标系内车辆当前三轴偏离原始三轴的角度增量；

通过三轴石英加速度计，实时采集在所述载体坐标系内车辆沿原始三轴的速度增量；

所述载体坐标系的中心设置于车辆重心位置，x轴正向为沿车辆横轴向右，y轴正向为沿车辆纵轴向前，z轴正向为沿车辆立轴向上。

3.根据权利要求2所述的基于自适应滤波的车辆姿态对准方法，其特征在于，所述对角度增量进行误差补偿，进一步包括如下步骤：

连续获取等间隔时间的4组角度增量数据；

对上述角度增量数据进行4子样圆锥误差补偿，获得补偿后的等效旋转矢量φ(T)。

4.根据权利要求3所述的基于自适应滤波的车辆姿态对准方法，其特征在于，所述对速度增量进行误差补偿，进一步包括如下步骤：

连续获取等间隔时间的4组速度增量数据；

对上述速度增量数据进行4子样旋转误差补偿和划桨误差补偿，获得补偿后的速度增量ΔV(T)。

5.根据权利要求1-4之一所述的基于自适应滤波的车辆姿态对准方法，其特征在于，所述根据补偿后的等效旋转矢量和速度增量实施粗对准，得到粗对准后车辆的姿态和速度，进一步包括如下步骤：

根据补偿后的等效旋转矢量φ(T)和速度增量ΔV(T)，通过惯性系对准方法初步获取车辆的姿态矩阵

将上述姿态矩阵作为初始姿态，结合补偿后的等效旋转矢量φ(T)和补偿后的速度增量ΔV(T)进行捷联解算，更新车辆的姿态矩阵速度v和位置p，直到捷联解算算法收敛为止；

对上述捷联解算算法收敛后获得的车辆的姿态矩阵进行分析，获得粗对准后车辆的姿态，同时，将上述捷联解算算法收敛获得的车辆的速度作为获得粗对准后车辆的速度，完成车辆粗对准。

6.根据权利要求5所述的基于自适应滤波的车辆姿态对准方法，其特征在于，所述通过惯性系对准方法初步获取车辆的姿态矩阵进一步包括如下步骤：

以车辆重心为中心，指向东为x轴正向，指向北为y轴正向，指向上为z轴正向，建立导航坐标系；

通过下面公式获取初始时刻导航坐标系相对于当前时刻导航坐标系的转换矩阵

式中，ω_ie为地球自转角速率，t为采样时间，L为车辆所处地理纬度；

通过下面公式获取当前时刻载体坐标系相对于初始时刻载体坐标系的转换矩阵

式中，φ(T)×为向量φ(T)的反对称阵运算，I为单位阵；

通过下面公式获取初始时刻载体坐标系相对于初始时刻导航坐标系的转换矩阵

其中

式中，为当前解算时刻相对初始时刻导航坐标系速度，为上一解算时刻相对初始时刻导航坐标系速度，为当前解算时刻相对初始时刻载体坐标系速度，为上一解算时刻相对初始时刻载体坐标系速度，为上一解算时刻获得的车辆的姿态矩阵，初值为0；

根据上述通过下面公式获取车辆当前解算时刻的姿态矩阵

7.根据权利要求6所述的基于自适应滤波的车辆姿态对准方法，其特征在于，所述进行捷联解算更新车辆的姿态矩阵速度v和位置p，进一步包括如下步骤：

将上述车辆当前解算时刻的姿态矩阵作为初始姿态通过下面公式更新车辆的姿态矩阵

其中

式中，t为采样时间，T为导航解算时间，为向量的反对称阵运算，v_N为当前时刻导航坐标系速度的北向分量，v_E为当前时刻导航坐标系速度的东向分量，R_e为地球椭圆长半轴，R_M为子午圈主曲率半径，R_N为卯酉圈主曲率半径，ω_ie为地球自转角速率，L和h分别为车辆所处地理纬度和地理高度，分别为t_m和t_m-1时刻的姿态矩阵，m＝2，…，e为地球椭圆偏心率；

将与差值的绝对值与预设阈值进行比较，如果所述差值的绝对值大于等于预设阈值，重复上述步骤，直到所述差值的绝对值小于预设阈值为止，完成车辆的姿态矩阵的更新；

设置粗对准的初始速度为0，通过下面公式更新车辆的当前时刻速度

式中，分别表示t_m、t_m-1时刻的导航坐标系速度，m＝2,…，gⁿ为重力加速度，表示t_m-1时刻的姿态矩阵，T为导航解算时间，ΔV(T)为补偿后的速度增量；

将与差值的绝对值与预设阈值进行比较，如果所述差值的绝对值大于等于预设阈值，重复上述步骤，直到所述差值的绝对值小于预设阈值为止，完成车辆的速度v更新；

通过GPS获得车辆初始位置坐标p₁，通过下面公式更新车辆的当前时刻位置坐标p_m

其中

R_Mh＝R_M+h

R_Nh＝R_N+h

式中，p_m、p_m-1分别表示t_m、t_m-1时刻的位置坐标，分别表示t_m、t_m-1时刻的导航坐标系速度，m＝2,…，L为车辆所处地理纬度，R_M为子午圈主曲率半径，R_N为卯酉圈主曲率半径，h为车辆所处地理高度；

将p_m与p_m-1差值的绝对值与预设阈值进行比较，如果所述差值的绝对值大于等于预设阈值，重复上述步骤，直到所述差值的绝对值小于预设阈值，完成车辆的位置p更新。

8.根据权利要求1-7之一所述的基于自适应滤波的车辆姿态对准方法，其特征在于，所述更新车辆姿态对准系统的实时状态，根据更新后获得的失准角、速度误差修正粗对准后车辆的姿态和速度，进一步包括如下步骤：

建立车辆姿态对准系统的状态向量X，获得对应的状态均方误差矩阵P；

根据建立的状态向量X，获取车辆姿态对准系统上一时刻对应的状态向量X_k-1，得出上一时刻对应的状态均方误差矩阵P_k-1，进而通过下面公式获取车辆姿态对准系统当前时刻对应的状态向量X_k和当前时刻对应的状态均方误差矩阵P_k

X_k＝Φ_k/k-1X_k-1+Q_k

其中

Φ_k/k-1＝I+F(t_k-1)T

Q_k＝q(t_k-1)T

式中，()×为向量的反对称阵运算，为更新后车辆的姿态矩阵，gⁿ为重力加速度，T为导航解算时间，ω_db为陀螺随机常值，ω_eb为加速度计随机常值；

通过下面公式更新车辆姿态对准系统的状态向量X以及状态均方误差矩阵P，获得预测向量X_k/k-1以及预测矩阵P_k/k-1

X_k/k-1＝Φ_k/k-1X_k-1

式中，X_k/k-1为系统的状态一步预测向量，P_k/k-1为系统的状态均方误差一步预测矩阵；

根据上述预测向量X_k/k-1中的φ_E、φ_N、φ_U、δv_E、δv_N，通过下面公式进一步修正车辆的姿态矩阵速度v

v'＝v-aδv

式中，为修正后的姿态矩阵，为粗对准获得的姿态矩阵，v'为修正后的速度，v为粗对准获得的速度，a为修正系数，φ为向量[φ_E φ_N φ_U]^T，δv为向量[δv_E δv_N 0]^T。

9.根据权利要求8所述的基于自适应滤波的车辆姿态对准方法，其特征在于，所述车辆姿态对准系统自适应滤波的滤波增益，通过如下步骤获得：

通过下面公式获取滤波增益对应的当前时刻的系统噪声方差R_k

其中

Z_k/k-1＝H X_k/k-1+V_k

式中，R_k-1为上一时刻的系统噪声方差，β_k为当前时刻的自适应滤波系数，β_k-1为上一时刻的自适应滤波系数，k＝2,…β₀＝1，b为渐消因子，Z_k/k-1为量测预测误差矩阵，V_k为系统当前时刻的量测噪声向量，V_k-1为系统上一时刻的量测噪声向量，V₁＝[0.5,0.5,2.0]^T，T_H为系统的量测更新时间；

根据所述R_k，通过下面公式获取车辆姿态对准系统自适应滤波的滤波增益K_k

K_k＝P_k/k-1H^T(HP_k/k-1H^T+R_k)^-1。

10.根据权利要求9所述的基于自适应滤波的车辆姿态对准方法，其特征在于，所述根据车辆姿态对准系统自适应滤波的滤波增益获得车辆状态估计，进一步包括如下步骤：

通过下面公式获得车辆姿态校准系统的状态向量估计

式中，Z_k为系统当前时刻的量测向量，等于粗对准捷联解算获得的速度

通过下面公式获得上述状态向量估计的均方误差矩阵估计

判断所述估计与车辆当前状态X_k、P_k是否相同，如果二者有一个不同，用所述估计替代X_k-1、P_k-1，再次更新车辆姿态对准系统的实时状态，直到二者相同为止，完成车辆姿态精对准。