文章目录

0 前言
1 输入输出接口
- 1.1无人机状态量话题订阅
- 1.2 控制指令发布
2 程序流程
- 2.1 起飞准备工作
- - 2.1.1 等待解锁并起飞
  - 2.1.2 起飞至定点
- 2.2 优化问题求解
- - 2.2.1 MPC求解
  - 2.2.2 发布指令
  - 2.2.3 数据记录
- 2.3 算法结束返航
- 2.4 CMakeLists编译指令添加
3 仿真与实验测试
- 3.1 仿真测试
- 3.2 实验测试

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_msgs::LogMessageControl>("/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_msgs::DroneState>("/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_msgs::ControlCommand>("/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_POSint Move_mode = 0;// Move_frame: 0 for ENU_FRAME, 1 for BODY_FRAMEint 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();

具体的求解函数与前一篇博客的比较相似，只不过这里用的惯性系坐标与机体系速度的方程，比惯性系下的稍微复杂些，即
x˙=ucos⁡(ψ)−vsin⁡(ψ)y˙=usin⁡(ψ)+vcos⁡(ψ)ψ˙=r% x,y为水平坐标,psi为偏航角 % u,v为机体系水平速度,r为偏航角速率 \dot x = u \cos(\psi) - v \sin(\psi)\\ \dot y = u \sin(\psi) + v \cos(\psi)\\ \dot \psi = r x˙=ucos(ψ)−vsin(ψ)y˙=usin(ψ)+vcos(ψ)ψ˙=r
具体的优化问题构建与求解不再进行描述，可以参考对应求解函数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_AVOIDANCEfor (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;
#elsem_mpc_record << "legend([legend_record_actual_trajectory, legend_record_reference_trajectory], '实际轨迹', '期望轨迹', 'FontSize',legend_width);" << std::endl;
#endifm_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模式，而后启动本程序。

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

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

多旋翼无人机ROSC++开发例程(一)：环境配置
文章目录前言 1.ROS环境配置 2.串口绑定 3.添加开机启动项 4.Casadi环境配置前言作为一名无人机飞控方向的研究生,这几年在学习了许多大佬的开源项目和博客后进行了多旋翼无人机机载计算 ...
基于STM32F407四旋翼无人机---AK8975磁力计（四）
基于STM32F407四旋翼无人机---AK8975磁力计(四) 磁力计基本介绍 1.2.磁力计原理图 2.磁力计数据获取 3.磁力计椭球拟合校准 3.1.简单介绍椭球拟合磁力计基本介绍该模块采用 ...
AI开发者大会之AI学习与进阶实践：2020年7月3日《如何转型搞AI？》、《基于AI行业价值的AI学习与进阶路径》、《自动机器学习与前沿AI开源项目》、《使用TensorFlow实现经典模型》
AI开发者大会之AI学习与进阶实践:2020年7月3日<如何转型搞AI?>+<无行业不智能:基于AI行业价值的AI学习与进阶路径>.<自动机器学习与前沿AI开源项目> ...
我们的开源项目-2013年度开源社区线下聚会《JEECG微云快速开发平台-SAAS企业应用在线开发与微信移动应用》PPT分享
我们的开源项目-2013年度开源社区线下聚会<JEECG微云快速开发平台-SAAS企业应用在线开发与微信移动应用>PPT分享下载地址:http://pan.baidu.com/s/1hq ...
无人机开源项目_8个开源无人机项目
无人机开源项目编者注:本文最初发表于2016年12月,现已更新以包含其他信息. 在过去的几年中,对民用,军事和商用无人机的兴趣Swift增长,这也带动了制造商社区对开源无人机项目的兴趣. 符合无人机 ...
【音视频安卓开发（四）】AndroidStudio项目配置权限、jni库路径、ABI
读写权限权限(版本) 添加jni用到库所在的路径设置cmake和NDK参数 cmake项目配置
深度学习（四十）优化求解系列（2）简单理解神经网络求解过程-未完待续
对于神经网络的求解过程BP,其实说白了就是复合函数的求导过程,所以我们需要先复习一下高数复合函数的求导过程. 一.复合函数的求导法则 1.复合函数: 此函数是一个包含了三层映射过程的复合函数,为了跟后 ...
【源码】基于非线性Newmark方法的隐式动态求解器
求解函数 function Result=Newmark_Nonlinear(Elements,Material,Support,Free,M,C,f,fs,delta) Input Elements ...
四旋翼无人机硬件，飞控，基站，NX
四旋翼无人机硬件一总览四旋翼无人机的硬件组成:无刷电机(4个):电子调速器(简称电调,4个,常见有好盈.中特威.新西达等品牌):螺旋桨(4个,需要2个正浆,2个反浆):飞控:电池(11.1v航模 ...
四旋翼无人机建模与实现（一）
四旋翼无人机建模与实现(一) 本文是对GitHub上的一个开源的四旋翼无人机建模与编程实现的学习笔记,因为原作者的文档使用英文写的,花了很长的时间才看懂,因此,我觉得用自己的语言将这个开源项目分享出来 ...

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