CN109542117B - 基于改进rrt的水下航行器滚动规划算法 - Google Patents

Abstract

本发明涉及路径规划技术领域，具体的说是一种能够有效提高搜索效率的基于改进RRT的水下航行器滚动规划算法，相比较于基本RRT算法，改进RRT算法利用节点可见度，通过2层选择得到节点扩展方向以及节点扩展步长，仿真实验表明本发明算法相比较基于基本RRT的滚动规划，搜索效率更高，将改进RRT算法与基本RRT算法在多个复杂环境中进行搜索，实验数据表明改进RRT算法相比较RRT算法在复杂环境中搜索时间更少，对应的节点数目更少。

Description

基于改进RRT的水下航行器滚动规划算法

技术领域：

本发明涉及路径规划技术领域，具体的说是一种能够有效提高搜索效率的基于改进RRT的水下航行器滚动规划算法。

背景技术：

在路径规划任务中，水下环境信息分成完全已知和部分已知等情况，对于未知的环境信息航行器需要在运动过程中逐步获得。水下环境信息完全已知的路径规划称为全局路径规划，水下环境信息部分已知的路径规划称为局部路径规划。常见的全局路径规划算法有可视图法、进化算法、启发式搜索算法等，常见局部路径规划算法有人工势场法、基于模糊逻辑的路径规划算法、基于案例学习的路径规划算法；全局路径规划算法并不适用于环境信息未知的路径规划问题。

水下航行器的运动模型如图1所示，考虑到水下航行器自身大小与传感器感知范围比值大小，不能将水下航行器当作一个简单质点，在路径规划过程中应该考虑水下航行器的非完整性约束。非完整性约束是指含有系统广义坐标导数且不可积的约束，该约束使得系统的独立控制个数少于系统的位形自由度。常见的非完整性约束系统包括车辆、轮式机器人、水下航行器、欠驱动机器人和某些空间机器人。

基本RRT算法即快速搜索随机树(Rapidly-exploring Random Tree)是近几年广泛应用的路径规划算法，该算法已经被成功应用到机器人路径规划问题，相比较其他的路径规划算法，快速搜索随机树算法具有以下优点：(a)传统的路径规划方法如人工势场法、遗传算法、模拟退火算法等并不适合解决非完整性约束规划，而带有非完整性约束的机器人路径规划问题在实际问题中很常见。(b)RRT算法避免了对搜索空间的建模，适合解决高维空间中的机器人路径规划问题。该算法本身也存在着以下缺陷：(a)快速搜索随机树算法采用均匀一致的步长进行随机扩展，无任何偏向搜索，在开阔空间具有无意义的计算，在狭窄区域扩展效率低下。(b)由于采用随机扩展，导致最终生成的路径不平滑，常常不是最优路径。

图3为基本RRT算法的伪代码。其中起始点为Q_start，T为树形结构，C_space为规划空间。首先进入迭代过程，通过randomConfig(C_space)确定扩展方向q_rand，然后利用findNear(q_rand,T)从T中找出距离q_rand最近的节点q_near，然后利用getNew(q_near,q_rand,Δq)得到从q_near到q_rand方向、将q_near扩展Δq大小得到的q_new。通过isCollision(q_new,q_near,Δq)判断q_near与q_new的连线以及q_new是否位于障碍环境C_obs中，如果位于障碍环境C_obs中，则中断程序进入下一次循环。如果没有，则将q_new添加到T中。当q_new是目标点q_gaol或者q_new位于q_near与q_new的连线中或q_new与目标点q_gaol的距离小于某距离，说明算法已经找到了q_gaol，则结束算法。如果当前迭代次数i>Iteration_max的时候跳出循环，并报错。

图4为RRT算法扩展示意图，基本RRT算法扩展方向随机，扩展步长长度固定。图5分别为基本RRT算法在Δq＝10(采用冯楠论文中的Δq等于搜索空间对角线长度1.5％)，迭代步数分别为20、80、200、500、800、1200下的效果图，可以看出随着迭代步数的增加，基本RRT算法逐渐向搜索空间C_space的4个顶点扩展，如果迭代步数足够多，最终会充满搜索空间。图6步数为1200步时，随机搜索树节点构成的Voronoi图，从中可以看出基本RRT算法无偏向性搜索，向四周扩展的方向概率一致。

基本RRT算法采用固定长度步长和随机扩展方向，并没有考虑障碍物分布远近选择节点扩展方向和扩展步长长度。在障碍物分布开阔或者无障碍物区域，采用小步长扩展则扩展速度缓慢，采用大步长扩展可以快速通过此区域；在复杂障碍物分布区域或狭窄通道中，采用大步长扩展沿随机方向容易生成无效节点，导致扩展效率低下。

发明内容：

本发明针对上述缺点和不足，提出了一种根据节点可见度大小来选择扩展方向以及扩展步长大小的改进RRT算法，在进行节点扩展时考虑水下航行器的非完整性约束，进而有效提高搜索效率的基于改进RRT的水下航行器滚动规划算法。

本发明可以通过以下措施达到：

一种基于改进RRT的水下航行器滚动路径规划算法，其特征在于包括以下步骤：

步骤一：滚动路径规划，滚动路径规划是利用重复进行的局部路径规划来代替全局路径规划，步骤如下：

步骤1：充分利用航行器传感器得到的信息建立局部环境模型/规划窗口；

步骤2：如果目标位置不在当前窗口内，则根据目标点位置以及路径优化准则确定子目标，同时将该子目标作为下一个周期的起始点；如果目标位置在当前窗口内，则采用当前位置作为子目标，在规划窗口内，航行器按照算法规划好的局部路径前进，窗口也相应的往前滚动；

步骤3：在新的滚动窗口内，根据传感器获得的最新环境信息以及障碍物信息，对已知环境信息进行更新；

步骤二：节点扩展过中考虑水下航行器非完整性约束，运动空间中水下航行器的状态为(x,y,θ)，由于非完整性约束，水下航行器在任何时候得满足：

其中θ为水下航行器当前速度v的方向与坐标系原点的夹角，x、y为水下航行器的空间坐标，L为舵机到水下航行器重心之间的距离，水下航行器依靠机体尾部舵机更改方向，R为水下航行器的曲率半径，为舵机摆角，水下航行器是非完整性约束系统，从上式得知舵机摆角/>必须满足/>即路径的曲率半径小于水下航行器的最小转弯半径通过在拓展新节点时加入转角约束，可使规划出来的路径满足水下航行器的非完整性约束，具体算法是在引用附图3所示的基本RRT算法第8行flag＝＝0之后再添加一个判断条件/>如果新节点不满足非完整性约束，则进入下一次循环重新扩展新节点；

步骤三：确定滚动窗口子目标，采用启发式规则确定窗口子目标，在窗口内经过N次采样之后，在快速搜索随机树上寻找启发式值最小的节点，将该节点作为窗口子目标，同时建立新的快速搜索随机树，并将该节点作为新树的起点Q_start，从该节点出发，在局部窗口中进行N次采样，重复该过程直到局部窗口的范围内包含目标节点q_gaol，其中启发式值为节点与目标点的直线距离长度；

步骤四：算法运行构建滚动窗口，并且判断当前窗口内是否包含目标点q_gaol，如果不包含则以当前窗口为规划空间，利用RRT算法得到新节点q_new，接下来判断q_new是否满足避碰条件以及水下航行器的非完整性约束，如果不满足则进入下一次循环，如果满足则将新节点添加到随机树T上，重复以上过程直到当前迭代次数超过设置好的最大迭代次数，此时采用启发式规则确定局部子目标，并将局部子目标作为新的出发点Q_start，重复以上过程直到窗口范围内包含q_gaol，当窗口范围内包含q_gaol，则以利用RRT算法进行路径规划，直到到达目标点q_gaol。

本发明还包括在不同的区域采用不同的扩展方向，即在开阔区域采用随机扩展或设定一定的几率向q_gaol扩展，在狭窄区域应用障碍物和随机搜索树的信息进行扩展，提高搜索效率，并提出了节点可见度的概念，利用节点可见度判断当前节点所处空间扩展难易程度，根据节点度大小选择节点扩展方向以及节点扩展步长；其中选择节点扩展方向以及节点扩展步长的过程分2步，首先根据节点可见度找到对应的扩展方向集合或者扩展步长集合，然后根据随机选择得到扩展方向或者是扩展步长。

本发明所述节点可见度表示将当前节点q扩展生成有效新节点的难易程度，如果节点qNode相对于另外一个节点q是可见的，这说明可以通过RRT算法的节点扩展方案，在某种分辨率下产生连续的相邻序列ε，其中ε＝{coor₀,coor₁,……coor_n}，其中coor₀＝qNode，coor_n＝q，并且任意coor_i∈ε是有效的节点，即不与障碍物发生碰撞，虽然节点可见度Visibility的确切大小在高维空间中是很难计算的，但是它可以简单的基于节点与其他节点连接的成功率来近似表示，其中δ为距离度量函数，此处为2节点之间直线距离，Δq为扩展步长，在得到本次有效扩展的长度extendDist之后，利用extendDist除以Δq得到本次扩展百分比extendRation，然后更新q_near、q_new的节点可见度大小；

RRT算法节点扩展的可能出现的3种情况，(a)情况表明q_near位于障碍物边缘，此时无新的节点q_new生成；(b)情况表示从q_near向q_rand扩展的过程中，遇到了障碍物，此时从q_near向q_rand扩展的距离extendDist<Δq，此时q_new为靠近障碍物边界的节点；(c)情况表示从q_nearr向q_rand扩展的过程中，没有遇到障碍物，此时q_new与q_near之间的距离extendDist＝Δq；

以下公式用于计算节点可见度Visibility值大小

对于以上3种情况，q_near的节点可见度更新公式都是一样的，利用q_near未更新之前的节点可见度nearVis乘以该节点之前作为q_near扩展的次数k，再加上此次节点扩展率extendRatio(其计算公式为当前节点扩展距离extendDist除以Δq)，然后再除以次数k+1；对于(a)情况中的q_near，在随后的迭代过程中，由于extendRatio等于0，q_near值会迅速下降；对于(c)情况中的q_near，由于extendRatio等于1，所以其Visibility值会保持不变是1或者是逐渐提高；对于(b)情况，q_near如果离障碍物边界近，则扩展的距离extendDist就小，则此次的extendRatio就小；如果q_near如果离障碍物边界远，则扩展的距离extendDist就大，则此次的extendRatio就大；

对于q_new，如果是(a)情况，由于q_near紧挨着障碍物边界，因此无扩展距离，此时无新节点q_new的生成；如果是(c)情况，q_new的Visibility值大小等于q_near的未更新之前的节点可见度nearVis跟此次节点扩展率extendRatio的平均值；如果是(b)情况，q_new的Visibility值大小等于q_near的未更新之前的节点可见度nearVis跟此次节点扩展率extendRatio的加权之后大小，如果extendRatio值较大，则q_new的Visibility的值也较大，具体权值人为设置；

考虑到起点q_start的位置可能位于搜索空间任意位置，可能离障碍物很近，也可能位于开阔区域，不能设置一常数作为其初始值，定义q_start的Visibility大小等于以q_start为圆心，Δq为半径的圆与在障碍物上面的圆弧所对应角度大小α与圆周角360的比值，用比值大小表示起始点扩展生成有效新节点的容易程度；

通过上述定义，位于规划空间C_space中的狭窄空间内的节点将具有较低的可见性，因为扩展完全失败(情况a)和部分扩展(情况b)时常发生，而位于规划空间C_space中开阔空间内的节点，由于节点常常扩展成功(情况c，extendDist等于Δq)，因此常常具有较高的可见度，通过上面的函数以及公式，可以近似表示节点生成新节点的容易程度，即节点的可见度大小。

本发明总结以下4种节点扩展方向，根据节点可见度的大小来选择对应的节点扩展方向集合中的节点扩展方向：

(1)节点扩展方向1(Extend direction1,E1)：任意扩展方向，节点扩展方向为基本RRT算法的节点扩展方向—任意扩展方向，其中q_rand为空间内任意一点；

(2)节点扩展方向2(Extend direction2,E2)：q_near→q_goal节点扩展方向为从q_near到q_goal；物边界的节点；

(3)节点扩展方向3(Extend direction3,E3)：q_parent→q_near，节点扩展方向为q_parent到q_near；

(4)节点扩展方向4(Extend direction4,E4)：根据障碍物信息确定扩展方向，从q_near向周围环境发射长度为d、方向任意的射线，此处设置射线个数为2，如果射线与障碍物相交，保存节点；如果射线没有与障碍物相交，保存从q_near扩展d得到的新节点，q_near的扩展方向为两端点连线的方向。

表1-1为节点扩展方向集合对应关系示例。其中Low Vis、Mid Vis、High Vis为不同区间的节点可见度范围。在本发明的改进RRT算法中，确定随机搜索树上的q_near节点之后，根据q_near节点的Visibility的大小找到对应的扩展方向集合，然后从中选择某一扩展方向。

相比较于基本RRT算法，改进RRT算法利用节点可见度，通过2层选择得到节点扩展方向以及节点扩展步长，而即便RRT算法采用固定长度的扩展步长以及随机扩展方向。与基本RRT算法不同的一点是，在改进RRT算法中，如果发生部分扩展，q_new为近障碍物边界的节点，但是在基本RRT算法中，发生部分扩展之后无q_new生成。

本发明在工作时，起始点q_start在开阔区域，开阔区域是高可见度区域，因此在高可见度对应的大步长区间中选择扩步长，高可见度区间对应的大步长集合有利于探索未知空间。当随机搜索树扩展并接近狭窄通道入口时，由于扩展失败和局部扩展的发生，节点可见度下降，扩展步长从中可见度区间对应的中等步长集合中选择。当随机树搜索树扩展进入狭窄通道之后，复杂环境影响导致扩展失败次数增加，节点的可见度下降，因此选择低可见度区间对应的短步长集合中的步长进行扩展。当随机搜索树在狭窄区域中时，不同于基本RRT算法中的随机扩展导致扩展效率低下，随机搜索树采用节点扩展方案确定扩展方向，并采用短步长逐步扩展，逐渐扩展接近狭窄通道。当搜索树扩展到狭窄通道出口时，随着成功扩展次数的增加，节点的可见度不断上升，逐渐在中等长度步长集合和大步长集合中选择步长。当随机搜索树“脱离”狭窄通道后，随着扩展成功次数的增加，节点可见度不断增加，最终利用大步长快速到达目标点。

附图说明：

附图1是水下航行器简化运动模型。

附图2是滚动路径规划示意图。

附图3基本RRT算法伪代码。

附图4基本RRT扩展示意图。

附图5基本RRT算法在不同迭代次数扩展效果图。

附图6扩展节点Voronoi图。

附图7是的水下航行器滚动路径规划框架流程图。

附图8是节点可见度的更新函数伪代码。

附图9为RRT算法节点扩展的可能出现的3种情况。

附图10是起点可见度示意图。

附图11是节点扩展方向示意图1。

附图12是节点扩展方向示意图2。

附图13是节点扩展方向示意图3。

附图14是节点扩方向展示意图4。

附图15是改进RRT算法的算法伪代码。

附图16为采用改进RRT算法在具有狭窄通道的环境中逐步扩展示意图。

附图17是本发明实施例中复杂搜索环境示意图。

附图18是本发明实施例中包含狭窄通道的搜索环境示意图。

附图19是本发明实施例中“迷宫”型搜索环境示意图。

附图20是本发明实施例中“T型”搜索环境示意图。

附图21是随机搜索树数目均一化对比图。

附图22是本发明实施例中运行时间均一化对比图。

具体实施方式：

下面结合附图和实施例，对本发明作进一步的说明。

考虑到水下航行器自身大小与传感器感知范围比值大小，不能将水下航行器当作一个简单质点，在路径规划过程中应该考虑水下航行器的非完整性约束。

水下航行器的运动模型如图1所示，运动空间中水下航行器的状态为(x,y,θ)，其中θ为水下航行器当前速度v的方向与坐标系原点的夹角，x、y为水下航行器的空间坐标。水下航行器依靠机体尾部舵机更改方向，R为水下航行器的曲率半径，为舵机摆角。

非完整性约束是指含有系统广义坐标导数且不可积的约束，该约束使得系统的独立控制个数少于系统的位形自由度。常见的非完整性约束系统包括车辆、轮式机器人、水下航行器、欠驱动机器人和某些空间机器人。由于非完整性约束，水下航行器在任何时候得满足：

式中，L为舵机到水下航行器重心之间的距离，R为曲率半径。水下航行器是非完整性约束系统，从上式得知舵机摆角必须满足/>即路径的曲率半径小于水下航行器的最小转弯半径。

本发明提出考虑水下航行器非完整性约束的水下航行器滚动规划框架，该算法利用水下航行器自身携带的传感器感知周围环境来建立滚动窗口，在滚动窗口内利用改进的RRT算法进行路径规划。下面介绍滚动规划的概念、基本RRT算法和如何在路径规划时考虑非完整性约束和如何确定滚动窗口子目标和框架规划流程。

(1)滚动路径规划

滚动路径规划的主要思想是利用重复进行的局部路径规划来代替全局路径规划。图2为滚动路径规划的示意图。滚动路径规划的基本步骤如下：

步骤1：充分利用航行器传感器得到的信息建立局部环境模型/规划窗口。

步骤2：如果目标位置不在当前窗口内，则根据目标点位置以及路径优化准则确定子目标，同时将该子目标作为下一个周期的起始点；如果目标位置在当前窗口内，则采用当前位置作为子目标。在规划窗口内，航行器按照算法规划好的局部路径前进，窗口也相应的往前滚动。

步骤3：在新的滚动窗口内，根据传感器获得的最新环境信息以及障碍物信息，对已知环境信息进行更新。

(3)节点扩展过中考虑水下航行器非完整性约束

通过在拓展新节点时加入转角约束，可使规划出来的路径满足水下航行器的非完整性约束。具体算法是在基本RRT算法第8行flag＝＝0之后再添加一个判断条件如果新节点不满足非完整性约束，则进入下一次循环重新扩展新节点。

(4)确定滚动窗口子目标

采用启发式规则确定窗口子目标，在窗口内经过N次采样之后，在快速搜索随机树上寻找启发式值最小的节点，将该节点作为窗口子目标，同时建立新的快速搜索随机树，并将该节点作为新树的起点Q_start，从该节点出发，在局部窗口中进行N次采样，重复该过程直到局部窗口的范围内包含目标节点q_gaol。其中启发式值为节点与目标点的直线距离长度。

(5)框架流程

图7是的水下航行器滚动路径规划框架流程图，该框架采用RRT算法在局部窗口内进行路径规划，并在路径规划过程中考虑水下航行器的非完整性约束。算法运行构建滚动窗口，并且判断当前窗口内是否包含目标点q_gaol，如果不包含则以当前窗口为规划空间，利用RRT算法得到新节点q_new，接下来判断q_new是否满足避碰条件以及水下航行器的非完整性约束，如果不满足则进入下一次循环，如果满足则将新节点添加到随机树T上。重复以上过程直到当前迭代次数超过设置好的最大迭代次数，此时采用启发式规则确定局部子目标，并将局部子目标作为新的出发点Q_start，重复以上过程直到窗口范围内包含q_gaol。当窗口范围内包含q_gaol，则以利用RRT算法进行路径规划，直到到达目标点q_gaol。

基于节点可见度选择节点扩展方案的改进RRT算法：

基本RRT算法采用固定长度步长和随机扩展方向，并没有考虑障碍物分布远近选择节点扩展方向和扩展步长长度。在障碍物分布开阔或者无障碍物区域，采用小步长扩展则扩展速度缓慢，采用大步长扩展可以快速通过此区域；在复杂障碍物分布区域或狭窄通道中，采用大步长扩展沿随机方向容易生成无效节点，导致扩展效率低下。

针对以上问题，本发明首先提出“节点可见度”的概念，用来描述节点扩展成功难易程度，然后在节点可见度的基础上提出根据节点可见度选择节点扩展方案的改进RRT算法。节点扩展方案中包括2部分，第一部分是节点扩展方向的选择，另外一部分是节点扩展步长大小的选择。

当RRT算法扩展到狭窄通道或者是复杂区域时，扩展新节点q_new与q_near之间的连线常常跟障碍物发生碰撞，扩展效率缓慢。狭窄通道常常出现在高维空间，如复杂、狭窄的水下空间。PRM(probabilistic roadmap planning)算法在狭窄通道或复杂区域也存在路标构建缓慢的问题，针对这一问题Brock提出节点可见度的概念，利用节点可见度来过滤抽样以改进路线图。在关于PRM算法的改进研究中，节点可见度被近似估计为节点连接阶段成功连接次数与总尝试次数的简单比值。本发明借鉴其他文献节点可见度的定义，提出了节点可见度Visibility的定义以及计算方法。

节点可见度表示将当前节点q扩展生成有效新节点的难易程度。如果节点qNode相对于另外一个节点q是可见的，这说明可以通过RRT算法的节点扩展方案，在某种分辨率下产生连续的相邻序列ε，其中ε＝{coor₀,coor₁,……coor_n}，其中coor₀＝qNode，coor_n＝q，并且任意coor_i∈ε是有效的节点，即不与障碍物发生碰撞。虽然节点可见度Visibility的确切大小在高维空间中是很难计算的，但是它可以简单的基于节点与其他节点连接的成功率来近似表示。图8是节点可见度的更新函数伪代码。其中δ为距离度量函数，此处为2节点之间直线距离。其中Δq为扩展步长，在得到本次有效扩展的长度extendDist之后，利用extendDist除以Δq得到本次扩展百分比extendRation，然后更新q_near、q_new的节点可见度大小。如图9所示为RRT算法节点扩展的可能出现的3种情况。(a)情况表明q_near位于障碍物边缘，此时无新的节点q_new生成；(b)情况表示从q_near向q_rand扩展的过程中，遇到了障碍物，此时从q_near向q_rand扩展的距离extendDist<Δq，此时q_new为靠近障碍物边界的节点；(c)情况表示从q_nearr向q_rand扩展的过程中，没有遇到障碍物，此时q_new与q_near之间的距离extendDist＝Δq。

以下公式用于计算节点可见度Visibility值大小

对于以上3种情况，q_near的节点可见度更新公式都是一样的，利用q_near未更新之前的节点可见度nearVis乘以该节点之前作为q_near扩展的次数k，再加上此次节点扩展率extendRatio(其计算公式为当前节点扩展距离extendDist除以Δq)，然后再除以次数k+1。对于(a)情况中的q_near，在随后的迭代过程中，由于extendRatio等于0，q_near值会迅速下降。对于(c)情况中的q_near，由于extendRatio等于1，所以其Visibility值会保持不变是1或者是逐渐提高。对于(b)情况，q_near如果离障碍物边界近，则扩展的距离extendDist就小，则此次的extendRatio就小。如果q_near如果离障碍物边界远，则扩展的距离extendDist就大，则此次的extendRatio就大。

对于q_new，如果是(a)情况，由于q_near紧挨着障碍物边界，因此无扩展距离，此时无新节点q_new的生成。如果是(c)情况，q_new的Visibility值大小等于q_near的未更新之前的节点可见度nearVis跟此次节点扩展率extendRatio的平均值。如果是(b)情况，q_new的Visibility值大小等于q_near的未更新之前的节点可见度nearVis跟此次节点扩展率extendRatio的加权之后大小，如果extendRatio值较大，则q_new的Visibility的值也较大，具体权值可以人为设置。值得注意的一点是，不论是扩展失败还是部分扩展、扩展成功，q_new的Visibility都继承了一部分q_near的Visibility。考虑到起点q_start的位置可能位于搜索空间任意位置，可能离障碍物很近，也可能位于开阔区域，不能设置一常数作为其初始值。定义q_start的Visibility大小等于以q_start为圆心，Δq为半径的圆与在障碍物上面的圆弧所对应角度大小α与圆周角360的比值，用比值大小表示起始点扩展生成有效新节点的容易程度。图10为起始点示意图。

通过上述定义，位于规划空间C_space中的狭窄空间内的节点将具有较低的可见性，因为扩展完全失败(情况a)和部分扩展(情况b)时常发生。而位于规划空间C_space中开阔空间内的节点，由于节点常常扩展成功(情况c，extendDist等于Δq)，因此常常具有较高的可见度。通过上面的函数以及公式，可以近似表示节点生成新节点的容易程度，即节点的可见度大小。

改进的节点扩展方案：

节点扩展方案的组成分成2部分，一部分是节点扩展方向，另外一部分是节点扩展步长长度。主要思想是根据节点可见度大小选择不同的节点扩展方案，即选不同的择扩展方向以及不同的扩展步长长度。

节点扩展方向的确定：

基本RRT算法的扩展方向随机，并没有考虑环境情况以及扩展成功的容易程度，随机扩展方向并不适用于所有环境下的节点扩展，在狭窄区域采用随机扩展容易生成无效节点导致节点，导致扩展效率低下。

针对这一问题，本发明总结以往文献中提出的节点扩展方向，总结以下4种节点扩展方向，在改进RRT算法中，根据节点可见度的大小来选择对应的节点扩展方向集合中的节点扩展方向。

(1)节点扩展方向1(Extend direction1,E1)：任意扩展方向

节点扩展方向为基本RRT算法的节点扩展方向—任意扩展方向，其中q_rand为空间内任意一点。图11中(a)为从q_near向q_rand扩展Δq得到q_new，(b)从q_near向q_rand扩展Δq失败，q_new为近障碍物边界的节点。

(2)节点扩展方向2(Extend direction2,E2)：q_near→q_goal

节点扩展方向为从q_near到q_goal。图12中(a)为从q_near向q_goal扩展Δq得到q_new，从q_new向q_goal扩展Δq失败，q_new为近障碍物边界的节点。

(3)节点扩展方向3(Extend direction3,E3)：q_parent→q_near

节点扩展方向为q_parent到q_near。图13中(a)为从q_near沿着q_parent到q_near扩展Δq得到q_new，(b)为q_near沿着q_parent到q_near扩展Δq失败，q_new为近障碍物边界的节点。(4)节点扩展方向4(Extend direction4,E4)：根据障碍物信息确定扩展方向从q_near向周围环境发射长度为d、方向任意的射线，本发明设置射线个数为2。如果射线与障碍物相交，保存节点；如果射线没有与障碍物相交，保存从q_near扩展d得到的新节点。q_near的扩展方向为两端点连线的方向。图14为节点扩展方向4的示意图。

表1-1为节点扩展方向集合对应关系示例。其中Low Vis、Mid Vis、High Vis为不同区间的节点可见度范围。在改进RRT算法中，确定随机搜索树上的q_near节点之后，根据q_near节点的Visibility的大小找到对应的扩展方向集合，然后从中选择某一扩展方向。

表1-1节点扩展方向集合对应关系示例

节点扩展步长的确定是根据不同的节点可见度大小选择不同的长度的节点扩展步长，提高生成有效节点的概率。节点扩展步长为Δq。表1-2为节点扩展步长集合对应关系示例。在改进RRT算法中，根据q_near节点的Visibility的大小找到对应的扩展步长集合，然后从中选择某一长度的扩展步长。

表1-2节点扩展步长集合对应关系示例

改进RRT算法流程是根据节点可见度大小来近似判断选择新节点生成有效新节点的难易程度，然后根据节点可见度大小选择。

基本RRT算法采用均匀一致的步长在搜索空间随机扩展，在复杂环境中算法扩展效率低下。大致思路是在开阔区域采用大步长扩展，因为大步长相比较于小步长搜索速度快，采用大步长扩展可以迅速通过开阔区域，采用小步长扩展搜索速度慢；在狭窄区域采用小步长扩展，减少新生成节点是无效节点的概率，提高搜索效率，加快穿过狭窄、拥挤区域的速度。

同时考虑到基本RRT算法扩展方向随机，在开阔区域，随机扩展便于探索未知空间，但是在狭窄区域随机扩展，很有可能导致新生成的节点是无效节点，在迷宫陷阱，随机扩展有利于“逃脱”迷宫。本发明提出在不同的区域采用不同的扩展方向，比如在开阔区域采用随机扩展或设定一定的几率向q_gaol扩展，在狭窄区域应用障碍物和随机搜索树的信息进行扩展，提高搜索效率。同时提出了节点可见度的概念，利用节点可见度判断当前节点所处空间扩展难易程度，提出根据节点度大小选择节点扩展方向以及节点扩展步长的改进RRT算法。

值得注意的一点是选择节点扩展方向以及节点扩展步长的过程分2步，首先根据节点可见度找到对应的扩展方向集合或者扩展步长集合，然后根据随机选择得到扩展方向或者是扩展步长。表1-3为节点可见度范围区间对应的节点扩展方向集合和节点步长结合对应表示例。从表1-3中可以看出不同的可见度范围区间对应着不同的节点扩展方向集合以及不同的节点步长集合。

表1-3可见度范围对应集合：

图15为改进RRT算法的算法步骤，改进RRT算法与基本RRT算法总体类似。

相比较于基本RRT算法，改进RRT算法利用节点可见度，通过2层选择得到节点扩展方向以及节点扩展步长，而即便RRT算法采用固定长度的扩展步长以及随机扩展方向。与基本RRT算法不同的一点是，在改进RRT算法中，如果发生部分扩展，q_new为近障碍物边界的节点，但是在基本RRT算法中，发生部分扩展之后无q_new生成。其中蓝色节点表示高可见度节点，绿色节点表示为中可见度节点，红色节点为低可见度节点。

起始点q_start在开阔区域，开阔区域是高可见度区域，因此在高可见度对应的大步长区间中选择扩步长，高可见度区间对应的大步长集合有利于探索未知空间。当随机搜索树扩展并接近狭窄通道入口时，由于扩展失败和局部扩展的发生，节点可见度下降，扩展步长从中可见度区间对应的中等步长集合中选择。当随机树搜索树扩展进入狭窄通道之后，复杂环境影响导致扩展失败次数增加，节点的可见度下降，因此选择低可见度区间对应的短步长集合中的步长进行扩展。当随机搜索树在狭窄区域中时，不同于基本RRT算法中的随机扩展导致扩展效率低下，随机搜索树采用节点扩展方案确定扩展方向，并采用短步长逐步扩展，逐渐扩展接近狭窄通道。当搜索树扩展到狭窄通道出口时，随着成功扩展次数的增加，节点的可见度不断上升，逐渐在中等长度步长集合和大步长集合中选择步长。当随机搜索树“脱离”狭窄通道后，随着扩展成功次数的增加，节点可见度不断增加，最终利用大步长快速到达目标点。

实施例：

为证明提出的基于改进RRT的水下航行器滚动路径规划算法的有效性，将所提算法与基于基本RRT的水下航行器滚动路径规划算法在相同的复杂区域(图17)进行路径规划，起始点固定(63,402)为蓝色节点,目标点随机设置10个为红色节点。算法参数设置一样，其中Δq大小为规划区域对角线长度的5％，局部窗口中规划次数为50，局部窗口大小为规划区域对角线长度的10％，改进RRT算法采用表1-3中的可见度与扩展方向集合、扩展步长集合对应关系。表1-4为两算法对应的数据对比，其中每组仿真运行10次取平均值，表中数据按照终点坐标距离起点距离从小到大排列。其中t表示计算时间，nodeNum表示所有随机搜索树上的节点总和。

表1-4路径数据对比

序号3仿真实验的终点目标为(182，341)为图中的绿色节点，该点为狭窄通道的入口。随着搜索复杂度的增加，在序号3仿真中改进算法对应的时间t是基于基本RRT的水下航行器滚动路径规划算法对应时间t的85.2％，在序号10中是13％。从中可以推断出，随着搜索复杂度的增加，本发明算法相比较基于基本RRT的水下航行器滚动路径规划算法优势明显。

为了进一步比较改进RRT算法与基本RRT算法在不同的复杂环境中搜索效率，将改进RRT算法与基本RRT算法分别在以下搜索环境中进行路径规划。图18中为包含“狭窄通道”的搜索环境，搜索算法需要通过“狭窄通道”到达目标节点。图19为“迷宫”型搜索环境，搜索算法需要穿过迷宫型障碍物才能到达目标节点。图20为“T型搜索环境，搜索算法需要通过T型陷阱才能到达目标节点。其中蓝色节点为出发位置，红色节点为目标位置。将改进RRT算法与基本RRT算法在以上搜索环境中分别运行10次，得到平均运行时间，以及平均随机搜索树节点数目。

图21为将算法数据均一化得到的数据对比图。(a)中X轴为不同的搜索环境，Y轴为改进RRT算法与基本RRT算法在该环境下算法运行时间对比。(a)中X轴为不同的搜索环境，Y轴为改进RRT算法与基本RRT算法在该环境下算法对应的随机搜索树节点数目对比。从图21中可以看出，改进RRT算法在复杂环境中的运行时间、随机搜索树节点数目都小于基本RRT算法对应的运行时间、随机搜索树节点数目，通过均一化数据对比可以看出改进RRT算法相比较基本RRT算法的算法搜索效率更高。

本发明提出基于改进RRT的水下航行器滚动规划算法，以滚动方式在环境信息部分已知的情况下进行路径规划。提出根据节点可见度大小来选择扩展方向以及扩展步长大小的改进RRT算法，在进行节点扩展时考虑水下航行器的非完整性约束，在开阔区域采用大步长进行扩展，快速通过开阔区域，在狭窄区域采用小步长进行扩展减少生成无效节点的概率，提高搜索效率。仿真实验表明本发明算法相比较基于基本RRT的滚动规划，搜索效率更高。将改进RRT算法与基本RRT算法在多个复杂环境中进行搜索，实验数据表明改进RRT算法相比较RRT算法在复杂环境中搜索时间更少，对应的节点数目更少。

Claims (4)

1.一种基于改进RRT的水下航行器滚动路径规划算法，其特征在于包括以下步骤：

步骤一：滚动路径规划，滚动路径规划是利用重复进行的局部路径规划来代替全局路径规划，步骤如下：

步骤1：充分利用航行器传感器得到的信息建立局部环境模型/规划窗口；

步骤2：如果目标位置不在当前窗口内，则根据目标点位置以及路径优化准则确定子目标，同时将该子目标作为下一个周期的起始点；如果目标位置在当前窗口内，则采用当前位置作为子目标，在规划窗口内，航行器按照算法规划好的局部路径前进，窗口也相应的往前滚动；

步骤3：在新的滚动窗口内，根据传感器获得的最新环境信息以及障碍物信息，对已知环境信息进行更新；

步骤二：节点扩展过中考虑水下航行器非完整性约束，运动空间中水下航行器的状态为(x,y,θ)，由于非完整性约束，水下航行器在任何时候得满足：

其中θ为水下航行器当前速度v的方向与坐标系原点的夹角，x、y为水下航行器的空间坐标，L为舵机到水下航行器重心之间的距离，水下航行器依靠机体尾部舵机更改方向，R为水下航行器的曲率半径，为舵机摆角，水下航行器是非完整性约束系统，从上式得知舵机摆角/>必须满足/>即路径的曲率半径小于水下航行器的最小转弯半径通过在拓展新节点时加入转角约束，可使规划出来的路径满足水下航行器的非完整性约束，具体算法是在引用附图3所示的基本RRT算法第8行flag＝＝0之后再添加一个判断条件/>如果新节点不满足非完整性约束，则进入下一次循环重新扩展新节点；

步骤三：确定滚动窗口子目标，采用启发式规则确定窗口子目标，在窗口内经过N次采样之后，在快速搜索随机树上寻找启发式值最小的节点，将该节点作为窗口子目标，同时建立新的快速搜索随机树，并将该节点作为新树的起点Q_start，从该节点出发，在局部窗口中进行N次采样，重复该过程直到局部窗口的范围内包含目标节点q_gaol，其中启发式值为节点与目标点的直线距离长度；

步骤四：算法运行构建滚动窗口，并且判断当前窗口内是否包含目标点q_gaol，如果不包含则以当前窗口为规划空间，利用RRT算法得到新节点q_new，接下来判断q_new是否满足避碰条件以及水下航行器的非完整性约束，如果不满足则进入下一次循环，如果满足则将新节点添加到随机树T上，重复以上过程直到当前迭代次数超过设置好的最大迭代次数，此时采用启发式规则确定局部子目标，并将局部子目标作为新的出发点Q_start，重复以上过程直到窗口范围内包含q_gaol，当窗口范围内包含q_gaol，则以利用RRT算法进行路径规划，直到到达目标点q_gaol。

2.根据权利要求1所述的一种基于改进RRT的水下航行器滚动路径规划算法，其特征在于还包括在不同的区域采用不同的扩展方向，即在开阔区域采用随机扩展或设定一定的几率向q_gaol扩展，在狭窄区域应用障碍物和随机搜索树的信息进行扩展，提高搜索效率，并提出了节点可见度的概念，利用节点可见度判断当前节点所处空间扩展难易程度，根据节点度大小选择节点扩展方向以及节点扩展步长；其中选择节点扩展方向以及节点扩展步长的过程分2步，首先根据节点可见度找到对应的扩展方向集合或者扩展步长集合，然后根据随机选择得到扩展方向或者是扩展步长。

3.根据权利要求2所述的一种基于改进RRT的水下航行器滚动路径规划算法，其特征在于所述节点可见度表示将当前节点q扩展生成有效新节点的难易程度，如果节点qNode相对于另外一个节点q是可见的，通过RRT算法的节点扩展方案，在某种分辨率下产生连续的相邻序列ε，其中ε＝{coor₀,coor₁……coor_n}，其中coor₀＝qNode，coor_n＝q，并且任意coor_i∈ε是有效的节点，即不与障碍物发生碰撞，虽然节点可见度Visibility的确切大小在高维空间中是很难计算的，但是它可以简单的基于节点与其他节点连接的成功率来近似表示，其中δ为距离度量函数，此处为2节点之间直线距离，Δq为扩展步长，在得到本次有效扩展的长度extendDist之后，利用extendDist除以Δq得到本次扩展百分比extendRation，然后更新q_near、q_new的节点可见度大小；

RRT算法节点扩展的可能出现的3种情况，(a)情况表明q_near位于障碍物边缘，此时无新的节点q_new生成；(b)情况表示从q_near向q_rand扩展的过程中，遇到了障碍物，此时从q_near向q_rand扩展的距离extendDist<Δq，此时q_new为靠近障碍物边界的节点；(c)情况表示从q_nearr向q_rand扩展的过程中，没有遇到障碍物，此时q_new与q_near之间的距离extendDist＝Δq；

以下公式用于计算节点可见度Visibility值大小

对于以上3种情况，q_near的节点可见度更新公式都是一样的，利用q_near未更新之前的节点可见度nearVis乘以该节点之前作为q_near扩展的次数k，再加上此次节点扩展率extendRatio，计算公式为当前节点扩展距离extendDist除以Δq，然后再除以次数k+1；对于(a)情况中的q_near，在随后的迭代过程中，由于extendRatio等于0，q_near值会迅速下降；对于(c)情况中的q_near，由于extendRatio等于1，所以其Visibility值会保持不变是1或者是逐渐提高；对于(b)情况，q_near如果离障碍物边界近，则扩展的距离extendDist就小，则此次的extendRatio就小；如果q_near如果离障碍物边界远，则扩展的距离extendDist就大，则此次的extendRatio就大；

对于q_new，如果是(a)情况，由于q_near紧挨着障碍物边界，因此无扩展距离，此时无新节点q_new的生成；如果是(c)情况，q_new的Visibility值大小等于q_near的未更新之前的节点可见度nearVis跟此次节点扩展率extendRatio的平均值；如果是(b)情况，q_new的Visibility值大小等于q_near的未更新之前的节点可见度nearVis跟此次节点扩展率extendRatio的加权之后大小，如果extendRatio值较大，则q_new的Visibility的值也较大，具体权值人为设置；

考虑到起点q_start的位置可能位于搜索空间任意位置，可能离障碍物很近，也可能位于开阔区域，不能设置一常数作为其初始值，定义q_start的Visibility大小等于以q_start为圆心，Δq为半径的圆与在障碍物上面的圆弧所对应角度大小α与圆周角360的比值，用比值大小表示起始点扩展生成有效新节点的容易程度；通过上述定义，位于规划空间C_space中的狭窄空间内的节点将具有较低的可见性，因为扩展完全失败和部分扩展时常发生，而位于规划空间C_space中开阔空间内的节点，由于节点常常扩展成功extendDist等于Δq，因此常常具有较高的可见度，通过上面的函数以及公式，可以近似表示节点生成新节点的容易程度，即节点的可见度大小。

4.根据权利要求3所述的一种基于改进RRT的水下航行器滚动路径规划算法，其特征在于包括以下4种节点扩展方向，根据节点可见度的大小来选择对应的节点扩展方向集合中的节点扩展方向：

(1)节点扩展方向1，Extend direction1,E1：任意扩展方向，节点扩展方向为基本RRT算法的节点扩展方向—任意扩展方向，其中q_rand为空间内任意一点；

(2)节点扩展方向2，Extend direction2,E2：q_near→q_goal节点扩展方向为从q_near到q_goal；物边界的节点；

(3)节点扩展方向3，Extend direction3,E3：q_parent→q_near，节点扩展方向为q_parent到q_near；

(4)节点扩展方向4，Extend direction4,E4：根据障碍物信息确定扩展方向，从q_near向周围环境发射长度为d、方向任意的射线，此处设置射线个数为2，如果射线与障碍物相交，保存节点；如果射线没有与障碍物相交，保存从q_near扩展d得到的新节点，q_near的扩展方向为两端点连线的方向。