改进的RRT路径规划算法

该算法的核心由两个紧密相连的部分构成：

1. 考虑非完整约束的双向多步RRT路径搜索算法
2. 基于三次B样条曲线的路径平滑方法

以下是对该算法部分的详细展示：

---

一、算法核心思想

       本算法的目标是快速地为具有非完整性约束（如转向角限制）的车辆规划出一条平滑、可行、可跟踪的路径。其创新点在于将路径搜索与路径后处理两个步骤有机结合，同时保证了搜索效率和路径质量。

二、改进的RRT路径搜索算法

该部分是对传统RRT算法的三项主要改进。

1. 双向搜索（Bidirectional RRTs）

• 方法：同时构建两棵随机搜索树。

  • 一棵树（Ta）从起始点 Xinit 开始生长。
  • 另一棵树（Tb）从目标点 Xgoal 开始生长。

• 过程：在每次迭代中，交替地对两棵树进行扩展操作，尝试向对方区域生长。

• 终止条件：当两棵树生成的节点在预定阈值内相遇时，即 distance(Nodea,Nodeb)<δ，则路径连通。

• 优势：极大地提高了搜索速度，避免了单棵树盲目地向广阔空间搜索的问题。

2. 多步扩展（Multi-step Extension）

• 方法：在每次扩展时，不再只模拟一个控制输入 Δt 时间，而是连续模拟多个步长，生成一个更长的候选路径段。
• 优势：使得树结构能更快地探索空间，进一步加速搜索过程。与双向搜索结合，形成了双向多步RRT。

3. 融入非完整性约束（Incorporating Nonholonomic Constraints）

这是保证路径可行性的关键。改进在 Extend 函数中实现。

• 传统RRT判断：仅判断从最近点 Xnear 到新点 Xnew 的路径是否无碰撞。

• 本文改进判断：在无碰撞的基础上，增加了一个转向角约束判断。

  • 约束条件：新路径段产生的瞬时转向角 φ 必须小于车辆的最大转向角 φmax（文中设为 π/9，即20度）。
  • 算法修改：将 Extend 函数中的判断语句修改为：

  if collision_free_path(X_near, X_new) and |φ| < φ_max:
      # 将节点X_new和边(X_near, X_new)加入到树中
  else:
      # 放弃该扩展方向，重新采样或扩展

• 优势：确保搜索树生长出的每一条边都满足车辆的动力学约束，从而保证最终搜索出的原始路径本身就是可由车辆执行的，而不仅仅是一条几何上的连线。

三、路径平滑处理：B样条曲线拟合

尽管改进的RRT搜索出了可行的路径，但其随机性会导致路径曲折、不平滑。本文采用三次B样条曲线进行平滑处理。

1. 动机

• 平滑性：B样条曲线天然具有C²连续性（曲率连续），非常适合车辆平稳跟踪。
• 局部性：修改一个控制点只会影响曲线的一段局部形状，而不会改变整个路径。
• 灵活性：可以通过控制点方便地调整路径形状。

2. 方法

• 控制点：将RRT搜索成功后的路径点序列 {P0,P1,...,Pn} 作为B样条曲线的控制点。

• 曲线生成：使用三次B样条（k=4）基函数 Ni,k(u) 来拟合这些控制点，生成平滑路径 C(u)。

                                                

• 确保经过起终点：为了使B样条曲线精确地经过起始点 P0 和目标点 Pn，本文采用了节点向量钳制的方法。即将起始点和目标点在节点向量中设置为多重节点（3重节点），这使得曲线的一端和另一端会分别与第一个和最后一个控制点重合。

四、算法流程总结

该算法的整体工作流程可以清晰地概括为以下四个阶段：

flowchart TD
A[初始化环境与车辆模型] --> B[双向RRT搜索]
    
subgraph B[双向RRT搜索]
    B1[从起点与终点同步生长两棵树]
    B2[扩展时进行碰撞<br>与转向角约束检测]
    B1 --> B2
    B2 -- 循环扩展 --> B1
    B2 -- 满足条件 --> B3[两树连接<br>生成初始路径点集]
end

B --> C[路径后处理]
subgraph C[路径后处理]
    C1[将初始路径点作为<br>B样条控制点]
    C2[使用三次B样条曲线拟合]
end

C --> D[输出平滑、可跟踪的最终路径]

---

五、仿真实验

matlab代码片段：

%% 初始化数据结构
% 初始化树A（从起点开始）
treeA = struct('x', start(1), 'y', start(2), 'theta', start(3), 'parent', 0);
treeA = treeA([]);  % 转换为空结构数组
treeA(1) = struct('x', start(1), 'y', start(2), 'theta', start(3), 'parent', 0);

% 初始化树B（从目标点开始）
treeB = struct('x', goal(1), 'y', goal(2), 'theta', goal(3), 'parent', 0);
treeB = treeB([]);  % 转换为空结构数组
treeB(1) = struct('x', goal(1), 'y', goal(2), 'theta', goal(3), 'parent', 0);

% 路径和平滑路径
path = [];
smoothPath = [];

运行结果：

总结

该文的算法改进是一个系统性的工程：

1. 搜索阶段：通过双向策略和多步扩展来提升速度。
2. 约束处理：在扩展过程中直接嵌入非完整性约束来保证可行性。
3. 后处理阶段：利用B样条曲线的数学特性来提升路径质量。

最终实现了一个在效率、可行性和平滑度三个方面都表现优异的路径规划算法。

---

可复制一键运行代码（附详细注释）获取方式，直接复制链接打开：

https://m.tb.cn/h.i9vL8Vr?tk=u5AL5dqsmRY CA381