1、浙江大学理学院硕士学位论文机械臂运动学算法设计姓名：范叔炬申请学位级别：硕士专业：运筹学与控制论指导教师：杨启帆20080520摘 要运动学逆解问题在机械臂控制系统中具有相当重要的地位。本文利用DH方法建立机械臂的数学模型，提出具有普适性的关节单元模型，简化了对机械臂的描述。针对对某型号6关节机械臂设计了代数解法，几何解法，以及数值解法等解决了运动学求逆解的问题。在关节角取值存在最小改变量的情形下，对算法产生的误差进行了讨论，推导出最大理论位置误差的表达式。将各种逆解算法相结合，设计了混合代数(mAA)算法，混合代数数值(mAN)算法。通过实例计算表明，改良后的混合算法可以有效的提高精度。关键
2、词：机械臂，运动学，逆解，精度AbstractThe analysis of inverse kinematics solution plays a fundamental role in the systemcontrol of manipulatorsThis paper uses the Devavit-Hartenberg method to model the4 by 4 closedform matrix equation of general manipulatorsA model of joint cell isproposed to simplify the descript
3、ion of manipulatorsAlgorithms based on algebra，geometry and numeral are designed independently for a 6R manipulator to solve theinverse kinematicsDiscuss the error of the algorithms when the angles of jointvariable shouldnt be changed continuous，but discreteAnd the expression of themaximal theoretic
4、 error is deduced as wellTo improve the precision of inversesolution，we combine the algorithms to obtain the new one named mixed-algebra(mAA)and another one named mixed-algebranumeral(mAN)Key Word：manipulator,kinematics，inverse solution，precisionII浙江大学硕士论文机械臂运动学算法设计11引言第1章绪论机器人技术是综合了机械学、计算机、控制论、信息和传
5、感技术、人工智能、仿生学等多学科而形成的高新技术，是当代研究十分活跃，应用日益广泛的领域，机器人的应用情况，是一个国家工业自动化水平的重要标志【l】。工业机器人(又称关节式机器人，机械手，机械臂等)由操作机、控制器、伺服驱动系统和检测传感器装置构成，是一种仿人操作、自动控制，可重复编程、能在三维空间完成各种作业的机电一体化自动化生产设备。它对稳定提高产品质量，提高生产效率，改善劳动条件和产品的快速更新换代起着十分重要的作用。12机械臂简介机械臂是一个特殊的机电一体化的设备，从控制观点来分，机器人系统可以分成四部分：机器人、控制器、环境和任务【2】o机器人是由臂(连杆)、关节和末端执行装置(连接
6、工具)构成。控制器是个专用计算机，相当于机器人大脑，它以计算机程序方式来完成给定任务。环境是指机器人所处的周围环境，即机器人遇到的一些障碍物及其它们之间的相互关系等。任务是机器人要完成的操作，它需要用适当的程序语言来描述，并把它们存入控制计算机中。13机械臂的空间描述工业机器人的机械臂是一系列旋转或移动关节相连接的开链式杆件机构，一端通过支柱固定在机座；另一端自由，可实现装配、焊接等各种操作任务。为了能够描述机械臂的各连杆的空间位姿和它们之间的相对位姿，在机械臂的每一杆件固联上一个坐标系，利用齐次变换，来描述其相对位姿和相对运动。新式的工业机器人都是以关节坐标直接编制程序的，物体在工作空间内的
7、位置以及机器人手臂的位置，都是以某个确定的坐标系来描述的；而工作任务则是以某个中间坐标系来规定的，由笛卡尔坐标系来描述工作任务时，必须把上述这些规定变换为一系列能够由手臂驱动的关节位置，确定手臂位置和姿态的各关节位置的解答，即运动方程的求解。机器人运动学是专门研究物体运动规律，而在浙江大学硕士论文机械臂运动学算法设计研究中不考虑产生运动的力和力矩，它涉及到运动物体的位置、速度、加速度和位置变量对时间的高阶导数。机器人操作臂有两个基本问题：正向运动学和逆向运动学。根据己知的各个关节的角度值来求解机械手末端执行器的坐标位置，称为机械手的正向运动学问题：而根据末端执行器的坐标位置来计算机械手各个关节
8、的角度值，这就是所谓的机械手的逆向运动学问题，它是一个反过程。机器人是主动机械装置。一般情况下，它的每个自由度都是由一个单独的执行机构驱动，从控制观点来看，机器人代表了多变量的非线性的自动控制系统，每个控制任务本身就是一个动力学任务。动力学是研究各关节驱动力(矩)与终端操作装置的位移、速度和加速度之间的关系。动力学也有两个相反问题：正向动力学问题，逆向动力学问题。14机械手运动学逆解的研究现状机械手运动学逆解是已知末端执行器的坐标位置(或位姿)来计算机械手各个关节的角度值，也就是所谓的机械手的逆向运动学问题，它是正向运动学问题的反过程。正向运动学问题求解简单且其解唯一；而逆运动学问题的求解则复
9、杂，可能存在多解也可能无解。位置逆解问题是机械臂机构学乃至机械臂学中的最基础也是最重要的研究问题之一，它直接关系到机械臂运动分析、离线编程、轨迹规划和实时控制等工作。因为速度和加速度分析都要在进行位置分析的基础之上才能进行，所以位置逆解问题是机械臂运动规划和轨迹规划的基础，只有通过运动学逆解把空间位姿转换为关节变量，才能实现对机械臂末端执行器的控制。位置逆解的复杂程度往往与机械臂的结构有很大关系。由于一般情况下，六个自由度便可满足机械臂在工作空间内可达任一位姿，因此六自由度机械臂最具有研究价值和实用价值。如果机械臂的结构尺寸有些特殊，如轴线平行或相交或轴线长度为零等情况下，逆解运算相对比较简单
10、；而如果结构尺寸一般，且6个关节又都是转动副，则逆解运算较为困难，该问题被喻为是空间机构运动分析中的珠穆朗玛峰。无论是结构特殊还是一般，仅仅用某种方法求得6自由度机械臂的位置逆解不是不够的，还要兼顾计算效率，计算精度等。机械臂的位置逆解问题一般最终都归结为求解非线性方程组的问题。非线性方程组的求解方法有很多，主要包括数值方法和代数方法。2浙江大学硕士论文机械臂运动学算法设计在位置逆解问题中常用的数值方法主要包括牛顿拉夫森法、优化算法，遗传算法等方法。数值方法求解一般是先建立包括若干个未知量的一个方程组，然后提供一组初始值，再利用各种优化法进行迭代，使之逐步收敛于机构的一组解。这一类方法的优点是
11、求解过程比较简单，但是在计算中需要提供适当的初始值，因此涉及到初始值的选取问题。另外，采用数值方法不能根据方程组的情况来确定机械臂机构有多少组解，也很难得到全部解。在位置逆解问题中常用的代数法主要包括析配消元法，聚筛法，Groebner基法和吴文俊消元法。这些代数方法求解一般是先建立若干个关系式，然后进行消元，最终得到只含有一个变量的一元高次方程，求解该方程得到变量的全部根。然后对应此变量求出一系列的中间变量(被消去的变量)。在该过程中，只要保证各个步骤都是同解变换，就能够保证得出全部的解，而且不产生增根。这一类方法的优点是可以解出全部解，而且不需要初始值，但是求解过程较为复杂，有一定的难度。
12、对于六自由度机械臂的位置逆解问题，有许多学者作了大量的研究工作。PaulRP在文献【2中提出的解析算法对后来的机器人逆运动学研究起到了指导作用，但是这种方法需要大量使用矩阵逆乘运算，应用时比较复杂。毕洁明等采用位置和姿态分别迭代的数值算法进行分析，可以快速求得全部解，但是当机械臂末端位置和姿态高度藕合时会造成迭代过程发散，求解失败。Regnierl3】等根据分布式人工智能的概念，提出了一种新的数值方法，采用此迭代和分布式的算法，能够求出6R，SRIP，4R2P和3R3P结构6自由度机械臂的位置逆解全部解。廖启征【4】将位移封闭方程由三角函数形式转化为复指数形式，通过lO个方程求出一般6R机械臂
13、没有增根的全部逆解。于艳秋【5l将有理数逼近实数和三角函数的理论引入机械臂位置逆解算法中，提高了计算精度以及运算当中处理异常情况的能力。张智【6】等引进了免疫遗传算法思想，也取得了较为满意的结果。盛党红等结合神经网络和免疫遗传算法对运动续逆解进行研究，具有较高的求解精度。15本文研究的主要内容本文不考虑动力学以及机械控制的具体实现细节，主要针对机械臂的运动学问题进行研究，目标如下：1运用DH方法对机械臂建立数学模型，提出具有普适性的关节单元模型，从3浙江大学硕士论文机械臂运动学算法设计而可以将所有机械臂看成是关节单元的简单拼接。2为某型号6关节机械臂设计了代数算法，几何算法，数值算法等多种方案
14、用以解决运动学逆问题，并通过编程验证算法的正确性。3在关节角最小取值受限的情况下，对运动学逆解算法的误差进行了分析，推导出算法求解的最大理论误差。4将多种算法相结合，设计出混合算法，有效的提高求解精度。4浙江大学硕士论文机械臂运动学算法设计第2章机械臂运动学数学基础机械臂是机器人系统的机械运动部分，由它来实现既定的运动。本章首先介绍坐标系空间变换的数学理论；接着对机械臂的结构进行分析，提出结构单元模型；采用DH方法【5】求出相邻关节单元的位姿变换矩阵；最后给出机械臂的运动学方程。21机械臂数学模型目前一般用DH方法建立坐标系，进而用矩阵来建立机器人机械臂的数学模型。这种数学描述是以四阶方阵变换
15、三维空间点的齐次坐标为基础的，能够将运动、变换和影射与矩阵运算联系起来。机械臂在空间的描述将用位置和姿态来表示。211机械臂位姿描述(1)位置描述在三维直角坐标系中，一般用一个3 X 1的位置矢量来表示空中的点。如在下图的坐标系A)中，空间点P可表示为P=(只，Pv，只)71。本文中为了计算的方便，将用41的齐次坐标来表达，即P=(只，只，只，1)7。X图21空间点的表达(2)空间方位描述为了研究机械臂的运动与操作，不仅要表达物体的位置，有时还要表示物体5浙江大学硕士论文机械臂运动学算法设计B的方位，设置一直角坐标系B与此刚体固接。用坐标系B的三个单位向量i，歹，三，相对于参考坐标系A)的方向
16、余弦组成的3 X3矩阵来表示B相对于i)间的变换矩阵有如下关系：i丁=“：丁。篇r，三丁 (2-20)变换矩阵的性质机械臂运动学方程就是要建立机械臂末端坐标系n关于基坐标系O)的变换方程。根据(2-9)，只需要将n个关节的变换矩阵依次相乘即可。：丁=oI丁：；丁。二丁 (2-21)10浙江大学硕士论文机械臂运动学算法设计第3章机械臂运动学算法的设计与分析机器人运动学专门研究机械臂的运动规律，而在研究中不考虑产生运动的力和力矩等。机器人运动学问题包括两类：正向问题和逆问题。正问题是在给定机器人各关节角，求出机器人各关节位姿，尤其是末端的位姿。逆问题是已知末端位姿(或位置)求出满足要求的各关节角。
17、正问题具有唯一解，逆问题则相对复杂，具有多解。本章以某6R机械手为例，求解运动学的正反问题。设计了代数法，几何法，数值解法以及混合算法用以求解运动学逆问题。并针对由于关节角取值受限导致的误差问题，设计了混合算法，以有效得提高求解精度。31机械臂结构图某型号机械臂的示意图如下：图3-l机械臂结构图这种机器人一共有6个自由度，分别由六个旋转轴(关节)实现，使机器人的末端可以灵活地在三维空间中运动。为了便于分析和计算，对机器人结构进行简化如图32所示，该机械臂系统由四个关节单元连接而成，各个关节单元的参浙江大学硕士论文机械臂运动学算法设计数见表31。图中虚线表示该关节实际不存在，其对应的关节变量为常
18、数。该结构图可以更简单的表示为图33所示。-_-。_-_-_-单元2 ：一图3-2机器人结构简化图l表31机械臂结构参数图3-3机器人结构简化图2关节单元i L，mm 口f 谚 dimm1 140 -180。180。 125。125。 02 255 O。 138。138。 O3 255 270。-270。 -1335。-120。 O4 65 270。-270。 0。 O32建立变换矩阵根据结构特点求出关节单兀变换矩阵如=：丁=trans(O，0，L1)Rot(y，90。)Rot(z，1 80。)Rot(x，口1)譬r=Rot(z，q)trans(L2，0，o)Rot(x，口2)=Rot(z，岛
19、)trans(L3，0，o)Rot(x，口3)一Rot(z，岛)trans(L4，0，o)Rot(x，口4)其矩阵形式分别如下：12(31)(3-2)(33)(3-4)浙江大学硕士论文机械臂运动学算法设计=弦=矽=0 sincrl0COSOfl1 00 OcosOIsin BOOCOS幺sin020OCOSBsin包O0COS l 0sincrl 00 厶O 1-sin 01 0 L2 cosO,1cosSl 0 L2 sinOl 100 0101 j Jsin02coscr3 sin02sincr3 厶cos02COS02CO$Of_3 一cos02 sina3 L3 sin02sina3
20、CO$O3 0一sin03eoscr4cos岛coscsincr40sin03sincr4一cos03 sin嘞COSOf_4O厶COS岛厶sin岛O1从而，二丁=：丁(口。)暑丁(幺)尝丁(包，口，)丢丁(幺，口。)33运动学逆问题(一)(35)(3-6)(3-7)(38)(3-9)求解运动学方程也就是所谓机械臂的运动学逆问题，已知末端位置E(或末端位姿：丁)求口，B，口：，岛，口。，幺。这里将运动学逆问题划分为两类：己知末端位姿求关节角的逆问题和己知末端位置E求关节角的问题。本节先讨论第一类问题。如果已知末端位姿j丁，则运动学逆问题的本质就是要求解方程：丁(口。)暑丁(B)尝丁(幺，口，)
21、三丁(岛，口。)=二r (3-lo)通过对方程的分析，若不考虑各个关节角的限制，该系统具有以下性质：性质3-1：若访=(嘶，B，岛，岛，)是方程(310)的一个解，那么以下两个也是方程的解：欢=(口l，鼠，岛，口3+-，一只，口4+刀)九=(口l+万，一只，一岛，口3，一岛，口4+刀)；浙江大学硕士论文机械臂运动学算法设计由性质31，可以不断迭代，从而可以得到方程的一组解。因此只要求出方程在-90。口l90。，一180。B180。，一180。021 80。，-90。口390。，180。岛180。，-180。口4180。内的解(这些解我们叫做擅!坠鲤)，就可以通过性质31求出其它所有在关节活动范
22、围内的所有解，并通过关节角取值范围的检验排除不可能解(某些关节角度超出该角的取值范围的解)。剩下的解为实际可行的解(叫真鲤)。本文从三种不同的角度设计方程的解法。分别为代数算法(invAE)，几何算法(invGE)，数值算法(invNE)。331代数算法(invAE)将(310)重新表达如下：trans(O，0，L1)Rot(y，900)Rot(z，1 800)Rot(x，)Rot(z，q)护ans(，L2,0,0)Ro厶t(x，,。a，。z)Rot(z，,口0。2)trans(，L幺3,Rot(z 03)trans( Rot(x Rot(z。尺。x，口3 (31 1)， 厶，0，0) ，口。
23、) ，幺) ” 7=E月丁若记Rx(O)=Rot(x，0)，化(口)=Rot(z，0)Ml=trans(O，0，L1)Rot(y，900)Rot(z，1 800)；M2=trans(L2，0，0)Rot(x，口2)；M3=trans(L3，0，0)；M4=trans(L4，0，O)；，则(3一11)可以写成：MlRx(a1)比(砩)M2心(岛)M3戤(口3)Rz(a0M4戤(口4)=：r (312)将(3-12)变形为勉(幺)M2心(岛)M3戤(口3)心(岛)=戤(一口1)MI-ITRx(一口4)M4-I(313)nx oj以J， oJ，n= ozO Oal Exay Eya：EzO l将(3
24、13)用矩阵形式表示为：14(3-14)其中浙江大学硕士论文机械臂运动学算法设计减U12 Ui3UUU001 0122 23 24 I I心1乩：乩，U。J l坞tJ IK33圪3OK。圪4圪。1Ull=cos(Ol十幺)cos岛一sin(Ol+砬)cosa3 sin03；U12=-cos(O,+02)sin03一sin(01+02)cosa3 cos03；Ui3=sin(01+02)sina3；U14=L2 cosOl+L3 cos(O,十岛)；U2l=sin(01+晚)cos岛+eos(01+幺)COStZ3 sin03；U22=-sin(Oi+02)sin03+cos(01+a2)cos
25、a3 cos03；U23=-cos(O,+02)sina3；u24=L2 sinOl+L3 sin(01+幺)；U3l=sin口3 sin岛；U32=sin口3 cos03；U33 2 COSOf3；U34 2 O；Kl=；巧2=OzCOSOf4。azsina4；K3=ozsina4+azCOSOf4；K4 2。nzL4+EzLl；吃l=nxsinalnyCOSO，l；2=(o。sinal一OyCOS6t1)cosa4。(axsinal。aycosal)sincq；砭3=(OxsinalOyCOS口1)sina4+(axsinal。ayCOSa)cosa4；4=(E。一L413,x)sinal
26、y-L413_y)cosal；圪I 2 nxCOS口l+nysin口l；圪2=(OxCOSal+OySinctl)cosa4-(a xCOS(zl+aySinal)sinot4；蚝3=(OxCOSt2l+OySintzl)sincr4+(axCOSal+aySincrI)cosa4；巧4=(E。一L4n。)cosal+(Ey。L4ny)sinal；1求角OCl由U34=巧4，可得：(E。-L4nx)cosaI+(EyL4ny)sinaI=0从而可以求出铲arctan(糍)+k础Z，在180。180。内有两个解。(3-15)(3-16)(3-17)若E。-L4n。0，且EyL4n，0时，,Eft
27、(317)，该方程退化，可以为任意值。ol23、JUUO，。L浙江大学硕士论文机械臂运动学算法设计2求角B，吼由U14=K4；U24=4可得：2 cosel+L3 cos(el+皖)=K4L2 sinei+L3 sin(Ol+02)=4m(319)和(3-20)移项后平方相加可得：厶2=圪42+K42+L222L2(K4 cosOl+4 sinel)解得：B=锄cc。s(!基二2L掰+妒+2七万，七zI 2、K42+K42 J其帕s缈2丽g雨14,sin(a=焘代Xo,后，可以求得：鼠+岛=sgn(哆4-L2 sinq)卸rcc。s(兰二言幽+2尼万，尼z岛=sgnc圪4-L2 sinq，锄c
28、c。s(三五一=-严一q+2七刀=sgnc伊一幺，arcc。s(当二铲+2后万，尼z，其中缈由(321)定义。3求解口3，03由Ull=Kl，U21=l，U31=圪l可得：cos(el+02)cosO,一sin(el+02)cosa3 sine,=刀：sin(ei+e2)cose,+cos(el+岛)cosa3 sine,=lsin口3 sin 03=虼l由(324)x cos(e,+岛)+(3-25)X sin(e1+幺)得：16(318)(3-19)(3-20)(321)(322)(323)(324)(325)(326)浙江大学硕士论文机械臂运动学算法设计cos03=n：cos(O,+岛)
29、+1 sin(Ol+幺)由(325)cos(O,+岛)一(3-24)sin(01+岛)得：(3-27)COS Of3 sin 03=】COS(0l+幺)一n=sin(01+02) (3-28)所以：铲一(历丽两粤丽砌砖zp29，岛=sgn(粤)arccos(n：eos(O_l+岛)+心l sin(0l+02)+2kx，kZ(330)sm(a3J4求口4由U12=K2和U13=K3得：一。 U12 2 0：COSZ4。azslno!_4U13 2 OzSina4+azCOSa4解得：口。：sgn(sin(口。)arccos竺导乎笔垃+2k万，忌z0+a，332几何解法(invGE)(3-31)(
30、3-32)(3-33)(1)求D的坐标在c3，4，中，包含了E点的坐标E=荔，以及方位尺E=耋妻茎，其中另=(毫，就是tE，坐标系中x轴相对于极坐标系tA，的方位向量，同时也是历(2)求关节角口。17浙江大学硕士论文机械臂运动学算法设计注意到A，B，C，D(图3-3)四点共面，且在一个竖直平面上，而就是。卿。A Y”X rA)坐标系中yAz平面到该平面的转角。设D在底面的射影为D，则口，就是Y轴到AD直线的转角(如图3-4所示)。(3)求6I在B点建立坐标系如图所示后，A)坐标系到B)坐标系的变换为：=trans(O，0，厶)Rot(y，90。)Rot(z，180。)，C，D点都在xBy坐标平
31、面上。其中D点坐标已知，通过坐标变换求得相对B)坐标系的坐标：口D=(：丁)-1D(3-37)设占D-(8 Dx，口60，of，Bc=(三：cosa。，三：sin81，o)7，根据三，=lcDl=lB DBcl m千导：乜：cose,一占皿)2+0：sin鼠一占B)2=三，2 (338)解得：B=锄cc。s错+9+2七石，尼zc339，划叫=瓣，COS(p=商两咿高。于是C的坐标可以确定下来。(4)求角皖坐标系B)到坐标系c)的变换矩阵为：岳丁=Rot(z，B)trans(O，0，L：)利用坐标变换可以得到D在c)中的坐标cD=(C一)-1BD (340)设c。=(cq，cB，。)r，则有：c
32、cDqx=：L厶3 csions乏 (341)求得幺=ssnccB，arcc。s(鲁)+2七万，七z c3-42，18浙江大学硕士论文机械臂运动学算法设计(5)求角口，注意到口，是轴z。到轴线zD绕着xD顺时针的转角。已知zD的方位可以用向量叉乘求得：一aD=历xDE (343)乙的方位和zB相同：a产(cosa,，sincrI，O)r (3-44)记为可以根据向量内积：i。ac=Ii|Iac|c。s吗 (345)角度的正负可以由混合积(石二。)一CD的符号决定；因此：铲唧cc赫确ccos甬卜啪 p46，(6)求角岛注意到岛是历到历绕着z。顺时针的转角。已知z。的方位和zE相同：可以根据向量内
33、积：一CD一DE=lcz)llDElcosa3=L3三4cosa3 (347)角度的正负可以由混合积(面面)一ClD的符号决定；眦惘甄cCD xDE厩ao)arecos(筹卜肛z p48，(7)求角口。注意到口。是zD到zF绕着砸顺时针的转角。已知zD的方位和zE相同：可以根据向量内积：一ZD乏=陆I医lc。s (3-49)角度的正负可以由混合积(石云)DE的符号决定；眦铲唧ccZD乏)一DE)arecos(甬卜舡z p5。，毗铿砥藕一L晌卜舡z (3-50)333数值解法(invNE)数值解法的主要思想是把；b-程求解问题转化为等价的函数优化问题，并用基浙江大学硕士论文机械臂运动学算法设计(
34、1)位娶矩阵同的距离定义位姿矩阵具有形式丁=(言：)。，其中R为(2-1)所定义的33方位矩阵，P为3 X l位置坐标向量。设有位姿矩阵：互=(鲁，疋=(台)，定义距离：d(正，正)=rlie,一尺：82+11只一只02=Pll欲02+11尸112=L2莓欲盯2+；鹾2(351)其中三=1甲IlE()|I)，即末端到基座的最大距离(一般容易知道)。(2)将方程问题化为函数优化问题关节变量用符号q，统一表示后，记矽=(g。，92，q。)，(39)n-I“以写成：EI=E4L伊J=一Br(91)Cr(q2)2丁(93，q4)丢丁(95，q6) (352)假设目标矩阵为G=(警设三丁c，=(R了P?
35、)则(3-12)可以改写为：二丁()=G (353)(3-54)goal(C)=d(二丁()，G)=r饯2+凹2 (355)矿 ，则只要方程(353)有解，则函数(355)的最小值为零。于是函数求解问题转化为函数优化问题：rain goal(C)=rRj，2+E2，I(356) ，(3)求解函数优化问题该方法的思想是：反复依次调整g。，g：，q。，使得目标函数减小到零。调整g。的方法如下：固定除g。外的其它关节角，则goal(C)可以看作是g删的一元函数，记goaZ(#)=()。该函数具有简单的形式；杪。(g。)=K。sin q。+Kb cos q。+Kc=疋2+Kb2cos(q。一汐)+K。
36、(3-57)因此可以根据函数极值的一阶和二阶必要条件求出对应的，使得该函数最小化。算法框架如图35所示。2n浙江大学硕士论文机械臂运动学算法设计34运动学逆问题(二)图3-5数值解法框架图本节讨论第二类逆问题。即已知末端位置求解关节角口l，B，口2，02，口4，幺。设P=(o，0，0，1)r，则有关系式：E=二丁P (358)通过对系统的分析发现，末端位置只和前五个关节角有关。因为关节角只影响末端方位。第二类逆问题的本质是要求解方程：丁(口。)暑丁(B)尝r(幺，口，)暑丁(岛，口。)P=21(359)浙江大学硕士论文机械臂运动学算法设计容易算得：暑丁(岛，口4)P=(厶cos03，三4 si
37、n03，0，1)7 (360)将于是(3-59)就可以改写为：丁(口1)岳丁(B)芸r(以，口3)(三4 cos03，L4 sin03，0，1)7= (361)与(310)相比，该方程具有更多的冗余度，有五个变量三条独立方程，方程有无穷多解。所以对于该方程，我们只求一个可行解。与上节相对应，求解方法也可以分为代数算法(invAP)，几何算法(invGP)，数值算法(invNP)。341代数解法(invAP)(1)算法关节变量用符号吼统一表示后，记矽=(ql g：，q。)，(3-61)o-7以写成：()=：丁(91)CT(q2)尝丁(93，q4)(厶cos q5，L4 sin q5，0，1)7=
38、 (3-62)本方法需要提供一个初始状态丸，在丸附近寻找一个可行解。以下是算法的具体求解步骤。步骤1：固定gl，92，93，94，把厂(矽)看做是关于95的函数，记厂()=A(q5)；则方程A(q5)=E有解的充要(或充分)条件为K4；K4可能是等式或不等式构成的方程组。把K4看做由ql,q2，93，q4决定的二值函数，K4=K4(91，q2，q3，q4)。若K4=true，则根据fs(q5)=E求出q；否则转入步骤2。步骤2： 固定91，92，93，把K4看做是关于94的函数，记K4=六(94)；则方程K4=月(94)=true有解的充要(或充分)条件为K3；把K3看做由ql q2，q3决定
39、的二值函数。若K3=true，则根据六(q4)=true求出q：，返回步骤1：否则进入步骤3。步骤3：固定ql q2，K3看做是关于93的函数，记K3=f3(q3)；则方程K3=(93)=true有解的充要(或充分)条件为K2；把K2看做由ql q2决定的二值函数。若K2=true则根据f3(q3)=true求出q；，返回步骤2；否则进入步骤4。步骤4：固定gl，K2看做是关于92的函数，记K2=五(92)；则方程K2=左(92)=true浙江大学硕士论文机械臂运动学算法设计有解的充要(或充分)条件为K1；把K1看做由gl决定的二值函数。若Kl=true，则根据六(92)=true求出q；，返
40、回步骤3，则进入步骤5。步骤5： Kl关于ql的函数，iE Kl=石(91)；则方程石(91)=true有解的充要(或充分)条件为Ko；Ko是一个和关节角无关的式子。若Ko=true，根据石(91)=true求出一个g返回步骤四；否则该问题无解。注：KoK4，Z(91)六(95)的具体表达式如下：K。：IBEGI：+三，+三。，其中l砜I表示目标位置到B关节的距离。K2：&l三。 ，其中乞=(吃，易，&，1)7=()-1；一L3)2+圪2一三。2o，其中e=(圪，名，足，1)7=(三丁广1；K，：IDI=厶，其中l睨f表示目标位置到D关节的距离；K。：l警l=厶，其中只：(吃，吃，1)r：(广
41、1。 I屹=0 。 。ft(q1)：圪cos ql+乞sin ql=0，其中=(匕，乞，圪，1)r=；f2(q2)：K22 吃2+易2+三22-2L2(吃cosq2其中K=+sin q2)K12，Kl=L3一K，K2=三3+K；六(93)：(名-L3 cosq3)2+(乞一L3 sinq3)2+足2 =L42：_c吼，：乏：三乏了其中乞=(三TRot(z,q3)“r阳ns(L3,0,0)_1讹)：【厶L scions吼q5：=易Pa，,，J、【，L浙江大学硕士论文机械臂运动学算法设计342几何解法(invGP)图3-6代数算法框图本节的方法与332中的几何法类似，只需再给定一个D的位置坐标就可
42、以了。D点的给定方法：在以目标P为球心，厶为半径的球面上确定一点D，满足l肋IL：+厶。满足要求的点可能很多，为了简单起见，可以用以下方法来具体确定。24浙江大学硕士论文机械臂运动学算法设计方法1：设D关节的初始位置为Do，在D0和P的连线上确定D，满足IDPI=厶，共有两个点，检验是否满足l肋IL：+，；若至少有一个点满足要求，任取一个满足要求的点，否则用方法2确定点D。方法2：在关节B和目标P的连线上确定一点D使得lD户I=厶，计算公式为：IBPI+L,L2+L,时，。=尸+厶网BP否则，D=山罱(3-63)(3-64)D的位置一旦确定下来后，口。，幺，幺，口，B的求解方法同332中的方法
43、，不再赘述。343数值解法(in讣口)同333的方法基本一致，只需修改目标函数为：goal(q6)=l E一02=AEx2+AEy2+AE：2 (365)其中E=二丁P，为求解过程中某一状态下E的坐标，为目标位置坐标。求解方法同333。35混合算法在使用计算机进行计算时，难免会产生一定的误差；尤其是机械臂关节角存在最小改变量的情况下，采用四舍五入的方法求得的解，可能存在较大误差。为了有效得控制误差，提高求解精度，需要在原算法中稍加修改，引进误差控制的规则，采用混合算法进一步提高求解精度。本文研究了两类混合算法：混合代数算法(mAA)，混合代数一数值算法(mAN)。351误差控制方法本节所列的几
44、个原则，可以有效的控制误差在最小范围。1求解关节角时，直接四舍五入精确到指定精度，而不是在求出所有关节角后再进行四舍五入。浙江大学硕士论文机械臂运动学算法设计2解方程组S1nx27时，由于存在误差原因，可能导致口：+b：1。此时I COSX=D该问题可以转化为，求X使得f(x)=(sinx一口)2+(COSX一6)2最小。可以求得，最优解为：x= 一默邳 p66，arc伽(加晰础。 卜“3采用混合算法。352混合代数算法(mAA)将代数算法(invAE)和代数算法(invAP)相结合，就得到了所谓的混合代数算法(mAA)。算法原理如下：1对于给定的位姿矩阵G，用invAE求解得到全部解，2把每
45、个解矽作为算法invAP的初始状态，以G的位置为算法invAP的目标位置，求得改良解矽。输入日标位姿G1L所有解=invAE(G)上l对每个解矽，求改进解矽：f胛谢尸(矽，圪)1 r输出所有改进解图3-7混合算法mAA流程图353混合代数一数值算法(mAN)将代数算法(invAE)和数值解法(invNP)相结合，就得到mAN算法。算浙江大学硕士论文机械臂运动学算法设计法原理如下：1对于给定的位姿矩阵G，用invAE求解得到全部解，2把每个解矽作为算法invNP的初始状态，以G的位置为算法invNP的目标位置，求得改良解矽。输入目标位姿G土 l所有解=invAE(G)上I对每个解矽，求改进解：加
46、vNP(矽，只；)1 r输出所有改进解图3-8混合算法mAN流程图354算法的理论位置误差本节讨论在关节角取值的最小精度为万时，算法求解的最大理论位置误差。位置误差定义为：err=f睨J=(e一艮)2+(0一向)2+(一兄)2 (367)其中P为实际末端位置，尼为目标位置。所有算法中以代数算法最具有代表性，因此本节主要考察代数算法invAP的位置误差。算法invAP的误差在依次计算各个关节角时，及时使用精度要求取值(四舍五入法)：用取精度后的角度值进行计算各个关节的坐标，这样子，后面的关节角就会根据前面的关节角自动作出调整，使误差不会逐级放大。假设P的值只取决于g，g。g，的误差。调节g。的目标是使得lDJ=厶；但是在关节角取值受限时，该式一般不能精27
