考慮機(jī)器人尺度的變電站路徑規(guī)劃

江 南，吳振輝，吳凌健杰

(1.國(guó)網(wǎng)福州供電公司，福建 福州 350000；2.福建杭潤(rùn)科技有限公司，福建 福州 350000)

1 引言

隨著科技的發(fā)展和進(jìn)步以及電力系統(tǒng)自動(dòng)化水平的提高，變電站的數(shù)量急劇增多，對(duì)變電站進(jìn)行巡檢的工作量也越來(lái)越大。傳統(tǒng)的巡檢方式還是以人工為主，不僅效率低下，且由于受到巡檢員的素質(zhì)、狀態(tài)、心理等自身因素以及電磁輻射、風(fēng)雪天氣等外部環(huán)境因素的影響，巡檢任務(wù)中容易發(fā)生事故或?qū)撛诘娘L(fēng)險(xiǎn)無(wú)法及時(shí)發(fā)現(xiàn)。因此，變電站智能輔助巡檢機(jī)器人的意義就顯得十分重大，而智能機(jī)器人的路徑規(guī)劃是變電站巡檢機(jī)器人導(dǎo)航的重要組成部分。

路徑規(guī)劃是個(gè)交叉領(lǐng)域，研究者眾多，與自動(dòng)化、工業(yè)生產(chǎn)、交通運(yùn)輸行業(yè)均密切相關(guān)，應(yīng)用在機(jī)器人、無(wú)人機(jī)、USV等無(wú)人控制器中。根據(jù)環(huán)境建模方式的不同可以分為基于決策論、物理、數(shù)學(xué)的三種建模方法。如模糊控制、狀態(tài)聚類等方法的屬于基于決策論的建模方式，通過(guò)在環(huán)境中采取不同的決策來(lái)進(jìn)行路徑規(guī)劃，通過(guò)與環(huán)境的不斷交互來(lái)訓(xùn)練被控制目標(biāo)，主要用的求解方法是神經(jīng)網(wǎng)絡(luò)和強(qiáng)化學(xué)習(xí)；如線性規(guī)劃、最小樹(shù)規(guī)劃、動(dòng)態(tài)規(guī)劃等方法屬于數(shù)學(xué)類的建模方式，通常應(yīng)用在VRP等路線規(guī)劃當(dāng)中，通過(guò)單純形法、分支定界法等傳統(tǒng)算法或者如禁忌搜索、PSO算法等智能算法進(jìn)行求解；最后一類是物理的建模方式，主要用到的方法有人工勢(shì)場(chǎng)法[1]、路線圖法、柵格法[2]、決策樹(shù)法等等，這一類方法都是考慮實(shí)際的物理空間而不是抽象的運(yùn)籌建模，通常使用啟發(fā)式算法來(lái)進(jìn)行求解，例如蟻群算法[3]、粒子群算法、模擬退火算法[4]或者多種智能算法融合后的混合啟發(fā)式算法[6]。

鏈接圖法[7](Maklink)是路線圖的一種建模方式，通過(guò)將復(fù)雜的障礙物空間進(jìn)行簡(jiǎn)化，成為簡(jiǎn)單的多邊形體，再通過(guò)一系列的步驟以及規(guī)則連接多邊形的頂點(diǎn)或做頂點(diǎn)到邊界的垂線，將自由空間進(jìn)行劃分。該方法通常將被規(guī)劃目標(biāo)視為質(zhì)點(diǎn)，并把障礙物區(qū)域進(jìn)行一定擴(kuò)展，從而避免規(guī)劃目標(biāo)與障礙物發(fā)生碰撞。但是，在變電站的巡檢區(qū)域中，會(huì)有許多狹窄的通道，對(duì)障礙物區(qū)域進(jìn)行擴(kuò)展會(huì)使得這些通道消失，從而導(dǎo)致巡檢機(jī)器人無(wú)法到達(dá)檢測(cè)區(qū)域。

人工勢(shì)場(chǎng)法[8]在路徑規(guī)劃中應(yīng)用的也非常廣泛，這是因?yàn)槠渚哂泻?jiǎn)潔的數(shù)學(xué)模型以及較為平滑的規(guī)劃路線。但是，人工勢(shì)場(chǎng)法也有其自身的缺陷[9]，盡管計(jì)算過(guò)程中可以輸入全局的環(huán)境信息，但只有被規(guī)劃目標(biāo)周圍的力的作用才會(huì)對(duì)其移動(dòng)造成影響，這就導(dǎo)致人工勢(shì)場(chǎng)法無(wú)法使用的全局環(huán)境信息，容易陷入極值點(diǎn)。例如：當(dāng)終點(diǎn)處在障礙物區(qū)域之中時(shí)，斥力的作用可能導(dǎo)致機(jī)器人無(wú)法到達(dá)終點(diǎn)[10]；在狹窄的區(qū)域中斥力和引力的影響，可能導(dǎo)致規(guī)劃路線來(lái)回抖動(dòng)，無(wú)法前進(jìn)。

本文將鏈接圖法與人工勢(shì)場(chǎng)法進(jìn)行結(jié)合，首先通過(guò)dijkstra算法在鏈接線上的節(jié)點(diǎn)上規(guī)劃出一條初始路線，并將路線上的節(jié)點(diǎn)作為人工勢(shì)場(chǎng)的引力牽引點(diǎn)，障礙物邊界進(jìn)行離散化處理，將其上的點(diǎn)作為斥力點(diǎn)；然后，通過(guò)人工勢(shì)場(chǎng)法規(guī)劃出一條平滑的路線；最后，利用遺傳算法對(duì)節(jié)點(diǎn)的選擇進(jìn)行優(yōu)化，不斷的迭代尋優(yōu)，最終可以在有凹多邊形的仿真環(huán)境中得到一條安全性高且較為平滑的可通行路徑。該方法可以解決人工勢(shì)場(chǎng)法容易陷入極值點(diǎn)的問(wèn)題。

2 環(huán)境模型

2.1 改進(jìn)的Maklink圖路徑規(guī)劃環(huán)境建模方法

Maklink圖是一種簡(jiǎn)潔且有效的路徑規(guī)劃環(huán)境建模方法，它最早由Habib于1991年提出，并被廣泛應(yīng)用在機(jī)器人路徑規(guī)劃等領(lǐng)域當(dāng)中。在Maklink圖的傳統(tǒng)應(yīng)用當(dāng)中，為了避免障礙物與被規(guī)劃目標(biāo)發(fā)生碰撞，通常是將障礙物的邊界進(jìn)行一定的緩沖擴(kuò)大，并且將被規(guī)劃目標(biāo)視作一個(gè)質(zhì)點(diǎn)，不考慮其碰撞體積。該方法在障礙物相對(duì)總體空間較小的情況下是可行的，但當(dāng)總體空間較小，對(duì)障礙物的擴(kuò)張會(huì)導(dǎo)致原本可以通行的區(qū)域被覆蓋，尤其像是在變電站這樣過(guò)道狹窄的場(chǎng)景下。并且，如果障礙物的頂點(diǎn)較為尖銳，在該點(diǎn)處的擴(kuò)張會(huì)占據(jù)更多的可通行空間，導(dǎo)致路線變長(zhǎng)。同時(shí)，在尖銳的頂點(diǎn)處的規(guī)劃出來(lái)的路線會(huì)有急劇的轉(zhuǎn)折，如果通過(guò)圓的切線對(duì)路線進(jìn)行平滑處理，可能會(huì)導(dǎo)致被規(guī)劃目標(biāo)與障礙物發(fā)生碰撞，從而產(chǎn)生危險(xiǎn)，如圖1(b)所示。如果要避免這種情況發(fā)生，障礙物的擴(kuò)張至少是半倍的被規(guī)劃目標(biāo)長(zhǎng)度，這樣勢(shì)必會(huì)占據(jù)更多的原本可通行的空間。

Maklink圖原理是按一定規(guī)則將障礙物區(qū)域的頂點(diǎn)進(jìn)行連接，從而構(gòu)成全局連通圖，但原初的算法無(wú)法解決凹多邊形的問(wèn)題，存在凹多邊形構(gòu)建的全局連通圖會(huì)導(dǎo)致鏈接線穿過(guò)障礙物區(qū)域，從而導(dǎo)致無(wú)法規(guī)劃出安全的避開(kāi)障礙物的可通行路線。因此，本文在考慮機(jī)器人尺度的前提下，對(duì)Maklink圖進(jìn)行了改進(jìn)，從而可以解決存在凹多邊形的情況。

首先，在考慮了巡檢機(jī)器人尺度之后，Maklink鏈接線所連接的通行區(qū)域可能無(wú)法滿足機(jī)器人的通行條件，所以應(yīng)當(dāng)對(duì)鏈接線的繪制進(jìn)行進(jìn)一步的限制。在Maklink法的中，連接某一頂點(diǎn)與其它某一障礙物的頂點(diǎn)之前，計(jì)算該頂點(diǎn)到該障礙物的最短距離，最短距離產(chǎn)生的位置可能在該頂點(diǎn)與定點(diǎn)的連線(如圖2(a))或者與障礙物某一條邊做的垂線(如圖2(b))，當(dāng)該連線小于巡檢機(jī)器人的寬度時(shí)，表示不滿足通行要求，從而構(gòu)建連通圖時(shí)不在考慮該連接線。

其次，如上所述，原本的Maklink算法無(wú)法處理有凹多邊形的情況，會(huì)導(dǎo)致規(guī)劃出的路線穿過(guò)障礙物區(qū)域。事實(shí)上，在巡檢過(guò)程中，凹多邊形的情況會(huì)大量遇見(jiàn)，尤其是在復(fù)雜的變電站與配電房之中。如果要解決凹多邊形，一種可行的方法是對(duì)凹多邊形進(jìn)行劃分，從而形成多個(gè)凸多邊形后在進(jìn)行全局連通圖的構(gòu)建，具體分解凹多邊形的方法如下：

a)依次選取凹多邊形的頂點(diǎn)，將該頂點(diǎn)相鄰的兩個(gè)凹多邊形頂點(diǎn)進(jìn)行連接，如果這條連線沒(méi)有從凹多邊形中穿過(guò)，則該頂點(diǎn)被記錄為一個(gè)凹頂點(diǎn)，不斷重復(fù)找出所有的凹頂點(diǎn)。

b)隨機(jī)選取一個(gè)凹頂點(diǎn)p，將該頂點(diǎn)與該凹多邊形所有與其不相鄰的頂點(diǎn)進(jìn)行連接，組成集合S。

c)選取集合S中最短的線段，該線段將凹頂點(diǎn)p所在的內(nèi)角切分成兩個(gè)內(nèi)角，判斷兩個(gè)內(nèi)角是否都小于180°，若是則記錄該連線，進(jìn)入步驟e；若有一個(gè)內(nèi)角大于180°，則將該連線放入備選連接線集合A中

d)備選連接線結(jié)合中的線段將凹頂點(diǎn)p所在的內(nèi)角劃分為多個(gè)內(nèi)角，判斷是否每個(gè)內(nèi)角都小于180°。若存在大于180°的內(nèi)角，則返回步驟c，若不存在，則進(jìn)入步驟e。

e)檢查是否將該凹多邊形的頂點(diǎn)都遍歷的一遍，若否，則返回步驟b，若是，則結(jié)束整個(gè)流程。

通過(guò)如上方法，可以將一個(gè)凹多邊形分解為多個(gè)凸多邊形，隨后便可繪制鏈接圖。但由于劃分后的凸多邊形的頂點(diǎn)有所重合，因此需要對(duì)凸多邊形進(jìn)行一定的收縮，使凸多邊形的頂點(diǎn)不再重疊。通過(guò)對(duì)凸多邊形的每條邊進(jìn)行向內(nèi)移動(dòng)，距離選取為單位長(zhǎng)度的一半。此時(shí)凸多邊形之間有著狹窄的通道，但由于對(duì)鏈接線區(qū)域的約束，其間巡檢機(jī)器人不可通行，于是可以使整片區(qū)域近似為一個(gè)不可通行的障礙區(qū)域，如圖2(c)所示。

圖2 改進(jìn)Maklink示意圖

2.2 改進(jìn)的人工勢(shì)場(chǎng)法的路徑規(guī)劃環(huán)境模型

在傳統(tǒng)的人工勢(shì)場(chǎng)法中，障礙物用一個(gè)質(zhì)點(diǎn)來(lái)表示，障礙物的實(shí)際大小由影響距離和斥力系數(shù)的大小來(lái)控制。規(guī)劃的目標(biāo)只被終點(diǎn)的牽引力所吸引，這使得算法很容易進(jìn)入到極值點(diǎn)或在一定范圍內(nèi)來(lái)回抖動(dòng)。本文提出了一種基于Maklink圖的人工勢(shì)場(chǎng)法環(huán)境模型的改進(jìn)。

在上文改進(jìn)Maklink建模之后，通過(guò)Dijkstra算法在選取鏈接線上的節(jié)點(diǎn)中找到一條起止點(diǎn)的最短路徑來(lái)作為人工勢(shì)場(chǎng)法的預(yù)規(guī)劃路線，將該路線中的節(jié)點(diǎn)依次作為人工勢(shì)場(chǎng)法的引力牽引點(diǎn)。對(duì)于障礙物區(qū)域的描述，通常人工勢(shì)場(chǎng)法中都是簡(jiǎn)化成一個(gè)質(zhì)點(diǎn)，但該方法精度較低，尤其凹多邊形更無(wú)法簡(jiǎn)化成一個(gè)質(zhì)點(diǎn)的情況，因此本文將障礙物的邊界進(jìn)行離散化處理，以巡檢機(jī)器人寬度的一半作為間隔，在障礙物的每個(gè)邊上放置斥力點(diǎn)，將一個(gè)障礙物邊界上的所有斥力點(diǎn)的合力來(lái)代表一個(gè)障礙物的對(duì)被規(guī)劃目標(biāo)的影響，如圖3所示。通過(guò)該方法構(gòu)建的路徑規(guī)劃環(huán)境模型，通過(guò)人工勢(shì)場(chǎng)法進(jìn)行求解，就可以得到一條安全性高且平滑的路徑，并解決算法原本容易陷入極值點(diǎn)的問(wèn)題。

圖3 變電站環(huán)境模型

3 考慮巡檢機(jī)器人尺度的路徑規(guī)劃算法設(shè)計(jì)

3.1 算法流程圖

本文將人工勢(shì)場(chǎng)法與Maklink鏈接圖法進(jìn)行結(jié)合實(shí)現(xiàn)了對(duì)變電站區(qū)域的路徑規(guī)劃，提高路線安全性的同時(shí)也避免了人工勢(shì)場(chǎng)法容易陷入極值點(diǎn)等問(wèn)題。同時(shí)，本文又通過(guò)遺傳算法對(duì)每條鏈接線上節(jié)點(diǎn)的位置進(jìn)行優(yōu)化，將節(jié)點(diǎn)位置進(jìn)行遺傳編碼，每一條染色體對(duì)應(yīng)所有節(jié)點(diǎn)的位置，再通過(guò)Dijkstra算法在節(jié)點(diǎn)中找到一條最短路徑，把該路徑作為人工勢(shì)場(chǎng)法的預(yù)規(guī)劃路徑，人工勢(shì)場(chǎng)法求解之后得到的路徑長(zhǎng)度作為染色體的適應(yīng)度值，最后不斷地通過(guò)遺傳算法的選擇、交叉、變異進(jìn)行循環(huán)迭代，最終得到最優(yōu)的規(guī)劃路徑。算法具體流程圖如圖4所示。

圖4 考慮機(jī)器人尺度的路徑規(guī)劃算法流程圖

3.2 人工勢(shì)場(chǎng)法基本原理及改進(jìn)

人工勢(shì)場(chǎng)法[12]的基本思想仿照電磁場(chǎng)對(duì)電荷的作用而構(gòu)建的虛擬力場(chǎng)，被規(guī)劃目標(biāo)視為正電荷，障礙物同樣為正電荷對(duì)其產(chǎn)生斥力，斥力的大小與兩點(diǎn)間距離成反比，目標(biāo)點(diǎn)為負(fù)電荷對(duì)被規(guī)劃目標(biāo)產(chǎn)生引力，引力大小與兩點(diǎn)間距離成正比，最終被規(guī)劃目標(biāo)在合力的影響下像著目標(biāo)點(diǎn)前進(jìn)。傳統(tǒng)人工勢(shì)場(chǎng)法路線規(guī)劃示意圖如圖5(a)所示。

圖5 人工勢(shì)場(chǎng)法作用原理示意圖

為了方便計(jì)算并且提高計(jì)算效率，本文對(duì)巡檢機(jī)器人的形狀進(jìn)行了簡(jiǎn)化，通過(guò)多邊形線段集對(duì)巡檢機(jī)器人的形狀進(jìn)行表述，以便不會(huì)減少機(jī)器人的實(shí)際尺度以及占用過(guò)多的自由空間。如圖5(b)所示，在通過(guò)線段集對(duì)機(jī)器人外形進(jìn)行描述之后，障礙物對(duì)機(jī)器人的排斥力不再由原來(lái)質(zhì)點(diǎn)間距離進(jìn)行計(jì)算，而是通過(guò)點(diǎn)到多邊形的最短距離來(lái)進(jìn)行計(jì)算，如前所述，點(diǎn)到多邊形的最短距離只會(huì)在點(diǎn)到多邊形的頂點(diǎn)或邊的垂線之間。這樣之后，可以有效地考慮機(jī)器人的尺度，從而保證了在狹窄的通道之下也能找到一條安全的可行路徑。

引力場(chǎng)和斥力場(chǎng)函數(shù)公式如下所示

Uatt(X)=0.5αρ2(X，Xg)

(1)

(2)

式中：α、β分別為引力與斥力系數(shù)；X、Xg、XO分別表示巡檢機(jī)器人模型的中心點(diǎn)、目標(biāo)終點(diǎn)引力點(diǎn)、障礙物斥力點(diǎn)的笛卡爾系坐標(biāo)位置；ρ(X，Xg)、ρ(X′，Xo)分別表示機(jī)器人中心點(diǎn)到目標(biāo)點(diǎn)的距離和機(jī)器人邊界到障礙物的最短距離；ρ0為障礙物斥力點(diǎn)的斥力作用范圍。機(jī)器人所具有的的勢(shì)場(chǎng)勢(shì)能隨著距離越近就會(huì)越大，距離越遠(yuǎn)則會(huì)越小，當(dāng)機(jī)器人處在障礙物的最大影響距離之外時(shí)，所具有的的勢(shì)場(chǎng)勢(shì)能則為零。

通過(guò)對(duì)引力場(chǎng)和斥力場(chǎng)函數(shù)計(jì)算負(fù)梯度公式，則可以得到機(jī)器人受到的作用力：

(3)

(4)

機(jī)器人所受的合力為：

(5)

3.3 基于回轉(zhuǎn)半徑的最大轉(zhuǎn)向角控制

(6)

圖6 機(jī)器人最大轉(zhuǎn)向角約束

(7)

(8)

l=vn+1*t

(9)

圖7 計(jì)算機(jī)器人運(yùn)動(dòng)示意圖

3.4 切換目標(biāo)點(diǎn)的判別方法

人工勢(shì)場(chǎng)法是通過(guò)中間引力點(diǎn)接續(xù)的引力作用指導(dǎo)巡檢機(jī)器人到達(dá)最終目標(biāo)點(diǎn)，當(dāng)中間節(jié)點(diǎn)距離障礙物較遠(yuǎn)時(shí)，可以在到達(dá)該目標(biāo)點(diǎn)一定距離之內(nèi)切換到下一個(gè)中間節(jié)點(diǎn)，直到到達(dá)最終點(diǎn)。但當(dāng)節(jié)點(diǎn)距離障礙物過(guò)近的情況下，會(huì)導(dǎo)致無(wú)法到達(dá)目標(biāo)點(diǎn)的一定范圍之內(nèi)，從而陷入來(lái)回徘徊的情形之中，此時(shí)需要在合適的時(shí)候跳出極值點(diǎn)，選取下一個(gè)節(jié)點(diǎn)作為目標(biāo)點(diǎn)，本文給出的判別方法如下。

圖8 切換目標(biāo)點(diǎn)的判別方法

4 仿真研究

為了驗(yàn)證本文算法的有效性，在CPU為Intel Core3的計(jì)算機(jī)上用MATLAB軟件進(jìn)行仿真。在200*200的區(qū)域內(nèi)設(shè)置變電站工作區(qū)域，共8個(gè)，其中右下角的障礙物區(qū)域?yàn)榘级噙呅螀^(qū)域，仿真目標(biāo)為尋找一條從起點(diǎn)到終點(diǎn)的安全性較高的可通行路線。參數(shù)設(shè)定為：巡航機(jī)器人寬度為4、長(zhǎng)度為10、引力系數(shù)α的值為5、斥力勢(shì)場(chǎng)系數(shù)β的值為10、斥力作用范圍為10，起止點(diǎn)坐標(biāo)分別為(10，130)、(150，50)。

仿真結(jié)果如圖9所示，圖中的虛線為Maklink圖和遺傳算法求解出的可行路徑，Dijkstra為預(yù)規(guī)劃路線，實(shí)線為本文算法計(jì)算出的結(jié)果?？梢钥闯?，本文算法相較于傳統(tǒng)的Maklink圖計(jì)算出的路徑會(huì)更為平滑，且與障礙物留有一定的安全距離，從而提高了路線安全性。下面通過(guò)相關(guān)指標(biāo)對(duì)兩個(gè)路線進(jìn)行定量分析。

圖9 原算法與改進(jìn)后的機(jī)器人路徑規(guī)劃仿真結(jié)果

本文定義障礙物到機(jī)器人邊界的最小距離為最小安全距離，單純Maklink求解路線與本文算法結(jié)果的最小安全距離與機(jī)器人方向角的變化曲線圖如圖10所示，可以看出原算法某些時(shí)刻的最小安全距離小于零，會(huì)發(fā)生碰撞風(fēng)險(xiǎn)。比較航線角的變化情況也可以發(fā)現(xiàn)本文算法得到的路線更為平滑，而原算法會(huì)有突然地轉(zhuǎn)角，不符合巡檢機(jī)器人的運(yùn)動(dòng)規(guī)律。

圖10 規(guī)劃安全距離和路徑方向角曲線圖

5 結(jié)論

本文提出了一種對(duì)機(jī)器人進(jìn)行路徑規(guī)劃的新方式，結(jié)合Maklink圖和人工勢(shì)場(chǎng)法得到優(yōu)化路徑，再利用遺傳算法迭代尋優(yōu)，最終可以在包含凹多邊形的變電站工作區(qū)域中得到一條平滑且安全性高的最短路徑，并且可以解決人工勢(shì)場(chǎng)法本身的局限問(wèn)題，避免陷入極值點(diǎn)和消除震蕩點(diǎn)。最后利用計(jì)算機(jī)仿真并與原算法對(duì)比，驗(yàn)證了本文算法的可行性與有效性。