工业机器人空间曲线实时轨迹规划算法

http:/wnr.elecfans.con 电/发.烧友 电 j技术论如工业机器人空间曲线实时轨迹规划算法汪遽,马国红，张华(iWa人学机器人研究所，江两南昌330031)摘要：本文设计了一种六轴机械臂沿空间曲线行走的控制算法。算法使机器人对输入空间曲线 计算岀各关节的电机控制命令使机器人沿曲线运动。算法中推导出机器人逆运动学各关节解析解， 使机械臂控制器计算过程由解方程组简化为代数运算。对类似结构的机械肾都可以使用.貝何一定 的通用性。使用Mat lab Xj算法进彳j编程实现，从而验证了文章所设计算法的正确性。关键词：机器人；控制算法；轨迹规划；运动学中图分类号：TP242.2文献标识码：AIndustrial Robot Space Curve Real-time Path Planning AlgorithmWhng Liu , Ma Guo-hong, Zhang Hua(Robot Institute, Nanchang University Nanchang City; 330031, China)Abstract:Tlus paper presents a path planning algorithms for six-axis nnrupulator. The algonthm guide the robot move along the space curves, and figure out joint command of the robot motor. Algonthm including analytical solution of inverse kinematics so similar s:ructure type of the robot ami can be used. The algonthm is realized Matlab, and the effectiveness of algonthm is venfied uith programme.Keyword: Robot, Control algorithm, patli planning, kinenutcs1 引言(Introduction)机器人轨迹规划是使机器人在规定时间内，按一定的速度及加速度，从初始状态移动到目标状 态.本文首先用d-h 建立了运动学模型，并用酉矩Pi JJHC进行了求解，得出了机械宵逆运内学 解。使空间曲线转换到机器人关节空间的运动。在机器人关节空间使用多项式插值法使机械臂各关 节运动的速度和加速度连续，保证生成的各关节电机执行的角度増戢指令符合物理观律。最后使用 Mat lab对算法进行编程实现，从而验证了文章所设计算法的正确性。2 机器人模型(Robot kinematics model)对一个六轴的串联机械臂，如图1所示。图1机器人连杆坐标系Fig 1 Robot link coordinate svsrem 根据D-H法得到机械臂位姿的变换矩阵：*基金颁发部门：国家门然科学荃金委：项H名称：焊接机器人柔性系统pern网建模：编U： 50705ML基金川 请人:马国红.基金颁发部门：江西省教育厅项目基金;项目名称:基于机霧人汽车薄板焊接的旋转电弧传感器研究:编号: No 20078基金屮请人马国红。心4axPx7A(q)人(q)A(q)&(G)人(Q)人倪)=yyyPy仏O.ap.0001式中A”矩阵为根据D-H法建立的机械臂各个关节的变换矩阵。为了使机械臂处J：期里的位姿，盂耍求解具逆运动学解才能确定每个关节变磺的值。我们町以 注意到，运动方程屮冇许多角度的耦介，这就使得无法从矩阵中取出足够的元素來求解单个的正眩 和余弦项以计算角度。逆解方程组是一个关J：6个关节变鼠的由12个方程组成的非线性方程组，采 用西矩阵法求解：（1）求q。可用逆变换（q）左乘运动方程式两边：歼】（&）% = a2（2）a3（）a4（4）a5（）a6（6）（2）解得:q = arctail 工或 q = Px如-daypx-血(2)求&3。解得: xpyPz1人40）咛&5“6（&6）q =arcsui+arctan 丄或 0 = - arcsin a + aictan 丄6 6 (3)求Qc“(qPi + 儿化)+ S2ip:-(cc3) = dc4s5 sin(ft, + w) -Bn解得：dcft, = arcsinB-w&j =开一arcsin B一 w其中:tan w代-dazB g片 deg “/+(：Q = 03 Q = arc sin B 一 w- Q 或 0z-n - arcsin B-w-Q(4)求q05 = arccoss23(clax + 5/v) 一 c2/_ 或 05 = 2n-aiccos匡血乞 + slay)- c2ia:(5)求Q由 一 cm、. = S4S , uf 以解得：55（6）求仇s.a-c.ax= q = arcsin;或 0$5(3丿(4)(6丿(8)(9丿(10丿(H)(12丿(13丿(14丿$2心 + $0 ) 一 =巧6 =sin q =:smQa = arcsm; 或 0 =龙 _ aicsin;sin Qsin q3机器人运动控制（Robot Motion Control）对预先指定的路径曲线我们先要把它离散化，然后根据一定的路径约束和实际情况的约束给 出通过这些点时机械臂的姿态，代入逆运动学方程求出通过这些点时的q,$,址后在关节空间进行插补给出机器指令。设在连续轨迹的起始点和终点机器人的姿态为（，仇,）和（0,q，s），机器人运动连续平 滑，姿态角度随时间做线性变化。在时刻r,（zo f. 0手部姿态角为：纟=% +舛色（15）把姿态角和相映的位置值代入文章屮求出的机械宵逆运动学方程，就可以得出关节空间卜的 q,q值。同时注意这些,q值可能会冇多解，由机构的奇并问题产牛的无穷个解，或无解 （不可达）。对r多解在我们的程序中会自动选择离上一个解绘近的解，保证机械臂各个旋转轴运 动连续。在关节空间生成机器指令包=（比血,俎比，俎如。考虑到机械肾运动的实际情况，要求运动, 速度和加I速度都是连续的，否则就和物理规律违背不能桔确运动到指定的空间点如果长时间这样 操作，机械臂就容易损坏W 所以我们耍在关节空间屮插值。采用三次多项式插值，其表达式位: 生成机器指令包=（比,去去比，去斶。其误差为& = /4卑&（），其中/（X）为空间路径转换 o!6(t) = a0 + at + aj2 +角度约來位:。(0) = % 蚀)=0速度约朿位:0（0）= 4则）胡解上而的方程可得多项式系数为：i = 432 .1 A a2 = -r(0f -0Q)-0Q-0frfrf2 1 . .az = -yr -%)+厂(G + 0)然后等时间的取t（0/z这里设/, = 1）代入多项式: 6（t） = a0 + at + att2 + af(16)(17丿(18)(19)(20丿到关节空间的函数,叫（/） =（r-r0）（r-rj（/一t2）（t-/j。4编程实现及结果分析（Programming and Analysis）通过imdab软件编写了以上控制算法，程序对输入的I标点或空间曲线的参数方程就可以口动 生成机器人可以执行的各关节增彊指令。役定机器人实际参数b-255, c-255, d-65给出一空间 圆的方程厶的参数方程为： f y = 210+-cosI - V6v Z = -17.5 + 87.Ssina（-180 a 1995.http:/ww. elecfans, com 电/发.烧友 电 /技术论2 Ian Guanzheng,斶ng uehao. Theoretical and experimental research on time-optimal trajectoryplanning and control of mdusuial robotsJ. Journal of Control Theory & Application, 2003,2*2).3 朱学军，泉照之.机器人机械于臂关节驱动控制系统设计丁.微计算机信息，2007, 5-22： 265-267.4 amamoto M, anai N, Mohn A. Inverse dynamic and control of crane-type mampulatorA.Proceedings of IEEE/RSJ Internauonal Conference on Intelligence Robots and Systems. Piscataway, NJ, USA IEEE, 1999. 1228-1233作者简介：汪嬢(1983-),男(汉族)，江西南昌人。南昌人学机器人研究所在读硕士。主要从事机 器人控制，机器视觉研究。Biography Wang Liu (1983(HanJiangXi. Robot Institute (Nanchang University), MajorResearch Aiea is robot control, machine vision