基于无人测试船水面路径规划策略∗

石钊铭

（武汉市江夏区藏龙大道709号 武汉 430205）

1 引言

随着科学技术的高速发展，各种机器人越来越多地应用到人们的日常生活中，从事着与人们日常生活息息相关的服务工作，改善和提高了人们的生活质量［1］。无人水面艇作为集多传感器综合于一体的科技产品，在诸如测绘、军事演习等各行各业扮演着主要的角色。

机器人在封闭环境中的全覆盖路径规划一直是较为热门的话题，而应用场景也较为丰富，例如扫地机器人的全覆盖清扫路径，无人机的农药喷洒路径，国内外有众多学者对此展开了大量的研究，贺嘉等在《基于贪心蚁群算法的无人水面艇全局路径规划》提出了双向搜索法优化传统贪心算法，使得迭代时间和次数减少，并取得了较好的路径规划结果［2］。简毅等在《一种室内扫地机器人全遍历路径规划方法研究》中，提出了先学习子区域，然后由边缘向中靠拢的路径规划算法［3］。

本文首先对水面进行栅格化，然后采用旅行商算法进行全覆盖式路径规划，根据路径规划发布的路径点来动态的优化运动轨迹，采取了转角与直行同步的策略。提高了无人测试船的巡航效率，避免规划路径中锐角调整时间过长的现象，减少过多减速以及转折的次数，保证了无人测试船运动位姿的美观性。最后在ROS操作系统下，利用系统工具快速搭建实验平台，以便于测试算法的可行性。

本文算法流程如图1所示。

图1 算法流程

2 覆盖式路径规划算法

在进行覆盖式路径规划之前，本文首先运用占据栅格地图（Occupancy Grid Map）法［4］对已知水域进行建模。栅格法建模，是采用一系列同样大小的栅格对水域进行建模的方法。在栅格地图中，白色方块代表水域中的可行区域，黑色方块代表水域中的不可行区域，例如边界或者水中高地，需要绕开进行路径规划。

根据切片法进行细胞单元划分，如图2所示地图区域划分为共7个细胞区域，然后对于每个单元格有两种出入方式。则对于若干个子细胞而言，全覆盖路径规划是旅行商问题［5］。个体基因由数字1～N的排列组成，本文将个体基因确定为4N个细胞路径组合。单元id和出入点通过4N的模进行解码，基因之间不可重复。

图2 切片划分子细胞示意图

遗传根据适应度函数对群体进行遗传更新，本文采用如下适应度函数：

其中ωdist与ωtime分别代表总流程与总时间的权值。

根据遗传算法模型，本文采用全覆盖路径规划算法描述如下。

1）首先建立解码和编码的数学模型，采用洗牌算法随机打乱细胞排列进行初始化，然后随机选择一个细胞的入口和出口点；

2）依据初始种群计算最优路径，指定当前最优解为初始解；

3）随机选择两个细胞作为父母细胞，利用交叉算子［6］产生后代，当子代数目与初始数目相同时停止交叉，交叉后根据变异概率进行变异，并将子代变异前后交换位置；

4）对于步骤3）的子代，将规定突变概率最为交换规则，随机变换细胞的进入和退出点，然后找到每个细胞的最佳进入退出点；

5）用步骤4）的后代取代初始种群，计算更新后的种群单体的路径总长度，如果更新后的总长度比更新前的总长度短，则更新计算结果；

6）将步骤3）～5）进行迭代，直到进化与分配后代数目相同为止，得到遗传算法的最优解。

3 基于动态圆的路径优化

无人测试船在水面的运动模型解析为角速度和线速度，可无人测试船在水面的运动状态，实现转弯或直行等运动。通过控制两电机的转速差来实现机器人在二维平面里的运动。

根据水面环境的栅格地图建模得到地图数据，将第二节规划的路径输出为一系列路径点，在栅格地图中，路径规划后，发布的路径如图2所示，绿色线条即为发布的路径。

图2路径规划中，可归纳无人测试船的三种运动情况，测试船在遇到边界前进行直线运动，转角有锐角、直角、钝角三种情况。取其中钝角时如图3所示。

图3 钝角情况示意图

无人测试船先到达点A、转弯后到达点B、最后到达C，在图4中，假设AB=a，BC=b，AC=c，设定阈值为L，由此可得动态圆的半径：

根据C点相对直线AB的位置，即可确定出角速度的方向，已知两点 p1(x1，y1)，p2(x2，y2)，假设p1与 p2不重合，则任意点 p(x，y)相对直线L的位置，可根据Temp，可由以下模型得到：

判断点与直线位置关系步骤如下：

1）若x比Temp小，则点p位于直线L左侧；

2）若x比Temp大，则点p位于直线L右侧；

3）若x与Temp相等，则直线L穿过点p。

如图4所示，由半径R根据线速度和角速度的数学关系，计算转弯比例大小。则在转弯的路径单元内，无人测试船从点A到点C的过程为先由点A直线运动到点M，然后以点O为圆心，R为半径，弧线运动到点G，最后直线运动到达点C。

图4 动态圆计算示意图

4 平台测试并比较效率

ROS（Robot Operating System）是一个机器人软件平台，它能为异质计算机集群提供类似操作系统的功能［7］。在其可视化功能下RVIZ下可以发布路径点，能够实时的可视化路径，进而实现机器人的仿真实验。对于水面环境建模后利用本文算法得到的路径规划如图5所示，红色线条为本文算法规划的路径。

图5 实际栅格地图路径规划

在机器人的控制中，坐标系统是非常重要的，在ROS使用TF软件库进行坐标转换［8］。在不同节点中不断发布和接受TF转换信息，则可以得到父坐标系和子坐标系的转换关系，从而模拟实际机器人的移动，转换信息还包含一些调试需要的发送频率、运行时间等信息。利用ROS的工具，利用CLion集成软件编写程序，分别发布拉链等路径规划信息，来测试算法的效果。实际测试中用了多个基础单元，以减小误差，以拉链路径为例，如图6所示，其中两个正交坐标系分别对应为地图坐标map和无人测试船坐标base_footprint。

5 结语

本文基于无人测试船平台将水面环境建模为栅格地图，提出了一种高效率的测试船全覆盖路径规划策略，根据路径规划的路径点进行了转角的动态圆优化，最后搭建实验环境加以验证算法效果。首先在栅格地图上进行覆盖式路径规划，目标点实际位置关系计算出动态半径，然后确定角速度方向，最后确定速度大小发布主题，运动控制节点订阅主题数据，即实现无人测试船的路径规划。

下阶段的研究中将尝试用其他的路径优化方法［9］，同时会尝试使用贝塞尔曲线［10］或者椭圆来代替转角圆弧优化，以期得到更好的效果。