【技术实现步骤摘要】
一种基于数据链测距的多无人车协同导航方法
本专利技术属于协同导航
,特别涉及了一种多无人车协同导航方法。
技术介绍
在现代作战条件下,单一的无人车在执行任务时受载体承重、成本等条件的限制,不能满足现阶段的作战任务需求,无人车编队在执行作战任务时有着单一无人车无法达到的作战效能,而为了更好地实现无人车编队的协同作战,无人车编队的导航能力起着至关重要的作用。同时无人车在作战时受环境、电磁干扰的影响,GPS信号不能用,迫切需要研究在GPS拒止环境下,无人车编队的协同导航定位方法,提高无人车编队的导航性能,提高导航定位精度。现阶段无人车编队一般采用GPS卫星定位或惯导/里程计组合导航的方式为无人车编队各节点提供导航定位服务,然而在战场恶劣环境下,GPS信号不可用以及受限于惯导/里程计工作原理的限制,导航精度并不能满足战场环境下无人车编队导航定位的需求。
技术实现思路
为了解决上述
技术介绍
提出的技术问题,本专利技术旨在提供一种基于数据链测距的多无人车协同导航方法,提高无人车编队导航定位精度。为了实现上述技术目的,本专利技术的技术方案为:一种基于数据链测距的多无人车协同导航方法,包括以下步骤:(1)在各无人车节点上搭载惯性导航系统、车辆里程计和数据链收发装置;(2)周期性读取k时刻各无人车节点的里程计速度信息、陀螺仪信息和加速度计信息,并通过数据链收发装置获得无人车节点之间的相对距离信息;(3)预测k时刻各无人车节点的姿态四元数、速度和位置;(4)利用里程计速度信息,通过卡尔曼滤波校正惯性导航系统误差的状态方程和量测方程;(5)利用无人车节点之间的相对距离信息建立系 ...
【技术保护点】
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
还没有人留言评论。发表了对其他浏览者有用的留言会获得科技券。