復(fù)雜局部地形中的實(shí)時(shí)路徑規(guī)劃算法設(shè)計(jì)

周自維，李長(zhǎng)樂，趙 杰，徐望寶

復(fù)雜局部地形中的實(shí)時(shí)路徑規(guī)劃算法設(shè)計(jì)

周自維1，2，李長(zhǎng)樂1，趙 杰1，徐望寶2

（1.哈爾濱工業(yè)大學(xué)機(jī)器人研究所，150001哈爾濱；2.遼寧科技大學(xué)電子信息工程學(xué)院，114000遼寧鞍山）

針對(duì)復(fù)雜局部環(huán)境中機(jī)器人實(shí)時(shí)自主導(dǎo)航問題，設(shè)計(jì)了“雙向搜索多邊形構(gòu)造算法”和“基于勢(shì)場(chǎng)函數(shù)的機(jī)器人運(yùn)動(dòng)控制器”.“雙向搜索多邊形構(gòu)造算法”能夠在機(jī)器人被障礙物包圍的環(huán)境下搜索出障礙物的包圍多邊形，從而獲取基于障礙的最優(yōu)行進(jìn)路徑；“基于勢(shì)場(chǎng)函數(shù)的機(jī)器人運(yùn)動(dòng)控制器”是一個(gè)多變量控制器，輸入矢量由吸引勢(shì)場(chǎng)函數(shù)和排斥勢(shì)場(chǎng)函數(shù)組成，輸出矢量由速度和轉(zhuǎn)角組成，該控制器控制機(jī)器人實(shí)際運(yùn)動(dòng)，使機(jī)器人能夠有效躲避障礙物并逐步趨向目標(biāo)點(diǎn)；控制器還設(shè)定了機(jī)器人運(yùn)動(dòng)的基本速度，解決勢(shì)場(chǎng)為零時(shí)引起的局部極小化問題.與“沿墻走算法”、“人工勢(shì)場(chǎng)法”等方法的實(shí)驗(yàn)比較表明，本文算法能夠獲得更好的優(yōu)化性和實(shí)時(shí)性，具有更加廣泛的實(shí)際應(yīng)用范圍.

路徑規(guī)劃；局部最優(yōu)；運(yùn)動(dòng)控制器；自主導(dǎo)航；沿墻走算法

復(fù)雜二維環(huán)境中的機(jī)器人實(shí)時(shí)自主導(dǎo)航的研究劃分為兩大類，一類是基于靜態(tài)環(huán)境的全局優(yōu)化研究，另一類是基于傳感器數(shù)據(jù)的局部?jī)?yōu)化研究［1-2］.靜態(tài)全局優(yōu)化方法不但需要很大的存儲(chǔ)空間，而且需要耗費(fèi)較長(zhǎng)的計(jì)算時(shí)間，因此該類問題能夠求解的數(shù)據(jù)空間范圍和精度以及求解的實(shí)時(shí)性會(huì)有較大限制［3-4］，同時(shí)靜態(tài)全局問題對(duì)于機(jī)器人面臨的不斷變化的環(huán)境信息、隨時(shí)可能出現(xiàn)的移動(dòng)障礙，甚至機(jī)器人自身不斷變化的參數(shù)都不具有良好適應(yīng)性.雖然文獻(xiàn)［5］提出了重新規(guī)劃的方法，但是不斷重新規(guī)劃的策略無(wú)疑也增加了機(jī)器人額外的處理時(shí)間.另一類針對(duì)機(jī)器人傳感器信息的局部規(guī)劃算法中，人工勢(shì)場(chǎng)法（artificial potential field）［6-8］在解決局部規(guī)劃中發(fā)揮了重要作用.但是人工勢(shì)場(chǎng)法有其特定的缺陷即局部最小點(diǎn)問題，或者稱為局部陷阱問題.為解決該問題，科研人員提出一些新的勢(shì)場(chǎng)函數(shù)，例如超二次方程勢(shì)場(chǎng)法［9］，協(xié)調(diào)函數(shù)法［10］，人工力矩算法［11-12］等，還有些方法試圖改善障礙物的表達(dá)方式，比如虛擬障礙的方法［13-14］子目標(biāo)法［15-16］等，但是上述方法仍然沒有從根本上消除人工勢(shì)場(chǎng)法固有缺陷.沿墻走算法（wall?following or Bug method）［17-19］以及改進(jìn)算法［20-21］在逃離局部陷阱的方面做了新的嘗試，新算法試圖控制機(jī)器人始終沿著障礙物邊緣的一個(gè)方向運(yùn)動(dòng)，從而脫離局部陷阱，但是這些方法對(duì)可通行路徑的優(yōu)化考慮較少，因而有進(jìn)一步改善空間.

本文提出一種新的基于傳感器數(shù)據(jù)的實(shí)時(shí)路徑優(yōu)化算法.算法的設(shè)計(jì)分為兩個(gè)步驟，1）采用“雙向搜索多邊形構(gòu)造算法”，搜索出復(fù)雜障礙物的包圍多邊形，然后以包圍多邊形頂點(diǎn)構(gòu)造基于障礙物和機(jī)器人關(guān)系的可視圖，并以可視圖為基礎(chǔ)進(jìn)行路徑優(yōu)化.與傳統(tǒng)的凸包算法［22-23］相比，本文構(gòu)造算法的復(fù)雜度為O（K lg N），N為障礙物點(diǎn)數(shù)，K為包圍多邊形頂點(diǎn)數(shù)，小于傳統(tǒng)凸包算法復(fù)雜度.2）設(shè)計(jì)了“基于勢(shì)場(chǎng)函數(shù)的機(jī)器人運(yùn)動(dòng)控制器”.當(dāng)機(jī)器人的最終目標(biāo)不被障礙物阻擋，直接將該最終目標(biāo)定義為控制器目標(biāo)點(diǎn)，否則機(jī)器人從障礙物可視圖中計(jì)算最優(yōu)行進(jìn)路徑，以最優(yōu)行進(jìn)路徑上的第一個(gè)節(jié)點(diǎn)為趨近目標(biāo)，將其命名為“當(dāng)前目標(biāo)”.控制器設(shè)計(jì)中，目標(biāo)點(diǎn)（最終目標(biāo)或者當(dāng)前目標(biāo)）對(duì)機(jī)器人產(chǎn)生吸引勢(shì)，障礙物對(duì)機(jī)器人產(chǎn)生排斥勢(shì)，機(jī)器人以吸引勢(shì)和排斥勢(shì)構(gòu)成矢量和為輸入條件，根據(jù)機(jī)器人自身狀態(tài)計(jì)算出機(jī)器人行進(jìn)的速度和轉(zhuǎn)角，在該輸出控制下調(diào)整自身姿態(tài)，從而躲避障礙物走向目標(biāo)點(diǎn).同時(shí)控制器設(shè)置了“基本速度”，具備非常好的逃離局部陷阱的能力.

1 基本概念、定義與假設(shè)條件

為了將本文算法描述清楚，首先做如下定義：定義從點(diǎn)p1到p2的有向線段為l（p1，p2），有向線段的角度定義為βl，同時(shí)βl應(yīng)該滿足條件-π＜βl≤π，有向線段的長(zhǎng)度定義為Dis（l（p1，p2））.一個(gè)二值函數(shù)ξ（x）定義為

其中sgn（x）是變量X的符號(hào)函數(shù).

函數(shù)d gl（x）定義為d g l（x）函數(shù)將x的角度范圍定義在（-π，π］.

如果βi和βj分別是有向線段li和lj的方向角，那么有向線段li到有向線段lj之間的夾角就定義為βij，使用上述定義的函數(shù)，夾角表示為βij＝d g l（βi-βj）.

定義1 障礙物模型

定義點(diǎn)集合G［k］＝P（g1…gk），其中g(shù)1…gk是機(jī)器人獲取的障礙物點(diǎn).任意兩個(gè)相鄰的邊緣點(diǎn)gi、gi＋1之間的距離需要滿足如下條件：

其中min GP和max GP是預(yù)先定義的相鄰障礙點(diǎn)的最小和最大距離.假設(shè)機(jī)器人掃描到障礙點(diǎn)g1…gk后，機(jī)器人就能夠存這些障礙點(diǎn)，并且將這些信息作為機(jī)器人對(duì)障礙物的已知信息.

本文算法所適應(yīng)環(huán)境是具有移動(dòng)障礙物的復(fù)雜二維環(huán)境，由于一個(gè)控制周期以毫秒為計(jì)量單位，因此假設(shè)在一個(gè)控制周期內(nèi)，掃描到的障礙物位置沒有發(fā)生變化，另外還假設(shè)無(wú)論當(dāng)前的二維工作環(huán)境多復(fù)雜，總存在一條從機(jī)器人到達(dá)目標(biāo)點(diǎn)的可行路線，否則該路徑規(guī)劃算法無(wú)法發(fā)揮作用.最后假設(shè)機(jī)器人能夠定位自身位置和要到達(dá)的最終目標(biāo)的位置，上述假設(shè)，無(wú)論是面對(duì)理論要求還是真實(shí)實(shí)驗(yàn)環(huán)境，都是能夠?qū)崿F(xiàn)且比較合理的.

2 基于局部復(fù)雜障礙的路徑優(yōu)化算法

2.1雙向搜索多邊形構(gòu)造算法描述

該構(gòu)造算法總是試圖尋找障礙物的突起點(diǎn)，因?yàn)橥黄瘘c(diǎn)是障礙物中一部分區(qū)域的代表點(diǎn)，包圍障礙物的多邊形就是由障礙物的多個(gè)凸起點(diǎn)組成［24］.

對(duì)于障礙點(diǎn)，首先利用凸包算法［25-26］構(gòu)造障礙區(qū)域輪廓，獲得障礙邊緣GL［n］.根據(jù)機(jī)器人R和目標(biāo)T生成線段l（R，T），判斷l(xiāng)（R，T）和障礙物GL［n］是否相交，如果不相交，則說明障礙物G沒有處在機(jī)器人R和目標(biāo)點(diǎn)T之間，最短路徑就是機(jī)器人R和目標(biāo)T之間的連線；如果相交，則說明障礙物阻擋了機(jī)器人R走向目標(biāo)T，顯然這時(shí)機(jī)器人不能直接走向目標(biāo)點(diǎn).在該條件下用l（R，T）將障礙物分為兩部分，分別命名為“順時(shí)針部分“和”逆時(shí)針部分”，以“順時(shí)針部分”為例描述算法的構(gòu)造順序.

將線段l（R，T）和障礙GL［n］的交點(diǎn)命名為PC，并且以順時(shí)針方向沿著“順時(shí)針部分”的邊緣移動(dòng)PC，把這條搜索路線定義為“正向搜索線”如圖1所示.沿此路線，每當(dāng)PC點(diǎn)移動(dòng)一步，就判斷l(xiāng)（PC，T）和障礙線GL［n］是否相交，如果相交，則繼續(xù)沿著“正向搜索線”移動(dòng)PC點(diǎn)，直到PC點(diǎn)移動(dòng)到一個(gè)能夠和目標(biāo)T直接相聯(lián)的位置，如圖1中位置T1，這時(shí)記錄下T1的位置.定義該點(diǎn)為“目標(biāo)最近點(diǎn)”.

圖1 雙向搜索多邊形生成算法示意圖

總之，在所有的能夠直接到達(dá)目標(biāo)點(diǎn)T而不被障礙物G所阻礙的位置點(diǎn)中，“目標(biāo)最近點(diǎn)”是距目標(biāo)點(diǎn)T的距離最小的一個(gè)點(diǎn)（這里只考慮“順時(shí)針部分”障礙）.

算法的下一步驟是繼續(xù)搜索包圍障礙物多邊形的其他頂點(diǎn).在下一步的搜索過程中，反過來(lái)以T1為起始點(diǎn)，以機(jī)器人R為搜索的目標(biāo)點(diǎn).計(jì)算線段l（T1，R）和障礙GL［n］的“順時(shí)針部分”是否相交，如果相交，記錄交點(diǎn)為PC，并讓PC沿著“反向搜索線”移動(dòng)，每移動(dòng)一步判斷l(xiāng)（PC，R）和障礙GL［n］的“順時(shí)針部分”是否相交，如果相交就將交點(diǎn)PC繼續(xù)沿“反向搜索線”移動(dòng)，直到PC點(diǎn)能夠直接和機(jī)器人R聯(lián)通，這時(shí)PC點(diǎn)處于R1所在位置，如圖1所示.命名該點(diǎn)為“起始最近點(diǎn)”.可以看出，“起始最近點(diǎn)”是所有從T1開始，越過障礙G尋找機(jī)器人R位置的所有點(diǎn)中，距離機(jī)器人R最小的一個(gè)點(diǎn).（同樣這里也只考慮“順時(shí)針部分”的障礙）.通過上述描述可以做如下假設(shè)，如果存在一條從機(jī)器人R到達(dá)目標(biāo)T，并且繞過障礙物G的多邊形，盡管還不能找出這條多邊形的全部節(jié)點(diǎn)，但是“目標(biāo)最近點(diǎn)”和“起始最近點(diǎn)”一定在這條多邊形上，而且這兩個(gè)點(diǎn)分別是該多邊形的頭點(diǎn)和尾點(diǎn).由于這條預(yù)期多邊形的頭和尾已經(jīng)找到，再以頭尾開始，繼續(xù)向中間收斂，那么這個(gè)逆時(shí)針部分的多邊形就能夠全部搜索出來(lái).因此，重新以R1開始，以T1為目標(biāo)，重復(fù)上述搜索過程，會(huì)繼續(xù)得到包圍障礙物多邊形的另一組節(jié)點(diǎn)，繼續(xù)命名為R2、T2.重復(fù)上述正向、反向搜索過程，直到起始點(diǎn)和終止點(diǎn)位置重合，就找到了包圍障礙物邊緣的多邊形的所有點(diǎn)位置（順時(shí)針部分），如圖1所示的線路2.由于每次搜索都是以正向搜索／反向搜索為基本步驟，因此該算法命名為“雙向搜索多邊形構(gòu)造算法”.在搜索過程中，每次搜索出來(lái)的節(jié)點(diǎn)都被用作下一次搜索的初始點(diǎn)，每個(gè)搜索出來(lái)的節(jié)點(diǎn)的信息都得到充分的利用，因此冗余計(jì)算少，算法簡(jiǎn)潔，收斂速度快，實(shí)時(shí)性好.

“逆時(shí)針部分”的搜索過程，和“順時(shí)針部分”的搜索過程類似，最終得到的包圍多邊形如圖1所示的線路1.

2.2多邊形構(gòu)造算法偽代碼

雙向搜索多邊形構(gòu)造算法：

Step 1 根據(jù)機(jī)器人R、障礙物T生成線段l（R，T），計(jì)算線段l（R，T）和障礙物GL［N］的交點(diǎn)，如果l（R，T）和障礙GL［n］不相交，則算法結(jié)束，說明障礙物沒有阻礙機(jī)器人走向目標(biāo)點(diǎn).否則記錄交點(diǎn)為PC，并按照障礙物正向部分生成“正向搜索線”和“反向搜索路線”，同時(shí)生成兩條路線F ront-R oute［n］和A pposite-Route［n］，并清空這兩條路線.

Step 2 沿“正向搜索線”移動(dòng)交點(diǎn)PC，每次移動(dòng)PC后，判斷l(xiāng)（PC，T）和障礙線GL［n］是否相交.如果相交，返回Step 2.否則將PC插入路徑F ront-Route［n］頭部，設(shè)置機(jī)器人R為目標(biāo)點(diǎn)，生成線段l（PC，R）

Step 3 沿“反向搜索線”移動(dòng)點(diǎn)PC，每次移動(dòng)PC后，判斷l(xiāng)（PC，R）是否和GL［n］相交.如果相交，返回Step3，否則，將PC點(diǎn)插入路徑A pposite-R ou te［n］的尾部.

Step 4 如果點(diǎn)F ront-R ou te［n］和點(diǎn)A pposite-R ou te［n］位置相等，跳轉(zhuǎn)到下一步.否則設(shè)置F ront-R oute［n］為目標(biāo)點(diǎn)T，A pposite-R ou te［n］為起始點(diǎn)R，并返回至Step 2.

Step 5 設(shè)置Fron t-R ou te［n］為“目標(biāo)最近點(diǎn)”，A pposite-R oute［n］為“起始最近點(diǎn)”，連接路徑A pposite-Route［n］和Front-R ou te［n］為一條路徑，并命名為ClockMinRoute，算法結(jié)束.

通過算法上述步驟得到包圍障礙物的多邊形，同時(shí)該多邊形將機(jī)器人隔離在障礙物之外.

2.3基于可視圖的最短路徑搜索算法

Dijkstra算法［23］是對(duì)網(wǎng)絡(luò)可視圖進(jìn)行最短路徑尋優(yōu)的基礎(chǔ)算法，該算法能夠搜索出一個(gè)可視圖中各個(gè)頂點(diǎn)距離指定頂點(diǎn)的最短距離，同時(shí)該算法收斂速度快，具有良好的實(shí)時(shí)效果.在前一節(jié)中，本文所述的“雙向搜索多邊形構(gòu)造算法”搜索出了包圍復(fù)雜障礙物的凸多邊形，利用該凸多邊形可構(gòu)造描述障礙物的“可視圖”.有了可視圖之后，采用Dijkstra最優(yōu)搜索算法，就可以得到通過障礙物的理論最短路徑.算法的基本過程和證明參考文獻(xiàn)［23］，Dijkstra算法的時(shí)間復(fù)雜度為O（N2），N為頂點(diǎn)數(shù).

3 基于勢(shì)函數(shù)的機(jī)器人運(yùn)動(dòng)控制器設(shè)計(jì)

前節(jié)算法雖然搜索出了從機(jī)器人到障礙物的最短路徑，但是最短路徑的獲取是通過用障礙物包圍多邊形構(gòu)造的可視圖，并在可視圖基礎(chǔ)上采用Dijkstra算法求得，其最優(yōu)的概念是針對(duì)障礙物的最優(yōu).而實(shí)際的機(jī)器人運(yùn)動(dòng)是機(jī)器人自身的運(yùn)動(dòng)，因此機(jī)器人實(shí)際的行走路線需要以最優(yōu)路徑為目標(biāo)進(jìn)行自我調(diào)整.在最優(yōu)路徑的構(gòu)造中機(jī)器人自身參數(shù)并未考慮在優(yōu)化之內(nèi)，否則優(yōu)化算法復(fù)雜性將大大增加.局部最優(yōu)路徑是基于瞬時(shí)局部障礙信息，而實(shí)際機(jī)器人的運(yùn)動(dòng)是連貫不斷的過程，在機(jī)器人運(yùn)動(dòng)期間，“可視圖”信息將不斷變化，因此最優(yōu)路徑是機(jī)器人運(yùn)動(dòng)的預(yù)期和近似，實(shí)際的機(jī)器人具備自身物理尺寸和安全距離（如定義1所描述），其行進(jìn)路線不可能和最優(yōu)路線一致，而是盡量貼近最優(yōu)路線.

針對(duì)上述描述設(shè)計(jì)機(jī)器人運(yùn)動(dòng)控制器，該控制器以目標(biāo)點(diǎn)和障礙物對(duì)機(jī)器人的吸引勢(shì)和排斥勢(shì)做為輸入矢量，以機(jī)器人運(yùn)動(dòng)的速度和方向?yàn)檩敵鍪噶浚M(jìn)而控制機(jī)器人的整體運(yùn)動(dòng).在控制器設(shè)計(jì)中，為保證機(jī)器人能夠運(yùn)動(dòng)到最終位置，定義機(jī)器人的目標(biāo)T為機(jī)器人的“最終目標(biāo)”，如果機(jī)器人無(wú)法直接到達(dá)“最終目標(biāo)”T，則將首先能夠到達(dá)的最優(yōu)路徑上的第一個(gè)頂點(diǎn)定義為趨近目標(biāo)，并將其命名為機(jī)器人的“當(dāng)前目標(biāo)”，每個(gè)控制周期內(nèi)，最短路徑搜索算法都會(huì)提供機(jī)器人一個(gè)“當(dāng)前目標(biāo)”，不斷指引機(jī)器人向最終目標(biāo)前進(jìn).

控制器的設(shè)計(jì)還保證即便發(fā)生吸引勢(shì)與排斥勢(shì)的和為零的情況發(fā)生，機(jī)器人仍會(huì)以一個(gè)基本速度運(yùn)動(dòng)，這樣即使機(jī)器人處于零勢(shì)場(chǎng)空間，仍然能夠在“當(dāng)前目標(biāo)”的指引作用下脫離局部陷阱.同文獻(xiàn)［12，24，27］中的定義相比，該運(yùn)動(dòng)控制器所采用的參數(shù)具有明確簡(jiǎn)潔的物理意義，因而在最后的實(shí)驗(yàn)中能夠更好地體現(xiàn)控制器算法的效果.

控制器設(shè)計(jì)基本步驟介紹如下：

機(jī)器人半徑為Dr，機(jī)器人安全距離為Ds，機(jī)器人方向角為βR，機(jī)器人要達(dá)到的目標(biāo)位置定義為T，βRT為機(jī)器人和目標(biāo)點(diǎn)之間的夾角，而βTR是目標(biāo)點(diǎn)和機(jī)器人之間的夾角.

歸一化函數(shù)amt（x）的定義如下：

其中sgn（x）表示變量x的標(biāo)號(hào)函數(shù)，δθ表示一個(gè)遠(yuǎn)遠(yuǎn)小于π／2的角度，表達(dá)為δθ?π／2，該函數(shù)是一個(gè)歸一化函數(shù)，體現(xiàn)了變量x在零點(diǎn)附近集中的程度.

吸引勢(shì)函數(shù)：

1）當(dāng)“當(dāng)前目標(biāo)”T不是機(jī)器人的最終位目標(biāo)時(shí)，吸引勢(shì)函數(shù)定義為

吸引勢(shì)函數(shù)表達(dá)了機(jī)器人目標(biāo)點(diǎn)對(duì)機(jī)器人的吸引作用.函數(shù)值的大小由βR（k）-βRT（k）決定，其中βRT（k）是有向線段l（T，R）的方向，βR（k）-βRT（k）是有向線段l（T，R）和機(jī)器人方向的夾角.該函數(shù)表達(dá)了當(dāng)目標(biāo)T在機(jī)器人后方時(shí)，其吸引勢(shì)函數(shù)對(duì)機(jī)器人角度取值最大，機(jī)器人會(huì)盡可能轉(zhuǎn)向目標(biāo)T.而當(dāng)目標(biāo)在機(jī)器人前進(jìn)方向時(shí)則機(jī)器人角度不變，直接走向目標(biāo)T，因而目標(biāo)對(duì)機(jī)器人角度的吸引勢(shì)較弱.其中λa為一個(gè)常量，通過設(shè)計(jì)的實(shí)驗(yàn)的結(jié)果分析，當(dāng)λa＝0.55時(shí)，函數(shù)較好的體現(xiàn)了目標(biāo)對(duì)機(jī)器人的吸引作用.

2）當(dāng)“當(dāng)前目標(biāo)”T是機(jī)器人最終目標(biāo)時(shí)，吸引勢(shì)函數(shù)定義為

該吸引勢(shì)函數(shù)仍舊由βR（k）-βRT（k）決定，但是由于最終目標(biāo)具有方向βT，因而函數(shù)在運(yùn)動(dòng)方向上有所變化，其目的是期望當(dāng)機(jī)器人到達(dá)最終目標(biāo)點(diǎn)時(shí)位置不但要和最終目標(biāo)的位置一致，方向也盡可能接近最終目標(biāo)點(diǎn)方向，如果“當(dāng)前目標(biāo)”T不是最終目標(biāo)，則不需要考慮目標(biāo)T的方向問題.同時(shí)該函數(shù)也使得機(jī)器人到達(dá)最終目標(biāo)后，不會(huì)震蕩，而是穩(wěn)定的靠近目標(biāo)位置.

排斥勢(shì)函數(shù)：

排斥勢(shì)函數(shù)體現(xiàn)了機(jī)器人安全半徑內(nèi)的障礙物對(duì)機(jī)器人的排斥作用，通過障礙物的排斥作用迫使機(jī)器人遠(yuǎn)離障礙物.排斥勢(shì)函數(shù)的定義為其中λp＝（Ds-DGR）／Ds.

可以看到，排斥勢(shì)函數(shù)是多個(gè)障礙物點(diǎn)Gi對(duì)機(jī)器人排斥作用的代數(shù)和.排斥勢(shì)函數(shù)的大小由（βR（k）-βGi，R（k））決定，βGi，R（k）是有向線段l（Gi，R）的夾角.當(dāng)（βR（k）-βGi，R（k））最小時(shí)，說明障礙物處在背對(duì)機(jī)器人的位置，因此障礙物對(duì)機(jī)器人排斥勢(shì)能較低，當(dāng)（βR（k）-βGi，R（k））最大時(shí)，說明障礙物處在機(jī)器人運(yùn)動(dòng)方向的前方，因而障礙物會(huì)用較大的勢(shì)能推動(dòng)機(jī)器人轉(zhuǎn)向，使得機(jī)器人及時(shí)躲開障礙物.

根據(jù)以上吸引勢(shì)函數(shù)和排斥勢(shì)函數(shù)，機(jī)器人運(yùn)動(dòng)控制器的遞推方式定義如下：

梯度函數(shù)定義為

結(jié)合式（1）～（8），可得

式（9）是吸引勢(shì)函數(shù)的調(diào)整結(jié)果.

如果機(jī)器人的“當(dāng)前目標(biāo)”和機(jī)器人的最終目標(biāo)一致，那么結(jié)合式（1）～（9），有

結(jié)合式（10）排斥勢(shì)能產(chǎn)生的控制步長(zhǎng)，

式（11）中，Δ2βR（k）表示由各個(gè)障礙物對(duì)機(jī)器人產(chǎn)生的排斥勢(shì)能的代數(shù)和.

機(jī)器人實(shí)際步長(zhǎng)函數(shù)Sx（k）和Sy（k）定義為

其中SR是機(jī)器人行走的最大步長(zhǎng).

從上述分析可知，控制器的每個(gè)參數(shù)都有明確的物理意義，因此該控制器算法清楚，并且適合實(shí)際的機(jī)器控制.

4 實(shí)驗(yàn)與分析

為了驗(yàn)證本文提出算法的有效性，設(shè)計(jì)了兩個(gè)實(shí)驗(yàn)，機(jī)器和環(huán)境參數(shù)設(shè)置見表1.

表1 機(jī)器人和障礙參數(shù)設(shè)置像素

在實(shí)驗(yàn)1中，采用兩類人工障礙物（即手動(dòng)設(shè)定的障礙點(diǎn)）：第一類人工障礙簡(jiǎn)單但是障礙物的數(shù)目較多；第二類人工障礙物為復(fù)雜的螺旋形障礙，并且機(jī)器人的初始和目標(biāo)位置都處于螺旋障礙物環(huán)抱中間.實(shí)驗(yàn)的程序采用Visual Studio 6.0平臺(tái)的C＋＋程序設(shè)計(jì)，能體現(xiàn)算法的實(shí)時(shí)性和實(shí)際應(yīng)用價(jià)值.圖2給出了第一類人工障礙環(huán)境中，本文算法同“人工力矩法”和“沿墻走算法”行走路徑的對(duì)比示意圖，其具體結(jié)果見表2.

圖2 第一類人工障礙中不同算法的規(guī)劃結(jié)果

表2 人工障礙1中的實(shí)驗(yàn)對(duì)比結(jié)果

從表2和圖2中可以看到，本文得到路徑結(jié)果最短，步數(shù)最少，體現(xiàn)了本文最優(yōu)路徑規(guī)劃的有效性，機(jī)器人行走的軌跡平滑，體現(xiàn)了運(yùn)動(dòng)控制器良好的控制效果，控制時(shí)間也保證了算法的實(shí)時(shí)性.

實(shí)驗(yàn)2中采用AS-R輪式機(jī)器人，該機(jī)器人由3部分組成，分別是“傳感器模塊”、“中央控制模塊”和“運(yùn)動(dòng)執(zhí)行模塊”.“傳感器模塊”由紅外和聲納傳感器組成，沿機(jī)器人圓周排布著8個(gè)聲納傳感器和8個(gè)紅外傳感器.聲納傳感器和紅外傳感器分別以π／4角度互相排列，紅外傳感器的最佳探測(cè)距離為0.3～0.9 m，聲納傳感器的最佳探測(cè)距離為0.6～4.0 m，這兩類傳感器覆蓋了從遠(yuǎn)到近的探測(cè)范圍.“中央控制模塊”由計(jì)算機(jī)和PCI總線的傳感器接口卡、運(yùn)動(dòng)控制卡組成.計(jì)算機(jī)基于Windows系統(tǒng)運(yùn)行，整個(gè)開發(fā)和測(cè)試平臺(tái)采用Visual Studio6.0開發(fā).“運(yùn)動(dòng)執(zhí)行模塊”由電機(jī)驅(qū)動(dòng)器、兩個(gè)驅(qū)動(dòng)輪、一個(gè)輔助輪和電池組成.驅(qū)動(dòng)輪采用閉環(huán)控制，位置反饋采用2 000線碼盤，精度足夠滿足實(shí)驗(yàn)需求，電池充滿電后能夠滿足2 h的工作時(shí)間.采用VC6.0開發(fā)的測(cè)試平臺(tái)能夠完成位置傳感器控制、電機(jī)控制、攝像機(jī)和無(wú)線網(wǎng)絡(luò)控制功能，保證機(jī)器人處于良好的工作狀態(tài)，同時(shí)提供了用戶開發(fā)程序接口.實(shí)驗(yàn)2中，針對(duì)機(jī)器人的運(yùn)動(dòng)，設(shè)計(jì)了兩種運(yùn)動(dòng)模式，分別是直行模式和轉(zhuǎn)彎模式.直行模式中，雙輪同速同向運(yùn)動(dòng)，機(jī)器人行走的距離就是雙輪轉(zhuǎn)過的周長(zhǎng)；轉(zhuǎn)彎模式中，雙輪反向同速運(yùn)動(dòng).機(jī)器人轉(zhuǎn)動(dòng)的弧度δ＝（α?R）／r，其中R是機(jī)器人輪半徑，r是機(jī)器人半徑，α是輪轉(zhuǎn)過的弧度.設(shè)機(jī)器人位置（Xr，Yr），機(jī)器人角度為βR，傳感器與機(jī)器人正向夾角為α，障礙點(diǎn)返回距離為ddis，則障礙物位置（Xo，Yo）的計(jì)算公式為

實(shí)驗(yàn)2中每個(gè)控制周期為20 ms，根據(jù)機(jī)器人實(shí)際的運(yùn)動(dòng)速度要求，本文算法能夠比較好的滿足實(shí)時(shí)性的要求，圖3（a）中是實(shí)驗(yàn)所用機(jī)器人，圖3（b）中機(jī)器人向目標(biāo)運(yùn)動(dòng)，當(dāng)有障礙進(jìn)入機(jī)器人安全半徑，機(jī)器人及時(shí)轉(zhuǎn)彎以避免碰撞（見圖3（c）），并選擇除了可通行路徑如圖3（d）、圖3（e）所示，最后達(dá)到設(shè)定位置如圖3（f）所示.為驗(yàn)證實(shí)際應(yīng)用中機(jī)器人躲避移動(dòng)障礙物的能力，在實(shí)驗(yàn)2中添加了一個(gè)障礙機(jī)器人如圖4所示.試驗(yàn)中兩個(gè)機(jī)器人互為障礙，該實(shí)驗(yàn)不但能夠體現(xiàn)機(jī)器人躲避移動(dòng)障礙物的能力，而且很好地體現(xiàn)機(jī)器人檢測(cè)移動(dòng)障礙能力.實(shí)驗(yàn)2的結(jié)果見表3.其中控制周期的時(shí)間包括：機(jī)器人對(duì)障礙檢測(cè)時(shí)間、路徑優(yōu)化時(shí)間和勢(shì)函數(shù)控制器迭代運(yùn)算時(shí)間.

圖3 單機(jī)器人在實(shí)際障礙中避碰與規(guī)劃

圖4 兩個(gè)移動(dòng)機(jī)器人相互避讓實(shí)驗(yàn)

表3 實(shí)驗(yàn)2中實(shí)際機(jī)器人運(yùn)動(dòng)數(shù)據(jù)對(duì)比結(jié)果

由表3可知，本文算法在實(shí)際機(jī)器人應(yīng)用中體現(xiàn)了良好的優(yōu)化和控制效果.

5 結(jié) 論

1）提出了“雙向搜索多邊形構(gòu)造算法”，根據(jù)機(jī)器人掃描到的障礙點(diǎn)生成障礙物包圍多邊形.該多邊形構(gòu)造算法使得機(jī)器人在處于被障礙物包圍的情況搜索出繞過障礙物的可行路徑，并在基于多邊形基礎(chǔ)的可視圖中求取最優(yōu)路徑.

2）設(shè)計(jì)了基于吸引勢(shì)和排斥勢(shì)的機(jī)器人運(yùn)動(dòng)控制器，控制器在已獲得最優(yōu)路徑的前提下，不斷用優(yōu)化目標(biāo)引導(dǎo)機(jī)器人脫離障礙，走向最終目標(biāo)，同時(shí)控制器的“基本速度”保證機(jī)器人不會(huì)停在零勢(shì)場(chǎng)范圍內(nèi).

3）本文算法無(wú)論在人工地圖中還是在實(shí)際機(jī)器人應(yīng)用中都能對(duì)復(fù)雜障礙物有很好的通過能力，能夠有效脫離復(fù)雜螺旋形障礙物的包圍，并對(duì)行走的路徑有優(yōu)化能力，保證機(jī)器人行進(jìn)路線穩(wěn)定、平滑.和其他算法如人工勢(shì)場(chǎng)法、沿墻走算法比較，本文算法規(guī)劃能力強(qiáng)，實(shí)時(shí)性好，具備廣泛的應(yīng)用前景.

［1］UNDEGER C，POLAT F.Real?time edge follow：a real?time path search approach［J］.IEEE Transactions on Systems，Man and Cybernetics?Part C：Application and Reviews，2007，37（1）：860-872.

［2］JANABI?SHARIFI F，WILSON W J.A fast approach for robot motion planning［J］.Journal of Intelligent and Robotic Systems，1999，12（25）：187-212.

［3］MINGUEZ J，MONTANO L.Nearness diagram（ND）navigation：collision avoidance in troublesome scenarios［J］.IEEE Trans Robot Automat，2004，20（1）：45-59.

［4］MINGUEZ J，MONTANO L.Sensor?based robotmotion generation in unknown，dynamic and troublesomescenarios［J］.Robot Autonomous Syst，2005，52（1）：290-311.

［5］STENTZ A，HEBERT M.A complete navigation，system for goal acquisition in unknown environments［J］.Autonomous Robots，1995，2（2）：127-145.

［6］RIMON E，KODITSCHEK D E.Exact robot navigation using artificial potential functions［J］.IEEE Transactions on Robotics and Automation，1992，8（5）：501-518.

［7］GE S S，CUI Y J.Now potential functions for mobile robot path planning［J］.IEEE Trans on Robotics and Automation，2000，16（5）：615-620.

［8］KIM D H.Escaping route method for a trap situation in local path planning［J］.International Journal of Control Automation and Systems，2009，7（3）：495-500.

［9］VOLPE R，KHOSLA P.Manipulator control with super quadric artificial potential functions：theory and experiments［J］.IEEE Transactions on Systems，Man，and Cybernetics，2000，20（6）：1423-1436.

［10］VASCAK J.Navigation of mobile robots using potential fields and computational intelligence means［J］.Acta Polytechnica Hungarica，2007，4（1）：63-74.

［11］XU Wangbao，CHEN Xuebo.Artificial moment method for swarm-robot formation control［J］.Science in China?Series F：Information Science，2008，51（10）：1521-1531.

［12］XU Wangbao，CHEN Xuebo.A dynamical formation control approach based on artificial moments［J］.Control Theory＆Applications（in Chinese），2009，26（11）：1232-1238.

［13］PARK M G，LEE M C.A new technique to escape local minimum in artificial potential field based path planning［J］.KSME International Journal，2003，17（12）：1876-1885.

［14］PARK M G，LEE M C，SON K.Real?time path planning in unknown environments using a virtual hill［C］／／Proceeding of the 16th IFAC Word Congress.Laxenburg，Astralia：IFAC Secretariat，2005：61-65.

［15］BELL G，WEIR M.Forward chaining for robot and agent navigation using potential fields［C］／／Proceedings of the 27th Australasian Computer Science Conference.New Zealand：Australian Computer Society，2004：265-274.

［16］WEIR M，BUCK A，LEWIS J.A mind’s eye approach to providing BUG?like guarantees for adaptive obstacle navigation using dynamic potential fields［J］.Lecture Notes in Computer Science，2006，4095：239-250.

［17］YUN X P，TAN K C.Wall?following method for escaping local minima in potential field based motion planning［C］／／Proceedings of the International Conference on Advanced Robotics’97.Piscataway，NJ，United States：IEEE，1997：421-426.

［18］JAMES N，THOMAS B.Performance comparison of bug navigation algorithms［J］.Journal of Intelligent and Robotic Systems，2007，50（1）：73-84.

［19］MABROUK M H，MCLNNES C R.An emergent wall following behaviour to escape local minima for swarms of agents［J］.International Journal of Computer，2008，35（4）：463-76.

［20］JAVIER A，ALBERTO O，JAVIER M.A bug?inspired algorithm for efficient anytime path planning［C］／／2009 IEEE／RSJ International Conference on Intelligent Robots and Systems.St.Louis：IEEE Robotics and Automation Society，2009：5407-5413.

［21］ZHU Yi，ZHANG Tao，SONG Jingyan.A new bug?type navigation algorithm for mobile robots in unknown environments containing moving obstacles［J］.Industrial Robot：An International Journal，2012，39（1）：27-39.

［22］GRAHAM R L.An efficient algorithm for determining the convex hull of a finite planar set［J］.Information Processing Letter，1986，31（1）：132-134.

［23］THOMAS H，CORMEN，CHARLES E，et al.Introduction to Algorithms［M］.Second Edition.New York：MIT Press and McGraw?Hill，2011：955-956.

［24］ZHAO Jie，ZHOU Ziwei，LI Ge，et al.The apposite way path planning algorithm based on local message［C］／／2012 IEEE International Conference on Mechatronics and Automation.Washington D C，United States：IEEE，2012：1563-1568.

［25］TOUSSAINT G T.Solving geometric problems with the rotating calipers［C］／／Proceedings of IEEE MELECON.New York：IEEE，1983.

［26］戴光明，杜安紅.壁障問題最短路徑的兩級(jí)動(dòng)態(tài)規(guī)劃算法［J］.華中科技大學(xué)學(xué)報(bào)：自然科學(xué)版，2006，34（3）：122-124.

［27］CHANG Y C，YAMAMOTO Y.Path planning of wheeled mobile robot with simultaneous free space locating capability［J］.Intel Serv Robot，2009，2（1）：9-22.

（編輯楊 波）

A real time path planning algorithm based on local complicated environment

ZHOU Ziwei1，2，LI Changle1，ZHAO Jie1，XU Wangbao2
（1.State Key Laboratory of Robotic and System，Harbin Institute of Technology，150001 Harbin，China；2.School of Electronics＆Information Engineering，Liaoning University of Science and Technology，114000 Anshan，Liaoning，China）

A novel algorithm，which comprises with convex hull construction algorithm and robot controller is proposed for robot path planning based on complicated local data in robot’s autonomous navigation system.First the algorithm searches out the local optimal path from the robot’s current position to its target according to the localobstacle data.When the robotcan notreach the finaltargetdirectly，a temporary targetpointin the optimal path will be set to instruct the robot to avoid the obstacle and reach the final target.Next，a controller is design based on attractive force field and repulsive force field to control the robot’s motion，the combined effect of both attractive force field and repulsive force field drives the robot move toward the objective acquired from the optimal path and avoid obstacles at the same time.The experiment results show that this method can provide a better planning path compared with traditional path planning algorithms such as artificial potential field（APF），the wall?following（Bug）and the artificialmomentmethod，and ithas a fast reaction speed that is suitable for practical applications.

path planning；local optimal；motion controller；autonomous navigation；wall?following

TH137

A

0367-6234（2014）08-0065-07

2013-05-04.

國(guó)家自然科學(xué)基金資助項(xiàng)目（51105101）.

周自維（1974—），男，博士，講師；

趙 杰（1968—），男，教授，博士生導(dǎo)師.

李長(zhǎng)樂，8561388＠qq.com.