模型预测控制-基于NMPC的无人艇轨迹跟踪控制（C++代码实现）

一、NMPC轨迹跟踪问题描述

1 WAM-V14无人艇数学模型

无人艇坐标系示意图如下：

图片引自文献[1]

双体无人艇数学模型表示如下：
$$\begin{array}{r}\{\begin{array}{rl}\dot{\eta }& =R(\psi )\nu ,\\ M\dot{\nu }& =-C(\nu )\nu -D(\nu )\nu +BT+{\tau }_w,\end{array}\end{array}\text{\hspace{1em}(1)}$$
模型参数见[1]. 其中：
$$M=\left[\begin{array}{ccccc}m-X_{\dot{u}}& & 0& & -m{y}_G\\ 0& & m-Y_{\dot{v}}& & m{x}_G-Y_{\dot{r}}\\ -m{y}_G& & m{x}_G-N_{\dot{v}}& & I_z-N_{\dot{r}}\end{array}\right]$$
$$C(\nu )=\left[\begin{array}{ccccc}0& & 0& & \frac{Y_{\dot{v}}v+(\frac{Y_{\dot{r}}+N_{\dot{v}}}{2})}{200}r\\ 0& & 0& & -X_{\dot{u}}u\\ -\frac{Y_{\dot{v}}v+(\frac{Y_{\dot{r}}+N_{\dot{v}}}{2})}{200}r& & X_{\dot{u}}u& & 0\end{array}\right]$$
$$D(\nu )=D_l+D_n$$
$$D_l=-\left[\begin{array}{ccccc}{X}_u& & 0& & 0\\ 0& & Y_v& & Y_r\\ 0& & N_v& & N_r\end{array}\right]$$
$$D_n=-\left[\begin{array}{ccccc}{X}_{u|u|}|u|& & 0& & 0\\ 0& & Y_{v|v|}|v|+Y_{v|r|}|r|& & Y_{r|v|}|v|+Y_{r|r|}|r|\\ 0& & N_{v|v|}|v|& & N_{r|v|}|v|+N_{r|r|}|r|\end{array}\right]$$

2 NMPC轨迹跟踪模型

预测控制算法具有三大本质特征 ：预测模型，滚动优化和反馈校正。它最大的特点是不断滚动的局部优化，使模型失配、畸变、干扰等引起的不确定性及时得到弥补，从而得到较好的动态控制性能。

2.1 预测模型

令$x=[{\nu }^T,{\eta }^T{]}^T$，则公式（1）可写为
$$\dot{x}=f(x,\tau )\text{\hspace{1em}(2)}$$
其中$\tau =BT$，$f(x,\tau )$为:
$$f(x,\tau )=\left[\begin{array}{c}R(\psi )\nu \\ M^{-1}(-C(\nu )\nu -D_l\nu +\tau )\end{array}\right]$$
注意到阻尼矩阵我们之用到了线性的，为的是方便casadi优化求解 。由于NMPC为离散控制方法，将上述模型欧拉离散为如下表达式：
$$x_{k+1}=x_k+d_{t}f(x_k,{\tau }_k)\text{\hspace{1em}(3)}$$
上式即为预测模型，其中：$d_t$为采样时间。

2.2 参考轨迹生成

参考轨迹采用和无人艇相同的模型生成，如下：
$$x_{k+1}^{ref}=x_k^{ref}+d_{t}f(x_k^{ref},{\tau }_k^{ref})\text{\hspace{1em}(4)}$$

2.3 滚动优化

优化模型描述如下：
J ∗ ( x t ) = min ⁡ u t : t + N − 1 ∣ t { p ( x t + N ∣ t ) + ∑ k = 0 N − 1 q ( x k , τ k ) } (5a) \begin{aligned} J^*(x_{t})=\min_{u_{t:t+N-1|t}}\{p(x_{t+N|t})+\sum_{k=0}^{N-1}q(x_k,\tau_{k})\} \tag{5a} \end{aligned} J∗(xt​)=ut:t+N−1∣t​min​{p(xt+N∣t​)+k=0∑N−1​q(xk​,τk​)}​(5a)
$$\begin{array}{r}s.t.\{\begin{array}{rl}& x_{k+1}=x_k+d_{t}f(x_k,{\tau }_k)\\ & x_k\in X,{\tau }_k\in U,k=0,1,...,N-1\end{array}\end{array}\text{\hspace{1em}(5b)}$$
其中：$X,U$为状态和控制量的可行域，$N$为预测步数，终端代价$p(x_{t+N|t})$， 过程代价函数$q(x_k,{\tau }_k)=||x_k-x_k^{ref}|{|}_Q+||u_k-u_k^{ref}|{|}_R$, $Q,R$为相应的权值矩阵。

NMPC原理即是每一时刻都求解上述优化问题，获取一系列控制量，并将第一个控制量用于控制对象，然后下一时刻继续求解该优化问题，也即滚动优化。

下面用代码实现上述NMPC轨迹跟踪控制算法。

二、代码实现

1 代码链接

基于ros通信框架与casadi的NMPC轨迹跟踪代码链接：

Github 基于NMPC的无人艇轨迹跟踪与定点控制.

具体代码编译运行过程在github上已经给出。

2 代码详解

2.1 功能包wamv_model

主要完成无人艇模型的搭建，利用ros话题通信发布无人艇实时状态信息。

文件wamv_model.cpp

#include "wamv_model.h"

WAMV14::WAMV14(float ts, int integration_step, Eigen::Matrix<float,6,1> x_init)
{
   this->x = x_init;
   this->Thrust<<0,0;
   this->Ths=0.1;
   this->Thrustdot_max = 50;
   this->Thrust_max = 204;
   this->m_sample_time = ts;
   this->m_integration_step = integration_step;
   this->m_dt = m_sample_time/m_integration_step;
   std::cout<<"无人艇初始状态:u:"<<x(0)<<",v:"<<x(1)<<",r:"<<x(2)
       <<",x:"<<x(3)<<",y:"<<x(4)<<",psi:"<<x(5)<<std::endl;
}
WAMV14::WAMV14(Eigen::Matrix<float, 6, 1>  x0,Eigen::Matrix<float, 2, 1>  Thrust0, float ts, int integration_step)
{
   this->x = x0;
   this->Thrust = Thrust0;
   this->Ths=0.5;
   this->Thrustdot_max = 50;
   this->Thrust_max = 204;
   this->m_sample_time = ts;
   this->m_integration_step = integration_step;
   this->m_dt = m_sample_time/m_integration_step;
}

WAMV14::~WAMV14()
{

}

//状态更新
void WAMV14::state_update(Eigen::Matrix<float, 2, 1> Thrust_c,Eigen::Matrix<float,3,1> tau_w)
{
   for(int j=0;j<m_integration_step;j++)
   {
       // 实时获取无人艇状态
       float u = this->x(0);
       float v = this->x(1);
       float r = this->x(2);
       float psi = this->x(5);
       float U = sqrt(pow(u,2)+pow(v,2));
       // 无人艇参数
       // float Xudot = -11.25, Nvdot = -679.8304, Iz = 400, Yvdot = -195.6398, 
       // Nrdot = -600.2102, xg = 0, Yrdot = -54.3864;
       float Xudot = -11.25, Nvdot = 0.2, Iz = 400, Yvdot = -195.6398, 
       Nrdot = -600.2102, xg = 0, Yrdot = 0.2;
       float Xu = -50.5870, Xuu = -5.8722;
       float Yv = -20*rho*abs(v)*2.1092;
       float Nr = -(float)(0.02)*rho*M_PI*U*pow(T,2)*pow(L,2);
       float Nv = -(float)(0.06)*rho*M_PI*U*pow(T,2)*L;
       float Yr = -6*rho*M_PI*U*pow(T,2)*L;
       float Yvv = -599.3130, Yrv = -524.3989, Yvr = -524.3989, Yrr = -1378;
       float Nvv = -524.3989, Nrv = -1378, Nvr = -1378, Nrr = -2996;
       float m11 = m-Xudot;
       float m22 = m-Yvdot;
       float m23 = m*xg-Yrdot;
       float m32 = m*xg-Nvdot;
       float m33 = Iz-Nrdot;
       float c13 = -m*v+(Yvdot*v+(Yrdot+Nvdot)*r/2)/200;
       float c23 = m*u-Xudot*u;
       float c31 = -c13, c32 = -c23;
       float d11 = -Xu-Xuu*abs(u);
       float d22 = -Yv-Yrv*abs(v)-Yvr*abs(r);
       float d23 = -Yr-Yrv*abs(v)-Yrr*abs(r);
       float d32 = -Nv-Nvv*abs(v)-Nvr*abs(r);
       float d33 = -Nr-Nrv*abs(v)-Nrr*abs(r);
       //系统矩阵描述
       Eigen::Matrix<float,3,3> Rbn;
       Rbn << cos(psi), -sin(psi), 0,
           sin(psi), cos(psi), 0,
           0, 0, 1;
       Eigen::Matrix<float,3,3> M;
       M << m11,0,0,
           0,m22,m23,
           0,m32,m33;
       Eigen::Matrix<float,3,3> Crb;
       Crb << 0,0,c13,
           0,0,c23,
           c31,c32,0;
       Eigen::Matrix<float,3,3> Dv;
       Dv << d11,0,0,
           0,d22,d23,
           0,d32,d33;
       //无人艇推力速度
       Eigen::Matrix<float,2,1> Thrustdot;
       Thrustdot = -(Thrust-Thrust_c)/Ths;
       // 无人艇推力速率限幅
       for(int i=0;i<2;i++)
       {
           if(abs(Thrustdot(i))>=Thrustdot_max)
           {
               Thrustdot(i) = Thrustdot_max*mySign(Thrustdot(i));
           }
       }
       //推力更新
       Thrust = Thrustdot*m_dt+Thrust;
       
       // 无人艇推力限幅   
       for(int i=0;i<2;i++)
       {
           if(abs(Thrust(i))>=Thrust_max)
           {
               Thrust(i) = Thrust_max*mySign(Thrust(i));
           }
       }
       //计算总扰动项
       Eigen::Matrix<float,3,1> nu;
       nu << u,v,r;
       Fur = M.inverse()*(-Crb*nu-Dv*nu+tau_w);
       //无人艇状态更新
       Eigen::Matrix<float,3,2> BT;
       BT << 1,1,0,0,B/2,-B/2;
       Eigen::Matrix<float,3,1> tau;
       tau = BT*Thrust;
       Eigen::Matrix<float,3,1> nudot;
       nudot = M.inverse()*tau+Fur;
       Eigen::Matrix<float,3,1> etadot;
       etadot = Rbn*nu;
       Eigen::Matrix<float,6,1> xdot;
       
       xdot<< nudot(0),nudot(1),nudot(2),etadot(0),etadot(1),etadot(2);
       
       x = xdot*m_dt+x;
   }  
}
//获取当前状态
Eigen::Matrix<float,6,1> WAMV14::get_states()
{
   return x;
}
//获取当前推力
Eigen::Matrix<float,2,1> WAMV14::get_thrust()
{
   return Thrust;
}

//获取模型总扰动项
Eigen::Matrix<float,3,1> WAMV14::get_disturbance()
{
   return Fur;
}
//符号函数
int WAMV14::mySign(float x)
{
   if (x<0)
       return -1;
   else if(x=0.0)
       return 0;
   else
       return 1;
}

文件test_wamv_model.cpp

#include <ros/ros.h>
#include "wamv_model.h"        //小车模型头文件
#include "wamv_model/states.h" //自定义消息头文件
#include "wamv_model/controls.h"

/**
 * This tutorial demonstrates simple receipt of messages over the ROS system.
 */

//由于回调函数不能返回数值，若想处理回调函数接收的话题消息，需要定义如下Listener类，
//利用类中成员来处理接收到的消息
class Listener
{
public:
    Eigen::Matrix<float,2,1> controller_controls;

public:
    Listener();
    ~Listener();
    void callback(const wamv_model::controls::ConstPtr& msg);
};

Listener::Listener()
{
    controller_controls<<0,0;
}

Listener::~Listener()
{

}

void Listener::callback(const wamv_model::controls::ConstPtr& msg)
{
    // ROS_INFO("I heard states: [%f, %f, %f]", msg->x, msg->y, msg->theta);

    controller_controls<<msg->thrust_left,msg->thrust_right;

    // ROS_INFO("current states: [%f, %f, %f]", pos[0], pos[1], psi);
}

int main(int argc, char **argv)
{
    //初始化ros节点
    ros::init(argc,argv,"test_car_model"); 
    ros::NodeHandle nh;
    // 创建一个Publisher，发布名为/hello的topic，消息类型为std_msgs::String，队列长度100
    ros::Publisher msg_pub = nh.advertise<wamv_model::states>("/wamv_states", 10);
    // 创建一个监听对象，接收控制器的控制指令
    Listener mlistener;
    ros::Subscriber msg_sub = nh.subscribe<wamv_model::controls>("/nmpc_controls",10,&Listener::callback,&mlistener);
    //初始化小车
    float sample_time = 0.02;     //采样时间，用来更新无人艇状态
    Eigen::Matrix<float,6,1> states;       //小车x,y,theta状态
    states<<0,0,0,0,0,M_PI/2;
    Eigen::Vector2f controls;     //小车控制量
    controls<<0,0;
    int integration_step = 1;
    WAMV14 wamv(sample_time, integration_step, states); //创建无人艇
    // 设置循环的频率
    ros::Rate loop_rate(50);
    //开始仿真
    while(ros::ok())
    {
        //循环回调
        ros::spinOnce();
        //接收控制指令
        controls = mlistener.controller_controls;
        //干扰
        Eigen::Matrix<float,3,1> tau_w;
        tau_w<<0,0,0;
        //无人艇
        wamv.state_update(controls,tau_w);
        states = wamv.get_states();
        //发布无人艇状态
        wamv_model::states states_msg;
        states_msg.u = states[0];
        states_msg.v = states[1];
        states_msg.r = states[2];
        states_msg.x = states[3];
        states_msg.y = states[4];
        states_msg.psi = states[5];
        msg_pub.publish(states_msg);

        //休眠延迟
        loop_rate.sleep();

    }
}

2.2 功能包mycontroller

主要实现NMPC轨迹跟踪与定点保持控制器，利用ros话题通信订阅无人艇实时状态信息，完成闭环控制，主要文件解析如下。

文件nmpc_trajectory_tracking.cpp

初始化函数TrajectoryTrackingNMPC(int predict_step, float sample_time, std::vector<Eigen::Matrix<float,8,1>> trajectory_sequence)，用来初始化NMPC控制器的，传入参数有预测步长、采样时间、轨迹点，里面需要调用求解器函数set_my_nmpc_solver_for_TrajectoryTracking().

//轨迹跟踪初始化函数
TrajectoryTrackingNMPC::TrajectoryTrackingNMPC(int predict_step, float sample_time, std::vector<Eigen::Matrix<float,8,1>> trajectory_sequence)
{
   m_predict_step = predict_step;
   m_sample_time = sample_time;
   m_nmpc_stage = -1;
   m_trajectory_sequence = trajectory_sequence;

   m_control_command<<0,0;
   m_task = "TrajectoryTracking";

   //最优解猜测值初始化
   for(int j=0;j<2*m_predict_step;j++)
   {
       m_initial_guess.push_back(0);
   }

   //设置求解器
   set_my_nmpc_solver_for_TrajectoryTracking();
}

函数set_my_nmpc_solver_for_TrajectoryTracking() 主要用来定义轨迹跟踪求解器的，只需要在初始化时调用一次就可以。

预测模型定义：首先需要建立预测模型（基于casadi符号描述），用来预测的模型就是在1中介绍的无人船模型，只不过在这里编写代码时给简化了，不然casadi无法对含有绝对值的模型求梯度。然后构建模型函数casadi::Function m_f = casadi::Function(“f”, {states, controls}, {rhs});方便后续的使用

代价函数定义：利用opt_para（6+NX6行1列的向量）参数获取无人艇当前状态（6行1列）和期望跟踪的轨迹（NX6行1列，N为预测步数），根据预测模型函数和获得的当前状态向前预测，结合期望轨迹和预测的状态获得代价函数cost_fun的定义

优化求解变量：opt_para（NX2行1列），欠驱动无人艇只有2个控制量，预测N步，即需要优化求解出NX2个优化解

//创建轨迹跟踪求解器
void TrajectoryTrackingNMPC::set_my_nmpc_solver_for_TrajectoryTracking()
{
    //模型建立
    casadi::SX x = casadi::SX::sym("x");
    casadi::SX y = casadi::SX::sym("y");
    casadi::SX psi = casadi::SX::sym("psi");
    casadi::SX u = casadi::SX::sym("u");
    casadi::SX v = casadi::SX::sym("v");
    casadi::SX r = casadi::SX::sym("r"); //状态

    casadi::SX Tu = casadi::SX::sym("Tu");
    casadi::SX Tr = casadi::SX::sym("Tr"); //推力

    casadi::SX states = casadi::SX::vertcat({u,v,r,x,y,psi});
    casadi::SX controls = casadi::SX::vertcat({Tu,Tr});
    int n_states = states.size1(); //共有n个状态-----6
    int n_controls = controls.size1(); //共有n个控制输入-----2

    //运动学模型参数
    casadi::SX Us = casadi::SX::sqrt(u*u+v*v);
    casadi::SX Xudot = -11.25, Nvdot = 0.2, Iz = 400, Yvdot = -195.6398;
    casadi::SX Nrdot = -600.2102, xg = 0, Yrdot = 0.2;
    casadi::SX Xu = -50.5870, Xuu = -5.8722;

    casadi::SX Yv = 100;
    
    casadi::SX Nr = -(float)(0.02)*rho*M_PI*Us*pow(T,2)*pow(L,2);
    casadi::SX Nv = -(float)(0.06)*rho*M_PI*Us*pow(T,2)*L;
    casadi::SX Yr = -6*rho*M_PI*Us*pow(T,2)*L;

    casadi::SX Yvv = -599.3130, Yrv = -524.3989, Yvr = -524.3989, Yrr = -1378;
    casadi::SX Nvv = -524.3989, Nrv = -1378, Nvr = -1378, Nrr = -2996;
    casadi::SX m11 = m-Xudot;
    casadi::SX m22 = m-Yvdot;
    casadi::SX m23 = m*xg-Yrdot;
    casadi::SX m32 = m*xg-Nvdot;
    casadi::SX m33 = Iz-Nrdot;

    casadi::SX c13 = -m*v+(Yvdot*v+(Yrdot+Nvdot)*r/2)/200;
    casadi::SX c23 = m*u-Xudot*u;

    casadi::SX c31 = -c13, c32 = -c23;

    casadi::SX rhs = casadi::SX::vertcat({
        (-50*u+Tu)/m11,
        (-150*v)/m22,
        (-15*r+Tr)/m33,
        u*casadi::SX::cos(psi)-v*casadi::SX::sin(psi),
        u*casadi::SX::sin(psi)+v*casadi::SX::cos(psi),
        r
    });   

    //定义模型函数
    casadi::Function m_f = casadi::Function("f", {states, controls}, {rhs});

    // 求解问题符号表示
    casadi::SX U = casadi::SX::sym("U",n_controls,m_predict_step); //待求解的控制变量
    casadi::SX X = casadi::SX::sym("X",n_states,m_predict_step+1); //系统状态

    //优化参数（需要给出当前运动状态和预测视野内的期望点或者期望轨迹）
    casadi::SX opt_para = casadi::SX::sym("opt_para",n_states+(n_states+n_controls)*m_predict_step);
    //优化变量（需要求解的控制序列）
    casadi::SX opt_var = casadi::SX::reshape(U.T(),-1,1);

    //根据上述模型函数向前预测无人艇运动状态
    X(casadi::Slice(),0) = opt_para(casadi::Slice(0,6,1)); //状态初始值

    for(int i=0;i<m_predict_step;i++)
    {
        std::vector<casadi::SX> input_X;
        casadi::SX X_current = X(casadi::Slice(),i);
        casadi::SX U_current = U(casadi::Slice(),i);
        input_X.push_back(X_current);
        input_X.push_back(U_current);
        X(casadi::Slice(),i+1) = m_f(input_X).at(0)*m_sample_time+X_current;
    }

    //控制序列与输出的关系函数（预测函数）
    m_predict_fun = casadi::Function("m_predict_fun",{U,opt_para},{X});

    //惩罚矩阵
    casadi::SX m_Q = casadi::SX::zeros(6,6);
    casadi::SX m_R = casadi::SX::zeros(2,2);
    m_Q(0,0) = 1;
    m_Q(1,1) = 0.1;
    m_Q(2,2) = 0.01;
    m_Q(3,3) = 100;
    m_Q(4,4) = 100;
    m_Q(5,5) = 3000;
    m_R(0,0) = 0.001;
    m_R(1,1) = 0.001;
    
    //计算代价函数
    casadi::SX cost_fun = casadi::SX::sym("cost_fun");
    cost_fun = 0;

    casadi::SX Xref = casadi::SX::reshape(opt_para(casadi::Slice(6,m_predict_step*8+6)),8,m_predict_step);
    for(int k=0;k<m_predict_step;k++)
    {
        casadi::SX states_err = X(casadi::Slice(),k)-Xref(casadi::Slice(0,6,1),k);
        casadi::SX controls_err = U(casadi::Slice(),k)-Xref(casadi::Slice(6,8,1),k);
        // casadi::SX controls_err = U(casadi::Slice(),k);
        cost_fun = cost_fun+casadi::SX::mtimes({states_err.T(),m_Q,states_err})+
                            casadi::SX::mtimes({controls_err.T(),m_R,controls_err});
    }

    //构建求解器(暂时不考虑约束)
    casadi::SXDict nlp_prob= {
        {"f", cost_fun},
        {"x", opt_var},
        {"p",opt_para}
    };

    std::string solver_name = "ipopt";
    casadi::Dict nlp_opts;
    nlp_opts["expand"] = true;
    nlp_opts["ipopt.max_iter"] = 5000;
    nlp_opts["ipopt.print_level"] = 0;
    nlp_opts["print_time"] = 0;
    nlp_opts["ipopt.acceptable_tol"] =  1e-6;
    nlp_opts["ipopt.acceptable_obj_change_tol"] = 1e-4;

    m_solver = nlpsol("nlpsol", solver_name, nlp_prob, nlp_opts);

}

函数 opti_solution_for_TrajectoryTracking(Eigen::Matrix<float,6,1> current_states) 主要用来求解优化控制量的，需要实时调用，传入参数为当前无人艇状态。

//轨迹跟踪优化求解函数
void TrajectoryTrackingNMPC::opti_solution_for_TrajectoryTracking(Eigen::Matrix<float,6,1> current_states)
{
    //设置控制约束
    std::vector<float> lbx;
    std::vector<float> ubx;
    std::vector<float> parameters;
    for (int j = 0; j < m_predict_step; j++)
    {
        lbx.push_back(-300);

        ubx.push_back(300);
    }
    for (int j = 0; j < m_predict_step; j++)
    {
        lbx.push_back(-100);

        ubx.push_back(100);
    }
    //设置求解器输入参数
    m_nmpc_stage++;
    // std::cout<<"m_nmpc_stage: "<<m_nmpc_stage<<std::endl;

    for(int j=0;j<6;j++)
    {
        parameters.push_back(current_states[j]);
    }
    int index_end;
    index_end = m_nmpc_stage+m_predict_step;
    if(index_end<m_trajectory_sequence.size()-1)
    {
        for(int i=m_nmpc_stage;i<index_end;i++)
        {
            for(int j=0;j<8;j++)
            {
                parameters.push_back(m_trajectory_sequence.at(i)[j]);
            }
        }
    }
    else
    {
        if(m_nmpc_stage<m_trajectory_sequence.size()-1)
        {
            int k_index=0; //记录已经填充几段轨迹
            for(int i=m_nmpc_stage;i<m_trajectory_sequence.size()-1;i++)
            {
                k_index++;
                for(int j=0;j<8;j++)
                {
                    parameters.push_back(m_trajectory_sequence.at(i)[j]);
                }
            }
            //填充剩余的轨迹数据
            for(int i=0;i<m_predict_step-k_index;i++)
            {
                for(int j=0;j<8;j++)
                {
                    parameters.push_back(m_trajectory_sequence.at(m_trajectory_sequence.size()-1)[j]);
                }
            }     
        }
        else
        {
            for(int i=0;i<m_predict_step;i++)
            {
                for(int j=0;j<8;j++)
                {
                    parameters.push_back(m_trajectory_sequence.at(m_trajectory_sequence.size()-1)[j]);
                }
            }    
        }
    
    }

    //当前期望位置
    current_desired_pos<< parameters.at(9),parameters.at(10),parameters.at(11);

    //打印第一个轨迹点
    // std::cout<<"Deisired states: ["<<parameters.at(9)<<", "<<parameters.at(10)<<", "<<parameters.at(11)<<"]"<<std::endl;
    
    //求解参数设置
    m_args["lbx"] = lbx;
    m_args["ubx"] = ubx;
    m_args["x0"] = m_initial_guess;
    m_args["p"] = parameters;
    //求解
    m_res = m_solver(m_args);

    //获取代价函数
    casadi::SX cost_f = m_res.at("f");
    // std::cout<<"代价值: "<<cost_f<<std::endl;

    //获取优化变量
    std::vector<float> res_control_all(m_res.at("x"));

    std::vector<float> res_control_Tu, res_control_Tr;

    res_control_Tu.assign(res_control_all.begin(), res_control_all.begin() + m_predict_step);
    res_control_Tr.assign(res_control_all.begin() + m_predict_step, res_control_all.begin() + 2 * m_predict_step);

    //存储当前控制序列作为下一时刻的最优解猜测值
    std::vector<float> initial_guess;
    for (int j = 0; j < m_predict_step-1; j++)
    {
        initial_guess.push_back(res_control_Tu.at(j+1));
    }
    initial_guess.push_back(res_control_Tu.at(m_predict_step-1));
    for (int j = 0; j < m_predict_step-1; j++)
    {
        initial_guess.push_back(res_control_Tr.at(j+1));
    }
    initial_guess.push_back(res_control_Tr.at(m_predict_step-1));
    m_initial_guess = initial_guess;

    // 采用求解得到的控制序列的第一组作为当前控制量
    m_control_command << res_control_Tu.front(), res_control_Tr.front();

    //预测轨迹
    std::vector<Eigen::Matrix<float,6,1>> predict_trajectory;
    predict_trajectory.push_back(current_states);
    Eigen::Matrix<float,6,1> next_states;
    for(int j=0;j<m_predict_step;j++)
    {
        float u,v,r,x,y,psi;
        u = predict_trajectory.at(j)[0];
        v = predict_trajectory.at(j)[1];
        r = predict_trajectory.at(j)[2];
        x = predict_trajectory.at(j)[3];
        y = predict_trajectory.at(j)[4];
        psi = predict_trajectory.at(j)[5];

        next_states<<   (-50*u+res_control_Tu.at(j))/161.25*m_sample_time+u,
                        (-150*v)/345.6398*m_sample_time+v,
                        (-15*r+res_control_Tr.at(j))/1000.2102*m_sample_time+r,
                        (u*std::cos(psi)-v*std::sin(psi))*m_sample_time+x,
                        (u*std::sin(psi)+v*std::cos(psi))*m_sample_time+y,
                        r*m_sample_time+r;

        predict_trajectory.push_back(next_states);
    }
    m_predict_trajectory = predict_trajectory;
}

文件test_trajectory_tracking.cpp

该文件用来测试上面的NMPC控制器，主要利用ros通信框架获取无人艇当前状态，代码较为简单且都有注释，不再详细介绍，具体代码如下：

#include <ros/ros.h>
#include "wamv_model/states.h"
#include "wamv_model/controls.h"
#include "nmpc_trajectory_tracking.h"
#include "single_trajectory.h"

/**
 * This tutorial demonstrates simple receipt of messages over the ROS system.
 */

//由于回调函数不能返回数值，若想处理回调函数接收的话题消息，需要定义如下Listener类，
//利用类中成员来处理接收到的消息
class Listener
{
public:
    Eigen::Matrix<float,6,1> current_states;

public:
    Listener();
    ~Listener();
    void callback(const wamv_model::states::ConstPtr& msg);
};

Listener::Listener()
{
    current_states<<0,0,0,0,0,0;

}

Listener::~Listener()
{

}

void Listener::callback(const wamv_model::states::ConstPtr& msg)
{
    // ROS_INFO("I heard states: [%f, %f, %f]", msg->x, msg->y, msg->theta);

    current_states<<msg->u,msg->v,msg->r,msg->x,msg->y,msg->psi;

    // ROS_INFO("current states: [%f, %f, %f]", pos[0], pos[1], psi);
}

//主函数
int main(int argc, char **argv)
{
    //初始化ros节点
    ros::init(argc, argv, "test_trajectory_tracking");

    ros::NodeHandle n;

    //生成轨迹
    float sample_time = 0.02;
    float total_time = 200;
    int integration_step=1;
    Eigen::Matrix<float,6,1> x_init;
    x_init<<0,0,0,5,5,M_PI/3; 
    SingleTTG singlettg(sample_time,total_time,integration_step,x_init);
    std::vector<Eigen::Matrix<float,8,1>> trajectory_sequence;
    trajectory_sequence = singlettg.TTG();

    //创建NMPC控制器
    int predict_step = 50;

    TrajectoryTrackingNMPC nmpc(predict_step, sample_time, trajectory_sequence);

    //创建监听对象
    Listener mlistener;

    ros::Subscriber sub = n.subscribe("/wamv_states",10,&Listener::callback,&mlistener);

    //创建发布对象
    ros::Publisher pub_controls =n.advertise<wamv_model::controls>("/nmpc_controls",10);
    ros::Publisher pub_desired_pos =n.advertise<wamv_model::states>("/desired_pos",10);

    // 设置循环的频率
    ros::Rate loop_rate(50);

    int count=0;
    while(ros::ok())
    {
        //循环回调函数
        ros::spinOnce();
        
        //该节点执行次数
        count++;
        // std::cout<<"count: "<<count<<std::endl;

        //调用nmpc求解
        nmpc.opti_solution_for_TrajectoryTracking(mlistener.current_states);
        //获取控制量
        Eigen::Vector2f nmpc_controls = nmpc.get_controls();

        //nmpc求解得到的控制量需要进行坐标转换得到最终的期望双桨推力
        Eigen::Matrix<float,2,2> BT;
        BT << 1,1,1.83/2,-1.83/2;
        Eigen::Vector2f Thrust_command;
        Thrust_command = BT.inverse()*nmpc_controls;

        //发布求解得到的控制量
        wamv_model::controls pub_msg;
        pub_msg.thrust_left = Thrust_command[0];
        pub_msg.thrust_right = Thrust_command[1];
        pub_controls.publish(pub_msg);

        //发布当前期望位置
        Eigen::Vector3f desired_pos;
        desired_pos = nmpc.get_current_desired_pos();
        wamv_model::states pub_msg1;
        pub_msg1.x = desired_pos[0];
        pub_msg1.y = desired_pos[1];
        pub_msg1.psi = desired_pos[2];

        pub_desired_pos.publish(pub_msg1);

        //休眠
        loop_rate.sleep();
    }
  return 0;
}

三、运行结果

通过roslaunch分别启动无人艇仿真节点、控制器节点、绘图节点：

roslaunch mycontroller test_trajectory_tracking.launch

运行结果如下：

四、小结

NMPC是一个有远见的控制器，能够预测未来N步的事情，滚动优化求解，从而获得较好的控制效果，原理上预测时域越长，效果越好，但是其优化求解效率会变低，因此选择合适的预测步数十分重要。

参考文献：

[1] W. B. Klinger, I. R. Bertaska, K. D. von Ellenrieder and M. R. Dhanak, “Control of an Unmanned Surface Vehicle With Uncertain Displacement and Drag,” in IEEE Journal of Oceanic Engineering, vol. 42, no. 2, pp. 458-476, April 2017, doi: 10.1109/JOE.2016.2571158.