個(gè)體機(jī)器人局部路徑規(guī)劃的人工力矩方法

徐 望 寶， 陳 雪 波， 趙 杰

(1.大連理工大學(xué) 電子信息與電氣工程學(xué)部，遼寧 大連 116024；2.遼寧科技大學(xué) 電子與信息工程學(xué)院，遼寧 鞍山114051；3.哈爾濱工業(yè)大學(xué) 機(jī)器人技術(shù)與系統(tǒng)國(guó)家重點(diǎn)實(shí)驗(yàn)室，黑龍江 哈爾濱150001)

0 引 言

二維障礙平面中個(gè)體自主移動(dòng)機(jī)器人的局部路徑規(guī)劃問(wèn)題，是機(jī)器人學(xué)中的一個(gè)重要問(wèn)題.其目的就是要求一個(gè)對(duì)環(huán)境無(wú)任何先驗(yàn)知識(shí)的機(jī)器人，僅依靠其傳感器的實(shí)時(shí)探測(cè)和運(yùn)動(dòng)過(guò)程中積累的環(huán)境知識(shí)，規(guī)劃出一條從起點(diǎn)位置到目標(biāo)位置的無(wú)碰較優(yōu)路徑，或證明不存在這樣的路徑.

針對(duì)上述問(wèn)題，目前人們已提出了較多的解決方法，其中比較重要的有“沿墻走”方法或“Bug”方法[1～3]、人工勢(shì)場(chǎng)法[4～6]等.“Bug”方法的基本原理是:機(jī)器人總試圖直接向著目標(biāo)點(diǎn)運(yùn)動(dòng)；在遇到障礙時(shí)，才沿著障礙往前走直到確定已繞過(guò)了障礙.“Bug”方法有直觀、簡(jiǎn)單、在一定條件下能保證收斂的優(yōu)點(diǎn)；缺點(diǎn)是未曾考慮運(yùn)動(dòng)路徑的長(zhǎng)度，即路徑優(yōu)化問(wèn)題.人工勢(shì)場(chǎng)法的基本原理是:機(jī)器人活動(dòng)在一個(gè)人工力場(chǎng)中，其中目標(biāo)對(duì)機(jī)器人有一個(gè)吸引力，障礙對(duì)機(jī)器人有一個(gè)排斥力；在合力作用下，機(jī)器人沿使合力減小的方向運(yùn)動(dòng).人工勢(shì)場(chǎng)法有實(shí)時(shí)性強(qiáng)、計(jì)算簡(jiǎn)單、實(shí)現(xiàn)容易等優(yōu)點(diǎn)；缺點(diǎn)除了未曾考慮路徑優(yōu)化問(wèn)題外，還存在一個(gè)比較難以解決的“局部極小點(diǎn)”問(wèn)題[2、6].

在滿足約束的條件下，機(jī)器人的運(yùn)動(dòng)路徑越短越好.所以在局部路徑規(guī)劃問(wèn)題中，也應(yīng)考慮路徑優(yōu)化問(wèn)題，即利用現(xiàn)有的環(huán)境知識(shí)，得到一條盡可能短的運(yùn)動(dòng)路徑.基于公路圖的方法，如可視圖方法[7]、切 線 圖 方 法[8]、Voronoi圖 方 法[9、10]等，是注重路徑優(yōu)化的方法.這類方法的基本原理是:首先，機(jī)器人要得到一個(gè)由可行路徑組成的公路圖，該圖必包含機(jī)器人當(dāng)前位置與目標(biāo)間的最短可行路徑；然后，運(yùn)用某種技術(shù)如Dijkstra算法或圖理論[9]，機(jī)器人從該公路圖中選出最短可行路徑并沿該路徑運(yùn)動(dòng).這類方法的主要優(yōu)點(diǎn)是常能得到最優(yōu)可行路徑；缺點(diǎn)是在局部路徑規(guī)劃問(wèn)題中，由于環(huán)境知識(shí)不斷改變，需要經(jīng)常重新計(jì)算公路圖及最短可行路徑，計(jì)算量很大，難以在局部路徑規(guī)劃問(wèn)題中使用.

本文首先將公路圖方法中的有關(guān)概念進(jìn)行推廣，提出廣義可行路徑和最優(yōu)路徑代表點(diǎn)(Powr)等概念.最優(yōu)路徑代表點(diǎn)，除在優(yōu)化路徑時(shí)，有與最短廣義可行路徑相同的效果外，還有存儲(chǔ)量小，不需要像公路圖方法一樣考慮機(jī)器人的半徑與安全距離[8]，也不需要對(duì)環(huán)境做預(yù)處理等優(yōu)點(diǎn).然后將文獻(xiàn)[11、12]提出的用于多機(jī)器人隊(duì)形控制的人工力矩方法進(jìn)行推廣，設(shè)計(jì)在通常情況下會(huì)努力使機(jī)器人基本運(yùn)動(dòng)方向線(PMDline)指向Powr的新吸引矩函數(shù)和總努力使機(jī)器人的PMDline背向相應(yīng)危墻代表點(diǎn)的新排斥矩函數(shù).基于上述兩種人工力矩，設(shè)計(jì)機(jī)器人運(yùn)動(dòng)控制器.當(dāng)機(jī)器人與目標(biāo)間的線段被障礙阻斷時(shí)，該控制器會(huì)使機(jī)器人沿PMDline以最大步幅運(yùn)動(dòng)，從而即使面臨復(fù)雜的障礙，機(jī)器人也不會(huì)停止運(yùn)動(dòng)，即不會(huì)被陷住.所以基于人工力矩規(guī)劃?rùn)C(jī)器人路徑不會(huì)產(chǎn)生類似“局部極小點(diǎn)”的問(wèn)題.

本文還給出最優(yōu)路徑代表點(diǎn)的求解方法、個(gè)體機(jī)器人局部路徑規(guī)劃的基本步驟及可達(dá)性證明.

1 系統(tǒng)模型與相關(guān)定義

本文僅討論二維平面中個(gè)體機(jī)器人的局部路徑規(guī)劃問(wèn)題，其中工作環(huán)境中分布有靜態(tài)的多邊形障礙；機(jī)器人對(duì)環(huán)境無(wú)任何先驗(yàn)知識(shí)，要求僅依靠傳感器的實(shí)時(shí)探測(cè)和運(yùn)動(dòng)過(guò)程中積累的環(huán)境知識(shí)來(lái)規(guī)劃一條能安全到達(dá)目標(biāo)的路徑；到達(dá)目標(biāo)后，機(jī)器人還要盡可能地具有給定的位姿.

在討論問(wèn)題的解決方法前，首先規(guī)定文中使用的一些符號(hào)和概念，其中一有向線(射線或有向線段)到另一有向線的角，及有向線方向角的定義與文獻(xiàn)[11]相同，即將一有向線繞其端點(diǎn)旋轉(zhuǎn)到與全局坐標(biāo)系中橫坐標(biāo)軸方向相同時(shí)所轉(zhuǎn)的絕對(duì)值不大于π的有向角就是該有向線的方向角.設(shè)βi、βj分別是有向線li、lj的方向角，函數(shù)

那么β＝agl(βi－βj)就是從li到lj的角.

定義Time＝｛tk｜k＝0，1，…｝為一離散時(shí)間點(diǎn)集.任意變量X在tk時(shí)刻的值用X(k)表示；如果一變量值的時(shí)間沒(méi)有指定，則為當(dāng)前時(shí)刻；如果X是一條連續(xù)曲線段，則｜X｜是它的長(zhǎng)度.AB表示以A、B為端點(diǎn)的線段；Sd(A，B)表示以A為起點(diǎn)、B為終點(diǎn)的有向線段，β(A，B)為Sd(A，B)的方向角；Lr(A，B)表示以A為起點(diǎn)、經(jīng)過(guò)B的射線. (W，－A1，…，－An)表示連續(xù)曲線W上除A1，…，An外的點(diǎn)； (Lr(A1，A2)，－A1A2)表示Lr(A1，A2)上除A1A2外的所有點(diǎn).

R表示機(jī)器人；SR表示R的最大運(yùn)動(dòng)步幅；Ds是一個(gè)與機(jī)器人的安全相關(guān)的正常數(shù).R的模型如圖1所示，其形狀是一個(gè)半徑為DR(Ds＞2SR＋2DR)的圓.R的位置(圓心位置)記為PR，坐標(biāo)記為(xR，yR).像文獻(xiàn)[11、12]中的機(jī)器人模型一樣，R有且只有一條以其所在位置為端點(diǎn)的基本運(yùn)動(dòng)方向線(PMDline).該P(yáng)MDline是一條以PR為端點(diǎn)、βR為方向角的射線，主要用來(lái)指示R的位姿或主要運(yùn)動(dòng)方向.

圖1 相關(guān)模型Fig.1 Associated models

R有一個(gè)全方位傳感器，其有效半徑為Dv.對(duì)于工作空間中的點(diǎn)F，只有當(dāng)且PRF不經(jīng)過(guò)任何障礙時(shí)，R才能探測(cè)到F，否則探測(cè)不到F.如在圖1中，R在當(dāng)前時(shí)刻只能探測(cè)到灰色區(qū)域內(nèi)及其邊界上的點(diǎn)，該區(qū)域外的點(diǎn)如A1(A1在初始時(shí)刻能被R探測(cè)到)、A5就不能被探測(cè)到.

目標(biāo)記為T(mén)，其模型為一個(gè)靜態(tài)的質(zhì)點(diǎn)，如圖1所示，坐標(biāo)記為(xT，yT).T有且只有一條以其位置為端點(diǎn)、βT為方向角的PMDline，用來(lái)指示其位姿.文中要求R到達(dá)目標(biāo)后盡可能地具有給定的位姿，即要求R到達(dá)目標(biāo)后，盡可能地小.

假設(shè)1 在任意時(shí)刻，R通過(guò)通信等方式，總能知道T的位置及PMDline的方向角.

假設(shè)2 任意兩障礙間的最近距離都不小于Ds；R與T間至少存在一條寬度不小于Ds的路徑.

定義1 設(shè)Σ是障礙物邊緣線上一段連續(xù)的曲線.如果Σ上不含從未被R探測(cè)過(guò)的點(diǎn)，且對(duì)于任意的與Σ在同一個(gè)障礙物邊緣曲線上的曲線段Σ＊，Σ＊Σ就意味著Σ＊上至少有一點(diǎn)從未被R探測(cè)到，那么稱Σ為知識(shí)障礙墻.

如在圖1中，折線段A1A2A3A4就是一條知識(shí)障礙墻；而折線段A1A2A3A4A5不是，因?yàn)樯厦嬗行c(diǎn)，如A5，從來(lái)沒(méi)有被R探測(cè)到.

顯然，知識(shí)障礙墻是環(huán)境知識(shí)積累的結(jié)果.R會(huì)記下所有的知識(shí)障礙墻并及時(shí)更新，以方便應(yīng)用.

定義2 設(shè)W是一條以F1、F2為端點(diǎn)的連續(xù)曲線，S是所有以F1、F2為端點(diǎn)，上面的點(diǎn)或在W上或無(wú)限地靠近W的連續(xù)曲線組成的集合.①如果S中存在一個(gè)元素W＊，使得集合 (W＊，－F1，－F2)中不含任何知識(shí)障礙墻上的點(diǎn)，那么W就是一條以F1、F2為端點(diǎn)的廣義可行路徑.②如果存在一條知識(shí)障礙墻Σ，使得S中的每一個(gè)元素，都至少與Σ有一個(gè)公共點(diǎn)，那么就說(shuō)W被Σ阻斷.③如果一條射線上面的任何一條線段都沒(méi)有被Σ阻斷，那么該射線沒(méi)有被Σ阻斷，否則它就被Σ阻斷.

如在圖1中，折線段PRA4T和PRA2A1A3T都是以PR、T為端點(diǎn)的廣義可行路徑；而折線段PRA2A3T就不是，因?yàn)樗恢R(shí)障礙墻A1A2A3A4阻斷.

廣義可行路徑與完全可行路徑密切相關(guān)，因?yàn)樵谝粭l廣義可行路徑的附近一定存在一條完全可行路徑.但由于廣義可行路徑上可能存在障礙點(diǎn)，R的模型具有幾何形狀且R在運(yùn)動(dòng)過(guò)程中要與障礙保持一定的安全距離，所以廣義可行路徑又不一定完全可行.

定義3 設(shè)F是T或知識(shí)障礙墻上的一個(gè)點(diǎn)，如果PRF位于以PR、T為端點(diǎn)的最短廣義可行路徑上，且F≠T就意味著對(duì)任意的滿足A∈ (Lr(PR，F(xiàn))，－PRF)且｜FA｜＜Ds的點(diǎn)A，集合 (FA，－F)中都不含知識(shí)障礙墻的點(diǎn)，那么稱F為最優(yōu)路徑代表點(diǎn)，記為Powr.

例如，當(dāng)PRT不被任何知識(shí)障礙墻阻斷時(shí)，PRT就是以PR、T為端點(diǎn)的最短廣義可行路徑，此時(shí)T就是Powr.

概念“最優(yōu)路徑代表點(diǎn)”含有兩層意思:一是指Powr能夠代表以PR、T為端點(diǎn)的最短／最優(yōu)廣義可行路徑，因?yàn)橹苯酉蛑鳳owr運(yùn)動(dòng)就會(huì)沿著該路徑運(yùn)動(dòng)；二是指在所有能代表該路徑的點(diǎn)中，Powr幾乎是最優(yōu)的，因?yàn)樗鼛缀跏撬屑任挥谠撀窂缴?，同時(shí)與PR的連線段又不被任何知識(shí)障礙墻阻斷的點(diǎn)中，距PR最遠(yuǎn)的點(diǎn).

對(duì)于路徑優(yōu)化，Powr與以PR、T為端點(diǎn)的最短廣義可行路徑的作用完全相同；但顯然Powr需要的存儲(chǔ)量要少得多.同時(shí)Powr也不需要像完全可行路徑那樣考慮機(jī)器人的大小和安全距離，也不需要預(yù)處理環(huán)境，計(jì)算容易，所以在本文中，R主要是在Powr的引導(dǎo)下向著T運(yùn)動(dòng).

定義4 設(shè)G是知識(shí)障礙墻Σ上能被R探測(cè)且到PR的距離不大于Ds的點(diǎn)，如果存在一個(gè)正常數(shù)δ，使得對(duì)任意的滿足在Σ上且｜AG｜＜δ的點(diǎn)A，都有｜PRA｜＞｜PRG｜，那么稱G為危墻代表點(diǎn).

顯然，危墻代表點(diǎn)就是對(duì)R具有安全威脅的知識(shí)障礙墻上的局部最近點(diǎn).由于僅是局部最近點(diǎn)，同一知識(shí)障礙墻上的危墻代表點(diǎn)可能不只一個(gè)，如在圖1中的當(dāng)前時(shí)刻，在知識(shí)障礙墻 ——折線段A1A2A3A4上就有兩個(gè)危墻代表點(diǎn)G1、G2.危墻代表點(diǎn)的主要作用是防止R向其所在的區(qū)域靠近，所以同一知識(shí)障礙墻上有多個(gè)危墻代表點(diǎn)的特點(diǎn)可以防止機(jī)器人在避開(kāi)該墻的一側(cè)后，又撞上該墻的另一側(cè).

2 基于人工力矩的機(jī)器人運(yùn)動(dòng)控制器

人工力矩概念由文獻(xiàn)[11、12]等根據(jù)物理學(xué)中力的大小與方向共同決定力矩的基本思想而提出，其與人工勢(shì)場(chǎng)力的基本區(qū)別是:人工力矩的大小不但與機(jī)器人和目標(biāo)、障礙間的遠(yuǎn)近等這些在人工力場(chǎng)中通常被定義成吸引力或排斥力的距離有關(guān)，還與機(jī)器人的PMDline等相關(guān)有向線的方向有關(guān)，即還與“力的方向”有關(guān).相對(duì)于人工勢(shì)場(chǎng)力的主要優(yōu)點(diǎn)是:人工勢(shì)場(chǎng)力只能影響機(jī)器人的位置，而人工力矩可以根據(jù)系統(tǒng)需要，同時(shí)影響機(jī)器人的位置和PMDline方向或只影響其中的一個(gè)，從而為設(shè)計(jì)帶來(lái)了更多的便利.

例如，本文設(shè)計(jì)的用于路徑規(guī)劃的吸引矩和排斥矩，其中吸引矩常在R的PMDline指向Powr時(shí)取得最大值，即通常只影響R的PMDline；排斥矩則總在R的PMDline背離相應(yīng)危墻代表點(diǎn)時(shí)取得最大值，即僅影響R的PMDline.基于上述兩種人工力矩函數(shù)設(shè)計(jì)的用于個(gè)體機(jī)器人路徑規(guī)劃的運(yùn)動(dòng)控制器，使R在大部分情況下，一方面沿合力矩增大的方向調(diào)整PMDline方向，另一方面則沿PMDline以最大步幅運(yùn)動(dòng).即使在復(fù)雜的環(huán)境中，R也不會(huì)停止運(yùn)動(dòng)，即不會(huì)被陷住.所以基于人工力矩規(guī)劃?rùn)C(jī)器人路徑，不會(huì)產(chǎn)生類似人工勢(shì)場(chǎng)法中的“局部極小點(diǎn)”問(wèn)題.

吸引矩與排斥矩函數(shù)具體設(shè)計(jì)如下.設(shè)當(dāng)前時(shí)刻為tk，δθ為遠(yuǎn)小于π／2的正常數(shù)，函數(shù)

則Powr對(duì)R的吸引矩Ma(k)設(shè)計(jì)如下:

(1)如果Powr不是目標(biāo)T或到PR的距離不小于Ds，則

其中βRP(k)是Sd(PR，Powr)的方向角.λa是一常數(shù)，本文的取值方法是:如果tk時(shí)刻有一危墻代表點(diǎn)G，使得Sd(PR，Powr)到Sd(PR，G)的角的絕對(duì)值小于π／2，則λa＝0.5；否則λa＝0.8.按上述方法取λa的理由是:如果Sd(PR，Powr)到Sd(PR，G)的角的絕對(duì)值小于π／2，則向著Powr運(yùn)動(dòng)就有向著G運(yùn)動(dòng)的趨勢(shì)，在此情況下吸引矩太大會(huì)使R的PMDline在吸引矩的作用下向障礙物方向偏轉(zhuǎn)太多，從而使R的運(yùn)動(dòng)在障礙物前產(chǎn)生振蕩.否則吸引矩大可以加快R趨向Powr的速度.

(2)如果Powr是目標(biāo)T且到PR的距離小于Ds，則

設(shè)G是tk時(shí)刻的一個(gè)危墻代表點(diǎn)，βGR(k)是Sd(G，PR)的方向角，DGR＝｜Sd(G，PR)｜，λp＝，則G對(duì)R的排斥矩Mp(k)設(shè)計(jì)為

基于人工力矩的機(jī)器人運(yùn)動(dòng)控制器設(shè)計(jì)為

其中有關(guān)符號(hào)的意義和計(jì)算方法如下:

(1)(S(k)S(k))T是R在 時(shí) 間 段

xy(tktk＋1]內(nèi)沿其 PMDline的運(yùn)動(dòng)量.計(jì)算方法是:如果Powr是T且到PR的距離小于Ds，則(Sx(k)Sy(k))T＝ (0 0)T；否則R將以最大步幅沿PMDline方向運(yùn)動(dòng)，即

(2)(Δβ(k)Δx(k)Δy(k))T是能增加

1R1R1RR吸引矩的變化量，是吸引矩的梯度方向.設(shè)函數(shù)

則據(jù)式(3)、(4)得

①如果Powr不是目標(biāo)T或到PR的距離不小于Ds，則

②如果Powr是目標(biāo)T且到PR的距離小于Ds，則

(3)當(dāng)R沒(méi)有危墻代表點(diǎn)時(shí)，∑Δ2βR(k)＝0.否則∑Δ2βR(k)是所有危墻代表點(diǎn)排斥矩梯度方向的和.設(shè)Δ2βR(k)是危墻代表點(diǎn)G對(duì)R排斥矩的梯度方向，則由式(5)得

從式(6)～(12)可以看出，基于人工力矩的運(yùn)動(dòng)控制器，使得R的運(yùn)動(dòng)速度，不會(huì)因?yàn)槲睾团懦饩氐南嗷サ窒档?，因?yàn)榕懦饩夭挥绊憴C(jī)器人的運(yùn)動(dòng)速度.這一特點(diǎn)，不僅使得R不至于陷在某一局部極值點(diǎn)而不能自拔，即在人工力矩法中，不會(huì)出現(xiàn)類似“局部極小點(diǎn)”的問(wèn)題；同時(shí)也增加了機(jī)器人的平均運(yùn)動(dòng)速度，從而加快了收斂速度.

將本文的人工力矩函數(shù)和運(yùn)動(dòng)控制器與文獻(xiàn)[11、12]中相應(yīng)的人工力矩函數(shù)和運(yùn)動(dòng)控制器相比可以發(fā)現(xiàn)，本文的人工力矩函數(shù)和運(yùn)動(dòng)控制器要簡(jiǎn)單得多，原理也更易懂.

盡管運(yùn)動(dòng)控制器(6)有許多優(yōu)點(diǎn)，但如果存在一個(gè)危墻代表點(diǎn)G，滿足

那么直接運(yùn)用該運(yùn)動(dòng)控制器，則R就有可能不趨近Powr.

R不趨近Powr的原因，是因?yàn)椴坏仁?13)意味著R的 PMDline到Sd(PR，Powr)的角與到Sd(G，PR)的角的符號(hào)相反，如圖2所示，因此相應(yīng)吸引矩與排斥矩的作用就可能相互抵消，R就可能繼續(xù)沿原來(lái)PMDline的方向運(yùn)動(dòng)；如果R原來(lái)的PMDline不指向甚或背向Powr，那么在此情況下，R的運(yùn)動(dòng)就會(huì)不趨近Powr，或雖然趨近，但速度緩慢.

圖2 R的PMDline背向PowrFig.2 R′s PMDline opposite to the Powr

改進(jìn)方法就是在運(yùn)用如式(6)所述的運(yùn)動(dòng)控制器前，先檢查所有的危墻代表點(diǎn)，判斷式(13)是否成立.如果都不成立，則直接運(yùn)用；否則先令βR(k)＝β(PR，Powr)，即令R的 PMDline指向Powr，然后再運(yùn)用前述控制器.經(jīng)過(guò)上述改進(jìn)后發(fā)現(xiàn)，在當(dāng)Sd(PR，Powr)不被知識(shí)障礙墻阻斷時(shí)，基于人工力矩的運(yùn)動(dòng)控制器總能使R逐漸趨近Powr.

3 最優(yōu)路徑代表點(diǎn)求解及局部路徑規(guī)劃的基本步驟

首先探討Powr的求解方法，因?yàn)槿绻磺蟮肞owr，則基于人工力矩的運(yùn)動(dòng)控制器就無(wú)法進(jìn)行，路徑優(yōu)化也無(wú)從談起.為了方便論述Powr的求解，下面給出兩個(gè)新概念.

定義5 設(shè)F是廣義可行路徑上的一個(gè)點(diǎn)，A是知識(shí)障礙墻Σ上的一個(gè)點(diǎn).(1)如果 (Lr(F，A)，－FA)∩Σ＝且Lr(F，A)不被Σ阻斷，那么稱Lr(F，A)為從F到Σ的單向切線，A為切點(diǎn).(2)當(dāng) (Lr(F，A)，－FA)∩Σ不為空時(shí)，設(shè)A＊為其中距A最近的點(diǎn).如果AA＊與Σ的一部分能形成一條將F和T分開(kāi)的閉合曲線，那么Lr(F，A)為從F到Σ的閉合切線，A為切點(diǎn)，A＊為閉合點(diǎn).

如在圖3中，Lr(PR，A4)是從PR到Σ1的單向切線，A4為切點(diǎn)；Lr(PR，A1)是從PR到Σ1的閉合切 線，A1為 切 點(diǎn)、F1為 閉 合 點(diǎn)；Lr(A1，A2)和Lr(A2，A3)則分別是從A1、A2到Σ1的閉合切線.

為了簡(jiǎn)化Powr的求解，在運(yùn)動(dòng)過(guò)程中，R還會(huì)按照下述規(guī)則在環(huán)境中設(shè)置人工障礙線:(1)當(dāng)PRT同時(shí)被知識(shí)障礙墻Σ1、Σ2阻斷時(shí)，設(shè)A1、A2分別為PRT與Σ1、Σ2的交點(diǎn)，那么A1A2將被設(shè)置為一條人工障礙線；(2)當(dāng)PRT僅被Σ1阻斷時(shí)，設(shè)A1為從PR到Σ1的單向或閉合切線的切點(diǎn).如果PRA1被Σ2阻斷，那么設(shè)A2為PRA1與Σ2交點(diǎn)中，距A1最近的點(diǎn)，則A1A2將被設(shè)置為一條人工障礙線.

圖3 單向切線、閉合切線與人工障礙線Fig.3 Single－direction tangents，closed tangents and artificial obstacle segments

如在圖3中，V1V2就是R設(shè)置的人工障礙線.設(shè)置人工障礙線的主要根據(jù)是“沿墻走”方法中盡量不讓機(jī)器人往回走的原則.因?yàn)槿绻鸄1A2是當(dāng)前時(shí)刻設(shè)置的人工障礙線，則根據(jù)設(shè)置方法知，A1A2在當(dāng)前時(shí)刻不能被R探測(cè)，但由于A1、A2都在知識(shí)障礙墻上，所以A1A2在過(guò)去一定可以被R探測(cè)到.上述分析說(shuō)明了R從曾經(jīng)距A1A2較近的位置運(yùn)動(dòng)到了現(xiàn)在距A1A2較遠(yuǎn)的位置.所以把A1A2設(shè)置成人工障礙線，防止R向A1A2運(yùn)動(dòng)就可以防止R往回走，從而在一定程度上可以保證全局收斂性.

雖然通常情況下，人工障礙線被當(dāng)作知識(shí)障礙墻的一部分處理，但如果人工障礙線的設(shè)置導(dǎo)致形成一條能將R、T分開(kāi)的閉合知識(shí)障礙墻，則該墻上的所有人工障礙線都會(huì)被撤除.

結(jié)論1 如果R在當(dāng)前時(shí)刻不能設(shè)置人工障礙線，那么，(1)PRT最多被一條知識(shí)障礙墻阻斷；(2)設(shè)PRT被知識(shí)障礙墻Σ阻斷，A1為從PR到Σ的單向或閉合切線的切點(diǎn)，那么PRA1不會(huì)被任何知識(shí)障礙墻阻斷.

結(jié)論2 如果PRT被知識(shí)障礙墻Σ阻斷且從PR到Σ有一條閉合切線，那么該閉合切線的切點(diǎn)為Powr.

結(jié)論3 當(dāng)PRT被知識(shí)障礙墻Σ阻斷，從PR到Σ有兩條單向切線時(shí)，設(shè)兩單向切線的切點(diǎn)分別是A1、Q1，那么根據(jù)下面算法1可以得到一條記為WA1的路徑PRA1A2…AnT.

算法1 (求路徑WA1)

Step 1 令A(yù)i＝A1，A0＝PR.

Step 2 如果AiT沒(méi)有被Σ阻斷，那么停止運(yùn)算，Ai、T為WA1上的最后兩個(gè)節(jié)點(diǎn)；否則轉(zhuǎn)Step 3.

Step 3 如果R有一條從Ai到Σ的閉合切線，則Ai＋1就是該閉合切線的切點(diǎn)；令A(yù)i＝Ai＋1，然后返回Step 2.否則轉(zhuǎn)Step 4.

Step 4 如果Ai＝A1，那么A2就是從A1到Σ滿足如下條件單向切線的切點(diǎn):該切線與Lr(PR，Q1)沒(méi)有交點(diǎn).否則Ai＋1就是從Ai到Σ滿足如下條件單向切線的切點(diǎn):該切線與Lr(Ai－2，Ai－1)沒(méi)有交點(diǎn).令A(yù)i＝Ai＋1，然后返回Step 2.

結(jié)論4 當(dāng)PRT被知識(shí)障礙墻Σ阻斷且從PR到Σ沒(méi)有閉合切線時(shí)，則(1)R有兩條從PR到Σ的單向切線；(2)設(shè)兩條單向切線的切點(diǎn)分別是A1、Q1，那么如果WA1、WQ1都是廣義可行路徑，則其中有一條必是以PR、T為端點(diǎn)的最短廣義可行路徑；Powr必是A1、Q1中的一個(gè).

綜合前面的討論，得到了一個(gè)計(jì)算Powr的算法，即算法2.

算法2 (計(jì)算Powr的近似算法)

Step 1 如果PRT未被知識(shí)障礙墻阻斷，那么T就是Powr；否則轉(zhuǎn)Step 2.

Step 2 設(shè)PRT被知識(shí)障礙墻Σ阻斷.如果從PR到Σ有一條閉合切線，則該閉合切線的切點(diǎn)為Powr；否則轉(zhuǎn)Step3.

Step 3 計(jì)算出從PR到Σ的兩條單向切線的切點(diǎn)，設(shè)分別是A1、Q1.

Step 4 運(yùn)用算法1，分別得到WA1、WQ1；然后計(jì)算出｜WA1｜、｜WQ1｜.

Step 5 如果｜WA1｜＜｜WQ1｜，則A1是Powr；否則Q1是Powr.

據(jù)算法2得到的Powr不一定位于以PR、T為端點(diǎn)的最短廣義可行路徑上，原因是WA1、WQ1不一定是以PR、T為端點(diǎn)的廣義可行路徑，不一定滿足結(jié)論3的條件，所以算法2只是一個(gè)近似算法.但據(jù)算法2得到的解，對(duì)路徑仍有顯著的優(yōu)化效果.

算法2在最壞情況下的計(jì)算量，就是要分別得到WA1、WQ1.設(shè)知識(shí)障礙墻中邊數(shù)最多知識(shí)障礙墻的節(jié)點(diǎn)數(shù)為m，則根據(jù)算法1知:最多經(jīng)過(guò)m步的計(jì)算，就可以得到WA1／WQ1；所以算法2的時(shí)間復(fù)雜度為O(2m).

綜合算法2和前面關(guān)于機(jī)器人運(yùn)動(dòng)控制器的討論，得到了算法3.

算法3 (個(gè)體機(jī)器人局部路徑的規(guī)劃)

Step 1 給定系統(tǒng)參數(shù)值；對(duì)R、T及環(huán)境初始化；令tk＝t0.

Step 2 判斷當(dāng)前時(shí)刻能否設(shè)置人工障礙線.如果能夠，則設(shè)置好人工障礙線.

Step 3 調(diào)用算法2，得到Powr；同時(shí)得到所有危墻代表點(diǎn).

Step 4 對(duì)每一個(gè)危墻代表點(diǎn)，判斷式(13)是否成立；一旦式(13)成立，即令βR(k)＝β(PR，Powr).

Step 5 在基于人工力矩運(yùn)動(dòng)控制器的控制下，R運(yùn)動(dòng)一步到達(dá)下個(gè)時(shí)刻的位置.

Step 6 如果R還沒(méi)有到達(dá)T，則更新知識(shí)障礙墻集，然后轉(zhuǎn)Step 2.如此循環(huán)，直到到達(dá)T，結(jié)束程序.

定理1 如果運(yùn)用算法3規(guī)劃個(gè)體機(jī)器人的路徑，那么在本文所給條件下，R能最終到達(dá)T，即系統(tǒng)具有可達(dá)性.

證明 根據(jù)算法3和前面的討論，可以得到如下結(jié)論:如果tk時(shí)刻沒(méi)有探測(cè)到新的障礙點(diǎn)，沒(méi)有也不能設(shè)置人工障礙線，那么在不考慮那些不能阻斷PR(k－1)T與PR(k)T的知識(shí)障礙墻的條件下，以PR(k)、T為端點(diǎn)的最短廣義可行路徑一定比以PR(k－1)、T為端點(diǎn)的最短廣義可行路徑短.得到上述結(jié)論的理由是R會(huì)逐漸靠近Powr，從而在環(huán)境沒(méi)有變化的情況下，相應(yīng)的最短廣義可行路徑會(huì)縮短.

根據(jù)上述結(jié)論可以推出，在最壞情況下:當(dāng)所有可能被探測(cè)到的障礙點(diǎn)全都被探測(cè)，所有的知識(shí)障礙墻都通過(guò)人工障礙線連成了一條，那么環(huán)境就不可能再變化，以PR、T為端點(diǎn)的最短廣義可行路徑就會(huì)逐漸縮短并最終為零，即R能最終到達(dá)T，所以系統(tǒng)具有可達(dá)性.證畢.

4 仿真結(jié)果與分析

下面給出一個(gè)比較典型的仿真，以證明本文所給方法的可行性與有效性.在該仿真中，系統(tǒng)參數(shù)值如表1所示；R不具備任何先驗(yàn)的環(huán)境信息，要求它僅依靠傳感器的實(shí)時(shí)探測(cè)和運(yùn)動(dòng)過(guò)程中積累的環(huán)境知識(shí)，運(yùn)用本文所提方法來(lái)規(guī)劃出一條到達(dá)目標(biāo)T的路徑.

表1 參數(shù)值Tab.1 Values of parameters

仿真結(jié)果如圖4所示，其中圖4(a)、(b)分別顯示了R在運(yùn)動(dòng)79步和147步后的狀態(tài).從圖4可以看出，R在障礙物前的運(yùn)動(dòng)像“沿墻走”方法一樣光滑；但顯然又不是簡(jiǎn)單的“沿墻走”.如當(dāng)R在M點(diǎn)時(shí)，發(fā)現(xiàn)如果繼續(xù)近似沿1號(hào)障礙的墻走，有可能要走更長(zhǎng)的路徑，就轉(zhuǎn)向墻的另一端運(yùn)動(dòng).如此調(diào)整顯然極大地縮短了R的運(yùn)動(dòng)路徑.同時(shí)從圖4可以看出，R安全地通過(guò)了2號(hào)與3號(hào)障礙間等狹窄通道，準(zhǔn)確地到達(dá)了距障礙很近的目標(biāo)，在整個(gè)運(yùn)動(dòng)過(guò)程中沒(méi)有出現(xiàn)被陷住的現(xiàn)象.而上述結(jié)果，是人工勢(shì)場(chǎng)法難以得到的，所以人工力矩法相對(duì)于人工勢(shì)場(chǎng)法有顯著的優(yōu)勢(shì).

圖4 復(fù)雜環(huán)境中個(gè)體機(jī)器人的路徑規(guī)劃Fig.4 Path planning of single robot in a complicated environment

5 結(jié) 語(yǔ)

利用當(dāng)前全部環(huán)境知識(shí)而得到的最優(yōu)路徑代表點(diǎn)，不但可以幫助機(jī)器人優(yōu)化路徑和最終到達(dá)目標(biāo)，且有存儲(chǔ)量小，不需要預(yù)處理環(huán)境等優(yōu)點(diǎn).最優(yōu)路徑代表點(diǎn)的近似計(jì)算方法，雖然不一定總能得到精確解，但卻有時(shí)間復(fù)雜度低、具有良好優(yōu)化效果的特點(diǎn).基于人工力矩的運(yùn)動(dòng)控制器，繼承了人工勢(shì)場(chǎng)法的實(shí)時(shí)性強(qiáng)、計(jì)算容易等優(yōu)點(diǎn)，而使機(jī)器人不會(huì)在復(fù)雜環(huán)境中被陷住，且能控制機(jī)器人準(zhǔn)確地到達(dá)距障礙很近的目標(biāo).

下一步將研究更一般環(huán)境中，最優(yōu)路徑代表點(diǎn)的計(jì)算方法及如何運(yùn)用本文所提方法解決多機(jī)器人的避碰和路徑規(guī)劃等問(wèn)題.

[1]GE S S，LAI X，MAMUM A A.Boundary following and globally convergent path using instant goals[J].IEEE Transactions on Systems，Man，and Cybernetics，Part B:Cybernetics，2005，35(2):240－254

[2]ZHU Yi， ZHANG Tao，SONG Jing－yan. An improved wall following method for escaping from local minimum in artificial potential field based path planning [C]／／ Proceedings of Joint 48th IEEE Conference on Decision and Control and 28th Chinese Control Conference.Shanghai:IEEE Press，2009:6017－6022

[3]NG J，BR UNL T.Performance comparison of bug navigation algorithms [J].Journal of Intelligent and Robotic Systems，2007，50(1):73－84

[4]RIMON E， KODITSCHEK D E. Exact robot navigation using artificial potential functions [J].IEEE Transactions on Robotics and Automation，1992，8(5):501－518

[5]GE S S，CUI Y J.New potential functions for mobile robot path planning [J].IEEE Transactions on Robotics and Automation，2000，16(5):615－620

[6]KIM Dong－h(huán)un.Escaping route method for a trap situation in local path planning [J].International Journal of Control，Automation，and Systems，2009，7(3):495－500

[7]HUANG H P，CHUNG S Y.Dynamic visibility graph for path planning[C]／／Proceedings of IEEE／RSJ International Conference on Intelligent Robots and Systems.Sendal:IEEE Press，2004:2813－2818

[8]LIU Y H，ARIMOTO S.Proposal of tangent graph and extended tangent graph for path planning of mobile robots [C]／／ Proceedings of IEEE International Conference on Robotics and Automation.Sacramento:IEEE Press，1991:312－317

[9]TAKAHASHI O， SCHILLING R J. Motion planning in a plane using generalized Voronoi diagrams [J].IEEE Transactions on Robotics and Automation，1989，5(2):143－150

[10]SUD A，ANDERSEN E，CURTIS S，etal.Realtime path planning in dynamic virtual environments using multiagent navigation graphs [J].IEEE Transactions on Visualization and Computer Graphics，2008，14(3):526－538

[11]XU Wang－bao，CHEN Xue－bo.Artificial moment method for swarm－robot formation control [J].Science in China Series F:Information Science，2008，51(10):1521－1531

[12]徐望寶，陳雪波.一種基于人工力矩的動(dòng)態(tài)隊(duì)形控制方法[J].控制理論與應(yīng)用，2009，26(11):1232－1238