未知環(huán)境下基于三次螺線Bug算法的移動機器人路徑規(guī)劃

康 亮， 趙春霞， 郭劍輝

（南京理工大學(xué)計算機與科學(xué)技術(shù)學(xué)院，江蘇 南京 210094）

移動機器人技術(shù)是近年來發(fā)展起來的一門綜合學(xué)科，集中了機械、電子、計算機、自動控制以及人工智能等多學(xué)科最新研究成果，代表了機電一體化的最高成就[1]。在移動機器人相關(guān)技術(shù)的研究中，導(dǎo)航技術(shù)是其核心，而機器人路徑規(guī)劃是解決機器人導(dǎo)航的核心問題之一。

移動機器人路徑規(guī)劃可以分為兩類：一類是基于已知或部分已知數(shù)字地圖的全局路徑規(guī)劃[2]；另一類是基于傳感器信息的局部路徑規(guī)劃[3-5]。很多實際情況下，機器人并不具備完備的環(huán)境知識和復(fù)雜的障礙物信息。因為未知的障礙物可能出現(xiàn)在事先規(guī)劃好的路徑上，這樣機器人導(dǎo)航系統(tǒng)中需要基于傳感器信息的，以在線方式進行避碰規(guī)劃。此時，Bug算法[6]在實際中被應(yīng)用。這種算法是完備的。如果在機器人起點和目標(biāo)終點間存在可行路徑的話，那么這種純粹的應(yīng)激式算法肯定會完成路徑規(guī)劃，保證算法的全局收斂。但這種算法卻不能保證規(guī)劃路徑的光滑，這在實際應(yīng)用中會使得移動機器人執(zhí)行起來很困難，精度下降。而曲率連續(xù)對于非完整移動機器人路徑設(shè)計是最基本的。

作為非完整移動機器人控制任務(wù)之一的路徑規(guī)劃，由于先規(guī)劃出來的路徑可能由若干線段組成，而且線段之間可能出現(xiàn)不連續(xù)。為了避免移動機器人在運動過程中產(chǎn)生滑動現(xiàn)象而降低機器人的實時到達性，有必要將路徑光滑化。路徑光滑的任務(wù)就是尋求一種連接移動機器人任意兩個姿態(tài)的曲線，對于時間和長度都是最優(yōu)的。在沒有解決曲線路徑跟蹤難題之前，一般就是用直線來連接各個姿態(tài)。直線連接實現(xiàn)簡單，路徑長度最短，但需要移動機器人頻繁地停止以改變方向，靈活性不夠。后來使用直線-圓弧-直線的連接方案使得移動機器人的靈活性得到提高。此算法中一個主要問題是生成路徑的曲率不能連續(xù)。在直線與圓弧的連接點處，曲線的曲率會發(fā)生跳變，移動機器人的角速度等參數(shù)也隨之發(fā)生跳變。因此，當(dāng)移動機器人進行路徑跟蹤時跟蹤精度必然受到影響。故嚴(yán)格意義上講，機器人是不能平滑跟蹤路徑的。

本文考慮未知環(huán)境下移動機器人的路徑規(guī)劃。將Bug算法與基于滾動窗口的路徑規(guī)劃相結(jié)合，先由路徑規(guī)劃器產(chǎn)生一條局部無碰撞可行路徑，再利用三次螺線對滾動窗口內(nèi)已規(guī)劃的路徑光滑化，從而使得移動機器人易于跟蹤所規(guī)劃的路徑，隨滾動窗口的推進來完成未知環(huán)境下的移動機器人路徑規(guī)劃。

三次螺線[7]（cubic spiral）是一種曲線。這種類型的曲線有連續(xù)曲率變化，切線方向由路徑距離的三次方函數(shù)描述。三次螺線與直線連接時曲率連續(xù)，生成的路徑滿足幾何學(xué)特性，符合移動機器人動力學(xué)特性。結(jié)果顯示三次螺線在路徑最大曲率處是較好的。同時三次螺線的各個參量容易得到，滿足移動機器人實時性計算的要求。

1 Bug算法

在Bug算法中，移動機器人沿連接目標(biāo)點和起點的最短直線前進，在遇到障礙物時采用邊緣跟蹤的方法繞過障礙物，然后再次沿直線前進。Bug算法在移動機器人未知環(huán)境導(dǎo)航中是經(jīng)典通用的算法。需求的存儲容量小。如果在機器人起點和目標(biāo)終點間存在可行路徑的話，那么這種純粹的應(yīng)激式算法肯定會完成路徑規(guī)劃，保證算法的全局收斂。

Bug算法在尋找兩點間可行路徑上并不能保證最優(yōu)。標(biāo)準(zhǔn)的算法是在起點和終點的直線方向上產(chǎn)生，遇到障礙物時沿相同的方向繞行。這種算法能保證尋找到一條安全路徑，但并不保證能迅速到達目標(biāo)終點，如圖1所示。

圖1 標(biāo)準(zhǔn)的Bug算法

Bug算法由兩種模式來共同保證全局收斂：趨向目標(biāo)模式（Moving toward the Goal）和邊線沿走模式（Boundary Following）。開始時移動機器人執(zhí)行“趨向目標(biāo)”模式，遇到障礙物時轉(zhuǎn)向“邊線沿走”模式。

1.1 趨向目標(biāo)模式

趨向目標(biāo)模式的目的是使機器人以到目標(biāo)點的距離單調(diào)遞減的方式向目標(biāo)點運動，從而保證全局收斂。在此模式下，機器人或者在自由區(qū)域直接向目標(biāo)運動或者繞過障礙物向目標(biāo)運動。

機器人沿直線移動，趨向目標(biāo)，直到發(fā)生下列情況：① 到達目標(biāo)，移動終止；② 如果移動機器人遇到障礙物，則轉(zhuǎn)入“邊線沿走模式”。

1.2 邊線沿走模式

邊線沿走模式的目的是繞過障礙物，使得機器人可以繼續(xù)向目標(biāo)點運動。此時需要機器人選擇較優(yōu)的繞行方向，而錯誤的繞行方向常常嚴(yán)重增加路徑長度，降低效率。

標(biāo)準(zhǔn)的Bug算法是遇到障礙物時永遠沿相同的方向繞行。Distbug算法[8]對Bug算法進行了改進：機器人用符號化的累積參數(shù)Dir來選擇遇到障礙時的繞行方向，Dir為正數(shù)表示左側(cè)可安全通行的區(qū)域更大，應(yīng)選擇左側(cè)繞行；反之則選擇右側(cè)繞行。Tangentbug算法[9]選擇最小化距離作為繞行方向，直到發(fā)生下列情況：① 到達目標(biāo)，移動終止；② 滿足算法提出的離開條件，轉(zhuǎn)入“趨向目標(biāo)模式”；③ 機器人繞障礙物閉合循環(huán)一周，則宣布目標(biāo)無法到達，移動終止。

由圖1可看出，原始標(biāo)準(zhǔn)Bug算法使用傳感器數(shù)據(jù)信息生成的路徑并不是最優(yōu)路徑，它需要改進。而且該算法有個前提是假設(shè)機器人為一個質(zhì)點，沒有實際尺寸?？稍趯嶋H的機器人研究中，這個假設(shè)總是不成立。因為大多數(shù)移動機器人為非完整移動機器人，對所規(guī)劃路徑有一定的要求。曲率不連續(xù)的路徑很難保證機器人跟蹤所規(guī)劃的路徑。這在圖1中就可以看到。機器人在兩種行為模式轉(zhuǎn)換時，規(guī)劃路徑曲率發(fā)生跳變。移動方向的突然變化使得移動機器人的角速度等參數(shù)也隨之發(fā)生跳變，則移動機器人進行路徑跟蹤時跟蹤精度必然受到影響。

同時，作為非完整移動機器人，在實際的系統(tǒng)執(zhí)行中，方向角度變化不可能太大。當(dāng)轉(zhuǎn)變的方向角度為90°時，輪式移動機器人的前輪會和兩前輪之間的軸碰撞，也就是說機械裝置把方向角限制在一個特定的范圍之內(nèi)。這要求系統(tǒng)路徑規(guī)劃器能規(guī)劃出平滑的路徑來適應(yīng)實際需要。

2 三次螺線

路徑的平滑對于移動機器人導(dǎo)航是最基本的，因為不平滑的運動可能會導(dǎo)致輪子打滑，而這會降低機器人剎車能力。為了控制路徑的平滑性，本文定義了路徑平滑的成本函數(shù)：路徑曲率導(dǎo)數(shù)的平方。通過定義，得到三次螺線的簡單路徑。Boissonnat[10]采用直線和圓弧來描述路徑，并做了很多研究。然而，此算法中一個主要問題是生成路徑的曲率不能連續(xù)。因此，嚴(yán)格意義上講，移動機器人是不能平滑跟蹤路徑的。三次螺線是一種曲線。它的切線方向由路徑距離的三次方函數(shù)描述。這種類型的曲線有連續(xù)曲率變化，生成路徑滿足幾何學(xué)特性，符合移動機器人動力學(xué)特性，可以擴展移動機器人的應(yīng)用。同時三次螺線的各個參量容易得到，滿足移動機器人實時計算的要求。

2.1 相關(guān)定義

本文描述了一種平滑局部路徑的算法，是通過一連串二維世界坐標(biāo)系中移動機器人位姿（位置和方向）代替一連串曲線段來描述路徑。在連接給出位姿(x,y,θ)的同時，平滑移動機器人的路徑。

定義p1=(x1,y1,θ1)，p2= (x2,y2,θ2)是移動機器人的兩個路徑位姿節(jié)點，此時(x1,y1)≠ (x2,y2)。從點(x1,y1)到點(x2,y2)的方向β為

如果以下關(guān)系滿足，則兩個位姿p1和p2被稱為對稱。

如果兩個位姿p1和p2對稱，記為sym(p1,p2)，否則記為 ～sym(p1,p2)。

對稱位姿(p1,p2)的大小d和偏差α被定義為兩點間歐式距離平方和兩點方向角的差值

d=((x2-x1)2+(y2-y1)2)，α=θ2-θ1

如果sym(p1,q)和sym(q,p2)，則位姿q被稱為(p1,p2)的切分位姿。

定義一個有限長度的封閉路徑表示法。路徑Π是一對(l,k)。其中l(wèi)是正的路徑長度，k是連續(xù)曲率函數(shù)。曲率在(x(s),y(s) )處的曲線半徑和切線方向?qū)?shù)。函數(shù)θ由下式給出

如果曲率函數(shù)k是對稱的，也就是如果k(-s)=k(s)，則對于所有s，有以下關(guān)系

路徑點 (x,y)= (x(s),y(s))由下式給出

在平滑路徑過程中，須定義一個關(guān)于路徑平滑度的成本函數(shù)。對于每一個路徑Π，路徑平滑成本cost(Π)是明確具體的。這里定義路徑平滑的成本函數(shù)為

根據(jù)路徑上兩個位姿點的關(guān)系情況，可以分為以下兩種情況：

（1） 如果sym(p1,p2)，在具有同樣大小和偏差的(p1,p2)集合S中尋找路徑，確定適當(dāng)?shù)钠揭坪托D(zhuǎn)來適應(yīng)末端位姿。這種合成的簡單路徑表示為 Π (p1,p2)。注意，大小和偏差就完全描述了一個簡單路徑。

（2） 如果 ～sym(p1,p2)，首先，尋找(p1,p2)的切分軌跡，接著尋找切分位姿q，使得 成 本 總 和 cost(Π(p1,q))+cost(Π(q,p2))是最小的。

2.2 位姿對稱

根據(jù)路徑平滑成本函數(shù)，使得方程（2）的路徑平滑成本最小，將路徑分類。

對于固定路徑長度l，以路徑曲率導(dǎo)數(shù)的平方為路徑平滑成本函數(shù)。取路徑平滑成本函數(shù)cost(Π )最小，通過解方程（2）可以得到

這里的A、B、C、D是積分常數(shù)。因為上式描述的方向函數(shù)θ是三次方函數(shù)，所以稱這類路徑曲線為三次螺線，其曲率是連續(xù)的。

當(dāng)兩個位姿p1和p2是對稱情況時，(p1,p2)的大小d和偏移α被給出。在平滑路徑規(guī)劃問題中，用三次螺線來連接兩個對稱位姿點，則三次螺線的長度、曲率和成本表達式為

這里的θ0是常數(shù)。其中

2.3 位姿非對稱

當(dāng)輸入位姿p1=(x1,y1,θ1)，p2=(x2,y2,θ2)是非對稱時，根據(jù)方向角關(guān)系，將(p1,p2)分兩種 情 況 。 這 里 (x1,y1)≠ (x2,y2)。當(dāng)且僅當(dāng)θ1=θ2時，(p1,p2)被稱為平行的，否則稱為不平行。根據(jù)sym(p1,q)和sym(q,p2)，尋找切分位姿q，使得成本總和 cost(Π(p1,q))+cost(Π(q,p2))是最小的。則q(x,y,θ)點的位置是：

（1） 如果(p1,p2)是平行的，則由點p1和p2來確定直線

（2） 如果(p1,p2)不是平行的，通過點p1和p2的曲線為

對于路徑平滑成本函數(shù)，當(dāng)θ=θ1=θ2，(p1,p2)是平行時，取成本總和cost(Π(p1,q))+cost(Π(q,p2))是最小，則切分位姿q點是

當(dāng)(p1,p2)不是平行時，切分點q位于方程（5）曲線中，使成本總和 cost(Π (p1,q))+cost(Π (q,p2))是最小。因為沒有解析方法，這里采用Newton-Raphson迭代算法來尋找最小值。

3 三次螺線Bug算法

本文改進Bug算法的缺點，通過構(gòu)建滾動窗口，選擇較優(yōu)的路徑節(jié)點，利用上節(jié)所述的三次螺線來平滑路徑。最終隨著滾動窗口的推進，移動機器人能夠沿規(guī)劃的平滑路徑，在未知環(huán)境中到達目標(biāo)位置。

3.1 滾動窗口構(gòu)建

信息不完全、環(huán)境不確定情況下的路徑規(guī)劃問題，是傳統(tǒng)全局規(guī)劃方法無法解決的。注意到機器人在運動過程中能探知其傳感范圍內(nèi)一有限區(qū)域的環(huán)境信息，這部分信息必須充分利用。因此，解決這一問題的指導(dǎo)思想是：采用反復(fù)進行的局部優(yōu)化規(guī)劃代替一次性的全局優(yōu)化的結(jié)果，并在每次局部優(yōu)化中充分利用該時刻最新的局部環(huán)境信息[11]。

以周期方式驅(qū)動，在滾動的每一步，定義以機器人當(dāng)前位置為中心的一區(qū)域為優(yōu)化窗口。Win(PR(t) )= {P|P∈W,d(P,PR(t) )≤r} 稱為機器人在PR(t)處的視野域，亦即該點的滾動窗口，其中PR(t)為機器人工作環(huán)境中的可行區(qū)域，r為機器人傳感器的探測半徑，W為機器人的工作環(huán)境。

局部子目標(biāo)最優(yōu)點Oi和最短路徑由啟發(fā)式函數(shù)l(x,G)=min(l(x,Oi)+d(Oi,G))決定。這種子目標(biāo)的選擇方法反映了全局優(yōu)化的要求與局部有限信息約束的折衷，是在給定信息環(huán)境下企圖實現(xiàn)全局優(yōu)化的自然選擇。

該區(qū)域內(nèi)的預(yù)測模型一方面是全局環(huán)境信息向該區(qū)域的映射，另一方面還補充了傳感器系統(tǒng)檢測到的原來未知的障礙物。以當(dāng)前點為起點，根據(jù)全局先驗信息采用本文的啟發(fā)式方法確定該窗口區(qū)域的局部目標(biāo)，根據(jù)窗口內(nèi)信息所提供的場景預(yù)測進行規(guī)劃，找出適當(dāng)?shù)木植柯窂?，機器人依此路徑移動，直到下一周期。

3.2 趨向目標(biāo)模式

首先判斷機器人和給定的目標(biāo)位置之間是否存在障礙物。傳感器輸入是機器人當(dāng)前位置和在傳感器探測半徑范圍內(nèi)機器人與障礙物的距離。機器人掃描直接指向目標(biāo)的扇形，并以此扇形區(qū)域作為機器人滾動窗口來搜索路徑。扇形半徑是機器人的探測半徑，扇形弦長為機器人的碰撞直徑，本文取機器人正前方3個傳感器的探測區(qū)域。

以G代表目標(biāo)位置，其坐標(biāo)為(xG，yG)，以R、O分別代表機器人及障礙物，坐標(biāo)為(xR，yR)、(xO，yO)。設(shè)置機器人和障礙物的碰撞半徑，也就是說在其半徑以外無碰撞的危險，保證規(guī)劃的路徑寬度。若機器人與目標(biāo)位置之間不存在障礙物，機器人可按“趨向目標(biāo)”模式直接到達目標(biāo)位置。此時的直線方程可由兩點式確定

在趨向目標(biāo)直線行走模式下，機器人只利用指向目標(biāo)點的扇形區(qū)域內(nèi)的探測數(shù)據(jù)來作為滾動窗口數(shù)據(jù)信息規(guī)劃路徑。當(dāng)此扇形區(qū)域內(nèi)出現(xiàn)障礙物時機器人需擴展探測區(qū)域，規(guī)劃繞行路徑。本文是將扇形區(qū)域擴展為機器人周身360°的探測圓，作為滾動規(guī)劃窗口。

當(dāng)機器人探測到障礙物時，如果障礙物位于預(yù)規(guī)劃的前進路徑，則取消原來的直線行進計劃，改為沿三次螺線靠近障礙物向目標(biāo)前進。此三次螺線為機器人當(dāng)前點與所探測到障礙物邊界登陸點之間的三次螺線。

當(dāng)下列條件滿足時，“趨向目標(biāo)”模式結(jié)束：

（1） 機器人到達目標(biāo)點，算法停止；

（2） 機器人探測到障礙物阻擋前進路徑，切換到“邊線沿走”模式。

3.3 邊線沿走模式

首先定義兩個概念：① 登陸點：滾動窗口內(nèi)可見的障礙物上最近的頂點。② 分離點：繞過障礙物時探索線的起點。

如圖2所示，當(dāng)移動機器人沿直線前進，如果傳感器探測到障礙物，則障礙物可見邊界的頂點必定分布在直線路徑的兩側(cè)。把障礙物可見邊界的頂點分成兩組。圖中以藍色小圓圈表示。直線左側(cè)的頂點歸入L組，右側(cè)的歸入R組。另設(shè)OL和OR分別是L和R組中離原來路徑最遠的兩個頂點。

根據(jù)距離長度公式li=l(x,Oi)+d(Oi,G)，Oi分別是OL和OR。l(x,Oi)為當(dāng)前機器人到Oi點的三次螺線長度。d(Oi,G)為障礙物Oi點到目標(biāo)終點G的直線距離。引入d(Oi,G)是為了保證機器人所規(guī)劃的路徑能夠全局收斂，不至于因為局部信息的不完整而導(dǎo)致非全局最優(yōu)的路徑。如果lR＜lL，則令C=R，否則令C=L。OC就是登陸點。移動機器人繞障礙物行走時首先向登陸點靠近。

圖2 探測到障礙物

如果移動機器人滾動窗口內(nèi)探測到身處多個障礙物包圍中，結(jié)合上文，利用以下距離長度公式在探測到的各個障礙物邊界中求登陸點

其中Oi分別為移動機器人能夠探測到的各個障礙物可見邊界的頂點，l(x,G)表示在探測到的各個障礙物登陸點中機器人距離目標(biāo)最短路徑的長度。移動機器人選擇路徑最短的頂點作為下一步運動的登陸點。如上所述，l(x,G)的計算表達式反映了機器人的全局意識，保證了算法路徑規(guī)劃中的全局收斂。

移動機器人找到登陸點后，就開始規(guī)劃如何繞過障礙物。先把機器人與登陸點用三次螺線方程連接，再把登陸點OC與目標(biāo)連接起來。視OC為當(dāng)前點，記做P。如果機器人在此處的滾動窗口內(nèi)沒有探測到障礙物，表明已繞過了障礙物，稱此P點為移動機器人繞過障礙物的分離點，否則把當(dāng)前登陸點從P出發(fā)沿著障礙物邊界移到機器人探測到的障礙物邊界下一個節(jié)點上，進入“邊線沿走”模式。同時由距離長度公式li=l(x,Oi)+d(Oi,G)，不斷選擇窗口內(nèi)的最佳的局部路徑目標(biāo)點。

當(dāng)滿足下列條件時，“邊線沿走”模式結(jié)束：

（1） 到達目標(biāo)點，算法結(jié)束；

（2） 滿足離開條件，探測不到障礙物，切換到“趨向目標(biāo)”模式。

離開條件為當(dāng)上一個登陸節(jié)點到目標(biāo)終點距離大于機器人其他傳感器定位點到目標(biāo)終點的距離時，算法選擇距離最小值的傳感器定位點為機器人路徑的下一節(jié)點，同時模式切換為趨向目標(biāo)模式。

4 仿真實驗

算法驗證采用四輪移動式機器人。機器人在運行環(huán)境中的定位由GPS完成。測距系統(tǒng)是由安裝在機器人車體前后的16個超聲波傳感器來實現(xiàn)，用來提供機器人周圍的障礙物距離信息。路徑規(guī)劃算法由主機完成，規(guī)劃完成后把控制命令發(fā)給機器人，使它在運行環(huán)境中移動。

這里有一個對比實驗。當(dāng)取路徑平滑成本函數(shù)為路徑節(jié)點曲率的平方時，即

則將成本函數(shù)最小化，可得方向函數(shù)θ(s)=As+B，分別代入圓弧方程和回旋螺線方程

圖3是偏差α是π的兩個位姿的連接情況。分別用圓弧、三次螺線和回旋螺線來平滑路徑?？梢钥吹剑瑘A弧連接時曲線長度最小，回旋螺線連接時曲線長度最大。相比于圓弧連接的曲率不連續(xù)和回旋螺線連接的曲線長度最大，三次螺線是兩個路徑位姿點用于平滑路徑的最佳選擇。

移動機器人路徑規(guī)劃仿真環(huán)境是用matlab開發(fā)的，運行于PC機，CPU主頻512M。起始點和終點位姿分別是(0,0,π/4)，(30,30,π/4)。在30×30(m)環(huán)境下的路徑規(guī)劃結(jié)果如圖4所示。

圖3 方向偏差為π時的曲線連接

圖4 路徑規(guī)劃結(jié)果比較

圖4顯示了分別利用標(biāo)準(zhǔn)Bug算法和用本文三次螺線Bug算法及回旋螺線Bug算法來規(guī)劃路徑的結(jié)果比較。無論在趨向目標(biāo)模式還是邊線沿走模式，傳統(tǒng)Bug算法都有嚴(yán)格規(guī)定，不能隨意改變方向。從圖4試驗結(jié)果來看，本文構(gòu)造滾動窗口時，搜索機器人周圍各點的最佳行走方向，考慮全局收斂標(biāo)準(zhǔn)，因此可以在任何模式下都能根據(jù)具體情況，按照最佳的行走方向來規(guī)劃路徑，提高了路徑的優(yōu)化性。同時由于三次螺線的使用，使規(guī)劃路徑曲率連續(xù)，移動機器人跟蹤精度得到明顯改善，直線與三次螺線連接處平滑過渡，可以規(guī)劃出光滑的路徑，實現(xiàn)了四輪移動機器人連續(xù)的移動。表1給出了相關(guān)算法在圖4環(huán)境中的路徑規(guī)劃結(jié)果對比。

表1 算法性能對照表

結(jié)合表1，從圖4中看到無論是長度還是運行時間，本文算法都比標(biāo)準(zhǔn)Bug算法要較優(yōu)。而且標(biāo)準(zhǔn)Bug算法的路徑曲率不連續(xù)，不能應(yīng)用于非完整移動機器人?；匦菥€雖然可以保證曲率連續(xù)使路徑平滑，但規(guī)劃的路徑長度卻增加，運行時間也同比增長。本文算法既滿足了規(guī)劃路徑曲率連續(xù)，保證機器人運動平滑，同時在路徑長度和運行時間上達到了綜合最優(yōu)。圖5是利用本文算法的移動機器人在復(fù)雜環(huán)境中的路徑規(guī)劃結(jié)果，更清晰顯示了路徑的光滑性。

圖5 路徑規(guī)劃結(jié)果

5 收斂性和完備性證明

在趨向目標(biāo)模式中，距離函數(shù)d(x,G)是單調(diào)遞減的，因而此模式下的路徑長度是有限的。在邊線沿走模式中，總是選擇距離長度最小值節(jié)點為規(guī)劃路徑下一節(jié)點，因而此模式下的每一步路徑長度也是有限的。因此，算法最終會在一個有限長度的路徑上停止。如果目標(biāo)終點可以到達，那么規(guī)劃路徑收斂到目標(biāo)終點的可能性就會得到保證。

首先定義機器人路徑是由一連串的節(jié)點組成。起點S，目標(biāo)終點G。登陸點Hi是機器人首先探測到障礙物時的路徑節(jié)點。離開點Di是在趨向目標(biāo)模式中機器人開始遠離障礙物邊界時的路徑節(jié)點。轉(zhuǎn)換節(jié)點Pi為機器人從趨向目標(biāo)模式轉(zhuǎn)為邊線沿走模式時的節(jié)點。每一個轉(zhuǎn)換節(jié)點一定也是屬于l(x,G)的局部極值點Mi。轉(zhuǎn)換離開節(jié)點Li為機器人從邊線沿走模式轉(zhuǎn)為趨向目標(biāo)模式時的路徑節(jié)點。

假設(shè)機器人工作在一個有限的幾何空間中，環(huán)境空間中的每一個障礙物都是有限周長，因此有以下結(jié)論：

（1） 機器人趨向目標(biāo)模式的路徑片斷必定會終止在目標(biāo)點G，或者終止在局部極值點Mi。

（2） 在趨向目標(biāo)模式中，路徑長度必定是有限長度。

（3） 如果從局部極值點可以到達目標(biāo)終點，那么由于l(x,G)=min(l(x,Oi)+d(Oi,G))，機器人離開障礙物的路徑長度將是有限長度。

下面給出算法的收斂性證明：如果移動機器人可以到達目標(biāo)，那么成功規(guī)劃的路徑長度必定是有限長度。

證明：機器人的路徑長度等于趨向目標(biāo)模式下和邊線沿走模式下的路徑長度之和。由結(jié)論（2），在趨向目標(biāo)模式中，機器人路徑長度必定是有限長度。因為機器人工作在一個有限的幾何空間中，所以障礙物的個數(shù)也必定是有限的。而每一個障礙物又都是有限周長，所以在邊線沿走模式中，機器人的路徑長度也必定是有限長度。由結(jié)論（3），如果機器人可以從局部極值點到達目標(biāo)終點，那么機器人離開障礙物的路徑長度也必將是有限長度。

因此，算法成功規(guī)劃的路徑長度必定是有限長度。

下面再給出算法的完備性證明：只要移動機器人可以到達目標(biāo)，那么算法必定可以規(guī)劃出從起點到終點的路徑。

證明：由結(jié)論（1），趨向目標(biāo)模式中的每一個機器人路徑片斷都必定會終止在目標(biāo)點G，或者終止在局部極值點Mi。當(dāng)發(fā)現(xiàn)局部極小值點Mi時，機器人轉(zhuǎn)為邊線沿走模式。如果目標(biāo)是可以到達的，那么由結(jié)論（3），繞障礙物行走模式必定會終止在轉(zhuǎn)換離開節(jié)點Li。因為機器人工作空間有限，則障礙物個數(shù)也必定是有限個數(shù)，所以繞障礙物行走模式下的最終路徑會是趨向目標(biāo)模式下的路徑片斷。因此，最終的路徑必定會終止在目標(biāo)點。由此證明了算法的完備性。

6 結(jié) 論

本文考慮未知環(huán)境下移動機器人的路徑規(guī)劃。將Bug算法與基于滾動窗口的路徑規(guī)劃相結(jié)合，先由路徑規(guī)劃器生成一條局部無碰撞可行路徑，再利用三次螺線對滾動窗口內(nèi)已規(guī)劃的路徑光滑化，從而使得移動機器人易于跟蹤所規(guī)劃的路徑，隨滾動窗口的推進來完成未知環(huán)境下的移動機器人路徑規(guī)劃。

在原有Bug算法基礎(chǔ)上，本文搜索機器人周圍各點的最佳行走方向，考慮引入的全局收斂標(biāo)準(zhǔn)，因此可以在任何模式下都能根據(jù)具體情況，按照最佳的行走方向來規(guī)劃路徑，提高了路徑的優(yōu)化性。

三次螺線與直線連接時曲率連續(xù)，生成的路徑滿足幾何學(xué)特性，符合移動機器人動力學(xué)特性，曲率有界。同時三次螺線的各個參量容易得到，滿足移動機器人實時性計算的要求。這種類型的曲線有連續(xù)曲率變化，可以擴展移動機器人的應(yīng)用。本文中，給出了機器人路徑平滑成本函數(shù)。路徑平滑成本是路徑曲率導(dǎo)數(shù)平方。通過對比實驗，在克服了圓弧曲線連接曲率不連續(xù)的基礎(chǔ)上，可知機器人的運動要比沿回旋曲線平滑，路徑長度也相應(yīng)降低，證明三次螺線在路徑平滑上是不錯的選擇方法。

[1]孫迪生, 王 炎. 機器人控制技術(shù)[M]. 北京: 機械工業(yè)出版社, 1997. 6-10.

[2]Park M G, Jeon J H, Lee M C. Obstacle avoidance for mobile robots using artificial potential field approach with simulated annealing [C]//ISIE. Pusan,Korea, 2001: 1530-1535.

[3]Faisal S, Raja Q S. Path finder in unknown environment [C]//IEEE International Conf. on Emerging Technologies. Peshawar, Pakistan, 2006:575-580.

[4]唐振民, 趙春霞, 楊靜宇, 等. 地面自主平臺的局部路徑規(guī)劃[J]. 機器人, 2001, 23(7): 742-745.

[5]趙春霞, 唐振民, 陸建峰, 等. 面向自主車輛的局部路徑規(guī)劃仿真系統(tǒng)[J].南京理工大學(xué)學(xué)報,2002, 26(6): 570-574.

[6]Kamon I, Rivlin E, Rimon E. A new range-sensor based globally convergent navigation algorithm for mobile robots [C]//Proceedings of IEEE International Conf. on Robotics and Automation.Minneapolis, MN: IEEE Computer Society Press,1996: 22-28.

[7]Kanayama Y, Hartman B I. Smooth local path planning for autonomous vehicle [C]//Proceedings of Int’l Conf on Robotics and Automation. IEEE Publish Society. Scottsdale, Arizona, 1989:1265-1270.

[8]Kamon 1, Rivlin E. Sensory based motion planning with global proofs [C]//IROS'95. Pittsburgh, PA,USA, 1995: 435-440.

[9]Kamon I, Rivlin E, Rimon E. A new range-sensor based globally convergent navigation algorithm for mobile robots [C]//Proceeding of the 1996 IEEE International Conf. on Robotics and Automation.Minnesota, 1996(4): 429-435.

[10]Boissonnat J D, Cerezo A, Leblond J. Shortest paths of bounded curvature in the plane [C]//Proceedings of the IEEE International Conf. on Robotics and Automation. Piscataway, NJ, USA: IEEE, 1992:2315-2320.

[11]席裕庚. 動態(tài)不確定環(huán)境下廣義控制問題的預(yù)測控制[J]. 控制理論與應(yīng)用, 2007, 17(5): 665-670.