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

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

1 引言

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

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

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

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

2 多機(jī)器人的編隊(duì)模型

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

因此，動(dòng)力學(xué)模型為

本文采用的多機(jī)器人編隊(duì)方式如圖1-2所示.

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

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

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

因此，多移動(dòng)機(jī)器人編隊(duì)的運(yùn)動(dòng)方程為

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

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

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

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

圖3 機(jī)器人相對(duì)航標(biāo)示意圖Fig.3 Diagram of robot relative navigation’s beacon

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

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

3.1 降級(jí)重組

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

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

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

3.2 引入速度、加速度約束

考慮到一些輪式移動(dòng)機(jī)器人只提供速度控制接口，而機(jī)器人電機(jī)不能跟蹤加速度過大的速度指令，對(duì)于不考慮約束的機(jī)器人，當(dāng)控制量超過約束界限時(shí)，應(yīng)用于物理機(jī)器人系統(tǒng)的控制輸入會(huì)截?cái)?，截?cái)嗫刂戚斎霑?huì)降低控制器的性能，甚至使被控系統(tǒng)失穩(wěn).因此引入了速度和加速度約束.

其中vc和ac是常數(shù).機(jī)器人速度和加速度約束大小約為vc=0.6 m/s，ac=1.12 m/s2.當(dāng)速度超過速度束時(shí)，系統(tǒng)會(huì)做出截?cái)啵財(cái)嗟捻憫?yīng)時(shí)間為t=100 s，因此在此期間內(nèi)系統(tǒng)會(huì)增大控制增益kα和kb使之迅速達(dá)到要求.

當(dāng)vi＞vc時(shí)，則

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

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

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

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

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

證定義編隊(duì)中心初始位置為(x1，y1)，運(yùn)動(dòng)目標(biāo)位置為(x2，y2)，如圖4-5所示，其中M1-M4分別代表不同的機(jī)器人.

圖4 機(jī)器人編隊(duì)安全邊界Fig.4 Safety boundary of robot formation

圖5 機(jī)器人編隊(duì)中心運(yùn)動(dòng)軌跡Fig.5 Trajectory of robot formation center

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

最短路徑長度lk為

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

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

所以整個(gè)機(jī)器人編隊(duì)到達(dá)目標(biāo)點(diǎn)的安全的最短路徑為

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

此時(shí)可得懲罰因子為

證畢.

4 仿真結(jié)果

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

圖6 NlCyP控制編隊(duì)運(yùn)行軌跡Fig.6 NlCyP control formation trajectory

圖7 NlCyP&NB-a控制編隊(duì)運(yùn)行軌跡Fig.7 NlCyP&NB-a control formation trajectory

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

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

表1 算法對(duì)比Table 1 Algorithm comparison

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

4.1 降級(jí)重組

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

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

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

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

圖9 四機(jī)器人相對(duì)航標(biāo)距離Fig.9 Distance of four robots relative to navigation’s beacon

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

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

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

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

4.2 引入速度、加速度約束

從圖12-13中可以看出，機(jī)器人的速度和加速度最終都趨向穩(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

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

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

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

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

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

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

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

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

表3中λ為編隊(duì)完成后4個(gè)機(jī)器人的相位角.

圖16-18分別表示障礙物位置對(duì)降級(jí)重組后4個(gè)機(jī)器人的影響.圖16為障礙物遠(yuǎn)離機(jī)器人編隊(duì)，圖17表示障礙物處于起止點(diǎn)連線下方，圖18表示障礙物處于起止點(diǎn)連線上方，因此圖16的障礙物對(duì)機(jī)器人編隊(duì)基本沒有影響，圖17-18只是有影響，只是因?yàn)檎系K物位置不同，運(yùn)動(dòng)軌跡不一樣.

圖16 無障礙運(yùn)動(dòng)軌跡Fig.16 Obstacle-free trajectories of motion

圖17 有障礙運(yùn)動(dòng)軌跡Fig.17 Motion trajectories with obstacles

圖18 有障礙運(yùn)動(dòng)軌跡Fig.18 Motion trajectories with obstacles

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

5 結(jié)論

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

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

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

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