[發明專利]一種距離限制的工業機器人運動學參數估計方法

有效

• 申請號: 202210193809.5
• 申請日: 2022-03-01
• 公開（公告）號: CN114523474B
• 公開（公告）日: 2023-06-27
• 發明（設計）人: 盧榮勝;施文松
• 申請（專利權）人: 合肥工業大學
• 主分類號: B25J9/16
• 分類號: B25J9/16
• 代理公司: 安徽省合肥新安專利代理有限責任公司 34101
• 代理人: 陸麗莉;何梅生

1.一種工業機器人的運動學參數估計方法，是應用于工業機器人的標定過程中，并在所述工業機器人的末端裝載有位置測量裝置；其特征是，所述運動學參數估計方法是按如下步驟進行：

步驟1、在所述位置測量裝置的測量范圍內，建立工業機器人基坐標系O_B、機器人末端坐標系O_S，工具坐標系O_t，并將測量裝置坐標系設置為世界坐標系O_W；

步驟2、建立所述工業機器人的運動學參數模型；

步驟2.1、對于j軸驅動裝置輸出的n自由度工業機器人中的每一個軸建立一個關節坐標系，通過標準D-H參數建立相鄰第i個和第i+1個關節坐標系之間的位姿關系T_i；i∈[1,j-1]；

步驟2.2、由所述位姿關系T_i獲得機器人末端坐標系O_S相對于機器人基坐標系O_B的位姿關系^BT_S；

步驟3、工具坐標系O_t的標定；

步驟3.1、根據工業機器人的各關節之間的位姿關系與機器人末端坐標系O_S的關系，通過單軸旋轉法，得到機器人末端坐標系O_S的原點位置和機器人末端坐標系O_S的Z軸方向；

步驟3.2、以工業機器人末端執行器旋轉不同角度得到的兩點距離以及測量裝置得到相同兩點的距離誤差最小為目標函數，利用最小二乘法優化所述目標函數，從而得到工具坐標系O_t與機器人末端坐標系O_S的位姿關系記為^ST_t；

步驟4、建立工業機器人的運動學參數估計距離誤差模型；

步驟4.1、通過機器人末端坐標系O_S相對于機器人基坐標系O_B的位姿關系^BT_S和工具坐標系O_t相對于機器人末端坐標系O_S的位姿關系^ST_t，利用式(1)得到工具坐標系O_t相對于機器人基坐標系O_B的位姿關系^BT_t：

式(1)中，R_3×3為工具坐標系O_t相對機器人基坐標系O_B的旋轉矩陣；p_x,p_y,p_z分別表示工具坐標系O_t相對機器人基坐標系O_B在X軸、Y軸和Z軸的平移向量；

步驟4.2、當僅考慮位姿關系^BT_t中的位置向量p＝(p_x,p_y,p_z)^T，利用式(2)得到式(1)的化簡式：

p＝g(θ,η′)????(2)

式(2)中，θ表示工業機器人各軸旋轉角度，即為輸入變量，η′為待估計的運動學參數，即為待估計的常量；g表示通過θ和η′得到位置向量p的函數；

相對于機器人基坐標系O_B，工具坐標系O_t在工業機器人運動軌跡中的第n個位形的位置向量為P_n＝(p_x,n,p_y,n,p_z,n)^T＝g(θ_n,η′)；θ_n表示工業機器人第n個位形的各軸旋轉角度；

相對于測量裝置坐標系O_W，工具坐標系O_t在測量裝置中第n個位形的位置為P_n′＝(p′_x,n,p′_y,n,p_z′_,n)^T；

利用式(3)構造第n個位形的相對距離誤差函數f_n(θ_n,η′)：

f_n(θ_n,η′)＝||P_n-P_n-1|||₂-||P_n′-P_n′_-1||₂＝||g(θ_n,η′)-g(θ_n-1,η′)||₂-||P_n′-P_n′_-1||₂(3)

式(3)中，P_n-1表示相對于機器人基坐標系O_B，工具坐標系O_t在工業機器人運動軌跡中的第n-1個位形的位置向量，P_n′_-1表示相對于測量裝置坐標系O_W，工具坐標系O_t在測量裝置中第n-1個位形的位置，θ_n-1表示工業機器人第n-1個位形的各軸旋轉角度；

步驟4.3、根據相對距離誤差函數f_n(θ_n,η′)構造誤差函數e(η′)＝[f₁(θ₁,η′),f₂(θ₂,η′),…,f_n(θ_n,η′),…,f_m(θ_m,η′)]^T，m表示測量數據的組數，從而利用式(4)構造最小二乘目標函數E(η′)：

步驟5、在估計運動學參數η′中，去除對最小二乘目標函數E(η′)沒有影響的冗余參數，得到有效的估計參數η；

步驟6、利用Dog-Leg算法估計工業機器人的運動學參數；

定義k表示迭代次數，并令k＝0；定義最大迭代次數為kmax；

初始化第k次迭代的信賴域半徑為Δ_k，將機器人理論D-H參數作為第k次迭代有效的估計參數η_k，設定三個控制誤差為e₁,e₂,e₃；

步驟6.1、計算第k次迭代的第n個位形的相對距離誤差函數f_n(θ_n,η_k)對應的雅可比矩陣J_n，對于m組數據，由J_n構造第k次迭代的目標函數E(η_k)的雅可比矩陣J(η_k)＝[J₁?J₂…J_n…J_m]^T，從而構造第k次迭代Gauss-Newton法的下降方向和最速下降法的下降方向

步驟6.2、如果則轉到步驟6.7，否則根據η_k求解第k次迭代的誤差函數e(η_k)，如果||e(η_k)||₂≤e₃或者第k次迭代的信賴域半徑Δ_k≤e₂(||η_k||+e₂)，則轉到步驟6.7，否則執行步驟6.3；

步驟6.3、當使用為目標函數E(η_k)的下降方向時，計算第k次迭代沿最速下降法下降方向移動的步長λ_k和比例參數β_k，從而利用式(5)計算Dog-Leg算法的下降方向

如果則轉到步驟6.7，否則執行步驟6.4；

步驟6.4、令新的運動學參數計算增益比ρ，若ρ＞0，則更新第k+1次迭代有效的估計參數η_k+1＝η_new；否則，更新第k+1次迭代有效的估計參數η_k+1＝η_k；

步驟6.5、利用式(6)更新第k次迭代的信賴域半徑Δ_k，從而得到第k+1次迭代的信賴域半徑Δ_k+1：

式(6)中，σ表示所設定的閾值；

步驟6.6、令k+1賦值給k后，判斷k＜kmax是否成立，若成立，則轉到步驟6.1，否則，執行步驟6.7；

步驟6.7、輸出第k次迭代有效的估計參數η_k即為所需要的D-H參數估計值。