[發(fā)明專利]一種錨桿支護機器人工作臂軌跡規(guī)劃方法及系統(tǒng)

有效

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

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

技術(shù)領(lǐng)域

本發(fā)明涉及錨桿支護機器人自動錨桿支護技術(shù)領(lǐng)域，特別是涉及一種錨桿支護機器人工作臂軌跡規(guī)劃方法及系統(tǒng)。

背景技術(shù)

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

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

發(fā)明內(nèi)容

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

為實現(xiàn)上述目的，本發(fā)明提供了如下方案：

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

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

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

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

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

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

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

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

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