基于RRT算法的混合动力水下滑翔机的路径规划

Path Planning of Hybrid Underwater Glider Based on RRT Algorithm

摘要: 混合动力水下滑翔机相对于传统的滑翔机，有运动速度快、机动性好，运动模式多等优点。为了更好地利用混合动力水下滑翔机的优势，开展滑翔机的路径规划研究，通过引入RRT算法向终点采样概率Rsample并调整合适的生长步长α，可以更快地生成滑翔机的规划路径。并简化了滑翔机在水平面侧向的运动学方程，增加了滑翔机的转向半径约束。最后使用B样条曲线对生成路径进行光滑化处理，改善了生成路径的质量。

Abstract: Compared with the traditional glider, the hybrid underwater glider has the advantages of fast speed, good maneuverability and multiple motion modes. In order to make better use of the advantages of hybrid underwater glider, this paper studies the path planning of glider. By introducing the RRT algorithm to sample the probability Rsample from the end point and adjusting the appropriate growth step α, the planned path of the glider can be generated more quickly. The lateral kinematics equation of glider in horizontal plane is simplified and the steering radius constraint is added. Finally, B-spline curve is used to smooth the generated path, and the quality of the generated path is improved.

1. 引言

海洋拥有丰富的矿产资源和生物资源，当前越来越多国家开始重视海洋开发，以此从中获取丰富的能源和资源。为满足未来海底矿产资源工程开发的需要，越来越多可以适应海洋环境的开发设备得到国家重视。混合动力水下滑翔机具有较高的智能水平及灵活的作业能力，可以用于海底搜索及海底矿区的探测 [1]。滑翔机在执行任务之前，需要提前了解矿区的实际地形数据，并将地形数据用于路径规划，将路径规划结果作为滑翔机实际勘探路径。其目的是要将规划结果作为实际行动的具体指导。

与传统水下航行器不同的是，水下滑翔机由于其独特的外形，成了一种新型的水下航行器。它们具有低成本、自主性和长距离、长时间部署的能力。传统的水下滑翔机通过内部执行器使内部油箱体积变化控制浮力，通过移动内部质量并结合机翼和尾翼来控制姿态，实现在海洋中滑行。但传统的水下滑翔机的工作模式单一，滑翔飞行是由浮力驱动的，不使用推进器或螺旋桨。而混合驱动水下滑翔机是一种新型水下航行器。与传统水下滑翔机不同的是，混合驱动水下滑翔机可以行驶更远的距离，同时能耗更低，而且由于其隐蔽性良好，可以被考虑用于军事研究。为了更好地利用混合动力水下滑翔机，需要对其动力学理解和建模，以进一步利用水下滑翔机的独特优势 [2]。

由于滑翔机的体积限制了其能量容量，规划好混合动力滑翔机的路径是节省耗能，提高续航，完成长期连续工作任务的一种途径 [3]。滑翔机的路径规划一般分为障碍物全部已知的全局路径规划和障碍物未知的局部路径规划 [4]。全局路径规划就是从宏观的角度已知工作区域上所有障碍物的分布信息，同时按照我们选择的算法，计算出一条合理的路径，这条路径不仅要尽可能地短，而且要避开行进过程中的所有障碍物。其中，任务决策和规划起着重要作用，全局路径规划和局部路径规划是相辅相成的，滑翔机首先要按指定的全局路径规划去行驶，在遇到随机障碍物时，再用局部路径规划去规避，如果在行驶过程中障碍物的位置发生了移动和变化，则利用滑翔机的声呐系统或视觉系统获取障碍物的运动，并避开障碍物得到可行的路径，即局部路径规划 [5]。经过长期的技术发展，滑翔机的路径规划已经相对成熟 [6]。目前常用的路径规划有三大类：第一种需要在栅格地图进行路径规划，第二种是一种智能的仿生算法，第三类是基于采样的路径规划算法 [7] [8] [9]。采样的搜索算法包括快速扩展随机树算法和概率图算法。快速扩展随机树算法是Steven M. Lavalle [10] [11] [12] 提出的一种路径规划算法。它的优点是不需要对规划空间进行建模。它是一种随机抽样算法。并且它考虑了滑翔机的客观约束，从而使其得到了较为广泛的应用，但与此同时，传统的采样算法也存在很多的问题。

综上所述，路径规划是水下滑翔机技术的重要组成部分。本文采用快速扩展随机树算法应用于滑翔机的路径规划。本文分析了快速扩展随机树算法的实现过程，主要包括随机点和路径的生成方式。并且在算法生成路径的基础上，采用B样条曲线对其进行光滑化处理，提升了路径规划的质量，使最后生成的路径更加符合滑翔机的实际滑翔条件。随后，通过结合滑翔机实际的滑翔角、增大最小转弯半径，将路径构造方法拓展到三维空间。最后通过Matlab仿真试验，验证了算法的可行性。

快速扩展随机树(Rapidly-Exploring Random Tree，RRT算法)由S.M于1998年提出。RRT算法建立的思想来自于最优控制理论，非完整规划，随机路径规划；这种算法在解决高维空间还有复杂约束的时候，表现的非常优秀，它主要是用控制理论的思想，它首先模拟出一棵可以生长的树，这棵树会不断的长出新的节点，可行的节点会继续长出新的节点，不断地向外界探索，最终形成一条可行进的路径。

快速扩展随机树算法具有的特性，吸引了诸多研究学者对其进行深入的研究，相继地提出了一些新的改进算法，提高了原算法的性能和应用领域。如采用双向搜索技术，提高搜索效率；Kuffner于LaValle提出RRT-connect算法，大大提高了节点的扩展效率；RPP，RRT-GoalBias，RRT-GoalZoom等算法的提出，使得搜索树的生长方向偏向于目标点生长；路径规划中，还有一些关于距离函数、变分法技术等方面的研究改进。目前热门的研究方向是将快速扩展随机树算法应用到动态环境中，为此，一些学者引入了导向因子，用于引导节点扩展方向，增强搜索树生长的目的性。提出了在大规模的分布式内存体系结构下并行RRT。在复杂的工作环境空间、存在微分约束的环境及高维状态空间中，快速扩展随机树算法已经获得了广泛的应用。

与此同时，国内外很多学者积极参与再融合算法上的研究上，把RRT与其他的路径规划算法融合，发挥各自的长处与不足，各种实验都表明，融合算法往往能起到很好的避障效果。

2. 混合动力水下滑翔机动力学模型

2.1. 基本理论

FEM/SPH耦合方法是将FEM和SPH耦合起来 [13]，发挥2种方法各自的优势，针对大变形问题。

在建立、求解水下滑翔机运动模型并研究其运动性能，需要建立合适的坐标系，本文选用的坐标系有惯性坐标系、机体坐标系、速度坐标系，如图1所示。惯性坐标系O₀x₀y₀z₀是作为建立运动学方程时作为惯性参考系，也是水下滑翔机运动时位姿参数的测量基准。机体坐标Oxyz固连于水下滑翔机机体。速度坐标系Ox_vy_vz_v是将水下滑翔机航行速度同水下滑翔机机体相联系的坐标系。

水下滑翔机相对于惯性系的空间运动参数主要包含其在惯性系中的空间位置和空间姿态。水下滑翔机的空间位置可用机体坐标系原点O在惯性坐标系中的位置坐标确定。水下滑翔机的空间姿态用欧拉角表示，分别为偏航角 $\psi$，俯仰角 $\theta$ 和横滚角 $\phi$。水下滑翔机航行速度矢量 $V$ 相对滑翔机机体坐标的方位可用攻角 $\alpha$ 和侧滑角 $\beta$ 这两个角度确定。水下滑翔机航行速度矢量 $V$ 相对于地面坐标系的方位可用倾角Ψ，偏角Θ和倾斜角Φ表示。

$J_0$ 为滑翔机相对于浮心的转动惯量矩阵，滑翔机自身的动量及动量矩为

$Q_1=m{v}_c=m\left(v_0+\omega \times r_c\right)$ (1)

$K_1=J_0\omega +r_c\times m{v}_0$ (2)

$\omega$ 是滑翔机的角速度， $v_0$ 是滑翔机的线速度， $r_c$ 为转动半径，混合驱动水下滑翔器没入水中，其外壳与水完全接触。因此混合滑翔器与水之间存在作用力和力矩。这些作用多种多样，大致可以包括浮力

Figure 1. Outline and coordinate system representation of underwater glider elementary prototype

图1. 水下滑翔机原理样机外形及坐标系表示

粘性水动力和惯性水动力等。设流体的势函数为 $\phi \left(x,y,z,t\right)$， $V_b$ 为滑翔机所在的流体体积，并结合滑翔机表面 $\Omega$ 的边界条件 $n_0=-n$，得到流场的动量 $Q_2$ 和动量矩 $K_2$ 为

$Q_2=\rho {\displaystyle {\int }_{V_b}grad\phi \text{d}{V}_b}=\rho {\displaystyle {\int }_{V_b}\phi n_0\text{d}\Omega }=-\rho {\displaystyle {\int }_{\Omega }\phi n\text{d}\Omega }$ (3)

$K_2=\rho {\displaystyle {\int }_{V_b}r\times grad\phi \text{d}{V}_b}=-\rho {\displaystyle {\int }_{\Omega }\left(r\times n\right)\phi \text{d}\Omega }$ (4)

将滑翔机自身的动量及动量矩投影式与流体的动量及动量矩的投影式对应相加，即可得出滑翔机在理想流体中作任意运动时的表观动量及表观动量矩

$Q_b=Q_1+Q_2$ (5)

$K_b=K_1+K_2$ (6)

黏性位置力 $F_{\alpha \mu }$，黏性阻尼力 $F_{\omega \mu }$，滑翔机的浮力B，滑翔机的重力G，滑翔机的推力T在雷体坐标系中的各分量为：

$F_{\alpha \mu }=\left[\begin{array}{c}{X}_{\alpha \mu }\\ Y_{\alpha \mu }\\ Z_{\alpha \mu }\\ M_{\alpha \mu x}\\ M_{\alpha \mu y}\\ M_{\alpha \mu z}\end{array}\right]$， $F_{\omega \mu }=\left[\begin{array}{c}{X}_{\omega \mu }\\ Y_{\omega \mu }\\ Z_{\omega \mu }\\ M_{\omega \mu x}\\ M_{\omega \mu y}\\ M_{\omega \mu z}\end{array}\right]$，(7)

$B=\left[\begin{array}{c}{X}_B\\ Y_B\\ Z_B\end{array}\right]$， $G=\left[\begin{array}{c}{X}_G\\ Y_G\\ Z_G\end{array}\right]$， $X_T=T$ (8)

滑翔机的动力学方程组可根据动量和动量矩定理建立，在机体坐标系中可表示为

$\frac{\delta Q_b}{\delta t}+\omega \times Q_b=R$ (9)

$\frac{\delta K_b}{\delta t}+V\times Q_b+\omega \times K_b=M$ (10)

现将滑翔机的动量及动量矩，外力及外力矩代入式方程，并加以整理，得

$A_{m\lambda }\left[\begin{array}{c}{\stackrel{\dot{}}{v}}_{Ox}\\ {\stackrel{\dot{}}{v}}_{Oy}\\ {\stackrel{\dot{}}{v}}_{Oz}\\ {\stackrel{\dot{}}{\omega }}_x\\ {\stackrel{\dot{}}{\omega }}_y\\ {\stackrel{\dot{}}{\omega }}_z\end{array}\right]+\frac{\text{d}{A}_{m\lambda }}{\text{d}t}\left[\begin{array}{c}{v}_{Ox}\\ v_{Oy}\\ v_{Oz}\\ {\omega }_x\\ {\omega }_y\\ {\omega }_z\end{array}\right]+A_{vw}\left\{A_{m\lambda }\left[\begin{array}{c}{v}_{Ox}\\ v_{Oy}\\ v_{Oz}\\ {\omega }_x\\ {\omega }_y\\ {\omega }_z\end{array}\right]\right\}=A_{FM}$ (11)

式中： $A_{m\lambda }$ 为质量和附加质量矩阵， $A_{vw}$ 为速度和角速度矩阵， $A_{FM}$ 为合外力矩阵。

滑翔机的运动方程组仅仅是实际滑翔过程的抽象化建模。上式数学模型过于复杂，在计算和数据处理造成了困难。需要对提出问题的数学模型进行适当的简化。结合滑翔机的实际运动状态，通常可以将水下滑翔机的运动方程组简化为纵向运动和侧向运动。选择合适的坐标系将7个参数 $\beta ,\psi ,\phi ,{\omega }_x,{\omega }_y,z_0,\Psi$ 等于零。这7个参数为侧向运动参数，用于描述侧向运动。剩下的参数 $\alpha ,\theta ,{\omega }_z,\Theta ,x_0,y_0,v$ 称为纵向运动参数描述纵向运动。

本文将在三维空间的水平面完成路径规划，三维空间的水平面运动是侧向运动的一种十分重要且特殊的情况。这种情况下滑翔机运动的横滚角 $\phi$ 不大，所以在研究水平面运动时常 $\Theta =\phi =0$。攻角 $\alpha$ 和俯仰角 $\theta$ 很小，可认为 $\cos \alpha =\cos \theta =1$， $\sin \alpha =\sin \theta =0$。最后得到滑翔机在水平面的侧向运动方程为：

${\lambda }_{35}{\stackrel{\dot{}}{\omega }}_y-\left(m-Z^{{\omega }_y}\right)v{\omega }_y+\left(m+{\lambda }_{33}\right)v\stackrel{\dot{}}{\beta }+\left(Z^{\beta }+A_x\right)v^2\beta =-Z^{{\delta }_v}{v}^2{\delta }_v$

$\left(J_{xx}+{\lambda }_{55}\right){\stackrel{\dot{}}{\omega }}_y+M_y^{{\omega }_y}v{\omega }_y+{\lambda }_{35}v\stackrel{\dot{}}{\beta }-M_y^{\beta }{v}^2\beta =-M_y^{{\delta }_v}{v}^2{\delta }_v$

$\frac{\text{d}\stackrel{\dot{}}{\psi }}{\text{d}t}={\omega }_y$ (12)

$\frac{\text{d}{\stackrel{\dot{}}{x}}_0}{\text{d}t}=v\cos \Psi$ (13)

$\frac{\text{d}{\stackrel{\dot{}}{z}}_0}{\text{d}t}=v\sin \Psi$ (14)

$\Psi =\psi -\beta$ (15)

2.2. 快速扩展随机树算法

伊利诺伊大学LaValle和Kuffner提出的RRT算法 [12] (快速扩展随机树算法)是用于解决带有运动学约束的机器人或者车辆在路径规划时的问题。该算法在提出后得到了广泛的应用。在进行路径规划时，快速扩展随机树算法作为一种能够迅速找到可通行路径的算法，因此在机器人和无人驾驶领域得到了一致的认可 [14]。本文将使用该算法用于滑翔机的路径规划。

RRT算法在一开始前需要输入滑翔机工作空间的地图(M)、滑翔机啊的起点pinit和终点pgoal。算法的具体执行过程如下：

1.在自由空间中随机抽样一个节点prand1；

2.在所有节点中寻找距prand1最近节点pnear；由于只有一个初始起点pinit，所以pnear=pinit；

3.从pnear向prand1设置步长α作为树枝的长度；

4.产生一个新的节点pnew1，如图2所示；

5.继续在自由空间中随机抽样一个节点prand2；

6.在所有节点(pinit，pnew1)中寻找距离prand2最近的节点pnear，即pnew1；

7.从pnew1向prand2设置步长α作为树枝的长度；

8.产生一个新的节点pnew2；

9.继续在自由空间中随机抽样一个节点prand3；

10.从pnew2向prand3生长步长α会穿过障碍物；

11.舍去该随机节点prand3，重新抽样随机点，继续上述步骤。

算法停止的条件是pnew生长到终点，但pnew生长到终点的概率很小，需要设置停止条件，即：判断pnew到pgoal的距离d小于生长步长α。若距离d小于生长步长α，且pnew到pgoal之间没有障碍物，就可以直接将pnew连接pgoal。结束随机抽样，得到最终的路径。

Figure 2. Schematic diagram of RRT algorithm

图2. RRT算法示意图

本文利用Matlab对基本快速扩展随机树算法进行仿真，图3~6所示采用RRT算法在给定的空间分别迭代扩展10，50，150，330步的效果。RRT算法在迭代的过程中，在给定的空间内随机均匀采样，快速生成随机树。由于设定了向终点概率Rsample，随着迭代次数的增加，随机树逐渐向终点扩展。RRT算法搜索路径成功率较大和搜索的稳定性较好。

Figure 3. Number of iterations = 10

图3. 迭代次数 = 10

Figure 4. Number of iterations = 50

图4. 迭代次数 = 50

Figure 5. Number of iterations = 150

图5. 迭代次数 = 150

Figure 6. Number of iterations = 330

图6. 迭代次数 = 330

假如以一定概率Rsample选择终点作为采样点，可以加快算法找到最终的路径，选择不同的概率，算法耗费的时间不同。图7~10表示设置向终点不同采样概率Rsample生成的随机树，有图可知随机树的节点在没有充满整个采样空间的基础上也可以完成路径规划。并且概率Rsample设置的越大，随机树向周围扩展的分支越少，从而减少了随机采样的时间和数量，使得算法的效率更高。但是并不是将设置的越高越好，如果起始点到终点之间存在较多的障碍时，直接向终点采样会使得路径穿过障碍物。所以需要设置合理的Rsample。

Figure 7. Rsample = 1%

图7. Rsample = 1%

Figure 8. Rsample = 5%

图8. Rsample = 5%

Figure 9. Rsample = 10%

图9. Rsample = 10%

Figure 10. Rsample = 20%

图10. Rsample = 20%

图11~13为RRT算法向终点采样概率为Rsample = 1%，设置不同的生长步长α = 0.5，1，2。相同的Rsample情况下，即使有不同的生长步长，随机树向周围扩展的分支几乎相同。但是越大的采样步长可以减少了随机采样的时间和数量，同样使得算法的效率更高。

Figure 11. α = 0.5

图11. α = 0.5

Figure 12. α = 1

图12. α = 1

Figure 13. α = 2

图13. α = 2

2.3. 路径光滑处理

RRT算法生成的路径是各个节点直接连接而形成的折线，该折线并不是最优路径。并且混合动力水下滑翔机机动性能有限，滑翔机转向时有一定的转弯半径，并不能实现折线转弯。本文使用B样条曲线对RRT算法生成的路径进行光滑处理 [15]。

对于路径中选定的p + m + 1节点P_i，可以定义p + 1条m次方的曲线 $F_{k,m}\left(t\right)$，

$F_{k,m}\left(t\right)=\frac{1}{m!}{\displaystyle {\sum }_{j=0}^{m-k}{\left(-1\right)}^j\cdot C_{m+1}^j\cdot {\left(t+m-k-j\right)}^m}$ (16)

将p + 1条曲线连接起来形成B样条曲线，其表达式为：

$P_{i,m}\left(t\right)={\displaystyle {\sum }_{k=0}^m{P}_{i+m}\cdot F_{k,m}\left(t\right)}$ (17)

2.4. 仿真及结果分析

前文在二维空间中讨论了RRT算法的实现过程及特点，接下来RRT算法扩展到规划三维空间地形上的路径，三维空间的计算范围的长 × 宽 × 高 = 2000 × 2000 × 400。并已知地形在任何位置障碍物的高度。本文的障碍物的高度是预先设定的几个高度。然后又在随机一个高度作为滑翔机水平面侧向运动的运动平面，选择向终点采样概率Rsample = 1%，生长步长α = 50。得到滑翔机规划路径，如图14所示。

Figure 14. Trajectory generated by RRT algorithm in three-dimensional space

图14. RRT算法在三维空间生成的轨迹路径

图14所示的红色曲线是RRT算法生成的路径，但是该路径是各个节点直接连接而形成的折线。黑色折线表示的路径是对红色路径进行初步的光滑处理，并且黑色曲线没有穿过障碍物。结合B样条曲线的算法结合滑翔机水平面侧向运动的方程组对黑色折线进一步光滑处理，得到如图15所示的蓝色曲线为最终规划的滑翔机路径 [16]。图15中的红线表示滑翔机的速度矢量。进行光滑化处理后，缩短了路径的长度，使生成的路径更加符合滑翔机的行驶条件，提高了滑翔机对于优化后路径的可行性。

Figure 15. Final planned glider path

图15. 最终规划的滑翔机路径

3. 结束语

本文通过简化水下滑翔机的动力学模型，得到水下滑翔机在水平面侧向运动方程组。通过介绍基本RRT算法的实现过程，引入了向终点随机采样概率Rsample和调节合适的生长步长α，提高了RRT算法的计算效率。并将RRT算法扩展到三维空间中，选择向终点采样概率Rsample = 1%，生长步长α = 50生成了初步的规划路径。同时在理论的基础上进行仿真实验，验证了该算法在路径规划上有不错的效果，针对改进的RRT算法得出的路径不平滑的问题，在此基础上结合滑翔机的水平面侧向运动方程组和B样条曲线对滑翔机的路径进行光滑处理，最终得到一条滑翔机可执行的路径，这也使得滑翔机可以更加快速地到达目的地，同时，该路径方法可为水下矿区勘探提供一种参考。

本文的贡献有：1) 通过引入了向终点随机采样概率Rsample和调节合适的生长步长α，提高了RRT算法的计算效率。2) 对改进的RRT算法进行B样条曲线路径光滑处理，并在二维和三维环境进行了仿真。3) 将RRT算法应用于水下机模型上，这也给滑翔机的水下路径规划和水下矿区勘探提供了一种参考。