1. 项目背景与核心需求
自动驾驶汽车的路径规划是智能交通系统的核心技术之一。在实际道路环境中，车辆需要实时生成安全、高效的行驶路径，同时考虑车辆自身的动力学约束和静态障碍物的规避。传统路径规划算法如A*、Dijkstra等在复杂动态环境中往往计算效率不足，难以满足实时性要求。
快速扩展随机树(Rapidly-exploring Random Tree, RRT)算法因其在复杂环境中的高效搜索能力，成为解决这一问题的理想选择。本项目实现了基于RRT算法的自动驾驶路径规划系统，特别针对以下核心需求：
- 考虑车辆动力学约束（如转向半径、加速度限制）
- 有效避开静态障碍物（如路障、停驶车辆）
- 保证路径的平滑性和可跟踪性
- 满足实时计算要求（单次规划时间<100ms）
2. 算法原理与实现框架
2.1 RRT算法基础原理
RRT是一种基于采样的路径规划算法，其核心思想是通过随机采样扩展树结构来探索配置空间。基本流程包括：
- 初始化：从起点q_init开始构建树T
- 随机采样：在自由空间中随机生成采样点q_rand
- 最近邻搜索：在T中找到距离q_rand最近的节点q_near
- 扩展尝试：从q_near向q_rand方向扩展步长η，得到新节点q_new
- 碰撞检测：检查q_new与障碍物的碰撞情况
- 节点添加：若无碰撞，将q_new加入T
matlab复制function [T, success] = buildRRT(q_init, goal, obstacles, params)
T = q_init;
for i = 1:params.max_iter
q_rand = randomSample();
q_near = nearestNeighbor(T, q_rand);
q_new = extend(q_near, q_rand, params.step_size);
if ~collisionCheck(q_new, obstacles)
addNode(T, q_new);
if reachGoal(q_new, goal)
success = true;
return;
end
end
end
success = false;
end
2.2 车辆动力学约束建模
为考虑车辆动力学特性，我们在标准RRT基础上引入以下改进：
- 转向约束：使用自行车模型限制最大转向角
code复制R_min = L / tan(δ_max) % 最小转弯半径 - 加速度约束：限制节点间的速度变化
code复制Δv ≤ a_max * Δt - 曲率连续：使用B样条曲线平滑路径，确保方向盘转向平顺
2.3 障碍物规避策略
静态障碍物处理采用分层检测方法：
- 粗检测：使用包围盒快速排除明显无碰撞的情况
- 精检测：对可能碰撞的障碍进行精确几何检测
- 安全距离：保持最小安全间距d_safe=0.5m
3. MATLAB实现详解
3.1 核心数据结构设计
matlab复制classdef RRTNode
properties
position % [x,y]坐标
velocity % 当前速度
parent % 父节点索引
cost % 从起点到该节点的代价
end
end
classdef Obstacle
properties
vertices % 障碍物顶点坐标
type % 障碍物类型（矩形/圆形）
end
end
3.2 关键函数实现
3.2.1 随机采样函数
matlab复制function q_rand = randomSample(goal, bias)
% 目标偏向采样提高收敛速度
if rand() < bias
q_rand = goal.position;
else
q_rand = [rand()*map_width; rand()*map_height];
end
end
3.2.2 扩展函数（含动力学约束）
matlab复制function q_new = extend(q_near, q_rand, step_size, vehicle_params)
direction = q_rand - q_near.position;
dist = norm(direction);
% 考虑最大转向角约束
max_steer = vehicle_params.max_steer;
if dist > 0
direction = direction/dist;
actual_step = min(step_size, dist);
% 计算可达位置（简化自行车模型）
delta = atan2(direction(2), direction(1)) - q_near.theta;
delta = wrapToPi(delta);
delta = sign(delta)*min(abs(delta), max_steer);
q_new.position = q_near.position + actual_step*[cos(q_near.theta + delta);
sin(q_near.theta + delta)];
q_new.theta = q_near.theta + delta;
else
q_new = q_near;
end
end
3.3 完整算法流程
matlab复制function [path, tree] = RRT_Planner(start, goal, obstacles, params)
% 初始化
tree = RRTNode(start, 0, 0, 0);
for k = 1:params.max_iter
% 随机采样（含目标偏向）
q_rand = randomSample(goal, params.goal_bias);
% 最近邻搜索
q_near = nearestNeighbor(tree, q_rand);
% 动力学约束扩展
q_new = extend(q_near, q_rand, params.step_size, params.vehicle);
% 碰撞检测
if ~collisionCheck(q_new, obstacles, params.safety_margin)
% 计算代价
cost = q_near.cost + norm(q_new.position - q_near.position);
% 添加到树
new_node = RRTNode(q_new.position, q_new.theta, length(tree), cost);
tree = [tree; new_node];
% 目标检查
if norm(q_new.position - goal.position) < params.goal_tolerance
path = extractPath(tree, goal);
return;
end
end
end
path = []; % 规划失败
end
4. 参数调优与性能优化
4.1 关键参数设置建议
| 参数名称 | 推荐值 | 影响分析 |
|---|---|---|
| 步长(step_size) | 1.5-2.5m | 过大导致路径粗糙，过小增加计算量 |
| 最大迭代次数 | 2000-5000 | 保证95%场景能找到路径 |
| 目标偏向(bias) | 0.05-0.1 | 提高收敛速度但可能陷入局部最优 |
| 安全距离 | 0.3-0.5m | 平衡安全性与路径可行性 |
4.2 计算效率优化技巧
-
KD-Tree加速最近邻搜索：
matlab复制function idx = nearestNeighbor(tree, q) [~,idx] = pdist2([tree.position], q', 'euclidean', 'Smallest',1); end -
并行碰撞检测：
matlab复制function collision = collisionCheck(q, obstacles) collision = false; parfor i = 1:length(obstacles) if checkSingleObstacle(q, obstacles(i)) collision = true; break; end end end -
自适应步长调整：
matlab复制function step = adaptiveStepSize(q_near, q_rand, min_step, max_step) dist = norm(q_rand - q_near.position); step = min(max_step, max(min_step, 0.2*dist)); end
5. 实际应用案例
5.1 停车场自动泊车场景
matlab复制% 场景设置
start = [2; 2; 0]; % [x,y,theta]
goal = [18; 18; pi/2];
obstacles = createParkingLotObstacles();
% 参数配置
params.max_iter = 3000;
params.step_size = 1.8;
params.vehicle.wheelbase = 2.7;
params.vehicle.max_steer = 0.6; % ~35度
% 路径规划
[path, ~] = RRT_Planner(start, goal, obstacles, params);
% 可视化
plotScenario(start, goal, obstacles);
plotPath(path);
5.2 城市道路避障场景
考虑更复杂的道路边界和随机障碍物：
matlab复制function obstacles = createUrbanScenario()
% 道路边界
road_width = 3.5;
obstacles(1) = createRectObstacle([0;0], [100;0], road_width/2);
obstacles(2) = createRectObstacle([0;road_width*2], [100;road_width*2], road_width/2);
% 随机车辆障碍
for i = 1:5
pos = [20+15*i; road_width + randn()*0.3];
obstacles(end+1) = createCarObstacle(pos, 4.8, 1.8);
end
end
6. 常见问题与解决方案
6.1 规划失败分析
| 问题现象 | 可能原因 | 解决方案 |
|---|---|---|
| 无法到达目标区域 | 步长过大/迭代次数不足 | 增加max_iter，减小step_size |
| 路径过于曲折 | 缺少路径平滑处理 | 添加B样条后处理 |
| 碰撞误检测 | 安全距离设置过大 | 调整safety_margin至0.3-0.4m |
6.2 实时性优化技巧
- 增量式规划：在车辆移动过程中持续优化路径，而非每次都重新规划
- 多分辨率搜索：先粗后精的两阶段规划策略
- 热点区域预测：根据车辆运动趋势优先采样可能区域
6.3 车辆跟踪问题
当生成路径曲率变化剧烈时，可能导致跟踪误差增大。解决方法：
- 增加曲率约束：
matlab复制function feasible = checkCurvature(q1, q2, max_curvature) delta_theta = abs(wrapToPi(q2.theta - q1.theta)); dist = norm(q2.position - q1.position); curvature = delta_theta / max(dist, 0.1); feasible = curvature <= max_curvature; end - 速度规划：根据曲率限制速度
code复制v_max = sqrt(a_max * R_min)
7. 算法扩展与改进方向
7.1 RRT*优化版本
通过引入代价函数和重布线机制，获得渐进最优路径：
matlab复制function tree = rewire(tree, q_new, radius, obstacles)
near_nodes = findNearNodes(tree, q_new, radius);
for q_near = near_nodes
new_cost = q_new.cost + norm(q_new.position - q_near.position);
if new_cost < q_near.cost && ~collisionCheck(q_new, q_near, obstacles)
q_near.parent = length(tree);
q_near.cost = new_cost;
end
end
end
7.2 动态障碍物处理
结合预测算法处理移动障碍物：
- 使用卡尔曼滤波预测障碍物轨迹
- 在配置空间中引入时间维度(x,y,θ,t)
- 扩展碰撞检测函数考虑时间因素
7.3 多车协同规划
通过优先级机制解决多车路径冲突：
- 为每辆车分配优先级
- 高优先级车辆的路径作为低优先级车辆的动态障碍
- 使用协商机制解决死锁情况
实际工程中发现，在复杂场景下RRT算法的随机性可能导致规划结果不稳定。一个实用的解决方案是运行算法多次(3-5次)选择最优路径，虽然增加了计算时间，但显著提高了规划可靠性。