开放科学（资源服务）标志码（OSID）：

0 概述

随着人工智能领域的发展，移动机器人路径规划技术相对地也获得了较大的提升。目前，移动机器人的需求量正逐步扩大，如家庭中的扫地机器人、物流工厂中的送货机器人等。移动机器人作为一种重要的生产生活工具，自主路径导航是其核心，所以如何使移动机器人在较短的时间内规划出一条有效的路径成为该领域的一项热门研究。

常见的路径规划算法包括蚁群算法^[1]、粒子群优化算法^[2]、遗传算法^[3]、人工势场法^[4]和A*算法^[5]。近年来，基于深度学习和深度强化学习的方法来解决路径规划的问题备受研究人员的关注。文献[6]采用卷积神经网络和强化学习相结合的方式来解决路径规划问题。文献[7]提出一种改进的深度Q学习算法，减少过估计对机器人在选择动作时的影响，达到所选策略最优。文献[8-9]介绍了基于采样的路径规划算法，通过对状态空间的采样点进行碰撞检测，避免了对空间的建模，并且可以直接添加运动约束。其优势主要在于无需对搜索区域进行几何划分，在搜索空间的覆盖率高，搜索的范围广以及尽可能地探索未知区域。

1998年，LAVALLE等^[10]提出一种基于采样的路径规划工具——快速搜索随机树（Rapidly-exploring Random Tree，RRT）。由于其具有收敛速度快、可探索未知障碍物的空间等优点，被广泛应用于路径规划领域。文献[11-12]提出了相应的改进算法，这些改进算法都以初始点为根节点，并通过迭代探索未知区域来构建一组轨迹生成随机树。文献[13]指出由于随机采样的性质，RRT及其改进的相关算法并没有考虑路径成本，也不能保证解的最优性。

RRT-Connect算法是在RRT算法的基础上采用两棵树双向搜索的引导策略。同时，该算法在扩展方式上利用贪婪策略加快了搜索速度，减少了非障碍空间的无用搜索，节省了搜索时间。文献[14-15]提出由于RRT-Connect算法缺乏优化过程，其无法保证解的最优性。

为了解决解的最优性问题，文献[16]提出一种渐进最优的路径规划算法RRT*，其通过重新布线原则并为新节点选择代价最小的父节点来保证解的最优性。文献[17]介绍了RRT*是渐近最优的，当采样次数达到无穷大时可以找到最优解，但其探索过程会消耗大量的时间。文献[18]提出一种基于简化地图的区域采样RRT*算法，其通过简化地图并在地图中随机采样N个节点来引导生成初始路径。

文献[19]提出一个RRT*的修改版本，称为知情RRT*（Informed-RRT*，IRRT*）算法。在IRRT*算法中，通过RRT算法找到初始路径后，根据初始路径构建一个会逐渐收敛的超椭球体，其采样子集被限制在超椭球体中。文献[15]介绍了IRRT*算法虽然在完整性和最优性方面具有与RRT*算法相同的性能，但相比于RRT*算法在优化路径时对整个空间进行探索，IRRT*算法在超椭球体内进行采样可以提高探索效率。文献[20-21]提出一种改进的IRRT*算法，称为STIRRT*算法，其首先对栅格地图进行骨架提取，然后通过角点检测算法对有用的特征点进行提取，这样可以加快IRRT*寻找到初始路径，并对其进行优化。但其通过角点检测算法在不规则的地方所保留的节点通常有过多的冗余节点，这样会使初始路径代价变大。在起点和终点距离很远时，其算法所构成的椭圆仍会包含整个探索空间，无法有效地提高算法的效率。由于采样优化半径为单调递减函数，因此在距离较远的相邻特征点很难得到有效的优化。

针对STIRRT*算法中特征点的冗余造成初始路径代价较大和得到初始路径后其优化效果不明显的问题，本文提出一种应用拓扑结构的高效路径规划算法ATIRRT*。该算法通过引入拓扑节点代替STIRRT*算法中利用Harris角点检测算法得到的特征点进行采样，以减少寻找初始路径的代价，同时给出非单一父节点的连接方式提高算法的鲁棒性。通过节点扩充策略在相邻的两个拓扑节点间增加节点的数量，加快优化算法的收敛。最终根据栅格地图的大小和障碍物在地图中的位置和数量定义相关约束条件来约束拓扑节点之间的距离，并将初始路径分段并进行逐段优化。

1 STIRRT^*算法

STIRRT^*算法相较于RRT算法有以下4点改进：

1）对栅格地图进行骨架提取并通过Harris角点检测算法得到相应的特征点以引导随机树扩展，加快得到初始路径。

2）为新节点选择代价最小的父节点。如图 1所示，首先根据设定的起点$ \mathrm{n}\mathrm{o}\mathrm{d}{\mathrm{e}}_{\mathrm{s}\mathrm{t}\mathrm{a}\mathrm{r}\mathrm{t}}\left(\mathrm{n}\mathrm{o}\mathrm{d}{\mathrm{e}}_{\mathrm{s}}\right) $和终点$ \mathrm{n}\mathrm{o}\mathrm{d}{\mathrm{e}}_{\mathrm{g}\mathrm{o}\mathrm{a}\mathrm{l}}\left(\mathrm{n}\mathrm{o}\mathrm{d}{\mathrm{e}}_{\mathrm{g}}\right) $通过RRT算法在栅格地图中进行采样。然后寻找目前随机树中距离采样点$ \mathrm{n}\mathrm{o}\mathrm{d}{\mathrm{e}}_{\mathrm{r}\mathrm{a}\mathrm{n}\mathrm{d}}\left(\mathrm{n}\mathrm{o}\mathrm{d}{\mathrm{e}}_{\mathrm{r}}\right) $最近的节点$ \mathrm{n}\mathrm{o}\mathrm{d}{\mathrm{e}}_{\mathrm{n}\mathrm{e}\mathrm{a}\mathrm{r}}\left(\mathrm{n}\mathrm{o}\mathrm{d}{\mathrm{e}}_{\mathrm{n}}\right) $，通过RRT算法中的扩展方式得到了一个新节点$ \mathrm{n}\mathrm{o}\mathrm{d}{\mathrm{e}}_{\mathrm{n}\mathrm{e}\mathrm{w}}\left(\mathrm{n}\mathrm{o}\mathrm{d}{\mathrm{e}}_{\mathrm{n}\mathrm{e}}\right) $。最后以$ \mathrm{n}\mathrm{o}\mathrm{d}{\mathrm{e}}_{\mathrm{n}\mathrm{e}} $为圆心，一定半径画一个圆。逐个计算起点到圆内节点的距离与圆内节点到新节点$ \mathrm{n}\mathrm{o}\mathrm{d}{\mathrm{e}}_{\mathrm{n}\mathrm{e}} $的距离和，其中最小的距离和就是起点到新节点$ \mathrm{n}\mathrm{o}\mathrm{d}{\mathrm{e}}_{\mathrm{n}\mathrm{e}} $的路径代价，相应的节点为$ \mathrm{n}\mathrm{o}\mathrm{d}{\mathrm{e}}_{\mathrm{n}\mathrm{e}} $的父节点。

3）重布线原则，即为新节点寻找子节点的过程。在由新节点$ \mathrm{n}\mathrm{o}\mathrm{d}{\mathrm{e}}_{\mathrm{n}\mathrm{e}} $所构成的圆中，对圆中的任意一个节点，计算以新节点为圆中节点的父节点时，其距离代价是否会降低。如果降低，那么将该节点的父节点赋值为新节点，对应的代价更新为新的代价。从图 2可以看出，节点e原来的代价为23，如果以节点c为父节点，那么新的代价为20，小于原来的代价。因此，将节点e的父节点改为节点c，结果如图 2所示。

4）根据得到的初始路径，将采样空间限制在由起点、终点和初始路径所构成的椭圆中，并且随着路径长度的不断缩短，逐渐缩小椭圆区域。

2 拓扑结构的高效路径规划算法

拓扑结构的高效路径规划（STIRRT*）算法可以快速得到初始路径，加快进入到优化过程，同时在优化过程中把采样点限制在椭圆中以避免不必要的探索。但在起点和终点距离较远的情况下，其由初始路径所生成的椭圆往往会包含整个场景；在优化过程中算法的收敛速度较慢，在距离较远的相邻特征点很难得到有效的优化。基于上述原因，本文提出ATIRRT*算法，其与STIRRT*算法相同，主要分为两个部分：寻找初始路径和基于椭圆采样。但是本文算法做出以下3点改进：1）为了消除通过Harris算法生成的冗余特征点，本文根据栅格地图的大小提出了一种阈值的自适应选择方法，同时给出非单一父节点的连接方式，提高了算法的鲁棒性；2）为了加快优化算法的收敛，在相邻的两个拓扑节点之间通过节点扩充策略，增加节点的数量；3）ATIRRT^*算法在STIRRT^*算法的基础上加入相关约束条件，该条件可以自适应调整采样范围，减少无用的空间扩展从而减少迭代次数。

2.1 拓扑结构

文献[22-23]介绍了骨架提取算法。该算法可以将连通区域细化成一个像素的宽度，主要用于特征提取和目标拓扑表示。图 3所示为对仿真的室内场景通过骨架提取算法处理后的结果。

经过骨架提取算法处理后，其连通区域会被细化成一条细线，仿真室内场景的骨架主要由一些重要的点连接而成。这些点被称为特征点，本文采用角点检测算法检测这些特征点。目前，角点检测算法主要分为基于图像边缘的方法和基于图像灰度的方法两大类。前者严重依赖于图像边缘检测结果，后者算法简单，结果稳定可靠。文献[24]介绍了在基于图像灰度的典型角点检测算法中，Harris算法提取的角点较为理想。因此本文采用Harris角点检测算法来确定这些特征点。文献[25]介绍了Harris角点检测算法应用邻近像素点灰度差值概念，从而进行判断是否为角点、边缘、平滑区域。Harris角点检测过程的主要步骤如下：

1）对图像中的逐个像素点通过水平和竖直两种差分算子进行滤波求得$ {I}_{x}\mathrm{、}{I}_{y} $，从而得到矩阵$ \mathit{\boldsymbol{M}} $中的4个元素的值：

$ \mathit{\boldsymbol{M}}=\left[{I}_{x}^{2}, {I}_{x}\times {I}_{y};{I}_{x}\times {I}_{y}, {I}_{y}^{2}\right] $

(1)

2）对矩阵$ \mathit{\boldsymbol{M}} $中的4个元素进行高斯滤波，从而消除一些冗余的点和凸起，得到新的矩阵$ \mathit{\boldsymbol{M}} $。

3）利用矩阵$ \mathit{\boldsymbol{M}} $计算对应每个像素的角点响应函数$ \mathit{\boldsymbol{R}} $，这里令$ \mathit{\boldsymbol{R}} $为：

$ \mathit{\boldsymbol{R}}=\mathrm{d}\mathrm{e}\mathrm{t}\left(\mathit{\boldsymbol{M}}\right)-k{\left(\mathrm{t}\mathrm{r}\mathrm{a}\mathrm{c}\mathrm{e}\left(\mathit{\boldsymbol{M}}\right)\right)}^{2} $

(2)

其中$ ：\mathrm{d}\mathrm{e}\mathrm{t}\left(\mathit{\boldsymbol{M}}\right) $为矩阵$ \mathit{\boldsymbol{M}} $的行列式；$ \mathrm{t}\mathrm{r}\mathrm{a}\mathrm{c}\mathrm{e}\left(\mathit{\boldsymbol{M}}\right) $为矩阵$ \mathit{\boldsymbol{M}} $的迹。文献[26]给出了经验值$ k $的取值范围为$ 0.04\sim 0.06 $，在本文中取值为$ 0.04 $。

4）在矩阵R中，满足点$ R(i, j) $大于一定阈值，在本文中阈值取$ 0.01{R}_{\mathrm{m}\mathrm{a}\mathrm{x}}(i, j) $并且$ R(i, j) $是其邻域内的局部极大值，则该点被认为是角点。

通过上述方式在得到所有特征点（角点）之后，由于获得的角点在栅格地图分布并不均匀，在骨架不均匀的地方生成的特征点较为密集，为了消除冗余的特征点，ATIRRT*算法根据栅格地图的大小，采用式（3）对所获得的特征点进行自适应选取：

$ {T}_{\mathrm{T}\mathrm{h}\mathrm{r}\mathrm{e}\mathrm{s}\mathrm{h}\mathrm{o}\mathrm{l}\mathrm{d}}=\left(a+b\right)|\left(\left[b|\varepsilon \right]\times \sqrt[]{\varepsilon }\right) $

(3)

其中$ ：a $为仿真栅格地图的高；$ b $为仿真栅格地图的宽；$ \varepsilon $为地图大小的数量级。以当前特征点为圆心、$ \mathrm{T}\mathrm{h}\mathrm{r}\mathrm{e}\mathrm{s}\mathrm{h}\mathrm{o}\mathrm{l}\mathrm{d} $为半径，在其所组成的圆内剔除圆心以外的特征点。图 4为对特征点根据式（3）进行阈值处理后的效果。

其中，图 4（a）为直接利用Harris角点检测算法所得到的特征点建立的结构，图 4（b）为在对所得特征点进行阈值处理后建立的拓扑结构。从图 4（b）可以看出，通过自适应阈值式（3）所保留的拓扑节点$ \mathrm{n}\mathrm{o}\mathrm{d}{\mathrm{e}}_{\mathrm{t}\mathrm{o}\mathrm{p}\mathrm{o}\mathrm{l}\mathrm{o}\mathrm{g}\mathrm{y}}\left(\mathrm{n}\mathrm{o}\mathrm{d}{\mathrm{e}}_{\mathrm{t}}\right) $可以有效体现所有可行路径。

在ATIRRT^*算法中，对于初始路径的寻找是通过RRT算法与拓扑结构相结合的方式来进行路径搜索，相比STIRRT^*算法通过RRT算法与Harris角点检测算法所得到的特征点相结合的方式来进行初始路径的探索，ATIRRT^*算法探索的初始路径长度较短。首先将拓扑节点添加到本文的初始树中，其初始树按如下步骤生成：

步骤1  所有闭合环路生成为一颗初始树；

步骤2  在交叉支路上定义相关拓扑节点的父节点为$ \mathrm{p}\mathrm{a}\mathrm{r}\mathrm{e}\mathrm{n}{\mathrm{t}}_{1} $，$ \mathrm{p}\mathrm{a}\mathrm{r}\mathrm{e}\mathrm{n}{\mathrm{t}}_{2} $，…，$ \mathrm{p}\mathrm{a}\mathrm{r}\mathrm{e}\mathrm{n}{\mathrm{t}}_{n} $，以这样的方式生成一颗初始树；

步骤3  统计由拓扑节点所组成的所有可行路径，并对其进行比较，选取长度最短的作为初始路径；

步骤4  当节点和最近的拓扑节点的子节点连线穿过障碍物时，令起点为最近的拓扑节点的父节点，然后把起点插入到初始树中；

步骤5  当节点和最近拓扑节点的子节点的连线没有穿过障碍物时，令起点为最近拓扑节点的子节点的父节点，同样把起点插入到初始树中。

在RRT的相关算法中，随机树中的每个节点只有一个父节点，这样节点不能和拓扑结构相结合。本文提出的算法让拓扑节点可以同时具有多个父节点，这样可以有效地将拓扑结构以树的形式表示出来。ATIRRT^*算法寻找初始路径的主要思想如下：首先在随机树中找到距离起点和终点拓扑节点；然后通过所设定的父子关系将起点和终点分别替代最近的拓扑节点；经过步骤3和步骤4的判断，可以有效地将起点和终点加入到拓扑结构中。ATIRRT^*算法寻找初始路径的伪代码如下：

算法1  ATIRRT^*算法寻找初始路径的伪代码

Tree$ \leftarrow \mathrm{F}\mathrm{i}\mathrm{n}\mathrm{d}\ \mathrm{i}\mathrm{n}\mathrm{i}\mathrm{t}\mathrm{i}\mathrm{a}\mathrm{l}\ \mathrm{p}\mathrm{a}\mathrm{t}\mathrm{h}(\mathrm{n}\mathrm{o}\mathrm{d}{\mathrm{e}}_{\mathrm{s}}, \mathrm{n}\mathrm{o}\mathrm{d}{\mathrm{e}}_{\mathrm{g}}, \mathrm{n}\mathrm{o}\mathrm{d}{\mathrm{e}}_{\mathrm{t}}, \mathrm{K}, \mathrm{s}\mathrm{t}\mathrm{e}\mathrm{p}\_\mathrm{l}\mathrm{e}\mathrm{n}) $

1.Tree.init（$ \mathrm{n}\mathrm{o}\mathrm{d}{\mathrm{e}}_{\mathrm{s}} $）

2.TreeTopology（$ \mathrm{n}\mathrm{o}\mathrm{d}{\mathrm{e}}_{\mathrm{t}} $）

3.$ \mathrm{n}\mathrm{o}\mathrm{d}{\mathrm{e}}_{\mathrm{t}\mathrm{n}} $= Nearest（TreeTopology，$ \mathrm{n}\mathrm{o}\mathrm{d}{\mathrm{e}}_{\mathrm{s}} $）

4.if Collision（$ \mathrm{n}\mathrm{o}\mathrm{d}{\mathrm{e}}_{\mathrm{t}\mathrm{n}}, \mathrm{n}\mathrm{o}\mathrm{d}{\mathrm{e}}_{\mathrm{s}} $）then

5.  $ \mathrm{n}\mathrm{o}\mathrm{d}{\mathrm{e}}_{\mathrm{t}\mathrm{n}}.\mathrm{p}\mathrm{a}\mathrm{r}\mathrm{e}\mathrm{n}\mathrm{t} $=$ \mathrm{n}\mathrm{o}\mathrm{d}{\mathrm{e}}_{\mathrm{s}} $

6.  TreeTopology.AddNode（$ \mathrm{n}\mathrm{o}\mathrm{d}{\mathrm{e}}_{\mathrm{s}} $）

7. Tree= TreeTopology

8.else

9.  $ \mathrm{n}\mathrm{o}\mathrm{d}{\mathrm{e}}_{\mathrm{t}\mathrm{n}}.\mathrm{c}\mathrm{h}\mathrm{i}\mathrm{l}\mathrm{d}.\mathrm{p}\mathrm{a}\mathrm{r}\mathrm{e}\mathrm{n}\mathrm{t} $=$ \mathrm{n}\mathrm{o}\mathrm{d}{\mathrm{e}}_{\mathrm{s}} $

10. TreeTopology.AddNode（$ \mathrm{n}\mathrm{o}\mathrm{d}{\mathrm{e}}_{\mathrm{s}} $）

11.   Tree= TreeTopology

12.for k = 1 to K

13.  $ \mathrm{n}\mathrm{o}\mathrm{d}{\mathrm{e}}_{\mathrm{r}} $ = Generate_random_node（）

14.  $ \mathrm{n}\mathrm{o}\mathrm{d}\mathrm{e}{}_{\mathrm{n}} $= Nearest（Tree，$ \mathrm{n}\mathrm{o}\mathrm{d}{\mathrm{e}}_{\mathrm{r}} $）

15.  if Distance（$ \mathrm{n}\mathrm{o}\mathrm{d}\mathrm{e}{}_{\mathrm{n}}, \mathrm{n}\mathrm{o}\mathrm{d}{\mathrm{e}}_{\mathrm{g}} $） < Threshold then

16.   return Tree

17.  $ \mathrm{n}\mathrm{o}\mathrm{d}{\mathrm{e}}_{\mathrm{n}\mathrm{e}\mathrm{w}} $ = Extend（$ \mathrm{n}\mathrm{o}\mathrm{d}\mathrm{e}{}_{\mathrm{n}}, \mathrm{n}\mathrm{o}\mathrm{d}{\mathrm{e}}_{\mathrm{r}} $，step_len）

18.  if Collision（$ \mathrm{n}\mathrm{o}\mathrm{d}\mathrm{e}{}_{\mathrm{n}}, \mathrm{n}\mathrm{o}\mathrm{d}{\mathrm{e}}_{\mathrm{n}\mathrm{e}} $）then

19.   $ \mathrm{n}\mathrm{o}\mathrm{d}{\mathrm{e}}_{\mathrm{n}\mathrm{e}} $=NULL

20.  if $ \mathrm{n}\mathrm{o}\mathrm{d}{\mathrm{e}}_{\mathrm{n}\mathrm{e}} $ ≠ NULL then

21.   Tree.AddNode（$ \mathrm{n}\mathrm{o}\mathrm{d}{\mathrm{e}}_{\mathrm{n}\mathrm{e}} $）

22.  return False

在算法1中，TreeTopology代表拓扑结构地图生成的初始树，通过Nearest（）函数找到初始树中距离起点和目标点最近，然后将起点和终点插入到初始树中，并结合拓扑结构加快找到初始路径。Collision（）函数为碰撞检测函数，如果扩展的新节点和与其最近的节点的连线穿过障碍物，则重新进行扩展。

2.2 节点扩充策略

在寻找到初始路径后，拓扑结构图中节点的信息过少。由于STIRRT*算法在对初始路径进行优化时，其仍采用文献[19]IRRT*算法中的采样优化半径，通过对当前节点并以一定半径进行采样优化。其采样优化半径$ r $的一般形式如式（4）所示：

$ r=k\times {\left(\frac{\mathrm{l}{\mathrm{g}}_{}n}{n}\right)}^{{}^{1}\!\!\diagup\!\!{}_{d}\;} $

(4)

其中：k为自适应参数；n代表随机树含有的节点数量，由于随机树在初始化时就已经包含起止点，因此$ n\ge 2\mathrm{。} $由于随着节点数量的增加，会增加比较的次数，其收敛速度会变慢，本文为加快采样半径的收敛，取$ d=2 $。如图 5所示，$ {\left(\frac{\mathrm{l}{\mathrm{g}}_{}n}{n}\right)}^{{}^{1}\!\!\diagup\!\!{}_{2}} $函数特性在出现第2个节点后就单调下降。

随机树在进行采样时会根据现有的节点向外进行扩展。随着节点数量的增加，节点的采样优化半径的长度是单调递减的，通过拓扑节点可以快速有效地找到初始路径。为了加快优化算法的收敛，本文通过节点扩充策略，并以半个步长对其进行节点扩充。但是相较于RRT算法在栅格地图中随机生成节点，本文是在生成的初始路径相邻的两个拓扑节点之间进行扩充，如图 6所示。

在图 6中，$ \mathrm{n}\mathrm{o}\mathrm{d}{\mathrm{e}}_{\mathrm{t}} $和$ \mathrm{n}\mathrm{o}\mathrm{d}{\mathrm{e}}_{\mathrm{t}\mathrm{o}\mathrm{p}\mathrm{o}\mathrm{l}\mathrm{o}\mathrm{g}\mathrm{y}\mathrm{n}\mathrm{e}\mathrm{a}\mathrm{r}}\left(\mathrm{n}\mathrm{o}\mathrm{d}{\mathrm{e}}_{\mathrm{t}\mathrm{n}}\right) $为拓扑结构地图中的相邻拓扑节点，新节点的增加方式是沿着两节点之间的直线距离和角度并以1/2的扩展步长进行添加。其扩展公式如式（5）~式（8）所示：

$ {D}_{\mathrm{D}\mathrm{i}\mathrm{s}}=({x}_{2}-{x}_{1}{)}^{2}+{\left({y}_{2}-{y}_{1}\right)}^{2} $

(5)

$ \theta =\mathrm{a}\mathrm{r}\mathrm{c}\mathrm{t}\mathrm{a}\mathrm{n}\left(\frac{{y}_{2}-{y}_{1}}{{x}_{2}-{x}_{1}}\right) $

(6)

$ \mathrm{n}\mathrm{o}\mathrm{d}{\mathrm{e}}_{\mathrm{a}\mathrm{d}\mathrm{d}}.x=\mathrm{n}\mathrm{o}\mathrm{d}{\mathrm{e}}_{\mathrm{t}}.x+\frac{\mathrm{s}\mathrm{t}\mathrm{e}{\mathrm{p}}_{\mathrm{l}\mathrm{e}\mathrm{n}}}{2}\times \mathrm{c}\mathrm{o}\mathrm{s}\left(\mathrm{a}\mathrm{r}\mathrm{c}\mathrm{t}\mathrm{a}\mathrm{n}\theta \right) $

(7)

$ \mathrm{n}\mathrm{o}\mathrm{d}{\mathrm{e}}_{\mathrm{a}\mathrm{d}\mathrm{d}}.y=\mathrm{n}\mathrm{o}\mathrm{d}{\mathrm{e}}_{\mathrm{t}}.y+\frac{\mathrm{s}\mathrm{t}\mathrm{e}{\mathrm{p}}_{\mathrm{l}\mathrm{e}\mathrm{n}}}{2}\times \mathrm{s}\mathrm{i}\mathrm{n}\left(\mathrm{a}\mathrm{r}\mathrm{c}\mathrm{t}\mathrm{a}\mathrm{n}\theta \right) $

(8)

其中：$ ({x}_{1}, {y}_{1}) $和$ ({x}_{2}, {y}_{2}) $分别代表$ \mathrm{n}\mathrm{o}\mathrm{d}{\mathrm{e}}_{\mathrm{t}} $和$ \mathrm{n}\mathrm{o}\mathrm{d}{\mathrm{e}}_{\mathrm{t}\mathrm{n}} $的横纵坐标；$ {D}_{\mathrm{D}\mathrm{i}\mathrm{s}} $和$ \theta $代表它们之间的距离和角度；$ (\mathrm{n}\mathrm{o}\mathrm{d}{\mathrm{e}}_{\mathrm{a}\mathrm{d}\mathrm{d}}.x, \mathrm{n}\mathrm{o}\mathrm{d}{\mathrm{e}}_{\mathrm{a}\mathrm{d}\mathrm{d}}.y) $代表生成新节点的坐标。通过节点扩充的方式可以使节点之间建立有效的联系，加快算法的收敛。

2.3 椭圆采样策略

在对初始路径进行节点扩充后，当对扩充后的初始路径进行优化时，采用椭圆采样的方式代替整个地图采样，这样可以有效地减少搜索路径的次数。采样点均匀分布在椭圆内部，如果通过采样点生成的路径长度小于原先规划的长度，则替代原来的长度并根据新的路径长度逐渐调整椭圆的大小。当两点之间没有障碍物时，其会渐渐收敛为一条线段。其中二维空间中椭圆最基本的形式如式（9）所示：

$ \frac{{x}^{2}}{{a}^{2}}+\frac{{y}^{2}}{{b}^{2}}=1 $

(9)

椭圆的两轴为别是x轴及y轴，轴长分别是2a和2b，将式（9）写成矩阵的形式，其结果如式（10）所示：

$ {\left[\begin{array}{c}\mathit{\boldsymbol{x}}\\ \mathit{\boldsymbol{y}}\end{array}\right]}^{\mathrm{T}}\left[\begin{array}{cc}\frac{1}{{a}^{2}}& 0\\ 0& \frac{1}{{b}^{2}}\end{array}\right]\left[\begin{array}{c}\mathit{\boldsymbol{x}}\\ \mathit{\boldsymbol{y}}\end{array}\right]={\mathit{\boldsymbol{x}}}^{\mathrm{T}}\mathit{\boldsymbol{B}}\mathit{\boldsymbol{x}}=1 $

(10)

由此可知矩阵$ \mathit{\boldsymbol{B}}\in {\mathbb{R}}^{n\times n} $为对称正定矩阵，对其进行Cholesky分解，其可分解为一个下三角矩阵L和其转置的乘积：

$ \mathit{\boldsymbol{B}}=\mathit{\boldsymbol{L}}{\mathit{\boldsymbol{L}}}^{\mathrm{T}} $

(11)

经过Cholesky分解，根据文献[27]对椭圆方程进行如式（12）~式（14）的变换：

$ {X}_{\mathrm{c}\mathrm{i}\mathrm{r}\mathrm{c}\mathrm{l}\mathrm{e}}=\left\{\mathit{\boldsymbol{x}}\in X, \frac{{c}_{\mathrm{b}\mathrm{e}\mathrm{s}\mathrm{t}}^{2}-{c}_{\mathrm{m}\mathrm{i}\mathrm{n}}^{2}}{4}\right\} $

(12)

$ \mathit{\boldsymbol{B}}=\mathrm{d}\mathrm{i}\mathrm{a}\mathrm{g}\left\{\frac{{c}_{\mathrm{b}\mathrm{e}\mathrm{s}\mathrm{t}}^{2}}{4}, \frac{{c}_{\mathrm{b}\mathrm{e}\mathrm{s}\mathrm{t}}^{2}-{c}_{\mathrm{m}\mathrm{i}\mathrm{n}}^{2}}{4}\right\} $

(13)

$ \mathit{\boldsymbol{L}}=\mathrm{d}\mathrm{i}\mathrm{a}\mathrm{g}\left\{\frac{{c}_{\mathrm{b}\mathrm{e}\mathrm{s}\mathrm{t}}}{2}, \frac{\sqrt[]{{c}_{\mathrm{b}\mathrm{e}\mathrm{s}\mathrm{t}}^{2}-{c}_{\mathrm{m}\mathrm{i}\mathrm{n}}^{2}}}{2}\right\} $

(14)

其中：$ {X}_{\mathrm{c}\mathrm{i}\mathrm{r}\mathrm{c}\mathrm{l}\mathrm{e}} $为平面内圆的方程；$ \mathrm{d}\mathrm{i}\mathrm{a}\mathrm{g}\{.\} $代表对角矩阵；$ {c}_{\mathrm{m}\mathrm{i}\mathrm{n}} $代表起点和终点的直线距离；$ {c}_{\mathrm{b}\mathrm{e}\mathrm{s}\mathrm{t}} $为设定的长度，其值要大于$ {c}_{\mathrm{m}\mathrm{i}\mathrm{n}} $。

逐次对$ {c}_{\mathrm{b}\mathrm{e}\mathrm{s}\mathrm{t}} $减小一定的数值直到减小到$ {c}_{\mathrm{m}\mathrm{i}\mathrm{n}} $停止采样，同时根据文献[28]进行如式（15）的旋转变换：

$ C=\mathit{\boldsymbol{U}}\mathrm{d}\mathrm{i}\mathrm{a}\mathrm{g}\left\{1, \mathrm{d}\mathrm{e}\mathrm{t}\left(\mathit{\boldsymbol{U}}\right)\mathrm{d}\mathrm{e}\mathrm{t}\left(\mathit{\boldsymbol{V}}\right)\right\}{\mathit{\boldsymbol{V}}}^{\mathrm{T}} $

(15)

其中$ ：\mathrm{d}\mathrm{e}\mathrm{t}\ (\cdot ) $代表矩阵的行列式；$ \mathit{\boldsymbol{U}}\in {\mathbb{R}}^{n\times n} $和$ \mathit{\boldsymbol{V}}\in {\mathbb{R}}^{n\times n} $是酉矩阵；定义$ {x}_{\mathrm{c}\mathrm{e}\mathrm{n}\mathrm{t}\mathrm{e}\mathrm{r}}=({x}_{a}+{x}_{b})/2 $为椭圆的中心，$ {x}_{a}\mathrm{、}{x}_{b} $为椭圆的两个焦点。通过式$ \left(14\right)\mathrm{、}\mathrm{式}\left(15\right) $，可以在椭圆中根据式（16）进行采样：

$ {x}_{\mathrm{s}\mathrm{a}\mathrm{m}\mathrm{p}\mathrm{l}\mathrm{e}}=CL{x}_{\mathrm{c}\mathrm{i}\mathrm{r}\mathrm{c}\mathrm{l}\mathrm{e}}+{x}_{\mathrm{c}\mathrm{e}\mathrm{n}\mathrm{t}\mathrm{e}\mathrm{r}} $

(16)

图 7为在起点$ \left(\mathrm{0, 0}\right) $和终点$ \left(\mathrm{100, 100}\right) $所构成的椭圆空间中进行采样。从图 7可以看到，当起点和终点之间没有障碍物时，其逐渐收敛为由起点和终点所构成的一条线段。

基于椭圆采样的伪代码如下：

算法2  基于椭圆采样的伪代码

$ \mathrm{n}\mathrm{o}\mathrm{d}{\mathrm{e}}_{\mathrm{r}}\leftarrow \ \mathrm{E}\mathrm{l}\mathrm{l}\mathrm{i}\mathrm{p}\mathrm{s}\mathrm{e}\_\mathrm{s}\mathrm{a}\mathrm{m}\mathrm{p}\mathrm{l}\mathrm{i}\mathrm{n}\mathrm{g}\_\mathrm{n}\mathrm{o}\mathrm{d}\mathrm{e}\ (\mathrm{n}\mathrm{o}\mathrm{d}{\mathrm{e}}_{\mathrm{s}}, \mathrm{n}\mathrm{o}\mathrm{d}{\mathrm{e}}_{\mathrm{g}}, \mathrm{c}\mathrm{m}\mathrm{a}\mathrm{x})$

1.if cmax < ∞ then

2. $ \mathrm{c}\mathrm{m}\mathrm{i}\mathrm{n}\ \ \ \ \ ={||\mathrm{n}\mathrm{o}\mathrm{d}{\mathrm{e}}_{\mathrm{g}}-\mathrm{n}\mathrm{o}\mathrm{d}{\mathrm{e}}_{\mathrm{s}}||}_{2} $

3. $ \mathrm{n}\mathrm{o}\mathrm{d}{\mathrm{e}}_{\mathrm{c}\mathrm{e}\mathrm{n}\mathrm{t}\mathrm{e}\mathrm{r}}=\left(\mathrm{n}\mathrm{o}\mathrm{d}{\mathrm{e}}_{\mathrm{s}}+\mathrm{n}\mathrm{o}\mathrm{d}{\mathrm{e}}_{\mathrm{g}}\right)/2 $

4. $ \mathrm{C}\ \ \ \ \ =\mathrm{R}\mathrm{o}\mathrm{t}\mathrm{a}\mathrm{t}\mathrm{i}\mathrm{o}\mathrm{n}\mathrm{T}\mathrm{o}\mathrm{W}\mathrm{o}\mathrm{r}\mathrm{l}\mathrm{d}\mathrm{F}\mathrm{r}\mathrm{a}\mathrm{m}\mathrm{e}(\mathrm{n}\mathrm{o}\mathrm{d}{\mathrm{e}}_{\mathrm{s}}, \mathrm{n}\mathrm{o}\mathrm{d}{\mathrm{e}}_{\mathrm{g}}, \mathrm{c}\mathrm{m}\mathrm{i}\mathrm{n}) $

5. $ {\mathrm{r}}_{1}={\mathrm{c}}_{\mathrm{m}\mathrm{a}\mathrm{x}}/2 $

6. $ {\mathrm{r}}_{2}=\left(\sqrt[2]{{\mathrm{c}}_{\mathrm{m}\mathrm{a}\mathrm{x}}^{2}-{\mathrm{c}}_{\mathrm{m}\mathrm{i}\mathrm{n}}^{2}}\right)/2 $

7. $ \mathrm{L}=\mathrm{d}\mathrm{i}\mathrm{a}\mathrm{g}\{{\mathrm{r}}_{1}, {\mathrm{r}}_{2}\} $

8. $ \mathrm{n}\mathrm{o}\mathrm{d}{\mathrm{e}}_{\mathrm{b}\mathrm{a}\mathrm{l}\mathrm{l}}=\mathrm{S}\mathrm{a}\mathrm{m}\mathrm{p}\mathrm{l}\mathrm{e}\mathrm{U}\mathrm{n}\mathrm{i}\mathrm{t}\mathrm{B}\mathrm{a}\mathrm{l}\mathrm{l}\left(\right) $

9. $ \mathrm{n}\mathrm{o}\mathrm{d}{\mathrm{e}}_{\mathrm{r}}=\mathrm{C}\times \mathrm{L}\times \mathrm{n}\mathrm{o}\mathrm{d}{\mathrm{e}}_{\mathrm{b}\mathrm{a}\mathrm{l}\mathrm{l}}+\mathrm{n}\mathrm{o}\mathrm{d}{\mathrm{e}}_{\mathrm{c}\mathrm{e}\mathrm{n}\mathrm{t}\mathrm{e}\mathrm{r}} $

10. return $ \mathrm{n}\mathrm{o}\mathrm{d}{\mathrm{e}}_{\mathrm{r}} $

11.else

12. $ \mathrm{n}\mathrm{o}\mathrm{d}{\mathrm{e}}_{\mathrm{r}}=\mathrm{S}\mathrm{a}\mathrm{m}\mathrm{p}\mathrm{l}\mathrm{e}\mathrm{F}\mathrm{r}\mathrm{e}\mathrm{e}\mathrm{S}\mathrm{p}\mathrm{a}\mathrm{c}\mathrm{e}\left(\right) $

13. return $ \mathrm{n}\mathrm{o}\mathrm{d}{\mathrm{e}}_{\mathrm{r}} $

在算法2中$ ，\mathrm{S}\mathrm{a}\mathrm{m}\mathrm{p}\mathrm{l}\mathrm{e}\mathrm{U}\mathrm{n}\mathrm{i}\mathrm{t}\mathrm{B}\mathrm{a}\mathrm{l}\mathrm{l}\left(\right) $函数是在单位球中进行取点，$ \mathrm{S}\mathrm{a}\mathrm{m}\mathrm{p}\mathrm{l}\mathrm{e}\mathrm{F}\mathrm{r}\mathrm{e}\mathrm{e}\mathrm{S}\mathrm{p}\mathrm{a}\mathrm{c}\mathrm{e}\left(\right) $函数代表在非障碍物所在的空间进行采样。

在STIRRT*算法中，$ {c}_{\mathrm{b}\mathrm{e}\mathrm{s}\mathrm{t}} $为规划路径的长度，规划路径由采样点所组成，$ {c}_{\mathrm{m}\mathrm{i}\mathrm{n}} $为起点和终点的直线距离。虽然采用椭圆采样的方式可以有效地减小采样点探索的空间，避免了大量的无效搜索，但是当起点和终点距离较远时，其生成的椭圆仍会覆盖整个地图空间。因此，根据栅格地图的大小和障碍物在地图中的位置和数量，本文提出的算法定义相关约束条件来约束拓扑节点之间的距离，通过该约束条件把初始路径进行分段并对每一段进行优化。通过将不同的障碍物设定不同的权重，障碍物在室内场景中主要抽象成3种：矩形障碍物$ \alpha $，圆形障碍物$ \beta $和类似过道或门一样的障碍物$ \gamma $，其满足关系如式（17）所示，约束条件如式（18）所示：

$ \alpha +\beta +\gamma =1 $

(17)

$ {C}_{\mathrm{C}\mathrm{o}\mathrm{n}\mathrm{s}\mathrm{t}\mathrm{r}\mathrm{a}\mathrm{i}\mathrm{n}\mathrm{t}}=\left[\frac{a+b}{m\mu \alpha +n\theta \beta +p\gamma }\right] $

(18)

其中：$ a $为栅格地图的宽；$ b $为栅格地图中的长；$ m\mathrm{、}n\mathrm{、}p $为3种不同障碍物的数量；$ \mu \mathrm{和}\theta $是根据障碍物在整个地图中的具体位置可调整的自适应参数，其取值范围为$ 0\sim 1 $，障碍物在地图中的位置影响程度越大其值越接近$ 1 $；[]为取整符，将所得到的值进行取整。从式（18）中可以得到，在同种地图中，当障碍物的数量越多影响程度越大时，约束条件的值越小。通过该约束条件可以将初始路径进行分段，提高了算法的优化效率，ATIRRT*算法分段优化的伪代码如下：

算法3  ATIRRT*算法分段优化的伪代码

Tree$ \leftarrow \mathrm{B}\mathrm{u}\mathrm{i}\mathrm{l}\mathrm{d}\_\mathrm{s}\mathrm{t}\mathrm{e}\mathrm{p}\ (\mathrm{n}\mathrm{o}\mathrm{d}{\mathrm{e}}_{\mathrm{s}}, \mathrm{n}\mathrm{o}\mathrm{d}{\mathrm{e}}_{\mathrm{g}}, \mathrm{K}, \mathrm{s}\mathrm{t}\mathrm{e}\mathrm{p}\_\mathrm{l}\mathrm{e}\mathrm{n}, \mathrm{r}\mathrm{a}\mathrm{d}\mathrm{i}\mathrm{u}\mathrm{s}) $

1.Tree.init（$ \mathrm{n}\mathrm{o}\mathrm{d}{\mathrm{e}}_{\mathrm{s}} $）

2.initial_path= Find initial path（）

3.n=len（initial_path）

4.while n！= 0

5. for $ \mathrm{n}\mathrm{o}\mathrm{d}{\mathrm{e}}_{\mathrm{t}} $ in initial path

6.  if sum（$ \mathrm{n}\mathrm{o}\mathrm{d}{\mathrm{e}}_{\mathrm{t}\mathrm{d}} $） > $ \mathrm{C}\mathrm{o}\mathrm{n}\mathrm{s}\mathrm{t}\mathrm{r}\mathrm{a}\mathrm{i}\mathrm{n}\mathrm{t} $

7.   return $ \mathrm{n}\mathrm{o}\mathrm{d}{\mathrm{e}}_{\mathrm{t}\mathrm{i}} $

8.   $ \mathrm{n}\mathrm{o}\mathrm{d}{\mathrm{e}}_{\mathrm{s}} $=$ \mathrm{n}\mathrm{o}\mathrm{d}{\mathrm{e}}_{\mathrm{t}0} $

9. $ \mathrm{n}\mathrm{o}\mathrm{d}{\mathrm{e}}_{\mathrm{g}} $=$ \mathrm{n}\mathrm{o}\mathrm{d}{\mathrm{e}}_{\mathrm{t}\mathrm{i}} $

10. Xsoln = [$ \mathrm{n}\mathrm{o}\mathrm{d}{\mathrm{e}}_{\mathrm{t}0}, \cdots, \mathrm{n}\mathrm{o}\mathrm{d}{\mathrm{e}}_{\mathrm{t}\mathrm{a}\mathrm{d}\mathrm{d}}, \cdots, \mathrm{n}\mathrm{o}\mathrm{d}{\mathrm{e}}_{\mathrm{t}1}, \cdots, \mathrm{n}\mathrm{o}\mathrm{d}{\mathrm{e}}_{\mathrm{t}\mathrm{i}} $]

11. for k = 1 to K

12.   cbest = {$ \mathrm{m}\mathrm{i}\mathrm{n}\mathrm{x}\mathrm{s}\mathrm{o}\mathrm{l}\mathrm{n}\in \mathrm{X}\mathrm{s}\mathrm{o}\mathrm{l}\mathrm{n}\left\{\mathrm{C}\mathrm{o}\mathrm{s}\mathrm{t}\left(\mathrm{x}\mathrm{s}\mathrm{l}\mathrm{o}\mathrm{n}\right)\right\} $

13.   $ \mathrm{n}\mathrm{o}\mathrm{d}{\mathrm{e}}_{\mathrm{r}} $ = Sample（$ \mathrm{n}\mathrm{o}\mathrm{d}{\mathrm{e}}_{\mathrm{s}}, \mathrm{n}\mathrm{o}\mathrm{d}{\mathrm{e}}_{\mathrm{g}} $，cbest）

14.   $ \mathrm{n}\mathrm{o}\mathrm{d}{\mathrm{e}}_{\mathrm{n}\mathrm{r}} $ = Nearest（Tree，$ \mathrm{n}\mathrm{o}\mathrm{d}{\mathrm{e}}_{\mathrm{r}} $）

15.  $ \mathrm{n}\mathrm{o}\mathrm{d}{\mathrm{e}}_{\mathrm{n}\mathrm{e}} $ = Steer（$ \mathrm{n}\mathrm{o}\mathrm{d}{\mathrm{e}}_{\mathrm{n}\mathrm{r}}, \mathrm{n}\mathrm{o}\mathrm{d}{\mathrm{e}}_{\mathrm{r}\mathrm{a}\mathrm{n}\mathrm{d}} $

16.  if CollisionFree（$ \mathrm{n}\mathrm{o}\mathrm{d}{\mathrm{e}}_{\mathrm{n}\mathrm{r}} $ $ \mathrm{n}\mathrm{o}\mathrm{d}{\mathrm{e}}_{\mathrm{n}\mathrm{e}\mathrm{w}} $）

17.    Tree.AddNode（$ \mathrm{n}\mathrm{o}\mathrm{d}{\mathrm{e}}_{\mathrm{n}\mathrm{e}\mathrm{w}} $）

18.    $ \mathrm{N}\mathrm{o}\mathrm{d}{\mathrm{e}}_{\mathrm{n}\mathrm{e}\mathrm{a}\mathrm{r}} $ = Near（Tree，$ \mathrm{n}\mathrm{o}\mathrm{d}{\mathrm{e}}_{\mathrm{n}\mathrm{e}} $，radius）

19.   cmin = Cost（$ \mathrm{n}\mathrm{o}\mathrm{d}{\mathrm{e}}_{\mathrm{n}\mathrm{r}} $）+ c·Line（$ \mathrm{n}\mathrm{o}\mathrm{d}{\mathrm{e}}_{\mathrm{n}\mathrm{e}\mathrm{w}}, \mathrm{n}\mathrm{o}\mathrm{d}{\mathrm{e}}_{\mathrm{n}\mathrm{r}} $）

20.   for ∀$ \mathrm{n}\mathrm{o}\mathrm{d}{\mathrm{e}}_{\mathrm{n}} $∈ $ \mathrm{N}\mathrm{o}\mathrm{d}{\mathrm{e}}_{\mathrm{n}\mathrm{e}\mathrm{a}\mathrm{r}} $ do

21.    cnew = Cost（$ \mathrm{n}\mathrm{o}\mathrm{d}{\mathrm{e}}_{\mathrm{n}} $）+ c·Line（$ \mathrm{n}\mathrm{o}\mathrm{d}{\mathrm{e}}_{\mathrm{n}\mathrm{e}}, \mathrm{n}\mathrm{o}\mathrm{d}{\mathrm{e}}_{\mathrm{n}} $）

22.    if cnew < cmin then

23.     if CollisionFree（$ \mathrm{n}\mathrm{o}\mathrm{d}{\mathrm{e}}_{\mathrm{n}}, \mathrm{n}\mathrm{o}\mathrm{d}{\mathrm{e}}_{\mathrm{n}\mathrm{e}} $）

24.      $ \mathrm{n}\mathrm{o}\mathrm{d}{\mathrm{e}}_{\mathrm{n}\mathrm{e}} $.parent = $ \mathrm{n}\mathrm{o}\mathrm{d}{\mathrm{e}}_{\mathrm{n}} $

25.     cmin=cnew

26.  for ∀$ \mathrm{n}\mathrm{o}\mathrm{d}{\mathrm{e}}_{\mathrm{n}} $∈ $ \mathrm{N}\mathrm{o}\mathrm{d}{\mathrm{e}}_{\mathrm{n}\mathrm{e}\mathrm{a}\mathrm{r}} $ do

27.   cnear = Cost（$ \mathrm{n}\mathrm{o}\mathrm{d}{\mathrm{e}}_{\mathrm{n}} $）

28.   cnew = Cost（$ \mathrm{n}\mathrm{o}\mathrm{d}{\mathrm{e}}_{\mathrm{n}\mathrm{e}} $）+ c·Line（$ \mathrm{n}\mathrm{o}\mathrm{d}{\mathrm{e}}_{\mathrm{n}\mathrm{e}} $，$ \mathrm{n}\mathrm{o}\mathrm{d}{\mathrm{e}}_{\mathrm{n}} $）

29.   if cnew < cnear

30.    if CollisionFree（$ \mathrm{n}\mathrm{o}\mathrm{d}{\mathrm{e}}_{\mathrm{n}\mathrm{e}}, \mathrm{n}\mathrm{o}\mathrm{d}{\mathrm{e}}_{\mathrm{n}} $）then

31.     $ \mathrm{n}\mathrm{o}\mathrm{d}{\mathrm{e}}_{\mathrm{n}\mathrm{e}} $.parent = $ \mathrm{n}\mathrm{o}\mathrm{d}{\mathrm{e}}_{\mathrm{n}} $

32.  if InGoalRegion（$ \mathrm{n}\mathrm{o}\mathrm{d}{\mathrm{e}}_{\mathrm{n}\mathrm{e}} $）

33.   Xsoln.AddNode（$ \mathrm{n}\mathrm{o}\mathrm{d}{\mathrm{e}}_{\mathrm{n}\mathrm{e}} $）

34. for i=0 to i

35.  remove $ \mathrm{n}\mathrm{o}\mathrm{d}{\mathrm{e}}_{\mathrm{t}\mathrm{i}} $ in initial_path

36.  n=len（initial_path）

37.return Tree

在算法3中，初始路径$ \left(\mathrm{i}\mathrm{n}\mathrm{i}\mathrm{t}\mathrm{i}\mathrm{a}{\mathrm{l}}_{\mathrm{p}\mathrm{a}\mathrm{t}\mathrm{h}}\right) $是由拓扑节点所组成的，$ \mathrm{n}\mathrm{o}\mathrm{d}{\mathrm{e}}_{\mathrm{t}\mathrm{o}\mathrm{p}\mathrm{o}\mathrm{l}\mathrm{o}\mathrm{g}\mathrm{y}\mathrm{d}\mathrm{i}\mathrm{s}\mathrm{t}\mathrm{a}\mathrm{n}\mathrm{c}\mathrm{e}}\left(\mathrm{n}\mathrm{o}\mathrm{d}{\mathrm{e}}_{\mathrm{t}\mathrm{d}}\right) $表示拓扑节点之间的距离，$ \mathrm{n}\mathrm{o}\mathrm{d}{\mathrm{e}}_{\mathrm{t}\mathrm{o}\mathrm{p}\mathrm{o}\mathrm{l}\mathrm{o}\mathrm{g}\mathrm{y}}\left(\mathrm{n}\mathrm{o}\mathrm{d}{\mathrm{e}}_{\mathrm{t}\mathrm{i}}\right) $表示构成$ \mathrm{i}\mathrm{n}\mathrm{i}\mathrm{t}\mathrm{i}\mathrm{a}\mathrm{l}\_\mathrm{p}\mathrm{a}\mathrm{t}\mathrm{h} $中的第$ i $个拓扑节点。$ \mathrm{N}\mathrm{e}\mathrm{a}\mathrm{r}\mathrm{e}\mathrm{s}\mathrm{t}\left(\right) $函数返回随机树中离采样点最近的节点$ \mathrm{n}\mathrm{o}\mathrm{d}{\mathrm{e}}_{\mathrm{n}\mathrm{e}\mathrm{a}\mathrm{r}\mathrm{e}\mathrm{s}\mathrm{t}}\left(\mathrm{n}\mathrm{o}\mathrm{d}{\mathrm{e}}_{\mathrm{n}\mathrm{r}}\right) $，$ \mathrm{s}\mathrm{t}\mathrm{e}\mathrm{e}\mathrm{r}\left(\right) $函数通过随机采样点和最近的节点来生成新节点。

3 仿真实验结果与分析

为了对ATIRRT*算法进行性能比较，本文实验对如图 8所示的地图进行仿真，地图包含3种情况，分别为常规环境（地图1）、狭长空间（地图2）和仿真的室内环境（地图3）。其中地图中空白区域代表无障碍物区域，灰色区域代表障碍物区域。仿真所使用的硬件平台为Intel^® Core^TM i5-5200U 2.2 GHz CPU，RAM 8 GB和Intel^® Xeon^® Silver 4114 2.2 GHz CPU 2.19 GHz CPU（2处理器），RAM 128 GB，算法通过Python编程实现。在实验中，分别以RRT、RRT-Connect、RRT*、Informed-RRT*、STIRRT*和DQN作为改进算法的比较算法，通过比较证明ATIRRT*寻找路径的优越性。图 9为对特征点进行阈值处理后所构成的拓扑结构地图，表 1为3种仿真地图下得到角点和拓扑节点所需的时间。

本文在相同的情况下，对ATIRRT*算法与RRT和RRT-Connect算法所寻找的路径进行了10次对比实验。其中根据拓扑节点的阈值式（3）来设定扩展步长，其扩展步长和阈值相等。为了尽可能地找到规划路径，设定最大的迭代次数为50 000次。其在3种地图上的起始位置和目标位置如表 2所示。

图 10、图 11、图 12为随机抽取地图1、地图2、地图3中一组对比实验结果。

从上述的实验结果可知，传统算法在仿真地图中，其无用的迭代次数过多会极大地占用计算机的内存，这样不仅增加了寻找路径时间，而且降低了路径的平滑性。通过对实验结果进行比对，可以发现ATIRRT*算法在规划路径的迭代次数和路径的平滑性上都有较大的提升。为了更直观地体现所提出算法的优越性，分别在3种地图上进行了10次实验，图 13通过在3种地图上比较规划时间、迭代次数和路径长度来验证本文算法的性能。

从图 13可以看出，地图2（狭长空间）RRT算法和RRT-Connect算法在最大迭代次数为50 000次的10次实验中，其成功率分别为40%和60%。而本文所提出的算法其成功率可以达到100%。相比RRT和RRT-Connect算法，在其余两种地图中，本文所提出的算法在规划时间、迭代次数和路径长度上均有不同程度的提升，同时提高了规划路径的稳定性。在得到初始路径后，利用式（17）、式（18），根据栅格地图的大小和障碍物在地图中的数量和位置，得到本文中3种仿真地图中的$ \mathrm{C}\mathrm{o}\mathrm{n}\mathrm{s}\mathrm{t}\mathrm{r}\mathrm{a}\mathrm{i}\mathrm{n}\mathrm{t} $。通过计算，其值分别为$ \mathrm{C}\mathrm{o}\mathrm{n}\mathrm{s}\mathrm{t}\mathrm{r}\mathrm{a}\mathrm{i}\mathrm{n}{\mathrm{t}}_{1}=250，\mathrm{C}\mathrm{o}\mathrm{n}\mathrm{s}\mathrm{t}\mathrm{r}\mathrm{a}\mathrm{i}\mathrm{n}{\mathrm{t}}_{2}=259，\mathrm{C}\mathrm{o}\mathrm{n}\mathrm{s}\mathrm{t}\mathrm{r}\mathrm{a}\mathrm{i}\mathrm{n}{\mathrm{t}}_{3}=300\mathrm{。} $通过约束条件对初始路径分段并进行逐段优化，每一段优化都限制在椭圆中，防止其进行无用的探索。

将分段优化结果与RRT*、Informed RRT*和STIRRT^*算法进行比较。为了尽可能地在较短的时间内得到较优的规划路径，令每次优化的迭代次数为2 000次。由于RRT*和Informed RRT*两种算法都是基于RRT算法在寻找初始路径的同时进行优化，区别在于两种算法找到初始路径后，其随机扩展的采样空间不同。因此，根据RRT算法得到初始路径的平均值再加上分布优化的迭代次数，这样就保证了其探索次数相同，其结果如图 14、图 15、图 16所示。同时，本文将提出的算法和文献[7]深度强化学习（DQN）算法进行比较。为了减少深度强化学习算法的训练时间，加快算法的收敛。本文根据实验场景中的栅格地图，对其按相应的比例进行缩放以建立训练场景。为此，根据障碍物在栅格地图上的占位信息建立如图 17所示的仿真模型。DQN算法训练和测试时使用Intel^® Xeon^® Silver 4114 2.2 GHz CPU 2.19 GHz CPU（2处理器），RAM 128 GB服务器。

网络框架为Pytorch 1.4.0，选用python 3.6.13版本进行编程实现。其中图 17（b）、图 17（d）、图 17（f）分别对应图 17（a）、图 17（c）、图 17（e）建立的仿真训练环境。本文中对比的DQN算法中的Q值维数为4，即可执行动作的个数为4个，分别是上、下、左、右。由于栅格地图在向训练环境转换时，栅格地图中的圆形障碍物并不能占满整个栅格。但由于可执行动作的仅有上下左右，为了尽可能地让规划的路径不经过障碍物，只要栅格地图中的障碍物映射到训练环境时占据训练环境中的部分栅格，在训练时该栅格会被标记为障碍物。

为了加快算法的收敛，本文中DQN算法的奖惩函数公式如式（19）所示：

$ r\left({s}_{t}, {a}_{t}\right)=\left\{\begin{array}{l}10, \mathrm{到}\mathrm{达}\mathrm{终}\mathrm{点}\\ -\mathrm{d}\mathrm{i}\mathrm{s}/1\ 000, \mathrm{未}\mathrm{到}\mathrm{达}\mathrm{终}\mathrm{点}\end{array}\right. $

(19)

其中：dis代表当前状态距离终点的距离。为了能让算法收敛，在3种仿真训练环境中分别训练了100万步、50万步、150万步。对训练得到的模型进行测试，其结果如图 18所示。同时根据相应的缩放比例还原规划路径的长度，具体数据如表 3所示，其中，DQN算法中的训练和测试的硬件平台为Intel^® Xeon^® Silver 4114 2.2 GHz CPU 2.19 GHz CPU（2处理器），RAM 128 GB服务器。其余算法的硬件平台为Intel^® Core^TM i5-5200U 2.2 GHz CPU，RAM 8 GB笔记本。

从上述的实验结果可知，ATIRRT*算法可以有效地减少路径的生成时间和路径长度。相较于Informed-RRT*算法，ATIRRT*算法在3种地图中所规划的路径长度分别缩短了4%、7%、11%，在时间上分别减少了63%、52%、20%。相较于STIRRT*算法，ATIRRT*算法在3种地图中所规划的路径长度分别缩短了6%、13%、16%，在时间上分别减少了16%、8%、6%。相较于DQN算法在3种地图中所规划的路径长度分别缩短了12%、20%、16%。

同时DQN算法在不同的场景进行路径规划时，需要重新训练，训练时间较长。因此，针对环境地图变化的场景其往往很难快速地找到有效的路径。而针对同一场景，其通过训练后，可以做到更换不同的起止点快速地规划出路径，但是其训练的时间会大幅增加。

综上所述，本文所提出的算法在不同的仿真地图环境中均取得了良好的效果，同时在更换地图场景后可以快速地适应新的场景，找到规划路径。

4 结束语

本文提出一种应用拓扑结构的高效路径规划算法，通过自适应阈值消除路径骨架上提取的冗余特征点，并给出非单一父节点的连接方式加强交叉支路上的拓扑节点间的联系。根据节点扩充策略增加相邻拓扑节点间的节点数量以加快优化算法的收敛，定义相关约束条件将初始路径进行分段并进行逐段优化。仿真实验结果表明，该算法具有一定的可扩展性，相较于传统算法在路径平滑性及规划路径的效率上都有不同程度的提升。本文算法将初始路径分段并进行逐段优化，可以缩短时间，但是对于路径长度的优化只是在局部进行，下一步将障碍物的边缘轮廓添加相关约束条件，以使规划出的路径达到全局最优。