1. 项目概述
在自动驾驶和移动机器人领域,路径规划与跟踪控制是核心问题之一。传统离散路点规划存在路径不光滑、缺乏时间信息等问题,容易导致车辆跟踪时出现急加速/减速、方向盘抖动等不良现象。本文将分享一个基于Simulink的完整解决方案,通过多段三次B样条曲线生成平滑路径,并结合曲率约束进行自适应速度规划,最终实现稳定可靠的轨迹跟踪。
这个方案特别适合以下场景:
- 自动驾驶车辆的城市道路行驶
- 移动机器人在复杂环境中的导航
- 工业AGV的精确路径跟踪
- 无人机航迹规划与跟踪
需要模型API调用? 免费领10W Token,多模型网关一键接入 Claude、DeepSeek 等主流模型。
2. 系统架构设计
2.1 整体框架
系统采用模块化设计,主要包含以下核心组件:
- 路径生成模块:将离散路点转化为连续平滑的B样条曲线
- 曲率计算模块:实时计算路径各点的曲率值
- 速度规划模块:根据曲率约束生成符合车辆动力学限制的速度曲线
- 时间参数化模块:将几何路径转化为时间参数化轨迹
- 跟踪控制模块:采用Stanley控制器实现轨迹跟踪
- 车辆模型:模拟真实车辆动力学特性
2.2 技术选型考量
选择B样条曲线而非贝塞尔曲线或多项式曲线的主要原因是:
- 局部控制性:修改单个控制点不会影响整条曲线
- 连续性保证:天然满足C2连续性要求
- 计算效率:递推算法计算量适中
Stanley控制器相比纯追踪和MPC控制器的优势在于:
- 计算复杂度低
- 参数物理意义明确
- 在中等速度下表现稳定
3. 关键技术实现
3.1 多段三次B样条路径生成
3.1.1 算法原理
三次B样条曲线的数学表达式为:
matlab复制C(u) = Σ[Ni,3(u)*Pi] (i=0 to n)
其中:
- Pi为控制点
- Ni,3(u)为三次B样条基函数
- u∈[0,1]为参数
基函数通过Cox-de Boor递推公式计算:
matlab复制Ni,0(u) = 1 if ui ≤ u < ui+1, else 0
Ni,k(u) = (u-ui)/(ui+k-ui)*Ni,k-1(u) + (ui+k+1-u)/(ui+k+1-ui+1)*Ni+1,k-1(u)
3.1.2 Simulink实现
在Simulink中建立如下子系统:
- 控制点输入端口
- 参数u生成器(0到1线性变化)
- 基函数计算模块(Embedded MATLAB Function)
- 曲线求和模块
关键参数设置:
- 节点向量采用均匀分布
- 每段曲线采样点数建议50-100
- 控制点间距不超过5米(城市道路场景)
注意:首末控制点需要重复以达到插值效果
3.2 曲率计算模块
3.2.1 曲率公式
曲率k的计算公式:
matlab复制k = |x'y'' - y'x''| / (x'^2 + y'^2)^(3/2)
其中x',x'',y',y''为路径参数方程的一阶和二阶导数。
3.2.2 数值实现
在Simulink中使用Derivative模块计算导数,注意:
- 设置适当的采样时间(建议0.01s)
- 添加低通滤波器消除数值噪声
- 对接近零的速度进行保护处理
3.3 曲率自适应速度规划
3.3.1 速度约束计算
最大允许速度v_max由以下因素决定:
- 横向加速度限制:v_max = sqrt(alat_max/|k|)
- 纵向加速度限制:v_max = min(v_prev + amax*Δt, v_max)
- 全局速度限制:v_max = min(v_max, v_desired)
典型参数值:
- alat_max = 2.5 m/s²(舒适性要求)
- amax = 3.0 m/s²
- v_desired = 15 m/s(城市道路)
3.3.2 速度曲线生成
实现步骤:
- 前向传递计算加速度约束下的速度
- 后向传递计算减速度约束下的速度
- 取两次传递结果的交集
- 应用平滑滤波器
3.4 时间参数化实现
3.4.1 弧长计算
使用累积梯形法计算弧长s:
matlab复制ds = sqrt(dx^2 + dy^2)
s = cumsum(ds)
3.4.2 时间-弧长关系
通过数值积分建立s-t关系:
matlab复制t = ∫(1/v(s))ds
在Simulink中使用Memory模块和累加器实现。
4. Simulink建模详解
4.1 模型架构
整体模型包含以下子系统:
- PathGenerator:路径生成
- SpeedPlanner:速度规划
- TrajectoryGenerator:轨迹生成
- StanleyController:跟踪控制
- VehicleDynamics:车辆模型
4.2 关键模块实现
4.2.1 B样条路径生成器
实现步骤:
- 创建Embedded MATLAB Function实现基函数计算
- 使用MATLAB Function模块实现曲线求和
- 配置From Workspace模块输入控制点
- 添加Scope模块可视化路径
技巧:使用Bus Signal组织控制点数据
4.2.2 Stanley控制器
核心公式:
matlab复制steer = atan(k*e/v) + psi_error
其中:
- e:横向误差
- psi_error:航向误差
- k:调谐参数
参数调优建议:
- 初始k值设为2.0
- 根据实际跟踪效果调整
- 添加输出饱和限制(如±30度)
4.3 车辆模型配置
采用单轨模型,主要参数:
- 质量:1500 kg
- 轴距:2.8 m
- 最大转向角:30 deg
- 轮胎侧偏刚度:80000 N/rad
使用Simulink的Vehicle Body模块和Tire模块搭建。
5. 仿真与结果分析
5.1 测试场景设置
设计两种测试路径:
- 城市道路场景:包含直线、缓弯和直角转弯
- 停车场场景:包含多个90度转弯和S形弯道
5.2 性能指标
评估以下指标:
- 最大横向误差
- 平均横向误差
- 速度跟踪误差
- 转向角变化率
5.3 结果对比
与传统线性插值方法对比:
| 指标 | B样条方法 | 线性插值 |
|---|---|---|
| 最大误差(m) | 0.12 | 0.35 |
| 平均误差(m) | 0.05 | 0.18 |
| 转向抖动(deg/s) | 8.2 | 23.7 |
| 舒适度评分 | 4.5/5 | 2.8/5 |
6. 工程实践要点
6.1 实时性优化
- 预计算B样条基函数表
- 采用定点数运算
- 简化曲率计算(五点差分法)
6.2 鲁棒性增强
- 添加路径有效性检查
- 实现速度规划异常处理
- 控制器输出限幅
6.3 参数调优指南
- 先调路径平滑度(控制点间距)
- 再调速度规划参数(加速度限制)
- 最后调控制器增益
7. 扩展与改进方向
- 动态避障:结合势场法实时调整路径
- 多车协同:增加车车通信接口
- 学习增强:使用神经网络优化速度规划
- 硬件部署:生成C代码进行实车测试
在实际项目中,我们发现以下经验特别有价值:
- 控制点间距应为车辆长度的1.5-2倍
- 曲率计算时添加0.01m-1的常数项可避免奇异值
- Stanley控制器的k参数应与速度成反比关系(k=2.5/v)
- 仿真步长不应大于0.05s以保证数值稳定性
