用于变电站场景的路径规划方案

REF：变电站智能巡检车路径规划算法的研究与实现

1. 系统架构

智能巡检车硬件主要由五部分组成：控制系统、定位导航模块、环境采集模块、数据传输模块和人机智能交互模块，具体包括上位机主控制器、激光雷达、惯性传感器和电机驱动等部件，上位机作为系统主控单元，负责统筹调度各硬件组件的协同工作，激光雷达等传感器则辅助进行环境的精确感知、精确定位、导航与目标识别等任务，确保智能巡检车能够在多变的工作环境中稳定运行。

be3990e792f5235dab0c83887fc24c3b.png

2. 概念解释

• 快速随机扩展树（RRT）：一种基于随机采样的全局路径规划算法，扩展过程类似于树枝向外扩散的状态，步骤规划过程为随机采样、寻找最近节点、直线扩展、碰撞检测、节点入树、找到目标、输出路径；旨在解决高维空间中的路径规划问题，尤其是在复杂环境中机器人的导航问题，搜索步骤如下： 但是普遍存在一些局限性，所以需要融合更多策略进行优化，默认缺陷如下：
  1. 采样过程过度依赖目标点，容易陷入局部最优，无法完成任务。
  2. 对目标点位置敏感，当目标点设置不当时，搜索过程无法找到可行路径。
  3. 当面对终点附近障碍较多时，会出现目标点不达的情况。
  4. 环境适应度低，在复杂环境中无法利用有效节点，需要大量迭代才能找到路径，搜索效率低。
  5. 创建空树 T：将起始节点 Xstar 加入 T，然后开始循环迭代
  6. 生成采样点 Xrand：在空树 T 中随机采样
  7. 生成新的节点 Xnew：在树 T 中找到距离该采样点最近节点 Xnear，计算从最近节点到采样点进行直线扩展
  8. 连接最近节点更新路径：检测新生成的节点与最近节点之间的连线是否与障碍物相交，如果没有相交，则视该节点有效
  9. 重复步骤（2）（3）（4）进行采样扩展，最后检测新生成的节点是目标点 Xgoal 则输出路径

4e3528805d995b14f386182be98c66eb.png

• 双向快速随机树（BI-RRT）：同时从起点和目标点构建两棵随机树，通过双向搜索加快路径生成速度。分别以起点和终点初始化起点树和终点树，进入循环迭代，在每次迭代中，先在搜索空间内随机采样获取一个点，接着在起点树和终点树中找到距离该随机点最近的节点，通过欧几里得距离计算，基于最近节点和随机点生成新节点，并检查新节点是否与障碍物碰撞，若碰撞则返回随机采样步骤，否则对新节点与树中其他节点的连接进行调整，并将新节点加入对应树，更新树结构。最后检查两棵树是否存在共同节点或节点间距离是否小于阈值，判定是否相连，若相连则回溯生成从起点到终点的路径并输出，算法流程结束。

43747855193a6dc5a488a57c76412e26.png

• 人工势场快速随机树（APF-RRT）：模仿自然界中的物理引力和斥力，来引导巡检车从起始点到达目标点，同时避开障碍物。将目标点和障碍物分别视为不同种类的势场源，将无人车视为一个质点，目标点和采样点对无人车施加引力，旨在引导机器人向目标点移动，障碍物对无人车施加斥力，促使无人车远离障碍物，将其受到的吸引力和斥力叠加，就可以判断出无人车的运动状态，运动方向为合力的方向，运动速度则与合力的大小成正比。

7f0d4c3f7d216b2fdcaa0aa7770e9a88.png

• 动态步长策略：根据障碍物密度调整步长的大小，在障碍物较少的区域，增大步长，有效减少路径搜索的时间，避免过度细致的探索，提升整体效率；在障碍物密集的区域，减小步长，使其更加细致地探索环境，避免过大的步长跨越障碍物，导致路径规划失败。这种动态策略增强了算法对不同环境的适应能力，可以有效处理复杂的环境，减少与障碍物发生碰撞的风险，增强了算法的鲁棒性。
• 贪婪策略：在每一步决策中都选择当前状态下的最优解，通过一系列局部最优的选择最终达到全局最优，可以对贪婪策略路径节点的选择增加约束，比如路径段长度不超过最大允许距离、路径不与障碍物碰撞等。

4600d9adc1de4a8a13f95cfefe4df4d5.png

• B 样条曲线插值：基于控制点和节点向量的分段多项式曲线，是贝塞尔曲线的扩展版本，优势在于可以能够通过调整控制点和节点来精确地控制曲线的形状，同时保持曲线的平滑性。

3. 全局路径规划（BI-APF-RRT）

结合动态步长、 BI-RRT与 APF-RRT算法，通过从起点和目标点同步构建随机树，利用双向搜索策略提升路径生成效率，引入人工势场引力和斥力机制，有效避免了算法陷入局部最优问题，并结合 RRT的渐进最优性特性，进一步保证了路径的全局优化质量，设计一个在评估路径规划算法的优劣的衡量指标，在当前实验场景下，以路径长度、执行时间和探索节点数视作关键的三个评价指标：

1. 路径长度：作为衡量路径质量最直观的指标，较短的路径意味着巡检车在执行任务的过程中消耗的能量更低，行驶的里程更少，一定程度上减少了电能消耗，降低巡检成本。
2. 执行时间：反映了算法生成可行路径的速度，在巡检过程中遇到突发状况需要快速响应环境变化，重新规划路径，执行时间短的算法能够更快地提供新的行驶路径，避免碰撞等危险情况的发生，提高整个系统的运行效率。
3. 探索节点数：探索节点数越少，说明算法能够在更短的时间内找到可行路径，避免在不必要的区域进行搜索，减少计算资源的浪费。过多的探索节点数则会使算法陷入复杂搜索空间，增加计算成本。

4. 局部路径规划（DWA）

DWA 算法将位置控制转换为速度控制，将障碍物规避问题视为具有速度空间约束的优化问题，算法计算机器人当前状 态和运动模型下的速度空间，并在一定时间内通过成本函数评估移动机器人的候选路径，通过轨迹评价函数通过评估每条路径轨迹的目标接近代价、速度值代价和障碍避让代价从而确定最佳轨迹。

将约束贪婪策略和 B 样条插值进行结合，形成路径剪枝策略融入DWA算法，实现对全局路径的分段优化：约束贪婪算法首先对路径进行节点处理，在满足约束条件的前提下尽可能减少冗余节点；接着由 B 样条插值对路径的平滑进行精细处理，弥补约束贪婪算法可能导致的路径不平滑问题，提高整体效率。

5. 算法融合

融合全局和局部规划算法，融合算法执行的详细步骤为：

1. 地图构建：智能车启动后，SLAM 技术将激光雷达和 IMU 的数据进行融合，绘制出全局地图
2. 全局路径规划：基于获取的环境地图，全局规划算法生成一条从起点到目标点的初步全局路径
3. 路径后处理：采用剪枝策略去除全局路径中的不必要拐点，运用样条插值算法生成平滑曲线
4. 启动车辆：根据优化后的全局路径，按照设定好的速度启动车辆运动
5. 局部环境监测：在行驶过程中，传感器监测到局部环境发生变化时，触发局部路径规划流程
6. 局部路径规划：DWA 算法生成多个候选轨迹，并根据局部环境信息，选择最优的轨迹作为局部路径
7. 调整行驶方向：根据局部路径计算出控制信号，精确调整车辆的行驶方向和速度。
8. 目标点判断：通过基于地图匹配的定位实时判断是否到达目标点。当位置与目标点位置的偏差在设定的精度范围内时，判定车辆到达目标点，若未到达目标点，则车辆继续按路径行驶并监测环境变化，重复上述路径规划与行驶流程，直至到达目标点。

9ab1bd0f0dee4c9f71392d6868e573cd.png