中国科技核心期刊      中国指挥与控制学会会刊     军事装备类重点期刊
指挥控制与仿真

指挥控制与仿真 >

2023 , Vol. 45 >Issue 3: 143 - 148

DOI: https://doi.org/10.3969/j.issn.1673-3819.2023.03.021

雷达/通信/导航

一种改进的主从式协同导航技术研究

  • 陈东隅
展开
  • 江苏自动化研究所, 江苏 连云港 222061

陈东隅(1990—),男,硕士,工程师,研究方向为导航制导与控制。

Office editor: 张培培

收稿日期: 2022-08-31

  修回日期: 2022-09-27

  网络出版日期: 2023-06-12

收起

Research on an improved leader-follower cooperative navigation technology

  • CHEN Dongyu
Expand
  • Jiangsu Automation Research Institute, Lianyungang 222061, China

Received date: 2022-08-31

  Revised date: 2022-09-27

  Online published: 2023-06-12

Fold

摘要

为提高传统基于测距信息的主从式协同导航定位算法中从艇的位置解算精度,将从艇间的测距信息作为量测值融入主从式协同导航算法,重新构建了系统状态方程和量测方程,采用了扩展卡尔曼滤波进行滤波解算。仿真结果表明,相比于传统计算方式,增加了从艇间测距信息的主从式协同导航算法可进一步抑制定位误差的发散,提高了从艇的导航定位精度。

关键词： 主从式协同导航; 扩展卡尔曼滤波; 从艇间测距

本文引用格式

陈东隅 . 一种改进的主从式协同导航技术研究[J]. 指挥控制与仿真, 2023 , 45(3) : 143 -148 . DOI: 10.3969/j.issn.1673-3819.2023.03.021

Abstract

To improve the positioning accuracy of the followers in leader-follower cooperative navigation algorithm for multi-USVs based on distance measurement, this algorithm integrates the distance measurements between the followers into the traditional leader-follow structure, reconstructs the equations of state and measurements in this system, calculating positions through extended Kalman filter. Simulation results shows that the algorithm added distance measurements between the followers compared with the traditional algorithm can restrain the errors diffuse effectively and improve positioning accuracy of the followers.

Key words： leader-follower cooperative navigation; extended Kalman filter; distance measurements between the followers

利用水面无人艇编队参与区域搜救、反潜等特殊任务时,需要多艘无人艇之间相互协同作业才能完成任务。但在复杂的战场环境下,各种针对卫星导航系统的干扰和拒止手段层出不穷,各无人艇仅依靠自身搭载的惯性组件,其导航定位精度很难满足协同作业的发展需求,因此,如何让无人艇编队在卫星导航信号拒止环境下长时间保持良好的导航定位精度,是遂行相关任务的关键[1-2]。
近年来,国内外研究人员对于陆上机器人、水下自主潜航器和无人机的协同导航定位技术研究较为深入[3⇓-5]。美国明尼苏达大学的S.I.Roumeliotis等人对机器人协同导航进行了研究,提出了一种新颖的分散式协同导航算法,使用基于卡尔曼滤波的算法来实现协同定位[6];2016年,欧洲多家科研单位在欧盟的资金支持下开展了名为MORPH的研究项目,该项目对多个AUV进行协调作业时的系统结构和通信问题进行了研究,并完成了基于多AUV系统的协调控制和协同定位等多项技术的试验验证[7]。
目前,对于水面无人艇编队的协同导航研究尚处于起步阶段,其中,主从式协同导航技术是该领域的重要研究方向[8],其分为单主艇和多主艇两种技术途径。无人艇可通过自身搭载的光电、雷达、水声或无线电测距等设备获取相对其他无人艇的距离信息和方位信息等,再利用无线通信交换各自的运动状态信息进行协同导航定位,从而提高自身的导航定位精度[9]。
本文以一艘搭载高精度组合导航设备或能够正常获取卫星导航信号的无人艇作为主艇,其他搭载廉价的低精度惯导组件的无人艇为从艇,组成了单主多从的无人艇编队。其中,主艇的高精度定位信息在无人艇编队中可以实时共享。在传统的单主多从协同导航系统中,各从艇仅与主艇建立关系,独立解算自身的位置信息。本文将任意两艘从艇与主艇组成了改进的主从式协同导航系统,通过测量并共享三者间的距离改正从艇的位置,最终提高编队中从艇的导航定位精度。该方法避免了在每艘无人艇上都安装价格昂贵的高精度导航设备,减小了无人艇编队的成本,且相比传统方法可进一步抑制误差累积速度,提升导航定位精度。

1 改进的主从式协同导航算法

1.1 基本原理

无人艇编队在卫星导航信号拒止环境下进行航位推算时,由于受惯导组件误差特性影响,导航定位时会随着时间推移产生较大的累积误差。本文中的无人艇编队包含1艘主艇和2艘从艇,从艇均处于卫星导航信号拒止区域。无人艇编队的运动过程可看作二维平面运动,可忽略高程因素,分别将主艇Μ和从艇Α、Β的位置记为PM=(xM,yM)、PA=(xA,yA)和PB=(xB,yB)。本文认为主艇的位置是包含高斯噪声的准确位置。而从艇在卫星导航信号拒止环境下,仅依靠自身搭载的低精度惯性导航组件进行航迹推算,则从艇A和B的航迹推算模型可表示为
x A / B k + 1 = x A / B k + t v A / B k c o s φ A / B k y A / B k + 1 = y A / B k + t v A / B k s i n φ A / B k γ A / B k + 1 = φ A / B k + t ω A / B k
式中, x A / B k + 1 、 y A / B k + 1 为k+1时刻从艇Α和Β的位置,t为采样周期, v A / B k 为k时刻向前的位移速度, φ A / B k + 1 为k+1时刻的航向角, ω A / B k 为k时刻的航向角速度。其中, v A / B k 和 φ A / B k 均包含噪声,分别为δvA和δφA。
传统的主从式协同导航方法中,从艇通过无线网络实时获取主艇的精确位置,再利用测距设备获取与主艇之间的相对距离,最终通过与本艇运动状态信息的融合滤波计算出该时刻的位置,提高了从艇导航定位精度。
改进的主从式协同导航算法在此基础上,引入从艇之间的测距信息作为一种误差约束条件,使得基于EKF的主从式协同导航技术在一主多从的应用场景中,导航定位精度得到进一步提高。
改进的主从式协同导航定位原理如图1所示。
图1 改进的主从式协同导航原理

Fig.1 Principle of the improved leader-follower cooperative navigation

主艇在k时刻的运动状态(包括位置、速度、加速度、航向角等)以及与从艇之间的相对距离可实时广播至各从艇,将主艇与从艇、从艇与从艇间测距信息以及各艇的运动状态进行融合解算,即构建相应的运动方程和量测方程进行扩展卡尔曼滤波,可有效抑制从艇的航迹误差随时间增加的速率,提升从艇的导航定位精度[10]。改进的算法中增加了从艇间的测距信息,且该信息较为精准,因此,在融合计算时,可以将两艘从艇的位置误差约束在一定范围以内。
图1中虚线圆圈表示从艇航迹推算误差,斜纹圆圈表示采用传统主从式协同导航后的航迹误差,深灰色圆圈表示增加了从艇间距离观测量后进行滤波计算的误差。可以发现,随着已知条件的增加,从艇的导航定位精度改善较为明显。

1.2 数学模型

本文针对1艘主艇和2艘从艇组成的协同导航系统建立离散非线性系统状态方程如下:
Xk+1=f(Xk,μk,υk)=Xk+Γ(μk+υk)
式中,Xk=( x A k, y A k, φ A k, x B k, y B k, φ B k)T为状态方程的线性项,表示从艇A和从艇B的航行位置和航向角,可视为状态量;Γ(μk+υk)为状态方程的非线性项;μk=( v A k, ω A k, v B k, ω B k)T表示从艇的速度和航向角速度,可视为控制量。由于从艇的速度和航向角均受到高斯白噪声υk的影响,系统过程噪声的协方差阵Qk可表示为
Qk= σ v A k 2 0 0 0 0 σ φ A k 2 0 0 0 0 σ v B k 2 0 0 0 0 σ φ B k 2
在此应用场景下,主艇和从艇以及从艇和从艇之间的距离观测量组成的量测方程Zk可表示为
Zk= r A k r B k r A B k= ( x A k - x M k ) 2 + ( y A k - y M k ) 2 ( x B k - x M k ) 2 + ( y B k - y M k ) 2 ( x A k - x B k ) 2 + ( y A k - y B k ) 2
式中, r A k 、 r B k 和 r A B k 分别表示k时刻主艇和从艇A、从艇B之间的距离观测量,以及从艇A和B之间的距离观测量。距离观测噪声为零均值的高斯白噪声,其方差记为 σ r A k 2 、 σ r B k 2 和 σ r A B k 2,则量测噪声的协方差矩阵Rk可表示为
Rk= σ r A k 2 0 0 0 σ r B k 2 0 0 0 σ r A B k 2

1.3 模型的线性化及滤波过程

由式(2)和式(4)可知,系统的状态方程和量测方程均包含非线性项,卡尔曼滤波(KF)一般只针对线性系统,因此,本文采用扩展卡尔曼滤波(EKF)对以上系统进行滤波[11]。根据EKF算法,首先分别围绕状态向量Xk和控制向量μk将系统的状态方程非线性部分进行泰勒级数展开:
Xk+1≈f[ X ^ k,k]+ ∂ f ∂ X ^ k[Xk- X ^ k]+Γ[ X ^ k,k]υk+…
μk+1≈f[ μ ^ k,k]+ ∂ f ∂ μ ^ k[Xk- μ ^ k]+Γ[ μ ^ k,k]Wk+…
将状态方程的泰勒级数展开式略去二阶以上的高阶项,则f(Xk,μk,υk)关于状态向量Xk和控制向量μk的雅可比矩阵(Jacobian)分别为:
F= 1 0 - t · v A k · s i n φ A k 0 0 0 0 1 t · v A k · c o s φ A k 0 0 0 0 0 1 0 0 0 0 0 0 1 0 - t · v B k · s i n φ B k 0 0 0 0 1 t · v B k · c o s φ B k 0 0 0 0 0 1
U= t · c o s φ A k 0 0 0 t · s i n φ A k 0 0 0 0 t 0 0 0 0 t · c o s φ B k 0 0 0 t · s i n φ B k 0 0 0 0 t
同时,量测方程Zk围绕 X ^ k进行泰勒级数展开:
Zk+1=h[ X ^ k,k]+ ∂ h ∂ X ^ k[Xk- X ^ k]+…+Vk
将量测方程的泰勒级数展开式略去二阶以上的高阶项,则量测方程关于状态向量Xk的雅可比矩阵H可表示为
H= x A k - x M k r A k y A k - y M k r A k 0 0 0 0 0 0 0 x B k - x M k r B k y B k - y M k r B k 0 x A k - x B k r A B k y A k - y B k r A B k 0 x B k - x A k r A B k y B k - y A k r A B k 0
对系统线性化后的运动模型应用卡尔曼滤波基本方程可得到扩展卡尔曼滤波递推方程,其具体步骤如下:
1)系统状态一步预测
X ^ k + 1 , k=f( X ^ k,uk,0)
2)系统一步预测的协方差矩阵
Pk+1,k=FkPk(Fk)T+UkQk(Uk)T
式中,Pk为从艇运动状态的协方差阵,Qk为系统过程噪声协方差阵,Fk和Uk分别为系统状态方程的雅可比矩阵。
3)滤波增益
Kk+1=Pk+1,k(Hk+1)T(Hk+1Pk+1,k(HK+1)T+Rk+1)-1
式中,Rk+1为量测误差矩阵,Hk+1为量测方程的雅可比矩阵。
4)系统状态估计
X ^ k + 1= X ^ k + 1 , k+Kk+1(Zk+1- Z ^ k + 1 , k)
式中,k+1时刻系统量测值的估计值为 Z ^ k + 1 , k=h( X ^ A k + 1, X ^ B k + 1, X ^ M k + 1)。
5)系统状态估计误差的协方差阵更新
Pk+1=Pk+1,k-Kk+1Hk+1Pk+1,k
以上就是扩展卡尔曼滤波器的一个计算周期,各个时刻EKF对主从式协同导航系统的处理就是这个周期不断重复的过程。

1.4 系统误差源分析

改进的主从式协同导航系统的误差主要来源于以下几个方面:
1)主艇和从艇本身的运动状态误差,即主从艇本身的航行位置、速度、加速度和航向角等产生的误差。其中,主艇的位置误差包含艇载的高精度导航定位设备误差(可优于米级)以及无人艇受航行控制系统和水文条件等因素影响产生的航行控制误差[12]。
2)主从艇之间的测距误差,即主艇与从艇间的测距以及从艇与从艇间的测距误差。目前,艇间距离测量可采用激光测距、水声测距或者雷达测距等手段完成,但随着艇间距离逐渐增大,测距误差也逐步增大[13]。
3)信息传输时延误差。由于各观测量均通过无线网络传输并进行计算,在传输过程中存在一定的网络时延,解算的定位结果与实际位置存在误差。本文假设所有数据都是实时获取和处理的,暂不讨论信息传输时延产生的误差。
4)数学模型误差。由于系统状态方程和量测方程在线性化过程中省略了高阶项,从而产生了计算误差。本文仅讨论主从艇以固定航向进行的匀速直线运动,相应的运动方程不属于高度非线性系统,因此,在线性化过程中,略去的高阶项产生的系统误差可以忽略。

2 仿真结果分析

为验证改进的主从式协同导航算法相比于传统方法对从艇导航定位误差的抑制效果,本节进行了仿真计算。
1艘主艇和2艘从艇在编队航行时,进入卫星导航信号拒止区域,主艇和从艇相互之间保持一定的距离,且主从艇之间具备无线网络通信及相互观测的条件。设定主艇和从艇的仿真条件如表1所示。
表1 仿真实验参数

Tab. 1 Parameters of the simulation experiment

参数名称 主艇 从艇A 从艇B
初始位置/m (300,300) (0,100) (100,200)
主艇位置误差/m 5 — —
航行速度m/s 5.0 3.0 7.0
速度误差m/s 0.1 0.1 0.1
航行加速度m/s2 0 0 0
测距误差/m 10.0 10.0 10.0
航向角/(°) 90 90 90
航向角误差/(°) 0.1 0.1 0.1
航行时间/s 2 000 2 000 2 000
主艇和从艇A、B按照表1中设定的参数航行约2 000 s,其中,主艇航速约10 kn,从艇A航速约6 kn,从艇B航速约14 kn,不考虑水流、风速等环境因素的影响,三艘艇航行的实际轨迹如图2所示。
图2 主艇和从艇A、B真实航行轨迹图

Fig.2 Real route of leader boat and follower boats

图2中,黑色线段表示主艇实际航行轨迹;红色线段表示从艇A的航行轨迹;蓝色线段表示从艇B的航行轨迹。
现分别对从艇A和从艇B采用传统的主从式协同导航方法解算各自的航行轨迹,如图3和图4所示。
图3 从艇A的协同导航定位结果

Fig.3 The result of cooperative navigation for follower A

图4 从艇B的协同导航定位结果

Fig.4 The result of cooperative navigation for follower B

图3a)和图4a)中,黑线分别表示从艇A、B的实际航行轨迹;红线分别表示航迹推算轨迹;蓝线分别表示采用传统的主从式协同导航算法,从艇A、B的航行轨迹。图3b)和图4b)中,黑色线段表示从艇A、B的航迹推算误差,红色线段表示协同导航后的航迹误差。可以看出,传统的主从式协同导航技术可在一定程度上抑制从艇的导航定位误差。
这里采用以上改进的协同导航算法,对于一艘主艇多艘从艇的协同导航情况,任意选取两艘从艇之间的测距信息进行融合解算,仿真结果如图5和图6所示。
图5 改进后从艇A的协同导航定位结果

Fig.5 The result of improved cooperative navigation for follower A

图6 融合解算后,从艇B的协同导航定位结果

Fig.6 The result of improved cooperative navigation for follower B

图5a)和图6a)中,黑线分别表示从艇A、B实际的航行轨迹;红线分别表示航推轨迹;蓝线表示传统的协同导航轨迹;绿线表示改进后的主从式协同导航解算的轨迹。图5b)和图6b)中,黑线表示从艇A、B的航推误差;红线表示传统的主从式协同导航算法解算误差;蓝线表示改进后的主从式协同导航算法解算的误差。对比可以发现,主从式协同导航过程中,融合解算了从艇之间的测距信息,相比较传统的主从式协同导航方法,从艇A、B位置误差的发散速度得到了有效抑制,如表2所示。
表2 传统算法与改进后算法的误差对比表

Tab. 2 Error comparison between traditional algorithm and improved algorithm

时间 从艇A的位置误差/m 从艇B的位置误差/m
传统方法 改进方法 传统方法 改进方法
200 s 4.1 2.6 6.1 4.2
400 s 6.8 4.1 9.6 6.5
600 s 23.1 7.2 20.7 6.7
800 s 27.8 13.8 38.2 27.7
1 000 s 39.5 25.4 52.6 38.3
1 200 s 69.7 23.1 66.1 25.7
1 400 s 87.5 34.5 80.7 44.3
1 600 s 90.1 38.2 99.7 67.3
1 800 s 99.7 40.3 133.0 87.8
2 000 s 110.0 61.9 173.0 115.2

3 结束语

本文针对无人艇编队的协同导航问题,基于传统的主从式协同导航算法,提出了一种增加从艇间距离观测信息的改进算法,并通过了仿真试验验证。由仿真结果可知,在一艘主艇两艘从艇的情况下,将从艇间的测距信息作为量测值进行融合解算比传统单主艇与单从艇的协同导航定位结果对误差的抑制更有效。

参考文献

原文顺序 | 文献年度倒序 | 文中引用次数倒序
[1]
胡建章, 唐国元, 王建军, 等. 水面无人艇集群系统研究[J]. 舰船科学技术, 2019, 41(7): 83-88.

HU J Z, TANG G Y, WANG J J, et al. Research on unmanned surface vehicle cluster system[J]. Ship Science and Technology, 2019, 41(7): 83-88.

[2]
谢启龙, 宋龙, 鲁浩, 等. 协同导航技术研究综述[J]. 航空兵器, 2019, 26(4): 23-30.

XIE Q L, SONG L, LU H, et al. Review of collaborative navigation technology[J]. Aero Weaponry, 2019, 26(4): 23-30.

[3]
孙成娇. 基于声学测距的水下协同导航状态估计方法研究[D]. 哈尔滨: 哈尔滨工程大学, 2019.

SUN C J. Research on underwater cooperative navigation state estimation method based on acoustic ranging[D]. Harbin: Harbin Engineering University, 2019.

[4]
许晓伟, 赖际舟, 吕品, 等. 多无人机协同导航技术研究现状及进展[J]. 导航定位与授时, 2017, 4(4): 1-9.

XU X W, LAI J Z, LV P, et al. A literature review on the research status and progress of cooperative navigation technology for multiple UAVs[J]. Navigation Positioning and Timing, 2017, 4(4): 1-9.

[5]
金红新, 杨涛, 王小刚, 等. 多传感器信息融合理论在无人机相对导航中的应用[J]. 国防科技大学学报, 2017, 39(5): 90-95.

JIN H X, YANG T, WANG X G, et al. Application of multi-sensor information fusion in UAV relative navigation method[J]. Journal of National University of Defense Technology, 2017, 39(5): 90-95.

[6]
TRAWNY N, MOURIKIS A I, ROUMELIOTIS S I, et al. Vision-aided inertial navigation for pin-point landing using observations of mapped landmarks: research Articles[J]. Journal of Field Robotics, 2007, 24(5): 357-378.

DOI

[7]
LEISHMAN R C, MCLAIN T W. Multiplicative extended Kalman filter for relative rotorcraft navigation[J]. Journal of Aerospace Information Systems, 2014, 12(12): 728-744.

DOI

[8]
李松, 陈琪锋, 钟日进. 不完整测量下集群飞行器协同相对定位方法[J]. 飞控与探测, 2019, 2(6): 18-25.

LI S, CHEN Q F, ZHONG R J. Collaborative relative positioning method for cluster aircraft under incomplete measurement[J]. Flight Control & Detection, 2019, 2(6): 18-25.

[9]
徐璟, 杨功流, 孙艺丁. 基于因子图的卫星拒止环境下的多信息融合技术[J]. 导航与控制, 2019, 18(6): 34-38.

XU J, YANG G L, SUN Y D. Research on the multi-source information fusion technology in satellite-jamming environment based on factor graph[J]. Navigation and Control, 2019, 18(6): 34-38.

[10]
杨少凡, 余华兵, 陈新华, 等. 基于扩展Kalman滤波的单领航者自主水下航行器协同导航判别式训练方法研究[J]. 电子与信息学报, 2015, 37(11): 2756-2761.

YANG S F, YU H B, CHEN X H, et al. Discriminative training of Kalman filters based cooperative navigation for multiple autonomous underwater vehicles with a single leader[J]. Journal of Electronics & Information Technology, 2015, 37(11): 2756-2761.

[11]
秦永元, 张洪钺, 汪叔华. 卡尔曼滤波与组合导航原理[M]. 3版. 西安: 西北工业大学出版社, 2015.

QIN Y Y, ZHANG H Y, WANG S H. Kalman filter and principle of integrated navigation[M]. 3rd ed. Xi’an: Northwestern Polytechnical University Press, 2015.

[12]
李鹏. 基于滑模自抗扰的无人水面艇路径跟踪控制研究[D]. 大连: 大连海事大学, 2017.

LI P. Research on path tracking control of unmanned underwater vehicle based on sliding mode auto disturbances rejection control[D]. Dalian: Dalian Maritime University, 2017.

[13]
张松, 张亚. 基于伪随机序列的无线电测距系统设计[J]. 航空兵器, 2016, 23(4): 42-46.

ZHANG S, ZHANG Y. Radio ranging system design based on pseudo-random sequences[J]. Aero Weaponry, 2016, 23(4): 42-46.

Options
文章导航

/

〈 〉