小型六自由度的工业机械手的控制设计1、系统设计目的及意义工业机器人，又称机械臂，在现代化工业生产中正发挥着越来越重要的作用，它被广泛应用到流水生产线上，代替人类从事焊接、喷涂、搬运等许多较繁重的劳动，这不仅大大提高了生产效率，同时也极大地提高了产品的加工精度和产品质量。

随着时代的进步，机器臂技术的应用越来越普及，已逐渐渗透到军事、航天、医疗、日常生活及教育娱乐等各个领域，可以说工业机器人的应用对现代工业的发展起到了巨大的推动作用。

论文围绕哈尔滨工程大学自动化学院学生创新实验室购买的教学用小型六自由度机械臂，完成对其关节控制系统的设计开发，开发内容包括硬件系统和软件系统。

从而使其能够达到一定的控制精度，为后续的控制算法的研究提供一个完整的平台。

2、主要研究内容论文的主要内容包括：对机械臂结构的改进、安装合适的控制系统、数学建模、运动学分析，轨迹规划研究、控制算法研究和三维仿真研究等部分。

图2.1 机械臂系统研究内容图2.1、机械臂结构改造创新实验室购买的教学用六自由度机械臂存在以下两个明显不足，需要对其进行较大的改动：（1）原机械臂包括爪子在内共六个自由度，实际上爪子是机械臂的末端执行器，它不影响位置和姿态，不能作为一个独立自由度；（2）原机械臂的关节执行器使用的是6个舵机，各关节处没有任何反馈装置，只能做简单开环控制，生产厂家的技术人员证实了这一点。

为了完成本次论文的立题要求，需要至少在一个关节上做成闭环控制。

针对以上两点不足，在不破坏机械臂的前提下，作者对机械臂的结构和控制部分做了以下相应改动：（1）去掉爪部关节，另外增加两个关节，使机械臂达到六个自由度，同时利于建立其数学模型；（2）将底座舵机换成直流减速电机，增加一定的机械结构来安装角度反馈装置，从而使机械臂能够完成一定的闭环控制，满足论文的立题要求。

图2.2改装后的六自由度机械臂实物图2.2、数学建模六自由度链式(6R)机器臂是具有六个关节的空间机构，为描述末端执行器在空间的位置和姿态，可以在每个关节上建立一个坐标系，利用坐标系之间的关系来描述末端执行器的位置。

一般采用Denavit-HartenBerg法(D-H法)建立坐标系并推导机械臂的运动方程。

D-H法是1995年由Denavit和HartenBerg提出的一种建立相对位姿的矩阵方法。

利用齐次变换描述各个连杆相对于固定参考坐标系的空间几何关系，用一个4×4的齐次变换矩阵描述相邻两连杆的空间关系，从而推导出末端执行器坐标系相对于基坐标系的等价齐次坐标变换矩阵，建立机械臂的运动方程。

依据D-H法，相邻两坐标系之间的关系表示为：1(,)(0,0,)(,0,0)(,)n n n n n n T A Rot z Trans d Trans a Rot x a q -==创参数定义如下（参考图3坐标系）：n a 表示沿n x 轴方向1n z -轴与n z 轴之间的距离；n a 表示绕n x 轴线由1n z -轴到n z 轴所旋转的角度；n d 表示沿1n z -轴方向1n x -轴到n x 轴的距离；n q 表示绕n z 轴由1n x -轴到n x 轴所旋转的角度。

图2.3 六自由度机械臂坐标系图2.3、运动分析 2.3.1正运动学分析正运动学的求解过程是根据已知关节变量1q ，2q ，3q ，4q ，5q ，6q 求末端抓持器相对于参考坐标系的位姿的过程。

要对机械臂进行分析，首先要对机械臂建立坐标系，其坐标系如图3所示，各个关节变量分别是：1q =0，2q =0，3q =0，4q =0，5q =90，6q =0。

将参考坐标系设在6R 机械臂的基座上，于是可以从基座开始变换到第一关节，然后到第二关节……,最后到末端抓持器。

若把每个变换定义为n A ，那么6R 机械臂的基座和手之间的总变换为：012345012345012345RR H H H T T T T T T T T A A A A A A A ==A0为基座坐标系到坐标系0（关节一）之间的变换矩阵；A1为坐标系0到坐标系1之间的变换矩阵；A2为坐标系1到坐标系2之间的变换矩阵；A3为坐标系2到坐标系3之间的变换矩阵；A4为坐标系3到坐标系4之间的变换矩阵；A5为坐标系4到坐标系5之间的变换矩阵；AH 为坐标系5到坐标系H 之间的cos sin cos sin sin cos sin cos cos cos sin sin 0sin cos 0000n n n n nn n nn n n n n nn n n a a d q q a q a q q q a q a q a a 轾-犏犏-犏=犏犏犏臌变换矩阵。

01000010000110001A L 轾犏犏犏=犏犏犏臌11010101001000001C S S C A 轾犏犏-犏=犏犏犏臌2220222202200100001C S C L S C S L A 轾-犏犏犏=犏犏犏臌 3330333303300100001C S C L S C S L A 轾-犏犏犏=犏犏犏臌4404444044401000001C S C L S C S L A 轾-犏犏犏=犏-犏犏臌 55050505001000001C S S C A 轾犏犏-犏=犏犏犏臌 6600660001050001H C S S C A L 轾-犏犏犏=犏犏犏臌根据六个关节角度以及机械臂的参数，通过以上7个矩阵相乘即可得出期望矩阵：0001x xx x y y y y RH z z z zn o a p n o a p T n o a p 轾犏犏犏=犏犏犏臌列向量n ，o ，a 表示姿态，x p ，yp ，z p 表示空间位置。

2.3.2正运动学仿真MATLAB 在7.1版本中增加了Robotics Toolbox 工具箱，利用该工具箱可以对链式结构的机械臂进行运动学仿真。

正运动学仿真过程如下：l1=link([pi/2 0 0 L1],'standard'); l2=link([0 L2 0 0],'standard'); l3=link([0 L3 0 0],'standard'); l4=link([-pi/2 L4 0 0],'standard'); l5=link([pi/2 0 0 0],'standard'); l6=link([0 0 0 L5],'standard'); r=robot({l1 l2 l3 l4 l5 l6}); ='6R_Rrobotarm';q=[0.430437 0.102693 -1.28542 1.7628 1.33703 -1.24998]; drivebot(r,q);l1,l2,l3,l4,l5,l6表示各连杆，L1，L2，L3，L4，L5表示连杆长度，具体值由实际机械臂参数决定，standard 表示采用标准的D-H 建模方法，robot 函数将各连杆连接起来，q 向量表示各关节的初始角度，均为弧度值，drivebot 是绘制机械臂函数。

现设定：L1=2,L2=4,L3=4,L4=2,L5=2;1q =2.5632，2q =1.2145，3q =1.0256，4q =1.2105，5q =1.5809，6q =1.1078。

仿真结果如图：图2.4 基于MA TLAB Robot Toolbox 工具箱的机械臂正运动学仿真正运动学的仿真数据在实际机械臂上运行结果如下：图2.5 机械臂正运动学实物图2.3.3逆运动学分析逆运动学的求解过程是根据已知的末端抓持器相对于参考坐标系的位姿，求关节变量1q ，2q ，3q ，4q ，5q ，6q 的过程，它是机器人运动规划和轨迹控制的基础，也是运动学最重要的部分。

然而运动学逆解的求解要比正解求解复杂得多，要建立通用算法是相当困难的，许多人为此付出巨大的努力，做了大量的工作。

有关机器人运动学逆解的求解方法很多，其中主要有解析法、几何法、符号及数值方法、几何解析法。

由于逆运动学求解非常复杂，这里不再给出，只是引用已有结论，在后面的三维仿真和运动规划时使用。

给出机械臂期望位姿为：0001x xx x y y y y RH z z z zn o a p n o a p T n o a p 轾犏犏犏=犏犏犏臌可以计算出各关节角的解析式为：15arctan()5y y x x p a L p a L q -=- 或 11q q p =+23411arctan()zx ya C a S a q =+ 或 234234q q p =+2222112342343[(5)(5)4][154]23223x x y y z z p a L C p a L S C L p L a L S L L L C L L -+--+-----=3S =?333arctan()S C q = 323431123423112343234(32)(154)3[(5)(5)4]arctan(32)[(5)(5)4]3(154)z z x x y y x x y y z z C L L p L a L S L S L p a L C p a L S C L C L L p a L C p a L S C L S L p L a L S L q ++-----+--=+-+--+---423423q q q q =--23411234511()arctanx y zx yC C a S a S a S a C a q ++=-23411234623411234()arctan()x y z x y zS C n S n C n S C o S n C o q -++=-++六自由度机械臂在相同的位姿下可能有8组解，根据实际情况选择其中最合适的一组解即可。

2.3.4逆运动学仿真同样利用MATLAB 中的Robotics Toolbox 工具箱，先建立六自由度机械臂模型，再使用逆解函数ikine 求出期望位姿下的6个关节角度值。

具体过程如下：L1=link([pi/2 0 0 2],'standard'); L2=link([0 4 0 0],'standard'); L3=link([0 4 0 0],'standard'); L4=link([-pi/2 2 0 0],'standard'); L5=link([pi/2 0 0 0],'standard'); L6=link([0 0 0 2],'standard'); r=robot({L1 L2 L3 L4 L5 L6}); ='6R_Robotarm';q=[-1.3402 1.0145 0.8256 1.0986 1.5809 2.1078]; drivebot(r,q);T=fkine(r,q);q1=ikine(r,T);fkine是求机械臂的正解函数，q向量为机械臂的六个关节角变量，ikine是求逆解函数，T为正运动学算出来的位姿，如果仿真结果正确，计算的q1应该与q相同。