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

有效

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

1.一種錨桿支護機器人工作臂軌跡規劃方法，其特征在于，所述方法包括：

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

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

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

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

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

計算工作臂剩余關節的三次樣條軌跡S_ik(t)的具體公式為：

其中，i表示工作臂的關節序號，t為關節i的運動時間，t_ik為關節i到達路徑點k時對應的時刻，t_i(k+1)為關節i到達路徑點k+1時對應的時刻，Δt_ik為關節i在路徑點k和k+1之間的運動時間，q_ik為路徑點k對應的關節i的坐標，為工作臂末端關節到達路徑點k時關節i的加速度，為工作臂末端關節到達路徑點k+1時關節i的加速度；

工作臂末端關節到達剩余路徑點時關節i的加速度的計算過程具體包括：

將工作臂到達剩余路徑點時關節i的加速度記為集合a_i，令所述剩余路徑點為除第一路徑點和第K+1路徑點以外的路徑點；

計算工作臂到達剩余路徑點時關節i的加速度集合a_i的具體公式為：

a_i＝A_i^-1b_i；

其中，a_i為工作臂到達剩余路徑點時關節i的加速度集合，A_i為方程的系數項，b_i為方程的常數項，為工作臂末端到達第K+1路徑點K時關節i的速度，為工作臂末端到達第K+1路徑點K時關節i的加速度。

2.根據權利要求1所述的錨桿支護機器人工作臂軌跡規劃方法，其特征在于，在所述“基于工作臂的擺動范圍和各個路徑點相對于所述工作臂的關節坐標，計算所述工作臂在相鄰路徑點之間的運動時間”之前，還包括：

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

3.根據權利要求2所述的錨桿支護機器人工作臂軌跡規劃方法，其特征在于，計算所述工作臂在相鄰路徑點之間的運動時間Δt_ik采用公式：

其中，k為路徑點序號；k∈[0，1，......，K-1]，S_k為工作臂到達第k個路徑點的軌跡，v_max為工作臂的速度約束，Δt_ik為關節i在路徑點k和k+1之間的運動時間。