２０１４年２月． 机床与液压 ＭＡＣＨＩＮＥ ＴＯＯＬ＆ＨＹＤＲＡＵＬＩＣＳ Ｆｅｂ．２０１４ Ｖｏ１．４２ Ｎｏ．３ 第４２卷第３期 ＤＯＩ：１０．３９６９／ｊ．ｉｓｓｎ．１００１—３８８１．２０１４．０３．００８ 机器人运动学逆解及奇异和多解的处理 叶上高，刘电霆 （桂林理工大学机械与控制工程学院，广西桂林５４１００４） 摘要：针对后３个关节轴线相交于一点的６Ｒ工业机器人，提出一种有别于传统方法的位姿分离逆解算法，对逆解涉 及的奇异和多解处理也做了详细分析，并仿真验证了该算法的正确性。该算法完全避免了矩阵求逆的运算，因此比一般的 解析算法更加简单高效，便于实时控制。 关键词：工业机器人；逆运动学；奇异；多解 中图分类号：ＴＰ２４ 文献标识码：Ａ 文章编号：１００１—３８８１（２０１４）３—０２７—３ Ｒｏｂｏｔ Ｉｎｖｅｒｓｅ Ｋｉｎｅｍａｔｉｃｓ Ａｌｇｏｒｉｔｈｍ Ｉｎｃｌｕｄｉｎｇ Ｍｕｌｔｉ－Ｓｏｌｕｔｉｏｎｓ ａｎｄ Ｓｉｎｇｕｌａｒｉｔｙ Ａｎａｌｙｓｉｓ ＹＥ Ｓｈａｎｇｇａｏ，ＬＩＵ Ｄｉａｎｔｉｎｇ （Ｃｏｌｌｅｇｅ ｏｆ Ｍｅｃｈａｎｉｃａｌ ａｎｄ Ｃｏｎｔｒｏｌ Ｅｎｇｉｎｅｅｒｉｎｇ，Ｇｕｉｌｉｎ Ｕｎｉｖｅｒｓｉｔｙ ｏｆ Ｔｅｃｈｎｏｌｏｇｙ， Ｇｕｉｌｉｎ Ｇｕａｎｇｘｉ ５４１００４，Ｃｈｉｎａ） Ａｂｓｔｒａｃｔ：Ｂａｓｅｄ ｏｎ ｔｈｅ ｓｅｐａｒａｔｉｏｎ ｂｅｔｗｅｅｎ ｔｈｅ ｅｎｄ ｅｆｆｅｅｔｏｒ ｐｏｓｉｔｉｏｎ ａｎｄ ｐｏｓｔｕｒｅ，ａ ｎｏｖｅｌ ｉｎｖｅｒｓｅ ｋｉｎｅｍａｔｉｃｓ ａｌｇｏｒｉｔｈｍ ｗａｓ ｐｒｏ— ｐｏｓｅｄ ｆｏｒ ６Ｒ ｉｎｄｕｓｔｒｉａｌ ｒｏｂｏｔｓ，ｗｈｉｃｈ ｌａｓｔ ｔｈｒｅｅ ａｘｉｓ ｉｎｔｅｒｓｅｃｔｅｄ ａｔ ｏｎｅ ｐｏｉｎｔ．Ｍｏｒｅ ｏｖｅｒ，ｍｕｌｔｉ—ｓｏｌｕｔｉｏｎｓ ａｎｄ ｓｉｎｇｕｌａｒｉｔｙ ａｎａｌｙｓｉｓ ｗｅｒｅ ａｌｓｏ ｍａｄｅ．Ｔｈｅ ｓｉｍｕｌａｔｉｎｇ ｒｅｓｕｌｔ ｉｎｄｉｃａｔｅｓ ｔｈａｔ ｔｈｅ ｍｅｔｈｏｄ ｉｓ ｅｆｆｅｃｔｉｖｅ．Ｕｓｉｎｇ ｔｈｉｓ ａｌｇｏｒｉｔｈｍ，ｔｈｅ ｉｎｖｅｒｓｅ ｍａｔｒｉｘ ｉｓ ａｖｏｉｄｅｄ ｃｏｍｐｌｅｔｅｌｙ． Ｓｏ ｔｈｅ ａｌｇｏｒｉｔｈｍ ｉｓ ｓｉｍｐｌｅｒ ａｎｄ ｍｏｒｅ ｓｕｉｔａｂｌｅ ｆｏｒ ｒｏｂｏｔ ｒｅａｌ—ｔｉｍｅ ｃｏｎｔｒｏｌ ｃｏｍｐａｒｅｄ ｗｉｔｈ ｃｏｎｖｅｎｔｉｏｎａｌ ａｎａｌｙｔｉｃ ｍｅｔｈｏｄｓ． Ｋｅｙｗｏｒｄｓ：Ｉｎｄｕｓｔｒｉａｌ ｒｏｂｏｔ；Ｉｎｖｅｒｓｅ ｋｉｎｅｍａｔｉｃｓ；Ｍｕｌｔｉ—ｓｏｌｕｔｉｏｎｓ；Ｓｉｎｇｕｌａｒｉｔｙ 机器人运动学逆解，即根据工具坐标系相对于基 及的奇异和多解问题的处理也进行了详细分析。最 坐标系的目标位姿，求解机器人各关节角。逆运动学 后，用ＡＤＡＭＳ对该逆解算法的正确性进行了仿真验 在机器人学中占有非常重要的地位，是机器人轨迹规 证。 划和运动控制的基础，直接影响着控制的快速性与准 １运动学正解 确性 。一般机器人运动学逆解算法可分为以下几 ＩＲＢ２６００为６Ｒ机构，采用Ｄ—Ｈ方法 ，建立连 种 ：解析法（又称反变换法）、几何法和数值解 杆坐标系如图１所示。 法。由ＰＡＵＬ等提出的反变换法求解过程直观，因而 被广泛采纳，但其求解过程中需多次进行齐次变换矩 阵的逆运算和４×４维矩阵的乘积运算，导致求解过 程复杂耗时 。 大多数工业机器人的几何结构都满足Ｐｉｅｐｅｒ准 则 ，即３个相邻关节轴交于一点或相互平行，其运 动学逆解可以得到数量一定的若干组封闭解。对于最 后３个关节为旋转关节而且轴线相交于一点的六自由 度机器人，其前３个关节决定末端执行器的位置，后 ３个关节决定末端执行器的姿态 。故文中以其中的 典型代表——ＡＢＢ公司的ＩＲＢ２６００机器人为研究对 象，提出了一种有别于传统方法的位姿分离逆解算 法，根据末端执行器的位置矢量计算前３个关节变 量，利用末端执行器的旋转矩阵计算后３个关节角。 此算法逆解过程完全避免了矩阵求逆和４×４维矩阵 相乘的运算，过程直观，因此，比一般的解析算法更 图１ ＩＲＢ２６００机器人的连杆坐标系 加简单高效，便于实时控制。此外，文中对逆解所涉 收稿日期：２０１３—０３—０３ 基金项目：广西自然科学基金项目（２０１２ＧＸＮＳＦＡＡ０５３１９３） 作者简介：叶上高（１９８５一），男，硕士研究生，研究领域为工业机器人。Ｅ—ｍａｉｌ：４９１８６７９２＠ｑｑ．ｃｏｎ。 ・２８・ 机床与液压 第４２卷 ＩＲＢ２６００机器人的Ｄ．Ｈ参数如表１所示。 表１ ＩＲＢ２６００机器人的Ｄ—Ｈ参数 由此求得各连杆变换矩阵后连乘得机器人腕点的 齐次变换矩阵 Ｅｌｌ ｒ１２ Ｅｌ３ Ｗ Ｆ２１ Ｆ２２ ｒ２３ ｗｙ ｒ３１ ｒ３２ ｒ３３ ｗ： ０ ０ ０ １ 其中：ｗ ＝ｃｌ（ａｌ＋ａ３ｃ２３一ｄ４ｓ２３＋ａ２ｃ２） （２） ＝Ｓｌ（ａｌ＋ａ３ｃ２３一ｄ４ｓ２３＋ａ２ｃ２） （３） Ｗ ＝ｄｌ—ｄ４ ２３一ａ３ｓ２３一ａ２ｓ２ （４） ２运动学逆解算法 ：Ｔ＝； ＴＴｃ Ｔ （５） ２．１ 前３个关节角 求０ｌ。因ａｌ＋Ⅱ］ｃ２３一ｄ４ｓ２３＋Ⅱ２ｃ２正负未定，故由 式（２）、（３）求得０。有两个可能解 ０１Ａ＝ａｒｃｔａｎ２（Ｗ ，Ｗ ），０ｌＢ＝ａｒｃｔａｎ２（一Ｗ ，一Ｗ ） 求０ 。若ｃ．≠０，由式（２）、（４）整理得 ，一。 ｃ ＝ｎ ，一ｄｎ ，其中 。＝ 一Ⅱ （６） ｋ３＋ａ２ｓ２＝一ｄ４ｃ２３一Ⅱ３ｓ２３，其中ｋ３＝Ｗ 一ｄｌ （７） 将式（６）和（７）等号两边平方后取和，整理 得 一 。。：＋ ， ：： ，其中Ｋ： ＿． ＿＝ ＝ａｒｃｔａｎ２（ｋ ，ｋ３）一ａｒｃｔａｎ２（一Ｋ，±Ｊｋ＋ ；一 ） （８） 若Ｃ ＝０，为避免分母为０，则改用式（３）、（４） 用同样的方法求解。 求０，。若Ｃ ≠０，因０。和０ 已求得，则由式 （２）、（４）整理所得的式（６）、（７）两式等号左边 已知，即式（６）和（７）构成二元一次方程组，可 解得未知数为 ｄ４ ｚ１＋ｒ上３ Ⅱ３ ｚｌ— ４ 一ｄ４—０３ 口４十ｎ３ 其中：ｆｌ＝ｋｌ—ａ２ｃ２，ｆ３＝ｋ３＋０２ｓ２ 故０３＝Ａｔａｎ２（ ２３，ｃ２３）一０２。若ｃｌ＝０，为避免 ｒ ：ｒ ｔ ：Ｒ＝ Ｒ ：足＝ｌ ｒ 。ｒ ｒ Ｉ＝：Ｒ４ Ⅸ ５ ＝ Ｌ ｒｔ ３１ ｒ Ｐ３２ Ｆｔ３３Ｊ ［ｌＬ ５一 一ｃ４ｓ。ｃ６ ６ ｓ５ ｃ５４ ｃ６６—８４ —ｃ５。ｓ ４ｓ—：６ ８ 一５￥。 ｊ６ｃ４。ｃ５ ６６—’８ｃ４。５ｓ４５ ］Ｉ 关节 活动范围 关节１ —１８０。～＋１８０。 关节２ １８５。～＋６５。 关节３ 一ｌ８０。～ ＋７５。 关节４ 一４００。～Ｊ－４００。 关节５ 一１２０。～ １２０。 关节６ 一４００。～Ｊ－４００。
