文章目錄

• 0 前言
• 1 輸入輸出接口
• • 1.1無人機狀態(tài)量話題訂閱
  • 1.2 控制指令發(fā)布
• 2 程序流程
• • 2.1 起飛準(zhǔn)備工作
  • • 2.1.1 等待解鎖并起飛
    • 2.1.2 起飛至定點
  • 2.2 優(yōu)化問題求解
  • • 2.2.1 MPC求解
    • 2.2.2 發(fā)布指令
    • 2.2.3 數(shù)據(jù)記錄
  • 2.3 算法結(jié)束返航
  • 2.4 CMakeLists編譯指令添加
• 3 仿真與實驗測試
• • 3.1 仿真測試
  • 3.2 實驗測試

0 前言

這篇博客主要介紹基于Prometheus開源項目進行多旋翼無人機開發(fā)，在前面所介紹的通過Casadi開源優(yōu)化求解器實時求解模型預(yù)測控制(MPC)優(yōu)化問題應(yīng)用進去，實現(xiàn)仿真與實際應(yīng)用的簡單開發(fā)。
示例程序的代碼倉庫下載鏈接為prometheus_for_mpc，相關(guān)環(huán)境配置可以按照Prometheus項目的wiki配置環(huán)境，如果已經(jīng)在自己的電腦上配置過Prometheus的相關(guān)環(huán)境并能正常運行，則只需要再按照第一篇博客中的方法配置Casadi的環(huán)境即可。

如果已經(jīng)下載了Prometheus的開源代碼，可以只拷貝MPC程序,即Modules\control\src\mpc_test.cpp與Modules\control\include\mpc_test.h到同樣的路徑，并按照后續(xù)的CMakeLists編譯指令在Modules\control\CMakeLists.txt添加幾行所需的語句即可。

1 輸入輸出接口

通過Prometheus中的相關(guān)話題可以方便的將之前編寫的程序調(diào)整輸入輸出接口，完成移植，在使用中用到的一些話題如下所示。

1.1無人機狀態(tài)量話題訂閱

參考ground_station.cpp中訂閱的相關(guān)消息如下：

prometheus_control模塊回傳的消息:

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

回調(diào)函數(shù):

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.
回調(diào)函數(shù)中各個返回值為:
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中發(fā)布的話題:

// 【發(fā)布】無人機狀態(tài)量drone_state_pub = nh.advertise<prometheus_msgs::DroneState>("/prometheus/drone_state", 10);

無人機狀態(tài)量通過mavros獲取，在px4_pos_estimator.cpp中定義了這樣的變量:

// 用于與mavros通訊的類，通過mavros接收來至飛控的消息【飛控->mavros->本程序】state_from_mavros _state_from_mavros;

state_from_mavros類的定義在state_from_mavros.h中,該類訂閱了無人機的當(dāng)前位置(ENU),當(dāng)前速度(ENU),當(dāng)前姿態(tài)(ENU)和相對高度(僅針對戶外).在px4_pos_estimator.cpp定義了該類的對象，在主循環(huán)中實時刷新ros服務(wù),然后發(fā)布各種從mavros獲得的數(shù)據(jù).

1.2 控制指令發(fā)布

發(fā)布話題:

//　【發(fā)布】　控制指令 move_pub = nh.advertise<prometheus_msgs::ControlCommand>("/prometheus/control_command", 10);

控制消息的類型在ControlCommand.msg中定義，包含怠速(0)，起飛(1)，懸停(2)，降落(3)，移動(4)，解鎖(5)以及兩個待添加的用戶模式(6)(7)。

在一開始運行時初始化各個控制量，怠速等待其他控制指令

// 初始化命令 - Idle模式 電機怠速旋轉(zhuǎn) 等待來自上層的控制指令 Command_to_pub.Mode = prometheus_msgs::ControlCommand::Idle;//一開始的控制指令I(lǐng)D為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以外的模式，在發(fā)送控制指令時包含的內(nèi)容有

// 當(dāng)前的時間 Command_to_pub.header.stamp = ros::Time::now(); // 模式，比如Idle、Takeoff、Hold、Land、Disarm Command_to_pub.Mode = prometheus_msgs::ControlCommand::Idle; // 指令I(lǐng)D，每次發(fā)送的時候累加 Command_to_pub.Command_ID = Command_to_pub.Command_ID + 1; // NODE_NAME表明是哪個程序發(fā)送的控制指令，在一開始對該量進行宏定義，比如#define NODE_NAME "terminal_control" Command_to_pub.source = NODE_NAME; // 最后發(fā)送指令 move_pub.publish(Command_to_pub);

在發(fā)布MOVE類型的控制指令時需要用到PositionReference.msg中的各種消息，如位置和偏航角等，詳細可查看該msg。

發(fā)布的內(nèi)容包括：

// 時間 Command_to_pub.header.stamp = ros::Time::now(); // MOVE模式 Command_to_pub.Mode = prometheus_msgs::ControlCommand::Move; // 控制指令I(lǐng)D Command_to_pub.Command_ID = Command_to_pub.Command_ID + 1; // 節(jié)點名 Command_to_pub.source = NODE_NAME; // 追蹤模式 Command_to_pub.Reference_State.Move_mode = prometheus_msgs::PositionReference::XYZ_VEL; // 坐標(biāo)系 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 起飛準(zhǔn)備工作

在開始MPC測試之前，首先要完成相關(guān)的準(zhǔn)備工作，相關(guān)的步驟在mission_prepare函數(shù)中實現(xiàn)，完整的代碼見發(fā)布的開源倉庫，這里只記錄下一些關(guān)鍵步驟。

2.1.1 等待解鎖并起飛

這部分只是個提醒，提示在運行程序前確認遙控器已經(jīng)解鎖，并進入了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模式后發(fā)布起飛指令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程序。

首先是發(fā)布期望坐標(biāo)指令：

// 發(fā)布期望位置，讓飛機到指定點作為程序的出發(fā)點// 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;// 出發(fā)點設(shè)置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);

然后用一個循環(huán)去判斷是否已經(jīng)到達了期望點并穩(wěn)定了下來：

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 優(yōu)化問題求解

完成了上述準(zhǔn)備工作后，啟動定時器周期求解優(yōu)化問題，并向無人機發(fā)送指令，以實現(xiàn)軌跡跟蹤與避障(障礙物坐標(biāo)與半徑為已知，程序不包含障礙物檢測)。

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

定時器回調(diào)函數(shù)主要完成了以下幾個功能：

2.2.1 MPC求解

// 調(diào)用求解函數(shù)得到最優(yōu)解m_control_input_now = mpc_solve_OCP_body_frame();

具體的求解函數(shù)與前一篇博客的比較相似，只不過這里用的慣性系坐標(biāo)與機體系速度的方程，比慣性系下的稍微復(fù)雜些，即
$$\begin{gathered} \dot{x}=u\cos (\psi )?v\sin (\psi ) \\ \dot{y}=u\sin (\psi )+v\cos (\psi ) \\ \dot{\psi }=r \end{gathered}$$
具體的優(yōu)化問題構(gòu)建與求解不再進行描述，可以參考對應(yīng)求解函數(shù)mpc_solve_OCP_body_frame()。

2.2.2 發(fā)布指令

調(diào)用求解函數(shù)后返回的解作為當(dāng)前時刻的控制指令發(fā)布給飛控：

// 由于姿態(tài)環(huán)的響應(yīng)，需要一定時間后才跟上發(fā)送的指令，不過在實驗中對于小型的四旋翼效果還可以// 求解得到的是機體坐標(biāo)系下的速度,需要轉(zhuǎn)換為慣性系下的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);// 發(fā)布速度控制指令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 數(shù)據(jù)記錄

為了方便分析實驗結(jié)果，程序在開始時創(chuàng)建了根據(jù)當(dāng)前時刻命名的.m文件，并在運行時實時記錄當(dāng)前的相關(guān)數(shù)據(jù)，在程序完成時又添加了繪圖指令方便一鍵出圖。

MPC_record_data()函數(shù)記錄了無人機的坐標(biāo)、偏航角等信息，比如：

// 輸出當(dāng)前時間m_mpc_record << "t(" << m_iterator_step << ")=" << m_mission_time << ";" << std::endl;// 輸出當(dāng)前狀態(tài)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;// 輸出當(dāng)前期望狀態(tài)和輸入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 算法結(jié)束返航

實時求解優(yōu)化問題至設(shè)定的時間后便完成了程序測試，調(diào)用MPC_plot_data()函數(shù)添加最后的繪圖指令方便出圖，比如避障與軌跡跟蹤效果圖：

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;

而后調(diào)用mission_finish()進行返航，即回到原點上方，并降落至地面，不過降落命令可能在實驗中存在問題,由于高度的誤差并不像水平那樣錯一點沒太大關(guān)系,高度錯一點還是沒落地，如果傳感器存在偏差則可能在接近落地的時候還沒完全落地便停機了。在測試的時候出現(xiàn)過飛機距離地面還有大概十幾厘米直接停機的情況。

2.4 CMakeLists編譯指令添加

首先在開頭添加Casadi環(huán)境相關(guān)的指令

# 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()

而后在聲明可執(zhí)行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仿真場景(具體指令建議按照相關(guān)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

通過終端確認機載計算機已經(jīng)與飛控連接后，通過遙控器解鎖并切換至offboard模式，而后啟動本程序。

最后，進行實驗測試務(wù)必注意安全，如果出現(xiàn)問題及時退出程序并切回手動。

總結(jié)

以上是生活随笔為你收集整理的多旋翼无人机ROSC++开发例程(四)：基于Prometheus开源项目与Casadi开源优化求解器的模型预测控制简单应用例程的全部內(nèi)容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網(wǎng)站內(nèi)容還不錯，歡迎將生活随笔推薦給好友。