機(jī)器人運(yùn)動(dòng)規(guī)劃方法綜述（1）

隨著應(yīng)用場(chǎng)景的日益復(fù)雜，機(jī)器人對(duì)旨在生成無(wú)碰撞路徑（軌跡）的自主運(yùn)動(dòng)規(guī)劃技術(shù)的需求也變得更加迫切。雖然目前已產(chǎn)生了大量適應(yīng)于不同場(chǎng)景的規(guī)劃算法，但如何妥善地對(duì)現(xiàn)有成果進(jìn)行歸類(lèi)，并分析不同方法間的優(yōu)劣異同仍是需要深入思考的問(wèn)題。以此為切入點(diǎn)，首先，闡釋運(yùn)動(dòng)規(guī)劃的基本內(nèi)涵及經(jīng)典算法的關(guān)鍵步驟；其次，針對(duì)實(shí)時(shí)性與解路徑（軌跡）品質(zhì)間的矛盾，以是否考慮微分約束為標(biāo)準(zhǔn)，有層次地總結(jié)了現(xiàn)有的算法加速策略；最后，面向不確定性（即傳感器不確定性、未來(lái)狀態(tài)不確定性和環(huán)境不確定性）下的規(guī)劃和智能規(guī)劃提出的新需求，對(duì)運(yùn)動(dòng)規(guī)劃領(lǐng)域的最新成果和發(fā)展方向進(jìn)行了評(píng)述，以期為后續(xù)研究提供有益的參考。機(jī)器人學(xué)是研究如何通過(guò)計(jì)算機(jī)控制裝置感知并操縱客觀世界的學(xué)科，現(xiàn)今已被應(yīng)用于各個(gè)領(lǐng)域，如行星探索中的移動(dòng)平臺(tái)、空間機(jī)械臂、無(wú)人機(jī)、自動(dòng)駕駛汽車(chē)等（圖1）。設(shè)想這樣的場(chǎng)景：一架無(wú)人機(jī)正在高樓聳立的城市中穿行，未知環(huán)境要求其應(yīng)利用新接收到的信息不斷修正它的運(yùn)動(dòng)；雜亂的障礙物要求其應(yīng)避免與眾多物體發(fā)生碰撞；傳感器誤差、陣風(fēng)干擾、模型誤差、控制約束等則要求真實(shí)運(yùn)動(dòng)與規(guī)劃運(yùn)動(dòng)間的誤差應(yīng)盡量小。因此為了能在充滿未知性和不確定性的真實(shí)世界中可靠地完成任務(wù)，機(jī)器人必須具有自主感知、快速?zèng)Q策、運(yùn)動(dòng)規(guī)劃與控制的能力。其中運(yùn)動(dòng)規(guī)劃作為上層決策與底層控制的連接模塊，負(fù)責(zé)將決策序列轉(zhuǎn)化為控制器可執(zhí)行的參考路徑（軌跡），在機(jī)器人研究中占有重要地位。圖1 典型的機(jī)器人系統(tǒng)運(yùn)動(dòng)規(guī)劃領(lǐng)域早期側(cè)重于處理有障礙物的二維或三維世界中的開(kāi)環(huán)運(yùn)動(dòng)規(guī)劃問(wèn)題，其結(jié)果可以是一條無(wú)碰路徑，也可以是一條無(wú)碰軌跡。前者是幾何意義上的連續(xù)（光滑）曲線，后者則是這一曲線的時(shí)間參數(shù)化表達(dá)。對(duì)機(jī)器人而言，路徑規(guī)劃（本文稱(chēng)其為傳統(tǒng)運(yùn)動(dòng)規(guī)劃）一般在構(gòu)型空間（Configuration Space，C-space）內(nèi)進(jìn)行，軌跡規(guī)劃（本文稱(chēng)其為考慮微分約束的開(kāi)環(huán)運(yùn)動(dòng)規(guī)劃）則在狀態(tài)空間（即構(gòu)型空間或相空間）內(nèi)進(jìn)行。但無(wú)論構(gòu)型空間亦或相空間，都是無(wú)限不可數(shù)的，故開(kāi)環(huán)運(yùn)動(dòng)規(guī)劃的一個(gè)中心主題是將連續(xù)空間離散化，進(jìn)而借助人工智能領(lǐng)域的離散搜索思想，將求解運(yùn)動(dòng)規(guī)劃問(wèn)題視為在高維自由構(gòu)型空間（,）或自由相空間中的搜索過(guò)程。除一般的可行性問(wèn)題外，滿足某類(lèi)性能指標(biāo)最優(yōu)（如時(shí)間最優(yōu)、路徑長(zhǎng)度最優(yōu)等）的開(kāi)環(huán)運(yùn)動(dòng)規(guī)劃問(wèn)題亦是學(xué)者們的關(guān)注焦點(diǎn)。一條最優(yōu)路徑往往可參數(shù)化為許多條軌跡，它們的速度時(shí)間歷程各不相同，而最優(yōu)軌跡僅是其中一條。不幸的是，計(jì)算最優(yōu)路徑（軌跡）的時(shí)間復(fù)雜度往往較高（甚至對(duì)某些問(wèn)題來(lái)講，計(jì)算可行路徑就已十分困難），很難滿足機(jī)器人實(shí)時(shí)規(guī)劃的需求，這便促使研究人員不斷開(kāi)發(fā)新的算法以實(shí)現(xiàn)兩者間的平衡。另外，經(jīng)典控制理論的發(fā)展歷史表明：反饋是應(yīng)對(duì)不確定性的有效手段。但開(kāi)環(huán)運(yùn)動(dòng)規(guī)劃過(guò)程常常忽略了這一點(diǎn)，從而容易造成機(jī)器人沿規(guī)劃路徑（軌跡）運(yùn)動(dòng)時(shí)意外碰撞的發(fā)生（圖2）。反饋運(yùn)動(dòng)規(guī)劃則通過(guò)對(duì)不確定性進(jìn)行建模，并將該模型融入規(guī)劃過(guò)程，為機(jī)器人的實(shí)際運(yùn)動(dòng)提供了安全保障。雖然近年關(guān)于反饋運(yùn)動(dòng)規(guī)劃的研究取得了一系列進(jìn)展，但鮮有文章對(duì)這一主題進(jìn)行歸納總結(jié)。即使有所提煉，包括反饋運(yùn)動(dòng)規(guī)劃在內(nèi)的各類(lèi)內(nèi)容也已稍顯陳舊，而且將運(yùn)動(dòng)規(guī)劃的研究范圍局限于基于采樣的運(yùn)動(dòng)規(guī)劃（Sampling-based Motion Planning，SBMP）算法的做法無(wú)疑限制了讀者的視野。圖2 運(yùn)動(dòng)規(guī)劃與控制的解耦可能造成意外碰撞根據(jù)是否考慮微分約束和反饋，Lavalle的經(jīng)典專(zhuān)著將運(yùn)動(dòng)規(guī)劃問(wèn)題分為表1中的4 項(xiàng)子課題，本文則不區(qū)分最右側(cè)2項(xiàng)，并將其統(tǒng)稱(chēng)為反饋運(yùn)動(dòng)規(guī)劃。進(jìn)而在這種分類(lèi)方式的基礎(chǔ)上以單機(jī)器人為研究對(duì)象，依次總結(jié)了現(xiàn)有的傳統(tǒng)運(yùn)動(dòng)規(guī)劃、考慮微分約束的開(kāi)環(huán)運(yùn)動(dòng)規(guī)劃和反饋運(yùn)動(dòng)規(guī)劃相關(guān)算法，形成了類(lèi)似于“譜”的運(yùn)動(dòng)規(guī)劃算法歸類(lèi)，便于讀者對(duì)比分析各種方法間的區(qū)別與聯(lián)系。其中針對(duì)解路徑（軌跡）品質(zhì)與求解效率間存在的矛盾，重點(diǎn)詳述了如何利用已有信息來(lái)加速漸近最（近）優(yōu)算法。另外則對(duì)不確定性建模方式、動(dòng)態(tài)環(huán)境中的規(guī)劃、學(xué)習(xí)算法與運(yùn)動(dòng)規(guī)劃算法的融合等先進(jìn)課題的最新成果進(jìn)行了總結(jié)，以期為后續(xù)研究提供思路。表1 運(yùn)動(dòng)規(guī)劃問(wèn)題分類(lèi)01  傳統(tǒng)運(yùn)動(dòng)規(guī)劃設(shè)為映射到的拓?fù)鋱D（可參考圖3~圖6），其中為圖中節(jié)點(diǎn)的集合，每個(gè)節(jié)點(diǎn)相對(duì)應(yīng)一個(gè)構(gòu)型。為圖中邊的集合，每條邊相對(duì)應(yīng)兩個(gè)構(gòu)型 間的無(wú)碰撞路徑。對(duì)于所有，定義式中：表示路徑的像；為可以通過(guò)）到達(dá)的所有構(gòu)型的集合。解決傳統(tǒng)運(yùn)動(dòng)規(guī)劃問(wèn)題的思路正是用包含可數(shù)個(gè)節(jié)點(diǎn)的圖結(jié)構(gòu)捕捉的連通信息，并基于此離散結(jié)構(gòu)搜索解路徑。由于 的構(gòu)建過(guò)程顯然受障礙物形狀及其位置分布的影響，故一般可根據(jù)是否在中顯式表示障礙物區(qū)域（），將處理傳統(tǒng)運(yùn)動(dòng)規(guī)劃問(wèn)題的算法分為組合運(yùn)動(dòng)規(guī)劃（Combina?torial Motion Planning， CMP）算法和基于采樣的運(yùn)動(dòng)規(guī)劃算法兩類(lèi)。1.1 組合運(yùn)動(dòng)規(guī)劃基于顯式表示的障礙物，CMP首先構(gòu)造滿足下列條件：1）可達(dá)性（Accessibility）：對(duì)于任一，可以簡(jiǎn)單高效地計(jì)算一條以其為起點(diǎn)，以中任一點(diǎn)為終點(diǎn)的路徑，即，。2）確保連通性（Connectivity-Preserving）：對(duì)于起始構(gòu)型、目標(biāo)構(gòu)型及它們?cè)?img class="rich_pages wxw-img" height="38" width="43" src="https://mmbiz.qpic.cn/mmbiz_png/tL6kLzIKAkr5Pyialghhoy4Bk0F1TQCzX6MvFF1DjB6Gxka19Fm77xu1en75Bm4ZmibiaKXia4kWZ79wp4gV5E1oicA/640?wx_fmt=png&tp=wxpic&wxfrom=5&wx_lazy=1&wx_co=1" alt="圖片" />中的連接點(diǎn)、，如果存在一條路徑，使得，，那么也將存在一條路徑，使得的路圖，其或由的胞腔剖分間接生成，如垂直胞腔剖分、圓柱代數(shù)剖分等；或通過(guò)其他方法直接構(gòu)造，如可視圖法、廣義維羅尼圖法、輪廓法等。進(jìn)而利用圖搜索算法如Dijkstra算法、A*算法、ARA*算法、D*Lite算法、AD*算法及Theta*算法等尋找解路徑。后四者都是A*的變體：ARA*通過(guò)放松對(duì)啟發(fā)式的一致性要求并重復(fù)使用先前的搜索信息，可快速產(chǎn)生因子可控的次優(yōu)路徑，具有Anytime特性，適用于計(jì)算時(shí)間受限的靜態(tài)環(huán)境；D*Lite是一種遞增搜索算法，其先用A*算法從目標(biāo)頂點(diǎn)反向計(jì)算相關(guān)節(jié)點(diǎn)的最優(yōu)路徑代價(jià)，待環(huán)境發(fā)生改變后，再以此有效信息為基礎(chǔ)進(jìn)一步調(diào)整相關(guān)節(jié)點(diǎn)的最優(yōu)路徑代價(jià)，適用于含動(dòng)態(tài)障礙物的場(chǎng)景；AD*則是結(jié)合了ARA*和D*Lite的優(yōu)點(diǎn)，既具有Anytime特性，又有遞增特性，真正實(shí)現(xiàn)了動(dòng)態(tài)場(chǎng)景中的實(shí)時(shí)規(guī)劃。另外由于A*算法找出的解路徑角度往往被網(wǎng)格劃分方式所限制，因而其并非真實(shí)地形中的最優(yōu)路徑，Theta*通過(guò)解除這一約束，可在相同時(shí)間復(fù)雜度下得到更高質(zhì)量的解路徑。條件1）其實(shí)保證了內(nèi)的任一起始、目標(biāo)構(gòu)型對(duì)（）均可被連接到中，條件2）則保證了在解路徑存在的情況下，搜索總是可以成功。而這正是CMP 完備性的來(lái)源，即在有限時(shí)間內(nèi)，算法要么找到一條解路徑，要么正確地報(bào)告無(wú)解。雖然其幾乎能解決任何運(yùn)動(dòng)規(guī)劃問(wèn)題，而且在一些簡(jiǎn)單情形（如二維世界中，為多邊形，機(jī)器人僅進(jìn)行平移運(yùn)動(dòng)）下可以高效地得到較好解，但對(duì)于大多數(shù)具有高維C-space且障礙物數(shù)量巨大的問(wèn)題，較長(zhǎng)的運(yùn)行時(shí)間和執(zhí)行困難使此類(lèi)方法失去了吸引力。1.2 基于采樣的運(yùn)動(dòng)規(guī)劃與CMP不同，SBMP通過(guò)碰撞檢測(cè)模塊來(lái)避免顯式構(gòu)建，并且利用可數(shù)的采樣點(diǎn)集或采樣序列及滿足一定條件的連接方式近似捕捉無(wú)限不可數(shù)的Cfree的連通特性。盡管其永遠(yuǎn)不可能知曉的精確形狀，但卻節(jié)省了大量計(jì)算時(shí)間，是一種行之有效的折中方式，可用于高維空間中的運(yùn)動(dòng)規(guī)劃。不過(guò)與此同時(shí)也犧牲了算法的完備性，并代之以更弱的概念：使用隨機(jī)采樣方法的運(yùn)動(dòng)規(guī)劃算法是概率完備（Probabilistically Complete）的，使用確定性采樣方法的運(yùn)動(dòng)規(guī)劃算法是分辨率完備（Resolution Complete）的。為滿足弱完備性要求：①中的采樣序列必須稠密；②算法的搜索過(guò)程應(yīng)具備“系統(tǒng)性”。更多關(guān)于 SBMP產(chǎn)生的歷史原因和早期發(fā)展過(guò)程可參考Lindemann和Lavalle的綜述文章。對(duì)于給定數(shù)個(gè)起始-目標(biāo)構(gòu)型對(duì)的多疑問(wèn)（Multiple-Query）運(yùn)動(dòng)規(guī)劃問(wèn)題，如果環(huán)境結(jié)構(gòu)不發(fā)生改變，則非常有必要花費(fèi)時(shí)間對(duì)模型進(jìn)行預(yù)計(jì)算以生成路圖，從而提高后續(xù)搜索效率。概率路圖法（Probabilistic Roadmap，PRM）是其中的典型代表，它最初是為應(yīng)對(duì)高自由度運(yùn)動(dòng)規(guī)劃問(wèn)題而發(fā)展的，算法流程如圖3所示。PRM構(gòu)建的路圖可看作是對(duì)CMP所導(dǎo)出路圖的一種近似，因此也常被與CMP共同歸入基于圖搜索的運(yùn)動(dòng)規(guī)劃算法中。Kavraki等通過(guò)對(duì)簡(jiǎn)化的PRM（Simplified PRM，s-PRM）進(jìn)行分析，建立了算法失敗概率與路徑長(zhǎng)度、路徑和障礙物間距、采樣點(diǎn)數(shù)量之間的函數(shù)關(guān)系，其中隨的增加而線性增加，隨的增加而指數(shù)減小到0，從而證明了PRM的概率完備性。但由于路徑特性很難提前得知，故該完備性分析方法不易使用。另一分析方法則借助ε-goodness、β-lookout和（ε，α，β）-expansive的概念，證明算法失敗概率為圖3 PRM算法流程式中：為正常量。從式（2）不僅能得出與的指數(shù)關(guān)系，而且可以發(fā)現(xiàn)如果暗含可視特性的越大，那么完成求解所需的采樣點(diǎn)就越少，算法運(yùn)行時(shí)間就越短。因?yàn)橐鹂梢曁匦韵陆档莫M窄通道不可能偶然出現(xiàn)，故實(shí)際中遇到的許多都具有良好的可視特性，即相應(yīng)的較大。另外可視特性是用體積比率定義的，而不直接依賴(lài)于C-space維數(shù)，這便解釋了為什么當(dāng)維數(shù)在一定范圍內(nèi)增加時(shí)，PRM仍能較好地運(yùn)行。