机械臂逆运动控制模型与方法

机械臂逆运动控制模型与方法

作者：董芳 赵文 廖锋

来源：《价值工程》2014年第03期

摘要： 据机械臂关节轴线方向建立了连杆坐标系，采用Denavit-Hartenberg（D-H）法求出各关节之间的坐标系变换矩阵，利用机械臂关节间的联体关系建立了机械臂正运动学模型，通过反向求解正运动模型得到机械臂逆运动学模型。为了解决逆运动控制问题，利用种群平均信息，提出了改进的多种群差分演化算法。最后数值试验表明新算法可以有效求解机械臂逆运动学模型。

Abstract： The link bar coordinate system is established based on axis direction of joint axis. Then the coordinate system transformation matrix is derived according to Denavit-Hartenberg（D-H） notation， and the forward kinematics model is set up using the united relationship between joints. The inverse kinematics model is built by working out forward kinematics model in opposite direction. In order to solve the problem of inverse kinematics manipulation， an improved multiple population differential evolution based on the average information of current population is designed. The final numerical result indicates that the new algorithm could solve inverse kinematics model efficiently.

关键词： 机械臂；关节；自由度

Key words： manipulator；joint；degree of freedom

中图分类号：TP241 文献标识码：A 文章编号：1006-4311（2014）03-0035-03 0 引言

随着工业生产线自动化程度的提高，机械臂被广泛应用于喷涂、包装、焊接、装配等生产环节，甚至在极端环境中可以代替人完成高强度的重复或危险的任务，总之机械臂控制技术的进步对提高劳动生产率，实现工业生产自动化具重要意义。

机械臂的正、逆运动问题是机械臂运动学研究领域的基本问题[1]，各类机械臂逆运动是机械臂运动学领域的研究热点[2，3，4]。由于机械臂各关节角度间复杂的耦合关系，解析法求解三角方程时候可能有增根，此时需要根据机械臂的结构特点确定位姿，所以不满足实时控制的要求[5]。此外机械臂各关节角度与机械臂末端位置并非一一对应关系，所以传统解析法很难得到单一的优化解[6]，此时需要从众多优化解中人为判断选择，人工干预因素较多。 近年来群智能优化算法已经成为解决机械臂逆运动问题的新方法[7，8]，较传统逆运动控制方法，群智能优化算法具有如下优势：不需要反向求解三角不等式，求解精度高，求解速度快等特点。智能优化算法种类众多且各有优势，但从算法的综合性能来看差分演化算法

龙源期刊网 http://www.qikan.com.cn

（differential evolution-DE）[9]效率最高、运算速度最快，由于其独特的杂交和变异机制使得具有很强的全局搜索能力，得到广泛应用[9-14]。本文通过分析机械臂连杆结构特征，构建了新的机械臂逆运动模型，该模型为约束优化模型；为了求解该模型，提出了基于种群平均信息的新差分演化算法。新算法能够以较高精度实时对机械臂进行控制，得到比较满意的控制效果。

1 机械臂正运动模型建立

六自由度机械臂示意图见图1，根据其结构特征，机械臂初始姿态见图2，其中AB‖GH，BC‖DE‖FG，CD‖EF且AB⊥BC，BC⊥CD。

由于各关节角度改变时，机械臂末端点H的坐标会改变，为了定量分析各关节角度与点H坐标的关系，运用Denavit-Hartenberg（D-H）法[1]，建立如图3所示的连杆坐标系oixiyizi（i=0，1，…，6），原点oi（i=0，1，…，6）分别位于点A～H，o0x0y0z0表示基坐标系，o6x6y6z6表示机械臂末端坐标系。由图3知：oi-1xi-1yi-1zi-1经过两次旋转和两次平移变换到oixiyizi，这四次变换分别为：第一次：oixiyizi绕Ji旋转θi时，oi-1xi-1yi-1zi-1需要绕zi-1轴旋转γi才能使新的xi-1轴与xi轴同向；第二次：沿新的zi-1轴平移li，使得新的oi-1移动到Ji与Ji+1的公垂线与Ji交点处；第三次：沿xi轴平移di使得新的oi-1与oi重合；第四次：绕新的xi轴旋转αi，使得新的zi-1与zi轴同向。 上述四次变换可用四个齐次变换矩阵实现：

i-1Ti=Rot（zi，γi）Trans（0，0，li）Trans（di，0，0）Rot（xi，αi）（1） 其中Rot（zi，γi）=cosγi -sinγi 0 0sinγi cosγi 0 0 0 0 1 0 0 0 0 1， Trans（0，0，li）=1 0 0 00 1 0 00 0 1 l■0 0 0 1， Trans（di，0，0）=1 0 0 di0 1 0 00 0 1 00 0 0 1，

Rot（xi，αi）=1 0 0 00 cosαi -sinαi 00 sinαi cosαi 00 0 0 1，连杆参数设定情况见表1，式（1）中i-1Ti表示oixiyizi对oi-1xi-1yi-1zi-1的变换 矩阵。

由于机械臂各关节的连杆坐标系具联动关系，所以o6x6y6z6对o0x0y0z0的变换矩阵为： 0T6=0T11T22T33T44T55T6=nx ox ax pxny oy ay pynz oz az pz 0 0 0 1■■ ■ ■ ■0 0 0 1（2）

龙源期刊网 http://www.qikan.com.cn

式（2）称为机械臂正运动模型，其中■，■，■分别表示o6x6y6z6的三个坐标轴x6，y6，z6在基坐标系中的方向矢量，向量■表示o6x6y6z6的坐标原点（机械臂末端点H）在基坐标系中的位置。

2 机械臂逆运动模型

由于机械臂末端旋转角度θ6对指尖H在基坐标系中的位置没有影响，所以只有（θ1，θ2，θ3，θ4，θ5）对指尖H的位置有影响，（θ1，θ2，θ3，θ4，θ5）与H在基坐标系中的位置有确定的对应关系，只要给出一组关节角组合，H就可以运动到对应的位置上。 机械臂路逆运动控制指对于基坐标系中某点X0=（x0，y0，z0）T，如何调整关节角（θ1，θ2，θ3，θ4，θ5）使得机械臂指尖H刚好运动到X0。此时向量■与X0=（x0，y0，z0）T的欧氏距离为零，从而得到机械臂逆运动模型：

minF（θ1，θ2，θ3，θ4，θ5）=■s.t.-αi？燮θi？燮βi，（i=1，2，…，5）（3）

其中αi，βi（i=1，2，…，5）表示θi的变化范围（见表1）。显然使目标函数F（·）为0的关节变量（θ1，θ2，θ3，θ4，θ5）就是（3）的解。模型（3）为非线性约束优化问题，目标函数

F（·）是一个非线性多峰函数，传统优化算法无法有效求解。DE是一种实数编码的智能优化算法，特别适合求解连续性优化问题，尤其对复杂多峰函数的寻优问题非常有效。为求解模型（3）提出一种改进的多种群差分演化算法。 3 差分演化算法及其编码

群智能优化算法的性能体现在算法运行期间种群离散程度维持能力上，离散程度越高，算法才能以更高概率在更大范围内搜索，才尽可能避免早熟现象。多种群策略[7]提出的初始目的并不是机械简单地增加个体的数量，可让不同群体执行不同的搜索任务，建立合适的信息共享机制，更好指引个体的进化或变异。文献[10]受PSO信息共享机制的启发，提出多群体差分演化算法，文献[11]利用种群平均信息对[10]的变异模式进行改进得到： vi=xi+F1×（gbest-x■）+F2×（■-x■）（4）

其中■表示各子种群最优个体的平均值，表示种群的平均信息，本文给出两种平均信息模式，即

■=■Lbest■n或■=■Fl■■Fl■×l■（5）

同时为避免变异模式的单一性，新算法中引入变异模式选择概率Mt，如果rand

龙源期刊网 http://www.qikan.com.cn

步骤1：在搜索空间中随机初始化pop_num×pop_scale个个体xi（0），其中pop_num为子群体个数，pop_scale为子群体规模，个体xi（0）为D维向量，选取全局最优个体gbest以及每个子种群的局部最优个体lbest。

步骤2：判断是否满足终止条件，如果满足则算法终止，否则转步骤3。 步骤3：对每个子群体进行如下操作： 步骤3.1：如果rand

步骤3.2：将xi（t）与vi（t）杂交得到

uij（t）=v■（t），（rand（）？燮R）or（j=r■）x■（t），otherwise，其中r3是1与D之间的随机整数，R为杂交概率。

步骤3.3：选择：xi（t+1）=ui（t），F（ui（t））

步骤3.4：对当前子群体中每个个体与lbest进行适应值比较，更新局部最优个体lbest。 步骤4：对各个子群体的lbest与gbest进行适应值比较，更新全局最优个体gbest。 与文献[10]比较，本文算法避免[10]单一的变异模式，增强了种群多样性。 4 数值模拟与仿真

在Matlab7.0环境下编程实现，算法参数设定情况如下：R=0.85，F=0.7，Mt=0.95，迭代次数为1000。

已知空间曲线C：x=20cost，y=30sint，z=40t，t∈[0，π]，在曲线C上取m（本文取150）个离散点X1，X2，…，Xm，对每个目标点Xi用机械臂逆运动模型可得求与之对应的关节角组合θ■■，θ■■，θ■■，θ■■，θ■■，机械臂正驱动各关节旋转到制定角度，由正运动模型计算机械臂末端达到位置X■■，实际跟踪曲线X■■（i=1，2，…，m）与目标曲线Xi（i=1，2，…，m）点与点之间的欧氏距离构成误差曲线见图4，各个关节角度变化见图5，6。 图4表明实际跟踪曲线与目标曲线的误差很小，表明新算法具有较高控制精度。从图5～6可以看出，各关节角度变化曲线光滑，表明逆运动控制指令具有连续性，那么机械臂工作时相邻指令间变化幅度平缓，表明机械臂运动幅度平稳，具有很强的稳定性与可靠度。 5 结束语

龙源期刊网 http://www.qikan.com.cn

为了建立各关节角度与机械臂末端点H坐标的对应关系，通过分析机械臂结构特征，利用D-H法建立了机械臂正运动模型（2）；然后通过反向求解正运动模型建立逆运动控制模型（3），该模型本质上是约束优化问题。为求解模型（3）本文提出了改进的多种群算法，最后的数值模拟表明新算法可以有效求解机械臂逆运动模型。 参考文献：

[1]侯增广，谭明，徐德等.先进机器人控制[M].北京：高等教育出版社，2007.

[2]姜宏超，刘士荣，张波涛.六自由度模块化机械臂的逆运动学分析[J].浙江大学学报（工学版），2010，44（7）：1348-1354.

[3]孙亮，马江，阮晓钢.六自由度机械臂轨迹规划与仿真研究[J].控制工程，2010，17（3）：387-392.

[4]刘松国，朱世强，王宣银.基于矩阵分解的一般6R机器人实时高精度逆运动学算法[J].机械工程学报，2008，44（11）：304-309.

[5]付荣，居鹤华.高精度解耦六自由度机械臂逆运动学解法[J].计算机测量与控制，2010，18（7）：1637-1640.

[6]黄刚，李德华，杨捷.机械臂最优运动规划问题的混合粒子群算法[J].计算机科学，2009，36 （11）：232-234.

[7]Cai Z X， Peng Z H. Cooperative Co-evolutionary Adaptive Genetic Algorithm in Path Planning of Cooperative Multi-mobile Robot Systems [J]. Journal of Intelligent and Robotic Systems， 2002， 33（4）：61-71.

[8]Tian Lianfang， Collins C. An Effective Robot Trajectory Planning Method Using a Genetic Algorithm[J]. Mechatronics，2004，14（5）：455-470.

[9]R Storn， K Price. Differential Evolution-a Simple and Efficient Adaptive Scheme for Global Optimization over Continuous Spaces[R]. International Computer Science Institute，1995：22-25. [10]廖锋，高兴宝.多种群差分演化算法及其应用[J].计算机仿真，2011，28（1）：230-233. [11]廖锋，毛铭桦，赵文.基于平均信息差分演化算法的非线性系统参数估计[J].数学的实践与认识，2013，43（19）：123-128.

[12]Hui Yuanfan， Jouni Lampinen. A Trigonometric Mutation Operation to Differential Evolution[J].Journal of Global Optimization，2003，27（1）：105-129.

龙源期刊网 http://www.qikan.com.cn

[13] M E Mezura， C A Coello， M I Tun. Simple Feasibility rules and Differential Evolution for Constrained Optimization[C].//Proceedings of the 3rd Mexican International Conference on Artificial Intelligence，2004：707-716.

[14]刘志军，唐柳，刘克铜，吴冬方.差分演化算法中变异策略的改进与算法的优化[J].化工自动化及仪表，2010，37（9）：5-8.