[發(fā)明專利]一種雙足機器人的路徑規(guī)劃方法

有效

• 申請?zhí)枺?/td>: 202011491208.X
• 申請日: 2020-12-16
• 公開（公告）號: CN112650234B
• 公開（公告）日: 2022-05-17
• 發(fā)明（設計）人: 甘春標;李子靜;葛一敏;王小瑩;袁璐;張金霖;朱小京;余天浩
• 申請（專利權）人: 浙江大學
• 主分類號: G05D1/02
• 分類號: G05D1/02
• 代理公司: 杭州求是專利事務所有限公司 33200
• 代理人: 劉靜

本發(fā)明公開了一種雙足機器人的路徑規(guī)劃方法，使用基于多源傳感器RGBD的orbslam2建立3D稠密點云圖，并使用Octomap進行壓縮濾除地面信息。然后通過2D投影生成占據(jù)格地圖，最后利用代價地圖Costmap加障礙層，用改進的D*和DwA進行全局和局部路徑規(guī)劃導航實時避障。本方法采用全局與局部路徑規(guī)劃方法的融合，通過在D*算法的子節(jié)點實際代價值中加入角度評價，減少不必要的轉彎過程，同時通過限制子節(jié)點的選取，使路線規(guī)劃更加安全可靠。

技術領域

本發(fā)明涉及機器人領域，尤其涉及一種雙足機器人的路徑規(guī)劃方法。

背景技術

移動機器人現(xiàn)有的路徑規(guī)劃根據(jù)地圖環(huán)境和障礙物類型等可以分為全局路徑規(guī)劃和局部路徑規(guī)劃。全局路徑規(guī)劃主要用于規(guī)劃機器人的路徑，局部路徑規(guī)劃主要用于避障。D*算法為常用的局部路徑規(guī)劃算法，但是在避障中存在一定的缺點，D*算法是一種長度優(yōu)先的算法并且向周圍8個子節(jié)點進行擴展搜索，生成的路徑會產(chǎn)生損壞路徑質量的不必要轉彎，在實際導航過程中，這些不必要的轉彎會增加移動機器人系統(tǒng)的能耗和時間，長度優(yōu)先也導致生成的路徑十分貼近障礙物，某些時候會從兩個“危險”障礙物之間穿過，對實際應用追求的安全性和低能耗考慮的并不周到。同時一般的算法一般是基于輪式機器人所編寫的，要在雙足機器人中實現(xiàn)，需要進行一定的轉換，并不容易。

發(fā)明內容

本發(fā)明目的在于針對現(xiàn)有技術的不足，提出一種雙足機器人的路徑規(guī)劃方法。

本發(fā)明的目的是通過以下技術方案來實現(xiàn)的：一種雙足機器人的路徑規(guī)劃方法，包括以下步驟：

步驟一，構建地圖

基于多源傳感器RGBD的orbslam2建立3D稠密點云圖，基于隨機一致性采樣(RANSAC)進行點云的地面濾除，然后將Octomap進行2D投影生成占據(jù)柵格地圖，最后利用Costmap代價地圖疊加動態(tài)障礙層作為最終的導航地圖。

步驟二，路徑規(guī)劃

采用改進的基于啟發(fā)式的D*算法，算法在選取子節(jié)點時，根據(jù)父節(jié)點與周圍節(jié)點的代價函數(shù)C(X,Y)值，連續(xù)計算兩層父子節(jié)點值，篩選掉靠近障礙物的子節(jié)點，防止算法生成路徑貼近障礙物。同時在父節(jié)點周圍擴展子節(jié)點，計算擴展子節(jié)點代價值時，根據(jù)角度余弦值進行修正，優(yōu)先選擇擴展使得路徑平滑的子節(jié)點，減少路徑轉彎次數(shù)。

在根據(jù)步驟一構建的地圖和目標位置，規(guī)劃一條機器人到達目標位置的全局路徑(只考慮工作空間的幾何約束，不考慮機器人的運動學模型和約束)。

步驟三，避障規(guī)劃

采用動態(tài)窗口算法，根據(jù)所得到的實時傳感器測量信息計算最佳的速度指令，調整局部路徑以避免發(fā)生碰撞。具體過程為：在機器人控制空間(dx,dy,dθ)中離散采樣，對于每個采樣速度，從機器人的當前狀態(tài)執(zhí)行正向移動模擬，以預測如果在某時間段內應用采樣速度將會發(fā)生什么。然后從以下方面來評價正向移動模擬產(chǎn)生的每個軌跡：接近障礙物，接近目標，接近全局路徑和速度。并且放棄非法軌跡，所述非法軌跡為與障礙物相撞的軌跡，選擇最高得分的軌跡，并將最佳速度指令發(fā)送到下位機的移動基座。然后重復軌跡評價直到軌跡空間遍歷完成。

步驟四，軌跡生成

將機器人模型轉化為線性倒立擺模型，確定每步之間落腳點信息的轉換關系，然后進行步態(tài)規(guī)劃，包括實際落腳點的求算、質心軌跡規(guī)劃、踝關節(jié)軌跡規(guī)劃，以及逆運動學求算關節(jié)角度。根據(jù)機器人的運動學模型和約束，發(fā)送控制命令給機器人下位機，將可行的直線路徑轉化為可行的腳印軌跡。

進一步地，所述步驟2中，改進的基于啟發(fā)式的D*算法具體步驟如下：

S1，在步驟一構建的地圖中確定起始點S與目標點G的位置，將G點加入open表中，同時置空open表與close表。

S2，判斷open表是否為空，若為空，表示目標不可達。