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

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

Info

Publication number
CN110207691B
CN110207691B CN201910378890.2A CN201910378890A CN110207691B CN 110207691 B CN110207691 B CN 110207691B CN 201910378890 A CN201910378890 A CN 201910378890A CN 110207691 B CN110207691 B CN 110207691B
Authority
CN
China
Prior art keywords
unmanned vehicle
coordinate system
vehicle node
equation
error
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.)
Active
Application number
CN201910378890.2A
Other languages
English (en)
Other versions
CN110207691A (zh
Inventor
朱徐东
赖际舟
吕品
周子寒
何容
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Nanjing University of Aeronautics and Astronautics
Original Assignee
Nanjing University of Aeronautics and Astronautics
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by Nanjing University of Aeronautics and Astronautics filed Critical Nanjing University of Aeronautics and Astronautics
Priority to CN201910378890.2A priority Critical patent/CN110207691B/zh
Publication of CN110207691A publication Critical patent/CN110207691A/zh
Application granted granted Critical
Publication of CN110207691B publication Critical patent/CN110207691B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

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

Abstract

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

Description

一种基于数据链测距的多无人车协同导航方法
技术领域
本发明属于协同导航技术领域，特别涉及了一种多无人车协同导航方法。
背景技术
在现代作战条件下，单一的无人车在执行任务时受载体承重、成本等条件的限制，不能满足现阶段的作战任务需求，无人车编队在执行作战任务时有着单一无人车无法达到的作战效能，而为了更好地实现无人车编队的协同作战，无人车编队的导航能力起着至关重要的作用。同时无人车在作战时受环境、电磁干扰的影响，GPS信号不能用，迫切需要研究在GPS拒止环境下，无人车编队的协同导航定位方法，提高无人车编队的导航性能，提高导航定位精度。
现阶段无人车编队一般采用GPS卫星定位或惯导/里程计组合导航的方式为无人车编队各节点提供导航定位服务，然而在战场恶劣环境下，GPS信号不可用以及受限于惯导/里程计工作原理的限制，导航精度并不能满足战场环境下无人车编队导航定位的需求。
发明内容
为了解决上述背景技术提出的技术问题，本发明旨在提供一种基于数据链测距的多无人车协同导航方法，提高无人车编队导航定位精度。
为了实现上述技术目的，本发明的技术方案为：
一种基于数据链测距的多无人车协同导航方法，包括以下步骤：
(1)在各无人车节点上搭载惯性导航系统、车辆里程计和数据链收发装置；
(2)周期性读取k时刻各无人车节点的里程计速度信息、陀螺仪信息和加速度计信息，并通过数据链收发装置获得无人车节点之间的相对距离信息；
(3)预测k时刻各无人车节点的姿态四元数、速度和位置；
(4)利用里程计速度信息，通过卡尔曼滤波校正惯性导航系统误差的状态方程和量测方程；
(5)利用无人车节点之间的相对距离信息建立系统约束方程，通过等式约束卡尔曼滤波完成对惯性导航系统状态量的约束估计。
进一步地，设定机体坐标系的X、Y、Z轴分别为无人车的右向、前向、上向，设定导航坐标系的X、Y、Z轴分别为东向、北向、天向。
进一步地，在步骤(3)中，采用下式预测无人车节点i在k时刻的姿态四元数：
Figure BDA0002052645230000021
上式中，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为离散采样周期；
Figure BDA0002052645230000022
通过下式计算：
Figure BDA0002052645230000023
其中
Figure BDA0002052645230000024
通过下式计算：
Figure BDA0002052645230000025
其中，
Figure BDA0002052645230000026
为k时刻陀螺仪读取的无人车节点i机体坐标系相对于导航坐标系的角速度在机体坐标系X、Y、Z轴上的分量，
Figure BDA0002052645230000027
Figure BDA0002052645230000031
为k时刻陀螺仪的零偏在机体坐标系X、Y、Z轴上的分量；
采用下式预测速度信息：
Figure BDA0002052645230000032
其中，
Figure BDA0002052645230000033
Figure BDA0002052645230000034
为k时刻加速度计读取的无人车节点i机体系相对于导航系的加速度在机体坐标系X、Y、Z轴上的分量；
Figure BDA0002052645230000035
Figure BDA0002052645230000036
为k时刻加速度计零偏在机体坐标系X、Y、Z轴上的分量；g＝[0 0 g]T，g为当地重力加速度值；
Figure BDA0002052645230000037
Figure BDA0002052645230000038
为k时刻机体坐标系相对于导航坐标系的线速度在导航坐标系X、Y、Z轴上的分量；
Figure BDA0002052645230000039
Figure BDA00020526452300000310
Figure BDA00020526452300000311
为k-1时刻机体坐标系相对于导航坐标系的线速度在导航坐标系X、Y、Z轴上的分量；
Figure BDA00020526452300000312
为机体坐标系到导航坐标系的姿态矩阵，通过下式计算：
Figure BDA00020526452300000313
Figure BDA00020526452300000314
采用下式预测位置信息：
Figure BDA00020526452300000315
上式中，
Figure BDA00020526452300000316
Figure BDA00020526452300000317
分别为k时刻无人车节点i在导航坐标系X、Y、Z轴上的位置坐标；
Figure BDA0002052645230000041
Figure BDA0002052645230000042
分别为k-1时刻无人车节点i在导航坐标系X、Y、Z轴上的位置坐标。
进一步地，步骤(4)的具体过程如下：
(401)确定无人车节点i的等式约束卡尔曼滤波状态量：
Figure BDA0002052645230000043
上式中，
Figure BDA0002052645230000044
为无人车节点i惯性导航系统的平台误差角，δvEi,δvNi,δvUi为无人车节点i的陀螺仪在东向、北向、天向速度误差，δLi,δλi,δhi为无人车节点i的陀螺仪纬度、精度、高度误差位置误差，εbxi,εbyi,εbzi为无人车节点i的陀螺仪随机常数误差，εrxi,εryi,εrzi为无人车节点i的陀螺仪一阶马尔科夫随机噪声，▽xi,▽yi,▽zi为无人车节点i的加速度计一阶马尔科夫过程；
(402)建立惯性导航系统的误差方程，包括平台误差角方程、速度误差方程和位置误差方程：
平台误差角方程：
Figure BDA0002052645230000045
其中，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；
速度误差方程：
Figure BDA0002052645230000051
其中，vUi为无人车节点i惯性导航系统天向速度，δvUi为对应的速度误差，fEi、fNi、fUi为无人车节点i加速度计测得的加速度值，▽Ei、▽Ni、▽Ui为无人车节点i东向、北向、天向加速度计一阶马尔可夫过程白噪声，R为地球半径，g为当地重力加速度值；
位置误差方程：
Figure BDA0002052645230000052
(403)建立无人车节点i的等式约束卡尔曼滤波状态方程：
Figure BDA0002052645230000061
其中，Gi为系统噪声系数矩阵，Wi为噪声控制向量，
Figure BDA0002052645230000062
FNi为平台误差角、速度误差、位置误差构成的系统矩阵，
Figure BDA0002052645230000063
Figure BDA0002052645230000064
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)中，首先利用无人车节点间的相对距离信息建立系统的约束方程
Figure BDA0002052645230000065
Figure BDA0002052645230000066
上式中，X为状态变量，T1为与状态变量同维数的矩阵，D0表示导航数据链相对测距高斯白噪声，上标T表示转置；
然后构建拉格朗日函数L(X,λ)：
Figure BDA0002052645230000067
上式中，
Figure BDA0002052645230000071
为X的估计值，λ为拉格朗日乘子，Pk为均方误差；
求解得到：
X＝GL -1VL(λΔΔT+I)-1PL
上式中，GL为下三角矩阵，VL为奇异值分解酉矩阵，
Figure BDA0002052645230000072
Figure BDA0002052645230000073
I为单位阵，
Figure BDA0002052645230000074
为n个非零奇异值；
将上式代入约束方程中，得：
Figure BDA0002052645230000075
上式中，Pi为PL中的第i个元素；
对上式求导并利用牛顿迭代法解得：
Figure BDA0002052645230000076
上式中，λk为λ的第k次迭代值，λk+1为λ的第k+1次迭代值，
Figure BDA0002052645230000077
是h(λ)的求导值；
通过以上各式在初始条件下的情况，完成系统的时间更新、量测更新和约束更新过程。
进一步地，在步骤(5)中，通过等式约束卡尔曼滤波进行时间更新和量测更新的过程如下：
Figure BDA0002052645230000078
Figure BDA0002052645230000079
Figure BDA00020526452300000710
Figure BDA00020526452300000711
Figure BDA00020526452300000712
上式中，
Figure BDA00020526452300000713
为k-1到k时刻的估计状态量，Φk,k-1为k-1到k时刻的状态转移矩阵，
Figure BDA00020526452300000714
为上一步的估计状态量，
Figure BDA00020526452300000715
为本步的估计状态量，Kk为系统k时刻的滤波增益，Zk为k时刻的量测量，Hk为系统k时刻的量测矩阵，Pk/k-1为
Figure BDA0002052645230000081
对应的均方误差，Rk为系统k时刻的量测噪声矩阵，Pk-1为上一步的均方误差，Γk,k-1为k-1到k时刻的系统输入矩阵，Pk/k为
Figure BDA0002052645230000082
对应的均方误差，I为单位阵，上标T表示转置。
采用上述技术方案带来的有益效果：
本发明适用于卫星拒止环境下的无人车编队协同导航，利用数据链对无人车之间相对距离进行测量，进而通过多导航传感器信息融合方法对编队中每辆无人车节点的位置进行估计，导航精度相较于GPS拒止环境下惯导/里程计组合导航系统精度更高。
附图说明
图1是本发明的方法流程图。
具体实施方式
以下将结合附图，对本发明的技术方案进行详细说明。
本发明设计了一种基于数据链测距的多无人车协同导航方法，如图1所示，具体步骤如下。
步骤1：在各无人车节点上搭载惯性导航系统、车辆里程计和数据链收发装置；
步骤2：周期性读取k时刻各无人车节点的里程计速度信息、陀螺仪信息和加速度计信息，并通过数据链收发装置获得无人车节点之间的相对距离信息；
步骤3：预测k时刻各无人车节点的姿态四元数、速度和位置；
步骤4：利用里程计速度信息，通过卡尔曼滤波校正惯性导航系统误差的状态方程和量测方程；
步骤5：利用无人车节点之间的相对距离信息建立系统约束方程，通过等式约束卡尔曼滤波完成对惯性导航系统状态量的约束估计。
在本实施例中，设定机体坐标系的X、Y、Z轴分别为无人车的右向、前向、上向，设定导航坐标系的X、Y、Z轴分别为东向、北向、天向。
在本实施例中，步骤3采用如下优选方案实现：
采用下式预测无人车节点i在k时刻的姿态四元数：
Figure BDA0002052645230000091
上式中，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为离散采样周期；
Figure BDA0002052645230000092
通过下式计算：
Figure BDA0002052645230000093
其中
Figure BDA0002052645230000094
通过下式计算：
Figure BDA0002052645230000095
其中，
Figure BDA0002052645230000096
为k时刻陀螺仪读取的无人车节点i机体坐标系相对于导航坐标系的角速度在机体坐标系X、Y、Z轴上的分量，
Figure BDA0002052645230000097
Figure BDA0002052645230000098
为k时刻陀螺仪的零偏在机体坐标系X、Y、Z轴上的分量；
采用下式预测速度信息：
Figure BDA0002052645230000099
其中，
Figure BDA0002052645230000101
Figure BDA0002052645230000102
为k时刻加速度计读取的无人车节点i机体系相对于导航系的加速度在机体坐标系X、Y、Z轴上的分量；
Figure BDA0002052645230000103
Figure BDA0002052645230000104
为k时刻加速度计零偏在机体坐标系X、Y、Z轴上的分量；g＝[0 0 g]T，g为当地重力加速度值；
Figure BDA0002052645230000105
Figure BDA0002052645230000106
为k时刻机体坐标系相对于导航坐标系的线速度在导航坐标系X、Y、Z轴上的分量；
Figure BDA0002052645230000107
Figure BDA0002052645230000108
Figure BDA0002052645230000109
为k-1时刻机体坐标系相对于导航坐标系的线速度在导航坐标系X、Y、Z轴上的分量；
Figure BDA00020526452300001010
为机体坐标系到导航坐标系的姿态矩阵，通过下式计算：
Figure BDA00020526452300001011
Figure BDA00020526452300001012
采用下式预测位置信息：
Figure BDA00020526452300001013
上式中，
Figure BDA00020526452300001014
Figure BDA00020526452300001015
分别为k时刻无人车节点i在导航坐标系X、Y、Z轴上的位置坐标；
Figure BDA00020526452300001016
Figure BDA00020526452300001017
分别为k-1时刻无人车节点i在导航坐标系X、Y、Z轴上的位置坐标。
在本实施例中，步骤4采用如下优选方案如下：
401、确定无人车节点i的等式约束卡尔曼滤波状态量：
Figure BDA0002052645230000111
上式中，
Figure BDA0002052645230000112
为无人车节点i惯性导航系统的平台误差角，δvEi,δvNi,δvUi为无人车节点i的陀螺仪在东向、北向、天向速度误差，δLi,δλi,δhi为无人车节点i的陀螺仪纬度、精度、高度误差位置误差，εbxi,εbyi,εbzi为无人车节点i的陀螺仪随机常数误差，εrxi,εryi,εrzi为无人车节点i的陀螺仪一阶马尔科夫随机噪声，▽xi,▽yi,▽zi为无人车节点i的加速度计一阶马尔科夫过程；
402、建立惯性导航系统的误差方程，包括平台误差角方程、速度误差方程和位置误差方程：
平台误差角方程：
Figure BDA0002052645230000113
其中，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；
速度误差方程：
Figure BDA0002052645230000121
其中，vUi为无人车节点i惯性导航系统天向速度，δvUi为对应的速度误差，fEi、fNi、fUi为无人车节点i加速度计测得的加速度值，▽Ei、▽Ni、▽Ui为无人车节点i东向、北向、天向加速度计一阶马尔可夫过程白噪声，R为地球半径，g为当地重力加速度值；
位置误差方程：
Figure BDA0002052645230000122
403、建立无人车节点i的等式约束卡尔曼滤波状态方程：
Figure BDA0002052645230000123
其中，Gi为系统噪声系数矩阵，Wi为噪声控制向量，
Figure BDA0002052645230000124
FNi为平台误差角、速度误差、位置误差构成的系统矩阵，
Figure BDA0002052645230000131
Figure BDA0002052645230000132
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采用如下优选方案实现：
首先利用无人车节点间的相对距离信息建立系统的约束方程
Figure BDA0002052645230000133
Figure BDA0002052645230000134
上式中，X为状态变量，T1为与状态变量同维数的矩阵，D0表示导航数据链相对测距高斯白噪声，上标T表示转置；
然后构建拉格朗日函数L(X,λ)：
Figure BDA0002052645230000135
上式中，
Figure BDA0002052645230000136
为X的估计值，λ为拉格朗日乘子，Pk为均方误差；
求解得到：
X＝GL -1VL(λΔΔT+I)-1PL
上式中，GL为下三角矩阵，VL为奇异值分解酉矩阵，
Figure BDA0002052645230000141
Figure BDA0002052645230000142
I为单位阵，
Figure BDA0002052645230000143
为n个非零奇异值；
将上式代入约束方程中，得：
Figure BDA0002052645230000144
上式中，Pi为PL中的第i个元素；
对上式求导并利用牛顿迭代法解得：
Figure BDA0002052645230000145
上式中，λk为λ的第k次迭代值，λk+1为λ的第k+1次迭代值，
Figure BDA0002052645230000146
是h(λ)的求导值；
通过以上各式在初始条件下的情况，完成系统的时间更新、量测更新和约束更新过程。
通过等式约束卡尔曼滤波进行时间更新和量测更新的过程如下：
状态一步预测方程：
Figure BDA0002052645230000147
状态估值方程：
Figure BDA0002052645230000148
滤波增益方程：
Figure BDA0002052645230000149
一步预测均方误差方程：
Figure BDA00020526452300001410
估计均方误差方程：
Figure BDA00020526452300001411
上式中，
Figure BDA0002052645230000151
为k-1到k时刻的估计状态量，Φk,k-1为k-1到k时刻的状态转移矩阵，
Figure BDA0002052645230000152
为上一步的估计状态量，
Figure BDA0002052645230000153
为本步的估计状态量，Kk为系统k时刻的滤波增益，Zk为k时刻的量测量，Hk为系统k时刻的量测矩阵，Pk/k-1为
Figure BDA0002052645230000154
对应的均方误差，Rk为系统k时刻的量测噪声矩阵，Pk-1为上一步的均方误差，Γk,k-1为k-1到k时刻的系统输入矩阵，Pk/k为
Figure BDA0002052645230000155
对应的均方误差，I为单位阵，上标T表示转置。
实施例仅为说明本发明的技术思想，不能以此限定本发明的保护范围，凡是按照本发明提出的技术思想，在技术方案基础上所做的任何改动，均落入本发明保护范围之内。

Claims (3)

1.一种基于数据链测距的多无人车协同导航方法，其特征在于，包括以下步骤：
(1)在各无人车节点上搭载惯性导航坐标系统、车辆里程计和数据链收发装置；
(2)周期性读取k时刻各无人车节点的里程计速度信息、陀螺仪信息和加速度计信息，并通过数据链收发装置获得无人车节点之间的相对距离信息；
(3)预测k时刻各无人车节点的姿态四元数、速度和位置；
采用下式预测无人车节点i在k时刻的姿态四元数：
Figure FDA0002752213650000011
上式中，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为离散采样周期；
Figure FDA0002752213650000012
通过下式计算：
Figure FDA0002752213650000013
其中
Figure FDA0002752213650000014
通过下式计算：
Figure FDA0002752213650000015
其中，
Figure FDA0002752213650000016
为k时刻陀螺仪读取的无人车节点i机体坐标系相对于导航坐标系的角速度在机体坐标系X、Y、Z轴上的分量，
Figure FDA0002752213650000017
Figure FDA0002752213650000018
为k时刻陀螺仪的零偏在机体坐标系X、Y、Z轴上的分量；
采用下式预测速度信息：
Figure FDA0002752213650000021
其中，
Figure FDA0002752213650000022
Figure FDA0002752213650000023
为k时刻加速度计读取的无人车节点i机体坐标系相对于导航坐标系的加速度在机体坐标系X、Y、Z轴上的分量；
Figure FDA0002752213650000024
Figure FDA0002752213650000025
Figure FDA0002752213650000026
为k时刻加速度计零偏在机体坐标系X、Y、Z轴上的分量；g＝[0 0 g]T，g为当地重力加速度值；
Figure FDA0002752213650000027
Figure FDA0002752213650000028
Figure FDA0002752213650000029
为k时刻机体坐标系相对于导航坐标系的线速度在导航坐标系X、Y、Z轴上的分量；
Figure FDA00027522136500000210
Figure FDA00027522136500000211
Figure FDA00027522136500000212
为k-1时刻机体坐标系相对于导航坐标系的线速度在导航坐标系X、Y、Z轴上的分量；
Figure FDA00027522136500000213
为机体坐标系到导航坐标系的姿态矩阵，通过下式计算：
Figure FDA00027522136500000214
Figure FDA00027522136500000215
采用下式预测位置信息：
Figure FDA00027522136500000216
上式中，
Figure FDA00027522136500000217
Figure FDA00027522136500000218
分别为k时刻无人车节点i在导航坐标系X、Y、Z轴上的位置坐标；
Figure FDA00027522136500000219
Figure FDA00027522136500000220
分别为k-1时刻无人车节点i在导航坐标系X、Y、Z轴上的位置坐标；
(4)利用里程计速度信息，通过卡尔曼滤波校正惯性导航坐标系统误差的状态方程和量测方程；具体过程如下：
(401)确定无人车节点i的等式约束卡尔曼滤波状态量：
Figure FDA0002752213650000031
上式中，
Figure FDA0002752213650000032
为无人车节点i惯性导航坐标系统的平台误差角，δvEi,δvNi,δvUi为无人车节点i的陀螺仪在东向、北向、天向速度误差，δLi,δλi,δhi为无人车节点i的陀螺仪纬度、精度、高度误差位置误差，εbxi,εbyi,εbzi为无人车节点i的陀螺仪随机常数误差，εrxi,εryi,εrzi为无人车节点i的陀螺仪一阶马尔科夫随机噪声，
Figure FDA0002752213650000033
为无人车节点i的加速度计一阶马尔科夫过程；
(402)建立惯性导航坐标系统的误差方程，包括平台误差角方程、速度误差方程和位置误差方程：
平台误差角方程：
Figure FDA0002752213650000034
其中，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；
速度误差方程：
Figure FDA0002752213650000041
其中，vUi为无人车节点i惯性导航坐标系统天向速度，δvUi为对应的速度误差，fEi、fNi、fUi为无人车节点i加速度计测得的加速度值，
Figure FDA0002752213650000042
为无人车节点i东向、北向、天向加速度计一阶马尔可夫过程白噪声，R为地球半径，g为当地重力加速度值；
位置误差方程：
Figure FDA0002752213650000043
(403)建立无人车节点i的等式约束卡尔曼滤波状态方程：
Figure FDA0002752213650000051
其中，Gi为系统噪声系数矩阵，Wi为噪声控制向量，
Figure FDA0002752213650000052
FNi为平台误差角、速度误差、位置误差构成的系统矩阵，
Figure FDA0002752213650000053
Figure FDA0002752213650000054
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)利用无人车节点之间的相对距离信息建立系统约束方程，通过等式约束卡尔曼滤波完成对惯性导航坐标系统状态量的约束估计；
首先利用无人车节点间的相对距离信息建立系统的约束方程
Figure FDA0002752213650000055
Figure FDA0002752213650000056
上式中，X为状态变量，T1为与状态变量同维数的矩阵，D0表示导航数据链相对测距高斯白噪声，上标T表示转置；
然后构建拉格朗日函数L(X,λ)：
Figure FDA0002752213650000061
上式中，
Figure FDA0002752213650000062
为X的估计值，λ为拉格朗日乘子，Pk为均方误差；
求解得到：
X＝GL -1VL(λΔΔT+I)-1PL
上式中，GL为下三角矩阵，VL为奇异值分解酉矩阵，
Figure FDA0002752213650000063
Figure FDA0002752213650000064
I为单位阵，
Figure FDA0002752213650000065
为n个非零奇异值；
将上式代入约束方程中，得：
Figure FDA0002752213650000066
上式中，Pi为PL中的第i个元素；
对上式求导并利用牛顿迭代法解得：
Figure FDA0002752213650000067
上式中，λk为λ的第k次迭代值，λk+1为λ的第k+1次迭代值，
Figure FDA0002752213650000068
是h(λ)的求导值；
通过以上各式在初始条件下的情况，完成系统的时间更新、量测更新和约束更新过程。
2.根据权利要求1所述基于数据链测距的多无人车协同导航方法，其特征在于，设定机体坐标系的X、Y、Z轴分别为无人车的右向、前向、上向，设定导航坐标系的X、Y、Z轴分别为东向、北向、天向。
3.根据权利要求1所述基于数据链测距的多无人车协同导航方法，其特征在于，在步骤(5)中，通过等式约束卡尔曼滤波进行时间更新和量测更新的过程如下：
Figure FDA0002752213650000069
Figure FDA00027522136500000610
Figure FDA0002752213650000071
Figure FDA0002752213650000072
Figure FDA0002752213650000073
上式中，
Figure FDA0002752213650000074
为k-1到k时刻的估计状态量，Φk,k-1为k-1到k时刻的状态转移矩阵，
Figure FDA0002752213650000075
为上一步的估计状态量，
Figure FDA0002752213650000076
为本步的估计状态量，Kk为系统k时刻的滤波增益，Zk为k时刻的量测量，Hk为系统k时刻的量测矩阵，Pk/k-1为
Figure FDA0002752213650000077
对应的均方误差，Rk为系统k时刻的量测噪声矩阵，Pk-1为上一步的均方误差，Γk,k-1为k-1到k时刻的系统输入矩阵，Pk/k为
Figure FDA0002752213650000078
对应的均方误差，I为单位阵，上标T表示转置。
CN201910378890.2A 2019-05-08 2019-05-08 一种基于数据链测距的多无人车协同导航方法 Active CN110207691B (zh)

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 CN110207691A (zh) 2019-09-06
CN110207691B true 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)

Families Citing this family (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110598184B (zh) * 2019-09-18 2020-04-28 南京山鹞航空科技有限公司 一种编队复合跟踪系统数据注册误差校准方法
CN111044075B (zh) * 2019-12-10 2023-09-15 上海航天控制技术研究所 基于卫星伪距/相对测量信息辅助的sins误差在线修正方法
CN111238469B (zh) * 2019-12-13 2023-09-29 南京航空航天大学 一种基于惯性/数据链的无人机编队相对导航方法
CN111273687A (zh) * 2020-02-17 2020-06-12 上海交通大学 基于gnss观测量和机间测距的多无人机协同相对导航方法
CN112050809B (zh) * 2020-10-08 2022-06-17 吉林大学 轮式里程计与陀螺仪信息融合的无人车定向定位方法
CN113175931B (zh) * 2021-04-02 2022-08-16 上海机电工程研究所 基于约束卡尔曼滤波的集群组网协同导航方法及系统
CN113984073A (zh) * 2021-09-29 2022-01-28 杭州电子科技大学 一种基于方位的移动机器人协同校正算法

Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2018149145A1 (en) * 2017-02-15 2018-08-23 Beijing Didi Infinity Technology And Development Co., Ltd. Systems and methods for on-demand service
CN108828140A (zh) * 2018-04-26 2018-11-16 中国计量大学 一种基于粒子群算法的多无人机协同恶臭溯源方法
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的无人机自主定位方法

Family Cites Families (11)

* Cited by examiner, † Cited by third party
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
US9552503B2 (en) * 2012-05-01 2017-01-24 5D Robotics, Inc. Distributed positioning and collaborative behavior determination
DE102015211735A1 (de) * 2015-06-24 2016-12-29 Bayerische Motoren Werke Aktiengesellschaft Engstellenassistenzsystem in einem Kraftfahrzeug
US9798329B2 (en) * 2015-07-27 2017-10-24 Genghiscomm Holdings, LLC Airborne relays in cooperative-MIMO systems
CN105824039A (zh) * 2016-03-17 2016-08-03 孙红星 基于里程计的克服卫星失锁时的gnss/ins车载组合定位定向算法
CN107289942B (zh) * 2017-06-20 2020-11-03 南京航空航天大学 一种用于编队飞行的相对导航系统及方法
CN108052110A (zh) * 2017-09-25 2018-05-18 南京航空航天大学 基于双目视觉的无人机编队飞行方法和系统
CN107831783B (zh) * 2017-11-10 2019-10-08 南昌航空大学 一种支持多无人机自主飞行的地面站控制系统
CN108415057B (zh) * 2018-01-25 2022-04-01 南京理工大学 一种无人车队与路侧单元协同工作的相对定位方法
CN108682145B (zh) * 2018-05-31 2020-06-02 大连理工大学 无人驾驶公交车的编组方法
CN109029422B (zh) * 2018-07-10 2021-03-05 北京木业邦科技有限公司 一种多无人机协作构建三维调查地图的方法和装置

Patent Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2018149145A1 (en) * 2017-02-15 2018-08-23 Beijing Didi Infinity Technology And Development Co., Ltd. Systems and methods for on-demand service
CN109099912A (zh) * 2017-08-11 2018-12-28 黄润芳 室外精确定位导航方法、装置、电子设备及存储介质
CN108828140A (zh) * 2018-04-26 2018-11-16 中国计量大学 一种基于粒子群算法的多无人机协同恶臭溯源方法
CN109084785A (zh) * 2018-07-25 2018-12-25 吉林大学 多车辆协同定位与地图构建方法、装置、设备及存储介质
CN109211241A (zh) * 2018-09-08 2019-01-15 天津大学 基于视觉slam的无人机自主定位方法

Also Published As

Publication number Publication date
CN110207691A (zh) 2019-09-06

Similar Documents

Publication Publication Date Title
CN110207691B (zh) 一种基于数据链测距的多无人车协同导航方法
CN109813311B (zh) 一种无人机编队协同导航方法
CN107270893B (zh) 面向不动产测量的杆臂、时间不同步误差估计与补偿方法
CN111156994B (zh) 一种基于mems惯性组件的ins/dr&gnss松组合导航方法
CN106990426B (zh) 一种导航方法和导航装置
CN111121766B (zh) 一种基于星光矢量的天文与惯性组合导航方法
CN108594283B (zh) Gnss/mems惯性组合导航系统的自由安装方法
CN109945859B (zh) 一种自适应h∞滤波的运动学约束捷联惯性导航方法
CN110440830B (zh) 动基座下车载捷联惯导系统自对准方法
CN112697138B (zh) 一种基于因子图优化的仿生偏振同步定位与构图的方法
CN111024064A (zh) 一种改进Sage-Husa自适应滤波的SINS/DVL组合导航方法
CN112504275B (zh) 一种基于级联卡尔曼滤波算法的水面舰船水平姿态测量方法
CN107741240B (zh) 一种适用于动中通的组合惯导系统自适应初始对准方法
CN111024074B (zh) 一种基于递推最小二乘参数辨识的惯导速度误差确定方法
CN111238469B (zh) 一种基于惯性/数据链的无人机编队相对导航方法
CN110849360A (zh) 面向多机协同编队飞行的分布式相对导航方法
CN112212862A (zh) 一种改进粒子滤波的gps/ins组合导航方法
CA2699137A1 (en) Hybrid inertial system with non-linear behaviour and associated method of hybridization by multi-hypothesis filtering
CN113503892A (zh) 一种基于里程计和回溯导航的惯导系统动基座初始对准方法
CN114526731A (zh) 一种基于助力车的惯性组合导航方向定位方法
CN112525204B (zh) 一种航天器惯性和太阳多普勒速度组合导航方法
CN111912427A (zh) 一种多普勒雷达辅助捷联惯导运动基座对准方法及系统
CN111220151B (zh) 载体系下考虑温度模型的惯性和里程计组合导航方法
CN110375773B (zh) Mems惯导系统姿态初始化方法
CN113008229A (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