[發(fā)明專利]一種基于改進蟻群算法的可碰撞檢測機械臂路徑規(guī)劃方法

在審

• 申請?zhí)枺?/td>: 202010301237.9
• 申請日: 2020-04-16
• 公開（公告）號: CN111347429A
• 公開（公告）日: 2020-06-30
• 發(fā)明（設(shè)計）人: 倪偉;葉棋偉;張粵;王子祥;徐敏;張鈺
• 申請（專利權(quán)）人: 淮陰工學院
• 主分類號: B25J9/16
• 分類號: B25J9/16
• 代理公司: 淮安市科文知識產(chǎn)權(quán)事務所 32223
• 代理人: 李鋒

1.一種基于改進蟻群算法的可碰撞檢測機械臂路徑規(guī)劃方法，其特征在于，包括如下步驟：

步驟1：建立機械臂模型，并根據(jù)建立的機械臂模型進行機械臂的運動學分析，通過修正施密特QR分解求逆方法完成空間機械臂運動學求逆解；

步驟2：考慮可選節(jié)點周圍障礙物的分布狀況，對蟻群算法的狀態(tài)轉(zhuǎn)移概率進行改進，對螞蟻k在t時刻由節(jié)點i到節(jié)點j的狀態(tài)轉(zhuǎn)移概率進行修正，修正后的計算公式為：

其中，γ表示障礙柵格狀況的權(quán)值，因為可選節(jié)點周圍障礙柵格狀況只是作為一個參考，不能作為主要考慮因素，故γ的值應在[0,1]內(nèi)取；τ_ij(t)表示各條路徑上殘留的信息量，η_ij表示路徑的啟發(fā)信息；allowed_k表示在t時刻螞蟻k下一步允許選擇的節(jié)點，α表示信息啟發(fā)式因子，反映蟻群在運動中殘留的信息量的相對重要程度，β表示期望啟發(fā)式因子，反映了期望值的相對重要程度，η_ij(t)表示t時刻節(jié)點i轉(zhuǎn)移到節(jié)點j的期望啟發(fā)函數(shù)，一般取節(jié)點i與節(jié)點j之間距離的倒數(shù)；j_ob表示節(jié)點j周圍的障礙物柵格數(shù)量，j_sum表示節(jié)點j周圍的柵格數(shù)量；

步驟3：采用長方體包絡法，進行機械臂與障礙物以及機械臂連桿之間的碰撞檢測；

步驟4：機械臂末端執(zhí)行器在工作空間內(nèi)搜索最優(yōu)路徑，進行關(guān)節(jié)轉(zhuǎn)角求解和碰撞檢測，實現(xiàn)路徑規(guī)劃。

2.根據(jù)權(quán)利要求1所述的基于改進蟻群算法的可碰撞檢測機械臂路徑規(guī)劃方法，其特征在于，所述步驟1的空間機械臂運動學求逆解具體方法為：

步驟1.1：根據(jù)D-H法，確定相鄰兩連桿i和i+1之間的變換矩陣：

其中，i表示桿件號，θ表示關(guān)節(jié)角，α表示扭角，a表示連桿長度，d表示連桿偏距；

步驟1.2：建立第i連桿在空間中的位姿矩陣：

步驟1.3：建立機械臂運動學方程：

其中，[n o a]為姿態(tài)矩陣，[P]為位置向量；

步驟1.4：針對步驟1.3中機械臂運動學方程，帶入已知的關(guān)節(jié)變量θ和連桿偏距d，即可獲得機械臂末端執(zhí)行器的位姿，記為基坐標；

步驟1.5：計算相對于基坐標的雅克比矩陣J的偽逆解J⁺：

J⁺＝J^T(JJ^T)^-1

步驟1.6：利用修正施密特QR分解方法計算(JJ^T)^-1，使上三角矩陣R的元素不是按列，而是按行計算；

步驟1.7：通過QR分解將JJ^T分解為一個正交矩陣和一個上三角矩陣，正交矩陣通過轉(zhuǎn)置得逆解，上三角矩陣通過初等變換得逆解：

JJ^T＝QR

(JJ^T)^-1＝R^-1Q^-1＝R^-1Q^T；

步驟1.8：通過修正施密特QR分解求逆方法得到(JJ^T)^-1后，再通過矩陣相乘從而求得J⁺，最后乘上末端速度得到關(guān)節(jié)角速度，完成空間機械臂運動學求逆解。