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

基于感知信息的 USV 目标环绕跟踪方法宋吉广1,2,李德隆1,2*,冯亮1,2,刘阳1,2,林扬1,2,孙铁铭1,2(1.中国科学院沈阳自动化研究所机器人学国家重点实验室,辽宁沈阳,110016;2.中国科学院机器人与智能制造创新研究院,辽宁沈阳,110169)摘要:无人水面艇(USV)作为一种智能水面工具,可实现航行控制、水面目标识别与感知等基本功能,但伴随着工程需求的多元化,如水面失事货轮、未知船只特征识别、失事飞机等搜救探测任务,该基本功能已无法完成任务,因此需要开展一种可对目标进行稳定跟踪并完成 360环视的方法研究。文中提出一种基于感知目标信息,通过对目标环绕跟踪,实现目标检测的方法。在对感知信息进行目标运动状态估计后,采用虚拟目标点跟踪算法计算出期望航向及速度,通过航向控制器及速度控制器实现对 USV 的位姿控制,同时加入重规划跟踪策略,以适应水面复杂环境并感知设备提供信息的误差扰动。以 USV 模型辨识方法获得模型并搭建仿真平台进行仿真验证,通过实航试验测试证明该算法设计的合理性。关键词:无人艇;感知信息;目标环绕跟踪;目标运动状态估计;虚拟目标点跟踪算法;重规划跟踪策略中图分类号:U674.91;TJ630.64文献标识码:A文章编号:2096-3920(2023)05-0696-07DOI:10.11993/j.issn.2096-3920.202206011TargetSurroundTrackingMethodofUSVsBasedonPerceptionInformationSONGJiguang1,2,LIDelong1,2*,FENGLiang1,2,LIUYang1,2,LINYang1,2,SUNTieming1,2(1.StateKeyLaboratoryofRobotics,ShenyangInstituteofAutomation,ChineseAcademyofSciences,Shenyang110016,China;2.InstitutesforRoboticsandIntelligentManufacturing,ChineseAcademyofSciences,Shenyang,110169,China)Abstract:Unmannedsurfacevehicle(USV),asanintelligentsurfacetool,canrealizebasicfunctionssuchasnavigationcontrol,aswellassurfacetargetrecognitionandperception.However,withthediversificationofengineeringrequirements,suchassearchandrescuemissionsorientedtowreckedfreighter,unknownshipfeaturerecognition,andwreckedaircraft,thesebasicfunctionsfailtocompletethetask.Therefore,itisnecessarytodevelopamethodtotrackthetargetstablyandcomplete360-degreescanning.Inthispaper,basedontheperceptualtargetinformation,atargetsurroundtrackingmethodwasproposedfortargetdetection.Afterestimatingthetargetmotionstatefromthesensinginformation,thevirtualtargetpointtrackingalgorithmwasusedtocalculatethedesiredheadingandvelocity.TheheadingcontrollerandspeedcontrollerwereusedtocontrolthepositionandattitudeoftheUSV.Atthesametime,are-planningandtrackingstrategywasaddedtoadaptto the complex environment of the water surface and the error disturbance of the information provided by the sensingequipment.TheUSVmodelidentificationmethodwasusedtoobtainthemodelandbuildasimulationplatformforsimulationverification.Theactualflighttestprovestherationalityofthealgorithmdesign.Keywords:unmannedsurfacevehicle;perceptualinformation;targetsurroundtracking;targetmotionstateestimation;virtualtargetpointtrackingalgorithm;re-planningandtrackingstrategy收稿日期:2022-06-22;修回日期:2022-09-30.作者简介:宋吉广(1992-)男,硕士,助理研究员,主要研究方向为无人航行器模型建立、航行控制、控制算法等.*通信作者简介:李德隆(1981-),男,硕士,正高级工程师,硕士生导师,主要研究方向为水上/水下无人航行器电控系统、总体技术.第31卷第5期水下无人系统学报Vol.31 No.52023年10月JOURNALOFUNMANNEDUNDERSEASYSTEMSOct.2023引用格式 宋吉广,李德隆,冯亮,等.基于感知信息的 USV 目标环绕跟踪方法 J.水下无人系统学报,2023,31(5):696-702.696JournalofUnmannedUnderseaSystemssxwrxtxb.xml-0引言目前,无人系统已经成为未来军事和民用领域发展的重点。近 20 年来,无人水面艇(unmannedsurfacevehicle,USV)技术成为国内外研究热点,在反水雷、侦查、监控、电子战以及反侦察作战中具有很大的优势,保证了无人员伤亡前提下完成作战任务,同时在民用领域可执行探测资源、搜救、气象监测和预报、信号中继站等任务,因此深入研究 USV 技术,对于提升我国国防实力具有重要的意义1-2。目标跟踪是 USV 研究领域的一项关键技术,USV 目标跟踪方法的研究,对于海底矿物探测、港口防御和目标打击等领域有着广泛的应用前景。USV 作为探知海洋丰富资源的一种智能多功能探测载体,需能够在无人状态下自主完成规划任务。目前,在 USV 目标跟踪技术领域,可实现对锁定目标进行基于感知信息下的目标跟踪任务,Qiao 等3使用基于平方根 Sage-Husa 自适应鲁棒卡尔曼滤波方法进行 USV 雷达目标跟踪,在对测量和观测噪声进行联合估计后,从仿真和试验中验证了该算法的有效性;饶六中等4使用基于单目视觉对水面艇目标进行跟踪,具体方式是通过比较运动过程中标志物图像与期望图像的位置关系,实时计算出偏航角和偏航距离作为控制器输入量,并控制跟踪船航向和航速,从而实现对被跟踪船的跟踪;秦世洋等5使用模型预测控制(modelpre-dictivecontrol,MPC)的方法,将环境干扰等问题转换为二次规划问题,可实现稳定的目标点跟踪。但在实际工程发展中,存在一些对目标信息采集更加全面具体的任务,如对于突发事故的船只,人工无法靠近或抵达,不能获得其最新发展动态并及时处理,当前技术状态下 USV 处于相对目标的单一方位而无法满足该需求。因此,文中提出了一种基于 USV 平台、搭载雷达载荷、通过实时感知目标信息、基于虚拟目标点跟踪算法的目标环绕跟踪方法,对目标进行环绕跟踪,完成对目标的360全方位监视,通过此种方法能够快速、高效且安全地完成任务,提高工作效率和安全性。使用“勇士号”USV 作为算法验证载体,如图 1 所示。“勇士号”USV 是由中国科学院沈阳自动化研究所联合有关合作单位研制的新型多模式操控高速 USV。图1“勇士号”USV 实航效果图Fig.1ActualnavigationeffectoftheWarriorUSV通过建立的仿真平台进行理论验证,使其具备良好的自主环绕跟踪能力,从而完成设定的任务,实航试验验证了文中所提方法的有效性。1基于无迹卡尔曼滤波器的目标运动状态估计方法采用无迹卡尔曼滤波(unscentedKalmanfilter,UKF)对描述系统状态的均值和方差的采样点进行非线性变换,得到的点分布近似真实值和方差值于 2 阶以上,同时 UKF 不需要采用雅克比矩阵对状态方程和量测方程线性化,对于非线性系统,UKF 的滤波效果优于扩展卡尔曼滤波(extendedkalmanfilter,EKF)。同时,由于滤波对象为感知信息获得,其数据分布具有非线性特点,故选用UKF 方法对目标运动状态进行估计6-7。y=f(x)xDx2N+1iWiyi=f(i)无迹变换(unscentedtransform,UT)方法中,假设非线性变换,向量 x 是 n 维随机变量,设其均值为、方差为,y 的特性统计计算使用UT 得到的个点和对应的权值。将向量x 的取值范围近似为高斯分布,并假设变化点近似服从高斯分布,再通过取权值和求加权方差获得期望所求8-9。点集相对均值为对称分布,同时对称点加权值相同,故其样本均值和方差与原始均值和方差相等。使用 UKF 的目标运动状态估计,其计算方法如下。1)随机变量均值及方差计算 x=Ex(0)D(0)=Ex0 x(0)x0 x(0)T(1)2)时间更新(预测)i2N+1构建点权值,共有个,过程如下:2023年10月宋吉广,等:基于感知信息的 USV 目标环绕跟踪方法第5期水下无人系统学报sxwrxtxb.xml-697i=x,i=0 x+(N+)Dx)i,i 1,Nx(N+)Dx)i,i 1,2N(2)W(a)i=W(b)i=/(N+),i=01/2(N+),i 1,2N(3)(k1)=x(k1),x(k1)+(N+)Dx(k1)i x(k1)(N+)Dx(k1)i,i 1,N(4)W(a)iW(b)i(N+)Dx(N+)Dxi式中:W 为 权值,且权值之和为 1;为均值使用 权 值;为 方 差 使 用 权 值;为 微 调 参 数,为平方根的第 i 行或第 j 列向量,则预测点、均值和方差计算如下:i(k)=fi(k1),i 0,2N(5)x(k)=2Ni=0W(a)ii(k)D(k)=2Ni=0W(b)ii(k)x(k)i(k)x(k)T+Q(k)(6)3)量测更新z(k)新测量值获取后,状态均值和方差更新如下:x(k)=x(k)+K z(k)z(k)Dx(k)=Px(k)KDz(k)KTK=Dxz(k)D1z(k)(7)z(k)=2Ni=0W(a)ihi(k)(8)Dz(k)=2Ni=0W(b)ihi(k)z(k)hi(k)z(k)T+R(k)(9)Dxz(k)=2Ni=0W(b)ii(k)x(k)hi(k)z(k)T(10)2目标环绕跟踪方法原理基于艇雷达感知目标信息,包括目标的中心位置、航向、运动方向和目标标记 ID 号,目标环绕跟踪模块对指定 ID 号目标进行环绕跟踪。依托于水平面航向控制器10,采用虚拟目标点跟踪算法实现目标环绕跟踪。根据运动目标航向、速度信息,自适应调整 USV 环绕半径及运动速度,充分考虑 USV 本身运动特性,确定组成环绕一圈所需的虚拟目标点数量,以目标位置为原点,设定运动目标航向为 Y 轴正方向,根据右手定则建立运动目标坐标系,计算各点在运动坐标系中的位置。航行器在运动过程中实时计算到达虚拟目标点所需的航向角,通过水平面航向控制器完成航向闭环,航行器依次跟踪每个虚拟目标点,以重规划策略保障,进而实现目标环绕跟踪11-12。算法原理如图 2 所示,算法总体框图如图 3 所示。R目标Xs1XEYEXs1Ys1XA1YA1OO1USVXA2YA2环行方向PAPAUSVYs1图2虚拟目标点跟踪算法原理Fig.2Principleofvirtualtargetpointtrackingalgorithm切换判定控制计算重规划艇位姿信息感知目标信息算法输入信息处理目标环绕跟踪方法是否水面航行控制器执行机构准备结束任务继续否是图3算法总体框架图Fig.3OverallframeworkofalgorithmXEOYEXs1OYs1XsO1YsXA1PAYA1图 2 中:为大地坐标系;和为 不 同 时 刻 下 USV 运 动 坐 标 系;和2023年10月水下无人系统学报第31卷698JournalofUnmannedUnderseaSystemssxwrxtxb.xml-XA2PAYA2为不同时刻的目标运动坐标系。1)虚拟目标点划分首先根据经验拟合设计 USV 的环绕速度,并自主解算环绕半径,即v=(va+0.5)(1+ev/10)(11)R=floor(1v3+2v2+3v+4)(12)vafloor()1234式中:为比例系数;为量测目标速度;v 为航行器速度值;为取数字整数函数;为 3 阶速度参数;为 2 阶速度参数;为速度变化率参数;为偏移参数;R 为环绕半径。根据输入速度及半径计算虚拟目标点数量N=floorR(v2)1+31(13)123式中:为比例系数;为速度参数;为偏移参数。然后将圆等分形成虚拟目标点集P。2)首虚拟目标点选取及环绕方向选择PVePVe获得虚拟目标点点数后,根据当前位置对首虚拟目标点进行选取,并确定环绕方向,当在小于设定半径处时,如图 4 所示,以中心点为原点(x,y)a建立运动坐标系,设航行器在运动坐标系下坐标点为,当前目标航向角为,当前无人航行器航向角为,令 H 为环绕方向决断参数,有H=sign(deal(2+a)(14)deal()0,2H 0PVe式中:为对括号内的取值规范化(在区间);时,令环绕方向为顺时针,否则为逆时针;当在圆外时,以航行器位置为原点,若航行器的航向与 Y 轴的正方向夹角小于 90,令环绕方向为顺时针,否则为逆时针。PVePa当在圆内时,如图 4(a)所示,设首虚拟目标点对应点集合序号为 a,即对应点为,则a=floorR(v2)1+31deal(2+a)+ad(15)adPVeP0PaP0PVePaPaP0PVePVePA1PA1PA1PFiPFi式中:为由、量测目标位置和虚拟目标点位置所形成的中值。在圆外时,如图 4(b)所示,依据环绕方向确定切点位置,在虚拟目标点序列中距离最近的点则为首虚拟目标点,然后将首虚拟目标点向外延长至点,以保证 USV 跟踪距离的稳定收敛至设定距离。3)坐标变换Pan将所有虚拟目标点在运动坐标系下的坐标转换为在大地坐标系下的坐标,即设转换矩阵为 R,虚拟目标点序列在大地坐标系下的虚拟目标点序列为、航向为,则有R=sin()cos()cos()sin()Pan=PaR=deal(2+a)(16)a式中:为目标航向角;为大地坐标系下 USV 航向角。4)虚拟目标点切换判据虚拟目标点切换判据是是否改变 USV 当前所跟踪的虚拟目标点的判别依据,其切换原理如图 5 所示。其切换条件为(x x0)(y y0)(17)顺时针逆时针PFiPA2PAxyPA1首次执行期望路线非首次执行期望路线XYOPVe(a)PVe 在圆内部(b)PVe 在圆外部PVePa0PaP0adXYO图4首虚拟目标点求取示意图Fig.4Solutionofthefirstvirtualtargetpoint2023年10月宋吉广,等:基于感知信息的 USV 目标环绕跟踪方法第5期水下无人系统学报sxwrxtxb.xml-699x0y0式中:x 和 y 分别为在虚拟目标坐标系下航行器位置的横、纵坐标;为 X 轴偏移参数;为 Y 轴偏移参数。5)重规划策略重规划主要应对海面复杂环境及传感器误差带来的干扰,是一个容错过程。当实际航迹偏离理论计算时,结合 USV 性能,进行当前 USV 的最新未跟踪以及接下来的虚拟目标点位置重规划,即最近虚拟目标点距离目标进行最大距离的拉伸或缩短,然后随着 USV 运动位置变化逐渐收敛至初始规划虚拟目标点位置,以最快的响应时间完成收敛进而修正航迹,实现良好的环绕跟踪13-14。3验证情况基于 USV 模型辨识方法15构建艇模型,并搭建 USV 仿真平台进行仿真实验16,设目标航速1.3m/s,USV 自主规划环绕半径 90m,环绕速度18kn,USV 依据输入信息进行虚拟目标点划分,并根据当前自身以及目标信息进行坐标转换,将虚拟目标点转换至大地坐标系下坐标,根据初始位置和环绕方向进行首虚拟目标跟踪点进行选择,再实时检测是否到达虚拟目标点切换判据,实现对所有虚拟目标点的跟踪,在此过程中,检测是否达到重规划判据,设定重规划触发条件为偏离设计虚拟目标点距离占比环绕半径超 10%,以保证轨迹的快速收敛。算法输出结果通过航行控制器对执行机构进行实时控制,进而实现对目标的实时环绕跟踪,其仿真结果如图 6 和图 7 所示。仿真结果表明,该过程中未触发重规划功能,此过程基于虚拟目标点跟踪算法的目标环绕跟踪方法能够在虚拟目标情况下对目标进行稳定的环绕跟踪,表明算法的理论正确性。2019 年在大连开展的海上试验中,基于“勇士号”USV 平台实时感知信息,对动态目标进行目标环绕跟踪。根据算法设计,自主规划环绕半径为150m,环绕速度为 18kn,设定重规划触发条件为偏离设计虚拟目标点距离占比环绕半径超 10%,对目标进行实时的环绕跟踪,针对感知设备量测目标航向、速度数据波动较大对目标环绕跟踪效果有较大影响问题,使用基于 UKF 的目标运动状态估计对雷达感知信息输入进行处理,如图 8 和图 9 所示,分别为对航向和速度进行滤波和状态估计处理。目标环绕试验结果如图 10 和图 11 所示。实航试验结果表明,在“勇士号”USV 平台下,基于感知信息的目标环绕跟踪方法,能够实现对未知状态的运动目标进行自主规划的目标环绕跟踪,并能够适应 3 级海况,试验过程中,触发了重规PaimaXYXYOUSV(x0,0)(0,y0)图5动态虚拟目标点切换判据原理图Fig.5Principle of dynamic virtual target point swich-ingcriterion80060040020002004003002001000100200 x/my/m初始位置结束位置初始位置结束位置*USV轨迹目标轨迹图6目标环绕跟踪仿真Fig.6Targetsurroundtrackingsimulation0200400USV航向/()234USV航速/(m/s)02004006008001 000024t/s目标速度/(m/s)图7目标环绕跟踪姿态信息Fig.7Targetsurroundtrackingattitudeinformation2023年10月水下无人系统学报第31卷700JournalofUnmannedUnderseaSystemssxwrxtxb.xml-划功能,如图 12 所示。经过重规划功能调用,运动轨迹快速收敛至可接受范围内并保持,说明了该算法具备良好的抗干扰能力和稳定性,证明了算法设计的合理性与实用性。020040060080020020406080100120t/sD/m520 522 524 52616.015.515.0图12USV 和目标间距离与环绕半径差值Fig.12DistanceandradiusdifferencebetweenUSVandtarget4结束语为增强 USV 的自主跟踪能力,提出了基于感知信息的目标环绕跟踪方法,算法以“勇士号”USV 作为验证平台,根据设定的目标环绕跟踪需求,结合自身航行性能,设计相关试验。该算法可对未知运动状态下的目标进行环绕跟踪,在对感知信息进行运动状态估计后,结合重规划策略,使得 USV 在复杂海况下具备良好的目标环绕跟踪能力,在仿真平台下及实航效果均达到了设计需求,充分验证了算法设计的合理性、稳定性及实用性。该方法可有效持续地对锁定目标进行环绕跟踪,通过搭载其他探测载荷,可高效地对目标进行特征采集等任务,但通过试验结果发现,使用雷达感知信息作为 USV 目标环绕跟踪算法输入,对雷达输入信息进行一系列处理误差仍较大,对目标环绕跟踪精度造成一定的影响,因此在未来研究中,将增加 USV 载荷如光电、红外、测距仪等与雷达数据进行融合处理,以获得更为精确的信息输入,进而提高目标环绕跟踪的稳定性和精确性。100200300400500600700050100150200250源信息UKF处理信息/()t/s图8航向滤波处理结果Fig.8Coursefilteringprocessingresult100200300400500600700012345t/sv/(m/s)源信息UKF处理信息图9速度滤波处理结果Fig.9Velocityfilteringprocessingresult400200020020002004006008001 000 x/my/m结束位置初始位置结束位置初始位置*USV轨迹目标轨迹图10目标环绕跟踪实航轨迹Fig.10Actualflightoftargetsurroundtracking0200400USV航向/()51015USV航速/(m/s)010020030040050060070005t/s目标速度/(m/s)图11目标环绕跟踪姿态信息Fig.11Targetsurroundtrackingattitudeinformation2023年10月宋吉广,等:基于感知信息的 USV 目标环绕跟踪方法第5期水下无人系统学报sxwrxtxb.xml-701参考文献:黄琰,李岩,俞建成,等.AUV智能化现状与发展趋势J.机器人,2020,42(2):215-231.HuangYan,LiYan,YuJiancheng,etal.State-of-the-artanddevelopmenttrendsofAUVintelligenceJ.Robot,2020,42(2):215-231.1陈霄,刘忠,董蛟,等.欠驱动无人艇路径跟踪控制算法J.海军工程大学学报,2018,30(3):108-112.ChenXiao,LiuZhong,DongJiao,etal.Pathfollowingalgorithm of underactuated USVJ.Journal of NavalUniversityofEngineering,2018,30(3):108-112.2QiaoSH,FanYS,WangGF.Radartargettrackingforunmanned surface vehicle based on square root Sage-HusaadaptiverobustKalmanfilterJ.Sensors,2022(22):2924.3饶六中,王建华,郭翔,等.基于单目视觉的无人水面艇目标跟踪方法J.传感器与微系统,2022,41(7):44-51.RaoLiuzhong,WangJianhua,GuoXiang,etal.Mon-ocular vision-based method for target tracking of USVJ.Transducer and Microsystem Technologies,2022,41(7):44-51.4秦世洋,张腾,武绍宽,等.水上无人艇目标跟踪控制方法研究J.中北大学学报(自然科学版),2020,41(2):149-160.QinShiyang,ZhangTeng,WuShaokuan,etal.Targettracking of unmanned surface vehicle based on modelpredicitive controlJ.Journal of North University ofChina(NaturalSciencEdition),2020,41(2):149-160.5黄胜昔,刘华.基于加性无迹卡尔曼滤波的目标跟踪方法J.计算机工程与应用,2010,46(8):214-216.HuangShengxi,LiuHua.Methodforradartargettrack-ing based on additive sequential unscented KalmanfilterJ.ComputerEngineeringandApplications,2010,46(8):214-216.6高剑,徐德民,严卫生,等.无迹卡尔曼滤波及其在三维水下目标跟踪系统中的应用J.船舶工程,2005,27(3):24-28.GaoJian,XuDemin,YanWeisheng,etal.UKFanditsapplicationto3-DunderwatertargettrackingsystemJ.ShipEngineering,2005,27(3):24-28.7李峰.自主水下航行器目标跟踪方法研究D.哈尔滨:8哈尔滨工程大学,2013.丁浩晗,冯辉,徐海洋.基于自适应无迹卡尔曼滤波的动力定位状态估计J.大连海事大学学报,2016,42(4):8-12.DingHaohan,FengHui,XuHaiyang.Attitudeestima-tionofdynamicpositioningsystembasedonadaptiveun-scented Kalman filterJ.Journal of Dalian MaritimeUniversity,2016,42(4):8-12.9朱齐丹,马俊达,刘可.基于扰动观测器的无人水面船鲁棒轨迹跟踪J.电机与控制学报,2016,20(12):66-73.ZhuQidan,MaJunda,LiuKe.Anonlineardisturbanceobserverbasedonrobustapproachtothetrajectorytrack-ing of an unmanned surface vehicleJ.Electric Ma-chinesandControl,2016,20(12):66-73.10陈烨.近岸浅海环境下UUV的动目标跟踪方法研究D.哈尔滨:哈尔滨工程大学,2016.11付悦文.小型无人艇的无模型自适应跟踪方法研究D.哈尔滨:哈尔滨工程大学,2017.12时晶晶.小型自主水下航行器目标跟踪控制方法研究D.哈尔滨:哈尔滨工程大学,2012.13庄佳园,苏玉民,廖煜雷,等.基于航海雷达的水面无人艇局部路径规划J.上海交通大学学报,2012,46(9):1372-1375.ZhuangJiayuan,SuYumin,LiaoYulei,etal.Unmannedsurface vehicle local path planning based on marineradarJ.JournalofShanHaiJiaotongUniversity,2012,46(9):1372-1375.14周焕银,封锡盛,胡志强,等.基于多辨识模型优化切换的USV航向动态反馈控制J.机器人,2013,35(5):553-557.ZhouHuanyin,FengXisheng,HuZhiqiang,etal.Dy-namicfeedbackcontrollerbasedonoptimizedswitchingofmultipleidentificationmodelsforcoursecontrolofun-mannedsurfacevehicleJ.Robot,2013,35(5):553-557.15FanYS,SunYT,WangGF.Onmodelparameteridenti-ficationandtrajectorytrackingcontrolforUSVbasedonbacksteppingC/Proceedingsof the 36th Chinese Con-trolConference.Dalian,China:ChineseControlConfer-ence,2017:4757-4761.16(责任编辑:许妍)2023年10月水下无人系统学报第31卷702JournalofUnmannedUnderseaSystemssxwrxtxb.xml-