一種六輪/腿機器人復(fù)合運動路徑規(guī)劃方法
【專利摘要】本發(fā)明公開一種六輪/腿機器人復(fù)合運動路徑規(guī)劃方法，首先建立機器人工作環(huán)境的DEM圖，并進行地貌特征提?。徊捎玫孛蔡卣髋c評價指標之間的映射，遍歷DEM圖中每個柵格，得到機器人的通過性地圖及腿式、輪式運動代價圖；然后在代價圖上，以機器人出發(fā)點為隨機樹的根節(jié)點，采用單純腿式運動，以標準隨機樹算法進行擴展，逐漸增加葉節(jié)點直至隨機樹的葉節(jié)點中包含了終點，得到從出發(fā)點到終點之間的一條單純腿式運動目標路徑，并估計該路徑的代價，作為初始代價迭代求解，至所用的時間超過用戶設(shè)定的最大時限。本發(fā)明為輪/腿機器人提供了一種混合運動規(guī)劃的解決方法，運動規(guī)劃結(jié)果可以顯著提高機器人的通過性及平均運行速度。
【專利說明】
一種六輪/腿機器人復(fù)合運動路徑規(guī)劃方法
技術(shù)領(lǐng)域
[0001] 本發(fā)明屬于機器人技術(shù)領(lǐng)域，涉及一種具有多功能輪/腿機器人復(fù)合運動路徑規(guī) 劃方法
【背景技術(shù)】
[0002] 腿式機器人可通過泥濘路面、崎嶇山地等復(fù)雜環(huán)境，具有較強的環(huán)境適應(yīng)能力，但 是具有速度慢，能耗高，控制困難等缺點。輪式機器人可以快速，高效地通過平坦路面，但是 其越障能力較差。輪/腿復(fù)合運動模式機器人旨在通過結(jié)合輪式、腿式運動各自的優(yōu)點，在 不同環(huán)境中應(yīng)用不同的運動模式，實現(xiàn)發(fā)揮輪式的高速機動同時又具有良好的越障能力。 輪/腿復(fù)合機器人為移動機器人的發(fā)展提供了新的方向，在探測，救災(zāi)，軍事偵查，星球探測 等方面具有廣闊的應(yīng)用前景。
[0003] 針對輪/腿復(fù)合移動機器人，目前開展了許多輪/腿復(fù)合運動機構(gòu)的設(shè)計研究，如 申請?zhí)枮?01410025261.9的發(fā)明專利一種多功能腿輪復(fù)合機器人及其多運動模式智能切 換方法，提供了一種新型的輪/腿復(fù)合運動機構(gòu)，并提供了多種運動模式之間的切換方法， 為輪/腿復(fù)合運動的實現(xiàn)提供了基礎(chǔ)。但是對輪/腿復(fù)合運動技術(shù)的核心，即決策在環(huán)境中 何處采用輪式運動，何處采用腿式運動以及如何通過輪式、腿式運動組合實現(xiàn)快速、低耗地 運動到目標地點，缺乏有效的解決方法，這嚴重地制約了輪/腿復(fù)合運動機器人的發(fā)展和應(yīng) 用。因此，設(shè)計一種高效的輪/腿復(fù)合運動路徑規(guī)劃方法是當前急需要解決的問題。

【發(fā)明內(nèi)容】

[0004] 本發(fā)明為實現(xiàn)輪/腿機器人在復(fù)雜環(huán)境中快速、穩(wěn)定的運動，提供了一種面向不確 定環(huán)境，最大限度發(fā)揮輪/腿混合運動優(yōu)勢的路徑規(guī)劃方法?；趯Νh(huán)境的感知建模，對輪、 腿各自運動特點的總結(jié)分析，能夠迅速在復(fù)雜環(huán)境中找出滿意的最佳混合運動路徑。
[0005] 1、本發(fā)明一種六輪/腿機器人復(fù)合運動路徑規(guī)劃方法，其特征在于:通過下述步驟 完成：
[0006] 步驟1:探測并確定機器人工作環(huán)境信息，建立其工作環(huán)境的DEM圖。
[0007] 步驟2:對步驟1中建立的DEM圖進行特征提取，包括:坡度、臺階類障礙高度與粗糙 度三種地貌特征;采用三種地貌特征與評價指標之間的映射，遍歷的DEM圖中每個柵格，得 到機器人的通過性地圖及腿式、輪式運動代價圖，圖中標記有機器人輪式及腿式運動都可 以通過的柵格，輪式及腿式運動都不能通過的柵格，以及只有腿式運動可以通過的柵格。
[0008] 步驟3:規(guī)劃獲得機器人最佳混合路徑。
[0009] A、在步驟2中得到的腿式運動代價圖上，以機器人出發(fā)點qstart為隨機樹的根節(jié)點， 機器人采用單純腿式運動，以標準隨機樹算法進行擴展，逐漸增加葉節(jié)點直至隨機樹的葉 節(jié)點中包含了終點q g〇al，得到從qstart到qg〇al之間的一條單純腿式運動目標路徑T{1}。
[0010] B、估計目標路徑T {1}的代價Tc。
[0011] C、以T。為初始代價，進行迭代求解，至所用的時間超過用戶設(shè)定的最大時限;每次 迭代中尋找到的混合運動路徑代價需小于(1-ε)Τ。，其中ε為迭代因子。
[0012]本發(fā)明的優(yōu)點為：
[0013] 1、本發(fā)明六輪/腿機器人復(fù)合運動路徑規(guī)劃方法，為輪/腿機器人提供了一種混合 運動規(guī)劃的解決方法，運動規(guī)劃結(jié)果可以顯著提高機器人的通過性及平均運行速度。
[0014] 2、本發(fā)明六輪/腿機器人復(fù)合運動路徑規(guī)劃方法，應(yīng)用范圍廣，該運動規(guī)劃方法采 用參數(shù)化建模思想，適用于各種具有輪腿兩種運動形式的機器人。
[0015] 3、本發(fā)明六輪/腿機器人復(fù)合運動路徑規(guī)劃方法，運行速度快，該運動規(guī)劃方法， 采用改進型rrt算法，顯著的提高了運行速度。
【附圖說明】
[0016] 圖1本發(fā)明六輪/腿機器人復(fù)合運動路徑規(guī)劃方法流程圖；
[0017] 圖2為本發(fā)明六輪/腿機器人復(fù)合運動路徑規(guī)劃方法中機器人工作環(huán)境的DEM圖；
[0018] 圖3為改進型RRT尋找混合路徑方法中已經(jīng)生成的部分隨機樹示意圖；
[0019]圖4為改進型RRT尋找混合路徑方法中隨機樹的子路徑拓展示意圖；
[0020]圖5為改進型RRT尋找混合路徑方法中隨機樹的子路徑優(yōu)化示意圖。
【具體實施方式】
[0021]下面結(jié)合附圖對本發(fā)明作進一步詳細說明。
[0022]本發(fā)明六輪/腿機器人復(fù)合運動路徑規(guī)劃方法，如圖1所示，通過下述步驟實現(xiàn)： [0023]步驟1:探測并確定機器人工作環(huán)境信息，建立其工作環(huán)境的DEM(數(shù)字高程)圖；
[0024] 根據(jù)機器人的直徑及運動狀態(tài)，利用感知系統(tǒng)探測機器人周圍一定區(qū)域的地貌特 征，包括機器人周圍障礙物的位置形狀與地表紋理特征，并建立機器人工作環(huán)境的DEM(數(shù) 字高程)圖。
[0025] 步驟2:生成相應(yīng)的柵格化輪/腿運動代價圖；
[0026] a、對步驟1中建立的DEM圖進行特征提取，包括:坡度、臺階類障礙高度與粗糙度三 種地貌特征。
[0027] 如圖1所示，令機器人所處的世界坐標系為Σ(；，機器人本體中心在世界坐標系中 的坐標為Z1^機器人本體中心在X-Y平面內(nèi)的投影處于柵格cell(i，j)內(nèi)，其中i，j為柵格 坐標。機器人本體在X-Y平面內(nèi)投影的最小包絡(luò)矩形為R(i，j)，在R(i，j)內(nèi)每個柵格提取上 述三種地形特征：
[0028] ①坡度是影響機器人運動的主要特征，當機器人采用腿式運動形式爬坡或者采用 輪式通過坑洼地形時，機器人傾覆的風(fēng)險會隨著坡度的增大而急劇增加。目前有許多方法 提取坡度特征，本發(fā)明中采用R( i，j)內(nèi)的平均傾角作為坡度特征，表示為：
[0029]
(1)
[0030]其中，θχ表示沿X軸方向的平均傾角，?y表示沿y方向的平均傾角。
[0031]②粗糙度表征了機器人運動環(huán)境表面的崎嶇及粗糙程度，是影響輪式運動的主要 因素。當粗糙度過大時，機器人采用腿式行走也變得困難。本發(fā)明中粗糙度采用R(i，j)區(qū)域 內(nèi)尚程的標準差進彳丁估計，表不為：
[0032]
C2)
[0033] 其中，表示R(i，j)區(qū)域內(nèi)的高程平均值，η為R(i，j)區(qū)域內(nèi)柵格的個數(shù)。 [0034]③臺階類障礙高度；
[0035] 臺階類障礙高度定義為投影區(qū)域內(nèi)的最大絕對高程差，主要用來表征臺階、峽谷 與峭壁等地形特征。對于這些地形特征，坡度指標變化不大，而絕對高度的變化超出了機器 人的移動能力。本發(fā)明中臺階類障礙高度表示為：
[0036] Eij = Zmax(Rij)-Zmin(Rij) (3)
[0037]其中，Zmax和Zmin為R(i，j)內(nèi)最高和最低高程值。
[0038] b、生成輪/腿運動形式的代價圖；
[0039] 根據(jù)步驟a提取的地形特征，結(jié)合機器人輪式運動與腿式運動的特點，分別計算機 器人在R( i，j)內(nèi)每個柵格的輪/腿運動形式的評價指標，包括輪運動形式的評價指標與 腿運動形式的評價指標(? 4亥指標既描述了機器人的通過性，也描述了機器人采用不同運 動形式通過區(qū)域R(i，j)時所消耗的代價，具體的包括能耗、時間等指標;評價指標Clj具有如 下形式[Trc ,Effc]，其中，Trc為通過性指標，Trc = O表示可通過區(qū)域R(i，j) ;Trc = 0表示不 可通過區(qū)域R( i，j);Ef f c為代價評價指標，描述通過區(qū)域R( i，j)的代價，Ef f c = (0，I)，為0 到1之間的連續(xù)值，數(shù)值越高，代價越高。
[0040] 通過BP人工神經(jīng)網(wǎng)絡(luò)，建立每個柵格內(nèi)提取的各個地形特征?^，8^4^和評價 指標之間的映射。建立三層BP人工神經(jīng)網(wǎng)絡(luò)，并通過ADAMS仿真環(huán)境及實驗中獲得的 不同地形中機器人能耗，時間數(shù)據(jù)對神經(jīng)網(wǎng)絡(luò)進行訓(xùn)練，模擬地形特征與評價指標之間的 映射。采用上述模擬地形特征與評價指標之間的映射，遍歷步驟1中建立的DEM圖中每個柵 格，得到機器人的通過性地圖及腿式、輪式運動代價圖，圖中標記有機器人輪式及腿式運動 都可以通過的柵格，輪式及腿式運動都不能通過的柵格(障礙物區(qū)域），以及只有腿式運動 可以通過的柵格。
[0041 ]步驟3:規(guī)劃獲得機器人最佳混合路徑。
[0042] 本發(fā)明中采用對快速隨機樹法進行改進，來規(guī)劃獲得機器人的最佳混合路徑，具 體方式為：
[0043] A、在步驟2中得到的腿式運動代價圖上，以機器人出發(fā)點qstart為隨機樹的根節(jié)點， 機器人采用單純腿式運動，以標準隨機樹算法進行擴展，逐漸增加葉節(jié)點直至隨機樹的葉 節(jié)點中包含了終點q g〇al，得到從qstart到qg〇al之間的一條單純腿式運動目標路徑T{1}。
[0044] B、估計目標路徑T {1}的代價；
[0045] 中，路徑上每個柵格的評價指標為(^·:，則目標路徑T{ 1}的代價 Tc為：
[0046]
[0047] U利用c：又進型KKT尋找輪腿混合運動路徑；
[0048]以T。為初始代價，進行迭代求解，至所用的時間超過用戶設(shè)定的最大時限。每次迭 代中尋找到的混合運動路徑代價需小于（1-ε)Τ。，其中ε為迭代因子;ε決定每次迭代過程路 徑的優(yōu)化程度，如果ε很大，則要求每次運動代價減小很多。
[0049] 每一次迭代步驟如下：
[0050] I、假設(shè)機器人周圍的通過性地圖，如圖3、圖4與圖5所示，其中，圖3為通過性地圖 中已生成的部分隨機樹，起始點為q start，終點為qg〇ai，由操作者設(shè)定；圓點代表隨機樹的節(jié) 點，其中實心點表示采用腿式運動通過的點，空心點表示采用輪式運動通過的點。節(jié)點之間 的連線表示機器人的運動路徑，若一段路徑的兩端節(jié)點分別為實心和空心點，或兩端節(jié)點 均為實心點，則機器人采用腿式運動模式通過該段路徑;若一段路徑的兩端節(jié)點均為空心 點，則機器人采用輪式運動模式通過該段路徑。圖中A區(qū)表示輪式運動和腿式運動都不能通 過的障礙物區(qū)域，B區(qū)表示只有腿式運動可以通過，C區(qū)表示輪式及腿式運動都可以通過的 區(qū)域。
[0051 ]以起始點qstart點為隨機樹的起始點，初始化qnew=qstart，qnew代表通過最新一次拓 展獲得的可以加入隨機樹的節(jié)點。
[0052] Π 、若qnew與qgoal之間的距離小于設(shè)定值閾值dis_threshold，則進行步驟VI，否則 進行步驟m。
[0053] m、在已建立的通過性地圖上選取一個目標點qtarget，選取方法如下：
[0054] 在區(qū)間[0，1 ]內(nèi)生成一個隨機數(shù)P，且P服從均勾分布，若p〈pg，Pg為設(shè)定值，則qtarget = qg〇ai，否則qtarget從通過性地圖上的機器人輪式及腿式運動都可以通過的區(qū)域內(nèi)隨機選 取，且要求選取的q target滿足：
[0055] h(q start, Qtarget ) ( qtarget, Qgoal ) ^Tc
[0056] 其中，h為定義的代價啟發(fā)函數(shù)：
[0057]
[0058]其中，qi，q2分別為起始和目標點變量，I ine(qi，q2)為連接qi，q2的線段上所有柵格 點的集合。
[0059] h(qi，q2)提供了連接qi，q2路徑的最小能耗估計，若：
[0060] h ( qstart, Qtarget ) ( qtarget, Qgoal ) ^Tc
[0061 ] 則添加qtarg(3t后，整個路徑的最小能耗估計超過能耗限制Tc，因此q targ(3t不能作為 目標點。通過上述目標點選取方法，大大的減小了目標點的選取范圍，同時保證了目標點選 取方法的探索性，加快了隨機樹的建立過程。
[0062] IV、如圖4所示，在已生成的隨機樹上選取距離qtarget依次最近的SffAq nearJ, q_r_2，qn__3，并通過以下方法，從選取的3個節(jié)點擴展3條子路徑到 qtarge3t，每條子路徑為從 q_r_k向qtarget生長的直線段，k為自路徑編號，k= 1，2，3;并估計這三條子路徑的可行性： [0063] 1)若子路徑經(jīng)過機器人輪式及腿式運動不可以通過的柵格，則子路徑擴展失敗， 進行其它子路徑的擴展；
[0064] 2)子路徑從qnear_k向qtarget生長時，遇到輪式及腿式運動都可以通過的柵格時，采 用輪式運動方式通過。
[0065] 3)子路徑從qnear_k向qtarget生長時，遇到只有腿式運動可以通過的柵格時，機器人 轉(zhuǎn)化為腿式運動模式，采用腿式通過。
[0066] 4)形成的子路徑中，若存在一個輪式及腿式運動都可以通過的柵格時，相鄰的兩 個柵格均為機器人輪式及腿式運動不可以通過的柵格時，則將這個輪式及腿式運動都可以 通過的柵格的通過方式改為機器人輪式及腿式運動不可以通過的柵格，如圖5所示。
[0067] 上述生成的子路徑可行性的判斷方法為：
[0068] CoS t ( Qstart，Qnear ) +CoS t ( Qnear，Qtarget) +h ( Qtarget，Qgoal )〈Tc
[0069] 其中，函數(shù)c〇st(a,b) = Σ,?〇辦為連接a，b的子路徑的代價總 和，即a，b的子路徑上每個柵格的輪/腿運動形式的評價指標，其中node為自路 徑上柵格編號，mode代表w，I，postion為連接a，b的子路徑上的節(jié)點的累積。
[0070] V、如3條子路徑均滿足上述代價要求，則三條子路徑可行，若3條子路徑中，有一 條及以上的路徑可行，則隨機選取一條子路徑加入到現(xiàn)有隨機樹中，并令q new=qtarget，返回 步驟Π。若3條子路徑均擴展不成功，則在每個子路徑上重新獲取目標點，
[0071 ] (hargetjc - - )〇qneurk + X(hii.ryet 入仁(〇，.l).
[0072] λ = λ-〇.2,初始時，令λ = 〇.8,獲取每個子路徑新目標點后，重復(fù)步驟IV進行擴展； 若λ=ο時，則整個子擴展過程失敗，返回步驟m，否則返回步驟V繼續(xù)進行子擴展。
[0073] VI、返回形成的RRT，獲得從起始點qstart到目標Aqgcial的路徑，并估計該路徑的代 價，令其等于Tc，至此完成一次路徑規(guī)劃迭代子過程。
【主權(quán)項】
1. 本發(fā)明一種六輪/腿機器人復(fù)合運動路徑規(guī)劃方法，其特征在于：通過下述步驟完 成： 步驟1:探測并確定機器人工作環(huán)境信息，建立其工作環(huán)境的DEM圖； 步驟2:對步驟1中建立的呢Μ圖進行特征提取，包括:坡度、臺階類障礙高度與粗糖度Ξ 種地貌特征;采用Ξ種地貌特征與評價指標之間的映射，遍歷DEM圖中每個柵格，得到機器 人的通過性地圖及腿式、輪式運動代價圖，圖中標記有機器人輪式及腿式運動都可W通過 的柵格，輪式及腿式運動都不能通過的柵格，W及只有腿式運動可W通過的柵格； 步驟3:規(guī)劃獲得機器人最佳混合路徑； A、 在步驟2中得到的腿式運動代價圖上，W機器人出發(fā)點qstart為隨機樹的根節(jié)點，機器 人采用單純腿式運動，W標準隨機樹算法進行擴展，逐漸增加葉節(jié)點直至隨機樹的葉節(jié)點 中包含了終點qgoal，得到從qstart到qgoal之間的一條單純腿式運動目標路徑T { 1 }; B、 估計目標路徑Τ{1}的代價Tc; C、 WT。為初始代價，進行迭代求解，至所用的時間超過用戶設(shè)定的最大時限;每次迭代 中尋找到的混合運動路徑代價需小于(l-OTc，其中ε為迭代因子。2. 如權(quán)利要求1所述一種六輪/腿機器人復(fù)合運動路徑規(guī)劃方法，其特征在于：步驟2 中，坡度、臺階類障礙高度與粗糖度Ξ種地貌特征提取方式為： ① 坡度:令i，j為柵格坐標，機器人本體在Χ-Υ平面內(nèi)投影的最小包絡(luò)矩形為R( i，j)，將 R( i，j)內(nèi)的平均傾角作為坡度特征，表示為：其中，Θχ表示沿X軸方向的平均傾角，Θγ表示沿y方向的平均傾角； ② 粗糖度:采用R( i，j)區(qū)域內(nèi)高程的標準差進行估計，表示為：其中，刮表示R(i，j)區(qū)域內(nèi)的高程平均值，η為R(i，j)區(qū)域內(nèi)柵格的個數(shù)； ③ 臺階類障礙高度:表示為： Ei, j - Zmax (Ri,j)~Zmin(Ri,j) 其中，Zmax和Zmin為R( i，j )內(nèi)最高和最低高程值。 3 .如權(quán)利要求1所述一種六輪/腿機器人復(fù)合運動路徑規(guī)劃方法，其特征在于:步驟2的 b中生成輪/腿運動形式的代價圖的具體方式為:根據(jù)地形特征，結(jié)合機器人輪式運動與腿 式運動的特點，分別計算機器人在R(i，j)內(nèi)每個柵格的輪/腿運動形式的評價指標，包括輪 運動形式的評價指標與腿運動形式的評價指標4;通過BP人工神經(jīng)網(wǎng)絡(luò)，建立每個柵格 內(nèi)提取的各個地形特征01,^，81,^，61,神日評價指標;巧；.（^之間的映射;建立^層8?人工神 經(jīng)網(wǎng)絡(luò)，并通過ADMS仿真環(huán)境及實驗中獲得的不同地形中機器人能耗，時間數(shù)據(jù)對神經(jīng)網(wǎng) 絡(luò)進行訓(xùn)練，模擬地形特征與評價指標之間的映射;采用上述模擬地形特征與評價指標之 間的映射，遍歷步驟1中建立的DEM圖中每個柵格，得到機器人的通過性地圖及腿式、輪式運 動代價圖，圖中標記有機器人輪式及腿式運動都可W通過的柵格，輪式及腿式運動都不能 通過的柵格(障礙物區(qū)域），w及只有腿式運動可w通過的柵格。4. 如權(quán)利要求3所述一種六輪/腿機器人復(fù)合運動路徑規(guī)劃方法，其特征在于:評價指 標Cij的形式為[TrCiEffc]，其中，Trc為通過性指標，T;rc = 0表示可通過區(qū)域R(i，j) ;T;rc = l 表示不可通過區(qū)域R(i，j); Ef f c為代價評價指標，描述通過區(qū)域R(i，j)的代價，Ef f c = (Ο， 1)，為Ο到1之間的連續(xù)值。5. 如權(quán)利要求1所述一種六輪/腿機器人復(fù)合運動路徑規(guī)劃方法，其特征在于:所述步 驟A中的每一次迭代步驟如下： I、在機器人周圍的通過性地圖中已生成的部分隨機樹，設(shè)定起始點qstart，終點為qgoal 點，由操作者設(shè)定；W點代表隨機樹的節(jié)點，其中，實屯、點表示采用腿式運動通過的點，空屯、 點表示采用輪式運動通過的點；節(jié)點之間的連線表示機器人的運動路徑，若一段路徑的兩 端節(jié)點分別為實屯、和空屯、點，或兩端節(jié)點均為實屯、點，則機器人采用腿式運動模式通過該 段路徑，；若一段路徑的兩端節(jié)點均為空屯、點，則機器人采用輪式運動模式通過該段路徑； W起始點qstart點為隨機樹的起始點，初始化qnew= qstart，qnew代表通過最新一次拓展獲得的 可W加入隨機樹的節(jié)點； Π 、若qnew與qgoal之間的距離小于設(shè)定值闊值dis_threshold，則進行步驟VI，否則進行 步驟虹； 虹、在已建立的通過性地圖上選取一個目標點qtarget，選取方法如下： 在區(qū)間[〇，1]內(nèi)生成一個隨機數(shù)P，且P服從均勻分布，若P<Pg，Pg為設(shè)定值，則qtarget = qgnal，否則qtarget從通過性地圖上的機器人輪式及腿式運動都可W通過的區(qū)域內(nèi)隨機選取， 且要求選取的qtarget滿足： h ( qstart) qi;arge〇 +h ( qi;arget) qgoal〈Tc 其中，h為定義的代價啟發(fā)函數(shù)：其中，qi，Q2分別為起始和目標點變量，1 ine (qi，Q2)為連接qi，Q2的線段上所有柵格點的 集合； IV、在已生成的隨機樹上選取距離qtarget依次最近的3個節(jié)點qnear_l，qnear_2，qnear_3，并通 過W下方法，從選取的3個節(jié)點擴展3條子路徑到qtarget，每條子路徑為從qnear_k向qtarget生長 的直線段，k為自路徑編號，k= 1，2，3 ;并估計運Ξ條子路徑的可行性： 1 )若子路徑經(jīng)過機器人輪式及腿式運動不可W通過的柵格，則子路徑擴展失敗，進行 其它子路徑的擴展； 2) 子路徑從qnear_k向qtarget生長時，遇到輪式及腿式運動都可W通過的柵格時，采用輪 式運動方式通過； 3) 子路徑從qnear_k向qtarget生長時，遇到只有腿式運動可W通過的柵格時，機器人轉(zhuǎn)化 為腿式運動模式，采用腿式通過； 4) 形成的子路徑中，若存在一個輪式及腿式運動都可W通過的柵格時，相鄰的兩個柵 格均為機器人輪式及腿式運動不可W通過的柵格時，則將運個輪式及腿式運動都可W通過 的柵格的通過方式改為機器人輪式及腿式運動不可W通過的柵格； 生成的子路徑可行性的判斷方法為： Cost ( Qstart, Qiiear )+C〇St ( qnear , qtarget) +h (qtarget, Qgoal )〈Tc 其中，函數(shù)cost紅b)二二Pf加知b)旬茲f品迸Li，.為連接a，b的子路徑的代價總和，即 a,b的子路徑上每個柵格的輪/腿運動形式的評價指標C品藍品益ij，。，其中node為自路徑上 柵格編號，mode代表W, 1，pos t i on為連接a, b的子路徑上的節(jié)點的累積； ν、Ξ條子路徑可行，若3條子路徑中，有一條及W上的路徑可行，則隨機選取一條子路 徑加入到現(xiàn)有隨機樹中，并令qnew= qtarget，返回步驟Π ;若3條子路徑均擴展不成功，則在每 個子路徑上重新獲取目標點， Qtarget k _ ( 1 -入)Qneark+入Ql;arget入 E ( 〇 , 1 ) 入=λ-0.2，初始時，令λ = 0.8，獲取每個子路徑新目標點后，重復(fù)步驟IV進行擴展;若入 =0時，則整個子擴展過程失敗，返回步驟虹，否則返回步驟V繼續(xù)進行子擴展； VI、返回形成的RRT，獲得從起始點qstart到目標點qgoal的路徑，并估計該路徑的代價，令 其等于Tc，至此完成一次路徑規(guī)劃迭代子過程。
【文檔編號】G05D1/02GK105843222SQ201610158085
【公開日】2016年8月10日
【申請日】2016年3月18日
【發(fā)明人】丁希侖, 楊帆, 彭賽金, 鄭羿, 尹業(yè)成
【申請人】北京航空航天大學(xué)