基于Gazebo和PX4的強(qiáng)化學(xué)習(xí)訓(xùn)練仿真環(huán)境接口設(shè)計(jì)與實(shí)現(xiàn)

孫翔龍 梁彥剛

(國防科技大學(xué)空天科學(xué)學(xué)院 湖南省長沙市 410073）

PX4是一款致力于為學(xué)術(shù)界、業(yè)余愛好者和產(chǎn)業(yè)社區(qū)提供低成本高性能飛控的獨(dú)立開源項(xiàng)目，Gazebo是內(nèi)置物理引擎的動力學(xué)仿真軟件，直接集成在ROS（Robot Operating System）開發(fā)環(huán)境中，二者都是智能無人機(jī)開發(fā)者常用的工具，國內(nèi)外有不少相關(guān)的研究和工程開發(fā)?；赑X4飛控的四旋翼無人機(jī)在Gazebo仿真環(huán)境中進(jìn)行強(qiáng)化學(xué)習(xí)訓(xùn)練的相關(guān)研究過程中，需要一個接口程序使得強(qiáng)化學(xué)習(xí)訓(xùn)練過程中能夠與仿真環(huán)境進(jìn)行交互。本文就該程序的設(shè)計(jì)與實(shí)現(xiàn)方法進(jìn)行探討，以期為其他研究者提供參考。

1 PX4飛控、外部程序、Gazebo仿真環(huán)境之間的交互方式

1.1 PX4飛控與外部程序及仿真環(huán)境之間的交互方式

軟件在環(huán)仿真（SITL）中，PX4可以通過MAVLink消息與外部程序進(jìn)行交互。同時，PX4也通過MAVLink消息與仿真環(huán)境（如Gazebo）進(jìn)行交互。與Gazebo進(jìn)行交互時，通常情況下，PX4向Gazebo發(fā)布驅(qū)動無人機(jī)螺旋槳電機(jī)的控制指令，Gazebo向PX4發(fā)布無人機(jī)模型（simulator）上的各種傳感器在仿真環(huán)境中感知到的信息。

在利用ROS開發(fā)環(huán)境進(jìn)行編程時，由于各ROS節(jié)點(diǎn)（外部控制程序）之間通過ROS話題、服務(wù)進(jìn)行交互，還需要利用MavROS功能包進(jìn)行MAVLink消息與ROS話題、服務(wù)之間的轉(zhuǎn)換。消息流如圖1所示。

圖1：外部程序到simulator的消息流圖

1.2 外部程序與Gazebo仿真環(huán)境之間的交互方式

Gazebo提供了一組ROS API，允許用戶修改并獲取有關(guān)模擬世界各個方面的信息。在ROS編程環(huán)境中，外部程序可以通過話題或服務(wù)直接與Gazebo仿真環(huán)境進(jìn)行交互，如在仿真環(huán)境中增加、刪除模型、獲得模型狀態(tài)、設(shè)置模型狀態(tài)、暫停仿真等。例如：Gazebo訂閱的話題有“/gazebo/set_model_state”話題等，該話題可用于快速設(shè)置模型的姿勢，而無需等待姿勢設(shè)置動作完成；發(fā)布的話題有“gazebo_msgs/ModelStates”等，該話題發(fā)布仿真中所有模型的狀態(tài)；服務(wù)有“gazebo_msgs/DeleteModel”等，此服務(wù)允許用戶從仿真環(huán)境中刪除模型。

2 強(qiáng)化學(xué)習(xí)中訓(xùn)練智能體的一般方法

2.1 強(qiáng)化學(xué)習(xí)的基本模型

強(qiáng)化學(xué)習(xí)主要解決的問題是智能體，即運(yùn)行強(qiáng)化學(xué)習(xí)算法的實(shí)體，在環(huán)境中執(zhí)行動作以獲得最大的累計(jì)獎勵。強(qiáng)化學(xué)習(xí)應(yīng)用廣泛，所有需要做決策和控制的地方，都有它的身影。典型的包括游戲與博弈，如打星際爭霸、Atari游戲：算法需要根據(jù)當(dāng)前的游戲畫面和狀態(tài)決定其要執(zhí)行的動作，如按游戲的鍵盤、手柄，鼠標(biāo)；圍棋、象棋等棋類游戲：算法需要根據(jù)當(dāng)前的棋局決定當(dāng)前該怎么走子；自動駕駛系統(tǒng)、無人車：算法需要根據(jù)當(dāng)前的路況，無人車自身的狀態(tài)（如速度、加速度）決定其行駛的行為，如控制方向盤，油門，剎車等；機(jī)器人控制：機(jī)器人要根據(jù)當(dāng)前所處的環(huán)境，自身的狀態(tài)，決定其要執(zhí)行的動作。

其基本模型如圖2所示。

圖2：強(qiáng)化學(xué)習(xí)模型

本文基于強(qiáng)化學(xué)習(xí)研究無人機(jī)在著陸地標(biāo)上自主著陸問題。在該問題中，無人機(jī)為智能體，無人機(jī)與著陸地標(biāo)之間的相對位置和相對速度信息為狀態(tài)信息。無人機(jī)智能體感知環(huán)境，得到狀態(tài)信息，并根據(jù)該信息通過智能體內(nèi)部的神經(jīng)網(wǎng)絡(luò)得出無人機(jī)的控制指令。無人機(jī)執(zhí)行該控制指令后，經(jīng)過設(shè)定時間步后在環(huán)境中到達(dá)新的位置，并獲得新的速度，從而可以計(jì)算出新的相對位置和相對速度，得到新的狀態(tài)信息。環(huán)境根據(jù)無人機(jī)智能體的狀態(tài)信息給出獎勵或者懲罰，并根據(jù)新的狀態(tài)信息得出新的控制指令。通過程序的不斷循環(huán)實(shí)現(xiàn)無人機(jī)智能體逐步接近著陸地標(biāo)并到達(dá)適合上鎖的位置，程序判斷出無人機(jī)到達(dá)該位置后，通過上鎖指令使無人機(jī)著陸。

2.2 強(qiáng)化學(xué)習(xí)訓(xùn)練的一般方法

智能體能夠正確地執(zhí)行動作，關(guān)鍵在于得到一個映射函數(shù)π，稱為策略函數(shù)：a = π(s)，其輸入s是當(dāng)前時刻環(huán)境狀態(tài)信息，輸出a是智能體要執(zhí)行的動作。策略函數(shù)由神經(jīng)網(wǎng)絡(luò)來進(jìn)行實(shí)現(xiàn)，為了得到策略函數(shù)，需要運(yùn)用大量的樣本通過特定的算法來訓(xùn)練該神經(jīng)網(wǎng)絡(luò)。一般情況下，強(qiáng)化學(xué)習(xí)訓(xùn)練用到的樣本可用一個類似元組或數(shù)組的數(shù)據(jù)結(jié)構(gòu)[observation，action，reward，observation_next，done]來表示，其中observation表示從環(huán)境中感知到的與問題相關(guān)的信息，action表示智能體執(zhí)行的動作信息，reward表示智能體從環(huán)境中獲得的獎勵信息，observation_next表示智能體執(zhí)行動作后在環(huán)境中感知到的新的信息，done標(biāo)記該狀態(tài)是否為終結(jié)狀態(tài)。

大量的樣本通常由智能體在環(huán)境中通過海量的試錯來獲得，由于試錯的次數(shù)比較龐大，且一些控制類試錯容易造成智能體的損壞，因此，通過仿真環(huán)境開展強(qiáng)化學(xué)習(xí)訓(xùn)練是一種非常有效的手段。

3 仿真環(huán)境接口程序的設(shè)計(jì)與實(shí)現(xiàn)

3.1 設(shè)計(jì)思路

OpenAI Gym是一款非常著名的強(qiáng)化學(xué)習(xí)訓(xùn)練的仿真環(huán)境，包含一個測試問題集；每個問題成為一個環(huán)境，可以用于用戶自己的強(qiáng)化學(xué)習(xí)算法開發(fā)；這些環(huán)境有共享的接口，允許用戶設(shè)計(jì)通用的算法。其環(huán)境接口是Env，典型調(diào)用方法如表1所示。

表1：Gym的Env接口調(diào)用示例

if done:observation = env.reset()env.close()

作為統(tǒng)一的環(huán)境接口，Env包含以下核心方法：env.reset(self):仿真環(huán)境復(fù)位，重置環(huán)境的狀態(tài)，返回observation；env.step(self,action):逐步推進(jìn)仿真，推進(jìn)一個時間步長，返回observation，reward，done等結(jié)果。

在強(qiáng)化學(xué)習(xí)訓(xùn)練時，這兩個方法作為仿真環(huán)境的接口具有很強(qiáng)的通用性。由于強(qiáng)化學(xué)習(xí)的訓(xùn)練往往需要進(jìn)行非常多的輪次（episode），每個輪次的初始狀態(tài)要有某種相似性，這就需要解決仿真環(huán)境復(fù)位的問題。其方法通常是設(shè)計(jì)一個reset函數(shù)，每個輪次開始時調(diào)用該函數(shù)使仿真環(huán)境恢復(fù)到設(shè)定的初始狀態(tài)。由于強(qiáng)化學(xué)習(xí)訓(xùn)練用到的樣本是一組一組的離散數(shù)據(jù)，而在連續(xù)運(yùn)行的控制類強(qiáng)化學(xué)習(xí)訓(xùn)練情形中，該離散數(shù)據(jù)的獲得方式通常是按照預(yù)先設(shè)定的時間步長使仿真過程逐步向前推進(jìn)，這就需要逐步推進(jìn)仿真的方法。逐步推進(jìn)仿真習(xí)慣上用step函數(shù)來實(shí)現(xiàn)，step函數(shù)通常以控制指令作為參數(shù)。

但是，Gym提供的仿真環(huán)境的場景是有限的，研究者可能還需要結(jié)合研究任務(wù)，構(gòu)建適合自己需求的仿真環(huán)境。由于完善的、仿真程度高的仿真環(huán)境是一個復(fù)雜的軟件系統(tǒng)，研究者自己編寫的仿真環(huán)境往往存在仿真程度差等問題。如果需要在仿真研究的基礎(chǔ)上進(jìn)行真機(jī)驗(yàn)證，最好使用其它機(jī)構(gòu)開發(fā)的較為成熟的仿真環(huán)境。但是，類似Gazebo的相對成熟的仿真環(huán)境往往并非專門為強(qiáng)化學(xué)習(xí)訓(xùn)練而設(shè)計(jì)，需要另外設(shè)計(jì)接口程序以使其適于進(jìn)行強(qiáng)化學(xué)習(xí)的訓(xùn)練。設(shè)計(jì)仿真環(huán)境的接口可以借鑒Gym的核心方法，特別是強(qiáng)化學(xué)習(xí)訓(xùn)練中兩個最重要的方法：仿真環(huán)境復(fù)位方法和逐步推進(jìn)仿真方法。

3.2 實(shí)現(xiàn)方法

3.2.1 仿真環(huán)境復(fù)位

對基于Gazebo仿真環(huán)境和PX4飛控?zé)o人機(jī)（下文所述無人機(jī)均為基于PX4飛控的無人機(jī)，所述飛控為PX4開源飛控）在ROS開發(fā)環(huán)境中開展的基于強(qiáng)化學(xué)習(xí)的無人機(jī)自主著陸仿真研究中仿真環(huán)境復(fù)位用到的reset函數(shù)實(shí)現(xiàn)方法如下：

（1）通過發(fā)布控制指令的方式使無人機(jī)進(jìn)入OFFBOARD模式。OFFBOARD模式是利用外部程序控制無人機(jī)時，無人機(jī)應(yīng)處于的工作模式。方法是先訂閱MavROS功能包發(fā)布的“/mavros/state”的話題來獲得無人機(jī)的工作模式，如果工作模式不是OFFBOARD模式，則通過MavROS功能包提供的“/mavros/set_mode”服務(wù)將無人機(jī)的工作模式設(shè)置為OFFBOARD模式。

（2）通過發(fā)布控制指令的方式使無人機(jī)解鎖（arm）。調(diào)用MavROS模塊提供的“/mavros/set_mode”服務(wù)向無人機(jī)的飛控發(fā)布解鎖指令。為確保飛控接收到該指令，可采取重復(fù)發(fā)布數(shù)次指令的方式?？紤]到每次復(fù)位都會調(diào)用reset函數(shù)，解鎖時應(yīng)先判斷無人機(jī)是否處于鎖定（disarm）狀態(tài)，如果不處于鎖定狀態(tài)則不發(fā)布解鎖指令。

（3）設(shè)置無人機(jī)及其它模型的初始位置。在恢復(fù)仿真環(huán)境時，本文的研究只需要考慮無人機(jī)的位置信息，故采用了控制無人機(jī)飛行到預(yù)定位置的方式。方法是發(fā)布“/mavros/setpoint_raw/local”話題至MavROS功能包，轉(zhuǎn)換成MAVLink消息后發(fā)送給飛控來控制無人機(jī)飛行到預(yù)定位置。判斷無人機(jī)是否飛行到預(yù)定位置的方法是先通過MavROS功能包，利用Gazebo仿真環(huán)境提供的“/gazebo/get_model_state”服務(wù)，獲取無人機(jī)的位置信息，然有將該位置信息與預(yù)定位置進(jìn)行比較，偏差在一定范圍內(nèi)即認(rèn)為無人機(jī)到達(dá)預(yù)定位置。選擇其它的MavROS話題也可以實(shí)現(xiàn)無人機(jī)的速度、加速度或姿態(tài)控制。關(guān)鍵代碼如表2所示。

表2：控制無人機(jī)飛行到預(yù)先設(shè)定的隨機(jī)位置

也可以采用外部程序直接與Gazebo仿真環(huán)境進(jìn)行交互的方式控制無人機(jī)的初始位置，方法是先向MavROS功能申請“/gazebo/set_model_state”服務(wù)，轉(zhuǎn)換成MAVLink消息后發(fā)送給Gazebo仿真環(huán)境直接控制無人機(jī)瞬移到預(yù)定位置。但是，經(jīng)驗(yàn)證，瞬移的方法會導(dǎo)致無人機(jī)不能穩(wěn)定飛行，非常容易墜機(jī)，原因可能是瞬移會導(dǎo)致飛控感知到的位置信息紊亂，無法控制無人機(jī)穩(wěn)定飛行。

本文的仿真環(huán)境中還有一個著陸地標(biāo)，該地標(biāo)不包含飛控，故采用了直接與Gazebo仿真環(huán)境進(jìn)行交互的方式，利用“/gazebo/set_model_state”服務(wù)設(shè)置其初始位置為一定范圍內(nèi)的隨機(jī)位置。

（4）計(jì)算reset函數(shù)返回值。在進(jìn)行強(qiáng)化學(xué)習(xí)訓(xùn)練時通常需要從reset函數(shù)獲得狀態(tài)信息。該狀態(tài)信息由reset函數(shù)的返回值來提供。本文研究的狀態(tài)信息是無人機(jī)和著陸地標(biāo)之間的相對位置和相對速度信息。利用Gazebo仿真環(huán)境提供的“/gazebo/get_model_state”服務(wù)可獲得無人機(jī)和著陸地標(biāo)的位置、速度信息，簡單計(jì)算后可獲得相對位置和相對速度信息。

3.2.2 逐步推進(jìn)仿真

仿真環(huán)境中的step函數(shù)實(shí)現(xiàn)方法如下：

（1）發(fā)布無人機(jī)控制指令。在step函數(shù)中將作為函數(shù)參數(shù)的控制指令傳遞到“/mavros/setpoint_raw/local”話題中，并將該話題發(fā)送給MavROS功能包，轉(zhuǎn)換成MAVLink消息后發(fā)送給飛控實(shí)現(xiàn)對無人機(jī)的控制。本文的研究以xyz方向的速度作為控制指令來控制無人機(jī)逐步飛向目標(biāo)。也可以根據(jù)不同的問題以無人機(jī)的位置、姿態(tài)、加速度作為控制指令選用其它的MavROS話題發(fā)送給飛控。關(guān)鍵代碼如表3所示。

表3：發(fā)布無人機(jī)控制指令

（2）設(shè)定一段休眠時間。因其它指令的執(zhí)行時間非常短，休眠時間基本上就是仿真的時間步長。在休眠的過程中，由于Gazebo仿真環(huán)境是并行運(yùn)行的，無人機(jī)在收到的控制指令的控制下向前運(yùn)動一個時間步。

（3）獲取狀態(tài)信息。無人機(jī)運(yùn)動一個時間步后，系統(tǒng)進(jìn)入一個新的狀態(tài)，利用Gazebo仿真環(huán)境提供的“/gazebo/get_model_state”服務(wù)分別獲得無人機(jī)和著陸地標(biāo)的位置和速度信息，經(jīng)簡單計(jì)算后得到相對位置和相對速度信息。根據(jù)狀態(tài)信息進(jìn)一步判斷無人機(jī)所處的狀態(tài)，如果在該步仿真中無人機(jī)達(dá)到預(yù)定目標(biāo)，則進(jìn)入終結(jié)狀態(tài)，如果沒有達(dá)到目標(biāo)則仍處于中間狀態(tài)。通常用標(biāo)記變量done來標(biāo)記無人機(jī)所處的狀態(tài)，done的值為1則進(jìn)入終結(jié)狀態(tài)，為0則為中間狀態(tài)。

（4）給出獎勵。需要在step函數(shù)中給出該步仿真后無人機(jī)智能體從環(huán)境中獲得的獎勵。如果在該步仿真中無人機(jī)達(dá)到目標(biāo)，如到達(dá)著陸地標(biāo)上方的設(shè)定點(diǎn)，則給予較大獎勵；如果無人機(jī)觸發(fā)懲罰條件，如飛行高度低于安全高度，則給予一定的負(fù)獎勵。由于在絕大多數(shù)仿真步是未到達(dá)終結(jié)狀態(tài)的中間步，無人機(jī)既沒有達(dá)到目標(biāo)又沒有觸發(fā)懲罰，如果沒有適當(dāng)獎勵則會出現(xiàn)強(qiáng)化學(xué)習(xí)訓(xùn)練中的獎勵稀疏（Sparse Reward）問題，容易導(dǎo)致訓(xùn)練不收斂。因此，應(yīng)加入獎勵函數(shù)塑形（reward shaping）以引導(dǎo)智能體學(xué)習(xí)到最終目標(biāo)。經(jīng)驗(yàn)證，以無人機(jī)和著陸地標(biāo)之間的相對距離以及相對位置矢量和相對速度矢量之間的夾角為函數(shù)變量，經(jīng)過適當(dāng)運(yùn)算給中間步一個較小的或正或負(fù)的獎勵，進(jìn)行獎勵函數(shù)塑形，引導(dǎo)無人機(jī)朝向目標(biāo)點(diǎn)飛行和接近目標(biāo)點(diǎn)，效果較好。關(guān)鍵代碼如表4所示。

表4：獎勵函數(shù)

（5）計(jì)算reset函數(shù)返回值。將狀態(tài)信息和獲得獎勵進(jìn)行整合，給出reset函數(shù)的返回值。返回值可用一個形如[observation，reward，done]的數(shù)組或元組來表示，其中observation表示狀態(tài)信息，reward表示智能體從環(huán)境中獲得的獎勵，done標(biāo)記該狀態(tài)是否為終結(jié)狀態(tài)。

（6）暫停和恢復(fù)仿真。Gazebo仿真環(huán)境具有暫停和恢復(fù)仿真的功能，可以在發(fā)布完控制指令后恢復(fù)仿真，在程序休眠和獲得狀態(tài)信息后暫停仿真，以使強(qiáng)化學(xué)習(xí)的訓(xùn)練程序下一次調(diào)用step函數(shù)時智能體和仿真環(huán)境能夠緊接著上一次的狀態(tài)向前推進(jìn)，排除了執(zhí)行其它代碼的時間對仿真連貫性的影響?？衫谩癵azebo/unpause_physics”服務(wù)恢復(fù)仿真環(huán)境，利用“gazebo/pause_physics”服務(wù)暫停仿真環(huán)境。

4 可能遇到問題的處理方法

4.1 意外墜機(jī)

在仿真環(huán)境中如果無人機(jī)意外墜機(jī)則程序就不再繼續(xù)往下運(yùn)行了，這顯然不利于強(qiáng)化學(xué)習(xí)的訓(xùn)練。針對此問題，本文采用了一個簡化問題的方法，在reset函數(shù)和step函數(shù)中將目標(biāo)點(diǎn)設(shè)置到著陸地標(biāo)上方10 m處，當(dāng)無人機(jī)飛行高度低于9.5 m時本輪次訓(xùn)練結(jié)束，通過控制指令使無人機(jī)飛到初始位置。抬高飛行高度后，無人機(jī)基本上沒有墜機(jī)的可能，并且對強(qiáng)化學(xué)習(xí)訓(xùn)練來說，將目標(biāo)點(diǎn)設(shè)定在較高位置和設(shè)定在著陸地標(biāo)上沒有本質(zhì)不同。當(dāng)飛行高度低于安全高度時，理論上也可采用調(diào)用“/gazebo/reset_world”服務(wù)與Gazebo仿真環(huán)境進(jìn)行交互，使仿真環(huán)境復(fù)位的方法解決此問題。但是，實(shí)驗(yàn)發(fā)現(xiàn)該方法同樣會導(dǎo)致無人機(jī)飛行不穩(wěn)定。

4.2 程序與飛控交互時不能實(shí)現(xiàn)預(yù)期功能

外部程序與飛控進(jìn)行交互時有一些技術(shù)細(xì)節(jié)，開發(fā)者如果想更多地關(guān)注功能實(shí)現(xiàn)而不想陷入到技術(shù)細(xì)節(jié)中，可借用中間程序。筆者利用了國內(nèi)阿木實(shí)驗(yàn)室開源的Prometheus項(xiàng)目中px4_sender.cpp等程序文件中提供的功能函數(shù)與飛控進(jìn)行交互，避免了自行設(shè)計(jì)交互函數(shù)的繁瑣細(xì)節(jié)。

5 結(jié)束語

本文借鑒OpenAI Gym的統(tǒng)一環(huán)境接口，針對基于Gazebo和PX4強(qiáng)化學(xué)習(xí)訓(xùn)練這個具體的應(yīng)用場景，介紹了仿真環(huán)境復(fù)位和逐步推進(jìn)仿真兩個程序接口的實(shí)現(xiàn)方法，以及可能遇到的問題的處理方法。實(shí)際強(qiáng)化學(xué)習(xí)訓(xùn)練驗(yàn)證了本文接口程序設(shè)計(jì)與實(shí)現(xiàn)方法的有效性，可以為相關(guān)研究提供一定的借鑒和參考。