Wong Kosheng1. 项目概述
在自动驾驶和移动机器人领域，路径规划与跟踪控制是核心问题之一。传统方法直接跟踪离散路点会导致车辆运动不连续、抖动等问题。本项目提出了一种基于Simulink的多段B样条路径平滑方法，结合曲率自适应速度规划，实现了从离散路点到平滑轨迹的全流程解决方案。
这个方案特别适合处理包含直角弯、急转弯等复杂场景的路径，比如停车场自动泊车、园区物流车导航等应用。通过MATLAB/Simulink环境，我们可以快速搭建完整的仿真系统，验证算法效果。
需要模型API调用？ 免费领10W Token，多模型网关一键接入 Claude、DeepSeek 等主流模型。
2. 系统架构设计
2.1 整体工作流程
系统采用模块化设计，主要包含以下几个关键部分：
- 路径生成模块：将离散路点转化为连续平滑的B样条曲线
- 曲率计算模块：实时计算路径各点的曲率值
- 速度规划模块：根据曲率约束生成安全速度曲线
- 时间参数化模块：将路径-速度信息转化为时间参数化轨迹
- 跟踪控制模块：使用Stanley控制器实现轨迹跟踪
- 车辆模型：模拟真实车辆动力学特性
2.2 模块交互关系
各模块通过Simulink信号线连接，形成闭环系统。路径生成器输出平滑路径后，曲率计算模块会分析路径几何特性，速度规划器据此生成安全速度曲线，时间参数化模块将两者结合生成时间参数化轨迹，最后由控制器驱动车辆模型跟踪该轨迹。
3. 关键技术实现
3.1 多段三次B样条路径生成
B样条曲线因其局部可控性和连续性优势，成为路径平滑的理想选择。我们采用三次B样条，确保路径C²连续（即位置、速度、加速度都连续）。
实现步骤：
- 输入离散路点序列
- 计算控制点（通常比路点多2个）
- 确定节点向量（均匀或非均匀分布）
- 分段计算B样条曲线
在Simulink中，可以通过MATLAB Function模块实现这一算法。关键是要处理好段与段之间的连接点，确保整条路径的连续性。
3.2 曲率计算
曲率是速度规划的重要依据，计算公式为：
code复制k = (x'y" - y'x") / (x'² + y'²)^(3/2)
其中x'、y'是一阶导数，x"、y"是二阶导数。在Simulink中，可以使用Derivative模块获取导数，然后通过MATLAB运算实现上述公式。
注意：数值微分容易引入噪声，建议对原始路径进行平滑处理后再计算曲率。
3.3 曲率自适应速度规划
安全速度与曲率成反比关系。基本规划原则是：
code复制v = min(v_max, √(a_max / |k|))
其中：
- v_max：系统允许的最大速度
- a_max：最大横向加速度（通常取2-3 m/s²）
在Simulink中，可以使用Lookup Table模块实现这种非线性关系，输入曲率，输出建议速度。
3.4 时间参数化
将路径长度s与速度v结合，通过积分关系得到时间参数化轨迹：
code复制t = ∫(1/v) ds
x(t) = x(s(t))
y(t) = y(s(t))
这需要在Simulink中构建一个积分器，将速度倒数对路径长度积分得到时间，再通过插值获取对应位置的坐标。
4. Simulink建模实现
4.1 建模步骤详解
第一步：定义原始路点
在MATLAB工作区定义路点矩阵，例如：
matlab复制waypoints = [0, 0; 5, 0; 5, 5; 10, 5; 10, 10; 0, 10; 0, 15];
第二步：实现B样条路径生成器
创建MATLAB Function模块，实现B样条算法。核心代码框架：
matlab复制function [x, y] = bspline_generator(waypoints, t)
% 计算控制点
% 计算节点向量
% 分段计算B样条
% 返回x,y坐标
end
第三步：实现速度规划器
使用MATLAB Function模块，输入曲率，输出建议速度：
matlab复制function v = speed_planner(k)
v_max = 5; % m/s
a_max = 2.5; % m/s²
v = min(v_max, sqrt(a_max ./ abs(k)));
end
第四步：时间参数化实现
使用Integrator模块计算累计时间，配合Interpolation模块获取时间参数化轨迹。
第五步：集成Stanley跟踪控制器
Stanley控制器计算前轮转角：
matlab复制function delta = stanley_controller(x, y, yaw, v)
% 计算路径跟踪误差
% 计算航向误差
% 计算前轮转角
end
第六步：车辆模型
使用Simulink自带的Vehicle Body模块或自定义自行车模型。
4.2 关键参数设置
- 仿真步长：建议0.01s（固定步长）
- 求解器：ode4（Runge-Kutta）
- 最大仿真时间：根据路径长度和速度设定
5. 仿真结果分析
5.1 典型场景测试
以停车场路径为例：
code复制路点序列：[0,0;5,0;5,5;10,5;10,10;0,10;0,15]
路径平滑效果：
- 直角弯被平滑为连续曲线
- 曲率变化连续，无突变
- 最大曲率控制在安全范围内
速度规划效果：
- 直线段达到最大速度5m/s
- 转弯处速度自动降低至2-3m/s
- 速度变化平滑，无突变
5.2 性能指标
- 路径跟踪误差：<0.1m（直线段），<0.2m（弯道）
- 最大横向加速度：<2.5m/s²
- 速度跟踪误差：<0.1m/s
6. 工程实践建议
6.1 常见问题排查
-
路径抖动问题：
- 检查B样条控制点计算是否正确
- 确认节点向量设置合理
- 尝试增加平滑权重
-
速度突变问题：
- 检查曲率计算是否准确
- 确认速度规划函数连续
- 考虑添加加速度约束
-
跟踪不稳定问题：
- 调整Stanley控制器增益
- 检查车辆模型参数准确性
- 确认仿真步长足够小
6.2 高级优化方向
-
动态障碍物避碰：
- 在路径生成阶段加入障碍物约束
- 实现局部路径重规划
-
与MPC结合：
- 将平滑路径作为MPC的参考轨迹
- 在MPC中考虑更多车辆动力学约束
-
多目标优化：
- 在速度规划中同时考虑舒适性和效率
- 使用优化算法自动调整参数
7. 扩展应用
这套方法不仅适用于自动驾驶，还可应用于：
- 工业机器人轨迹规划
- 无人机航迹规划
- AGV物流车导航
- 游戏AI路径规划
在实际项目中，我通常会先在小规模Simulink模型中验证算法，确认效果后再移植到实际系统。这种方法能显著降低开发风险，提高效率。对于更复杂的场景，可以考虑将B样条替换为五次多项式样条，获得更高的连续性。