多边形之间相交求交点的算法_路径规划算法总结
本文來自知乎網友@搬磚的旺財，地平線機器人算法工程師。作者根據自己本科和碩士階段的學習經歷，整理歸納了所接觸過的規劃算法。
1.自主機器人近距離操作運動規劃體系
在研究自主運動規劃問題之前，首先需建立相對較為完整的自主運動規劃體系，再由該體系作為指導，對自主運動規劃的各項具體問題進行深入研究。本節將根據自主機器人的思維方式、運動形式、任務行為等特點，建立與之相適應的自主運動規劃體系。并按照機器人的數量與規模，將自主運動規劃分為單個機器人的運動規劃與多機器人協同運動規劃兩類規劃體系。1.1單個自主機器人的規劃體系運動規劃系統是自主控制系統中主控單元的核心部分，因此有必要先研究自主控制系統和其主控單元的體系結構問題。
自主控制技術研究至今，先后出現了多種體系結構形式，目前被廣泛應用于實踐的是分布式體系結構，其各個功能模塊作為相對獨立的單元參與整個體系。隨著人工智能技術的不斷發展，基于多Agent的分布式體系結構逐漸成為了主流，各功能模塊作為獨立的智能體參與整個自主控制過程，該體系結構應用的基本形式如圖1所示。一方面，主控單元與測控介入處理、姿態控制系統、軌道控制系統、熱控系統、能源系統、數傳、有效載荷控制等功能子系統相互獨立為智能體，由總線相連；另一方面，主控單元為整個系統提供整體規劃，以及協調、管理各子系統Agent的行為。測控介入處理Agent保證地面系統對整個系統任意層面的控制介入能力，可接受上行的使命級任務、具體的飛行規劃和底層的控制指令；各子系統Agent存儲本分系統的各種知識和控制算法，自主完成主控單元發送的任務規劃，并將執行和本身的健康等信息傳回主控單元，作為主控單元Agent運行管理和調整計劃的依據。
圖1 基于多Agent的分布式自主控制系統體系結構基本形式示意圖主控單元Agent采用主流的分層遞階式結構，這種結構層次鮮明，并且十分利于實現，其基本結構如圖2所示。主控單元由任務生成與調度、運動行為規劃和控制指令生成三層基本結構組成，由任務生成與調度層獲得基本的飛行任務，經過運動行為規劃層獲得具體的行為規劃，再由控制指令生成層得到最終的模塊控制指令，發送給其它功能Agent。各功能Agent發送狀態信息給主控單元的狀態檢測系統，狀態檢測系統將任務執行情況和子系統狀態反饋回任務生成與調度層，以便根據具體情況對任務進行規劃調整。當遇到突發情況時，還可啟用重規劃模塊，它可根據當時情況迅速做出反應快速生成行為規劃，用以指導控制指令生成層得到緊急情況的控制指令。此外，地面控制系統在三個層次上都分別具有介入能力。圖2中，點劃線內是主控單元全部模塊，虛線內為運動規劃系統，包括運動行為規劃模塊和重規劃模塊，這也是運動規劃系統的主要功能。
圖2 主控單元基本結構示意圖明確了自主控制系統與其主控單元的基本結構，以及運動規劃系統在主控單元中的基本功能，便可建立運動規劃系統的體系結構。運動規劃系統的體系結構如圖3所示，該系統由規劃器和重規劃器兩大執行單元組成，分別承擔對飛行任務的一般規劃和對突發事件緊急處理的運動規劃。當然，這兩部分也可理解為離線規劃與在線規劃兩種，離線規劃一般解決平時按部就班的飛行任務，在線規劃一般解決突然下達的飛行任務。除規劃器以外，系統還配有知識域模塊，用以利用特定語言描述相關知識。知識域包括行為域和模型域兩個部分，行為域用來存儲服務系統一般的運動行為描述和緊急情況下的一些運動行為方面的處理方法(如急停、轉向等)，模型域用來存儲規劃所需模型知識，包括環境模型、組裝體模型、組裝任務對象模型和任務模型等等。
圖3 運動規劃系統體系結構示意圖1.2多自主機器人協同規劃體系多智能體系統的群體體系結構一般分為集中式、分散式兩種基本結構，分散式結構又可以進一步分為分層式和分布式結構。集中式結構通常由一個主控單元掌握全部環境和受控機器人信息，運用規劃算法對任務進行分解，并分配給各受控機器人，組織它們完成任務。其優點是理論條理清晰，實現較為直觀；缺點是容錯性、靈活性和對環境的適應性較差，與各受控機器人存在通訊瓶頸問題。相對于集中式結構，分散式結構無法得到全局最優解，但它憑借著可靠性、靈活性和較強的環境適應性越來越受到廣泛的青睞。分散式結構中的分布式結構沒有主控單元，各智能體地位平等，通過各智能體間的通訊和信息交流達到協商的目的，實現最終的決策，但該結構容易片面強調個體，導致占用資源過多，且難于得到磋商結果。分層式結構介乎于集中式和分布式之間，存在主控單元，但并不是由主控單元掌控一切，各智能體也具備一定的自主性，上下級之間按照一定的規則，通過信息流形成完整的整體，共同完成協同任務。
多自主機器人系統應采用分層式結構，以保證整個系統既適于統一領導，又滿足系統靈活、快速的需求。多自主機器人協同規劃體系結構如圖4所示，按照分層式結構建立兩種工作模式：事先的離線規劃由主控單元負責，首先獲得協同任務，經過規劃器得到具體的行為運動規劃，并分發給各分系統執行單元，相關的知識域中主要是用于描述各分系統協商規則的協商域，主控單元從外界獲取環境信息，從各分系統獲取狀態信息；當遇到突發事件或緊急任務變更以及主控單元停止工作時，各分系統采用分布式結構，單獨規劃各自運動行為，并從各自的知識域中獲取協商方式，外界環境信息由主控單元發送和自我感知相結合獲得(主控單元停止工作時，僅靠自我感知獲取信息)，其它機器人信息的傳輸由機器人間的數據鏈實現。
圖4 多自主機器人協同規劃體系結構示意圖2.路徑規劃研究
當給定了某一特定的任務之后，如何規劃機器人的運動方式將至關重要。機器人的規劃包括兩部分內容：基座移動到適合操作的位置和轉動手臂關節完成操作。包括三個問題：基座點到點運動規劃；關節空間規劃；綜合規劃。
本章研究幾種常用的運動規劃算法：圖搜索法、RRT算法、人工勢場法、BUG算法。并對部分算法的自身缺陷進行了一些改進。
2.1圖搜索法圖搜索法依靠已知的環境地圖以及地圖中的障礙物信息構造從起點到終點的可行路徑。主要分成深度優先和廣度優先兩個方向。深度優先算法優先擴展搜索深度大的節點，可以快速的得到一條可行路徑，但是深度優先算法得到的第一條路徑往往是較長的路徑。廣度優先算法優先擴展深度小的節點，呈波狀的搜索方式。廣度優先算法搜索到的第一條路徑就是最短路徑。
2.1.1 可視圖法
可視圖法由Lozano-Perez和Wesley于1979年提出，是機器人全局運動規劃的經典算法。可視圖法中，機器人用點來描述，障礙物用多邊形描述。將起始點??、目標點??和多邊形障礙物的各頂點(設??是所有障礙物的頂點構成的集合)進行組合連接，要求起始點和障礙物各頂點之間、目標點和障礙物各頂點之間以及各障礙物頂點與頂點之間的連線均不能穿越障礙物，即直線是“可視的”。給圖中的邊賦權值，構造可見圖??。其中點集??，??為所有弧段即可見邊的集合。然后釆用某種優化算法搜索從起始點??到目標點??的最優路徑，那么根據累加和比較這些直線的距離就可以獲得從起始點到目標點的最短路徑。
圖5 可視圖由此可見，利用可視圖法規劃避障路徑主要在于構建可視圖，而構建可視圖的關鍵在于障礙物各頂點之間可見性的判斷。判斷時主要分為兩種情況，同一障礙物各頂點之間可見性的判斷以及不同障礙物之間頂點可見性的判斷。
同一障礙物中，相鄰頂點可見(通常不考慮凹多邊形障礙物中不相鄰頂點也有可能可見的情況)，不相鄰頂點不可見，權值賦為??。
不同障礙物之間頂點可見性的判斷則轉化為判斷頂點連線是否會與其它頂點連線相交的幾何問題。如下圖虛線所示，、?分別是障礙物?、?的頂點，但??與??連線與障礙物其它頂點連線相交，故?、?之間不可見；而實線所示的??與??連線不與障礙物其它頂點連線相交，故??、??之間可見。
可視圖法能求得最短路徑，但搜索時間長，并且缺乏靈活性，即一旦機器人的起始點和目標點發生改變，就要重新構造可視圖，比較麻煩。可視圖法適用于多邊形障礙物，對于圓形障礙物失效。切線圖法和Voronoi圖法對可視圖法進行了改進。切線圖法用障礙物的切線表示弧，因此是從起始點到目標點的最短路徑的圖，移動機器人必須幾乎接近障礙物行走。其缺點是如果控制過程中產生位置誤差，機器人碰撞障礙物的可能性會很高。Voronoi圖法用盡可能遠離障礙物和墻壁的路徑表示弧。因此，從起始點到目標點的路徑將會增長，但采用這種控制方式時，即使產生位置誤差，移動機器人也不會碰到障礙物。
2.1.2 Dijkstra算法
Dijkstra算法由荷蘭計算機科學家艾茲赫爾·戴克斯特拉(Edsger Wybe Dijkstra)發明，通過計算初始點到自由空間內任何一點的最短距離可以得到全局最優路徑。算法從初始點開始計算周圍4個或者8個點與初始點的距離，再將新計算距離的點作為計算點計算其周圍點與初始點的距離，這樣計算像波陣面一樣在自由空間內傳播，直到到達目標點。這樣就可以計算得到機器人的最短路徑。
Dijkstra算法是一種經典的廣度優先的狀態空間搜索算法，即算法會從初始點開始一層一層地搜索整個自由空間直到到達目標點。這樣會大大增加計算時間和數據量。而且搜索得到的大量對于機器人運動是無用的。
詳情請參考：路徑規劃——Dijkstra算法：https://zhuanlan.zhihu.com/p/51112799
2.1.3 A*算法
為了解決Dijkstra算法效率低的問題，A*算法作為一種啟發式算法被提出。該算法在廣度優先的基礎上加入了一個估價函數。
詳情請參考：路徑規劃——A*算法：https://zhuanlan.zhihu.com/p/51099376
2.2RRT算法快速搜索隨機樹(RRT)算法是一種增量式采樣的搜索方法，該方法在應用中不需要任何參數整定，具備良好的使用性能。它利用增量式方法構建搜索樹，以逐漸提高分辨能力，而無須設置任何分辨率參數。在極限情況，該搜索樹將稠密的布滿整個空間，此時搜索樹由很多較短曲線或路經構成，以實現充滿整個空間的目的。增量式方法構建的搜索樹其導向取決于稠密采樣序列，當該序列為隨機序列時，該搜索樹稱為快速搜索隨機樹(Rapidly Exploring Random Tree，RRT)，而不論該序列為隨機還是確定性序列，都被稱為快速搜索稠密樹(Rapidly Exploring Dense Trees，RDTs)，這種規劃方法可處理微分等多種約束。
2.2.1 算法步驟
考慮二維和三維工作空間，環境中包含靜態障礙物。初始化快速隨機搜索樹T，只包括根節點，即初始狀態S。在自由空間中隨機選取一個狀態點??，遍歷當前的快速隨機搜索樹T，找到T上距離??最近的節點??，考慮機器人的動力學約束從控制輸入集??中選擇輸入??，從狀態??開始作用，經過一個控制周期??到達新的狀態??。滿足?與?的控制輸入??為最佳控制量。將新狀態??添加到快速隨機搜索樹T中。按照這樣得到方法不斷產生新狀態，直到到達目標狀態G。完成搜索樹構建后，從目標點開始，逐次找到父節點直到到達初始狀態，即搜索樹的根節點。
圖7 隨機樹構建過程由于在搜索過程中考慮了機器人的動力學約束，因此生成的路徑的可行性很好。但是算法的隨機性導致其只具備概率完備性。
2.2.2 改進算法
LaValle等人的工作奠定了RRT方法的基礎。在采樣策略方面，RRTGoalBiaS方法在控制機器人隨機運動的同時，以一定概率向最終目標運動；RRTGoalZoom方法分別在整個空間和目標點周圍的空間進行采樣；RRTCon方法則通過加大隨機步長改進規劃速度。雙向規劃思想也被采用，衍生出RRTExtExt，RRTExtCon，RRTConCon等多種算法。
基本RRT算法收斂到終點位姿的速度可能比較慢。為了提高算法的效率和性能，需不斷對該算法進行改進。如為了提高搜索效率采用雙向隨機搜索樹(Bi~RRT)，從起始點和目標點并行生成兩棵RRT，直至兩棵樹相遇，算法收斂。由于這個算法相比于原始RRT有更好的收斂性，因此在目前路徑規劃中是很常見的。NikAMelchior提出的粒子RRT算法，考慮了地形的不確定性，保證了在不確定性環境下搜索樹的擴展。Kuffner和Lavane又提出RRT-connectlv，使得節點的擴展效率大大提高。運動規劃中，距離的定義非常復雜，Pengcheng研究了在RRT生長過程中距離函數不斷學習的算法以降低距離函數對環境的敏感性。考慮到基本RRT規劃器得到的路徑長度一般是最優路徑的1.3~1.5倍，英國的J.desmithl研究了變分法技術使其達到最優。Amna A引入KD樹作為二級數據結構加速查找距離從環境中取出的隨機點最近的葉節點，降低了搜索成本。該算法在動態障礙物、高維狀態空間和存在運動學、動力學等微分約束的環境中的運動規劃已經得到廣泛的應用。
關于改進RRT算法詳情可參考：路徑規劃——改進RRT算法：https://zhuanlan.zhihu.com/p/51087819
2.3滾動在線RRT算法基本RRT算法傾向于遍歷整個自由空間直到獲得可行路徑，這使其不可能用于未知或動態環境中的機器人在線運動規劃。利用滾動規劃的思想可以將RRT算法進行改進，使其具備在線規劃能力。
2.3.1 滾動規劃
機器人在未知或動態環境中運動時，只能探知其傳感器范圍內有限區域內的環境信息。機器人利用局部信息進行局部運動規劃，并根據一定的評價準則得到局部目標。機器人到達局部目標后再次進行新的局部規劃。如此反復進行直到到達全局目標。
滾動規劃算法的基本原理：
環境信息預測：在滾動的每一步，機器人根據探測到的視野內的信息、或所有已知的環境信息，建立環境模型，包括設置已知區域內的節點類型信息等；
局部滾動優化：將上述環境信息模型看成一個優化的窗口，在此基礎上，根據目標點的位置和特定的優化策略計算出下一步的最優子目標，然后根據子目標和環境信息模型，選擇局部規劃算法，確定向子目標行進的局部路徑，并實施當前策略，即依所規劃的局部路徑行進若干步，窗口相應向前滾動；
反饋信息校正：根據局部最優路徑，驅動機器人行走一段路徑后，機器人會探測到新的未知信息，此時可以根據機器人在行走過程探測到的新信息補充或校正原來的環境模型，用于滾動后下一步的局部規劃。
其中，局部子目標是在滾動窗口中尋找一個全局目標的映射，它必須避開障礙物，且滿足某種優化指標。子目標的選擇方法反映了全局優化的要求與局部有限信息約束的折衷，是在給定信息環境下企圖實現全局優化的自然選擇。
基于滾動窗口的路徑規劃算法依靠實時探測到的局部環境信息，以滾動方式進行在線規劃。在滾動的每一步，根據探測到的局部信息，用啟發式方法生成優化子目標，在當前滾動窗口內進行局部路徑規劃，然后實施當前策略(依局部規劃路徑移動一步)，隨滾動窗口推進，不斷取得新的環境信息，從而在滾動中實現優化與反饋的結合。由于規劃問題壓縮到滾動窗口內，與全局規劃相比其計算量大大下降。
基于滾動窗口的路徑規劃算法的具體步驟如下：
步驟0：對起點、終點、工作環境、機器人的視野半徑、步長進行初始化；
步驟1：如果終點到達，規劃中止；
步驟2：對當前滾動窗口內的環境信息進行刷新；
步驟3：產生局部子目標；
步驟4：根據子目標及已知環境信息，在當前滾動窗口內規劃一條優化的局部可行路徑；
步驟5：依規劃的局部路徑行進一步，步長小于視野半徑；
步驟6：返回步驟1。
2.3.2 滾動在線RRT算法流程
在一個滾動窗口內，隨機樹以當前位置為起始點，構建傳感器范圍內的隨機樹。構建方法與基本RRT算法一致。為了使全局環境中隨機樹具有向目標方向生長的趨勢，在運動規劃時引入啟發信息，減少隨機樹的隨機性，提高搜索效率。
令??代表隨機樹中兩個位姿節點間的路徑代價，??代表隨機樹中兩個位姿節點間的歐幾里德距離。類似于A*算法，本算法為隨機樹中每個節點定義一個估價函數：??。其中??是隨機節點??到樹中節點??所需的路徑代價。??為啟發估價函數，這里取隨機節點??到目標點??的距離為估價值，??。因此??表示從節點??經隨機節點??到目標節點??的路徑估計值。遍歷滾動窗口內隨機樹T，取估價函數最小值的節點??，有??。這使得隨機樹沿著到目標節點估價值??最小的方向進行擴展。
由于在隨機樹生長中引入了導向目標的啟發估價因子，葉節點??總是選擇離目標最近的節點，這可能會使隨機樹遇到局部極小值問題。因此隨機樹生長的新節點??必須要克服這個問題，引導隨機樹更好的探索未知空間。
這里利用統計學中回歸分析生成新節點，將RRT算法探索未知空間的能力進一步增強以避免因啟發估價因子導致的局部極小。其思想是探索以前到過的空間是無用的，而且容易陷入局部極小。引進回歸分析(regression analysis)是考察新節點與其他節點之間關系，利用回歸函數約束，使得隨機樹不探索以前到過的空間，因此避免了局部極小。
新節點生成方法是遍歷隨機樹，如果??與其父節點??的距離小于??與擴展樹上其他任意節點的距離，即??，則選擇該節點為隨機樹新生節點。下圖解釋了新節點的判斷過程。
圖8 新節點的判斷上圖中各個空心點是中間的父節點的可能擴展。橢圓圈起的空心點表示這個新節點不符合回歸函數約束，剩下的兩個未被圈起的空心節點到其父節點的距離小于該節點到隨機樹上任意節點的距離，這兩個點可以成為隨機樹的新節點。
綜上，滾動窗口內隨機樹構建的具體步驟如下：
對滾動窗口隨機樹T初始化，T開始只包含初始位置S；
滾動窗口自由空間中隨機選擇一個狀態?；
根據最短路徑思想尋找樹T中和??距離最近的節點?；
選擇輸入??，使機器人狀態由??到?；
確定??是否符合回歸分析，不符合則回到第4步；
將??作為隨機樹T的一個新節點，??則被記錄在連接節點??和??的邊上。
滾動窗口狀態空間進行K次采樣后，遍歷隨機樹，根據啟發估價思想尋找滾動窗口子目標?。??是當前滾動窗口中的子樹中估價函數最小的點。確定子目標后，機器人前進到子目標點，進行下一輪的滾動RRT規劃。如此反復，直到到達目標點G。
2.4人工勢場法人工勢場法是由Khatib提出的一種用于機器人運動規劃的虛擬力方法。其基本思想是將目標和障礙物對機器人運動的影響具體化成人造勢場。目標處勢能低，障礙物處勢能高。這種勢差產生了目標對機器人的引力和障礙物對機器人的斥力，其合力控制機器人沿勢場的負梯度方向向目標點運動。人工勢場法計算方便，得到的路徑安全平滑，但是復雜的勢場環境可能在目標點之外產生局部極小點導致機器人無法到達目標。為了解決人工勢場法的局部極小點問題，學者們提出了各種改進方法。主要分成兩個方向：一個是構造合適的勢函數以減小或避免局部極小點的出現；另一種是在機器人遇到局部極小點后結合其他的方法使機器人離開局部極小點。前者一般需要全局地圖信息，并且依賴于障礙物的形狀。當環境復雜時難以應用。后者多利用搜索法、多勢場法和沿墻行走法等方法使機器人離開局部極小點。搜索法利用最佳優先、模擬退火、隨即搜索等策略尋找比局部極小點勢場值更低的點使機器人繼續移動。由于未知環境中大多缺乏啟發信息，搜索方法的效率很低。多勢場法構造多個全局極小點相同，而局部極小點不同的勢函數，在機器人陷入某個局部極小點時，規劃器就切換勢函數使機器人離開該點。但是在未知的環境中這樣的多個勢場很難構造，而且該方法可能導致機器人在回到曾逃離的局部極小點。由于局部極小點是某個或多個障礙物的斥力勢場與引力勢場共同作用產生，其位置與障礙物距離必然不遠，沿墻行走法正是利用這樣的遠離，使機器人在遇到局部極小點后參照類似BUG算法的環繞行為繞過產生局部極小點的障礙物繼續前進。這種方法可靠性高，不依賴環境的先驗信息和障礙物形狀。
本節構造人工勢場進行機器人平動的在線運動規劃，利用一種沿墻行走法對基本的人工勢場法進行改進。
2.4.1 基本人工勢場法
作用在機器人上的假想引力和斥力為勢函數的負梯度，因而人工勢函數應該具有以下特征：
非負且連續可微；
斥力勢強度距離障礙物越近其強度越大；
引力勢強度離目標位置越近其強度越小。
空間中的合勢場是引力勢場與斥力勢場之和：?
其中，??是目標產生的引力勢場；??是各個障礙物產生的斥力勢場之和，即：?。
這里構造如下的引力勢函數和斥力勢函數：
其中，??表示引力勢的相對影響；??表示第??個障礙物的斥力勢的相對影響，??表示機器人當前位置，??表示目標點位置，??表示機器人距目標的距離，??的作用是在機器人距離目標較遠時，削弱目標引力勢的作用，??表示機器人距離第??個障礙物的距離，??表示第??個障礙物的斥力勢作用范圍。
和?對勢場形狀的影響很大，適當的增大??能夠增強引力勢場的作用，有助于減少產生局部極小點的可能，并加快機器人向目標運動。??影響機器人在障礙物附近的運動特性，??比較大可以使機器人距離障礙物更遠，運動路徑更安全；??比較小，機器人在避開障礙物時運動比較平滑。
利用上面勢函數的梯度可以計算機器人收到的假想引力和斥力：
2.4.2 人工勢場法算法改進
當機器人的運行環境中包含形狀復雜或者距離很近的障礙物時，可能出現勢場局部極小點，導致機器人在該處停止或在其周圍振動。如下圖所示，當環境中出現“陷阱”形障礙物或者與目標成特定位置關系的障礙物時，可能在人工勢場中產生局部極小點(圖中L點)，當機器人運動到局部極小點附近時，勢場的負梯度方向指向L點。機器人將在L點處停止或在其附近振動或作圓周運動。
圖9 人工勢場法的局部極小點為了使機器人從局部極小點中逃離，在人工勢場法的基礎上引入應激行為，即增加繞行行為。當機器人遇到局部極小點時，忽略目標引力勢的作用，沿著斥力勢的等勢面方向移動，直到機器人離開局部極小區域。改進的算法流程如下：
根據傳感器信息計算當前位置的引力和斥力；
判斷是否處于繞行行為，若是，執行3；若否，執行4；
判斷是否離開局部極小區域，若是，機器人沿著合力方向運動，結束繞行行為；若否，機器人沿著斥力場等勢線運動，繼續繞行行為；
判斷是否遇到局部極小點，若是，機器人沿著斥力場等勢線運動，開始繞行行為；若否，機器人沿著合力方向運動；
判斷是否到達目標，若是，退出算法；若否，繼續1；
使用下面的判別條件判斷機器人是否遇到局部極小點。
條件1：?
條件2：?
當條件1或者條件2出現時，就認為機器人遇到了局部極小點。條件1中??是一個很小的正數，其含義是機器人受到的虛擬合力接近0。這是最直接局部極小點判斷方法。條件2中??為0,1之間某一正數，??為機器人運動過程中某一狀態，??表示機器人從??到達當前位置??的總路程，條件2成立意味著機器人在運動很長路程后，位移很小。用來檢測機器人在局部極小點附近發生的振動和圓周運動。
2.5BUG算法BUG算法是一種完全應激的機器人避障算法。其算法原理類似昆蟲爬行的運動決策策略。在未遇到障礙物時，沿直線向目標運動；在遇到障礙物后，沿著障礙物邊界繞行，并利用一定的判斷準則離開障礙物繼續直行。這種應激式的算法計算簡便，不需要獲知全局地圖和障礙物形狀，具備完備性。但是其生成的路徑平滑性不夠好，對機器人的各種微分約束適應性比較差。
2.5.1 BUG1算法
該算法的基本思想是在沒有障礙物時，沿著直線向目標運動可以得到最短的路線。當傳感器檢測到障礙物時，機器人繞行障礙物直到能夠繼續沿直線目標運動。BUG1算法實現了最基本的向目標直行和繞行障礙物的思想。
假設機器人能夠計算兩點之間的距離，并且不考慮機器人的定位誤差。初始位置和目標位置分別用??和??表示；機器人在??時刻的位置表示為??；??表示連接機器人位置??和目標點的直線。初始時，??。若沒有探測到障礙物，那么機器人就沿著??向目標直行，直到到達目標點或者遇到障礙物。當遇到障礙物時，記下當前位置??。然后機器人環繞障礙物直到又一次到達??，找到環繞路線上距離目標最近的點??，并沿著障礙物邊界移動到該點。隨后，直線??更新，機器人繼續沿直線向目標運動。如果沿這條直線運動時還會遇到該障礙物，那么機器人不能到達目標點。否則算法不斷循環直到機器人到達目標點或者規劃器認為機器人無法到達目標點。
圖10 BUG1算法運動規劃圖11 BUG1算法中認為機器人無法到達目標點的情況圖12 BUG1算法偽代碼2.5.2 BUG2算法
BUG2算法也有兩種運動：朝向目標的直行和沿邊界繞行。與BUG1算法不同的是，BUG2算法中的直線??是連接初始點和目標點的直線，在計算過程中保持不變。當機器人在點遇到障礙物時，機器人開始繞行障礙物，如果機器人在繞行過程中在距離目標更近的點再次遇到直線??，那么就停止繞行，繼續沿著直線??向目標直行。如此循環，直到機器人到達目標點??。如果機器人在繞行過程中未遇到直線??上與目標更近的??點而回到了??點，那么得出結論，機器人不能到達目標。
圖13 BUG2算法運動規劃圖14 BUG2算法中認為機器人無法到達目標點的情況圖15 BUG2算法偽代碼BUG1和BUG2算法提供了搜索問題的兩種基本方法：比較保守的BUG1算法進行詳細的搜索來獲得最佳的離開點。這需要機器人環繞整個障礙物的邊界。而BUG2算法使用一種投機的方法。機器人不環繞完整的障礙物，而選擇第一個可用的點作為離開點。對于一般的環境，BUG2算法的效率更高；而對于復雜形狀的障礙物，保守的BUG1算法性能更優。
2.5.3 TangentBUG算法
TangentBUG算法是對BUG2算法的提高。它利用機器人上距離傳感器的讀數對障礙物做出提前規避，可以獲得更短更平滑的機器人路徑。在一個靜態環境中，傳感器讀數??是機器人位置??和傳感器掃描角度??的函數，具體點說，??是沿著??的射線以角度??到達最近障礙物的距離，
其中??。
對于某一個固定的位置??，??被傳感器視野內的障礙物分割成多個連續區間。如下圖所示。??出現不連續或者到達傳感器最大測量范圍的角度就是這些連續區間的端點。TangentBUG算法利用這些區間的端點避開工作空間中的障礙物。
圖16 距離傳感器掃描障礙物對??不連續的情況做出說明(如圖17所示)：點??，??，??，??，??，??，??和??與障礙物的不連續性相關；請注意，這里的射線與障礙物相切。點??是不連續的，因為障礙物邊界落在傳感器的范圍之外。??和??，??和??，??和??，??和??之間的free space邊界上的點集是連續性的間隔(加粗線部分)。
圖17 不連續的示意圖與其他的BUG算法一樣，TangentBUG算法也有兩種行為：直行(motion-to-go)和環繞障礙物(boundary-following)。
首先機器人沿著直線向目標運動，直到它利用傳感器觀測到在其運動方向的前方有障礙物。不在機器人前方的障礙物對其向目標運動沒有影響。比如下圖中的障礙物?，障礙物??在傳感器視野內，但是不阻礙機器人的運動。機器人在剛剛探測到障礙物時，傳感器視野圓與障礙物邊界相切。隨著機器人繼續向前移動，這個切點分裂成兩個交點??和??(??和??)，??和??之間的障礙物邊界區間與機器人運動直線相交。
圖18 機器人向目標運動遇到障礙物此時，機器人不能繼續向目標運動，而從兩個交點中選擇一個作為局部目標。為比較兩個交點對于機器人向目標運動的優劣性，定義探索距離?。在沒有關于障礙物的其他信息時，可以將探索距離??定義為從??經過一個交點到目標點的折線長度，即：?。
例如圖19，機器人“看見”了障礙??，并選擇向??移動，因為??。
當機器人位于??時，它無法知道??阻擋了從??到目標??的路徑。
在圖20中，當機器人位于??但目標??不同時，它具有足夠的傳感器信息來得出結論：??確實阻擋了從??到目標??的路徑，從而朝??行駛。
因此，選擇向??行駛剛開始的時候可能使得??最小化，而不是向??行駛，但是planner有效地為??分配無限大的成本代價，因為它有足夠的信息來推斷任何通過??的路徑都不是最理想的。
圖19圖20機器人將選擇探索距離短的交點作為局部目標，向之運動。隨著機器人不斷運動，交點不斷更新，探索距離也不斷減小。如下圖所示。當??時，機器人還沒有探測到障礙物，因而它向目標作直線運動；當??時，機器人開始探測到障礙物，并朝向障礙物探索距離近的一側運動；當?和?時，機器人繼續移動，同時更新探測區域，在這個過程中探索距離不斷減小。
圖21 機器人運動時不斷更新局部目標和探測距離在機器人運動過程中，探索距離不再減小時，就停止向目標運動行為，切換到環繞邊界行為。此時，機器人找到了探測距離的一個極小值，并可計算已探測的障礙物邊界與目標??的最近距離?。機器人按照原來的方向環繞障礙物運動，同時機器人更新當前探測的障礙物邊界與目標的最近距離?。當發現??時，機器人停止障礙物環繞行為，繼續向目標運動。
圖22 機器人環繞障礙物運動如上圖所示，當機器人探索到障礙物上的??點后，探索距離就不再減小，即??點是機器人探索距離在障礙物邊界上的局部極小點。機器人開始沿著障礙物邊界進行環繞，圖中虛線路徑就是機器人環繞障礙物時所走的路徑。當機器人探測到與目標距離相比??點更近的點時，重新開始接近目標的運動。
2.6增量式啟發算法2.6.1 LPA*算法
LPA*算法，即Lifelong Planning A*算法，該算法于2001年由Sven Koenig和Maxim Likhachev提出，是一種增量啟發式搜索版本的A*算法，這種路徑規劃算法適用于隨著時間改變導致有限柵格地圖上的邊緣代價c(s1,s2)改變的問題，也就是隨著時間改變障礙物增多或減少，網格點發生增刪等，在許多場合下比再利用A*重新搜索更高效。
詳情請參考：路徑規劃——Lifelong Planning A*算法：https://zhuanlan.zhihu.com/p/51114323
2.6.2 D* Lite算法
D* Lite算法是以LPA*為基礎，是Maxim Likhachev和Sven Koenig于2002年基于LPA*，結合A*算法思想，提出一種增量啟發式算法，適用于在未知環境中的導航以及路徑規劃，廣泛用于目前各種移動機器人和自主車輛載具，例如“機遇號”和“勇氣號”火星車測試的原型導航系統。
詳情請參考：路徑規劃——D* Lite算法：https://zhuanlan.zhihu.com/p/51124179
關于A*、LPA*、D* Lite算法的小結，詳情請參考：
(1)路徑規劃——關于A*、LPA*、D* Lite算法的小結：https://zhuanlan.zhihu.com/p/51131414
(2)路徑規劃——啟發式動態規劃算法發展研究：https://zhuanlan.zhihu.com/p/51098426
2.7小結本章研究了幾種常用的運動規劃算法。其中，人工勢場法應用靈活，可以在保證安全的情況下獲得一條平滑路徑，并且對于動態環境可以實現實時運動控制。適合用于長距離機動且障礙物較少的情況。而基于隨機采樣的搜索樹方法可以在復雜約束環境中獲得可行解，適合用于機械臂近距離操作。
參考文獻：
【1】Choset H , Kantor G A , Thrun S . Principles of Robot Motion: Theory, Algorithms, and Implementations[M]. MIT Press, 2005.
總結
以上是生活随笔為你收集整理的多边形之间相交求交点的算法_路径规划算法总结的全部內容，希望文章能夠幫你解決所遇到的問題。
- 上一篇： java实现语法分析器_语法分析 | 语
- 下一篇： python变量名包括_Python –