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

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

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

0 引 言

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

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

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

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

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

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

2 協(xié)同避障運(yùn)動(dòng)規(guī)劃

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

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

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

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

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

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

圖4 協(xié)同避障運(yùn)動(dòng)模式

3 相對(duì)定位算法

3.1 運(yùn)動(dòng)補(bǔ)償

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

圖5 相對(duì)運(yùn)動(dòng)的坐標(biāo)補(bǔ)償

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

3.2 相對(duì)位置估計(jì)

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

展開得

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

根據(jù)

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

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

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

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

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

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

圖6 機(jī)器人位置誤差模型

4 實(shí)驗(yàn)結(jié)果與分析

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

圖7 實(shí)驗(yàn)系統(tǒng)

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

圖8 多機(jī)器人運(yùn)動(dòng)軌跡

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

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

5 結(jié)束語

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