[發明專利]一種基于構型平面的水下機械臂運動學的求解方法

有效

• 申請號: 201510271086.6
• 申請日: 2015-05-25
• 公開（公告）號: CN104991448B
• 公開（公告）日: 2017-10-31
• 發明（設計）人: 魏延輝;王澤鵬;楊子揚;于媛媛;劉合偉;周星和;胡加興;賈獻強;高葦杭
• 申請（專利權）人: 哈爾濱工程大學
• 主分類號: G05B13/04
• 分類號: G05B13/04
• 代理公司: 暫無信息
• 代理人: 暫無信息

1.一種基于構型平面的水下機械臂運動學的求解方法，其特征是，包括如下步驟：

(1)輸入水下機械臂關節參數：輸入水下機械臂關節參數，進行機械臂關節分析，將已知的機器人工作構形按照關節模塊的形式進行分解，對機器人關節根據運動形式分解成相應的基本關節的形式輸入機器人目標點的位置矩陣；

(2)基本關節建模：對組成空間機械臂進行基本運動關節進行運動學建模，并進行歸一化處理，形成統一形式的建模方法：

(2.1)搖擺模塊建模

搖擺模塊運動模型矩陣：

式中β_yb—搖擺模塊兩部分旋轉相對角度；h_yb—搖擺中心到下一個模塊連接面的長度；

(2.2)移動模塊建模

移動模塊運動模型矩陣：

式中H_yd—移動模塊中心到下一模塊中心的長度；h_yd—移動模塊的移動量；

(2.3)回轉模塊建模

回轉模塊運動模型矩陣：

式中θ_hz—回轉模塊兩部分旋轉相對角度；l_hz—回轉中心到下一個模塊中心的長度；

(2.4)連接模塊建模

連接模塊運動模型矩陣：

式中H_lj—模塊長度；

(2.5)模塊的統一表達形式

基于前面各模塊的建模方法，建立可重構機器人模塊的統一表達方式：

式中θ為該模塊是回轉模塊時的回轉角度，若為其他模塊時為零；β為該模塊是搖擺模塊時的搖擺角度，若為其他模塊時為零；h為該模塊是搖擺模塊時的連接長度，若為其他模塊時為零；l為該模塊是回轉模塊或連接模塊時的連接長度，若為其他模塊時為零；w為該模塊是移動模塊時的移動量，若為其他模塊時為零；

(2.6)通過基本模塊的統一表達方式，在已知模塊類型的情況下，建立整個機器人運動學形式：

T(總)＝T₁*T₂…T_n；

(3)構形平面分解：

對水下機械臂進行構形平面分解，對多角度連接模塊進行處理，形成合理分解，將機械臂的位置和姿態進行有效分解；

機械臂關鍵關節中心位置求解、通過位置求解其中三個關節運動量、通過姿態求解剩余三個關節運動量、運動量校核、輸入控制器進行控制；

求解關鍵關節中心的三維空間位置坐標，以這些位置坐標和分解的兩個構形平面求解組成構形平面的關節廣義運動量；

求解關鍵關節中心點：

目標點位姿矩陣為：

2關節和5關節的中心位置

TS2＝[0 0 d1+d2]

將2關節的中心設為新的原點，5關節的中心設為目標點，則新的目標點的位置表示為TD1＝[p_x-d6*a_x p_y-d6*a_y p_z-d6*a_z-d1-d2]

分解的兩個構形平面模型表達式

構形平面簡化的轉化矩陣為：

利用位置關系求解幾個關節的廣義運動量

設p'_x＝p_x-d₆*a_x

p'_y＝p_y-d₆*a_y

p'_z＝p_z-d₆*a_z-d₁-d₂

TGX1*TGX2*[0 0 0 1]^T＝[p'_x p'_y p'_z 1]^T

可得：

利用三角函數關系，進行消元處理，得到如下公式：

令

得到僅有θ₄的等式：

經過處理可得到θ₃表達式；

進一步可計算得到θ₂，

則

(4)利用姿態關系求解剩余關節的廣義運動量

根據此式可求出θ₅，n'_zcθ₅-a'_zsθ₅＝a_z；

根據此式可求出θ₁，n'_xcθ₅-a'_xsθ₅＝a_xcθ₁+a_ysθ₁；

-n'_xsθ₅cθ₆+o'_xsθ₆-a'_xcθ₅cθ₆＝n_xcθ₁+n_ysθ₁，

θ₆，(-n'_xsθ₅-a'_xcθ₅)cθ₆+o'_xsθ₆＝n_xcθ₁+n_ysθ₁

根據已求解的三個關節廣義運動量，利用機械臂末端姿態要求，建立姿態關系等式，求解剩余運動關節的廣義運動量，并對該運動量進行校核；

將得到的水下機械臂的廣義運動量輸給到機器人控制器，進而實現機器人關節控制。