服務機器人路徑規(guī)劃及路徑跟蹤摘要：服務機器人由于所處的環(huán)境不可能是完全確定的，使得機器人導航問題 在服務型機潛人應用中越發(fā)關(guān)鍵。本文討論了環(huán)境部分未知的情況下的導航問 題中的全局路徑規(guī)劃，根據(jù)環(huán)境中的靜態(tài)已知障礙物利用蟻群算法找出全局最 優(yōu)路徑，采用了鏈接圖來建立環(huán)境全局地圖，將工作空間轉(zhuǎn)換為帶權(quán)圖的形式， 然后使用蟻群算法對帶權(quán)圖進行搜索，得到從起始點到目標的一條全局最優(yōu)路 徑。關(guān)鍵詞：蟻群算法；路徑跟蹤；避障Service Robots path planning and path followingAbstract： Because the working environment of the service robot can t be completely determined, the navigation is more important to resarch for service robot than other robots. The thesis proposes a solution to the robot navigation in part unknowen environment. The solution is the global path-planning. The main purpose is to lay out a global path in the static global environment using Ant Colony Algorithm. MAKLINK Graph is used to build the global working evrionment, which converts the workspace to weighted graph, then Ant Colony Algorithm is used to search the weighted graph and get the global optimal path from the begin position to the goal position.Key words: Ant Colony Algorithm; Path following; obstacle avoidance家庭服務機器人是為了倡導機潛人技術(shù)步入人們的生活以及機器人技術(shù)的實用化 普及化，最終實現(xiàn)自主機器人及人類真正意義上的人機協(xié)作和自然交互（如語音交互、 姿態(tài)交互）。機器人在家庭環(huán)境中快速、準確地導航是機港人進入家庭、提供各類服務 的前提。區(qū)別于工業(yè)機器人所處的準備好的確定性環(huán)境，家庭環(huán)境變動性大，由不規(guī) 則形狀的家具、相對狹窄的門和頻繁活動于其間的人類構(gòu)成，且不能因為機器人的進 入，大規(guī)模改動家庭環(huán)境，從而給生活主體人類帶來不便。因此尋找實用且效率高的 環(huán)境獲取方法，同時在不大量增加傳感器數(shù)量的前提下，實現(xiàn)精確定位，從而支持家 庭服務機落入快速、準確導航成為一個極富挑戰(zhàn)性的課題。在移動機器人路徑規(guī)劃理論和方法的研究中，確定性環(huán)境的路徑規(guī)劃問題即離線 的全局路徑規(guī)劃方已取得了大量的研究和應用成果。使用鏈接圖將全局確定性環(huán)境轉(zhuǎn) 換成帶權(quán)值的圖，而對于帶權(quán)圖求兩固定點之間的最短距離可以用已經(jīng)成熟的遺傳算 法以及蟻群算法求解但是，對于存在未知障礙物的環(huán)境下的路徑規(guī)劃雖然也取得 了一定的研究成果，但是還沒有完善的體系結(jié)構(gòu)，仍然有不少關(guān)鍵理論和技術(shù)問題急 需解決。要想讓機器人進入服務領(lǐng)域，必須解決環(huán)境部分未知情況下的路徑規(guī)劃，因 為服務機器人的服務對象是人，而有人存在的環(huán)境必然是變化的，比如人的行走，人 往環(huán)境中移入或移除物體。1機器人路徑規(guī)劃1.1 路徑規(guī)劃的基本概念路徑規(guī)劃是移動機器人導航中最重要的任務之一，對移動機器人路徑規(guī)劃系統(tǒng)的 主要要求是巴（1）在環(huán)境地圖中尋找一條路徑，保證機器人沿該路徑移動時不及外界發(fā)生碰撞;（2）能夠處理用傳感器感知的環(huán)境模型中的不確定因素和路徑執(zhí)行中出現(xiàn)的誤差;（3）通過使機器人避開外界物體而使其對機器人傳感器感知范圍的影響降到最小;（4）能夠按照需要找到最優(yōu)路徑。不論采用何種導航方式，智能移動機涔人主要完成路徑規(guī)劃，定位和避障等任務。 根據(jù)機器人對環(huán)境信息的了解程度，機潛人路徑規(guī)劃可以劃分為：環(huán)境信息完全已知 的全局路徑規(guī)劃和環(huán)境信息完全未知或部分未知，需要通過傳感器對障礙物位置、形 狀或尺寸進行探測的局部路徑規(guī)劃。全局路徑規(guī)劃的任務是根據(jù)先驗地理環(huán)境信息找 出從起始點到目標點的符合一定性能的可行或最優(yōu)路徑。局部路徑規(guī)劃使得機器人在 沿全局路徑的行進過程中，根據(jù)傳感器不斷感知的周圍環(huán)境信息和自身的狀態(tài)信息， 規(guī)劃出一條無障礙、可通行的局部路徑。在實際的工作中，環(huán)境是由已知的靜態(tài)障礙 物和每天可能變化位置的靜態(tài)障礙物（比如桌椅擺放的位置）以及動態(tài)障礙物（比如 在房間中行走的人）所組成。1.2 路徑規(guī)劃的分類及現(xiàn)狀從到目前為止的研究來看,移動機器人路徑規(guī)劃方法主要可以分為以下三種類型 （1）基于事例的學習規(guī)劃方法基于事例的學習規(guī)劃方法依靠過去的經(jīng)驗進行學習及問題求解，一個新的事例可 以通過修改事例庫中及當前情況相似的舊的事例來獲得。將其應用于移動機器人的路 徑規(guī)劃中可以描述為：首先，利用路徑規(guī)劃所用到的或已產(chǎn)生的信息建立一個事例庫, 庫中的任一事例包含每一次規(guī)劃時的環(huán)境信息和路徑信息，這些事例可以通過特定的 索引取得：隨后，將由當前規(guī)劃任務和環(huán)境信息產(chǎn)生的事例及事例庫中的事例進行匹 配，以尋找出一個最優(yōu)匹配事例，然后對該事例進行修正，并以此作為最后的結(jié)果。移動機器人導航需要良好的自適應性和穩(wěn)定性，而基于事例的方法能滿足這個需 求。Ram A將基于事例的在線匹配和增強式學習相結(jié)合，提高了機器人的自適應性能, 較好地適應了環(huán)境的變化。利用基于事例的方法時要注意保持事例庫中的事例數(shù)量， 以防止增加機器人在線規(guī)劃時間或產(chǎn)生信息爆炸問題。MarefatM把基于事例的方法作 為一個特征輔助規(guī)劃及全局規(guī)劃結(jié)合從而提高了全局規(guī)劃的效率。Kruusmaa M通過 創(chuàng)建種群事例庫在理論上覆蓋了關(guān)于路徑搜尋問題所有可能的路徑解空間，克服了啟 發(fā)式搜索方法在此方面的缺陷。（2）基于環(huán)境模型的規(guī)劃方法該方法首先需要建立一個關(guān)于機港人運動環(huán)境的環(huán)境模型。在很多時候由于移動 機器人的工作環(huán)境具有不確定性（包括非結(jié)構(gòu)性、動態(tài)性等），使得移動機器人無法建 立全局環(huán)境模型，而只能根據(jù)傳感器信息實時地建立局部環(huán)境模型，因此局部模型的 實時性、可靠性成為影響移動機器人是否可以安全、連續(xù)、平穩(wěn)運動的關(guān)鍵。環(huán)境建 模的方法基本上可以分為兩類：網(wǎng)絡/圖建模方法、基于網(wǎng)格的建模方法。前者主要包 括自由空間法、頂點圖像法、廣義錐法等，利用它們在進行路徑規(guī)劃時可得到比較精 確的解，但所耗費的計算量相當大，不適合于實際的應用。而后者在實現(xiàn)上要簡單許 多，所以應用比較廣泛，其典型代表就是四叉樹建模法及其擴展算法（如基于位置碼 四義樹建模法、Framed-quadtrees建模法等基于環(huán)境模型的規(guī)劃方法根據(jù)掌握環(huán)境信息的完整程度可以細分為環(huán)境信息完全 已知的全局路徑規(guī)劃和環(huán)境信息完全未知或部分未知的局部路徑規(guī)劃。由于環(huán)境模型 是已知的，全局路徑規(guī)劃的設(shè)計標準是盡量使規(guī)劃的效果達到最優(yōu)。在此領(lǐng)域已經(jīng)有 了許多成熟的方法，包括可視圖法、切線圖法、Voronoi圖法、拓撲法、懲罰函數(shù)法、 柵格法等。前4種方法都是采用基于圖論的思想，將目標、機器人及其工作空間用一 個連接圖表示，如此一來,路徑規(guī)劃問題就轉(zhuǎn)化為在圖上尋找一條從起始節(jié)點到目標節(jié) 點的路線。懲罰函數(shù)法將路徑規(guī)劃這個有約束的問題（受到障礙物的限制）轉(zhuǎn)化為一 個無約束最優(yōu)化問題，再求解就可得出解答。柵格法用網(wǎng)格描述機器人的工作環(huán)境， 根據(jù)柵格的可信度值可確定出障礙物的分布，此時通過避障規(guī)劃就可得到無碰路徑。（3）基于行為的路徑規(guī)劃方法基于行為的方法由Brooks在他著名的包容式結(jié)構(gòu)中建立，它是一門從生物系統(tǒng) 收到啟發(fā)而產(chǎn)生的用來設(shè)計自主機潛人的技術(shù)，它采用類似動物進化的H底向上的原 理體系，嘗試從簡單的智能體來建立一個復雜的系統(tǒng)。將其用于解決移動機器人路徑 規(guī)劃問題是一種新的發(fā)展趨勢，它把導航問題分解為許多相對獨立的行為單元，比如 跟蹤、避碰、目標制導等。這些行為單元是一些由傳感器和執(zhí)行器組成的完整的運動 控制單元，具有相應的導航功能，各行為單元所采用的行為方式各不相同，這些單元 通過相互協(xié)調(diào)工作來完成導航任務?；谛袨榈姆椒ù篌w可以分為反射式行為、反應式行為、慎思行為3種類型。反 射式行為類似于青蛙的膝跳反射，是一種瞬間的應激性本能反應，它可以對突發(fā)性情 況作出迅速反應，如移動機器人在運動中緊急停止等，但該方法不具備智能性，一般 是及其他方法結(jié)合使用。基于反應式行為的方法是由Brooks最先提出，它直接讀取傳 感器數(shù)據(jù)來規(guī)劃下一步的動作，可以穩(wěn)定及時地對不可預知的障礙和環(huán)境變化作出反 應，但由于缺乏全局環(huán)境知識，因此所產(chǎn)生的動作序列可能不是全局最優(yōu)的，不適合 于復雜環(huán)境下移動機器人的路徑規(guī)劃，機器人的沿墻走行為策略是其典型的代表。慎 思行為利用已知的全局環(huán)境模型為智能體系統(tǒng)到達某個特定目標提供最優(yōu)動作序列， 適合于復雜靜態(tài)環(huán)境下的規(guī)劃，移動機器人在運動中的實時重規(guī)劃就是一種慎思行為, 機器人可能出現(xiàn)倒退的動作以走出危險區(qū)域,但由于慎思規(guī)劃需要一定的時間去執(zhí)行, 所以它對于環(huán)境中不可預知的改變反應較慢。反應式行為、慎思行為可以通過傳感潛 數(shù)據(jù)、全局知識、反應速度、推理論證能力和計算的復雜性這兒方面來加以區(qū)分。近 來，在慎思行為的發(fā)展中出現(xiàn)了一種類似于人的大腦記憶的陳述性認知行為，基于此 的規(guī)劃不僅僅依靠傳感器和己有的先驗信息，還取決于所要到達的目標。比如對于距 離較遠且暫時不可見的目標，有可能存在一個行為分叉點，即有幾種行為可供采用， 機器人要擇優(yōu)選擇，這種決策性行為就是陳述性認知行為。將它用于路徑規(guī)劃中能使 移動機器人具有更高的智能，但由于決策的復雜性，該方法難以用于實際之中，這方 面工作還有待進一步研究。2全局路徑規(guī)劃關(guān)于路徑規(guī)劃的一種流傳甚廣但是不準確的說法是，路徑規(guī)劃本質(zhì)上是由一些碰 撞檢測或避障組成的。實際上，路徑規(guī)劃不僅如此。它包含如下方面：在移動障礙 物之間計算出無碰路徑；協(xié)調(diào)幾個機器人之間的運動；獲取物體之間的精確關(guān)系；分 析基于傳感信息所做的運動策略的不穩(wěn)定性；處理物理模型的特性以及機器人對物體 的抓取?；镜倪\動規(guī)劃問題描述如下：令A是在歐式空間W中運動的一個機器人，W就是 機器人A的工作空間，可以描述為Ri N=2或3；令功,.,4是空間W中分布的靜態(tài)障 礙物。假設(shè)A,坊,，紇的幾何特性以及8,的位置已知的話，再進一步假設(shè)A的運動沒有 運動學限制。那么運動規(guī)劃問題就是，給定A在W中起始位置和方向以及目標點位置 和方向，計算出從起始點到目標點并且不碰到B,的一系列連續(xù)的線段。2.1全局地圖的創(chuàng)建全局規(guī)劃的第一步就是要建立全局地圖，獲得4,名，,紇在機器人工作空間W中的 精確描述。機器人根據(jù)8,在W中的描述，規(guī)劃出一條無碰路徑。全局地圖的構(gòu)建方法 分為自由空間法和構(gòu)造空間法。2. 1. 1自由空間法自由空間法(Free Space Approach) 9：采用預先定義的如廣義錐形和凸多邊形等 基本形狀構(gòu)造自由空間，并將自由空間表示為連通圖，然后通過搜索連通圖來進行路 徑規(guī)劃。此方法比較靈活，即使起始點和目標點改變，也不必重構(gòu)連通圖。但是算法 的復雜程度及障礙物的多少成正比，且不能保證任何情況下都能獲得最短路徑。因而 該方法僅適用于路徑精度要求不高，機器人速度不快的場合。2.1.2構(gòu)造空間法構(gòu)造空間是指在一個適當?shù)目臻g(機器人的構(gòu)造空間)中將機器人看成一個點， 然后將障礙物映射進該空間中。也就是說將障礙物根據(jù)機器人的幾何尺寸進行相應的 擴展，擴展后，機器人就可以看成一個質(zhì)點了。如圖1所示：其中左邊的為實際工作 空間，右邊的為構(gòu)造空間。圖1構(gòu)造空間Fig.1 Structural atial對于構(gòu)造空間，可以進一步劃分為可視圖法(Visibility Graph)、沃羅諾伊圖法(Voronoi Diagram)和柵格法(Grids)o可視圖法是將機器人視為一點，將機器人、目標點和擴展后的多邊形障礙物的各 個頂點進行組合連接，要求機潛入和障礙物各頂點之間、目標點和障礙物各頂點之間以及各障礙物頂點及頂點之間的連線均不能穿越障礙物，即直線是可視的。如圖2所 示。搜索最優(yōu)路徑的問題變?yōu)榍筮@些可視直線的最短距離問題，可運用優(yōu)化算法比如Dijkstra算法，簡化可視圖，縮短搜索時間。圖2可視圖Fig. 2 Visual chart假設(shè)最優(yōu)路徑是一條固定起始點的繩子，那么拉動繩子處于目標點的一端，當拉 不動時得到的就是最優(yōu)路徑。由此可知，全局最優(yōu)路徑必然經(jīng)過障礙物頂點，因此， 可視圖法有可能求得全局最優(yōu)解。但是根據(jù)可視圖法求得的最優(yōu)路徑距離障礙物太近 甚至接觸，當機器人的輪子打滑或是定位不準的時候就會發(fā)生碰撞。常用的解決方法 是：把機器人擴展的半徑增加，當然，這樣犧牲了可視圖路徑規(guī)劃的最優(yōu)長度。另外, 可視圖法由于建圖的時候?qū)⑵瘘c和目標點考慮進去，因此當起點或目標點發(fā)生改變的 時候就要重新構(gòu)造可視圖，比較麻煩?？梢晥D還有一個缺點就是搜索時間太長，已經(jīng) 有人證明對于有N條線段的環(huán)境來說，時間復雜度是0(N?)。對于帶有圓弧的障礙物, 可視圖法失效，于是產(chǎn)生了沃羅諾伊圖法。Voronoi是在19世紀初由俄國數(shù)學家J. Voronoi忤先提出的一個幾何概念。及可 視圖相比，Voronoi圖傾向于使圖中機器人及障礙物之間的距離最大化。對H由空間 中的各點，計算它到最近障礙物的距離。在離兩個或多個障礙物等距離的點上，這種距離圖就有陡的山脊，Voronoi圖就是由這些陡的山脊點所形成的邊緣組成。當構(gòu)造空間的障礙物都是多邊形時，Voronoi圖由直線和拋物線組成。如圖3所示。圖3沃羅諾伊圖Fig. 3 Voronoi chart在有限距離的定位傳感器情況下，Voronoi圖有一個重要的弱點。因為這種路徑規(guī) 劃算法使機器人及物體之間的距離最大化，機器人上的任何短距離傳感器會有感知不 到周圍環(huán)境的危險。但Voronoi圖由一個重要的難得優(yōu)點，即Voronoi圖具有可執(zhí)行 性。通過Voronoi圖規(guī)劃，給定一個特殊的已規(guī)劃路徑，配備有距離傳感器的機器人 可以使用簡單的控制規(guī)則，跟蹤在物理世界中Voronoi圖的邊緣。這些規(guī)則及創(chuàng)建 Voronoi圖的規(guī)則相匹配：即機器人使其傳感器值的局部極小值最大化。這種控制系 統(tǒng)會自然地使機器人呢保持在Voronoi圖的邊緣上，所以基于Voronoi圖的運動減少 了編碼器的不準確性。柵格法是由Howden W. E.在1968年提出的，他在進行路徑規(guī)劃時采用了柵格表示 地圖。柵格法將機器人的工作空間劃分成一系列具有二值信息的網(wǎng)絡單元。若對環(huán)境 中的某個方向不敏感，則可以將該方向的柵格長度增加，使得單元柵格的形狀為長方 形。另一種柵格表示法是基于四叉樹的表示方法，基于環(huán)境中非相似區(qū)域不斷遞歸分 解。這一方法的好處是在一定程度上節(jié)約了存儲空間。柵格大小的選取直接影響到控制算法的性能。柵格選得小，環(huán)境分辨率高，但抗 干擾能力弱，環(huán)境信息存儲量大，決策速度慢；柵格選得大，抗干擾能力強，環(huán)境信 息存儲量小，決策速度快，但分辨率下降，在密集障礙物環(huán)境中發(fā)現(xiàn)路徑的能力減弱。 柵格大小的選取也及傳感器的性能有關(guān)。如果傳感器精度高而且速度快，柵格可以選 得小些。2.2鏈接圖法環(huán)境建模鏈接圖是一種改進的自由空間法，它可以減少搜索的復雜性。建立鏈接圖的時候 基于以下假設(shè)：1、多邊形障礙物的高度要平行于Z軸，整個路徑存在于XY平面內(nèi)。2、將障礙物的邊界依據(jù)機器人的最大尺寸和機器人正常感知所需的最小范圍進行 擴展，從而可以將機器人相應的簡化為一個質(zhì)點。鏈接圖將環(huán)境中的障礙物以其頂點表示，假設(shè)第i個障礙物。，有，個頂點，則。，可 表示為。=（ m ,）,（/，y2），（/，%,）,而整個環(huán)境可以表示為w=朋民a0, j,其中 WSB為環(huán)境的邊界。環(huán)境的改變只是W中相應元素的增減。要想構(gòu)造鏈接圖，先得定義自由鏈接線（free link）。自由鏈接線滿足以下三個 條件：（1）鏈接線的兩端必須是兩個多邊形障礙物的兩個頂點或者其中一個是障礙物頂 點而另一個在環(huán)境的邊界上。（2）每條自由鏈接線是相鄰兩個自由凸區(qū)域的公共界線。（3）自由鏈接線不能穿越環(huán)境中的任何障礙物。自由鏈接線的計算服從以下規(guī)則：（1）、從障礙物頂點做到環(huán)境邊界的垂線，并將所有障礙物頂點兩兩連接。(2)、將第1步得到的線段按長度的大小從小到達排列。(3)、選擇排序好的線段中的第一條。(4)、檢測該線段是否及任何障礙物的邊界相交。如果相交的話，則該線段不能作 為自由鏈接線，忽略它，并從排好序的線段中取出下一條進行相應的處理。如果不及 任何障礙物的邊界相交，則進入第5步。(5)、檢查這條鏈接線及障礙物頂點所產(chǎn)生的兩個外角。a)如果兩個外角都小于或等于180度，則這條鏈接線是該頂點的最佳鏈接線。 刪去該頂點的其它鏈接線，然后轉(zhuǎn)向第8步。b)如果這條鏈接線的某個外角大于180度，則將這條鏈接線加入到該頂點的候 選鏈接線。(6)、檢查該頂點的所有候選鏈接線中是否還有外角大于180度，如果還有的話， 轉(zhuǎn)第4步，否則轉(zhuǎn)下一步。(7)、刪除該頂點的冗余鏈接線。(8)、對所有的頂點重復第r7步。當從上述步驟中得到自由鏈接線之后，取每條鏈接線的中點并連接起來，則得到 了我們所要的鏈接圖。環(huán)境地圖的自由鏈接線如圖4所示，圖中的黑點表示自由鏈接 線的中點。相應的鏈接圖如圖5所示。圖4環(huán)境中的自由鏈接線Fig. 4 Unconstrained threaded line in the environment圖5鏈接圖Fig. 5 Threaded chart假設(shè)S和G分別是機器人的起始點和目標點，首先看S和G之間的直接連線是否 穿越障礙物，若不穿越，則就得到了最優(yōu)路徑；若穿越，則將S和G加入鏈接圖。加 入S和G以后的鏈接圖如下：連接S和它所處的自由區(qū)域的相鄰中點并對G做同樣的 處理。3使用蟻群算法求解最短路徑將機落人的起始點和目標點加入鏈接圖以后，就可以開始我們的搜索過程了。將 鏈接圖的各個頂點以及起始點和目標點之間的連線賦上權(quán)值。如果頂點之間有連線， 則權(quán)值為相應連線的實際長度，如果兩點之間沒有連線，則權(quán)值為無窮大。經(jīng)過上述 賦予權(quán)值的過程后，我們就將實際工作環(huán)境的靜態(tài)已知部分轉(zhuǎn)換為帶權(quán)圖。而對于帶 權(quán)圖求兩點之間的最短距離可以使用蟻群算法。3.1 蟻群算法的基本原理蟻群黨法是最近兒年才由意大利學者Dorigo, Manierio, Collorni等人提出的一種 新型的模擬進化算法例。在自然界中螞蟻幾乎是瞎子，卻能發(fā)現(xiàn)食物及蟻穴之間最短 的距離。生態(tài)學的研究表明，螞蟻是借助信息素(pheromone)來實現(xiàn)這一點的。信息 素是螞蟻分泌的一種化學物質(zhì)，螞蟻在尋找食物過程中會分泌這種物質(zhì)。蟻群在尋找 食物時，會有一些螞蟻四處游蕩，當螞蟻發(fā)現(xiàn)食物并返回巢穴的過程中會在途中留下 信息素。如果假定所有螞蟻以相同的速度前進的話，則第一只返回巢穴的螞蟻發(fā)現(xiàn)的 路徑是當前所有螞蟻發(fā)現(xiàn)的路徑中最好的。較短的路徑上信息素會比較濃一些，而較 濃的信息素能吸引更多的螞蟻走這條路徑，從而發(fā)現(xiàn)比較好的路徑。我們引用Dorigo所舉的例子來具體說明蟻群系統(tǒng)的原理(Colorni etal, 1991), (張紀會等，1999) o如圖6所示，設(shè)A是巢穴，E是食物源，HC為一障礙物。由于有 障礙物存在，螞蟻只能由H或C由A到達E,或由E到達A,各點之間的距離如圖所示。 設(shè)每個時間單位有30只螞蟻由A到達B,有30只螞蟻由E到達D點，螞蟻過后留下 的信息素為1。為方便，設(shè)該物質(zhì)停留時間為1。在初始時刻，由于路徑BH、BC、DH、 DC上均無信息素存在，位于B和D的螞蟻可以隨機選擇路徑。從統(tǒng)計的角度可以認為 它們以相同的概率選擇BH、BC、DH、DCO經(jīng)過一個時間單位后，路徑BCD上的信息素 濃度是BHD上的2倍。t=l時刻，將有20只螞蟻由B和D到達C,有10只螞蟻由B 和D到達H。隨著時間的推移，螞蟻將會以越來越大的概率選擇路徑BCD,最終完全選 擇路徑BCD,從而找到由蟻巢到食物源的最短路徑。由此可見，螞蟻個體之間的信息 交換是一個正反饋過程。圖6蟻群系統(tǒng)示意圖Fig. 6 The diagrammatic drawing of Ant System3.2 蟻群算法的基本原理我們已經(jīng)使用鏈接圖法將機器人的已知工作環(huán)境轉(zhuǎn)換成了帶權(quán)圖，現(xiàn)在開始講述 蟻群算法在帶權(quán)圖搜索上的應用。假設(shè)帶權(quán)圖中共有n個結(jié)點，機器人起始位置位于 結(jié)點0,目標點位于結(jié)點n-L并以符號m表示螞蟻個數(shù)，%表示結(jié)點i和j之間的 距離，也就是帶權(quán)圖上兩點之間的權(quán)值。%表示t時刻路徑ij上的信息素濃度，” 表示t時刻位于結(jié)點i上的螞蟻個數(shù)。螞蟻一開始都位于結(jié)點0,權(quán)值小于無窮大的路徑上設(shè)置初始信息濃度C。螞蟻從 結(jié)點0開始按概率選擇下一結(jié)點。下一結(jié)點i被選中的概率正比于路徑3上的信息素 濃度以及啟發(fā)信息，本文中將啟發(fā)信息選取為反比于兩節(jié)點之間的路徑段的權(quán) 值。假設(shè)第k只螞蟻選擇了結(jié)點i作為下一結(jié)點后，將結(jié)點i放入該螞蟻的禁忌表中, 然后第k只螞蟻就繼續(xù)從結(jié)點i開始按同樣的方法選擇下一結(jié)點，如此繼續(xù)下去，直 到該螞蟻到達目標節(jié)點nT。當所有螞蟻都到達目標結(jié)點后，就完成了一個搜索周期。 接下來更新每個路徑段上的信息素并清空每只螞蟻的禁忌表。當這些做完了以后，將 所有螞蟻位置重置為結(jié)點0,然后開始下一個周期的搜索。如此往復，直到最終達到 搜索終止條件。路徑段上的信息素隨著時間的推移不斷揮發(fā)，當完成一次搜索后，螞蟻又為走過 的路徑段上增加一定數(shù)量的信息素，使得被較多螞蟻走過的路徑段上留下更多的信息 素，而較少螞蟻走過的路徑段上信息素由于揮發(fā)作用而不斷減少，這稱為信息素的更 新。每只螞蟻的下一步可選結(jié)點為及當前結(jié)點直接相連的并且未放入禁忌表中的結(jié)點。 禁忌表是為了防止螞蟻往回走。信息素濃度的更新方式如公式1所示：% + l) = 0F*) + f(fj + l)其中p為揮發(fā)系數(shù)，+ 為第t次循環(huán)完成后在路徑段ij上增加的信息素濃 度。馬為開始第t次循環(huán)時路徑段ij上的信息素濃度，行" + 1)為完成第t次循環(huán) 后路徑段ij上的信息素濃度。其中金(小+ 1)的計算公式如2式所示：mr/+ l) = Z耳(小 + 1)(2)其中力(S+1)為第k只螞蟻在第k次循環(huán)中為路徑段ij增加的信息素濃度。根 據(jù)力(U+1)的計算方式以及何時更新得可以得到三種不同的算法模型： ANT-density, ANT-quantity和ANT-cycle。三種模型的表達式分別如下3, 4, 5所示:(3)(4)(5)K 八f<21 若第人只螞蟻葩和1 + 1之間經(jīng)過路徑段/ w+f否則2 若第I只螞蟻在/和，+1之間經(jīng)過路徑質(zhì)/0 否則& 若第只螞蟻在本次循環(huán)中經(jīng)武路徑段% “，f+i)=r 40 否則在第t次循環(huán)中，第k只螞蟻從結(jié)點i轉(zhuǎn)移到結(jié)點j的概率為/，其計算公式 如6式所示:M明為明若. d一!- !百 彳 e allowed.P” Z監(jiān)帆酬（6）sallowdk0否則.其中，“0卬64=。-必骰表示螞蟻k下一步允許選擇的結(jié)點集合；C表示帶權(quán)圖 所有結(jié)點集合；禁忌表必修（攵=12表示螞蟻k當前循環(huán)中走過的結(jié)點集合；啟發(fā) 信息為的計算方式如式7所示；參數(shù)a表示信息素濃度的重要性，a越大表示信息 素濃度在螞蟻路徑選擇中所起的作用越大，螞蟻更傾向于選擇有更多螞蟻走過的路徑, 螞蟻之間的協(xié)作性就越強；參數(shù)人表示啟發(fā)信息的重要程度，夕越大表示啟發(fā)信息在 螞蟻路徑選擇中所起的作用越大，螞蟻更傾向于選擇下一路徑段最短的結(jié)點，蟻群算 法就越接近及貪心算法。（7）蟻群算法的基本流程如下：（1）將所有螞蟻的初始位置設(shè)置為機器人起始點0,并且對所有的可行路徑（即權(quán) 值小于正無窮的路徑）賦予初始信息濃度C,即令%（0） =。（2）對每一只螞蟻搜索可行路徑，根據(jù)每一條可行路徑上的信息素濃度和啟發(fā)信 息計算得到螞蟻選擇該路徑作為下一條路徑段得概率，螞蟻根據(jù)概率來選擇下一條路 徑段。并將選擇的路徑段的另一個結(jié)點放入該螞蟻的禁忌表。（3）當所有螞蟻到達目標點后更新全局信息素和局部信息素，然后清空每只螞蟻 的禁忌表。（4）根據(jù)搜索到的路徑長度是否明顯減少或是否達到最大搜索次數(shù)來判斷是否結(jié) 束搜索。如果搜索結(jié)束條件未達到，則轉(zhuǎn)向第2步進行下一個周期的搜索。由算法復雜性理論可知，m只螞蟻要遍歷n個結(jié)點，如果搜索次數(shù)為N,那么算法 的時間復雜度是O（N%2）。4路徑跟蹤4.1路徑跟蹤的概念路徑跟蹤是要讓機器人沿一條預先指定的路徑移動。在這里預先指定的路徑即是 前面使用蟻群算法規(guī)劃出來的全局最優(yōu)路徑。全局最優(yōu)路徑由一系列首尾相連的線段 組成，每條線段用該線段的兩個端點來表示。在這里介紹了兩種路徑跟蹤方法:Follow the Carrot和Pure Pursuit,這兩種 跟蹤方法都是使用一個始終處于全局最優(yōu)路徑上的Carrot點來引導機器人運動。這兩 種方法簡單而有效。4. 2 Follow the Carrot 方法在Follow the Carrot方法中，將Carrot設(shè)置為路徑段上距離機器人一定距離的 點，這個距離通常稱之為Look Ahead Distance,跟蹤策略就是機器人一直朝著Carrot 點的方向前進。Follow the Carrot方法中，最關(guān)鍵的就是Carrot的求取。當?shù)玫搅?Carrot點以 后之間連接機落人和Carrot點，連線的方向就是機器人下一步前進的方向。要求取 Carrot點首先得找到機器人在路徑上的最近點pnear,然后從最近點pnear開始沿著 路徑段前進長度為Look Ahead Distance的距離，此時得到的點就是我們想要的Carrot 點了。機港人到路徑段上的距離d的定義如圖7所示。-22-/22V=p= Center rointPl圖7機器人到路徑段上的距離Fig. 7 The distance of robot geting to the path segment圖中P表示機器人當前位置的中心點，兒和耳分別表示全局最優(yōu)路徑上的一條路徑 段的兩個端點，丹表示路徑段出上距離P最近的點，以尸記,)表示機器人到路徑段庶匕 上的距離。d的計算方法如下所示："(P/) 如果w，小于等于0d< d(P、PJ 如果wKwv(8)d(P、PJ 其他.其中，W表示向量P-玲，V表示向量-0，6,的計算如式9,"(,)表示兩點之間的 歐式距離。在式8的前兩種情況下，R分別等于庶和R。(9)求取機器人到全局最優(yōu)路徑的所有路徑段上的距離，并找出其中最小的一個，距 離最小的一個對應的打就是我們所需要的2e，如圖8所示。Fig. 8 Find out the point of pnear黑點表示機器人，P, Q, R是機器人分別到三條路徑段得距離，由于Q到機器人的 距離最短，因此在圖中情況下，就是Q點。由Pi點求取Carrot點的方法如圖9 所示。圖9求取Carrot點Fig. 9 Work out the point of Carrot在上圖中，從匕.點開始沿著路徑段44前進Look Ahead Distance的距離就得到 Carrot點。如果Pnear到R的距離小于Look Ahead Distance,則進入下一條路徑 段并前進在外片 中不足的長度。如果一直到最后一條路徑段都沒有達到Look Ahead Distance的長度，則將Carrot點設(shè)置為最后一條路徑段的末端端點。8表示機器人當前方位角，為Carrot點及機器人中心點之間的連線及X軸的夾 角，機器人的轉(zhuǎn)向角 =核-夕，轉(zhuǎn)向角保證在-"和乃之間。4. 3 Pure Pursuit 方法Pure Pursuit是Follow the Carrot的改進版本，它們二者十分相似,主要區(qū)別 在于Follow the Carrot方法是機潛人沿直線趨向于Carrot點，而Pure Pursuit方 法是以去圓弧的方式趨向于Carrot點。機器人所走的圓弧是唯一的，它保證機器人在 每一時刻的前進方向都是圓弧的切線方向。這種沿圓弧運動的方式使得機器人的轉(zhuǎn)向 更加平滑，減少了震蕩的可能性。Pure Pursuit算法描述如下:(1)、獲取機器人的當前位置。(2)、按照跟Follow the Carrot同樣的方式找出Pnear點。(3)、按照和Follow the Carrot同樣的方式找到Carrot點。(4)、將Carrot點坐標用機器人的局部坐標系表示，因為計算圓弧的曲率的公式 只有在機器人局部坐標系下才有效。機器人局部坐標系的y軸選取為當前前進方向。(5)、計算曲率/ ="，其中r是機器人的轉(zhuǎn)向半徑。(6)、計算轉(zhuǎn)向角。如圖10所示，XY表示全局坐標系，xy表示機器人局部坐標 系，(天,久)表示Carrot點在局部坐標系下的坐標，VCP表示機器人中心點，0表示圓 弧的圓心位置，D為機器人中心點及Carrot點之間的距離。轉(zhuǎn)向角計算公式的推導過 程如下：片 + 4=。2( 10)x + d =r(11)d2 + y- = r2(12)將式11代入12式中展開并代入10式化簡得到2rAx = D2(13)由式(13)可得，由于a = /-e + ,At = Ocosa所以可求出_ 1 _ D2 ，2at 2-D-cos(y- + /)(14)(15)式(14)中的Length表示機器人的長度，至此可以求出轉(zhuǎn)向角了。圖10 Pure Pursuit計算轉(zhuǎn)向角的示意圖Fig. 10 Calculate the angle of turn with Pure Pursuit5總結(jié)機港人導航問題一直是機潛人應用中的熱點和難點問題，特別是對于服務類型的 機器人來說。雖然全局環(huán)境已知的情況下，機器人的路徑規(guī)劃問題已經(jīng)研究的比較透 徹了，但是對于環(huán)境部分未知的情況下還沒有一個普適性的算法。機器人的一個重要 應用就在及服務領(lǐng)域，而服務機器人的工作環(huán)境大部分都是不斷改變的，因此，研究 不確定環(huán)境下的導航問題具有十分重要的意義。本文簡單介紹了機器人的發(fā)展歷史， 國內(nèi)外在移動服務機器人方面的研究現(xiàn)狀，概括性地描述了移動機器人導航的研究方 法。主要介紹了全局地圖的構(gòu)造方法、現(xiàn)有的主要兒種全局路徑規(guī)劃方法：自由空間 法和構(gòu)造空間法。著重介紹了自由空間法中的鏈接圖法，將全局路徑規(guī)劃問題轉(zhuǎn)化為 帶權(quán)圖中查找兩個節(jié)點的最短路徑問題。然后采用蟻群算法規(guī)劃出全局最優(yōu)路徑。重 點介紹/ Follow The Carrot和Pure Pursuit兩種路徑跟蹤方法。致謝本論文的工作是在陳萬米老師的指導下完成的，在此我要向陳老師表示衷心的感謝！陳老師的指導和幫助，讓我學會怎樣去獨立思考、分析、設(shè)計，如何發(fā)現(xiàn) 問題的本質(zhì)，如何通過查閱資料來獲取自己所需的知識。除在科研方面陳老師給予了 我很大的指導和幫助外，同樣感謝陳老師在我平時的生活中給予的幫助。此外，感謝 實驗室?guī)熜?、同學在論文工作期間給予的幫助。我還要感謝參考文獻論文的作者，是 你們給了我豐富的資料和理論依據(jù)，在此感謝你們。參考文獻：1許心德.環(huán)境部分未知情況下的服務機器人導航研究D.中國科學技術(shù)大 學 20092森相博.豪庭環(huán)境只能空間中服務機器人導航技術(shù)研究D.山東大學,2010.3董承鵬.家庭服務機器人導航系統(tǒng)研究DL天津大學,2008.4蔡自興,賀漢根,陳虹.未知環(huán)境中移動機器人導航控制研究的若干問題J.控制 及決策，2002 (14):385-463.5陳華志,謝存禧.移動機器人導航及其相關(guān)技術(shù)的研究J.機床及液 壓，2003(4):12-15.6陳煒.帶雜交算子的蟻群算法J.計算機工程,2001(12) : 74-76.7郭戈,胡征峰,董江輝.移動機器人導航及定位技術(shù)J.微計算機信 S,2003(8) : 10-12.8 Khatib M, Chatila R. 1995. An extended potential field approach for mobile robot sensor-based motionsC. Proceedings of Intelligent Autonomous Systems, 490-496._9 Fan Wu, Cabral, M. , Li Jiang. Design and Implementation of a Path Finding Autonomous Robot System Using Multiple SensorsC. 2010 International Conference on Computational Intelligence and Software Engineering. IEEE, Piscataway, NJ, USA, 2010.10 Rong Du, Xiaobin Zhang, Cailian Chen, Xinping Guan. Path planning with obstacle avoidance in PEGs: Ant colony optimization method. 2010 IEEE/ACM Int 1 Conference on Green Computing and Communications (GreenCom) and Int1 Conference on Cyber, Physical and Social Computing (CPSCom). IEEE Computer Society, Los Alamitos, CA, USA, 2010:768-73.