通镌电.潦技】I: 2014年11月25 13第3l卷第6期 Teleeom Power Technology Nov.25,2014,Vo1.31 No.6 文章编号:1009—3664(2014)06—0019—05 4-DOF多功能机器人运动控制算法的研究 郝旭乾,陈劲杰,邓光伟 (上海理工大学机械工程学院,上海200093) 摘要:随着机器人技术的不断发展,机器人在现在社会发展中发挥着越来越重要的作用。文中对实验室研发的多功 能4一DOF机器人的运动控制相关问题进行研究。首先对4一DOF机器人的机构整体设计并PR0E三维建模,然后通过基 于I)_H法建立机械臂坐标系,对4-DOF机器人进行运动学正逆解分析,在逆解计算中,提出了当两轴平行两轴耦合的计 算方法,即将两轴关节角视为整体计算,再利用代数方法依次得出单个关节角。同时针对机械臂的连续轨迹运动给出轨 迹规划的方法,为设计机械臂控制器的实现和机械臂的运动控制提供了依据。 关键词:机械臂;位姿;运动学分析;轨迹规划 中图分类号:TP319,TP242 文献标识码:A Research on Motion Control Algorithm of 4-DOF Multi—Function Robots HAO Xmqian,CHEN Jin jie,DENG Guang-wei (University of Shanghai for Science and Technology,Shanghai 200093,China) Abstract:With the continuous development of robotics,robots are playing a more and more important role in social development.This article iS mainly to address the robot motion contro1 and related issues from the mult讧unctiona1 4一DOF which is invented by our laboratory.After designing the overall structure of 4-DOF robot,a Proe 3D model is built.Then mechanical arnl coordinate system iS established based on the D-H method tO analyze the 4一DOF robot kinematic.In in— verse kinematics calculation,the two axis parallel shaft coupling calculation method is introduced.This method regards the two shaft joint angles as a whole to obtain the single joint angle by algebraic tools.At the same time,trajectory planning method is given in view of straight trajectory motion of the mechanical arnl based on which the mechanical arm controller design can be accomplished. Key words:robot arm;pose position and orientation;kinematics analysis;trajectory planning 0 引 言 必须以特定的姿态沿着给定的路径在一定精度范围 内运动。当指定机械臂执行某项操作作业时,往往 近几年,机器人技术的发展正向多功能化、精密 还会附带一些约束条件,如要求运动平稳及整体协 化、智能化方向发展。随着科学技术的不断提高,工业 调等。这时,运动轨迹除了位姿约束外,还存在各个 机器人在现代工业生产自动化领域得到广泛的应用, 路径点之间的运动学要求和时间分配问题。因此, 并且越来越多的智能机器人走进千家万户,服务在人 合理地设计轨迹规划的算法对整个机器人的运动控 们生活各个领域。机器人的出现大大提高了社会的生 制起到至关重要的作用。 产水平和人类的生活质量。 在机器人技术中,机械臂的运动控制是一个非 1 机器人结构设计与位姿描述 常复杂的过程,规划机械臂的运动不仅需要对机械 本文所研究的4-DOF机械人结构是由四个旋转 臂进行路径规划,还需要对运动路径进行轨迹规划。 关节、两根连杆和一个末端执行器组成的四自由度的 路径规划主要是根据作业要求,结合现场环境,规划 机械臂。其中,腰关节与基座相连接,手腕与末端执行 出机械臂从起始位姿到终点位姿之间的无碰撞运动 器相连接,通过驱动各关节回转就可以控制末端执行 路径。而轨迹规划主要是生成一条没有干涉的路 器运动。用Proe建立的模型如图1所示。 径,并结合机械臂的运动学和动力学,以及机械结构 机械臂的位姿包括位置和姿态。空间坐标系中机 约束条件,将这条路径上机械臂的位置映射为时间 械臂的位置和姿态统称为机械臂的位姿。对于该机械 的函数。机械臂的连续路径作业要求机械臂的末端 臂在三维空间中的任何一点P,设在三维坐标系{A) 中的位置坐标表示 收稿日期:2014—08—18 P一(P ,P ,P ) (1) 基金项目:中央财政支持地方高校专项资金建设项目2012上 理移动读书机器人。 对于三维空间物体Q,假定直角坐标系{B}固定 作者简介:郝旭乾,男,河北邯郸永年县人,硕士研究生,研究 于物体Q上,则物体R在坐标系{A)中的姿态为 方向:机器人与智能控制。 通馋电潦 】.: 2014年11月25日第31卷第6期 郝旭乾,等:4-DOF多功能机器人 运动控制算法的研究 Telecom Power Technology Nov.25,2014,Vo1.31 No.6 符合机械臂实际位姿,如图3所示。 当电机转过角度 =0, =90。, =0, =一 9()。时,末端腕部对应角度0 =0,0 =90。,0 O O 1 0 3=180。, 0 =一90。时,将这些参数代人式(7)得到末端腕部位 Oo 1 姿为 一。一 图5机械臂运动后的实际位姿 上述验证表明末端腕部姿态没有变化,其位置坐 标相对于基座标系,X方向移动d ,Y方向移动d ,Z 方向没有变化,与机械臂实际运动情况符合,表明正运 动学方程正确。 2.2运动学逆解分析 机械臂运动学逆解描述的是末端执行器的笛卡尔 空间到各关节空间的映射。逆解过程复杂,涉及到巧 妙的代数技巧。逆解即为已知关节末端位姿 丁,求解 各关节变量 、 、 、 。假设机械臂腕部位姿为 a P a P a P 0 1 由等式 T=2T,可得 P ==:a1 sl+d3 S12 (8) P 一a1 s1一d3C12 (9) 式(8)的平方加上式(9)的平方得到 S2一( +P;一d;一a{)/(2a ds) (10) 整理得到 Oz—Atan2(s2,±C2) 一Atan2((pl+ 一d;一a;) ̄(2aid3), ± ̄/1一E(pl+ 一d;一a{)/(2a d3)]。)(11) 的实际运动范围是[一丌,丌],此时02= 或者 02= +180。。实验从 运动到T 过程,此处取正 号。计算如下矩阵得到 C12 0 al52 l 0 1 一d2 l 0 0 1 1 C3 S4 S3 0 C4 0 一d3 i丁一i Ti T=== 53S4 一C3 0 0 0 1 又有式(7)两边同乘以(2 T)I1 (2T)4 T一;T (12) 式(12)两边第二行第四列对应相等得 s12 +C12P +al 52一一d3 (13) 令P =pcosg;P =psinqfp ̄0),即lD  ̄/ ;+P;; Atan2(p ,P ),代人式(13)得到 c12 s。一¥12f。一(一d3一a1 s2)/p (14) 由差角公式得到 sin(9—0l2)一(一d3一a1 s2)/p 可得 012一Atan2((一d3一a1 s2)/p, ± ̄/1一(一d3一a1 s2) /p ) (15) 2===Atan2(p ,P )一Atan2(一d3一al 52, ± ̄/ + ;一(一d3一a1 s2) ) (16) 因此 = :或者 := :+180。。本实验从 , 运动到T1过程,此处取正号。计算得到 , 后,得 到0 =0 一 因为 和 对应两种不同值,所以 对应两种不同值,本实验从 运动到Tl过程,符号 由 和 决定。 同理可得 03===Atan2(s3,±C3)一Atan2(c12a +S12a , ±,/1一(f12a +S12a )。) (17) 因此 =03或者 = +180。,本实验从 ,运动 到丁1过程,此处取负号。 同理可得 04===Atan2(s4,-t-C4)=== Atan2(± ̄/1一(s12O 一C120 ) ,¥12Oz—C12Ov) (18) 因此04= 或者04=04+180。,本实验从 运动 到T1过程,此处取负号。 结果产生增根的原因有两个,一是解得到的角度 超出了机械臂关节允许转动的范围,二是运用代数方 法解答过程中三角函数的开根号造成的。第一种原因 可根据关节角度范围来确定关节角度,第二种原因可 通往电.潦 2014年11月25日第31卷第6期 术 Nov.25,2014,Vo1.31 No.6 Teleeom Power Technology 根据几何方法来确定角度。 动速度没有约束的连续轨迹规划中(起始点和目标点 除外),可以用一个五次多项式来拟合最后一个轨迹 3机械臂运动轨迹规划 设机械臂完成连续轨迹运动的总时间为了、,并将 这段时间分为 段(n的值越大表示插补精度越高):t。 ~£ ,t ~ 2,…,t ~ ,其中t0=0,t =T。令每个时 间段为At =t 一t卜1(i=1,2,…, ),将连续轨迹以如 下形式的参数方程表示: r ===fl } 一 1 ===-厂3 ㈣ ls—s(t 式中: 、 z为连续轨迹的直角坐标分量,5=s( )表示 位移或角度与时间的关系。对于特殊轨迹,这里的参 数方程也可以由用户以具体的坐标形式给出。假定在 连续轨迹的起点和终点,机械臂末端的姿势角度分别 为( ,届 , )、(a , , ),且随时间作线性变化。在t (O≤ ≤了、)时刻机械臂末端的姿势角为: ai—ao+ £ (20) D——D 一 + — t (21) 7 一70+ t (22) 式中,a 、 、7 分别为机械臂末端与基坐标系中X、y、 Z轴的夹角。 由式(19)~(22)能很方便地求出t 时刻机械臂 末端在基座标系下的位姿(z , ,Z ;a , ,71),然后计 算出机械臂末端相对基坐标系的变换矩阵 (N为 机械臂的自由度数),再由 进行运动学反解计算出 每个关节的变量值。通常机械臂的运动学逆解会出现 多解情况,可根据各关节的工作空间先去掉其中无意 义的解,然后选择一组与当前形态最接近的解作为各 个关节的变量值0 Oi 、… 。当At 很小时,各个关 节的速度和加速度可由以下近似公式求出: 0 (0o一0( 1)I)/ === / (23) 0 ( 一 ( z) )/( ) (24) 式中,A0 =0 一0(卜1),; =1,2,…,N。 当求出在ti 和t 时各关节的变量值、速度和加 速度后,便可规划t。一 到t 内的关节运动。为使机械 臂运动平稳,在每个时间段内用一个多项式来拟合各 关节的运动。由于在每个关节轨迹中要确定,z段轨 迹,这里引入归一化时间变量,以简化求解过程。设多 项式的形式为 一0o( )一aijo 4-aijl +……+aij (25) 式中, 为归一化时间变量, =(r—t 一 )/(t 一t );r 为实际时间,r∈Et ,ti];(p∈[-o,1]。对于 段轨迹, 每个关节可以构造出 个多项式。在对每个关节的运 段,用三次多项式来拟合其余各轨迹段: 第1个轨迹段为: 1J( )一aljo+口1j1 +a ̄j29 +a1) ̄9。 (26) 第i个轨迹段为: Oo( )一aqo 4-aql+ao2p +aoa ̄o。 (27) 第 个轨迹段为: ( )一n 0 4-anjl 4-anj2 4-a,j39。4- 4 +anj5 (28) 式中,各个关节在起点和终点的位移、速度和加速度 钆、 、G,O.i、 、 以及各中间点处各关节的变量值 0∽02j,…, 均为已知。因此各多项式系数能很方 便的求出。 在对每个关节在中间点时运动速度有约束的连续 轨迹规划中,可以分别用一个四次多项式来拟合起始 和终点轨迹段,用三次多项式来拟合其余各轨迹段,将 对机械臂末端速度的约束转化为对各关节速度的限 制: 对于第1个轨迹段:01 ( )=a )+a 9 +a 93 +alj4 ,由于01j(0) Ooj; 1J(0) u; 1J(0) 0 ; 1, (1)=0 ,(1)= ,可计算得: . 1. aljO Ooj;aljl一 ;alj2=音 a1,3—40l —Oof一38o 7—4 7一 l . 1 (.1lj4一 1J+20oi+告 +30oj一3 l 对于第i(2三三三i 一1)个轨迹段; ,( )=倪 ,+ aid1 十a62 十aij3 ,由于 (0) ( 一 ; (()) ),;0o(1)= ; (1)= ,可计算各系数得: a6o O( ̄-1)j;affl (卜 (-2 2—300—30( 一20( 1) 一 aq3一Oo 4- ( l 4-20( ̄1)j一2 对于第n个轨迹段: ( )=a.j()+a 1 +a 29 + 3 +a.j494,由于 (0)=0( 1)J; (())= 1 (1) = ; (1)= ; (1)= ,可计算各系数得:a = ("一1)j;anjl ( 一1)j . 1 . a.j2===6 一6 ( 1)J一3 ( 1)J 4-去 一3 a.j3=5 4-3 (,r1) 4-80(, 1)j一80,o一0 . 1 . anj4—6 一6 ( 1)J一3 ( l 4-去 一30 求出每个轨迹段的多项式系数后,关节的运动也 便规划出来了。 如图6可以看出,分段数n的大小直接影响着插 补的精度。而确定,z值的近似方法是:假设轨迹误差 的最大允许值为 ,先预估一 值,并将总的运动时间 丁分成,2段。根据式(19)~(22)求出 +1个路径点 在基座标系下的位姿( ,Y ;or , , ),并对这1l"+1 通馋电凉技术 2014年11月25日第31卷第6期 郝旭乾,等:4-DOF多功能机器人 运动控制算法的研究 Telecom Power Technology Nov.25,2014,V0l_31 No.6 个路径点进行运动学逆解求得每个 然后根据上 述算法规划出各个关节在每个时间段的轨迹00( )。 再针对每个时间段的中点所对应的规划轨迹点进行运 动学正解,并计算出运动学正解所得到的直角坐标下 相应的位姿与实际连续路径之间的误差 。比较 与 的大小,如果 ≥ ,则在当前时间段增加一中间 点。保持当前时间段的起点不变,以该中点为终点,重 新验证新的时间段内轨迹误差是否满足要求。当所有 时间段内的轨迹误差都达到要求以后输出此时的已更 新过的 值。 4 总 结 本文主要介绍了四自由度机械臂运动学的正解和 逆解求解过程。特别是在逆解计算中,提出了当两轴 平行两轴耦合的计算方法,即将两轴关节角视为整体 计算,再利用代数方法依次得出单个关节角。同时针 对机械臂的连续轨迹运动给出轨迹规划的方法。在连 续轨迹控制模式基础上,将轨迹描述为机械臂的直角 坐标位姿对时间t的函数,然后在此基础上规划连续 运动轨迹,为设计机械臂控制器和实现机械臂的预期 运动控制提供了依据。 参考文献: [1]尚久浩.自动机械设计I-M].北京:中国轻工业出版社, 2003. [2]蔡自新.机器人学[M].北京:清华大学出版社,2000. -13]史胜利.刘乃钊.陆钟吕.一种求解机器人运动学问题的 ] 有效方法[J].哈尔滨船舶工程学院学报,1994,15(3):72— 82. [4]郭洪红.工业机器人运用技术[M].北京:科学出版社, 2008. [5]刘成良,张为公.RV12L6R焊接机器人运动学正解及计 算仿真系统[J].南京大学学报,2003,21(6):63—68. E6]Mason M T,Salisbury J K.Robot hand and the mechan- ics of Manipulation[M].Boston:MIT Press,2005:203— 209. [7] R P Paul,H.Zong.RohotMotion Trajectory Specifica— tionand Generation EC].2nd International Symposium on Robotics Research,Kyoto,Japan,1994. 图6分段数,l值计算流程 (上接第9页) [3]Esram T,Chapman P L.Comparison of photovoltaic array 孙自勇,宇航,严干贵,等.基于PSCAD的光伏阵列和 MPPT控制器的仿真模型[J].电力系统保护与控制, 2009,37(19):61—64. Walker,G.Evaluating MPPT converter topologies using maximum power point tracking techniques[J].IEEE Transacti-ons on Energy Conversion,2007,22(2):439— 499. a matlab PV model[J].Journal of Electrical&Electronics Engineering Australia,2001,21(1):49—55. 1-4]Jung Youngseok,So Junghun,Yu Gwonjong,et a1.Im— proved perturbation and observation method(IP&O)of MPPT control for photovoltaic power systems.IEEE E— 宋巨龙,钱富才.基于黄金分割的全局最优化方法口].计 算机工程与应用,2005,(4):94-95. (美)Steven C Chapr ̄工程与科学数值方法的Matlab实 lectron Device Society -1C].Conference Record of the IEEE Photovoltaic Specialists Conference,2005,Lake Buena Vista,FL,United states.Institute of Electrical and Electronics Engineers lnc:2005. 现[M].北京:清华大学出版社,2009,(5):179—184. 徐鹏威,刘飞,刘邦银,等.几种光伏系统MPPT方法的 分析比较及改进EJ].电力电子技术,2007,(5):3-5.