機器人控制理論與技術(shù)課程論文

1、機器人控制理論與技術(shù)課程論文基于RRT及其改進(jìn)型的路徑規(guī)劃算法報告摘要：本設(shè)計學(xué)習(xí)并分析了基本RRT路徑規(guī)劃的原理，并通過參考資料及自己分析，提出了一種改進(jìn)的RRT路徑規(guī)劃算法。為了驗證改進(jìn)型RRT算法的正確性以及合理性。最后在VS2010開發(fā)環(huán)境下用C+編寫了兩種RRT算法的程序代碼及演示界面。通過一定量的實驗得到了大量數(shù)據(jù)。經(jīng)過數(shù)據(jù)分析，驗證了改進(jìn)型RRT是正確的，并且在不破壞基本RRT算法的隨機性的前提下，有效的將隨機性和目的性結(jié)合起來，提高了RRT算法的效率和路徑的質(zhì)量。關(guān)鍵詞：機器人 路徑規(guī)劃 快速擴展隨機樹(RRT) 隨機性與目的性一. 引言路徑規(guī)劃是智能機器人研究領(lǐng)域的一個重要方

2、向，主要解決在有障礙的環(huán)境中，機器人如何自主尋找一條從給定起點到終點適當(dāng)?shù)倪\動路徑，使其能在運動過程中安全、無碰地繞過障礙物。傳統(tǒng)的路徑規(guī)劃有多邊形擬合法、遺傳算法、柵格法、人工勢能法等。但這些方法都需要在一個確定性空間內(nèi)對障礙物進(jìn)行確定的建模和描述，計算復(fù)雜度與機器人自由度呈指數(shù)關(guān)系，不適合于解決多自由度機器人在復(fù)雜環(huán)境中的規(guī)劃 3。最近十幾年來，基于隨機采樣的路徑規(guī)劃算法漸漸增多，該類算法可以適用于不確定環(huán)境的高維空間?？焖贁U展隨機樹(Rapidly-Exploring Random Tree，RRT)算法，是由SMLaValle于1998年提出。RRT是一種基于隨機采樣的單查詢運動規(guī)劃方

3、法。該方法特點是能夠?qū)崿F(xiàn)隨機采樣點，把搜索導(dǎo)向空白區(qū)域，從而尋找到一條從起始點到目標(biāo)點的規(guī)劃路徑，適合于解決多自由度機器人在復(fù)雜環(huán)境下和變化場景中的運動規(guī)劃。二. RRT算法原理介紹快速擴展隨機樹算法是一種高效的數(shù)據(jù)結(jié)構(gòu)和算法，該算法適用于解決未知環(huán)境下的完整性規(guī)劃、以及涉及動力運動學(xué)約束的非完整性規(guī)劃。RRT算法是一種隨機算法，不需要特定的啟發(fā)式函數(shù)的幫助。在隨機樹擴展時，每次產(chǎn)生一個隨機點，由于隨機點產(chǎn)生的隨機性，可以引導(dǎo)隨機樹向著空間中的任意一個方向生長，從而該算法對未知空間有著強烈的搜索傾向。最終，隨機樹經(jīng)過有限次的生長，將覆蓋整個區(qū)域。這是RRT算法的一個非常顯著的優(yōu)點。這種特殊的擴

4、展方式能夠漸漸的較少樹節(jié)點與目標(biāo)點之間的距離，并最終能夠使樹延伸到目標(biāo)點。在每次規(guī)劃中，RRT選取狀態(tài)空間中的起始點作為根節(jié)點，通過隨機采樣，逐漸增加葉節(jié)點，生成一個隨機擴展樹。當(dāng)隨機樹的葉節(jié)點中包含了目標(biāo)點或目標(biāo)區(qū)域中的點，便停止生長，并從目標(biāo)點或目標(biāo)區(qū)域開始逆向查找可以找到從目標(biāo)點到起始點的連通路徑，即為需要的路線?；綬RT的運行過程如圖1所示：圖1.快速擴展隨機樹的擴展過程示意圖三. 一種改進(jìn)型的RRT算法機器人RRT路徑規(guī)劃的核心思想是利用概率論中的隨機性，使機器人通過RRT算法，能夠探索所有的自由空間。這是RRT相對于其他路徑規(guī)劃方式的最顯著優(yōu)點。但是這一優(yōu)點同時也帶來了缺點，即擴

5、展方式過于平均，路徑的質(zhì)量不高。這是由于隨機性帶來的。如果能在不破壞隨機性，即RRT的概率完整性的前提下，將目的性或者方向性引入到RRT算法中，使RRT的隨機性與目的性達(dá)到一個協(xié)調(diào)統(tǒng)一，那么將能夠在一定程度上克服基本RRT擴展方式過于平均，路徑的質(zhì)量不高的缺點。目前國內(nèi)外有關(guān)學(xué)者，針對RRT的缺點提出了各種改進(jìn)型的RRT算法。其中有，根據(jù)A *搜索算法中尋找最短路徑的思想，在構(gòu)建擴展樹時引入了啟發(fā)式估價函數(shù)。這種方法確實可以有效地提高路徑質(zhì)量，但是卻帶了個很大的計算量。本文中，在研究了RRT算法的思想，以及參考其他各種改進(jìn)型RRT的基礎(chǔ)上，也提出了一種改進(jìn)型的RRT算法。即在產(chǎn)生隨機點Qran

6、d之后，判斷隨機點Qrand與目標(biāo)點Qtarget之間的距離。如果隨機點Qrand落在Qend的一個圓形鄰域中（|Qrand-Qend|<=d，d為一個根據(jù)機器人所處的環(huán)境設(shè)置的一個判斷值），那么就用目標(biāo)點Qend代替隨機點Qrand，否則不做替換。公式如下： Qrand=Qend 當(dāng)|Qrand-Qend|<=d Qrand=Qrand 當(dāng)|Qrand-Qend|>d 其中d為一個根據(jù)機器人所處的環(huán)境設(shè)置的一個判斷值 這種改進(jìn)不會破壞基本RRT的概率完整性，并且使RRT帶有了偏向目標(biāo)方向擴展的趨勢，即實現(xiàn)了RRT隨機性與目的性的協(xié)調(diào)，并且不會增加算法的運算量。通過實驗調(diào)試驗

7、證，這種方式確實可以提高路徑的質(zhì)量，以及RRT算法的速度。也就是說新的改進(jìn)算法繼承了基本RRT算法的全部優(yōu)點，同時，收斂速度快。具體分析見第五部分，程序調(diào)試。四. 本設(shè)計中的構(gòu)思，算法實現(xiàn)，程序介紹4.1構(gòu)思本設(shè)計中，選取二維平面作為機器人的運動環(huán)境。即在二維平面下進(jìn)行RRT算法設(shè)計，將機器人看做二維平面上的一個點。機器人的運動是連續(xù)的。用C+語言實現(xiàn)算法，在VS2010集成開發(fā)環(huán)境下運行調(diào)試程序。在演示軟件中，將機器人所處的環(huán)境大小設(shè)置為600*400。機器人的起始位置用一個半徑為30的大圓的圓心表示。目標(biāo)點用一個半徑為20的圓的圓心表示。并且為了程序設(shè)計的方便，障礙物設(shè)計成一個個的圓形區(qū)域

8、，半徑為10。如果需要的話，可以將多個圓形區(qū)域連在一起構(gòu)成大的障礙物。程序的設(shè)計主要包括兩個方面：1. RRT及改進(jìn)型RRT程序設(shè)計。2. 演示界面設(shè)計。4.2算法實現(xiàn)4.2.1基本RRT算法流程Build-RRT(Qinit，Qend )1 T.init(Qinit)；2 for i= 1 to K do3 if(Extend-Tree(T )= Reached)then4 return Path(Qinit，Qend )；5 return Failure；Extend_Tree(T)1 Qrand=Random - Configuration()；2 Qnear= Nearest-Neig

9、hbor(T，Qrand)；3 Qnew =New- Configuration(Qrand，Qnear)4 if Qnew Null then5 T.add-node(Qnew)6 T.add-edge(Qnew )；7 if |Qnew-Qend|<dmin then8 return Reached；9 else10 return Advanced；11 else12 return Trapped；其中：函數(shù)Extend-Tree( )是表示擴展樹的葉節(jié)點，它有3個返回值，Trapped代表沒有找到新的葉節(jié)點，Advanced代表找到新的葉節(jié)點，Reached代表到達(dá)目標(biāo)節(jié)點 。當(dāng)擴

10、展樹到達(dá)目標(biāo)節(jié)點后，返回找到的路徑Path(Qinit，Qend )。在Extend-Tree( )中，函數(shù)Random- Configuration()表示隨機選取采樣點Qrand；函數(shù)Nearest-Neighbor(T，Qrand)表示在擴展樹中尋找距Qrand 最近的葉節(jié)點Qnear ；函數(shù)New- Configuration(Qrand，Qnear)完成從Qnear到 Qrand的方向上以固定的步長L計算出新的葉節(jié)點 ，計算公式為：Qnew=Qnear+L*（Qrand-Qnear）/|Qrand-Qnew| 其中在本程序中L=5；基本RRT流程圖如圖2（在下一頁）所示；4.2.2改

11、進(jìn)型RRT算法流程產(chǎn)生隨機點QrandQrand=Qend找出樹Tk上的最近點Qnear|Qrand-Qend|<=d？NY改進(jìn)型RRT算法和基本RRT算法總體上是一樣的。唯一不同的一點是在產(chǎn)生了隨機點之后，判斷隨機點Qrand與Qend之間的距離，如果它們之間的距離小于一個給定的值d（d為一個根據(jù)機器人所處的環(huán)境設(shè)置的一個判斷值，在程序調(diào)試過程中，d=30，可以根據(jù)具體情況設(shè)定）。以下僅給出改進(jìn)的那部分流程圖，其他同基本RRT的流程圖。圖3 改進(jìn)型RRT算法流程圖(僅僅是改進(jìn)部分的)開始結(jié)束輸入起始點Qstart，目標(biāo)點QendQnew=Qend？Qnew在障礙物中？Qstart=Qe

12、nd？將Qnew添加到樹Tk上，Tk成為T（k+1）生成隨機點Qrand，找到隨機樹Tk上的最近點Qnear算出新節(jié)點QnewYYYNNN圖2 基本RRT算法流程圖4.3程序介紹1. 定義存儲隨機樹每個節(jié)點的結(jié)構(gòu)體為：struct Treestrdouble x,y;int prtpos; ; 其中x和y是樹上節(jié)點的坐標(biāo)，因為地圖是連續(xù)的，所以定義成double型，prtpos為當(dāng)前節(jié)點的父節(jié)點在randomTree中的編號。2. 定義存儲隨機樹的數(shù)據(jù)為：vector<Treestr>randomTree 因為樹的大小是未知的，所以定義成vector。3. 定義存儲路徑每個節(jié)點的結(jié)

13、構(gòu)體：struct posdouble x,y;pos()x=-20,y-20;4. 定義存儲路徑的數(shù)據(jù)為：vector<pos>TreePath5. 各功能函數(shù)如下：基本RRT路徑規(guī)劃：randTree_build()改進(jìn)型RRT路徑規(guī)劃：OPT_randTree_build()測距函數(shù)：double getDis(pos a,pos b)產(chǎn)生隨機點的函數(shù)：pos Random_Configuration()尋找新點的函數(shù)：pos New_Configuration(pos randPos,pos nearPos )其他功能模塊都包含在OPT_randTree_build()和r

14、andTree_build()函數(shù)中，不在具體說明。6. 在程序中設(shè)置每次路徑生成節(jié)點大小的上線為10000.超過之后程序結(jié)束。如果沒有找到路徑，可以再找一次。通過多次實驗發(fā)現(xiàn)，在本環(huán)境下，一般在5000次以內(nèi)都可以找到路徑。五. 程序調(diào)試分析5.1程序調(diào)試過程中有錯誤的程序介紹由于在對隨機數(shù)數(shù)組排序時，程序設(shè)計有一點錯誤，產(chǎn)生了下圖錯誤，經(jīng)過調(diào)試發(fā)現(xiàn)并改正了錯誤。圖4 錯誤程序示例圖5.2演示界面介紹以圖7為例。其中在環(huán)境設(shè)置一欄，有三個選項，分別在演示區(qū)域放置起點，終點，和障礙物。有四個按鈕，分別為rrt路徑規(guī)劃對應(yīng)基本RRT，改進(jìn)型rrt路徑對應(yīng)改進(jìn)型RRT，只顯示路徑對應(yīng)基本RRT使只

15、顯示最終生成的路徑而不顯示樹。關(guān)閉對應(yīng)關(guān)閉演示界面。輸出欄顯示隨機樹生成的結(jié)果。在上邊的空白區(qū)域為演示區(qū)。注意：由于沒有設(shè)置清除按鈕，如果在執(zhí)行了一次路徑規(guī)劃后，如果需要在執(zhí)行一次，需要重新選擇起點。5.3兩種樹的展示下面分別是在無障礙物的情況下生成的隨機樹。其中，圖5是基本RRT無障礙物時生成的隨機樹，圖6是改進(jìn)型RRT無障礙物時生成的隨機樹。從兩幅圖中可以看出改進(jìn)型RRT明顯優(yōu)于基本型。 圖5 基本RRT無障礙物時生成的隨機樹 圖6 改進(jìn)型RRT無障礙物時生成的隨機樹5.4數(shù)據(jù)分析由于一次的路徑搜索具有隨機性，下面將在相同復(fù)雜的環(huán)境下分別用基本RRT和改進(jìn)型RRT進(jìn)行20次路徑規(guī)劃。在相同

16、環(huán)境下實驗的隨機樹如圖7和8所示。通過實驗測試得到的數(shù)據(jù)如下表格所示：表一 基本RRT數(shù)據(jù)隨機樹大小271334962210266828002780380525424401路徑大小231247231242229218226213232隨機樹大小225928122273175130092635242816441771路徑大小236200202221221202209199229隨機樹大小41373060路徑大小246243表二 改進(jìn)型RRT數(shù)據(jù)隨機樹大小171316302083166124342278257017932660路徑大小201220221245192197234203197隨機樹大小

17、178120722264244419011956166821532916路徑大小207205240204208201222208218隨機樹大小19312305路徑大小205209表三 數(shù)據(jù)比較20次平均值隨機數(shù)大小路徑大小基本型RRT2760224改進(jìn)型RRT2100212圖7 相同環(huán)境下基本RRT生成的隨機樹實例圖8 相同環(huán)境下改進(jìn)型RRT生成的隨機樹實例通過數(shù)據(jù)比較，以及隨機樹的示例圖7和8可以看出改進(jìn)型RRT在減小隨機樹方面有明顯的優(yōu)勢，并能較好的減小路徑，找到更短的路徑，而不會增加計算量。即驗證了前面提出的RRT改進(jìn)思想是正確的。這種改進(jìn)不會破壞基本RRT的概率完整性，并且使RRT帶

18、有了偏向目標(biāo)方向擴展的趨勢，即實現(xiàn)了RRT隨機性與目的性的協(xié)調(diào)，并且不會增加算法的運算量。通過實驗調(diào)試驗證，這種方式確實可以提高路徑的質(zhì)量，以及RRT算法的速度。也就是說新的改進(jìn)算法繼承了基本RRT算法的全部優(yōu)點，同時，收斂速度快。六. 總結(jié)及展望1. 由于存儲樹節(jié)點的數(shù)組vector<Treestr>randomTree在存儲數(shù)據(jù)時，順序比較復(fù)雜，給整個隨機樹的最后排序顯示帶來了一定的困難。花費了較多的時間來解決這個問題。后來發(fā)現(xiàn)通過指針數(shù)據(jù)結(jié)構(gòu)，可以有效解決這個問題，提高程序的執(zhí)行效率。2. 通過自己編寫RRT算法，程序，以及程序調(diào)試。進(jìn)一步加深了對機器人RRT路徑規(guī)劃算法思想的理解。這種算法的優(yōu)勢在于，由于每一步探索的方向是隨機的，可以指向自由空間的任意一點。所以隨機樹可以擴展到自由空間中的任何一個小的區(qū)域，可以說能夠遍歷整個自由空間。所以這種路徑搜索方式特別適合機器人在未知的復(fù)雜環(huán)境下進(jìn)行路徑規(guī)劃。并且從某種角度說，在調(diào)試程序，解決程序中的錯誤時，如果把錯誤看做目標(biāo)點的話，也在使用這種方式，即隨機性與目的性的統(tǒng)一。3. 在復(fù)雜程序調(diào)試過程中，如果感覺算法和程序整體上是對的，而運行的結(jié)果卻和期望的有一定差距，可以寫一些功能相似而相對簡單的程序段代替復(fù)雜程序段，從而使程序調(diào)試更容易些，從而能更快的發(fā)現(xiàn)錯誤的那個點。4. 在程