c++ 最小二乘法 [ToolsChains CPP] Osqp的使用之二：MPCdemo-自动驾驶纵向mpc控制

写在前面

本文是上一篇的后续，https://blog.csdn.net/weixin_46479223/article/details/135070489

本文将先解读官网mpc的例子， 然后实现一个自己设计的mpc的控制器；不远的未来的目标是成为我闭环仿真器（轨迹处理、规划、控制接入基于学习的车辆模型）的一环（目前是pp控制），遥远的未来实现强化学习自动调整参数。

outline

官网案例解释MPC 自己设计的demo-自动驾驶纵向mpc控制 （理解之后 其实很简单）代码在附件里

官网案例

官网讲解也很清晰，我只做总结和部分重要补充。

关于mpc的理解 和osqp的前置学习 分别在我的文章：

MPC的acado实现：https://blog.csdn.net/weixin_46479223/article/details/133743263osqp eigen 安装和入门使用 ：https://blog.csdn.net/weixin_46479223/article/details/135070489 上图是我对mpc的理解简单总结。

官网案例如上

官网案例中提到

需要将mpc问题转化为二次规划问题。 这个在Acado文章https://blog.csdn.net/weixin_46479223/article/details/133743263里提到过，如果使用acado的话，它自动帮我们实现了这一步骤，就不需要繁琐的对齐了。而osqp还是需要自己构建。 构建过程被封装在了这里。 构建步骤还是如之前，至此一个mpc问题被构建在了osqp的结构中。 通过

if (solver.solveProblem() != OsqpEigen::ErrorExitFlag::NoError)

return 1;

问题会被求解，同时以上任何一个步骤出现问题，程序都会返回1。如果希望改变默认设置，可以通过set接口调节，如：

solver.settings()->setWarmStart(true);

可以通过solver的getSolution（）接口，把ospq的结果取出来，结果是一个不定长的eigen域下的vector

VectorXd QPSolution = solver.getSolution()

注意

mpc问题并不是一次求解就可以结束：这里针对自动驾驶来说，如果我的控制跑0.02s一个周期，同时规划轨迹为0.1s。那么在每0.02s，我要根据mpc结果外推一次我的新的位置重新计算和参考线的误差，同时也要在每0.02s内设定一定数量的迭代周期（其实可以超过0.02也无所谓）这个称step，在step间我同样需要一个外推方程，来确定这个小step的根据控制量计算的车辆位置的和真实参考的误差。

这其实是两个概念，一个是算法整体的时间步 T，一个是mpc本身迭代优化的时间步 Step。

这里，step osqp无法像acado一样直接通过参数设定需要自己用代码来控。

官网示例用了一个for循环来处理,示例用了50个循环（T），至于step的时间会在矩阵a和b的值中携带，step通过mpcwindows来设定，

// controller input and QPSolution vector

Eigen::Vector4d ctr;

Eigen::VectorXd QPSolution;

// number of iteration steps

int numberOfSteps = 50;

for (int i = 0; i < numberOfSteps; i++)

{

// solve the QP problem

if (solver.solveProblem() != OsqpEigen::ErrorExitFlag::NoError)

return 1;

// get the controller input

QPSolution = solver.getSolution();

ctr = QPSolution.block(12 * (mpcWindow + 1), 0, 4, 1);

// save data into file

auto x0Data = x0.data();

// 可以设置成自己的文件路径，用文件输出流

// propagate the model

x0 = a * x0 + b * ctr;

// update the constraint bound

updateConstraintVectors(x0, lowerBound, upperBound);

if (!solver.updateBounds(lowerBound, upperBound))

return 1;

}

官网教程地址：https://robotology.github.io/osqp-eigen/md_pages_mpc.html

要特别注意他的后三行propagate the model and update the constraint bound，这里的外推和更新其实是在做展示的场景用而和mpc本身无关，属于casebycase的需要自己看情况使用的。我自己的例子中注释掉了，因为我的场景不同。（这一点有问题也请看官大佬们指出）

搭建自己的案例

其实很简单，我们不需要像老王视频里那样推导，直接复用官网结构和转化二次型的公式。重要的是模型的建立。

见上图，我先用纵向做demo（因为我的闭环仿真器还缺一个纵向控制，横向pp先用着展示），关于横向用动力学还是运动学也有很多案例可以直接带入公式了。

由于我在这里展示的demo是开环的，所以差值我设定为恒定，也就相当于自动驾驶的最简单的无限速前车的巡航工况（还句话说 预瞄参数和预瞄信息稳定），同时假定不倒车。

换句话说 在iterator的的循环里，参考信息恒定。 这个循环我理解是进行了一个模拟的过程，每次迭代都模拟系统的响应并更新控制输入，然后通过运行多次迭代来观察系统的跟踪效果。只不过由于我的假设，我的车辆在自动巡航，预瞄准点永远是在我10m前，参考车速永远是5mps

但在mpc自动计算里的误差会在windows减小，mpc根据误差和权重计算的cost来自动迭代找到考虑了整个序列的当下最优解，或者说是我们人为从解序列里去拿出来。

我的mpc代码

借助官网架构可以很方便的搭建出自己的例子，但是注意一点，由于以上我的设定（自动驾驶巡航场景），这三步我需要注释掉，官网的原demo我理解想表现出从起点靠近终末状态的形式，而我这个例子终点相对于起点的位置永远不变化。

运行成功！ 计算结果

附录 我的main代码

全代码在我的附件里

int main()// there is only a section of my codes

{

// set the preview window

int mpcWindow = 20;

// allocate the dynamics matrices

Eigen::Matrix a;

Eigen::Matrix b;

// allocate the constraints vector

Eigen::Matrix xMax;

Eigen::Matrix xMin;

Eigen::Matrix uMax;

Eigen::Matrix uMin;

// allocate the weight matrices

Eigen::DiagonalMatrix Q;

Eigen::DiagonalMatrix R;

// allocate the initial and the reference state space

Eigen::Matrix x0;

Eigen::Matrix xRef;

// allocate QP problem matrices and vectores

Eigen::SparseMatrix hessian;

Eigen::VectorXd gradient;

Eigen::SparseMatrix linearMatrix;

Eigen::VectorXd lowerBound;

Eigen::VectorXd upperBound;

// set the initial and the desired states

x0 << 0.,0.;

xRef << 10.,5.;

// set MPC problem quantities

setDynamicsMatrices(a, b);

setInequalityConstraints(xMax, xMin, uMax, uMin);

setWeightMatrices(Q, R);

// cast the MPC problem as QP problem

castMPCToQPHessian(Q, R, mpcWindow, hessian);

castMPCToQPGradient(Q, xRef, mpcWindow, gradient);

castMPCToQPConstraintMatrix(a, b, mpcWindow, linearMatrix);

castMPCToQPConstraintVectors(xMax, xMin, uMax, uMin, x0, mpcWindow, lowerBound, upperBound);

// instantiate the solver

OsqpEigen::Solver solver;

// settings

// solver.settings()->setVerbosity(false);

solver.settings()->setWarmStart(true);

// set the initial data of the QP solver

solver.data()->setNumberOfVariables(2 * (mpcWindow + 1) + 1 * mpcWindow);

solver.data()->setNumberOfConstraints(2 * 2 * (mpcWindow + 1) + 1 * mpcWindow);

if (!solver.data()->setHessianMatrix(hessian))

return 1;

if (!solver.data()->setGradient(gradient))

return 1;

if (!solver.data()->setLinearConstraintsMatrix(linearMatrix))

return 1;

if (!solver.data()->setLowerBound(lowerBound))

return 1;

if (!solver.data()->setUpperBound(upperBound))

return 1;

// instantiate the solver

if (!solver.initSolver())

return 1;

// controller input and QPSolution vector

Eigen::VectorXd ctr;

Eigen::VectorXd QPSolution;

// number of iteration steps

int numberOfSteps = 50;

// Open a file for writing

std::ofstream outputFile("output.txt");

for (int i = 0; i < numberOfSteps; i++)

{

// solve the QP problem

if (solver.solveProblem() != OsqpEigen::ErrorExitFlag::NoError)

return 1;

// get the controller input

QPSolution = solver.getSolution();

ctr = QPSolution.block(2 * (mpcWindow + 1), 0, 1, 1);

std::cout<<"acc calculated:"<

// save data into file

auto x0Data = x0.data();

// Write the value of ctr to the file

outputFile << "acc calculated: " << ctr << std::endl;

// // propagate the model

// x0 = a * x0 + b * ctr;

// // update the constraint bound

// updateConstraintVectors(x0, lowerBound, upperBound);

// if (!solver.updateBounds(lowerBound, upperBound))

// return 1;

}

// Close the file

outputFile.close();

return 0;

}

精彩链接

 您阅读本篇文章共花了：