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

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

Multi-UAV Cooperative Navigation Method Based on Data Link Ranging

【技术实现步骤摘要】
一种基于数据链测距的多无人车协同导航方法
本专利技术属于协同导航
，特别涉及了一种多无人车协同导航方法。
技术介绍
在现代作战条件下，单一的无人车在执行任务时受载体承重、成本等条件的限制，不能满足现阶段的作战任务需求，无人车编队在执行作战任务时有着单一无人车无法达到的作战效能，而为了更好地实现无人车编队的协同作战，无人车编队的导航能力起着至关重要的作用。同时无人车在作战时受环境、电磁干扰的影响，GPS信号不能用，迫切需要研究在GPS拒止环境下，无人车编队的协同导航定位方法，提高无人车编队的导航性能，提高导航定位精度。现阶段无人车编队一般采用GPS卫星定位或惯导/里程计组合导航的方式为无人车编队各节点提供导航定位服务，然而在战场恶劣环境下，GPS信号不可用以及受限于惯导/里程计工作原理的限制，导航精度并不能满足战场环境下无人车编队导航定位的需求。
技术实现思路
为了解决上述
技术介绍
提出的技术问题，本专利技术旨在提供一种基于数据链测距的多无人车协同导航方法，提高无人车编队导航定位精度。为了实现上述技术目的，本专利技术的技术方案为：一种基于数据链测距的多无人车协同导航方法，包括以下步骤：本文档来自技高网...

【技术保护点】
1.一种基于数据链测距的多无人车协同导航方法，其特征在于，包括以下步骤：(1)在各无人车节点上搭载惯性导航系统、车辆里程计和数据链收发装置；(2)周期性读取k时刻各无人车节点的里程计速度信息、陀螺仪信息和加速度计信息，并通过数据链收发装置获得无人车节点之间的相对距离信息；(3)预测k时刻各无人车节点的姿态四元数、速度和位置；(4)利用里程计速度信息，通过卡尔曼滤波校正惯性导航系统误差的状态方程和量测方程；(5)利用无人车节点之间的相对距离信息建立系统约束方程，通过等式约束卡尔曼滤波完成对惯性导航系统状态量的约束估计。

【技术特征摘要】
1.一种基于数据链测距的多无人车协同导航方法，其特征在于，包括以下步骤：(1)在各无人车节点上搭载惯性导航系统、车辆里程计和数据链收发装置；(2)周期性读取k时刻各无人车节点的里程计速度信息、陀螺仪信息和加速度计信息，并通过数据链收发装置获得无人车节点之间的相对距离信息；(3)预测k时刻各无人车节点的姿态四元数、速度和位置；(4)利用里程计速度信息，通过卡尔曼滤波校正惯性导航系统误差的状态方程和量测方程；(5)利用无人车节点之间的相对距离信息建立系统约束方程，通过等式约束卡尔曼滤波完成对惯性导航系统状态量的约束估计。2.根据权利要求1所述基于数据链测距的多无人车协同导航方法，其特征在于，设定机体坐标系的X、Y、Z轴分别为无人车的右向、前向、上向，设定导航坐标系的X、Y、Z轴分别为东向、北向、天向。3.根据权利要求1所述基于数据链测距的多无人车协同导航方法，其特征在于，在步骤(3)中，采用下式预测无人车节点i在k时刻的姿态四元数：上式中，Qi(k)＝[qi0(k)qi1(k)qi2(k)qi3(k)]T为k时刻的姿态四元数，上标T表示矩阵的转置；Qi(k-1)＝[qi0(k-1)qi1(k-1)qi2(k-1)qi3(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＝[00g]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惯性导航系统的平台误差角，δvEi,δvNi,δvUi为无人车节点i的陀螺仪在东向、北向、天向速度误差，δLi,δλi,δhi为无人车节点i的陀螺仪纬度、精度、高度误差位置误差，εbxi,εbyi,εbzi为无人车节点i的陀螺仪随机常数误差，εrxi,εryi,εrzi为无人车节点i的陀螺仪一阶马尔科夫随机噪声，为无人车节点i的加速度计一阶马尔科夫过程；(402)建立惯性导航系统的误差方程，包括平台误差角方程、速度误差方程和位置误差方程：平台误差角方程：其中，wie为地球自转角速度，vEi、vNi为无人车节点i惯性导...

【专利技术属性】
技术研发人员：朱徐东，赖际舟，吕品，周子寒，何容，
申请(专利权)人：南京航空航天大学，
类型：发明
国别省市：江苏,32