CN106003029A - 一种教学用三核快速关节机器人控制系统 - Google Patents

Abstract

本发明公开了一种教学用三核快速关节机器人控制系统，所述的控制系统包括主站控制计算机和控制器，所述控制器包括DSP芯片控制器、集成芯片处理器和ARM控制器，DSP芯片控制器和ARM控制器均与所述主站控制计算机通信连接，DSP芯片控制器与ARM控制器之间通信连接，四台永磁直流伺服电机均与所述DSP芯片控制器通信连接，多个避障位移传感器、定位传感器S5、加速度传感器A1、加速度传感器A2和加速度传感器A3均与所述DSP芯片控制器和ARM控制器通信连接。本发明的教学用三核快速关节机器人控制系统计算速度快，使关节机器人手臂转向灵活、动作稳定精确、体积小巧，性能稳定，系统抗干扰能力强。

Description

一种教学用三核快速关节机器人控制系统

技术领域

本发明涉及一种教学用三核快速关节机器人控制系统, 属于装配作业的四关节机器人手臂应用领域。

背景技术

在工业生产中,工业机器人可以代替人类做一些比较单调、比较频繁和重复率较高的长时间作业，或是在危险、恶劣环境下的作业，一般用作搬取零件和装配工作，在微电子制造业、塑料工业、汽车工业、电子产品工业、药品工业和食品工业等领域得到广泛的应用，它对于提高生产自动化水平、劳动生产率和经济效益、保证产品质量、保障人身安全、改善劳动环境，减轻劳动强度、节约原材料消耗以及降低生产成本等有着十分重要的意义。

SCARA工业机器人即装配作业的机器人手臂是一种圆柱坐标型的工业机器人，它依靠旋转关节大臂和小臂来实现X-Y平面内的快速定位，依靠一个手腕移动关节和一个手腕旋转关节在Z方向上做伸缩和旋转运动，其具有四个运动自由度，该系列的操作手在其动作空间的四个方向具有有限刚度，而在剩下的其余两个方向上具有无限大刚度。这种结构特性使得SCARA机器人擅长从一点抓取物体，然后快速的安放到另一点，因此SCARA机器人在自动装配生产线上得到了广泛的应用。SCARA机器人结构紧凑、动作灵活，速度快、位置精度高，它的使用大大提高了机器人对复杂装配任务的适应性，同时也降低了成本，提高了工作空间利用率。

SCARA机器人结合了多学科知识，对于提升在校学生的动手能力、团队协作能力和创新能力，促进学生课堂知识的消化和扩展学生的知识面都非常有帮助。教学用SCARA机器人技术的开展可以培养大批相关领域的人才，进而促进相关领域的技术发展和产业化进程。但是SCARA机器人在搬运货物过程中要时刻判断主控制器输入的位置参数，并判断周围的环境时刻避障，然后由运动控制器反复控制其精确的加速和减速进行搬运货物，稍微的误差累计就有可能在多回合运动中导致运输失败。国内对SCARA机器人的使用虽然有几十年，但是由于国内工业机器人发展起步比较晚，受较多关键技术的影响，SCARA机器人的发展也受所影响，传统的机器人原理如图1所示，在长期使用期间出现众多问题：

（1）作为SCARA机器人的电源采用的是一般交流电源整流后的直流电源，当突然停电时会使整个搬运运动失败。

（2）受控制技术影响，所有的SCARA机器人均采用单核控制器，控制器的计算能力较弱，SCARA无法快速处理实时环境。

（3）对于采用步进电机和直流电机驱动的SCARA机器人来说，受电机本体影响，电源利用率较低，导致机器人耗能较高。

（4）对于采用步进电机驱动的SCARA机器人来说，经常会遇到丢失脉冲的问题出现，导致对位置的记忆出现错误。

（5）对于采用步进电机驱动的SCARA机器人来说，使得机体发热比较严重，有的时候需要进行散热。

（6）对于采用步进电机驱动的SCARA机器人来说，使得系统运转的机械噪声大大增加，不利于环境保护。

（7）对于采用步进电机和直流电机驱动的SCARA机器人来说，系统的转动惯量大，不利于机器人的快速加速和减速。

（8）对于采用步进电机和直流电机驱动的SCARA机器人来说，系统的启动力矩较小，不利于机器人的快速启动。

（9）在SCARA机器人的整个运动控制过程，旋转的各个角度没有实时反馈，时有造成三轴不同步的现象发生。

（10）在SCARA机器人的整个运动控制过程，各个旋转臂与外围环境之间没有进行有效的避障，有时会造成碰撞其他货物的现象发生。

（11）在SCARA机器人多轴伺服控制系统中，基于单片机或者是DSP的控制系统都需要编写伺服控制软件，使得系统开发周期较长，而且大大降低了系统的运算速度，并且程序可移植能力较差。

发明内容

本发明主要解决的技术问题是提供一种教学用三核快速关节机器人控制系统，该教学用三核快速关节机器人控制系统计算速度快，使关节机器人手臂转向灵活、动作稳定精确、体积小巧，性能稳定。

为解决上述技术问题，本发明采用的技术方案是：提供一种教学用三核快速关节机器人控制系统，所述关节机器人采用永磁直流伺服电机X驱动机器人大臂旋转运动、采用永磁直流伺服电机Y驱动机器人小臂旋转运动、采用永磁直流伺服电机Z驱动机器人手腕旋转运动、采用永磁直流伺服电机R驱动机器人手腕升降运动，所述机器人大臂上安装有避障位移传感器S1、避障位移传感器S2和加速度传感器A1，所述机器人小臂上安装有避障位移传感器S3、避障位移传感器S4和加速度传感器A2，所述机器人手腕上安装有定位传感器S5和加速度传感器A3，所述的控制系统包括主站控制计算机和控制器，所述控制器包括DSP芯片控制器、集成芯片处理器和ARM控制器，所述DSP芯片控制器、集成芯片处理器和ARM控制器均与所述主站控制计算机通信连接，所述DSP芯片控制器、集成芯片处理器与ARM控制器彼此之间通信连接，所述永磁直流伺服电机X、永磁直流伺服电机Y、永磁直流伺服电机Z和永磁直流伺服电机R均与所述DSP芯片控制器通信连接，所述避障位移传感器S1、避障位移传感器S2、避障位移传感器S3、避障位移传感器S4、定位传感器S5、加速度传感器A1、加速度传感器A2和加速度传感器A3均同时与所述DSP芯片控制器、集成芯片处理器和ARM控制器通信连接。

在本发明一个较佳实施例中，还包括为所述关节机器人和控制系统提供电源的主电源和备用电源，所述关节机器人中的各个所述电机和传感器以及控制系统中的控制器均与所述主电源和/或备用电源电性连接。

在本发明一个较佳实施例中，所述永磁直流伺服电机X、永磁直流伺服电机Y、永磁直流伺服电机Z和永磁直流伺服电机R上匀设置有光电编码器，所述光电编码器与所述DSP芯片控制器电性连接。

在本发明一个较佳实施例中，所述DSP芯片控制器内设置有记录永磁直流伺服电机位置的位置寄存器。

本发明的有益效果是：本发明的教学用三核快速关节机器人控制系统计算速度快，使关节机器人手臂转向灵活、动作稳定精确、体积小巧，性能稳定，系统抗干扰能力强。

附图说明

为了更清楚地说明本发明实施例中的技术方案，下面将对实施例描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其它的附图，其中：

图1为传统SCARA机器人控制器原理图；

图2为基于四轴永磁直流伺服电机SCARA机器人二维结构图；

图3为DSP与LM628连接示意图；

图4为基于三核四轴永磁直流伺服电机SCARA机器人控制器原理图；

图5为基于三核四轴永磁直流伺服电机SCARA机器人程序框图；

图6为基于三核四轴永磁直流伺服电机运动原理图；

图7为四轴电机加减速曲线图；

图8为本发明的教学用三核快速关节机器人控制系统结构示意图。

附图中各部件的标记如下：1、主站控制计算机，2、控制器，3、DSP芯片控制器，4、ARM控制器，5、主电源，6、备用电源，7、永磁直流伺服电机X，8、永磁直流伺服电机Y，9、永磁直流伺服电机Z，10、永磁直流伺服电机R，11、光电编码器，12、避障位移传感器S1，13、避障位移传感器S2，14、避障位移传感器S3，15、避障位移传感器S4，16、定位传感器S5，17、加速度传感器A1，18、加速度传感器A2，19、加速度传感器A3，20、集成芯片处理器。

具体实施方式

下面将对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅是本发明的一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其它实施例，都属于本发明保护的范围。

请参阅图2至图8，本发明实施例包括：一种教学用三核快速关节机器人控制系统，该机器人为基于四轴永磁直流伺服电机的SCARA机器人，即所述关节机器人采用永磁直流伺服电机X7驱动机器人大臂旋转运动、采用永磁直流伺服电机Y8驱动机器人小臂旋转运动、采用永磁直流伺服电机Z9驱动机器人手腕旋转运动、采用永磁直流伺服电机R10驱动机器人手腕升降运动，所述机器人大臂上安装有避障位移传感器S1 12、避障位移传感器S213和加速度传感器A1 17，所述机器人小臂上安装有避障位移传感器S3 14、避障位移传感器S4 15和加速度传感器A2 18，所述机器人手腕上安装有定位传感器S5 16和加速度传感器A3 19。

所述的控制系统包括主站控制计算机1和控制器2，所述控制器2包括DSP芯片控制器3、集成芯片处理器20和ARM控制器4，所述DSP芯片控制器3、集成芯片处理器20和ARM控制器4均与所述主站控制计算机1通信连接，所述DSP芯片控制器3、集成芯片处理器20与ARM控制器4彼此之间通信连接，所述永磁直流伺服电机X7、永磁直流伺服电机Y8、永磁直流伺服电机Z9和永磁直流伺服电机R10均与所述DSP芯片控制器3通信连接，所述避障位移传感器S1 12、避障位移传感器S2 13、避障位移传感器S3 14、避障位移传感器S4 15、定位传感器S5 16、加速度传感器A1 17、加速度传感器A2 18和加速度传感器A3 19均同时与所述DSP芯片控制器3、集成芯片处理器20和ARM控制器4通信连接。

优选地，本发明的教学用三核快速关节机器人控制系统还包括为所述关节机器人和控制系统提供电源的主电源5和备用电源6，所述关节机器人中的各个所述电机和传感器以及控制系统中的控制器2均与所述主电源5和/或备用电源6电性连接。

优选地，所述永磁直流伺服电机X 7、永磁直流伺服电机Y 8、永磁直流伺服电机Z9和永磁直流伺服电机R10上匀设置有光电编码器11，所述光电编码器11与所述DSP芯片控制器3电性连接。

优选地，所述DSP芯片控制器3内设置有记录永磁直流伺服电机位置的位置寄存器。

本发明采用DSP（TMS320F2812）+集成芯片处理器20+ ARM（STM32F746）三核控制器，其中集成芯片处理器20采用美国国家半导体公司生产的LM628大规模集成芯片。LM628主要特点如下：32bit位置、速度、加速度寄存器；16bit的可编程数字化PID调节器；可编程微分采样周期；8bit或12bit DAC输出；8bit PWM输出；内部梯形速度特性产生器；在运动期间速度、目标位置和滤波器参数可以改变；具有位置和速度两种操作模式，16位的可编程数字PID调节器可经增量码盘反馈构成位置闭环，并能对位置误差实行PID运算。LM628通过8bit并行I/O口与DSP进行数据交换，DSP通过命令指令对LM628的梯形速度特性和PID数字滤波器进行参数设定，利用LM628和DSP微处理器配合可实现低成本、高精度永磁直流伺服电机的PID伺服系统，二者链接示意图如图3。

本发明的控制器原理图如图4：控制板以DSP（TMS320F28335）和LM628为处理器核心，ARM(STM32F746)实现货物搬运从一点到另外一点的大臂、小臂、手腕旋转角度和手腕上升高度的计算，并实时响应总站人为搬运货物的中断，并与DSP（TMS320F28335）通讯，把DSP（TMS320F28335）从复杂的工作当中解脱出来，DSP（TMS320F28335）配合LM628实现四轴永磁直流伺服电机的实时控制，并响应ARM中断，实现数据通信和存储实时信号。

本系统用效率高、功率密度高、转动惯量较小、启动转矩大的永磁直流伺服电机替代步进电机和普通直流电机，以提高能源的利用率和提高机器人的动态特性；为了提高运算速度，保证SCARA机器人系统的稳定性和可靠性，本发明在基于DSP的控制器中引入精密运动控制器LM628、嵌入式控制器ARM和多种传感器，形成基于DSP+LM628+ARM的全新三核控制器，此控制器充分考虑电源在这个系统的作用，双电源的使用保证了能源的供给；三核系统把工作量最大的四轴永磁直流伺服电机控制系统交给LM628处理，充分发挥LM628作为电机伺服控制处理器的作用，而位置读取、路径规划、信息存储、电源监控与切换等交给DSP处理，基于Denavit-Hartenberg算法的大臂、小臂和手腕旋转角度以及手腕上升高度计算、数据存储等功能交给ARM完成，充分发挥ARM数据处理速度相对较快的特点，这样就实现了ARM、DSP和LM628的分工，同时三者之间也可以实时通讯，并进行数据交换和调用。

DSP（TMS320F28335）+LM628+ ARM（STM32F746）三核控制器，在电源打开状态下，ARM控制器先对机器人备用电源SOC（荷电状态）和主电源进行判断，如果备用能源较低，控制器会发出报警信号；如果备用电源和主电源工作正常，先由PC机通过USB接口把搬运货物大臂、小臂和手腕旋转角度以及升降高度或者是搬用货物的各个坐标信息输入给ARM，由ARM计算出机器人伺服系统的参数；SCARA机器人在人工导引下恢复到零位置，并先进入自锁状态，一旦搬运命令开始后，SCARA机器人携带的避障传感器、定位传感器和加速度传感器均开启，SCARA机器人按照设定ARM优化的搬运路径快速搬运，DSP与LM628通讯，LM628按照伺服参数和传感器反馈实时调整SCARA机器人永磁直流伺服电机X、永磁直流伺服电机Y、永磁直流伺服电机Z和永磁直流伺服电机R的PWM输出，实现四台永磁直流伺服电机的实时伺服控制，DSP实时采集机器人运动信息并存储货物位置信息；如果ARM对搬运某一个位置有疑问，将与DSP通讯，DSP通过LM628使SCARA机器人停车，然后人工对DSP存储信息进行判断，确认无误后二次人工启动SCARA机器人继续未完成的任务。

参照图1，图2，图3和图4、图5、图6、图7，其具体的功能实现如下：

1）SCARA机器人电源打开后，ARM会对备用电源SOC和主电源进行判断，如果备用电源SOC较低时，DSP将禁止控制四台永磁直流伺服电机的LM628工作，电机输入PWM波控制信号被封锁，同时报警传感器将工作并发出报警信号；如果电池SOC正常，SCARA机器人进入待工作状态，等待工作命令。

2）人工通过PC机把大臂、小臂长度和升降杆的长度等信息通过USB接口传入到ARM控制器，然后人工引导SCARA机器人到设定工作的零位置位，此时设定各个旋转角度，手腕上升高度。

3）为了满足SCARA机器人的加减速运动需要，本发明采用如图7的运动时间梯形图，此梯形图包含的面积就是机器人大臂、小臂和手腕要旋转的角度或者是手腕升降的高度，为了方便控制，本发明采用单一加速度模式。

4）SCARA机器人读取其工作模式如果是人工工作模式，主站开始与ARM控制器通讯，由主站输入SCARA机器人大臂、小臂和手腕需要旋转的角度，，以及SCARA机器人手腕需要上升或者是下降的高度给ARM控制器，ARM控制器根据Denavit-Hartenberg算法开始机器人位置和姿态正向求解：ARM控制器首先根据SCARA机器人大臂、小臂和手腕需要旋转的角度，，以及SCARA机器人手腕需要上升的高度计算出相邻两杆件坐标系之间的位姿矩阵，，，，并用各自用4*4二维数组标志，，，，分别表示如下：

，，

，。

ARM控制器然后通过公式就可以求出手腕执行器完成任务后的位置和姿态，并与DSP通讯，传输人工输入参数给DSP。

5）SCARA机器人读取工作模式如果是正常自动搬运工作模式，主站开始与ARM控制器通讯，由主站输入SCARA机器人所处的初始位置和给定位置三维坐标给ARM控制器，ARM控制器根据Denavit-Hartenberg算法开始机器人逆向求解：ARM控制器首先根据大臂、小臂长度以及最终的三维坐标中的X和Y坐标求出大臂需要旋转的角度，并借助的值求出的值，并根据三维坐标中的Z坐标求出手腕上升或降低的高度，最终求出旋转角度，由于在求解的时候方程具有双解，所以SCARA机器人求出大臂、小臂和手腕需要旋转的角度，，以及SCARA机器人手腕需要上升的高度后，ARM控制器会对求解结果进行优化，然后ARM控制器与DSP通讯，把机器人最有伺服运动参数传输给DSP控制器。

6）DSP控制器接受SCARA机器人大臂、小臂和手腕需要旋转的角度，，以及SCARA机器人手腕需要上升的高度后，大臂、小臂和手腕上的传感器S1~S5将开启，首先SCARA机器人对各个旋转臂旋转角度内的障碍物进行判断，如存在障碍物将向DSP发出中断请求，DSP会对中断做第一时间响应，然后DSP禁止四轴LM628工作，四轴永磁直流伺服电机PWM波控制信号被封锁， SCARA机器人禁止永磁直流伺服电机X、永磁直流伺服电机Y、永磁直流伺服电机Z和电机永磁直流伺服电机R工作，机器人自锁在原地，DSP控制器二次判断运动范围内的障碍物信息，防止信息误判。

7）如果DSP控制器确定无障碍物进入运动区域， DSP控制器按照图7的速度时间曲线对运动位置进行分解，DSP首先把三个旋转角度，，转化为三台永磁直流伺服电机的加速度、速度和位置初始指令值，然后DSP与LM628通讯，LM628结合电机X、电机Y和电机Z的光电编码器反馈，经LM628内部伺服调节程序生成电机X、电机Y和电机Z的PWM波控制信号，PWM波控制信号经驱动桥放大后驱动各个电机运动。DSP控制器根据输入偏差大小实时调整LM628内部伺服控制程序的PID参数，LM628控制器通过调整永磁直流伺服电机的驱动脉冲数目调整其旋转角度，通过调整永磁直流伺服电机控制信号的频率实现角速度快慢的变化，使三轴永磁直流伺服电机时刻同步工作，DSP时刻记录机器人已经运动的位置参数。

8）在SCARA机器人移动过程中，传感器S1~S4时刻对运动范围内的移动障碍物进行判断，如果有障碍物进入运动范围，DSP立即通过LM628使SCARA机器人的大臂、小臂和手腕按照图7的减速运动曲线立即停车，DSP控制器记录下当前旋转角度，，信息。等障碍物消失后，重新计算对新位置的旋转角度，，，然后DSP控制LM628按照图7的运动曲线重新经过三段运动轨迹：加速运动、匀速运动和减速运动，最终到达设定点。

9）在移动过程中，DSP时刻记录加速度计反馈的大臂、小臂和手腕旋转的角加速度，控制器通过二次积分得到大臂、小臂和手腕的旋转角度，并与设定的位置角度值相比较，如果偏差大于设定阀值， DSP把这个偏差转化为三台永磁直流伺服电机新的加速度、速度和位置初始指令值，在下一个采样周期，DSP与LM628通讯，LM628结合电机X、电机Y和电机Z电机光电编码器反馈的反馈，经LM628内部伺服调节程序生成电机X、电机Y和电机Z的PWM波控制信号，PWM波控制信号经驱动桥放大后驱动各个电机运动。DSP控制器根据输入偏差大小实时调整LM628内部伺服控制程序的PID参数，LM628控制器通过调整永磁直流伺服电机的驱动脉冲数目调整其旋转角度，通过调整永磁直流伺服电机控制信号的频率实现角速度快慢的变化，进而消除上一个采样周期产生的误差，使SCARA机器人按照设定轨迹完成任务。

10）在SCARA机器人运动过程中，DSP会时刻储存所经过的SCARA机器人所处的位置或者是经过的参考点，并根据这些距离信息由DSP计算得到相对下一个参考点SCARA机器人永磁直流伺服电机X、永磁直流伺服电机Y、永磁直流伺服电机Z分别要运行的角度、角度速度和角加速度，LM628结合电机X、电机Y和电机Z电机光电编码器反馈的反馈，经内部伺服调节程序生成电机X、电机Y和电机Z的PWM波控制信号，PWM波控制信号经驱动桥放大后驱动各个电机运动。DSP控制器根据输入偏差大小实时调整LM628内部伺服控制程序的PID参数，通过调整永磁直流伺服电机的驱动脉冲数目调整其旋转角度，通过调整永磁直流伺服电机控制信号的频率实现角速度快慢的变化，使SCARA机器人按照设定速度快速前行。

11）在SCARA机器人完成三轴旋转角度，，的伺服控制后，DSP二次检测加速度传感器的积分值，如果发现SCARA机器人在运动过程受到外界干扰后三个旋转角度，，与设定角度的误差超过设定阀值时，DSP把三个旋转角度偏差，，转化为三台永磁直流伺服电机位置微调的加速度、速度和位置初始指令值，然后DSP与LM628通讯，LM628结合电机X、电机Y和电机Z电机光电编码器反馈的反馈，经LM628内部伺服调节程序生成电机X、电机Y和电机Z的PWM波控制信号，PWM波控制信号经驱动桥放大后驱动各个电机运动。DSP控制器根据输入偏差大小实时调整LM628内部伺服控制程序的PID参数，LM628控制器通过调整永磁直流伺服电机的驱动脉冲数目调整其旋转角度，通过调整永磁直流伺服电机控制信号的频率实现角速度快慢的变化，通过三轴永磁直流伺服电机的再次工作使机器人大臂、小臂和手腕到达设定位置。

12）当SCARA机器人的大臂、小臂和手腕完成角度，，的角度补偿到达设定位置后，DSP按照图7的速度时间曲线，把手腕要升降的距离转化为永磁直流伺服电机R的加速度、速度和位置初始指令值，然后DSP与LM628通讯，LM628结合电机R光电编码器的反馈，经内部伺服调节程序生成电机R的PWM波控制信号，PWM波控制信号经驱动桥放大后驱动各个电机运动。DSP控制器根据输入偏差大小实时调整LM628内部伺服控制程序的PID参数，DSP控制器通过调整永磁直流伺服电机的驱动脉冲数目调整其旋转角度，通过调整永磁直流伺服电机控制信号的频率实现角速度快慢的变化，使手腕平稳在设定时间内到达设定位置。

13）在运动过程中如果SCARA机器人发现位置参数求解出现死循环将向ARM发出中断请求，ARM会对中断做第一时间响应，ARM控制器将立即与DSP通讯，DSP立即通过LM628封锁四台永磁直流伺服电机的控制信号，然后机器人原地自锁，并放弃搬运工作，然后由主站根据储存信息分析进行分析。

14）装在永磁直流伺服电机X、永磁直流伺服电机Y、永磁直流伺服电机Z、永磁直流伺服电机R上的光电编码器会输出其位置信号A和位置信号B，光电编码器的位置信号A脉冲和B脉冲逻辑状态每变化一次，DSP内的位置寄存器会根据电机的运行方向加1或者是减1；光电编码器的位置信号A脉冲和B脉冲和Z脉冲同时为低电平时，就产生一个INDEX信号给DSP内部寄存器，记录永磁直流伺服电机的绝对位置，然后换算成SCARA机器人大臂、小臂或者是手腕在三维坐标系统中的具体位置。

15）SCARA机器人在运行过程ARM控制器时刻对AC交流主电源进行监控，如果控制器发现主电源出现故障突然断电时，ARM会与DSP通讯，并开通备用电源，由备用电源为四轴永磁直流伺服电机提供能量，DSP通过LM628内部伺服调节程序实时调整四台永磁直流伺服电机的PWM输出，LM628控制器通过调整永磁直流伺服电机的驱动脉冲数目调整其旋转角度，通过调整永磁直流伺服电机控制信号的频率实现角速度快慢的变化，使SCARA机器人完成这次搬运任务，然后SCARA机器人与主站进行通讯，并通知主站进行检修。

16）在SCARA机器人多轴伺服系统工作过程中，如果DSP伺服控制器检测到某一个永磁直流伺服电机的转矩出现脉动，由于本发明采用的永磁直流伺服电机力矩与电机电流成正比例，因此控制器会很容易补偿此干扰，并根据干扰大小DSP实时调整LM628内部的PID参数，减少了电机转矩扰动对SCARA机器人运动过程的影响。

17）当SCARA机器人完成任务，实现位置归零时，其携带的加速度传感器A1~A3会时刻检测其加速度，当加速度超过预设阀值时，在下一个周期DSP控制器会通过LM628修正上一个周期带来的误差，确保机器人停止在设定零位置，SCARA机器人按照图7的速度-时间运动曲线实现从某一点到零位的归位，然后零位自锁，等待下一个搬运命令。

本发明可以达到以下有益效果：

1、在SCARA机器人运动过程中，充分考虑了电源在这个系统中的作用，为了保证主电源断电后工作的完整性，系统加入了备用电源。基于ARM+DSP+LM628三核控制器时刻都在对电源状态和电机状态进行监测和运算，既避免了电路大电流的产生，也有效保护了SCARA机器人的伺服硬件系统；

2、由LM628处理SCARA机器人四只永磁直流伺服电机的实时控制，减少了伺服控制软件的编写，使得控制比较简单，大大提高了运算速度，解决了单片机软件运行较慢的瓶颈，缩短了开发周期短，并且程序可移植能力强；

3、本发明基本实现全贴片元器件材料，实现了单板控制，不仅节省了控制板占用空间，而且有利于SCARA机器人体积和重量的减轻；

4、本发明SCARA机器人采用永磁直流伺服电机替代了传统机器人系统中常用的步进电机和直流电机，由于其体积较小，效率较高，使得SCARA机器人体积可以进一步缩小，能源利用率大大提高；

5、本发明SCARA机器人采用永磁直流伺服电机替代了传统机器人系统中常用的步进电机和直流电机，由于其转动惯量较小，有利于提高机器人的快速加速和减速的动态性能；

6、由于采用永磁直流伺服电机，使得调速范围比较宽，调速比较平稳，即使在低速阶段电机的脉动转矩也非常小，有利于提高系统的动态性能；

7、由于本控制器采用ARM处理Denavit-Hartenberg算法中大臂、小臂和手腕的旋转角度和手腕上升高度，由LM628生成四轴永磁直流伺服电机的PWM波控制信号，把DSP从繁重的工作量中解脱出来，有效地防止了程序的“跑飞”，抗干扰能力大大增强；

8、在SCARA机器运动过程中，DSP控制器可以根据SCARA机器人外围运行情况适时调整LM628的PID参数，实现分段P、PD、PID控制和非线性PID控制，使系统动态性能大大提高；

9、SCARA机器人上配备有避障传感器，可以轻易测得SCARA机器人的大臂、小臂和周围货物的距离，可有效避免碰撞的发生；

10、SCARA机器人手腕上配备了定位系统，可以轻易地检测手腕与货物之间的距离，可有效实现平稳的抓取货物；

11、基于DSP的数据存储功能使得SCARA机器人完成任务后方便工作人员读取搬运货物结果，可以轻易的从存储结果中读取搬运错误信息和具体位置，然后及时更改；

12、多个两轴加速度计的加入可有效测量出机器人在同步旋转时的加速度、速度和旋转角度，DSP控制器会时刻通过LM628对此角度进行补偿调整，有效控制了SCARA机器人的姿态和位置；

13、基于加速度传感器和直流伺服电机光电编码器传感器的双套传感器系统使多轴SCARA机器人具有一定的冗余度，极大提高了SCARA机器人旋转的精确性，有效消除了误差。

以上所述仅为本发明的实施例，并非因此限制本发明的专利范围，凡是利用本发明说明书内容所作的等效结构或等效流程变换，或直接或间接运用在其它相关的技术领域，均同理包括在本发明的专利保护范围内。

Claims (4)

1.一种教学用三核快速关节机器人控制系统，其特征在于，所述关节机器人采用永磁直流伺服电机X驱动机器人大臂旋转运动、采用永磁直流伺服电机Y驱动机器人小臂旋转运动、采用永磁直流伺服电机Z驱动机器人手腕旋转运动、采用永磁直流伺服电机R驱动机器人手腕升降运动，所述机器人大臂上安装有避障位移传感器S1、避障位移传感器S2和加速度传感器A1，所述机器人小臂上安装有避障位移传感器S3、避障位移传感器S4和加速度传感器A2，所述机器人手腕上安装有定位传感器S5和加速度传感器A3，所述的控制系统包括主站控制计算机和控制器，所述控制器包括DSP芯片控制器、集成芯片处理器和ARM控制器，所述DSP芯片控制器、集成芯片处理器和ARM控制器均与所述主站控制计算机通信连接，所述DSP芯片控制器、集成芯片处理器与ARM控制器彼此之间通信连接，所述永磁直流伺服电机X、永磁直流伺服电机Y、永磁直流伺服电机Z和永磁直流伺服电机R均与所述DSP芯片控制器通信连接，所述避障位移传感器S1、避障位移传感器S2、避障位移传感器S3、避障位移传感器S4、定位传感器S5、加速度传感器A1、加速度传感器A2和加速度传感器A3均同时与所述DSP芯片控制器、集成芯片处理器和ARM控制器通信连接。

2.根据权利要求1所述的教学用三核快速关节机器人控制系统，其特征在于，还包括为所述关节机器人和控制系统提供电源的主电源和备用电源，所述关节机器人中的各个所述电机和传感器以及控制系统中的控制器均与所述主电源和/或备用电源电性连接。

3.根据权利要求1所述的教学用三核快速关节机器人控制系统，其特征在于，所述永磁直流伺服电机X、永磁直流伺服电机Y、永磁直流伺服电机Z和永磁直流伺服电机R上匀设置有光电编码器，所述光电编码器与所述DSP芯片控制器电性连接。

4.根据权利要求1至3任一项所述的教学用三核快速关节机器人控制系统，其特征在于，所述DSP芯片控制器内设置有记录永磁直流伺服电机位置的位置寄存器。