康 亮， 趙春霞， 郭劍輝

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

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

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

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

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

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

1 Bug算法

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

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

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

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

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

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

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

1.2 邊線沿走模式

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

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

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

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

2 三次螺線

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

2.1 相關(guān)定義

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

定義p1=(x1,y1,θ1)，p2= (x2,y2,θ2)是移動(dòng)機(jī)器人的兩個(gè)路徑位姿節(jié)點(diǎn)，此時(shí)(x1,y1)≠ (x2,y2)。從點(diǎn)(x1,y1)到點(diǎn)(x2,y2)的方向β為

如果以下關(guān)系滿足，則兩個(gè)位姿p1和p2被稱(chēng)為對(duì)稱(chēng)。

如果兩個(gè)位姿p1和p2對(duì)稱(chēng)，記為sym(p1,p2)，否則記為 ～sym(p1,p2)。

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

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

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

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

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

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

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

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

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

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

2.2 位姿對(duì)稱(chēng)

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

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

這里的A、B、C、D是積分常數(shù)。因?yàn)樯鲜矫枋龅姆较蚝瘮?shù)θ是三次方函數(shù)，所以稱(chēng)這類(lèi)路徑曲線為三次螺線，其曲率是連續(xù)的。

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

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

2.3 位姿非對(duì)稱(chēng)

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

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

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

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

當(dāng)(p1,p2)不是平行時(shí)，切分點(diǎn)q位于方程（5）曲線中，使成本總和 cost(Π (p1,q))+cost(Π (q,p2))是最小。因?yàn)闆](méi)有解析方法，這里采用Newton-Raphson迭代算法來(lái)尋找最小值。

3 三次螺線Bug算法

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

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

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

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

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

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

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

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

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

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

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

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

（1） 機(jī)器人到達(dá)目標(biāo)點(diǎn)，算法停止；

（2） 機(jī)器人探測(cè)到障礙物阻擋前進(jìn)路徑，切換到“邊線沿走”模式。

3.3 邊線沿走模式

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

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

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

圖2 探測(cè)到障礙物

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

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

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

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

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

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

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

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

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

這里有一個(gè)對(duì)比實(shí)驗(yàn)。當(dāng)取路徑平滑成本函數(shù)為路徑節(jié)點(diǎn)曲率的平方時(shí)，即

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

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

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

圖3 方向偏差為π時(shí)的曲線連接

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

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

表1 算法性能對(duì)照表

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

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

5 收斂性和完備性證明

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

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

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

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

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

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

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

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

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

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

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

6 結(jié) 論

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

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

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

[1]孫迪生, 王 炎. 機(jī)器人控制技術(shù)[M]. 北京: 機(jī)械工業(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]唐振民, 趙春霞, 楊靜宇, 等. 地面自主平臺(tái)的局部路徑規(guī)劃[J]. 機(jī)器人, 2001, 23(7): 742-745.

[5]趙春霞, 唐振民, 陸建峰, 等. 面向自主車(chē)輛的局部路徑規(guī)劃仿真系統(tǒng)[J].南京理工大學(xué)學(xué)報(bào),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]席裕庚. 動(dòng)態(tài)不確定環(huán)境下廣義控制問(wèn)題的預(yù)測(cè)控制[J]. 控制理論與應(yīng)用, 2007, 17(5): 665-670.