CN110207691A - 一种基于数据链测距的多无人车协同导航方法 - Google Patents
一种基于数据链测距的多无人车协同导航方法 Download PDFInfo
- Publication number
- CN110207691A CN110207691A CN201910378890.2A CN201910378890A CN110207691A CN 110207691 A CN110207691 A CN 110207691A CN 201910378890 A CN201910378890 A CN 201910378890A CN 110207691 A CN110207691 A CN 110207691A
- Authority
- CN
- China
- Prior art keywords
- unmanned vehicle
- moment
- vehicle node
- error
- coordinate system
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Granted
Links
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
- G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
- G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
- G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
- G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
- G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
- G01C21/18—Stabilised platforms, e.g. by gyroscope
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Automation & Control Theory (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Navigation (AREA)
Abstract
本发明公开了一种基于数据链测距的多无人车协同导航方法。本发明适用于卫星拒止环境下的无人车编队协同导航,利用数据链对无人车之间相对距离进行测量,进而通过多导航传感器信息融合方法对编队中每辆无人车节点的位置进行估计。无人车编队中每辆无人车节点搭载惯性导航***、车辆里程计、数据链收发装置,首先通过卡尔曼滤波器对每辆无人车节点中的惯性导航***、车辆里程计进行融合,得到每辆无人车节点的速度、位置、姿态信息;进而利用数据链测得的无人车之间的距离信息,通过等式约束对每辆无人车的位置估计结果进行优化。通过本方法能够提高无人车编队在卫星拒止条件下的导航定位精度。
Description
技术领域
本发明属于协同导航技术领域,特别涉及了一种多无人车协同导航方法。
背景技术
在现代作战条件下,单一的无人车在执行任务时受载体承重、成本等条件的限制,不能满足现阶段的作战任务需求,无人车编队在执行作战任务时有着单一无人车无法达到的作战效能,而为了更好地实现无人车编队的协同作战,无人车编队的导航能力起着至关重要的作用。同时无人车在作战时受环境、电磁干扰的影响,GPS信号不能用,迫切需要研究在GPS拒止环境下,无人车编队的协同导航定位方法,提高无人车编队的导航性能,提高导航定位精度。
现阶段无人车编队一般采用GPS卫星定位或惯导/里程计组合导航的方式为无人车编队各节点提供导航定位服务,然而在战场恶劣环境下,GPS信号不可用以及受限于惯导/里程计工作原理的限制,导航精度并不能满足战场环境下无人车编队导航定位的需求。
发明内容
为了解决上述背景技术提出的技术问题,本发明旨在提供一种基于数据链测距的多无人车协同导航方法,提高无人车编队导航定位精度。
为了实现上述技术目的,本发明的技术方案为:
一种基于数据链测距的多无人车协同导航方法,包括以下步骤:
(1)在各无人车节点上搭载惯性导航***、车辆里程计和数据链收发装置;
(2)周期性读取k时刻各无人车节点的里程计速度信息、陀螺仪信息和加速度计信息,并通过数据链收发装置获得无人车节点之间的相对距离信息;
(3)预测k时刻各无人车节点的姿态四元数、速度和位置;
(4)利用里程计速度信息,通过卡尔曼滤波校正惯性导航***误差的状态方程和量测方程;
(5)利用无人车节点之间的相对距离信息建立***约束方程,通过等式约束卡尔曼滤波完成对惯性导航***状态量的约束估计。
进一步地,设定机体坐标系的X、Y、Z轴分别为无人车的右向、前向、上向,设定导航坐标系的X、Y、Z轴分别为东向、北向、天向。
进一步地,在步骤(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=[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惯性导航***的平台误差角,δvEi,δvNi,δvUi为无人车节点i的陀螺仪在东向、北向、天向速度误差,δLi,δλi,δhi为无人车节点i的陀螺仪纬度、精度、高度误差位置误差,εbxi,εbyi,εbzi为无人车节点i的陀螺仪随机常数误差,εrxi,εryi,εrzi为无人车节点i的陀螺仪一阶马尔科夫随机噪声,▽xi,▽yi,▽zi为无人车节点i的加速度计一阶马尔科夫过程;
(402)建立惯性导航***的误差方程,包括平台误差角方程、速度误差方程和位置误差方程:
平台误差角方程:
其中,wie为地球自转角速度,vEi、vNi为无人车节点i惯性导航***东向、北向速度,δvEi、δvNi为对应的速度误差,εEi、εNi、εUi为无人车节点i东、北、天向陀螺噪声角,Li,λi,hi为无人车节点i的陀螺仪纬度、经度、高度值,RM=Re(1-2f+3fsin2Li),RN=Re(1+fsin2Li),Re=6378137米,f=1/298.257;
速度误差方程:
其中,vUi为无人车节点i惯性导航***天向速度,δvUi为对应的速度误差,fEi、fNi、fUi为无人车节点i加速度计测得的加速度值,▽Ei、▽Ni、▽Ui为无人车节点i东向、北向、天向加速度计一阶马尔可夫过程白噪声,R为地球半径,g为当地重力加速度值;
位置误差方程:
(403)建立无人车节点i的等式约束卡尔曼滤波状态方程:
其中,Gi为***噪声系数矩阵,Wi为噪声控制向量,FNi为平台误差角、速度误差、位置误差构成的***矩阵, Tgxi、Tgyi、Tgzi为三轴陀螺仪相关时间常数,Taxi、Tayi、Tazi为三轴加速度计相关时间常数;
(404)无人车右向、上向两个方向采取零速校正的方式修正惯导东向、天向累积的速度误差,利用无人车前向的速度信息为等式约束卡尔曼滤波的量测信息对惯导北向累积的速度误差进行修正;将无人车节点i通过车辆里程计获得载体在地理坐标系下的速度信息与惯导解算的速度信息差分整理成量测方程:
Zi=HiXi+Vi
其中,量测矩阵Hi=[03×3 diag[1 1 1] 03×12],量测噪声矩阵Vi=[NEi NNi NUi],NEi、NNi、NUi分别为车辆里程计在无人车右向、前向、上向的测速高斯白噪声。
进一步地,在步骤(5)中,首先利用无人车节点间的相对距离信息建立***的约束方程
上式中,X为状态变量,T1为与状态变量同维数的矩阵,D0表示导航数据链相对测距高斯白噪声,上标T表示转置;
然后构建拉格朗日函数L(X,λ):
上式中,为X的估计值,λ为拉格朗日乘子,Pk为均方误差;
求解得到:
X=GL -1VL(λΔΔT+I)-1PL
上式中,GL为下三角矩阵,VL为奇异值分解酉矩阵, I为单位阵,为n个非零奇异值;
将上式代入约束方程中,得:
上式中,Pi为PL中的第i个元素;
对上式求导并利用牛顿迭代法解得:
上式中,λk为λ的第k次迭代值,λk+1为λ的第k+1次迭代值,是h(λ)的求导值;
通过以上各式在初始条件下的情况,完成***的时间更新、量测更新和约束更新过程。
进一步地,在步骤(5)中,通过等式约束卡尔曼滤波进行时间更新和量测更新的过程如下:
上式中,为k-1到k时刻的估计状态量,Φk,k-1为k-1到k时刻的状态转移矩阵,为上一步的估计状态量,为本步的估计状态量,Kk为***k时刻的滤波增益,Zk为k时刻的量测量,Hk为***k时刻的量测矩阵,Pk/k-1为对应的均方误差,Rk为***k时刻的量测噪声矩阵,Pk-1为上一步的均方误差,Γk,k-1为k-1到k时刻的***输入矩阵,Pk/k为对应的均方误差,I为单位阵,上标T表示转置。
采用上述技术方案带来的有益效果:
本发明适用于卫星拒止环境下的无人车编队协同导航,利用数据链对无人车之间相对距离进行测量,进而通过多导航传感器信息融合方法对编队中每辆无人车节点的位置进行估计,导航精度相较于GPS拒止环境下惯导/里程计组合导航***精度更高。
附图说明
图1是本发明的方法流程图。
具体实施方式
以下将结合附图,对本发明的技术方案进行详细说明。
本发明设计了一种基于数据链测距的多无人车协同导航方法,如图1所示,具体步骤如下。
步骤1:在各无人车节点上搭载惯性导航***、车辆里程计和数据链收发装置;
步骤2:周期性读取k时刻各无人车节点的里程计速度信息、陀螺仪信息和加速度计信息,并通过数据链收发装置获得无人车节点之间的相对距离信息;
步骤3:预测k时刻各无人车节点的姿态四元数、速度和位置;
步骤4:利用里程计速度信息,通过卡尔曼滤波校正惯性导航***误差的状态方程和量测方程;
步骤5:利用无人车节点之间的相对距离信息建立***约束方程,通过等式约束卡尔曼滤波完成对惯性导航***状态量的约束估计。
在本实施例中,设定机体坐标系的X、Y、Z轴分别为无人车的右向、前向、上向,设定导航坐标系的X、Y、Z轴分别为东向、北向、天向。
在本实施例中,步骤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=[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惯性导航***的平台误差角,δvEi,δvNi,δvUi为无人车节点i的陀螺仪在东向、北向、天向速度误差,δLi,δλi,δhi为无人车节点i的陀螺仪纬度、精度、高度误差位置误差,εbxi,εbyi,εbzi为无人车节点i的陀螺仪随机常数误差,εrxi,εryi,εrzi为无人车节点i的陀螺仪一阶马尔科夫随机噪声,▽xi,▽yi,▽zi为无人车节点i的加速度计一阶马尔科夫过程;
402、建立惯性导航***的误差方程,包括平台误差角方程、速度误差方程和位置误差方程:
平台误差角方程:
其中,wie为地球自转角速度,vEi、vNi为无人车节点i惯性导航***东向、北向速度,δvEi、δvNi为对应的速度误差,εEi、εNi、εUi为无人车节点i东、北、天向陀螺噪声角,Li,λi,hi为无人车节点i的陀螺仪纬度、经度、高度值,RM=Re(1-2f+3fsin2Li),RN=Re(1+fsin2Li),Re=6378137米,f=1/298.257;
速度误差方程:
其中,vUi为无人车节点i惯性导航***天向速度,δvUi为对应的速度误差,fEi、fNi、fUi为无人车节点i加速度计测得的加速度值,▽Ei、▽Ni、▽Ui为无人车节点i东向、北向、天向加速度计一阶马尔可夫过程白噪声,R为地球半径,g为当地重力加速度值;
位置误差方程:
403、建立无人车节点i的等式约束卡尔曼滤波状态方程:
其中,Gi为***噪声系数矩阵,Wi为噪声控制向量,FNi为平台误差角、速度误差、位置误差构成的***矩阵, Tgxi、Tgyi、Tgzi为三轴陀螺仪相关时间常数,Taxi、Tayi、Tazi为三轴加速度计相关时间常数;
404、无人车右向、上向两个方向采取零速校正的方式修正惯导东向、天向累积的速度误差,利用无人车前向的速度信息为等式约束卡尔曼滤波的量测信息对惯导北向累积的速度误差进行修正;将无人车节点i通过车辆里程计获得载体在地理坐标系下的速度信息与惯导解算的速度信息差分整理成量测方程:
Zi=HiXi+Vi
其中,量测矩阵Hi=[03×3 diag[1 1 1] 03×12],量测噪声矩阵Vi=[NEi NNi NUi],NEi、NNi、NUi分别为车辆里程计在无人车右向、前向、上向的测速高斯白噪声。
在本实施例中,步骤5采用如下优选方案实现:
首先利用无人车节点间的相对距离信息建立***的约束方程
上式中,X为状态变量,T1为与状态变量同维数的矩阵,D0表示导航数据链相对测距高斯白噪声,上标T表示转置;
然后构建拉格朗日函数L(X,λ):
上式中,为X的估计值,λ为拉格朗日乘子,Pk为均方误差;
求解得到:
X=GL -1VL(λΔΔT+I)-1PL
上式中,GL为下三角矩阵,VL为奇异值分解酉矩阵, I为单位阵,为n个非零奇异值;
将上式代入约束方程中,得:
上式中,Pi为PL中的第i个元素;
对上式求导并利用牛顿迭代法解得:
上式中,λk为λ的第k次迭代值,λk+1为λ的第k+1次迭代值,是h(λ)的求导值;
通过以上各式在初始条件下的情况,完成***的时间更新、量测更新和约束更新过程。
通过等式约束卡尔曼滤波进行时间更新和量测更新的过程如下:
状态一步预测方程:
状态估值方程:
滤波增益方程:
一步预测均方误差方程:
估计均方误差方程:
上式中,为k-1到k时刻的估计状态量,Φk,k-1为k-1到k时刻的状态转移矩阵,为上一步的估计状态量,为本步的估计状态量,Kk为***k时刻的滤波增益,Zk为k时刻的量测量,Hk为***k时刻的量测矩阵,Pk/k-1为对应的均方误差,Rk为***k时刻的量测噪声矩阵,Pk-1为上一步的均方误差,Γk,k-1为k-1到k时刻的***输入矩阵,Pk/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时刻的姿态四元数:
上式中,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=[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惯性导航***的平台误差角,δvEi,δvNi,δvUi为无人车节点i的陀螺仪在东向、北向、天向速度误差,δLi,δλi,δhi为无人车节点i的陀螺仪纬度、精度、高度误差位置误差,εbxi,εbyi,εbzi为无人车节点i的陀螺仪随机常数误差,εrxi,εryi,εrzi为无人车节点i的陀螺仪一阶马尔科夫随机噪声,为无人车节点i的加速度计一阶马尔科夫过程;
(402)建立惯性导航***的误差方程,包括平台误差角方程、速度误差方程和位置误差方程:
平台误差角方程:
其中,wie为地球自转角速度,vEi、vNi为无人车节点i惯性导航***东向、北向速度,δvEi、δvNi为对应的速度误差,εEi、εNi、εUi为无人车节点i东、北、天向陀螺噪声角,Li,λi,hi为无人车节点i的陀螺仪纬度、经度、高度值,RM=Re(1-2f+3f sin2Li),RN=Re(1+f sin2Li),Re=6378137米,f=1/298.257;
速度误差方程:
其中,vUi为无人车节点i惯性导航***天向速度,δvUi为对应的速度误差,fEi、fNi、fUi为无人车节点i加速度计测得的加速度值,为无人车节点i东向、北向、天向加速度计一阶马尔可夫过程白噪声,R为地球半径,g为当地重力加速度值;
位置误差方程:
(403)建立无人车节点i的等式约束卡尔曼滤波状态方程:
其中,Gi为***噪声系数矩阵,Wi为噪声控制向量,FNi为平台误差角、速度误差、位置误差构成的***矩阵, Tgxi、Tgyi、Tgzi为三轴陀螺仪相关时间常数,Taxi、Tayi、Tazi为三轴加速度计相关时间常数;
(404)无人车右向、上向两个方向采取零速校正的方式修正惯导东向、天向累积的速度误差,利用无人车前向的速度信息为等式约束卡尔曼滤波的量测信息对惯导北向累积的速度误差进行修正;将无人车节点i通过车辆里程计获得载体在地理坐标系下的速度信息与惯导解算的速度信息差分整理成量测方程:
Zi=HiXi+Vi
其中,量测矩阵Hi=[03×3 diag[1 1 1]03×12],量测噪声矩阵Vi=[NEi NNi NUi],NEi、NNi、NUi分别为车辆里程计在无人车右向、前向、上向的测速高斯白噪声。
5.根据权利要求1所述基于数据链测距的多无人车协同导航方法,其特征在于,在步骤(5)中,首先利用无人车节点间的相对距离信息建立***的约束方程
上式中,X为状态变量,T1为与状态变量同维数的矩阵,D0表示导航数据链相对测距高斯白噪声,上标T表示转置;
然后构建拉格朗日函数L(X,λ):
上式中,为X的估计值,λ为拉格朗日乘子,Pk为均方误差;
求解得到:
X=GL -1VL(λΔΔT+I)-1PL
上式中,GL为下三角矩阵,VL为奇异值分解酉矩阵, I为单位阵,为n个非零奇异值;
将上式代入约束方程中,得:
上式中,Pi为PL中的第i个元素;
对上式求导并利用牛顿迭代法解得:
上式中,λk为λ的第k次迭代值,λk+1为λ的第k+1次迭代值,是h(λ)的求导值;
通过以上各式在初始条件下的情况,完成***的时间更新、量测更新和约束更新过程。
6.根据权利要求1所述基于数据链测距的多无人车协同导航方法,其特征在于,在步骤(5)中,通过等式约束卡尔曼滤波进行时间更新和量测更新的过程如下:
上式中,为k-1到k时刻的估计状态量,Φk,k-1为k-1到k时刻的状态转移矩阵,为上一步的估计状态量,为本步的估计状态量,Kk为***k时刻的滤波增益,Zk为k时刻的量测量,Hk为***k时刻的量测矩阵,Pk/k-1为对应的均方误差,Rk为***k时刻的量测噪声矩阵,Pk-1为上一步的均方误差,Γk,k-1为k-1到k时刻的***输入矩阵,Pk/k为对应的均方误差,I为单位阵,上标T表示转置。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201910378890.2A CN110207691B (zh) | 2019-05-08 | 2019-05-08 | 一种基于数据链测距的多无人车协同导航方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201910378890.2A CN110207691B (zh) | 2019-05-08 | 2019-05-08 | 一种基于数据链测距的多无人车协同导航方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN110207691A true CN110207691A (zh) | 2019-09-06 |
CN110207691B CN110207691B (zh) | 2021-01-15 |
Family
ID=67786966
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201910378890.2A Active CN110207691B (zh) | 2019-05-08 | 2019-05-08 | 一种基于数据链测距的多无人车协同导航方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN110207691B (zh) |
Cited By (9)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN110598184A (zh) * | 2019-09-18 | 2019-12-20 | 南京山鹞航空科技有限公司 | 一种编队复合跟踪***数据注册误差校准方法 |
CN111044075A (zh) * | 2019-12-10 | 2020-04-21 | 上海航天控制技术研究所 | 基于卫星伪距/相对测量信息辅助的sins误差在线修正方法 |
CN111238469A (zh) * | 2019-12-13 | 2020-06-05 | 南京航空航天大学 | 一种基于惯性/数据链的无人机编队相对导航方法 |
CN111273687A (zh) * | 2020-02-17 | 2020-06-12 | 上海交通大学 | 基于gnss观测量和机间测距的多无人机协同相对导航方法 |
CN112050809A (zh) * | 2020-10-08 | 2020-12-08 | 吉林大学 | 轮式里程计与陀螺仪信息融合的无人车定向定位方法 |
CN113175931A (zh) * | 2021-04-02 | 2021-07-27 | 上海机电工程研究所 | 基于约束卡尔曼滤波的集群组网协同导航方法及*** |
CN113984073A (zh) * | 2021-09-29 | 2022-01-28 | 杭州电子科技大学 | 一种基于方位的移动机器人协同校正算法 |
CN116147429A (zh) * | 2023-03-10 | 2023-05-23 | 中国电子科技集团公司第二十六研究所 | 一种基于数据链和雷达的辅助弹体导航方法 |
CN117804475A (zh) * | 2023-11-15 | 2024-04-02 | 诚芯智联(武汉)科技技术有限公司 | 基于惯性卫星里程计和运动学约束的车载融合导航方法 |
Citations (16)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US20060074558A1 (en) * | 2003-11-26 | 2006-04-06 | Williamson Walton R | Fault-tolerant system, apparatus and method |
CN104685432A (zh) * | 2012-05-01 | 2015-06-03 | 5D机器人公司 | 分布式定位和协作行为测定 |
CN105824039A (zh) * | 2016-03-17 | 2016-08-03 | 孙红星 | 基于里程计的克服卫星失锁时的gnss/ins车载组合定位定向算法 |
DE102015211735A1 (de) * | 2015-06-24 | 2016-12-29 | Bayerische Motoren Werke Aktiengesellschaft | Engstellenassistenzsystem in einem Kraftfahrzeug |
WO2017019595A1 (en) * | 2015-07-27 | 2017-02-02 | Genghiscomm Holdings, LLC | Airborne relays in cooperative-mimo systems |
CN107289942A (zh) * | 2017-06-20 | 2017-10-24 | 南京航空航天大学 | 一种用于编队飞行的相对导航***及方法 |
CN107831783A (zh) * | 2017-11-10 | 2018-03-23 | 南昌航空大学 | 一种支持多无人机自主飞行的地面站控制*** |
CN108052110A (zh) * | 2017-09-25 | 2018-05-18 | 南京航空航天大学 | 基于双目视觉的无人机编队飞行方法和*** |
CN108415057A (zh) * | 2018-01-25 | 2018-08-17 | 南京理工大学 | 一种无人车队与路侧单元协同工作的相对定位方法 |
WO2018149145A1 (en) * | 2017-02-15 | 2018-08-23 | Beijing Didi Infinity Technology And Development Co., Ltd. | Systems and methods for on-demand service |
CN108682145A (zh) * | 2018-05-31 | 2018-10-19 | 大连理工大学 | 无人驾驶公交车的编组方法 |
CN108828140A (zh) * | 2018-04-26 | 2018-11-16 | 中国计量大学 | 一种基于粒子群算法的多无人机协同恶臭溯源方法 |
CN109029422A (zh) * | 2018-07-10 | 2018-12-18 | 北京木业邦科技有限公司 | 一种多无人机协作构建三维调查地图的方法和装置 |
CN109084785A (zh) * | 2018-07-25 | 2018-12-25 | 吉林大学 | 多车辆协同定位与地图构建方法、装置、设备及存储介质 |
CN109099912A (zh) * | 2017-08-11 | 2018-12-28 | 黄润芳 | 室外精确定位导航方法、装置、电子设备及存储介质 |
CN109211241A (zh) * | 2018-09-08 | 2019-01-15 | 天津大学 | 基于视觉slam的无人机自主定位方法 |
-
2019
- 2019-05-08 CN CN201910378890.2A patent/CN110207691B/zh active Active
Patent Citations (16)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US20060074558A1 (en) * | 2003-11-26 | 2006-04-06 | Williamson Walton R | Fault-tolerant system, apparatus and method |
CN104685432A (zh) * | 2012-05-01 | 2015-06-03 | 5D机器人公司 | 分布式定位和协作行为测定 |
DE102015211735A1 (de) * | 2015-06-24 | 2016-12-29 | Bayerische Motoren Werke Aktiengesellschaft | Engstellenassistenzsystem in einem Kraftfahrzeug |
WO2017019595A1 (en) * | 2015-07-27 | 2017-02-02 | Genghiscomm Holdings, LLC | Airborne relays in cooperative-mimo systems |
CN105824039A (zh) * | 2016-03-17 | 2016-08-03 | 孙红星 | 基于里程计的克服卫星失锁时的gnss/ins车载组合定位定向算法 |
WO2018149145A1 (en) * | 2017-02-15 | 2018-08-23 | Beijing Didi Infinity Technology And Development Co., Ltd. | Systems and methods for on-demand service |
CN107289942A (zh) * | 2017-06-20 | 2017-10-24 | 南京航空航天大学 | 一种用于编队飞行的相对导航***及方法 |
CN109099912A (zh) * | 2017-08-11 | 2018-12-28 | 黄润芳 | 室外精确定位导航方法、装置、电子设备及存储介质 |
CN108052110A (zh) * | 2017-09-25 | 2018-05-18 | 南京航空航天大学 | 基于双目视觉的无人机编队飞行方法和*** |
CN107831783A (zh) * | 2017-11-10 | 2018-03-23 | 南昌航空大学 | 一种支持多无人机自主飞行的地面站控制*** |
CN108415057A (zh) * | 2018-01-25 | 2018-08-17 | 南京理工大学 | 一种无人车队与路侧单元协同工作的相对定位方法 |
CN108828140A (zh) * | 2018-04-26 | 2018-11-16 | 中国计量大学 | 一种基于粒子群算法的多无人机协同恶臭溯源方法 |
CN108682145A (zh) * | 2018-05-31 | 2018-10-19 | 大连理工大学 | 无人驾驶公交车的编组方法 |
CN109029422A (zh) * | 2018-07-10 | 2018-12-18 | 北京木业邦科技有限公司 | 一种多无人机协作构建三维调查地图的方法和装置 |
CN109084785A (zh) * | 2018-07-25 | 2018-12-25 | 吉林大学 | 多车辆协同定位与地图构建方法、装置、设备及存储介质 |
CN109211241A (zh) * | 2018-09-08 | 2019-01-15 | 天津大学 | 基于视觉slam的无人机自主定位方法 |
Non-Patent Citations (5)
Title |
---|
BAI, SHIYU等: ""A System-Level Self-Calibration Method for Installation Errors in A Dual-Axis Rotational Inertial Navigation System"", 《SENSORS》 * |
ZHANG LIANG等: ""Multiple UAVs cooperative formation forming control based on back-stepping-like approach"", 《A SYSTEM-LEVEL SELF-CALIBRATION METHOD FOR INSTALLATION ERRORS IN A DUAL-AXIS ROTATIONAL INERTIAL NAVIGATION SYSTEM》 * |
李世超等: ""基于 TD-LTE 的多机协同导航通信***研究"", 《农业机械学报》 * |
许晓伟等: ""多无人机协同导航技术研究现状及进展"", 《导航定位与授时》 * |
陈泽: ""分队协同导航算法研究"", 《中国优秀硕士学位论文全文数据库 信息科技辑》 * |
Cited By (15)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN110598184B (zh) * | 2019-09-18 | 2020-04-28 | 南京山鹞航空科技有限公司 | 一种编队复合跟踪***数据注册误差校准方法 |
CN110598184A (zh) * | 2019-09-18 | 2019-12-20 | 南京山鹞航空科技有限公司 | 一种编队复合跟踪***数据注册误差校准方法 |
CN111044075B (zh) * | 2019-12-10 | 2023-09-15 | 上海航天控制技术研究所 | 基于卫星伪距/相对测量信息辅助的sins误差在线修正方法 |
CN111044075A (zh) * | 2019-12-10 | 2020-04-21 | 上海航天控制技术研究所 | 基于卫星伪距/相对测量信息辅助的sins误差在线修正方法 |
CN111238469A (zh) * | 2019-12-13 | 2020-06-05 | 南京航空航天大学 | 一种基于惯性/数据链的无人机编队相对导航方法 |
CN111238469B (zh) * | 2019-12-13 | 2023-09-29 | 南京航空航天大学 | 一种基于惯性/数据链的无人机编队相对导航方法 |
CN111273687A (zh) * | 2020-02-17 | 2020-06-12 | 上海交通大学 | 基于gnss观测量和机间测距的多无人机协同相对导航方法 |
CN112050809A (zh) * | 2020-10-08 | 2020-12-08 | 吉林大学 | 轮式里程计与陀螺仪信息融合的无人车定向定位方法 |
CN112050809B (zh) * | 2020-10-08 | 2022-06-17 | 吉林大学 | 轮式里程计与陀螺仪信息融合的无人车定向定位方法 |
CN113175931A (zh) * | 2021-04-02 | 2021-07-27 | 上海机电工程研究所 | 基于约束卡尔曼滤波的集群组网协同导航方法及*** |
CN113175931B (zh) * | 2021-04-02 | 2022-08-16 | 上海机电工程研究所 | 基于约束卡尔曼滤波的集群组网协同导航方法及*** |
CN113984073A (zh) * | 2021-09-29 | 2022-01-28 | 杭州电子科技大学 | 一种基于方位的移动机器人协同校正算法 |
CN113984073B (zh) * | 2021-09-29 | 2024-05-28 | 杭州电子科技大学 | 一种基于方位的移动机器人协同校正算法 |
CN116147429A (zh) * | 2023-03-10 | 2023-05-23 | 中国电子科技集团公司第二十六研究所 | 一种基于数据链和雷达的辅助弹体导航方法 |
CN117804475A (zh) * | 2023-11-15 | 2024-04-02 | 诚芯智联(武汉)科技技术有限公司 | 基于惯性卫星里程计和运动学约束的车载融合导航方法 |
Also Published As
Publication number | Publication date |
---|---|
CN110207691B (zh) | 2021-01-15 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN110207691A (zh) | 一种基于数据链测距的多无人车协同导航方法 | |
CN109813311B (zh) | 一种无人机编队协同导航方法 | |
CN105043415B (zh) | 基于四元数模型的惯性系自对准方法 | |
CN102169184B (zh) | 组合导航***中测量双天线gps安装失准角的方法和装置 | |
CN104655152B (zh) | 一种基于联邦滤波的机载分布式pos实时传递对准方法 | |
CN108731670A (zh) | 基于量测模型优化的惯性/视觉里程计组合导航定位方法 | |
CN105318876A (zh) | 一种惯性里程计组合高精度姿态测量方法 | |
CN109974697A (zh) | 一种基于惯性***的高精度测绘方法 | |
CN109186597B (zh) | 一种基于双mems-imu的室内轮式机器人的定位方法 | |
CN109596018A (zh) | 基于磁测滚转角速率信息的旋转弹飞行姿态高精度估计方法 | |
CN112432642B (zh) | 一种重力灯塔与惯性导航融合定位方法及*** | |
CN111399023B (zh) | 基于李群非线性状态误差的惯性基组合导航滤波方法 | |
CN108387236B (zh) | 一种基于扩展卡尔曼滤波的偏振光slam方法 | |
CN103822633A (zh) | 一种基于二阶量测更新的低成本姿态估计方法 | |
CN108151737A (zh) | 一种动态互观测关系条件下的无人机蜂群协同导航方法 | |
CN103542851A (zh) | 一种基于水下地形高程数据库的水下航行器辅助导航定位方法 | |
CN110440830A (zh) | 动基座下车载捷联惯导***自对准方法 | |
CN110006421A (zh) | 一种基于mems和gps的车载导航方法及*** | |
CN111220151B (zh) | 载体系下考虑温度模型的惯性和里程计组合导航方法 | |
CN105928515A (zh) | 一种无人机导航*** | |
CN117289322B (zh) | 一种基于imu、gps与uwb的高精度定位算法 | |
CN113503892A (zh) | 一种基于里程计和回溯导航的惯导***动基座初始对准方法 | |
CN110243362A (zh) | 一种中高空超声速靶标导航方法 | |
CN110849360A (zh) | 面向多机协同编队飞行的分布式相对导航方法 | |
CN103712621A (zh) | 偏振光及红外传感器辅助惯导***定姿方法 |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
PB01 | Publication | ||
PB01 | Publication | ||
SE01 | Entry into force of request for substantive examination | ||
SE01 | Entry into force of request for substantive examination | ||
GR01 | Patent grant | ||
GR01 | Patent grant |