从零推导：基于MPC的履带式差速底盘轨迹跟踪算法实现

1. 从自行车模型到履带差速模型的思维转换第一次接触轨迹跟踪算法时大多数人都会从经典的自行车模型入门。这个模型简单直观用前轮转向角和后轮速度就能描述车辆运动。但当我真正把算法移植到履带式差速底盘上时发现事情没那么简单——履带底盘没有转向角这个概念它的运动完全依赖两侧履带的差速实现。这里有个关键区别自行车模型的转向是几何约束下的纯滚动而履带底盘在转向时必然伴随滑动。我曾在实验室地板上做过标记测试发现履带转向时的实际轨迹半径总是比理论计算值大15%左右这就是滑动效应在作怪。要建立准确的数学模型必须考虑这种非完整约束特性。举个例子当左履带静止、右履带前进时自行车模型会认为这是零半径转向原地旋转实际履带底盘会形成某个转向半径R且R值与地面摩擦系数相关2. 履带差速模型的运动学推导2.1 建立坐标系与变量定义我们先设定车身坐标系原点O位于两履带几何中心X轴指向车头方向Y轴指向车身左侧定义状态量x,y位置θ航向角v线速度ω角速度关键参数包括履带间距D两履带中心线距离左履带线速度vl右履带线速度vr瞬时转向半径R2.2 差速与运动关系根据刚性车身假设可以推导出v (vr vl)/2 ω (vr - vl)/D但实际测试发现当差速较大时这个关系会出现明显偏差。后来我在模型中加入了一个滑移修正系数λ0.8~1.2之间通过实验标定得到ω λ*(vr - vl)/D3. MPC控制器的模型适配3.1 状态空间方程重构经典MPC使用的自行车模型状态方程为ẋ v*cos(θ) ẏ v*sin(θ) θ̇ v*tan(δ)/L对于履带底盘我们需要改写为ẋ v*cos(θ) ẏ v*sin(θ) θ̇ ω λ*(vr - vl)/D3.2 控制量转换最大的改动在于控制输入原模型加速度a和前轮转角δ新模型左履带加速度al和右履带加速度ar这带来两个挑战控制变量维度变化2维→2维但物理意义不同输入约束需要重新定义如|vr-vl| ≤ D*ω_max4. C实现中的工程细节4.1 矩阵维度调整原自行车模型的预测模型矩阵是3x3而履带模型需要扩展为包含速度项的5x5矩阵。这里有个坑如果直接套用Eigen库的默认配置会导致内存越界。我的解决方案是MatrixXd A(5,5); // 明确指定维度 A 1, 0, -v*sinθ*dt, cosθ*dt, 0, 0, 1, v*cosθ*dt, sinθ*dt, 0, 0, 0, 1, 0, dt, 0, 0, 0, 1-μ*dt, 0, 0, 0, 0, 0, 1-μ*dt;4.2 滑移补偿的实现在代码中加入实时滑移估计double estimateSlipFactor(double vr, double vl) { static dequepairdouble,double history; history.push_back({vr,vl}); if(history.size() 10) history.pop_front(); // 使用最近10次数据进行最小二乘拟合 MatrixXd A(history.size(), 2); VectorXd b(history.size()); for(int i0; ihistory.size(); i) { A(i,0) history[i].first - history[i].second; b(i) imu.getAngularVelocity() * trackWidth; } Vector2d x A.bdcSvd(ComputeThinU|ComputeThinV).solve(b); return x(0)/trackWidth; // 返回λ估计值 }5. 实测调参经验分享在水泥地面上我建议初始参数设置为预测时域N10控制时域M5采样时间dt0.1sQ矩阵对角元[10,10,5,1,1]x,y,θ,v,ωR矩阵对角元[0.1,0.1]al,ar调试时重点关注当转向抖动严重时增大R矩阵的ω相关项轨迹跟踪滞后时适当减小Q矩阵的v相关项出现之字形振荡时需要重新标定滑移系数λ记得第一次室外测试时底盘在草地上疯狂打转——就是因为没考虑草地的低摩擦系数导致λ偏小。后来我增加了地面类型识别模块针对不同地面预存不同的λ参数效果立竿见影。