改进RRT 算法用于电磁驱动机器鱼路径规划

朱红秀，郑 权，杜 闯，徐 亮

（中国矿业大学（北京）机电与信息工程学院，北京 100083）

0 引言

随着各国对海洋的探测和研究活动日渐频繁，高性能的水下机器人研发工作迫在眉睫。机器鱼作为典型的水下机器人，具有作业范围广、机动性强等特点，可以高效地完成海洋军事侦查、海底勘测、海洋生物诱导、海洋救援等多种任务［1-2］。机器鱼的作业环境往往是复杂的、随机变化的，因此，合理的路径规划对于机器鱼作业十分重要［3-6］。

在路径规划方面，许多专家和学者进行了相关研究。李连鹏等应用遗传算法提升了机器鱼路径规划的速度，但是受限于复杂环境，路径规划精确性难以保证［7］。王建等提出基于改进AFSA 的机器鱼群路径规划算法，但是该算法研究尚处于理论阶段，灵敏度等参数难以满足机器鱼的控制要求［8］。胡静波等基于改进的强化学习算法实现了机器鱼的路径规划，但是该算法需要大量的样本数据作为支撑［9］。洪强等通过改进A*算法实现已知环境的机器鱼最优路径规划，但是算法搜索范围大，效率较低［10］。除此以外，应用较多的算法还有模糊逻辑法［11］、图搜索法［12］和道路图规划法［13］等。目前应用于机器鱼路径规划的算法在处理路径规划问题时具有一定的优越性，但是当机器鱼处于复杂工作环境时，这些算法的计算量会随之增加，这严重限制了机器鱼的作业能力［14］。在快速扩展随机树（RRT）算法［15］的基础上对其改进，并将其应用在一种以电磁驱动器作为动力源的机器鱼的路径规划上，通过仿真和实验验证了该算法应用在电磁驱动机器鱼路径规划上的合理性。

1 电磁驱动机器鱼

1.1 电磁驱动原理

图1 机器鱼总体装配

图2 尾部装配图

电磁驱动机器鱼的总体装配如图1 所示，动力源为尾部的电磁驱动器，尾部装配图如图2 所示。电磁驱动器结构简单，无需传动机构，可以直接与机器鱼本体相连。电磁驱动器工作时始终处于通电状态，通过输入双极性正弦交流电的控制信号，驱动器带动尾柄从一侧摆动到另一侧位置［16］。

图3 电磁驱动器结构示意图

图3 所示为电磁驱动器的结构示意图，机器鱼初始工作时，线圈处于左侧的极限位置，当正弦控制信号处于正半周期时，通入正向电流，线圈左端磁化为N 极右端磁化为S 极，使其与左侧永磁体相互排斥，与右侧永磁体相互吸引，在二者合力作用下线圈绕旋转轴顺时针摆动，到达右侧极限位置后，正弦控制信号处于负半周期，通入反向电流，线圈左端磁化为S 极右端磁化为N 极，使线圈与右侧永磁体相互排斥，与左侧永磁体相互吸引，在二者的合力作用下，永磁体推动线圈绕旋转轴进行逆时针摆动。永磁体和线圈之间的气隙是电磁驱动器的主要工作气隙，该处磁场主要由永磁铁磁场和线圈磁场共同组成，在气隙磁场力的作用下线圈带动鱼尾绕旋转轴摆动，从而控制机器鱼实现往复摆动。

在路径规划需要机器鱼进行转弯时，电磁驱动器通电线圈的摆动方式为由单侧极限位置向中间位置往复摆动，从而实现尾鳍的单侧摆动。单侧摆动信号设置为正负周期具有不同的占空比如图4 所示，左图为电磁驱动机器鱼左转时通电线圈信号，右图为电磁驱动机器鱼左转时通电线圈信号。

图4 电磁驱动控制信号

1.2 电磁驱动机器鱼控制系统

为了实现机器鱼的自主游动，首先，通过控制模块读取传感器模块的融合姿态角信息；然后，控制模块根据已知信息进行机器鱼路径规划并输出对应的左转、右转，或直线游动的机器鱼数字控制信号。最后驱动模块将数字信号转换为模拟信号并进行功率放大，输出可以负载电磁驱动器线圈的控制信号，控制机器鱼不断地调整位姿使其逼近期望值直至到达目标点。控制系统的各个模块配合流程如下页图5 所示。

2 RRT 算法

2.1 RRT 算法描述

图5 控制系统

快速扩展随机树（RRT）算法是一种基于采样的路径规划算法，通过随机采样的方式探索空间生成规划路径，可以显著降低计算量。同时，算法无需进行任何预处理步骤，只需对其产生的随机节点采用树型结构存储，因此，算法被广泛应用于机器人路径规划。

算法分为随机树的构建和路径查询两个过程［17-18］。

2.1.1 随机树构建

空间为C∈RN，其中包括机器鱼的位置坐标与姿态角度。假设状态空间C 中存在一组复杂的约束条件，则机器鱼的任何规划路径都必须保持处于此约束内。

定义Ctree∈RN为所有满足约束条件的状态空间集合。为了确保机器鱼在水下环境的安全性，RRT算法只能在Ctree∈RN中进行扩展构建。机器鱼是否会发生碰撞可以通过检测选取的状态点q 是否满足状态空间C 的全局约束来检测，即通过验证q∈Ctree是否成立来决定状态点q 是否可以抵达，以及是否可以成为快速扩展随机树的新节点。

设Tk是一个拥有k 个节点的随机树，且Tk∈Ctree，即快速扩展随机树Tk生成树枝所产生的规划路径为无碰撞路径。设q 是Tk的节点且q∈Tk，定义qinit是状态起始点，qgoal是状态目标点，Cgoal∈Ctree为目标点区域。路径规划任务可以看作是在状态空间Ctree中搜索一条连续的路径，该路径从qinit出发，到qgoal或Cgoal截止。

RRT 算法的构建如图6 所示［19］:

图6 RRT 算法构建过程

RRT 算法构建流程图如图7 所示。

2.1.2 路经查询

图7 RRT 算法构建流程图

路径查询的具体步骤:完成随机树的构建后，停止添加新的状态点，从qgoal开始，逆向遍历随机树Tk，依次遍历其父节点，反复迭代直到找到qinit为止，这样就可以找到一条从qinit到qgoal的规划路径，并且该路径能够满足机器鱼的全局约束条件。

因随机树在随机采样状态点的过程中，其添加的新状态点符合机器鱼的约束条件，因此，最终得到的规划路径也是合理的、符合约束条件的。

2.2 RRT 算法分析

由于RRT 算法采用随机采样方式对状态空间进行探索，可以通过采样法将随机树的生长引向未知空白的区域，避免了对空间环境的建模，因此，RRT 算法更加适用于高维度机器鱼在复杂环境中的路径规划。但是因为RRT 算法的随机性，同样也会导致一些问题［20］:

1）不确定性:随机采样具有随机性，算法最终扩展形成的随机树也是不同的，在相同环境下同一起始点和目标点的多次重复规划得到的规划路径也可能会各不相同。

2）偏差性:利用RRT 得到的规划路径不一定是最优的。

3）缓慢性:随机树的扩展是通过随机选取状态点的方式，扩展方向是随机的，因此，随机树对目标状态点的收敛速度可能会十分缓慢。

2.3 RRT 算法的改进

2.3.1 收敛加速

随机树扩展节点是固定步长的，所以扩展方向决定了算法的效率，针对RRT 算法随机树扩展中的特点，可以采用目标导向的方法减小随机采样的随机性，使随机树更加倾向于目标区域的方向生长。

目标导向的具体操作方法是:人为地引导随机点的生成，在产生随机点qrand时，先以均匀概率原则获得一个随机概率值，如果这个概率值小于预先设置的阈值，则选取目标状态点作为循环中的qrand，即qrand=qgoal，如果概率值大于预先设置的阈值，则随机选取状态点qrand。

qrand在随机树构建的过程中相当于一个生长的方向，以一定的概率将状态目标点qgoal作为qrand，就可以让随机树更加快速地朝向目标区域扩展，达到快速收敛的效果。

2.3.2 路径优化

2）路径点优化

通过之前的分析可知，快速扩展随机树算法得到的规划路径往往不是最优结果，为此，提出基于起始点导向的约束检测原理以缩短随机树的规划路径。

路径点优化原理如图8 所示:

图8 路径点的重新选取

其具体实现流程图如图9 所示:

图9 路径点优化流程图

2.4 仿真实验与分析

为验证改进RRT 算法在路径规划中的快速性、高效性和有效性，在仿真环境下设置机器鱼为质点，对RRT 算法和改进RRT 算法进行验证。

2.4.1 RRT 仿真实验

仿真环境设置为500×500 的平面区域，步长20。起始状态点设置为［450 450］，用绿色圆标识，目标状态点设置为［1 1］，用红色圆标识。在地图中，障碍物的分布是简单而有序的，为了验证算法的可靠性，进行平面障碍的路径规划仿真，仿真实验次数为100 次。

下页图10 所示为RRT 算法在地图中的部分仿真结果，红色路线为算法的规划路径，仿真结果验证了RRT 算法作为全局搜索算法的优点，由于是基于随机采样的路径搜索算法，该算法即使在复杂地形中也不会出现陷入局部极小点的情况，但是其路径的随机性、不平滑性以及非最优性也十分明显。

2.4.2 改进RRT 仿真实验

在相同地图环境和参数下进行改进RRT 算法的仿真实验进行对比，在改进算法中设置机器鱼最大航偏角为60°，实验次数为100 次。

图10 RRT 算法路径规划仿真

图11 改进RRT 算法路径规划仿真

图11 所示为改进RRT 算法的部分仿真结果，红色路线为算法的初步规划路径，黑色路线为算法的优化路径。由仿真结果可得，在改进的RRT 算法中随机树扩展效率极高，尤其通过路径点的优化，规划路径长度明显下降且规划路径趋于稳定。

2.5 仿真结果分析

在总共进行的200 次仿真实验中，无论是基础RRT 算法还是改进RRT 算法，均能够完成路径规划的任务，图12 和图13 分别是200 次仿真实验的路径长度和规划时间对比数据，横坐标为实验的次数。

图12 路径长度对比数据

图13 规划时间对比数据

图14 机器鱼路径规划实验

通过路径长度和规划时间对比数据可以发现，改进RRT 相比RRT 算法，路径长度和规划时间显著下降，数据的波动幅度下降，算法稳定性极大提升。

下页表1 为具体的对比数据。

表1 仿真实验数据对比

3 路径规划实验

为了进一步验证改进算法取得有效性，在搭建好的电磁驱动机器鱼游动实验平台上进行路径规划实验。实验场地为2 m×2 m 的平面水域环境，障碍物布局如图14 所示。

图14 所示为机器鱼在起始点、路径点和目标点位置的运动状态，其中绿色圆点表示机器鱼在起始点，蓝色圆点表示路径点，红色圆点表示目标点。通过多次实验验证，改进的RRT 算法可以快速、高效地规划出避开障碍物，同时满足机器鱼运动性能约束的较优路径。

4 结论

通过对电磁驱动机器鱼驱动器工作原理的分析，得到了电磁驱动器在机器鱼不同工况下的驱动信号类型，并以此为根据设计机器鱼路径规划方案。在机器鱼的路径规划设计中，分析了经典RRT算法路径规划的不足，引入目标导向、路径点优化和路径约束的方法对RRT 算法进行改进。最后，分别对两种算法的路径规划进行了仿真验证，实验结果证明，所提出的改进RRT 算法比经典RRT 算法在规划时间和规划路径长度上均有明显减小。但是算法仍存在许多问题及需要进一步改进的地方，在下一步研究中将尝试结合局部优化算法改进RRT算法，提升其动态规划能力。