一种重载无线传输五核快速关节机器人控制系统的制作方法
【专利摘要】本发明公开了一种重载无线传输五核快速关节机器人控制系统，所述的控制系统包括主站控制计算机和控制器，所述控制器包括DSP芯片控制器、语音识别处理器、PLC控制器、永磁同步电机闭环控制芯片、图像采集模块、无线传输模块和ARM控制器，DSP芯片控制器和ARM控制器均与主站控制计算机通信连接，DSP芯片控制器、语音识别处理器和ARM控制器彼此通信连接，四台稀土永磁同步伺服电机均与DSP芯片控制器通信连接，多个避障位移传感器、定位传感器、陀螺仪、加速度传感器均与DSP芯片控制器和ARM控制器通信连接。本发明的重载无线传输五核快速关节机器人控制系统计算速度快，使机器人手臂转向灵活、动作稳定精确，便于无线监控。
【专利说明】
一种重载无线传输五核快速关节机器人控制系统
技术领域
[0001 ] 本发明涉及一种重载无线传输五核快速关节机器人控制系统，属于装配作业的 四关节机器人手臂应用领域。
【背景技术】
[0002]在工业生产中，工业机器人可以代替人类做一些比较单调、比较频繁和重复率较 高的长时间作业，或是在危险、恶劣环境下的作业，一般用作搬取零件和装配工作，在微电 子制造业、塑料工业、汽车工业、电子产品工业、药品工业和食品工业等领域得到广泛的应 用，它对于提高生产自动化水平、劳动生产率和经济效益、保证产品质量、保障人身安全、改 善劳动环境，减轻劳动强度、节约原材料消耗以及降低生产成本等有着十分重要的意义。 [0003] SCARA工业机器人即装配作业的机器人手臂是一种圆柱坐标型的工业机器人，它 依靠旋转关节大臂和小臂来实现X-Y平面内的快速定位，依靠一个手腕移动关节和一个手 腕旋转关节在Z方向上做伸缩和旋转运动，其具有四个运动自由度，该系列的操作手在其动 作空间的四个方向具有有限刚度，而在剩下的其余两个方向上具有无限大刚度。这种结构 特性使得SCARA机器人擅长从一点抓取物体，然后快速的安放到另一点，因此SCARA机器人 在自动装配生产线上得到了广泛的应用。SCARA机器人结构紧凑、动作灵活，速度快、位置精 度高，它的使用大大提高了机器人对复杂装配任务的适应性，同时也降低了成本，提高了工 作空间利用率。
[0004] SCARA关节机器人在搬运货物过程中要时刻判断主控制器输入的位置参数，并判 断周围的环境时刻避障，然后由运动控制器反复控制其精确的加速和减速进行搬运货物， 稍微的误差累计就有可能在多回合运动中导致运输失败。国内对SCARA机器人的使用虽然 有几十年，但是由于国内工业机器人发展起步比较晚，受较多关键技术的影响，SCARA机器 人的发展也受所影响，传统的机器人原理如图1所示，在长期使用期间出现众多问题： (1)在SCARA机器人进行零位置归位时，或者是机器人复位时，均依靠人工目测的方式， 使得机器人的零位置设定有误。
[0005] (2)虽然永磁直流伺服电机较直流电机、步进电机功能有所提高，但是普通永磁直 流伺服电机为有刷结构，因存在集电环炭刷结构，在电机运行时产生火花，特别是在高速时 将产生严重的环火，产生无线电干扰，不适合高速旋转，不适合运用于带载运行的SCARA机 器人。
[0006] (3 )永磁直流伺服电机为有刷结构，集电环和碳刷之间直接接触且高速相对运动， 这种机械结构使得有机械摩擦存在，产生较大的机械噪声，使得SCARA机器人的噪声污染增 大。
[0007] (4)虽然永磁无刷直流伺服电机较步进电机、直流电机和直流伺服电机性能有所 提高，但是其带载能力相对较弱，无法满足重载SCARA机器人系统要求。
[0008] (5)当机器人遇到紧急情况需要停车或者是停车后启动均是通过按键完成，机器 人的智能程度不高。
【发明内容】
[0009]本发明主要解决的技术问题是提供一种重载无线传输五核快速关节机器人控制 系统，该重载无线传输五核快速关节机器人控制系统计算速度快，使关节机器人手臂转向 灵活、动作稳定精确、承载力大，性能稳定，便于无线监控，机器人的智能程度高。
[0010]为解决上述技术问题，本发明采用的技术方案是：提供一种重载无线传输五核快 速关节机器人控制系统，所述关节机器人采用稀土永磁同步伺服电机X驱动机器人大臂旋 转运动、采用稀土永磁同步伺服电机Y驱动机器人小臂旋转运动、采用稀土永磁同步伺服电 机Z驱动机器人手腕旋转运动、采用稀土永磁同步伺服电机R驱动机器人手腕升降运动，所 述机器人大臂上安装有避障位移传感器S1、避障位移传感器S2、陀螺仪和加速度传感器Al， 所述机器人小臂上安装有避障位移传感器S3、避障位移传感器S4、陀螺仪和加速度传感器 A2,所述机器人手腕上安装有定位传感器S5、陀螺仪和加速度传感器A3,所述的控制系统包 括主站控制计算机和控制器，所述控制器包括DSP芯片控制器、PlC控制器、永磁同步电机闭 环控制芯片、语音识别处理器、图像采集模块、无线传输模块和ARM控制器，所述图像采集模 块与无线传输模块之间通信连接，所述DSP芯片控制器、永磁同步电机闭环控制芯片、语音 识别处理器和PlC控制器均与所述主站控制计算机通信连接，所述DSP芯片控制器、永磁同 步电机闭环控制芯片、语音识别处理器和ARM控制器彼此通信连接，所述无线传输模块和 ARM控制器通信连接，所述PlC控制器与ARM控制器通信连接，所述稀土永磁同步伺服电机X、 稀土永磁同步伺服电机Y、稀土永磁同步伺服电机Z和稀土永磁同步伺服电机R均与所述DSP 芯片控制器通信连接，所述避障位移传感器S1、避障位移传感器S2、避障位移传感器S3、避 障位移传感器S4、定位传感器S5、各陀螺仪、加速度传感器Al、加速度传感器A2和加速度传 感器A3均同时与所述DSP芯片控制器和ARM控制器通信连接。
[0011]在本发明一个较佳实施例中，还包括为所述关节机器人和控制系统提供电源的主 电源和备用电源，所述关节机器人中的各个所述电机和传感器以及控制系统中的控制器均 与所述主电源和/或备用电源电性连接。
[0012]在本发明一个较佳实施例中，所述稀土永磁同步伺服电机X、稀土永磁同步伺服电 机Y、稀土永磁同步伺服电机Z和稀土永磁同步伺服电机R上匀设置有光电编码器，所述光电 编码器与所述DSP芯片控制器电性连接。
[0013] 在本发明一个较佳实施例中，所述图像采集模块为CCD摄像头组件。
[0014] 在本发明一个较佳实施例中，所述图像采集模块内设置有解码芯片。
[0015] 在本发明一个较佳实施例中，所述机器人大臂上安装有磁电传感器EMl，所述机器 人小臂上安装有磁电传感器EM2,所述机器人手腕上安装有磁电传感器EM3和EM4,所述磁电 传感器EMl、磁电传感器EM2、磁电传感器EM3和EM4均与所述DSP芯片控制器和ARM控制器通 信连接。
[0016] 本发明的有益效果是:本发明的重载无线传输五核快速关节机器人控制系统计算 速度快，使关节机器人手臂转向灵活、动作稳定精确、承载力大，性能稳定，便于无线监控， 机器人的智能程度高，无线传输系统的加入使得主站可以实时了解工作结果，同时主站可 以随时根据实际需要实时更改SCARA机器人工作任务，主站与ARM控制器之间通过PLC进行 通讯，使得总站与ARM控制器之间可以实时进行数据通讯和调用，使得临时任务加入运动队 列非常简单，主站通过基于语音识别处理器向ARM控制器输入临时任务或者是紧急启停命 令，减少了紧急状态下通过键盘输入命令的时间，可有效提高机器人工作效率，基于PLC控 制器和基于语音识别的两种输入方式使得机器人更改任务更加方便，永磁同步电机闭环控 制芯片和DSP控制器可以对稀土永磁同步伺服电机进行实时控制，减少了伺服控制软件的 编写，使得控制比较简单，大大提高了运算速度，解决了单片机软件运行较慢的瓶颈，缩短 了开发周期短，并且程序可移植能力强。
【附图说明】
[0017] 为了更清楚地说明本发明实施例中的技术方案，下面将对实施例描述中所需要使 用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于 本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其它 的附图，其中： 图1为传统SCARA机器人控制器原理图 图2为基于四轴稀土永磁同步伺服电机SCARA机器人二维结构图 图3为基于五核四轴稀土永磁同步伺服电机SCARA机器人控制器原理 图4为基于五核四轴稀土永磁同步伺服电机SCARA机器人程序框图。
[0018] 图5为基于五核四轴稀土永磁同步伺服电机运动原理图 图6为四轴电机加减速曲线图； 图7为本发明的重载五核快速关节机器人控制系统结构示意图。
[0019] 附图中各部件的标记如下：1、主站控制计算机，2、控制器，3、DSP芯片控制器，4、 ARM控制器，5、主电源，6、备用电源，7、稀土永磁同步伺服电机X，8、稀土永磁同步伺服电机 Y，9、稀土永磁同步伺服电机Z，10、稀土永磁同步伺服电机R，11、光电编码器，12、避障位移 传感器Sl，13、避障位移传感器S2,14、避障位移传感器S3,15、避障位移传感器S4,16、定位 传感器S5，17、加速度传感器Al，18、加速度传感器Α2，19、加速度传感器A3，20、PLC控制器， 21、磁电传感器EMl，22、磁电传感器EM2，23、磁电传感器EM3，24、磁电传感器EM4，25、无线传 输模块，26、图像采集模块，27、解码芯片，28、语音识别处理器，29、陀螺仪，30、永磁同步电 机闭环控制芯片。
【具体实施方式】
[0020] 下面将对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施 例仅是本发明的一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通 技术人员在没有做出创造性劳动前提下所获得的所有其它实施例，都属于本发明保护的范 围。
[0021] 请参阅图2至图7,本发明实施例包括:一种重载无线传输五核快速关节机器人控 制系统，该机器人为基于四轴稀土永磁同步伺服电机的SCARA机器人，即所述关节机器人采 用稀土永磁同步伺服电机X7驱动机器人大臂旋转运动、采用稀土永磁同步伺服电机Y8驱动 机器人小臂旋转运动、采用稀土永磁同步伺服电机Z9驱动机器人手腕旋转运动、采用稀土 永磁同步伺服电机RlO驱动机器人手腕升降运动，所述机器人大臂上安装有避障位移传感 器Sl 12、避障位移传感器S2 13、陀螺仪29和加速度传感器Al 17,所述机器人小臂上安装 有避障位移传感器S3 14、避障位移传感器S4 15、陀螺仪29和加速度传感器A2 18,所述机 器人手腕上安装有定位传感器S5 16、陀螺仪29和加速度传感器A3 19。
[0022]所述的控制系统包括主站控制计算机1和控制器2,所述控制器2包括DSP芯片控制 器3、语音识别处理器28、永磁同步电机闭环控制芯片30、P1C控制器20、图像采集模块26、无 线传输模块25和ARM控制器4,所述图像采集模块26与无线传输模块25之间通信连接，所述 DSP芯片控制器3、永磁同步电机闭环控制芯片30、语音识别处理器28和PlC控制器20均与所 述主站控制计算机1通信连接，所述DSP芯片控制器3、永磁同步电机闭环控制芯片30、语音 识别处理器28和ARM控制器4彼此通信连接，所述无线传输模块25和ARM控制器4通信连接， 所述PlC控制器20与ARM控制器4通信连接，所述图像采集模块26内设置有解码芯片27。 [0023] 所述稀土永磁同步伺服电机X7、稀土永磁同步伺服电机Y8、稀土永磁同步伺服电 机Z9和稀土永磁同步伺服电机RlO均与所述DSP芯片控制器3通信连接，所述避障位移传感 器Sl 12、避障位移传感器S2 13、避障位移传感器S3 14、避障位移传感器S4 15、定位传感 器S5 16、陀螺仪29、加速度传感器Al 17、加速度传感器A2 18和加速度传感器A3 19均同时 与所述DSP芯片控制器3、PLC控制器20和ARM控制器4通信连接。
[0024]优选地，本发明的重载无线传输五核快速关节机器人控制系统还包括为所述关节 机器人和控制系统提供电源的主电源5和备用电源6,所述关节机器人中的各个所述电机和 传感器以及控制系统中的控制器2均与所述主电源5和/或备用电源6电性连接。
[0025] 优选地，所述稀土永磁同步伺服电机X 7、稀土永磁同步伺服电机Y 8、稀土永磁同 步伺服电机Z 9和稀土永磁同步伺服电机RlO上匀设置有光电编码器11，所述光电编码器11 与所述DSP芯片控制器3电性连接。
[0026]优选地，所述机器人大臂上安装有磁电传感器EMl 21，所述机器人小臂上安装有 磁电传感器EM2 22,所述机器人手腕上安装有磁电传感器EM3 23和EM4 24,所述磁电传感 器EMl 21、磁电传感器EM2 22、磁电传感器EM3 23和EM4 24均与所述DSP芯片控制器3和ARM 控制器4通信连接。这些磁电传感器分别读取各自的零位设置标志，当四者均探测到信号 时，SCARA机器人实现精确复位，提高了复位的精确度。
[0027] 本发明采用DSP控制器3(TMS320F2812)+PLC控制器20+语音识别处理器28(芯片型 号LD3320)+永磁同步电机闭环控制芯片30(即IRMCK203控制器)+ARM控制器4(STM32F746) 五核进行系统控制。在电源打开状态下，ARM控制器先动态编辑LD3320的关键词语列表，增 加机器人语音识别率，然后对机器人备用电源SOC(荷电状态)和主电源进行判断，如果备用 能源较低，控制器将会通过无线装置向总站发出报警信号；如果备用电源和主电源工作正 常，先由总站通过PLC控制器或者基于LD3320语音识别系统把搬运货物大臂、小臂和手腕旋 转角度以及升降高度等信息输入给ARM，由ARM计算出机器人伺服系统的参数队列；SCARA机 器人携带的零位置传感器EMl、EM2、EM3、EM4和基于CCD的图像采集系统开始工作，二者引导 机器人复位到设定零位置，机器人进入自锁状态;一旦搬运命令开始后，机器人携带的避障 传感器、定位传感器、加速度传感器和陀螺仪均开启，SCARA机器人按照设定ARM优化的搬运 路径快速搬运，DSP与IRMCK203通讯，IRMCK203按照伺服参数和传感器反馈实时调整SCARA 机器人稀土永磁同步伺服电机X、稀土永磁同步伺服电机Y、稀土永磁同步伺服电机Z和稀土 永磁同步伺服电机R的PWM输出，实现四台稀土永磁同步伺服电机的实时伺服控制，CCD摄像 头实时检测搬运结果，ARM对数字图像的处理和存储并通过无线系统传输，主站通过图像对 比技术自动检测搬运结果。如果ARM对搬运某一个位置有疑问，将与DSP通讯，DSP通过 IRMCK203使SCARA机器人停车，然后主站通过图像对比对搬运信息进行判断，确认无误后通 过无线装置二次启动SCARA机器人继续未完成的任务;在机器人运动过程中，如果遇到紧急 情况，主站将通过麦克风装置向控制器输入控制命令，语音识别器LD3320识别后与ARM通 讯，有ARM处理后与DSP通讯，DSP通过IRMCK203把主站命令转化为各个稀土永磁同步伺服电 机的控制信号。
[0028]参照图1，图2，图3和图4、图5、图6，其具体的功能实现如下： DSCARA机器人电源打开后，为了提高机器人语音识别效果，进一步降低误识别率，ARM 控制器先向LD3320输入SCARA机器人常用的一些命令和指令，用来吸收错误识别，从而达到 降低LD3320误识别率的目的。
[0029] 2)ARM先对备用电源SOC和主电源状态进行判断，如果备用电源SOC较低时，DSP将 禁止四台稀土永磁同步伺服电机工作，电机输入PffM波被封锁，同时报警传感器将工作并发 出报警信号;如果电池 SOC正常，SCARA机器人进入待工作状态，等待工作命令。
[0030] 3)-旦总站工作命令开始，总站可以选择基于LD3320语音识别系统或者是PLC控 制器与ARM通讯，任何一个控制器都可以把大臂、小臂长度和升降杆的长度等信息传入到 ARM控制器，然后机器人开始校正零位置，SCARA机器人携带的磁电传感器EMl、EM2、EM3、EM4 和基于CCD的图像采集系统开始工作，并各自寻找设定的零位置标志，当磁电传感器EMl、 EM2、EM3和EM4均有信号输出时，ARM通过无线系统实时传输CXD采集图像，总站通过图像对 比技术确定位置无误后，通过无线装置与DSP通讯，DSP通过IRMCK203封锁四路稀土永磁同 步伺服电机的ΠΜ波控制信号，SCARA机器人自动引导零位置复位完成，ARM和DSP控制器此 时设定各个旋转角度爲二沒2 =.:?二?'，手腕上升尚度^V:==·?。
[0031 ] 4 )为了满足SCARA机器人的加减速运动需要，本发明采用如图6的运动时间梯形 图，此梯形图包含的面积就是机器人大臂、小臂和手腕要旋转的角度或者是手腕升降的高 度，为了方便控制，本发明采用单一加速度模式。
[0032] 5)SCARA机器人读取其工作模式如果是人工工作模式，主站开始通过PLC或者是基 于LD3320的语音识别系统与ARM控制器通讯，由主站输入SCARA机器人大臂、小臂和手腕需 要旋转的角度巧，:巧，巧以及SCARA机器人手腕需要上升或者是下降的高度鳩给六冊控制器， ARM控制器根据Denavit-Hartenberg算法开始机器人位置和姿态正向求解:ARM控制器首先 根据SCARA机器人大臂、小臂和手腕需要旋转的角度礓，涔，巧以及3041^机器人手腕需要上 升的高度鳩计算出相邻两杆件坐标系之间的位姿矩阵^4 1，％，1#^，财54，并用各自用4*4 二维数组标志，:，Jli5t分别表示如下：
[0033] ARM控制器然后通过公式
就可以求出手腕执行器完成任务 后的位置和姿态，并与DSP通讯，传输人工输入参数给DSP。
[0034] 6) SCARA机器人读取工作模式如果是正常自动搬运工作模式，主站开始通过PLC 或者是基于LD3320的语音识别系统与ARM控制器通讯，由主站输入SCARA机器人所处的初始 位置和给定位置三维坐标给ARM控制器4, ARM控制器4根据Denavit-Hartenberg算法开始机 器人逆向求解:ARM控制器4首先根据大臂、小臂长度以及最终的三维坐标中的X和Y坐标求 出大臂需要旋转的角度巧:，并借助:?的值求出巧的值，并根据三维坐标中的Z坐标求出手腕 上升或降低的高度禹，最终求出旋转角度咚，由于在求解巧的时候方程具有双解，所以 SCARA机器人求出大臂、小臂和手腕需要旋转的角度碭，巧，咚以及SCARA机器人手腕需要上 升的高度嶠后，ARM控制器会对求解结果进行优化，然后ARM控制器与DSP通讯，把机器人最 有伺服运动参数传输给DSP控制器。
[0035] 7)DSP控制器4接受SCARA机器人大臂、小臂和手腕需要旋转的角度以及 SCARA机器人手腕需要上升的高度右后，大臂、小臂和手腕上的传感器S1~S5、EM1、EM2和EM3 将开启，首先SCARA机器人要对零位置进行判断，确认初始位置无误后，SCARA机器人对各个 旋转臂旋转角度内的障碍物进行判断，如存在障碍物将向DSP发出中断请求，DSP会对中断 做第一时间响应，然后DSP禁止四轴IRMCK203工作，四轴稀土永磁同步伺服电机HVM波控制 信号被封锁，SCARA机器人禁止稀土永磁同步伺服电机X、稀土永磁同步伺服电机Y、稀土永 磁同步伺服电机Z和稀土永磁同步伺服电机R工作，机器人自锁在原地，基于CCD的图像采集 系统开启，ARM处理后通过无线装置向总站发出中断请求，为了防止信息误判，总站通过无 线传输图像二次判断运动范围内的障碍物信息，由总站人为处理障碍物，防止障碍物影响 搬运工作。
[0036] 8)如果总站二次通过图像采集结果确定无障碍物进入运动区域，将通过无线装置 与DSP通讯，DSP控制器按照图6的速度时间曲线对运动位置进行分解，DSP首先把三个旋转 角度:?,?,:?转化为三台稀土永磁同步伺服电机的加速度、速度和位置初始指令值，然后 DSP与IRMCK203通讯，IRMCK203结合电机X、电机Y和电机Z的光电编码器、加速度计和陀螺仪 反馈，经IRMCK203内部伺服调节程序生成电机X、电机Y和电机Z的HVM波控制信号，Pmi波控 制信号经驱动桥放大后驱动各个电机运动。DSP控制器根据输入偏差大小实时调整 IRMCK203内部伺服控制程序的PI参数，IRMCK203控制器通过调整稀土永磁同步伺服电机的 驱动脉冲数目调整其旋转角度，通过调整稀土永磁同步伺服电机控制信号的频率实现角速 度快慢的变化，使三轴稀土永磁同步伺服电机时刻同步工作，DSP时刻记录机器人已经运动 的位置参数，并通过无线实时向总站传输。
[0037] 9)在SCARA机器人移动过程中，传感器S1~S4时刻对运动范围内的移动障碍物进行 判断，如果有障碍物进入运动范围，DSP立即通过IRMCK203使SCARA机器人的大臂、小臂和手 腕按照图6的减速运动曲线立即停车，DSP控制器记录下当前旋转角度今，咚，咚信息，并通 过无线与主站通讯;等障碍物消失后，主站通过无线装置向DSP输入新位置的旋转角度M， 劣，鍈，然后DSP控制IRMCK203按照图6的运动曲线重新经过三段运动轨迹:加速运动、匀速 运动和减速运动，最终到达设定点。
[0038] 10)在移动过程中，DSP时刻记录加速度计反馈的大臂、小臂和手腕旋转的角加速 度，控制器通过二次积分得到大臂、小臂和手腕的旋转角度，并与设定的位置角度值相比 较，如果偏差大于设定阀值，DSP把这个偏差转化为三台稀土永磁同步伺服电机新的加速 度、速度和位置初始指令值，在下一个采样周期，DSP与IRMCK203通讯，IRMCK203结合电机X、 电机Y和电机Z电机光电编码器、加速度计和陀螺仪的反馈，经IRMCK203内部伺服调节程序 生成电机X、电机Y和电机Z的PffM波控制信号，PWM波控制信号经驱动桥放大后驱动各个电机 运动。DSP控制器根据输入偏差大小实时调整IRMCK203内部伺服控制程序的PI参数， IRMCK203控制器通过调整稀土永磁同步伺服电机的驱动脉冲数目调整其旋转角度，通过调 整稀土永磁同步伺服电机控制信号的频率实现角速度快慢的变化，进而消除上一个采样周 期产生的误差，使SCARA机器人按照设定轨迹完成任务。
[0039] 11)在SCARA机器人运动过程中，DSP会时刻储存所经过的SCARA机器人所处的位置 或者是经过的参考点，并根据这些距离信息由DSP计算得到相对下一个参考点SCARA机器人 稀土永磁同步伺服电机X、稀土永磁同步伺服电机Y、稀土永磁同步伺服电机Z分别要运行的 角度、角度速度和角加速度，IRMCK203结合电机X、电机Y和电机Z电机光电编码器、加速度计 和陀螺仪反馈，经内部伺服调节程序生成电机X、电机Y和电机Z的HVM波控制信号，Pmi波控 制信号经驱动桥放大后驱动各个电机运动。DSP控制器根据输入偏差大小实时调整 IRMCK203内部伺服控制程序的PID参数，通过调整稀土永磁同步伺服电机的驱动脉冲数目 调整其旋转角度，通过调整稀土永磁同步伺服电机控制信号的频率实现角速度快慢的变 化，使SCARA机器人按照设定速度快速前行。
[0040] 12)在SCARA机器人运动过程中，ARM和DSP实时记录机器人大臂、小臂和手腕旋转 的角度，如果总站发现机器人运行速度较慢，可以通过LD3320语音识别系统向ARM输入加速 命令，ARM处理后与DSP通讯，DSP根据加速时间要求，把剩余的角度转化为稀土永磁同步伺 服电机X、稀土永磁同步伺服电机Y、稀土永磁同步伺服电机Z分别要运行的角度、角度速度 和角加速度指令，IRMCK203控制器再结合电机电流反馈、加速度传感器、陀螺仪和光电编码 器的反馈，根据其内部三闭环伺服控制程序重新调整三台稀土永磁同步伺服电机的PWM控 制信号信号，使机器人快速完成任务。
[0041 ] 13)在SCARA机器人运动过程中，ARM和DSP实时记录机器人大臂、小臂和手腕旋转 的角度，如果总站发现机器人需要紧急检修，可以通过LD3320语音识别系统向ARM输入停车 命令，ARM处理后与DSP通讯，DSP根据停车时间要求，再结合电机电流反馈、加速度传感器、 陀螺仪和光电编码器的反馈，根据其内部三闭环伺服控制程序重新调整三台稀土永磁同步 伺服电机的PWM控制信号信号，使机器人迅速完成停车，DSP控制器通过加速度和陀螺仪传 感器实时记录机器人旋转的角度并储存。主站通过图像对比检测完毕后，通过LD3320语音 识别系统向ARM输入启动命令，ARM处理后与DSP通讯，DSP根据加速时间要求，把剩余的角度 转化为稀土永磁同步伺服电机X、稀土永磁同步伺服电机Y、稀土永磁同步伺服电机Z分别要 运行的角度、角度速度和角加速度指令，IRMCK203控制器再结合电机电流反馈、加速度传感 器、陀螺仪和光电编码器的反馈，根据其内部三闭环伺服程序重新调整三台稀土永磁同步 伺服电机的PWM控制信号信号，使机器人快速完成剩余任务。
[0042] 14)在SCARA机器人完成三轴旋转角度咚，咚，:?的伺服控制后，DSP二次检测加速 度传感器的积分值，如果发现SCARA机器人在运动过程受到外界干扰后三个旋转角度:?，咚 ，咚与设定角度的误差超过设定阀值时，C⑶图像采集系统实时采集搬运信息，经ARM处理后 通过无线装置向总站发出图像传输请求，总站通过图像对比技术自动检测搬运结果，DSP把 三个旋转角度偏差转化为三台稀土永磁同步伺服电机位置微调的加速度、速 度和位置初始指令值，然后DSP与IRMCK203通讯，IRMCK203结合电机X、电机Y和电机Z电机电 流、加速度计、陀螺仪和光电编码器，经IRMCK203内部伺服调节程序生成电机X、电机Y和电 机Z的PWM波控制信号，PWM波控制信号经驱动桥放大后驱动各个电机运动。DSP控制器根据 输入偏差大小实时调整IRMCK203内部伺服控制程序的PI参数，IRMCK203控制器通过调整稀 土永磁同步伺服电机的驱动脉冲数目调整其旋转角度，通过调整稀土永磁同步伺服电机控 制信号的频率实现角速度快慢的变化，通过三轴稀土永磁同步伺服电机的再次工作使机器 人大臂、小臂和手腕到达设定位置。
[0043] 15)当SCARA机器人的大臂、小臂和手腕完成角度礓，咚，咚的角度补偿到达设定位 置后，手腕上的传感器ΕΜ4将再次开启，首先SCARA机器人要对零位置进行判断，确认初始位 置无误后，DSP按照图6的速度时间曲线，把手腕要升降的距离心转化为稀土永磁同步伺服 电机R的加速度、速度和位置初始指令值，然后DSP与IRMCK203通讯，IRMCK203结合电机R电 机电流、加速度计、陀螺仪和光电编码器的反馈，经内部伺服调节程序生成电机R的PWM波控 制信号，PWM波控制信号经驱动桥放大后驱动各个电机运动。DSP控制器根据输入偏差大小 实时调整IRMCK203内部伺服控制程序的PID参数，DSP控制器通过调整稀土永磁同步伺服电 机的驱动脉冲数目调整其旋转角度，通过调整稀土永磁同步伺服电机控制信号的频率实现 角速度快慢的变化，使手腕平稳在设定时间内到达设定位置。
[0044] 16)在运动过程中如果SCARA机器人发现位置参数求解出现死循环将向ARM发出中 断请求，ARM会对中断做第一时间响应，ARM控制器将立即与DSP通讯，DSP立即通过IRMCK203 封锁四台稀土永磁同步伺服电机的控制信号，机器人原地自锁，ARM控制器通过无线装置与 主站进行通讯，CCD时刻采集周围信息并通过无线传输，主站对图像采集信息进行分析，并 根据采集结果二次重新启动SCARA机器人，通过PLC控制器二次向SCARA机器人输入搬运信 息。
[0045] 17)在SCARA机器人多次反复运动过程中，如果主站发现有临时重要任务需要加入 工作队列中，主站将开启LD3320控制器，由主站通过LD3320控制器与ARM通讯，向ARM输入重 要任务的位置伺服参数，ARM与DSP通讯，首先由DSP控制IRMCK203完成当前未完成的任务， 同时ARM更新DSP原有的伺服运动控制参数队列，并传输新的伺服位置参数给DSP，使DSP控 制IRMCK203完成重要临时任务。
[0046] 18)在SCARA机器人多次反复运动过程中，如果当磁电传感器EMl、EM2、EM3和EM4读 到传感信号时，DSP控制器将以零位置参数代替现有的位置参数进行新的位置伺服控制，并 计算出误差，在下一个采样周期，DSP通过IRMCK203内部三闭环稀土永磁同步伺服电机控制 程序对误差进行补偿，及时消除累计误差。
[0047] 19)在SCARA机器人搬运过程中，主站会对基于CCD的采集图像进行自动分析对比， 如发现问题某一位置存在问题时，主站先通过无线装置与DSP通讯，DSP首先控制IRMCK203 完成当前任务并在设定零位置停车，由主站自动检测搬运结果，自动检测完毕后通过ARM更 新DSP下一个伺服控制的位置参数，由DSP控制IRMCK203实现新位置的伺服控制。
[0048] 20)装在稀土永磁同步伺服电机X、稀土永磁同步伺服电机Y、稀土永磁同步伺服电 机Z、稀土永磁同步伺服电机R上的光电编码器会输出其位置信号A和位置信号B，光电编码 器的位置信号A脉冲和B脉冲逻辑状态每变化一次，DSP和ARM内的位置寄存器会根据电机的 运行方向加1或者是减1;光电编码器的位置信号A脉冲和B脉冲和Z脉冲同时为低电平时，就 产生一个INDEX信号给DSP和ARM内部寄存器，记录稀土永磁同步伺服电机的绝对位置，然后 换算成SCARA机器人大臂、小臂或者是手腕在三维坐标系统中的具体位置，ARM控制器实时 通过PLC与主站通讯，把重要的位置参数传输给主站。当磁电传感器EMl、EM2和EM3读到传感 信号时，DSP控制器将以零位置参数代替现有的位置参数，及时消除累计误差。
[0049] 21 )SCARA机器人在运行过程ARM控制器时刻对AC交流主电源进行监控，如果控制 器发现主电源出现故障突然断电时，ARM会与DSP通讯，并开通备用电源，由备用电源为四轴 稀土永磁同步伺服电机提供能量，DSP通过IRMCK203内部伺服调节程序实时调整四台稀土 永磁同步伺服电机的ΠΜ输出，IRMCK203控制器通过调整稀土永磁同步伺服电机的驱动脉 冲数目调整其旋转角度，通过调整稀土永磁同步伺服电机控制信号的频率实现角速度快慢 的变化，使SCARA机器人完成这次搬运任务，然后SCARA机器人通过PLC与主站进行通讯，并 通知主站进行检修。
[0050] 22)在SCARA机器人多轴伺服系统工作过程中，如果DSP伺服控制器检测到某一个 稀土永磁同步伺服电机的转矩出现脉动，由于本发明采用直接转矩控制，因此控制器会很 容易补偿此干扰，并根据干扰大小DSP实时调整IRMCK203内部的PI参数，减少了电机转矩扰 动对SCARA机器人运动过程的影响。
[0051 ] 23)当SCARA机器人完成任务，实现位置归零时，其携带的加速度传感器Al~A3、陀 螺仪、磁电传感器EM1、EM2、EM3、EM4和图像采集系统会时刻工作，先有加速度传感器检测其 加速度，当加速度超过预设阀值时，在下一个周期DSP控制器会通过IRMCK203修正上一个周 期带来的误差，当磁电传感器EM1、EM2、EM3、EM4均有信号输出时，C⑶图像采集系统实时采 集复位信息，经ARM处理后通过无线装置向总站发出图像传输请求，总站通过图像对比技术 自动复位结果，DSP通过IRMCK203封锁四路稀土永磁同步伺服电机的PffM波控制信号，SCARA 机器人自动引导零位置复位完成，SCARA机器人按照图6的速度-时间运动曲线实现从某一 点到零位的归位，然后零位自锁，等待下一批工作命令。
[0052]以上所述仅为本发明的实施例，并非因此限制本发明的专利范围，凡是利用本发 明说明书内容所作的等效结构或等效流程变换，或直接或间接运用在其它相关的技术领 域，均同理包括在本发明的专利保护范围内。
【主权项】
1. 一种重载无线传输五核快速关节机器人控制系统，其特征在于，所述关节机器人采 用稀土永磁同步伺服电机X驱动机器人大臂旋转运动、采用稀土永磁同步伺服电机Y驱动机 器人小臂旋转运动、采用稀土永磁同步伺服电机Z驱动机器人手腕旋转运动、采用稀土永磁 同步伺服电机R驱动机器人手腕升降运动，所述机器人大臂上安装有避障位移传感器S1、避 障位移传感器S2、陀螺仪和加速度传感器A1，所述机器人小臂上安装有避障位移传感器S3、 避障位移传感器S4、陀螺仪和加速度传感器A2，所述机器人手腕上安装有定位传感器S5、陀 螺仪和加速度传感器A3,所述的控制系统包括主站控制计算机和控制器，所述控制器包括 DSP芯片控制器、P1C控制器、永磁同步电机闭环控制芯片、语音识别处理器、图像采集模块、 无线传输模块和ARM控制器，所述图像采集模块与无线传输模块之间通信连接，所述DSPS 片控制器、永磁同步电机闭环控制芯片、语音识别处理器和P1C控制器均与所述主站控制计 算机通信连接，所述DSP芯片控制器、永磁同步电机闭环控制芯片、语音识别处理器和ARM控 制器彼此通信连接，所述无线传输模块和ARM控制器通信连接，所述P1C控制器与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中任一项所述的教学用无线传输三核快速关节机器人控制系统， 其特征在于，所述图像采集模块为CCD摄像头组件。5. 根据权利要求4所述的教学用无线传输三核快速关节机器人控制系统，其特征在于， 所述图像采集模块内设置有解码芯片。6. 根据权利要求1至3中任一项所述的重载无线传输五核快速关节机器人控制系统，其 特征在于，所述机器人大臂上安装有磁电传感器EM1，所述机器人小臂上安装有磁电传感器 EM2,所述机器人手腕上安装有磁电传感器EM3和EM4,所述磁电传感器EM1、磁电传感器EM2、 磁电传感器EM3和EM4均与所述DSP芯片控制器和ARM控制器通信连接。7. 根据权利要求4所述的重载无线传输五核快速关节机器人控制系统，其特征在于，所 述机器人大臂上安装有磁电传感器EM1，所述机器人小臂上安装有磁电传感器EM2,所述机 器人手腕上安装有磁电传感器EM3和EM4,所述磁电传感器EM1、磁电传感器EM2、磁电传感器 EM3和EM4均与所述DSP芯片控制器和ARM控制器通信连接。
【文档编号】B25J9/04GK106041940SQ201610412090
【公开日】2016年10月26日
【申请日】2016年6月14日
【发明人】张好明
【申请人】江苏若博机器人科技有限公司