[發明專利]一種雙足機器人的路徑規劃方法

有效

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

1.一種雙足機器人的路徑規劃方法，其特征在于，包括以下步驟：

步驟一，構建地圖

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

步驟二，路徑規劃

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

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

步驟三，避障規劃

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

步驟四，軌跡生成

將機器人模型轉化為線性倒立擺模型，確定每步之間落腳點信息的轉換關系，然后進行步態規劃，首先需要運用輪式運動學模型，將整車線位移映射到雙足機器人質心線位移上，但是在轉換系數上除二，角速度以步行方向在單位時間ΔT的改變來實現；然后進行實際落腳點的求算、質心軌跡規劃、踝關節軌跡規劃，以及逆運動學求算關節角度；最后根據機器人的運動學模型和約束，發送控制命令給機器人下位機，將可行的直線路徑轉化為可行的腳印軌跡。

2.根據權利要求1中所述的一種雙足機器人的路徑規劃方法，其特征在于，所述步驟2中，改進的基于啟發式的D*算法具體步驟如下：

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

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

S3，若open表不為空，將當前節點作為父節點，將其周圍的所有子節點全部放入open表中；

S4，將代價函數值最小的子節點作為下一節點next，并將節點next放入closed表中，若節點next為目標點，則找到路徑，算法結束；若并非目標點，開始以節點next為父節點擴展周圍子節點sub；

S5，計算節點next與節點sub之間的路徑開銷C(next，sub)，當開銷為無窮大時，表明節點next周圍存在障礙，則將節點next從close表中剔除，并按照代價函數值從小到大順序再從open表中選擇下一個子節點作為下一節點next，并返回S4進行計算；

S6，當節點next與節點sub之間的路徑開銷C(next，sub)均不為無窮大時，判斷節點sub是否在open表中，若在，則計算節點sub的代價函數值，并與節點next的代價函數值進行比較；當節點sub的代價函數值小于節點next的代價函數值，則直接跳到S7，反之則舍棄節點sub；若節點sub不在open表中，繼續判斷是否在close表中，如果不在，則將節點sub插入open表中，然后執行S7；若節點sub在節點open表中，判斷節點sub與節點next代價函數的大小關系，若節點sub代價函數值小于節點next的代價函數值，將節點sub從Close表移至Open表，并更新Close表代價值，反之舍棄節點sub；

S7，更新Open表中的代價值，并將指針指向Next；然后跳到S2。