（运筹学与控制论专业论文）机械臂运动学算法设计.pdf

摘要 运动学逆解问题在机械臂控制系统中具有相当重要的地位。本文利用d h 方 法建立机械臂的数学模型，提出具有普适性的关节单元模型，简化了对机械臂的 描述。针对对某型号6 关节机械臂设计了代数解法，几何解法，以及数值解法等 解决了运动学求逆解的问题。在关节角取值存在最小改变量的情形下，对算法产 生的误差进行了讨论，推导出最大理论位置误差的表达式。将各种逆解算法相结 合，设计了混合代数( m a a ) 算法，混合代数数值( m a n ) 算法。通过实例计算表 明，改良后的混合算法可以有效的提高精度。 关键词：机械臂，运动学，逆解，精度 a b s t r a c t t h ea n a l y s i so fi n v e r s ek i n e m a t i c ss o l u t i o np l a y saf u n d a m e n t a lr o l ei nt h es y s t e m c o n t r o lo fm a n i p u l a t o r s t h i sp a p e ru s e st h ed e v a v i t - h a r t e n b e r gm e t h o dt om o d e lt h e 4b y4c l o s e d f o r mm a t r i xe q u a t i o no fg e n e r a lm a n i p u l a t o r s am o d e lo fj o i n tc e l li s p r o p o s e dt os i m p l i f yt h ed e s c r i p t i o no fm a n i p u l a t o r s a l g o r i t h m sb a s e do na l g e b r a ， g e o m e t r ya n dn u m e r a la r ed e s i g n e di n d e p e n d e n t l yf o ra6 rm a n i p u l a t o rt os o l v et h e i n v e r s ek i n e m a t i c s d i s c u s st h ee r r o ro ft h ea l g o r i t h m sw h e nt h ea n g l e so fj o i n t v a r i a b l es h o u l d n tb ec h a n g e dc o n t i n u o u s ，b u td i s c r e t e a n dt h ee x p r e s s i o no ft h e m a x i m a lt h e o r e t i ce r r o ri sd e d u c e da sw e l l t oi m p r o v et h ep r e c i s i o no fi n v e r s e s o l u t i o n ，w ec o m b i n et h ea l g o r i t h m st oo b t a i nt h en e wo n en a m e dm i x e d - a l g e b r a ( m a a ) a n da n o t h e ro n en a m e dm i x e d - a l g e b r a n u m e r a l ( m a n ) k e yw o r d ：m a n i p u l a t o r , k i n e m a t i c s ，i n v e r s es o l u t i o n ，p r e c i s i o n i i 浙江大学硕士论文机械臂运动学算法设计 1 1 引言 第1 章绪论 机器人技术是综合了机械学、计算机、控制论、信息和传感技术、人工智能、 仿生学等多学科而形成的高新技术，是当代研究十分活跃，应用日益广泛的领域， 机器人的应用情况，是一个国家工业自动化水平的重要标志【l 】。 工业机器人( 又称关节式机器人，机械手，机械臂等) 由操作机、控制器、 伺服驱动系统和检测传感器装置构成，是一种仿人操作、自动控制，可重复编程、 能在三维空间完成各种作业的机电一体化自动化生产设备。它对稳定提高产品质 量，提高生产效率，改善劳动条件和产品的快速更新换代起着十分重要的作用。 1 2 机械臂简介 机械臂是一个特殊的机电一体化的设备，从控制观点来分，机器人系统可以 分成四部分：机器人、控制器、环境和任务【2 】o 机器人是由臂( 连杆) 、关节和末端 执行装置( 连接工具) 构成。控制器是个专用计算机，相当于机器人大脑，它以计 算机程序方式来完成给定任务。环境是指机器人所处的周围环境，即机器人遇到 的一些障碍物及其它们之间的相互关系等。任务是机器人要完成的操作，它需要 用适当的程序语言来描述，并把它们存入控制计算机中。 1 3 机械臂的空间描述 工业机器人的机械臂是一系列旋转或移动关节相连接的开链式杆件机构，一 端通过支柱固定在机座；另一端自由，可实现装配、焊接等各种操作任务。为了 能够描述机械臂的各连杆的空间位姿和它们之间的相对位姿，在机械臂的每一杆 件固联上一个坐标系，利用齐次变换，来描述其相对位姿和相对运动。 新式的工业机器人都是以关节坐标直接编制程序的，物体在工作空间内的位 置以及机器人手臂的位置，都是以某个确定的坐标系来描述的；而工作任务则是 以某个中间坐标系来规定的，由笛卡尔坐标系来描述工作任务时，必须把上述这 些规定变换为一系列能够由手臂驱动的关节位置，确定手臂位置和姿态的各关节 位置的解答，即运动方程的求解。机器人运动学是专门研究物体运动规律，而在 浙江大学硕士论文机械臂运动学算法设计 研究中不考虑产生运动的力和力矩，它涉及到运动物体的位置、速度、加速度和 位置变量对时间的高阶导数。机器人操作臂有两个基本问题：正向运动学和逆向 运动学。根据己知的各个关节的角度值来求解机械手末端执行器的坐标位置，称 为机械手的正向运动学问题：而根据末端执行器的坐标位置来计算机械手各个关 节的角度值，这就是所谓的机械手的逆向运动学问题，它是一个反过程。 机器人是主动机械装置。一般情况下，它的每个自由度都是由一个单独的执 行机构驱动，从控制观点来看，机器人代表了多变量的非线性的自动控制系统， 每个控制任务本身就是一个动力学任务。 动力学是研究各关节驱动力( 矩) 与终端操作装置的位移、速度和加速度之间 的关系。动力学也有两个相反问题：正向动力学问题，逆向动力学问题。 1 4 机械手运动学逆解的研究现状 机械手运动学逆解是已知末端执行器的坐标位置( 或位姿) 来计算机械手各 个关节的角度值，也就是所谓的机械手的逆向运动学问题，它是正向运动学问题 的反过程。正向运动学问题求解简单且其解唯一；而逆运动学问题的求解则复杂， 可能存在多解也可能无解。 位置逆解问题是机械臂机构学乃至机械臂学中的最基础也是最重要的研究 问题之一，它直接关系到机械臂运动分析、离线编程、轨迹规划和实时控制等工 作。因为速度和加速度分析都要在进行位置分析的基础之上才能进行，所以位置 逆解问题是机械臂运动规划和轨迹规划的基础，只有通过运动学逆解把空间位姿 转换为关节变量，才能实现对机械臂末端执行器的控制。 位置逆解的复杂程度往往与机械臂的结构有很大关系。由于一般情况下，六 个自由度便可满足机械臂在工作空间内可达任一位姿，因此六自由度机械臂最具 有研究价值和实用价值。如果机械臂的结构尺寸有些特殊，如轴线平行或相交或 轴线长度为零等情况下，逆解运算相对比较简单；而如果结构尺寸一般，且6 个关节又都是转动副，则逆解运算较为困难，该问题被喻为是空间机构运动分析 中的珠穆朗玛峰。无论是结构特殊还是一般，仅仅用某种方法求得6 自由度机械 臂的位置逆解不是不够的，还要兼顾计算效率，计算精度等。 机械臂的位置逆解问题一般最终都归结为求解非线性方程组的问题。非线性 方程组的求解方法有很多，主要包括数值方法和代数方法。 2 浙江大学硕士论文机械臂运动学算法设计 在位置逆解问题中常用的数值方法主要包括牛顿拉夫森法、优化算法，遗传 算法等方法。数值方法求解一般是先建立包括若干个未知量的一个方程组，然后 提供一组初始值，再利用各种优化法进行迭代，使之逐步收敛于机构的一组解。 这一类方法的优点是求解过程比较简单，但是在计算中需要提供适当的初始值， 因此涉及到初始值的选取问题。另外，采用数值方法不能根据方程组的情况来确 定机械臂机构有多少组解，也很难得到全部解。 在位置逆解问题中常用的代数法主要包括析配消元法，聚筛法，g r o e b n e r 基 法和吴文俊消元法。这些代数方法求解一般是先建立若干个关系式，然后进行消 元，最终得到只含有一个变量的一元高次方程，求解该方程得到变量的全部根。 然后对应此变量求出一系列的中间变量( 被消去的变量) 。在该过程中，只要保证 各个步骤都是同解变换，就能够保证得出全部的解，而且不产生增根。这一类方 法的优点是可以解出全部解，而且不需要初始值，但是求解过程较为复杂，有一 定的难度。 对于六自由度机械臂的位置逆解问题，有许多学者作了大量的研究工作。p a u l r p 在文献【2 中提出的解析算法对后来的机器人逆运动学研究起到了指导作用， 但是这种方法需要大量使用矩阵逆乘运算，应用时比较复杂。毕洁明等采用位置 和姿态分别迭代的数值算法进行分析，可以快速求得全部解，但是当机械臂末端 位置和姿态高度藕合时会造成迭代过程发散，求解失败。r e g n i e r l 3 】等根据分布式 人工智能的概念，提出了一种新的数值方法，采用此迭代和分布式的算法，能够 求出6 r ，s r i p ，4 r 2 p 和3 r 3 p 结构6 自由度机械臂的位置逆解全部解。廖启 征【4 】将位移封闭方程由三角函数形式转化为复指数形式，通过l o 个方程求出一 般6 r 机械臂没有增根的全部逆解。于艳秋【5 l 将有理数逼近实数和三角函数的理 论引入机械臂位置逆解算法中，提高了计算精度以及运算当中处理异常情况的能 力。张智【6 】等引进了免疫遗传算法思想，也取得了较为满意的结果。盛党红等结 合神经网络和免疫遗传算法对运动续逆解进行研究，具有较高的求解精度。 1 5 本文研究的主要内容 本文不考虑动力学以及机械控制的具体实现细节，主要针对机械臂的运动学 问题进行研究，目标如下： 1 运用d h 方法对机械臂建立数学模型，提出具有普适性的关节单元模型，从 3 浙江大学硕士论文机械臂运动学算法设计 而可以将所有机械臂看成是关节单元的简单拼接。 2 为某型号6 关节机械臂设计了代数算法，几何算法，数值算法等多种方案用 以解决运动学逆问题，并通过编程验证算法的正确性。 3 在关节角最小取值受限的情况下，对运动学逆解算法的误差进行了分析，推 导出算法求解的最大理论误差。 4 将多种算法相结合，设计出混合算法，有效的提高求解精度。 4 浙江大学硕士论文机械臂运动学算法设计 第2 章机械臂运动学数学基础 机械臂是机器人系统的机械运动部分，由它来实现既定的运动。本章首先介 绍坐标系空间变换的数学理论；接着对机械臂的结构进行分析，提出结构单元模 型；采用d h 方法【5 】求出相邻关节单元的位姿变换矩阵；最后给出机械臂的运动 学方程。 2 1 机械臂数学模型 目前一般用d h 方法建立坐标系，进而用矩阵来建立机器人机械臂的数学模 型。这种数学描述是以四阶方阵变换三维空间点的齐次坐标为基础的，能够将运 动、变换和影射与矩阵运算联系起来。机械臂在空间的描述将用位置和姿态来表 示。 2 1 1 机械臂位姿描述 ( 1 ) 位置描述 在三维直角坐标系中，一般用一个3x1 的位置矢量来表示空中的点。如 在下图的坐标系 a ) 中，空间点p 可表示为p = ( 只，p v ，只) 7 1 。本文中为了计算的 方便，将用4 1 的齐次坐标来表达，即p = ( 只，只，只，1 ) 7 。 x 图2 1 空间点的表达 ( 2 ) 空间方位描述 为了研究机械臂的运动与操作，不仅要表达物体的位置，有时还要表示物体 5 浙江大学硕士论文机械臂运动学算法设计 b 的方位，设置一直角坐标系 b 与此刚体固接。用坐标系 b 的三个单位向量 i ，歹，三，相对于参考坐标系 a ) 的方向余弦组成的3x 3 矩阵来表示b 相对于 关于基 坐标系 o ) 的变换方程。根据( 2 - 9 ) ，只需要将n 个关节的变换矩阵依次相乘即可。 ：丁= o i 丁：；丁。二丁 ( 2 - 2 1 ) 1 0 浙江大学硕士论文机械臂运动学算法设计 第3 章机械臂运动学算法的设计与分析 机器人运动学专门研究机械臂的运动规律，而在研究中不考虑产生运动的力 和力矩等。机器人运动学问题包括两类：正向问题和逆问题。正问题是在给定机 器人各关节角，求出机器人各关节位姿，尤其是末端的位姿。逆问题是已知末端 位姿( 或位置) 求出满足要求的各关节角。正问题具有唯一解，逆问题则相对复 杂，具有多解。本章以某6 r 机械手为例，求解运动学的正反问题。设计了代数 法，几何法，数值解法以及混合算法用以求解运动学逆问题。并针对由于关节角 取值受限导致的误差问题，设计了混合算法，以有效得提高求解精度。 3 1 机械臂结构图 某型号机械臂的示意图如下： 图3 - l 机械臂结构图 这种机器人一共有6 个自由度，分别由六个旋转轴( 关节) 实现，使机器人 的末端可以灵活地在三维空间中运动。为了便于分析和计算，对机器人结构进行 简化如图3 2 所示，该机械臂系统由四个关节单元连接而成，各个关节单元的参 浙江大学硕士论文机械臂运动学算法设计 数见表3 1 。图中虚线表示该关节实际不存在，其对应的关节变量为常数。该结 构图可以更简单的表示为图3 3 所示。 - - _ _ - 。_ - _ - - _ - - 单元2 ：一 图3 - 2 机器人结构简化图l 表3 1 机械臂结构参数 图3 - 3 机器人结构简化图2 关节单元i l ，m m口f谚d i m m 11 4 0- 1 8 0 。1 8 0 。1 2 5 。1 2 5 。0 22 5 5o 。1 3 8 。1 3 8 。o 32 5 52 7 0 。- 2 7 0 。- 1 3 3 5 。- 1 2 0 。o 46 5 2 7 0 。- 2 7 0 。 0 。o 3 2 建立变换矩阵 根据结构特点求出关节单兀变换矩阵如= ： ：丁= t r a n s ( o ，0 ，l 1 ) r o t ( y ，9 0 。) r o t ( z ，18 0 。) r o t ( x ，口1 ) 譬r = r o t ( z ，q ) t r a n s ( l 2 ，0 ，o ) r o t ( x ，口2 ) = r o t ( z ，岛) t r a n s ( l 3 ，0 ，o ) r o t ( x ，口3 ) 一r o t ( z ，岛) t r a n s ( l 4 ，0 ，o ) r o t ( x ，口4 ) 其矩阵形式分别如下： 1 2 ( 3 1 ) ( 3 - 2 ) ( 3 3 ) ( 3 - 4 ) 浙江大学硕士论文机械臂运动学算法设计 = 弦= 矽= = 0 s i n c r l 0 c o s o f l 10 0o c o s o i s i nb o o c o s 幺 s i n 0 2 0 o c o s b s i n 包 o 0 c o s l 0 s i n c r l 0 0 厶 o1 - s i n0 10 l 2c o s o , 1 c o s s l 0 l 2s i n o l1 0 00 1 0 1 j j s i n 0 2 c o s c r 3s i n 0 2 s i n c r 3厶c o s 0 2 c o s 0 2 c o $ o f _ 3一c o s 0 2s i n a 3l 3s i n 0 2 s i n a 3c o $ o 3 0 一s i n 0 3 e o s c r 4 c o s 岛c o s c s i n c r 4 0 s i n 0 3 s i n c r 4 一c o s 0 3s i n 嘞 c o s o f _ 4 o 厶c o s 岛 厶s i n 岛 o 1 从而，二丁= ：丁( 口。) 暑丁( 幺) 尝丁( 包，口，) 丢丁( 幺，口。) 3 3 运动学逆问题( 一) ( 3 5 ) ( 3 - 6 ) ( 3 - 7 ) ( 3 8 ) ( 3 - 9 ) 求解运动学方程也就是所谓机械臂的运动学逆问题，已知末端位置e ( 或末 端位姿：丁) 求口，b ，口：，岛，口。，幺。这里将运动学逆问题划分为两类：己知末 端位姿求关节角的逆问题和己知末端位置e 求关节角的问题。本节先讨论第 一类问题。如果已知末端位姿j 丁，则运动学逆问题的本质就是要求解方程： ：丁( 口。) 暑丁( b ) 尝丁( 幺，口，) 三丁( 岛，口。) = 二r ( 3 - l o ) 通过对方程的分析，若不考虑各个关节角的限制，该系统具有以下性质： 性质3 - 1 ：若访= ( 嘶，b ，岛，岛，) 是方程( 3 1 0 ) 的一个解，那么以下两个 也是方程的解： 欢= ( 口l ，鼠，岛，口3 + - ，一只，口4 + 刀) 九= ( 口l + 万，一只，一岛，口3 ，一岛，口4 + 刀) ； 浙江大学硕士论文机械臂运动学算法设计 由性质3 1 ，可以不断迭代，从而可以得到方程的一组解。因此只要求出方 程在- 9 0 。口l 9 0 。，一1 8 0 。b 1 8 0 。，一1 8 0 。0 2 18 0 。，- 9 0 。口3 9 0 。， 1 8 0 。岛1 8 0 。，- 1 8 0 。口4 1 8 0 。内的解( 这些解我们叫做擅! 坠鲤) ，就可 以通过性质3 1 求出其它所有在关节活动范围内的所有解，并通过关节角取值 范围的检验排除不可能解( 某些关节角度超出该角的取值范围的解) 。剩下的解 为实际可行的解( 叫真鲤) 。 本文从三种不同的角度设计方程的解法。分别为代数算法( i n v a e ) ，几何算 法( i n v g e ) ，数值算法( i n v n e ) 。 3 3 1 代数算法( i n v a e ) 将( 3 1 0 ) 重新表达如下： t r a n s ( o ，0 ，l 1 ) r o t ( y ，9 0 0 ) r o t ( z ，18 0 0 ) r o t ( x ，) r o t ( z ，q ) 护ans(，l2,0,0)ro厶t(x，,。a，。z)rot(z，,口0。2)trans(，l幺3,rot(z0 3 ) t r a n s (r o t ( xr o t ( z 。尺。x ，口3 ( 3 11 ) ， 厶，0 ，0 ) ，口。) ，幺) ”7 = e 月丁 若记r x ( o ) = r o t ( x ，0 ) ，化( 口) = r o t ( z ，0 ) m l = t r a n s ( o ，0 ，l 1 ) r o t ( y ，9 0 0 ) r o t ( z ，18 0 0 ) ； m 2 = t r a n s ( l 2 ，0 ，0 ) r o t ( x ，口2 ) ；m 3 = t r a n s ( l 3 ，0 ，0 ) ；m 4 = t r a n s ( l 4 ，0 ，o ) ；， 则( 3 一1 1 ) 可以写成： m l r x ( a 1 ) 比( 砩) m 2 心( 岛) m 3 戤( 口3 ) r z ( a 0 m 4 戤( 口4 ) = ：r ( 3 1 2 ) 将( 3 - 1 2 ) 变形为 勉( 幺) m 2 心( 岛) m 3 戤( 口3 ) 心( 岛) = 戤( 一口1 ) m i - i t r x ( 一口4 ) m 4 - i ( 3 1 3 ) n xo j 以j ，o j ， n =o z oo a le x a ye y a ：e z ol 将( 3 1 3 ) 用矩阵形式表示为： 1 4 ( 3 - 1 4 ) 其中 浙江大学硕士论文机械臂运动学算法设计 减u12 u i 3 uu u00 10 1 2 22 32 4i i 心1 乩：乩，u 。jl 坞t ji k 3 3 圪3 o k 。 圪4 圪。 1 u l l = c o s ( o l 十幺) c o s 岛一s i n ( o l + 砬) c o s a 3s i n 0 3 ； u 1 2 = - c o s ( o , + 0 2 ) s i n 0 3 一s i n ( 0 1 + 0 2 ) c o s a 3c o s 0 3 ； u i 3 = s i n ( 0 1 + 0 2 ) s i n a 3 ；u 1 4 = l 2c o s o l + l 3c o s ( o , 十岛) ； u 2 l = s i n ( 0 1 + 晚) c o s 岛+ e o s ( 0 1 + 幺) c o s t z 3s i n 0 3 ； u 2 2 = - s i n ( o i + 0 2 ) s i n 0 3 + c o s ( 0 1 + a 2 ) c o s a 3c o s 0 3 ； u 2 3 = - c o s ( o , + 0 2 ) s i n a 3 ；u 2 4 = l 2s i n o l + l 3s i n ( 0 1 + 幺) ； u 3 l = s i n 口3s i n 岛；u 3 2 = s i n 口3c o s 0 3 ； u 3 32c o s o f 3 ；u 3 42 o ； k l = ；巧2 = o z c o s o f 4 。a z s i n a 4 ； k 3 = o z s i n a 4 + a z c o s o f 4 ；k 42 。n z l 4 + e z l l ； 吃l = n x s i n a l n y c o s o ，l ； 2 = ( o 。s i n a l 一o y c o s 6 t 1 ) c o s a 4 。( a x s i n a l 。a y c o s a l ) s i n c q ； 砭3 = ( o x s i n a l o y c o s 口1 ) s i n a 4 + ( a x s i n a l 。a y c o s a ) c o s a 4 ； 4 = ( e 。一l 4 1 3 , x ) s i n a l y - l 4 1 3 _ y ) c o s a l ； 圪i 2 n x c o s 口l + n y s i n 口l ； 圪2 = ( o x c o s a l + o y s i n c t l ) c o s a 4 - ( ax c o s ( z l + a y s i n a l ) s i n o t 4 ； 蚝3 = ( o x c o s t 2 l + o y s i n t z l ) s i n c r 4 + ( a x c o s a l + a y s i n c r i ) c o s a 4 ； 巧4 = ( e 。一l 4 n 。) c o s a l + ( e y 。l 4 n y ) s i n a l ； 1 求角o c l 由u 3 4 = 巧4 ，可得： ( e 。- l 4 n x ) c o s a i + ( e y l 4 n y ) s i n a i = 0 从而可以求出 铲a r c t a n ( 糍) + k 础z ， 在1 8 0 。1 8 0 。内有两个解。 ( 3 - 1 5 ) ( 3 - 1 6 ) ( 3 - 1 7 ) 若e 。- l 4 n 。0 ，且e y l 4 n ，0 时，, e f t ( 3 1 7 ) ，该方程退化，可以为任意值。 o l 2 3 、j u u o ，。l 浙江大学硕士论文机械臂运动学算法设计 2 求角b ，吼 由u 1 4 = k 4 ；u 2 4 = 4 可得： 2c o s e l + l 3c o s ( e l + 皖) = k 4 l 2s i n e i + l 3s i n ( o l + 0 2 ) = 4 m ( 3 1 9 ) 和( 3 - 2 0 ) 移项后平方相加可得： 厶2 = 圪4 2 + k 4 2 + l 2 2 2 l 2 ( k 4c o s o l + 4s i n e l ) 解得： b=锄cc。s(!基二2l掰+妒+2七万，七z i2 、k 4 2 + k 4 2j 其帕s 缈2 丽g 雨1 4 , s i n ( a = 焘 代x o , 后，可以求得： 鼠+ 岛= s g n ( 哆4 - - l 2 s i n q ) 卸r c c 。s ( 兰二 言幽 + 2 尼万，尼z 岛= s g n c 圪4 - - l 2 s i n q ，锄c c 。s ( 三五一= = - 严 一q + 2 七刀 = s g n c 伊一幺，a r c c 。s ( 当二铲 + 2 后万，尼z ， 其中缈由( 3 2 1 ) 定义。 3 求解口3 ，0 3 由u l l = k l ，u 2 1 = l ，u 3 1 = 圪l 可得： c o s ( e l + 0 2 ) c o s o , 一s i n ( e l + 0 2 ) c o s a 3s i n e , = 刀： s i n ( e i + e 2 ) c o s e , + c o s ( e l + 岛) c o s a 3s i n e , = l s i n 口3s i n0 3 = 虼l 由( 3 2 4 ) xc o s ( e , + 岛) + ( 3 - 2 5 ) xs i n ( e 1 + 幺) 得： 1 6 ( 3 1 8 ) ( 3 - 1 9 ) ( 3 - 2 0 ) ( 3 2 1 ) ( 3 2 2 ) ( 3 2 3 ) ( 3 2 4 ) ( 3 2 5 ) ( 3 2 6 ) 浙江大学硕士论文机械臂运动学算法设计 c o s 0 3 = n ：c o s ( o , + 岛) + 1s i n ( o l + 幺) 由( 3 2 5 ) c o s ( o , + 岛) 一( 3 - 2 4 ) s i n ( 0 1 + 岛) 得： ( 3 - 2 7 ) c o s o f 3s i n0 3 = 】c o s ( 0 l + 幺) 一n = s i n ( 0 1 + 0 2 ) ( 3 - 2 8 ) 所以：铲一( 历丽两粤丽 砌砖z p 2 9 ， 岛= s g n ( 粤) a r c c o s ( n ：e o s ( o _ l + 岛) + 心ls i n ( 0 l + 0 2 ) ) + 2 k x ，k z ( 3 3 0 ) s m ( a 3 j 4 求口4 由u 1 2 = k 2 和u 1 3 = k 3 得： 一。 u 1 220 ：c o s z 4 。a z s l n o ! _ 4 u 1 32o z s i n a 4 + a z c o s a 4 解得：口。：s g n ( s i n ( 口。) ) a r c c o s 竺导乎笔垃+ 2 k 万，忌z 0 + a ， 3 3 2 几何解法( i n v g e ) ( 3 - 3 1 ) ( 3 - 3 2 ) ( 3 - 3 3 ) ( 1 ) 求d 的坐标 在c 3 ，4 ，中，包含了e 点的坐标e = 荔 ，以及方位尺e = 耋妻茎 ，其 中另= ( 毫 ，就是t e ，坐标系中x 轴相对于极坐标系t a ，的方位向量，同时也是历 ( 2 ) 求关节角口。 1 7 浙江大学硕士论文机械臂运动学算法设计 注意到a ，b ，c ，d ( 图3 - 3 ) 四点共面，且在一个竖直平面上，而就是 。 卿。 a y ” x r a ) 坐标系中y a z 平面到该平面的转角。设 d 在底面的射影为d ，则口，就是y 轴到a d 直线的转角( 如图3 - 4 所示) 。 ( 3 ) 求6 i 在b 点建立坐标系如图所示后， a ) 坐标系到 b ) 坐标系的变换为： = t r a n s ( o ，0 ，厶) r o t ( y ，9 0 。) r o t ( z ，1 8 0 。) ，c ，d 点都在x b y 坐标平面上。其中 d 点坐标已知，通过坐标变换求得相对 b ) 坐标系的坐标：口d = ( ：丁) - 1 d ( 3 - 3 7 ) 设占d - - ( 8d x ，口6 0 ，o f ，b c = ( 三：c o s a 。，三：s i n 8 1 ，o ) 7 ，根据三，= l c d l = l bd b c lm 千导： 乜：c o s e , 一占皿) 2 + 0 ：s i n 鼠一占b ) 2 = 三，2 ( 3 3 8 ) 解得：b = 锄c c 。s 错+ 9 + 2 七石，尼z c 3 3 9 ， 划叫= 瓣，c o s ( p = 商两咿高。 于是c 的坐标可以确定下来。 ( 4 ) 求角皖 坐标系 b ) 到坐标系 c ) 的变换矩阵为：岳丁= r o t ( z ，b ) t r a n s ( o ，0 ，l ：) 利用坐标变换可以得到d 在 c ) 中的坐标c d = ( c 一) - 1 b d ( 3 4 0 ) 设c 。= ( c q ，c b ，。) r ，则有： c c d q x = ：l 厶3c s i o n s 乏 ( 3 4 1 ) 求得幺= s s n c c b ，a r c c 。s ( 鲁) + 2 七万，七z c 3 - 4 2 ， 1 8 浙江大学硕士论文机械臂运动学算法设计 ( 5 ) 求角口， 注意到口，是轴z 。到轴线z d 绕着x d 顺时针的转角。已知z d 的方位可以用向量 叉乘求得：一a d = 历x d e( 3 4 3 ) 乙的方位和z b 相同：a 产( c o s a , ，s i n c r i ，o ) r ( 3 - 4 4 ) 记为可以根据向量内积：i 。a c = i i | i a c | c 。s 吗 ( 3 4 5 ) 角度的正负可以由混合积( 石二。) 一c d 的符号决定；因此： 铲唧c c 赫确c c o s 甬卜啪 p 4 6 ， ( 6 ) 求角岛 注意到岛是历到历绕着z 。顺时针的转角。已知z 。的方位和z e 相同：可以 根据向量内积：一c d 一d e = l c z ) l l d e l c o s a 3 = l 3 三4 c o s a 3 ( 3 4 7 ) 角度的正负可以由混合积( 面面) 一c l d 的符号决定； 眦惘甄c c d x d e 厩a o ) a r e c o s ( 筹卜肛z p 4 8 ， ( 7 ) 求角口。 注意到口。是z d 到z f 绕着砸顺时针的转角。已知z d 的方位和z e 相同：可以 根据向量内积：一z d 乏= 陆i 医l c 。s ( 3 - 4 9 ) 角度的正负可以由混合积( 石云) d e 的符号决定； 眦铲唧c c z d 乏) 一d e ) a r e c o s ( 甬卜舡z p 5 。， 毗铿砥藕一l 晌卜舡z ( 3 - 5 0 ) 3 3 3 数值解法( i n v n e ) 数值解法的主要思想是把；b - 程求解问题转化为等价的函数优化问题，并用基 浙江大学硕士论文机械臂运动学算法设计 ( 1 ) 位娶矩阵同的距离定义 位姿矩阵具有形式丁= ( 言：) 。，其中r 为( 2 - 1 ) 所定义的3 3 方位矩阵， p 为3xl 位置坐标向量。 设有位姿矩阵：互= ( 鲁 ，疋= ( 台) ，定义距离： d ( 正，正) = r l i e , 一尺：8 2 + 1 1 只一只0 2 = p l l 欲0 2 + 1 1 尸1 1 2 = l 2 莓欲盯2 + ；鹾2 ( 3 5 1 ) 其中三= 1 甲 i l e ( ) | i ) ，即末端到基座的最大距离( 一般容易知道) 。 ( 2 ) 将方程问题化为函数优化问题 关节变量用符号q ，统一表示后，记矽= ( g 。，9 2 ，q 。) ，( 3 9 ) n - i 以写成： e i = e 4 l 伊j = 一b r ( 9 1 ) c r ( q 2 ) 2 丁( 9 3 ，q 4 ) 丢丁( 9 5 ，q 6 ) ( 3 5 2 ) 假设目标矩阵为g = ( 警 设三丁c ，= ( r 了p ? ) 则( 3 - 1 2 ) 可以改写为：二丁( ) = g( 3 5 3 ) ( 3 - 5 4 ) g o a l ( c ) = d ( 二丁( ) ，g ) = r 饯2 + 凹2( 3 5 5 ) 矿 ， 则只要方程( 3 5 3 ) 有解，则函数( 3 5 5 ) 的最小值为零。于是函数求解问题转化为函 数优化问题： r a i n g o a l ( c ) = r r j ，2 + e 2 ，i ( 3 5 6 ) ， ( 3 ) 求解函数优化问题 该方法的思想是：反复依次调整g 。，g ：，q 。，使得目标函数减小到零。 调整g 。的方法如下：固定除g 。外的其它关节角，则g o a l ( c ) 可以看作是g 删的 一元函数，记g o a z ( # ) = ( ) 。该函数具有简单的形式； 杪。( g 。) = k 。s i nq 。+ k bc o sq 。+ k c = 疋2 + k b 2 c o s ( q 。一汐) + k 。( 3 - 5 7 ) 因此可以根据函数极值的一阶和二阶必要条件求出对应的，使得该函数最 小化。算法框架如图3 5 所示。 2 n 浙江大学硕士论文机械臂运动学算法设计 3 4 运动学逆问题( 二) 图3 - 5 数值解法框架图 本节讨论第二类逆问题。即已知末端位置求解关节角 口l ，b ，口2 ，0 2 ，口4 ，幺。设p = ( o ，0 ，0 ，1 ) r ，则有关系式：e = 二丁p ( 3 5 8 ) 通过对系统的分析发现，末端位置只和前五个关节角有关。因为关节角只 影响末端方位。第二类逆问题的本质是要求解方程： ：丁( 口。) 暑丁( b ) 尝r ( 幺，口，) 暑丁( 岛，口。) p = 2 1 ( 3 5 9 ) 浙江大学硕士论文机械臂运动学算法设计 容易算得：暑丁( 岛，口4 ) p = ( 厶c o s 0 3 ，三4s i n 0 3 ，0 ，1 ) 7 ( 3 6 0 ) 将于是( 3 - 5 9 ) 就可以改写为： ：丁( 口1 ) 岳丁( b ) 芸r ( 以，口3 ) ( 三4c o s 0 3 ，l 4s i n 0 3 ，0 ，1 ) 7 = ( 3 6 1 ) 与( 3 1 0 ) 相比，该方程具有更多的冗余度，有五个变量三条独立方程，方程 有无穷多解。所以对于该方程，我们只求一个可行解。与上节相对应，求解方 法也可以分为代数算法( i n v a p ) ，几何算法( i n v g p ) ，数值算法( i n v n p ) 。 3 4 1 代数解法( i n v a p ) ( 1 ) 算法 关节变量用符号吼统一表示后，记矽= ( q lg ：，q 。) ，( 3 - 6 1 ) o - 7 以写成： ( ) = ：丁( 9 1 ) c t ( q 2 ) 尝丁( 9 3 ，q 4 ) ( 厶c o sq 5 ，l 4s i nq 5 ，0 ，1 ) 7 = ( 3 - 6 2 ) 本方法需要提供一个初始状态丸，在丸附近寻找一个可行解。以下是算法的 具体求解步骤。 步骤1 ：固定g l ，9 2 ，9 3 ，9 4 ，把厂( 矽) 看做是关于9 5 的函数，记厂( ) = a ( q 5 ) ；则方程 a ( q 5 ) = e 有解的充要( 或充分) 条件为k 4 ；k 4 可能是等式或不等式构成的方程组。 把k 4 看做由q l , q 2 ，9 3 ，q 4 决定的二值函数，k 4 = k 4 ( 9 1 ，q 2 ，q 3 ，q 4 ) 。若k 4 = t r u e ， 则根据f s ( q 5 ) = e 求出q ；否则转入步骤2 。 步骤2 ：固定9 1 ，9 2 ，9 3 ，把k 4 看做是关于9 4 的函数，记k 4 = 六( 9 4 ) ；则方程 k 4 = 月( 9 4 ) = t r u e 有解的充要( 或充分) 条件为k 3 ；把k 3 看做由q lq 2 ，q 3 决定的 二值函数。若k 3 = t r u e ，则根据六( q 4 ) = t r u e 求出q ：，返回步骤1 ：否则进入步骤3 。 步骤3 ：固定q lq 2 ，k 3 看做是关于9 3 的函数，记k 3 = f 3 ( q 3 ) ；则方程k 3 = ( 9 3 ) = t r u e 有解的充要( 或充分) 条件为k 2 ；把k 2 看做由q lq 2 决定的二值函数。若k 2 = t r u e 则根据f 3 ( q 3 ) = t r u e 求出q ；，返回步骤2 ；否则进入步骤4 。 步骤4 ：固定g l ，k 2 看做是关于9 2 的函数，记k 2 = 五( 9 2 ) ；则方程k 2 = 左( 9 2 ) = t r u e 浙江大学硕士论文机械臂运动学算法设计 有解的充要( 或充分) 条件为k 1 ；把k 1 看做由g l 决定的二值函数。若k l = t r u e ，则 根据六( 9 2 ) = t r u e 求出q ；，返回步骤3 ，则进入步骤5 。 步骤5 ： k l 关于q l 的函数，i ek l = 石( 9 1 ) ；则方程石( 9 1 ) = t r u e 有解的充要( 或充分) 条件为k o ；k o 是一个和关节角无关的式子。若k o = t r u e ，根据石( 9 1 ) = t r u e 求出 一个g 返回步骤四；否则该问题无解。 注：k o k 4 ，z ( 9 1 ) 六( 9 5 ) 的具体表达式如下： k 。：i b e g i ：+ 三，+ 三。，其中l 砜i 表示目标位置到b 关节的距离。 k 2 ： & l 三。 ，其中乞= ( 吃，易，& ，1 ) 7 = ( ) - 1 ； 一l 3 ) 2 + 圪2 一三。2 o ，其中e = ( 圪，名，足，1 ) 7 = ( 三丁广1 ； k ，：i d i = 厶，其中l 睨f 表示目标位置到d 关节的距离； k 。： l 警l = 厶，其中只：( 吃，吃，1 ) r ：( 广1 。 i 屹= 0 。 。 f t ( q 1 ) ：圪c o sq l + 乞s i nq l = 0 ，其中= ( 匕，乞，圪，1 ) r = ； f 2 ( q 2 ) ：k 2 2吃2 + 易2 + 三2 2 - 2 l 2 ( 吃c o s q 2 其中k = + s i nq 2 ) k 1 2 ， ，k l = l 3 一k ，k 2 = 三3 + k ； 六( 9 3 ) ：( 名- l 3c o s q 3 ) 2 + ( 乞一l 3s i n q 3 ) 2 + 足2= l 4 2 ： _ c 吼，