地下狭窄空间探索无人车的改进路径优化算法研究

杨 超， 袁晓宇

(1.中国电子科技集团公司 第十研究所，成都 610207；2.浙江科技学院，杭州 310023)

路径规划算法的研究是无人车导航系统中的关键技术，但应用在地下狭窄空间较多的环境中仍是困难的.根据普遍情况下的导航任务需求，研究者通常考虑算法中的性能指标.近几年，国内外学者已经提出许多运动规划算法，例如人工势场法[1]、遗传算法[2-3]、蚁群算法[4-5]、RRT算法及其变体[6-8].文献[9]中针对传统RRT算法随机性强、生长步长固定等问题，结合了引力场和动态步长两种策略，收敛速度和时间.文献[10]提出RRT森林算法解决了搜索区域受限、耗时较长等问题.文献[11]通过对状态空间随机采样，检测碰撞点，解决了三维状态的路径规划问题.对于连接策略问题上，Lin等[12]提出了通过简化节点连接策略来生成满足操作环境约束的可行路径.在随机性高，收敛速度慢，飞行轨迹弯曲的问题上，Fan等人[13]引入了ACO(蚁群优化)使规划路径渐近最优.RRT*算法作为RRT算法的一种改进型，在不断的研究与拓展中Qureshi A H等对此类算法做出总结：只要时间足够，RRT*算法会收敛到最优解，但效率大大减少[14].由Karaman等提出的Informed RRT*算法是RRT*的改进，旨在缩短搜索时间[15].虽然针对传统路径规划问题进行了诸多研究，但仍存在很难通过狭窄通道或入口的问题.这是必须穿越低采样区域完成的任务，这就提高了无人车导航系统对路径规划的要求.

文中提出了一种针对狭窄空间探索的改进IA-RRT算法，该算法可以产生更有效的导航路径.它引入了一种距离优先的启发式采样策略，通过距离计算，优点选点等任务进行阶段规划，以及采用了一种适应环境的步长选择策略，通过环境中障碍物的占比实现合理分配更适合环境的步长.

1 改进RRT算法

1.1 改进思想

RRT算法是一种基于随机采样和迭代的路径规划算法.它通过逐步生长一棵树，从初始点开始，不断随机采样并连接新采样点，构建出一个树状结构，其中包含了可行路径.在构建树的过程中，通过判断采样点是否接近目标点来逐步扩展树的结构，直至找到一条有效的路径，如图1所示.但由于从节点向外搜索的方式具有一定的随机性和多样性，所以每次生成的路径都是不同的，这会导致规划所需的时间增加，降低工作效率.

图1 RRT采样示意图

RRT及其变体已经被广泛地应用于解决许多领域的路径规划问题，都具有类似树的结构，并且在寻路中逐步增加这些树的节点.其采样方式是空间内随机，环境中有大量狭窄通道时，采样点落在宽开放区域的可能性大大高于狭窄通道，可能会变得低效，在很多情况下，树的扩展会更快填充整个空间，使每次路径规划完成时间很长.一种改进的IA-RRT算法被提出，该方法限制采样区域和方向，在生成圆上采样，如图2所示.

图2 改进算法采样示意图

采样计算方式如下

(1)

式中：Xnew为每次寻找到的可以加入树中的节点；σ为以Xnew为圆心建立坐标系的x轴的正方向；τ为负方向；L为步长；i为圆上正方向或负方向生成节点的个数.

(2)

式中：Δx1和Δx2分别为正方向和负方向上的距离.

选择节点的纵坐标可以表示为

(3)

由(1)、(2)、(3)式可以得到圆上的全部节点集合.

Xall={(xσ，y1)，(xτ，y2)}.

(4)

把Xall保存到列表A中，每次采样的第一个节点Xall1，是由启发函数计算列表A中所有节点，得出距离目标点代价值最小的节点.假设有n个节点，要计算的区间为

[(X1，Xgoal)，(X2，Xgoal)…(Xn，Xgoal)].

(5)

当此启发式采样点Xrand与障碍物的连线之间存在障碍物时，在列表A中删除这个Xrand，并在Xall中随机选择下一个点为Xrand，再次判断碰撞.每次得到的可行的Xrand重新命名为Xnew存入列表B中.这样选择在一定程度上减少了重复采样，既体现了使用启发式搜索的有效性，又保留了原始RRT的随机性.

(6)

式中：x，y分别表示横向距离和纵向距离.

由(6)可得新节点到目标节点的间距和角度：

(7)

(8)

式中：d表示新节点到目标节点的间距；θ表示新节点与目标节点间的角度.

1.2 步长选择

为了提高导航的成功率和安全性，选择适合环境的步长来进行路径规划，改进算法提出了一种适应全局的步长选择策略，在开始导航之前，先计算该环境的障碍物占比，障碍物占比较多，说明环境复杂，狭窄通道较多，选择较小的步长对于通过该环境更有效，步长选择方法为

(9)

式中：c表示障碍物占比；m为先验地图中根据地图大小随机生成的点的个数；k为计数器计算的随机点与障碍物碰撞次数.

当c大于或等于阈值p时，步长选择为2；当c小于阈值p时，步长选择为3[16-17].即

(10)

1.3 距离优先的启发式采样策略

改进RRT算法的启发式搜索是在空间中对每一个搜索位置进行评估，得到最好的位置，再从这个位置重复搜索步骤一直到目标为止，传统RRT启发式搜索的采样方法存在随机性和发散性，为了提高采样效率和缩短通过狭窄通道的时间，改进算法提出一种距离优先采样的启发式搜索方法.

(11)

式中：Xnow为当前节点；Xrand为随机选择节点.

计算列表中的随机节点和目标点的距离，以固定的步长L向Xnow、Xnew方向进行生长，优先选择使总代价值最小的点，生成新节点，此方法的代价函数为

f(s)=g(s)+h(s)，

(12)

式中：f(s)为路径的总代价值；g(s)为已存在列表B中的当前节点Xnew到选择点Xrand的代价值，由步长L加上优先级∂决定.计算方法为

(13)

式中：∂为在以当前点为原点所形成的二维坐标系中象限优先级.目标点所在的象限α优先级记为1，对称象限β记为3，其它两个象限γ记为2，优先选择1象限里的目标点，其次选择2，最后选择3.

g(s)=L+∂，∂[1，2，3].

(14)

h(s)为选择点Xrand到目标点Xgoal的代价值，计算方法为

(15)

(16)

2 仿真实验设计及测试

2.1 仿真实验设计

为了验证本算法的有效性和保证环境变量唯一性，分别设置3个障碍物占比不同，但大小相同的50米×30米的环境场景，该环境(2，2)为起始位置，(40，20)为目标位置，设置启发式概率为0.05，设置最大节点数为2 000，最大迭代次数为2 000.通过仿真实验进行验证并分别与RRT、RRT*、Informed RRT*算法在多环境中的路径规划结果进行对比.

如图3所示，在只考虑规划路径问题上，无人车会将自身看成一个居于中心位置的质点，但在实际工作空间中，无人车和障碍物边缘通常是不规则形状，为保证安全性和科学性，在配置空间中将障碍物边缘设置膨胀层扩大障碍物实体尺寸，将无人车身统一看成正圆，膨胀层厚度设置为车身半径.对于一些较简单的不规则物体，设置适当的膨胀层可能能够有效地避免碰撞.然而，对于非常复杂或特殊形状的不规则物体，需要结合激光雷达或深度摄像头等传感器来实时感知环境，可以提供更精细的避障信息，从而使路径规划更加准确和可靠.

图3 转化配置空间示意图

2.2 仿真实验结果

环境A中障碍物占比为16.74%；环境B障碍物占比为22.45%，环境C障碍物占比为52.15%，各进行100组重复实验，4种算法运行效果如图4所示.结果见表1.

表1 环境A的实验结果

图4 环境A仿真实验

结合图4和表1，在狭窄通道较少的环境中，改进的RRT算法的平均搜索时间比传统RRT减少约53%，搜索覆盖率减少约28%.

环境B中，结合图5和表2，在障碍物较分散的环境中下，改进的RRT算法的平均时间较传统RRT减少约58%，并且改进算法只需要搜索较少的自由区域，搜索覆盖率减少约33%.

表2 环境B的实验结果

图5 环境B仿真实验

环境C中，结合图6和表3，在狭窄通道较多的环境中，改进的RRT算法具有较强的适应能力，寻路时间减少了约62%，搜索覆盖率减少约25.6%，体现出了启发式搜索的有效性.

表3 环境C的实验结果

图6 环境C仿真实验

3 改进IA-RRT算法在无人车上的测试

改进IA-RRT算法需要有控制轨迹和速度以及建图和定位能力，综合考虑成本和实现算法的可能性，文中设计的是Nvidia公司的jetson nano做计算板，Jetson Nano采用四核64位ARM CPU和128核集成NVIDIA GPU，动力系统是有刷编码电机通过电机的脉冲返回值可以测出无人车的速度，从而实现无人车的速度闭环控制.感知模块是建图与路径规划的重要数据来源，主传感器装备了激光雷达和双目相机，其中，激光雷达用来避障和构建室内地图，双目相机识别障碍物.实验装置如图7所示.

图7 实验装置

经过一系列仿真实验已经基本验证算法的有效性和对于环境变化下的自适应性.如图8所示，设计了真实环境下的动态实验.在该实验中泡沫块设置为未知障碍物.真实实验中，在微型无人车执行任务遇到未知障碍物时，无人车能够灵活地避开障碍物，优化路径，高效地完成任务，传统算法进行路径规划将导致整体路径冗余、运行时间过长、死锁，用改进方法后，无人车加强了对环境的理解能力，优化了环境带来的路径规划问题.

图8 真实环境实验

如图9所示，设计一个回环路径指令，展示了无人车从起点A到终点B的回环路径.

图9 实际环境中运行的点云图

图中曲线轨迹为无人车轨迹，点云图形为无人车识别出的障碍物.实验过程中，无人车能快速、准确的计算路径和通过障碍物，该实验展现了改进IA-RRT算法对真实环境的适应性和准确性.

4 结 论

文中提出了一种改进的IA-RRT算法，改进算法的启发式搜索是在空间中对每一个搜索位置进行评估，得到最好的位置，再从这个位置重复搜索步骤一直到目标为止，提高RRT算法在复杂环境中的路径规划效率.数值仿真实验结果表明：改进的IA-RRT算法具有较强的适应能力，体现出了启发式搜索的有效性，并在微型无人车导航系统的路径规划测试中，能够实时进行路径规划，展现了改进的IA-RRT算法对真实环境的适应性和准确性.