多旋翼无人机ROS&C++开发例程(三)：基于开源求解器Casadi编写C++程序实时求解无人机轨迹跟踪优化问题

这篇博客主要介绍如何基于开源求解器Casadi编写C++程序构建优化问题并实时求解，从而实现无人机的轨迹跟踪与避障等应用，Casadi库的安装方法可以参考前面的博客。

1.问题描述

以一个最简单的运动学方程为例(其他复杂的方程，比如包含偏航角等的运动方程也可以按照这样的思路进行构建)，即二维位置和速度的关系:
$$\begin{gathered} \dot{x}=v_x \\ \dot{y}=v_y \end{gathered}$$
并且期望的轨迹$[x_r,y_r{]}^T$和对应的期望速度$[v_x,v_y{]}^T$已知，我们希望通过实时求解优化问题得到合适的无人机水平速度传递给飞控，从而使无人机沿期望的轨迹飞行，并避开已知的障碍物$[x_{oi},y_{oi}{]}^T,i=1,2,...,N_o$。

那么优化问题的成本函数可以用期望轨迹与实际轨迹之间的偏差，输入成本，以及无人机与障碍物的位置关系来获取，即
$$L=(\mathbf{x}-{\mathbf{x}}_r{)}^T\mathbf{P}(\mathbf{x}-{\mathbf{x}}_r)+(\mathbf{u}-{\mathbf{u}}_r{)}^T\mathbf{Q}(\mathbf{u}-{\mathbf{u}}_r)+V_o$$
其中$\mathbf{x}=[x,y{]}^T$，${\mathbf{x}}_r=[x_r,y_r{]}^T$，$\mathbf{u}=[v_x,v_y{]}^T$，${\mathbf{u}}_r=[v_{x_r},v_{y_r}{]}^T$，$\mathbf{P}$和$\mathbf{Q}$为权重矩阵，$V_o$可以用势场的方法获取:
$$V_{oi}=(\min (0,\frac{d_{oi}^2-r_d^2}{d_{oi}^2-r_{oi}^2}){)}^2,i=1,2,...,N_o$$
$d_{oi}$表示了当前时刻无人机与该障碍物的距离，$r_d$表示无人机的探测距离，$r_{oi}$表示对应障碍物的半径，然后相加得到整体的势函数。
$$V_o=k_o{\Sigma }_{i=1}^{N_o}{V}_{oi}$$
$k_o>0$用于调节权重。
然后基于上述成本函数和运动方程，预测N个周期的整体成本函数，基于模型预测控制(MPC,具体的说明可以搜索相关书籍论文等，这里就不再说理论方面了)构建优化问题实时求解合适的速度使得整体成本最小，从而实现轨迹跟踪与避障等。

2. C++代码构建

下面针对上面描述的简单的优化问题例子编写对应的代码实现求解，为了便于说明，这里把整个求解函数根据不同部分的意义分别说明用途以及给出对应的代码，完整的代码拼起来并自行补充相关参数的值即可。

2.1 初值与相关参数变量

首先需要确定一些需要用到的参数、变量以及初值等。

对于每个时刻的优化问题，初始值肯定是不一样的，我们可以每时刻获取当前的无人机位置等信息，这个信息从对应的ROS话题中获取。

Eigen::Vector3d zdlab_mpc_for_M600::mpc_solve_OCP_ENU_frame()
{
    SX x = m_current_local_pos.x;
    SX y = m_current_local_pos.y;
    //接下文....   
}

然后定义一些用到的变量:

Eigen::Vector3d zdlab_mpc_for_M600::mpc_solve_OCP_ENU_frame()
{
    //接上文...
    // 控制输入
    // int型的m_mpc_predict_step表示了MPC的预测周期，在类的构造函数中已给值
    SX Vx = SX::sym("Vx", m_mpc_predict_step);
    SX Vy = SX::sym("Vy", m_mpc_predict_step);

    // 独立变量
    // 存储每个周期的期望值
    double x_r[m_mpc_predict_step];
    double y_r[m_mpc_predict_step];

    double Vx_r[m_mpc_predict_step];
    double Vy_r[m_mpc_predict_step];

    // 记录每个预测时刻的实际量与期望量的偏差
    SX e_x = SX::sym("e_x", m_mpc_predict_step);
    SX e_y = SX::sym("e_y", m_mpc_predict_step);

    SX e_Vx = SX::sym("e_Vx", m_mpc_predict_step);
    SX e_Vy = SX::sym("e_Vy", m_mpc_predict_step);


    // 积分步长
    // double型的m_mission_step表示了整个程序的运行周期，即步长，可以自行取值，这里取的0.05s
    // int型的m_integration_steps表示了积分步数，如果为了节省时间，直接取1
    double dt = m_mission_step / (double)m_integration_steps;

    // 成本值
    SX f = 0;

    // 记录每个时刻的状态量
    std::vector x_k, y_k;    
    //接下文....
}

这里的SX是Casadi库中定义的类型，用来表示优化问题中的变量、数组和矩阵等，待寻最优的变量也必须是该类型，具体的说明可以参考文献，博主在编写时是基于相关例程编写的，具体的数据类型意义也了解不多。

2.2预测周期与成本值计算

然后根据MPC的思想构建N个预测步长的成本计算，代码如下:

Eigen::Vector3d zdlab_mpc_for_M600::mpc_solve_OCP_ENU_frame()
{
    //接上文...
    //-------MPC问题表述与计算-------

    // 外层为每个周期的预测
    // 一共m_mpc_predict_step个预测步长,从0开始到m_mpc_predict_step-1
    for (int k = 0; k < m_mpc_predict_step; k++)
    {
        // 内层为当前周期的数值积分
        for (int j = 0; j < m_integration_steps; j++)
        {
            // 这里的cos sin是casadi里定义的,使用std::cos std::sin会报错
            // \dot x = u * cos(\psi) - v sin(\psi)
            x += dt * Vx(k);
            // \dot y = u * sin(\psi) + v cos(\psi)
            y += dt * Vy(k);
        }

        x_k.push_back(x);
        y_k.push_back(y);

        // 根据编写的函数获得当前时刻的期望值
        Eigen::Matrix reference_now = reference_caculate_ENU_frame(m_mission_time + k * m_mission_step);

        x_r[k] = reference_now(0);
        y_r[k] = reference_now(1);

        Vx_r[k] = reference_now(3);
        Vy_r[k] = reference_now(4);     

        e_x(k) = x - x_r[k];
        e_y(k) = y - y_r[k];

        e_Vx(k) = Vx(k) - Vx_r[k];
        e_Vy(k) = Vy(k) - Vy_r[k];

        // 避障成本计算
        for (int i = 0; i < m_obstacle_num; i++)
        {
            // double型数组m_obstacle_r[i]存储了每个障碍物对应的半径
            SX radius_obstacle = m_obstacle_r[i];
            SX dection_radius = m_detection_radius;

            // double型的m_obstacle_x[i]和m_obstacle_y[i]即为障碍物的坐标
            // 注意这里的sqrt和下面的fmin不是std::sqrt以及std::fmin，而是casadi::fmin以及casadi::sqrt，因为这里的变量类型存在SX类型的，std里的无法对该类型处理，casadi库中有相关数学函数的重定义
            // 代码里未加std::等的一般都是casadi::的，在头文件中添加了using namespace casadi;
            SX distance_to_obstacle = sqrt((x - m_obstacle_x[i]) * (x - m_obstacle_x[i]) + (y - m_obstacle_y[i]) * (y - m_obstacle_y[i]));
            SX cost_obstacle;

            SX flag_obstacle = fmin(dection_radius, distance_to_obstacle);

            // V_o = (min(0, (d_o^2 - R^2)/(d_o^2 - r^2)))^2
            cost_obstacle = (flag_obstacle * flag_obstacle - dection_radius * dection_radius) / (flag_obstacle * flag_obstacle - radius_obstacle * radius_obstacle);
            cost_obstacle = m_obstacle_gain * cost_obstacle * cost_obstacle;
            f += cost_obstacle;
        }
    }    
    //接下文....
}

这里的期望值获取可以参考下面的函数

Eigen::Matrix zdlab_mpc_for_M600::reference_caculate_ENU_frame(double sim_time_now)
{
    double wt = m_trajectory_omega * m_trajectory_rho * sim_time_now;

    double y_ref = m_trajectory_rho * sim_time_now;
    double x_ref = m_trajectory_amplitude * std::sin(wt);

    double Vy_ref = m_trajectory_rho;
    double Vx_ref = m_trajectory_amplitude * m_trajectory_omega * m_trajectory_rho * std::cos(wt);

    Eigen::Matrix reference_state_and_input;
    reference_state_and_input << x_ref, y_ref, 0, Vx_ref, Vy_ref, 0;
    return reference_state_and_input;
}

完成了预测周期的描述以及势函数的计算后，再追加状态成本和输入成本的计算:

Eigen::Vector3d zdlab_mpc_for_M600::mpc_solve_OCP_ENU_frame()
{
    //接上文...
    // 两组权值已在类的构造函数中赋值
    // 状态成本
    f += m_mpc_K_state[0] * dot(e_x, e_x) + m_mpc_K_state[1] * dot(e_y, e_y);
    // 输入成本
    f += m_mpc_K_input[0] * dot(e_Vx, e_Vx) + m_mpc_K_input[1] * dot(e_Vy, e_Vy);
    //接下文....
}

2.3 优化问题构建与求解

然后调用相关函数，利用上述变量表明要求解的问题。

Eigen::Vector3d zdlab_mpc_for_M600::mpc_solve_OCP_ENU_frame()
{
    //接上文...
    SX x_all = vertcat(x_k);
    SX y_all = vertcat(y_k);

    // 需要加上SX::,参考casadi库文件中C++示例文件里rocket相关代码
    SX input = SX::vertcat({Vx, Vy});
    SX constraint = SX::vertcat({x_all, y_all});

    // "x"表示微分状态,即待求解最优的输入变量
    // "f"表示目标函数,即要求最优的成本
    SXDict nlp = {{"x", input}, {"f", f}, {"g", constraint}};

    Dict opts;
    opts["ipopt.tol"] = 1e-5;
    opts["ipopt.max_iter"] = 100;

    Function solver = nlpsol("solver", "ipopt", nlp, opts);    
    //接下文....
}

而后为了对相关状态添加限制条件，构造相关的vector添加各个状态的限制条件

Eigen::Vector3d zdlab_mpc_for_M600::mpc_solve_OCP_ENU_frame()
{
    //接上文...

    // 状态和输入限制
    // 各个上下界参数在构造函数中赋值
    // Vx, Vy上下界
    double value_input_min[2 * m_mpc_predict_step];
    double value_input_max[2 * m_mpc_predict_step];

    double value_constraint_min[2 * m_mpc_predict_step];
    double value_constraint_max[2 * m_mpc_predict_step];

    // 输入限制具体值设置
    for (int i = 0; i < m_mpc_predict_step; i++)
    {
        // Vx上下界
        value_input_min[i] = m_constraint_u_min;
        value_input_max[i] = m_constraint_u_max;
        // Vy上下界
        value_input_min[i + m_mpc_predict_step] = m_constraint_v_min;
        value_input_max[i + m_mpc_predict_step] = m_constraint_v_max;
    }

    for (int i = 0; i < m_mpc_predict_step; i++)
    {
        // x上下界
        value_constraint_min[i] = m_constraint_x_min;
        value_constraint_max[i] = m_constraint_x_max;

        // y上下界
        value_constraint_min[i + m_mpc_predict_step] = m_constraint_y_min;
        value_constraint_max[i + m_mpc_predict_step] = m_constraint_y_max;
    }

    std::vector input_min(value_input_min, value_input_min + 2 * m_mpc_predict_step);
    std::vector input_max(value_input_max, value_input_max + 2 * m_mpc_predict_step);

    std::vector constraint_min(value_constraint_min, value_constraint_min + 2 * m_mpc_predict_step);
    std::vector constraint_max(value_constraint_max, value_constraint_max + 2 * m_mpc_predict_step);

    //接下文....
}

完成了上述从预测周期描述，成本计算到状态限制等一系列步骤后，调用相关函数添加限制条件并进行求解。

Eigen::Vector3d zdlab_mpc_for_M600::mpc_solve_OCP_ENU_frame()
{
    //接上文...
    // "lbx"表示状态量(这里定义的是输入,即v_x,v_y)的下界
    // "ubx"表示状态量(这里定义的是输入,即v_x,v_y)的上界
    // "lbg"表示定义的变量的下界
    // "ubg"表示定义的变量的上界
    // "x0"表示状态量(这里定义的是输入,即v_x,v_y)的寻优初始值,一共2*m_mpc_predict_step个,方便起见直接全部从0开始迭代寻优
    DMDict arg = {{"lbx", input_min}, {"ubx", input_max}, {"lbg", constraint_min}, {"ubg", constraint_max}, {"x0", 0}};

    DMDict res = solver(arg);

    // 输出成本值
    double res_cost(res.at("f"));

    // 输出最优解(控制序列)
    std::vector res_control_all(res.at("x"));

    std::vector res_control_u, res_control_v, res_control_r;

    res_control_u.assign(res_control_all.begin(), res_control_all.begin() + m_mpc_predict_step);
    res_control_v.assign(res_control_all.begin() + m_mpc_predict_step, res_control_all.begin() + 2 * m_mpc_predict_step);
    
    // 采用求解得到的控制序列的第一组作为当前控制量
    Eigen::Vector3d control_input_now;
    // 由于该函数里不计算角速度,这里的为0
    control_input_now << res_control_u.front(), res_control_v.front(), 0;
    return control_input_now;
}

至此，完成了整个求解函数的编写，在ROS定时器回调函数中调用该函数求解，不断地向无人机发送速度指令即可完成程序的控制目标。

3. 遇到的一些bug

在写上面的程序时遇到了一些bug，记录一下可能对改程序有用的问题。

3.1 段错误相关

由于代码里用到了比较多的向量和矩阵等，因此很容易出现超出矩阵索引的问题，在跑程序如果遇到了segmentation fault的问题，仔细查阅写的代码看哪里有没有越界访问。

3.2 相关参数未赋值

在类的构造函数中需要对相关参数进行赋值，以便在求解优化问题的时候计算，如果一些变量只是在类中定义而没有赋值，则对应参数很可能会是一个非常大的数，比如预测步数或者积分步数未赋值，会导致卡在预测周期与成本值计算中一直无法完成，程序在第一次求优便始终无法获取最优解。

3.3 期望值给出错误

在程序能正常运行后，一开始的结果比较奇怪，实际轨迹与期望相差了太多，完全看不出两条曲线有什么一致的地方，后来仔细检查发现每个时刻计算的期望值实际上是远远超过这个时刻的一个时间点的期望值，从而导致跟踪出现了问题。