室內(nèi)機器人導(dǎo)航系統(tǒng)及方法
【專利摘要】本發(fā)明提供一種室內(nèi)機器人導(dǎo)航系統(tǒng)及方法，其中系統(tǒng)包括：人機交互裝置用于接收用戶輸入的終點位置坐標(biāo)及終點位置姿態(tài)信息；數(shù)據(jù)采集裝置用于獲取當(dāng)前位置坐標(biāo)和當(dāng)前位置姿態(tài)信息；數(shù)據(jù)分析裝置用于采用第一算法生成全局參考路徑；數(shù)據(jù)采集裝置還用于實時更新局部區(qū)域地圖，并通過坐標(biāo)轉(zhuǎn)換獲取動態(tài)障礙物在二維全局靜態(tài)地圖中對應(yīng)的第二坐標(biāo)；數(shù)據(jù)分析裝置還用于采用第二算法對機器人在下一刻的速度向量進行最優(yōu)規(guī)劃，以獲得最優(yōu)速度向量，用于供機器人根據(jù)所述最優(yōu)速度向量進行移動。本發(fā)明提供一種室內(nèi)機器人導(dǎo)航系統(tǒng)及方法，實用性更高。
【專利說明】
室內(nèi)機器人導(dǎo)航系統(tǒng)及方法
技術(shù)領(lǐng)域
[0001] 本發(fā)明設(shè)及人工智能及自動化領(lǐng)域內(nèi)的一種機器人導(dǎo)航系統(tǒng)及方法，尤其設(shè)及一 種室內(nèi)機器人導(dǎo)航系統(tǒng)及方法。
【背景技術(shù)】
[0002] 導(dǎo)航技術(shù)是引導(dǎo)某一設(shè)備從指定航線的當(dāng)前起始點運動到終點的方法。導(dǎo)航技術(shù) 無論是在交通運輸，還是家庭服務(wù)、軍事領(lǐng)域等方面都擁有廣闊的應(yīng)用前景，也是當(dāng)下研究 的熱口技術(shù)之一。在交通運輸上，導(dǎo)航技術(shù)可W用來實現(xiàn)無人駕駛汽車的自主駕駛、規(guī)避障 礙物行為，也可完成大型倉庫中的物流智能調(diào)度等;在家庭服務(wù)方面，可W幫助類似掃地機 器人的小型機器人完成室內(nèi)的定點移動;在軍事領(lǐng)域，導(dǎo)航技術(shù)可W運用在導(dǎo)彈的定點打 擊、無人機、無人車的定點巡邏偵查規(guī)避威脅區(qū)域的場景。
[0003] 目前，機器人導(dǎo)航系統(tǒng)主要采用GPS、路標(biāo)、機器人底部的碼盤、光學(xué)運動捕捉系統(tǒng) 等的協(xié)同工作來提供機器人的位置姿態(tài)信息，W及，采用激光雷達(dá)和超聲波的協(xié)同工作來 提供障礙物的深度信息，從而實現(xiàn)對機器人進行導(dǎo)航。但是，現(xiàn)有的機器人導(dǎo)航系統(tǒng)存在的 缺點是，機器人導(dǎo)航系統(tǒng)的靜態(tài)路徑規(guī)劃算法完全依賴于靜態(tài)全局地圖進行路徑規(guī)劃，沒 有將動態(tài)障礙物加 W考慮，實用性不高。

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

[0004] 本發(fā)明要解決的技術(shù)問題是提供一種室內(nèi)機器人導(dǎo)航系統(tǒng)及方法，采用全局靜態(tài) 規(guī)劃與局部動態(tài)避障規(guī)劃相結(jié)合的路徑規(guī)劃算法對機器人的路徑進行實時規(guī)劃，使得機器 人可W根據(jù)局部近距離動態(tài)障礙物的更新做出路徑的調(diào)整，實用性更高。
[0005] 為解決上述技術(shù)問題，本發(fā)明的技術(shù)方案是，
[0006] -方面，本發(fā)明提供一種室內(nèi)機器人導(dǎo)航系統(tǒng)，應(yīng)用于機器人，包括:人機交互裝 置：用于接收用戶輸入的機器人的終點位置坐標(biāo)及機器人的終點位置姿態(tài)信息;數(shù)據(jù)采集 裝置:用于獲取機器人的當(dāng)前位置坐標(biāo)和機器人的當(dāng)前位置姿態(tài)信息;數(shù)據(jù)分析裝置，用于 根據(jù)終點位置坐標(biāo)，終點位置姿態(tài)信息，當(dāng)前位置坐標(biāo)，當(dāng)前位置姿態(tài)信息W及預(yù)先獲取的 二維全局靜態(tài)地圖，采用第一算法生成一條位于當(dāng)前點與終點之間的代價最小的全局參考 路徑;數(shù)據(jù)采集裝置，還用于實時更新距離機器人固定半徑內(nèi)的局部區(qū)域地圖，獲取動態(tài)障 礙物在局部區(qū)域地圖中的第一坐標(biāo)，并根據(jù)第一坐標(biāo)進行坐標(biāo)轉(zhuǎn)換W獲取動態(tài)障礙物在二 維全局靜態(tài)地圖中對應(yīng)的第二坐標(biāo);數(shù)據(jù)分析裝置，還用于根據(jù)第二坐標(biāo)，終點位置坐標(biāo)， 當(dāng)前位置坐標(biāo)，當(dāng)前位置姿態(tài)信息，終點位置姿態(tài)信息和全局參考路徑，采用第二算法對機 器人在下一刻的速度向量進行最優(yōu)規(guī)劃，W獲得最優(yōu)速度向量，用于供機器人根據(jù)最優(yōu)速 度向量進行移動。
[0007] 進一步地，人機交互裝置包括一個可視化的交互界面。
[000引進一步地，數(shù)據(jù)采集裝置包括= RGB-D傳感器、超聲波測距儀、慣性測量傳感器。 [0009] 進一步地，第一算法為A*算法。
[0010] 進一步地，第二算法為DWA算法。
[0011] 另一方面，本發(fā)明提供一種室內(nèi)機器人導(dǎo)航方法，應(yīng)用于機器人，包括:人機交互 裝置接收用戶輸入的機器人的終點位置坐標(biāo)及機器人的終點位置姿態(tài)信息;數(shù)據(jù)采集裝置 獲取機器人的當(dāng)前位置坐標(biāo)和機器人的當(dāng)前位置姿態(tài)信息;數(shù)據(jù)分析裝置根據(jù)終點位置坐 標(biāo)，終點位置姿態(tài)信息，當(dāng)前位置坐標(biāo)，當(dāng)前位置姿態(tài)信息W及預(yù)先獲取的二維全局靜態(tài)地 圖，采用第一算法生成一條位于當(dāng)前點與終點之間的代價最小的全局參考路徑;數(shù)據(jù)采集 裝置實時更新距離機器人固定半徑內(nèi)的局部區(qū)域地圖，獲取動態(tài)障礙物在局部區(qū)域地圖中 的第一坐標(biāo)，并根據(jù)第一坐標(biāo)進行坐標(biāo)轉(zhuǎn)換W獲取動態(tài)障礙物在二維全局靜態(tài)地圖中對應(yīng) 的第二坐標(biāo)；當(dāng)機器人沒有被障礙物包圍時，為正常導(dǎo)航狀態(tài)，數(shù)據(jù)分析裝置根據(jù)第二坐 標(biāo)，終點位置坐標(biāo)，當(dāng)前位置坐標(biāo)，當(dāng)前位置姿態(tài)信息，終點位置姿態(tài)信息和全局參考路徑， 采用第二算法對機器人在下一刻的速度向量進行最優(yōu)規(guī)劃，W獲得最優(yōu)速度向量，用于供 機器人根據(jù)最優(yōu)速度向量進行移動。
[0012] 進一步地，當(dāng)機器人被障礙物包圍而無法進行導(dǎo)航時，還包括:數(shù)據(jù)分析裝置發(fā)出 一個預(yù)設(shè)的速度向量給底盤W進行原地旋轉(zhuǎn);數(shù)據(jù)采集裝置持續(xù)掃描并更新局部區(qū)域地 圖，直到局部區(qū)域地圖中的障礙物的分布允許機器人通過，返回正常導(dǎo)航狀態(tài)，其中，障礙 物包括動態(tài)障礙物和靜態(tài)障礙物。
[0013] 進一步地，二維全局靜態(tài)地圖的預(yù)先獲取步驟包括:數(shù)據(jù)采集裝置圍繞當(dāng)前環(huán)境 進行一次全局掃描W獲得若干帖=維點云，并采用=維SLAM算法將若干帖=維點云拼接成 為=維全局靜態(tài)地圖；數(shù)據(jù)分析裝置對=維全局靜態(tài)地圖進行降維度處理，W獲得二維全 局靜態(tài)地圖。
[0014] 進一步地，第一算法為A*算法。
[0015] 進一步地，第二算法為DWA算法。
[0016] 本發(fā)明提供的室內(nèi)機器人導(dǎo)航系統(tǒng)及方法，使用人機交互裝置接收用戶輸入的導(dǎo) 航的終點位置坐標(biāo)和終點位置姿態(tài)信息;數(shù)據(jù)采集裝置獲取機器人的當(dāng)前位置坐標(biāo)和當(dāng)前 位置姿態(tài)信息;之后，數(shù)據(jù)分析裝置，再根據(jù)終點位置坐標(biāo)，終點位置姿態(tài)信息，當(dāng)前位置坐 標(biāo)，位置姿態(tài)信息W及預(yù)先獲取的二維全局靜態(tài)地圖，采用第一算法生成一條位于當(dāng)前點 與終點之間的代價最小的全局參考路徑;在獲得全局參考路徑的同時，數(shù)據(jù)采集裝置實時 更新距離機器人固定半徑內(nèi)的局部區(qū)域地圖，并獲取動態(tài)障礙物在局部區(qū)域地圖中的第一 坐標(biāo)，并通過坐標(biāo)轉(zhuǎn)換將得到的第一坐標(biāo)轉(zhuǎn)換到二維全局靜態(tài)地圖坐標(biāo)系中W獲取第二坐 標(biāo)；當(dāng)機器人沒有被障礙物包圍時，為正常導(dǎo)航狀態(tài)，數(shù)據(jù)分析裝置再根據(jù)第二坐標(biāo)，終點 位置坐標(biāo)，當(dāng)前位置坐標(biāo)，當(dāng)前位置姿態(tài)信息，終點位置姿態(tài)信息和全局參考路徑，采用第 二算法對機器人在下一刻的速度向量進行最優(yōu)規(guī)劃，W獲得最優(yōu)速度向量，W使得底盤根 據(jù)最優(yōu)速度向量進行動作，從而使得機器人向終點位置進行移動。此外，需要說明的是，本 發(fā)明的室內(nèi)機器人導(dǎo)航系統(tǒng)及方法是通過控制底盤的運動來實現(xiàn)對機器人的運動進行導(dǎo) 航。
[0017] 本發(fā)明提供的室內(nèi)機器人導(dǎo)航系統(tǒng)及方法，先采用第一算法進行全局靜態(tài)規(guī)劃， 規(guī)劃出一條位于當(dāng)前點與終點之間的代價最小的全局參考路徑，之后再采用第二算法進行 局部動態(tài)避障規(guī)劃，根據(jù)局部近距離動態(tài)障礙物對路徑進行實時調(diào)整。本發(fā)明提供的室內(nèi) 機器人導(dǎo)航系統(tǒng)及方法，采用全局靜態(tài)規(guī)劃與局部動態(tài)避障規(guī)劃相結(jié)合的路徑規(guī)劃算法對 機器人的路徑進行實時規(guī)劃，使得機器人可W根據(jù)局部近距離動態(tài)障礙物的更新做出路徑 的調(diào)整，實用性高。
【附圖說明】
[0018] 圖1是本發(fā)明實施例提供的一種室內(nèi)機器人導(dǎo)航系統(tǒng)的框圖；
[0019] 圖2是本發(fā)明實施例提供的一種室內(nèi)機器人導(dǎo)航方法的流程圖。
【具體實施方式】
[0020] 下面通過具體的實施例進一步說明本發(fā)明，但是，應(yīng)當(dāng)理解為，運些實施例僅僅是 用于更詳細(xì)具體地說明之用，而不應(yīng)理解為用于W任何形式限制本發(fā)明。
[0021 ]實施例一
[0022] 結(jié)合圖1，本實施例提供一種室內(nèi)機器人導(dǎo)航系統(tǒng)100,應(yīng)用于機器人，包括；
[0023] 人機交互裝置1:用于接收用戶輸入的機器人的終點位置坐標(biāo)及機器人的終點位 置姿態(tài)信息；
[0024] 數(shù)據(jù)采集裝置2:用于獲取機器人的當(dāng)前位置坐標(biāo)和機器人的當(dāng)前位置姿態(tài)信息；
[0025] 數(shù)據(jù)分析裝置3,用于根據(jù)終點位置坐標(biāo)，終點位置姿態(tài)信息，當(dāng)前位置坐標(biāo)，當(dāng)前 位置姿態(tài)信息W及預(yù)先獲取的二維全局靜態(tài)地圖，采用第一算法生成一條位于當(dāng)前點與終 點之間的代價最小的全局參考路徑；
[0026] 數(shù)據(jù)采集裝置2,還用于實時更新距離機器人固定半徑內(nèi)的局部區(qū)域地圖，獲取動 態(tài)障礙物在局部區(qū)域地圖中的第一坐標(biāo)，并根據(jù)第一坐標(biāo)進行坐標(biāo)轉(zhuǎn)換W獲取動態(tài)障礙物 在二維全局靜態(tài)地圖中對應(yīng)的第二坐標(biāo)；
[0027] 數(shù)據(jù)分析裝置3，還用于根據(jù)第二坐標(biāo)，終點位置坐標(biāo)，當(dāng)前位置坐標(biāo)，當(dāng)前位置姿 態(tài)信息，終點位置姿態(tài)信息和全局參考路徑，采用第二算法對機器人在下一刻的速度向量 進行最優(yōu)規(guī)劃，W獲得最優(yōu)速度向量，用于供底盤根據(jù)所述最優(yōu)速度向量進行移動。
[0028] 本發(fā)明實施例提供的室內(nèi)機器人導(dǎo)航系統(tǒng)100,使用人機交互裝置1接收用戶輸入 的導(dǎo)航的終點位置坐標(biāo)和終點位置姿態(tài)信息;數(shù)據(jù)采集裝置2獲取機器人的當(dāng)前位置坐標(biāo) 和當(dāng)前位置姿態(tài)信息；之后，數(shù)據(jù)分析裝置3，再根據(jù)終點位置坐標(biāo)，終點位置姿態(tài)信息，當(dāng) 前位置坐標(biāo)，位置姿態(tài)信息W及預(yù)先獲取的二維全局靜態(tài)地圖，采用第一算法生成一條位 于當(dāng)前點與終點之間的代價最小的全局參考路徑;在獲得全局參考路徑的同時，數(shù)據(jù)采集 裝置2實時更新距離機器人固定半徑內(nèi)的局部區(qū)域地圖，并獲取動態(tài)障礙物在局部區(qū)域地 圖中的第一坐標(biāo)，并通過坐標(biāo)轉(zhuǎn)換將得到的第一坐標(biāo)轉(zhuǎn)換到二維全局靜態(tài)地圖坐標(biāo)系中W 獲取第二坐標(biāo);數(shù)據(jù)分析裝置3再根據(jù)第二坐標(biāo)，終點位置坐標(biāo)，當(dāng)前位置坐標(biāo)，當(dāng)前位置姿 態(tài)信息，終點位置姿態(tài)信息和全局參考路徑，采用第二算法對機器人在下一刻的速度向量 進行最優(yōu)規(guī)劃，W獲得最優(yōu)速度向量，W使得底盤根據(jù)最優(yōu)速度向量進行動作，從而使得機 器人向終點位置進行移動。此外，需要說明的是，本發(fā)明實施例的室內(nèi)機器人導(dǎo)航系統(tǒng)100 是通過控制底盤的運動來實現(xiàn)對機器人的運動進行導(dǎo)航。
[0029] 本發(fā)明實施例提供的室內(nèi)機器人導(dǎo)航系統(tǒng)100,先采用第一算法進行全局靜態(tài)規(guī) 劃，規(guī)劃出一條位于當(dāng)前點與終點之間的代價最小的全局參考路徑，之后再采用第二算法 進行局部動態(tài)避障規(guī)劃，根據(jù)局部近距離動態(tài)障礙物對路徑進行實時調(diào)整。本發(fā)明實施例 提供的室內(nèi)機器人導(dǎo)航系統(tǒng)100,采用全局靜態(tài)規(guī)劃與局部動態(tài)避障規(guī)劃相結(jié)合的路徑規(guī) 劃算法對機器人的路徑進行實時規(guī)劃，使得機器人可W根據(jù)局部近距離動態(tài)障礙物的更新 做出路徑的調(diào)整，實用性高。
[0030] 優(yōu)選地，人機交互裝置1包括一個可視化的交互界面?？蒞讓用戶自行設(shè)置導(dǎo)航終 點，并且將導(dǎo)航過程的地圖信息、動態(tài)障礙物信息、規(guī)劃出的全局參考路徑、機器人的位置 姿態(tài)信息等實時顯示在界面上，便于用戶與機器人導(dǎo)航系統(tǒng)的交互。
[0031] 進一步優(yōu)選地，數(shù)據(jù)采集裝置2包括:RGB-D傳感器、超聲波測距儀、慣性測量傳感 器。本發(fā)明實施例中，數(shù)據(jù)采集裝置2為S維環(huán)境掃描設(shè)備，RGB-D傳感器和超聲波測距儀用 于采集室內(nèi)環(huán)境顏色深度信息，慣性測量傳感器用于采集機器人的位置姿態(tài)信息。
[0032] 此外，需要說明的是，與現(xiàn)有技術(shù)相比，本發(fā)明實施例中，數(shù)據(jù)采集裝置2中的各設(shè) 備均為非激光雷達(dá)設(shè)備，成本更低。且本實施例沒有使用現(xiàn)有技術(shù)中的路標(biāo)和光學(xué)運動捕 捉系統(tǒng)，也就是說，本實施例不需要在環(huán)境中安裝設(shè)置額外的裝置來協(xié)助定位與導(dǎo)航，對外 部設(shè)備的依賴小，更加方便使用。同時，本實施例采用RGB-D傳感器和超聲波測距儀用于采 集室內(nèi)環(huán)境顏色深度信息，與現(xiàn)有技術(shù)中的僅使用超聲波、紅外測距傳感器相比，本實施例 采用視覺信息進行定位，精度更高，且對于障礙物實際位置的判斷也更加精確，具有更高的 導(dǎo)航穩(wěn)定性。
[0033] 優(yōu)選地，第一算法為A*算法。本實施例的機器人依據(jù)二維全局靜態(tài)地圖、當(dāng)前位置 坐標(biāo)，當(dāng)前位置姿態(tài)信息，目標(biāo)位置坐標(biāo)，目標(biāo)位置姿態(tài)信息，在數(shù)據(jù)分析平臺上利用A*算 法在全局靜態(tài)地圖上逐個迭代柵格點計算各條候選路徑的代價，最終選擇代價最小的候選 路徑作為全局參考路徑。
[0034] 此外，需要說明的是，本實施例中的A*算法為英譯名稱，在本領(lǐng)域還具有另外兩個 名稱，分別為A-Star算法和A星算法，但是，無論是稱為A*算法，A-Star算法或A星算法，均是 本領(lǐng)域技術(shù)人員能夠理解且接受的。
[00對進一步地，第二算法為DWA算法，其中，DWA算法又名：動態(tài)窗口方法（Dynamic Window ApproachiDWA)，需要說明的是，DWA算法是本領(lǐng)域技術(shù)人員能夠理解并接收的通用 縮寫簡稱。本實施例在獲取全局參考路徑的同時，數(shù)據(jù)采集裝置2實時更新距離機器人固定 半徑內(nèi)的局部區(qū)域地圖，并獲取動態(tài)障礙物在局部區(qū)域地圖中的第一坐標(biāo)，并通過坐標(biāo)轉(zhuǎn) 換將得到的第一坐標(biāo)轉(zhuǎn)換到二維全局靜態(tài)地圖坐標(biāo)系中W獲取第二坐標(biāo)，之后數(shù)據(jù)分析裝 置3，再根據(jù)第二坐標(biāo)，終點位置坐標(biāo)，當(dāng)前位置坐標(biāo)，當(dāng)前位置姿態(tài)信息，終點位置姿態(tài)信 息和全局參考路徑，采用DWA算法對機器人在下一刻的速度向量進行最優(yōu)規(guī)劃，W獲得最優(yōu) 速度向量，W使得底盤根據(jù)最優(yōu)速度向量進行動作，從而使得機器人向終點位置進行移動。
[0036]具體地，速度向量包括線速度和角速度。更加具體地，DWA算法的依據(jù)是:在一個較 短的時間間隔里機器人能夠達(dá)到的線速度值變化范圍和角速度值變化范圍是一定的，且線 速度值變化范圍與角速度值變化范圍是根據(jù)實際所采用的電機的性能和地面摩擦系數(shù)決 定。一般的，為了便于計算，通常將線速度值變化范圍和角速度值變化范圍賦予選定的常數(shù) 進行計算。DWA算法根據(jù)線速度值變化范圍和角速度值變化范圍，通過構(gòu)建一個動態(tài)窗口， 在動態(tài)窗口內(nèi)部，通過計算下一時刻的機器人速度值來對可行的規(guī)劃區(qū)域進行最終的調(diào) 整，最后在該可行的規(guī)劃區(qū)域內(nèi)遍歷所有的線速度與角速度的取值，并將線速度與角速度 的值代入一個設(shè)定好的目標(biāo)函數(shù)中，從而可W規(guī)劃出機器人在下一時刻的速度向量。
[0037] 更加具體地，根據(jù)動力學(xué)原理，每一個時刻內(nèi)通過執(zhí)行DWA算法返回的線速度的模 與角速度可W得到一段圓弧軌跡，即每一個時刻行走的圓弧都可W獨一無二地被一個速度 向量(vi，wi)所表示，其中i = l，2,3,…n。所W為了生成接下來n個時間間隔內(nèi)當(dāng)前點到目標(biāo) 點P的軌跡，在不碰撞障礙物的前提下，數(shù)據(jù)分析需要在當(dāng)前時刻決策ti到tn運n個時間間隔 內(nèi)每一時刻的速度向量(Vi,Wi) dDWA算法的原理中指出運樣的算法復(fù)雜度會隨著時間間隔 數(shù)n成指數(shù)增長。
[0038] 為了讓決策速度向量(vi，wi)的最優(yōu)化問題具有可行性。DWA算法在當(dāng)前時刻只考 慮接下來所有n個時刻的第一個時間間隔，從而假設(shè)接下來的n-1個時間間隔內(nèi)的速度向量 與第一時刻決策的速度向量保持一致為一個常向量，即ti到tn運n-1個時刻的加速度為0。
[0039] 運樣的簡化是依據(jù)如下觀測事實而提出的：
[0040] (1)簡化后的規(guī)劃空間是二維的，所W具有易處理的特點；
[0041] (2)每一個時刻的規(guī)劃是重復(fù)進行的（沒有必要在當(dāng)前時刻計算接下來所有時刻 的最優(yōu)化問題，否則復(fù)雜度急劇上升）；
[0042] (3)速度狀態(tài)在沒有新的控制指令傳遞下將會自動保持為一個常量。
[0043] 距離機器人周圍較近的障礙物會對其線速度和角速度施加約束。最大的可行速度 取決于路徑上下一個障礙物與機器人本身的距離，即當(dāng)前的可行速度的集合是保證機器人 能夠在接觸障礙物前停下的速度范圍（受到機器人底盤電機能夠提供的最大加速度的影 響）。令對于速度向重(V ,W)的制動距罔為dist(v ,W),且^與如是加速度的臨界值。由牛頓 第二定律可W計算出向量(V，w)的初步?jīng)Q策空間為：
[0044]
CD
[0045] 其中Va就是允許機器人不碰撞障礙物的所有速度向量(v，w)的集合。
[0046] 為了將電機等動力設(shè)備能夠提供給機器人的極限加速度考慮進入對決策規(guī)劃空 間的進一步約束。DWA算法將速度向量決策空間進一步篩選成下一個時間間隔內(nèi)憑借加速 度極限值能夠達(dá)到的速度范圍。令t為時間間隔，巧日W為極限的加速度，（Va，Wa)為當(dāng)前的速 度向量。所W動杰窗口 Vri被定父化下：
[0047]
(2)
[0048] 動態(tài)窗口是一塊W當(dāng)前速度為中屯、的W極限加速度為參數(shù)擴張成的一個矩形區(qū) 域。所有在動態(tài)窗口外的速度向量在下一個時間間隔內(nèi)均不可能實現(xiàn)，所W都不被考慮。
[0049] 最后令線速度極限值與角速度極限值確定的決策空間外圍為Vs。那么綜上，最終 的決策區(qū)域Vr就被定義Vs、Va、Vd =個決策空間的交集：
[0050] Vr = Vs n Va n Vd (3)
[0051 ] Vr決策空間定義完成之后，將建立目標(biāo)函數(shù)G( V，W)，通過在Vr向量空間中求解目標(biāo) 函數(shù)G(V，w)的最大值來對下一時刻的速度向量(V，w)進行最終的決策。下面定義目標(biāo)函數(shù) 為：
[0052] G(v,w)=曰? heading(v,w)+0 ? dist_ob(v,w)+丫 ? dist_gpath(v,w) (4)
[0053] 其中，目標(biāo)函數(shù)是指向目標(biāo)測量角函數(shù)heading(v，w)、與障礙物的間隙函數(shù)dist_ ob(v,w)、與全局參考路徑的距離函數(shù)dist_gpath(v,w)的加權(quán)線性組合，且a，0，丫為S個 函數(shù)分別對應(yīng)的權(quán)值，當(dāng)=個權(quán)值分別取得合適的值時可W完成高效的動態(tài)避障任務(wù)。其 中指向目標(biāo)測量角函數(shù)heading(v，w)描述了機器人朝向與目標(biāo)點方向的同向程度，用180 ~0來表示，0是機器人正方向與目標(biāo)點方向的夾角。由于運個方向?qū)τ诓煌?v，w)都會有 所改變，e是針對機器人的下一預(yù)測位置計算得到的，通過假設(shè)機器人在下一間隔內(nèi)W選中 的速度向量運動，從而求得下一預(yù)測位置坐標(biāo)。實際的測量中，需要考慮到旋轉(zhuǎn)過程的動力 學(xué)模型。所W0需要在下一個時間間隔后施加一個在最大減速度條件下最終到達(dá)的位置點 來計算得到。與障礙物的間隙函數(shù)dist_ob(v，w)描述了下一時刻機器人與路徑上最近障礙 物的距離。如果路徑上沒有任何障礙物則方程返回一個較大的常數(shù)。與全局參考路徑的距 離函數(shù)dist_gpath(v，w)返回了下一時刻機器人將處于的位置點與規(guī)劃出的全局參考路徑 的距離，表征了所規(guī)劃的實時速度組合使得機器人下一時刻所處位置相對于最小代價位置 的差異。優(yōu)選的，目標(biāo)函數(shù)中的=個權(quán)值a，e，丫都被標(biāo)準(zhǔn)化到[0，1 ]之間。
[0054] 實施例二
[0055] 結(jié)合圖2,本實施例提供一種室內(nèi)機器人導(dǎo)航方法，應(yīng)用于機器人，包括；
[0056] 步驟SI:人機交互裝置1接收用戶輸入的機器人的終點位置坐標(biāo)及機器人的終點 位置姿態(tài)信息；
[0057] 步驟S2:數(shù)據(jù)采集裝置2獲取機器人的當(dāng)前位置坐標(biāo)和機器人的當(dāng)前位置姿態(tài)信 息；
[0058] 步驟S3:數(shù)據(jù)分析裝置討良據(jù)終點位置坐標(biāo)，終點位置姿態(tài)信息，當(dāng)前位置坐標(biāo)，當(dāng) 前位置姿態(tài)信息W及預(yù)先獲取的二維全局靜態(tài)地圖，采用第一算法生成一條位于當(dāng)前點與 終點之間的代價最小的全局參考路徑；
[0059] 步驟S4:數(shù)據(jù)采集裝置2實時更新距離機器人固定半徑內(nèi)的局部區(qū)域地圖，獲取動 態(tài)障礙物在局部區(qū)域地圖中的第一坐標(biāo)，并根據(jù)第一坐標(biāo)進行坐標(biāo)轉(zhuǎn)換W獲取動態(tài)障礙物 在二維全局靜態(tài)地圖中對應(yīng)的第二坐標(biāo)；
[0060] 步驟S5:當(dāng)機器人沒有被障礙物包圍時，為正常導(dǎo)航狀態(tài)，數(shù)據(jù)分析裝置3根據(jù)第 二坐標(biāo)，終點位置坐標(biāo)，當(dāng)前位置坐標(biāo)，當(dāng)前位置姿態(tài)信息，終點位置姿態(tài)信息和全局參考 路徑，采用第二算法對機器人在下一刻的速度向量進行最優(yōu)規(guī)劃，W獲得最優(yōu)速度向量，用 于供底盤根據(jù)所述最優(yōu)速度向量進行移動。
[0061] 本發(fā)明實施例提供的室內(nèi)機器人導(dǎo)航方法，使用人機交互裝置1接收用戶輸入的 導(dǎo)航的終點位置坐標(biāo)和終點位置姿態(tài)信息;數(shù)據(jù)采集裝置2獲取機器人的當(dāng)前位置坐標(biāo)和 當(dāng)前位置姿態(tài)信息；之后，數(shù)據(jù)分析裝置3，再根據(jù)終點位置坐標(biāo)，終點位置姿態(tài)信息，當(dāng)前 位置坐標(biāo)，位置姿態(tài)信息W及預(yù)先獲取的二維全局靜態(tài)地圖，采用第一算法生成一條位于 當(dāng)前點與終點之間的代價最小的全局參考路徑;在獲得全局參考路徑的同時，數(shù)據(jù)采集裝 置2實時更新距離機器人固定半徑內(nèi)的局部區(qū)域地圖，并獲取動態(tài)障礙物在局部區(qū)域地圖 中的第一坐標(biāo)，并通過坐標(biāo)轉(zhuǎn)換將得到的第一坐標(biāo)轉(zhuǎn)換到二維全局靜態(tài)地圖坐標(biāo)系中W獲 取第二坐標(biāo)；當(dāng)機器人沒有被障礙物包圍時，數(shù)據(jù)分析裝置3再根據(jù)第二坐標(biāo)，終點位置坐 標(biāo)，當(dāng)前位置坐標(biāo)，當(dāng)前位置姿態(tài)信息，終點位置姿態(tài)信息和全局參考路徑，采用第二算法 對機器人在下一刻的速度向量進行最優(yōu)規(guī)劃，W獲得最優(yōu)速度向量，W使得底盤根據(jù)最優(yōu) 速度向量進行動作，從而使得機器人向終點位置進行移動。此外，需要說明的是，本發(fā)明實 施例的室內(nèi)機器人導(dǎo)航方法是通過控制底盤的運動來實現(xiàn)對機器人的運動進行導(dǎo)航。
[0062] 本發(fā)明實施例提供的室內(nèi)機器人導(dǎo)航方法，先采用第一算法進行全局靜態(tài)規(guī)劃， 規(guī)劃出一條位于當(dāng)前點與終點之間的代價最小的全局參考路徑，之后再采用第二算法進行 局部動態(tài)避障規(guī)劃，根據(jù)局部近距離動態(tài)障礙物對路徑進行實時調(diào)整。本發(fā)明實施例提供 的室內(nèi)機器人導(dǎo)航方法，采用全局靜態(tài)規(guī)劃與局部動態(tài)避障規(guī)劃相結(jié)合的路徑規(guī)劃算法對 機器人的路徑進行實時規(guī)劃，使得機器人可W根據(jù)局部近距離動態(tài)障礙物的更新做出路徑 的調(diào)整，實用性高。
[0063] 優(yōu)選地，步驟S5中，當(dāng)機器人被障礙物包圍而無法進行導(dǎo)航時，還包括:數(shù)據(jù)分析 裝置3發(fā)出一個預(yù)設(shè)的速度向量給底盤W進行原地旋轉(zhuǎn);數(shù)據(jù)采集裝置2持續(xù)掃描并更新局 部區(qū)域地圖，直到局部區(qū)域地圖中的障礙物的分布允許機器人通過，返回正常導(dǎo)航狀態(tài)，其 中，障礙物包括動態(tài)障礙物和靜態(tài)障礙物。當(dāng)動態(tài)障礙物或者動態(tài)障礙物與靜態(tài)障礙物一 起形成包圍將機器人圍住時，機器人將進入原地旋轉(zhuǎn)狀態(tài)，持續(xù)掃描并更新周圍的障礙物 信息，與此同時，數(shù)據(jù)分析裝置3將會在局部區(qū)域地圖中清除上一周期旋轉(zhuǎn)掃描得到的周圍 障礙物信息，并重建當(dāng)前周期旋轉(zhuǎn)掃描得到的障礙物信息。直到周圍障礙物的分布允許機 器人通過后，終止運一循環(huán)，返回正常的導(dǎo)航狀態(tài)。
[0064] 優(yōu)選地，步驟S3中，二維全局靜態(tài)地圖的預(yù)先獲取步驟包括:數(shù)據(jù)采集裝置2圍繞 當(dāng)前環(huán)境進行一次全局掃描W獲得若干帖=維點云，并采用=維SLAM算法將若干帖=維點 云拼接成為=維全局靜態(tài)地圖；數(shù)據(jù)分析裝置3對=維全局靜態(tài)地圖進行降維度處理，W獲 得二維全局靜態(tài)地圖。本實施例通過預(yù)先對=維點云地圖進行投影并獲得二維全局靜態(tài)地 圖，其目的在于，節(jié)省存儲空間，簡化數(shù)據(jù)存儲的空間開銷，W提高導(dǎo)航效率。
[0065] 優(yōu)選地，第一算法為A*算法。本實施例的機器人依據(jù)二維全局靜態(tài)地圖、當(dāng)前位置 坐標(biāo)，當(dāng)前位置姿態(tài)信息，目標(biāo)位置坐標(biāo)，目標(biāo)位置姿態(tài)信息，在數(shù)據(jù)分析平臺上利用A*算 法在全局靜態(tài)地圖上逐個迭代柵格點計算各條候選路徑的代價，最終選擇代價最小的候選 路徑作為全局參考路徑。
[0066] 此外，需要說明的是，本實施例中的A*算法為英譯名稱，在本領(lǐng)域還具有另外兩個 名稱，分別為A-Star算法和A星算法，但是，無論是稱為A*算法，A-Star算法或A星算法，均是 本領(lǐng)域技術(shù)人員能夠理解且接受的。
[0067] 進一步地，第二算法為DWA算法。其中，DWA算法又名：動態(tài)窗口方法（Dynamic Window ApproachiDWA)，需要說明的是，DWA算法是本領(lǐng)域技術(shù)人員能夠理解并接收的通用 縮寫簡稱。本實施例在獲取全局參考路徑的同時，數(shù)據(jù)采集裝置2實時更新距離機器人固定 半徑內(nèi)的局部區(qū)域地圖，并獲取動態(tài)障礙物在局部區(qū)域地圖中的第一坐標(biāo)，并通過坐標(biāo)轉(zhuǎn) 換將得到的第一坐標(biāo)轉(zhuǎn)換到二維全局靜態(tài)地圖坐標(biāo)系中W獲取第二坐標(biāo)，之后數(shù)據(jù)分析裝 置3，再根據(jù)第二坐標(biāo)，終點位置坐標(biāo)，當(dāng)前位置坐標(biāo)，當(dāng)前位置姿態(tài)信息，終點位置姿態(tài)信 息和全局參考路徑，采用DWA算法對機器人在下一刻的速度向量進行最優(yōu)規(guī)劃，W獲得最優(yōu) 速度向量，W使得底盤根據(jù)最優(yōu)速度向量進行動作，從而使得機器人向終點位置進行移動。
[0068] 具體地，速度向量包括線速度和角速度。更加具體地，DWA算法的依據(jù)是:在一個較 短的時間間隔里機器人能夠達(dá)到的線速度值變化范圍和角速度值變化范圍是一定的，且線 速度值變化范圍與角速度值變化范圍是根據(jù)實際所采用的電機的性能和地面摩擦系數(shù)決 定。一般的，為了便于計算，通常將線速度值變化范圍和角速度值變化范圍賦予選定的常數(shù) 進行計算。DWA算法根據(jù)線速度值變化范圍和角速度值變化范圍，通過構(gòu)建一個動態(tài)窗口， 在動態(tài)窗口內(nèi)部，通過計算下一時刻的機器人速度值來對可行的規(guī)劃區(qū)域進行最終的調(diào) 整，最后在該可行的規(guī)劃區(qū)域內(nèi)遍歷所有的線速度與角速度的取值，并將線速度與角速度 的值代入一個設(shè)定好的目標(biāo)函數(shù)中，從而可W規(guī)劃出機器人在下一時刻的速度向量。
[0069] 更加具體地，根據(jù)動力學(xué)原理，每一個時刻內(nèi)通過執(zhí)行DWA算法返回的線速度的模 與角速度可W得到一段圓弧軌跡，即每一個時刻行走的圓弧都可W獨一無二地被一個速度 向量(vi，wi)所表示，其中i = l，2,3,…n。所W為了生成接下來n個時間間隔內(nèi)當(dāng)前點到目標(biāo) 點P的軌跡，在不碰撞障礙物的前提下，數(shù)據(jù)分析需要在當(dāng)前時刻決策ti到tn運n個時間間隔 內(nèi)每一時刻的速度向量(Vi,Wi) dDWA算法的原理中指出運樣的算法復(fù)雜度會隨著時間間隔 數(shù)n成指數(shù)增長。
[0070] 為了讓決策速度向量(Vi，Wi)的最優(yōu)化問題具有可行性。DWA算法在當(dāng)前時刻只考 慮接下來所有n個時刻的第一個時間間隔，從而假設(shè)接下來的n-1個時間間隔內(nèi)的速度向量 與第一時刻決策的速度向量保持一致為一個常向量，即ti到tn運n-1個時刻的加速度為0。
[0071] 運樣的簡化是依據(jù)如下觀測事實而提出的：
[0072] (1)簡化后的規(guī)劃空間是二維的，所W具有易處理的特點；
[0073] (2)每一個時刻的規(guī)劃是重復(fù)進行的（沒有必要在當(dāng)前時刻計算接下來所有時刻 的最優(yōu)化問題，否則復(fù)雜度急劇上升）；
[0074] (3)速度狀態(tài)在沒有新的控制指令傳遞下將會自動保持為一個常量。
[0075] 距離機器人周圍較近的障礙物會對其線速度和角速度施加約束。最大的可行速度 取決于路徑上下一個障礙物與機器人本身的距離，即當(dāng)前的可行速度的集合是保證機器人 能夠在接觸障礙物前停下的速度范圍（受到機器人底盤電機能夠提供的最大加速度的影 響）。令對于速度向量(V，W)的制動距離為diSt (V，W)，且磚與是加速度的臨界值。由牛頓 第二定律可W計算出向量(v，w)的初步?jīng)Q策空間為：
[0076]
( 1 )
[0077] 其中Va就是允許機器人不碰撞障礙物的所有速度向量(v，w)的集合。
[0078] 為了將電機等動力設(shè)備能夠提供給機器人的極限加速度考慮進入對決策規(guī)劃空 間的進一步約束。DWA算法將速度向量決策空間進一步篩選成下一個時間間隔內(nèi)憑借加速 度極限值能夠達(dá)到的速度范圍。令t為時間間隔，1)和W為極限的加速度，（Va，Wa)為當(dāng)前的 速度向音。所W動杰窗口 W獻吿公如下，
[0079]
(2)
[0080] 動態(tài)窗口是一塊W當(dāng)前速度為中屯、的W極限加速度為參數(shù)擴張成的一個矩形區(qū) 域。所有在動態(tài)窗口外的速度向量在下一個時間間隔內(nèi)均不可能實現(xiàn)，所W都不被考慮。
[0081] 最后令線速度極限值與角速度極限值確定的決策空間外圍為Vs。那么綜上，最終 的決策區(qū)域Vr就被定義Vs、Va、Vd =個決策空間的交集：
[0082] Vr = Vs n Va n Vd (3)
[0083] Vr決策空間定義完成之后，將建立目標(biāo)函數(shù)G( V，W)，通過在Vr向量空間中求解目標(biāo) 函數(shù)G(V，w)的最大值來對下一時刻的速度向量(V，w)進行最終的決策。下面定義目標(biāo)函數(shù) 為：
[0084] G(v,w)=曰? heading(v,w)+0 ? dist_ob(v,w)+丫 ? dist_gpath(v,w) (4)
[0085] 其中，目標(biāo)函數(shù)是指向目標(biāo)測量角函數(shù)heading(v，w)、與障礙物的間隙函數(shù)dist_ ob(v,w)、與全局參考路徑的距離函數(shù)dist_gpath(v,w)的加權(quán)線性組合，且a，0，丫為S個 函數(shù)分別對應(yīng)的權(quán)值，當(dāng)=個權(quán)值分別取得合適的值時可W完成高效的動態(tài)避障任務(wù)。其 中指向目標(biāo)測量角函數(shù)heading(v，w)描述了機器人朝向與目標(biāo)點方向的同向程度，用180 ~0來表示，0是機器人正方向與目標(biāo)點方向的夾角。由于運個方向?qū)τ诓煌?v，w)都會有 所改變，e是針對機器人的下一預(yù)測位置計算得到的，通過假設(shè)機器人在下一間隔內(nèi)W選中 的速度向量運動，從而求得下一預(yù)測位置坐標(biāo)。實際的測量中，需要考慮到旋轉(zhuǎn)過程的動力 學(xué)模型。所W0需要在下一個時間間隔后施加一個在最大減速度條件下最終到達(dá)的位置點 來計算得到。與障礙物的間隙函數(shù)dist_ob(v，w)描述了下一時刻機器人與路徑上最近障礙 物的距離。如果路徑上沒有任何障礙物則方程返回一個較大的常數(shù)。與全局參考路徑的距 離函數(shù)dist_gpath(v，w)返回了下一時刻機器人將處于的位置點與規(guī)劃出的全局參考路徑 的距離，表征了所規(guī)劃的實時速度組合使得機器人下一時刻所處位置相對于最小代價位置 的差異。優(yōu)選的，目標(biāo)函數(shù)中的=個權(quán)值a，e，丫都被標(biāo)準(zhǔn)化到[0，1 ]之間。
[0086]盡管本發(fā)明已進行了一定程度的描述，明顯地，在不脫離本發(fā)明的精神和范圍的 條件下，可進行各個條件的適當(dāng)變化?？蒞理解，本發(fā)明不限于所述實施方案，而歸于權(quán)利 要求的范圍，其包括所述每個因素的等同替換。
【主權(quán)項】
1. 一種室內(nèi)機器人導(dǎo)航系統(tǒng)，應(yīng)用于機器人，其特征在于，包括： 人機交互裝置：用于接收用戶輸入的所述機器人的終點位置坐標(biāo)及所述機器人的終點 位置姿態(tài)信息； 數(shù)據(jù)采集裝置:用于獲取所述機器人的當(dāng)前位置坐標(biāo)和所述機器人的當(dāng)前位置姿態(tài)信 息； 數(shù)據(jù)分析裝置，用于根據(jù)所述終點位置坐標(biāo)，所述終點位置姿態(tài)信息，所述當(dāng)前位置坐 標(biāo)，當(dāng)前位置姿態(tài)信息以及預(yù)先獲取的二維全局靜態(tài)地圖，采用第一算法生成一條位于當(dāng) 前點與終點之間的代價最小的全局參考路徑； 數(shù)據(jù)采集裝置，還用于實時更新距離機器人固定半徑內(nèi)的局部區(qū)域地圖，獲取動態(tài)障 礙物在所述局部區(qū)域地圖中的第一坐標(biāo)，并根據(jù)所述第一坐標(biāo)進行坐標(biāo)轉(zhuǎn)換以獲取所述動 態(tài)障礙物在所述二維全局靜態(tài)地圖中對應(yīng)的第二坐標(biāo)； 數(shù)據(jù)分析裝置，還用于根據(jù)所述第二坐標(biāo)，所述終點位置坐標(biāo)，所述當(dāng)前位置坐標(biāo)，所 述當(dāng)前位置姿態(tài)信息，所述終點位置姿態(tài)信息和所述全局參考路徑，采用第二算法對機器 人在下一刻的速度向量進行最優(yōu)規(guī)劃，以獲得最優(yōu)速度向量，用于供機器人根據(jù)所述最優(yōu) 速度向量進行移動。2. 根據(jù)權(quán)利要求1所述的室內(nèi)機器人導(dǎo)航系統(tǒng)，其特征在于，所述人機交互裝置包括一 個可視化的交互界面。3. 根據(jù)權(quán)利要求1所述的室內(nèi)機器人導(dǎo)航系統(tǒng)，其特征在于，所述數(shù)據(jù)采集裝置包括： RGB-D傳感器、超聲波測距儀、慣性測量傳感器。4. 根據(jù)權(quán)利要求1至3中任一項所述的室內(nèi)機器人導(dǎo)航系統(tǒng)，其特征在于，所述第一算 法為A*算法。5. 根據(jù)權(quán)利要求4所述的室內(nèi)機器人導(dǎo)航系統(tǒng)，其特征在于，所述第二算法為DWA算法。6. -種室內(nèi)機器人導(dǎo)航方法，應(yīng)用于機器人，其特征在于，包括： 步驟SI:人機交互裝置接收用戶輸入的所述機器人的終點位置坐標(biāo)及所述機器人的終 點位置姿態(tài)信息； 步驟S2:數(shù)據(jù)采集裝置獲取所述機器人的當(dāng)前位置坐標(biāo)和所述機器人的當(dāng)前位置姿態(tài) 信息； 步驟S3:數(shù)據(jù)分析裝置根據(jù)所述終點位置坐標(biāo)，所述終點位置姿態(tài)信息，所述當(dāng)前位置 坐標(biāo)，當(dāng)前位置姿態(tài)信息以及預(yù)先獲取的二維全局靜態(tài)地圖，采用第一算法生成一條位于 當(dāng)前點與終點之間的代價最小的全局參考路徑； 步驟S4:數(shù)據(jù)采集裝置實時更新距離機器人固定半徑內(nèi)的局部區(qū)域地圖，獲取動態(tài)障 礙物在所述局部區(qū)域地圖中的第一坐標(biāo)，并根據(jù)所述第一坐標(biāo)進行坐標(biāo)轉(zhuǎn)換以獲取所述動 態(tài)障礙物在所述二維全局靜態(tài)地圖中對應(yīng)的第二坐標(biāo)； 步驟S5:當(dāng)所述機器人沒有被障礙物包圍時，為正常導(dǎo)航狀態(tài)，數(shù)據(jù)分析裝置根據(jù)所述 第二坐標(biāo)，所述終點位置坐標(biāo)，所述當(dāng)前位置坐標(biāo)，所述當(dāng)前位置姿態(tài)信息，所述終點位置 姿態(tài)信息和所述全局參考路徑，采用第二算法對機器人在下一刻的速度向量進行最優(yōu)規(guī) 劃，以獲得最優(yōu)速度向量，用于供機器人根據(jù)所述最優(yōu)速度向量進行移動。7. 根據(jù)權(quán)利要求6所述的室內(nèi)機器人導(dǎo)航方法，其特征在于，所述步驟S5中，當(dāng)所述機 器人被障礙物包圍而無法進行導(dǎo)航時，還包括： 數(shù)據(jù)分析裝置發(fā)出一個預(yù)設(shè)的速度向量給底盤以進行原地旋轉(zhuǎn)； 數(shù)據(jù)采集裝置持續(xù)掃描并更新所述局部區(qū)域地圖，直到所述局部區(qū)域地圖中的障礙物 的分布允許機器人通過，返回正常導(dǎo)航狀態(tài)，其中，所述障礙物包括所述動態(tài)障礙物和靜態(tài) 障礙物。8. 根據(jù)權(quán)利要求7所述的室內(nèi)機器人導(dǎo)航方法，其特征在于，所述步驟S3中，所述二維 全局靜態(tài)地圖的預(yù)先獲取步驟包括： 數(shù)據(jù)采集裝置圍繞當(dāng)前環(huán)境進行一次全局掃描以獲得若干幀三維點云，并采用三維 SLAM算法將所述若干幀三維點云拼接成為三維全局靜態(tài)地圖； 數(shù)據(jù)分析裝置對所述三維全局靜態(tài)地圖進行降維度處理，以獲得二維全局靜態(tài)地圖。9. 根據(jù)權(quán)利要求8所述的室內(nèi)機器人導(dǎo)航系統(tǒng)，其特征在于，所述第一算法為A*算法。10. 根據(jù)權(quán)利要求9所述的室內(nèi)機器人導(dǎo)航系統(tǒng)，其特征在于，所述第二算法為DWA算 法。
【文檔編號】G05D1/02GK105955273SQ201610353911
【公開日】2016年9月21日
【申請日】2016年5月25日
【發(fā)明人】杭蒙, 陳震, 朱騁, 王皓月, 李經(jīng)緯, 蔣申璐, 楊永全, 任海波, 杜新鵬
【申請人】速感科技（北京）有限公司