【PX4&Simulink&Gazebo聯(lián)合仿真】在Simulink中使用ROS2控制無人機(jī)沿自定義圓形軌跡飛行并在Gazebo中可視化

這篇具有很好參考價值的文章主要介紹了【PX4&Simulink&Gazebo聯(lián)合仿真】在Simulink中使用ROS2控制無人機(jī)沿自定義圓形軌跡飛行并在Gazebo中可視化。希望對大家有所幫助。如果存在錯誤或未考慮完全的地方，請大家不吝賜教，您也可以點擊"舉報違法"按鈕提交疑問。

本篇文章介紹如何使用ROS2控制無人機(jī)沿自定義圓形軌跡飛行并在Gazebo中可視化，提供了Matlab/Simulink源代碼，以及演示效果圖。

環(huán)境：

MATLAB : R2022b

Ubuntu ：20.04 LTS

Windows ：Windows 10

ROS ：ROS2 Foxy

Python: 3.8.2

Visual Studio ：Visual Studio 2019

PX4 ：1.13.0

系統(tǒng)架構(gòu)

ROS2的應(yīng)用程序管道非常簡單，這要歸功于本地通信中間件（DDS/RTPS）。microRTPS橋接工具由運行在PX4上的客戶端和運行在計算機(jī)上的服務(wù)端組成，它們進(jìn)行通信以提供uORB和ROS2話題格式之間的雙向數(shù)據(jù)交換和話題轉(zhuǎn)換。使得可以創(chuàng)建直接與PX4的uORB話題接口的ROS2訂閱服務(wù)器或發(fā)布服務(wù)器節(jié)點，其結(jié)構(gòu)如下圖所示。

【PX4&Simulink&Gazebo聯(lián)合仿真】在Simulink中使用ROS2控制無人機(jī)沿自定義圓形軌跡飛行并在Gazebo中可視化,PX4-AutoPilot,無人機(jī),PX4,ROS2,Gazebo,仿真,Simulink,圓形軌跡

ROS 2使用px4_msgs包和px4_ROS_com包來確保使用匹配的話題定義來創(chuàng)建客戶端和服務(wù)端代碼。

px4_msgs包：px4 ROS話題定義，當(dāng)構(gòu)建該項目時會生成相應(yīng)的兼容ROS2節(jié)點的話題類型，以及IDL文件，由fastddsgen用于生成microRTPS代碼。

px4_ros_com包：服務(wù)端發(fā)布者和訂閱者的microRTPS代碼模板，構(gòu)建過程運行一個fastddsgen實例來生成micrortps_agent的代碼，該代碼可編譯為單個可執(zhí)行文件。

這樣在Ubuntu中就生成了一個可以調(diào)用uORB話題接口的ROS2節(jié)點，這個節(jié)點可以和運行在同一局域網(wǎng)下的Matlab/Simulink上的ROS2節(jié)點進(jìn)行通信，以實現(xiàn)PX4&Simulink&Gazebo聯(lián)合仿真。

Matlab官方例程Control a Simulated UAV Using ROS 2 and PX4 Bridge

Matlab官方給出了一個示例，該示例演示了如何從具有PX4自動駕駛儀的模擬無人機(jī)接收傳感器讀數(shù)和自動駕駛儀狀態(tài)，并發(fā)送控制命令來導(dǎo)航模擬無人機(jī)，可以作為參考。

Control a Simulated UAV Using ROS 2 and PX4 Bridge文章來源地址http://www.zghlxwxcb.cn/news/detail-836190.html

可以在Matlab命令行中輸入以下命令打開該例程所在位置。

openExample('uav_ros/ControlASimulatedUAVUsingROS2AndPX4BridgeExample')

運行所需的環(huán)境配置

請確保已經(jīng)安裝前一篇文章配置好了PX4+Gazebo+ROS2+FastDDS+Matlab+Simulink聯(lián)合調(diào)試環(huán)境。

【PX4-AutoPilot教程-開發(fā)環(huán)境】搭建PX4+Gazebo+ROS2+FastDDS+Matlab+Simulink聯(lián)合調(diào)試環(huán)境

PX4&Simulink&Gazebo聯(lián)合仿真實現(xiàn)方法

建立Simulink模型并完成基本配置

在Matlab工作文件夾中models文件夾中新建一個Simulink模型，我這里命名為TrajectoryFlight.slx，雙擊使用Simulink打開。

在【建模】欄打開【模型設(shè)置】，【求解器】欄中【求解器類型】選為【定步長】。

【硬件實現(xiàn)】欄中【Hardware board】選擇【ROS2】。

【代碼生成】欄中【接口】勾選【連續(xù)時間】。

仿真調(diào)速界面勾選【啟用調(diào)速以減慢仿真】。

整體框架

整體框架如下，主體是對時鐘進(jìn)行判斷，1-3秒是觸發(fā)Arm子系統(tǒng)，3-5秒是觸發(fā)Enable Offboard Control子系統(tǒng)，5-7秒是觸發(fā)Takeoff子系統(tǒng)，7秒后是觸發(fā)Trajectory Flight子系統(tǒng)。

各子系統(tǒng)實現(xiàn)原理

Arm子系統(tǒng)

Arm子系統(tǒng)中使用ROS2 Subscribe模塊訂閱/fmu/timesync/out話題，并使用Bus Selector分解話題獲取時間戳，將時間戳傳入Subsystem子系統(tǒng)。

無人機(jī)的解鎖是通過vehicle_command話題進(jìn)行的，它的定義在源碼Firmware/msg/vehicle_command.msg中，這個話題是地面站/nsh等終端發(fā)送的控制指令用的。

我們可以從任意已經(jīng)編譯過的固件中的Firmware\build\px4_fmu-v5_default\uORB\topics\vehicle_command.h文件中看到vehicle_command話題的結(jié)構(gòu)體定義。

	uint64_t timestamp;
	double param5;
	double param6;
	float param1;
	float param2;
	float param3;
	float param4;
	float param7;
	uint32_t command;
	uint8_t target_system;
	uint8_t target_component;
	uint8_t source_system;
	uint8_t source_component;
	uint8_t confirmation;
	bool from_external;
	uint8_t _padding0[2]; // required for logger

可以看到其結(jié)構(gòu)為：

時間戳+command命令+目標(biāo)系統(tǒng)號+目標(biāo)組件號+發(fā)出命令系統(tǒng)號+發(fā)出命令組件號+收到命令次數(shù)+數(shù)據(jù)包

在源碼Firmware/msg/vehicle_command.msg中可以檢索到解鎖的命令I(lǐng)D是：

uint16 VEHICLE_CMD_COMPONENT_ARM_DISARM = 400		# Arms / Disarms a component |1 to arm, 0 to disarm|

可以在注釋中看到用法，只需將param1的值賦值為1即可解鎖。

綜上，通過ROS2對無人機(jī)進(jìn)行解鎖的方法為：

訂閱/fmu/timesync/out獲得時間戳–>command設(shè)置為400、param1設(shè)置為1、target_system設(shè)置為1–>發(fā)布/fmu/vehicle_command/in話題。

Subsystem子系統(tǒng)中使用ROS2 Blank Message獲得px4_msgs/vehicle_command的話題類型，導(dǎo)入獲取到的時間戳、命令編號、傳入?yún)?shù)等，并使用ROS2 Publish模塊發(fā)布該話題。

Enable Offboard Control子系統(tǒng)

Enable Offboard Control子系統(tǒng)中使用ROS2 Subscribe模塊訂閱/fmu/timesync/out話題，并使用Bus Selector分解話題獲取時間戳，將時間戳傳入Subsystem子系統(tǒng)。

無人機(jī)進(jìn)入Offboard模式也是通過vehicle_command話題進(jìn)行的。

在源碼Firmware/msg/vehicle_command.msg中可以檢索到設(shè)置系統(tǒng)模式的命令I(lǐng)D是：

uint16 VEHICLE_CMD_DO_SET_MODE = 176			# Set system mode. |Mode, as defined by ENUM MAV_MODE| Empty| Empty| Empty| Empty| Empty| Empty|

這里的注釋寫的是將第一個參數(shù)param1設(shè)為模式的ID號，之后param2到param7設(shè)置為空，但是這里的注釋好像寫錯了。

在源碼Firmware/src/modules/commander/Commander.cpp中，官方寫的調(diào)節(jié)模式的命令是：

send_vehicle_command(vehicle_command_s::VEHICLE_CMD_DO_SET_MODE, 1, PX4_CUSTOM_MAIN_MODE_OFFBOARD);

send_vehicle_command()函數(shù)的定義為：

static bool send_vehicle_command(const uint32_t cmd, const float param1 = NAN, const float param2 = NAN,
				 const float param3 = NAN,  const float param4 = NAN, const double param5 = static_cast<double>(NAN),
				 const double param6 = static_cast<double>(NAN), const float param7 = NAN)
{
	vehicle_command_s vcmd{};
	vcmd.command = cmd;
	vcmd.param1 = param1;
	vcmd.param2 = param2;
	vcmd.param3 = param3;
	vcmd.param4 = param4;
	vcmd.param5 = param5;
	vcmd.param6 = param6;
	vcmd.param7 = param7;

	uORB::SubscriptionData<vehicle_status_s> vehicle_status_sub{ORB_ID(vehicle_status)};
	vcmd.source_system = vehicle_status_sub.get().system_id;
	vcmd.target_system = vehicle_status_sub.get().system_id;
	vcmd.source_component = vehicle_status_sub.get().component_id;
	vcmd.target_component = vehicle_status_sub.get().component_id;

	uORB::Publication<vehicle_command_s> vcmd_pub{ORB_ID(vehicle_command)};
	vcmd.timestamp = hrt_absolute_time();
	return vcmd_pub.publish(vcmd);
}

可以看出需要將param1賦值為1，將param2賦值為PX4_CUSTOM_MAIN_MODE_OFFBOARD才能切換為Offboard模式。

查詢PX4_CUSTOM_MAIN_MODE_OFFBOARD的定義，在源碼Firmware/src/modules/commander/px4_custom_mode.h中找到：

enum PX4_CUSTOM_MAIN_MODE {
	PX4_CUSTOM_MAIN_MODE_MANUAL = 1,
	PX4_CUSTOM_MAIN_MODE_ALTCTL,
	PX4_CUSTOM_MAIN_MODE_POSCTL,
	PX4_CUSTOM_MAIN_MODE_AUTO,
	PX4_CUSTOM_MAIN_MODE_ACRO,
	PX4_CUSTOM_MAIN_MODE_OFFBOARD,
	PX4_CUSTOM_MAIN_MODE_STABILIZED,
	PX4_CUSTOM_MAIN_MODE_RATTITUDE_LEGACY,
	PX4_CUSTOM_MAIN_MODE_SIMPLE /* unused, but reserved for future use */
};

PX4_CUSTOM_MAIN_MODE_OFFBOARD對應(yīng)的數(shù)字是6。

綜上，通過ROS2對無人機(jī)進(jìn)入Offboard模式的方法為：

訂閱/fmu/timesync/out獲得時間戳–>command設(shè)置為176、param1設(shè)置為1、param2設(shè)置為6、target_system設(shè)置為1–>發(fā)布/fmu/vehicle_command/in話題。

Takeoff子系統(tǒng)

Takeoff子系統(tǒng)中使用ROS2 Subscribe模塊訂閱/fmu/timesync/out話題，并使用Bus Selector分解話題獲取時間戳，將時間戳傳入SendCommand子系統(tǒng)。

offboard_control_mode話題是Offboard模式的心跳包，為了保證飛行的安全性，心跳包必須以最低2Hz的頻率發(fā)布，PX4在兩個Offboard命令之間有一個500ms的延時，如果超過此延時，系統(tǒng)會將回到無人機(jī)進(jìn)入Offboard模式之前的最后一個模式。

在源碼Firmware/msg/offboard_control_mode.msg中可以看到offboard_control_mode話題的定義。

# Off-board control mode

uint64 timestamp		# time since system start (microseconds)

bool position
bool velocity
bool acceleration
bool attitude
bool body_rate
bool actuator

因為要進(jìn)行位置控制所以需要將position賦值為true。

trajectory_setpoint話題是期望的位置，在源碼Firmware/msg/vehicle_local_position_setpoint.msg中可以看到trajectory_setpoint話題的定義。

# Local position setpoint in NED frame
# setting something to NaN means the state should not be controlled

uint64 timestamp	# time since system start (microseconds)

float32 x		# in meters NED
float32 y		# in meters NED
float32 z		# in meters NED
float32 yaw		# in radians NED -PI..+PI
float32 yawspeed	# in radians/sec
float32 vx		# in meters/sec
float32 vy		# in meters/sec
float32 vz		# in meters/sec
float32[3] acceleration # in meters/sec^2
float32[3] jerk # in meters/sec^3
float32[3] thrust	# normalized thrust vector in NED

# TOPICS vehicle_local_position_setpoint trajectory_setpoint

其中trajectory_setpoint話題和vehicle_local_position_setpoint話題的內(nèi)容是一樣的，源碼Firmware/msg/tools/urtps_bridge_topics.yaml中可以看到以下代碼。

  - msg:     vehicle_local_position_setpoint
    receive: true
  - msg:     trajectory_setpoint # multi-topic / alias of vehicle_local_position_setpoint
    base:    vehicle_local_position_setpoint
    receive: true

可以看出trajectory_setpoint話題是基于vehicle_local_position_setpoint話題的。

這里需要注意坐標(biāo)系是NED坐標(biāo)系，即北東地坐標(biāo)系，所以想讓無人機(jī)飛起來，z的賦值應(yīng)該為負(fù)數(shù)。

綜上，通過ROS2對無人機(jī)進(jìn)入Offboard模式起飛懸停的方法為：

訂閱/fmu/timesync/out獲得時間戳–>position設(shè)置為true、x設(shè)置為0、y設(shè)置為0、z設(shè)置為-5、target_system設(shè)置為1–>發(fā)布offboard_control_mode話題和trajectory_setpoint話題。

SendCommand子系統(tǒng)中使用ROS2 Blank Message獲得offboard_control_mode的話題類型和trajectory_setpoint的話題類型，導(dǎo)入獲取到的時間戳、傳入?yún)?shù)、期望位置等，并使用ROS2 Publish模塊發(fā)布這些話題。

Trajectory Flight子系統(tǒng)

Trajectory Flight子系統(tǒng)跟Takeoff子系統(tǒng)大體一樣，只不過在Desired Position部分有所改動，改為實時的發(fā)送自定義圓軌跡上的位置。

MATLAB Function將仿真時間作為輸入，輸出的是期望位置，函數(shù)內(nèi)部的代碼為：

function y = fcn(t)

r = 5;%圓的半徑
w = 0.5;%繞圓心的角速度

t = t - 7;%去掉軌跡飛行開始前的時間

position_x = r * (1 - cos(w * t));
position_y = r * sin(w * t);

y = single([position_x position_y -5]);

這里給定了圓的半徑、繞圓心的角速度，結(jié)算出每一時刻無人機(jī)的期望位置。

實現(xiàn)效果

Ubuntu中啟動Gazebo仿真和microrts_agent守護(hù)進(jìn)程，運行Simulink模型，可以看到Gazebo中的無人機(jī)已經(jīng)進(jìn)入Offboard模式并起飛懸停在5m的高度后沿自定義圓形軌跡飛行。

無人機(jī)在Gazebo中飛行時，無人機(jī)始終處于畫面中央，會帶著視角亂晃，在Gazebo中進(jìn)行任何操作視角都無法固定，分析原因是PX4在Gazebo仿真中寫了一個腳本來使無人機(jī)一直處于畫面中央。

在Tools/sitl_run.sh文件中有如下的代碼，控制Gazebo中的視角跟隨無人機(jī)。

# Disable follow mode
if [[ "$PX4_NO_FOLLOW_MODE" != "1" ]]; then
    follow_mode="--gui-client-plugin libgazebo_user_camera_plugin.so"
else
    follow_mode=""
fi

在運行仿真命令時加上前綴PX4_NO_FOLLOW_MODE=1來屏蔽視角跟隨部分代碼。

PX4_NO_FOLLOW_MODE=1 make px4_sitl_rtps gazebo

之后再次運行即可在固定視角下觀察無人機(jī)的運動。

參考資料：

PX4 Gazebo Simulation

Control a Simulated UAV Using ROS 2 and PX4 Bridge

到了這里，關(guān)于【PX4&Simulink&Gazebo聯(lián)合仿真】在Simulink中使用ROS2控制無人機(jī)沿自定義圓形軌跡飛行并在Gazebo中可視化的文章就介紹完了。如果您還想了解更多內(nèi)容，請在右上角搜索TOY模板網(wǎng)以前的文章或繼續(xù)瀏覽下面的相關(guān)文章，希望大家以后多多支持TOY模板網(wǎng)！