淘房记1. 自动驾驶轨迹规划与控制框架设计
在自动驾驶系统中，轨迹规划与运动控制是两个紧密耦合的核心模块。就像人类驾驶员需要同时处理"往哪开"和"怎么开"的问题，自动驾驶系统也需要将高层决策与底层控制有机结合。本方案采用分层架构设计，上层基于动态规划算法生成安全走廊，下层通过非线性模型预测控制实现精准跟踪。
1.1 系统架构设计思路
整个系统的工作流程可以分为三个主要阶段：
-
环境感知与建模：通过传感器获取周围障碍物信息，在Frenet坐标系下建立概率化表示。与固定障碍物不同，动态障碍物需要预测其运动轨迹并转化为时空概率分布。
-
安全走廊生成：使用动态规划算法在考虑障碍物概率分布的情况下，计算从起点到终点的最小代价路径，并提取出可行区域的上下边界。
-
运动控制执行：将安全走廊作为硬约束，设计非线性模型预测控制器，实时计算最优控制输入（加速度和方向盘转角）。
关键设计原则：上层规划侧重安全性，允许一定保守性；下层控制追求精确性，需要兼顾舒适性和执行器限制。
1.2 Frenet坐标系的优势选择
传统笛卡尔坐标系在处理道路跟随问题时存在计算复杂的问题。本方案采用Frenet坐标系，将车辆运动分解为：
- 纵向运动（s方向）：沿参考路径的进展
- 横向运动（ey方向）：相对于参考路径的偏差
这种表示方法的优势在于：
- 直观反映道路几何特性
- 简化障碍物约束的表达
- 便于与人类驾驶行为类比理解
需要模型API调用？ 免费领10W Token，多模型网关一键接入 Claude、DeepSeek 等主流模型。
2. 动态规划轨迹规划实现
2.1 动态规划算法设计
动态规划(DP)特别适合处理具有随机性的动态障碍物场景。算法核心是通过递推方式求解最优子结构问题，在本应用中的具体实现如下：
matlab复制% 道路网格参数设置
max_s = 200; % 纵向距离(m)
max_ey = 3; % 最大横向偏差(m)
ds = 5; % 纵向分辨率(m)
dey = 0.5; % 横向分辨率(m)
s_grid = 0:ds:max_s;
ey_grid = -max_ey:dey:max_ey;
% 障碍物概率分布生成
obstacle_prob = zeros(length(s_grid), length(ey_grid));
obstacle_prob(15:20, 5:8) = 0.7; % 动态障碍物区域
% DP代价矩阵初始化
J = inf(size(obstacle_prob));
J(1,:) = 0; % 起点代价为0
% 递推计算
for i = 2:length(s_grid)
for j = 1:length(ey_grid)
% 考虑前一步所有可能位置
for k = max(1,j-1):min(length(ey_grid),j+1)
cost = J(i-1,k) + dey^2*(j-k)^2 + ds^2;
cost = cost + 1000*obstacle_prob(i,j); % 障碍物惩罚项
if cost < J(i,j)
J(i,j) = cost;
end
end
end
end
代价函数设计考虑了三个关键因素：
- 路径平滑性（横向变化惩罚）
- 障碍物规避（高概率区域惩罚）
- 行驶效率（距离惩罚）
2.2 安全走廊提取技术
DP算法输出的不是单一路径，而是整个可行区域的代价分布。安全走廊提取的核心是设定合理的代价阈值：
matlab复制function [upper, lower] = extract_boundary(J, threshold)
% 转换代价为概率形式
J_norm = exp(-J/max(J(:)));
upper = zeros(size(J,1),1);
lower = zeros(size(J,1),1);
for k = 1:size(J,1)
valid_region = find(J_norm(k,:) > threshold);
if ~isempty(valid_region)
upper(k) = ey_grid(max(valid_region));
lower(k) = ey_grid(min(valid_region));
else
% 无可行区域时的处理
upper(k) = ey_grid(end);
lower(k) = ey_grid(1);
end
end
end
阈值选择经验法则：
- 城市道路：0.3-0.5（保守）
- 高速公路：0.1-0.3（激进）
- 停车场景：0.5以上（非常保守）
3. 车辆动力学建模与MPC设计
3.1 车辆动力学模型
采用自行车模型结合准静态轮胎模型，在保证精度的同时控制计算复杂度：
matlab复制function dxdt = vehicle_model(x, u)
% 状态量: x = [vx; vy; s; ey; yaw]
% 控制量: u = [acc; delta]
% 参数设置
m = 1200; % 质量(kg)
lf = 1.4; % 前轴到CG距离(m)
lr = 1.6; % 后轴到CG距离(m)
Cf = 8000; % 前轮侧偏刚度(N/rad)
Cr = 8000; % 后轮侧偏刚度(N/rad)
% 轮胎侧偏角计算
alpha_f = u(2) - (x(2)+lf*x(5))/x(1);
alpha_r = -(x(2)-lr*x(5))/x(1);
% 轮胎力计算
Fyf = Cf * alpha_f;
Fyr = Cr * alpha_r;
% 动力学方程
dxdt = [
u(1) - (Fyf*sin(u(2)))/m; % 纵向加速度
(Fyf*cos(u(2)) + Fyr)/m - x(1)*x(5); % 横向加速度
x(1)*cos(x(4)) - x(2)*sin(x(4)); % Frenet_s变化率
x(1)*sin(x(4)) + x(2)*cos(x(4)); % Frenet_ey变化率
(Fyf*lf*cos(u(2)) - Fyr*lr)/(m*x(1)) % 横摆角速度
];
end
模型特点：
- 考虑纵向、横向和横摆运动耦合
- 线性轮胎模型简化计算
- Frenet坐标系转换内置在模型中
3.2 非线性MPC设计
模型预测控制的核心是将控制问题转化为优化问题：
matlab复制% MPC参数设置
horizon = 10; % 预测时域
dt = 0.1; % 时间步长(s)
% 优化问题构造
opti = casadi.Opti();
% 决策变量
U = opti.variable(2, horizon); % 控制输入序列
% 初始状态
X = opti.parameter(5,1);
boundary_upper = opti.parameter(horizon,1);
boundary_lower = opti.parameter(horizon,1);
% 模拟预测轨迹
X_pred = X;
for k = 1:horizon
X_pred = vehicle_model_integration(X_pred, U(:,k), dt);
% 安全走廊约束
opti.subject_to(X_pred(4) <= boundary_upper(k));
opti.subject_to(X_pred(4) >= boundary_lower(k));
% 执行器约束
opti.subject_to(-3 <= U(1,k) <= 3); % 加速度限制(m/s^2)
opti.subject_to(-0.5 <= U(2,k) <= 0.5); % 方向盘转角限制(rad)
opti.subject_to(-0.3 <= diff(U(2,:)) <= 0.3); % 转向速率限制
end
% 代价函数
cost = 0;
for k = 1:horizon
cost = cost + 10*X_pred(4)^2 + U(1,k)^2 + 50*U(2,k)^2;
end
opti.minimize(cost);
% 求解器配置
opti.solver('ipopt');
关键设计要点：
- 多目标代价函数（跟踪精度+控制舒适度）
- 硬约束与软约束结合
- 考虑执行器物理限制
- 使用CasADi实现自动微分
4. 系统集成与调试技巧
4.1 联合仿真框架
建立完整的仿真验证环境：
code复制Simulation Loop:
1. 感知模块更新障碍物信息
2. DP规划器生成安全走廊
3. MPC控制器计算控制指令
4. 车辆模型执行控制
5. 可视化与数据记录
4.2 典型问题排查指南
| 问题现象 | 可能原因 | 解决方案 |
|---|---|---|
| 车辆频繁震荡 | MPC权重设置不当 | 增加横向误差权重，降低控制量权重 |
| 无法避开障碍物 | DP代价函数不合理 | 增大障碍物惩罚系数 |
| 优化求解失败 | 初始猜测不合理 | 使用上一周期解作为初始猜测 |
| 控制延迟明显 | 预测时域过长 | 缩短时域，增加迭代频率 |
4.3 参数调优经验
-
DP参数：
- 网格分辨率：平衡计算精度与速度（建议：纵向2-5m，横向0.2-0.5m）
- 障碍物惩罚：根据传感器可靠性调整（建议：500-2000）
-
MPC参数：
- 预测时域：1-2秒为宜（车速高则取大值）
- 权重分配：横向误差 > 航向误差 > 控制量
- 约束裕度：保留10-20%安全余量
-
车辆参数：
- 轮胎刚度：通过试验数据校准
- 质量分布：影响横摆动力学
5. 实际应用中的工程考量
5.1 计算效率优化
实时���保障措施：
- DP算法采用多分辨率网格（远处粗，近处细）
- MPC使用热启动技术（warm start）
- 并行化计算（规划与控制分离线程）
- 代码生成（将Matlab代码转为C++）
5.2 不确定性处理
鲁棒性增强方法：
- 障碍物概率分布模糊化处理
- MPC中增加扰动观测器
- 安全走廊宽度动态调整
- 多假设预测（考虑障碍物不同运动模式）
5.3 人机交互设计
考虑接管场景：
- 安全走廊可视化显示
- 控制权平滑过渡
- 紧急停止触发条件
- 驾驶员意图识别
在实现这套系统时，最深的体会是理论算法与实际工程间的鸿沟。比如教科书上的DP算法很完美，但实际应用中需要考虑网格离散化带来的量化误差；MPC理论可以处理各种约束，但实际求解器的数值稳定性可能成为瓶颈。经过多次迭代后发现，好的自动驾驶系统不是追求单个模块的最优，而是各个组件间的协调配合。