復雜局部地形中的實時路徑規劃算法設計

周自維，李長樂，趙 杰，徐望寶

復雜局部地形中的實時路徑規劃算法設計

周自維1，2，李長樂1，趙 杰1，徐望寶2

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

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

路徑規劃；局部最優；運動控制器；自主導航；沿墻走算法

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

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

1 基本概念、定義與假設條件

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

其中sgn（x）是變量X的符號函數.

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

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

定義1 障礙物模型

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

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

本文算法所適應環境是具有移動障礙物的復雜二維環境，由于一個控制周期以毫秒為計量單位，因此假設在一個控制周期內，掃描到的障礙物位置沒有發生變化，另外還假設無論當前的二維工作環境多復雜，總存在一條從機器人到達目標點的可行路線，否則該路徑規劃算法無法發揮作用.最后假設機器人能夠定位自身位置和要到達的最終目標的位置，上述假設，無論是面對理論要求還是真實實驗環境，都是能夠實現且比較合理的.

2 基于局部復雜障礙的路徑優化算法

2.1雙向搜索多邊形構造算法描述

該構造算法總是試圖尋找障礙物的突起點，因為突起點是障礙物中一部分區域的代表點，包圍障礙物的多邊形就是由障礙物的多個凸起點組成［24］.

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

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

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

總之，在所有的能夠直接到達目標點T而不被障礙物G所阻礙的位置點中，“目標最近點”是距目標點T的距離最小的一個點（這里只考慮“順時針部分”障礙）.

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

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

2.2多邊形構造算法偽代碼

雙向搜索多邊形構造算法：

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

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

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

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

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

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

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

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

3 基于勢函數的機器人運動控制器設計

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

針對上述描述設計機器人運動控制器，該控制器以目標點和障礙物對機器人的吸引勢和排斥勢做為輸入矢量，以機器人運動的速度和方向為輸出矢量，進而控制機器人的整體運動.在控制器設計中，為保證機器人能夠運動到最終位置，定義機器人的目標T為機器人的“最終目標”，如果機器人無法直接到達“最終目標”T，則將首先能夠到達的最優路徑上的第一個頂點定義為趨近目標，并將其命名為機器人的“當前目標”，每個控制周期內，最短路徑搜索算法都會提供機器人一個“當前目標”，不斷指引機器人向最終目標前進.

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

控制器設計基本步驟介紹如下：

機器人半徑為Dr，機器人安全距離為Ds，機器人方向角為βR，機器人要達到的目標位置定義為T，βRT為機器人和目標點之間的夾角，而βTR是目標點和機器人之間的夾角.

歸一化函數amt（x）的定義如下：

其中sgn（x）表示變量x的標號函數，δθ表示一個遠遠小于π／2的角度，表達為δθ?π／2，該函數是一個歸一化函數，體現了變量x在零點附近集中的程度.

吸引勢函數：

1）當“當前目標”T不是機器人的最終位目標時，吸引勢函數定義為

吸引勢函數表達了機器人目標點對機器人的吸引作用.函數值的大小由βR（k）-βRT（k）決定，其中βRT（k）是有向線段l（T，R）的方向，βR（k）-βRT（k）是有向線段l（T，R）和機器人方向的夾角.該函數表達了當目標T在機器人后方時，其吸引勢函數對機器人角度取值最大，機器人會盡可能轉向目標T.而當目標在機器人前進方向時則機器人角度不變，直接走向目標T，因而目標對機器人角度的吸引勢較弱.其中λa為一個常量，通過設計的實驗的結果分析，當λa＝0.55時，函數較好的體現了目標對機器人的吸引作用.

2）當“當前目標”T是機器人最終目標時，吸引勢函數定義為

該吸引勢函數仍舊由βR（k）-βRT（k）決定，但是由于最終目標具有方向βT，因而函數在運動方向上有所變化，其目的是期望當機器人到達最終目標點時位置不但要和最終目標的位置一致，方向也盡可能接近最終目標點方向，如果“當前目標”T不是最終目標，則不需要考慮目標T的方向問題.同時該函數也使得機器人到達最終目標后，不會震蕩，而是穩定的靠近目標位置.

排斥勢函數：

排斥勢函數體現了機器人安全半徑內的障礙物對機器人的排斥作用，通過障礙物的排斥作用迫使機器人遠離障礙物.排斥勢函數的定義為其中λp＝（Ds-DGR）／Ds.

可以看到，排斥勢函數是多個障礙物點Gi對機器人排斥作用的代數和.排斥勢函數的大小由（βR（k）-βGi，R（k））決定，βGi，R（k）是有向線段l（Gi，R）的夾角.當（βR（k）-βGi，R（k））最小時，說明障礙物處在背對機器人的位置，因此障礙物對機器人排斥勢能較低，當（βR（k）-βGi，R（k））最大時，說明障礙物處在機器人運動方向的前方，因而障礙物會用較大的勢能推動機器人轉向，使得機器人及時躲開障礙物.

根據以上吸引勢函數和排斥勢函數，機器人運動控制器的遞推方式定義如下：

梯度函數定義為

結合式（1）～（8），可得

式（9）是吸引勢函數的調整結果.

如果機器人的“當前目標”和機器人的最終目標一致，那么結合式（1）～（9），有

結合式（10）排斥勢能產生的控制步長，

式（11）中，Δ2βR（k）表示由各個障礙物對機器人產生的排斥勢能的代數和.

機器人實際步長函數Sx（k）和Sy（k）定義為

其中SR是機器人行走的最大步長.

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

4 實驗與分析

為了驗證本文提出算法的有效性，設計了兩個實驗，機器和環境參數設置見表1.

表1 機器人和障礙參數設置像素

在實驗1中，采用兩類人工障礙物（即手動設定的障礙點）：第一類人工障礙簡單但是障礙物的數目較多；第二類人工障礙物為復雜的螺旋形障礙，并且機器人的初始和目標位置都處于螺旋障礙物環抱中間.實驗的程序采用Visual Studio 6.0平臺的C＋＋程序設計，能體現算法的實時性和實際應用價值.圖2給出了第一類人工障礙環境中，本文算法同“人工力矩法”和“沿墻走算法”行走路徑的對比示意圖，其具體結果見表2.

圖2 第一類人工障礙中不同算法的規劃結果

表2 人工障礙1中的實驗對比結果

從表2和圖2中可以看到，本文得到路徑結果最短，步數最少，體現了本文最優路徑規劃的有效性，機器人行走的軌跡平滑，體現了運動控制器良好的控制效果，控制時間也保證了算法的實時性.

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

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

圖3 單機器人在實際障礙中避碰與規劃

圖4 兩個移動機器人相互避讓實驗

表3 實驗2中實際機器人運動數據對比結果

由表3可知，本文算法在實際機器人應用中體現了良好的優化和控制效果.

5 結 論

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

2）設計了基于吸引勢和排斥勢的機器人運動控制器，控制器在已獲得最優路徑的前提下，不斷用優化目標引導機器人脫離障礙，走向最終目標，同時控制器的“基本速度”保證機器人不會停在零勢場范圍內.

3）本文算法無論在人工地圖中還是在實際機器人應用中都能對復雜障礙物有很好的通過能力，能夠有效脫離復雜螺旋形障礙物的包圍，并對行走的路徑有優化能力，保證機器人行進路線穩定、平滑.和其他算法如人工勢場法、沿墻走算法比較，本文算法規劃能力強，實時性好，具備廣泛的應用前景.

［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］.華中科技大學學報：自然科學版，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.

國家自然科學基金資助項目（51105101）.

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

趙 杰（1968—），男，教授，博士生導師.

李長樂，8561388＠qq.com.