小型六自由度的工业机械手的控制设计

1、小型六自由度的工业机械手的控制设计1、系统设计目的及意义工业机器人，又称机械臂，在现代化工业生产中正发挥着越来越重要的作用，它被广泛应用到流水生产线上，代替人类从事焊接、喷涂、搬运等许多较繁重的劳动，这不仅大大提高了生产效率，同时也极大地提高了产品的加工精度和产品质量。随着时代的进步，机器臂技术的应用越来越普及，已逐渐渗透到军事、航天、医疗、日常生活及教育娱乐等各个领域，可以说工业机器人的应用对现代工业的发展起到了巨大的推动作用。论文围绕哈尔滨工程大学自动化学院学生创新实验室购买的教学用小型六自由度机械臂，完成对其关节控制系统的设计开发，开发内容包括硬件系统和软件系统。从而使其能够达到一定的控

2、制精度，为后续的控制算法的研究提供一个完整的平台。2、主要研究内容论文的主要内容包括：对机械臂结构的改进、安装合适的控制系统、数学建模、运动学分析，轨迹规划研究、控制算法研究和三维仿真研究等部分。图2.1 机械臂系统研究内容图2.1、机械臂结构改造创新实验室购买的教学用六自由度机械臂存在以下两个明显不足，需要对其进行较大的改动：（1）原机械臂包括爪子在内共六个自由度，实际上爪子是机械臂的末端执行器，它不影响位置和姿态，不能作为一个独立自由度；（2）原机械臂的关节执行器使用的是6个舵机，各关节处没有任何反馈装置，只能做简单开环控制，生产厂家的技术人员证实了这一点。为了完成本次论文的立题要求，需要

3、至少在一个关节上做成闭环控制。针对以上两点不足，在不破坏机械臂的前提下，作者对机械臂的结构和控制部分做了以下相应改动：（1）去掉爪部关节，另外增加两个关节，使机械臂达到六个自由度，同时利于建立其数学模型；（2）将底座舵机换成直流减速电机，增加一定的机械结构来安装角度反馈装置，从而使机械臂能够完成一定的闭环控制，满足论文的立题要求。图2.2 改装后的六自由度机械臂实物图2.2、数学建模六自由度链式(6R)机器臂是具有六个关节的空间机构，为描述末端执行器在空间的位置和姿态，可以在每个关节上建立一个坐标系，利用坐标系之间的关系来描述末端执行器的位置。一般采用Denavit-HartenBerg法(D

4、-H法)建立坐标系并推导机械臂的运动方程。D-H法是1995年由Denavit和HartenBerg提出的一种建立相对位姿的矩阵方法。利用齐次变换描述各个连杆相对于固定参考坐标系的空间几何关系，用一个4×4的齐次变换矩阵描述相邻两连杆的空间关系，从而推导出末端执行器坐标系相对于基坐标系的等价齐次坐标变换矩阵，建立机械臂的运动方程。依据D-H法，相邻两坐标系之间的关系表示为： 参数定义如下（参考图3坐标系）：表示沿轴方向轴与轴之间的距离；表示绕轴线由轴到轴所旋转的角度；表示沿轴方向轴到轴的距离；表示绕轴由轴到轴所旋转的角度。图2.3 六自由度机械臂坐标系图2.3、运动分析2.3.1正运

5、动学分析正运动学的求解过程是根据已知关节变量，求末端抓持器相对于参考坐标系的位姿的过程。要对机械臂进行分析，首先要对机械臂建立坐标系，其坐标系如图3所示，各个关节变量分别是：=0，=0，=0，=0，=90，=0。将参考坐标系设在6R机械臂的基座上，于是可以从基座开始变换到第一关节，然后到第二关节,最后到末端抓持器。若把每个变换定义为，那么6R机械臂的基座和手之间的总变换为：A0为基座坐标系到坐标系0（关节一）之间的变换矩阵；A1为坐标系0到坐标系1之间的变换矩阵；A2为坐标系1到坐标系2之间的变换矩阵；A3为坐标系2到坐标系3之间的变换矩阵；A4为坐标系3到坐标系4之间的变换矩阵；A5为坐标系

6、4到坐标系5之间的变换矩阵；AH为坐标系5到坐标系H之间的变换矩阵。 根据六个关节角度以及机械臂的参数，通过以上7个矩阵相乘即可得出期望矩阵： 列向量，表示姿态，表示空间位置。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 L

7、4 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-

8、H建模方法，robot函数将各连杆连接起来，q向量表示各关节的初始角度，均为弧度值，drivebot是绘制机械臂函数。现设定：L1=2,L2=4,L3=4,L4=2,L5=2;=2.5632，=1.2145，=1.0256，=1.2105，=1.5809，=1.1078。仿真结果如图：图2.4 基于MATLAB Robot Toolbox工具箱的机械臂正运动学仿真正运动学的仿真数据在实际机械臂上运行结果如下：图2.5 机械臂正运动学实物图2.3.3逆运动学分析逆运动学的求解过程是根据已知的末端抓持器相对于参考坐标系的位姿，求关节变量，的过程，它是机器人运动规划和轨迹控制的基础，也是运动学最重要

9、的部分。然而运动学逆解的求解要比正解求解复杂得多，要建立通用算法是相当困难的，许多人为此付出巨大的努力，做了大量的工作。有关机器人运动学逆解的求解方法很多，其中主要有解析法、几何法、符号及数值方法、几何解析法。由于逆运动学求解非常复杂，这里不再给出，只是引用已有结论，在后面的三维仿真和运动规划时使用。给出机械臂期望位姿为： 可以计算出各关节角的解析式为： 或 或 六自由度机械臂在相同的位姿下可能有8组解，根据实际情况选择其中最合适的一组解即可。2.3.4逆运动学仿真同样利用MATLAB中的Robotics Toolbox工具箱，先建立六自由度机械臂模型，再使用逆解函数ikine求出期望位姿下的

10、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

11、.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相同。程序运行后：q1 =4.9430 -4.4431 18.0240 1.9242 -10.9855 8.3910将q1转换到-pi，pi可以发现q与q1完全相同。图2.6 基于MATLAB Robot Toolbox工具箱的机械臂逆运动学仿真逆运动学仿真数据在实际机械臂上的运行结果

12、如下：图2.7 机械臂逆运动学实物图2.4、轨迹规划研究轨迹是机械臂位姿关于时间的函数，轨迹规划是给出一条无冲突路径，求出沿这条路径机械臂的位置和姿势的时间经历。轨迹规划既可以在关节空间也可以在笛卡儿空间中进行。在关节空间中进行轨迹规划优点是直接用运动时的受控关节变量来规划轨迹，这样容易做到实时控制，缺点是难以确定各杆和末端抓持器的位置。而这正是面向笛卡儿坐标空间轨迹规划的优点，但是现在还没有可用于笛卡儿坐标测量末端抓持器位置的传感器，所以算法都是建立在关节坐标基础上的。论文将分别讨论关节空间中的三次多项式插值算法和笛卡尔空间中的空间直线插补算法。2.4.1关节空间三次多项式插值轨迹规划三次多

13、项式共有四个待定系数，需要四个约束条件来唯一确定。如果把三次多项式看做关节角度的时间函数，那么三次多项式的一阶倒数可以看作关节角速度的时间函数。为求出，四个系数，同时对起始点和目标点的角度和角速度给出约束条件，列出四个方程式：将起始时间设为0，即=0，解得系数为： 对于起始点和目标点来说，它们的速度为零，即=0，=0,带入解得：将以上四个系数带回原三次多项式即可求出关节角的时间函数。但是，三次多项式的约束条件只考虑了角度和角速度，没有考虑角加速度，因此三次插补算法不能保证角加速度的连续性和平滑性。为了获得更好的控制特性，需要保证角加速度的平滑性，因此可以采用五次多项式插值算法。2.4.2笛卡尔

14、空间直线插补轨迹规划空间直线插补是给定直线始末两点的位姿，求轨迹中间点(插补点)的位姿。直线插补时，机械臂的姿态变化按照给定的步长从初始姿态均匀向末端点姿态变化。己知直线始末两点的坐标值为， 可以通过以下步骤进行直线轨迹的定步长插补:（1）首先给定插补周期；（2）求两点空间之间的距离L:（3）求出插补总时间；（4）计算插补次数N： 或 （5）计算插补增量：（6）计算第i个插补点的坐标值：（7）求出齐次变换矩阵，用齐次变换矩阵乘已新坐标下的点坐标，就可以得到空间点相对于基坐标系下的坐标值。再经过运动学反解，就可以得到各个点的关节角。2.5、轨迹规划三维仿真为了验证以上规划算法的正确性，利用基于V

15、c+MFC和OpenGL的三维仿真工具，对三次插补算法和空间直线插补算法进行了三维仿真，仿真过程如下： void display() / 建立机械臂三维模型 glClear (GL_COLOR_BUFFER_BIT); glMatrixMode(GL_MODELVIEW); glPushMatrix(); /保存原点坐标 base();arm_1();arm_2();arm_3(); arm_4();arm_5(); zhua(); glPopMatrix(); / 还原原点坐标 glutSwapBuffers();图2.8 六自由度机械臂三维仿真图图2.9 三维仿真控制界面轨迹规划代码太多，

16、这里不再给出，可以参见附件，里面有详细的代码。仿真结果表明，关节空间轨迹规划只能使机械臂末端执行器在两点间平滑过渡但不能保证运行路线，而直线插补可以使机械臂末端执行器按照直线路径在两点间过渡。2.6、控制系统硬件设计控制系统的主要任务是接收上位机根据机械臂期望的终端位置坐标，通过轨迹规划和运动学逆解得出的各关节角度，以接收的角度作为期望角度，通过系统控制器的控制，使各关节达到预期的位置，从而完成机械臂终端位置的改变。硬件系统主要包括STC89C52RC主控电路、电机驱动电路、角度检测电路、通信电路等四部。图2.10 机械臂控制系统硬件原理框图2.6.1主控制器电路单片机是数字集成器件，正常工作

17、时离不开时钟，因此外围电路必须提供适当的时钟电路；另外还要有复位电路，以便单片机工作中可以正常复位。这些就构成了单片机正常工作的最基本条件单片机最小系统。为了通信方便，该最小系统采用11.0592MHz的时钟晶振，时钟电路电容33pf；复位电路电解电容10uf，电阻值1k。原理图如下：图2.11 控制器最小系统原理图2.6.2电机驱动电路机械臂底座关节使用的是直流减速电机，要想对直流电机进行控制，需要有专门的直流电机驱动器，以实现PWM控制。STC89C52RC单片机自身没有集成的PWM模块，因此需要外接电机驱动芯片。综合分析市场上常用的直流电机驱动芯片的性能和价格后，选择L298n作为机械臂

18、底座电机的驱动器。L298n管脚图如图2.12，功能表见表2.1。图2.12 L298n 管脚图 表2.1 L298n功能表En In1 In2 运转状态 0 停止 1 1 0 正传1 0 1 反转1 1 1 刹停1 0 0 停止电机的控制信号由单片机产生，而电机的驱动电路直接引入16v的电压，如果电路出现问题，电流会直接进入单片机，对单片机和其他外围控制电路造成损害，所以所有的控制信号以及反馈信号必须由光电隔离器件进行隔离，使控制电路和驱动电路隔离开来，从而形成对控制电路的保护。单片机光耦 VCC VSSPWM GNDEN OUT1 OUT2电源电机 图2.13 驱动电路与单片机的接口框图实

19、际接口电路中可能存在外部干扰，可行的解决办法是增加滤波电路，即在PWM信号端增加对地滤波电容，电容大小视具体情况而定。如果干扰很小则可略掉滤波电路。下图为接口电路的原理图，PWM信号可由5脚7脚产生，也可由使能端6脚产生，具体由软件程序决定。图2.14 驱动模块与单片机的接口电路原理图2.6.3角度检测电路位置检测元件是控制系统的一个重要组件，由控制理论知，系统反馈通道的误差将直接影响系统控制精度。考虑到机械臂系统的实际情况，决定采用高精度旋转电位计作为角度传感器，电位计测量电机转角，输出05v的模拟电压，而单片机处理的是数字信号，因此需增加A/D转换模块，将采样模拟信号转换为单片机可以处理的

20、数字信号。旋转电位计采用WDD35D-4，AD采样模块采用ADC0809模数转换芯片。图2.15 电位计原理图图2.16 ADC0809管脚图ADC0809内部没有时钟电路，其逐次比较使用的时钟须由外部电路经CLK管脚提供，时钟频率400kHz640kHz之间，典型值为500kHz。单片机使用的晶振为11.0592MHZ，单片机内部将该频率六分频后由ALE脚输出，大小为1.8432MHZ。该频率可作为源时钟频率，经三次分频电路分频后即可得到合适的时钟频率。74LS161十六进制计数器和74LS00与非门电路组合成三进制计数器可作为三次分频电路使用，电路原理图如图：图2.17 ADC809时钟电

21、路原理图单片机P2.7脚接ADC0809的ST脚，用于启动模数转换；P2.6脚接ADC0809的EOC脚，用于查询转换是否结束；P2.5脚接ADC0809的OE脚，用于控制是否允许数据输；P3.5，P3.6，P3.7脚选择模拟通道（程序中选择的是0通道），转换结束后数据送到单片机P0口。接口电路原理图如图2.18图2.18 ADC0809与单片机接口图2.6.4通信电路通信电路的主要任务是接收上位机（PC）的控制信号并将采集到的角度信号送到上位机处理。单片机和PC机之间的通信常采用串行异步通信，采用这种通信方式的主要优点是设备简单，控制容易；缺点是传输速率较慢，但是对于一般的传输速度（例如96

22、00bit/s）这种通信方式完全可以满足。异步工作模式USART是进行产品开发和系统设计中最常用的模式，这种模式就是常说的RS232C。RS232C电平是负逻辑电平（逻辑0：5V15V，逻辑1：15V5V），PC的串口就是RS232C电平的。而单片机的串口是TTL电平的，TTL为正逻辑电平（带负载时，逻辑1：+5V+12V，逻辑0：-5V-12V。不带负载时，逻辑1：2.4V，逻辑0：0.4V），所以单片机和PC机进行通信时，两者之间必须有一个电平转换电路。常用的RS232C电平和TTL电平转换芯片是MAX232，封装结构如图2.19：图2.19 MAX232封装图我们在这里采用了三线制连接串

23、口：PC机的9针串口只连接其中的3根线：第5脚的GND、第2脚的RXD、第3脚的TXD。连接方法：MAX232的第10脚和单片机的P3.1连接，第9脚和单片机的P3.0连接，第15脚和单片机的GND连接。如图：图2.20 MAX232接线图2.7、控制系统软件设计2.7.1电机驱动程序电机驱动程序是指PWM信号的生成和调制程序。驱动器L298n需要三个输入，即正反转控制和使能端控制，分别接单片机的P2.0、P2.3、P2.2管脚。PWM信号由使能端P2.2脚通过延时产生，改变使能端高低电平的延时时间即可对PWM信号进行调制。主要代码如下：sbit PWM=P22; / PWM输出脚sbit P

24、2_0=P20; / M1sbit P2_3=P23; / M1while(!k) PWM=1; delay_nus (speed+1000); PWM=0; delay_nus(9500-speed); 2.7.2角度检测程序角度检测程序主要指ADC0809的驱动程序，ADC0809的ST管脚接收到控制脉冲的上升沿时清空内部寄存器，并在脉冲下降沿启动AD转换；转换结束后，ADC0809在EOC输出高电平表明转换结束；单片机可以通过外部中断或查询方式访问EOC脚，获得转换结束信号后给ADC0809的OE脚一个高电平即可输出转换数据。主要代码如下：OE=0;ST=0; /启动AD转换 ST=1;

25、 ST=0;delay_nus(60);while(!EOC); /等待转换结束OE=1; /输出使能P1=P0; /数据输出2.7.3串口通信程序单片机串口通信程序使用定时器中断实现，该通信程序采用9600bit/s波特率，8位数据位，1位停止位，没有校验位。主要代码如下：void com_initialize(void) /初始化串行口和UART波特率 TMOD |=0x20; /定时器工作在方式2 TH1=0xfd; TR1=1; SCON=0xd8; ES=1; static void com_isr(void) interrupt 4 using 1 /串口中断 char c; if

26、(RI) /接收数据中断 if(TI) /发送数据中断 2.7.4上位机控制界面程序上位机控制界面的主要功能是：发送控制方式和期望角度给下位机，同时实时接收下位机采样得到的角度值，并在“绘制实时曲线区域”绘制实际曲线，用以观察控制效果和对比控制算法。由于代码太多，这里不再给出。下面只给出编制完成后的控制界面：图2.21 上位机控制界面2.7.5 系统软件流程图图2.22 系统流程图3、系统调试和结果分析3.1 硬件调试3.1.1 驱动模块调试编写一个简单的驱动程序，通过单片机的P2.0脚和P2.3脚的高低电平控制电机正反转，在P2.2脚利用延时函数产生PWM波，PWM占空比通过外部按键可调。用

27、示波器观察L298n的OUT1和OUT2之间是否产生相应的PWM波形，如果波形的占空比与预期一致且可调，说明驱动模块正确。下面是示波器观察到的波形： 图3.1 占空比50% 图3.2 占空比80% 首先，外部直流电源调节为16v，通过按键设置PWM占空比为50%。用示波器观察L298n的OUT1和OUT2脚，其波形输出如图5.1所示，占空比与设定值一致，同时用万用表电压档测量OUT1和OUT2脚间的电压，输出值为7.8v，与理论输出基本一致。然后，通过外部按键改变占空比为80%，用示波器观察上述管脚，波形如图5.2所示，用万用表测量两脚间电压为12.8v，与理论输出一致。测量结果表明，驱动电路

28、设计良好，工作正常。3.1.2 角度检测模块调试编写ADC0809的驱动程序，令ADC0809每20ms对0通道进行一次采样，用示波器观察ADC0809的ST脚，EOC脚及OE脚，看是否有信号脉冲产生， 同时观察脉冲时序是否正确。下面是示波器观察到的波形： 图3.3 ST脚 图3.4 EOC脚 图3.5 OE脚单片机通过P2.7脚向ADC0809的ST脚发送一个脉冲，在脉冲的上升沿ADC0809清空寄存器，在脉冲的下降沿启动AD采样。大约100us后采样结束，ADC0809的EOC脚发出一个高电平信号，通知单片机采样结束。接到采样结束信号后，单片机向ADC0809的OE脚发送高电平信号，表示允

29、许采样输出，采样数据送到P0口。示波器显示波形与整个采样过程的时序相同，证明采样模块设计正确。为了检测采样数据的准确性，分别测量ADC0809各数据输出管脚的高低电平，并将电平值转换为十六进制数据。同时用万用表测量电位计输出的模拟电压值，将该值转换为十六进制数据，两个数据进行对比。测试时，电位计模拟电压输出为2.7v，对应十六进制的89H，ADC0809的管脚输出电平同样为89H。实验结果表明，该采样模块设计合理，工作正常，采样数据准确。3.1.3 通信模块调试RS232串口通信的调试可以通过串口调试助手完成，为了同时检验接收与发送功能都正确，编写单片机程序，令单片机接收到上位机发送的数据后，

30、将该数据原样发回上位机，通过串口调试助手观察数据知否正确。图3.6 串口助手显示结果实验时，下位机采用的是11.0592MHZ的时钟晶振，通信波特率设置为9600bit/s，数据帧格式为1位起始位，8位数据位，1未停止位，没有校验位。串口助手的通信波特率和帧格式与单片机相同。令串口调试助手以十六进制的形式向下位机发送“12 34 56 78”，单片机接到数据后立即将数据原样发回上位机，串口助手接收数据并显示，显示结果为“12 34 56 78”。实验结果表明通信模块设计合理，能够在设定的波特率和帧格式下正常收发数据。3.2 软件调试3.2.1 下位机软件调试将下位机程序下载到单片机中，驱动一个

31、空载（未安装在机械臂上）的直流减速电机，这样做是为了设备安全考虑。在试验模糊控制算法和模糊PID控制算法的程序时，发现电机出现严重抖动，只能“爬行”旋转。仔细对比分析后发现，基于误差E和误差变化率EC的双输入模糊控制器对位置闭环控制不是很合适，特别是位置误差很大的情况。这主要基于以下原因：双输入模糊控制器可以等效为PD控制，在位置闭环控制时，起始位置误差E很大（本系统为-255 +255），位置误差变化率很小（本系统为-5 +5），这样E和EC由基本论域转换到模糊论域的过程中无疑放大了EC的作用，即微分作用被加强。我们已经知道，微分的作用主要是加快系统响应速度，但是微分作用过强会使系统出现震荡。特别是在控制的起始阶段，很容易发生因为微分作用过强而出现微分饱和的现象。针对这种情况，可以采取以下措施：1、采用双模模糊控制器，即制作两个模糊查询表。粗调控制查询表对E的加权值大，EC的加权值小；细调模糊控制表对EC的加权值大，对E的加权值小。在大误差时查询粗调控制查询表，防止微分饱和；在小误差时查询细调控制查询表，增加系统的动态性能。2、采用一维模糊控制器，即模糊控制器的输入只有误差