[發明專利]一種手腕偏置型6軸機器人實時逆解算法

有效

• 申請號: 202010842411.0
• 申請日: 2020-08-20
• 公開（公告）號: CN111958602B
• 公開（公告）日: 2022-05-20
• 發明（設計）人: 宋寶;唐小琦;王國棟;周向東;熊爍;劉永興;鐘靖龍;肖千紅
• 申請（專利權）人: 華中科技大學
• 主分類號: B25J9/16
• 分類號: B25J9/16
• 代理公司: 北京金智普華知識產權代理有限公司 11401
• 代理人: 楊采良

1.一種手腕偏置型6軸機器人實時逆解算法，其特征在于，包括以下步驟：

S1：對機器人各關節建立坐標系，并得連桿參數；改進DH坐標系的連桿變換矩陣表達通式為：

代入DH參數并按照變換矩陣右乘規則，可得末端連桿坐標系{6}到基座標系{0}的變換矩陣如下：

S2：通過多次迭代、修正回歸系數，直至原模型的殘差平方和||r_n||小于誤差ε(ε0)；采用雅各比矩陣J的乘積J^TJ近似代替牛頓法中的二階漢森矩陣H，從而省略了復雜的二階矩陣運算；其基本迭代公式如下：

x_n+1＝x_n-(J^TJ)^-1J^Tr_n (3)

對于6R機器人，定義操作臂的速度雅各比矩陣J(θ)是指從關節速度矢量向操作速度矢量傳遞的廣義傳動比，即

由于速度可以認為是單位時間內的微分運動，上式等效為從關節空間dθ到笛卡爾空間的微分運動D的映射，即

D＝J(θ)dθ (5)

其中，D＝[dx dy dz δx δy δz]；J(θ)為微分變換所得的速度雅各比矩陣；將上式(3)代入式(5)，可得

θ_n+1＝θ_n-(J^TJ)^-1J^TD (6)

S3：為了解決矩陣奇異帶來的弊端，引入阻尼系數λ將對J^TJ的求逆替換為對(J^TJ+λI)的求逆，有效規避雅各比矩陣的奇異性、病態性問題；其中I為6階單位矩陣，λ∈(0，1]；

θ_n+1＝θ_n-(J^TJ+λI)^-1J^TD (7)

并且在每次迭代過程中，依據迭代殘差平方和e(殘差向量D的二范數||D||)的變化對系數λ進行調整；當e_ie_i-1時，說明迭代殘差收斂，則同樣令λ減小；反之，則殘差仍較大，增大λ以控制迭代步長快速收斂；

S4：為減少迭代運算量，在計算之前設定阻尼系數λ以加速收斂；并給定迭代次數上限imax與殘差精度e，建立終止條件以完成計算或避免迭代時間過長；

當迭代起點距離精確點較遠或不可靠時；

首先，需對運動學模型進行修偏；將J5、J6關節的軸向偏置置為0，即在DH參數基礎上更新d′₅＝0、d′₆＝0，并根據式(2)構建運動學模型

其次，為避免修正后的無偏置模型相較于偏置6R機器人的工作空間縮小；對于偏置6R機器人提供的目標點位姿T_end進行修偏；將目標點位中的J6軸向偏置進行修正，提高同構型無偏置模型與修正目標點位T′_end的工作空間重合度；修正公式如下

對于修正后滿足Pieper準則的與目標點T′_end，可采用解析方法完成求解；此處采用反變換法，得到對應該點位姿的8組初步關節空間反解；

此時引入關節屬性，對上述初步反解進行劃分；按照以奇異位型作為劃分邊界，分別采用前后、上下、俯仰3種邊界組合，將位型劃分為8組狀態；當機器人手腕中心點位于J1關節旋轉軸線上時，以J1和J2軸線組成的平面為前后邊界，分別定義平面兩邊為前、后屬性；當機械臂連桿2、連桿3平行時，以連桿2和J3關節軸線確定的平面為邊界，分別定義兩邊為上、下屬性；當J4、J6關節的軸線平行時，以J4、J5關節相交軸線確定的平面為界，分別定義兩側為俯、仰屬性；

當a2*c2+d4*s23≥0時，m置0表前屬性，反之置1為后屬性；θ3≤90°時，n置0表下屬性，反之置1為上屬性；θ5≤0°時，j置0表俯，反之置1為仰；

其中ci＝cos(θi)，si＝sin(θi)，s23＝sin(θ2+θ3)，c23＝cos(θ2+θ3)以此類推；定義變量

k31＝p′_xc1+p′_ys1，c5＝(a_xc1+a_ys1)s23-a_zc23，c4c5＝a_xs1-a_yc1，c4s5＝(a_xc1+a_ys1)c23+a_zs23，s23＝a₂c3p′_z+k31(a₂s3+d₄)，c23＝(-d₄+a₂s3)p′_z+k31·a₂c3；

通過使用mnj置位排列，建立起機器人位姿形態與動態編號間的關系；使用解析法反解時，按照提供的動態表編號即可唯一確定對應的初步反解角度θ₁′～θ₆′；再將該關節角θ₁′～θ₆′用作迭代起點，即完成了由輸入動態編號-＞轉為關節屬性-＞唯一確定形位-＞輸出迭代起點的動態表查詢步驟；

在迭代法快速收斂的基礎上，針對普通牛頓-拉夫森迭代法在求解過程中的迭代次數多、奇異位型無法求解的問題，使用改進的高斯-牛頓算法完成計算；并對于迭代算法依賴起始點靠近精確解的問題，引入動態表完成迭代起點的動態提供；在機器人的實際工業應用場合進行測試，驗證了該算法具有較高的實時性和準確性。