CasADi库C++用法整理学习---以NMPC代码为例

、达西先生

于 2024-10-17 11:41:55 发布

阅读量1.1k

点赞数 5

文章标签： c++ MPC CasADi

本文链接：https://blog.csdn.net/qq_51013517/article/details/143001275

版权

参考几个使用方法博客
1
官方文档写的很清楚

对SM，DM，XM数据类型疑惑。什么时候使用什么样的类型，还是都可以？

x = MX.sym(“x”)
这将创建一个 1×1 矩阵，即一个包含名为 x 的符号基元的标量。这只是显示名称，而不是标识符。多个变量可以具有相同的名称，但仍然不同。identifier 是返回值。您还可以通过向 SX.sym 提供其他参数来创建向量值或矩阵值符号变量：

在这里插入图片描述

#include <casadi/casadi.hpp>
using namespace casadi;
int main() {
  SX x = SX::sym("x");
  SX y = SX::sym("y",5);
  SX Z = SX::sym("Z",4,2)
  SX f = pow(x,2) + 10;
  f = sqrt(f);
  std::cout << "f: " << f << std::endl;
  return 0;
}

//sqrt((10+sq(x)))

DM 与 SX 非常相似，但不同之处在于非零元素是数值，而不是符号表达式。语法也是相同的，除了 SX.sym 等函数没有等效项。
DM 主要用于将矩阵存储在 CasADi 中，并用作函数的输入和输出。它不用于计算密集型计算。

MX 类型允许像 SX 一样构建由一系列基本操作组成的表达式。但与 SX 不同的是，这些初等运算并不局限于标量一元或二进制运算（R→R 或 R×R→R）。相反，用于形成 MX 表达式的基本运算可以是通用的多个稀疏矩阵值输入、多个稀疏矩阵值输出函数：Rn1×m1×…×RnN×mN→Rp1×q1×…×RpM×qM

SX和，MX两者对比

请注意，结果仅包含使用 MX 符号的两次运算（一次乘法和一次加法），而 SX 等效项则有八次运算（结果矩阵的每个元素两次）。因此，在处理具有许多元素的自然向量或矩阵值运算时，MX 可能更经济。
您不能将 SX 对象与 MX 对象相乘，也不能执行任何其他操作来在同一表达式图中混合两者。但是，您可以在 MX 图中包含对 SX 表达式定义的 Function 的调用。

MPC 代码
注意：opti.subject_to，opti.minimize，MX::mtimes，opti.variable，opti.parameter都是怎么用的
总的来说：

opti.variable: 定义变量。
opti.parameter: 定义参考值，已知的常量。
opti.set_value: 设置参数的具体值。
opti.minimize: 定义目标函数。
opti.subject_to: 添加约束条件。
MX::mtimes: 执行矩阵乘法。

#include "mpc_tracking/mpc.h"

Mpc::Mpc() {
    
    
    N_ = 10;
    dt_ = 0.1;
    u_max_ = 1;
    w_max_ = 0.5;
    vector<double> weights = {1,1,1,2,2}; //Q,R
    u_min_ = - u_max_;
    w_min_ = - w_max_;
    
    Q_ = DM::zeros(3,3); //索引之前初始化size
    R_ = DM::zeros(2,2);
    

    setWeights(weights);
    kinematic_equation_ = setKinematicEquation();
}
Mpc::~Mpc() {}

void Mpc::setWeights(vector<double> weights) {
    //cout << "setweights" << endl;
    Q_(0, 0) = weights[0];
    Q_(1, 1) = weights[1];
    Q_(2, 2) = weights[2];

    R_(0, 0) = weights[3];
    R_(1, 1) = weights[4];
    //R_(2, 2) = weights[5];
    //cout << "set weight finish" << endl;

}

Function Mpc::setKinematicEquation() {
    //cout << "set kinematic" << endl;
    MX x = MX::sym("x");
    MX y = MX::sym("y");
    MX theta = MX::sym("theta");
    MX state_vars = MX::vertcat({x, y, theta});  //将状态变量组合成一个垂直向量（列向量）。
    MX v = MX::sym("v");
    MX w = MX::sym("w");
    MX control_vars = MX::vertcat({v, w});  //控制变量control_vars是一个包含两个元素的垂直向量，分别是v（线速度）和w（角速度）
    
    //rhs means right hand side
    MX rhs = MX::vertcat({v * MX::cos(theta), v * MX::sin(theta), w}); // 动力学方程
    return Function("kinematic_equation", {state_vars, control_vars}, {rhs});
    // MX x = MX::sym("x");
    // MX y = MX::sym("y");
    // MX theta = MX::sym("theta");
    // MX state_vars = MX::vertcat({x, y, theta});

    // MX u = MX::sym("u");
    // MX v = MX::sym("v");
    // MX w = MX::sym("w");
    // MX control_vars = MX::vertcat({u, v, w});

    // MX rhs = u * MX::cos(theta) - v * MX::sin(theta); 
    //这个方程描述了一个更复杂的非线性系统，例如四轮独立驱动的车辆或具有横向控制输入的系统
    // rhs = MX::vertcat({rhs, u * MX::sin(theta) + v * MX::cos(theta), w});
    // return Function("kinematic_equation", {state_vars, control_vars}, {rhs});
}


bool Mpc::solve(Eigen::Vector3d current_states, Eigen::MatrixXd desired_states) {
    
    //cout << "jinqiujiele" << endl;
    Opti opti = Opti();

    Slice all;

    MX cost = 0;
    X = opti.variable(3, N_ + 1);// 定义一个3行N_+1列的矩阵变量X 注意：是变量。
    //第一个时间步（索引为0）表示当前时刻的状态 后面N_个时间步表示未来N_个时刻的状态和U对应
    U = opti.variable(2, N_);
    MX x = X(0, all);
    MX y = X(1, all);
    MX theta = X(2, all);
    MX v = U(0, all);
    MX w = U(1, all);
    //MX w = U(2, all);


    MX X_ref = opti.parameter(3, N_ + 1);   // 定义一个3行N_+1列的参数矩阵X_ref 注意：是已知的常量
    MX X_cur = opti.parameter(3);
    DM x_tmp1 = {current_states[0], current_states[1], current_states[2]};

    opti.set_value(X_cur, x_tmp1);  //set current state opti.set_value 用于设置参数的具体值
    cout << "set current state success" << endl;

    
    //按列索引
    vector<double> X_ref_v(desired_states.data(), desired_states.data() + desired_states.size());
    //auto tp1 = std::chrono::steady_clock::now();
    DM X_ref_d(X_ref_v);
    
    //X_ref_d.resize(3, N_ + 1);
    
    //cout << "desired_states:" << desired_states << endl;
    //cout << "X_ref_v:" << X_ref_v << endl;
    //cout << "X_ref_d:" << X_ref_d << endl;
    // DM x_tmp2(3, N_ + 1);
    // for (int i = 0; i < 3; ++i) {
    //     for (int j = 0; j <= N_; ++j) {
    //         x_tmp2(i, j) = desired_states(i, j);
    //     }
    // }
    X_ref = MX::reshape(X_ref_d, 3, N_ + 1);
    
    
    
    //opti.set_value(X_ref, X_ref_d);  //set reference traj

    // auto tp2 = std::chrono::steady_clock::now();
    // cout <<"set trajectory time:" 
    // << chrono::duration_cast<chrono::microseconds>(tp2 - tp1).count() 
    // << "microseconds" << endl;
    //cout << "set reference traj success" << endl;
    //cout << "x_ref:" << X_ref.size() << endl;

    //set costfunction
    for (int i = 0; i < N_; ++i) {
        MX X_err = X(all, i) - X_ref(all, i); 
        MX U_0 = U(all, i);
        //cout << "U_0 size:" << U_0.size() << endl;
        //cout << "cost size:" << cost_.size() << endl;
        cost += MX::mtimes({X_err.T(), Q_, X_err}); //目标函数是状态误差和控制输入的成本之和
        //cout << "cost size:" << cost_.size() << endl; 
        cost += MX::mtimes({U_0.T(), R_, U_0});
        //cout << "cost size:" << cost_.size() << endl;
    }
    //cout << "cost size:" << cost_.size() << endl;
    //MX::mtimes 用于执行矩阵乘法，用于计算两个矩阵的乘积
    cost += MX::mtimes({(X(all, N_) - X_ref(all, N_)).T(), Q_,
                        X(all, N_) - X_ref(all, N_)});  //终端项
    //cout << "cost:" << cost << endl;
    opti.minimize(cost); //opti.minimize 用于定义优化问题的目标函数
    //cout << "set cost success" << endl;

    //kinematic constrains opti.subject_to 用于添加约束条件到优化问题中
    for (int i = 0; i < N_; ++i) {
        vector<MX> input(2);
        input[0] = X(all, i);
        input[1] = U(all, i);
        MX X_next = kinematic_equation_(input)[0] * dt_ + X(all, i); 
        //这里体现了模型预测控制，模型。  下一步的状态=通过模型计算的速度* dt_ +当前状态
        opti.subject_to(X_next == X(all, i + 1));  // 动力学约束

    }

    //init value 初始化，初始状态赋值第一列所有行
    opti.subject_to(X(all, 0) == X_cur);//第一个时间步（索引为0）表示当前时刻的状态
    //speed angle_speed limit 控制输入的约束
    opti.subject_to(0 <= v <= u_max_);
    opti.subject_to(w_min_ <= w <= w_max_);

    //set solver
    casadi::Dict solver_opts; // 设置求解器选项
    solver_opts["expand"] = true; //MX change to SX for speed up
    solver_opts["ipopt.max_iter"] = 100;
    solver_opts["ipopt.print_level"] = 0;
    solver_opts["print_time"] = 0;
    solver_opts["ipopt.acceptable_tol"] = 1e-6;
    solver_opts["ipopt.acceptable_obj_change_tol"] = 1e-6;

    opti.solver("ipopt", solver_opts);

    //auto sol = opti.solve();
    solution_ = std::make_unique<casadi::OptiSol>(opti.solve());

    return true;
}
vector<double> Mpc::getFirstU() {
    vector<double> res;
    auto first_v =  solution_->value(U)(0, 0);
    auto first_w = solution_->value(U)(1, 0);
    
    //cout << "first_u" << first_u << " " << "first_v" << first_v << endl;

    res.push_back(static_cast<double>(first_v));
    res.push_back(static_cast<double>(first_w));
    return res;
}

vector<double> Mpc::getPredictX() {
    vector<double> res;
    auto predict_x = solution_->value(X);
    cout << "nomal" << endl;
    //cout << "predict_x size :" << predict_x.size() << endl;
    for (int i = 0; i <= N_; ++i) {
        res.push_back(static_cast<double>(predict_x(0, i)));
        res.push_back(static_cast<double>(predict_x(1, i)));
    }
    return res;
}