Dijkstra改進算法在機器人避障問題的應用

金敬

摘要： 本文研究了機器人避障問題中如何計算最短路徑，建立了相應的數(shù)學模型。利用Dijkstra最短路徑改進算法對該模型進行求解，解決了由確定起點經過若干目標點到達終點的問題。

Abstract： Author is to set up a corresponding mathematical model by studying how to work out the shortest routes in the robot collision avoidance problem. The model is solved by using the shortest routes of improved Dijkstra Algorithm， thus the problem of arriving at the terminal point from the fixed starting point with several target points in the course has been settled.

關鍵詞： 機器人避障；最優(yōu)化問題；Dijkstra算法

Key words： robot collision avoidance；problem of optimization；Dijkstra Algorithm

中圖分類號：TP390 文獻標識碼：A 文章編號：1006-4311（2013）03-0232-02

1 問題重述

在一個800×800的平面場景圖1中，原點O（0，0）點處有一個機器人，它只能在該平面場景范圍內活動。該場景圖中有12個不同形狀的區(qū)域是機器人不能與之發(fā)生碰撞的障礙物，障礙物的數(shù)學描述如表1。

機器人的行走路徑由直線段和圓弧組成，其中圓弧是機器人轉彎路徑。機器人不能折線轉彎，轉彎路徑由與直線路徑相切的一段圓弧組成，也可以由兩個或多個相切的圓弧路徑組成，但每個圓弧的半徑最小為10個單位。為了不與障礙物發(fā)生碰撞，同時要求機器人行走線路與障礙物間的最近距離為10個單位，否則將發(fā)生碰撞，若碰撞發(fā)生，則機器人無法完成行走。機器人直線行走的最大速度為v0=5個單位/秒，最大轉彎速度為v=v（ρ）=■，其中ρ是轉彎半徑。如果超過該速度，機器人將發(fā)生側翻，無法完成行走。

對場景圖中4個點O（0，0），A（300，300），B（100，700），C（700，640），具體計算：機器人從O（0，0）出發(fā)，O→A→B→C→O的最短路徑。

2 符號說明和基本假設

R：機器人與障礙物的最小安全距離（10個單位）；v0：機器人直線行走的最大速度；v（ρ）：最大轉彎速度，其中ρ為轉彎半徑；L：路徑的總長度；di：第i段切線的長度；lj：第j段圓弧的長度；r：轉彎半徑；k：障礙物上的任意點與行走路徑之間的最短距離?；炯僭O：①機器人抽象為點；②機器人以最大速度v0勻速直線前進，以v（ρ）速度勻速轉彎；③機器人除了與障礙物相碰撞的情況以外均正常行走。

3 問題分析

問題要求機器人從O（0，0）出發(fā)，繞過障礙物按照一定的行走規(guī)則行走。求出O→A→B→C→O的最短路徑。要考慮機器人不能與障礙物發(fā)生碰撞，始終保持一定的安全距離?？紤]到機器人在運動過程中，并非關心所有的障礙物，它僅避開阻礙其向目標點運動的障礙物即可，沒有必要將到目標點所有的障礙物考慮進來。機器人經過中間的若干節(jié)點按照一定的規(guī)則繞過障礙物到達目標點，我們考慮經過障礙物拐彎點的問題，同時也考慮經過路徑中的目標點處轉彎的問題，利用Dijkstra算法計算出確定起點到達目標點的最短路徑。

4 模型的建立

4.1 節(jié)點路徑圖 利用Dijkstra算法計算最短路徑之前，首先要給出節(jié)點的定義。我們用以下方法定義了節(jié)點：每個障礙物的角對應一個節(jié)點，該節(jié)點在這個角的角平分線上，并且位于角頂點略大于安全距離R（R=10）個單位的位置，這樣就避免了機器人和障礙物發(fā)生摩擦或碰撞。給平面區(qū)域圖中每個節(jié)點一個標識符，把任意兩個節(jié)點的連線不經過障礙物的兩個節(jié)點連結起來，如果連接時和已經連好的路徑相交，理論上也可以把該路徑加入圖中，但這樣做并不可取。因為一來這樣會加大數(shù)據(jù)量，給計算處理造成很大的負擔，另外，計算處理后期還可以對路徑距離進行優(yōu)化。

如何確定節(jié)點的連線經過障礙物或與障礙物小于安全距離安全距離R（R=10）個單位呢？根據(jù)障礙物的不同形狀設計規(guī)則如下：

①檢測兩個節(jié)點的連線是否在障礙物內部。對于三角形的障礙物，即檢查兩個節(jié)點連線的直線方程與三角形三條邊的直線方程的聯(lián)立方程組是否有解；對于正方形、矩形、平行四邊形、圓（外切正方形）則檢查兩個節(jié)點連線的直線方程與障礙物兩條對角線的直線方程的聯(lián)立方程組是否有解。以上兩種情況，如有解說明相交連線與障礙物相交。②檢測兩個節(jié)點的連線與障礙物邊界小于安全距離。實際上檢查兩個節(jié)點連線的直線方程與障礙物的節(jié)點的距離是否小于安全距離。

在連接好節(jié)點的基礎上，計算出每條路徑的長度，就可以得到路徑網絡圖。如圖2所示。其中，圖2中圓形障礙物的節(jié)點就是該圓的外切正方形的頂點所對應的節(jié)點。障礙物與邊界相接的交點不計入節(jié)點。

4.2 Dijkstra最短路徑改進算法實現(xiàn) 我們可以運用經典的Dijkstra最短路徑算法。經典的Dijkstra算法是計算對某一確定起點到其它任何一節(jié)點的最短路徑。本模型只關心從確定起點到另一確定終點的最短路徑，沒必要計算所有節(jié)點。

Dijkstra改進算法的基本思想可以如下：以始發(fā)點（假設為uo）為樹根，按照據(jù)Uo的最短距離以及節(jié)點的相鄰關系，逐次放入樹中．由近至遠，直到目標點放入樹中，形成最短路樹，樹上每一個節(jié)點與UO的路徑都是最短路徑，其節(jié)點標記與Dijkstra算法完全相同，算法步驟的方法、原理也相同，只是算法的停止條件是目標點是否進入最短路樹，而不是把所有節(jié)點是否都標記作為運算停止的判斷條件，因此計算量則大大減少。

經過以上改進Dijkstra算法的初步處理，可以得出最短路徑。但此路徑多是折線，機器人不能此法行走，我們可以對該路徑進行進一步的優(yōu)化。在機器人行走速度一定的情況下，可以大大減少到達目的地的時間。

優(yōu)化算法如下：

①初始化，令K=l，p=N，N為節(jié)點數(shù)；②從k號節(jié)點開始，給p節(jié)點作連線，檢查該連線是否通過障礙物（不能忽略機器人的半徑R，否則會發(fā)生摩擦或碰撞）。不通過則轉④；通過障礙物時，如果p=K+2時，轉③，否則p=p-l，轉②。③若k=N．轉⑤：否則K=k+1，p=N，N=N-[（p-l）-（k+1）+1]，p=n．并對剩余的且大干k的節(jié)點進行重新順序編號，k=k+l，轉②。④把該直線作為路徑加入圖中，刪除編號為k+l到p-l的節(jié)點和相關邊。⑤算法結束。

4.3 最短路徑中圓弧長度的計算 最短路徑中的直線段長度的計算很簡單，根據(jù)Dijkstra改進算法計算出節(jié)點及其對應的坐標可得。具有圓形限定區(qū)域的最短路徑是由兩部分組成的：一部分是平面上的自然最短路徑（即直線段），另一部分是限定區(qū)域的部分邊界，這兩部分是相切的，互相連接的。以上分析表明從起點到目標點無論中間有多少障礙物，最短路徑都應該是若干個直線段和圓弧結構所組成，如圖3所示。最短路徑中圓弧長度的計算即求出圓弧■的長度。

如圖3，設Ax■，y■為起點，Bx■，y■為目標點，Cx■，y■和Dx■，y■分別為機器人經過拐點分別于隔離危險線拐角小圓弧的切點，圓心為Ox■，y■，圓的半徑為r，角度∠AOB=α，∠AOC=β，∠BOD=γ，∠COD=K。求A■B的長度。

如圖3可得有以下關系：AB=■AO=■BO=■，在ΔAOB中：α=arccos■，在RtΔAOC中：β=arccos■，在RtΔBOD中：γ=arccos■，因此，K=2π-α-β-γ。從而可得A■B的長度：

■+■+rK

假設機器人從起點O到到目標點路徑一定是由圓弧和線段組成，設有m條直線段，n條圓弧。那么路徑總長度L的目標函數(shù)可表示為：

minL=■d■+■l■d■∈D，l■∈D。

其中，d■第i段切線的長度，l■表示第j段圓弧的長度。D表示路徑集合。

5 模型的求解

利用Dijkstra改進算法經計算機器人從O（0，0）出發(fā)，O→A→B→C→O的最短總距離為：2726.623個單位，路徑總時間為：565.56秒。機器人從O（0，0）出發(fā)，O→A→B→C→O的最短路徑圖4所示。

6 模型評價

本模型的優(yōu)點有：①運用多個方案對路徑進行優(yōu)化，在相對優(yōu)化之中取得最優(yōu)解；②模型優(yōu)化后用解析幾何進行求解，精確度較高；③模型簡單易懂，便于實際檢驗及應用。本模型的缺陷有：1）此模型適于給予環(huán)境先驗完全信息的全局規(guī)劃，對于動態(tài)規(guī)劃卻失去了效率。2）在障礙物較多時，且形狀不規(guī)則時，本模型還需進一步改進。

參考文獻：

[1]姜啟源.數(shù)學模型[M]，北京：高等教育出版社，1993.

[2]鄧博斌.Dijkstra改進算法在地震救援中的應用[J].2008，（22）.

[3]周培德.計算幾何—算法與設計[M].北京：清華大學出版社，2005.