具有速度、加速度約束的機器人編隊避障控制

張志偉，滕英元，楊慧欣，倪智宇

(沈陽航空航天大學航空宇航學院，遼寧沈陽 110136)

1 引言

機器人技術(shù)是一項融合跨學科的高科技領(lǐng)域技術(shù)，機器人編隊控制廣泛應用于巡邏、搜救、偵察、空間探測等領(lǐng)域[1-6].機器人編隊避障控制問題是一項基礎(chǔ)且關(guān)鍵的重要問題，受到人們廣泛的關(guān)注，已經(jīng)成為科技研究的熱點[7-9].最新文獻表明，機器人可以作為一個工作組合作完成監(jiān)視任務或者在敵對地區(qū)從事間諜工作[10-12].在民用應用中，需要系統(tǒng)來自動駕駛車輛.此外，在物聯(lián)網(wǎng)中，需要機器人來合作運送物品.

目前比較流行的循環(huán)追蹤(cyclic pursuit)控制思想實際上是從生物學的角度得來的，如蟻群算法[13]、飛行中的鳥群[14]等，可以分為線性循環(huán)追蹤(linear cyclic pursuit，LCyP)和非線性循環(huán)追蹤(nonlinear cyclic pursuit，NlCyP).研究表明[15-17]，當人們用線性循環(huán)追蹤的思想描述多個體系統(tǒng)達到平衡狀態(tài)時，個體的運動軌跡在平衡點處穩(wěn)定，而多個體的線性循環(huán)追蹤將收斂到編隊的初始中心[18-19].人們對于非線性模型下的循環(huán)追蹤控制問題和穩(wěn)定性進行了分析，研究表明[20-23]，非線性的循環(huán)追蹤編隊形成穩(wěn)定構(gòu)形時，其編隊中心也被初始位置固定.當LCyP與NlCyP應用于即時、在線控制時，可以收斂到期望的構(gòu)形，且具有較好的穩(wěn)定性與魯棒性.穩(wěn)定運行的編隊，其中心位置與運動過程無關(guān)，始終保持在初始幾何的中心，而且在編隊達到穩(wěn)定前，無法提前確定穩(wěn)定構(gòu)形的中心位置.從實際應用角度看，這就降低了其適用性，使得一些具有特征典型的任務設(shè)計無法實施.要改進編隊中心可變的性質(zhì)，就需要引入新的控制算法，如引入非成員的導引參考，用于保證固定信標周圍機器人運動的全局漸近穩(wěn)定性.Ceccarelli等在研究多個體系統(tǒng)(multi-agent system，MAS)的集體定位行為時，提出了引入虛擬參考航標作為運動導引的思想[24]，對單個具有指定旋轉(zhuǎn)方向的成員，設(shè)計了具有全局漸近穩(wěn)定性的“航標”導引控制法則，較好地解決成員與指定點的相對位置問題.

同時，在編隊運動過程中，對機器人速度和加速度大小沒有限制.而在實際應用中，機器人本身對速度和加速度的大小有一定的限制，當速度和加速度大小超過機器人的截斷速度時，機器人只會以自己允許的最大速度運行.Ren、張玉杰等[25-26]利用控制Lyapunov函數(shù)方法克服了輸入約束對控制器設(shè)計帶來的困難.Gu、顧萬里等[27-28]提出了一種適用于速度受限移動機器人的跟蹤控制器.該算法考慮了移動機器人的速度約束，對于移動機器人的執(zhí)行器，無法跟蹤加速度過大的速度命令.Chen X等[29]研究了單機避障問題，結(jié)果表明，在速度和加速度約束條件下，單機的避障識別和控制效果很好.Wang Zhiteng等[30]分析單機的避障控制原理，在不考慮最短路徑時，避障控制效果顯著.對多移動機器人避障問題的研究，以往人們常采用B rooks[31]提出的基于行為的反應式控制及宋梅萍等[32]提出的基于規(guī)則的控制策略等方法，這些方法適用于任務和環(huán)境很簡單的情況.四叉樹法[33]更適用于模型環(huán)境不發(fā)生動態(tài)變化的情況.張純剛等[34]提出的基于局部感知的窗口滾動法，適用于未知環(huán)境，并給出了局部最優(yōu)解.Giuseppe Franzè 和Walter Lucia[35]提出了滾動時域策略，用于解決動態(tài)環(huán)境中線性定常系統(tǒng)所描述的自主移動機器人的路徑規(guī)劃問題.

本文在前人工作的基礎(chǔ)上，聚焦于避障識別的實域性，即避障的識別及算法的時效性，即收斂的效率.設(shè)置了不同的局部控制增益，考慮了速度和加速度約束，同時引入了懲罰因子.采用該優(yōu)化算法，實現(xiàn)了以下3個功能:1)實現(xiàn)非失效機器人的降級重組，避免機器人之間發(fā)生碰撞;2)實現(xiàn)控制算法的穩(wěn)定與收斂;3)解決了機器人編隊構(gòu)型穩(wěn)定后向前行進過程中的避障問題.研究結(jié)果驗證了所提控制方法的正確性和有效性.

2 多機器人的編隊模型

機器人的位姿用[x y θ]T表示，(x，y)∈R2表示個體在平面內(nèi)的位置，θ表示運動方向，(u，ω)∈R2表示切向速度和角速度控制輸入.因此，機器人的運動方程為

因此，動力學模型為

本文采用的多機器人編隊方式如圖1-2所示.

圖1 循環(huán)追蹤系統(tǒng)相對坐標Fig.1 Relative coordinates of cyclic pursuit system

圖2 多機器人編隊構(gòu)型Fig.2 Multi-robot formation configuration

圖中:(xi，yi，θi)表示前邊第i個機器人的位置和導向角，(xi-1，yi-1，θi-1)表示跟隨的第i-1個機器人的位置和方向.ri-1表示機器人i與i-1之間的距離，αi-1指從機器人i-1速度方向與機器人i連線方向的夾角，βi-1表示機器人i-1速度方向與機器人i速度方向夾角的補角.

因此，多移動機器人編隊的運動方程為

其中:T=[ri-1φ]T; φ=π-αi-1-βi-1; ε=θiθi-1是機器人i和機器人i-1的相對方位;zi和Zi-1分別是機器人i和機器人i-1的速度矢量.

其中γ=φ+ε.值得注意的是T，γ和φ分別是T(t)，γ(t)和φ(t)的瞬時值，所以T的值為

3 控制算法優(yōu)化

對于NlCyP控制的機器人編隊，在其期望的編隊中心設(shè)置“航標”(navigation’s beacon，NB)，引導機器人循環(huán)追蹤完成均勻分布的同時，通過引入了非成員的參考功能解決其集體定位問題，使其能夠收斂到相對“航標”的指定距離.“航標”示意圖如圖3所示.

圖3 機器人相對航標示意圖Fig.3 Diagram of robot relative navigation’s beacon

由于定速追蹤的運動是變速追蹤的穩(wěn)定情況，所以，設(shè)機器人編隊達到穩(wěn)定速率vR前，速率按vi=krri變化，kr為常數(shù).基于NB 控制律[24]對系統(tǒng)實行全員“航標”引導的非線性循環(huán)追蹤控制編隊時(nonlinear cyclic pursuit in navigation’s beacon to all agents，NlCyP&NB-a)，運動學方程為

機器人組成的MAS編隊，假設(shè)編隊前機器人的初始坐標定義為[x，y].

3.1 降級重組

當編隊過程中個別成員出現(xiàn)監(jiān)測執(zhí)行功能失效問題(但仍能運行)時，NlCyP&NB-a控制算法可實現(xiàn)對非失效機器人進行系統(tǒng)降級重組，其目的是實現(xiàn)正常運行的機器人仍保持均勻分布進行工作，同時也要保證控制過程中正常機器人與監(jiān)測執(zhí)行功能失效機器人不會發(fā)生碰撞.

如果失效機器人成為新的避障問題，本文從識別到避障會形成合理的閉路循環(huán)，新的避障會循環(huán)至初始，形成新的識別和新的避障.可以實現(xiàn)對失效機器人的處理，從而實現(xiàn)多移動機器人的編隊避障.

NlCyP&NB-a控制下的相位調(diào)整，其關(guān)鍵是機器人的速率、控制增益kα與期望狀態(tài)的匹配.若各機器人穩(wěn)定速率相同，則通過調(diào)整局部控制增益kα值來達到目的效果.改變ui，通過分別設(shè)定不同的控制增益值kα1，kα2，kα3，kα4來實現(xiàn)降級重組.

3.2 引入速度、加速度約束

考慮到一些輪式移動機器人只提供速度控制接口，而機器人電機不能跟蹤加速度過大的速度指令，對于不考慮約束的機器人，當控制量超過約束界限時，應用于物理機器人系統(tǒng)的控制輸入會截斷，截斷控制輸入會降低控制器的性能，甚至使被控系統(tǒng)失穩(wěn).因此引入了速度和加速度約束.

其中vc和ac是常數(shù).機器人速度和加速度約束大小約為vc=0.6 m/s，ac=1.12 m/s2.當速度超過速度束時，系統(tǒng)會做出截斷，截斷的響應時間為t=100 s，因此在此期間內(nèi)系統(tǒng)會增大控制增益kα和kb使之迅速達到要求.

當vi＞vc時，則

3.3 路徑規(guī)劃、避障控制

為了避免機器人在編隊形成穩(wěn)定的構(gòu)型后，向前行進過程中與障礙物發(fā)生碰撞的危險，提出了引入懲罰因子的概念以淘汰不安全路徑.本文主要研究編隊中心到目標點的最短路徑規(guī)劃.懲罰因子是一個施加在路徑長度的系數(shù)，見式(15):

式中:δ代表懲罰因子，d0為單個機器人與障礙物之間的安全距離，dp表示選擇的路徑和障礙物之間的最短距離.

假設(shè)最短路徑長度為lk，綜合考慮路徑中的障礙，得到安全最短路徑長度Lk:

由式(15)-(16)可知，當0≤dp＜ρ0+d0時，說明路徑和障礙物之間的距離過近，機器人選擇的路徑為不安全路徑，懲罰因子δ變大，得到的安全最短路徑長度Lk也相應變大;當dp≥ρ0+d0時，說明路徑和障礙物之間為安全距離，機器人選擇的路徑為安全路徑，懲罰因子δ值為0，安全最短路徑長度Lk與lk等長.

證定義編隊中心初始位置為(x1，y1)，運動目標位置為(x2，y2)，如圖4-5所示，其中M1-M4分別代表不同的機器人.

圖4 機器人編隊安全邊界Fig.4 Safety boundary of robot formation

圖5 機器人編隊中心運動軌跡Fig.5 Trajectory of robot formation center

x2＞x1，障礙物的位置坐標為(a，b)，且0≤dp＜ρ0+d0，所以最短路徑為

最短路徑長度lk為

由于編隊為圓形，所以考慮每個機器人與障礙物的安全距離時，可直接轉(zhuǎn)化為編隊中心與障礙物的安全距離為(ρ0+d0)，因此在障礙物周邊的最短安全路徑為

同時聯(lián)立式(17)(19)即可求出交點坐標分別為A(m1，n1)，B(m2，n2)，其中m2＞m1.

所以整個機器人編隊到達目標點的安全的最短路徑為

所以，此時安全的最短安全路徑長度Lk為

此時可得懲罰因子為

證畢.

4 仿真結(jié)果

此時，假設(shè)由三機器人組成的MAS編隊(M1，M2，M3)，初始坐標分別為M1[0.496，-99.999]，M2[86.849，-49.57]，M3[-86.354，-50.429].當改進ui中的kα都為統(tǒng)一值kα=時，對系統(tǒng)實行NlCyP控制，穩(wěn)定時編隊中心與初始幾何中心(原點)不重合，如圖6所示.此時若設(shè)kb=0.01，cb=2，其余參數(shù)保持不變，對系統(tǒng)實行航標在原點的NlCyP&NB-a控制時，編隊中心在原點附近穩(wěn)定，運行結(jié)果如圖7所示.以下所有圖中彩色圖形代表了不同機器人的初始位置，黑色圖形代表了不同機器人的最終位置.

圖6 NlCyP控制編隊運行軌跡Fig.6 NlCyP control formation trajectory

圖7 NlCyP&NB-a控制編隊運行軌跡Fig.7 NlCyP&NB-a control formation trajectory

相比于以往控制方法，NlCyP&NB-a控制可以實現(xiàn)“航標”為原點進行{n/1}追蹤運動，編隊中心可變，整個編隊在收斂到穩(wěn)定構(gòu)形的同時，其編隊中心也將向“航標”收斂，當速率均穩(wěn)定在vR，則該系統(tǒng)將進化為以原點為中心的廣義正多邊形.

與文獻[23]的經(jīng)典算法進行比較(取穩(wěn)定值浮動1%時的計算步step作為對比)見表1.

表1 算法對比Table 1 Algorithm comparison

由此可見，本文所采用的算法在效率、穩(wěn)定性、收斂速度具有較好的效果.

4.1 降級重組

編隊成員為4 個機器人(M1，M2，M3，M4)，假定M3機器人失效，即期望正常機器人均布，失效機器人M3位于M2與M4中間.其他的初始參數(shù)設(shè)置見表2，ψ為4個機器人的初始相位角，U為控制輸入電壓，單個步長step為100 s.

表2 初始參數(shù)值Table 2 Value of initial parameters

于是，得到的仿真結(jié)果如圖8-9所示.圖8中彩色圖形代表了機器人的初始位置，黑色圖形代表了最終位置.可以看出，編隊的半徑為1 m，失效機器人M3位于M2，M4之間，所有的機器人達到了穩(wěn)定的狀態(tài)，正常機器人實現(xiàn)了均勻分布狀態(tài)，編隊中心坐標穩(wěn)定在原點.仿真達到400步時，機器人相對參考中心距離趨向穩(wěn)定.

圖8 四機器人NlCyP&NB-a控制軌跡Fig.8 NlCyP&NB-a control trajectory of four robots

圖9 四機器人相對航標距離Fig.9 Distance of four robots relative to navigation’s beacon

正常機器人相對距離與正常機器人相對監(jiān)測執(zhí)行功能失效機器人距離分別如圖10-11所示.可以看出，正常機器人之間相對距離約為1.75 m，失效機器人M3與正常機器人M2，M4之間距離約為1 m，與正常機器人M1的距離約為2 m.

圖10 正常機器人(M1，M2，M4)相對距離Fig.10 Relative distance of normal robot(M1，M2，M4)

圖11 正常機器人(M1，M2，M4)與失效機器人(M3)距離Fig.11 Distance between normal robot(M1，M2，M4)and failure robot(M3)

因此，在個別成員出現(xiàn)監(jiān)測執(zhí)行功能失效的情況下，只要參數(shù)匹配，編隊仍能達到循環(huán)追蹤的理想構(gòu)形，為多機器人的編隊增強了魯棒性，防干擾和編隊繼續(xù)執(zhí)行任務的恢復能力.

4.2 引入速度、加速度約束

從圖12-13中可以看出，機器人的速度和加速度最終都趨向穩(wěn)定，速度約為0.313 m/s，最大約為0.323 m/s，加速度最大約為8.2×10-4m/s2，最終加速度趨向于0.

圖12 速度圖像Fig.12 Velocity diagram

圖13 加速度圖像Fig.13 Acceleration diagram

當速度超過0.6 m/s的速度約束時(見圖14)，控制增益變大，此時，

機器人在100 s內(nèi)迅速降低到約束值以下，而不被截斷，繼續(xù)執(zhí)行任務，見圖15.

圖14 機器人速度超過約束Fig.14 Robot speed exceeds constraint

圖15 增大kα，kb后速度Fig.15 Speed after increasing kα，kb

由此可見，在一定范圍內(nèi)調(diào)整kα，kb以滿足速度變化的要求，機器人編隊調(diào)整應使相位、速率與控制增益kα，kb匹配.控制增益參數(shù)kα與kb二者要平衡，以保證匹配的預期.當較多成員分擔的NB控制時，kb取值也相應減小.控制增益具體參數(shù)的選取，以參考文獻[18，24，36]得到大致范圍為初始參數(shù)，通過多次反復調(diào)試，最終得到識別和收斂效率最好的優(yōu)化參數(shù)組合.

4.3 路徑規(guī)劃、避障控制

具體參數(shù)設(shè)置見表3，其余參數(shù)與前邊得到的數(shù)據(jù)相同.

表3 初始參數(shù)值Table 3 Value of initial parameters

表3中λ為編隊完成后4個機器人的相位角.

圖16-18分別表示障礙物位置對降級重組后4個機器人的影響.圖16為障礙物遠離機器人編隊，圖17表示障礙物處于起止點連線下方，圖18表示障礙物處于起止點連線上方，因此圖16的障礙物對機器人編隊基本沒有影響，圖17-18只是有影響，只是因為障礙物位置不同，運動軌跡不一樣.

圖16 無障礙運動軌跡Fig.16 Obstacle-free trajectories of motion

圖17 有障礙運動軌跡Fig.17 Motion trajectories with obstacles

圖18 有障礙運動軌跡Fig.18 Motion trajectories with obstacles

此懲罰因子引入的優(yōu)點是:無論障礙物多少或處于什么位置，編隊規(guī)劃的路徑始終為最短路徑，這樣大大節(jié)省了機器人編隊運行時間.

5 結(jié)論

為了實現(xiàn)多機器人復雜情況的編隊避障控制，本文研究了具有速度、加速度約束的改進的控制算法，并且引入懲罰因子的不同控制增益的NlCyP&NB-a控制方案.該方法關(guān)鍵特征是全局穩(wěn)定性和魯棒性，提高了抗干擾及規(guī)避障礙的能力，實現(xiàn)了只有局部控制信息的多機器人編隊.

1) 不同的局部控制增益，可實現(xiàn)非失效機器人的降級重組，避免機器人之間發(fā)生碰撞;

2) 速度和加速度約束的引入，更符合實際情況參數(shù)更簡單，且實現(xiàn)了控制算法的穩(wěn)定與收斂;

3) 通過引入懲罰因子，使多機器人成功規(guī)避障礙，保持編隊的同時搜尋出到達目標位置的最短路徑，節(jié)省了運行時間.