[發明專利]一種機器人避碰路徑的規劃方法、裝置

有效

• 申請號: 201310586189.2
• 申請日: 2013-11-19
• 公開（公告）號: CN104154917A
• 公開（公告）日: 2014-11-19
• 發明（設計）人: 李華忠;梁永生;但唐仁;唐飛
• 申請（專利權）人: 深圳信息職業技術學院;李華忠
• 主分類號: G01C21/20
• 分類號: G01C21/20
• 代理公司: 深圳中一專利商標事務所 44237
• 代理人: 張全文

1.一種機器人避碰路徑的規劃方法，其特征在于，所述方法包括：

步驟1、根據機器人的初始位形q_s和目標位形q_g計算得到碰撞檢測距離參數的初始值r_col；

步驟2、根據所述初始位形q_s、目標位形q_g構造雙向快速密集搜索樹，所述雙向快速密集搜索樹包括正向快速密集搜索樹和逆向快速密集搜索樹；

步驟3、設置所述雙向快速密集搜索樹的碰撞檢測距離參數的值為r_col；

步驟4、根據所述初始位形q_s、目標位形q_g采用自適應雙向快速密集搜索樹動態擴展方法生成一條機器人規劃路徑；

步驟5、對所述機器人規劃路徑上的離散采樣位形點分別進行碰撞檢測，如果所有離散位形點均沒有與障礙物發生碰撞，則將由所有離散采樣位形點組成的機器人規劃路徑作為有效路徑，如果有離散位形點與障礙物發生碰撞，則執行步驟6；

步驟6、更新r_col的值為r_col/2；

步驟7、循環執行步驟3至6，直至生成的機器人規劃路徑上的所有離散位形點均沒有與障礙物發生碰撞為止。

2.如權利要求1所述的方法，其特征在于，所述步驟4包括：

根據所述初始位形q_s、目標位形q_g計算得到擴展步長的初始值r_ext；

根據所述初始位形q_s、目標位形q_g和所述擴展步長的初始值r_ext配置所述雙向快速密集搜索樹；

局部調整所述擴展步長的初始值r_ext，獲得一條機器人規劃路徑。

3.如權利要求2所述的方法，其特征在于，所述局部調整所述擴展步長的初始值r_ext，獲得一條機器人規劃路徑包括：

從位形空間隨機采樣獲得一個隨機采樣位形點q_rand；

搜索所述正向快速密集搜索樹上與所述隨機采樣位形q_rand最近鄰的邊e₁；

在邊e₁上搜索與所述隨機采樣位形q_rand最近鄰的位形點

根據所述q_rand、以及所述擴展步長r_ext計算得到不與障礙物發生碰撞的新位形點q_new；

根據所述最近鄰的位形點以及所述新位形點q_new做碰撞檢測；

如果連接所述最近鄰的位形點和所述新位形點q_new之間的路徑沒有與障礙物發生碰撞，則搜索所述逆向快速密集搜索樹上與所述新位形點q_new最近鄰的邊e₂；

在邊e₂上搜索與所述新位形點q_new最近鄰的位形點

根據所述最近鄰的位形點以及所述新位形點q_new做碰撞檢測；

若連接所述最近鄰的位形點和所述新位形點q_new之間的路徑沒有與障礙物發生碰撞時，將所述邊e₂、所述最近鄰的位形點和所述新位形點q_new添加至所述逆向快速密集搜索樹中，并獲得新邊e_new，同時分別使與所述邊e₂和所述新邊e_new對應的擴展步長r_ext加倍；

若連接所述最近鄰的位形點和所述新位形點q_new之間的路徑與障礙物發生碰撞時，計算機器人從向q_new移動而不與障礙物發生碰撞的最合適的位形點，將所述最適合的位形點、所述邊e₂和所述新位形點q_new添加至所述逆向快速密集搜索樹中，并獲得新邊e_new，同時分別使與所述邊e₂和所述新邊e_new對應的擴展步長r_ext減半，直至連接所述最近鄰的位形點和所述新位形點q_new之間的路徑沒有與障礙物發生碰撞。