基于有限資源的微小型多機器人協(xié)同避障策略

李 月，高峻峣，趙靖超，劉 軼

（北京理工大學(xué) 機電學(xué)院，北京100081）

0 引 言

相對位置測量是機器人編隊與環(huán)境探測的實現(xiàn)基礎(chǔ)，但目前常用的相對定位方法需要機器人擁有昂貴的傳感器、大存儲容量與高計算能力，如利用激光雷達、攝像頭等［3，4］，因此并不適用于低成本、低功耗的微小型機器人。全球定位系統(tǒng) （GPS）［5］相對于微小型機器人誤差較大，在有遮擋或室內(nèi)環(huán)境下無法獲得位置信息。有些研究利用超聲環(huán)進行相對位置估計［6］，但機器人體積限制了可固定聲吶的個數(shù)，從而會產(chǎn)生較大定位誤差，傳播較慢的超聲信號還會降低實時定位速度。面向資源有限的微小型多機器人系統(tǒng)，設(shè)計了一種基于價格低廉、質(zhì)量較輕的紅外測距傳感器與微型舵機的局部測量系統(tǒng)，利用擴展卡爾曼濾波技術(shù)，融合機器人運動補償，在線快速測量與估計相鄰機器人或障礙物的相對距離與角度。針對非結(jié)構(gòu)化環(huán)境，采用 “領(lǐng)航者－跟隨者”的編隊結(jié)構(gòu)［7］，根據(jù)領(lǐng)航者實時探測到的局部環(huán)境信息，多機器人進行隊形保持與靈活的隊形變換，從而保證機器人群作為整體順利通過障礙區(qū)域。

1 相對定位系統(tǒng)設(shè)計

式中：ds——紅外傳感器探測面距離旋轉(zhuǎn)中心的距離。

圖1 局部測量系統(tǒng)的硬件實現(xiàn)

令一臺機器人為總領(lǐng)航者，其余機器人分別跟蹤?quán)徑鼨C器人軌跡并保持一定位置關(guān)系，組成 “分領(lǐng)航者－跟隨者”編隊結(jié)構(gòu)。局部感知系統(tǒng)具有兩種工作模式：①障礙物探測模式：如圖2 （a）所示，總領(lǐng)航機器人負責(zé)環(huán)境探測與導(dǎo)航，舵機的旋轉(zhuǎn)角度從0°到90°，兩個傳感器同時工作，形成相對于機器人航向－90°到90°的探測范圍；②位置估計模式：跟隨機器人的軌跡跟蹤需要實時獲取鄰近機器人的相對距離與角度，為減少對機器人的識別與相對定位時間，對舵機角度范圍α 采用自調(diào)節(jié)控制，如圖2 （b）所示，僅激活一個紅外傳感器的探測功能，通過控制舵機軸旋轉(zhuǎn)不斷測量與領(lǐng)航機器人的距離，同時存儲距離值dk與舵機角度值γk，如果在設(shè)定的時間閾值TD內(nèi)探測不到機器人邊緣，舵機軸反方向旋轉(zhuǎn)，開始新的測量周期。根據(jù)機器人典型的外形形狀，可利用局部感知系統(tǒng)對外界環(huán)境中的機器人進行有效區(qū)分與識別，機器人相對位置的估計方法具體見第3部分。

圖2 傳感系統(tǒng)的兩種操作模式

2 協(xié)同避障運動規(guī)劃

本文主要研究基于局部感知的多機器人協(xié)同避障，而領(lǐng)航機器人的路徑導(dǎo)航不是主要研究內(nèi)容，僅考慮領(lǐng)航機器人對環(huán)境的探測與跟隨機器人的協(xié)同編隊行為，并做出如下假設(shè)：①遇到障礙物前機器人隊形已形成；②領(lǐng)航機器人與跟隨機器人間的距離不超過紅外傳感器所能探測到的范圍；③一個采樣周期中機器人的線速度與角速度恒定不變。

由于通信約束與較弱的感知能力，微小型多機器人在避障時需要保持一定的隊形與網(wǎng)絡(luò)連通性。考慮由n 個機器人組成的多機器人系統(tǒng)，rL為總領(lǐng)航機器人，為防止其它機器人rFi（i＝1，2，…，n－1）對rL與環(huán)境的識別混淆，rL負責(zé)障礙物的探測，而rFi實時跟蹤各自被分配的領(lǐng)航者的軌跡。多機器人隊形的協(xié)同變換主要取決于隊形尺寸和障礙物相對位置，以圖3的多機器人系統(tǒng)為例，探測到障礙物前機器人呈三角隊形，rL的探測范圍為xL∈［0，xm＋dL］，yL∈［－dL，dL］，dL是傳感器測量極值，機器人半徑為Rr，rF1與rF2相對于rL的距離和角度分別為（l1，α1）T和（l2，α2）T。為避免碰撞，機器人間或機器人與障礙物間的最小距離不可超過設(shè)定的安全閾值Ds。

圖3 掃描范圍的區(qū)域劃分

若在180°范圍內(nèi)檢測到障礙物，由兩個傳感器測得的障 礙 點 坐 標(biāo) 分 別 被 存 為（，），k ＝1，2，…，p 與（，），k＝1，2，…，q。取點集｛｝與｛｝中距離XL軸最近的點與，即滿足＝min｛｝與，點與之間沿YL軸的距離定義為。結(jié)合隊形尺寸，將rL的探測范圍分為5個區(qū)域，劃分規(guī)則見表1，進而得到如下協(xié)同避障規(guī)則：

（1）如果障礙點在區(qū)域I，rL規(guī)劃路徑躲避障礙物，如圖4 （a）所示。

表1 掃描區(qū)域劃分原則

圖4 協(xié)同避障運動模式

3 相對定位算法

3.1 運動補償

由于機器人的運動會影響傳感器的測量準(zhǔn)確性，所以要對所測量點進行坐標(biāo)補償。如圖5所示，從t＝t1至t＝tm的測量周期內(nèi)，rF探測到rL的一系列點PFk（k＝1，2，…，m），通過存儲的距離與角度值（dk，γk），由式 （1）～式（2）得到相對于Xrk－Yrk坐標(biāo)系下的坐標(biāo)值（x）。rL從t＝tk到t＝tm走過的距離為＝vL（m－k）Δt。設(shè)在t＝tm，被測點移動到新的位置），則點相對Xrk－Yrk坐標(biāo)系的位置坐標(biāo)為

圖5 相對運動的坐標(biāo)補償

式中：（xkm，ykm）為Xrk－Yrk坐標(biāo)系原點相對于Xrm－Yrm坐標(biāo)系的位置坐標(biāo)

3.2 相對位置估計

根 據(jù)所獲取的rL系 列 邊 緣 點 進 行 對rL位 置（）T的估計計算，先利用馬氏距離方法去除噪聲點，得到一系列坐標(biāo)值｛（xm（k），ym（k））｝。設(shè)這些點位于圓上

展開得

式中：η（k）為狀態(tài)變量［C1，C2，C3］T，C1＝－，C2＝－，C3＝＾＋－，y（k）為 理 想 測 量 值［xm（k），ym（k）］T。下面 采 用 擴 展 卡 爾 曼 濾 波 （EKF）算 法［8，9］求t＝tm時刻rL中心在局部坐標(biāo)系Xrm－Yrm下的坐標(biāo)。為了得到標(biāo) 準(zhǔn) 形 式 的 測 量 方 程，對f（η（k），y（k））關(guān) 于 （（k），（k））展開，其中（k）是η（k）的一階估計，）是實際測量值

根據(jù)

給定狀態(tài)變量的初始值為η（0），誤差協(xié)方差矩陣的初始值為P（0），（0）＝E［η（0）］，P（0）＝E［η（0）ηT（0）］狀態(tài)變量和誤差協(xié)方差的預(yù)測值如下

卡爾曼增益矩陣、狀態(tài)估計與狀態(tài)誤差協(xié)方差矩陣的更新形式如下

為令rF按期望的幾何位置關(guān)系跟蹤rL軌跡，基于建立如圖6的rF位置誤差模型，rFd為rF下個時刻的期望位置，θL與θF分別為rL與rF的方向角度，給定的相對距離與角度分別為ld和αd。設(shè)rF的位置誤差［xe，ye，θe］T為

式中：β＝π＋θF－θL－αd。根據(jù)rL的控制輸入［vL，ωL］T，求出rF期望位置的線速度與角速度

式中：χ＝θL＋αd－π，＝ωL。本文借鑒基于反步法的機器人軌跡跟蹤控制器，令rF的位置誤差收斂到0［10］

式中：kx與kθ為正常數(shù)，且滿足：當(dāng)θe→0 時s→1成立。

圖6 機器人位置誤差模型

4 實驗結(jié)果與分析

為了驗證基于相對測量系統(tǒng)的多機器人編隊控制的有效性，構(gòu)建了室內(nèi)環(huán)境下微小型多機器人實驗系統(tǒng)，如圖7所示，該實驗系統(tǒng)由3臺微小型移動機器人、頂部攝像頭與靜態(tài)障礙物構(gòu)成。機器人平臺為Φ200mm×100mm 的兩輪差分機器人，每臺機器人均安裝了相對定位系統(tǒng)，載有ARM9嵌入式控制平臺進行信息處理與運動控制，通過Zigbee無線通信模塊 （CC2530）與PC 機通信。領(lǐng)航機器人rL與跟隨機器人rF1、rF2的初始位置分別為 （500mm，0mm，0°），（0mm，500mm，0°）與 （0mm，－500mm，0°），rL控制輸入為vL＝0.06m／s，ωL＝0rad／s。兩個障礙物尺寸分別為350mm×450mm 和200mm×300mm，障礙物的尺寸與位置對于機器人來講均為未知。通過PC 機Visual Studio 2008對頂部CCD 攝像頭 （640×480）進行圖像處理，實時測量與存儲機器人的實際位置，為基于相對定位系統(tǒng)的位置估計提供比較標(biāo)準(zhǔn)。

圖7 實驗系統(tǒng)

圖8為機器人的實際運動軌跡。初始階段rF1與rF2保持三角隊形對rL進行跟蹤。t＝2s，第一個障礙物出現(xiàn)在rL的掃描范圍內(nèi)，rf1與rf2將測得的距離與角度值（l1，α1）T與（l2，α2）T發(fā)送給rL，rL進行隊形變換判斷，通過減少rf1與rL之間的橫向距離避免與障礙物的碰撞；t＝20s，第二個障礙物被測到，通過減少rf2相對rL的橫向距離，從而得到無碰撞的光滑路徑。圖9，圖10 中虛線所示的是rf1與rf2利用相對定位系統(tǒng)分別得到的rL在x軸與y軸的相對坐標(biāo)值。通過對比頂部攝像頭測得的實際相對位置 （黑色實線），可以看出該相對定位系統(tǒng)及估計算法在實際機器人編隊與避障應(yīng)用中效果較好，可滿足應(yīng)用要求，同時微小型多機器人系統(tǒng)通過保持一定編隊形式的協(xié)同運動，順利駛出障礙區(qū)域。

圖8 多機器人運動軌跡

圖9 領(lǐng)航機器人相對于跟隨機器人1實際的與被估計的相對坐標(biāo)

圖10 領(lǐng)航機器人相對于跟隨機器人2實際的與被估計的相對坐標(biāo)

5 結(jié)束語

本文提出了一種基于紅外測距傳感器的相對定位系統(tǒng)，用于機器人對鄰近機器人相對位置的估計與周圍環(huán)境的探測。同時，利用該相對定位方法，通過靈活調(diào)整機器人間的相對距離和角度，實現(xiàn)了適用于微小型多機器人在靜態(tài)障礙物環(huán)境中的協(xié)同運動，有效避免與外部環(huán)境的碰撞。真實環(huán)境下的實驗結(jié)果驗證了所提途徑的有效性與可靠性。下一步的研究工作將會考慮在更加復(fù)雜環(huán)境中多機器人的運動規(guī)劃與協(xié)同方法。

［1］Mehrjerdi Hasan，Ghomman Jawhar，Saad Maarouf.Nonlinear coordination control for a group of mobile robots using a virtual structure［J］.Mechatronics，2011，21 （7）：1147－1155.

［2］Han Jinlu，Xu Yaojin，Di Long，et al.Low－cost multi－UAV technologies for contour mapping of nuclear radiation field ［J］.Journal of Intelligent and Robotic Systems，2013，70 （1－4）：401－410.

［3］Juan Marcos Toibero，F(xiàn)lavio Roberti，Ricardo Carelli，et al.Switching control approach for stable navigation of mobile robots in unknown environments ［J］.Robotics and Computer－Integrated Manufacturing，2011，27 （3）：558－568.

［4］Filippo Arrichiello，Stefano Chiaverini，Vaibhav Kumar Mehta.Experiments of obstacles and collision avoidance with a distributed multi－robot system ［C］／／IEEE International Conference on Information and Automation，2012：727－732.

［5］Akkaya Ramazan，Omer Aydogdu，Suleyman Canan.An ANN based NARX GPS／DR system for mobile robot positioning and obstacle avoidance ［J］.Journal of Automation and Control，2013，1 （1）：6－13.

［6］Atsushi Fujimori，Hiroshi Kubota，Naoya Shibata，et al.Leader－follower formation control with obstacle avoidance using sonar－equipped mobile robots ［J］.Proceedings of the Institution of Mechanical Engineers，Part I：Journal of Systems and Control Engineering，2014，228 （5）：303－315.

［7］Xu Lingyi，Cao Zhiqiang，Zhao Peng，et al.The identifierbased relative position estimation for leader－follower robotic system ［C］／／IEEE International Conference on Mechatronics and Automation，2014：1691－1695.

［8］Rusdinar Angga，Kim Jungmin，Lee Junha，et al.Implementation of real－time positioning system using extended Kalman filter and artificial landmark on ceiling ［J］.Journal of Mechanical Science and Technology，2012，26 （3）：949－958.

［9］Wei Zhuo，Yang Simon X.Neural network based extended Kalman filter for localization of mobile robots［C］／／World Congress on Intelligent Control and Automation，2011：937－942.

［10］Jan Chen，Sun Dong.Resource constrained multirobot task allocation based on leader–follower coalition methodology［J］.The International Journal of Robotics Research，2011，30 （12）：1423－1434.