Deriving the Inverse Solution Formula of 6R Robot with Non-spherical Wrist with Multi-cut Point Separation and Reconstruction Technique
-
摘要: 本文提出一种利用多切断点分解—重构技术求解非球型手腕机器人逆解新方法,简化了该类机器人逆运动学公式的推导过程。首先利用指数积公式的变形,说明了n自由度机器人的可分解性,以及重构连接的通用几何约束;然后以一款手腕前端偏置型机器人为例,给出了针对常见非球型手腕机器人的分解方法及重新连接的几何约束条件。以满足约束条件为目的,得到了只含θ6的非线性封闭方程,以及其它关节角关于θ6的求解公式,推导过程具有更直观的物理几何意义。最后使用二分法求解,算法约2 ms可得到目标位姿所有解,计算位姿误差在10−12 mm以内。Abstract: To simplify the derivation of the inverse solution formula of the 6R robot, firstly, the decompositions of a n-degree-of-freedom robot and the general geometric constraints of its reconstructed joints are explained with the deformation of the product of exponential formula based on the screw theory. Then, taking a wrist front offset robot as an example, the decomposition method and the geometric constraints for the reconnection of common non-spherical wrist robots are given. In order to satisfy the constraint conditions, the nonlinear closed equation with only θ6 and the solution formula of other joint angles with θ6 are obtained. The derivation process is of more intuitively physical and geometric significance. Finally, the dichotomy method is used to obtain the solution. The algorithm can obtain all the solutions of the target pose at about 2 ms, and the calculated pose error is within 10−12 mm.
-
表 1 各关节螺旋轴参数
i ωi ri 1 [0 0 1]T [0 0 0]T 2 [0 −1 0]T [l2 0 l1]T 3 [0 −1 0]T [l2 0 l1+l3]T 4 [1 0 0]T [l2+l5 0 l1+l3+l4]T 5 [0 −1 0]T [l2+l5 0 l1+l3+l4]T 6 [0 0 −1]T [l2+l5+l6 0 l1+l3+l4-l7]T 表 2 目标位姿T的12组逆解
θ1 /rad θ2 /rad θ3 /rad θ4 /rad θ5 /rad θ6 /rad e /mm 2.25038889 −1.49034188 −1.33993387 0.00270934 −4.60171619 1.62595816 3.6×10−12 2.10476338 −1.51441021 −1.26985578 −2.04447081 1.42114528 3.61769904 1.1×10−12 2.25113961 −1.53222813 −1.24044842 3.13432347 1.51772173 4.77781001 8.3×10−12 2.39481362 −1.51464078 −1.26962076 2.04910934 1.42269348 5.91770968 1.9×10−12 2.24998619 1.60871108 −2.40742833 −0.00070966 −0.35008963 1.62824259 1.7×10−12 2.24989405 1.75768624 −2.50691536 3.14077780 −2.74201495 4.76972390 2.9×10−12 −0.73193189 −2.52178659 −1.07766202 1.78079767 −1.71897230 0.33580739 1.3×10−12 −0.89041575 −2.50228309 −1.11729876 3.13409508 −1.62679779 1.62148785 1.2×10−12 −1.05141999 −2.52193887 −1.07750491 −1.77807632 −1.72037572 2.92379962 3.6×10−12 −0.89084055 −2.54228100 −1.01474944 0.00490210 −1.57734789 4.77529433 1.1×10−12 −0.89169201 0.99331192 −2.63006233 3.14079817 0.35603520 1.62872788 8.3×10−12 −0.89160433 1.10799726 −2.73261364 −0.00071240 −3.50976187 4.77033592 1.9×10−12 -
[1] ZHAO R B, SHI Z P, GUAN Y, et al. Inverse kinematic solution of 6R robot manipulators based on screw theory and the Paden–Kahan subproblem[J]. International Journal of Advanced Robotic Systems, 2018, 15(6): 1-11 [2] PIEPER D L. The kinematics of manipulators under computer control[D]. Stanford: Stanford University, 1968 [3] XU J, SONG K C, HE Y, et al. Inverse kinematics for 6-DOF serial manipulators with offset or reduced wrists via a hierarchical iterative algorithm[J]. IEEE Access, 2018, 6: 52899-52910 doi: 10.1109/ACCESS.2018.2870332 [4] 付中涛, 杨文玉, 杨震, 等. 非球型腕部6R机器人实时高精度逆运动学算法[J]. 华中科技大学学报(自然科学版), 2013, 41(S1): 29-33FU Z T, YANG W Y, YANG Z, et al. Real-time and high-precision algorithm for inverse kinematics of the 6R robots with non-spherical wrist[J]. Journal of Huazhong University of Science & Technology (Natural Science Edition), 2013, 41(S1): 29-33 (in Chinese) [5] 李宁森, 周波, 张盼盼, 等. 非球型手腕6R机器人实时逆运动学算法[J]. 华中科技大学学报(自然科学版), 2015, 43(S1): 461-467LI N S, ZHOU B, ZHANG P P, et al. Real-time inverse kinematics algorithm for 6R robots with non-spherical wrist[J]. Journal of Huazhong University of Science & Technology (Natural Science Edition), 2015, 43(S1): 461-467 (in Chinese) [6] FU Z T, YANG W Y, YANG Z. Solution of inverse kinematics for 6R robot manipulators with offset wrist based on geometric algebra[J]. Journal of Mechanisms and Robotics, 2013, 5(3): 031010 doi: 10.1115/1.4024239 [7] 林阳, 赵欢, 丁汉. 基于多种群遗传算法的一般机器人逆运动学求解[J]. 机械工程学报, 2017, 53(3): 1-8 doi: 10.3901/JME.2017.03.001LIN Y, ZHAO H, DING H. Solution of inverse kinematics for general robot manipulators Based on multiple population genetic algorithm[J]. Journal of Mechanical Engineering, 2017, 53(3): 1-8 (in Chinese) doi: 10.3901/JME.2017.03.001 [8] HARGIS B E, DEMIRJIAN W A, POWELSON M W, et al. Investigation of neural-network-based inverse kinematics for a 6-DOF serial manipulator with non-spherical wrist[C]//Proceedings of the ASME 2018 International Design Engineering Technical Conferences and Computers and Information in Engineering Conference. Quebec City, Quebec, Canada: ASME, 2018 [9] 周枫林, 游雨龙, 李光. 空间3R机械臂逆向运动学的奇异轨迹线方法研究[J]. 机械科学与技术, 2019, 38(3): 365-372ZHOU F L, YOU Y L, LI G. A solving method for inverse kinematics of space 3R manipulator based on singular trajectory theory[J]. Mechanical Science and Technology for Aerospace Engineering, 2019, 38(3): 365-372 (in Chinese) [10] 章晓峰, 李光, 肖帆, 等. 基于BP神经网络的包装分拣机器人视觉标定算法[J]. 包装学报, 2019, 11(4): 74-81 doi: 10.3969/j.issn.1674-7100.2019.04.011ZHANG X F, LI G, XIAO F, et al. Calibration of packaging sorting robot based on BP neural network[J]. Packaging Journal, 2019, 11(4): 74-81 (in Chinese) doi: 10.3969/j.issn.1674-7100.2019.04.011 [11] 肖帆, 李光, 游雨龙. 空间3R机械手逆向运动学的多模块神经网络求解[J]. 中国机械工程, 2019, 30(10): 1233-1238 doi: 10.3969/j.issn.1004-132X.2019.10.014XIAO F, LI G, YOU Y L. Multiple module neural network solving for inverse kinematics of space 3R manipulators[J]. China Mechanical Engineering, 2019, 30(10): 1233-1238 (in Chinese) doi: 10.3969/j.issn.1004-132X.2019.10.014 [12] 肖帆, 李光, 杨加超, 等. 改进CMA-ES算法及其在7自由度仿人臂逆运动学求解中的应用[J]. 机械科学与技术, 2020, 39(6): 844-851XIAO F, LI G, YANG J C, et al. Applying improved CMA-ES algorithm to solve inverse kinematics of 7-DOF humanoid arm[J]. Mechanical Science and Technology for Aerospace Engineering, 2020, 39(6): 844-851 (in Chinese) [13] 李光, 肖帆, 杨加超, 等. 基于唯一域方法的机器人逆向运动学求解[J]. 农业机械学报, 2019, 50(10): 386-394 doi: 10.6041/j.issn.1000-1298.2019.10.045LI G, XIAO F, YANG J C, et al. Solution of inverse kinematics of robots based on unique domain method[J]. Transactions of the Chinese Society for Agricultural Machinery, 2019, 50(10): 386-394 (in Chinese) doi: 10.6041/j.issn.1000-1298.2019.10.045 [14] 刘志忠, 柳洪义, 罗忠, 等. 基于偏置补偿的6自由度腕部偏置机器人逆解算法[J]. 东北大学学报(自然科学版), 2012, 33(6): 870-874 doi: 10.12068/j.issn.1005-3026.2012.06.027LIU Z Z, LIU H Y, LUO Z, et al. Inverse kinematics algorithm for 6-DOF robots with offset wrist based on offset compensation[J]. Journal of Northeastern University (Natural Science), 2012, 33(6): 870-874 (in Chinese) doi: 10.12068/j.issn.1005-3026.2012.06.027 [15] 韩磊, 刁燕, 张希斌, 等. 基于改进牛顿迭代法的手腕偏置型六自由度关节机器人逆解算法[J]. 机械传动, 2017, 41(1): 127-130,150HAN L, DIAO Y, ZHANG X B, et al. Inverse kinematics algorithm for 6-dof joint robot with offset wrist based on modified newton iteration method[J]. Journal of Mechanical Transmission, 2017, 41(1): 127-130,150 (in Chinese) [16] WU M K, KUNG Y S, LEE F C, et al. Inverse kinematics of robot manipulators with offset wrist[C]//Proceedings of 2015 International Conference on Advanced Robotics and Intelligent Systems (ARIS). Taipei, China: IEEE, 2015: 1-6 [17] TRINH C, ZLATANOV D, ZOPPI M, et al. A geometrical approach to the inverse kinematics of 6r serial robots with offset wrists[C]//Proceedings of the ASME 2015 International Design Engineering Technical Conferences and Computers and Information in Engineering Conference. Boston, Massachusetts, USA: ASME, 2015 [18] PASHKEVICH A. Real-time inverse kinematics for robots with offset and reduced wrist[J]. Control Engineering Practice, 1997, 5(10): 1443-1450 doi: 10.1016/S0967-0661(97)00142-1 [19] KUCUCK S, BINGUL Z. The inverse kinematics solutions of fundamental robot manipulators with offset wrist[C]//Proceedings of the IEEE International Conference on Mechatronics, 2005. ICM'05. Taipei, China: IEEE, 2005: 197-202 [20] HUSTY M L, PFURNER M, SCHRÖCKER H P. A new and efficient algorithm for the inverse kinematics of a general serial 6R manipulator[J]. Mechanism and Machine Theory, 2007, 42(1): 66-81 doi: 10.1016/j.mechmachtheory.2006.02.001 [21] 卜王辉, 刘振宇, 谭建荣. 基于切断点自由度解耦的手腕偏置型6R机器人位置反解[J]. 机械工程学报, 2010, 46(21): 1-5 doi: 10.3901/JME.2010.21.001BU W H, LIU Z Y, TAN J R. Inverse displacement analysis of 6R robots with offset wrists based on decoupling degrees of freedom at the cutoff points[J]. Journal of Mechanical Engineering, 2010, 46(21): 1-5 (in Chinese) doi: 10.3901/JME.2010.21.001 [22] MANOCHA D, CANNY J F. Efficient inverse kinematics for general 6R manipulators[J]. IEEE Transactions on Robotics and Automation, 1994, 10(5): 648-657 doi: 10.1109/70.326569 [23] CHEN Q C, ZHU S Q, ZHANG X Q. Improved inverse kinematics algorithm using screw theory for a six-DOF robot manipulator[J]. International Journal of Advanced Robotic Systems, 2015, 12(10): 140 doi: 10.5772/60834 [24] LYNCH K M, PARK F C. Modern robotics[M]. Cambridge: Cambridge University Press, 2017 [25] 刘国峰, 廖烨, 李康, 等. 基于二分法改进的Chirp信号参数估计算法[J]. 光通信研究, 2020(2): 61-66LIU G F, LIAO Y, LI K, et al. Chirp signal parameter estimation algorithm based on dichotomy[J]. Study on Optical Communications, 2020(2): 61-66 (in Chinese)