本發(fā)明涉及機器人領域，具體而言，涉及一種機器人路徑規(guī)劃方法及規(guī)劃路線的機器人。

背景技術：

隨著計算機技術、傳感器技術和人工智能等技術的快速發(fā)展，機器人技術也變得日趨成熟，而其中的移動機器人類型應用最為廣泛，在家用服務、航天、工業(yè)等眾多的行業(yè)中扮演著越來越重要的角色，這些各種各樣的機器人能夠在特定環(huán)境下很好地完成工作。

但是目前的機器人還是存在很多不足之處。因為大多數情況下機器人的工作環(huán)境是未知或者不確定的，但是目前的機器人還不能很好地完成在陌生的環(huán)境中完成指定路線。

技術實現要素：

有鑒于此，本發(fā)明實施例的目的在于提供一種機器人路徑規(guī)劃方法及規(guī)劃路線的機器人。

本發(fā)明實施例提供的一種機器人路徑規(guī)劃方法，該方法包括：根據機器人的位姿和里程計的控制信息獲取所述機器人的初始位姿；

根據所述初始位姿及傳感器觀測到的數據創(chuàng)建先驗地圖；

根據自適應調整的快速擴展樹算法在所述先驗地圖計算出控制所述機器人行走的全局路徑；

獲取機器人的傳感器的實時探測數據，判斷在機器人的行走過程中，傳感器是否探測到所述全局路徑中的障礙物；

當傳感器探測到所述全局路徑中的障礙物時，建立動態(tài)窗口模型規(guī)劃該機器人的局部避障路線，以控制機器人根據該局部避障路線避開障礙物。

本發(fā)明實施例還提供一種規(guī)劃路線的機器人，所述機器人包括：存儲器、處理器及存儲在所述存儲器中，由處理器處理的路線規(guī)劃裝置；

所述路線規(guī)劃裝置包括：

初始位姿獲取模塊，用于根據機器人的位姿和里程計的控制信息獲取所述機器人的初始位姿；

先驗地圖創(chuàng)建模塊，用于根據所述初始位姿及傳感器觀測到的數據創(chuàng)建先驗地圖；

全局路線計算模塊，用于根據自適應調整的快速擴展樹算法在所述先驗地圖計算出控制所述機器人行走的全局路徑；

障礙物探測模塊，用于獲取機器人的傳感器的實時探測數據，判斷在機器人的行走過程中，傳感器是否探測到所述全局路徑中的障礙物；

避障路線規(guī)劃模塊，用于當傳感器探測到所述全局路徑中的障礙物時，建立動態(tài)窗口模型規(guī)劃該機器人的局部避障路線，以控制機器人根據該局部避障路線避開障礙物。

與現有技術相比，本發(fā)明的機器人路徑規(guī)劃方法及規(guī)劃路線的機器人。在較復雜環(huán)境下所述機器人在基于自適應調整的快速擴展樹算法搜索出全局最優(yōu)路徑，從而保證了全局路徑最優(yōu)，同時對于復雜環(huán)境和動態(tài)障礙物環(huán)境結合動態(tài)窗口模型，使所述機器人的方向明確，搜索高效，可以有效提高機器人自主避障能力，安全到達目標點。

為使本發(fā)明的上述目的、特征和優(yōu)點能更明顯易懂，下文特舉較佳實施例，并配合所附附圖，作詳細說明如下。

附圖說明

為了更清楚地說明本發(fā)明實施例的技術方案，下面將對實施例中所需要使用的附圖作簡單地介紹，應當理解，以下附圖僅示出了本發(fā)明的某些實施例，因此不應被看作是對范圍的限定，對于本領域普通技術人員來講，在不付出創(chuàng)造性勞動的前提下，還可以根據這些附圖獲得其他相關的附圖。

圖1為本發(fā)明較佳實施例提供的機器人的方框示意圖。

圖2是本發(fā)明提供的機器人的路線規(guī)劃裝置的功能模塊示意圖。

圖3本發(fā)明較佳實施例提供的機器人路徑規(guī)劃方法的流程圖。

圖4是圖3中的步驟S105的詳細流程圖。

圖5為傳感器獲取周圍環(huán)境信息并建立局部環(huán)境模型示意圖。

具體實施方式

下面將結合本發(fā)明實施例中附圖，對本發(fā)明實施例中的技術方案進行清楚、完整地描述，顯然，所描述的實施例僅僅是本發(fā)明一部分實施例，而不是全部的實施例。通常在此處附圖中描述和示出的本發(fā)明實施例的組件可以以各種不同的配置來布置和設計。因此，以下對在附圖中提供的本發(fā)明的實施例的詳細描述并非旨在限制要求保護的本發(fā)明的范圍，而是僅僅表示本發(fā)明的選定實施例?；诒景l(fā)明的實施例，本領域技術人員在沒有做出創(chuàng)造性勞動的前提下所獲得的所有其他實施例，都屬于本發(fā)明保護的范圍。

應注意到：相似的標號和字母在下面的附圖中表示類似項，因此，一旦某一項在一個附圖中被定義，則在隨后的附圖中不需要對其進行進一步定義和解釋。同時，在本發(fā)明的描述中，術語“第一”、“第二”等僅用于區(qū)分描述，而不能理解為指示或暗示相對重要性。

如圖1所示，是機器人100的方框示意圖。所述機器人100包括路線規(guī)劃裝置200、存儲器102、存儲控制器104、處理器106、外設接口108、定位模塊110及傳感器112。

所述存儲器102、存儲控制器104、處理器106、外設接口108、定位模塊110及傳感器112各元件相互之間直接或間接地電性連接，以實現數據的傳輸或交互。例如，這些元件相互之間可通過一條或多條通訊總線或信號線實現電性連接。所述路線規(guī)劃裝置200包括至少一個可以軟件或固件(firmware)的形式存儲于所述存儲器中或固化在所述機器人100的操作系統(tǒng)(operating system，OS)中的軟件功能模塊。所述處理器106用于執(zhí)行存儲器中存儲的可執(zhí)行模塊，例如所述路線規(guī)劃裝置200包括的軟件功能模塊或計算機程序。

其中，存儲器102可以是，但不限于，隨機存取存儲器(Random Access Memory，RAM)，只讀存儲器(Read Only Memory，ROM)，可編程只讀存儲器(Programmable Read-Only Memory，PROM)，可擦除只讀存儲器(Erasable Programmable Read-Only Memory，EPROM)，電可擦除只讀存儲器(Electric Erasable Programmable Read-Only Memory，EEPROM)等。其中，存儲器102用于存儲程序，所述處理器106在接收到執(zhí)行指令后，執(zhí)行所述程序，本發(fā)明實施例任一實施例揭示的流過程定義的機器人100所執(zhí)行的方法可以應用于處理器106中，或者由處理器106實現。

處理器106可能是一種集成電路芯片，具有信號的處理能力。上述的處理器106可以是通用處理器，包括中央處理器(Central Processing Unit，簡稱CPU)、網絡處理器(Network Processor，簡稱NP)等；還可以是數字信號處理器(DSP)、專用集成電路(ASIC)、現場編程門陣列(FPGA)或者其他可編程邏輯器件、分立門或者晶體管邏輯器件、分立硬件組件?？梢詫崿F或者執(zhí)行本發(fā)明實施例中的公開的各方法、步驟及邏輯框圖。通用處理器可以是微處理器或者該處理器也可以是任何常規(guī)的處理器等。

所述外設接口108將各種輸入/輸入裝置耦合至處理器以及存儲器102。在一些實施例中，外設接口108，處理器106以及存儲控制器104可以在單個芯片中實現。在其他一些實例中，他們可以分別由獨立的芯片實現。

所述定位模塊110用于接收GPS衛(wèi)星播報的定位信號，并根據定位信號計算出自身的位置。上述位置例如可用經度、緯度以及海拔高度來表示。可以理解，實現定位的方式并不限于GPS系統(tǒng)。例如，其他可用的衛(wèi)星定們系統(tǒng)還包括北斗衛(wèi)星定位系統(tǒng)(Compass Navigation Satellite System，CNSS)或者格洛納斯系統(tǒng)(Global Navigation Satellite System，GLONASS)。此外，定位也并不限于采用衛(wèi)星定位技術，例如，還可采用無線定位技術，例如基于無線基站的定位技術或者無線熱點的定位技術。此時，定位模塊110可被替換成相應的模塊，或者直接經由處理器106執(zhí)行特定的定位程序來實現。

所述傳感器112是一種檢測裝置，能感受到被測量的信息，并能將感受到的信息，按一定規(guī)律變換成為電信號或其他所需形式的信息輸出，以滿足信息的傳輸、處理、存儲、顯示、記錄和控制等要求。

如圖2所示，圖2是本發(fā)明提供的機器人100的路線規(guī)劃裝置200的功能模塊示意圖。所述路線規(guī)劃裝置200包括：初始位姿獲取模塊210、先驗地圖創(chuàng)建模塊220、全局路線計算模塊230、障礙物探測模塊240及避障路線規(guī)劃模塊250。其中，所述避障路線規(guī)劃模塊250包括：環(huán)境模型建立單元251、局部路徑計算單元252及環(huán)境模型更新單元253。本實施例中的所述路線規(guī)劃裝置200中的各個模塊或單元用于執(zhí)行機器人100路徑規(guī)劃方法中的各個步驟，下面將結合圖3及機器人100路徑規(guī)劃方法實施例詳細描述各個模塊及單元的功能。

請參閱圖3，是本發(fā)明較佳實施例提供的應用于圖1所示的機器人100的機器人100路徑規(guī)劃方法的流程圖。下面將對圖3所示的具體流程進行詳細闡述。

步驟S101，根據機器人100的位姿和里程計的控制信息獲取所述機器人100的初始位姿。步驟S101可由上述的初始位姿獲取模塊210執(zhí)行。

步驟S102，根據所述初始位姿及傳感器觀測到的數據創(chuàng)建先驗地圖。步驟S102可由上述的先驗地圖創(chuàng)建模塊220執(zhí)行。

步驟S103，根據自適應調整的快速擴展樹算法在所述先驗地圖計算出控制所述機器人100行走的全局路徑。步驟S103可由上述的全局路線計算模塊230執(zhí)行。

快速擴展樹(Rapidly-exploring Random Tree，RRT)算法為通過引入一個目標引力函數，促使擴散樹隨機朝目標點方向生長的算法。

全局路線計算模塊230可通過所述快速擴展樹算法構建全局路徑對應的路徑生長樹T，所述路徑生長樹T中含有n個節(jié)點，T＝{x_i},i＝1,2,...,n，選取x_i為生長點，其中定義ε為探索步長，f_i為節(jié)點x_i探索失敗的次數，即如果節(jié)點探索成功，記f_i為1；如果節(jié)點探索失敗一次，則f_i＝f_i+1，

定義δ_j為節(jié)點x_j的影響因子為：

其中，r_ij＝x_j-x_i表示當前節(jié)點x_i與其余節(jié)點x_j(x_j∈T)的距離，

w_j＝δ_j/D_j表示節(jié)點x_j的權值，

D_j＝x_j-x_rand表示節(jié)點x_j與隨機節(jié)點x_rand間的距離；

其中，權值w_j的大小與影響因子δ_j成正比，與其余節(jié)點x_j與隨機節(jié)點x_rand間的距離D_j成反比，所以為使搜索樹朝最有利方向進行，根據權值大小選取生長點。

在規(guī)定步長范圍下以所述機器人100最大轉角θ_max為限制條件計算探索點公式如下：

其中，表示未知區(qū)域隨機產生的隨機目標點；

表示所述機器人100當前位置；

θ_k表示所述當前位置到的運動方向改變量；

θ_max表示步長范圍內機器人100最大轉角；

由計算探索點公式可知：當機器人100當前位置到的距離小于步長ε時，如果運動方向改變量θ_k小于等于最大轉角時θ_max，探索點即可直接設置為如果航向改變量θ_k大于最大轉角時θ_max時，探索點即以最大轉角限制探索點范圍。如果當前位置到的距離大于步長ε時，且航向改變量θ_k大于最大轉角時，即此時以最大轉角限制探索點范圍，即

以當前探索點距目標的距離為啟發(fā)信息，計算每個探索點到目標點的直線距離J_k：選擇距離目標點最近的探索點為最優(yōu)節(jié)點加入路徑生長樹。

首先對標準快速擴展樹算法流程生長階段和探索過程進行改進。在生長階段，選取生長點時引入衡量節(jié)點探索失敗次數的影響因子，影響因子的大小由于考慮了隨機方向點和樹節(jié)點之間的距離和探索失敗次數，故可實現自適應調整生長階段節(jié)點的生長權值，從而保證搜索樹朝著最有利的方向生長。與此同時，在探索過程中，結合機器人100自身運動約束，以機器人100自身最大轉角限制探索方向，保證規(guī)劃路徑實際安全性，最后以當前節(jié)點到目標點的距離作為啟發(fā)因子，使得搜索出的路徑分支節(jié)點少、路徑更平滑。

步驟S104，獲取機器人100的傳感器的實時探測數據，判斷在機器人100的行走過程中，傳感器是否探測到所述全局路徑中的障礙物。步驟S104由所述障礙物探測模塊240執(zhí)行。

步驟S105，當傳感器探測到所述全局路徑中的障礙物時，建立動態(tài)窗口模型規(guī)劃該機器人100的局部避障路線，以控制機器人100根據該局部避障路線避開障礙物。步驟S105由所述避障路線規(guī)劃模塊250執(zhí)行。

如圖4所示，所述步驟S105具體包括以下步驟S1051、S1052以及S1053。

步驟S1051，通過所述傳感器探測環(huán)境信息，建立感知環(huán)境模型。步驟S1051由上述的環(huán)境模型建立單元251執(zhí)行。

所述環(huán)境信息可由所述傳感器探測得到，并且每一個障礙物300到所述機器人100相對距離可由所述傳感器計算得到，圖5為傳感器獲取周圍環(huán)境信息并建立局部環(huán)境模型示意圖。在圖5的環(huán)境模型示意圖中，假設原點處為機器人100的位置坐標(x_R,y_R)，機器人100前進方向設為θ_R，r_R為機器人100自身大小半徑。假設激光傳感器掃描范圍為180°，頻率為1Hz，每秒掃描間隔為1°，故可得到181個掃描點。

將所述傳感器掃描的掃描點以逆時針標記，從x軸正向開始，分別為l₁,l₂,…,l_i,…,l₁₈₁，其中掃描點l_i用極坐標形式表示，即以掃描角和對應的測量距離d_i來表示，所述掃描點l_i的公式為：

其中，i＝1,2,…,181，傳感器測量的最大距離為d_max，當d_i<d_max時，即被判定為一個障礙物，為l_i相對于x軸的角度，當時，l_i與機器人100同向；

基于上述對激光傳感器環(huán)境建模分析知，利用傳感器探測到的局部環(huán)境信息，設置一個半徑大小為r_min，可根據環(huán)境信息動態(tài)調整(r_min≤r_win≤d_max，r_min大于安全半徑r_saf)的動態(tài)自適應規(guī)劃窗口，因此可在半徑為r_win的動態(tài)半圓窗口中將t時刻測距范圍內探測到的局部環(huán)境信息中表示出來，其中窗口大小為win(r_win(t))＝{l₁,l₂,…,l_i,…l₁₈₁}，其中掃描點l_i被重新定義為：

其中，ρ表示重新定義后的測量距離。

步驟S1052，根據所述全局路徑中的目標點位置及預設的局部路徑算法計算得到局部優(yōu)化路徑。步驟S1052由上述的局部路徑計算單元252執(zhí)行。

局部路徑計算單元252可根據所述機器人100可實現的速度集及探測到的障礙物300計算出所述機器人100的局部優(yōu)化速度。

其中，所述機器人100的運動軌跡的每一步都包含一個速度向量(v,ω)，v表示所述機器人100平移速度，ω表示機器人100旋轉速度，探測出的局部窗口中的環(huán)境障礙物300將限制平移速度和旋轉速度。

所述機器人100最大的許可速度由機器人100到下一障礙物的距離dist(v,ω)可以確定，所述許可速度為所述機器人100以該許可速度運行能夠及時停止防止碰撞的速度；其中，和表示剎車用加速度和角加速度，許可速度集V_a定義如下：

以t表示時間間隔，和表示平移加速度和角加速度，(v_a,ω_a)表示真實速度，局部滾動優(yōu)化速度集V_d可定義為：

步驟S1053，根據所述局部優(yōu)化路徑更新所述感知環(huán)境模型。步驟S1053由上述的環(huán)境模型更新單元253執(zhí)行。

確定所述局部滾動優(yōu)化速度集V_d后，環(huán)境模型更新單元253可從中選取一個速度，通過目標函數進行評價，實時進行反饋校正；所述目標函數定義如下：

G(v,ω)＝σ(α·heading(v,ω)+β·dist(v,ω)+γ·velocity(v,ω))；

其中，所述heading(v,ω)為用于測量機器人100的運動方向與目標點的一致性的變量，其中所述heading(v,ω)的值為1-θ/π，θ為目標點與機器人100運動方向的夾角，所述θ通過所述機器人100的預測位置進行計算，預測位置為所述機器人100以當前速度繼續(xù)運行一個時間間隔，并以最大減速度停下的位置；

所述dist(v,ω)表示圓弧到最近障礙物300的距離，如果沒有障礙物300在圓弧上，則設為一個較大的常數；

所述velocity(v,ω)＝v/v_max表示相應軌跡上機器人100的前進程度。

所述α、β和γ用來對所述heading(v,ω)、dist(v,ω)及velocity(v,ω)＝v/v_max進行加權。其中可以通過改變所述α、β和γ的值實現修正機器人100的運動軌跡的修正。

根據本實施例中的方法，在較復雜環(huán)境下所述機器人100在基于自適應調整的快速擴展樹算法搜索出全局最優(yōu)路徑，從而保證了全局路徑最優(yōu)，同時對于復雜環(huán)境和動態(tài)障礙物環(huán)境結合動態(tài)窗口模型，使所述機器人100的方向明確，搜索高效，可以有效提高機器人100自主避障能力，安全到達目標點。

在本申請所提供的幾個實施例中，應該理解到，所揭露的裝置和方法，也可以通過其它的方式實現。以上所描述的裝置實施例僅僅是示意性的，例如，附圖中的流程圖和框圖顯示了根據本發(fā)明的多個實施例的裝置、方法和計算機程序產品的可能實現的體系架構、功能和操作。在這點上，流程圖或框圖中的每個方框可以代表一個模塊、程序段或代碼的一部分，所述模塊、程序段或代碼的一部分包含一個或多個用于實現規(guī)定的邏輯功能的可執(zhí)行指令。也應當注意，在有些作為替換的實現方式中，方框中所標注的功能也可以以不同于附圖中所標注的順序發(fā)生。例如，兩個連續(xù)的方框實際上可以基本并行地執(zhí)行，它們有時也可以按相反的順序執(zhí)行，這依所涉及的功能而定。也要注意的是，框圖和/或流程圖中的每個方框、以及框圖和/或流程圖中的方框的組合，可以用執(zhí)行規(guī)定的功能或動作的專用的基于硬件的系統(tǒng)來實現，或者可以用專用硬件與計算機指令的組合來實現。

另外，在本發(fā)明各個實施例中的各功能模塊可以集成在一起形成一個獨立的部分，也可以是各個模塊單獨存在，也可以兩個或兩個以上模塊集成形成一個獨立的部分。

所述功能如果以軟件功能模塊的形式實現并作為獨立的產品銷售或使用時，可以存儲在一個計算機可讀取存儲介質中?；谶@樣的理解，本發(fā)明的技術方案本質上或者說對現有技術做出貢獻的部分或者該技術方案的部分可以以軟件產品的形式體現出來，該計算機軟件產品存儲在一個存儲介質中，包括若干指令用以使得一臺計算機設備(可以是個人計算機，服務器，或者網絡設備等)執(zhí)行本發(fā)明各個實施例所述方法的全部或部分步驟。而前述的存儲介質包括：U盤、移動硬盤、只讀存儲器(ROM，Read-Only Memory)、隨機存取存儲器(RAM，Random Access Memory)、磁碟或者光盤等各種可以存儲程序代碼的介質。需要說明的是，在本文中，諸如第一和第二等之類的關系術語僅僅用來將一個實體或者操作與另一個實體或操作區(qū)分開來，而不一定要求或者暗示這些實體或操作之間存在任何這種實際的關系或者順序。而且，術語“包括”、“包含”或者其任何其他變體意在涵蓋非排他性的包含，從而使得包括一系列要素的過程、方法、物品或者設備不僅包括那些要素，而且還包括沒有明確列出的其他要素，或者是還包括為這種過程、方法、物品或者設備所固有的要素。在沒有更多限制的情況下，由語句“包括一個……”限定的要素，并不排除在包括所述要素的過程、方法、物品或者設備中還存在另外的相同要素。

以上所述僅為本發(fā)明的優(yōu)選實施例而已，并不用于限制本發(fā)明，對于本領域的技術人員來說，本發(fā)明可以有各種更改和變化。凡在本發(fā)明的精神和原則之內，所作的任何修改、等同替換、改進等，均應包含在本發(fā)明的保護范圍之內。應注意到：相似的標號和字母在下面的附圖中表示類似項，因此，一旦某一項在一個附圖中被定義，則在隨后的附圖中不需要對其進行進一步定義和解釋。

以上所述，僅為本發(fā)明的具體實施方式，但本發(fā)明的保護范圍并不局限于此，任何熟悉本技術領域的技術人員在本發(fā)明揭露的技術范圍內，可輕易想到變化或替換，都應涵蓋在本發(fā)明的保護范圍之內。因此，本發(fā)明的保護范圍應所述以權利要求的保護范圍為準。