LM628在工业机器人伺服驱动中的应用

图1 典型工业机器人本体结构

    工业机器人运动实现的数学模型主要为运动学方程和动力学方程。运动学方程是关节空间向操作空间的映射（即相对于静系的空间模型建立），其逆解则是由映射求其在关节空间中的原像（即位姿控制）；动力学正是根据各关节的驱动力（力矩），求解机器人的运动（关节位移、速度和加速度），其逆解是已知关节位移、速度和加速度，求所需关节力（力矩），是实时控制的需要。

    设某6轴（关节）工业机器人在D-H坐标系中轴间连杆的D-H参数如表1。

表1 工业机器人参数

    根据关节传递关系，可求得该机器人的运动方程为：

    

    式中，Ai＝A1，A2，A3，A3，A4，A6为第i连杆的齐次变换位姿矩阵，n、o、a为动系单位矢量，p为位置坐标分量。以下用sθ表示sinθ，cθ表示cosθ，θi为i连杆关节转动角度。则式（1）有：



    由于末端执行器位姿根据其运动方程已经确定，为求关节变量θi（即电机转动角度），用未知的连杆逆变换（Ai-1）乘运动方程式（T6）也就是求出运动方程逆解，即可求出θi：



    在求解运动方程逆解时，会存在多解问题，应根据实际取最优解（即涉及路径规划问题，本文不讨论）。

    根据运动方程及执行末端的位姿变化量微分，可得出工业机器人执行末端在操作空间的广义速度v：

    v＝dx／dt＝J（q）·（dq／dt）
    即：v＝xJ（q）·q

    式中，q为关节在关节空间的关节速度；J（q）为确定关节空间速度q与操作空间速度关系的雅可比矩阵。

    

    其前三行为相对执行末端的线速度v的传递比，后三行为相对执行末端角速度ω的传递比。J（q）每一列（第i列），表示其它关节不动，第i关节运动产生的端点速度。当给定工业机器人执行端运动速度，可求出相应关节速度为：q＝J^-1v。式中，J^-1为工业机器人逆速度雅可比矩阵。
分页
2 LM628在运动伺服控制中的应用

    2.1 LM628简介

    LM628为28脚双列直插封装形式。

    上位计算机通过命令的方式为LM628进行控制和参数设定、读取。在这些命令中，一般可分为两大类，一类只有命令代码，另一类在命令代码后还要加上相应的数据代码（例如：设定的参数值）。

    LM628主要特点如下：32bit位置、速度、加速度寄存器；16bit的可编程数字化PID调节器；可编程微分采样周期；8bit或12bit DAC输出；8bit PWM输出；内部梯形速度特性产生器；在运动期间速度、目标位置和滤波器参数可以改变；具有位置和速度两种操作模式；实时可编程的中断；8bit异步并行接口，用于积分增量编码器标准脉冲输入接口。

    2.2 LM628的应用实现

    工业机器人系统主要由上位计算机、关节（轴）运动伺服系统、执行机构、限位报警等部分组成。上位计算机是工业机器人系统的主控制器，完成通信、设置LM628参数、监控电机运行等。执行机构为电机及其驱动器等。

    传统的关节运动伺服控制器一般都采用通用微机或单片机实现伺服控制，但外围电路复杂，计算速度慢，从而导致控制精度不够理想，而采用电机专用智能控制芯片LM628能很好地实现精度与速度控制要求。

    从工程观点看，工业机器人运动控制系统的基本单元是机器人单关节的位置、速度控制。根据前述分析的6关节工业机器人的运动方程逆解求出的θi，可确定关节位置（电机转动角度），反馈元件用增量编码器。据逆速度雅可比矩阵求解出的关节速度q的测量反馈使用能直接测得代表转速的电压且最有良好实时性的速度传感元件，即测速发电机。其单关节伺服控制基本系统如图2。

图2 单关节伺服控制基本系统

    LM628通过8bit并行I／O口和6条控制线与上位计算机进行数据交换。上位计算机每间隔一个访问周期（如10ms）把根据工业机器人运动方程、动力学方程等规划好的关节空间位移、速度、加速度数据送到LM628接口寄存器（32bit）中，以命令的形式对LM628的梯形速度特性和PID数字滤波器进行参数设定（以32bit双字节存于RAM）。LM628在其每个时钟周期 （如341μs）进行一次位置、速度检查，如未达到指定位置或速度，则继续运动或加速，将要达到指定位置时会自动地按照设定的加速度进行减速，完成关节空间的位置闭环控制，见图3。同时用于检测电机旋转位置的增量编码器的输出信号（实际位置θi）经整形后作为伺服单元的位置反馈量经光电隔离器通过编码器输入接口（IN、A、B）送入LM628，并与在LM628中与设定的位置信号（上位机给定）相减，形成位置误差信号。该信号送人PID数字滤波器进行处理后形成控制信号，通过12bitDAC并行接口以数字化的形式输出，经电平转换和放大输入给关节伺服单元作为速度调节器的给定，从而实现关节电机的位置闭环，使工业机器人执行端到达空间坐标系的准确位置。同时，LM628要对位置给定进行平滑处理，以满足机器人和运动时关节平滑的要求。

图3 两种典型速度曲线

    由于LM628具有位置、速度控制两种方式，在设定位置控制方式时，上位机把位置、速度、加速度送入LM628，LM628利用这些数据计算运行轨迹，如图3（a）。在电动机运行时，可改变运行参数，产生如图3（b）所示的轨迹。设定速度控制方式时，电动机仅需要加速度和速度两个参数，电动机以规定加速度至规定速度，并维持这个速度直到指令结束。

    LM628内的梯形速度曲线发生器计算出期望位置与时间关系。在位置控制方式时，上位机确定了加速度（减速度）、最高速度和最后位置并通过用户指令传递给LM628，LM628利用这些信息使电动机以规定加速度到最高速度，或开始减速，直至停到规定的最终位置上。在某些运动过程中，最高速度或目标位置允许改变，电动机相应进行加速或减速，如图3（b）。

    为了提高控制精度，工业机器人中的LM628数据输出并行接口采用12位方式。而LM628的8位数据口与12位的数据模转换芯片DAC1210一起使用时，要用74LS378（触发器）来完成数据的传输，如图4所示。LM628首先送出低8位，其中DAO为输入触发器的时钟信号CLK，DAI为触发器的允许信号G，DA2-DA7为低6位数据。然后，LM628送出高8位字节，此时DAO为送给数模转换芯片的写信号WR1，DA1是片选信号CS，DA2-DA7是高6位数据，这样就合成了12位的DAC输入。信号经数模转换后在放大合成±10V的信号输出到伺服控制器，驱动电机运动。

图4 8位DAC扩展为12位DAC电路

分页
    2.3 软件结构

    LM628主要工作程序流程如图5。其中需LM628运算处理的程序主要是根据实时检测信号（位置θi与速度等）和上位计算机给定相应值的偏差，进行PID计算，经12位DAC输出并驱动电动机。为了保证静态位置误差为零，减小系统的超调，保证系统的动态特性良好，便于实时快速计算处理，在工业机器人中采用增量式PID运算控制调节器。

图5 主程序流程图

    在工业机器人运动连续控制系统中，PID控制规律基本形式为：

    

    式中，e（t）为调节器输入函数，即给定量与输出量的偏差；Y为调节器输出函数， 为比例系数；为积分时间常数； 为微分时间常数。

    经将连续形式的微分方程变换为离散形式的差分方程，即PID控制规律的离散化形式：

    

    式中，u（k）为采样时刻k时的输出值，e（k）为采样时刻k时的偏差值，e（k－1）为采样时刻k－1时的偏差值， 为采样周期。

    该式对应于被控对象的执行机构每次采样时刻应达到的位置，因此称为PID位置控制算式。式中输出值u（k）与过去所有状态有关，计算时要占用大量的内存和花费大量的时间。为避免此情况，故需采用增量型控制，即输出量是两个采样周期之间输出增量△u（k）。PID增量式控制算式为：

    

    为加快计算速度，可将上式进行算式简化合并整理为：

    

    式中，系数a0、a1、a2可先进行计算，然后代入再进行程序运算。其程序流程如图6。

图6 增量PID软件流程

3 结 语

    LM628作为电机专用运动智能控制芯片，应用范围非常广泛，通过应用研究，LM628能较好地实现连续、实时控制，达到工业机器人关节伺服驱动的速度和精度要求，从而提高生产率和产品质量。