CN109059913A - 一种用于车载导航系统的零延迟组合导航初始化方法 - Google Patents

Abstract

本发明提供一种用于车载导航系统的零延迟组合导航初始化方法，在车载的GNSS/惯性组合导航系统中加入里程计辅助，在系统断电时存储当前时刻的位置和姿态信息并在系统下一次通电工作时读取该信息；当系统上电时有GNSS定位信息和定向信息时，采用GNSS定位信息和定向信息对系统进行初始化；当系统上电时只有GNSS定位信息时，采用GNSS和IMU输出进行快速初始化并平滑系统初始姿态；采用GNSS和里程计快速修正初始姿态角。本发明实现的车载低精度组合导航方案可以满足在复杂应用环境下快速准确初始化的需求。

Description

一种用于车载导航系统的零延迟组合导航初始化方法

技术领域

本发明涉及导航领域，特别是涉及一种用于车载导航系统的零延迟组合导航初始化方法。

背景技术

导航技术多年来一直受到各个国家的重视且发展迅速，并取得了一定的成果，但在复杂应用环境下快速准确初始化一直是一大难题。尤其是在智能驾驶技术快速发展的背景下，对低精度惯性组合导航系统初始化时间和精度提出了更高的要求。

低精度捷联惯导系统(Strapdown Inertial Navigation System，SINS)在进入导航工作之前，必须进行初始对准。初始对准的精度和时间是其两项重要指标，对于车载导航系统来说，初始对准的高精度和快速性标志着系统的精确定位定姿和快速反应的能力。为了提高车载导航系统的快速反应能力，要求车载捷联惯导系统能够在复杂使用环境条件下完成初始对准。通常来讲，低精度SINS要实现初始对准，需要外界提供辅助信息，采用全球导航卫星系统(Global Navigation Satellite System，GNSS)提供位置和姿态信息进行初始对准的方法，它在GNSS信号丢失或者受到干扰时，无法完成初始对准，对于智能驾驶系统的应用环境考虑，系统可能在工作初期的某一段时间都无法接受到有效的卫星信号，在这种情况下如果不对惯性组合导航系统初始化方式进行优化，系统很难满足车载导航系统实时快速反应的需求。

发明内容

本发明为了满足车载导航系统实时快速反应的需求，提出一种用于车载导航系统的零延迟组合导航初始化方法。

本发明的技术方案提供一种用于车载导航系统的零延迟组合导航初始化方法，在车载的GNSS/惯性组合导航系统中加入里程计辅助，在系统断电时存储当前时刻的位置和姿态信息并在系统下一次通电工作时读取该信息；

当系统上电时有GNSS定位信息和定向信息时，采用GNSS定位信息和定向信息对系统进行初始化；当系统上电时只有GNSS定位信息时，采用GNSS和IMU输出进行快速初始化并平滑系统初始姿态；

采用GNSS和里程计快速修正初始姿态角。

而且，所述采用GNSS和里程计快速修正初始姿态角，包括结合GNSS输出信息和里程计信息，利用里程计DR推算的位置信息和GNSS位置信息建立系统量测方程，并通过初始误差传播方程建立系统状态方程，对系统初始获取的姿态角进行估计和补偿。

而且，所述采用GNSS和里程计快速修正初始姿态角，采用以下步骤实现，

A、建立GNSS和里程计组合系统状态向量X_k如下，

X_k＝[δk_od δθ_od δψ_od] (1)

其中，δk_od为刻度系数误差，δψ_od为航向角误差，δθ_od为俯仰角误差；

B、建立GNSS和里程计组合的系统量测状态向量Z(k)如下，

Z(k)＝[E_g(k) N_g(k) U_g(k)]^T-[E_dr(k) N_dr(k) U_dr(k)]^T (2)

其中，E_g(k)、N_g(k)和U_g(k)分别是GNSS在k时刻相对k-1时刻在地理东向、北向和天向的相对位置；E_dr(k)、N_dr(k)和U_dr(k)分别是里程计在k时刻相对k-1时刻在地理东向、北向和天向的相对位置；

C、并建立传递对准的系统状态方程和系统量测方程，如下，

Z_k＝HX_k+V (3)

其中，w为状态向量噪声矩阵，V为量测向量噪声矩阵，H为量测矩阵，X_k-1是k-1时刻的GNSS和里程计组合系统状态向量。

D、根据步骤C建立的系统状态方程和系统量测方程，采用Kalman滤波器实时估计GNSS和里程计系统状态变量X_k，并采用估计结果对系统初始状态进行修正。

本发明所提出用于车载导航系统的零延迟组合导航初始化方法，相较于现有技术具有以下优点：

(1)提出采用系统断电存储姿态信息对下一次上电工作赋初值，完成初始化。该技术能适应于复杂的车载应用环境，例如系统在GNSS失锁环境下启动，根据上电读取的信息、惯性组件敏感值和里程敏感值完成导航功能。断电存储信息中包含导航滤波系统状态向量和误差矩阵信息，该信息用于导航状态中滤波器快速初始化，降低系统导航各误差状态滤波收时间。

(2)提出采用GNSS和惯性测量单元(Inertial Measurement Unit,IMU)输出信息进行初始化。考虑到断电存储的姿态信息可能与真值存在偏差，当系统上电时有GNSS定位信息和定向信息时，采用GNSS定位信息和定向信息对系统进行初始化；当系统上电时只有GNSS定位信息时，采用GNSS和IMU输出进行快速初始化，对连续接收到的几秒GNSS数据和IMU数据进行平滑，快速计算系统当前时刻姿态角，既弥补GNSS信号复杂情况下，双天线航向不可用的功能缺陷；又规避了单独依赖单天线GNSS航迹角初始化时需要连续直线行驶的约束。

(3)提出采用GNSS和里程计快速修正初始姿态角。考虑到系统获取初始姿态角与真值之间存在一定偏差，为了提高系统初始化精度，结合GNSS输出信息和里程计信息，利用里程计DR推算的位置信息和GNSS位置信息建立系统量测方程，并通过初始误差传播方程建立系统状态方程，对系统初始获取的姿态角进行估计和补偿。

正是在上述三种技术的辅助下，本发明实现的车载低精度组合导航技术方案可以满足在复杂应用环境下快速准确初始化的需求。

附图说明

图1为本发明实施例的流程图。

具体实施方式

为了更好地理解本发明的技术方案，下面结合附图和实施例对本发明做进一步的详细说明。

本发明通过在GNSS/惯性组合导航系统中加入里程计辅助，同时通过软硬件共同设计突破系统工作模式创新，在系统断电时存储当前时刻的位置和姿态信息并在系统下一次通电工作时读取该信息，在车载组合导航系统复杂的应用环境条件下满足系统快速准确初始化的能力，同时将惯性器件数据、里程数据和GNSS数据进行数据融合，兼顾GNSS系统、惯性导航系统和里程计DR系统各自的优势，对各系统时间和空间进行统一，满足系统在各类环境中快速准确初始化的需求。

本发明相较于现有的初始对准技术，一方面通过软硬件共同设计突破系统工作模式创新，在里程计、GNSS、惯性系统和寄存器读取初始信息的辅助下，降低车载低精度组合导航系统对GNSS信号的依赖，提高系统初始化的效率和精度，另一方面通过里程辅助GNSS和惯性系统提高系统定位精度和容错性。

参见图1，本发明实施例包括以下步骤：

本发明提出了一种零延迟组合导航初始化方法，系统的工作流程如图1所示，其主要步骤包括：

第一步：系统上电启动；

首先车载导航系统上电启动。

第二步：采用系统断电存储的位置信息(经度信息λ₀，纬度信息高度信息h₀)和姿态信息(航向角信息ψ₀，横滚角信息γ₀，俯仰角信息θ₀)，进行初始化。

具体实施时，可采用FLASH进行断电存储，断电存储信息中可包含导航滤波系统状态向量和误差矩阵信息，用于导航状态中滤波器快速初始化，降低系统导航各误差状态滤波收时间。

实施例中，系统自动读取寄存器存储的经度信息λ₀，纬度信息高度信息h₀，航向角信息ψ₀，横滚角信息γ₀，俯仰角信息θ₀，里程计刻度系数误差δk_od0，里程计航向角误差δψ_od0，里程计俯仰角误差δθ_od0；

第三步：系统根据GNSS信息状态判断当前定位信息是否可用，如果GNSS定位信息有效，采用GNSS定位信息给系统位置赋初值并进入第四步，如果GNSS定位信息无效，进入第五步；

第四步：采用GNSS定位信息辅助组合导航系统初始化，

当系统上电时有GNSS定位信息和定向信息时，采用GNSS定位信息和定向信息对系统进行初始化；当系统上电时只有GNSS定位信息时，采用GNSS和IMU输出进行快速初始化，对连续接收到的几秒GNSS数据和IMU数据进行平滑，快速计算系统当前时刻姿态角。具体步骤如下：

A、系统根据GNSS信息状态判断当前定向信息是否可用，如果GNSS定向信息有效，进入B，如果GNSS定向信息无效，进入C；

B、根据GNSS定向信息和IMU信息平滑系统初始姿态，计算GNSS航向增量信息和IMU角增量信息进行平滑，将数据平滑结果作为系统姿态初值，完成姿态初始化过程，然后进入步骤D；

C、根据GNSS定位信息和IMU信息平滑系统初始姿态，可以计算前后连续几帧GNSS定位信息对应的航迹角，同时计算航迹角增量信息和IMU角增量信息进行平滑，将数据平滑结果作为系统姿态初值，完成姿态初始化过程，然后进入步骤D；

D、系统获得初始位置和姿态信息后，进入第六步，对初始信息进行滤波修正；

第五步：采用寄存器读取信息辅助组合导航系统初始化，将寄存器读取的位置信息(经度信息λ₀，纬度信息高度信息h₀)和姿态信息(航向角信息ψ₀，横滚角信息γ₀，俯仰角信息θ₀)给系统各状态赋初值，同时采用陀螺输出信息更新航向角，根据里程计信息和IMU输出信息修正系统水平姿态信息(具体实施时，可以通过里程计输出的速度信息和IMU推算的速度信息建立滤波方程，再根据速度传播方程与水平姿态误差之间的函数关系修正系统水平姿态信息)，然后进入第六步；

第六步：系统继续根据GNSS信息状态判断当前定位信息是否可用，

如果GNSS定位信息无效，根据寄存器存储的误差修正信息(里程计刻度系数误差δk_od0，里程计航向角误差δψ_od0，里程计俯仰角误差δθ_od0)对系统初始信息进行修正；

如果GNSS定位信息有效，根据GNSS信息和里程计信息构建里程状态向量误差的状态方程和量测方程(详见公式(1))，共同估计系统初始信息的误差值并进行修正，具体步骤如下：

A、建立GNSS和里程计组合系统状态向量X_k如下：

X_k＝[δk_od δθ_od δψ_od] (1)

其中，δk_od为刻度系数误差，δψ_od为航向角误差，δθ_od为俯仰角误差，X_k是k时刻的GNSS和里程计组合系统状态向量。

B、建立GNSS和里程计组合的系统量测状态向量Z(k)如下：

Z(k)＝[E_g(k) N_g(k) U_g(k)]^T-[E_dr(k) N_dr(k) U_dr(k)]^T (2)

其中，E_g(k)、N_g(k)和U_g(k)分别是GNSS在k时刻相对(k-1)时刻在地理东向、北向和天向的相对位置；E_dr(k)、N_dr(k)和U_dr(k)分别是里程计在k时刻相对(k-1)时刻在地理东向、北向和天向的相对位置。

C、并建立传递对准的状态方程和量测方程，如下：

Z_k＝HX_k+V (3)

其中，w为状态向量噪声矩阵，V为量测向量噪声矩阵，H为量测矩阵，X_k-1是(k-1)时刻的GNSS和里程计组合系统状态向量。

D、根据步骤C建立的系统状态方程和系统量测方程，采用Kalman滤波器实时估计GNSS和里程计系统状态变量X_k，并采用估计结果对系统初始状态进行修正。

滤波算法的作用就是利用已知的观测量对未知的误差参数进行估计。在滤波算法设计时，主要考虑是算法的精度、鲁棒性和实时性。本发明根据里程计DR推算的位置信息和GNSS位置信息之间各状态变量关系，对应选取状态向量和量测向量，并建立传递对准的状态方程和量测方程。

具体实施时，以上步骤可采用计算机软件技术实现自动运行流程。

以上所述为本发明的一种实施例而已，但本发明不应该局限于该实施例和附图所公开的内容。所以凡是不脱离本发明所公开的精神下完成的等效或修改，都落入本发明保护的范围。

Claims (3)

1.一种用于车载导航系统的零延迟组合导航初始化方法，其特征在于：在车载的GNSS/惯性组合导航系统中加入里程计辅助，在系统断电时存储当前时刻的位置和姿态信息并在系统下一次通电工作时读取该信息；

当系统上电时有GNSS定位信息和定向信息时，采用GNSS定位信息和定向信息对系统进行初始化；当系统上电时只有GNSS定位信息时，采用GNSS和IMU输出进行快速初始化并平滑系统初始姿态；

采用GNSS和里程计快速修正初始姿态角。

2.根据权利要求1所述用于车载导航系统的零延迟组合导航初始化方法，其特征在于：所述采用GNSS和里程计快速修正初始姿态角，包括结合GNSS输出信息和里程计信息，利用里程计DR推算的位置信息和GNSS位置信息建立系统量测方程，并通过初始误差传播方程建立系统状态方程，对系统初始获取的姿态角进行估计和补偿。

3.根据权利要求2所述用于车载导航系统的零延迟组合导航初始化方法，其特征在于：所述采用GNSS和里程计快速修正初始姿态角，采用以下步骤实现，

A、建立GNSS和里程计组合系统状态向量X_k如下，

X_k＝[δk_od δθ_od δψ_od] (1)

其中，δk_od为刻度系数误差，δψ_od为航向角误差，δθ_od为俯仰角误差；

B、建立GNSS和里程计组合的系统量测状态向量Z(k)如下，

Z(k)＝[E_g(k) N_g(k) U_g(k)]^T-[E_dr(k) N_dr(k) U_dr(k)]^T (2)

其中，E_g(k)、N_g(k)和U_g(k)分别是GNSS在k时刻相对k-1时刻在地理东向、北向和天向的相对位置；E_dr(k)、N_dr(k)和U_dr(k)分别是里程计在k时刻相对k-1时刻在地理东向、北向和天向的相对位置；

C、并建立传递对准的系统状态方程和系统量测方程，如下，

Z_k＝HX_k+V (3)

其中，w为状态向量噪声矩阵，V为量测向量噪声矩阵，H为量测矩阵，X_k-1是k-1时刻的GNSS和里程计组合系统状态向量。

D、根据步骤C建立的系统状态方程和系统量测方程，采用Kalman滤波器实时估计GNSS和里程计系统状态变量X_k，并采用估计结果对系统初始状态进行修正。