基于分布式PID控制的ROS小車編隊(duì)方法研究

北方工業(yè)大學(xué)信息學(xué)院 趙周權(quán) 劉博文 宋卓儒 任 德

在電子信息、計(jì)算機(jī)、以及AI等領(lǐng)域發(fā)展下，車輛集群已經(jīng)受到了眾多學(xué)者關(guān)注。浙江理工大學(xué)學(xué)者提出了一種分層式的智能小車軟件系統(tǒng)框架設(shè)計(jì)與路徑規(guī)劃方法研究，上海交通大學(xué)學(xué)者研制了CvberC3智能車，實(shí)現(xiàn)了基于視覺(jué)、磁傳感器、激光雷達(dá)等傳感器導(dǎo)航的實(shí)驗(yàn)驗(yàn)證。

本文以創(chuàng)樂(lè)博出品的小車平臺(tái)作為研究對(duì)象，采用話題訂閱作為車-車通訊協(xié)議，同時(shí)采用WIFI和互聯(lián)網(wǎng)進(jìn)行遠(yuǎn)端到主機(jī)的信息傳輸，使用PID算法實(shí)現(xiàn)小車的控制。實(shí)現(xiàn)小車集群的通訊、控制、協(xié)同等，為未來(lái)進(jìn)一步的現(xiàn)實(shí)測(cè)試提供理論和實(shí)踐基礎(chǔ)。

1 ROS小車編隊(duì)運(yùn)動(dòng)模型

小車基于室內(nèi)全向移動(dòng)機(jī)器人技術(shù)通過(guò)ROS STM32驅(qū)動(dòng)板控制，測(cè)量小車狀態(tài)。使用了ROS Ubuntu mate 16.04系統(tǒng)控制主板，傳輸信息，可視化界面，計(jì)算位置等功能。小車編隊(duì)由1輛領(lǐng)航者和2輛跟隨者組成。每輛小車都有左右兩個(gè)履帶馬達(dá)，一臺(tái)激光測(cè)距模塊和互相之間通訊的Wi-Fi模塊。PC端與領(lǐng)航者使用ROS實(shí)時(shí)通訊，領(lǐng)航者與跟隨者使用ROS創(chuàng)建話題進(jìn)行實(shí)時(shí)通訊。最后通過(guò)PC平臺(tái)分析小車傳回的數(shù)據(jù)，驗(yàn)證并實(shí)現(xiàn)了小車編隊(duì)。

1.1 小車運(yùn)動(dòng)學(xué)模型

如圖1所示，此模型使用[x,y,θ,ω]來(lái)確定小車的運(yùn)動(dòng)狀態(tài)。其中，[x,y]為小車質(zhì)心在笛卡爾坐標(biāo)平面的位置坐標(biāo)，θ為小車在水平方向的方向角，ω為小車的兩個(gè)輪子速度。由于小車是雙輪差分驅(qū)動(dòng)。通過(guò)雙輪差動(dòng)機(jī)器人曲線算法，可以使用如下計(jì)算小車的速度為：

其中v為車相對(duì)于地面的速度，ωL為左輪的線速度，ωR為右輪的線速度；小車的角速度為（左轉(zhuǎn)，即當(dāng)ωL<ωR時(shí)）：

其中ωC為小車的角速度，l為小車兩輪間距。

本文主要研究了輪式機(jī)器人楔形隊(duì)列編隊(duì)實(shí)現(xiàn)，為了能形成特定隊(duì)形，小車編隊(duì)需要確定隊(duì)形參考點(diǎn)。本文使用領(lǐng)航者作為參考點(diǎn)，基于領(lǐng)航者的位置決定參考點(diǎn)位置。

圖1 雙輪差分驅(qū)動(dòng)小車運(yùn)動(dòng)模型

圖2 小車編隊(duì)拓?fù)浣Y(jié)構(gòu)

圖3 小車楔形編隊(duì)

如圖2所示，PC創(chuàng)建一個(gè)話題，將設(shè)定的目標(biāo)點(diǎn)(x,y,θ)生成std_msgs通過(guò)topic發(fā)送到領(lǐng)航者節(jié)點(diǎn)。通過(guò)領(lǐng)航者內(nèi)置導(dǎo)航模塊，并實(shí)時(shí)獲取小車位置信息(x1,y1,θ1)由此計(jì)算出跟隨者1，跟隨者2的位置坐標(biāo)(x2,y2,θ2)和(x3,y3,θ3)將其生成std_msgs通過(guò)topic發(fā)送到跟隨者1節(jié)點(diǎn)和跟隨者2節(jié)點(diǎn)。

1.2 編隊(duì)實(shí)現(xiàn)PID算法

控制結(jié)構(gòu)是遠(yuǎn)端指揮小車集群的主要方式，控制結(jié)構(gòu)主要關(guān)心遠(yuǎn)端-主車（遠(yuǎn)端控制器）向一臺(tái)人為設(shè)置的，功能更加完善的主車發(fā)送消息，指定集群要到達(dá)的位置或完成的目標(biāo)的交互和主車-從車（主車將遠(yuǎn)端下發(fā)的任務(wù)拆解為一個(gè)個(gè)從車需要執(zhí)行的動(dòng)作，并通過(guò)內(nèi)部聯(lián)絡(luò)機(jī)制進(jìn)行通訊、分發(fā)和確認(rèn)）的交互，如果是以分布式系統(tǒng)為主要控制結(jié)構(gòu)的小車集群，則是遠(yuǎn)端-集群的控制方式，即遠(yuǎn)端作為集群網(wǎng)絡(luò)的一個(gè)節(jié)點(diǎn)，直接向網(wǎng)絡(luò)發(fā)送指令，邏輯上靠近遠(yuǎn)端或先收到指令的小車解析指令并轉(zhuǎn)化為自身執(zhí)行的動(dòng)作，并將該指令轉(zhuǎn)發(fā)給其他節(jié)點(diǎn)。

本文利用ROS_TOPIC實(shí)現(xiàn)無(wú)人車通信控制，并利用/odom與/cmd_vel實(shí)現(xiàn)無(wú)人車的移動(dòng)控制功能。采取控制算法是分布式PID算法。本文采用了分布式PID控制算法，每個(gè)小車僅使用其鄰居的相對(duì)距離誤差作為控制反饋，實(shí)現(xiàn)協(xié)同控制，通信拓?fù)淙鐖D2所示。控制算法如下：

2 分布式ROS小車編隊(duì)實(shí)驗(yàn)

實(shí)驗(yàn)環(huán)境地點(diǎn)為北方工業(yè)大學(xué)智能仿生感知聯(lián)合研究中心，實(shí)驗(yàn)時(shí)間為15:00，實(shí)驗(yàn)場(chǎng)地?zé)o遮擋，且地面光滑平坦。智能編隊(duì)系統(tǒng)采用了消費(fèi)級(jí)IMU，型號(hào)為MPU-6050，其角速度全格感測(cè)范圍為±250、±500、±1000與±2000°/sec，敏感度為131 LSBs/°/sec ，以數(shù)字輸出6軸或9軸的旋轉(zhuǎn)矩陣、四元數(shù)歐拉角格式的融合演算數(shù)據(jù)。激光雷達(dá)型號(hào)為RPLIDAR-A1，測(cè)量頻率為8000/s，掃描頻率為5.5HZ。本實(shí)驗(yàn)中智能無(wú)人車的編隊(duì)基于ROS的底層通訊，利用ROStopic從主機(jī)端傳輸ros_msg到從機(jī)端，如圖3所示，實(shí)驗(yàn)中，通過(guò)控制主車沿正方形路徑進(jìn)行移動(dòng)，并返回起點(diǎn)。運(yùn)動(dòng)過(guò)程中，主車不斷地發(fā)送自身的位置信息，兩輛從車根據(jù)接收到的主車運(yùn)動(dòng)位置的變化，會(huì)跟隨主車進(jìn)行協(xié)同運(yùn)動(dòng)并保持隊(duì)形不變。從實(shí)驗(yàn)結(jié)果表明，基于ROS的小型無(wú)人車智能協(xié)同編隊(duì)表現(xiàn)情況優(yōu)異，可應(yīng)用于實(shí)際中。

3 結(jié)論

本文在傳統(tǒng)ROS小車激光導(dǎo)航的基礎(chǔ)上增加了小車間通信的模塊，實(shí)現(xiàn)車輛實(shí)時(shí)的信息傳輸。并借此使用領(lǐng)航者-跟隨者模型實(shí)現(xiàn)了小車編隊(duì)間坐標(biāo)傳輸。分別實(shí)現(xiàn)了一字編隊(duì)與楔形隊(duì)列的算法。在未來(lái)研究中，可以基于此模型進(jìn)行更為復(fù)雜的六邊形編隊(duì)，以及實(shí)現(xiàn)對(duì)個(gè)別車輛的運(yùn)動(dòng)狀態(tài)進(jìn)行微調(diào)。