写在前面
- 本文是基于acado官网用例，提取要点，进行知识吸消化，解释重点代码，重点内容是按照教程做代码生成demo
- 最后想阐述一个借助acado实现的基于lqr、mpc的自动驾驶横纵向控制demo
网上查找相关教程可以看到以下相关工具概念 ACADO Toolkit、ACADOS 、Matlab环境下的ACADO ，笔者理解他们是同一个工具的不同扩展，解释如下，以免在学习中不同资料i产生混淆：
- ACADO Toolkit：ACADO （Automatic Control and Dynamic Optimization）是一个用于自动控制和动态优化的开源C++库。它提供了一组工具和功能，使用户能够轻松地建立、求解和部署动态优化问题，包括最优控制、模型预测控制（MPC）等。ACADO Toolkit的特点包括符号微分、生成C++代码、支持多种数值优化方法等。它是一个独立的C++库，用户可以直接使用它来建模和求解控制和优化问题。
- ACADOS：ACADOS（Automatic Control and Dynamic Optimization Suite）是一个开源的自动控制和动态优化套件，它是建立在ACADO Toolkit的基础上的。ACADOS扩展了ACADO Toolkit的功能，提供了更多的工具和接口，支持多个编程语言，包括C、MATLAB、Python和Julia。ACADOS的目标是为不同领域的研究人员和工程师提供一个更广泛的工具集，以满足不同需求。
- ACADO Matlab：ACADO Toolkit还提供了一个专门的MATLAB接口，称为ACADO Matlab。这个接口使MATLAB用户能够使用ACADO Toolkit的功能，尤其是在MATLAB环境中建立和求解动态优化问题。ACADO Matlab提供了与ACADO Toolkit相似的功能，但可以更容易地与MATLAB集成。
本文是基于github下载的ACADO C++版进行的操作， 类似的求解器还有：
- CasADi：CasADi 是一个用于符号计算、自动微分和非线性优化的工具包。它提供了丰富的功能，适用于各种优化和控制问题，包括MPC。它支持多种编程语言，包括Python、MATLAB和C++。
- GPOPS-II：GPOPS-II 是一种商业级别的工具，用于求解动态优化问题，包括最优控制和MPC。它具有强大的建模和求解功能，适用于复杂的多阶段、多变量问题。
- OPT++：OPT++ 是一个用于非线性优化的C++库，支持多种优化算法。它可以用于解决最优控制和参数估计问题。
- FORCES Pro：FORCES Pro 是一个专注于嵌入式优化的商业级别工具，适用于MPC和实时优化应用。它支持多种编程语言和硬件平台，可用于在嵌入式系统中实现高性能的优化控制。
- OSQP：OSQP 是一个高性能的开源二次规划求解器，适用于实时优化问题。它是一个轻量级库，可以用于嵌入式系统和大规模问题。
- IPOPT：IPOPT（Interior Point Optimizer）是一个用于求解非线性优化问题的开源库。它适用于一般的非线性优化问题，包括最优控制。
在网上收索资料的过程中，看到知乎评论里聊到Acado停止了维护，CasADi很多人在用，但是Acado的求解速度比CasADi快，笔者没有一一对比求证以上消息，仅供参考也欢迎大家留言交流，未来研究一下其他求解器。
1. Outline：
- 环境配置
- 简单示例（这个网络上可以查找的；示例很多）
- 基于代码生成器，完成一个NMPC（非线性MPC）求解问题
- tbd（计划基于acado结构，基于NMPC实现车辆横向控制）
2. 环境配置
这里参照 官网tutorios：http://acado.github.io/install_linux.html，简单的英语推荐阅读官网
2.1 前提
首先，前置条件是要配置好编译工具以及cmake工具，相信开发人员电脑上是标配，为了编译源码，这里不详细解释。
管理员模式安装：
sudo apt-get install gcc g++ cmake git gnuplot doxygen graphviz
Gnuplot, Doxygen and Graphviz 不是必须的，但是为了可视化和生成API，故推荐安装
2.2 下载源码
git clone https://github.com/acado/acado.git -b stable ACADOtoolkit
有特殊情况的兄弟从这里 下栽链接下载
2.3 编译
惯例，在git目录下建一个build文件夹，在其中进行操作编译。
cd ACADOtoolkit
mkdir build
cd build
cmake ..
make
2.4 检查测试
可以使用自带的范例测试，范例文件夹下有很多参考供研究
cd ..
cd examples/getting_started
./simple_ocp
其他官方示例源文件和可执行文件：
3. 简单示例
这里参考使用tzr0725朋友的做法
首先我们需要将Acado的库加入系统变量里面，方便以后使用，操作和所有加path一样，将：
source <YourAcadoPath>/build/acado_env.sh
加入到根目录.bashrc下
然后就可以正常建立工程文件目录，默认大家使用cmake。结构如图，其中红框中的文件必须要放置，否则编译如下的cmakelsit时候会报找不到一些文件，它在/cmake/FindACADO.cmake中
配置cmakelist，大家关注acado相关的内容即可，尤其是这一行增加了检索路径，去找到我们拷贝的cmake文件
set( CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} ${PROJECT_SOURCE_DIR} ) #
cmake_minimum_required(VERSION 2.8)
project(project_AcadoTest)
set( CMAKE_CXX_STANDARD 11 )
set( CMAKE_CXX_STANDARD_REQUIRED ON )
set( THREADS_PREFER_PTHREAD_FLAG ON )
SET( CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} ${PROJECT_SOURCE_DIR} ) # acado needed
#find_package( Threads REQUIRED )
#find_package(glog REQUIRED)
#find_package(Eigen3 REQUIRED)
find_package(ACADO REQUIRED)
#set(GLOG_LIBRARIES "/usr/local/lib/libglog.so")
message(STATUS "ACADO_INCLUDE_DIRS: ${ACADO_INCLUDE_DIRS}")
#message(STATUS "GLOG_INCLUDE_DIRS: ${GLOG_INCLUDE_DIRS}")
#message(STATUS "GLOG_LIBRARIES: ${GLOG_LIBRARIES}")
set(CMAKE_BUILD_TYPE "Debug")
set(CMAKE_INCLUDE_CURRENT_DIR ON)
aux_source_directory(${CMAKE_CURRENT_SOURCE_DIR} SRC_LIST)
aux_source_directory(${CMAKE_CURRENT_SOURCE_DIR}/src/. SRC_LIST)
include_directories(${CMAKE_CURRENT_SOURCE_DIR}
${CMAKE_CURRENT_SOURCE_DIR}/include
#${EIGEN3_INCLUDE_DIR}
${ACADO_INCLUDE_DIRS})
#message(STATUS "GLOG_LIBRARIES: ${EIGEN3_INCLUDE_DIR}")
add_executable(${PROJECT_NAME} ${SRC_LIST})
#target_include_directories(${PROJECT_NAME} PRIVATE ${GLOG_INCLUDE_DIRS})
#target_link_libraries(${PROJECT_NAME} PRIVATE ${GLOG_LIBRARIES})
#target_link_libraries(${PROJECT_NAME} ${EIGEN3_LIBRARIES})
target_link_libraries(${PROJECT_NAME} ${ACADO_SHARED_LIBRARIES})
关于src的示例，官方示例资源解释很详细了，可以直接观看上面给出的示例的源文件。这里写两个例子：
- 其一以最优控制问题官方示例src举例：火箭以最小时间走完。加入了笔者理解。
#include <acado_toolkit.hpp>
#include <acado_gnuplot.hpp>
int main( ){
USING_NAMESPACE_ACADO
// 定义变量， 类型acado库实现了，调转查看初始化方法
DifferentialState s,v,m ; // the differential states
Control u ; // the control input u
Parameter T ; // the time horizon T
DifferentialEquation f( 0.0, T ); // the differential equation这里重载实现，可以调转查看，根据需求设置，
// 设置最优控制问题，可以根据需要调整求解时长
OCP ocp( 0.0, T ); // time horizon of the OCP: [0,T] 设置求解时域
ocp.minimizeMayerTerm( T ); // the time T should be optimized，为cost函数设置，可以设置为mayer项也可以设置为Lagrange项
//带有权重的设置例如 ：ocp.minimizeMayerTerm(w1*heading*heading + w2*lateralDist*lateralDist); 也可以按照自己需求设置
//模型定义: 需要定义系统的动态，通常是通过差分方程或常微分方程来实现的。
f << dot(s) == v; // an implementation
f << dot(v) == (u-0.2*v*v)/m; // of the model equations
f << dot(m) == -0.01*u*u; // for the rocket.
//约束条件：初始和结束约束，不等式、等式约束
ocp.subjectTo( f ); // minimize T s.t. the model,
ocp.subjectTo( AT_START, s == 0.0 ); // the initial values for s,
ocp.subjectTo( AT_START, v == 0.0 ); // v,
ocp.subjectTo( AT_START, m == 1.0 ); // and m,
ocp.subjectTo( AT_END , s == 10.0 ); // the terminal constraints for s
ocp.subjectTo( AT_END , v == 0.0 ); // and v,
ocp.subjectTo( -0.1 <= v <= 1.7 ); // as well as the bounds on v
ocp.subjectTo( -1.1 <= u <= 1.1 ); // the control input u,
ocp.subjectTo( 5.0 <= T <= 15.0 ); // and the time horizon T.
//设置求解器
OptimizationAlgorithm algorithm(ocp); // the optimization algorithm
// 设置 GnuPlot 用于绘制结果
GnuplotWindow window;
window.addSubplot( s, "THE DISTANCE s" );
window.addSubplot( v, "THE VELOCITY v" );
window.addSubplot( m, "THE MASS m" );
window.addSubplot( u, "THE CONTROL INPUT u" );
algorithm << window;
//求解 MPC 问题
algorithm.solve(); // solves the problem.
return 0;
}
- 其二： MPC问题求解，同样可以在官方示例文件夹下找到，笔者加入了部分自己理解时的备注，其中自己在学习的时候有几个点容易混淆。
例子想求解这么一个控制问题：
#include <acado_toolkit.hpp>
#include <acado_gnuplot.hpp>
using namespace std;
USING_NAMESPACE_ACADO
int main( )
{
// INTRODUCE THE VARIABLES:
// -------------------------
DifferentialState xB;
DifferentialState xW;
DifferentialState vB;
DifferentialState vW;
Control R;
Control F;
double mB = 350.0;
double mW = 50.0;
double kS = 20000.0;
double kT = 200000.0;
// DEFINE A DIFFERENTIAL EQUATION:
// -------------------------------
DifferentialEquation f;// 创建微分方程
f << dot(xB) == vB;
f << dot(xW) == vW;
f << dot(vB) == ( -kS*xB + kS*xW + F ) / mB;
f << dot(vW) == ( kS*xB - (kT+kS)*xW + kT*R - F ) / mW;
// DEFINE LEAST SQUARE FUNCTION:
// -----------------------------
Function h;
//函数 h 用于定义最小二乘（Least Squares）问题中的cost函数
//平方和并没有在 h 中直接计算，而是在优化问题的目标函数中由 ocp.minimizeLSQ(Q, h, r) 执行。这个函数会将这些项加权并计算它们的平方和，其中权重由矩阵 Q 控制。也就是典型的最小二乘带权重，ps 推荐下bilibili老王和DR. Can 视频，最优估计，最优控制，机器学习感觉很多理论共用。
h << xB;
h << xW;
h << vB;
h << vW;
DMatrix Q(4,4);
Q.setIdentity();
Q(0,0) = 10.0;
Q(1,1) = 10.0;
//参考值 ，量测值或者 可观测值转为对应状态量测值
DVector r(4);
r.setAll( 0.0 );
// DEFINE AN OPTIMAL CONTROL PROBLEM:
// ----------------------------------
const double t_start = 0.0;
const double t_end = 1.0;
OCP ocp( t_start, t_end, 20 );
// 这里设置了步长20，则实际求解间隔为1/20 =0.05，可以从我截图的红框内看出
//换一句话说0.05 即为mpc根据状态空间方程外推下一步状态的时长
ocp.minimizeLSQ( Q, h, r );
// 这里不再使用mayer，使用了最小二乘作为cost函数
// Q：权重矩阵，用于调整每个目标项的相对重要性。
// h：目标函数的描述。
// r：期望的目标值。
ocp.subjectTo( f );
ocp.subjectTo( -500.0 <= F <= 500.0 );
ocp.subjectTo( R == 0.0 );
//笔者认为这里mpc设置已经ok，后续在设置仿真环境
// SETTING UP THE (SIMULATED) PROCESS:
// -----------------------------------
OutputFcn identity;
//创建了一个名为 identity 的输出函数对象。输出函数通常用于记录系统状态和控制输入的信息
DynamicSystem dynamicSystem( f,identity );
//创建了一个名为 dynamicSystem 的动态系统对象，它用于描述系统的动态行为。
//它需要两个参数： f：前面定义的微分方程对象，用于描述系统的状态变化。 identity：前面创建的输出函数对象，用于记录系统状态和控制输入。
Process process( dynamicSystem,INT_RK45 );
//创建了一个名为 process 的进程对象，用于模拟系统的行为。它需要两个参数：
// dynamicSystem：前面创建的动态系统对象，用于描述系统的动态行为. INT_RK45：表示使用4/5阶Runge-Kutta积分器进行系统模拟
// SETTING UP THE MPC CONTROLLER:
// ------------------------------
RealTimeAlgorithm alg( ocp,0.05 );
//建立算法对象
// 这里有一个值得注意的点
//RealTimeAlgorithm alg(ocp, 0.05); 也就是这里的0.05是执行一次ocp对象，也就是我们mpc算法的时间间隔
//而之前的OCP ocp(0,1, 20) 恰好也是0.05的时间长度，但他是一次mpc过程中，滚动迭代的小周期，也是利用状态方程外推下一步状态的时间步长。
//0到1的1秒时间长度为我的mpc的预测优化时域
DVector x0(4);
x0(0) = 0.01;
x0(1) = 0.0;
x0(2) = 0.0;
x0(3) = 0.0;
//初始化输出向量
alg.set( MAX_NUM_ITERATIONS, 2 );
//以下应该是是demo用来仿真的代码，建立controller和一些设置作图给用户展示，不再研究
StaticReferenceTrajectory zeroReference;
Controller controller( alg,zeroReference );
// SETTING UP THE SIMULATION ENVIRONMENT, RUN THE EXAMPLE...
// ----------------------------------------------------------
SimulationEnvironment sim( 0.0,3.0,process,controller );
if (sim.init( x0 ) != SUCCESSFUL_RETURN)
exit( EXIT_FAILURE );
if (sim.run( ) != SUCCESSFUL_RETURN)
exit( EXIT_FAILURE );
// ...AND PLOT THE RESULTS
// ----------------------------------------------------------
VariablesGrid sampledProcessOutput;
sim.getSampledProcessOutput( sampledProcessOutput );
VariablesGrid feedbackControl;
sim.getFeedbackControl( feedbackControl );
GnuplotWindow window;
window.addSubplot( sampledProcessOutput(0), "Body Position [m]" );
window.addSubplot( sampledProcessOutput(1), "Wheel Position [m]" );
window.addSubplot( sampledProcessOutput(2), "Body Velocity [m/s]" );
window.addSubplot( sampledProcessOutput(3), "Wheel Velocity [m/s]" );
window.addSubplot( feedbackControl(1), "Damping Force [N]" );
window.addSubplot( feedbackControl(0), "Road Excitation [m]" );
window.plot( );
return EXIT_SUCCESS;
}
结果见图：
总结：
要使用ACADO Toolkit求解MPC问题，通常需要设置提供以下条件：
- 模型定义: 定义系统的差分方程，给出状态量之间的关系
- 优化目标: 定义想要最小化或最大化的量。这可以是跟踪误差、控制努力、终端代价或其他任何相关度量。
- 约束: 如果存在，需要定义对控制输入、状态或其他变量的约束。
- 求解器设置: 根据应用和问题的性质，需要调整求解器的不同参数，如积分器的容差、最大迭代次数等、步长等。
- 初始条件和/或参考轨迹: 对于MPC应用，需要提供系统的初始状态，并需要一个参考轨迹。
4. 生成代码
概括翻译官网总结，生成代码一共三步骤：
- 设置你的mpc问题，配置目标文件夹和make环境
- 运行编译好的文件
- 如果使用第三方的求解二次规划的库，将库要配置在目标文件夹里，在下载的acado包里自带了一些库可以在external_packages/里寻找，将在设置里选用的库放到设定的输出文件夹下
例子说明：
4.1 用code描述你的求解问题：（之前语法句柄做出过解释，这里不再解释）
- 这里笔者按照官网教程教程地址操作的时候，在 ：
Matrix W = eye( h.getDim() );
Matrix WN = eye( hN.getDim() );
自己手写实现时可以编译过的，但是直接地用库里的方法会报错，cmakelist 确认设置无误，库也是对的。
结论是：官网教程版本部匹配，按照本地的示例教程改为：
DMatrix W = eye<double>(h.getDim());
DMatrix WN = eye<double>(hN.getDim());
就ok了。
{.cpp}// 这里为一部分cpp src
#include <acado_toolkit.hpp>
int main( )
{
USING_NAMESPACE_ACADO
// Variables:
DifferentialState p ; // the trolley position
DifferentialState v ; // the trolley velocity
DifferentialState phi ; // the excitation angle
DifferentialState omega; // the angular velocity
Control a ; // the acc. of the trolley
const double g = 9.81; // the gravitational constant
const double b = 0.20; // the friction coefficient
// Model equations:
DifferentialEquation f;
f << dot( p ) == v;
f << dot( v ) == a;
f << dot( phi ) == omega;
f << dot( omega ) == -g * sin(phi) - a * cos(phi) - b * omega;
- 设定cost 函数 加入约束
{.cpp}
// Reference functions and weighting matrices:
Function h, hN;
h << p << v << phi << omega << a;
hN << p << v << phi << omega;
Matrix W = eye( h.getDim() );
Matrix WN = eye( hN.getDim() );
WN *= 5;
//
// Optimal Control Problem
//
OCP ocp(0.0, 3.0, 10);
ocp.subjectTo( f );
ocp.minimizeLSQ(W, h);
ocp.minimizeLSQEndTerm(WN, hN);
ocp.subjectTo( -1.0 <= a <= 1.0 );
ocp.subjectTo( -0.5 <= v <= 1.5 );
- 设定相关输出参数
{.cpp}
// Export the code:
OCPexport mpc( ocp );
mpc.set( HESSIAN_APPROXIMATION, GAUSS_NEWTON );
mpc.set( DISCRETIZATION_TYPE, SINGLE_SHOOTING );
mpc.set( INTEGRATOR_TYPE, INT_RK4 );
mpc.set( NUM_INTEGRATOR_STEPS, 30 );
mpc.set( QP_SOLVER, QP_QPOASES );
mpc.set( GENERATE_TEST_FILE, YES );
mpc.set( GENERATE_MAKE_FILE, YES );
mpc.set( GENERATE_MATLAB_INTERFACE, YES );
mpc.set( GENERATE_SIMULINK_INTERFACE, YES );
if (mpc.exportCode( "getting_started_export" ) != SUCCESSFUL_RETURN)
exit( EXIT_FAILURE );
mpc.printDimensionsQP( );
return EXIT_SUCCESS;
}
4.2 配置目标地址文件夹
我们设定使用qpoases求解，所以需要在在安装acado的地方找到external_packages/qpoases ，将其放置到运行build中你的设置代码编译生成的可执行文件再次生成的目标文件夹中（有点绕）。即为下图中getting_started_export。
运行刚才我们编译生成的project，再设定输出文件夹内会发现新的文件，还有一个makefile，我们直接make 来编译这个makefile，完成输出。
生成立对应的test可执行文件，我们运行一下，以下下即为刚才print的结果们：
(base) kaifengqu@kaifengqu-HP-ProBook-440-G7:~/Desktop/H01Coding/H08WorkingDev/H09QPLongitudinal/build/getting_started_export$ ./test
ACADO Toolkit -- A Toolkit for Automatic Control and Dynamic Optimization.
Copyright (C) 2008-2015 by Boris Houska, Hans Joachim Ferreau,
Milan Vukov and Rien Quirynen, KU Leuven.
Developed within the Optimization in Engineering Center (OPTEC) under
supervision of Moritz Diehl. All rights reserved.
ACADO Toolkit is distributed under the terms of the GNU Lesser
General Public License 3 in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU Lesser General Public License for more details.
Real-Time Iteration 0: KKT Tolerance = 1.625e+00
Real-Time Iteration 1: KKT Tolerance = 5.895e-07
Real-Time Iteration 2: KKT Tolerance = 1.003e-13
Real-Time Iteration 3: KKT Tolerance = 5.009e-20
Real-Time Iteration 4: KKT Tolerance = 2.630e-26
Real-Time Iteration 5: KKT Tolerance = 3.360e-31
Real-Time Iteration 6: KKT Tolerance = 2.007e-31
Real-Time Iteration 7: KKT Tolerance = 1.480e-31
Real-Time Iteration 8: KKT Tolerance = 3.944e-33
Real-Time Iteration 9: KKT Tolerance = 2.326e-33
End of the RTI loop.
Differential variables:
[
1.000000e-01 1.000000e-01 1.000000e-01 1.000000e-01
1.233878e-01 5.591860e-02 9.088841e-02 -1.536102e-01
1.294342e-01 -1.560897e-02 2.570073e-02 -2.476023e-01
1.157576e-01 -7.556884e-02 -3.838386e-02 -1.487911e-01
9.011084e-02 -9.540932e-02 -5.748262e-02 2.911045e-02
6.425943e-02 -7.693337e-02 -2.962655e-02 1.415938e-01
4.614686e-02 -4.381710e-02 1.318072e-02 1.226962e-01
3.658784e-02 -1.990970e-02 3.531533e-02 1.497966e-02
3.154798e-02 -1.368941e-02 2.402310e-02 -8.363073e-02
2.668423e-02 -1.873556e-02 -5.864383e-03 -1.005828e-01
2.021838e-02 -2.437011e-02 -2.790116e-02 -3.603973e-02
]
Control variables:
[
-1.469380e-01
-2.384252e-01
-1.998662e-01
-6.613493e-02
6.158650e-02
1.103876e-01
7.969134e-02
2.073429e-02
-1.682051e-02
-1.878184e-02
]
4.3 再解释一下生成的文件以及结合代码简单讲一下我对mpc算法的理解
-
acado_common.h
当中定义或声明了必要的mpc计算所需要的变量类型和方法，他引用了辅助头文件acado_auxiliary_functions.h（即相当于他们一起提供了所有的类型和方法）。
其中acadoVariables用来调用算法，而acadoWorkspace用来存储一些中间变量 -
test.c
除非你在导出设置中关闭了选项，否则会自动生成一个test.c的文件，提供了入口main函数。
{.c}
#include "acado_common.h"
#include "acado_auxiliary_functions.h"
#include <stdio.h>
#include <string.h>
/* Some convenient definitions. */
#define NX ACADO_NX /* Number of differential state variables. */
#define NXA ACADO_NXA /* Number of algebraic variables. */
#define NU ACADO_NU /* Number of control inputs. */
#define NP ACADO_NP /* Number of parameters. */
#define NY ACADO_NY /* Number of measurements/references on nodes 0..N - 1. */
#define NYN ACADO_NYN /* Number of measurements/references on node N. */
#define N 10 /* Number of intervals in the horizon. */ //之前提到过mpc迭代次数（滚动预测时域/次数=迭代步长）和控制算法运行一次的周期是不同的。
#define NUM_STEPS 5 /* Number of real-time iterations. */
#define VERBOSE 1 /* Show iterations: 1, silent: 0. */
/* Global variables used by the solver. */
ACADOvariables acadoVariables;
ACADOworkspace acadoWorkspace;
这里定义了部分mpc问题的设定值，比如有一些矩阵维度的值，可以用在后续的for循环遍历中。
而main中则是自动帮你生成的一个测试用例，
/* A template for testing of the solver. */
int main( )
{
/* Some temporary variables. */
int i, iter;
acado_timer t;
/* Initialize the solver. */
acado_initializeSolver();
/* Initialize the states and controls. */
for (i = 0; i < NX * (N + 1); ++i) acadoVariables.x[ i ] = 0.0;
for (i = 0; i < NU * N; ++i) acadoVariables.u[ i ] = 0.0;
/* Initialize the measurements/reference. */
for (i = 0; i < NY * N; ++i) acadoVariables.y[ i ] = 0.0;
for (i = 0; i < NYN; ++i) acadoVariables.yN[ i ] = 0.0;
/* MPC: initialize the current state feedback. */
#if ACADO_INITIAL_STATE_FIXED
for (i = 0; i < NX; ++i) acadoVariables.x0[ i ] = 0.1;
#endif
if( VERBOSE ) acado_printHeader();
/* Prepare first step */
acado_preparationStep();
以上是测试用例在初始化mpc问题设定状态量，参考值，控制量（u=kx），还有当前周期的控制输出（x），他帮你考虑到了按照你设置的step数量，将值设定到了对应的矩阵位置上。无需自己去思考矩阵维度和位置问题。（好方便，当时看b站up老王的推导，这个很烦）
/* Get the time before start of the loop. */
acado_tic( &t );
/* The "real-time iterations" loop. */
for(iter = 0; iter < NUM_STEPS; ++iter)
{
/* Perform the feedback step. */
acado_feedbackStep( );
/* Apply the new control immediately to the process, first NU components. */
if( VERBOSE ) printf("\tReal-Time Iteration %d: KKT Tolerance = %.3e\n\n", iter, acado_getKKT() );
/* Optional: shift the initialization (look at acado_common.h). */
/* acado_shiftStates(2, 0, 0); */
/* acado_shiftControls( 0 ); */
/* Prepare for the next step. */
acado_preparationStep();
}
/* Read the elapsed time. */
real_t te = acado_toc( &t );
准备实时迭代，按照你设定的次数循环。这里笔者开始理解有混淆，因为设置的mpc迭代周期切好和这个实时迭代次数一样，但经过查询得知：“The ‘real-time iterations’ loop” 是指实时迭代循环，而不是典型的MPC动迭代求解循环。其实MPC的迭代循环在设置矩阵的时候已经体现了，这个实时迭代循环的目的不是用于在线优化控制问题，而是用于测试和演示ACADO Toolkit生成的代码以及其性能：通过运行一定数量的迭代步骤（在这里是NUM_STEPS），测量每个迭代的时间，评估代码在实际应用中的性能，实时控制循环通常不会像这个示例中的代码那样静态地运行，而是根据实际系统的测量和状态进行动态调整。
- 对于MPC的一点看法
MPC在每一个采样间隔里，首先得到当前状态测量（我理解时模型外推）值，然后算法根据测量值通过cost最小化，得出当前时刻下最优控制输入，然后将该输入应用到模型中，通过模型传递外推到下一时刻虚拟的状态，再根具该状态如法炮制求下一时刻最优控制输入。（LQR我的理解所谓无限的求解时长，一次性求解对整个控制过程的最优控制率or控制输入，而mpc是每一个小周期求最优，这样会求解出一个控制序列，比如这里我的例子是10个步长则会解出10个控制序列，然后我应用第1个控制量，对于定周期运行的控制算法，如果我的单个采样周期小于了控制算法运行周期，则或可以使用插值运用前2-3个控制量。由于mpc有考虑应用了上一小周期的控制量后的虚拟结果迭代着优化，如果模型设定比较好的话，控制结果会比一次性lqr求解更好，ps为啥更好只能是感觉，也推导过公式，但是为啥更优本人还没有直观理解，数理证明更优还需要大佬们帮我讲讲）
以下是用来打印结果的一些处理
if( VERBOSE ) printf("\n\nEnd of the RTI loop. \n\n\n");
/* Eye-candy. */
if( !VERBOSE )
printf("\n\n Average time of one real-time iteration: %.3g microseconds\n\n", 1e6 * te / NUM_STEPS);
acado_printDifferentialVariables();
acado_printControlVariables();
return 0;
}
控制的结果可以从函数printControlVariables中获取，实际应用了每一个求解循环的解序列中的第一个值u[0]或者取前几个值再做一定的处理。这里示例只有一个控制量 NU=1 ，设定的mpc迭代次数N=10，故会打印前10个结果
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
