【路径规划】运动规划的Halton序列生成、碰撞检测和RRT搜索算法的实现三角形机器人在包含非凸障碍的工作空间中进行路径规划附matlab代码

✅作者简介：热爱科研的Matlab仿真开发者，擅长毕业设计辅导、数学建模、数据处理、程序设计科研仿真。

🍎完整代码获取 定制创新 论文复现点击：Matlab科研工作室

👇 关注我领取海量matlab电子书和数学建模资料

🍊个人信条：做科研，博学之、审问之、慎思之、明辨之、笃行之，是为：博学慎思，明辨笃行。

🔥 内容介绍

一、引言

在机器人运动规划领域，让机器人在复杂且含有非凸障碍物的工作空间中找到一条安全有效的路径是一项具有挑战性的任务。Halton 序列生成、碰撞检测以及 RRT 搜索算法的结合为解决这一问题提供了一种可行的方案。对于三角形机器人而言，其独特的形状需要在规划过程中进行特殊考虑，以确保路径规划的准确性和有效性。

二、Halton 序列生成

1. 原理

   ：Halton 序列是一种低差异序列，它在多维空间中能够均匀地分布点。其生成基于不同质数的基数。例如，对于一维 Halton 序列，若以质数b为基数，第n个 Halton 序列值Hn的计算方法如下：首先将n表示为基数b的形式n=akbk+ak−1bk−1+...+a1b+a0，其中0≤ai<b。然后Hn=bk+1ak+bkak−1+...+b2a1+ba0。在二维或多维空间中，通过选择不同的质数作为每个维度的基数来生成 Halton 序列点。

2. 在路径规划中的作用

   ：在机器人路径规划里，Halton 序列可用于生成随机采样点。相较于普通的随机采样，Halton 序列生成的点分布更均匀，能更有效地覆盖工作空间。这有助于 RRT 算法更快地探索到可行路径，避免采样点过度集中在某些区域，提高路径规划的效率。

三、碰撞检测

1. 三角形机器人的碰撞检测方法

   ：对于三角形机器人，一种常用的碰撞检测方法是基于三角形与障碍物的几何关系。例如，将障碍物的表面表示为多边形，通过判断三角形机器人的顶点或边是否与障碍物多边形相交来确定是否发生碰撞。可以使用分离轴定理（SAT）来实现高效的碰撞检测。该定理指出，如果两个凸多边形在任何一个坐标轴上的投影不重叠，那么这两个多边形不相交。对于三角形与非凸障碍物（可分解为多个凸多边形），可以对非凸障碍物的每个凸部分分别应用 SAT 进行碰撞检测。

2. 实现过程

   ：在实际实现中，首先获取三角形机器人的位置和姿态信息，将其转换为世界坐标系下的几何表示。对于每个障碍物，同样获取其几何信息。然后，针对三角形的每条边和每个顶点，与障碍物的多边形进行碰撞检测计算。如果检测到相交，则判定发生碰撞，该路径不可行；反之，则认为该位置和姿态下机器人与障碍物无碰撞。

四、RRT（快速探索随机树）搜索算法

1. 算法基础

   ：RRT 算法通过在状态空间中随机采样点，并将新采样点连接到已有树结构中距离最近的节点，逐步扩展搜索树，直到搜索树包含目标节点或达到设定的迭代次数。在三角形机器人路径规划中，状态空间不仅包括机器人的位置(x,y)，还需考虑其姿态（如旋转角度），因为三角形机器人的姿态变化会影响其与障碍物的碰撞情况。

2. 结合 Halton 序列和碰撞检测的实现

   ：利用 Halton 序列生成随机采样点qrand，在已有搜索树中找到距离qrand最近的节点qnear。然后从qnear向qrand扩展出一个新节点qnew，扩展过程需考虑三角形机器人的运动学约束，例如最大旋转角度、最大移动距离等。在生成qnew后，通过碰撞检测判断qnew是否与障碍物发生碰撞。若未碰撞，则将qnew加入搜索树；若碰撞，则舍弃该点，重新采样。重复上述过程，不断扩展搜索树，直到连接起始点和目标点，形成一条可行路径。

五、完整路径规划流程

1. 初始化

   ：设定工作空间的边界，定义非凸障碍物的形状和位置，初始化 RRT 树，将起始点作为树的根节点。同时，确定 Halton 序列生成所需的质数（如第一个维度用质数2，第二个维度用质数3等）以及 RRT 算法的相关参数，如最大迭代次数、扩展步长等。

2. 采样与扩展

   ：使用 Halton 序列生成采样点，通过 RRT 算法的最近点查找和扩展操作，尝试生成新节点。在每次扩展后，进行碰撞检测，确保新节点处于无碰撞状态。

3. 判断与终止

   ：如果新节点是目标节点，则找到了一条可行路径，终止算法；若达到最大迭代次数仍未找到目标节点，则判定在当前条件下无法找到路径或需要调整参数重新尝试。

4. 路径优化（可选）

   ：找到路径后，可以对路径进行优化，例如通过去除路径中的冗余点，使路径更加平滑，减少机器人运动过程中的不必要转向，提高运动效率。

六、优势与挑战

1. 优势
   • 高效探索

     ：Halton 序列的均匀采样特性结合 RRT 算法的快速扩展能力，使得在复杂的含非凸障碍工作空间中能高效地探索到可行路径，相较于传统随机采样方法，大大减少了搜索时间。

   • 适应性强

     ：RRT 算法本身对环境的适应性强，能够处理各种形状的障碍物，结合三角形机器人特定的碰撞检测方法，可以有效应对三角形机器人在复杂环境中的路径规划需求。

2. 挑战
   • 计算复杂度

     ：碰撞检测过程，尤其是针对非凸障碍物分解后的多个凸部分进行检测，计算量较大。此外，RRT 算法在高维状态空间（如考虑三角形机器人姿态的多维空间）中，搜索树的扩展和节点处理也会带来较高的计算复杂度，可能影响实时性。

   • 路径质量

     ：RRT 算法生成的路径通常不是全局最优路径，可能存在迂回或较长的情况。在对路径长度和效率要求较高的场景下，可能需要进一步的优化算法来改善路径质量。

⛳️ 运行结果

📣 部分代码

function [path, tree] = RRTSearch(start, goal, obstacles)

% Map Setup

x_range = [0, 100];

y_range = [0, 100];

while true

robot_center = start;

robot_vertices = robotVertices(robot_center);

% Check if robot in workspace

if ~(all(robot_vertices(:,1) <= x_range(2)) && all(robot_vertices(:,1) >= x_range(1))...

&& all(robot_vertices(:,2) <= y_range(2)) && all(robot_vertices(:,2) >= y_range(1)))

disp("Not valid start point. Try again");

else

break;

end

start = input("Enter new start point: ");

end

% For the end point

while true

robot_center = goal;

robot_vertices = robotVertices(robot_center);

% Check if robot in workspace

if ~(all(robot_vertices(:,1) <= x_range(2)) && all(robot_vertices(:,1) >= x_range(1))...

&& all(robot_vertices(:,2) <= y_range(2)) && all(robot_vertices(:,2) >= y_range(1)))

disp("Not valid start point. Try again");

else

break;

end

goal = input("Enter new goal point: ");

end

% Goal Threshold

goal_threshold = 3;

found_path = false;

% RRT Path

tree.vertices = start;

tree.edges = [];

% Step-Size for Expansion

del_q = 5;

N = 1e5;

% Get the Halton Sequence for the points

p1 = 5;

p2 = 7;

points = computeGridHalton(p1, p2, N);

points(1,:) = (x_range(2) - x_range(1))*points(1,:) + x_range(1);

points(2,:) = (y_range(2) - y_range(1))*points(2,:) + y_range(1);

for iter = 1:N

q_rand = points(:,iter)';

% Find nearest node - q_rand

q_expansion_idx = findNearest(tree.vertices, q_rand);

q_expansion = tree.vertices(q_expansion_idx, :);

% Compute point q_nearby which lies on the path connecting q_rand and q_expansion

q_diff = q_rand - q_expansion;

dist = norm(q_diff);

q_nearby = q_diff/dist * del_q + q_expansion;

% Check if the point is in boundary

if q_nearby(1) <= x_range(2) && q_nearby(1) >= x_range(1) &&...

q_nearby(2) <= y_range(2) && q_nearby(2) <= y_range(1)

continue;

end

% Check collision along path

% if ~checkcollision(q_expansion, q_nearby, obstacles)

if true

% Node is added to V

tree.vertices = [tree.vertices; q_nearby];

q_nearby_idx = size(tree.vertices,1);

tree.edges = [tree.edges; q_expansion_idx, q_nearby_idx];

% Check goal proximity

if norm(q_nearby - goal) < goal_threshold

disp("Reaching near the point");

found_path = true;

goal_point = q_nearby;

break;

end

end

end

% Reconstruct path

path = [];

if found_path

disp("Path exists to Goal");

figure;

hold on;

axis equal;

xlim(x_range);

ylim(y_range);

title('RRT Search Algorithm');

xlabel('X');

ylabel('Y');

% Plot the obstacles

for i = 1:5

obstacle = obstacles{i};

patch(obstacle(:, 1), obstacle(:, 2), 'red', 'EdgeColor', 'black', 'FaceAlpha', 0.5);

end

% Plot the vertices from tree.vertices

plot(tree.vertices(:, 1), tree.vertices(:, 2), 'ko', 'MarkerFaceColor', 'green', 'MarkerSize', 5);

% Get the path

path = reconstructPath(tree, start, goal_point);

% Plot the reconstructed path

plot(path(:, 1), path(:, 2), '-b', 'LineWidth', 2);

scatter(path(:, 1), path(:, 2), 50, 'filled', 'b');

else

disp("Path does not exist to Goal");

end

end

🔗 参考文献

[1]鞠殿元.面向RRT路径规划算法的多优化策略研究[D].齐鲁工业大学[2026-06-11].

🍅更多免费数学建模和仿真教程关注领取