[发明专利]基于相对运动模型约束的车辆协同导航方法

权利要求书

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

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

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

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

γ^A+γ^B＝0

θ^A+θ^B＝0

ψ^A-ψ^B＝π

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

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

其中，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，对状态方程和量测方程离散化处理，在会车时进行量测更新，对系统状态量进行反馈校正。

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车辆加速度计在载体系下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轴上的一阶马尔可夫过程漂移。