基于感知信息的USV 目标环绕跟踪方法

宋吉广 ,李德隆 * ,冯 亮 ,刘 阳 ,林 扬 ,孙铁铭

(1.中国科学院沈阳自动化研究所 机器人学国家重点实验室,辽宁 沈阳,110016;2.中国科学院 机器人与智能制造创新研究院,辽宁 沈阳,110169)

0 引言

目前,无人系统已经成为未来军事和民用领域发展的重点。近20 年来,无人水面艇(unmanned surface vehicle,USV)技术成为国内外研究热点,在反水雷、侦查、监控、电子战以及反侦察作战中具有很大的优势,保证了无人员伤亡前提下完成作战任务,同时在民用领域可执行探测资源、搜救、气象监测和预报、信号中继站等任务,因此深入研究USV 技术,对于提升我国国防实力具有重要的意义[1-2]。

目标跟踪是USV 研究领域的一项关键技术,USV 目标跟踪方法的研究,对于海底矿物探测、港口防御和目标打击等领域有着广泛的应用前景。USV 作为探知海洋丰富资源的一种智能多功能探测载体,需能够在无人状态下自主完成规划任务。目前,在USV 目标跟踪技术领域,可实现对锁定目标进行基于感知信息下的目标跟踪任务,Qiao 等[3]使用基于平方根Sage-Husa 自适应鲁棒卡尔曼滤波方法进行USV 雷达目标跟踪,在对测量和观测噪声进行联合估计后,从仿真和试验中验证了该算法的有效性;饶六中等[4]使用基于单目视觉对水面艇目标进行跟踪,具体方式是通过比较运动过程中标志物图像与期望图像的位置关系,实时计算出偏航角和偏航距离作为控制器输入量,并控制跟踪船航向和航速,从而实现对被跟踪船的跟踪;秦世洋等[5]使用模型预测控制(model predictive control,MPC)的方法,将环境干扰等问题转换为二次规划问题,可实现稳定的目标点跟踪。但在实际工程发展中,存在一些对目标信息采集更加全面具体的任务,如对于突发事故的船只,人工无法靠近或抵达,不能获得其最新发展动态并及时处理,当前技术状态下USV 处于相对目标的单一方位而无法满足该需求。因此,文中提出了一种基于USV 平台、搭载雷达载荷、通过实时感知目标信息、基于虚拟目标点跟踪算法的目标环绕跟踪方法,对目标进行环绕跟踪,完成对目标的360°全方位监视,通过此种方法能够快速、高效且安全地完成任务,提高工作效率和安全性。

使用“勇士号”USV 作为算法验证载体,如图1 所示。“勇士号”USV 是由中国科学院沈阳自动化研究所联合有关合作单位研制的新型多模式操控高速USV。

图1 “勇士号”USV 实航效果图Fig.1 Actual navigation effect of the Warrior USV

通过建立的仿真平台进行理论验证,使其具备良好的自主环绕跟踪能力,从而完成设定的任务,实航试验验证了文中所提方法的有效性。

1 基于无迹卡尔曼滤波器的目标运动状态估计方法

采用无迹卡尔曼滤波(unscented Kalman filter,UKF)对描述系统状态的均值和方差的采样点进行非线性变换,得到的点分布近似真实值和方差值于2 阶以上,同时UKF 不需要采用雅克比矩阵对状态方程和量测方程线性化,对于非线性系统,UKF 的滤波效果优于扩展卡尔曼滤波(extended kalman filter,EKF)。同时,由于滤波对象为感知信息获得,其数据分布具有非线性特点,故选用UKF 方法对目标运动状态进行估计[6-7]。

无迹变换(unscented transform,UT)方法中,假设非线性变换y=f(x),向量x是n维随机变量,设其均值为、方差为Dx,y的特性统计计算使用UT 得到的 2N+1个 χi点和对应的权值Wi。将向量x的取值范围近似为高斯分布,并假设变化点yi=f(χi)近似服从高斯分布,再通过取权值和求加权方差获得期望所求[8-9]。

点集相对均值为对称分布,同时对称点加权值相同,故其样本均值和方差与原始均值和方差相等。使用UKF 的目标运动状态估计,其计算方法如下。

1) 随机变量均值及方差计算

2) 时间更新(预测)

构建 χi点权值,共有2N+1个,过程如下:

式中:W为χ权值,且权值之和为1;为均值使用权值;为方差使用权值;η为微调参数,为(N+η)Dx平方根的第i行或第j列向量,则预测点 χi、均值和方差计算如下:

3) 量测更新

新测量值z(k)获取后,状态均值和方差更新如下:

2 目标环绕跟踪方法原理

基于艇雷达感知目标信息,包括目标的中心位置、航向、运动方向和目标标记ID 号,目标环绕跟踪模块对指定ID 号目标进行环绕跟踪。

依托于水平面航向控制器[10],采用虚拟目标点跟踪算法实现目标环绕跟踪。根据运动目标航向、速度信息,自适应调整USV 环绕半径及运动速度,充分考虑USV 本身运动特性,确定组成环绕一圈所需的虚拟目标点数量,以目标位置为原点,设定运动目标航向为Y轴正方向,根据右手定则建立运动目标坐标系,计算各点在运动坐标系中的位置。航行器在运动过程中实时计算到达虚拟目标点所需的航向角,通过水平面航向控制器完成航向闭环,航行器依次跟踪每个虚拟目标点,以重规划策略保障,进而实现目标环绕跟踪[11-12]。算法原理如图2 所示,算法总体框图如图3 所示。

图2 虚拟目标点跟踪算法原理Fig.2 Principle of virtual target point tracking algorithm

图3 算法总体框架图Fig.3 Overall framework of algorithm

图2 中:XEOYE为大地坐标系;Xs1OYs1和XsO1Ys为不同时刻下USV 运动坐标系;XA1PAYA1和XA2PAYA2为不同时刻的目标运动坐标系。

1) 虚拟目标点划分

首先根据经验拟合设计USV 的环绕速度,并自主解算环绕半径,即

式中: ε为比例系数;va为量测目标速度;v为航行器速度值;floor(·)为取数字整数函数;η1为3 阶速度参数;η2为2 阶速度参数;η3为速度变化率参数;η4为偏移参数;R为环绕半径。

根据输入速度及半径计算虚拟目标点数量

式中:λ1为比例系数;λ2为速度参数;λ3为偏移参数。然后将圆等分形成虚拟目标点集P。

2) 首虚拟目标点选取及环绕方向选择

获得虚拟目标点点数后,根据当前位置PVe对首虚拟目标点进行选取,并确定环绕方向,当PVe在小于设定半径处时,如图4 所示,以中心点为原点建立运动坐标系,设航行器在运动坐标系下坐标点为(x,y),当前目标航向角为θa,当前无人航行器航向角为θ,令H为环绕方向决断参数,有

图4 首虚拟目标点求取示意图Fig.4 Solution of the first virtual target point

式中:deal(·)为对括号内的取值规范化(在 [0,2π]区间);H≥0时,令环绕方向为顺时针,否则为逆时针;当PVe在圆外时,以航行器位置为原点,若航行器的航向与Y轴的正方向夹角小于90°,令环绕方向为顺时针,否则为逆时针。

当PVe在圆内时,如图4(a)所示,设首虚拟目标点对应点集合序号为a,即对应点为P[a],则

式中: ψad为由PVe、量测目标位置P0和虚拟目标点位置Pa所形成的 ∆P0PVePa中 ∠PaP0PVe值。PVe在圆外时,如图4(b)所示,依据环绕方向确定切点位置PA1,在虚拟目标点序列中距离PA1最近的点则为首虚拟目标点,然后将首虚拟目标点向外延长PA1PFi至PFi点,以保证USV 跟踪距离的稳定收敛至设定距离。

3) 坐标变换

将所有虚拟目标点在运动坐标系下的坐标转换为在大地坐标系下的坐标,即设转换矩阵为R,虚拟目标点序列在大地坐标系下的虚拟目标点序列为Pan、航向为φ,则有

式中: θa为目标航向角;θ为大地坐标系下USV 航向角。

4) 虚拟目标点切换判据

虚拟目标点切换判据是是否改变USV 当前所跟踪的虚拟目标点的判别依据,其切换原理如图5 所示。

图5 动态虚拟目标点切换判据原理图Fig.5 Principle of dynamic virtual target point swiching criterion

其切换条件为

式中:x和y分别为在虚拟目标坐标系下航行器位置的横、纵坐标;x0为X轴偏移参数;y0为Y轴偏移参数。

5) 重规划策略

重规划主要应对海面复杂环境及传感器误差带来的干扰,是一个容错过程。当实际航迹偏离理论计算时,结合USV 性能,进行当前USV 的最新未跟踪以及接下来的虚拟目标点位置重规划,即最近虚拟目标点距离目标进行最大距离的拉伸或缩短,然后随着USV 运动位置变化逐渐收敛至初始规划虚拟目标点位置,以最快的响应时间完成收敛进而修正航迹,实现良好的环绕跟踪[13-14]。

3 验证情况

基于USV 模型辨识方法[15]构建艇模型,并搭建USV 仿真平台进行仿真实验[16],设目标航速1.3 m/s,USV 自主规划环绕半径90 m,环绕速度18 kn,USV 依据输入信息进行虚拟目标点划分,并根据当前自身以及目标信息进行坐标转换,将虚拟目标点转换至大地坐标系下坐标,根据初始位置和环绕方向进行首虚拟目标跟踪点进行选择,再实时检测是否到达虚拟目标点切换判据,实现对所有虚拟目标点的跟踪,在此过程中,检测是否达到重规划判据,设定重规划触发条件为偏离设计虚拟目标点距离占比环绕半径超10%,以保证轨迹的快速收敛。算法输出结果通过航行控制器对执行机构进行实时控制,进而实现对目标的实时环绕跟踪,其仿真结果如图6 和图7 所示。

图6 目标环绕跟踪仿真Fig.6 Target surround tracking simulation

图7 目标环绕跟踪姿态信息Fig.7 Target surround tracking attitude information

仿真结果表明,该过程中未触发重规划功能,此过程基于虚拟目标点跟踪算法的目标环绕跟踪方法能够在虚拟目标情况下对目标进行稳定的环绕跟踪,表明算法的理论正确性。

2019 年在大连开展的海上试验中,基于“勇士号”USV 平台实时感知信息,对动态目标进行目标环绕跟踪。根据算法设计,自主规划环绕半径为150 m,环绕速度为18 kn,设定重规划触发条件为偏离设计虚拟目标点距离占比环绕半径超10%,对目标进行实时的环绕跟踪,针对感知设备量测目标航向、速度数据波动较大对目标环绕跟踪效果有较大影响问题,使用基于UKF 的目标运动状态估计对雷达感知信息输入进行处理,如图8 和图9 所示,分别为对航向和速度进行滤波和状态估计处理。目标环绕试验结果如图10 和图11 所示。

图8 航向滤波处理结果Fig.8 Course filtering processing result

图9 速度滤波处理结果Fig.9 Velocity filtering processing result

图10 目标环绕跟踪实航轨迹Fig.10 Actual flight of target surround tracking

图11 目标环绕跟踪姿态信息Fig.11 Target surround tracking attitude information

实航试验结果表明,在“勇士号”USV 平台下,基于感知信息的目标环绕跟踪方法,能够实现对未知状态的运动目标进行自主规划的目标环绕跟踪,并能够适应3 级海况,试验过程中,触发了重规划功能,如图12 所示。经过重规划功能调用,运动轨迹快速收敛至可接受范围内并保持,说明了该算法具备良好的抗干扰能力和稳定性,证明了算法设计的合理性与实用性。

图12 USV 和目标间距离与环绕半径差值Fig.12 Distance and radius difference between USV and target

4 结束语

为增强USV 的自主跟踪能力,提出了基于感知信息的目标环绕跟踪方法,算法以“勇士号”USV 作为验证平台,根据设定的目标环绕跟踪需求,结合自身航行性能,设计相关试验。该算法可对未知运动状态下的目标进行环绕跟踪,在对感知信息进行运动状态估计后,结合重规划策略,使得USV 在复杂海况下具备良好的目标环绕跟踪能力,在仿真平台下及实航效果均达到了设计需求,充分验证了算法设计的合理性、稳定性及实用性。该方法可有效持续地对锁定目标进行环绕跟踪,通过搭载其他探测载荷,可高效地对目标进行特征采集等任务,但通过试验结果发现,使用雷达感知信息作为USV 目标环绕跟踪算法输入,对雷达输入信息进行一系列处理误差仍较大,对目标环绕跟踪精度造成一定的影响,因此在未来研究中,将增加USV 载荷如光电、红外、测距仪等与雷达数据进行融合处理,以获得更为精确的信息输入,进而提高目标环绕跟踪的稳定性和精确性。