[發明專利]S-R-S結構七自由度機械臂逆運動學解析解的數值穩定算法

有效

• 申請號: 202110322567.0
• 申請日: 2021-03-25
• 公開（公告）號: CN113378349B
• 公開（公告）日: 2022-05-20
• 發明（設計）人: 王君臣;盧春姮;孫振
• 申請（專利權）人: 北京航空航天大學
• 主分類號: G06F30/20
• 分類號: G06F30/20;G06F119/14
• 代理公司: 北京知元同創知識產權代理事務所(普通合伙) 11535
• 代理人: 聶稻波

1.一種S-R-S結構七自由度機械臂逆運動學解析解的數值穩定算法，其特征在于，所述數值穩定算法包括：(1)正運動學求解、(2)參考平面求解、(3)逆運動學求解、(4)奇異臂角獲取、(5)關節限位及奇異值規避；

所述(3)逆運動學求解包括：

當給定參數ψ，即可進行逆運動學求解；其中θ₄與ψ無關，僅由末端位置和姿態確定，即θ₄＝θ₄'；真實⁰R₄即為參考平面⁰R₄'沿旋轉ψ角求得，

⁰R₄＝⁰R_ψ⁰R′₄

θ₄＝θ′₄

³R₄＝³R′₄

⁰R₃³R₄＝⁰R_ψ⁰R′₃³R′₄

⁰R₃＝⁰R_ψ⁰R′₃；

通過以上推導得到真實⁰R₃即為參考平面⁰R₃'沿旋轉ψ角求得，其中⁰R_ψ為相對向量旋轉ψ角形成的旋轉矩陣，由羅德里格斯公式得到：

其中I為3×3單位矩陣，因此⁰R₃可以表示為：

⁰R₃(ψ)＝sinψA_s+cosψB_s+C_s

其中：

同時，由于1-3關節為ZYZ球關節角，通過正運動學求得的公式為：

由⁰R₃(θ₁,θ₂,θ₃)＝⁰R₃(ψ)可得：

當θ₂≠0或±π時

其中GC₂同GC₄，取值范圍為{-1,1}，表明θ₂的正負，且：

當θ₂＝0或±π時

因此對應可求出θ₁～θ₃，θ₄＝θ₄'，之后便求解θ₅～θ₇；

同理，5-7關節也為ZYZ球關節角，因此同樣可對應求出θ₅～θ₇，即將上式完成如下替換即可：

θ₁←θ₅,θ₂←θ₆,θ₃←θ₇,s←w,GC₂←GC₆

其中：

GC₂、GC₄和GC₆均為姿態參數，取值為{-1,1}，分別表明θ₂、θ₄和θ₆的正負，臂角ψ為自運動冗余參數，至此完成逆運動學求解過程；

所述(4)奇異臂角獲取包括：

奇異臂角是機械臂局部自運動流形中的奇異位置，在球關節θ₂＝0或±π時出現；

描述球關節的奇異臂角的關鍵是找到使θ_i＝0或±π的ψ_s，肩部i＝2，腕部i＝6，此臂角ψ_s同時也是與其相關的兩個正切關節的奇異臂角；

余弦關節表達式為：

θ_i＝GC_iarccos(a sinψ+b cosψ+c)

θ_i相對ψ求導公式為：

由表達式可知θ_i＝0或±π對應于a sin(ψ)+b cos(ψ)+c＝±1，而a sin(ψ)+b cos(ψ)+c的值均位于±1間；即使得θ_i＝0或±π的ψ_s一定為a sin(ψ)+b cos(ψ)+c的極值；因此，通過使其導數為零可得兩個ψ_s值：

容易判斷求得和對應于θ_i的最大值或最小值；為了檢驗是否存在奇異臂角ψ_s，只需將i∈{1,2}與1進行比較；采用一種數值穩定的確定ψ_s的方法：

所述(5)關節限位及奇異值規避包括：

將每個關節i的極限范圍映射到冗余臂角ψ的范圍內，求取各關節可行臂角Ψ_i的交集即可求得最終冗余臂角范圍；用和來表示第i個關節的上下關節角極限；其中θ₄與ψ無關，其關節限位規避可直接由末端位姿求得θ₄后與關節限位對比判斷是否超限；

5.1余弦關節

余弦關節包括第2和6關節，給定θ_i，通過萬能公式求得兩個對應的ψ，ψ的計算公式為：

和對應于θ_i的最大值或最小值，記為θ_i^max和θ_i^min；下一步將分別為關節下限和上限找到對應的可行范圍Ψ_il和Ψ_iu，最終得到此關節避免關節限位后對應的可行臂角范圍：Ψ_i＝Ψ_il∩Ψ_iu；其基本思想是利用上式(2)求出極限關節角對應的ψ₁和ψ₂，并通過ψ₁和ψ₂點的導數的正負判斷ψ₁、ψ₂是可行區間的入口點還是離開點；若且ψ₁為關節上限通過式(2)所求得，則1為離開點；5.2正切關節

給定θ_i，通過萬能公式求得兩個對應的ψ，ψ的計算公式為：

其中

a_p＝(c_d-b_d)tanθ_i+b_n-c_n

b_p＝2(a_dtanθ_i-a_n)

c_p＝(b_d+c_d)tanθ_i-b_n-c_n

接下來將根據這三種情況來解決正切關節的關節極限映射問題；

(I)Δ0，為單調

Δ0時ψ-θ_i圖除±π間翻轉外整體保持單調；即與和均各有一個交點，這意味著根據式(3)計算的只有一個解是正確的；判斷具體為哪個解只需將計算結果反代回θ關于ψ的公式驗證即可；求解時當a_p＝0時，解由給出；設ψ_l和ψ_u分別為與下極限和上極限的交點；設ψ₁＝min(ψ_l,ψ_u)，ψ₂＝max(ψ_l,ψ_u)，關節極限內的可選臂角范圍為Ψ_i＝[ψ₁,ψ₂]或者Ψ_i＝[-π,ψ₁]∪[ψ₂,π]；具體為哪個范圍可通過計算ψ₁是進入點還是離開點判斷，或者通過觀察偏轉是否發生在ψ₁和ψ₂之間判斷；偏轉是否發生可通過計算是否在極限關節角范圍內進行判斷；(II)Δ0

ψ-θ_i圖有兩個局部極值，可通過式(1)求得，并通過二階導的正負判斷出局部極小值和極大值，并分別表示為和則設定之后通過求解關節極限與ψ-θ_i圖的交點，可得關節極限和對應的可選臂角區域和對于如果極限超出了由和組成的區域，則選臂角范圍或否則，則用和表示與ψ-θ_i的兩個交點，并設定則關節下極限對應的可選臂角范圍通過判斷處導數的符號，判斷為進入點或離開點，確定臂角范圍或關節上極限對應臂角范圍使用相同的規則獲得；最后，Ψ_i由還是求得取決于是否存在翻轉及θ_i^min、θ_i^max的相對位置；

(III)Δ＝0

如前所述，當Δ＝0時存在奇異臂角；在求解時設定一個閾值tol，令和為了避免奇異，在臂角范圍內排除范圍此時考慮如果ψ_s接近±π，則或可能從[-π,π]中脫落，所以最終得到的奇異臂角范圍在實際計算可行臂角范圍時應避開此奇異臂角范圍；

設且如果則輪廓為“N”型，否則為“H”型；之后計算思路與第二種Δ0情況相似，即求得關節極限與圖的交點，并為上下極限各求得一個可行區域，最終的可行區域按照判斷選擇兩個極限線區域的交集或并集；

5.3整體算法

至止求得了各關節的可行臂角范圍Ψ_i，則最終避免關節限位及奇異值的臂角范圍為對于任一末端位姿，于其姿態對應的可行臂角范圍內任意選出一冗余臂角即可進行數值穩定的逆運動學求解，實現S-R-S結構七自由度機械臂逆運動學解析解的獲得。