北陌大叔1. 无人车轨迹规划的核心挑战与解决思路
第一次接触无人车轨迹规划时，我被这个看似简单实则复杂的问题深深吸引。想象一下，当你在城市道路上驾驶时，每秒钟都在做无数个微决策：加速、减速、变道、避让...而无人车需要将这些人类本能转化为精确的数学计算。轨迹规划正是解决这个问题的核心技术之一。
在Matlab环境下实现基于代价函数的最优轨迹探索，本质上是在解决一个多目标优化问题。我们需要在安全性、舒适性、效率等多个维度中找到平衡点。这就像在玩一个高维度的俄罗斯方块游戏，每个决策都会影响后续的一系列动作。
关键提示：轨迹规划不同于路径规划。路径规划解决的是"从A到B走哪条路"的问题，而轨迹规划解决的是"具体怎么走"的问题，包括速度、加速度、转向等运动参数。
需要模型API调用？ 免费领10W Token，多模型网关一键接入 Claude、DeepSeek 等主流模型。
2. 代价函数：轨迹优化的指挥棒
2.1 代价函数的组成要素
一个完整的轨迹代价函数通常包含以下几个核心组成部分：
- 安全性代价：确保车辆不与障碍物碰撞
- 舒适性代价：减少急加速、急刹车和急转弯
- 效率代价：尽可能缩短行驶时间或距离
- 规则代价：遵守交通规则（如车道保持、限速等）
- 可行性代价：确保轨迹符合车辆动力学约束
在Matlab中，我们可以这样构建一个基础的代价函数框架：
matlab复制function total_cost = trajectoryCost(trajectory, obstacles)
% 安全性代价
safety_cost = calculateSafetyCost(trajectory, obstacles);
% 舒适性代价
comfort_cost = calculateComfortCost(trajectory);
% 效率代价
efficiency_cost = calculateEfficiencyCost(trajectory);
% 加权求和
total_cost = w1*safety_cost + w2*comfort_cost + w3*efficiency_cost;
end
2.2 权重分配的技巧
权重分配是代价函数设计中最具挑战性的部分。经过多次实践，我总结出以下经验：
- 安全性权重(w1)通常设为最大（如0.5-0.7）
- 舒适性和效率权重可根据场景动态调整
- 城市道路场景应提高舒适性权重
- 高速公路场景可适当提高效率权重
实际项目中，我通常会先用均等权重进行测试，然后根据仿真结果逐步调整。记住，没有"完美"的权重组合，只有"适合当前场景"的最佳组合。
3. Matlab实现的关键技术点
3.1 轨迹表示方法
在Matlab中，我们常用多项式曲线表示轨迹。五阶多项式是最常见的选择，因为它可以满足位置、速度和加速度的连续性要求：
matlab复制% 五阶多项式轨迹生成
function trajectory = generateQuinticTrajectory(start_state, end_state, T)
% start_state = [x0, v0, a0]
% end_state = [xf, vf, af]
% T = 轨迹时间
A = [1, 0, 0, 0, 0, 0;
0, 1, 0, 0, 0, 0;
0, 0, 2, 0, 0, 0;
1, T, T^2, T^3, T^4, T^5;
0, 1, 2*T, 3*T^2, 4*T^3, 5*T^4;
0, 0, 2, 6*T, 12*T^2, 20*T^3];
b = [start_state'; end_state'];
coeffs = A\b;
trajectory = @(t) [polyval(coeffs(6:-1:1), t);
polyval([5*coeffs(6), 4*coeffs(5), 3*coeffs(4), 2*coeffs(3), coeffs(2)], t);
polyval([20*coeffs(6), 12*coeffs(5), 6*coeffs(4), 2*coeffs(3)], t)];
end
3.2 最优轨迹搜索算法
常用的搜索算法包括：
- A*算法：适合离散状态空间的搜索
- RRT算法：适合高维空间的随机采样
- 梯度下降法：适合连续空间的优化
在Matlab中实现A*算法的核心代码如下：
matlab复制function [path, cost] = astar(start, goal, costmap)
% 初始化开放集和关闭集
openSet = start;
cameFrom = containers.Map();
gScore = containers.Map(num2str(start), 0);
fScore = containers.Map(num2str(start), heuristic(start, goal));
while ~isempty(openSet)
% 找到fScore最小的节点
[~, currentIdx] = min(cell2mat(values(fScore)));
current = openSet(currentIdx);
if isequal(current, goal)
path = reconstructPath(cameFrom, current);
cost = gScore(num2str(current));
return;
end
% 从开放集中移除当前节点
openSet(currentIdx) = [];
remove(fScore, num2str(current));
% 遍历邻居节点
neighbors = getNeighbors(current, costmap);
for i = 1:size(neighbors,1)
neighbor = neighbors(i,:);
tentative_gScore = gScore(num2str(current)) + ...
movementCost(current, neighbor);
if ~isKey(gScore, num2str(neighbor)) || ...
tentative_gScore < gScore(num2str(neighbor))
cameFrom(num2str(neighbor)) = current;
gScore(num2str(neighbor)) = tentative_gScore;
fScore(num2str(neighbor)) = tentative_gScore + ...
heuristic(neighbor, goal);
if ~ismember(neighbor, openSet, 'rows')
openSet = [openSet; neighbor];
end
end
end
end
error('Path not found');
end
4. 实际应用中的问题与解决方案
4.1 局部最优陷阱
在复杂环境中，算法容易陷入局部最优。我常用的解决方案包括：
- 增加随机扰动：以一定概率接受次优解
- 多起点搜索：从不同初始点开始搜索
- 混合算法：结合A*和RRT的优势
4.2 实时性挑战
Matlab虽然开发效率高，但在实时性要求高的场景可能表现不佳。优化建议：
- 使用Mex文件加速关键代码
- 预计算常见场景的轨迹库
- 采用分层规划策略
4.3 车辆动力学约束
简单的几何规划可能忽略车辆的实际运动能力。解决方法：
- 在代价函数中加入曲率约束
- 使用车辆动力学模型验证轨迹可行性
- 考虑执行器延迟和响应时间
5. 完整实现案例
下面展示一个完整的Matlab轨迹规划实现框架：
matlab复制classdef TrajectoryPlanner < handle
properties
cost_weights = [0.6, 0.2, 0.2]; % 安全、舒适、效率
max_speed = 10; % m/s
max_accel = 2; % m/s^2
vehicle_width = 2; % 米
end
methods
function [best_traj, min_cost] = planTrajectory(obj, start, goal, obstacles)
% 生成候选轨迹
candidate_trajs = obj.generateCandidates(start, goal);
% 评估每条轨迹
costs = zeros(1, length(candidate_trajs));
for i = 1:length(candidate_trajs)
costs(i) = obj.evaluateTrajectory(candidate_trajs{i}, obstacles);
end
% 选择最优轨迹
[min_cost, idx] = min(costs);
best_traj = candidate_trajs{idx};
end
function trajs = generateCandidates(obj, start, goal)
% 实际项目中这里会有更复杂的轨迹生成逻辑
trajs = cell(1, 5);
for i = 1:5
T = 2 + i*0.5; % 不同的时间预算
trajs{i} = generateQuinticTrajectory(start, goal, T);
end
end
function cost = evaluateTrajectory(obj, traj, obstacles)
% 安全性评估
safety_cost = obj.calculateSafety(traj, obstacles);
% 舒适性评估
comfort_cost = obj.calculateComfort(traj);
% 效率评估
efficiency_cost = obj.calculateEfficiency(traj);
% 加权求和
cost = obj.cost_weights * [safety_cost; comfort_cost; efficiency_cost];
end
function sc = calculateSafety(obj, traj, obstacles)
% 简化的碰撞检测
min_dist = inf;
for t = 0:0.1:traj.T
state = traj(t);
for j = 1:size(obstacles,1)
dist = norm(state(1:2) - obstacles(j,:));
if dist < min_dist
min_dist = dist;
end
end
end
sc = exp(-min_dist/obj.vehicle_width);
end
function cc = calculateComfort(obj, traj)
% 计算加速度的均方根
accels = [];
for t = 0:0.1:traj.T
state = traj(t);
accels = [accels, norm(state(4:5))];
end
cc = rms(accels)/obj.max_accel;
end
function ec = calculateEfficiency(obj, traj)
% 基于轨迹时间的效率评估
ec = traj.T / 10; % 归一化
end
end
end
6. 进阶技巧与性能优化
6.1 并行计算加速
Matlab的并行计算工具箱可以显著提升轨迹评估速度：
matlab复制% 启用并行池
if isempty(gcp('nocreate'))
parpool;
end
% 并行评估轨迹
parfor i = 1:length(candidate_trajs)
costs(i) = evaluateTrajectory(candidate_trajs{i}, obstacles);
end
6.2 轨迹库预计算
对于固定场景，可以预先计算常见情况的轨迹库：
- 离线生成各种起点-终点组合的轨迹
- 存储最优轨迹及其代价
- 在线查询时优先检查轨迹库
6.3 混合整数规划
对于离散-连续混合问题，可以使用Matlab的intlinprog：
matlab复制% 定义混合整数规划问题
f = [1; 1; 1]; % 目标函数
intcon = [1, 2]; % 整数变量
A = [1, 1, 0; 0, 1, 1]; % 不等式约束
b = [1; 1];
lb = [0; 0; 0]; % 下界
ub = [1; 1; 1]; % 上界
[x, fval] = intlinprog(f, intcon, A, b, [], [], lb, ub);
7. 实际项目中的经验分享
在多个无人车项目中，我积累了一些宝贵的经验教训：
-
不要过度优化代价函数：在仿真中表现完美的代价函数，在实际环境中可能因为传感器噪声而失效。保持一定的鲁棒性更重要。
-
考虑感知不确定性：轨迹规划应该考虑感知模块的置信度。对于低置信度区域，应该自动增加安全裕度。
-
实时可视化至关重要：在Matlab中开发时，要建立实时可视化工具，直观显示轨迹和各项代价的变化。
-
测试极端场景：特别要测试"无解"场景，确保系统有合理的降级策略，而不是直接崩溃。
-
与其他模块的接口设计：提前定义好与感知、控制模块的接口规范，避免后期集成时的兼容性问题。
一个实用的调试技巧：在代价函数评估时，记录每个子代价的详细数值。当出现不合理轨迹时，可以快速定位是哪个子代价出了问题。