專利名稱：一種基于類三維地圖的移動(dòng)機(jī)器人全局路徑規(guī)劃方法
技術(shù)領(lǐng)域：
本發(fā)明屬于機(jī)器人及其智能控制領(lǐng)域，涉及一種基于類三維地圖的移動(dòng)機(jī)器人全局路徑規(guī)劃方法。

背景技術(shù)：
移動(dòng)機(jī)器人路徑規(guī)劃的任務(wù)是在有障礙物的工作環(huán)境中尋找一條從給定起點(diǎn)到終點(diǎn)的適當(dāng)?shù)倪\(yùn)動(dòng)路徑，使機(jī)器人在運(yùn)動(dòng)過程中能安全無(wú)碰的繞過所有障礙物。根據(jù)工作環(huán)境的不同，路徑規(guī)劃可以分為兩種工作環(huán)境的信息全部已知的全局路徑規(guī)劃；工作環(huán)境的信息全部未知或部分未知的局部路徑規(guī)劃。
目前應(yīng)用較多的全局路徑規(guī)劃算法包括拓?fù)浞?、可視圖法和柵格法。拓?fù)浞ń⑼負(fù)渚W(wǎng)絡(luò)的過程相當(dāng)復(fù)雜，特別在增加障礙物時(shí)如何有效的修正已經(jīng)存在的拓?fù)渚W(wǎng)絡(luò)及如何提高圖形速度是有待解決的問題；可視圖法在搜索最短距離路徑時(shí)對(duì)所有定點(diǎn)的價(jià)值不加以區(qū)別，僅僅只是將所有可行路徑列舉出來(lái)然后在其中選出一條最優(yōu)路徑，導(dǎo)致耗時(shí)較長(zhǎng)，特別是當(dāng)機(jī)器人的尺寸大小很小時(shí)搜索時(shí)間很長(zhǎng)；柵格法中柵格粒度太大，規(guī)劃的路徑很不準(zhǔn)確；柵格粒度太小，算法的搜索范圍將按指數(shù)增加，計(jì)算量大，占用大量的存儲(chǔ)空間，實(shí)時(shí)性不理想。
這些方法均側(cè)重于路徑的生成過程，只利用障礙物的邊界信息，而不能根據(jù)障礙物信息來(lái)進(jìn)行決策。因此充分發(fā)掘障礙物信息，設(shè)計(jì)一種簡(jiǎn)單高效且易于編程實(shí)現(xiàn)的全局路徑規(guī)劃算法，是提高程序執(zhí)行效率、準(zhǔn)確規(guī)劃路徑所必須考慮的問題。


發(fā)明內(nèi)容
本發(fā)明的目的是克服上述現(xiàn)有全局路徑規(guī)劃算法的不足，提出一種基于類三維地圖的移動(dòng)機(jī)器人全局路徑規(guī)劃方法，充分發(fā)掘障礙物信息，提高路徑規(guī)劃效率，易于實(shí)現(xiàn)，滿足移動(dòng)機(jī)器人全局路徑規(guī)劃的實(shí)時(shí)性要求。
本發(fā)明的技術(shù)方案如下 一種基于類三維地圖的移動(dòng)機(jī)器人全局路徑規(guī)劃方法，其特征在于，包括以下步驟 步驟1將普通柵格地圖改造為基于等高線原理的類三維地圖； 步驟2初始規(guī)劃將移動(dòng)機(jī)器人的起點(diǎn)和終點(diǎn)連成一條直線作為當(dāng)前路徑； 步驟3掃描當(dāng)前路徑穿過的柵格，若未掃描到障礙，則轉(zhuǎn)至步驟6；若掃描到障礙，則首先確定障礙的類型，障礙的類型包括與邊界接觸的障礙和不與邊界接觸的障礙；如果是與邊界接觸的障礙，轉(zhuǎn)至步驟4，否則轉(zhuǎn)至步驟5；所述的與邊界接觸的障礙包括只與一側(cè)邊界接觸的障礙以及與兩側(cè)邊界接觸的障礙； 步驟4處理與邊界接觸的障礙，更新當(dāng)前路徑，返回步驟3； 步驟5處理不與邊界接觸的障礙，更新當(dāng)前路徑，返回步驟3； 步驟6輸出當(dāng)前路徑，全局路徑規(guī)劃完成； 所述的步驟1的具體步驟為 給柵格地圖建立I、J坐標(biāo)系，柵格地圖中的柵格用數(shù)組ZG表示；數(shù)組ZG的元素ZG[I][J]表示高度值； 依據(jù)等高線原理建立類三維地圖，就是給地圖中的每一個(gè)柵格點(diǎn)都賦予高度值，ZG[I][J]＝H，H∈{0、1、2、3……}，H值越大表示高度越高； 柵格地圖分為兩種區(qū)域自由區(qū)域與障礙區(qū)域，自由區(qū)域的高度值H＝0；障礙區(qū)域的高度值H＞0，障礙區(qū)域分為三種不與邊界接觸的障礙區(qū)域，只與一側(cè)邊界接觸的障礙區(qū)域，與兩側(cè)邊界接觸的障礙區(qū)域；只與一側(cè)邊界接觸的障礙區(qū)域以及與兩側(cè)邊界接觸的障礙區(qū)域?qū)?yīng)的障礙統(tǒng)稱為障礙；以下分三種情況說(shuō)明障礙區(qū)域的類三維地圖的創(chuàng)建方法 (I)對(duì)于不與邊界接觸的障礙區(qū)域 iii 找出該區(qū)域I坐標(biāo)的最大值Imax、I坐標(biāo)的最小值Imin、J坐標(biāo)的最大值Jmax、J坐標(biāo)的最小值Jmin； iv 給該區(qū)域的所有I＝Imax和I＝Imin的兩列柵格的高度值賦予1，給該區(qū)域的所有I＝(Imax-1)和I＝(Imin+1)的兩列柵格的高度值賦予2，給該區(qū)域的所有I＝(Imax-2)和I＝(Imin+2)的兩列柵格的高度值賦予3，依此類推，直到該區(qū)域所有的柵格都被賦予一個(gè)高度值； v 給該區(qū)域的所有J＝Jmax和J＝Jmin的兩行柵格的高度值加上1，給該區(qū)域的所有J＝(Jmax-1)和J＝(Jmin+1)的兩行柵格的高度值加上2，給該區(qū)域的所有J＝(Jmax-2)和J＝(Jmin+2)的兩行柵格的高度值加上3，依此類推，直到該區(qū)域所有的柵格都被加上一個(gè)高度值； (II)對(duì)于只與一側(cè)邊界接觸的障礙區(qū)域 i、出該區(qū)域I坐標(biāo)的最大值Imax、I坐標(biāo)的最小值Imin、J坐標(biāo)的最大值Jmax、J坐標(biāo)的最小值Jmin； ii、判斷Imax、Imin、Jmax、Jmin四個(gè)值中哪個(gè)為邊界值，即判斷障礙區(qū)域與柵格地圖的上、下、左、右四個(gè)邊界中的哪個(gè)接觸，從與該邊界相對(duì)的一側(cè)開始給障礙區(qū)域賦予高度值；給障礙區(qū)域中離該邊界最遠(yuǎn)的一行或一列柵格賦予高度值1，給離該邊界第二遠(yuǎn)的一行或一列柵格賦予高度值2，給離該邊界第三遠(yuǎn)的一行或一列柵格賦予高度值3，依此類推，直到離該邊界最近的一行或一列柵格也被賦予高度值； iii、然后從另一個(gè)方向開始給障礙區(qū)域的柵格加上高度值，即不妨設(shè)上一步中是按I坐標(biāo)方向給柵格賦值，則本步驟中按J坐標(biāo)方向給柵格加上高度值；給該區(qū)域的所有J＝Jmax和J＝Jmin的兩行柵格的高度值加上1，給該區(qū)域的所有J＝(Jmax-1)和J＝(Jmin+1)的兩行柵格的高度值加上2，給該區(qū)域的所有J＝(Jmax-2)和J＝(Jmin+2)的兩行柵格的高度值加上3，依此類推，直到該區(qū)域所有的柵格都被加上一個(gè)高度值； (II)對(duì)于與兩側(cè)邊界接觸的障礙區(qū)域 a)找出該區(qū)域I坐標(biāo)的最大值Imax、I坐標(biāo)的最小值Imin、J坐標(biāo)的最大值Jmax、J坐標(biāo)的最小值Jmin； b)判斷Imax、Imin、Jmax、Jmin四個(gè)值中哪兩個(gè)為邊界值，即判斷障礙區(qū)域與地圖的上、下、左、右四個(gè)邊界中的哪兩個(gè)邊界接觸，然后從兩個(gè)被接觸的邊界中任選一個(gè)，從與該邊界相對(duì)的一側(cè)開始給障礙區(qū)域賦予高度值；不妨設(shè)選擇行對(duì)應(yīng)的邊界為參照，給離該邊界最遠(yuǎn)的一行柵格賦予高度值1，給離該邊界第二遠(yuǎn)的一行柵格賦予高度值2，給離該邊界第三遠(yuǎn)的一行柵格賦予高度值3，依此類推，直到離該邊界最近的一行柵格也被賦予高度值； 然后從與另一邊界相對(duì)的一側(cè)開始給障礙區(qū)域加上高度值；給離另一邊界最遠(yuǎn)的一列柵格的高度值加上1，給離另一邊界第二遠(yuǎn)的一列柵格的高度值加上2，給離另一邊界第三遠(yuǎn)的一列柵格的高度值加上3，依此類推，直到離另一邊界最近的一列柵格也被加上高度值。
所述的步驟4中的處理與邊界接觸的障礙的步驟以及所述的步驟5中的處理不與邊界接觸的障礙的步驟為當(dāng)前路徑上每段位于與邊界接觸的障礙區(qū)域的路徑段按照下降方向進(jìn)行基本規(guī)劃； 定義下降方向若路徑穿過了某個(gè)障礙區(qū)域，則以位于障礙區(qū)域內(nèi)的路徑段中高度最大的點(diǎn)為起點(diǎn)，垂直于該路徑段且所經(jīng)過的柵格的高度值降低的方向就是下降方向； 定義基本規(guī)劃若兩點(diǎn)之間的初始規(guī)劃路徑穿過了障礙區(qū)域，則找到每段位于障礙區(qū)域的路徑的下降方向，然后將每段位于障礙區(qū)域的路徑用沿著障礙區(qū)域從下降方向繞過障礙區(qū)域的新路徑段代替，基本規(guī)劃就是對(duì)初始規(guī)劃進(jìn)行修正，基本規(guī)劃后的路徑段稱為修正路徑段； 具體步驟分為以下四步 I、找出當(dāng)前路徑上每段位于與邊界接觸的障礙區(qū)域的路徑段的起點(diǎn)與終點(diǎn)， 修正路徑段上的點(diǎn)(Iy，Jy)需滿足以下三個(gè)條件 第一個(gè)條件位于下降方向一側(cè)，即，滿足下式
式中(Im，Jm)為該段穿過了障礙區(qū)域的路徑段的起點(diǎn)坐標(biāo)，(In，Jn)為該段穿過了障礙區(qū)域的路徑段的終點(diǎn)坐標(biāo)，(Iam，Jam)為該障礙區(qū)域的高度值最大的點(diǎn)的坐標(biāo)； 第二個(gè)條件高度值為0，ZG[Iy][Jy]＝0； 第三個(gè)條件與對(duì)應(yīng)的障礙區(qū)域直接相鄰；相鄰，即路徑所在柵格與障礙區(qū)域直接相鄰。
II、找出被當(dāng)前路徑穿過的每一塊與邊界接觸的障礙區(qū)域的犄角點(diǎn)； 定義犄角點(diǎn)在基本規(guī)劃中，修正路徑段上的點(diǎn)中與被修正路徑段的距離最大的點(diǎn)就是犄角點(diǎn)； III、找出與當(dāng)前路徑的每一段直線路徑段對(duì)應(yīng)的中轉(zhuǎn)點(diǎn)； 在所有的位于當(dāng)前路徑同一側(cè)的犄角點(diǎn)中離當(dāng)前路徑最遠(yuǎn)的點(diǎn)，稱為與當(dāng)前路徑對(duì)應(yīng)的中轉(zhuǎn)點(diǎn)；若當(dāng)前路徑為折線，則要找出與折線的每一段直線路徑段對(duì)應(yīng)的中轉(zhuǎn)點(diǎn)； IV、將起點(diǎn)、所有中轉(zhuǎn)點(diǎn)以及終點(diǎn)沿當(dāng)前路徑的方向用折線順次連接起來(lái)，將新得到的折線路徑作為當(dāng)前路徑；轉(zhuǎn)到步驟3。
所述移動(dòng)機(jī)器人為輪式移動(dòng)機(jī)器人，其移動(dòng)裝置采用輪式結(jié)構(gòu)，具備沿弧線與直線及折線運(yùn)動(dòng)的能力。
所述障礙的輪廓為塊狀。
所述的移動(dòng)機(jī)器人設(shè)有傳感器，能確定機(jī)器人在柵格地圖中的位置，所述的傳感器有GPS裝置、激光測(cè)距儀、紅外測(cè)距儀、或者超聲波測(cè)距儀。所述的移動(dòng)機(jī)器人所配置的傳感器能感知一定范圍內(nèi)的障礙物信息，包括與機(jī)器人的距離和障礙物的大小。
有益效果 與現(xiàn)有的技術(shù)相比，本發(fā)明用一種簡(jiǎn)單的方法對(duì)障礙區(qū)域進(jìn)行處理，并且當(dāng)環(huán)境中的障礙區(qū)域發(fā)生變化時(shí)處理算法也不會(huì)變復(fù)雜，克服了拓?fù)鋱D法建立拓?fù)鋱D很復(fù)雜的缺點(diǎn)；本發(fā)明創(chuàng)建類三維地圖后，提出了下降方向的概念，當(dāng)規(guī)劃過程中遇到障礙時(shí)，下降方向就像路標(biāo)一樣及時(shí)指出最佳避障方向，克服了可視圖法和普通柵格法要通過比較若干條不同路徑才能得出最佳避障路徑的缺點(diǎn)；本發(fā)明的規(guī)劃時(shí)間可以預(yù)期，最壞情況下需進(jìn)行的規(guī)劃次數(shù)為地圖中障礙區(qū)域個(gè)數(shù)，克服了可視圖法和普通柵格法要進(jìn)行地圖中障礙區(qū)域總數(shù)目的若干倍次規(guī)劃的缺點(diǎn)，也克服了可視圖法和普通柵格法在地圖中障礙區(qū)域增加時(shí)規(guī)劃次數(shù)呈指數(shù)增加的缺點(diǎn)；本發(fā)明在規(guī)劃過程中只需搜索起點(diǎn)與終點(diǎn)之間的直連線周圍一定范圍內(nèi)的障礙區(qū)域，并不對(duì)地圖中的所有障礙區(qū)域進(jìn)行搜索，大大提高了規(guī)劃效率，提高了規(guī)劃的時(shí)效性。



圖1為全局路徑規(guī)劃算法主要實(shí)現(xiàn)過程的程序流程圖； 圖2為類三維地圖的效果圖； 圖3為從一個(gè)坐標(biāo)方向?yàn)槊總€(gè)柵格賦予高度值的一個(gè)障礙區(qū)域； 圖4為從兩個(gè)坐標(biāo)方向?yàn)槊總€(gè)柵格賦予高度值的一個(gè)障礙區(qū)域； 圖5為下降方向示意圖； 圖6為修正路徑段示意圖； 圖7為犄角點(diǎn)示意圖； 圖8為當(dāng)前路徑為直線時(shí)中轉(zhuǎn)點(diǎn)示意圖； 圖9為當(dāng)前路徑為折線時(shí)每段直線的中轉(zhuǎn)點(diǎn)示意圖； 圖10為基于圖8中的柵格地圖輸出的最終全局規(guī)劃路徑。
圖中標(biāo)號(hào)說(shuō)明1，2，3均表示障礙區(qū)域， f表示下降方向， A，B表示路徑起點(diǎn)和終點(diǎn)， 圖7中C，D，E表示犄角點(diǎn)， 圖8中E點(diǎn)為中轉(zhuǎn)點(diǎn)， 圖9中折線ADEB為當(dāng)前路徑，C為犄角點(diǎn)和與AD段對(duì)應(yīng)的中轉(zhuǎn)點(diǎn)。

具體實(shí)施例方式 以下將結(jié)合圖和具體實(shí)施過程對(duì)本發(fā)明做進(jìn)一步詳細(xì)說(shuō)明 實(shí)施方式1 圖1為全局路徑規(guī)劃算法主要實(shí)現(xiàn)過程的程序流程圖，具體步驟如下(1)在普通二值柵格地圖的基礎(chǔ)上依據(jù)等高線原理建立類三維地圖，給地圖中的每一個(gè)柵格點(diǎn)都賦予高度值。給地圖建立I、J坐標(biāo)系，地圖中的柵格用數(shù)組ZG[I][J]表示。柵格的高度值用H表示，即，ZG[I][J]＝H，H∈{0、1、2、3……}，H值越大表示高度越高。
在本發(fā)明中將地圖分為兩種區(qū)域自由區(qū)域與障礙區(qū)域。自由區(qū)域的高度值H＝0；障礙區(qū)域的高度值H＞0，不與邊界接觸的障礙區(qū)域中位于重心的柵格點(diǎn)高度值最大，與邊界接觸的障礙區(qū)域中與邊界接觸的柵格點(diǎn)高度值最大，以最高點(diǎn)為中心向四周輻射高度值逐漸減小。障礙區(qū)域分為三種不與邊界接觸的障礙區(qū)域，只與一側(cè)邊界接觸的障礙區(qū)域，與兩側(cè)邊界接觸的障礙區(qū)域。以下分三種情況介紹障礙區(qū)域的類三維地圖的創(chuàng)建方法。
a)對(duì)于不與邊界接觸的障礙區(qū)域 先找出該區(qū)域I坐標(biāo)的最大值Imax、I坐標(biāo)的最小值Imin、J坐標(biāo)的最大值Jmax、J坐標(biāo)的最小值Jmin。
然后給該區(qū)域的所有I＝Imax和I＝Imin的兩列柵格的高度值賦予1，給該區(qū)域的所有I＝(Imax-1)和I＝(Imin+1)的兩列柵格的高度值賦予2，給該區(qū)域的所有I＝(Imax-2)和I＝(Imin+2)的兩列柵格的高度值賦予3，依此類推，直到該區(qū)域所有的柵格都被賦予一個(gè)高度值。
最后給該區(qū)域的所有J＝Jmax和J＝Jmin的兩行柵格的高度值加上1，給該區(qū)域的所有J＝(Jmax-1)和J＝(Jmin+1)的兩行柵格的高度值加上2，給該區(qū)域的所有J＝(Jmax-2)和J＝(Jmin+2)的兩行柵格的高度值加上3，依此類推，直到該區(qū)域所有的柵格都被加上一個(gè)高度值。
b)對(duì)于只與一側(cè)邊界接觸的障礙區(qū)域 先找出該區(qū)域I坐標(biāo)的最大值Imax、I坐標(biāo)的最小值Imin、J坐標(biāo)的最大值Jmax、J坐標(biāo)的最小值Jmin。
判斷Imax、Imin、Jmax、Jmin四個(gè)值中哪個(gè)為邊界值，即判斷障礙區(qū)域與地圖的上、下、左、右四個(gè)邊界中的哪個(gè)接觸，從與該邊界相對(duì)的一側(cè)開始給障礙區(qū)域賦予高度值。給障礙區(qū)域中離該邊界最遠(yuǎn)的一行或一列柵格賦予高度值1，給離該邊界第二遠(yuǎn)的一行或一列柵格賦予高度值2，給離該邊界第三遠(yuǎn)的一行或一列柵格賦予高度值3，依此類推，直到離該邊界最近的一行或一列柵格也被賦予高度值。
然后從另一個(gè)方向開始給障礙區(qū)域的柵格加上高度值，即若上一步中是按I(或J)坐標(biāo)方向給柵格賦值，則本步驟中按J(或I)坐標(biāo)方向給柵格加上高度值。給該區(qū)域的所有J＝Jmax和J＝Jmin(或I＝Imax和I＝Imin)的兩行(或兩列)柵格的高度值加上1，給該區(qū)域的所有J＝(Jmax-1)和J＝(Jmin+1)(或I＝(Imax-1)和I＝(Imin+1))的兩行(或兩列)柵格的高度值加上2，給該區(qū)域的所有J＝(Jmax-2)和J＝(Jmin+2)(或I＝(Imax-2)和I＝(Imin+2))的兩行(或兩列)柵格的高度值加上3，依此類推，直到該區(qū)域所有的柵格都被加上一個(gè)高度值。
c)對(duì)于與兩側(cè)邊界接觸的障礙區(qū)域 先找出該區(qū)域I坐標(biāo)的最大值Imax、I坐標(biāo)的最小值Imin、J坐標(biāo)的最大值Jmax、J坐標(biāo)的最小值Jmin。
判斷Imax、Imin、Jmax、Jmin四個(gè)值中哪兩個(gè)為邊界值，即判斷障礙區(qū)域與地圖的上、下、左、右四個(gè)邊界中的哪兩個(gè)接觸，然后從兩個(gè)被接觸的邊界中任選一個(gè)，從與該邊界相對(duì)的一側(cè)開始給障礙區(qū)域賦予高度值。給離該邊界最遠(yuǎn)的一行(或一列)柵格賦予高度值1，給離該邊界第二遠(yuǎn)的一行(或一列)柵格賦予高度值2，給離該邊界第三遠(yuǎn)的一行(或一列)柵格賦予高度值3，依此類推，直到離該邊界最近的一行(或一列)柵格也被賦予高度值。
然后從與另一邊界相對(duì)的一側(cè)開始給障礙區(qū)域加上高度值。給離另一邊界最遠(yuǎn)的一列(或一行)柵格的高度值加上1，給離另一邊界第二遠(yuǎn)的一列(或一行)柵格的高度值加上2，給離另一邊界第三遠(yuǎn)的一列(或一行)柵格的高度值加上3，依此類推，直到離另一邊界最近的一列(或一行)柵格也被加上高度值。
類三維地圖的效果圖如圖2所示。圖2中的數(shù)字表示該數(shù)字所在柵格的高度值，沒有寫數(shù)字的柵格的高度值為0(因?yàn)楦叨戎礖＝0的柵格的數(shù)目較大，所以未在圖中標(biāo)出)。
下面以圖2中的2號(hào)障礙區(qū)域?yàn)槔f(shuō)明類三維地圖的創(chuàng)建方法 2號(hào)障礙區(qū)域?yàn)椴慌c邊界接觸的障礙區(qū)域。
先找出該區(qū)域I坐標(biāo)的最大值Imax、I坐標(biāo)的最小值Imin、J坐標(biāo)的最大值Jmax、J坐標(biāo)的最小值Jmin。
然后給該區(qū)域的所有I＝Imax和I＝Imin的兩列柵格的高度值賦予I，給該區(qū)域的所有I＝(Imax-1)和I＝(Imin+1)的兩列柵格的高度值賦予2，給該區(qū)域的所有I＝(Imax-2)和I＝(Imin+2)的兩列柵格的高度值賦予3，依此類推，直到該區(qū)域所有的柵格(也即(Imax-n)＝(Imin+n)或(Imax-n)＝(Imin+n)+1)都被賦予一個(gè)高度值。如圖3所示。
然后給該區(qū)域的所有J＝Jmax和J＝Jmin的兩行柵格的高度值加上1，給該區(qū)域的所有J＝(Jmax-1)和J＝(Jmin+1)的兩行柵格的高度值加上2，給該區(qū)域的所有J＝(Jmax-2)和J＝(Jmin+2)的兩行柵格的高度值加上3，依此類推，直到該區(qū)域所有的柵格(也即(Jmax-m)＝(Jmin+m)或(Jmax-m)＝(Jmin+m)+1)都被加上一個(gè)高度值。如圖4所示。
(2)對(duì)起點(diǎn)與終點(diǎn)進(jìn)行初始規(guī)劃，所得到的路徑稱為起點(diǎn)與終點(diǎn)間的初始規(guī)劃路徑，并將初始規(guī)劃路徑作為當(dāng)前路徑。
初始規(guī)劃就是找到兩點(diǎn)之間的直線路徑。
初始規(guī)劃的實(shí)現(xiàn)方法為先確定給定的起點(diǎn)的坐標(biāo)(Im，Jm)和終點(diǎn)的坐標(biāo)(In，Jn)，設(shè)起點(diǎn)和終點(diǎn)之間直線連線上任意一點(diǎn)的坐標(biāo)為(Ix，Jx)，因?yàn)檫@三點(diǎn)在一條直線上，則這三點(diǎn)的坐標(biāo)須滿足下面的式子 令代入(1)式得 Im＜Ix＜In ….…………………………………(2) Ix在Im與In之間取整數(shù)值，對(duì)于每一個(gè)Ix的取值，由2式可求得一個(gè)Jx，但Jx可能不為整數(shù)，故定義運(yùn)算[。]為雙邊取整運(yùn)算，即若對(duì)整數(shù)進(jìn)行[。]運(yùn)算，則結(jié)果為該整數(shù)本身；若對(duì)非整數(shù)進(jìn)行[。]運(yùn)算，則結(jié)果為最接近該整數(shù)的兩個(gè)整數(shù)，例如[5]＝5，[5.4]＝5、6。
對(duì)2式中的Jx進(jìn)行[。]運(yùn)算即得 Im＜Ix＜In ….…………………………………(3) 所以點(diǎn)(Ix，Jx)的坐標(biāo)為
所以起點(diǎn)和終點(diǎn)之間初始規(guī)劃路徑上任意一點(diǎn)(Ix，Jx)的坐標(biāo)由(4)式確定。
(3)若當(dāng)前路徑穿過了任何一塊與邊界接觸的障礙區(qū)域，則轉(zhuǎn)到步驟 (4)去處理與邊界接觸的障礙區(qū)域；若當(dāng)前路徑穿過了任何一塊不與邊界接觸的障礙區(qū)域，則轉(zhuǎn)到步驟(5)去處理不與邊界接觸的障礙區(qū)域；否則，轉(zhuǎn)到步驟(6)。
(4)處理與邊界接觸的障礙區(qū)域。
步驟(4)分為以下四小步 I 找出當(dāng)前路徑上每段位于與邊界接觸的障礙區(qū)域的路徑段的起點(diǎn)與終點(diǎn)，然后對(duì)找出的每一對(duì)起點(diǎn)與終點(diǎn)進(jìn)行基本規(guī)劃。步驟(2)中的初始規(guī)劃不具備避障能力，基本規(guī)劃就是對(duì)初始規(guī)劃進(jìn)行修正，使其具有避障能力。
a 定義下降方向若路徑穿過了障礙區(qū)域，則以位于障礙區(qū)域內(nèi)的路徑段中高度最大的點(diǎn)為起點(diǎn)，垂直于該路徑段且所經(jīng)過的柵格的高度值降低的方向就是下降方向。
位于某段穿過了障礙區(qū)域的路徑的下降方向一側(cè)的點(diǎn)(Iy，Jy)需滿足(5)式
式中(Im，Jm)為該段穿過了障礙區(qū)域的路徑段的起點(diǎn)坐標(biāo)，(In，Jn)為該段穿過了障礙區(qū)域的路徑段的終點(diǎn)坐標(biāo)，(Iam，Jam)為該障礙區(qū)域的高度值最大的點(diǎn)的坐標(biāo)。
如圖5所示，圖中的箭頭f表示下降方向。
下降方向的意義對(duì)于與邊界接觸的障礙區(qū)域，下降方向指出了可以成功繞過障礙的方向，使得規(guī)劃程序不會(huì)陷入死胡同；對(duì)于不與邊界接觸的障礙區(qū)域，下降方向指出了障礙區(qū)域的劣弧方向，使得規(guī)劃程序可以以較短的路徑繞過障礙。
b 定義基本規(guī)劃若兩點(diǎn)之間的初始規(guī)劃路徑穿過了障礙區(qū)域，則找到每段位于障礙區(qū)域的路徑的下降方向，然后將每段位于障礙區(qū)域的路徑用沿著障礙區(qū)域從下降方向繞過障礙區(qū)域的新路徑段(稱為修正路徑段)代替。
基本規(guī)劃的實(shí)現(xiàn)方法先用初始規(guī)劃方法得到初始規(guī)劃路徑。比較初始規(guī)劃路徑所經(jīng)過的的所有柵格點(diǎn)的高度值H，即比較由1式確定的所有柵格點(diǎn)的高度值H，找出H＝0的連續(xù)柵格點(diǎn)段和H＞0的連續(xù)柵格點(diǎn)段。對(duì)于H＝0的連續(xù)柵格點(diǎn)段，其對(duì)應(yīng)的路徑段不用作修改；對(duì)于H＞0的連續(xù)柵格點(diǎn)段，其對(duì)應(yīng)的路徑段需要修正，修正后的路徑段稱為修正路徑段，修正方法為找出該障礙區(qū)域的下降方向，然后將沿著障礙區(qū)域從下降方向繞過障礙區(qū)域的折線路徑段作為修正路徑段。
基本規(guī)劃的具體實(shí)現(xiàn)方法分為以下兩步 a)先找到修正路徑段上的點(diǎn)須滿足的三個(gè)條件。
設(shè)修正路徑段上任意一點(diǎn)的坐標(biāo)為(Iy，Jy)。
第一個(gè)條件位于下降方向。
設(shè)初始規(guī)劃路徑上某段H＞0的連續(xù)柵格點(diǎn)段中H值最大的柵格點(diǎn)的坐標(biāo)為(Ia，Ja)；找出點(diǎn)(Ia，Ja)所在障礙區(qū)域的高度值最大的點(diǎn)，設(shè)其坐標(biāo)為(Iam，Jam)；被修正路徑段的起點(diǎn)坐標(biāo)為(Im，Jm)，被修正路徑段的終點(diǎn)坐標(biāo)為(In，Jn)。
易知若點(diǎn)(Iy，Jy)與點(diǎn)(Iam，Jam)位于被修正路徑段的不同側(cè)，則點(diǎn)(Iy，Jy)位于障礙區(qū)域的下降方向一側(cè)。
令 令 易知若則點(diǎn)(Iy，Jy)與點(diǎn)(Iam，Jam)位于被修正路徑段的不同側(cè)。
由此得到修正路徑段上的點(diǎn)(Iy，Jy)須滿足的第一個(gè)條件
第二個(gè)條件高度值為0。
即 ZG[Iy][Jy]＝0…………………..………………..……………(7) 第三個(gè)條件點(diǎn)(Iy，Jy)與對(duì)應(yīng)的障礙區(qū)域直接相鄰。
此條件在程序中用簡(jiǎn)單的掃描算法實(shí)現(xiàn)，故略去其數(shù)學(xué)表述。
b)用修正路徑段上的點(diǎn)(Iy，Jy)須滿足的三個(gè)條件對(duì)每一段經(jīng)過障礙區(qū)域的路徑加以修正就得到基本規(guī)劃路徑。
如圖6所示，對(duì)A點(diǎn)與B點(diǎn)進(jìn)行基本規(guī)劃，圖中AB間的直線連線上經(jīng)過障礙區(qū)域的路徑段都由一段繞過障礙區(qū)域的曲線段代替，所得到的路徑就是基本規(guī)劃路徑。
基本規(guī)劃的作用進(jìn)行基本規(guī)劃是為了找犄角點(diǎn)，從而優(yōu)化當(dāng)前路徑；基本規(guī)劃路徑并不被更新為當(dāng)前路徑。
II 找出被當(dāng)前路徑穿過的每一塊與邊界接觸的障礙區(qū)域的犄角點(diǎn)。
定義犄角點(diǎn)在基本規(guī)劃中，用初始規(guī)劃得到的路徑中位于障礙區(qū)域的路徑段要進(jìn)行修正，修正路徑段上的點(diǎn)中與被修正路徑段的距離最大的點(diǎn)就是犄角點(diǎn)。
找犄角點(diǎn)的具體實(shí)現(xiàn)方法 在每一次基本規(guī)劃之后，計(jì)算修正路徑段上的所有點(diǎn)與被修正路徑的距離，取距離最大的點(diǎn)為犄角點(diǎn)。
計(jì)算修正路徑段上的點(diǎn)與被修正路徑的距離的方法 設(shè)被修正路徑段的起點(diǎn)坐標(biāo)為(Im，Jm)，記為點(diǎn)A；被修正路徑段的終點(diǎn)坐標(biāo)為(In，Jn)，記為點(diǎn)B；修正路徑段中的點(diǎn)的坐標(biāo)為(Iy，Jy)，記為點(diǎn)C。
令 p＝(a+b+c)/2， 根據(jù)海倫公式，以A、B、C三點(diǎn)為頂點(diǎn)的三角形的面積為則由幾何知識(shí)知，點(diǎn)C與A、B之間連線的距離為 h＝s/2c………………………………………………………(8) (8)式即為修正路徑段上的點(diǎn)與被修正路徑的距離的計(jì)算公式。
圖7中的C點(diǎn)、D點(diǎn)與E點(diǎn)就是犄角點(diǎn)。
圖7中C、E兩點(diǎn)是當(dāng)前路徑穿過與邊界接觸的障礙區(qū)域而得到的犄角點(diǎn)；D點(diǎn)是當(dāng)前路徑穿過不與邊界接觸的障礙區(qū)域而得到的犄角點(diǎn)。本步驟中要找的是被當(dāng)前路徑穿過的每一塊與邊界接觸的障礙區(qū)域的犄角點(diǎn)，故要找的是C、E兩點(diǎn)。
III 找出與當(dāng)前路徑的每一段直線段對(duì)應(yīng)的中轉(zhuǎn)點(diǎn) 找出前一步驟中找出的位于當(dāng)前路徑同一側(cè)的犄角點(diǎn)中離當(dāng)前路徑最遠(yuǎn)的點(diǎn)，此點(diǎn)稱為與當(dāng)前路徑對(duì)應(yīng)的中轉(zhuǎn)點(diǎn)；若當(dāng)前路徑為折線，則要找出與當(dāng)前路徑的每一段直線段對(duì)應(yīng)的中轉(zhuǎn)點(diǎn)。
定義中轉(zhuǎn)點(diǎn)若一段直線路徑穿過了若干個(gè)障礙區(qū)域，則會(huì)產(chǎn)生與這段路徑相對(duì)應(yīng)的若干個(gè)犄角點(diǎn)，對(duì)位于這段路徑不同側(cè)的犄角點(diǎn)要分開處理，位于這段路徑同一側(cè)的犄角點(diǎn)中離這段路徑最遠(yuǎn)的點(diǎn)，稱為中轉(zhuǎn)點(diǎn)。
按中轉(zhuǎn)點(diǎn)定義可知，與一段穿過了障礙區(qū)域的路徑對(duì)應(yīng)的中轉(zhuǎn)點(diǎn)可能有一個(gè)或兩個(gè)。
a 以圖8為例具體說(shuō)明何謂中轉(zhuǎn)點(diǎn)，直線AB為當(dāng)前路徑，圖中有C點(diǎn)、D點(diǎn)、E點(diǎn)三個(gè)犄角點(diǎn)，C點(diǎn)與D點(diǎn)位于直線AB的同一側(cè)，且D點(diǎn)比C點(diǎn)離直線AB遠(yuǎn)，故D點(diǎn)是與當(dāng)前路徑對(duì)應(yīng)的中轉(zhuǎn)點(diǎn)；而直線AB的另一側(cè)只有E點(diǎn)一個(gè)犄角點(diǎn)，所以E點(diǎn)也是與當(dāng)前路徑對(duì)應(yīng)的中轉(zhuǎn)點(diǎn)。
b 若當(dāng)前路徑為折線，則折線的每一段直線段由于穿過障礙區(qū)域而產(chǎn)生的犄角點(diǎn)應(yīng)分開處理，也就是說(shuō)，假設(shè)折線由a1、a2兩段直線段組成，則由直線段a1穿過障礙區(qū)域而產(chǎn)生的犄角點(diǎn)不計(jì)算其與直線段a2的距離，由直線段a2穿過障礙區(qū)域而產(chǎn)生的犄角點(diǎn)不計(jì)算其與直線段a1的距離。
如圖9所示，圖中的當(dāng)前路徑ADEB為折線，當(dāng)前路徑中的AD段穿過了與邊界接觸的障礙區(qū)域，只得到犄角點(diǎn)C點(diǎn)，則C點(diǎn)是與AD段對(duì)應(yīng)的中轉(zhuǎn)點(diǎn)，而不是與DE段或EB段對(duì)應(yīng)的中轉(zhuǎn)點(diǎn)。
IV 將起點(diǎn)、終點(diǎn)與前面所有步驟中找出的所有中轉(zhuǎn)點(diǎn)沿當(dāng)前路徑的方向用折線順次連接起來(lái)，將新得到的折線路徑作為當(dāng)前路徑。轉(zhuǎn)到步驟(3)。
本步驟在實(shí)際實(shí)現(xiàn)時(shí)，是將起點(diǎn)、終點(diǎn)與前面所有步驟中找出的所有中轉(zhuǎn)點(diǎn)(設(shè)總數(shù)為N個(gè))沿當(dāng)前路徑的方向依次排列，然后對(duì)這個(gè)排列中所有相鄰的兩點(diǎn)進(jìn)行初始規(guī)劃，則共進(jìn)行(N-1)次初始規(guī)劃，得到(N-1)條初始規(guī)劃路徑，這些路徑首尾相連而形成一條新路徑。更新當(dāng)前路徑，轉(zhuǎn)到步驟(3)。
(5)處理不與邊界接觸的障礙區(qū)域。
首先找出當(dāng)前路徑上每段位于不與邊界接觸的障礙區(qū)域的直線路徑段的起點(diǎn)與終點(diǎn)，然后對(duì)找出的每一對(duì)起點(diǎn)與終點(diǎn)進(jìn)行基本規(guī)劃。再找出被當(dāng)前路徑穿過的每一塊不與邊界接觸的障礙區(qū)域的犄角點(diǎn)。找出與當(dāng)前路徑的每一段直線段對(duì)應(yīng)的中轉(zhuǎn)點(diǎn)。將起點(diǎn)、終點(diǎn)與前面步驟中找出的所有中轉(zhuǎn)點(diǎn)沿當(dāng)前路徑的方向用折線順次連接起來(lái)。將新得到的折線路徑作為當(dāng)前路徑。轉(zhuǎn)到步驟(3)。
步驟(5)各小步的實(shí)現(xiàn)方法與步驟(4)各小步相同，只將步驟(4)各小步實(shí)現(xiàn)方法中的“與邊界接觸的障礙區(qū)域”替換為“不與邊界接觸的障礙區(qū)域”。
(6)輸出當(dāng)前路徑，規(guī)劃結(jié)束。
當(dāng)前路徑經(jīng)過步驟(3)判斷，若沒穿過任何障礙區(qū)域，則找到了最終路徑，中斷步驟(3)，轉(zhuǎn)到步驟(6)，輸出當(dāng)前路徑，規(guī)劃結(jié)束。
由上述步驟可以看出，本發(fā)明全局路徑規(guī)劃方法的實(shí)現(xiàn)很簡(jiǎn)單，基本上只依靠簡(jiǎn)單的計(jì)算就可以完成完整的規(guī)劃計(jì)算過程，規(guī)劃可以在很短的時(shí)間內(nèi)完成，從而滿足移動(dòng)機(jī)器人的規(guī)劃實(shí)時(shí)性要求；對(duì)于障礙物很多的環(huán)境，在步驟(4)和步驟(5)中并不遍歷所有的障礙，而是只對(duì)當(dāng)前路徑經(jīng)過的障礙作處理，更新當(dāng)前路徑后再重新掃描當(dāng)前路徑是否經(jīng)過了障礙，從而極大的提高了程序的執(zhí)行效率；整個(gè)規(guī)劃算法流程簡(jiǎn)單明晰，易于理解，很方便編程實(shí)現(xiàn)。
權(quán)利要求
1.一種基于類三維地圖的移動(dòng)機(jī)器人全局路徑規(guī)劃方法，其特征在于，包括以下步驟
步驟1、將普通柵格地圖改造為基于等高線原理的類三維地步驟2初始規(guī)劃將移動(dòng)機(jī)器人的起點(diǎn)和終點(diǎn)連成一條直線作為當(dāng)前路徑；
步驟3掃描當(dāng)前路徑穿過的柵格，若未掃描到障礙，則轉(zhuǎn)至步驟6；若掃描到障礙，則首先確定障礙的類型，障礙的類型包括與邊界接觸的障礙和不與邊界接觸的障礙；如果是與邊界接觸的障礙，轉(zhuǎn)至步驟4，否則轉(zhuǎn)至步驟5；所述的與邊界接觸的障礙包括只與一側(cè)邊界接觸的障礙以及與兩側(cè)邊界接觸的障礙；
步驟4處理與邊界接觸的障礙，更新當(dāng)前路徑，返回步驟3；
步驟5處理不與邊界接觸的障礙，更新當(dāng)前路徑，返回步驟3；
步驟6輸出當(dāng)前路徑，全局路徑規(guī)劃完成；
2.根據(jù)權(quán)利要求1所述的基于類三維地圖的移動(dòng)機(jī)器人全局路徑規(guī)劃方法，其特征在于，所述的步驟1的具體步驟為
給柵格地圖建立I、J坐標(biāo)系，柵格地圖中的柵格用數(shù)組ZG表示；數(shù)組ZG的元素ZG[I][J]表示高度值；
依據(jù)等高線原理建立類三維地圖，就是給地圖中的每一個(gè)柵格點(diǎn)都賦予高度值，ZG[I][J]＝H，H∈{0、1、2、3……}，H值越大表示高度越高；
柵格地圖分為兩種區(qū)域自由區(qū)域與障礙區(qū)域，自由區(qū)域的高度值H＝0；障礙區(qū)域的高度值H＞0，障礙區(qū)域分為三種不與邊界接觸的障礙區(qū)域，只與一側(cè)邊界接觸的障礙區(qū)域，與兩側(cè)邊界接觸的障礙區(qū)域；只與一側(cè)邊界接觸的障礙區(qū)域以及與兩側(cè)邊界接觸的障礙區(qū)域?qū)?yīng)的障礙統(tǒng)稱為障礙；以下分三種情況說(shuō)明障礙區(qū)域的類三維地圖的創(chuàng)建方法
I、對(duì)于不與邊界接觸的障礙區(qū)域
i 找出該區(qū)域I坐標(biāo)的最大值Imax、I坐標(biāo)的最小值Imin、J坐標(biāo)的最大值Jmax、J坐標(biāo)的最小值Jmin；
ii 給該區(qū)域的所有I＝Imax和I＝Imin的兩列柵格的高度值賦予1，給該區(qū)域的所有I＝(Imax-1)和I＝(Imin+1)的兩列柵格的高度值賦予2，給該區(qū)域的所有I＝(Imax-2)和I＝(Imin+2)的兩列柵格的高度值賦予3，依此類推，直到該區(qū)域所有的柵格都被賦予一個(gè)高度值；
iii 給該區(qū)域的所有J＝Jmax和J＝Jmin的兩行柵格的高度值加上1，給該區(qū)域的所有J＝(Jmax-1)和J＝(Jmin+1)的兩行柵格的高度值加上2，給該區(qū)域的所有J＝(Jmax-2)和J＝(Jmin+2)的兩行柵格的高度值加上3，依此類推，直到該區(qū)域所有的柵格都被加上一個(gè)高度值；
II、對(duì)于只與一側(cè)邊界接觸的障礙區(qū)域
i 出該區(qū)域I坐標(biāo)的最大值Imax、I坐標(biāo)的最小值Imin、J坐標(biāo)的最大值Jmax、J坐標(biāo)的最小值Jmin；
ii 判斷Imax、Imin、Jmax、Jmin四個(gè)值中哪個(gè)為邊界值，即判斷障礙區(qū)域與柵格地圖的上、下、左、右四個(gè)邊界中的哪個(gè)接觸，從與該邊界相對(duì)的一側(cè)開始給障礙區(qū)域賦予高度值；給障礙區(qū)域中離該邊界最遠(yuǎn)的一行或一列柵格賦予高度值1，給離該邊界第二遠(yuǎn)的一行或一列柵格賦予高度值2，給離該邊界第三遠(yuǎn)的一行或一列柵格賦予高度值3，依此類推，直到離該邊界最近的一行或一列柵格也被賦予高度值；
iii 然后從另一個(gè)方向開始給障礙區(qū)域的柵格加上高度值，即不妨設(shè)上一步中是按I坐標(biāo)方向給柵格賦值，則本步驟中按J坐標(biāo)方向給柵格加上高度值；給該區(qū)域的所有J＝Jmax和J＝Jmin的兩行柵格的高度值加上1，給該區(qū)域的所有J＝(Jmax-1)和J＝(Jmin+1)的兩行柵格的高度值加上2，給該區(qū)域的所有J＝(Jmax-2)和J＝(Jmin+2)的兩行柵格的高度值加上3，依此類推，直到該區(qū)域所有的柵格都被加上一個(gè)高度值；
III、對(duì)于與兩側(cè)邊界接觸的障礙區(qū)域
i 找出該區(qū)域I坐標(biāo)的最大值Imax、I坐標(biāo)的最小值Imin、J坐標(biāo)的最大值Jmax、J坐標(biāo)的最小值Jmin；
ii 判斷Imax、Imin、Jmax、Jmin四個(gè)值中哪兩個(gè)為邊界值，即判斷障礙區(qū)域與地圖的上、下、左、右四個(gè)邊界中的哪兩個(gè)邊界接觸，然后從兩個(gè)被接觸的邊界中任選一個(gè)，從與該邊界相對(duì)的一側(cè)開始給障礙區(qū)域賦予高度值；不妨設(shè)選擇行對(duì)應(yīng)的邊界為參照，給離該邊界最遠(yuǎn)的一行柵格賦予高度值1，給離該邊界第二遠(yuǎn)的一行柵格賦予高度值2，給離該邊界第三遠(yuǎn)的一行柵格賦予高度值3，依此類推，直到離該邊界最近的一行柵格也被賦予高度值；
然后從與另一邊界相對(duì)的一側(cè)開始給障礙區(qū)域加上高度值；給離另一邊界最遠(yuǎn)的一列柵格的高度值加上1，給離另一邊界第二遠(yuǎn)的一列柵格的高度值加上2，給離另一邊界第三遠(yuǎn)的一列柵格的高度值加上3，依此類推，直到離另一邊界最近的一列柵格也被加上高度值。
3.根據(jù)權(quán)利要求2所述的基于類三維地圖的移動(dòng)機(jī)器人全局路徑規(guī)劃方法，其特征在于，所述的步驟4中的處理與邊界接觸的障礙的步驟以及所述的步驟5中的處理不與邊界接觸的障礙的步驟為當(dāng)前路徑上每段位于與邊界接觸的障礙區(qū)域的路徑段按照下降方向進(jìn)行基本規(guī)劃；
定義下降方向若路徑穿過了某個(gè)障礙區(qū)域，則以位于障礙區(qū)域內(nèi)的路徑段中高度最大的點(diǎn)為起點(diǎn)，垂直于該路徑段且所經(jīng)過的柵格的高度值降低的方向就是下降方向；
定義基本規(guī)劃若兩點(diǎn)之間的初始規(guī)劃路徑穿過了障礙區(qū)域，則找到每段位于障礙區(qū)域的路徑的下降方向，然后將每段位于障礙區(qū)域的路徑用沿著障礙區(qū)域從下降方向繞過障礙區(qū)域的新路徑段代替，基本規(guī)劃就是對(duì)初始規(guī)劃進(jìn)行修正，基本規(guī)劃后的路徑段稱為修正路徑段；
具體步驟分為以下四步
I、找出當(dāng)前路徑上每段位于與邊界接觸的障礙區(qū)域的路徑段的起點(diǎn)與終點(diǎn)，
修正路徑段上的點(diǎn)(Iy，Jy)需滿足以下三個(gè)條件
第一個(gè)條件位于下降方向一側(cè)，即，滿足下式
式中(Im，Jm)為該段穿過了障礙區(qū)域的路徑段的起點(diǎn)坐標(biāo)，(In，Jn)為該段穿過了障礙區(qū)域的路徑段的終點(diǎn)坐標(biāo)，(Iam，Jam)為該障礙區(qū)域的高度值最大的點(diǎn)的坐標(biāo)；
第二個(gè)條件高度值為0，ZG[Iy][Jy]＝0；
第三個(gè)條件與對(duì)應(yīng)的障礙區(qū)域直接相鄰；
找出被當(dāng)前路徑穿過的每一塊與邊界接觸的障礙區(qū)域的犄角點(diǎn)；
定義犄角點(diǎn)在基本規(guī)劃中，修正路徑段上的點(diǎn)中與被修正路徑段的距離最大的點(diǎn)就是犄角點(diǎn)；
III、找出與當(dāng)前路徑的每一段直線路徑段對(duì)應(yīng)的中轉(zhuǎn)點(diǎn)；
在所有的位于當(dāng)前路徑同一側(cè)的犄角點(diǎn)中離當(dāng)前路徑最遠(yuǎn)的點(diǎn)，稱為與當(dāng)前路徑對(duì)應(yīng)的中轉(zhuǎn)點(diǎn)；若當(dāng)前路徑為折線，則要找出與折線的每一段直線路徑段對(duì)應(yīng)的中轉(zhuǎn)點(diǎn)；
IV、將起點(diǎn)、所有中轉(zhuǎn)點(diǎn)以及終點(diǎn)沿當(dāng)前路徑的方向用折線順次連接起來(lái)，將新得到的折線路徑作為當(dāng)前路徑；轉(zhuǎn)到步驟3。
4.根據(jù)權(quán)利要求1～3任一項(xiàng)所述的基于類三維地圖的移動(dòng)機(jī)器人全局路徑規(guī)劃方法，其特征在于，所述機(jī)器人為輪式移動(dòng)機(jī)器人，其移動(dòng)裝置采用輪式結(jié)構(gòu)，具備沿弧線與直線及折線運(yùn)動(dòng)的能力。
5.根據(jù)權(quán)利要求4所述的基于類三維地圖的移動(dòng)機(jī)器人全局路徑規(guī)劃方法，其特征在于，所述障礙的輪廓為塊狀。
6.根據(jù)權(quán)利要求5所述的基于類三維地圖的移動(dòng)機(jī)器人全局路徑規(guī)劃方法，其特征在于，所述的移動(dòng)機(jī)器人設(shè)有傳感器，能確定機(jī)器人在柵格地圖中的位置，所述的傳感器有GPS裝置、激光測(cè)距儀、紅外測(cè)距儀、或者超聲波測(cè)距儀。
全文摘要
本發(fā)明公開了一種基于類三維地圖的移動(dòng)機(jī)器人全局路徑規(guī)劃方法，步驟1將普通柵格地圖改造為基于等高線原理的類三維地圖；步驟2初始規(guī)劃將移動(dòng)機(jī)器人的起點(diǎn)和終點(diǎn)連成一條直線作為當(dāng)前路徑；步驟3掃描當(dāng)前路徑穿過的柵格，若未掃描到障礙，則轉(zhuǎn)至步驟6；若掃描到障礙，則根據(jù)障礙類型轉(zhuǎn)至步驟4或步驟5；步驟4處理與邊界接觸的障礙，更新當(dāng)前路徑，返回步驟3；步驟5處理不與邊界接觸的障礙，更新當(dāng)前路徑，返回步驟3；步驟6輸出當(dāng)前路徑，全局路徑規(guī)劃完成。本發(fā)明充分發(fā)掘障礙物信息，提高程序執(zhí)行效率、易于實(shí)現(xiàn)，滿足移動(dòng)機(jī)器人全局路徑規(guī)劃的實(shí)時(shí)性要求。
文檔編號(hào)G01C21/32GK101769754SQ20101002208
公開日2010年7月7日 申請(qǐng)日期2010年1月19日 優(yōu)先權(quán)日2010年1月19日
發(fā)明者王耀南, 周良, 朱江, 印峰, 馬波, 聶鑫 申請(qǐng)人:湖南大學(xué)