[發明專利]一種六自由度非球型手腕機器人逆運動學求解方法

在審

• 申請號: 202011009936.2
• 申請日: 2020-09-23
• 公開（公告）號: CN112276940A
• 公開（公告）日: 2021-01-29
• 發明（設計）人: 王旭浩;張大衛;趙臣;馬力;孟祥智;康立
• 申請（專利權）人: 天津大學
• 主分類號: B25J9/16
• 分類號: B25J9/16
• 代理公司: 天津市北洋有限責任專利代理事務所 12201
• 代理人: 杜文茹

1.一種六自由度非球型手腕機器人逆運動學求解方法，六自由度非球型手腕機器人是由7個依次連接的回轉關節組成，為了避免冗余的出現，在六自由度非球型手腕機器的第5回轉關節和第6回轉關節轉角之間設置了運動約束：θ₆＝-θ₅；其特征在于，逆運動學求解方法包括如下步驟：

1)根據D-H參數法建立所述機器人的連桿坐標系{X_i-Y_i-Z_i}；連桿坐標系{X_i-1-Y_i-1-Z_i-1}到連桿坐標系{X_i-Y_i-Z_i}的變換為：

(1)繞Z_i-1軸旋轉θ_i，使X_i-1軸與X_i軸平行；

(2)沿Z_i-1軸平移d_i，使X_i-1軸與X_i軸重合；

(3)繞X_i軸旋轉α_i，使Z_i-1軸與Z_i軸平行；

(4)沿X_i軸平移a_i，使Z_i-1軸與Z_i軸重合；

其中，θ_i、d_i、α_i、a_i分別稱為第i關節的關節轉角、關節偏距、連桿扭角和連桿長度，且有θ₆＝-θ₅；

給定六自由度非球型手腕機器的D-H參數，并定義連桿齊次變換矩陣為：

其中，變量s_i＝sinθ_i，c_i＝cosθ_i，c_αi＝cosα_i，s_αi＝sinα_i；基于所述連桿齊次變換矩陣，定義六自由度非球型手腕機器正向運動學為：

其中，表示六自由度非球型手腕機器人末端在基礎坐標系下的位姿矩陣；p＝(p_x,p_y,p_z)^T表示連桿坐標系{X_i-Y_i-Z_i}相對連桿坐標系{X_i-1-Y_i-1-Z_i-1}的位置向量，n＝(n_x,n_y,n_z)^T、o＝(o_x,o_y,o_z)^T、a＝(a_x,a_y,a_z)^T表示連桿坐標系{X_i-Y_i-Z_i}相對連桿坐標系{X_i-1-Y_i-1-Z_i-1}的姿態向量；

2)建立六自由度非球型手腕機器人與6R正交球型手腕機器人之間的等效變換，所述等效變換包括正向變換和逆向變換；

3)根據給定的六自由度非球型手腕機器末端位姿矩陣T_end，求解等效6R正交球型手腕機器人逆運動學解析解；

4)根據步驟3)求得的等效6R正交球型手腕機器人逆運動學解析解和步驟2)建立的六自由度非球型手腕機器人與等效6R正交球型手腕機器人之間的逆向變換，計算所述六自由度非球型手腕機器人逆運動學近似解；

5)以步驟4)求得的六自由度非球型手腕機器人逆運動學近似解為初始值，利用基于運動雅可比矩陣的數值迭代法，計算六自由度非球型手腕機器人逆運動學精確解，所述基于運動雅可比矩陣的數值迭代法的迭代公式為：

δθ＝(J^TJ)^-1J^Te_T，θ_i＝θ_i-1+δθ

其中，為機器人末端當前位姿與目標位姿之間的誤差向量,J為運動雅可比矩陣，δθ＝(δθ₁,δθ₂,δθ₃,δθ₄,δθ₅,δθ₇)^T為獨立關節轉角增向量。當關節轉角增量的絕對值小于給定閾值||δθ||≤ε或達到最大迭代次數，迭代結束，并輸出最終運動學逆解，ε表示關節轉角增量絕對值的閾值。