USV 地形测量路径自主规划研究与应用

2024-01-03宋吉广张京晶李德隆谷海涛

舰船科学技术 2023年22期

宋吉广，张京晶，冯亮，林扬，李德隆，谷海涛

(1. 中国科学院沈阳自动化研究所机器人学国家重点实验室，辽宁沈阳 110016；2. 中国科学院机器人与智能制造创新研究院，辽宁沈阳 110169；3. 中国人民解放军91053 部队，北京 100070)

0 引言

一直以来，近岸海域水上水下地形测量是海洋工程领域的一项重要内容，对于海运安全、港口领航、维护国家主权、沿海城市及岛礁的海洋开发、规划决策等都具有重要意义[1]。近岸海域周围具有环境复杂、缺乏先验信息和海况恶劣等特点，而无人水面艇作为一种小型智能无人平台，具备自主感知环境信息、路径规划、自主航行的能力，可通过人工预置或自主信息采集处理的方式完成环境感知、目标识别与探测、自主规划等多种任务。近年来，相关研究人员针对不同的任务需求进行水运航道、水库、岛屿等水域的水上水下地形测量。但在测量过程中，多以人工设定测量路径的方式为主[2-5]，在测量区域较大或较为复杂的情况下，需耗费巨大人力、物力和财力，且布设测量路径带有人为主观性，影响最终测量效果。

操文芷[6]使用电子海图与雷达图像叠加匹配方式，在此基础上进行无人水面艇路径规划，可有效进行障碍物规避以及局部路径规划动任务。刘强等[7]采用船载多传感器一体化测量技术对海岛礁、港口地区与水中构筑物、湖泊、库区进行测量，完成了对测量区域的水上水下一体化点云无缝拼接。苗润龙等[8]使用基于重复海湾和遗漏海湾的搜索行为设计方法完成了海洋自主航行器多海湾区域完全遍历路径规划试验，规划结果在遍历性、重复率方面优于其他遍历方法。上海大学 “精海3 号”无人艇水面船在金塘大桥附近按设定的规划航线进行测绘工作，因人为规划航线只能扫测航线所覆盖部分的区域，钟雨轩等[9]提出一种考虑主动方向的动态栅格法与启发式搜索算法的无人水面艇岛礁海域完全遍历路径规划方法，并通过了仿真验证。

由现研究现状分析知，在进行水上水下一体化测量时均将海图作为已知输入，若在未知地形复杂的近岸海域进行测量工作时，无法进行测量相关的路径规划工作，同时易出现搁浅、刮底事故，不能保障研究人员及设备安全。因此，设计一种能够适应近岸复杂的水上水下环境，准确感知周围态势信息并合理规划测量路径的方法是解决以上问题的关键。现阶段，采用具有自主定位、导航、感知及控制功能的无人水面艇，携带相关测量设备，无疑是解决上述问题、并可以更快、更经济和更安全地完成近岸水上水下地形测量任务的一种理想手段。本文以USV 为研究平台，搭载导航雷达、深度计、POS MV、多波束测深仪及激光扫描仪和其他必要设备，面向近岸地形测量任务需求，基于离线电子海图，实现测量路径在线规划，以完全覆盖预设待测量海域，最终获取未知环境的水上水下地形地貌。首先，基于感知、测量设备信息，实现在线地图构建工作[10]，本文的在线地图信息输入来自于雷达感知及深度计信息，其中，雷达感知数据提供无人水面艇所在环境周围态势信息，由深度计提供无人水面艇实时深度信息，通过POS MV 获取USV 实时位姿，融合信息输入，实时构建测量地图。在此基础上，采用基于神经元激励方法下的完全遍历路径规划设计，结合A 星算法路径搜寻优势防止路径规划锁死，为USV 提供安全有效的规划路径，提高USV 测量工作的安全性、合理性、有效性。同时，考虑实时水深对测量设备扫测的影响，自主调节测量路径的梳型间距，以达到要求的扫描重叠率，实现完全覆盖预定测量区域[11]。

1 地图构建方法设计

首先，存在一个基本问题，即如何使用地图描述测量区域环境，目前已存在多种地图表示方法，主要分为栅格地图、特征地图和拓扑地图3 类[12]。本文采用栅格地图方式，使用占据概率的栅格地图将测量区域分成若干相同边长的栅格，将每个栅格分配概率值以表示被障碍物占据的可能性大小。无人艇搭载导航雷达和深度计设备，实时更新栅格地图，根据路径规划方法生成符合海底地形测量要求的实时梳型路径[13-14]，能应对复杂环境下各类问题，在实时性和安全性方面具有较强优势[15]。

在线地图构建过程如图1 所示。其中，基准地图是指由初始离线地图和实时的雷达感知信息以及水深等信息所构建的实时地图，地图北向为等深线[16]由深至浅方向；测量地图是由基准地图作为输入，根据水深情况以及海底地形测量标准需求更新栅格大小，该地图直接用于测量路径规划算法输入。

图1 在线地图构建流程图Fig. 1 Flow chart of online map construction

地图构建采用一种基于贝叶斯估计方式的占据栅格建图方法。占据栅格地图把测量范围分解成若干二元随机变量填充的栅格，分为占据或空闲2 种状态。每一个栅格具有独立性，估计环境地图需要对每个独立的栅格进行估计，更新地图时，通过传感器观测结果计算获得[17]。

初始离线地图/基准地图采用一系列二值随机变量表示，该二值数据表示该位置是否被占据，即是否存在障碍物[18]，而待更新地图处理方法是对二值随机变量进行后验估计。现已知USV 位姿信息x和测量值z，为构建规范化地图m，设其根据给定的位姿和测量信息计算整个地图的后验概率为p(m|z1:k,x1:k)。其中z1:k表示1 到k时刻的测量值，二值化后取0 或1（有障碍物1，无障碍物0），x1:k表示1 到k时刻的位姿组成的路径（梳型完全遍历），地图将二维空间分割为有限多个栅格单元（即网格），地图表示如下：

其中，mij为地图中第i行j列个栅格单元，mij的值代表栅格是否被占据（是否存在障碍物），M为地图的栅格最大行数，N为地图的栅格最大列数，如果被占据则值为“1”，未被占据则为“0”。p(mij)为该栅格单元被占据的可能性。对于后验概率p(m|z1:k,x1:k)，由于m的联合概率计算的维数为指数级2|MN|，因此假设地图中的每个栅格单元mij相互独立，通过条件独立性假设，可用边缘概率乘积近似表示整个地图m的后验概率。

地图中每个栅格单元的占据概率由静态二值贝叶斯滤波方法获得，该方法依靠传感器的测量信息来估计环境地图中一个固定的二值数，该二值数分别表示此次更新为占据或空闲，且假设地图在此期间的状态不改变。该方法中，占据概率用后验概率的对数几率表示，以保证0 和1 附近数值的稳定性。

则在时刻k第i行j列栅格单元的占据概率pk,ij为：

因此，可得后验概率为：

证明式（3）。

对于后验概率，由贝叶斯准则有：

对于zk，由马尔可夫过程属性可知，当xk已知时其与x1:k-1和z1:k-1无关，对式(5)有：

再对p(zk|mij,xk)应用贝叶斯准则有：

在zk未知时，有，且。再将式（8）代入式(5)中有：

对于mij的对立事件，同理有：

则式(9)除以式(10)，并取对数，可得：

其中，p(mij)为先验概率，定义测量前初值为：

则有：

将式(13)更新方式为迭代更新，则有：

对于环境中未被探索区域中栅格，设初始化值p0。pk为时刻k对地图的更新。

由于无人艇在不同位姿x时，雷达感知环境不同，经过研究雷达感知数据表明，无人艇姿态稳定状态下，雷达将会获得更优的感知数据，且对不同角度、不同距离的障碍物感知效果不一。故在获得感知数据后，取无人艇前向 2φ角度、距离dR∈[Dmin,Dmax]内数据。其中，Dmin和Dmax为设定的最近和最远可用感知数据距离。为排除雷达杂波的干扰，求取每个栅格倍距离内测量值z二值化竖直为“1”栅格的个数 κ，通过以下形式获得存在障碍物的概率值：

其中，pSij为构成当前栅格的输入小栅格面积相关概率值。

初始离线地图栅格化边长或输入雷达感知信息和深度计信息所构成的栅格边长设为l，均小于基准地图栅格大小L，已知2 个地图间的旋转角度 θ，首先依次判断小栅格的4 个顶点是否在大栅格内部，经对各种情况分析有：当有2 个点在基准地图栅格外时，查看基准地图端点是否有在小栅格内部，若不在且距离小栅格中心最近的端点和基准地图内部端点异侧，与基准地图栅格有3 个交点否则有2 个交点。在依次获得各个交点后，进行逆时针排序，得到多边形的边数n，通过如下公式进行面积占据求取：

通过以上方法得到基准地图每个栅格的面积大小。由此可得面积覆盖比率，等效为面积覆盖概率，有

式中：pSij为由雷达和深度计获得信息在基准地图中，第i行j列为占用状态的面积概率；K为更新信息落在该基准地图中的小栅格个数。

将式(15)结果代入式(3)获得在时刻k第i行j列栅格单元的占据概率pk,ij。通过设定阈值 α来确定栅格的二值化值zk,ij。

测量地图的设定需按照海底地形测量及多波束扫描重复率相关要求进行设计。首先，确定测量地图栅格边长Lsd，如下式：

式中： η为多波束扫描重复率设置值；dwat为当前的深度计测量最浅水深； Ω为测量状态下的多波束开角；Γ为当前基准地图的横向或纵向边长，其所在边与主体遍历方向垂直，与等深线方向约平行。L为基准地图栅格边长；Lsdmax为完全遍历梳型路径间的最大间距，即栅格的最大边长；dl为多波束射线可扫描的最远距离； Ω为多波束扫描开角；ceil()为任意数的向上取整函数；floor()为任意数的向下取整函数。

然后，将基准地图信息按照上述使用面积法求取概率，并依据式(17)求出在时刻k第i行j列栅格单元的占据概率p'k,ij。同时设定阈值 β来确定栅格的二值化值z'k,ij，所获得的z'k作为完全遍历路径规划方法输入。

2 完全遍历路径规划方法

在k时刻测量地图为Mc×Nc二值化矩阵，令每个栅格等效为神经元，并被赋予活性值，邻近活性值最大的神经元具有被选择优先性。神经元活性值由固定活性值和外部激励确定[19]，当为障碍物时，设其活性值为-1，若不为障碍物则其活性值计算公式为：

式中： λ为该栅格的活性值，由其所在位置及遍历方向决定，遍历方向与等深线方向垂直；x为在测量地图坐标系横轴坐标值；c为常数。 τ为神经元活性值外部激励，由于在海底地形测量过程中，无人艇频繁的转弯会使多波束测深仪扫描点云数据拼接后误差增大，为保证无人艇的良好运动姿态，设τ为关于转弯角度和移动距离的函数，则有：

式中： δ为当前规划路径方向转向预规划栅格所需要的角度；dm为当前所在栅格位置移动到预规划栅格的距离。

将式(21)代入式(20)，分别选择当前栅格周围的7 个栅格(不计上一周期所在栅格，尽量避免USV 原地掉头)作为下一移动目标，在其中选择活性值最大的作为移动目标。

在实际海底地形测量过程中，无人艇所在区域环境复杂，由于海底地形测量要求以及无人艇操纵性限制，在路径规划过程中可能出现锁死现象，因此加入A*算法[20]作为疏导策略，寻找距离当前位置最近的非障碍物栅格位置。同时，对A*所规划路径进行平滑处理，使得无人艇最快速度到达指定栅格位置。

此外，在出现危险状况时，如突然进入测量区域的非合作船舶、水深变浅等，伴随测量地图的变化，进行路径重规划工作，为防止再次将已完成测量的区域作为规划区域，在进行测量路径重规划时，将历史形式区域神经元活性值置-1，以保证测量工作的高效性。

在Matlab 仿真平台下，设定测量地图z栅格化后，行数Mc=30，列数Nc=29。随机设置障碍物的位置及形状，其仿真结果如图2 所示。

可知，该路径规划方法可在复杂环境中较好地规划出符合海底地形测量标准的梳型路径，对所测区域进行全覆盖遍历，且有效避免了规划过程中出现规划锁死现象，以最快方式到达符合要求的下一规划位置，继续完成路径规划任务。这种方法在实际的复杂环境中具有较好的环境适应性和安全性，尤其可较好地应对未知海域岛岸环境。

3 实航验证情况

基于水上水下一体化无人测量任务需求，以中科院沈阳自动化研究所研制的“问海”号无人水面测量艇作为试验平台，对本文提出的在线地图构建方法和完全遍历路径规划方法在松花湖某区域进行实航验证。

设定测量区域为700×800 m 近岸水域，多波束扫描重复率设置值η=21%，测量状态下的多波束开角Ω=120°，基准地图栅格边长L=15，完全遍历梳型路径间距Lsd=50，即栅格边长，多波束射线可扫描的最远距离dl=200，则有行数Mc=15，列数Nc=13。输入空白初始离线地图，设定测量艇航行速度为4 kn，“问海”号USV 通过搭载的导航雷达、深度计、多波束、激光扫描仪等设备，自主感知环境信息按照文中所设计方法进行地图构建，实时更新基准地图，当历史规划路径与现地图环境冲突时，进行路径重规划，无人艇对规划路径进行实时的轨迹跟踪，按此步骤直至完成设定区域的水上水下一体化测量。考虑到水深变化导致的USV 安全问题，在实际应用中加入等深线方向思路，同时自主更改测量区域的边界情况，以此方式提高测量工作的效率。其实航试验结果如图3所示。

图3 初始全局完全遍历路径规划结果Fig. 3 Initial global complete traversal path planning result

图4 初次基准地图构建结果Fig. 4 Initial baseline map construction results

由试验结果分析可知，在设定测量区域后，问海号USV 可实时感知周围环境。经过实时的雷达感知信息栅格化处理以及占据概率计算形成数据更新源，实时改变基准地图，最终构建测量地图，经过与实际的测量区域卫星图比对，地图构建精度满足测量路径规划要求。基准地图的实时更新驱动路径规划方法的重规划功能，如图5 和图7 所示。此时USV 在图中所示的起点位置附近，按照路径规划方法设计，其能够在此种环境中进行任意的梳型路径规划，且可保证避免规划过程中的锁死情况。通过图8 和图10 可知，基于完全遍历路径规划方法，可实现待测区域的测量路径全覆盖。图11 为一体化成图效果，完成了水上水下的无缝拼接。然后对测量质量进行评估，多波束测深仪测量内均方差不大于0.03 m，可形成1∶500 数字高程地图。该结果满足测量精度和测量效率的要求，即本文提出的在线地图构建方法以及完全遍历路径规划方法设计合理可行，测量结果满足海洋工程地形测量规范要求。