基于線性矩陣不等式的巡檢機(jī)器人路徑規(guī)劃

戴昊，崔志文，袁鵬，歐旋

(深圳供電局有限公司，廣東 深圳 518000)

0 引言

巡檢機(jī)器人是一種能夠完成輸電線路巡檢、應(yīng)急監(jiān)測(cè)、環(huán)境監(jiān)測(cè)和反恐處突等任務(wù)的機(jī)器人。圖1是某公司的巡檢機(jī)器人E100。E100面向電力、市政、通信和石化等行業(yè)，為智能運(yùn)維系統(tǒng)提供環(huán)境和工業(yè)設(shè)備狀態(tài)的監(jiān)測(cè)數(shù)據(jù)。

圖1 E100巡檢機(jī)器人

巡檢機(jī)器人在任務(wù)空間運(yùn)動(dòng)時(shí)，首先要解決的就是路徑獲取以及路徑規(guī)劃問(wèn)題。路徑規(guī)劃依賴于機(jī)器人和目標(biāo)點(diǎn)的位置信息，通過(guò)有效的算法，求得一條連接出發(fā)點(diǎn)和目標(biāo)點(diǎn)的最優(yōu)軌跡；同時(shí)需要滿足諸多約束，如障礙物規(guī)避、路程和時(shí)間要求等。針對(duì)機(jī)器人的軌跡規(guī)劃，國(guó)內(nèi)外學(xué)者提出了大量方法[1-5]。主要的路徑規(guī)劃有四類，第一類是基于地圖構(gòu)建的路徑規(guī)劃方法，KUWATA Y等[6]將可視圖法從二維擴(kuò)展到三維，并應(yīng)用到無(wú)人機(jī)的航跡規(guī)劃中，但該方法沒(méi)有充分考慮路徑中的約束；肖秦琨等[7]對(duì)軌跡規(guī)劃中的Voronoi圖法進(jìn)行了改進(jìn)，提高了機(jī)器人運(yùn)行的安全度，但不是最優(yōu)解；HRABAR S E[8]使用概率圖法，在搜索空間內(nèi)按照概率分布產(chǎn)生隨機(jī)路徑點(diǎn)，連接路徑點(diǎn)構(gòu)成完整的路徑，并在三維空間內(nèi)的軌跡規(guī)劃進(jìn)行了仿真研究，但該路徑不一定是最優(yōu)的。第二類是在基于虛擬勢(shì)場(chǎng)法軌跡規(guī)劃方面，侯翔[9]在人工勢(shì)場(chǎng)的基礎(chǔ)上，通過(guò)引入指數(shù)因子構(gòu)造斥力函數(shù)來(lái)平衡障礙物斥力，解決了障礙物附件目標(biāo)不可達(dá)問(wèn)題，但容易陷入局部最小點(diǎn)；貝前程等[10]對(duì)傳統(tǒng)人工勢(shì)場(chǎng)進(jìn)行了改進(jìn)，提出改進(jìn)的斥力函數(shù)方法，確保機(jī)器人能夠順利到達(dá)目標(biāo)點(diǎn)；LIU C A等[11]將流體力學(xué)理論應(yīng)用到機(jī)器人路徑規(guī)劃中，建立流場(chǎng)，可以獲得光滑的路徑，但對(duì)規(guī)劃中的約束考慮不足。第三類是一些學(xué)者將數(shù)學(xué)最優(yōu)化的方法應(yīng)用到軌跡規(guī)劃中，DOGAN A[12]在障礙物威脅概率密度函數(shù)的基礎(chǔ)上，提出了使用迭代步方法進(jìn)行路徑規(guī)劃，并推廣到三維規(guī)劃中，規(guī)劃得到的路徑安全性較高，但未必是最優(yōu)解；張劍等[13]用支持向量機(jī)對(duì)電力巡線無(wú)人機(jī)工作區(qū)域進(jìn)行非線性分割，并從分割平面中選擇適當(dāng)?shù)穆窂?，避障性能較好。第四類是基于生物智能的方法，這種方法具有較強(qiáng)的搜索能力，適合搜索最優(yōu)軌跡，袁佳泉等[14]在蟻群算法的基礎(chǔ)上，結(jié)合模擬退火算法來(lái)進(jìn)行機(jī)器人的軌跡規(guī)劃，提高了計(jì)算速度；張丹紅等[15]將A*算法與蟻群算法相結(jié)合，應(yīng)用到巡邏艇的路徑規(guī)劃中，求解全局最短巡邏路徑。這些方法在路徑光滑性、環(huán)境適應(yīng)性和實(shí)時(shí)性上都各有優(yōu)勢(shì)與局限。

本文針對(duì)巡檢機(jī)器人的任務(wù)特點(diǎn)，在主要的軌跡規(guī)劃方法的基礎(chǔ)上，提出一種結(jié)合單元分解和線性代數(shù)的航跡規(guī)劃方法。首先，描述基于Delaunay剖分法的任務(wù)環(huán)境三角劃分；然后，在此基礎(chǔ)上，對(duì)軌跡規(guī)劃進(jìn)行數(shù)學(xué)描述，定義變量以及約束條件，將規(guī)劃問(wèn)題轉(zhuǎn)化為基于線性矩陣不等式的最優(yōu)化問(wèn)題；最后，結(jié)合實(shí)際的任務(wù)環(huán)境，對(duì)環(huán)境進(jìn)行簡(jiǎn)化，并利用簡(jiǎn)化后的模型進(jìn)行仿真計(jì)算以驗(yàn)證本文方法的正確性。

1 任務(wù)環(huán)境三角劃分

定義二維三角剖分：二維實(shí)數(shù)域上的點(diǎn)集V，以此點(diǎn)集中的點(diǎn)作為端點(diǎn)，構(gòu)成封閉線段e，E為線段e的集合。該點(diǎn)集V的一個(gè)三角剖分T=(V,E)構(gòu)成一個(gè)平面圖G。該平面滿足條件：

1)平面圖中的邊不包含點(diǎn)集中除了端點(diǎn)外的任何點(diǎn)；

2)沒(méi)有相交的邊；

3)平面圖中所有的面都是三角面，且所有三角面的集合是散點(diǎn)集V的凸包。

三角剖分在圖形學(xué)和建模中被廣泛使用，用來(lái)模擬復(fù)雜物體的表面。在三角剖分的基礎(chǔ)上，滿足空?qǐng)A特性和最大化最小角特性的剖分稱為Delaunay剖分。即Delaunay三角形網(wǎng)中，任一三角形的外接圓范圍不會(huì)存在其他點(diǎn)且三角形的最小角最大。Delaunay三角剖分與Voronoi劃分互為對(duì)偶圖，如圖2所示。

圖2 點(diǎn)集的Delaunay三角剖分(實(shí)線)和Voronoi圖(虛線)

進(jìn)行Delaunay剖分前，需要確定任務(wù)空間中障礙物的邊以及頂點(diǎn)和內(nèi)角點(diǎn)的坐標(biāo)值。三角剖分后，對(duì)障礙物內(nèi)部的三角形區(qū)域進(jìn)行標(biāo)記并且從三角組網(wǎng)中剔除。剩下的三角形稱為自由三角形，組成自由空間。二維下的任務(wù)空間Delaunay三角剖分如圖3所示。

圖3 任務(wù)空間的Delaunay剖分

Delaunay三角剖分具有唯一性、最優(yōu)性和規(guī)則性等優(yōu)點(diǎn)，所以在對(duì)E100的任務(wù)空間進(jìn)行建模中使用該方法，使得算法更加可靠。

2 加權(quán)函數(shù)和優(yōu)化準(zhǔn)則

在線性矩陣不等式約束的優(yōu)化問(wèn)題中，目標(biāo)函數(shù)決定了哪一條路徑會(huì)被選中，所以，選擇可靠的目標(biāo)函數(shù)至關(guān)重要。目標(biāo)函數(shù)由兩個(gè)矩陣組成，權(quán)重矩陣和變量矩陣(將在第3部分中討論)。定義如下優(yōu)化準(zhǔn)則：

1)三角形的數(shù)量(WN)：在不考慮尺寸的情況下，從起點(diǎn)到終點(diǎn)的路徑盡可能地選擇三角形數(shù)量較少的一條；

2)三角形的面積(WA)：算法搜索的路徑盡可能地保證路徑中三角形組成的面積要?。?/p>

3)三角形的中線長(zhǎng)(WM)：選取三角形的中線作為三角形中機(jī)器人的要走的路徑，并盡可能地保證中線長(zhǎng)的和要短；

4)機(jī)器人尺寸約束(WR)：機(jī)器人穿過(guò)障礙物時(shí)，機(jī)器人的尺寸大于障礙物之間的距離時(shí)，障礙物之間的路徑不能被選擇。

將上述的準(zhǔn)則進(jìn)行加權(quán)組合，構(gòu)成加權(quán)目標(biāo)函數(shù)

W=α1×WN+α2×WA+α3×WM+α4×WR

(1)

用加權(quán)目標(biāo)函數(shù)對(duì)路徑進(jìn)行評(píng)價(jià)，以規(guī)劃出最優(yōu)路徑。

準(zhǔn)則3)中機(jī)器人通過(guò)三角形時(shí)，選擇三角形的連接兩條自由邊的中線作為路徑。自由邊位于Cfree自由區(qū)域中，不屬于任何障礙物的邊緣。這些中線代表各個(gè)三角形，中線長(zhǎng)度的和作為目標(biāo)方程中的一個(gè)權(quán)重。其中，對(duì)于起始點(diǎn)S和終點(diǎn)G，其連接相鄰三角形邊的中點(diǎn)所構(gòu)成的路線SE和FG來(lái)取代中線，構(gòu)成起始路徑和終止路徑。如圖4所示。

圖4 通道中的路徑

3 建立優(yōu)化模型

定義具有如下形式的不等式為線性矩陣不等式

(2)

線性矩陣不等式的約束條件定義了自變量空間的一個(gè)凸集，即{X|F(X)<0}，所以，這是一個(gè)凸約束。線性矩陣不等式的這一性質(zhì)使得其是求解線性凸二次規(guī)劃問(wèn)題的有效方法。很多系統(tǒng)的最優(yōu)化問(wèn)題都可以轉(zhuǎn)換為基于線性矩陣不等式的凸或準(zhǔn)凸優(yōu)化，并且能夠解決很多沒(méi)有解析解的問(wèn)題。線性矩陣不等式約束下的最優(yōu)問(wèn)題為：

minx∈Sf(x),f:S→R
s.t.F(x)<0

(3)

使用MATLAB內(nèi)自帶的線性矩陣不等式(LMIs)求解器進(jìn)行仿真運(yùn)算。

使用二進(jìn)制整數(shù)規(guī)劃法來(lái)描述Cfree中的三角形，即代表路徑規(guī)劃問(wèn)題的目標(biāo)函數(shù)的變量。所以，變量的二進(jìn)制整數(shù)規(guī)劃(BIP)定義如下：

(4)

為了保證從起點(diǎn)到目標(biāo)點(diǎn)的路徑連續(xù)，構(gòu)建軌跡通道的三角形必須滿足如下條件：

3.2.2 醫(yī)患關(guān)系緊張 在訪談中，大部分醫(yī)護(hù)工作者都提到了影響自身職業(yè)認(rèn)同水平的主要因素是緊張的醫(yī)患關(guān)系?！拔覀?cè)卺t(yī)院工作的人壓力都非常大，這種壓力并不是說(shuō)醫(yī)學(xué)素養(yǎng)不夠、診治技能差，我們對(duì)自己非常有信心，壓力主要來(lái)自和病人之間的關(guān)系。”當(dāng)前醫(yī)患關(guān)系緊張，甚至發(fā)生了多起襲醫(yī)事件，這導(dǎo)致部分醫(yī)護(hù)工作者對(duì)自身職業(yè)認(rèn)同程度降低?！坝幸恍┎』颊`解了醫(yī)護(hù)工作這個(gè)職業(yè)，覺(jué)得我們什么病癥都可以治好，這是不現(xiàn)實(shí)的想法”“我覺(jué)得在醫(yī)院工作，不論醫(yī)生還是護(hù)士，都是非常高尚的職業(yè)，我們也盡職盡責(zé)地幫助病患解除痛苦，但病患來(lái)醫(yī)院本來(lái)就帶著消極情緒，有時(shí)候不太理解我們的工作性質(zhì)?！?/p>

1)起始點(diǎn)和終點(diǎn)的三角形必須被選中，分別為xS和xG；

2)如果一個(gè)三角形被選中(除了xS和xG)，則它相鄰的兩個(gè)三角形也必須被選中(連續(xù)性條件)；

3)起始和目標(biāo)三角形只能有一個(gè)被選中的相鄰三角形(避免環(huán)回條件)；

4)為了避免環(huán)回，有三個(gè)自由邊的兩個(gè)相鄰三角形一定都被選中。

基于以上條件，求解最優(yōu)軌跡的BIP模型描述為：

MinimizeJ=WT·X

s.t.

xS=1,xG=1,xi∈{0,1},i?{S,G}。

(5)

根據(jù)上述約束，所以軌跡規(guī)劃問(wèn)題轉(zhuǎn)換為擁有k+p個(gè)約束的最優(yōu)規(guī)劃問(wèn)題(k+p-2個(gè)不等式和2個(gè)等式)，最優(yōu)規(guī)劃問(wèn)題的優(yōu)化準(zhǔn)則由方程J定義。

上述的最小化問(wèn)題模型可以轉(zhuǎn)化為基于線性矩陣不等式約束的優(yōu)化問(wèn)題。目標(biāo)方程為：

MinimizeJ=WT·X

(6)

式中Tk×1是變量[xi]的列向量；Wk×1是加權(quán)列向量。

對(duì)于約束方程，式(5)中的不等式和等式重新構(gòu)造成擁有如下3個(gè)線性矩陣不等式的系統(tǒng)：

H=diag(AX-CX)≥0
U=diag(A′X-C′X)=0
V=diag(A″X-C″)≤0

(7)

上述的線性矩陣不等式經(jīng)過(guò)解算后數(shù)值為1的變量代表最優(yōu)路徑中的三角形，最優(yōu)路徑從屬于Cfree空間中。直接使用二進(jìn)制整數(shù)規(guī)劃進(jìn)行求解得到的答案與使用線性矩陣不等式所得到的答案一樣，但使用線性矩陣不等式求解更簡(jiǎn)單，速度也更快。

4 仿真實(shí)驗(yàn)

構(gòu)建巡檢機(jī)器人的工作環(huán)境，進(jìn)行仿真，仿真結(jié)果如圖5所示。

圖5 最優(yōu)路徑

由圖5可以看出最優(yōu)通道中三角形的中線構(gòu)成機(jī)器人運(yùn)動(dòng)軌跡。圖中橢圓形標(biāo)注出的通道雖然可以獲得更加短的路徑，但由于機(jī)器人尺寸限制，該條通道不可選。

這里對(duì)規(guī)劃出的最優(yōu)路徑進(jìn)行一個(gè)優(yōu)化。利用通道中的三角形構(gòu)造凸多邊形。從起始三角形開(kāi)始，增加相鄰三角形，直到組成的多邊形變凹。下一個(gè)凸多邊形從這個(gè)斷開(kāi)的三角形開(kāi)始，依次進(jìn)行三角形的組合，將通道中的凸多邊形的邊的中點(diǎn)的連線作為運(yùn)動(dòng)軌跡，如圖6所示。

圖6 優(yōu)化后的最優(yōu)路徑

圖6中圈出的區(qū)域?yàn)槎鄠€(gè)三角形構(gòu)成的凸多邊形，調(diào)整后的軌跡比原先中線構(gòu)成的軌跡更短，而且凸多邊形也保證了兩條邊中點(diǎn)的連線在最優(yōu)通道中，不會(huì)穿過(guò)障礙物。

最后對(duì)軌跡進(jìn)行光滑處理，以使得機(jī)器人在運(yùn)行中的轉(zhuǎn)向較為平緩，得到優(yōu)化后的路徑曲線如圖7所示。

圖7 光滑處理后的最優(yōu)路徑

在仿真實(shí)驗(yàn)的基礎(chǔ)上，為了驗(yàn)證本文方法高效性，構(gòu)造多個(gè)工作環(huán)境，將本文方法的路徑規(guī)劃耗時(shí)和路徑長(zhǎng)度與其他相近方法進(jìn)行對(duì)比。這里選取與Delaunay三角剖分互為偶圖的Voronoi劃分，對(duì)任務(wù)空間進(jìn)行劃分，并使用高效的Dijkstra算法對(duì)劃分后的任務(wù)空間進(jìn)行最優(yōu)路徑的搜索。任務(wù)環(huán)境之間以環(huán)境中障礙物的數(shù)量n為變量，從5～30遞增，每次規(guī)劃都進(jìn)行多次計(jì)算仿真，取平均值。兩種方法對(duì)比仿真計(jì)算的結(jié)果如表1所示。

表1 規(guī)劃耗時(shí)和路徑長(zhǎng)度

從表1中可以看出，本文的算法在路徑長(zhǎng)度較短的情況下，規(guī)劃耗時(shí)也較短，尤其是障礙物數(shù)量較多時(shí)，規(guī)劃上的效率優(yōu)勢(shì)更為突出。

5 結(jié)語(yǔ)

本文提出了一種基于線性矩陣不等式的巡檢機(jī)器人路徑規(guī)劃方法，并以E100巡檢機(jī)器人為例，構(gòu)建任務(wù)環(huán)境，進(jìn)行仿真，驗(yàn)證了算法的正確性，同時(shí)與相近算法進(jìn)行對(duì)比，驗(yàn)證了高效性。本文的算法簡(jiǎn)單、完備，考慮了路徑中的約束，且機(jī)器人不會(huì)陷入局部最小點(diǎn)。