一种新的六自由度机械臂运动学反解方法研究.pdf

Computer Engineering and Applications计算机工程与应用2013， 49 （22） 1引言 机械臂的运动学分析是开展机械臂研究的基础， 利用 D-H1-2法建立模块化机械臂的运动学模型已经成为解决机 械臂运动学问题的标准方法， 主要包括运动学正解和反解 两个部分。其中反解部分是通过已知机械臂末端位姿， 求 解机械臂各个关节角度的过程， 计算量较大， 难度较高。 常见的反解方法有解析法、 几何法、 迭代法3-4等。在求解 机械臂运动学逆解问题中， 迭代法受到初始值选取的约 束， 只能求得一组解； 解析法计算较为复杂， 但可以得到全 部根； 几何法针对机械臂的某些特殊结构进行简化， 再进 行求解， 一般无法单独使用甚至根本无法使用。对于大多 数机械臂而言， 一般都是采用解析法求得全部封闭解， 再 采用其他方法选取其中一组较优的可行解。Pieper证明了 3 个连续关节轴交于一点的 6R 机械臂逆运动学可解 5； Duffy证明了3个连续关节轴平行的6R机械臂逆运动学可 解6， 称这两种结构的机械臂为构型机械臂， 本文研究的机 械臂不具有这种结构， 无法通过解析法对问题进行求解。 为了解决上述问题， 本文采用能量优化的遗传算法对 问题进行分析、 解决。在对机械臂进行运动学分析的基础 上， 采用遗传算法将各个关节的角度进行编码， 以能量最 优作为适应度评价标准， 实现了非构型机械臂的运动学反 解， 最后通过物理实验验证了结果的正确性。 2机械臂相关模型建立 2.1机械臂运动学模型 本文采用自主研发的 AS-MRobotEII型 6自由度机械 臂作为研究对象。采用标准 D-H 法建立机械臂运动学模 型， 并确定各参数， 结果如下图1和表1所示。 一种新的六自由度机械臂运动学反解方法研究 王俊龙， 张国良， 敬斌， 徐君 WANG Junlong, ZHANG Guoliang, JING Bin, XU Jun 第二炮兵工程大学 301教研室， 西安 710025 Teaching and Research Office 301, The Second Artillery Engineering University, Xi an 710025, China WANG Junlong, ZHANG Guoliang, JING Bin, et al. Research on way of inverse kinematic solution of six DOF manipula- tor. Computer Engineering and Applications, 2013, 49 （22） ： 266-270. Abstract：Using parse method to solve the problem of inverse kinematic solution only suits the manipulators with specifically structure. Aiming at the limitation, this paper proposes the GA （Genetic Algorithms）with optimization on energy consuming to get the solution. In order to the convenience on calculation, proposes the four-link simplified module based on the kinematic ana- lytic of manipulator. Under the limitation of the upper limit and the lower limit, use the real number to code the comparative chromosome. Use the change of potential energy and the distance between the end of manipulator and the expected position to calculate the adaptation. Use the way of probability to realize the selection, crossover and mutation. The results show that the proposed method is valid on inverse kinematic solution of manipulator. Key words： Genetic Algorithm; optimization on energy; manipulator; inverse kinematic solution 摘要： 针对运用解析法对机械臂进行运动学反解仅适用于具有特定结构机械臂的不足， 提出能量优化的遗传算法进行 机械臂运动学反解的方法。为便于能量的量化计算， 在进行机械臂运动学分析的基础上， 建立四连杆简化模型。采用实 数编码方式， 以机械臂各关节角上下限为依据， 建立与之相对应的染色体。以机械臂各连杆势能变化及机械臂末端与期 望值之差作为适应度函数， 评估个体的适应度。通过概率的方法实现选择、 交叉和变异操作。实验结果证明了该方法的 正确性。 关键词： 遗传算法； 能量优化； 机械臂； 运动学反解 文献标志码： A中图分类号： TP242doi： 10.3778/j.issn.1002-8331.1202-0308 作者简介： 王俊龙 （1987） ， 男， 硕士， 主要研究领域为先进控制理论及应用、 机械臂路径规划与运动控制； 张国良 （1970） ， 男， 博士， 教 授， 主要研究领域为先进控制理论及应用； 敬斌 （1976） ， 男， 讲师， 主要研究领域为多传感器信息融合、 模式识别； 徐君 （1986） ， 男， 硕士， 主要研究领域为先进控制理论及应用、 机器人SLAM数据关联问题。E-mail： junlong87 收稿日期： 2012-02-17修回日期： 2012-04-26文章编号： 1002-8331 （2013） 22-0266-05 CNKI出版日期： 2012-06-15 266 2013， 49 （22） 2.2机械臂4连杆简化模型 为实现能量的量化计算， 建立机械臂四连杆简化模 型。分析机械臂的结构， 模型如图 3所示。定义机械臂末 端为关节7。 基座、 关节1、 关节2构成一条线段， 定义为直线1。 关节2、 关节3、 关节4构成一条线段， 定义为直线2。 关节4、 关节5构成一条线段， 定义为直线3。 关节5、 关节6、 关节7构成一条线段， 定义为直线4。 通过机械臂运动学模型， 可由机械臂当前关节角组合 S=(i1i2i6)求解出基座、 关节 2、 关节 4、 关节 5和关 节 7的空间位置， 进一步求解出关节角变化所导致的直线 2、 3、 4克服重力势能所作的功。方法如下： 基座： P0= p0x p0y p0z = 0 0 0 （1） 关节2： P2 1 = p2x p2y p2z 1 = RT 0(0) 0T 1(i1) 0 0 0 1 = 0 0 282.5 1 （2） 关节4： P4 1 = p4x p4y p4z 1 = RT 0(0) 0T 1(i1) 1T 2(i2) 2T 3(i3) 0 0 0 1 （3） 关节5： P5 1 = p5x p5y p5z 1 = RT 0(0) 0T 1(i1) 1T 2(i2) 2T 3(i3) 3T 4(i4) 0 0 0 1 （4） 关节7： P7 1 = p7x p7y p7z 1 = RT 0(0) 0T 1(i1) 1T 2(i2) 2T 3(i3) 3T 4(i4) 4T 5(i5) 5T 6(i6) 0 0 0 1 （5） 上式中，pix,piy,piz分别表示关节i在世界坐标系下x,y,z 三个轴向上的分量， 即是世界坐标系下的三维直角坐标。 3基于遗传算法的机械臂反解算法 运用遗传算法7-8进行机械臂运动学反解， 其基本过程 包括初始种群的创建、 计算个体的适应度、 选择、 交叉、 变 异、 产生新的种群等过程。 3.1遗传基因编码 本文采用实数编码方式对个体进行编码， 对机械臂的 关节 R 1 2 3 4 5 6 / （） n 1 2 3 4 5 6 初始值/ （） 0 0 180 0 90 90 0 d/mm d0 d1 d2 d3 d4 d5 d6 d/mm 113 169.5 0 148.5+186.5 0 0 （200+163） d/mm 0 0 0 0 169.5 0 0 / （） 0 90 90 90 180 90 180 表1机械臂D-H模型参数 zL yL x4 xL z4 x5 x3 z5 x2 x1 z3 z2 z1 z0 y0 x0 zR xR yR d0 d1 d3 a4 d6 末端执行器 基座 图1机械臂D-H模型图2机械臂实物图 O 直线4 直线3 直线2 直线1 关节7 关节6 关节5 关节4 关节3 关节2 关节1 基座 图3机械臂4连杆简化模型 x y O 王俊龙， 张国良， 敬斌， 等： 一种新的六自由度机械臂运动学反解方法研究 267 Computer Engineering and Applications计算机工程与应用2013， 49 （22） 关节角度编码必须考虑各关节的实际运动上下限。表2列 出了AS-MRobotEII型机械臂各关节转动的范围。 机械臂由 6关节组成， 因而应当设定基因中染色体个 数为6。 在实际应用中， 由于工作环境等各方面的原因， 机械 臂各关节不能够达到其转角的上下限， 在进行运算时必须 考虑实际情况。设某一个关节的关节角上限和下限分别 为AupAlow， 将染色体i表示为： Ai= Ailow+ randi Rmax ( Aiup- Ailow) Rmax= 32 767（6） 其中Rmax=32 767为随机发生函数的最大值， 此时关节角i 的分辨率为 Aiup-Ailow Rmax 。 3.2能量优化的适应度函数 遗传算法是一种借鉴生物界自然选择与遗传机理的 高度并行、 随机的自适应的搜索算法。使用遗传算法， 首 先应当找到合理的适应度函数。由于机械臂多采用电池 供电， 从实际出发， 采用能量消耗作为适应度函数。在实 际的应用中， 由于各个关节之间的连接方式不同， 各关节 转动相同的角度所消耗的能量不同。 运用遗传算法进行机械臂的反解， 首先必须保证所求 得的关节角度值能够使机械臂末端达到期望的位置， 在此 前提下， 使机械臂能量消耗最低。 由 1.2节机械臂四连杆简化模型可得， 机械臂运动过 程中主要克服直线2、 3、 4运动带来的势能变化。将问题做 简化处理， 认为机械臂是质量分布均匀的刚体， 只考虑各 个连杆的质心位置的变化。取直线 2、 3、 4 的重量分别为 G2G3G4， 其中各个参数值可通过机械臂手册查得。 可通过前文方法， 求解得到各个直线在初始状态和当 前状态下的位置， 通过直线质心位置的变化， 可得到每个 最终状态下与初始状态下的势能变化， 设直线 2、 3、 4的位 置变化为Dl2Dl3Dl4， 可求解得到最终能量的变化为： DE = G2 Dl2+ G3 Dl3+ G4 Dl4（7） 通过机械臂运动学模型， 由式 （5） 可求解得到机械臂在 当前的关节末端位置与期望位置在三个坐标轴方向的差 值exeyez， 取e=ex2+ey2+ez2为两者之间插值判断标准。 由差值e与能量变化DE共同组成的适应度函数可表示为： Fit= - DE - e（8） 为了使结果能够快速收敛， 在适应度函数上采用罚函 数， 即设定机械臂当前关节末端与期望末端差的一个阈值 emax， 当机械臂末端与期望值之间插值小于该值时， 按照上 述公式计算； 当差值大于该值时有： Fit= -DE - 10 000 e 此时可得适应度度函数表达式为： Fit= -DE - e e Fit(i)， 则将个体i选择复制到下一代。 基因的交叉也称为基因重组， 方法是随机选择两个个体 的部分结构交替， 产生新的个体。方法是产生两个随机数 AB， 若A = B则重新产生AB， 然后利用随机数的方法， 随机产生一个数16之间的数值Point， 将AB中(Point6) 的染色体交换， 实现基因的重组。 基因的变异是子代变量的一部分以较小的概率自主 发生改变， 产生新的个体。设发生变异的概率为Pmutation， 进行变异操作方法是首先通过随机方法产生随机数p， 当 p Pmutation时， 随机发生改变。 3.4算法流程 进行机械臂反解的算法流程如图4所示。 关节 上限 下限 1 160 160 2 160 160 3 160 160 4 160 160 5 150 150 6 100 100 表2机械臂各关节转动上下限图 （） 关节 重量/kg 1 3.5 2 3.5 3 2.7 4 2.7 5 4.4 6 1.0 表3机械臂各个关节重量表 否 开始 GEN = 0 产生初始种群 计算每个个体适应度 是否满足 结束条件 保存最佳适应度个体信息 循环变量i= 0 i= = M? 依据概率选择操作 变异 选择一个个体变异 交叉 选择两个个体交叉 将新个体插入新种群 i= i+ 1 结束 保存结果并显示 产生新的种群 GEN = GEN + 1 是 否 是 图4算法流程图 268 2013， 49 （22） 算法的过程如下： 步骤 1 产生初始种群， 通过编码规则， 随机产生一个 初始种群。 步骤2 计算种群中每个个体的适应度， 判断结果是否 满足结束条件， 若满足条件， 结束运算并将计算的代数、 最 佳个体基因的信息保存并显示； 否则将适应度最高的个体 保存， 进行下一个步遗传操作。 步骤3 从第一个个体开始对每个个体用3.3所述的方 法， 进行遗传选择、 交叉和变异的操作， 判断是否计算完成 所有个体。 步骤4 更新种群， 将代数加1， 判断是否达到了结束条 件。结束条件首先判断机械臂末端位置与期望值的差是 否满足要求， 其次判断是否达到了最大运算代数。 4实验验证 为了验证该方法的正确性， 在物理平台上， 对上述结 果进行了验证。 设置遗传算法的基本参数如下： 种群数量M =300 终止代数T=5 000 交叉概率Pxover=0.9 变异概率Pmutation=0.3 机械臂的初始位置为S =(000000) 末端期望位置为机械臂可达空间内的一个随机点， 设 置为P =(540.2362.3487.4)， 单位为mm。 设置末端误差上限为emax=100， 即三个方向上误差平 方和不大于100 mm2。 如图 5所示， 遗传算法经过 1 024代运算， 反解得到机 械臂各个关节角度理论组合S =(41.40 -19.54 - 8.42 85.776.32-8.49)， 此时在XYZ方向上的理论误差分 别为： Ex= -5.56Ey= 3.33Ez= 5.20 E = E 2 x + E 2 y + E 2 z = 69.24 （10） 在实际工作中， 由于机械制造、 数据误差等各种原因， 机械臂并不能够到达理论角度。实时采集机械臂运动角 如图6所示。 机械臂最终姿态如图7所示。 通过机械臂运动学正解， 实时解算得到机械臂末端实 际位置与期待值之差如图8所示。 机械臂最终在(XYZ)方向上的误差分别是(-4.287 4.3233.109) (mm)， 这在实际应用中是完全允许的。 5结论 本文通过能量优化控制， 改进遗传算法的适应度函 数， 在保证机械臂末端能够到达期望位置的前提下， 使能 量消耗最少。实验结果验证了该方法的正确性， 在输入一 个预期的位置后， 能够有效地求解出一组关节值， 实现了 机械臂的运动学反解， 克服了非构型机械臂不能够运用解 析法进行运动学反解的不足。同时， 由于遗传算法本身属 于随机算法， 该算法存在计算代数上不确定、 运算效率不 图5软件界面操作界面图 关节1 关节2 关节3 关节4 关节5 关节6 100 80 60 40 20 0 20 10203040506070 机械臂关节位置/ （） 运行步骤/步数 0 图6机械臂关节运动轨迹图 图7机械臂终状态图 800 600 400 200 0 200 400 600 X方向误差 Y方向误差 Z方向误差 末端位置差/mm 10203040506070 运行步骤/步数 0 图8机械臂末端位置与期望值差值图 王俊龙， 张国良， 敬斌， 等： 一种新的六自由度机械臂运动学反解方法研究 269 Computer Engineering and Applications计算机工程与应用2013， 49 （22） 稳定的缺陷， 经过多次实验， 平均需要进行8001 300代循 环才能够得到结果， 在下一步的工作中， 将通过改进遗传 算法的方法， 提高方法的收敛速度， 使结果更加完善。 参考文献： 1 李宏庆.基于视觉的六自由机械臂控制技术研究D.南京： 南 京理工大学， 2009. 2 姜宏超， 刘士荣， 张波.六自由度模块化机械臂的逆运动学分 析J.浙江大学学报： 工学版， 2010 （7） :1348-1354. 3 Paulr P， Shimanob E， Mayer G.Kinematics control equations for simple manipulatorsJIEEE Transactions on Systems， Man and Cybernetics， 1981， 11 （6） ： 449-445. 4 Fuk S， Gonzalezr C， Leecg S. Robotics： control， sensing， vision and intelligenceM.NewYork： McGraw-Hill， 1987： 1-11. 5 Pieper D L.The kinematics of manipulators under computer controlD.California： Stanford University， 1968. 6 Duffy J.Analysis of mechanisms and robot manipulatorsM. London： Edward Arnold， 1980： 369-409. 7 王小平， 曹立明.遗传算法理论应用于软件实现M.西安： 西安交通大学出版社， 2005. 8 蔡自兴， 徐光祐.人工智能及其应用M.北京： 清华大学出版 社， 2004. 7 Zou A M， Zeng G H， Min T， et al.Tracking control of a mobile robot with kinematic uncertainty using neural networksC/The 13th International Conference on Neural Informational Pro- cessing.Berlin， Germany： Springer-Verlag， 2006， 4234： 721-730. 8 Cheah C， Kawamura S， Arimoto S， et al.Htuning for task- space feedback control of robot with uncertain jacobian ma- trixJ.IEEETransonAutomaticControl，2001，46（8）： 1313-1318. 9 吴忻生， 邓军.末端有未知扰动的分布参数柔性机械臂的鲁棒 边界控制J.控制理论与应用， 2011， 28 （4） ： 511-518. 10 Yang K J， Hong K S， Matsuno F.Energy-based control of axi- ally translating beams： varying tension， varying speed， and disturbance adaptationJ.IEEE Transactions on Control Sys- tems Technology， 2005， 13 （6） ： 1045-1054. 11 Wang D， Vidyasagar M.Transfer function for a single flexible linkC/Proceedings of the IEEE Internet Conference on Robot Automation.Scottsdale： s.n.， 1989： 1042-1047. 12 Jiang Z P， Nijmeuger H.Tracking control of mobile robots： a case study in backsteppingJ.Automatica， 1997， 33（7）： 1393-1399. 13 陈丽， 陈卫东， 王洪瑞.改进的机器人神经网络变结构混合控 制J.系统工程与电子技术， 2006， 28 （3） ： 429-433. 14 Young K D.Controller design for a manipulator using theory of variable structure theoryJ.IEEE Trans on System Man and Cybernetics， 1978， 8 （1） ： 101-109. 15 Y