本文是上一篇的后续,https://blog.csdn.net/weixin_46479223/article/details/135070489
本文将先解读官网mpc的例子,
然后实现一个自己设计的mpc的控制器;不远的未来的目标是成为我闭环仿真器(轨迹处理、规划、控制接入基于学习的车辆模型)的一环(目前是pp控制),遥远的未来实现强化学习自动调整参数。
官网讲解也很清晰,我只做总结和部分重要补充。
关于mpc的理解 和osqp的前置学习 分别在我的文章:
官网案例如上
官网案例中提到
需要将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来自动迭代找到考虑了整个序列的当下最优解,或者说是我们人为从解序列里去拿出来。
借助官网架构可以很方便的搭建出自己的例子,但是注意一点,由于以上我的设定(自动驾驶巡航场景),这三步我需要注释掉,官网的原demo我理解想表现出从起点靠近终末状态的形式,而我这个例子终点相对于起点的位置永远不变化。
运行成功!
计算结果
全代码在我的附件里
int main()
{
// set the preview window
int mpcWindow = 20;
// allocate the dynamics matrices
Eigen::Matrix<double, 2, 2> a;
Eigen::Matrix<double, 2, 1> b;
// allocate the constraints vector
Eigen::Matrix<double, 2, 1> xMax;
Eigen::Matrix<double, 2, 1> xMin;
Eigen::Matrix<double, 1, 1> uMax;
Eigen::Matrix<double, 1, 1> uMin;
// allocate the weight matrices
Eigen::DiagonalMatrix<double, 2> Q;
Eigen::DiagonalMatrix<double, 1> R;
// allocate the initial and the reference state space
Eigen::Matrix<double, 2, 1> x0;
Eigen::Matrix<double, 2, 1> xRef;
// allocate QP problem matrices and vectores
Eigen::SparseMatrix<double> hessian;
Eigen::VectorXd gradient;
Eigen::SparseMatrix<double> 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:"<<ctr<<std::endl;
// 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;
}