基于USARSim和ROS的無人平臺編隊(duì)仿真系統(tǒng)

張浩杰 蘇治寶 楊甜甜

美俄等軍事強(qiáng)國近年來積極推動(dòng)無人作戰(zhàn)平臺發(fā)展,帶來無人作戰(zhàn)平臺發(fā)展新高潮,其正在從輔戰(zhàn)裝備向主戰(zhàn)裝備發(fā)展,將成為未來戰(zhàn)爭取得勝利的決定性因素.長期以來,美軍一直致力于地面無人系統(tǒng)集群作戰(zhàn)能力生成,加快推進(jìn)地面無人自主系統(tǒng)集群實(shí)戰(zhàn)化應(yīng)用[1].地面無人平臺編隊(duì)協(xié)同是地面無人自主系統(tǒng)的一個(gè)重要研究方向,其致力于實(shí)現(xiàn)地面無人平臺間協(xié)同以及地面無人平臺與士兵協(xié)同[2],以提高未來部隊(duì)的防御、火力攻擊、精確打擊以及情報(bào)、監(jiān)視與偵察能力.

地面無人平臺編隊(duì)協(xié)同是指控制多個(gè)地面無人平臺的方向角、速度以及無人平臺之間的相對距離形成一致,在適應(yīng)環(huán)境約束的條件下保持設(shè)定的隊(duì)形,朝著同一個(gè)方向運(yùn)動(dòng).一般來說,早期的地面無人平臺編隊(duì)協(xié)同控制方法可以分為基于領(lǐng)航者?跟隨者的編隊(duì)控制方法[3]、基于虛擬結(jié)構(gòu)的編隊(duì)控制方法[4]、基于人工勢場的編隊(duì)控制方法[5]、基于行為的編隊(duì)控制方法和基于路徑跟隨的編隊(duì)控制方法[6].這類方法都只在編隊(duì)控制的某個(gè)環(huán)節(jié)有較好的效果,并沒有有效解決編隊(duì)組建、行進(jìn)、變換和避障等一系列完整的動(dòng)作[7].

信息一致性方法描述了每個(gè)個(gè)體與它相鄰個(gè)體間的信息交互過程,確定了個(gè)體之間相互作用的規(guī)則,它通過局部的信息交互而使整個(gè)群體的狀態(tài)信息量形成一致.因此,在一致性控制協(xié)議的基礎(chǔ)上通過簡單的線性變換就可以將一致性算法應(yīng)用于編隊(duì)控制.近年來越來越多的研究者致力于將信息一致性方法用于地面無人平臺編隊(duì)協(xié)同研究,并取得了一定的研究成果[8?9].文獻(xiàn)[10]介紹了一種基于一致性的多智能體編隊(duì)控制方法,通過選擇合適的一致性狀態(tài)信息量,將一致性變體算法運(yùn)用到編隊(duì)控制中.在網(wǎng)絡(luò)條件下,引入隨機(jī)通信噪聲和信息丟包問題,文獻(xiàn)[11]采用一致性策略研究基于信息交換的多邊形編隊(duì)控制方法.文獻(xiàn)[12]針對復(fù)雜地形地面崎嶇起伏的特點(diǎn),通過建立三維地形環(huán)境下編隊(duì)系統(tǒng)的誤差模型,并根據(jù)環(huán)境中的特定地形設(shè)計(jì)相應(yīng)的編隊(duì)行駛策略,實(shí)現(xiàn)了多輪式移動(dòng)機(jī)器人系統(tǒng)在復(fù)雜地形環(huán)境下的編隊(duì)控制.文獻(xiàn)[13]采用迭代學(xué)習(xí)控制方法產(chǎn)生一系列的控制信號,用于多智能體的編隊(duì)控制.編隊(duì)協(xié)同控制可使平臺保持一定的隊(duì)形進(jìn)行運(yùn)動(dòng),然而這種運(yùn)動(dòng)不具有目標(biāo)性,要實(shí)現(xiàn)其在執(zhí)行任務(wù)過程中運(yùn)動(dòng)到指定位置,則需要進(jìn)行地面無人平臺編隊(duì)導(dǎo)航控制.文獻(xiàn)[14]研究了領(lǐng)航者具有未知線速度、角速度及其未知上界等多種未知條件下的地面無人平臺編隊(duì)控制問題,該方法通過在控制器中引入一個(gè)時(shí)間函數(shù)對未知性進(jìn)行補(bǔ)償.文獻(xiàn)[15]開展了基于魚群效應(yīng)的無人車輛編隊(duì)控制方法研究,該方法將無人車輛編隊(duì)控制與魚群行為進(jìn)行類比建模,以提升無人車輛編隊(duì)效率.文獻(xiàn)[16]研究了傳感器約束條件下基于領(lǐng)航者信息的編隊(duì)導(dǎo)航,提出了一種局部控制策略,即采用動(dòng)態(tài)信息交互拓?fù)浣Y(jié)構(gòu),也能確保形成預(yù)定的編隊(duì)模式.

地面無人平臺編隊(duì)協(xié)同控制方法多種多樣,每種方法需要解決的目標(biāo)任務(wù)及最適合的應(yīng)用場景也不盡相同,直接在實(shí)際環(huán)境中進(jìn)行編隊(duì)控制算法性能測試既耗時(shí)又難于改進(jìn),而地面無人平臺編隊(duì)協(xié)同仿真系統(tǒng)則為編隊(duì)控制算法測試提供了一種快速驗(yàn)證途徑.目前,大多數(shù)的開源或者商業(yè)化的編隊(duì)仿真系統(tǒng)主要支持智能車輛在城市結(jié)構(gòu)化環(huán)境下的編隊(duì)算法測試,比如道路交通仿真系統(tǒng)Simulation of Urban Mobility[17?18]和自動(dòng)駕駛仿真軟件Carla[19]等.對于越野非結(jié)構(gòu)化環(huán)境,美國陸軍工程研究與發(fā)展中心開發(fā)的地面無人平臺自主導(dǎo)航仿真系統(tǒng)Virtual Autonomous Navigation Environment 能夠?qū)崿F(xiàn)對編隊(duì)控制算法的測試[20],然而該仿真系統(tǒng)并不開源,大多數(shù)開源的仿真系統(tǒng)僅限于提供仿真環(huán)境模塊,比如Real/Player、Microsoft Robotics Developer Studio 和Gazebo 等,在這些平臺下測試編隊(duì)協(xié)同控制算法需要進(jìn)行大量平臺自主性和環(huán)境感知等模塊的前期開發(fā)工作.因此,針對越野非結(jié)構(gòu)化環(huán)境下地面無人平臺編隊(duì)仿真系統(tǒng)存在功能模塊不完善及算法集成測試?yán)щy等問題,為了便于有效測試不同地面無人平臺編隊(duì)協(xié)同控制算法的性能及適用的特定任務(wù)場景,本文設(shè)計(jì)了一種基于USARSim 和ROS 的地面無人平臺編隊(duì)協(xié)同仿真系統(tǒng).USARSim 中含有豐富的高逼真環(huán)境模型和地面無人平臺模型[21],而ROS 開源了大量地面無人平臺導(dǎo)航相關(guān)的算法[22],將二者結(jié)合可用于實(shí)現(xiàn)在外界條件一致的情況下測試不同的地面無人平臺編隊(duì)協(xié)同控制算法.該仿真系統(tǒng)主要基于USARSim 和ROS 兩個(gè)開源架構(gòu)設(shè)計(jì),能夠有效地用于地面無人平臺編隊(duì)協(xié)同控制方法的設(shè)計(jì)、調(diào)試及二次集成開發(fā),降低地面無人平臺編隊(duì)協(xié)同系統(tǒng)的開發(fā)成本,為其研究提供了一種新的思路和方法.

1 系統(tǒng)結(jié)構(gòu)

基于USARSim 和ROS 的地面無人平臺編隊(duì)協(xié)同仿真系統(tǒng)主要由人機(jī)交互界面、基于ROS 架構(gòu)的地面無人平臺控制系統(tǒng)和基于USARSim 的虛擬仿真場景三個(gè)部分組成,其系統(tǒng)結(jié)構(gòu)如圖1 所示.地面無人平臺編隊(duì)協(xié)同仿真系統(tǒng)中各部分之間通過組建的局域網(wǎng)進(jìn)行數(shù)據(jù)信息交互,人機(jī)交互界面通過UDP 網(wǎng)絡(luò)協(xié)議將當(dāng)前任務(wù)下發(fā)給地面無人平臺控制器.在仿真系統(tǒng)中可以同時(shí)掛載 1,2,···,n個(gè)基于ROS 架構(gòu)的地面無人平臺控制器進(jìn)行編隊(duì)協(xié)同測試,每一個(gè)地面無人平臺控制器對應(yīng)于USARSim 仿真場景中的一個(gè)地面無人平臺運(yùn)動(dòng)控制模型,從而按照編隊(duì)控制方法生成的控制指令驅(qū)動(dòng)地面無人平臺運(yùn)動(dòng),實(shí)現(xiàn)在USARSim 虛擬場景中進(jìn)行動(dòng)態(tài)編隊(duì)演示.

圖1 基于USARSim 和ROS 的地面無人平臺編隊(duì)協(xié)同仿真系統(tǒng)結(jié)構(gòu)Fig.1 The architecture of formation simulation system for UGV based on USARSim and ROS

人機(jī)交互界面主要進(jìn)行遙控操作任務(wù)、單平臺自主路徑跟隨任務(wù)和多平臺編隊(duì)協(xié)同任務(wù)下發(fā).遙控操作任務(wù)和單平臺自主路徑跟隨任務(wù)主要用于測試單個(gè)地面無人平臺的自主性能,它們分別以遙控指令控制量和期望跟隨的路徑點(diǎn)文件發(fā)送給指定的地面無人平臺控制器進(jìn)行解析.多平臺編隊(duì)協(xié)同任務(wù)則將地面無人平臺設(shè)定保持的隊(duì)形信息、任務(wù)目標(biāo)點(diǎn)信息以及領(lǐng)航者編號等信息發(fā)送給仿真系統(tǒng)中的所有地面無人平臺控制器.同時(shí),人機(jī)交互界面也將接收地面無人平臺反饋的自身姿態(tài)信息,從而進(jìn)行數(shù)據(jù)記錄和位姿實(shí)時(shí)更新顯示.

基于ROS 架構(gòu)的地面無人平臺控制系統(tǒng)運(yùn)行Ubuntu Bionic 18.04/ROS Melodic 系統(tǒng),其上集成了開源的傳感器數(shù)據(jù)處理節(jié)點(diǎn)和導(dǎo)航模塊[23].根據(jù)人機(jī)交互界面下發(fā)的任務(wù)類型及從USARSim 虛擬場景中獲取的地面無人平臺自身的位姿信息,地面無人平臺控制器進(jìn)行解析計(jì)算相應(yīng)的運(yùn)動(dòng)控制量,并通過ROS-USARSim 接口模塊將該運(yùn)動(dòng)控制量發(fā)送至USARSim,從而驅(qū)動(dòng)USARSim 虛擬場景中相應(yīng)的地面無人平臺模型運(yùn)動(dòng).

基于USARSim 的虛擬仿真場景集成了豐富的地面無人平臺模型及特殊場景環(huán)境模型,也可根據(jù)實(shí)際的地面無人平臺特性參數(shù)進(jìn)行模型設(shè)計(jì).它通過ROS-USARSim 接口模塊將虛擬仿真場景中的地面無人平臺模型與地面無人平臺控制器一一關(guān)聯(lián),并根據(jù)接收到的地面無人平臺期望控制量驅(qū)動(dòng)模型運(yùn)動(dòng).同時(shí),將地面無人平臺模型在虛擬場景中的位姿通過ROS-USARSim 接口模塊反饋給地面無人平臺控制器,以進(jìn)行更高級的自主運(yùn)動(dòng)規(guī)劃.

2 人機(jī)交互界面設(shè)計(jì)

人機(jī)交互界面主要用于任務(wù)下發(fā)及地面無人平臺位姿實(shí)時(shí)更新顯示,它由遙控操作發(fā)送模塊、自主路徑跟隨任務(wù)發(fā)送模塊、編隊(duì)協(xié)同任務(wù)發(fā)送模塊和地面無人平臺位姿顯示模塊四部分組成,如圖2所示.

圖2 人機(jī)交互界面Fig.2 The GUI of human-machine interface

2.1 遙控操作發(fā)送模塊

遙控操作發(fā)送模塊主要用來測試單個(gè)地面無人平臺的運(yùn)動(dòng)響應(yīng)能力,如圖3 中左側(cè)所示.它通過選擇IP 地址,指定要連接測試的特定地面無人平臺,將期望的線速度v和角速度ω發(fā)送給相應(yīng)的地面無人平臺控制器,從而控制USARSim 虛擬場景中的地面無人平臺模型運(yùn)動(dòng).

2.2 自主路徑跟隨任務(wù)發(fā)送模塊

自主路徑跟隨任務(wù)發(fā)送模塊主要用來測試單個(gè)地面無人平臺對特定路徑的自主運(yùn)動(dòng)跟隨能力,如圖3 中中部所示.它通過選擇IP 地址,指定要連接測試的特定地面無人平臺,將預(yù)先設(shè)定的全局路徑點(diǎn)文件發(fā)送給相應(yīng)的地面無人平臺控制器,實(shí)現(xiàn)對全局路徑的自主跟蹤運(yùn)動(dòng)控制.

圖3 人機(jī)交互界面中的任務(wù)模塊Fig.3 Task modules in human-machine interface

2.3 編隊(duì)協(xié)同任務(wù)發(fā)送模塊

編隊(duì)協(xié)同任務(wù)發(fā)送模塊主要用來測試仿真系統(tǒng)中多個(gè)地面無人平臺編隊(duì)協(xié)同能力,如圖3 中右側(cè)所示.在該任務(wù)界面,它通過選擇IP 地址選定需要進(jìn)行編隊(duì)測試的所有地面無人平臺.同時(shí),也需要通過IP 地址指定某一個(gè)地面無人平臺作為編隊(duì)任務(wù)的領(lǐng)航者,其他地面無人平臺則成為跟隨者.

編隊(duì)協(xié)同任務(wù)的目標(biāo)點(diǎn)及領(lǐng)航者的路徑通過地面無人平臺位姿顯示模塊進(jìn)行人為設(shè)定,隊(duì)形設(shè)定包括各個(gè)跟隨者與領(lǐng)航者之間的距離和方位角.編隊(duì)任務(wù)測試中的各個(gè)地面無人平臺根據(jù)收到的編隊(duì)任務(wù),確定自身是領(lǐng)航者還是跟隨者.如果是領(lǐng)航者,則自主跟隨任務(wù)路徑行駛,相反,如果是跟隨者,則根據(jù)隊(duì)形保持算法計(jì)算目標(biāo)位置,并解算單平臺自主運(yùn)動(dòng)控制量.同時(shí),各個(gè)地面無人平臺控制器將地面無人平臺的位姿反饋給人機(jī)交互界面,在地面無人平臺位姿顯示模塊實(shí)時(shí)更新編隊(duì)隊(duì)形保持狀態(tài).

2.4 地面無人平臺位姿顯示模塊

地面無人平臺位姿顯示模塊主要用來設(shè)定任務(wù)目標(biāo)點(diǎn)及任務(wù)路徑,同時(shí),接收仿真系統(tǒng)中所有地面無人平臺控制器反饋的地面無人平臺在USARSim 虛擬場景中的位姿數(shù)據(jù),主要以數(shù)字和圖形化的形式顯示地面無人平臺在編隊(duì)任務(wù)執(zhí)行過程中的位姿與行駛軌跡.

3 基于ROS 架構(gòu)的地面無人平臺控制系統(tǒng)

基于ROS 架構(gòu)的地面無人平臺控制系統(tǒng)包括通信中間件、地面無人平臺控制器和ROS-USARSim 接口三個(gè)部分,如圖4 所示.通信中間件嵌入在地面無人平臺導(dǎo)航模塊中實(shí)現(xiàn),而地面無人平臺控制器和ROS-USARSim 兩者之間通過ROS 主題訂閱/發(fā)布消息機(jī)制進(jìn)行通信,實(shí)現(xiàn)信息傳遞.

圖4 基于ROS 架構(gòu)的地面無人平臺控制系統(tǒng)結(jié)構(gòu)Fig.4 The architecture of control system for UGV based on ROS

3.1 通信中間件

本文采用的通信中間件基于開放式機(jī)器人技術(shù)中間件(Open robot technology middleware,OpenRTM)實(shí)現(xiàn)[24],接收來自人機(jī)交互界面發(fā)送的任務(wù)類型及與任務(wù)相關(guān)聯(lián)的數(shù)據(jù),并將解析后的數(shù)據(jù)存入到指定的存儲(chǔ)區(qū)或賦值給指定的變量,同時(shí)將地面無人平臺自身的位姿數(shù)據(jù)發(fā)送給人機(jī)交互界面.任務(wù)類型由數(shù)據(jù)接收函數(shù)的返回值確定,其中1 代表遙控操作任務(wù),2 代表自主路徑跟隨任務(wù),3代表編隊(duì)協(xié)同任務(wù).同時(shí),根據(jù)不同的任務(wù)類型,通過相應(yīng)的通信中間件封裝函數(shù)獲取相應(yīng)的數(shù)據(jù),比如獲取的姿態(tài)數(shù)據(jù)主要包括仿真系統(tǒng)中其他地面無人平臺的位置、平移速度和橫擺角速度等信息.

3.2 地面無人平臺控制器

地面無人平臺控制器從人機(jī)交互界面獲取任務(wù)類型和其他地面無人平臺的位姿數(shù)據(jù),以符號v表示地面無人平臺的平移速度,符號ω表示其橫擺角速度,如圖5 所示.它根據(jù)任務(wù)類型進(jìn)入不同的線程,在獨(dú)立線程內(nèi)根據(jù)待執(zhí)行的任務(wù)進(jìn)行相應(yīng)的期望運(yùn)動(dòng)控制量計(jì)算,具體如下:

圖5 地面無人平臺控制器流程圖Fig.5 The flowchart of UGV controller

1)遙控操作任務(wù):將通過人機(jī)交互界面設(shè)定的地面無人平臺控制量(v,ω)轉(zhuǎn)發(fā)到/cmd_vel 主題上.

2)自主路徑跟隨任務(wù):將通過人機(jī)交互界面設(shè)定的全局路徑發(fā)送給地面無人平臺導(dǎo)航模塊.

3)編隊(duì)協(xié)同任務(wù):根據(jù)編隊(duì)任務(wù)數(shù)據(jù)確定自身的角色,若為領(lǐng)航者,則將任務(wù)目標(biāo)點(diǎn)發(fā)送給地面無人平臺導(dǎo)航模塊,反之,若為跟隨者,則將領(lǐng)航者位姿和隊(duì)形信息發(fā)送給領(lǐng)航者?跟隨者編隊(duì)協(xié)同控制方法,以計(jì)算跟隨者當(dāng)前的跟蹤目標(biāo)點(diǎn),并將其發(fā)送給地面無人平臺導(dǎo)航模塊進(jìn)行運(yùn)動(dòng)控制量計(jì)算.

地面無人平臺導(dǎo)航模塊計(jì)算單個(gè)地面無人平臺到達(dá)目標(biāo)點(diǎn)或者跟蹤期望全局路徑的實(shí)時(shí)控制量(v,ω),并發(fā)布在/cmd_vel 主題上.該模塊采用ROS 中已經(jīng)集成的大量開源導(dǎo)航算法,它并不是本文的研究重點(diǎn),具體可參見http://wiki.ros.org/navigation,在此不作更深入的介紹.

3.3 ROS-USARSim 接口

ROS-USARSim 接口的目的是在地面無人平臺控制器和USARSim 之間提供無縫數(shù)據(jù)交互橋梁,如圖6 所示.ROS-USARSim 與地面無人平臺控制器之間通過ROS 主題訂閱/發(fā)布消息機(jī)制進(jìn)行通信,而與USARSim 之間通過Socket 進(jìn)行通信.

圖6 ROS-USARSim 接口數(shù)據(jù)流Fig.6 The data flow of ROS-USARSim interface

ROS-USARSim 接口主要用來傳輸運(yùn)動(dòng)控制量和位姿數(shù)據(jù)兩部分,它可以自動(dòng)發(fā)現(xiàn)地面無人平臺和傳感器,訂閱地面無人平臺控制器發(fā)布在/cmd_vel 主題上的控制量,并傳遞給USARSim,從而驅(qū)動(dòng)地面無人平臺運(yùn)動(dòng).同時(shí),從USARSim 接收地面無人平臺位姿數(shù)據(jù)和傳感器數(shù)據(jù)(x,y,θ,···),將其發(fā)布在/ugv_pose 等主題上,供地面無人平臺控制器節(jié)點(diǎn)訂閱.

4 基于USARSim 的虛擬仿真場景

USARSim 是一個(gè)基于虛擬競技場引擎設(shè)計(jì)的高保真多機(jī)器人環(huán)境仿真平臺,旨在研究城市搜救環(huán)境下的機(jī)器人、多智能體和人員.通過使用UDK(Unreal development kit),USARSim 利用physX物理引擎和高質(zhì)量的3D 渲染工具創(chuàng)建了逼真的仿真環(huán)境,為機(jī)器人系統(tǒng)提供了經(jīng)過測試的平臺模型、傳感器模型和環(huán)境模型.

4.1 USARSim 仿真框架

USARSim 通過在游戲引擎之上構(gòu)建仿真,可以以低成本實(shí)現(xiàn)高保真度.仿真場景中包括各種環(huán)境模型、機(jī)器人模型以及傳感器模型,可以將使用者的全部精力投入到與機(jī)器人特定任務(wù)相關(guān)的平臺建模、控制系統(tǒng)、傳感器、交互工具和環(huán)境模型研究.

USARSim 最初建立了現(xiàn)實(shí)的災(zāi)難場景以及機(jī)器人測試方法,隨后,USARSim 獲得了廣泛的使用,并針對不同的使用場景開發(fā)了更多的環(huán)境,比如圖7 (a)的工廠環(huán)境及圖7 (b)用于DARPA 城市挑戰(zhàn)賽的道路環(huán)境.

圖7 USARSim 中的環(huán)境示例Fig.7 Sample of 3D environments in USARSim

USARSim 最初的開發(fā)重點(diǎn)是差速驅(qū)動(dòng)輪式移動(dòng)機(jī)器人,包括三種不同轉(zhuǎn)向模式的輪式平臺建模,即差速驅(qū)動(dòng)、全向驅(qū)動(dòng)和阿克曼轉(zhuǎn)向驅(qū)動(dòng),如圖8所示.USARSim 的開源框架吸引了研究者的廣泛關(guān)注和支持,這使得USARSim 現(xiàn)在可以提供多種機(jī)器人,包括水下機(jī)器人、足式機(jī)器人、類人機(jī)器人和機(jī)械臂等.像傳感器和執(zhí)行器一樣,機(jī)器人是通過特定的類別實(shí)現(xiàn)的,可以通過在運(yùn)行時(shí)讀取的配置文件進(jìn)行配置,比如指定傳感器和末端執(zhí)行器的類型,還可以配置機(jī)器人的屬性,例如電池壽命和數(shù)據(jù)傳輸頻率.因此,可以更輕松地開發(fā)對定制設(shè)計(jì)進(jìn)行建模的新平臺.

圖8 USARSim 中的地面無人平臺模型Fig.8 Models of UGVs in USARSim

4.2 地面無人平臺運(yùn)動(dòng)控制模型

在USARSim 虛擬場景中,地面無人平臺既可平移,又可轉(zhuǎn)動(dòng),共有6 個(gè)自由度,分別以位置量(x,y,z)和轉(zhuǎn)動(dòng)量 (φ,θ,ψ)表示,其中,φ、θ和ψ分別為側(cè)傾角、俯仰角和橫擺角.在本仿真系統(tǒng)中,假定地面無人平臺經(jīng)常工作于平坦地面,因此,可以對地面無人平臺的運(yùn)動(dòng)進(jìn)行簡化,僅考慮地面無人平臺沿x,y方向的平移及繞z軸的橫擺運(yùn)動(dòng),忽略沿垂直方向的位移、俯仰角和側(cè)傾角,即z=φ=ψ=0.基于這一假設(shè),以符號v表示地面無人平臺的平移速度,符號ω表示其橫擺角速度,它應(yīng)滿足如下的運(yùn)動(dòng)學(xué)約束條件:

如果以z=[x,y,θ]T表示狀態(tài)矢量,u=[v,ω]T表示控制矢量,由式(1)可得地面無人平臺控制系統(tǒng)的狀態(tài)轉(zhuǎn)移方程表達(dá)式為:

因此,根據(jù)式(2)的狀態(tài)轉(zhuǎn)移方程表達(dá)式可以推算估計(jì)地面無人平臺在控制量u的作用下的實(shí)時(shí)位姿狀態(tài)z.

5 結(jié)果與分析

為了驗(yàn)證所提出的基于USARSim 和ROS 的地面無人平臺編隊(duì)協(xié)同仿真系統(tǒng)的有效性,本文采用在實(shí)際工程中被廣泛應(yīng)用的領(lǐng)航者?跟隨者編隊(duì)控制方法進(jìn)行測試[25],并對比該控制方法分別在Matlab/Simulink 和LabVIEW 兩種仿真系統(tǒng)下的測試結(jié)果.在本次測試實(shí)驗(yàn)中,采用三輛地面無人平臺參與編隊(duì)任務(wù),其運(yùn)動(dòng)控制模型見第4.2 節(jié).在仿真測試實(shí)驗(yàn)階段,共采用了五臺計(jì)算機(jī)進(jìn)行模擬,計(jì)算機(jī)配置為酷睿TMi7-9 700 處理器,主頻可達(dá)3.0 GHz,其中三臺計(jì)算機(jī)作為地面無人平臺控制器,一臺計(jì)算機(jī)運(yùn)行USARSim 虛擬仿真場景,另一臺計(jì)算機(jī)運(yùn)行人機(jī)交互界面,具體的測試硬件配置見表1 所示.

表1 仿真測試硬件配置Table 1 Hardware configuration in simulation

5.1 仿真測試的編隊(duì)協(xié)同控制方法

地面無人平臺編隊(duì)協(xié)同仿真系統(tǒng)主要用來驗(yàn)證測試不同的編隊(duì)協(xié)同控制方法,本文以領(lǐng)航者?跟隨者編隊(duì)控制方法為例進(jìn)行該編隊(duì)協(xié)同仿真系統(tǒng)的性能測試.人機(jī)交互界面主要通過UDP 協(xié)議進(jìn)行編隊(duì)隊(duì)形任務(wù)發(fā)布、接收和發(fā)布各地面無人平臺的位姿,而三個(gè)地面無人平臺則根據(jù)編隊(duì)任務(wù)進(jìn)行自主決策.通過人機(jī)交互界面設(shè)定領(lǐng)航者的任務(wù)路徑,編隊(duì)隊(duì)形為三角形,如圖9 所示,以地面無人平臺UGV_0 為領(lǐng)航者,UGV_1 和UGV_2 為跟隨者.U G V_0 的運(yùn)動(dòng)根據(jù)任務(wù)路徑計(jì)算獲得,而UGV_1 和UGV_2 的運(yùn)動(dòng)控制量則根據(jù)自身的當(dāng)前位姿和編隊(duì)隊(duì)形控制方法實(shí)時(shí)計(jì)算獲得.

圖9 三角形編隊(duì)示意圖Fig.9 The diagram of triangle formation

以(xi,yi)表示地面無人平臺在世界坐標(biāo)系中的位置坐標(biāo),θi為地面無人平臺在世界坐標(biāo)系中的航向角,di為跟隨地面無人平臺質(zhì)心到領(lǐng)航地面無人平臺質(zhì)心的距離,φi是跟隨地面無人平臺的y軸到領(lǐng)航地面無人平臺質(zhì)心的角度,i為地面無人平臺編號.因此,領(lǐng)航者UGV_0 的位姿向量是[x0y0θ0]T,跟隨者UGV_1 的位姿向量是 [x1y1θ1]T,跟隨者UGV_2 的位姿向量是[x2y2θ2]T,而領(lǐng)航者和跟隨者的控制輸入分別是平移速度和角速度,即 [v0ω0]T、[v1ω1]T和 [v2ω2]T.領(lǐng)航者和跟隨者的航向角差值分別是α1=θ0?θ1和α2=θ0?θ2.因此,UGV_0 和UGV_1 編隊(duì)簡化運(yùn)動(dòng)控制模型如式(3)所示.

式中,δ1=φ1+α1.

為了達(dá)到和維持理想的地面無人平臺編隊(duì),地面無人平臺系統(tǒng)的狀態(tài)估計(jì)采用無跡卡爾曼濾波(Unscented Kalman filter,UKF)算法.從式(3)可以看出,對于距離?角度編隊(duì)控制方案,UKF濾波算法只需要觀測角度信息φ1和α1,即輸出觀測向量y=[φ1α1]T.利用文獻(xiàn)[26]中UKF 算法估計(jì)狀態(tài)s的角度參數(shù),帶有噪聲的地面無人平臺編隊(duì)系統(tǒng)的狀態(tài)方程和觀測方程分別以式(4)、(5)表示

式中,G是輸出轉(zhuǎn)換矩陣,O和N是零均值、協(xié)方差分別是PL和PN白高斯噪聲,且s(0)、O和N假定為不相關(guān).

對式(4)表示的編隊(duì)狀態(tài)方程進(jìn)行變形,得到與其等價(jià)的式(6),并對α1=θ0?θ1兩邊求導(dǎo),可得式(7).

式中,sr=[d1φ1]T,M(s),N(s)∈R2×2分別是f(s)的右上角和左上角的子矩陣.

基于編隊(duì)運(yùn)動(dòng)控制模型(3)和編隊(duì)狀態(tài)方程(6),采用的地面無人平臺編隊(duì)控制的輸入? 輸出狀態(tài)反饋控制規(guī)律為:

式中,C是輔助控制變量,控制增益k1,k2>0,上標(biāo)“des”指相應(yīng)變量的期望值.

UGV_0 和UGV_2 之間的編隊(duì)運(yùn)動(dòng)控制規(guī)律也可按照該方法計(jì)算,從而獲得UGV_2 的編隊(duì)控制矢量 [v2ω2]T.

在仿真測試過程中,通過人機(jī)交互界面設(shè)定發(fā)送給地面無人平臺的隊(duì)形信息d1=15 m,d2=15 m,φ1=5π/3,φ2=π/3,而領(lǐng)航者的控制輸入u0= [v0ω0]T根據(jù)任務(wù)路徑實(shí)時(shí)計(jì)算獲得,可視為已知量.依據(jù)式(8)和(9)表示的編隊(duì)運(yùn)動(dòng)控制規(guī)律,可計(jì)算獲得UGV_1 和UGV_2 保持隊(duì)形跟隨領(lǐng)航者UGV_0 的實(shí)時(shí)控制量u1=[v1ω1]T和u2= [v2ω2]T.由于受環(huán)境感知傳感器測距誤差的限制,在本次測試中設(shè)定地面無人平臺在x和y方向到達(dá)期望目標(biāo)點(diǎn)的閾值為±5 cm.

5.2 測試實(shí)驗(yàn)結(jié)果

在實(shí)驗(yàn)過程中,在USARSim 虛擬仿真場景中加載了三個(gè)地面無人平臺模型,如圖10 所示,該模型驅(qū)動(dòng)形式為四輪獨(dú)立差速,以平移速度和角速度作為輸入量進(jìn)行控制.通過人機(jī)交互界面設(shè)定領(lǐng)航者需要到達(dá)的任務(wù)目標(biāo)點(diǎn),如圖11 中圓點(diǎn)所示,三條運(yùn)動(dòng)軌跡末端的圓點(diǎn)分別表示領(lǐng)航者UGV_0和跟隨者UGV_1、UGV_2 在人機(jī)交互界面實(shí)時(shí)更新顯示的位姿.

圖10 仿真測試中的地面無人平臺模型Fig.10 The UGV model in simulation

圖11 地面無人平臺編隊(duì)運(yùn)動(dòng)軌跡Fig.11 The travelling path in formation control of UGV

依據(jù)人機(jī)交互界面設(shè)定的編隊(duì)隊(duì)形信息,在編隊(duì)任務(wù)執(zhí)行過程中,UGV_1 和UGV_2 逐漸調(diào)整為和UGV_0 保持等腰三角形隊(duì)形,三個(gè)地面無人平臺運(yùn)動(dòng)平順,直至UGV_0 運(yùn)動(dòng)到任務(wù)目標(biāo)點(diǎn)而中止運(yùn)動(dòng).UGV_0、UGV_1 和UGV_2 在任務(wù)執(zhí)行過程中的運(yùn)動(dòng)軌跡分別如圖11 中的中部、上部和下部的實(shí)線所示,從圖中可以看出,三個(gè)地面無人平臺的運(yùn)動(dòng)軌跡平滑,且隊(duì)形保持較為穩(wěn)定,更多關(guān)于這次實(shí)驗(yàn)的結(jié)果及視頻可見https://github.com/robotman801/Formation-control.

在編隊(duì)任務(wù)執(zhí)行過程中,在某一時(shí)間段內(nèi)UGV_1 和UGV_2 的平移速度和橫擺角速度的變化曲線如圖12 所示,航向角誤差曲線如圖13 所示,運(yùn)動(dòng)軌跡曲線如圖14 所示.在圖12 中,實(shí)線分別為UGV_1 的平移速度和橫擺角速度,而虛線分別為UGV_2 的平移速度和橫擺角速度.從圖12 可以看出,UGV_1 和UGV_2 的平移速度和橫擺角速度并沒有發(fā)生劇烈的突變,根據(jù)編隊(duì)控制規(guī)律輸出的控制量u1和u2合理,地面無人平臺在整個(gè)控制周期內(nèi)運(yùn)動(dòng)比較平順.在圖13 中,實(shí)線為在編隊(duì)控制過程中UGV_1 和UGV_2 的期望航向角,而虛線為UGV_1 和UGV_2 的實(shí)際航向角.從圖13可以看出,UGV_1 和UGV_2 的航向角逐漸調(diào)整并逼近期望航向角.在編隊(duì)航向角調(diào)整過程中,UGV_1 的航向角相對誤差最大為60.12 %,最小為0.26 %,如圖13(a)所示,而UGV_2 的航向角相對誤差最大為56.78 %,最小為0.43 %,如圖13 (b)所示.在圖1 4 中,實(shí)線為在編隊(duì)控制過程中UGV_1 和UGV_2 的期望軌跡曲線,而虛線為UGV_1 和UGV_2 的實(shí)際軌跡曲線.從圖14 可以看出,UGV_1 和UGV_2 的運(yùn)動(dòng)軌跡逐漸調(diào)整并逼近期望軌跡.在編隊(duì)運(yùn)動(dòng)控制過程中,UGV_1 的運(yùn)動(dòng)軌跡絕對位置誤差最大為1.98 m,最小為0.04 m,如圖14 (a)所示,而UGV_2 的運(yùn)動(dòng)軌跡絕對位置誤差最大為2.31 m,最小為0.06 m,如圖14 (b)所示.

圖12 三角形編隊(duì)任務(wù)中的速度變化Fig.12 Velocity value during triangle formation control

圖13 三角形編隊(duì)任務(wù)中的跟隨者航向角誤差Fig.13 Heading angle error for followers during triangle formation control

圖14 三角形編隊(duì)任務(wù)中的跟隨者軌跡曲線Fig.14 Trajectories for followers during triangle formation control

為了驗(yàn)證所提出的基于USARSim 和ROS 的無人平臺編隊(duì)仿真系統(tǒng)的性能優(yōu)劣,將領(lǐng)航者?跟隨者編隊(duì)控制方法在本仿真系統(tǒng)下的性能測試結(jié)果分別與在Matlab/Simulink[27]和LabVIEW[28]仿真系統(tǒng)下的測試結(jié)果進(jìn)行對比,對比結(jié)果見表2 所示.從表2 中可以看出,本文所提出的編隊(duì)仿真系統(tǒng)與Matlab/Simulink 和LabVIEW 搭建的編隊(duì)仿真系統(tǒng)相比,在編隊(duì)任務(wù)執(zhí)行過程中每個(gè)地面無人平臺均以較小誤差到達(dá)期望目標(biāo)點(diǎn),對領(lǐng)航者?跟隨者編隊(duì)控制算法的測試結(jié)果一致,表明本文所提出的仿真系統(tǒng)的有效性.然而,本文所提出的編隊(duì)仿真系統(tǒng)在場景逼真度、人機(jī)交互界面、開發(fā)簡易性和可擴(kuò)展性(比如增加環(huán)境感知和導(dǎo)航規(guī)劃)等方面具有較大優(yōu)勢.

表2 領(lǐng)航者?跟隨者編隊(duì)方法在不同仿真平臺下的測試對比Table 2 The comparison of leader-follower formation in different simulation systems

綜上所述,基于USARSim 和ROS 的地面無人平臺編隊(duì)協(xié)同仿真系統(tǒng)能夠在外界條件一致的情況下僅對編隊(duì)協(xié)同控制方法性能進(jìn)行測試,多個(gè)地面無人平臺能快速形成編隊(duì),并逐漸調(diào)整后以很小的航向角誤差和位置誤差沿期望的編隊(duì)軌跡運(yùn)動(dòng),保證了編隊(duì)的穩(wěn)定性.因此,該編隊(duì)協(xié)同仿真系統(tǒng)穩(wěn)定可靠,非常適合用于各種編隊(duì)協(xié)同控制方法的設(shè)計(jì)及驗(yàn)證測試.

6 結(jié)論

單一地面無人平臺在信息獲取及處理能力等方面是有限的,對于復(fù)雜的任務(wù)及多變的工作環(huán)境,單一地面無人平臺的能力更顯不足.多地面無人平臺比單一地面無人平臺具有更強(qiáng)的優(yōu)越性,在軍事應(yīng)用方面,通過多地面無人平臺編隊(duì)協(xié)作可以更有效地完成安全巡邏、區(qū)域偵察、探測及協(xié)同打擊等戰(zhàn)術(shù)任務(wù).

為了測試地面無人平臺編隊(duì)協(xié)同控制方法及其適用的任務(wù)場景,降低編隊(duì)協(xié)同系統(tǒng)的開發(fā)成本,本文提出了一種基于USARSim 和ROS 的地面無人平臺編隊(duì)協(xié)同仿真系統(tǒng),它由人機(jī)交互界面、基于ROS 架構(gòu)的地面無人平臺控制系統(tǒng)和基于USARSim 的虛擬仿真場景三個(gè)部分組成,具備編隊(duì)協(xié)同任務(wù)下發(fā)、任務(wù)場景設(shè)計(jì)、編隊(duì)控制算法測試和單平臺自主導(dǎo)航等功能.在該仿真系統(tǒng)中,各個(gè)地面無人平臺之間依賴局域網(wǎng)(UDP)共享位姿數(shù)據(jù),并以領(lǐng)航者?跟隨者編隊(duì)控制方法為例進(jìn)行該仿真系統(tǒng)的性能測試.實(shí)驗(yàn)結(jié)果表明,該仿真系統(tǒng)能夠在外界條件一致的情況下完成對編隊(duì)協(xié)同控制方法的性能測試,系統(tǒng)穩(wěn)定可靠,為研究地面無人平臺編隊(duì)協(xié)同控制算法提供了新的思路和快速驗(yàn)證工具.通過模塊化的接口設(shè)計(jì),該仿真系統(tǒng)具有較好的開放性與通用性,然而,各個(gè)模塊之間需通信網(wǎng)絡(luò)進(jìn)行信息傳遞,協(xié)作性能高度依賴通信網(wǎng)絡(luò)質(zhì)量,當(dāng)通訊受到強(qiáng)干擾時(shí)存在協(xié)作失敗風(fēng)險(xiǎn).因此,該仿真系統(tǒng)需進(jìn)一步開發(fā)不依賴通信網(wǎng)絡(luò)的編隊(duì)協(xié)同控制算法測試模塊,以滿足不依賴通信網(wǎng)絡(luò)的編隊(duì)協(xié)同控制算法的測試需求.