多旋翼无人机ROS&C++开发例程(四)：基于Prometheus开源项目与Casadi开源优化求解器的模型预测控制简单应用例程

0 前言

这篇博客主要介绍基于Prometheus开源项目进行多旋翼无人机开发，在前面所介绍的通过Casadi开源优化求解器实时求解模型预测控制(MPC)优化问题应用进去，实现仿真与实际应用的简单开发。
示例程序的代码仓库下载链接为prometheus_for_mpc，相关环境配置可以按照Prometheus项目的wiki配置环境，如果已经在自己的电脑上配置过Prometheus的相关环境并能正常运行，则只需要再按照第一篇博客中的方法配置Casadi的环境即可。

如果已经下载了Prometheus的开源代码，可以只拷贝MPC程序,即Modules\control\src\mpc_test.cpp与Modules\control\include\mpc_test.h到同样的路径，并按照后续的CMakeLists编译指令在Modules\control\CMakeLists.txt添加几行所需的语句即可。

1 输入输出接口

通过Prometheus中的相关话题可以方便的将之前编写的程序调整输入输出接口，完成移植，在使用中用到的一些话题如下所示。

1.1无人机状态量话题订阅

参考ground_station.cpp中订阅的相关消息如下：

prometheus_control模块回传的消息:

    ros::Subscriber log_control_sub = nh.subscribe("/prometheus/log/control", 10, log_control_cb);

回调函数:

void log_control_cb(const prometheus_msgs::LogMessageControl::ConstPtr& msg)
{
    control_type = msg->control_type;
    _DroneState = msg->Drone_State;
    Command_Now = msg->Control_Command;
    _AttitudeReference = msg->Attitude_Reference;
    ref_pose = msg->ref_pose;
}

该话题来自px4_pos_controller.cpp或px4_geometry_controller.cpp或px4_sender.cpp.
回调函数中各个返回值为:
msg->control_type在px4_sender.cpp中的返回值为PX4_SENDER(0),在px4_pos_controller.cpp中的返回值为PX4_POS_CONTROLLER(1),在px4_geometry_controller.cpp中的返回值为PX4_GEO_CONTROLLER(2).

msg->Drone_State来自px4_pos_estimator.cpp中发布的话题:

    // 【发布】无人机状态量
    drone_state_pub = nh.advertise("/prometheus/drone_state", 10);

无人机状态量通过mavros获取，在px4_pos_estimator.cpp中定义了这样的变量:

    // 用于与mavros通讯的类，通过mavros接收来至飞控的消息【飞控->mavros->本程序】
    state_from_mavros _state_from_mavros;

state_from_mavros类的定义在state_from_mavros.h中,该类订阅了无人机的当前位置(ENU),当前速度(ENU),当前姿态(ENU)和相对高度(仅针对户外).在px4_pos_estimator.cpp定义了该类的对象，在主循环中实时刷新ros服务,然后发布各种从mavros获得的数据.

1.2 控制指令发布

发布话题:

 //　【发布】　控制指令
move_pub = nh.advertise("/prometheus/control_command", 10);

控制消息的类型在ControlCommand.msg中定义，包含怠速(0)，起飞(1)，悬停(2)，降落(3)，移动(4)，解锁(5)以及两个待添加的用户模式(6)(7)。

在一开始运行时初始化各个控制量，怠速等待其他控制指令

    // 初始化命令 - Idle模式 电机怠速旋转 等待来自上层的控制指令   
	Command_to_pub.Mode                                = prometheus_msgs::ControlCommand::Idle;
	//一开始的控制指令ID为0,然后累加
    Command_to_pub.Command_ID                          = 0;
    Command_to_pub.source = NODE_NAME;
    Command_to_pub.Reference_State.Move_mode           = prometheus_msgs::PositionReference::XYZ_POS;
    Command_to_pub.Reference_State.Move_frame          = prometheus_msgs::PositionReference::ENU_FRAME;
    Command_to_pub.Reference_State.position_ref[0]     = 0;
    Command_to_pub.Reference_State.position_ref[1]     = 0;
    Command_to_pub.Reference_State.position_ref[2]     = 0;
    Command_to_pub.Reference_State.velocity_ref[0]     = 0;
    Command_to_pub.Reference_State.velocity_ref[1]     = 0;
    Command_to_pub.Reference_State.velocity_ref[2]     = 0;
    Command_to_pub.Reference_State.acceleration_ref[0] = 0;
    Command_to_pub.Reference_State.acceleration_ref[1] = 0;
    Command_to_pub.Reference_State.acceleration_ref[2] = 0;
    Command_to_pub.Reference_State.yaw_ref             = 0;

除了MOVE以外的模式，在发送控制指令时包含的内容有

// 当前的时间
Command_to_pub.header.stamp = ros::Time::now();
// 模式，比如Idle、Takeoff、Hold、Land、Disarm
Command_to_pub.Mode = prometheus_msgs::ControlCommand::Idle;
// 指令ID，每次发送的时候累加
Command_to_pub.Command_ID = Command_to_pub.Command_ID + 1;
// NODE_NAME表明是哪个程序发送的控制指令，在一开始对该量进行宏定义，比如#define NODE_NAME "terminal_control"
Command_to_pub.source = NODE_NAME;
// 最后发送指令
move_pub.publish(Command_to_pub);

在发布MOVE类型的控制指令时需要用到PositionReference.msg中的各种消息，如位置和偏航角等，详细可查看该msg。

发布的内容包括：

// 时间         
Command_to_pub.header.stamp = ros::Time::now();
// MOVE模式          
Command_to_pub.Mode = prometheus_msgs::ControlCommand::Move;
// 控制指令ID          
Command_to_pub.Command_ID = Command_to_pub.Command_ID + 1;
// 节点名          
Command_to_pub.source = NODE_NAME;
// 追踪模式
Command_to_pub.Reference_State.Move_mode       = prometheus_msgs::PositionReference::XYZ_VEL;
// 坐标系          
Command_to_pub.Reference_State.Move_frame      = prometheus_msgs::PositionReference::BODY_FRAME;
//  具体的各个期望值，这里只是一个，还可以包含多个         
Command_to_pub.Reference_State.velocity_ref[2]     -= VEL_Z_STEP_SIZE;

2 程序流程

2.1 起飞准备工作

在开始MPC测试之前，首先要完成相关的准备工作，相关的步骤在mission_prepare函数中实现，完整的代码见发布的开源仓库，这里只记录下一些关键步骤。

2.1.1 等待解锁并起飞

这部分只是个提醒，提示在运行程序前确认遥控器已经解锁，并进入了offboard模式。

    std::cout << "Please swith to offboard mode" << std::endl;

    char key = 'n';

    std::cout << "press y to take off" << std::endl;

    // 等待解锁并切到Offboard模式
    while (key != 'y')
    {
        std::cout << "Is offboard mode?(y/n)" << std::endl;
        std::cin >> key;
    }

    // 完成解锁与切到offboard模式后发布起飞指令
    m_Command_to_pub.header.stamp = ros::Time::now();
    m_Command_to_pub.Mode = prometheus_msgs::ControlCommand::Takeoff;
    m_Command_to_pub.Command_ID = m_Command_to_pub.Command_ID + 1;
    m_Command_to_pub.source = NODE_NAME;
    m_control_pub.publish(m_Command_to_pub);

    std::cout << "UAV takeoff" << std::endl;

2.1.2 起飞至定点

起飞后则需要再飞至指定点(比如飞到几米的高度)，以开始MPC程序。

首先是发布期望坐标指令：

   // 发布期望位置，让飞机到指定点作为程序的出发点

    // Move_mode: 0 for XYZ_POS, 1 for XY_POS_Z_VEL, 2 for XY_VEL_Z_POS, 3 for XYZ_VEL, 5 for TRAJECTORY
    // 这里只需要飞到指定点,即XYZ_POS
    int Move_mode = 0;

    // Move_frame: 0 for ENU_FRAME, 1 for BODY_FRAME
    int Move_frame = 0;

    // 出发点设置
    m_Command_to_pub.Reference_State.position_ref[0] = m_start_position[0];
    m_Command_to_pub.Reference_State.position_ref[1] = m_start_position[1];
    m_Command_to_pub.Reference_State.position_ref[2] = m_start_position[2];

    m_Command_to_pub.Reference_State.velocity_ref[0] = 0;
    m_Command_to_pub.Reference_State.velocity_ref[1] = 0;
    m_Command_to_pub.Reference_State.velocity_ref[2] = 0;

    m_Command_to_pub.Reference_State.acceleration_ref[0] = 0;
    m_Command_to_pub.Reference_State.acceleration_ref[1] = 0;
    m_Command_to_pub.Reference_State.acceleration_ref[2] = 0;

    m_Command_to_pub.Reference_State.yaw_ref = m_start_psi;
    m_Command_to_pub.Reference_State.yaw_rate_ref = 0;

    m_Command_to_pub.header.stamp = ros::Time::now();
    m_Command_to_pub.Mode = prometheus_msgs::ControlCommand::Move;
    m_Command_to_pub.Command_ID = m_Command_to_pub.Command_ID + 1;
    m_Command_to_pub.source = NODE_NAME;
    m_Command_to_pub.Reference_State.Move_mode = Move_mode;
    m_Command_to_pub.Reference_State.Move_frame = Move_frame;
    m_Command_to_pub.Reference_State.time_from_start = -1;
    m_control_pub.publish(m_Command_to_pub);

然后用一个循环去判断是否已经到达了期望点并稳定了下来：

    double dist = 10;
    int count = 0;

    while (count < 10)
    {
        ros::spinOnce();

        int x = m_state_from_mavros._DroneState.position[0];
        int y = m_state_from_mavros._DroneState.position[1];
        int z = m_state_from_mavros._DroneState.position[2];

        dist = std::sqrt(std::pow(x - m_start_position[0], 2) + std::pow(y - m_start_position[1], 2) + std::pow(z - m_start_position[2], 2));
         ROS_INFO("dist from start point now is [%f]", dist);
        if (dist < 0.1)
        {
            count++;
        }
        ros::Duration(0.2).sleep();
    }

2.2 优化问题求解

完成了上述准备工作后，启动定时器周期求解优化问题，并向无人机发送指令，以实现轨迹跟踪与避障(障碍物坐标与半径为已知，程序不包含障碍物检测)。

obj.m_mission_Timer = obj.m_nh.createTimer(ros::Duration(obj.m_mission_step), &mpc_test::mission_callback, &obj);  

定时器回调函数主要完成了以下几个功能：

2.2.1 MPC求解

    // 调用求解函数得到最优解
    m_control_input_now = mpc_solve_OCP_body_frame();

具体的求解函数与前一篇博客的比较相似，只不过这里用的惯性系坐标与机体系速度的方程，比惯性系下的稍微复杂些，即
$$\begin{gathered} \dot{x}=u\cos (\psi )-v\sin (\psi ) \\ \dot{y}=u\sin (\psi )+v\cos (\psi ) \\ \dot{\psi }=r \end{gathered}$$
具体的优化问题构建与求解不再进行描述，可以参考对应求解函数mpc_solve_OCP_body_frame()。

2.2.2 发布指令

调用求解函数后返回的解作为当前时刻的控制指令发布给飞控：

    // 由于姿态环的响应，需要一定时间后才跟上发送的指令，不过在实验中对于小型的四旋翼效果还可以
    // 求解得到的是机体坐标系下的速度,需要转换为惯性系下的
    double control_vel_x = m_control_input_now(0) * std::cos(m_state_from_mavros._DroneState.attitude[2]) - m_control_input_now(1) * std::sin(m_state_from_mavros._DroneState.attitude[2]);
    double control_vel_y = m_control_input_now(0) * std::sin(m_state_from_mavros._DroneState.attitude[2]) + m_control_input_now(1) * std::cos(m_state_from_mavros._DroneState.attitude[2]);
    double control_vel_z = 0;
    double control_yaw_rate = m_control_input_now(2);

    // 发布速度控制指令
    m_Command_to_pub.header.stamp = ros::Time::now();
    m_Command_to_pub.Mode = prometheus_msgs::ControlCommand::Move;
    m_Command_to_pub.Command_ID = m_Command_to_pub.Command_ID + 1;
    m_Command_to_pub.source = NODE_NAME;
    m_Command_to_pub.Reference_State.Move_mode = prometheus_msgs::PositionReference::XY_VEL_Z_POS;
    m_Command_to_pub.Reference_State.Move_frame = prometheus_msgs::PositionReference::ENU_FRAME;

    m_Command_to_pub.Reference_State.velocity_ref[0] = control_vel_x;
    m_Command_to_pub.Reference_State.velocity_ref[1] = control_vel_y;
    m_Command_to_pub.Reference_State.yaw_rate_ref = control_yaw_rate;

    m_Command_to_pub.Reference_State.velocity_ref[2] = 0;
    m_Command_to_pub.Reference_State.Yaw_Rate_Mode = true;
    m_control_pub.publish(m_Command_to_pub);

2.2.3 数据记录

为了方便分析实验结果，程序在开始时创建了根据当前时刻命名的.m文件，并在运行时实时记录当前的相关数据，在程序完成时又添加了绘图指令方便一键出图。

MPC_record_data()函数记录了无人机的坐标、偏航角等信息，比如：

   // 输出当前时间
    m_mpc_record << "t(" << m_iterator_step << ")=" << m_mission_time << ";" << std::endl;

    // 输出当前状态
    m_mpc_record << "x(" << m_iterator_step << ")=" << m_state_from_mavros._DroneState.position[0] << ";\t"
                 << "y(" << m_iterator_step << ")=" << m_state_from_mavros._DroneState.position[1] << ";\t"
                 << "psi(" << m_iterator_step << ")=" << m_state_from_mavros._DroneState.attitude[2] << ";"
                 << std::endl;

    // 输出当前期望状态和输入
    m_mpc_record << "x_r(" << m_iterator_step << ")=" << m_reference_state_and_input(0) << ";\t"
                 << "y_r(" << m_iterator_step << ")=" << m_reference_state_and_input(1) << ";\t"
                 << "psi_r(" << m_iterator_step << ")=" << m_reference_state_and_input(2) << ";"
                 << std::endl;

2.3 算法结束返航

实时求解优化问题至设定的时间后便完成了程序测试，调用MPC_plot_data()函数添加最后的绘图指令方便出图，比如避障与轨迹跟踪效果图：

    m_mpc_record << "%plot the results" << std::endl;

    m_mpc_record << "close all;" << std::endl;
    m_mpc_record << "line_width = 2;" << std::endl;
    m_mpc_record << "legend_width = 25;" << std::endl;
    m_mpc_record << "label_width = 25;" << std::endl;
    m_mpc_record << "axis_fontsize = 30;" << std::endl;
    m_mpc_record << "axis_linewidth = 1;" << std::endl;

    // 绘制二维示意图
    m_mpc_record << "real_main = figure('NumberTitle', 'off', 'Name', '避障效果图');" << std::endl;
    m_mpc_record << "legend_record_actual_trajectory = plot(x, y, '-r', 'LineWidth', line_width);" << std::endl;
    m_mpc_record << "hold on;" << std::endl;
    m_mpc_record << "legend_record_reference_trajectory = plot(x_r, y_r, '--g', 'LineWidth', line_width);" << std::endl;

#ifdef OBSTACLE_AVOIDANCE
    for (int i = 0; i < m_obstacle_num; i++)
    {
        m_mpc_record << "hold on;" << std::endl;
        m_mpc_record << "legend_obstacle = viscircles([" << m_obstacle_x[i] << "," << m_obstacle_y[i] << "]," << m_obstacle_r[i] << ", 'Color', 'b', 'linestyle', '-');" << std::endl;
        // m_mpc_record << "viscircles([" << m_obstacle_x << "," << m_obstacle_y << "]," << m_detection_radius << ", 'Color', 'y', 'linestyle', '--');" << std::endl;
    }
    m_mpc_record << "legend([legend_record_actual_trajectory, legend_record_reference_trajectory, legend_obstacle], '实际轨迹', '期望轨迹', '障碍物','FontSize',legend_width);" << std::endl;
#else
    m_mpc_record << "legend([legend_record_actual_trajectory, legend_record_reference_trajectory], '实际轨迹', '期望轨迹', 'FontSize',legend_width);" << std::endl;
#endif

    m_mpc_record << "axis equal;" << std::endl;
    m_mpc_record << "xlabel('$x(m)$','FontSize',label_width, 'Interpreter','latex');" << std::endl;
    m_mpc_record << "ylabel('$y(m)$','FontSize',label_width, 'Interpreter','latex');" << std::endl;
    // 带有中文字符的图例不能加'FontName','Times New Roman',不然图例会出问题
    m_mpc_record << "set(gca,'FontSize',axis_fontsize,'LineWidth',axis_linewidth);" << std::endl;
    m_mpc_record << "grid on;" << std::endl;

    m_mpc_record << "set(real_main, 'position', get(0,'ScreenSize'));" << std::endl;
    m_mpc_record << "saveas(gca, 'real_main', 'epsc');" << std::endl;
    m_mpc_record << std::endl;

而后调用mission_finish()进行返航，即回到原点上方，并降落至地面，不过降落命令可能在实验中存在问题,由于高度的误差并不像水平那样错一点没太大关系,高度错一点还是没落地，如果传感器存在偏差则可能在接近落地的时候还没完全落地便停机了。在测试的时候出现过飞机距离地面还有大概十几厘米直接停机的情况。

2.4 CMakeLists编译指令添加

首先在开头添加Casadi环境相关的指令

# Casadi path
# set(casadi_DIR ../../../libcasadi-linux-gcc5-v3.5.5/lib/cmake/casadi)
find_package(casadi REQUIRED)
if(casadi_FOUND)
        message(STATUS "Found casadi ${casadi_VERSION} installed in the system")
        set(casadi_INSTALLED TRUE)
        message(“casadi dir ${casadi_INCLUDE_DIRS}”)
        message(“casadi lib ${casadi_LIBS}”)
        include_directories(/usr/local/include/casadi)
else()
        message(STATUS "Did not find casadi in the system")
endif()

而后在声明可执行cpp文件最后添加cpp编译

add_executable(mpc_test src/mpc_test.cpp)
add_dependencies(mpc_test  prometheus_control_gencpp)
target_link_libraries(mpc_test casadi  ${catkin_LIBRARIES})

3 仿真与实验测试

3.1 仿真测试

在进行实验之前，需要通过gazebo仿真测试算法是否可靠，首先启动Gazebo仿真场景(具体指令建议按照相关launch文件确认，防止文件名变动)

roslaunch prometheus_gazebo sitl_control.launch

通过启动的terminal_control程序，按照说明输入0和999(仅在仿真中)解锁飞机并切换至offboard模式，启动本程序即可。

rosrun prometheus_control mpc_test

3.2 实验测试

实验中首先启动以下文件

roslaunch prometheus_experiment px4_sender_ground.launch
roslaunch prometheus_experiment px4_sender_onboard.launch

通过终端确认机载计算机已经与飞控连接后，通过遥控器解锁并切换至offboard模式，而后启动本程序。

最后，进行实验测试务必注意安全，如果出现问题及时退出程序并切回手动。