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

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

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