MPC无人驾驶车辆模型预测控制 基于动力学轨迹跟踪参考轨迹可任选包括双移线五次多项式等资料里包括代码各种设置信息附手写推导过程从动力学建模到mpc算法推导过程方向盘握在手里的感觉总是让人安心但无人驾驶车辆可没这玩意儿。今天咱们就聊聊怎么让钢铁盒子自己跑出漂亮的轨迹——拿模型预测控制MPC当缰绳拴住那个躁动的动力学模型。先拆个车看看底盘。二自由度动力学模型就像车辆的骨架横向位移y和横摆角φ是核心参数。还记得转向时的离心力吗这东西在数学里长这样def bicycle_model(x, u, dt): beta np.arctan(1.8 * np.tan(u[0])/3.6) # 轮胎侧偏角 dxdt [ x[3]*np.cos(x[1]) x[2]*np.sin(x[1]), x[4], u[1], (9.8*x[0] - 2500*x[3])/1200 x[2]*x[4], (1.2*2500*x[3] - 0.6*9.8*x[0])/(1600*0.6**2) ] return x np.array(dxdt)*dt这段代码藏着玄机前轮转角u[0]通过阿克曼几何换算成等效侧偏角纵向速度vx直接受油门/刹车控制。注意第4行那个1200kg的整备质量实际调参时这里得根据实车数据反复揉捏。MPC的魔法在于预见未来。假设现在要跟踪双移线轨迹得先让控制器看到接下来3秒的路况。构建优化问题时状态方程要离散化处理% 离散化处理前向欧拉法 Ad eye(5) Ac*dt; Bd Bc*dt;这里的Ac和Bc是雅可比矩阵dt0.1秒的采样时间。别小看这个简单的离散化方法实测比更高阶的龙格库塔法在实时性上更有优势——毕竟控制周期就10Hz算得太复杂反而容易翻车。MPC无人驾驶车辆模型预测控制 基于动力学轨迹跟踪参考轨迹可任选包括双移线五次多项式等资料里包括代码各种设置信息附手写推导过程从动力学建模到mpc算法推导过程实际求解QP问题时约束条件才是真正的拦路虎。比如轮胎侧向力不能超过摩擦圆限制// 摩擦圆约束 for(int i0; iN; i){ constraints.emplace_back(-mu*m*g, C_alpha*y_pred[i] D_alpha*phi_pred[i], mu*m*g); }这个mu0.8的摩擦系数在雨天得调到0.3否则仿真时车辆会表演托马斯回旋。代码里的Calpha和Dalpha是轮胎侧偏刚度相关的系数跟胎压、温度都有关系实验室里得用滑台实测。最后看看跟踪效果。当预测步长N20时双移线跟踪的最大横向误差能压到0.15米以内。不过代价是得在20ms内解完带120个约束的QP问题——这时候用OSQP求解器的ADMM算法比传统的内点法快3倍不止。纸上得来终觉浅真正调试时会发现参考轨迹的曲率变化率比轨迹形状更重要。试过在五次多项式轨迹里埋个曲率突变点结果MPC控制量直接爆表。后来改成在轨迹生成层做二阶导数平滑方向盘转角波动立刻温顺得像只猫。推导过程手稿图示从牛顿欧拉方程出发推导出三阶状态方程经泰勒展开线性化后得到预测模型最终构建包含32个优化变量的QP问题约束条件涉及执行器饱和与动力学可行性