CN108180925A - 一种里程计辅助车载动态对准方法 - Google Patents

Abstract

本发明涉及一种里程计辅助车载动态对准方法，其技术特点是包括：在惯导系统中，进行惯导解算输出姿态航向、速度和位置信息并更新；利用里程计的车辆位移增量输出与惯导解算的姿态角进行里程分解，完成里程计航位推算；以惯导系统的误差模型和航位推算的误差模型作为误差状态的传播方程，利用卡尔曼滤波器对惯导系统的姿态角误差进行估计和校准。本发明利用载车里程计提供的位移增量信息与惯性测量组件提供的陀螺加表数据，通过卡尔曼滤波器进行深度融合，在车辆行进过程中，完成姿态航向的精确对准与位置误差的校准功能的组合导航技术，可简化车载导航系统的启动过程，缩短了反应时间，提高了载车机动性能。

Description

一种里程计辅助车载动态对准方法

技术领域

本发明属于陆用车辆导航技术领域，尤其是一种里程计辅助车载动态对准方法。

背景技术

陆用车辆导航系统一般由惯性测量单元和里程计构成。惯性测量单元由三只陀螺仪和三只加速度计构成，敏感载体角增量和速度增量；里程计测量车辆行驶的位移增量。惯性测量单元构成惯性导航系统，装订初始位置完成对准后就可以进行导航解算。惯性导航系统又可以与里程计结合构成车辆航位推算系统，通过惯导系统的姿态航向分解车辆里程计测量的位移增量，从而完成车辆的定位解算功能。由于惯性导航系统的初始对准一般是在载车静止状态下完成的，因此，如何在车辆行驶过程中完成惯导系统的初始对准功能是目前迫切需要解决的问题。

发明内容

本发明的目地在于克服现有技术的不足，提出一种里程计辅助车载动态对准方法，解决导航系统初始启动过程中车辆必须静止不动的问题。

本发明解决其技术问题是采取以下技术方案实现的：

一种里程计辅助车载动态对准方法，包括以下步骤：

步骤1、在惯导系统中，进行惯导解算输出姿态航向、速度和位置信息并更新；

步骤2、利用里程计的车辆位移增量输出与惯导解算的姿态角进行里程分解，完成里程计航位推算；

步骤3、以惯导系统的误差模型和航位推算的误差模型作为误差状态的传播方程，利用卡尔曼滤波器对惯导系统的姿态角误差进行估计和校准。

所述步骤1采用三只陀螺仪和三只加速度构成惯性导航系统进行惯导解算，具体方法为：通过采集陀螺仪的输出角速度数据并扣除地球自转角速度以及牵连角速度，计算得到姿态角；加速度计测量的加速度通过方向余弦阵进行坐标变换到地理坐标系的北向、东向、天向轴上，积分地理坐标系上的加速度并扣除重力和有害加速度的影响得到地理坐标系的北向、东向和天向速度；利用地理坐标系的北向、东向速度，并考虑地球的曲率半径计算得到经度和纬度。

所述步骤3惯导系统误差模型包括：姿态角误差、速度误差、位置误差、陀螺常值误差和加表常值误差；所述里程计行位推算的误差模型包括：里程计定位误差、俯仰安装偏角误差、方位安装偏角误差和里程当量误差。

所述利用卡尔曼滤波器对惯导系统的姿态角误差进行估计和校准的方法包括以下步骤：

(1)卡尔曼滤波一步预测步骤：

(2)当滤波时间到时进行滤波更新步骤；

(3)当系统计时器到达修正时刻，以修正后的姿态四元数、速度、位置为初始值进入自主导航步骤。

所述步骤(1)包括状态转移阵的计算、输入噪声方差阵的计算和状态预测与误差方差预测的计算。

所述步骤(2)包括量测计算、滤波增益计算、状态估计更新和误差方差更新。

所述步骤(3)包括姿态四元数修正、速度修正和位置修正过程。

本发明的优点和积极效果是：

1、本发明以航位推算系统的位置信息与惯导系统的位置信息进行比较，利用卡尔曼滤波器对惯导系统的姿态航向误差进行实时估计与校准，可以在车辆行驶过程中完成惯导系统的初始对准，避免了在导航系统初始启动过程中车辆必须静止不动的限制，提高了导航系统的快速性和灵活性，更加高效地满足了用户车辆的行驶和导航需求。

2、本发明利用载车里程计提供的位移增量信息与惯性测量组件提供的陀螺加表数据，基于惯导系统误差模型和里程计航位推算误差模型，通过卡尔曼滤波器进行深度融合，在车辆行进过程中，完成姿态航向的精确对准与位置误差的校准功能的组合导航技术可简化车载导航系统的启动过程，缩短了反应时间，提高了载车机动性能，可解决车载导航系统在车辆行驶过程中的初始对准问题。

附图说明

图1是本发明的导航解算流程图；

图2是本发明的惯导里程计组合导航系统导航流程图。

具体实施方式

以下结合附图对本发明实施例做进一步详述。

一种里程计辅助车载动态对准方法，在包括惯性测量单元和里程计车载导航系统内实现，其利用惯性测量单元的角增量和速度增量输出进行惯性导航解算；利用里程计的车辆位移增量输出与惯导解算的姿态角进行里程分解，完成航位推算；再利用卡尔曼滤波器，以惯导系统的误差模型和航位推算的误差模型作为误差状态的传播方程，对惯导系统的姿态角误差进行估计和校准。具体包括如下步骤：

步骤1、在惯性导航系统中，进行惯导解算输出姿态航向、速度和位置信息并更新。

在本步骤中，在三只陀螺仪和三只加速度构成惯性导航系统中，进行捷联惯导解算。通过采集陀螺的输出角速度数据并扣除地球自转角速度以及牵连角速度，计算得到姿态角。加速度计测量的加速度通过方向余弦阵进行坐标变换到地理坐标系的北向、东向、天向轴上，积分地理坐标系上的加速度并扣除重力和有害加速度的影响得到地理坐标系的北向、东向和天向速度。利用地理坐标系的北向、东向速度，并考虑地球的曲率半径计算得到经度和纬度。导航解算流程如图1所示，具体方法为：

记初始姿态角对应的方向余弦矩阵对应的姿态四元数记为将转成姿态四元数形式：

k＝0，1，2，…时，利用四元数更新算法计算t_k+1时刻的姿态四元数

其中

Δθ＝[Δθ_x Δθ_y Δθ_z]^T为(t_k,t_k+1]采样周期内的陀螺输出角增量，单位：rad；

为(t_k,t_k+1]更新周期内n系相对于i系转过的角度。

为e系相对于i系的自转角速率在n系的投影，为n系相对于e系的角速率在n系上的投影，由以下两式计算：

式中的速度、纬度和曲率半径均是上一导航解算更新周期的结果。最后进行四元数归一化处理

得到更新后的姿态四元数

加速度计输出的速度增量Δv经由坐标变换得地理坐标系上的速度分量

其中，Δv＝[Δv_x Δv_y Δv_z]^T为加速度计输出的速度增量。

地理坐标系上的加速度分量为

fⁿ＝Δvⁿ/h

其中，记fⁿ＝[f_E f_N f_U]^T。

速度更新初始时刻，vⁿ(0)＝[0 0 0]^T；

k＝0，1，2，…时，在一个速度更新周期(t_k,t_k+1]内，速度更新为

其中，

δA为有害加速度，计算如下：

初始时刻，L(0)、λ(0)和S(0)为装订的纬度、经度和高程；

k＝0，1，2，…时，在一个速度更新周期(t_k,t_k+1]内，位置更新为

步骤2、利用里程计的车辆位移增量输出与惯导解算的姿态角进行里程分解，完成里程计航位推算。

记车轮接触的地平面内的右前上坐标系为车体系m系。则在该系内里程计速度表示为：

m系与惯组的b系之间的安装误差角为俯仰安装误差角α_θ和方位安装误差角α_ψ，有如下关系：

则导航系内的速度为：

其中，

则数字递推的位置更新算法如下：

其中R_N为当地地球子午圈曲率半径，R_E为当地地球卯酉圈曲率半径，h为航位推算的更新周期5ms。

步骤3、利用卡尔曼滤波器，以惯导系统的误差模型和航位推算的误差模型作为误差状态的传播方程，对惯导系统的姿态角误差进行估计和校准。

在本步骤中，选取了21维状态变量，分别是惯导系统误差状态(包括姿态角误差φ₃、速度误差δvⁿ、位置误差δp、陀螺常值误差ε和加表常值误差▽)和里程计定位系统误差状态(里程计定位误差δp_odo、俯仰安装偏角误差δα_θ、方位安装偏角误差δα_ψ和里程当量误差δK_odo)

惯导里程计组合导航的误差状态转移矩阵如下所示，包括惯导误差方程和里程计定位误差方程两部分：

其中惯导系统误差方程为

里程计定位误差方程为

F_Odo＝[F_Odo1 0_3×12 F_Odo2 F_Odo3]

如图2所示，惯导里程计组合导航系统的卡尔曼滤波器一步预测包括状态转移阵Φ_k,k-1的计算、输入噪声方差阵的计算、状态预测与误差方差预测P_k,k-1的计算三个阶段。下面分别进行说明

(1)状态转移阵Φ_k,k-1的计算

记(t_k-1,t_k]为一个预测周期，h＝t_k-t_k-1。预测周期h一般较短，状态转移阵计算如下

其中，F＝F_INS/Odo。

(2)输入噪声方差阵的计算

连续系统的系统噪声即三个陀螺和三个加速度计向量W(t)的协方差阵为Q(t)，则输入噪声的方差阵为

Q_q＝G(t)Q(t)G^T(t)

其中，Q(t)为常量，G(t)为噪声输入矩阵，重写如下：

Q＝diag[(ε_x)² (ε_y)² (ε_z)² (▽_x)² (▽_y)² (▽_z)²]

得到连续系统的输入噪声方差Q_q后计算Kalman滤波的输入噪声方差如下：

(3)状态预测与误差方差预测P_k,k-1的计算

初始时刻当k＝0时，对和P₀进行初始化；

k＝0，1，2，…时，递推计算

当滤波更新周期未到时，不进行滤波更新，继续进行预测，令

P_k＝P_k,k-1

当滤波更新周期到时，P_k按下一节的滤波更新计算。

当滤波时间到时进行滤波更新，滤波更新周期一般为外参考信息更新周期，滤波更新周期设为1s，分为四步计算：

(1)量测计算

按下式计算量测值：

下标s表示捷联惯导系统解算输出，下标r表示参考基准输出。

L_s、λ_s为解算纬度、经度，单位：rad；

S_s为解算高度，单位：m；

L_r、λ_r为装订纬度、经度，单位rad；

S_r为参考高度，单位：m。

(2)滤波增益计算

按下式计算滤波增益K_k：

其中，

P_k,k-1为误差方差预测计算得；

H_k＝[0_6×3 I_6×6 0_6×6]；

R_k＝diag[(R_ve)² (R_vn)² (R_vu)² (R_lat)² (R_lon)² (R_h)²]。

(3)状态估计更新

按下式计算状态估计

其中，为状态预测计算得。

(4)误差方差更新

按下式计算误差方差P_k：

当系统计时器到达修正时刻，以修正后的姿态四元数、速度、位置为初始值进入自主导航，假设t_k时刻为修正时间点：

姿态四元数修正时刻，利用Kalman滤波估计得到失准角φ_E、φ_N、φ_U，对应的第1、2、3个元素，修正角记为φ_c＝[φ_E φ_N φ_U]^T，采用四元数法修正t_k时刻的姿态四元数

其中，

通过以上步骤，实现在车辆行驶过程中完成惯导系统的初始对准功能。

需要强调的是，本发明所述的实施例是说明性的，而不是限定性的，因此本发明包括并不限于具体实施方式中所述的实施例，凡是由本领域技术人员根据本发明的技术方案得出的其他实施方式，同样属于本发明保护的范围。

Claims (7)

1.一种里程计辅助车载动态对准方法，其特征在于包括以下步骤：

步骤1、在惯导系统中，进行惯导解算输出姿态航向、速度和位置信息并更新；

步骤2、利用里程计的车辆位移增量输出与惯导解算的姿态角进行里程分解，完成里程计航位推算；

步骤3、以惯导系统的误差模型和航位推算的误差模型作为误差状态的传播方程，利用卡尔曼滤波器对惯导系统的姿态角误差进行估计和校准。

2.根据权利要求1所述的里程计辅助车载动态对准方法，其特征在于：所述步骤1采用三只陀螺仪和三只加速度构成惯性导航系统进行惯导解算，具体方法为：通过采集陀螺仪的输出角速度数据并扣除地球自转角速度以及牵连角速度，计算得到姿态角；加速度计测量的加速度通过方向余弦阵进行坐标变换到地理坐标系的北向、东向、天向轴上，积分地理坐标系上的加速度并扣除重力和有害加速度的影响得到地理坐标系的北向、东向和天向速度；利用地理坐标系的北向、东向速度，并考虑地球的曲率半径计算得到经度和纬度。

3.根据权利要求1所述的里程计辅助车载动态对准方法，其特征在于：所述步骤3惯导系统误差模型包括：姿态角误差、速度误差、位置误差、陀螺常值误差和加表常值误差；所述里程计行位推算的误差模型包括：里程计定位误差、俯仰安装偏角误差、方位安装偏角误差和里程当量误差。

4.根据权利要求1所述的里程计辅助车载动态对准方法，其特征在于：所述利用卡尔曼滤波器对惯导系统的姿态角误差进行估计和校准的方法包括以下步骤：

(1)卡尔曼滤波一步预测步骤：

(2)当滤波时间到时进行滤波更新步骤；

(3)当系统计时器到达修正时刻，以修正后的姿态四元数、速度、位置为初始值进入自主导航步骤。

5.根据权利要求4所述的里程计辅助车载动态对准方法，其特征在于：所述步骤(1)包括状态转移阵的计算、输入噪声方差阵的计算和状态预测与误差方差预测的计算。

6.根据权利要求4所述的里程计辅助车载动态对准方法，其特征在于：所述步骤(2)包括量测计算、滤波增益计算、状态估计更新和误差方差更新。

7.根据权利要求4所述的里程计辅助车载动态对准方法，其特征在于：所述步骤(3)包括姿态四元数修正、速度修正和位置修正过程。