CN110207691A - 一种基于数据链测距的多无人车协同导航方法 - Google Patents

Abstract

本发明公开了一种基于数据链测距的多无人车协同导航方法。本发明适用于卫星拒止环境下的无人车编队协同导航，利用数据链对无人车之间相对距离进行测量，进而通过多导航传感器信息融合方法对编队中每辆无人车节点的位置进行估计。无人车编队中每辆无人车节点搭载惯性导航系统、车辆里程计、数据链收发装置，首先通过卡尔曼滤波器对每辆无人车节点中的惯性导航系统、车辆里程计进行融合，得到每辆无人车节点的速度、位置、姿态信息；进而利用数据链测得的无人车之间的距离信息，通过等式约束对每辆无人车的位置估计结果进行优化。通过本方法能够提高无人车编队在卫星拒止条件下的导航定位精度。

Description

一种基于数据链测距的多无人车协同导航方法

技术领域

本发明属于协同导航技术领域，特别涉及了一种多无人车协同导航方法。

背景技术

在现代作战条件下，单一的无人车在执行任务时受载体承重、成本等条件的限制，不能满足现阶段的作战任务需求，无人车编队在执行作战任务时有着单一无人车无法达到的作战效能，而为了更好地实现无人车编队的协同作战，无人车编队的导航能力起着至关重要的作用。同时无人车在作战时受环境、电磁干扰的影响，GPS信号不能用，迫切需要研究在GPS拒止环境下，无人车编队的协同导航定位方法，提高无人车编队的导航性能，提高导航定位精度。

现阶段无人车编队一般采用GPS卫星定位或惯导/里程计组合导航的方式为无人车编队各节点提供导航定位服务，然而在战场恶劣环境下，GPS信号不可用以及受限于惯导/里程计工作原理的限制，导航精度并不能满足战场环境下无人车编队导航定位的需求。

发明内容

为了解决上述背景技术提出的技术问题，本发明旨在提供一种基于数据链测距的多无人车协同导航方法，提高无人车编队导航定位精度。

为了实现上述技术目的，本发明的技术方案为：

一种基于数据链测距的多无人车协同导航方法，包括以下步骤：

(1)在各无人车节点上搭载惯性导航系统、车辆里程计和数据链收发装置；

(2)周期性读取k时刻各无人车节点的里程计速度信息、陀螺仪信息和加速度计信息，并通过数据链收发装置获得无人车节点之间的相对距离信息；

(3)预测k时刻各无人车节点的姿态四元数、速度和位置；

(4)利用里程计速度信息，通过卡尔曼滤波校正惯性导航系统误差的状态方程和量测方程；

(5)利用无人车节点之间的相对距离信息建立系统约束方程，通过等式约束卡尔曼滤波完成对惯性导航系统状态量的约束估计。

进一步地，设定机体坐标系的X、Y、Z轴分别为无人车的右向、前向、上向，设定导航坐标系的X、Y、Z轴分别为东向、北向、天向。

进一步地，在步骤(3)中，采用下式预测无人车节点i在k时刻的姿态四元数：

上式中，Q_i(k)＝[q_i0(k) q_i1(k) q_i2(k) q_i3(k)]^T为k时刻的姿态四元数，上标T表示矩阵的转置；Q_i(k-1)＝[q_i0(k-1) q_i1(k-1) q_i2(k-1) q_i3(k-1)]^T为k-1时刻的姿态四元数；ΔT为离散采样周期；通过下式计算：

其中通过下式计算：

其中，为k时刻陀螺仪读取的无人车节点i机体坐标系相对于导航坐标系的角速度在机体坐标系X、Y、Z轴上的分量， 为k时刻陀螺仪的零偏在机体坐标系X、Y、Z轴上的分量；

采用下式预测速度信息：

其中， 为k时刻加速度计读取的无人车节点i机体系相对于导航系的加速度在机体坐标系X、Y、Z轴上的分量； 为k时刻加速度计零偏在机体坐标系X、Y、Z轴上的分量；g＝[0 0 g]^T，g为当地重力加速度值； 为k时刻机体坐标系相对于导航坐标系的线速度在导航坐标系X、Y、Z轴上的分量； 为k-1时刻机体坐标系相对于导航坐标系的线速度在导航坐标系X、Y、Z轴上的分量；为机体坐标系到导航坐标系的姿态矩阵，通过下式计算：

采用下式预测位置信息：

上式中， 分别为k时刻无人车节点i在导航坐标系X、Y、Z轴上的位置坐标； 分别为k-1时刻无人车节点i在导航坐标系X、Y、Z轴上的位置坐标。

进一步地，步骤(4)的具体过程如下：

(401)确定无人车节点i的等式约束卡尔曼滤波状态量：

上式中，为无人车节点i惯性导航系统的平台误差角，δv_Ei,δv_Ni,δv_Ui为无人车节点i的陀螺仪在东向、北向、天向速度误差，δL_i,δλ_i,δh_i为无人车节点i的陀螺仪纬度、精度、高度误差位置误差，ε_bxi,ε_byi,ε_bzi为无人车节点i的陀螺仪随机常数误差，ε_rxi,ε_ryi,ε_rzi为无人车节点i的陀螺仪一阶马尔科夫随机噪声，▽_xi,▽_yi,▽_zi为无人车节点i的加速度计一阶马尔科夫过程；

(402)建立惯性导航系统的误差方程，包括平台误差角方程、速度误差方程和位置误差方程：

平台误差角方程：

其中，w_ie为地球自转角速度，v_Ei、v_Ni为无人车节点i惯性导航系统东向、北向速度，δv_Ei、δv_Ni为对应的速度误差，ε_Ei、ε_Ni、ε_Ui为无人车节点i东、北、天向陀螺噪声角，L_i,λ_i,h_i为无人车节点i的陀螺仪纬度、经度、高度值，R_M＝R_e(1-2f+3fsin²L_i)，R_N＝R_e(1+fsin²L_i)，R_e＝6378137米，f＝1/298.257；

速度误差方程：

其中，v_Ui为无人车节点i惯性导航系统天向速度，δv_Ui为对应的速度误差，f_Ei、f_Ni、f_Ui为无人车节点i加速度计测得的加速度值，▽_Ei、▽_Ni、▽_Ui为无人车节点i东向、北向、天向加速度计一阶马尔可夫过程白噪声，R为地球半径，g为当地重力加速度值；

位置误差方程：

(403)建立无人车节点i的等式约束卡尔曼滤波状态方程：

其中，G_i为系统噪声系数矩阵，W_i为噪声控制向量，F_Ni为平台误差角、速度误差、位置误差构成的系统矩阵， T_gxi、T_gyi、T_gzi为三轴陀螺仪相关时间常数，T_axi、T_ayi、T_azi为三轴加速度计相关时间常数；

(404)无人车右向、上向两个方向采取零速校正的方式修正惯导东向、天向累积的速度误差，利用无人车前向的速度信息为等式约束卡尔曼滤波的量测信息对惯导北向累积的速度误差进行修正；将无人车节点i通过车辆里程计获得载体在地理坐标系下的速度信息与惯导解算的速度信息差分整理成量测方程：

Z_i＝H_iX_i+V_i

其中，量测矩阵H_i＝[0_3×3 diag[1 1 1] 0_3×12]，量测噪声矩阵V_i＝[N_Ei N_Ni N_Ui]，N_Ei、N_Ni、N_Ui分别为车辆里程计在无人车右向、前向、上向的测速高斯白噪声。

进一步地，在步骤(5)中，首先利用无人车节点间的相对距离信息建立系统的约束方程

上式中，X为状态变量，T₁为与状态变量同维数的矩阵，D₀表示导航数据链相对测距高斯白噪声，上标T表示转置；

然后构建拉格朗日函数L(X,λ)：

上式中，为X的估计值，λ为拉格朗日乘子，P_k为均方误差；

求解得到：

X＝G_L ^-1V_L(λΔΔ^T+I)^-1P_L

上式中，G_L为下三角矩阵，V_L为奇异值分解酉矩阵， I为单位阵，为n个非零奇异值；

将上式代入约束方程中，得：

上式中，P_i为P_L中的第i个元素；

对上式求导并利用牛顿迭代法解得：

上式中，λ_k为λ的第k次迭代值，λ_k+1为λ的第k+1次迭代值，是h(λ)的求导值；

通过以上各式在初始条件下的情况，完成系统的时间更新、量测更新和约束更新过程。

进一步地，在步骤(5)中，通过等式约束卡尔曼滤波进行时间更新和量测更新的过程如下：

上式中，为k-1到k时刻的估计状态量，Φ_k,k-1为k-1到k时刻的状态转移矩阵，为上一步的估计状态量，为本步的估计状态量，K_k为系统k时刻的滤波增益，Z_k为k时刻的量测量，H_k为系统k时刻的量测矩阵，P_k/k-1为对应的均方误差，R_k为系统k时刻的量测噪声矩阵，P_k-1为上一步的均方误差，Γ_k,k-1为k-1到k时刻的系统输入矩阵，P_k/k为对应的均方误差，I为单位阵，上标T表示转置。

采用上述技术方案带来的有益效果：

本发明适用于卫星拒止环境下的无人车编队协同导航，利用数据链对无人车之间相对距离进行测量，进而通过多导航传感器信息融合方法对编队中每辆无人车节点的位置进行估计，导航精度相较于GPS拒止环境下惯导/里程计组合导航系统精度更高。

附图说明

图1是本发明的方法流程图。

具体实施方式

以下将结合附图，对本发明的技术方案进行详细说明。

本发明设计了一种基于数据链测距的多无人车协同导航方法，如图1所示，具体步骤如下。

步骤1：在各无人车节点上搭载惯性导航系统、车辆里程计和数据链收发装置；

步骤2：周期性读取k时刻各无人车节点的里程计速度信息、陀螺仪信息和加速度计信息，并通过数据链收发装置获得无人车节点之间的相对距离信息；

步骤3：预测k时刻各无人车节点的姿态四元数、速度和位置；

步骤4：利用里程计速度信息，通过卡尔曼滤波校正惯性导航系统误差的状态方程和量测方程；

步骤5：利用无人车节点之间的相对距离信息建立系统约束方程，通过等式约束卡尔曼滤波完成对惯性导航系统状态量的约束估计。

在本实施例中，设定机体坐标系的X、Y、Z轴分别为无人车的右向、前向、上向，设定导航坐标系的X、Y、Z轴分别为东向、北向、天向。

在本实施例中，步骤3采用如下优选方案实现：

采用下式预测无人车节点i在k时刻的姿态四元数：

上式中，Q_i(k)＝[q_i0(k) q_i1(k) q_i2(k) q_i3(k)]^T为k时刻的姿态四元数，上标T表示矩阵的转置；Q_i(k-1)＝[q_i0(k-1) q_i1(k-1) q_i2(k-1) q_i3(k-1)]^T为k-1时刻的姿态四元数；ΔT为离散采样周期；通过下式计算：

其中通过下式计算：

其中，为k时刻陀螺仪读取的无人车节点i机体坐标系相对于导航坐标系的角速度在机体坐标系X、Y、Z轴上的分量， 为k时刻陀螺仪的零偏在机体坐标系X、Y、Z轴上的分量；

采用下式预测速度信息：

其中， 为k时刻加速度计读取的无人车节点i机体系相对于导航系的加速度在机体坐标系X、Y、Z轴上的分量； 为k时刻加速度计零偏在机体坐标系X、Y、Z轴上的分量；g＝[0 0 g]^T，g为当地重力加速度值； 为k时刻机体坐标系相对于导航坐标系的线速度在导航坐标系X、Y、Z轴上的分量； 为k-1时刻机体坐标系相对于导航坐标系的线速度在导航坐标系X、Y、Z轴上的分量；为机体坐标系到导航坐标系的姿态矩阵，通过下式计算：

采用下式预测位置信息：

上式中， 分别为k时刻无人车节点i在导航坐标系X、Y、Z轴上的位置坐标； 分别为k-1时刻无人车节点i在导航坐标系X、Y、Z轴上的位置坐标。

在本实施例中，步骤4采用如下优选方案如下：

401、确定无人车节点i的等式约束卡尔曼滤波状态量：

上式中，为无人车节点i惯性导航系统的平台误差角，δv_Ei,δv_Ni,δv_Ui为无人车节点i的陀螺仪在东向、北向、天向速度误差，δL_i,δλ_i,δh_i为无人车节点i的陀螺仪纬度、精度、高度误差位置误差，ε_bxi,ε_byi,ε_bzi为无人车节点i的陀螺仪随机常数误差，ε_rxi,ε_ryi,ε_rzi为无人车节点i的陀螺仪一阶马尔科夫随机噪声，▽_xi,▽_yi,▽_zi为无人车节点i的加速度计一阶马尔科夫过程；

402、建立惯性导航系统的误差方程，包括平台误差角方程、速度误差方程和位置误差方程：

平台误差角方程：

其中，w_ie为地球自转角速度，v_Ei、v_Ni为无人车节点i惯性导航系统东向、北向速度，δv_Ei、δv_Ni为对应的速度误差，ε_Ei、ε_Ni、ε_Ui为无人车节点i东、北、天向陀螺噪声角，L_i,λ_i,h_i为无人车节点i的陀螺仪纬度、经度、高度值，R_M＝R_e(1-2f+3fsin²L_i)，R_N＝R_e(1+fsin²L_i)，R_e＝6378137米，f＝1/298.257；

速度误差方程：

其中，v_Ui为无人车节点i惯性导航系统天向速度，δv_Ui为对应的速度误差，f_Ei、f_Ni、f_Ui为无人车节点i加速度计测得的加速度值，▽_Ei、▽_Ni、▽_Ui为无人车节点i东向、北向、天向加速度计一阶马尔可夫过程白噪声，R为地球半径，g为当地重力加速度值；

位置误差方程：

403、建立无人车节点i的等式约束卡尔曼滤波状态方程：

其中，G_i为系统噪声系数矩阵，W_i为噪声控制向量，F_Ni为平台误差角、速度误差、位置误差构成的系统矩阵， T_gxi、T_gyi、T_gzi为三轴陀螺仪相关时间常数，T_axi、T_ayi、T_azi为三轴加速度计相关时间常数；

404、无人车右向、上向两个方向采取零速校正的方式修正惯导东向、天向累积的速度误差，利用无人车前向的速度信息为等式约束卡尔曼滤波的量测信息对惯导北向累积的速度误差进行修正；将无人车节点i通过车辆里程计获得载体在地理坐标系下的速度信息与惯导解算的速度信息差分整理成量测方程：

Z_i＝H_iX_i+V_i

其中，量测矩阵H_i＝[0_3×3 diag[1 1 1] 0_3×12]，量测噪声矩阵V_i＝[N_Ei N_Ni N_Ui]，N_Ei、N_Ni、N_Ui分别为车辆里程计在无人车右向、前向、上向的测速高斯白噪声。

在本实施例中，步骤5采用如下优选方案实现：

首先利用无人车节点间的相对距离信息建立系统的约束方程

上式中，X为状态变量，T₁为与状态变量同维数的矩阵，D₀表示导航数据链相对测距高斯白噪声，上标T表示转置；

然后构建拉格朗日函数L(X,λ)：

上式中，为X的估计值，λ为拉格朗日乘子，P_k为均方误差；

求解得到：

X＝G_L ^-1V_L(λΔΔ^T+I)^-1P_L

上式中，G_L为下三角矩阵，V_L为奇异值分解酉矩阵， I为单位阵，为n个非零奇异值；

将上式代入约束方程中，得：

上式中，P_i为P_L中的第i个元素；

对上式求导并利用牛顿迭代法解得：

上式中，λ_k为λ的第k次迭代值，λ_k+1为λ的第k+1次迭代值，是h(λ)的求导值；

通过以上各式在初始条件下的情况，完成系统的时间更新、量测更新和约束更新过程。

通过等式约束卡尔曼滤波进行时间更新和量测更新的过程如下：

状态一步预测方程：

状态估值方程：

滤波增益方程：

一步预测均方误差方程：

估计均方误差方程：

上式中，为k-1到k时刻的估计状态量，Φ_k,k-1为k-1到k时刻的状态转移矩阵，为上一步的估计状态量，为本步的估计状态量，K_k为系统k时刻的滤波增益，Z_k为k时刻的量测量，H_k为系统k时刻的量测矩阵，P_k/k-1为对应的均方误差，R_k为系统k时刻的量测噪声矩阵，P_k-1为上一步的均方误差，Γ_k,k-1为k-1到k时刻的系统输入矩阵，P_k/k为对应的均方误差，I为单位阵，上标T表示转置。

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

Claims (6)

1.一种基于数据链测距的多无人车协同导航方法，其特征在于，包括以下步骤：

(1)在各无人车节点上搭载惯性导航系统、车辆里程计和数据链收发装置；

(2)周期性读取k时刻各无人车节点的里程计速度信息、陀螺仪信息和加速度计信息，并通过数据链收发装置获得无人车节点之间的相对距离信息；

(3)预测k时刻各无人车节点的姿态四元数、速度和位置；

(4)利用里程计速度信息，通过卡尔曼滤波校正惯性导航系统误差的状态方程和量测方程；

(5)利用无人车节点之间的相对距离信息建立系统约束方程，通过等式约束卡尔曼滤波完成对惯性导航系统状态量的约束估计。

2.根据权利要求1所述基于数据链测距的多无人车协同导航方法，其特征在于，设定机体坐标系的X、Y、Z轴分别为无人车的右向、前向、上向，设定导航坐标系的X、Y、Z轴分别为东向、北向、天向。

3.根据权利要求1所述基于数据链测距的多无人车协同导航方法，其特征在于，在步骤(3)中，采用下式预测无人车节点i在k时刻的姿态四元数：

上式中，Q_i(k)＝[q_i0(k) q_i1(k) q_i2(k) q_i3(k)]^T为k时刻的姿态四元数，上标T表示矩阵的转置；Q_i(k-1)＝[q_i0(k-1) q_i1(k-1) q_i2(k-1) q_i3(k-1)]^T为k-1时刻的姿态四元数；ΔT为离散采样周期；通过下式计算：

其中通过下式计算：

其中，为k时刻陀螺仪读取的无人车节点i机体坐标系相对于导航坐标系的角速度在机体坐标系X、Y、Z轴上的分量， 为k时刻陀螺仪的零偏在机体坐标系X、Y、Z轴上的分量；

采用下式预测速度信息：

其中， 为k时刻加速度计读取的无人车节点i机体系相对于导航系的加速度在机体坐标系X、Y、Z轴上的分量； 为k时刻加速度计零偏在机体坐标系X、Y、Z轴上的分量；g＝[0 0 g]^T，g为当地重力加速度值； 为k时刻机体坐标系相对于导航坐标系的线速度在导航坐标系X、Y、Z轴上的分量； 为k-1时刻机体坐标系相对于导航坐标系的线速度在导航坐标系X、Y、Z轴上的分量；为机体坐标系到导航坐标系的姿态矩阵，通过下式计算：

采用下式预测位置信息：

上式中， 分别为k时刻无人车节点i在导航坐标系X、Y、Z轴上的位置坐标； 分别为k-1时刻无人车节点i在导航坐标系X、Y、Z轴上的位置坐标。

4.根据权利要求1所述基于数据链测距的多无人车协同导航方法，其特征在于，步骤(4)的具体过程如下：

(401)确定无人车节点i的等式约束卡尔曼滤波状态量：

上式中，为无人车节点i惯性导航系统的平台误差角，δv_Ei,δv_Ni,δv_Ui为无人车节点i的陀螺仪在东向、北向、天向速度误差，δL_i,δλ_i,δh_i为无人车节点i的陀螺仪纬度、精度、高度误差位置误差，ε_bxi,ε_byi,ε_bzi为无人车节点i的陀螺仪随机常数误差，ε_rxi,ε_ryi,ε_rzi为无人车节点i的陀螺仪一阶马尔科夫随机噪声，为无人车节点i的加速度计一阶马尔科夫过程；

(402)建立惯性导航系统的误差方程，包括平台误差角方程、速度误差方程和位置误差方程：

平台误差角方程：

其中，w_ie为地球自转角速度，v_Ei、v_Ni为无人车节点i惯性导航系统东向、北向速度，δv_Ei、δv_Ni为对应的速度误差，ε_Ei、ε_Ni、ε_Ui为无人车节点i东、北、天向陀螺噪声角，L_i,λ_i,h_i为无人车节点i的陀螺仪纬度、经度、高度值，R_M＝R_e(1-2f+3f sin²L_i)，R_N＝R_e(1+f sin²L_i)，R_e＝6378137米，f＝1/298.257；

速度误差方程：

其中，v_Ui为无人车节点i惯性导航系统天向速度，δv_Ui为对应的速度误差，f_Ei、f_Ni、f_Ui为无人车节点i加速度计测得的加速度值，为无人车节点i东向、北向、天向加速度计一阶马尔可夫过程白噪声，R为地球半径，g为当地重力加速度值；

位置误差方程：

(403)建立无人车节点i的等式约束卡尔曼滤波状态方程：

其中，G_i为系统噪声系数矩阵，W_i为噪声控制向量，F_Ni为平台误差角、速度误差、位置误差构成的系统矩阵， T_gxi、T_gyi、T_gzi为三轴陀螺仪相关时间常数，T_axi、T_ayi、T_azi为三轴加速度计相关时间常数；

(404)无人车右向、上向两个方向采取零速校正的方式修正惯导东向、天向累积的速度误差，利用无人车前向的速度信息为等式约束卡尔曼滤波的量测信息对惯导北向累积的速度误差进行修正；将无人车节点i通过车辆里程计获得载体在地理坐标系下的速度信息与惯导解算的速度信息差分整理成量测方程：

Z_i＝H_iX_i+V_i

其中，量测矩阵H_i＝[0_3×3 diag[1 1 1]0_3×12]，量测噪声矩阵V_i＝[N_Ei N_Ni N_Ui]，N_Ei、N_Ni、N_Ui分别为车辆里程计在无人车右向、前向、上向的测速高斯白噪声。

5.根据权利要求1所述基于数据链测距的多无人车协同导航方法，其特征在于，在步骤(5)中，首先利用无人车节点间的相对距离信息建立系统的约束方程

上式中，X为状态变量，T₁为与状态变量同维数的矩阵，D₀表示导航数据链相对测距高斯白噪声，上标T表示转置；

然后构建拉格朗日函数L(X,λ)：

上式中，为X的估计值，λ为拉格朗日乘子，P_k为均方误差；

求解得到：

X＝G_L ^-1V_L(λΔΔ^T+I)^-1P_L

上式中，G_L为下三角矩阵，V_L为奇异值分解酉矩阵， I为单位阵，为n个非零奇异值；

将上式代入约束方程中，得：

上式中，P_i为P_L中的第i个元素；

对上式求导并利用牛顿迭代法解得：

上式中，λ_k为λ的第k次迭代值，λ_k+1为λ的第k+1次迭代值，是h(λ)的求导值；

通过以上各式在初始条件下的情况，完成系统的时间更新、量测更新和约束更新过程。

6.根据权利要求1所述基于数据链测距的多无人车协同导航方法，其特征在于，在步骤(5)中，通过等式约束卡尔曼滤波进行时间更新和量测更新的过程如下：

上式中，为k-1到k时刻的估计状态量，Φ_k,k-1为k-1到k时刻的状态转移矩阵，为上一步的估计状态量，为本步的估计状态量，K_k为系统k时刻的滤波增益，Z_k为k时刻的量测量，H_k为系统k时刻的量测矩阵，P_k/k-1为对应的均方误差，R_k为系统k时刻的量测噪声矩阵，P_k-1为上一步的均方误差，Γ_k,k-1为k-1到k时刻的系统输入矩阵，P_k/k为对应的均方误差，I为单位阵，上标T表示转置。