[發明專利]一種錨桿支護機器人工作臂軌跡規劃方法及系統

有效

• 申請號: 202210026005.6
• 申請日: 2022-01-11
• 公開（公告）號: CN114227695B
• 公開（公告）日: 2023-09-08
• 發明（設計）人: 鄧鎵敏;高志強;郭治富;閻志偉;金江;劉偉立;趙永紅
• 申請（專利權）人: 中國煤炭科工集團太原研究院有限公司;山西天地煤機裝備有限公司;煤炭科學研究總院
• 主分類號: B25J9/16
• 分類號: B25J9/16
• 代理公司: 北京高沃律師事務所 11569
• 代理人: 杜陽陽

本發明涉及一種錨桿支護機器人工作臂軌跡規劃方法及系統，根據第一錨孔和第二錨孔的位置確定錨桿支護機器人工作臂的起止位置；在起止位置間進行工作臂末端關節的無障礙路徑規劃，確定工作臂末端關節的無障礙路徑軌跡；將所述軌跡離散為若干個路徑點，計算各路徑點相對工作臂的關節坐標；基于工作臂擺動范圍和各路徑點相對工作臂的關節坐標，計算工作臂在相鄰路徑點間的運動時間，進而確定工作臂剩余關節的三次樣條軌跡。采用三次樣條插值法進行路徑規劃，保證工作臂可以經過預設路徑點，滿足錨桿支護機器人工作時的避障要求；且能夠保證錨桿支護機器人工作臂在運動過程中對速度和加速度的連續性要求，使錨桿支護機器人工作臂運動平穩，無沖擊。

技術領域

本發明涉及錨桿支護機器人自動錨桿支護技術領域，特別是涉及一種錨桿支護機器人工作臂軌跡規劃方法及系統。

背景技術

錨桿支護機器人是煤礦巷道支護的重要裝備，對巷道高效安全掘進具有巨大的意義。錨桿支護機器人工作臂的軌跡規劃是自動化錨桿支護的技術基礎。錨桿支護機器人工作臂軌跡規劃表示為位移、速度或加速度等位姿運動量關于時間的函數，一般在關節空間或笛卡爾空間中進行。煤礦井下復雜的工作環境，對錨桿支護機器人作業有很高的要求。錨桿支護機器人工作臂在運動過程中需要保證運動平穩且無沖擊，同時還需要合理的規避外界環境中的障礙。

目前錨桿支護機器人工作臂軌跡規劃一般采用點對點軌跡規劃，例如：多項式軌跡規劃、梯形軌跡規劃等方法，無法同時滿足上述要求，工作臂在相鄰兩錨孔運動時容易與機身和外界環境發生碰撞，為井下安全支護帶來了嚴重的安全隱患。

發明內容

本發明的目的是提供一種錨桿支護機器人工作臂軌跡規劃方法及系統，能夠保證錨桿支護機器人工作臂可以經過預設的路徑點，滿足錨桿支護機器人工作時的避障要求；同時保證錨桿支護機器人工作臂在運動過程中對速度和加速度的連續性要求，使得錨桿支護機器人工作臂能夠運動平穩，無沖擊。

為實現上述目的，本發明提供了如下方案：

本發明提供了一種錨桿支護機器人工作臂軌跡規劃方法，所述方法包括：

根據第一錨孔的位置確定錨桿支護機器人工作臂的起始位置，根據第二錨孔的位置確定錨桿支護機器人工作臂的終止位置；所述第一錨孔和所述第二錨孔為相鄰的兩個錨孔；

在所述起始位置和所述終止位置之間進行工作臂的末端關節的無障礙路徑規劃，確定所述工作臂的末端關節的無障礙路徑軌跡；

將所述無障礙路徑軌跡離散為若干個路徑點，并分別計算各個路徑點相對于所述工作臂的關節坐標；

基于工作臂的擺動范圍和各個路徑點相對于所述工作臂的關節坐標，計算所述工作臂在相鄰路徑點之間的運動時間；

根據所述工作臂在相鄰路徑點之間的運動時間，確定工作臂的剩余關節的三次樣條軌跡；所述剩余關節為所述工作臂中除末端關節以外的關節。

可選地，在所述“基于工作臂的擺動范圍和各個路徑點相對于所述工作臂的關節坐標，計算所述工作臂在相鄰路徑點之間的運動時間”之前，還包括：

在第一路徑點與第二路徑點間設置第一自由路徑點，在第K+1路徑點與第K路徑點間設置第二自由路徑點，并確定所述第一自由路徑點和所述第二自由路徑點相對于所述工作臂的關節坐標，以滿足起止位置的條件約束；所述條件約束為：所述末端關節在起始位置的速度為0，所述末端關節在起始位置的加速度為0，所述末端關節在終止位置的速度為0，所述末端關節在終止位置的加速度為0；所述第一路徑點為K+1個路徑點中的首個路徑點；所述第二路徑點為K+1個路徑點中的第二個路徑點，所述第K+1路徑點為末個路徑點，K+1為路徑點總個數。

可選地，計算所述工作臂在相鄰路徑點之間的運動時間Δt_ik采用公式：