1. 运动规划在自动驾驶中的核心定位
运动规划模块在自动驾驶系统中扮演着至关重要的角色。作为决策规划三层架构中的最后一环,它需要将上层的行为决策指令转化为可执行的车辆控制命令。这个转化过程需要考虑诸多现实约束条件,包括但不限于车辆动力学限制、道路几何特性以及实时交通环境。
从工程实现角度来看,运动规划需要解决两个核心问题:路径搜索和轨迹生成。路径搜索负责在复杂环境中找到一条从起点到目标点的可行路径,而轨迹生成则需要将这条路径转化为满足车辆动力学约束的平滑轨迹。这两个问题看似独立,实则紧密相关,共同构成了运动规划的整体解决方案。
需要模型API调用? 免费领10W Token,多模型网关一键接入 Claude、DeepSeek 等主流模型。
2. RRT*算法实现详解
2.1 算法原理回顾
RRT*(Rapidly-exploring Random Tree Star)算法是RRT算法的优化版本,通过渐进最优的特性,能够在保证计算效率的同时,逐步优化路径质量。与基础RRT相比,RRT*引入了重布线(rewiring)机制,这使得算法能够不断优化已有路径,最终收敛到最优解。
算法核心流程包括:
- 随机采样:在配置空间中随机生成采样点
- 最近邻搜索:在现有树结构中找到距离采样点最近的节点
- 新节点生成:在采样点方向以固定步长生成新节点
- 碰撞检测:验证新节点到最近邻节点的路径是否可行
- 近邻优化:在新节点附近寻找更优的父节点
- 重布线:优化树结构,确保路径最优性
2.2 MATLAB代码实现
matlab复制classdef RRTStar
properties
startNode
goalNode
nodes
obstacles
stepSize
searchRadius
maxIterations
end
methods
function obj = RRTStar(start, goal, obstacles, step, radius, maxIter)
obj.startNode = start;
obj.goalNode = goal;
obj.obstacles = obstacles;
obj.stepSize = step;
obj.searchRadius = radius;
obj.maxIterations = maxIter;
obj.nodes = [start];
end
function [path, nodes] = plan(obj)
for iter = 1:obj.maxIterations
randPoint = obj.generateRandomPoint();
nearestNode = obj.findNearestNode(randPoint);
newNode = obj.steer(nearestNode, randPoint);
if obj.checkCollision(nearestNode, newNode)
nearNodes = obj.findNearNodes(newNode);
[newNode, minCost] = obj.chooseParent(nearNodes, newNode);
obj.nodes = [obj.nodes; newNode];
obj.rewire(nearNodes, newNode, minCost);
if norm(newNode(1:2)-obj.goalNode(1:2)) < obj.stepSize
if obj.checkCollision(newNode, obj.goalNode)
path = obj.extractPath(newNode);
return;
end
end
end
end
path = [];
end
% 其他辅助方法实现...
end
end
2.3 工程实现关键点
2.3.1 参数调优经验
在实际工程应用中,以下几个参数对算法性能影响显著:
- 步长(stepSize):通常设置为车辆最小转弯半径的1.5-2倍
- 搜索半径(searchRadius):建议初始值为步长的3-5倍
- 最大迭代次数(maxIterations):根据场景复杂度调整,城市道路建议5000-10000次
提示:在MATLAB实现中,建议将碰撞检测函数向量化处理,可以显著提升计算效率。对于复杂场景,可以考虑使用KD-tree来加速最近邻搜索。
2.3.2 车辆动力学约束处理
为了确保生成的路径符合车辆运动特性,需要在以下几个环节加入约束:
- 转向约束:限制相邻节点间的最大转向角度
- 曲率约束:确保路径曲率不超过车辆最大允许值
- 速度约束:根据路径曲率动态调整速度上限
matlab复制function feasible = checkDynamicConstraints(node1, node2, maxSteering)
deltaTheta = abs(node2(3) - node1(3));
distance = norm(node2(1:2)-node1(1:2));
curvature = 2*sin(deltaTheta/2)/distance;
maxCurvature = tan(maxSteering)/wheelbase;
feasible = curvature <= maxCurvature;
end
3. 五次多项式轨迹生成
3.1 算法数学基础
五次多项式轨迹生成的核心思想是利用五次多项式来描述车辆的运动状态。选择五次多项式的原因在于它能够同时满足位置、速度、加速度三个维度的边界条件约束。
对于横向运动(以换道场景为例),轨迹可以表示为:
y(t) = a₅t⁵ + a₄t⁴ + a₃t³ + a₂t² + a₁t + a₀
对应的速度和加速度分别为:
ẏ(t) = 5a₅t⁴ + 4a₄t³ + 3a₃t² + 2a₂t + a₁
ÿ(t) = 20a₅t³ + 12a₄t² + 6a₃t + 2a₂
3.2 MATLAB实现代码
matlab复制function [traj, coeff] = quinticPolynomialTrajectory(start, goal, T)
% start/goal: [y, dy, ddy] 初始和终止状态
% T: 轨迹时间
A = [0, 0, 0, 0, 0, 1;
T^5, T^4, T^3, T^2, T, 1;
0, 0, 0, 0, 1, 0;
5*T^4, 4*T^3, 3*T^2,2*T, 1, 0;
0, 0, 0, 2, 0, 0;
20*T^3,12*T^2,6*T, 2, 0, 0];
b = [start(1); goal(1); start(2); goal(2); start(3); goal(3)];
coeff = A\b;
t = linspace(0,T,100);
traj.y = polyval(coeff, t);
traj.dy = polyval([5*coeff(1) 4*coeff(2) 3*coeff(3) 2*coeff(4) coeff(5)], t);
traj.ddy = polyval([20*coeff(1) 12*coeff(2) 6*coeff(3) 2*coeff(4)], t);
traj.t = t;
end
3.3 工程适配要点
3.3.1 舒适性优化技巧
在实际应用中,除了满足基本的动力学约束外,还需要考虑乘员舒适性。以下是几个实用技巧:
- 加加速度限制:确保加速度变化率不超过2 m/s³
- 轨迹时间调整:根据距离动态调整轨迹时间T
- 权重优化:对不同维度的状态量进行加权处理
matlab复制function T = optimizeTrajectoryTime(start, goal, maxAccel, maxJerk)
% 基于最大允许加速度和加加速度估算最优时间
deltaY = abs(goal(1) - start(1));
T_accel = sqrt(deltaY / maxAccel);
T_jerk = (deltaY / maxJerk)^(1/3);
T = max([T_accel, T_jerk, 3.0]); % 不低于3秒
end
3.3.2 多轨迹评估策略
在实际系统中,通常会生成多条候选轨迹,然后基于以下指标进行评估:
- 安全性:与障碍物的最小距离
- 舒适性:加速度和加加速度的积分
- 效率:到达目标点的时间
- 符合度:与全局路径的偏差
matlab复制function bestTraj = evaluateTrajectories(trajList, refPath, obstacles)
costs = zeros(length(trajList),1);
for i = 1:length(trajList)
% 安全性成本
minDist = minDistanceToObstacles(trajList(i), obstacles);
safetyCost = 1/minDist^2;
% 舒适性成本
jerkCost = trapz(trajList(i).t, trajList(i).ddy.^2);
% 符合度成本
devCost = mean(distanceToRefPath(trajList(i), refPath));
% 综合成本
costs(i) = 0.5*safetyCost + 0.3*jerkCost + 0.2*devCost;
end
[~, idx] = min(costs);
bestTraj = trajList(idx);
end
4. 运动规划系统集成
4.1 与上下游模块的接口设计
在实际自动驾驶系统中,运动规划模块需要与多个上下游模块进行交互。典型的接口包括:
-
输入接口:
- 全局路径(来自路径规划模块)
- 行为决策指令(如换道、跟车等)
- 环境感知信息(障碍物列表、交通规则等)
-
输出接口:
- 轨迹点序列(包含位置、速度、加速度等信息)
- 轨迹可行性标志位
- 紧急制动建议
matlab复制classdef MotionPlanner
properties
refPath
obstacles
vehicleState
config
end
methods
function traj = plan(obj, decision)
% 根据决策类型选择规划策略
switch decision.type
case 'LANE_CHANGE'
traj = planLaneChange(obj);
case 'FOLLOW'
traj = planFollowing(obj);
case 'EMERGENCY'
traj = planEmergencyStop(obj);
otherwise
error('Unknown decision type');
end
% 后处理验证
traj = validateTrajectory(obj, traj);
end
function traj = planLaneChange(obj)
% 使用RRT*生成初始路径
rrt = RRTStar(obj.vehicleState, obj.config.lcGoal, obj.obstacles, ...);
path = rrt.plan();
% 使用五次多项式生成平滑轨迹
startState = [0, obj.vehicleState.v, 0]; % 横向状态
goalState = [3.75, obj.vehicleState.v, 0]; % 目标车道中心
T = optimizeTrajectoryTime(startState, goalState, ...);
[traj, ~] = quinticPolynomialTrajectory(startState, goalState, T);
end
end
end
4.2 实时性优化技巧
为了满足自动驾驶系统实时性要求(通常需要100ms内完成规划),可以采用以下优化方法:
- 增量式规划:在上一周期规划结果基础上进行局部调整
- 并行计算:同时生成多条候选轨迹
- 简化模型:在紧急情况下使用简化动力学模型
- 预计算:对常见场景进行离线计算和缓存
matlab复制function traj = incrementalPlanning(obj, lastTraj, newObstacles)
% 检查上一周期轨迹是否仍然可行
if validateTrajectory(lastTraj, newObstacles)
traj = lastTraj;
return;
end
% 局部调整
localStart = lastTraj.points(10); % 跳过前几个点
localGoal = lastTraj.points(end);
localObstacles = getRelevantObstacles(newObstacles, localStart);
% 使用简化RRT进行快速重规划
simpleRRT = SimpleRRT(localStart, localGoal, localObstacles);
adjustedPath = simpleRRT.plan();
% 拼接轨迹
traj = combineTrajectories(lastTraj.points(1:9), adjustedPath);
end
5. 实际工程中的挑战与解决方案
5.1 复杂场景处理
在城市道路环境中,运动规划面临诸多复杂场景,如:
- 狭窄路段会车
- 无保护左转
- 施工区域绕行
- 行人密集区
针对这些场景,可以采用分层规划策略:
- 高层行为规划:确定通过策略(如等待、绕行)
- 中层运动规划:生成粗粒度轨迹
- 底层轨迹优化:进行细粒度调整
5.2 不确定性处理
实际环境中存在各种不确定性因素,包括:
- 传感器噪声
- 障碍物行为预测误差
- 定位漂移
应对策略包括:
- 鲁棒规划:生成多条备选轨迹
- 安全边际:在障碍物周围设置缓冲区域
- 反应式调整:根据实时感知动态调整轨迹
matlab复制function traj = robustPlanning(obj, predictedObstacles)
% 生成主轨迹
mainTraj = obj.plan();
% 生成应急轨迹
emergencyTraj = obj.generateEmergencyTrajectory();
% 评估风险
risk = evaluateCollisionRisk(mainTraj, predictedObstacles);
if risk > obj.config.riskThreshold
traj = blendTrajectories(mainTraj, emergencyTraj, risk);
else
traj = mainTraj;
end
end
5.3 量产工程化考量
将算法从实验室迁移到量产车时,需要考虑:
- 计算资源限制:优化算法以适应车载ECU
- 功能安全要求:符合ISO 26262标准
- 诊断与监控:实时检测规划模块异常
- 参数标定:提供可配置的参数接口
在MATLAB实现中,可以通过以下方式提高代码的工程适用性:
- 固定点运算:使用fi工具箱处理定点数转换
- 代码生成:使用MATLAB Coder生成C++代码
- 内存预分配:避免动态内存分配
- 边界检查:添加完善的输入验证
matlab复制function traj = safeTrajectoryPlanning(obj)
try
traj = obj.plan();
catch ME
logError(ME);
traj = obj.generateSafeStopTrajectory();
end
% 验证输出范围
assert(all(traj.v <= obj.config.maxSpeed), 'Speed limit violated');
assert(all(abs(traj.curvature) <= obj.config.maxCurvature), ...
'Curvature limit violated');
end
6. 算法评估与测试方法
6.1 仿真测试框架
建立完善的测试框架是确保算法可靠性的关键。一个典型的测试框架包括:
- 场景生成器:创建各种测试场景
- 算法模块:运动规划实现
- 评价指标:安全性、舒适性等量化指标
- 可视化工具:直观展示规划结果
matlab复制classdef MotionPlanningTester
methods
function runTestCases(obj)
testCases = obj.generateTestCases();
results = table();
for i = 1:length(testCases)
planner = MotionPlanner(testCases(i).scenario);
traj = planner.plan();
% 评估性能指标
metrics = obj.evaluateTrajectory(traj, testCases(i));
% 记录结果
results = [results; struct2table(metrics)];
end
obj.analyzeResults(results);
end
function metrics = evaluateTrajectory(obj, traj, testCase)
metrics.safety = minDistanceToObstacles(traj, testCase.obstacles);
metrics.comfort = max(abs(traj.acceleration));
metrics.duration = traj.time(end);
metrics.success = checkGoalReached(traj, testCase.goal);
end
end
end
6.2 实车测试要点
在进行实车测试时,需要特别注意:
- 安全员干预机制:设置紧急停止按钮
- 数据记录:保存完整的测试数据供分析
- 渐进式测试:从简单场景逐步过渡到复杂场景
- 回归测试:确保新修改不会破坏已有功能
重要提示:在实车测试前,务必在仿真环境中完成充分的验证。建议采用硬件在环(HIL)测试作为中间过渡阶段。
6.3 性能指标量化
常用的运动规划性能指标包括:
- 规划成功率:在指定时间内找到可行解的比例
- 计算耗时:从接收到输入到输出结果的时间
- 轨迹质量:平滑性、安全性等指标
- 资源占用:CPU、内存使用情况
matlab复制function report = generatePerformanceReport(planner, testCases)
report = struct();
totalTime = 0;
successCount = 0;
for i = 1:length(testCases)
tic;
traj = planner.plan(testCases(i));
elapsed = toc;
totalTime = totalTime + elapsed;
if ~isempty(traj)
successCount = successCount + 1;
end
end
report.avgTime = totalTime / length(testCases);
report.successRate = successCount / length(testCases);
report.memoryUsage = memoryUsage(planner);
end
7. 前沿技术与发展趋势
7.1 基于学习的运动规划
近年来,基于深度学习的方法在运动规划领域取得了显著进展:
- 模仿学习:从人类驾驶数据中学习规划策略
- 强化学习:通过试错优化规划策略
- 生成对抗网络:生成多样化的候选轨迹
这些方法能够处理传统方法难以解决的复杂场景,但也面临数据依赖性强、可解释性差等挑战。
7.2 多智能体协同规划
在交通流中,车辆之间的协同规划可以提高整体效率:
- 博弈论方法:建模车辆间的交互行为
- 共识算法:达成群体决策
- V2X通信:共享意图和规划结果
7.3 不确定性感知规划
未来的规划算法需要更好地处理不确定性:
- 概率规划:考虑感知和预测的不确定性
- 鲁棒优化:在最坏情况下保证安全性
- 自适应规划:根据不确定性水平调整策略
matlab复制classdef LearningBasedPlanner
properties
policyNetwork
costFunction
end
methods
function traj = plan(obj, observation)
% 使用神经网络直接生成轨迹
traj = predict(obj.policyNetwork, observation);
% 使用优化方法进行后处理
traj = optimizeWithCostFunction(obj, traj, obj.costFunction);
end
end
end
8. 开发工具与资源推荐
8.1 开源框架
- Apollo Auto:百度开源的自动驾驶平台,包含完整的运动规划模块
- Autoware:面向城市自动驾驶的开源软件
- ROS2 Navigation:机器人导航栈,部分算法可适配自动驾驶
8.2 仿真环境
- CARLA:开源自动驾驶仿真平台
- LGSVL Simulator:支持多传感器仿真的开源工具
- MATLAB Automated Driving Toolbox:提供丰富的自动驾驶算法开发和测试工具
8.3 学习资源
- 《自动驾驶中的运动规划》- Steven M. LaValle
- Coursera自动驾驶专项课程
- IEEE Transactions on Intelligent Transportation Systems期刊
9. 实际项目经验分享
在开发运动规划模块时,以下几个经验教训值得注意:
-
参数敏感性测试:即使是最优的算法,参数设置不当也会导致性能急剧下降。建议建立系统的参数测试流程。
-
极端场景覆盖:那些发生概率低但后果严重的场景往往容易被忽略,需要特别关注。
-
模块化设计:将算法分解为独立的组件(如碰撞检测、轨迹评估等),便于单独优化和替换。
-
可视化调试:开发强大的可视化工具可以极大提高调试效率。建议实时显示规划过程而不仅是结果。
-
性能分析:使用profiler工具识别性能瓶颈,有针对性地进行优化。
matlab复制function optimizePlanner(planner)
% 参数敏感性分析
params = {'stepSize', 'searchRadius', 'maxIterations'};
ranges = {[0.5 2.0], [1.0 5.0], [1000 10000]};
bestParams = [];
bestScore = -inf;
% 网格搜索
for step = linspace(ranges{1}(1), ranges{1}(2), 5)
for radius = linspace(ranges{2}(1), ranges{2}(2), 5)
for iter = linspace(ranges{3}(1), ranges{3}(2), 5)
planner.config.(params{1}) = step;
planner.config.(params{2}) = radius;
planner.config.(params{3}) = iter;
score = evaluatePlanner(planner);
if score > bestScore
bestScore = score;
bestParams = [step, radius, iter];
end
end
end
end
% 应用最优参数
planner.config.(params{1}) = bestParams(1);
planner.config.(params{2}) = bestParams(2);
planner.config.(params{3}) = bestParams(3);
end
10. 从仿真到实车的过渡建议
将运动规划算法从仿真环境迁移到实车时,以下几个环节需要特别注意:
- 时钟同步:确保规划周期与车辆控制周期严格同步
- 延迟补偿:考虑感知、通信等环节的延迟影响
- 执行误差:车辆执行机构可能无法完美跟踪规划轨迹
- 传感器噪声:实际传感器数据比仿真环境噪声更大
- 计算资源:车载计算单元性能通常低于开发工作站
建议采用以下过渡策略:
- 先在软件在环(SIL)环境中验证算法逻辑
- 然后在硬件在环(HIL)环境中测试实时性能
- 接着在封闭场地进行实车测试
- 最后在开放道路进行验证
matlab复制function adaptForRealVehicle(planner)
% 增加延迟补偿
planner.config.processingDelay = 0.1; % 100ms
planner.config.executionError = 0.05; % 5%误差
% 调整规划频率
planner.config.planningRate = 10; % 10Hz
% 简化复杂计算
planner.config.useSimplifiedModel = true;
planner.config.maxTrajectories = 3; % 减少候选轨迹数量
% 增加安全校验
planner.config.enableSafetyCheck = true;
planner.config.minSafetyDistance = 1.5; % 1.5米安全距离
end
