1. 项目背景与核心目标
在自动驾驶技术快速发展的今天,路径规划算法作为车辆自主导航的核心组件,其可靠性和效率直接影响着整个系统的性能表现。Autoware作为目前最成熟的开源自动驾驶软件框架之一,其内置的路径规划模块采用了基于障碍物几何边界的经典方法,这种方案在复杂城市环境中表现尤为出色。
我最近花了三周时间,用MATLAB完整复现了Autoware中的这一算法。选择MATLAB而非直接使用Autoware的C++实现,主要出于三个考虑:首先,MATLAB强大的矩阵运算和可视化能力,特别适合算法原型验证;其次,通过MATLAB实现可以更清晰地展示算法核心逻辑,避免工程细节的干扰;最后,这也是一个深入理解自动驾驶路径规划底层原理的绝佳机会。
这个项目的核心目标有三个层次:第一,准确还原Autoware路径规划算法处理障碍物几何边界的关键步骤;第二,建立完整的MATLAB仿真验证环境;第三,通过参数调整和场景测试,深入分析算法在不同工况下的表现特性。
2. 环境准备与基础配置
2.1 MATLAB工具链选择
要实现Autoware级别的路径规划算法,需要确保MATLAB环境具备以下工具包:
- Robotics System Toolbox(必备):提供机器人运动学和动力学计算支持
- Automated Driving Toolbox(推荐):包含自动驾驶专用函数和可视化工具
- Optimization Toolbox(必备):用于路径优化计算
- Computer Vision Toolbox(可选):辅助处理传感器数据
我使用的是MATLAB R2023a版本,这个版本对自动驾驶算法的支持已经相当完善。安装时需要注意,Automated Driving Toolbox需要额外勾选Sensor Fusion and Tracking子模块,这对后续的多障碍物处理很有帮助。
2.2 基础数据结构设计
Autoware中的路径规划涉及几个核心数据结构,在MATLAB中我们用类来模拟:
matlab复制classdef VehicleState
properties
position % [x,y]坐标
orientation % 航向角(弧度)
velocity % 当前速度
steering_angle % 前轮转角
end
end
classdef Obstacle
properties
vertices % 多边形顶点[N×2]
convex_hull % 凸包顶点[M×2]
safety_margin % 安全距离
end
methods
function obj = expand(obj, margin)
% 障碍物膨胀方法
end
end
end
这种面向对象的设计保持了与Autoware实现的结构一致性,同时利用了MATLAB的类特性。特别要注意的是障碍物的凸包表示,这是后续碰撞检测的关键。
3. 障碍物几何处理核心算法
3.1 障碍物边界提取与凸包计算
Autoware处理障碍物的第一步是将原始传感器数据(如激光雷达点云)转换为几何边界。在MATLAB中,我们可以模拟这个过程:
matlab复制function [obstacle] = processPointCloud(ptCloud)
% 点云聚类
[labels,numClusters] = pcsegdist(ptCloud, 0.5);
obstacles = [];
for i = 1:numClusters
cluster = select(ptCloud, labels == i);
xyPoints = cluster.Location(:,1:2);
% 计算alpha shape获取边界
shp = alphaShape(xyPoints, 1.5);
[~, vertices] = boundaryFacets(shp);
% 计算凸包
k = convhull(vertices);
convexVerts = vertices(k,:);
% 构建障碍物对象
obs = Obstacle();
obs.vertices = vertices;
obs.convex_hull = convexVerts;
obstacles = [obstacles; obs];
end
end
这里有几个关键参数需要注意:
- pcsegdist中的0.5是聚类距离阈值,需要根据传感器特性调整
- alphaShape的1.5参数决定了边界的光滑程度
- 实际工程中还需要考虑动态障碍物的运动状态估计
3.2 安全边界膨胀算法
为确保路径安全性,Autoware会对障碍物进行膨胀处理。MATLAB实现如下:
matlab复制function [expanded] = expandObstacle(obstacle, margin)
centroid = mean(obstacle.convex_hull);
expandedVerts = zeros(size(obstacle.convex_hull));
for i = 1:size(obstacle.convex_hull,1)
vec = obstacle.convex_hull(i,:) - centroid;
norm_vec = vec/norm(vec);
expandedVerts(i,:) = obstacle.convex_hull(i,:) + norm_vec * margin;
end
expanded = obstacle;
expanded.convex_hull = expandedVerts;
end
这个膨胀算法看似简单,但有三个工程细节需要注意:
- 膨胀方向应沿顶点到质心的方向,而不是简单的法线方向
- 对于凹多边形需要先进行凸分解
- 动态障碍物的膨胀量应考虑速度因素
提示:实际应用中,建议对不同类型障碍物使用不同的安全距离。例如行人建议1.5米,而静态车辆0.8米即可。
4. 路径搜索与优化实现
4.1 状态网格离散化方法
Autoware采用基于网格的搜索策略,在MATLAB中可以这样实现:
matlab复制function [grid] = createStateGrid(start, goal, bounds, resolution)
% 创建三维状态空间(x,y,theta)
x = bounds(1,1):resolution:bounds(1,2);
y = bounds(2,1):resolution:bounds(2,2);
theta = -pi:pi/8:pi;
[X,Y,TH] = meshgrid(x,y,theta);
grid = struct();
grid.X = X; grid.Y = Y; grid.TH = TH;
grid.cost = inf(size(X));
grid.parent = zeros(size(X));
grid.visited = false(size(X));
% 初始化起点
[~, startIdx] = min(abs(X(:)-start(1)) + abs(Y(:)-start(2)) + abs(TH(:)-start(3)));
grid.cost(startIdx) = 0;
end
这里的状态分辨率选择很有讲究:
- xy平面分辨率通常取车辆宽度的一半(约1米)
- 角度分辨率π/8(22.5°)已经足够
- 实际工程中可以采用多分辨率网格提高效率
4.2 基于A*的路径搜索
在离散状态空间上实现A*搜索:
matlab复制function [path] = astarSearch(grid, goal, obstacles)
% 启发式函数 - 考虑非完整约束
heuristic = @(x,y,th) 0.5*norm([x,y]-goal(1:2)) + ...
0.5*abs(wrapToPi(atan2(goal(2)-y,goal(1)-x) - th));
% 运动基元(前向、左转、右转)
motionPrimitives = [1 0 0; 0.8 0 pi/8; 0.8 0 -pi/8];
while ~isempty(find(~grid.visited, 1))
[~, curr] = min(grid.cost(:) + heuristic(grid.X(:),grid.Y(:),grid.TH(:)));
if norm([grid.X(curr),grid.Y(curr)] - goal(1:2)) < 0.5
path = reconstructPath(grid, curr);
return;
end
grid.visited(curr) = true;
% 扩展邻居节点
for m = 1:size(motionPrimitives,1)
newX = grid.X(curr) + motionPrimitives(m,1)*cos(grid.TH(curr));
newY = grid.Y(curr) + motionPrimitives(m,1)*sin(grid.TH(curr));
newTH = grid.TH(curr) + motionPrimitives(m,3);
% 碰撞检测
if checkCollision([newX,newY], obstacles)
continue;
end
% 更新节点
[~, idx] = min(abs(grid.X(:)-newX) + abs(grid.Y(:)-newY) + abs(grid.TH(:)-newTH));
newCost = grid.cost(curr) + motionPrimitives(m,1);
if newCost < grid.cost(idx)
grid.cost(idx) = newCost;
grid.parent(idx) = curr;
end
end
end
path = [];
end
这个实现有几个关键改进点:
- 混合启发式函数同时考虑距离和方向
- 使用车辆运动学约束的运动基元
- 增量式碰撞检测避免重复计算
5. 路径平滑与优化
5.1 基于B样条的路径平滑
Autoware采用B样条对初始路径进行平滑:
matlab复制function [smoothPath] = bsplineSmooth(path, obstacles)
% 提取路径点
points = [path.X, path.Y];
% 创建B样条对象
degree = 3;
knots = aptknt(linspace(0,1,size(points,1)), degree);
sp = spmak(knots, points');
% 优化控制点
options = optimoptions('fmincon','Display','off');
ctrlPts = fmincon(@(x) objectiveFunc(x,knots,degree,points), ...
fnbrk(sp,'coefs'),[],[],[],[],[],[],...
@(x) nonlcon(x,knots,degree,obstacles),options);
% 重建平滑路径
smoothSp = spmak(knots, ctrlPts);
smoothPath = fnval(smoothSp, linspace(0,1,100))';
end
function f = objectiveFunc(ctrlPts,knots,degree,points)
sp = spmak(knots, ctrlPts);
estPoints = fnval(sp, linspace(0,1,size(points,1)))';
f = sum(vecnorm(estPoints - points,2,2));
end
function [c,ceq] = nonlcon(ctrlPts,knots,degree,obstacles)
sp = spmak(knots, ctrlPts);
testPoints = fnval(sp, linspace(0,1,50))';
c = zeros(size(obstacles));
for i = 1:length(obstacles)
dists = p_poly_dist(testPoints(:,1), testPoints(:,2), ...
obstacles(i).convex_hull(:,1), obstacles(i).convex_hull(:,2));
c(i) = min(dists) - obstacles(i).safety_margin;
end
ceq = [];
end
这个平滑算法有三个工程考量:
- 使用三阶B样条平衡平滑性和计算复杂度
- 目标函数保持路径形状不变
- 非线性约束确保避障安全
5.2 速度剖面生成
最后还需要为平滑路径生成速度曲线:
matlab复制function [speedProfile] = generateSpeedProfile(path, maxSpeed, maxAccel)
curvatures = zeros(size(path,1)-2,1);
for i = 2:size(path,1)-1
v1 = path(i,:) - path(i-1,:);
v2 = path(i+1,:) - path(i,:);
curvatures(i-1) = abs(atan2(v1(1)*v2(2)-v1(2)*v2(1), v1(1)*v2(1)+v1(2)*v2(2)));
end
speedLimits = maxSpeed./(1 + 5*abs(curvatures));
speedProfile = zeros(size(path,1),1);
speedProfile(1) = 0;
for i = 2:size(path,1)
dist = norm(path(i,:) - path(i-1,:));
desiredSpeed = min([speedLimits(min(i-1,end)), ...
speedProfile(i-1) + maxAccel*dist]);
speedProfile(i) = desiredSpeed;
end
end
速度曲线生成的关键点:
- 基于曲率的速度限制保证舒适性
- 加速度约束确保可行性
- 前向积分避免速度突变
6. 完整仿真验证与调试
6.1 典型测试场景构建
为验证算法有效性,我设计了五种典型场景:
- 静态障碍物绕行
- 狭窄通道通过
- 多障碍物穿行
- 停车场泊车
- 动态障碍物避让
以停车场场景为例,设置代码如下:
matlab复制% 创建停车场场景
bounds = [-10 50; -20 20]; % x和y范围
start = [0, 0, pi/2]; % 起点位姿
goal = [40, -15, -pi/2]; % 目标位姿
% 创建障碍物(停车位和车辆)
obstacles = [];
for i = 0:4
% 停车位线
obs = Obstacle();
obs.vertices = [30, 5+i*5; 35,5+i*5; 35,7+i*5; 30,7+i*5];
obs.convex_hull = obs.vertices;
obstacles = [obstacles; obs];
% 随机停放的车辆
if rand > 0.5
car = Obstacle();
posX = 32 + rand*3;
posY = 5.5 + i*5;
car.vertices = [posX-1,posY-1.5; posX+1,posY-1.5;
posX+1,posY+1.5; posX-1,posY+1.5];
car.convex_hull = car.vertices;
obstacles = [obstacles; car];
end
end
6.2 可视化与性能分析
MATLAB的强大可视化能力可以帮助我们深入分析算法表现:
matlab复制function visualizeScenario(path, smoothPath, speedProfile, obstacles)
figure('Position',[100 100 1200 500])
% 路径和障碍物
subplot(1,2,1)
hold on
for i = 1:length(obstacles)
fill(obstacles(i).convex_hull(:,1), obstacles(i).convex_hull(:,2),'r')
end
plot(path(:,1), path(:,2), 'b--', 'LineWidth',1.5)
plot(smoothPath(:,1), smoothPath(:,2), 'g-', 'LineWidth',2)
axis equal
legend('障碍物','原始路径','平滑路径')
% 速度曲线
subplot(1,2,2)
plot(speedProfile, 'LineWidth',2)
xlabel('路径点')
ylabel('速度(m/s)')
title('速度剖面')
grid on
end
通过可视化可以直观评估:
- 路径的平滑性和安全性
- 速度曲线的合理性
- 算法计算效率
注意:在实际调试中发现,当障碍物密度较高时,需要调整A*启发式函数的权重,否则可能陷入局部最优。建议在复杂场景中将距离和方向的权重比调整为7:3。
7. 工程实践中的挑战与解决方案
在复现过程中遇到了几个典型的工程问题,这里分享我的解决方案:
问题1:狭窄通道中的振荡路径
当车辆需要通过狭窄通道时,原始算法会产生前后振荡的路径。这是因为A*的离散搜索和B样条平滑之间存在矛盾。
解决方案:
- 在A*搜索时增加方向保持代价
- 修改B样条目标函数,加入曲率约束
- 最终采用了两阶段平滑策略:先用二次规划做粗平滑,再用B样条精调
问题2:动态障碍物预测不准确
Autoware原本集成了完善的预测模块,但在简化版MATLAB实现中需要替代方案。
解决方案:
- 对动态障碍物建立恒定速度模型
- 在膨胀障碍物时考虑速度方向
- 引入时间维度的安全校验
matlab复制function [expanded] = dynamicExpand(obstacle, velocity, dt)
% 基于速度的预测膨胀
predictedPos = obstacle.convex_hull + velocity*dt;
expanded = convexHull([obstacle.convex_hull; predictedPos]);
expanded.safety_margin = obstacle.safety_margin * (1 + norm(velocity)/10);
end
问题3:实时性挑战
MATLAB的纯脚本实现效率较低,难以满足实时要求。
优化措施:
- 将碰撞检测函数转为MEX文件
- 使用并行计算处理多障碍物场景
- 实现增量式路径更新算法
matlab复制% 并行碰撞检测示例
parfor i = 1:length(obstacles)
collisionFlags(i) = checkCollision(path, obstacles(i));
end
经过这些优化后,算法在i7-11800H处理器上单次规划时间从最初的1.2秒降低到了0.15秒,基本满足实时性要求。
