CN110285804A - 基于相对运动模型约束的车辆协同导航方法 - Google Patents

Abstract

本发明公开了一种基于相对运动模型约束的车辆协同导航方法，属于车辆协同导航技术领域。该方法首先在惯性导航系统误差方程的基础上构建协同导航系统的状态方程；然后利用多普勒频移效应来计算车载无线广播由于车辆相对运动引起的频率偏移，进而判断相对会车时间；在会车时建立车辆相对运动模型和以相对运动模型为基础的量测方程；最后对状态方程和量测方程离散化处理，在会车时进行量测更新，对系统状态量进行反馈校正，实现对协同导航系统的有效修正。本发明能够对在隧道等卫星导航失效环境下有效利用车辆相对运动约束信息,实现与惯性导航的有效融合，提高车辆导航系统的精度和可靠性，适用于工程应用。

Description

基于相对运动模型约束的车辆协同导航方法

技术领域

本发明涉及一种基于相对运动模型约束的车辆协同导航方法，属于车辆协同导航技术领域。

背景技术

在现有技术下，广泛应用于车辆的导航系统多采用惯性器件(IMU-加速度计和陀螺仪)/卫星组合导航系统实现，但车辆在隧道等存在遮挡的环境下行驶时，卫星信号由于受到遮蔽导致卫星导航系统不可用，惯性/卫星组合导航系统将难以提供准确的导航信息。因此，在卫星失效的情况下，需要尽可能搜集可用的导航信息。当沿同一道路相反方向行驶的车辆会车时，其相对运动存在一定规律，利用相对运动规律构成相对运动模型约束，具有实现简单，成本低等优点，可以为车载导航系统提供重要的辅助信息。

惯性导航系统的误差主要由惯性传感器(IMU-加速度计和陀螺仪)测量误差引起，惯性导航系统的误差会随时间累积。当两辆车在平整道路上相对行驶时，在会车的瞬间姿态角存在对应的约束关系，利用这种相对约束关系构建相对运动模型，可以对两辆车的惯性导航系统误差进行修正，减小惯性导航系统的累积误差。利用车载无线广播结合多普勒频移效应原理可以有效识别会车时刻。因此，研究基于相对运动模型约束的车辆协同导航方法，借助相对运动模型来估计出惯性导航系统的测量误差，从而提高车载导航系统精度，将具有突出的应用价值。

发明内容

本发明提出了一种基于相对运动模型约束的车辆协同导航方法，在车辆行驶过程中有效利用相邻车辆的相对运动信息，提高在卫星信号不可用情况下车载导航系统的精度。

本发明为解决其技术问题采用如下技术方案：

一种基于相对运动模型约束的车辆协同导航方法，包括如下步骤：

步骤1，在惯性导航系统误差方程的基础上，将车载惯性导航系统的三个姿态误差量、三个速度误差量、三个位置误差量作为协同导航系统的状态向量构建基于卡尔曼滤波器的协同导航系统状态方程；

步骤2，针对相对行驶并不断广播导航信息的两辆车辆，利用多普勒频移效应来判断相对会车时间；

步骤3，建立车辆相对行驶情况下会车时的运动模型，模型主要包括横滚角、俯仰角和航向角的相对关系；

步骤4，根据步骤3所建立的运动模型构建协同导航系统量测方程；

步骤5，对状态方程和量测方程离散化处理，在会车时进行量测更新，对系统状态量进行反馈校正。

步骤1所述协同导航系统的状态向量为：

其中，X为协同导航系统的状态向量，X^A为协同导航系统中A车辆的状态向量，X^B为协同导航系统中B车辆的状态向量，为A车辆惯导系统的平台误差角，分别为A车辆捷联惯导系统x、y、z的三轴数学平台误差角，δV^A为A车辆的速度误差，为A车辆东北天三轴的速度误差，δp^A为A车辆的位置误差，δL^A、δλ^A、δh^A分别为A车辆经度、纬度、高度误差，为A车辆陀螺仪的随机常数漂移，分别为A车辆陀螺仪在载体系下x、y、z轴上的随机常数误差，为A车辆陀螺仪的一阶马尔科夫过程漂移，分别为A车辆陀螺仪在载体系下x、y、z轴上的一阶马尔可夫过程漂移，为A车辆加速度计一阶马尔可夫漂移，分别为A车辆加速度计在载体系下x、y、z轴上的一阶马尔可夫过程漂移，为B车辆惯导系统的平台误差角，分别为B车辆捷联惯导系统x、y、z的三轴数学平台误差角，δV^B为B车辆的速度误差，为B车辆东北天三轴的速度误差，δp^B为B车辆的位置误差，δL^B、δλ^B、δh^B分别为B车辆经度、纬度、高度误差，为B车辆陀螺仪的随机常数漂移，分别为B车辆陀螺仪在载体系下x、y、z轴上的随机常数误差，为B车辆陀螺仪的一阶马尔科夫过程漂移，分别为B车辆陀螺仪在载体系下x、y、z轴上的一阶马尔可夫过程漂移，为B车辆加速度计一阶马尔可夫过程漂移，分别为B车辆加速度计在载体系下x、y、z轴上的一阶马尔可夫过程漂移；

步骤1所述协同导航系统的状态方程为：

其中，为A车辆惯导系统的平台误差角的微分，为A车辆地理系相对于惯性系的角速率误差，为A车辆地理系相对于惯性系的角速率，表示A车辆从载体系到地理系的坐标变换矩阵，ε^A为A车辆惯导系统陀螺漂移误差，为A车辆的速度误差δv^A的微分，f^A为A车辆地理系下加速度计的输出比力，为A车辆地球系相对于惯性系的角速率误差，为A车辆地理系相对于地球系的角速率误差，v^A为A车辆的速度，为A车辆地球系相对于惯性系的角速率，为A车辆地理系相对于地球系的角速率， 分别为A车辆经度、纬度、高度误差的微分，L^A、λ^A、h^A分别为A车辆经度、纬度、高度，为A车辆陀螺仪的随机常数漂移的微分，为A车辆陀螺仪的一阶马尔科夫过程漂移的微分，为A车辆加速度计的一阶马尔可夫过程漂移；为B车辆惯导系统的平台误差角的微分，为B车辆地理系相对于惯性系的角速率误差，为B车辆地理系相对于惯性系的角速率，表示B车辆从载体系到地理系的坐标变换矩阵，ε^B为B车辆惯导系统陀螺漂移误差，为B车辆的速度误差的微分，f^B为B车辆地理系下加速度计的输出比力，为B车辆地球系相对于惯性系的角速率误差，为B车辆地理系相对于地球系的角速率误差，v^B为B车辆的速度，为B车辆地球系相对于惯性系的角速率，为B车辆地理系相对于地球系的角速率，分别为B车辆经度、纬度、高度误差的微分，L^B、λ^B、h^B分别为B车辆经度、纬度、高度，为B车辆陀螺仪的随机常数漂移的微分，为B车辆陀螺仪的一阶马尔科夫过程漂移的微分，为B车辆加速度计的一阶马尔可夫过程漂移，δg为重力加速度误差，R_m为卯酉圈半径，R_n为子午圈半径，T_g和T_a分别为一阶马尔科夫过程的相关时间，w_r和w_a分别为相关的驱动白噪声。

步骤2所述多普勒频移计算公式为：

其中，Δf_AB为频率偏移，f_t为车辆所播发广播信号的初始频率，f_d为车辆接收广播信号计算出的信号频率，r_AB为A、B两车的相对位置矢量，c为光速。

步骤3所述车辆相对行驶情况下会车时的运动模型为：

γ^A+γ^B＝0

θ^A+θ^B＝0

ψ^A-ψ^B＝π

其中，γ^A表示A车辆的横滚角，θ^A为车辆A的俯仰角，ψ^A为车辆A的航向角；γ^B表示B车辆的横滚角，θ^B为B车辆的俯仰角，ψ^B为B车辆的航向角。

步骤4所述协同导航系统量测方程为：

其中，Z(t)为量测向量；表示A车辆惯导系统输出的横滚角，为车辆A惯导系统输出的俯仰角，为车辆A惯导系统输出的航向角；表示B车辆惯导系统输出的横滚角，为B车辆惯导系统输出的俯仰角，为B车辆惯导系统输出的航向角；H(t)为量测系数矩阵；X(t)为系统状态向量；V(t)为量测噪声向量；0_15*3为所有元素均为0的15*3的矩阵；分别为A、B车辆惯性导航平台误差角到姿态误差角的转换矩阵，表示为：

其中：θ^A为车辆A的俯仰角，ψ^A为车辆A的航向角；γ^B表示B车辆的横滚角，θ^B为B车辆的俯仰角，ψ^B为B车辆的航向角。

步骤5所述的具体过程为：

(501)将系统状态方程和量测方程离散化处理：

X_k＝Φ_k,k-1X_k-1+Γ_k,k-1W_k-1

Z_k＝H_kX_k+V_k，

其中，X_k为t_k时刻系统状态量,X_k-1为t_k-1时刻系统状态量，Φ_k,k-1为t_k-1时刻至t_k时刻系统的状态转移矩阵，Γ_k,k-1为t_k-1时刻至t_k时刻系统的噪声驱动矩阵，W_k-1为t_k-1时刻系统的噪声矩阵，Z_k为t_k时刻系统的量测向量,H_k为t_k时刻的量测系数矩阵，V_k为t_k时刻的姿态观测量的噪声矩阵；

(502)对离散化处理得到的公式加入控制项U_k-1，并采用闭环修正系统状态方程对惯导系统误差进行修正：

其中，B_k,k-1为t_k-1时刻至t_k时刻系统的控制项系数矩阵，

上述公式中，F_k为t_k时刻系统矩阵的值，G_k为t_k时刻系统噪声系数矩阵的值，T为离散时间，n为阶数；

(503)得到系统的线性化卡尔曼滤波器方程：

P_k,k-1＝Φ_k,k-1P_k-1Φ_k,k-1 ^T+Γ_k-1Q_k-1Γ_k-1 ^T

其中，为t_k-1时刻至t_k时刻一步预测状态量；为t_k-1时刻滤波状态估计量；为t_k时刻滤波状态估计量；K_k为t_k时刻滤波增益矩阵，为K_k的转置；Z_k为t_k时刻的量测向量；P_k,k-1为t_k-1时刻至t_k时刻一步预测协方差矩阵；P_k-1为t_k-1时刻滤波状态估计协方差矩阵；Φ_k,k-1 ^T为Φ_k,k-1的转置矩阵；Γ_k-1为t_k-1时刻系统噪声系数矩阵；Q_k-1为t_k-1时刻的系统观测噪声估计协方差阵；Γ_k-1 ^T为Γ_k-1的转置矩阵；为H_k的转置矩阵；R_k为t_k时刻的量测噪声估计协方差阵；P_k为t_k时刻滤波状态估计协方差矩阵；I为单位矩阵；

(504)根据503)得到的线性化卡尔曼滤波器方程估计系统导航误差值，包括姿态、位置、速度误差，并用惯性导航系统推算的导航参数减去系统导航误差值，得到基于相对运动模型约束的车辆协同导航系统修正值。

本发明的有益效果如下：

1、本发明利用相对行驶的车辆在会车时的相对运动关系来修正车载惯性导航系统的误差，解决了车辆在存在遮挡的环境下卫星导航系统不可用时惯性导航系统发散的问题。

2、本发明通过将相对行驶的车辆的18维状态变量结合作为协同导航系统的状态量，建立36维状态量的卡尔曼滤波器状态方程和量测方程，以姿态角约束关系作为卡尔曼滤波器的观测量，实现对惯性导航系统误差的估计和修正，可以有效地提高组合导航系统的精度，适合工程应用。

附图说明

图1是本发明基于相对运动模型约束的车辆协同导航方法的架构图。

图2是本发明基于相对运动模型约束的车辆协同导航方法所设计相对行驶车辆的航迹图。

图3(a)是基于相对运动模型约束的车辆协同导航方法所估计出的横滚角误差曲线图；图3(b)是基于相对运动模型约束的车辆协同导航方法所估计出的俯仰角角误差曲线图；图3(c)是基于相对运动模型约束的车辆协同导航方法所估计出的航向角误差曲线图。

图4(a)代表基于相对运动模型约束的车辆协同导航方法所估计出的经度误差曲线图；图4(b)代表基于相对运动模型约束的车辆协同导航方法所估计出的纬度误差曲线图；图4(c)代表基于相对运动模型约束的车辆协同导航方法所估计出的高度误差曲线图。

图5(a)代表纯惯性方法所估计出的经度误差曲线图；图5(b)代表纯惯性方法所估计出的纬度误差曲线图；图5(c)代表纯惯性方法所估计出的高度误差曲线。

具体实施方式

下面详细描述本发明的实施方式，所述实施方式的示例在附图中示出。下面通过参考附图描述的实施方式是示例性的，仅用于解释本发明，而不能解释为对本发明的限制。

如图1所示，本发明所述的基于相对运动模型约束的车辆协同导航方法的原理是：相对行驶的车辆通过建立协同卡尔曼滤波状态方程，利用陀螺仪和加速度计原始输出进行捷联惯导解算，通过车载无线广播的多普勒频移效应判断会车时刻，在会车时刻构建相对运动模型，利用相对运动模型获得协同姿态角量测方程，进行量测更新，实现对惯性导航系统误差的估计，提高车载导航系统精度。

本发明的具体实施方式如下：

1、建立基于相对运动模型约束的车辆协同导航方法的状态方程

首先定义坐标系：地理坐标系选取“东-北-天”为导航系(n)，载体坐标系(b)选取为“右-前-上”。车载协同导航系统的状态向量为：

其中，X为协同导航系统的状态向量，X^A为协同导航系统中A车辆的状态向量，X^B为协同导航系统中B车辆的状态向量，为A车辆惯导系统的平台误差角，分别为A车辆捷联惯导系统x、y、z的三轴数学平台误差角，δV^A为A车辆的速度误差，为A车辆东北天三轴的速度误差，δp^A为A车辆的位置误差，δL^A、δλ^A、δh^A分别为A车辆经度、纬度、高度误差，为A车辆陀螺仪的随机常数漂移，分别为A车辆陀螺仪在载体系下x、y、z轴上的随机常数误差，为A车辆陀螺仪的一阶马尔科夫过程漂移，分别为A车辆陀螺仪在载体系下x、y、z轴上的一阶马尔可夫过程漂移，为A车辆加速度计一阶马尔可夫过程漂移，分别为A车辆加速度计在载体系下x、y、z轴上的一阶马尔可夫过程漂移，为B车辆惯导系统的平台误差角，分别为B车辆捷联惯导系统x、y、z的三轴数学平台误差角，δV^B为B车辆的速度误差，为B车辆东北天三轴的速度误差，δp^B为B车辆的位置误差，δL^B、δλ^B、δh^B分别为B车辆经度、纬度、高度误差，为B车辆陀螺仪的随机常数漂移，分别为B车辆陀螺仪在载体系下x、y、z轴上的随机常数误差，为B车辆陀螺仪的一阶马尔科夫过程漂移，分别为B车辆陀螺仪在载体系下x、y、z轴上的一阶马尔可夫过程漂移，为B车辆加速度计一阶马尔可夫过程漂移，分别为B车辆加速度计在载体系下x、y、z轴上的一阶马尔可夫过程漂移。协同导航系统的状态方程为：

其中，为A车辆惯导系统的平台误差角的微分，为A车辆地理系相对于惯性系的角速率误差，为A车辆地理系相对于惯性系的角速率，表示A车辆从载体系到地理系的坐标变换矩阵，ε^A为A车辆惯导系统陀螺漂移误差，为A车辆的速度误差δv^A的微分，f^A为A车辆地理系下加速度计的输出比力，为A车辆地球系相对于惯性系的角速率误差，为A车辆地理系相对于地球系的角速率误差，v^A为A车辆的速度，为A车辆地球系相对于惯性系的角速率，为A车辆地理系相对于地球系的角速率， 分别为A车辆经度、纬度、高度误差的微分，L^A、λ^A、h^A分别为A车辆经度、纬度、高度，为A车辆陀螺仪的随机常数漂移的微分，为A车辆陀螺仪的一阶马尔科夫过程漂移的微分，为A车辆加速度计的一阶马尔可夫过程漂移；为B车辆惯导系统的平台误差角的微分，为B车辆地理系相对于惯性系的角速率误差，为B车辆地理系相对于惯性系的角速率，表示B车辆从载体系到地理系的坐标变换矩阵，ε^B为B车辆惯导系统陀螺漂移误差，为B车辆的速度误差的微分，f^B为B车辆地理系下加速度计的输出比力，为B车辆地球系相对于惯性系的角速率误差，为B车辆地理系相对于地球系的角速率误差，v^B为B车辆的速度，为B车辆地球系相对于惯性系的角速率，为B车辆地理系相对于地球系的角速率，分别为B车辆经度、纬度、高度误差的微分，L^B、λ^B、h^B分别为B车辆经度、纬度、高度，为B车辆陀螺仪的随机常数漂移的微分，为B车辆陀螺仪的一阶马尔科夫过程漂移的微分，为B车辆加速度计的一阶马尔可夫过程漂移，δg为重力加速度误差，R_m为卯酉圈半径，R_n为子午圈半径，T_g和T_a分别为一阶马尔科夫过程的相关时间，w_r和w_a分别为相关的驱动白噪声。

2、利用多普勒频移公式计算无线广播信号的频率偏移，并判断会车时刻。多普勒频移计算公式为：

其中，f_t为车辆所播发广播信号的初始频率，f_d为车辆接收广播信号计算出的信号频率，r_AB为A、B两车的相对位置矢量，c为光速。

在两车A、B会车时，频率偏移Δf_AB存在由正到负的变换过程，因此的频率偏移Δf_AB的符号变换时刻即两车的会车时刻。

3、建立车辆相对行驶情况下会车时的运动模型，模型主要包括横滚角、俯仰角和航向角的相对关系，模型如下：

其中，γ^A表示A车辆的横滚角，θ^A为车辆A的俯仰角，ψ^A为车辆A的航向角；γ^B表示B车辆的横滚角，θ^B为B车辆的俯仰角，ψ^B为B车辆的航向角。

4.在式(4)的基础上建立会车时协同导航系统量测方程：

其中，Z(t)为量测向量；表示A车辆惯导系统输出的横滚角，为车辆A惯导系统输出的俯仰角，为车辆A惯导系统输出的航向角；表示B车辆惯导系统输出的横滚角，为B车辆惯导系统输出的俯仰角，为B车辆惯导系统输出的航向角；H(t)为量测系数矩阵；X(t)为系统状态向量；V(t)为量测噪声向量；0_15*3为所有元素均为0的15*3的矩阵，分别为A、B车辆惯性导航平台误差角到姿态误差角的转换矩阵，可表示为：

其中：θ^A为车辆A的俯仰角，ψ^A为车辆A的航向角；γ^B表示B车辆的横滚角，θ^B为B车辆的俯仰角，ψ^B为B车辆的航向角。

5.车辆基于相对运动模型的卡尔曼滤波

(5.1)将滤波器状态方程和量测方程离散化处理：

其中，X_k为t_k时刻系统状态量,X_k-1为t_k-1时刻系统状态量，Φ_k,k-1为t_k-1时刻至t_k时刻系统的状态转移矩阵，Γ_k,k-1为t_k-1时刻至t_k时刻系统的噪声驱动矩阵，W_k-1为t_k-1时刻系统的噪声矩阵，Z_k为t_k时刻系统的量测向量,H_k为t_k时刻的量测系数矩阵，V_k为t_k时刻的姿态观测量的噪声矩阵；

(5.2)在式(7)的基础上加入控制项U_k-1，并采用闭环修正系统状态方程对惯导系统误差进行修正：

其中，B_k,k-1为t_k-1时刻至t_k时刻系统的控制项系数矩阵，

上述公式中，F_k为t_k时刻系统矩阵的值，G_k为t_k时刻系统噪声系数矩阵的值，T为离散时间，n为阶数；

(5.3)得到系统的线性化卡尔曼滤波器方程：

其中，为t_k-1时刻至t_k时刻一步预测状态量；为t_k-1时刻滤波状态估计量；为t_k时刻滤波状态估计量；K_k为t_k时刻滤波增益矩阵，为K_k的转置；Z_k为t_k时刻的量测向量；P_k,k-1为t_k-1时刻至t_k时刻一步预测协方差矩阵；P_k-1为t_k-1时刻滤波状态估计协方差矩阵；Φ_k,k-1 ^T为Φ_k,k-1的转置矩阵；Γ_k-1为t_k-1时刻系统噪声系数矩阵；Q_k-1为t_k-1时刻的系统观测噪声估计协方差阵；Γ_k-1 ^T为Γ_k-1的转置矩阵；为H_k的转置矩阵；R_k为t_k时刻的量测噪声估计协方差阵；P_k为t_k时刻滤波状态估计协方差矩阵；I为单位矩阵；

(5.4)根据(5.3)得到的线性化卡尔曼滤波器方程估计系统导航误差值，包括姿态、位置、速度误差，并用惯性导航系统推算的导航参数减去系统导航误差值，得到基于相对运动模型约束的车辆协同导航系统修正值。

为了验证发明所提出的基于相对运动模型约束车辆协调导航方法的正确性和有效性，采用本发明方法建立模型，利用MATLAB仿真验证。设计两相对行驶车辆的航迹如图2所示。

图3是基于相对运动模型约束的车辆协同导航方法所估计出的姿态角误差与纯惯性导航系统姿态角误差对比，图3(a)、图3(b)、图3(c)分别是横滚角误差、俯仰角误差、航向角误差。

基于本发明所述的基于相对运动模型约束的车辆协同导航方法进行验证，本方法估计出的姿态角误差与纯惯性导航系统姿态角误差对比曲线如图3所示，可以看出在会车时刻，由于存在相对运动模型约束，姿态误差得到修正。

图4(a)、图4(b)、图4(c)分别代表本发明算法的经度、纬度、高度误差曲线，图5(a)、图5(b)、图5(c)分别代表纯惯性导航算法的经度、纬度、高度误差曲线。对比图4和图5的经度、纬度、高度误差曲线可以看出，采用本发明提出的基于相对运动模型约束的车辆协同导航方法，车载导航系统精度相较于纯惯性导航系统有明显提高，具有有益的工程应用价值。

以上实施例仅为说明本发明的技术思想，不能以此限定本发明的保护范围，凡是按照本发明提出的技术思想，在技术方案基础上所做的任何改动，均落入本发明保护范围之内。

Claims (7)

1.一种基于相对运动模型约束的车辆协同导航方法，其特征在于，包括如下步骤：

步骤1，在惯性导航系统误差方程的基础上，将车载惯性导航系统的三个姿态误差量、三个速度误差量、三个位置误差量作为协同导航系统的状态向量构建基于卡尔曼滤波器的协同导航系统状态方程；

步骤2，针对相对行驶并不断广播导航信息的两辆车辆，利用多普勒频移效应来判断相对会车时间；

步骤3，建立车辆相对行驶情况下会车时的运动模型，模型主要包括横滚角、俯仰角和航向角的相对关系；

步骤4，根据步骤3所建立的运动模型构建协同导航系统量测方程；

步骤5，对状态方程和量测方程离散化处理，在会车时进行量测更新，对系统状态量进行反馈校正。

2.根据权利要求1所述基于相对运动模型约束的车辆协同导航方法，其特征在于，步骤1所述协同导航系统的状态向量为：

其中，X为协同导航系统的状态向量，X^A为协同导航系统中A车辆的状态向量，X^B为协同导航系统中B车辆的状态向量，为A车辆惯导系统的平台误差角，分别为A车辆捷联惯导系统x、y、z的三轴数学平台误差角，δV^A为A车辆的速度误差，为A车辆东北天三轴的速度误差，δp^A为A车辆的位置误差，δL^A、δλ^A、δh^A分别为A车辆经度、纬度、高度误差，为A车辆陀螺仪的随机常数漂移，分别为A车辆陀螺仪在载体系下x、y、z轴上的随机常数误差，为A车辆陀螺仪的一阶马尔科夫过程漂移，分别为A车辆陀螺仪在载体系下x、y、z轴上的一阶马尔可夫过程漂移，▽^A为A车辆加速度计一阶马尔可夫漂移，分别为A车辆加速度计在载体系下x、y、z轴上的一阶马尔可夫过程漂移，为B车辆惯导系统的平台误差角，分别为B车辆捷联惯导系统x、y、z的三轴数学平台误差角，δV^B为B车辆的速度误差，为B车辆东北天三轴的速度误差，δp^B为B车辆的位置误差，δL^B、δλ^B、δh^B分别为B车辆经度、纬度、高度误差，为B车辆陀螺仪的随机常数漂移，分别为B车辆陀螺仪在载体系下x、y、z轴上的随机常数误差，为B车辆陀螺仪的一阶马尔科夫过程漂移，分别为B车辆陀螺仪在载体系下x、y、z轴上的一阶马尔可夫过程漂移，▽^B为B车辆加速度计一阶马尔可夫过程漂移，分别为B车辆加速度计在载体系下x、y、z轴上的一阶马尔可夫过程漂移。

3.根据权利要求2所述基于相对运动模型约束的车辆协同导航方法，其特征在于，步骤1所述协同导航系统的状态方程为：

其中，为A车辆惯导系统的平台误差角的微分，为A车辆地理系相对于惯性系的角速率误差，为A车辆地理系相对于惯性系的角速率，表示A车辆从载体系到地理系的坐标变换矩阵，ε^A为A车辆惯导系统陀螺漂移误差，为A车辆的速度误差δv^A的微分，f^A为A车辆地理系下加速度计的输出比力，为A车辆地球系相对于惯性系的角速率误差，为A车辆地理系相对于地球系的角速率误差，v^A为A车辆的速度，为A车辆地球系相对于惯性系的角速率，为A车辆地理系相对于地球系的角速率， 分别为A车辆经度、纬度、高度误差的微分，L^A、λ^A、h^A分别为A车辆经度、纬度、高度，为A车辆陀螺仪的随机常数漂移的微分，为A车辆陀螺仪的一阶马尔科夫过程漂移的微分，为A车辆加速度计的一阶马尔可夫过程漂移；为B车辆惯导系统的平台误差角的微分，为B车辆地理系相对于惯性系的角速率误差，为B车辆地理系相对于惯性系的角速率，表示B车辆从载体系到地理系的坐标变换矩阵，ε^B为B车辆惯导系统陀螺漂移误差，为B车辆的速度误差的微分，f^B为B车辆地理系下加速度计的输出比力，为B车辆地球系相对于惯性系的角速率误差，为B车辆地理系相对于地球系的角速率误差，v^B为B车辆的速度，为B车辆地球系相对于惯性系的角速率，为B车辆地理系相对于地球系的角速率，分别为B车辆经度、纬度、高度误差的微分，L^B、λ^B、h^B分别为B车辆经度、纬度、高度，为B车辆陀螺仪的随机常数漂移的微分，为B车辆陀螺仪的一阶马尔科夫过程漂移的微分，为B车辆加速度计的一阶马尔可夫过程漂移，δg为重力加速度误差，R_m为卯酉圈半径，R_n为子午圈半径，T_g和T_a分别为一阶马尔科夫过程的相关时间，w_r和w_a分别为相关的驱动白噪声。

4.根据权利要求1所述基于相对运动模型约束的车辆协同导航方法，其特征在于，步骤2所述多普勒频移计算公式为：

其中，Δf_AB为频率偏移，f_t为车辆所播发广播信号的初始频率，f_d为车辆接收广播信号计算出的信号频率，r_AB为A、B两车的相对位置矢量，c为光速。

5.根据权利要求1所述基于相对运动模型约束的车辆协同导航方法，其特征在于，步骤3所述车辆相对行驶情况下会车时的运动模型为：

γ^A+γ^B＝0

θ^A+θ^B＝0

ψ^A-ψ^B＝π

其中，γ^A表示A车辆的横滚角，θ^A为车辆A的俯仰角，ψ^A为车辆A的航向角；γ^B表示B车辆的横滚角，θ^B为B车辆的俯仰角，ψ^B为B车辆的航向角。

6.根据权利要求1所述基于相对运动模型约束的车辆协同导航方法，其特征在于，步骤4所述协同导航系统量测方程为：

其中，Z(t)为量测向量；表示A车辆惯导系统输出的横滚角，为车辆A惯导系统输出的俯仰角，为车辆A惯导系统输出的航向角；表示B车辆惯导系统输出的横滚角，为B车辆惯导系统输出的俯仰角，为B车辆惯导系统输出的航向角；H(t)为量测系数矩阵；X(t)为系统状态向量；V(t)为量测噪声向量；0_15*3为所有元素均为0的15*3的矩阵；分别为A、B车辆惯性导航平台误差角到姿态误差角的转换矩阵，表示为：

其中：θ^A为车辆A的俯仰角，ψ^A为车辆A的航向角；γ^B表示B车辆的横滚角，θ^B为B车辆的俯仰角，ψ^B为B车辆的航向角。

7.根据权利要求1所述基于相对运动模型约束的车辆协同导航方法，其特征在于，步骤5所述的具体过程为：

(501)将系统状态方程和量测方程离散化处理：

X_k＝Φ_k,k-1X_k-1+Γ_k,k-1W_k-1

Z_k＝H_kX_k+V_k，

其中，X_k为t_k时刻系统状态量,X_k-1为t_k-1时刻系统状态量，Φ_k,k-1为t_k-1时刻至t_k时刻系统的状态转移矩阵，Γ_k,k-1为t_k-1时刻至t_k时刻系统的噪声驱动矩阵，W_k-1为t_k-1时刻系统的噪声矩阵，Z_k为t_k时刻系统的量测向量,H_k为t_k时刻的量测系数矩阵，V_k为t_k时刻的姿态观测量的噪声矩阵；

(502)对离散化处理得到的公式加入控制项U_k-1，并采用闭环修正系统状态方程对惯导系统误差进行修正：

其中，B_k,k-1为t_k-1时刻至t_k时刻系统的控制项系数矩阵，

上述公式中，F_k为t_k时刻系统矩阵的值，G_k为t_k时刻系统噪声系数矩阵的值，T为离散时间，n为阶数；

(503)得到系统的线性化卡尔曼滤波器方程：

P_k,k-1＝Φ_k,k-1P_k-1Φ_k,k-1 ^T+Γ_k-1Q_k-1Γ_k-1 ^T

P_k＝(I-K_kH_k)P_k,k-1(I-K_kH_k)^T+K_kR_kK_k ^T，

其中，为t_k-1时刻至t_k时刻一步预测状态量；为t_k-1时刻滤波状态估计量；为t_k时刻滤波状态估计量；K_k为t_k时刻滤波增益矩阵，为K_k的转置；Z_k为t_k时刻的量测向量；P_k,k-1为t_k-1时刻至t_k时刻一步预测协方差矩阵；P_k-1为t_k-1时刻滤波状态估计协方差矩阵；Φ_k,k-1 ^T为Φ_k,k-1的转置矩阵；Γ_k-1为t_k-1时刻系统噪声系数矩阵；Q_k-1为t_k-1时刻的系统观测噪声估计协方差阵；Γ_k-1 ^T为Γ_k-1的转置矩阵；为H_k的转置矩阵；R_k为t_k时刻的量测噪声估计协方差阵；P_k为t_k时刻滤波状态估计协方差矩阵；I为单位矩阵；

(504)根据503)得到的线性化卡尔曼滤波器方程估计系统导航误差值，包括姿态、位置、速度误差，并用惯性导航系统推算的导航参数减去系统导航误差值，得到基于相对运动模型约束的车辆协同导航系统修正值。