1. 差速底盘与轨迹跟踪的核心挑战
差速底盘作为移动机器人最常见的驱动形式,通过左右轮速差实现转向,其运动学模型本质上是非线性的。当我们需要让这样的底盘精确跟踪预设轨迹时,会遇到几个典型问题:
- 系统存在时滞性:电机响应、传感器采样都存在延迟
- 动态约束复杂:轮速、加速度都有物理上限
- 耦合性强:转向动作直接影响后续位姿推算
传统PID控制在这里显得力不从心,因为:
- 无法显式处理多变量耦合
- 难以纳入系统约束
- 对非线性适应性差
而模型预测控制(MPC)恰好能解决这些问题。它通过滚动优化、反馈校正的方式,在每个控制周期:
- 基于当前状态预测未来N步的系统行为
- 求解最优控制序列
- 只执行第一步控制量
这种"预测-优化-执行"的循环机制,使得MPC特别适合处理带约束的非线性系统控制问题。
2. MPC控制框架设计要点
2.1 运动学建模
对于两轮差速底盘,其离散化运动学模型可表示为:
code复制x(k+1) = x(k) + v(k)*cos(θ(k))*dt
y(k+1) = y(k) + v(k)*sin(θ(k))*dt
θ(k+1) = θ(k) + ω(k)*dt
其中:
- (x,y)为机器人坐标
- θ为朝向角
- v为线速度
- ω为角速度
- dt为控制周期
这个模型虽然简单,但已经能反映差速底盘的核心运动特性。在实际应用中,我建议:
- 对工业级底盘,需考虑轮径、轴距等机械参数
- 对高速场景,建议加入加速度约束
- 对重载底盘,需建模电机动力学
2.2 预测时域与控制时域
这是MPC调参的两个关键参数:
- 预测时域(Np):预测未来多少步的状态
- 控制时域(Nc):优化多少步的控制量
经验法则:
- Np通常取能让系统达到稳态的最小步数
- Nc一般取Np的1/3~1/2
- 对于轨迹跟踪,Np要覆盖轨迹曲率变化周期
实测案例:
- 当dt=0.1s时,对常规S形轨迹:
- Np=20步(预测2秒)
- Nc=8步(优化0.8秒)
- 参数敏感性测试显示:
- Np过小会导致"短视"
- Np过大会增加计算负担
2.3 代价函数设计
一个典型的轨迹跟踪代价函数包含:
code复制J = Σ(位置误差 + 角度误差 + 控制量变化率)
具体可拆解为:
- 轨迹偏差惩罚:
math复制w_xy*(x-x_ref)^2 + w_θ*(θ-θ_ref)^2 - 控制平滑惩罚:
math复制w_v*(v_k - v_{k-1})^2 + w_ω*(ω_k - ω_{k-1})^2 - 终端惩罚(确保收敛):
math复制w_end*(x_N - x_ref_N)^2
权重调节技巧:
- 初始阶段侧重位置跟踪(w_xy较大)
- 接近目标时加强角度权重(w_θ增大)
- 高速场景需限制控制变化率(w_v/w_ω调高)
3. 仿真实现全流程
3.1 仿真环境搭建
推荐使用Python生态:
- 运动学模型:NumPy实现
- 优化求解:CVXPY或CasADi
- 可视化:Matplotlib
典型初始化代码:
python复制import numpy as np
from cvxpy import *
# 机器人参数
dt = 0.1 # 控制周期
L = 0.5 # 轴距
v_max = 1.0 # 最大线速度
w_max = np.pi/2 # 最大角速度
3.2 MPC控制器实现
核心算法流程:
- 获取当前状态(x,y,θ)
- 生成参考轨迹窗口
- 构建优化问题:
- 定义决策变量(v,ω)
- 添加动力学约束
- 设置代价函数
- 求解QP问题
- 应用最优控制量
关键代码段:
python复制# 定义优化变量
v = Variable(Nc)
w = Variable(Nc)
# 构建预测模型
states = []
for t in range(Np):
if t == 0:
x_pred = x_current
y_pred = y_current
theta_pred = theta_current
else:
x_pred += v[min(t,Nc-1)]*np.cos(theta_pred)*dt
y_pred += v[min(t,Nc-1)]*np.sin(theta_pred)*dt
theta_pred += w[min(t,Nc-1)]*dt
states.append([x_pred, y_pred, theta_pred])
# 设置代价函数
cost = 0
for t in range(Np):
cost += w_xy*(states[t][0]-x_ref[t])**2
cost += w_xy*(states[t][1]-y_ref[t])**2
cost += w_theta*(states[t][2]-theta_ref[t])**2
if t < Nc-1:
cost += w_v*(v[t+1]-v[t])**2
cost += w_w*(w[t+1]-w[t])**2
# 添加约束
constraints = [v <= v_max, v >= -v_max,
w <= w_max, w >= -w_max]
# 求解
prob = Problem(Minimize(cost), constraints)
prob.solve(solver=ECOS)
3.3 轨迹生成方法
常用参考轨迹生成方式:
- 参数化曲线:
- 直线:
x_ref = v*t, y_ref = 0 - 圆形:
x_ref = R*cos(ω*t), y_ref = R*sin(ω*t)
- 直线:
- 样条插值:
python复制from scipy.interpolate import CubicSpline t_points = np.array([0,1,2,3]) x_points = np.array([0,1,3,2]) x_ref = CubicSpline(t_points, x_points) - 运动规划算法(如RRT*、A*)输出的路径
实测建议:
- 对测试验证,先用简单轨迹(如8字形)
- 工业场景建议采用B样条平滑处理原始路径
- 确保轨迹的一阶连续(速度连续)
4. 调参与性能优化实战
4.1 参数敏感性分析
通过大量仿真测试,总结出关键参数影响:
| 参数 | 增大效果 | 减小效果 | 推荐范围 |
|---|---|---|---|
| Np | 提高前瞻性,但增加计算量 | 可能短视 | 15-30步 |
| w_xy | 加强位置跟踪 | 允许更大位置偏差 | 1.0-5.0 |
| w_θ | 朝向更精确 | 允许角度偏差 | 0.5-2.0 |
| w_v | 速度变化更平滑 | 响应更快但可能抖动 | 0.1-1.0 |
4.2 实时性优化技巧
当控制周期要求<50ms时,需要优化:
- 热启动:用上一周期的解作为初始猜测
python复制prob.solve(warm_start=True) - 减少变量:可固定Nc=1变为单步优化
- 简化模型:用线性化模型近似
- 代码级优化:
- 使用PyPy替代CPython
- 关键循环用Numba加速
实测数据对比:
- 完整MPC:平均35ms/步
- 简化版:平均12ms/步(跟踪精度下降约15%)
4.3 抗扰策略
应对实际场景中的扰动:
- 状态估计滤波:
python复制# 一阶低通滤波 x_est = 0.8*x_est + 0.2*x_measure - 鲁棒代价函数:
math复制J_robust = J + ρ*||Δu|| - 扰动观测器:
- 构建Luenberger观测器
- 在线估计并补偿扰动
5. 典型问题排查指南
5.1 发散振荡问题
症状:轨迹跟踪出现持续振荡或发散
排查步骤:
- 检查预测模型是否与真实系统匹配
- 验证轴距、轮径参数
- 检查dt设置是否合理
- 调整代价函数权重
- 增大控制变化惩罚w_v/w_w
- 减小位置权重w_xy
- 降低预测时域Np
- 添加终端约束:
python复制
constraints += [x[Np] == x_ref[Np]]
5.2 计算超时问题
症状:单步求解时间超过控制周期
优化方案:
- 换用更高效求解器:
- OSQP(针对QP问题)
- IPOPT(非线性问题)
- 减少优化变量:
- 降低Nc
- 参数化控制量(如用B样条表示)
- 启用求解器预处理:
python复制prob.solve(verbose=False, max_iter=100)
5.3 曲线跟踪滞后
症状:转弯时总是"切外道"
解决方案:
- 前馈补偿:
math复制v_ff = sqrt((dx/dt)^2 + (dy/dt)^2) - 增加曲率前馈项:
python复制
w_ref = v_ref * curvature - 自适应调整Np:
- 高曲率段增大Np
- 直线段减小Np
6. 进阶扩展方向
6.1 多机协同跟踪
当需要多机器人编队时:
- 分层控制架构:
- 上层:全局轨迹规划
- 中层:个体MPC跟踪
- 下层:电机控制
- 耦合代价函数:
math复制J_multi = ΣJ_i + w_formation*||d_ij - d_desired|| - 分布式优化:
- 采用ADMM算法
- 每个机���人只与邻居通信
6.2 动态避障集成
结合感知系统的避障策略:
- 势场法:
math复制J_avoid = Σ(1/||p - p_obs||) - 安全走廊:
- 在轨迹周围构建安全区域
- 作为硬约束加入MPC
- 动态重规划:
- 触发式局部轨迹调整
- 保持全局路径不变
6.3 学习增强MPC
融合机器学习的方法:
- 参数自整定:
- 用RL学习最优权重
- 根据场景自动调整Np/Nc
- 模型学习:
- 用NN拟合残差动力学
- 补偿模型误差
- 参考轨迹生成:
- GAIL学习专家演示
- 输出更自然的轨迹
