控制算法。如执行moveUpositionA,1,0. 2)运条语句时运动控制算法的计算过程如 下：首先查到运条语句表示从当前位置直线运行到positionA, positionA的位置由示教的 位置确定速度大小是1，转弯弧度是0. 2。然后运动控制算法计算当前点和POStionA点直 线运动过程，速度大小是1，转弯弧度为0. 2的各轴运动弧度。每个4ms周期内，将轴关节 弧度根据减速比转化为各轴电机的运动刻度。计算过程中要保证电机的速度、加速度、力矩 不能超过电机的限制范围，W防止电机抖动和失速。同时也要充分利用电机的性能，保证 码煤速度。经测试，采用SANYO(山洋）电机，1、2、3轴型号为R2AA18450HCP00,4轴型号为 R2AA13180DCP00,码煤运动速度大于800包/小时。运动的形式有直线运动，转弯运动。示 教点设置的坐标系有基本坐标系、工作台坐标系等。
[0034] (6)运动控制算法计算的电机刻度每隔4ms发送给电机解码器。电机解码器通过 化hercat总线和伺服电机驱动器通信，将电机运动刻度发送给驱动器，驱动器驱动电机转 动。
[00对 （7)到达位置后，通过返回电机的刻度，计算出实际位置和理论位置的误差，然后 在下一条语句的执行过程中，将误差进行补偿，保证运动的精度。经测试，重复定位精度在 0. 05mmW下。
[0036] (8)在码煤的过程中，机器人控制系统需要和外部设备如线体，光电开光等进行信 号交互才能实现自动码煤，信号交互通过10板来实现。
[0037] 如图2和图3所示，D-H参数如下：
[0038]
[0039] al= 0. 261，a2 = 0. 942,a3 =I. 022,a5 = 0. 22,dl= 0. 745,d6 = 0. 2515 ;
[0040] 根据D-H参数，机器人坐标系里面的任何一点A(X，y，z)，就可W求出对应的各轴 关节角度 0 1、0 2, 0 3, 0" 0 6。
[0041] 然后根据多次样条插值，通过减速比换算后，算出每4ms电机应转的刻度，发送给 电机解码器模块。在插补的过程中，保证电机的速度、加速度在合适的范围内，保证电机不 抖动、不失速，力矩输出足够带动120kg负载。
[0042] W上所述的本发明实施方式，并不构成对本发明保护范围的限定。任何在本发明 的精神和原则之内所作的修改、等同替换和改进等，均应包含在本发明的权利要求保护范 围之内。
【主权项】
1. 120公斤负载的四轴码垛机器人控制系统，其特征在于，包括示教器、机器人程序语 言解析模块、系统通信模块、主控单元、IO板、脉冲板、运动控制算法模块和电机解码器，其 中： 示教器用于编写机器人运动程序语言、设置机器人运动程序的指令、示教运动点； 机器人程序语言解析模块通过自带的程序解析器对自定义的机器人运动程序语言进 行语法检查，并生成对应的符号表和语法树； 系统通信模块负责机器人语句执行时主控单元、示教器、电机解码板、IO板的通信和调 度； 运动控制算法负责将用户示教的点进行运动插补，并在每一个插补周期时间内，机器 人末端坐标相对位置，转化为电机相对脉冲刻度，并发送给脉冲板； 电机解码器包括电机解码板，针对不同的电机，将脉冲板的相对刻度转化成电机的输 入，驱动电机运动，用户示教一些点，机器人结合外部IO信号，形成运动轨迹。2. 根据权利要求1所述120公斤负载的四轴码垛机器人控制系统，其特征在于，所述 示教器包括机器人程序编辑框、机器人指令按钮、机器人摇杆、相关菜单，示教器和主控单 元通过网口连接并交换数据，用户输入的命令，编辑的文件通过约定的协议发送给主控单 元；主控单元也发送给用户的信息。3. 根据权利要求2所述120公斤负载的四轴码垛机器人控制系统，其特征在于，网口 的连接有效性通过心跳信号保证。4. 根据权利要求1所述120公斤负载的四轴码垛机器人控制系统，其特征在于，所述 主控单元是系统的综合调度单元，系统的其他模块接收主控单元的指令完成各自的工作， 主控单元和示教器、程序解析器、运动控制算法、电机解码器、IO板互相通信，交换数据；其 中主控单元、程序解析器、运动控制算法集中在同一个物理设备工控机中，电机解码器和IO 板通过网口和主控单元通信。5. 根据权利要求1所述120公斤负载的四轴码垛机器人控制系统，其特征在于，机器人 程序语言解析模块对用户编辑的机器人程序语言进行语法分析，如果有错，就将错误信息 发送给主控单元，再转发给示教器，用于用户修改；如果没错，建立对应的符号表和语法树， 机器人程序语言为系统设计，语法规则由系统规定。6. 根据权利要求5所述120公斤负载的四轴码垛机器人控制系统，其特征在于，语法规 则中，moveL(positonA,speed, 0):表示以速度大小为speed，直线运行到positionA位置。7. 根据权利要求1所述120公斤负载的四轴码垛机器人控制系统，其特征在于，电机解 码器负责跟电机驱动器通信，驱动电机运动，电机解码器采用Ethercat总线和电机驱动器 通信。8. 根据权利要求1所述120公斤负载的四轴码垛机器人控制系统，其特征在于，IO板 负责系统和外部辅助设备的交互IO信号。9. 根据权利要求8所述120公斤负载的四轴码垛机器人控制系统，其特征在于，外部辅 助设备包括输送线、光电开关和抓手。10. 根据权利要求1所述120公斤负载的四轴码垛机器人控制系统，其特征在于，插补 周期为4ms。
【专利摘要】本发明公开了一种120公斤负载的四轴码垛机器人控制系统，包括示教器、机器人程序语言解析模块、系统通信模块、主控单元、IO板、脉冲板、运动控制算法模块和电机解码器，示教器用于编写机器人运动程序语言、设置机器人运动程序的指令、示教运动点；机器人程序语言解析模块对机器人运动程序语言进行语法检查，并生成对应的符号表和语法树；系统通信模块负责机器人语句执行时主控单元、示教器、电机解码板、IO板的通信和调度；运动控制算法将用户示教的点进行运动插补；电机解码器针对不同的电机，将脉冲板的相对刻度转化成电机的输入，驱动电机运动，本发明控制机器人工作，满足工厂、车间的自动码垛需求。
【IPC分类】B25J9/16
【公开号】CN105196293
【申请号】CN201510689964
【发明人】石江涛, 尤昕, 余田田, 孙昊阳, 韩茹, 李文鹏, 李守红
【申请人】合肥泰禾光电科技股份有限公司
【公开日】2015年12月30日
【申请日】2015年10月22日