基于Dijkstra算法的機(jī)器人避障最短線路模型

孫忠民

(濰坊工程職業(yè)學(xué)院，山東 青州 262500)

1 問(wèn)題重述

研究機(jī)器人避障問(wèn)題:一個(gè)800×800的平面場(chǎng)景圖，在原點(diǎn)O(0，0)點(diǎn)處有一個(gè)機(jī)器人，它只能在該平面場(chǎng)景范圍內(nèi)活動(dòng)。圖中有12個(gè)不同形狀的區(qū)域是機(jī)器人不能與之發(fā)生碰撞的障礙物，見(jiàn)圖1。

圖1 800×800平面場(chǎng)景圖

目標(biāo)點(diǎn)分別為A(300，300)，B(100，700)，C(700，640)。機(jī)器人的行走路徑由直線段和圓弧組成，其中圓弧是機(jī)器人轉(zhuǎn)彎路徑。機(jī)器人不能折線轉(zhuǎn)彎，轉(zhuǎn)彎路徑由與直線路徑相切的一段圓弧組成，也可以由兩個(gè)或多個(gè)相切的圓弧路徑組成，但每個(gè)圓弧的半徑最小為10個(gè)單位并要求目標(biāo)點(diǎn)與障礙物的距離至少超過(guò)10個(gè)單位。為了不與障礙物發(fā)生碰撞，同時(shí)要求機(jī)器人行走線路與障礙物間的最近距離為10個(gè)單位，否則將發(fā)生碰撞，若碰撞發(fā)生，則機(jī)器人無(wú)法完成行走。

已知機(jī)器人直線行走的最大速度為v0=5個(gè)單位/秒。機(jī)器人轉(zhuǎn)彎時(shí)，最大轉(zhuǎn)彎速度為v=v(ρ)=，其中ρ是轉(zhuǎn)彎半徑。如果超過(guò)該速度，機(jī)器人將側(cè)翻，行走失敗。

建立機(jī)器人從區(qū)域中一點(diǎn)到達(dá)另一點(diǎn)的避障最短路徑和最短時(shí)間路徑的數(shù)學(xué)模型。

2 問(wèn)題分析

2.1 問(wèn)題1

由于機(jī)器人從O繞過(guò)障礙物到達(dá)目標(biāo)點(diǎn)的路徑有多條，首先，在忽略路徑中拐角處圓弧長(zhǎng)度情況下，利用有向圖路徑尋優(yōu)問(wèn)題的Dijkstra算法找出多條路徑中的最短線路;其次，對(duì)于最短線路中只經(jīng)過(guò)一個(gè)拐角的情況(O→A)，機(jī)器人要安全經(jīng)過(guò)需在拐角處放置一個(gè)半徑是10單位的安全防碰圓，利用簡(jiǎn)單線圓結(jié)構(gòu)精確求出O到A的最短路徑;然后，對(duì)最短線路中有多個(gè)拐角(O→B、O→C)進(jìn)行分析，兩個(gè)相鄰拐角的轉(zhuǎn)彎方式有兩種，一是路徑與兩圓心連線相交，二是路徑與兩圓心平行，這時(shí)需要做簡(jiǎn)單地變換求出最短路徑;最后，對(duì)O→A→B→C→O中經(jīng)過(guò)拐點(diǎn)A、B、C的情況進(jìn)行分析，把A、B、C分別看作圓弧上的點(diǎn)，利用線圓結(jié)構(gòu)，求出經(jīng)過(guò)拐點(diǎn)處的最短路徑。

表1 障礙物的數(shù)學(xué)描述表

2.2 問(wèn)題2

利用問(wèn)題1求出的O→A的最短距離路徑，由于轉(zhuǎn)彎速度與圓弧半徑有關(guān)，轉(zhuǎn)彎半徑越大轉(zhuǎn)彎處的速度越快，在半徑10的基礎(chǔ)上適當(dāng)擴(kuò)大拐點(diǎn)處的轉(zhuǎn)彎半徑，可使從O到A的行走時(shí)間最短。由于機(jī)器人到障礙物的最短距離為10，可知機(jī)器人行走路徑經(jīng)過(guò)定點(diǎn)(72.9289，217.0711)，首先考慮圓心在(80，210)情況得出從O到A的時(shí)間;然后考慮圓心位置、半徑大小變化的情況，建立以圓心坐標(biāo)為變量的二元函數(shù)，通過(guò)求二元函數(shù)在(80，210)附近的極值得出所需時(shí)間，并對(duì)兩種情形進(jìn)行比較，最終求得最短時(shí)間路徑。

3 模型假設(shè)

3.1 假設(shè)機(jī)器人的體積為0，即為一個(gè)點(diǎn)

3.2 假設(shè)其速度是瞬間變化的，即加速或減速時(shí)間為0

3.3 假設(shè)機(jī)器人行走線路與障礙物間的最近距離至少為10個(gè)單位

3.4 假設(shè)機(jī)器人行走時(shí)未超過(guò)直線行走最大速度和最大轉(zhuǎn)彎速度

3.5 假設(shè)Dijkstra算法中有向圖中的權(quán)重為兩點(diǎn)間的距離

3.6 假設(shè)每個(gè)障礙物從左下角開(kāi)始按順時(shí)針?lè)謩e標(biāo)為①②③④

4 符號(hào)說(shuō)明

L:路徑的總長(zhǎng)度;di:第i段切線的長(zhǎng)度;lj:第j段圓弧的長(zhǎng)度;ρ:轉(zhuǎn)彎半徑;k:障礙物上的任意點(diǎn)與行走路徑之間的最短距離;Oi:第i段切線的圓心坐標(biāo)(xoi，yoi)

5 模型建立

5.1 從O到目標(biāo)點(diǎn)的最短路徑選擇

機(jī)器人從O到各目標(biāo)點(diǎn)的路徑有很多，利用圖論中求最短路問(wèn)題的Dijkstra算法，對(duì)從O到各點(diǎn)的路徑進(jìn)行選擇，分別找出O→A，O→B，O→C，O→A→B→C→O的最短路徑。

5.1.1 Dijkstra最短路徑算法

如果指定了起始節(jié)點(diǎn)，則該點(diǎn)到其他所有節(jié)點(diǎn)的最短路徑可以一次性求出。假設(shè)結(jié)點(diǎn)個(gè)數(shù)為n，起始節(jié)點(diǎn)為s，則該算法步驟為:

(1)初始化:建立三個(gè)向量存儲(chǔ)各節(jié)點(diǎn)的狀態(tài)，其中visiteb表示各節(jié)點(diǎn)是否更新，初始值為0;dist存儲(chǔ)起始點(diǎn)到本節(jié)點(diǎn)最短距離，初始值為無(wú)窮;parent向量存儲(chǔ)到本節(jié)點(diǎn)上一個(gè)節(jié)點(diǎn)，默認(rèn)值為0。另外設(shè)起始節(jié)點(diǎn)處dist(s)=0。

(2)循環(huán)求解:讓I做n-1次循環(huán)，更新能由本節(jié)點(diǎn)經(jīng)過(guò)一邊到達(dá)的節(jié)點(diǎn)信息，并更新有本節(jié)點(diǎn)可以到達(dá)的未訪問(wèn)節(jié)點(diǎn)的最短路徑信息。循環(huán)直到所有未訪問(wèn)節(jié)點(diǎn)完全處理完成。

(3)提取到終止接點(diǎn)t的最短路徑，利用perent向量逐步提取最優(yōu)路徑。

5.1.2 從O到A的路徑選擇

表2 O到A的有向圖節(jié)點(diǎn)數(shù)據(jù)及最短路徑

5.1.3 從O到B的路徑選擇

表3 O到B的有向圖節(jié)點(diǎn)數(shù)據(jù)及matlab程序

O到B 的最短路徑為:(0，0)→(60，300)→(150，435)→(220，470)→(220，530)→(150，600)→(100，700)，最短路徑長(zhǎng)度:d=816.5。

5.2 問(wèn)題1的模型建立

5.2.1 模型1

由O到A的最短路徑是繞障礙物5的左上角G(80，210)到達(dá)目標(biāo)點(diǎn)A，見(jiàn)圖2。

圖2 O通過(guò)G到達(dá)A的最短路徑

圖3 O到B行徑的圓弧求解圖

E和F分別為機(jī)器人經(jīng)過(guò)圓G拐點(diǎn)分別于隔離危險(xiǎn)線拐角小圓弧的切點(diǎn)，設(shè)O(x1，y1)為起點(diǎn)，A(x2，y2)為目標(biāo)點(diǎn)，E(x3，y3)和 F(x4，y4)，圓心為 G(x5，y5)，圓的半徑為 R=10，∠AGO=a，∠OGE=b，∠EGF=c，∠AGF=d，從O到A的長(zhǎng)度為L(zhǎng)。由圖2可得:

5.2.2 模型2

由O到B的最短路徑為:(0，0)→(60，300)→(150，435)→(220，470)→(220，530)→(150，600)→(100，700)到目標(biāo)點(diǎn)B，可以看出不能用簡(jiǎn)單的線圓結(jié)構(gòu)求出最短路徑，需要做簡(jiǎn)單的變換求弧長(zhǎng)，見(jiàn)圖3。

由兩圓弧的切線IJ向下平移到圓心H交KJ的延長(zhǎng)線于P，兩圓的半徑均為R，要求LJ的弧長(zhǎng)，需要求出c的弧度。由圖3很容易看出

同理，機(jī)器人從O到目標(biāo)點(diǎn)C的最短路徑，由模型1、模型2可求。

5.2.3 模型3

機(jī)器人從O點(diǎn)出發(fā)，由O→A→B→C→O的最短路徑，機(jī)器人在行徑路程中經(jīng)過(guò)不同的拐點(diǎn)A、B、C，由于機(jī)器人行走的拐點(diǎn)半徑不小于10個(gè)單位，所以需要對(duì)拐點(diǎn)處機(jī)器人的繞線路徑進(jìn)行分析。

以A點(diǎn)為例進(jìn)行分析，過(guò)A點(diǎn)作圓弧并過(guò)D、O兩點(diǎn)做該圓的切線，切點(diǎn)為E、F?？芍筄經(jīng)過(guò)A到D的路徑最短，其圓弧的圓心O＇必在∠OAD的角平分線上，見(jiàn)圖4。

圖4 拐點(diǎn)路徑求解圖

圖5 O到A的最短時(shí)間路徑圖

設(shè)圓的半徑為 ρ ，圓心 O ＇(x，y)，O(x1，y1)，A(x0，y0)，D(x2，y2)，由圖 4 可知:

綜上所述，拐點(diǎn)A處的最短路徑可求，同理可得過(guò)B、C兩點(diǎn)的最短路徑。

5.3 問(wèn)題2的模型建立

結(jié)合問(wèn)題1求出的O→A的最短路徑，由于轉(zhuǎn)彎速度與圓弧半徑有關(guān)，轉(zhuǎn)彎半徑越大轉(zhuǎn)彎處的速度越快，在半徑10的基礎(chǔ)上適當(dāng)擴(kuò)大拐角處的轉(zhuǎn)彎半徑，可使從O到A的行走時(shí)間最短。由于機(jī)器人到障礙物的最短距離為10，可知機(jī)器人行走路徑經(jīng)過(guò)定點(diǎn)首先考慮圓心在(80，210)，半徑為10的情況得出從O到A的時(shí)間，然后考慮圓心、半徑變動(dòng)的情況，建立最短時(shí)間路徑的的二元函數(shù)，其中圓心坐標(biāo)為變量，通過(guò)求二元函數(shù)在(80，210)附近的極值得出所需時(shí)間，并對(duì)兩種情形進(jìn)行比較，最終求得最短時(shí)間路徑。

5.3.1 模型4

機(jī)器人與障礙物間的最近距離為10，已知O(0，0)、A(300，300)，以障礙物左上頂點(diǎn)為圓心，半徑為10畫(huà)圓與正方形的對(duì)角線交障礙物外一點(diǎn)，在障礙物中任選一點(diǎn)M(x，y)，以MD為半徑ρ作圓，分別過(guò)O、A做圓的切線交于點(diǎn)H、G連接MH、MG、MO、MA。借助模型3的結(jié)論可得:

綜上所述總時(shí)間:T=T1+T2

求T的最小值即為最短時(shí)間，所經(jīng)過(guò)的路徑即為最短時(shí)間路徑。

6 模型求解

機(jī)器人從點(diǎn)O到目標(biāo)點(diǎn)M0，路徑是由圓弧和線段組成，設(shè)有m條線段，n條圓弧。

6.1 問(wèn)題1求解

(1)利用模型1得到O到A的最短路徑長(zhǎng)度為471.04，所用時(shí)間為99.13。

(2)利用模型1、2得到O到B的最短路徑長(zhǎng)度為853.71，所用時(shí)間為179.07。

(3)利用模型1、2得到O到C最短路徑長(zhǎng)度為:1087.97，所用時(shí)間為217.59。

(4)利用模型1、2、3得到O經(jīng)過(guò)A、B、C回到O的最短路徑長(zhǎng)度為2701.54，所用時(shí)間為568.57。

6.2 問(wèn)題2求解

利用模型4，以圓心坐標(biāo)為變量建立最短時(shí)間T的二元函數(shù)，利用求二元函數(shù)在(80，210)附近的極值的方法，得出所需時(shí)間T，并與圓心固定在(80，210)情形進(jìn)行比較，最終求得從O到A最短時(shí)間路徑。O到A的最短時(shí)間路徑為:從O點(diǎn)到切點(diǎn)H(70.2475，212.6321)，經(jīng)圓弧到切點(diǎn)(77.1135，220.0683)經(jīng)線段到A(300，300)。拐角處圓心M的坐標(biāo)為(81.1678，209.0244)，半徑r為11.5，最短時(shí)間路徑l長(zhǎng)為471.68，最短時(shí)間為94.8971。

7 模型的評(píng)價(jià)與改進(jìn)

7.1 優(yōu)點(diǎn)

(1)運(yùn)用Dijkstra搜索算法對(duì)路徑進(jìn)行篩選，有普遍性并便于推廣。

(2)模型簡(jiǎn)單易懂，便于實(shí)際檢驗(yàn)及應(yīng)用。

(3)模型優(yōu)化后用解析幾何進(jìn)行求解，精確度較高。

7.2 缺點(diǎn)

(1)此模型適于全局規(guī)劃，獲得精確解卻失去了效率。

(2)在障礙物較多且形狀不規(guī)則時(shí)，模型需要進(jìn)一步改進(jìn)。

7.3 模型改進(jìn)

模型也可采用蟻群優(yōu)化算法，利用差分演化算法進(jìn)行信息素的更新，同時(shí)對(duì)可能出現(xiàn)的停滯現(xiàn)象，在信息素更新時(shí)加入混沌擾動(dòng)因子，采用一個(gè)新的評(píng)價(jià)函數(shù)增強(qiáng)算法的逃逸能力，避免了路徑死鎖現(xiàn)象，高效率地搜索出最優(yōu)路徑，即使在障礙物非常復(fù)雜的環(huán)境下，也能快速地規(guī)劃出安全的最優(yōu)路徑，是一種與本模型相符的有效改進(jìn)方法。

［1］趙靜，但琦.數(shù)學(xué)建模與數(shù)學(xué)實(shí)驗(yàn)［M］.北京:高等教育出版社，2000.

［2］姜啟源.數(shù)學(xué)模型［M］.北京:高等教育出版社，2003.

［3］邦迪.圖論及其應(yīng)用［M］.西安:西安科學(xué)出版社，1984.

［4］薛定宇，陳陽(yáng)泉.高等應(yīng)用數(shù)學(xué)問(wèn)題的MATLAB求解［M］.北京:清華大學(xué)出版社，2004.