机械臂轨迹规划

　机械臂运动的轨迹规划

　摘

　要

　空间机械臂就是一个机、电、热、控一体化的高集成的空间机械系统。随着科技的发展,特别就是航空飞机、机器人等的诞生得到了广泛的应用,空间机械臂作为在轨迹的支持、服务等以备受人们的关注。本文将以空间机械臂为研究对象,针对空间机械臂的直线运动、关节的规划、空间直线以及弧线的轨迹规划几个面进行研究,对机械臂运动与工作空间进行了分析,同时对机械臂的轨迹规划进行了验证,利用 MATLAB 软件对机械臂的轨迹进行仿真,验证算法的正确性与可行性,同时此路径规划法可以提高机械臂的作业效率,为机械臂操作提高理论指导,为机器人更复杂的运动仿真与路径规划打下基础。

　本文一共分为四章: 第一章,首先总结了机械臂运动控制与轨迹规划问题的研究现状及研究法,归纳了各种轨迹规划的算法及其优化法,阐述了机械臂的研究背景与主要容。

　 第二章,对机械臂的空间运动进行分析研究,采用抽样求解数值法—蒙特卡洛法,进行机械臂工作空间求解,同时在 MATLAB 中进行仿真,直观展示机械臂工作围,为下一章的轨迹规划提供理论基础;同时通过 D-H 参数法对机械臂的正、逆运动分析求解,分析两者的区别与联系。

　第三章,主要针对轨迹规划的一般性问题进行分析,利用笛卡尔空间的轨迹规划法对机械臂进行轨迹规划,同时利用 MATLAB 对空间直线与空间圆弧进行轨迹规划,通过仿真验证算法的正确性与可行性。

　第四章,总结全文,分析本文应用到机械臂中的控制算法,通过 MATLAB 结果可以得出本文所建立的算确性,能够对机械臂运动提供有效的路径,而且改进了其她应用于空间机械臂的路径规划问题。

　【 关键词 】

　运动分析 工作空间 算法研究 轨迹规划 ABSTRACT

　Space manipulator is a machine, electricity, heat, charged with high integration of space mechanical system integration、 With the development

　of science and technology, especially the birth of aviation aircraft, a robot has been widely used, the trajectory of space manipulator as the support and services to people"s attention、 This article will space manipulator as the research object, according to the linear motion of the space manipulator, joint planning, space of the straight line and curve, the trajectory planning of several aspects of mechanical arm movement and working space are analyzed, and the trajectory planning of manipulator is verified, the trajectory of manipulator is to make use of MATLAB software simulation, verify the correctness and feasibility of the algorithm, at the same time this path planning method can improve the efficiency of mechanical arm, improve the theoretical guidance for mechanical arm operation, simulation and path planning for robot more complicated movement、

　 This article is divided into four chapters altogether:

　 The first chapter, first summarizes the mechanical arm motion control and path planning problem research status and research methods, summarizes the variety of trajectory planning algorithm and the method of optimization, and expounds the research background and main content of mechanical arm、

　 The second chapter, the paper studied the space motion of mechanical arm, the numerical method, monte carlo method are deduced with the method of sampling, the workspace for mechanical arm is, at the same time the simulation in MATLAB, intuitive display mechanical arm work scope, providing theoretical basis for the next chapter of trajectory planning、 At the same time through d-h method of positive and inverse kinematic analysis of the mechanical arm, analyze the difference and contact、

　 The third chapter, mainly aims at the general problem of trajectory planning is analyzed, using cartesian space trajectory planning method for trajectory planning, mechanical arm at the same time, MATLAB is used

　to analyse the spatial straight line and arc trajectory planning, through the simulation verify the correctness and feasibility of the algorithm、

　 The fourth chapter, summarizes the full text, analysis of the control algorithm is applied to the mechanical arm in this paper, through the MATLAB results can be concluded that the correctness of algorithm, can provide effective path of mechanical arm movement, and improved the other used in space manipulator path planning problem、 [key words] motion analysis,work space,trajectory planning,algorithm research

　目录

　摘 要........................................................... - 1 - ABSTRACT........................................................ - 1 - 第一章 绪论..................................................... - 5 - 第一节 研究背景及意义 ....................................... - 5 - 第二节 国外发展现状 ......................................... - 6 - 一、国现状............................................... - 6 - 二、国外现状............................................. - 6 - 第二章 机械臂的运动分析......................................... - 7 - 第一节 机械臂的正运动学分析 ................................. - 7 - 第二节 机械臂的逆运动学求解 ................................. - 9 - 第三章 五轴机械臂轨迹规划与仿真................................ - 10 - 第一节 轨迹规划一般问题 .................................... - 11 - 第二节 关节空间的轨迹规划 .................................. - 11 - 一、三次多项式插值法.................................... - 11 - 二、五次多项式插值...................................... - 15 - 第三节 笛卡尔空间的轨迹规划 ................................ - 17 - 一、空间直线轨迹规划.................................... - 17 - 二、空间圆弧的轨迹规划.................................. - 20 - 三、一般空间轨迹规划.................................... - 24 - 第四章 总结与展望.............................................. - 28 - 参考文献....................................................... - 29 -

　第一章

　绪论

　第一节

　研究背景及意义

　随着宇宙空间的开发,70 年代美国提出了在宇宙空间利用机器人系统的概念,并且在航天飞机上实施。当初的空间机器人就是由航天飞机舱的宇航员通过电视画面操纵的。随着空间技术的进一步发展使得未来空间操作任务急剧增加,空间站的建立、维修,卫星的回收、释放等工作会越来越多。如果所有这些工作都依靠宇航员来完成,其成本将十分高昂,也就是十分危险的,因为恶劣的太空环境会给宇航员的空间作业带来巨大的威胁。宇航员的舱外作业需要庞大而复杂的环境控制系统、生命保障系统、物质供给系统、救生系统等的支持,这些系统不但具有很高的技术难度,而且成本巨大。用空间机器人代替宇航员进行太空作业不仅可以使宇航员避免在恶劣太空环境中工作时可能受到的伤害,还可以降低成本,提高空间探索的效益。空间机械臂就是空间机器人的一种,已被考虑在未来的空间活动中承担大型空间站的在轨安装及对失效飞行器的的捕捉与维修,土壤与岩的取样等;并期望其在无人状态下承担未来空间实验室或工厂的日常工作。根据空间作业的需要,空间机器人上一般都安装了一个或多个模仿人手臂的多自由度机器臂。随着我国国民经济与国防工业技术的迅速发展,对航天器的需求量日益增加,对其能力的要求日臻提高。特别就是空间站在轨服务、深空探测等空间技术领域的迅速发展,对于空间机械臂技术的需求越来越迫切,而且对其工作能力与性能要求越来越高,对其安全性、寿命等面也提出了越来越高的要求。此外,受国外在高技术领域的技术限制与封锁,使得我们必须坚持自力更生、独立自主的高技术研发道路,坚持自主创新的思想,加速并加强空间机械臂技术的研发工作[1] 。

　将机器人用于空间服务,一项关键技术就就是路径规划。路径规划研究就是机器人研究领域中的一个重要分支,就是机器人导航中最重要的任务之一。对已知静态环境中机器人路径规划的研究已经进行了将近 40 年,路径规划问题的研究有很大的价值。多年的研究工作在取得进展的同时,愈加证明了路径规划就是一个复杂的难题。路径规划算法的计算量取决于任务、环境的复杂性以及对规划路径质量的要求,一个好的路径规划算法应该兼顾对规划速度与路径质量的期望。随着研究的深入,各种新的路径规划法层出不穷,使路径规划研究一直活跃在

　机器人学领域。

　 目前国对空间机械臂研究还处于起步阶段,因此开展空间机械臂相关领域的研究将极大促进我国空间科学试验、空间维护与建设、深空探测等空间技术的发展。本论文根据课题的技术要求,将空间机械臂路径规划作为切入点,研究路径规划问题,其研究成果具有重要的理论指导意义与工程应用价值。

　第二节

　国 外发展现状

　一、国现状

　我国的工业机器人从 80 年代“七五”科技攻关开始起步,目前已基本掌握了机器人操作机的设计制造技术、控制系统硬件与软件设计技术、运动学与轨迹规划技术,生产了部分机器人关键元件,开发出喷漆、弧焊、点焊、装配、搬运等机器人;但总的来瞧,我国工业机器人技术及其工程应用的水平与国外比还就是有一定的距离,如:可靠性低于国外产品;机器人应用工程起步较晚,应用领域窄,生产线系统技术与国外比有差距。

　我国的智能机器人与特种机器人在“863”计划的支持下,也取得不少成果。其中最突出的就是水下机器人,6000 米水下无缆机器人的成果居世界领先水平,还开发出直接遥控机器人、双臂协调控制机器人、爬壁机器人、管道机器人等机种;在机器人视觉、力觉、触觉、声觉等基础技术的开发应用上开展了不少工作,有一定的发展基础。但就是在多传感器信息融合控制技术、遥控加局部自主系统遥控机器人、只能装配机器人、机器人化机械等的开发应用面则刚刚起步,与国外先进水平差距较大。

　二、国外现状

　美国就是机器人的诞生地,早在 1962 年就研制出世界上第一台工业机器人,比起号称“机器人国”的日本起步至少早五六了年。1971 年,通用汽车公司又第一次用机器人进行点焊。

　西欧时仅次于日美机器人的生产基地,也就是日美机器人的重要市场。早在1966 年,美国 Unimation 公司的尤尼曼特机器人与 AMF 公司的沃莎特兰机器人就进入英国市场。接着,英国 Hall Automation 公司研制出自己的机器人 RAMP。德国工业机器人的总数占世界第三。德国对于一些有危险、有毒、有害的工作岗位,

　必须以机器人替代普通人的劳动。同时提出了 1985 年以后要向高级的、带感觉的智能型机器人转移目标。

　1954 年:美国人戴沃尔制造了世界第一台可编程的机械手。

　1959 年:戴沃尔与美国发明家英格伯格联手制造出第一台工业机器人。

　1962 年:美国 AFM 公司生产出万能搬运机器人,与 Unimation 公司生产的万能伙伴机器人一样成为真正商业化的工业机器人。

　1967 年:日本川崎重工公司与丰田公司分别从美国购买了工业机器人Unimat 与 Verstran 的生产可,开始对机器人的研究与制造。

　1968 年:美国斯坦福研究所公布她们研制的机器人 Shakey。

　1973 年:世界上机器人与小型计算机第一次携手合作,诞生了机器人 T3。

　1979 年:日本山梨大学发明了平面关节机器人 SCARA。

　1984 年:英格伯格在此推出机器人 Helpmate,这种机器人能在医院为病人送饭送药与送。

　1996 年:本田公司推出仿人型机器人 P2,双足行走机器人的研究达到了一个新的高度。

　2002 年:美国 iRobot 公司推出了吸尘器机器人 Roombar,为世界上商业化最成功的家用机器人。

　2006 年:微软公司推出 Microsoft Robitics Studio 机器人,从此机器人模块化平台同一化的趋势越来越明显。

　在工业机器人技术面,工业机器人有操作机(机械本体)、控制器、伺服驱动系统与检测传感器装置构成,就是一种仿人操作、自动控制、可重复编程、能在三维空间完成各种作业的机电一体化自动化生产设备。

　第二章

　机械臂的运动分析

　机械臂的运动就是其轨迹出现的直接原因。所以轨迹规划的前提就是机械臂的运动分析[1] 。本文通过对机械臂的正运动学与逆运动学进行求解,分析两者的区别与联系。通过对五轴机械臂关于坐标系几关系,针对常见轨迹规划案中起始与终止阶段进行研究,分析研究结果。

　第一节

　机械臂的正运动学分析

　机械臂从关节空间到末端笛卡尔空间的变换就是正向运动学描述。由坐标系

　中已知的各个关节角度,求解机械臂末端相对应于原点坐标系的位置与位姿。设矩阵 A 表示机械臂连杆的齐次变换: ) , ( ) 0 , 0 , ( ) , 0 , ( ) , (1 i n i i i iX Rot a Trans ld Trans Z Rot A   

　       1 0 0 0cos cos sin cos sin sinsin sin cos cos cos sin0 sin cos1 1 1 11 1 1 11i i i i i i ii i i i i i ii i idda           

　(2-1) 由于机械臂全就是旋转关节。对于文中采用的机械臂而言有五个其次变换矩阵,则末端连杆坐标系相对于基坐标系的齐次变换矩阵  1 0 0 033 32 3123 22 2113 12 115 4 3 2 105zyxp r r rp r r rp r r rA A A A A T

　 (2-2)

　 式即为机械臂的运动程,它反应各关节变量与机械臂末端位姿之间的关系,上式左边的五个矩阵含有五个关节变量Tq ] , , , , [5 4 3 2 1      。程右边为描述机械臂 末 端 关 节 位 置 与 姿 态 的 齐 次 矩 阵 , 由 刚 体 姿 态 的 描 述 可 知11r ,12r ,13r ,21r ,22r ,23r ,31r ,32r ,33r 分别为机械臂末关节坐标系的三个坐标轴与机械臂基坐标系三个坐标轴的向余弦,xp ,yp ,zp 为机械臂末关节的坐标原点在机械臂基坐标系中的三维坐标。

　 机械臂正运动学求解就就是已知各连杆的关节变量求解末端连杆的位姿矩阵。即已知关节变量Tq ] , , , , [5 4 3 2 1      ,求解上式机械臂运动学程中等式右边矩阵各元素的值[10] 。

　 将上式中的机械臂五个关节的齐次变换矩阵带入,即计算出 T05中各元素值为: 1 0 0 033 32 3123 22 2113 12 1105zyxp r r rp r r rp r r rT

　 (2-3) 其中:

　5 1 5 234 1 11s s c c c r  

　5 1 5 234 1 12c s s c c r   

　4 23 1 4 23 1 13c s c s c c r  

　5 1 5 234 1 21s c c c s r  

　5 1 5 234 1 22c c s c s r   

　4 23 1 4 23 1 23c s s s c s r  

　5 234 31c s r 

　5 234 32s s r  

　234 11c r  

　1 1 2 1 2 2 1 3 23 1a c ld s a c c a c c p x    

　1 1 2 1 2 2 1 3 23 1a s ld c a c s a c s p y    

　1 2 2 3 23ld a s a s p z   

　其中,1 1cos   c ,1 1sin   s , ) cos(4 3 2 234      c , ) sin(4 3 2 234      s 。

　第二节

　机械臂的逆运动学求解

　机械臂的逆运动学解就是对其运动学正解的反解,因而已知量与求解量相反,即已知机械臂末端的位置姿态对机械臂进行驱动,使各个关节从此刻的姿态运动到与末端位姿相对应的位置,进而得到关节变量[11] 。

　 机械臂的运动学正、逆求解实质就是机械臂关节空间与工作空间之间的非线性映射关系,两者可相互转换。关系图如下所示。

　杆件参数运动学正解 末端执行器位姿运动学逆解 逆解选取关节角量关节角量杆件参数 图 1 关节空间与工作空间的关系

　机械臂的逆运动学问题,指已知机械臂的末端位姿,即已知齐次变换矩阵 T05,求解各转动关节的角度i 。

　 机械臂的逆运行学问题,可以理解为通过运动学程: ) ( ) ( ) ( ) ( ) (545 434 323 212 10105     T T T T T T 

　(2-4) 求解i 。

　 整理式,将含有1 的部分移到程的左边 ) ( ) ( ) ( ) ( )] ( [545 434 323 212051101     T T T T T T 

　(2-5)

　 将 T01转置,上式可以表达成为: Td r r rp r r rp r r rdc ss cyx151 33 32 3123 22 2113 12 1111 11 11 0 0 0 1 0 0 01 0 00 00 0

　 (2-6)

　 假设上式的两边元素与式相等,得到: 01 1  y xp c p s

　 (2-7) 可以得出1 的解。

　第三章

　五轴机械臂轨迹规划与仿真

　目前关于空间轨迹规划的法主要有三种,三次多项式插值,高阶多项式插值以及样条曲线等法。主要讨论轨迹在关节空间中的位移、速度与加速度等变量的关系。规划实质就是根据需求,计算出预定的轨迹曲线,在轨迹规划时可以再运动学与动力学的基础上进行规划,所以规划就是建立在运动学与动力学基础上的。

　 图 2 机械臂的 matlab 生成 第一节

　轨迹规划一般问题

　轨迹规划的一般法就是在机械臂末端的初始与目标位置之间用多项式函数“插”来抵近给定的路径,并沿着时间轴产生一系列的可供操作机使用的“控制设定点”[3] 。其中关节坐标与笛卡尔坐标都可以对路径端点进行给出。一般就是在笛卡尔坐标中给出,由于在笛卡尔坐标中机械臂末端形态更容易观察。所以通常采用笛卡尔法。

　在给定的两端之间,常有多条可能路径。可以沿着直线与光滑多项式曲线运动。本文将讨论插值法,研究满足路径约束的简单轨迹规划[3] 。

　第二节

　关节空间的轨迹规划

　机械臂关节空间的轨迹规划解决机械臂从起始位姿到终止位姿去取放物体的问题、机械臂末端移动的过程并不重要,只要求运动就是平滑的且没有碰撞产生、在关节空间中进行轨迹规划时,算法简单、工具移动效率高、关节空间与直角坐标空间连续的对应关系就是不存在的,因此机构的奇异性问题一般不会发生。对于无路径的要求,应尽量在关节空间进行轨迹规划。

　一、三次多项式插值法

　三次多项式与其一阶导数函数,总计有四个待定系数,对起始点与目标点两者的角度、角角速度同时给出约束条件,本文采用的就是三次多项式插值法[5] 。可以对通过空间的 n 个点进行分析并进行轨迹规划,让速度与加速度在运动过程

　中保持轨迹平滑。本文算法可以实现对 ) 1 (  n 段中的每一段三次多项式系数求解,为了便,对其进行归一化处理。

　 (1)时间标准化算法

　 根据三次多项式轨迹规划流程,对每个关节进行轨迹规划时需要对 ) 1 (  n 段的轨迹进行设计,为了能对 ) 1 (  n 个轨迹规划程进行同样处理,本文首先设计了时间标准化算法将时间进行处理,经过处理后的时间 ] 1 , 0 [  t 。

　 首先定义:

　 t :标准化时间变量, ] 1 , 0 [  t ;

　  :未标准化时间,单位为秒;

　 i :第 i 段轨迹规划结束的未标准化时间,1  i i i   ;

　 机械臂执行第 i 段轨迹所需要的实际时间: ) /( ) (1 1    i i it     ,其中1 , 0 , ,1 ti i   。

　 时间归一化后的三次多项式为:3322 1 0t A t A t A A y    

　(2)机械臂轨迹规划算法实现过程

　 ①已知初始位置为1 ;

　 ②给定初始速度为 0;

　 ③已知第一个中间点位置2 ,它也就是第一运动段三次多项式轨迹的终点;

　 ④为了保证运动的连续性,需要设定2 所在点为三次多项式轨迹的起点,以确保运动的连续;

　 ⑤为了保证2 处速度连续,三次多项式在2 处一阶可导;

　 ⑥为了保证2 处加速度连续,三次多项式在2 处二阶可导;

　 ⑦以此类推,每一个中间点的位置i )) 1 ( 2 (    n i ,都一定要在其原运动段轨迹的终点,并且也就是它后运动段的起点。

　 ⑧1  i 的速度保持连续;

　⑨1  i 的加速度保持连续;

　 ⑩点位置n 。给定终点速度,设其为 0。

　 (3)约束条件

　 第一个三次曲线为:313212 11 10) ( t a t a t a a t     

　第二个三次曲线为:323222 21 20) ( t a t a t a a t     

　第三个三次曲线为:333232 31 30) ( t a t a t a a t     

　......

　 第 ) 1 (  n 个三次曲线为:33 ) 1 (22 ) 1 ( 1 ) 1 ( 0 ) 1 () ( t a t a t a a tn n n n        

　在同一时间段,三次曲线每次的起始时刻 0  t ,停止时刻nt t  ,其中 n i ... 1  。

　 ①在标准化时间 0  t 处,设定1 为第一条三次多项式运动段的起点,可以得出:10 1   ;

　 ②在标准化时间 0  t 处,三次多项式运动段第一条的初始速度就是已知变量,所以得出: 011 1  a  ;

　 ③第一中间点位置2 与第一条三次多项式运动段在标准化时间nt t  时的终点相同,所以可以得出:31 1321 12 1 11 10 2 f f ft a t a t a a      ;

　 ④第一中间点位置2 与第一运动段在标准化时间 0  t 时起点相同,所以得出:20 2a   ;

　 ⑤三次多项式在2 处一阶可导,因此可得出:2121 13 1 12 11 23 2 a t a t a af f      ;

　 ⑥三次多项式在2 处二阶可导,因此可得出:22 1 13 12 22 6 2 a t a af     ;

　 ⑦第二个空间点的位置3 与第二运动段在标准化时间12t 时的终点相同,所以有:32 2322 22 2 21 20 3 f f ft a t a t a a      ;

　 ⑧第二个中间点的位置3 应与第三运动段在标准化时间 0  t 时起点相同,所以有:30 3a   ;

　⑨三次多项式在3 处一阶可导,从而有:3122 23 2 22 21 33 2 a t a t a af f      ;

　 ⑩三次多项式在3 处二阶可导,从而有:32 2 23 22 32 6 2 a t a af      ;

　 ......

　 ⑪第 ) 2 (  n 个中间点位置1  n 与第 ) 1 (  n 运动段在标准化时间) 2 (  n ft 时的终点相同,所以有:3) 2 ( 3 ) 2 (2) 2 ( 2 ) 2 ( ) 2 ( 1 ) 2 ( 0 ) 2 ( 1           n f n n f n n f n n nt a t a t a a  。

　 ⑫第 ) 2 (  n 个中间点位置1  n 应与下一运动段在标准化时间 0  t 时的起点位置相同,所以有0 ) 1 ( 1  n na  ;

　 ⑬三次多项式在第 ) 2 (  n 个中间点处一阶可导,从而: 1 ) 1 (2) 2 ( 3 ) 2 ( ) 2 ( 2 ) 2 ( 1 ) 2 ( ) 1 (3 2         n n f n n f n n na t a t a a  

　 (3-1)

　 ⑭三次多项式在第 ) 2 (  n 个中间点处二阶可导,从而: 2 ) 1 ( ) 2 ( 3 ) 2 ( 2 ) 2 ( ) 1 (2 6 2      n n f n n na t a a   

　(3-2)

　 ⑮因此可以得出所有轨迹终点在标准化时间nt 时的位置n 为: 33 ) 1 (22 ) 1 ( 1 ) 1 ( 0 ) 1 ( fn n fn n fn n n nt a t a t a a       

　 (3-3)

　 ⑯因此可以得出所有轨迹终点在标准化时间nt 时的速度n 为: 23 ) 1 ( 2 ) 1 ( 1 ) 1 (3 2fn n fn n n nt a t a a      

　 (3-4)

　 以上公式改写为矩阵为: ] [ ] [ ] [1 M C 。由该矩阵计算1] [M 可以求出轨迹规划的全部参数,( ] [  由五轴机械臂运动学逆解求出)于就是求得 ) 1 (  n 段的运动程,从而使五轴机械臂末端执行器经过所给定的位置坐标。

　 通过以上分析可以确定机械臂在满足速度要求的两个位姿之间运动时各个关节轴的角度变化曲线。如下图 3 所示就是 MATLAB 仿真分析三次多项式插值:机械臂某关节角在 4 秒由初始点 A 经过中间点 B 到达终点 C 的变化情况。三个位置点的速度与角速度如下所示: 20 30 2040 60 30    C B AC B A      

　图中实线为角度变化曲线,虚线为角速度变化曲线。关节角度曲线平滑,而速度曲线在中间点 B 处出现突变。

　0 0.5 1 1.5 2 2.5 3 3.5 4-30-20-10010203040506070时 间 (s)角度(deg) 图 3 三次多项式插值法 二、五次多项式插值

　五次多项式共有六个待定系数,要想六个系数得到确定,至少需要六个条件。五次多项式可以瞧作就是关节角度的时间函数,因此其一阶可导与二阶可导分别可以瞧作就是关节角速度与关节角加速度的时间函数。五次多项式及一阶、二阶导数公式如下: 55443322 1 0 ) (t C t C t C t C t C Ct      

　 (3-5) 453423 2 1 ) (5 4 3 2 t C t C t C t C Ct      

　 (3-6) 3524 3 2 ) (20 12 6 2 t C t C t C Ct      

　(3-7)

　 为了求得待定系数5 4 3 2 1 0, , , , , C C C C C C ,对起始点与目标点同时给出关于角度与角加速度的约束条件: 50 540 430 320 2 0 1 0 ) ( 0t C t C t C t C t C Ct      

　 (3-8) 55443322 1 0 ) ( f f f f f tt C t C t C t C t C Cf      

　(3-9) 40 530 420 3 0 2 1 ) (5 4 3 20t C t C t C t C Ct      

　(3-10) 453423 2 1 ) (5 4 3 2f f f f tt C t C t C t C Cf      

　(3-11)

　30 520 4 0 3 2 ) (20 12 6 20t C t C t C Ct      

　 (3-12) 3524 3 2 ) (20 12 6 2f f f tt C t C t C Cf      

　 (3-13)

　 式中) ( 0 t 、) (ft 分别表示起始点与目标点的关节角,) ( 0 t  、) (ft  分别表示起始点与目标点的关节角速度,) ( 0 t   、) (ft   分别表示起始点与目标点的关节角加速度。将起始时间设为 0,即 00 t 得到解为:             520 0 05420 0 04320 0 03020 10 02) ( ) 6 6 ( 12 122) 2 3 ( ) 16 14 ( 30 302) 3 ( ) 12 8 ( 20 202ff f f f fff f f f fff f f f ftt tCtt tCtt tCCCC                               

　(3-14)

　 为了对比三次多项式关节插值算法与五次多项式插值算法的效果,同样要求机械臂从起始点开始运动,经过 4 秒到达终点,仿真时起始点与目标点的关节角速度为 0。中间点的关节角加速度还可以对相邻两段轨迹角加速度进行平均值求解,使该值为中间点的瞬时加速度[12] 。利用 MATLAB 对五次多项式插值进行仿真,将结果与三次多项式插值进行对比,发现三个位置点的速度、角速度两种法相同,同时增加角加速度约束: 2 4 220 30 2040 60 30      C B AC B AC B A            

　 仿真结果如图 4 所示,图中实线与虚线分别表示角度变化曲线、角速度变化曲线。点线则表示角加速度曲线。其中关节角度与角速度曲线显示的都相对平滑,而角加速度曲线在中间点 B 处变化稍大。结果分析得出,多项式插值法虽然计算量有所增加,但就是其关节空间轨迹平滑、运动稳定,且阶数越高满足的约束项越多。

　0 0.5 1 1.5 2 2.5 3 3.5 4-150-100-50050100Time(s)Joint(deg) 图 4 五次多项式插值法 第三节

　笛卡尔空间的轨迹规划

　在机械臂的笛卡尔空间轨迹规划中,中间点即插补点的坐标可以通过插补算法得到。得到中间点后,在把中间点的位姿转换成相应的关节角,再通过对关节角的控制,使得机械臂的末端能按照预先规划的路径运动。机械臂的笛卡尔空间轨迹规划位姿控制过程大致如下所示: 机械臂轨迹规划器(插补算法)机械臂轨运动学逆解求解机械臂关节角控制机械臂按照规划的轨迹运动轨迹上几个给定点的位姿轨迹中插补点的位姿各关节角 图 5 机械臂笛卡尔空间轨迹规划控制过程

　 空间直线与空间弧线的轨迹规划就是笛卡尔空间中不可或缺的两部分。因为空间的曲线可以分割为多直线与弧线;但就是也有会出现直线或弧线连接处尖角问题,为了使运动轨迹连续平滑,本文采用圆弧过度来平滑尖角。在笛卡尔空间中,空间直线与空间弧线的轨迹规划就是最常见的两部分,其她空间曲线可以通过这两者来逼近。

　一、空间直线轨迹规划

　所谓空间直线插补就就是在该直线起始点位姿已知的情况下,对轨迹中间点(插补点)的位姿坐标进行求解[6] 。

　直线插补法:

　 ①设已知起始点的位置坐标分别为: ) , , (0 0 0 0z y x p , ) , , (f f f fz y x p ,0p 与fp为相对基础坐标系计算其长度: 202020) ( ) ( ) (f f fz z y y x x L      

　(3-15)

　 ②求间隔行程,需要分匀速、加速、减速三种情况进行讨论:

　 匀速:设速度为 v ,则插补期sT 行程为svT d 1;

　 加 速 : 设 加 速 度 为 a , 起 始 点 速 度 为0v , 则 在 插 补 期 的 行 程为:20 221s saT T v d   ;整个加速度的路程为:2021vas  ,时间记为[6] :avt02 ;

　 ③计算总时间:3 2 1t t t t    ;

　 ④计算插补点数:sTtN  ;

　 ⑤对插补点所在段进行判断(匀速段、加速段、减速段),使各轴的增量得到确定,对各插补点坐标进行实时计算。

　 ⑥根据坐标值,通过运动学逆解求出各关节角。

　 ⑦利用五次多项式插值法对关节角的插值计算。

　 ⑧从以上各式分析可以瞧出,机械臂完成一个空间轨迹的过程,就是实现估计离散点的过程。让其尽量逼近,使机械臂轨迹尽可能的符合规划好的运动轨迹,本次采用定时插补法。

　 ⑨为了使机械臂的性能更好,让末端执行器的轨迹更平滑,在相邻两个插值点的关节角间选取插补函数使关节轴运动更加稳定。此法将笛卡尔空间、关节空间相结合。如:工具末端沿着一个直线运动,通过上面的计算把直线段上插补 199次即整体直线轨迹分为 200 个点,每个坐标点进行逆运动学求解得到 200 组关节角度值。最后通过关节空间轨迹规划的法将相邻的两组关节角之间进行角度插补,从而使工具末端的轨迹平滑且能很好的控制每个关节的角速度与角加速度[8] 。

　 在 MATLAB 中利用上述直线插补法对机械臂进行正形轨迹规划仿真,机械臂的末端由起始点 A,经过 B 点、C 点、D 点返回 A 点。其中点 A、B、C、D 的位姿分别用齐次变换矩阵表示为:

　1 0 0 0180 1 0 060 0 1 0410 0 0 1AT1 0 0 0180 1 0 060 0 1 0290 0 0 1BT

　(3-16) 1 0 0 0180 1 0 060 0 1 0290 0 0 1CT1 0 0 0180 1 0 060 0 1 0410 0 0 1DT

　(3-17)

　 正形的每个边长为 120mm,每个边上插补 30 步,总仿真时间为 120s。正形轨迹的仿真结果如图 6 所示,通过运动学求解得到五个关节角的位移数据并生成相关的数据曲线,如图 7 所示。

　0 0.5 1 1.5 2 2.5 3-20-18-16-14-12-10-8-6-4-20时 间 t/s关节变量值 /o位 姿 1时 关 节 角 1轨 迹 规 划 曲 线 图 6 关节角位移轨迹曲线

　0 0.5 1 1.5 2 2.5 3-20-1001020304050时 间 t/s关节角1的关节速度 0/s关 节 角 1时 关 节 角 1速 度 轨 迹 曲 线 0 0.5 1 1.5 2 2.5 3-30-20-100102030405060时 间 t/s关节角1的加速度 0/s2位 姿 1时 关 节 角 1加 速 度 轨 迹 曲 线 图 7 关节角速度与加速度轨迹仿真图

　 由上述仿真图可以瞧出,每个关节角度曲线均可划分为 4 段,每段关节角度变化平稳光滑,只在正形四个顶点出变化最大,故还需要对顶点附近的关节角进行空间轨迹规划。

　二、空间圆弧的轨迹规划

　在笛卡尔空间圆弧轨迹规划中,为了计算便,运用坐标变换,即先在圆弧所在平面建立一个新的直角坐标系,在这个直角坐标系中计算圆弧的各插补点在新坐标系中的值。然后再将这些值返回到原来的坐标系中,算出各插补点在原来坐标系中的值。圆弧插补的位移曲线也就是采用抛物线过度的线性函数,归一化因子的求解与上述一样[8] 。

　 三点确定一段弧。设机械臂末端执行器从起始位置1P 经过中间点2P 到达终点3P ,如果这三点不共线,就一定存在从起始点1P 经过中间点2P 到达终点3P 的圆弧轨迹规划算法。具体算法如下:

　 ①先求得圆弧的圆心 ) , , (0 0 0 0z y x P 与半径 r 。

　 ) , , (1 1 1 1z y x P 、 ) , , (2 2 2 2z y x P 与 ) , , (3 3 3 3z y x P 三点确定平面 M,其程为: 03 2 3 2 3 23 1 3 1 3 13 3 3      z z y y x xz z y y x xz z y y x x

　(3-18)

　 将 其 展 开 可 得 :0 ) )]( )( ( ) )( [() )]( )( ( ) )( [() )]( )( ( ) )( [(3 3 1 3 2 3 2 3 13 3 2 3 1 3 1 3 23 3 1 3 2 3 2 3 1                  z z y y x x y y x xy y z z x x z z x xx x z z y y z z y y

　(3-19) XYZOMP1P2P3P0T SWVU 图 8 空间圆弧插补示意图

　 过2 1 PP 的中点且与2 1 PP 垂直的平面 T 的程为:

　0 ) )]( (21[) )]( (21[ ) )]( (21[1 2 2 11 2 2 1 1 2 2 1          z z z z zy y y y y x x x x x

　 (3-20)

　 过点3 2 PP 的中点且垂直3 2 PP 的平面 S 的程为: 0 ) )]( (21[) )]( (21[ ) )]( (21[2 3 3 22 3 3 2 2 3 3 2          z z z z zy y y y y x x x x x

　 (3-21)

　 联立上式,求得圆心 ) , , (0 0 0 0z y x P 。圆弧的半径为: 21 021 021 0) ( ) ( ) ( z z y y x x r      

　(3-22)

　 ②以圆心 ) , , (0 0 0 0z y x P 为原点建立圆弧所在平面的新坐标 UVW O R  ,U 轴为坐标系原点0P 与点3P 的连线。单位向向量为3 03 0P PP Pu    ;

　 ③W 轴为平面 T 与平面 S 的交线,其单位向向量为:3 2 2 13 2 2 1P P P PP P P Pw        ;根据右手法则,V 轴在 W 轴与 U 轴的叉乘向,其单位向量为[13] : u w v  

　(3-23)

　 根据齐次坐标变换可得齐次坐标矩阵RT 为: 1 0 0 0o z z zo y y yo x x xRz w v uy w v ux w v uT

　 (3-24)

　 其逆矩阵1 RT 可以根据齐次变换矩阵求解逆得到: z z zy y yx x xw v uw v uw v uR

　oooozyxP

　(3-25)

　 可以得到: 1 01 oT TRP R RT ;

　 ④将点1P 、2P 、 3 P 以及圆心oP 从原来坐标系中的值转换到圆心所在 UVW P 0

　新坐标系中。设原来的坐标系中的值分别为 ) , , (1 1 1z y x 、 ) , , (2 2 2z y x 、 ) , , (3 3 3z y x 、) , , (o o oz y x ,在新坐标中值分别为 ) , , (1 1 1w v u 、 ) , , (2 2 2w v u 与 ) , , (3 3 3w v u ,则求解: 1 11111111zyxTwvuR1 12221222zyxTwvuR1 13331333zyxTwvuR

　(3-26)

　 由上式推到知 03 2 1 0      w w w w v uo o, r u 1;

　 ⑤求圆弧角度  。由于在 MATLAB 中部函数 Math、Atan2(x,y)的求解围在-1800 -180 0 之间。则:

　 当 03 v 时,则 ) , ( 2 tan3 3 3u v A  

　3  

　   0cossinwr ur v

　 (3-27)

　 ⑥将插补结果返回到原坐标中,设点 p 在原坐标系中坐标值为 ) , , ( z y x ,则有: 1 1wvuTzyxR

　(3-28)

　 由以上结果可以得到圆弧上各插补点的位置,各插补点的三个位姿角度可以各自按照位移曲线为抛物线过度的线性函数求得。把每个插补点的位姿通过运动学逆解,就可以得到各插补点对应的关节角。

　 空间三点的位姿1p 、2p 与3p 可以分别用下式表示: 1 0 0 0400 0 0 160 0 1 0360 1 0 01 pT1 0 0 0300 0 0 140 0 1 0330 1 0 02 pT1 0 0 0200 0 0 160 0 1 0280 1 0 03 pT

　(3-29)

　 通过空间弧线的插补法,插补步数设为 N=200,仿真时间 40s,在 MATLAB 中进行空间圆弧的仿真,如图 9 所示,同时获得机械臂关节角度的数据曲线,如图 10所示。机械臂的自由度数目应与所要完成的任务相匹配,空间圆弧在 M 平面上,机械臂的关节数对于空间圆弧轨迹而言就是冗余的。第五关节轴线与末端的工具

　轴线重合,关节五的角度并不影响空间圆弧的轨迹规划中工具的位置与姿态,所以图 10 中没有第五关节的角度曲线。

　图 9 圆弧轨迹插值 三、一般空间轨迹规划

　在大多数情况下,可将 TCP 的目标运动轨迹划分成若干段圆弧轨迹与直线轨迹的连接,从而在每个对应的小区段使用直线插值或圆弧插值的法完成整条 TCP的目标运动轨迹的插值。但就是,对于复杂程度相对较高的目标轨迹曲线,直线段加上圆弧段的组合在曲线精度面并不理想,常常难以满足用户需求。而且,将复杂程度高的轨迹曲线划分为若干段微小的直线与圆弧的组合,就是非常困难的工作,特别就是还要保证直线段与圆弧段交接点处过渡平滑的条件。因此,对于任意复杂的轨迹,使用更高级的插值法势在必行。较为常用的复杂轨迹插值法有:多项式插值法、分段线性插值法、分段多项式法、B 样条插值法等等。要保证较高的插值精度,往往需要给定更密集的插值点序列,而且,如果使用的就是多项式插值法,为获得高精度,多项式的次数也需要高。但就是,当次数高时,会产生龙格现象,即在插值区间两端,会产生剧烈的震荡现象,导致插值点不收敛于目标轨迹。三次样条就就是通过全部样点且具有连续二阶导的函数,因此,选择三次样条插值对机械臂轨迹曲线进行规划[10] 。

　 若三次样条曲线所经过的 1  n 个插值样点序列的X轴坐标为0x 、 1 x 、...、 n x 。则待求的三次样条曲线参数程 ) (u s x  就是通过所有样点,而且具有连续二阶导

　数的分段三次多项式,即满足条件:

　 (1) ) (u s 在每两个样点之间的函数表达式 ) (u s j 就是小于或等于三次多项式;

　 (2) ) (u s 在除起点与终点外的所有点处都有直到二阶的连续导数。

　 若 ) (u s 在每两个插值样点之间的三次多项 ) (u s j 的表达式为: j j j j jd u c u b u a u s    2 3) ( ) ,..., 2 , 1 ]; 1 , 0 [ ( n j u  

　 (3-30)

　 其中上式中的系数为常数。

　则:  个插值样点之间 和第 第个插值样点之间 和第 第个插值样点之间 和第 第1 ), (....3 2 ), (2 1 ), () (n21n n u su su su s x

　(3-31)

　 所以,需要求得 n 组位置常数系数,才能得到完整的三次样条参数程表达式。

　 假设在 ) (us 每个插值样点出的值为0M ,1M 、、、nM ,又因为每两个插值样点之间的三次多项式 ) (u s j 的二阶导 ) (us 师一次多项式,所以有: j j juM M u u s   1) 1 ( ) (   ) ... 2 , 1 ]; 1 , 0 [ ( n j u  

　(3-32)

　 对 ) (u s j   分别做一重与二重积分可得[14] : j j j j jM u M u du u s u s ~] ) 1 ( [21) ( ) (212        

　 j j j j j ju M u M u du u s u s  ~~] ) 1 [(61) ( ) (313       

　(3-33)

　 再把插值样点的已知坐标1) 0 (j jx s 与j jx s  ) 1 ( 分别代入上式中,可得: 1 161  j j jM x  ,j j jM x61   ,代入上式得出: )61( )61)( 1 ( ] ) 1 [(61) (1 1313j j j j j j jM x u M x u M u M u u s          

　(3-34)

　 由求得的易知在函数 ) (u s 中,总共含有 1  n 个待求未知数:0M ,1M ,、、、nM 。因此只要求得未知数,并且相互独立的 1  n 个程,才能确定函数 ) (u s 的全部未知数。

　 图 10 三次样条轨迹插值

　图 11 直线与姿态匀速轨迹仿真图

　 图 12 圆弧轨迹插值仿真图

　 图 13 三样条轨迹仿真图

　 通过仿真分析,针对简单且不复杂的目标轨迹曲线,一般采用直线轨迹规划或者圆弧轨迹规划,较复杂的可以采用直线轨迹规划与圆弧轨迹规划相结合的规划法,可以得到较好的效果,但就是对于复杂程度相对较高的目标轨迹曲线,直线段加上圆弧段的组合在曲线精度面并不理想,常常难以满足用户需求。而且,将复杂程度高的轨迹曲线划分为若干段微小的直线与圆弧的组合,就是非常困难的工作,特别就是还要保证直线段与圆弧段交接点处过渡平滑的条件。因此,对于任意复杂的轨迹,使用更高级的插值法势在必行,要保证较高的插值精度,往往需要给定更密集的插值点序列,而且,如果使用的就是多项式插值法,为获得高精度,多项式的次数也需要高。但就是,当次数高时,会产生龙格现象,即在插值区间两端,会产生剧烈的震荡现象,导致插值点不收敛于目标轨迹。三次样条就就是通过全部样点且具有连续二阶导的函数,所以采用三次样条轨迹规划的法能够达到要求的效果。

　第四章

　总结与展望

　空间机械臂作为就是一个机、电、热、控一体化的高集成度的空间机电系统,有着广阔的应用前景与很强的技术牵引与带动作用。它的的发展将拓展并支撑空间飞行器的在轨操作与任务完成能力,同时可以带动相关行业以及技术领域的发展。启动空间机械臂的工程性研究,对于我国载人航天具有极其重要的意义。本文深入分析空间机械臂的自身运动特点之后,建立了机械臂的运动学模型,分析了机械臂直线运动、末端运动以及基于改进的人工势场法的避障路径规划,设计

　开发了机械臂仿真软件,并对路径规划进行了仿真与验证。所得到的研究成果主要体现在以下几个面:

　 1.深入研究了机器人运动学理论,针对课题研究的机械臂,建立了机械臂的正逆运动学程,基于穷举法提出运动学正逆解的验证法,用于验证正逆解算法的精度及准确性。

　 2.研究了机械臂直线运动、末端运动的路径规划算法,并提出了衡量直线度的概率统计指标,来描述机械臂直线运动算法的精度及性能。

　 3、 研究了基于改进人工势场法的避障路径规划算法,并在算法仿真基础上,分析了位置增益系数对规划算法路径的影响,实现了机械臂末端障碍规避的路径规划。

　 4.基于 VC 与 OpenGL 设计开发了机械臂仿真软件,实现了路径规划算法,并在机械臂仿真软件平台上对直线运动、末端运动、避障进行了三维仿真与验证分析,仿真结果表明该系统能够满足课题所要求的功能与性能指标。

　 对应用于空间飞行器上的空间机械臂,本文限于时间与条件限制,在路径规划算法中假定飞行器就是固定的,未考虑机械臂运动与运动基座的动力学耦合,这在实际的空间应用中将会增大空间飞行器燃料的消耗;在机械臂的路径规划算法面研究更优化的算法;基于改进人工势场法的避障路径规划算法还可在臂杆上设置更多的目标标记点,用于实现机械臂整体结构的避障研究。

　参考文献

　[1] 于 登 云 , 京 , 马 兴 瑞 . 空 间 机 械 臂 技 术 及 发 展 建 议 . 航 天 器 工程.2007,16(4):1-8. [2] 畅,唐立军,定祥,贺慧勇,司妞,涛.六轴机械臂在冰箱能耗监测线中的轨迹分析[J].电子科技.2014、04. [3] 红强.工业机器人的时间最优轨迹规划[D].:大学,2004. [4] 马强.六自由度机械臂轨迹规划研究[D].:工程大学,2007. [5] 亮,马江,阮晓钢.六自由度机械臂轨迹规划与仿真研究[J].控制工程.2010、03. [6] 卢君宜.基于结构参数误差补偿的农业采摘机械臂的轨迹规划[D].:工业大

　学,2010. [7] 好明.156R 关节型机器人轨迹规划算法研究及仿真[D].:理工大学,2008. [8] 解 本 铭 , 伟 . 打 磨 机 械 臂 的 轨 迹 规 划 与 仿 真 [J]. 中 国 民 航 大 学 学报.2010,28[4]:1-2. [9] The Universal Encyclopedia of Machines,1974. [10] 习雷平,自力,田庆民.地面无人作战系统机械臂运动学建模与仿真.军械工程学院学报[J].2012,01. [11]Taylor L W 、 Continuum Modeling of the Space Shuttle Remote Manipulator System、 Proc、of IEEE Int、 Conf、 on Decision and Control、 1992:626~631 [12]树国,蔡鹤皋.空间智能机器人地面实验综合平台实时仿真系统.中国宇航学会 机 器 人 学 术 会 议 ( 空 间 机 器 人 专 题 ) 论 文 集 . 工 业 大 学 机 器 人 研 究所.1992:49~52. [13]新松.机器人学导论.科学技术出版社.1994:511~516,543~554. [14]Brooks R A、 Solving the Find - path problem by good representation of free space、 IEEE Trans on Sys Manand Cybern、 1983,13(3):190~1971 [15]树栋,曲彦宾.遗传算法在机器人路径规划中的应用研究.西北工业大 学学报.1998,16(1):79~83.

推荐访问： 轨迹 规划 机械