工业机器人实验指导书

工业机器人实验指导书张延恒郭磊北京邮电大学机电工程实验教学中心2007.91实验一一种新型3PSS并联机构的运动轨迹规划与控制1.1机器人控制的理论基础本文研制的3P-6SS并联机构实物如图1-1所示，该机构由动（上平台）、静（下平台）两个平台和三组导轨—滑块—支链组成,每条支链含有两根平行等长的杆,每组杆通过球铰链连接滑块和上下平台。滑块在伺服电机—丝杠—螺母副驱动下沿导轨作水平运动,进而使动平台运动。图3-13P-6SS并联机构仿真图图3-23P-6SS并联机构的结构简图图1-2为该3P-6SS并联机构的结构简图，在静平台上设定一个固定坐标系OXYZ，动平台上设定一个动坐标系O’X’Y’Z’，如图1-2所示，两坐标系初始位置相互平行，静平台上三条移动副轴线相交于中心O，两两夹角120度，方向设为ui，X轴与u1轴重合，滑块上球铰链中心点在固定坐标系中坐标设为Ai，动平台上球铰链中心点在动坐标系中坐标设为Bi并分布在半径为r的圆上，取AiAi+1的中点为Ci，BiBi+1的中点为Di且均匀分布在圆上，，杆长均相等设为l，如图示AiBiBi+1Ai+1(i=1,3,5)构成平行四边行。该机构是由11个构件(6个连杆，3个滑块，1个动平台，1个静平台，所以活动构件数为10=n)、6个球副(每个连杆分别与动平台及相应滑块组成球副，因此Ⅲ级副数为63=p)、3个移动副(滑块分别与静平台组成移动副，由此得Ⅴ级副数为35=p)组成。6个连杆(构件1～6)长度都为l，每个连杆都存在一个绕自身轴线转动的局部自由度，因此该机构的局部自由度数6=k。则机构的自由度[2]为：3631123106651=−×−×−×=−−=∑=kqpnFqq（式1-1）并联机器人机构运动学主要研究平台，连杆和静平台之间的互相运动关系。已知并联机2构动平台的空间位置和姿势，求连杆长度叫位置反解[3]。并联机构平台的空间位姿势设计者期望得到的,一般来说，首先确定平台位姿，接着根据平台位姿计算应该给与每个电机的位移量，控制电机运动。不断重复这一过程，平台就在空间按照设定要求运动起来,位置反解在并联机构实时控制中频繁使用。值得庆幸的是并联机构的位置反解同串联机构相比要简单得多，而且位置反解只有一个，不想串联机构有那么多个反解。所以，并联机构相对于串联机构来讲，容易控制。并联机构位置反解，是位置分析的一部分，研究快速、方便的位置反解方法对并联机构精度研究是由实际意义的。由于在图3-2（3P-6SS并联机构的结构简图）中AiBiBi+1Ai+1(i=1,3,5)构成平行四边行，则此机构在满足一定条件下可以只做三维平移运动，当给定滑块沿移动副轴线方向到中心O的距离di时，有如下关系式：()())3,2,1(2==−+⋅−+=idliiiiiiiuCCPRDCPRD(式1-2)式（1-2）中R——动坐标系相对固定坐标系的旋转矩阵，此处为3×3的单位矩阵P——动坐标系相对固定坐标系的移动矢量设为(x,y,z)由式(1)展开得0)()()(2)(222=−+++−−lddddTiTiiiTiiiTiiTiiTTiPPDDuuRDuPuPRD)3,2,1(=i(式1-3)则可得反解方程aacbbdi242−+−=(式1-4))3,2,1(=i式（1-4）中2222lcbaTiTiTTiiTiTiiTi−++=−−==PPDDPRDRDuPuuuXT——矩阵X的转置矩阵根据结构可知u1=(1,0,0)T,u2=(-1/2,3/2,0)T,u3=(-1/2,-3/2,0)T，D1=(r,0,0)T,D2=(-r/2,3r/2,0)T,D3=(-r/2,-3r/2,0)T，代入式(1-3)整理可得：32/433242/32/2/433242/32/244422223222222221zyxyxlxyrdzyxyxlxyrdzxlyrd−−+−+−−=−−−−++−=−−++=(式1-5)根据此式，给定x,y,z的轨迹曲线即可进行轨迹规划。使用伺服电机上的光电码盘作为位置反馈信号。设电机每转动一周所需要的脉冲数目0N（个/转）。使用以ARM7为核心的控制电路来完成并联机构的位置反解和对于伺服电机的控制任务。使用ARM7的3路独立PWM分别控制3个交流伺服电机。由于通用定时器1、通用定时器2、定时器3（PWM定时器）都是对系统时钟（Sys_Clk）进行计数，因此其比较输出有着相同的时基（系统时钟频率clkf），可以一起用于并联机构的多轴控制。定时器1、2、3的比较输出分别为T1PWM、T2PWM、T3PWM，这三个I/O口输出频率和占空比均可调的PWM波形（设其频率分别为：1f、2f、3f），将其作为伺服电机驱动器的脉冲输入位置信号。使用6个通用I/O口分别控制滑块1、2、3的运动方向和伺服使能。设1λ、2λ、3λ分别为通用定时器1、通用定时器2和通用定时器3对系统时钟（clkf）的分频系数。则有：iclkiffλ=，3,2,1=i。设交流伺服电机每转一周需要的脉冲数为lsP。则可以计算出：导轨i所对应的伺服电机需要的脉冲数为：1pdPnilsi=（id为导轨i需要移动的距离，3,2,1=i；1p为导轨的导程）。则导轨i完成位移id所需要的时间为：iiinft=，3,2,1=i。为使三个导轨在相同的时间内执行完反解所得的位移，以避免出现不期望的动平台位姿，则需要有如下关系成立：321ttt==即332211nfnfnf==。根据反解算法得到的三个滑块的位移，将位移最大的导轨的脉冲频率设定为能够稳定运行的PWM最高频率，另外两个导轨的PWM频率可以通过上述关系式计算出来。控制系统软件在程序启动后，首先调用初始化函数对ARM7内寄存器进行初始化，并驱动各个滑块回到初始位置。然后读取字符数组中的数据（以z,y,x座标表示的动平台中心笔端的坐标），调用位置反解函数，计算每个滑块需要运行的距离。同时计算每个滑块驱动器PWM脉冲所需要设置的频率及占空比，设置相应的通用定时器的周期与比较寄存器的初值，输出PWM脉冲控制滑块运行。当字符书写完成后，滑块回零。41.2ARM7控制机器人的实现图3-3显示了机器人控制的一个整体的控制流程。图3-3ARM7实现电机控制框图根据串口接收到的数据来控制电机的转动，串口数据的接收是通过中断的方式来实现的。一旦有数据到达，就产生一次中断，在中断服务程序中，新发送过来的数据将被保存起来，并且设置标志位new_recv为真，用以通知主任务有新的数据到达，可以调用电机驱动程序来实现电机的运动。由于串口数据数据是一位一位的发送的，例如发送数字6（二进制1010,八位二进制00001010）加上起始位停止位和奇偶校验那么一次发送的数据可以是10000101011，在接收端，按照通信约定知道发送过来的数据是6，这样一次串口通信完成。现在我们传送的是三个电机的转动圈数，以及滑块运动的方向，就会涉及到小数，比如3.2141这个时候用串口发送就需要自己定义一套合适的通信协议，使得发送双方都能理解所接收到的数据流的顺序及意义。这个与串口相关的协议的制定问题将在下一节中详细阐述。这里对串口数据进行解析的意思就是根据我自己实现定义好的串口相关通信协议对接收到的数据进行解析，从而知道各个电机的转向以及运动圈数。串口中断服务程序的流程图及核心代码如下：5图3-4串口中断服务程序流程由以上的功能模块，实现了对指定电机运动方向与运动圈数的输入转化为PWM波的输出。三路PWM波输出以及三路方向控制信号将连接到电机伺服器上，从而控制电机运动。1.3近端调试模式的实现1.3.1近端调试模式分析所谓近端调试模式是指不通过远程控制，而是利用在现场机器人附近的PC机实现对机器人的现场实时控制。即利用PC直接给ARM7的控制板发送控制命令，然后ARM7控制伺服驱动器，进而控制电机运动，从而达到机器人控制的目的。图3-5显示近端模式的整个框架。图3-5近端调试模式PC机通过串口与ARM7控制板相交互，ARM7控制板发送三路方向信号，三路电机转动圈数信号给伺服驱动器，从而控制电机转动，达到机器人控制的目的。由于远程控制中涉及到的模块很多，包括服务器，客户端，网络等，因此任何一个环节出现问题都会影响整个系统的工作性能，那么要实现远程控制的第一步是实现近端的控制，6所以近端模式的实现是机器人远程控制的一个基础。近端调试模式的实现是为了验证ARM7控制板是否能够根据指令对机器人做出有效地控制。从而确保在进一步地开发中，这个控制模块是有效的，从而提高开发速度。对于PC端，主要是按照本文定义的串口数据协议对用户所输入的指令做编译，然后再通过串口发送到ARM7上。同时，当一个指令执行完毕也会收到ARM7控制板发送过来的反馈信号，从而形成交互。1.3.2串口协议的制定与实现在上一节我们简要地分析了一下串口发送数据的机制。本文自己定义串口数据传送协议的目的是为了通信双方都能正确有效地理解所传送的数据序列。对于客户端而言，所要传送的六个量，三个方向以及三个电机运动圈数。而根据以前实验分析的结果，电机运动圈数的有效数据位是小数点后面３位，因此本文综合到考虑这些问题定制的串口数据传送协议如下：r,+.,+.,+.,上面的表格显示了整个一条指令序列的传送顺序。z首位用R来表示这是一个数据包的开始。z第二位用逗号表示接下来跟着的是一个电机轴的运动方向及运动圈数。z第三位用来表示第一个轴的运动方向，正号表示向偏离中心点的左侧运动，负号表示向偏离中心点右侧运动。z第4到10位用来表示第一根轴运动的圈数，小数点前后各三位。z依次类推，第11位以逗号开始到19位表示第二根轴的运动方向以及运动圈数，第20位到28位表示第三根轴的运动方向以及运动圈数。z最后一位以逗号结尾，表示整个数据包传送完毕，一条用户指令传送完成。如第一轴正方向转动1.123圈，二轴方向转动2.132圈，三轴保持不动；那么数据包的形式就是：r,+001.123,+002.132,+000.000,串口数据包在ARM7控制板端的解析过程如下：在上一节中关于ARM7控制板实现对机器人的控制中曾提到关于串口数据的解析问题，通过上面我们制定数据包协议的分析，下面阐述关于接收端（ARM7）接收到数据后地解析过程。7串口数据发送过来之后，中断服务程序会将数据接收到缓存中并且设置接收数据标志位。中断服务程序退出后，主程序会查询这个标志位，一旦发现接收到数据，就检查是否这个数据包的头以’r’开头并且长达29个字符，末位以逗号结束；一旦这些条件都满足，表示接收到有效的数据指令包，需要进行解析。上面的函数完成了这个解析并获取数据的功能。值得一提的是，由于串口数据发送的时候会将数字转化为相对应的ASC码，例如数字７,接收缓存rcv_buf中接收到并存放的是37,因此对每一位数据都需要将对应的ASC吗转化为数字再作计算。所以在计算电机转动圈数的时候，每一位数字都减掉了一个字符’0’,它的ASC码是30，数字0-9的ASC与自己本身的差值就是30。电机圈数的获取是这样实现的：在数据包中，每个电机轴转动的圈数是用７个位来表示的，小数点一位，它左右各三位，不足之位以零来补充。所以，电机轴转动圈数＝100*第一位数据＋10*第二位数据＋第三位数据＋第四位数据/10+第五位数据/100+第六位数据/1000。1.3.3近端调试客户端的实现近端客户端即近端PC，需要实现人机交互的界面的实现，对用户输入的指令进行整理，形成串口数据包，对串口进行操作。图3-6近端调试参数输入端图3-6显示了人机交互界面的主要部分。分别是对串口的操作以及三个轴的转动指令的输入。关于串口的所有操作都封装在一个类中，在这个类中启动了一个线程专门来监控串口，8并且基于消息/事件的机制来实现消息的传递。一旦串口接收到数据，他将给上层窗口发送一个消息，在此消息中包含接收到数据的消息以及所接收到的数据是什么。上层窗口将有一个专门的函数负责处理接收到的消息。这里之所以启动一个线程专门来监视串口是因为，windows不允许硬件中断，而我们也不可能