上下料机器人设计.doc

上下料机器人设计摘要：主要介绍基于plc与机械结构针对于自动化机床与工业梁用机械人。简述了工业机械人现状发展现状及前景，及其主要发展方向与用途。发达国家使用经验表明：使用工业机器人可以降低废品率与产品成本，提高了机床利用率，降低了工人误操作带来残次零件风险等，其带来一系列效益也是十分明显，例如减少人工用量、减少机床损耗、加快技术创新速度、提高企业竞争力等关键词：机床上下料，自动化，plc，工业机器人机器人自平衡控制系统机器人自平衡系统在各行各业具有广泛应用，而轮式移动自平衡机器人由于具有结构简单、可控性强、成本低等优点，成为移动式机器人研究一个主要方向。自平衡机器人采用水平布置两轮结构，本身是一个不稳定体。也就是说，自平衡机器人在静止状态下，不能保持平衡，车体总是要向前或向后倾倒；而在运动状态下，可以通过一定控制策略使它达到动态平衡一 。由于自平衡系统具有内在不稳定性与结构灵活性，国内外机器人爱好者设计了多种结构、外观各异自平衡机器人，尝试采用各种控制策略使其达到自平衡控制。通常这类机器人采用姿态传感器检测机器人车体倾倒角度与倾倒角速度，根据当前机器人姿态控制伺服电机驱动电压转向与转速，从而使机器人保持平衡。该方式制作自平衡机器人虽然控制性能良好，但成本高，不适合广泛推广。本文设计机器人尝试采用红外测距传感器测量车体及地面距离，通过计算获取机器人姿态信息，进而实现机器人自平衡 。系统结构自平衡机器人系统主要由机械行走装置、控制系统与传感器3部分组成，机械行走装置主要由车体平台、电机驱动器、直流无刷电机、齿轮减速机构与车轮组成。机器人采用48 V电池供电，通过DC-DC模块转换为5 V与24 V 电压。其中，5 V提供给控制系统与传感器，24 V提供给电机驱动器。控制系统以ATmegal28单片机为主控制芯片，通过SJA1000扩展CAN总线及电机驱动器通信。红外测距传感器输出是及距离成正比模拟电压值，输入到单片机AD采样端口进行处理从而得到距离信息。机器人系统结构框图如图2所示。图2 机器人系统结构框图姿态信息获取机器人姿态信息包括倾倒图3所示，机器人左右两侧水平位置各安装有1个红外测距传感器，可以测量及地面之间距离d 与d 。当机器人在竖直状态时，左右传感器距离差为零。当机器人倾倒时，距离差及倾倒角度 成函数关系如下： 图3 传感器设置位置简图sin 0一(d2一d1)D其中，D为传感器之问距离。机器人在竖直动态控制时倾倒角度范围较小，此时sin0-0，即可由传感器距离信息得到机器人倾倒角度。角速度可以通过 对时间求导获得。选用红外测距传感器为Sharp公司GP2D1 2，输出为0424 V模拟信号，对应测量距离为1O80 cm。在同一温度下，传感器输出及测量结果呈良好线性关系，可以满足自平衡机器人要求。3 控制系统设计31 机器人平衡控制原理当车体偏离平衡位置(竖直位置)向前倾斜时，传感器采集信息并传送到单片机进行计算与判断，车轮随之作出响应向前运动，将车体向平衡位置调整；同样当车体向后倾斜时，车轮将向后运动。这样机器人一直处在倾斜判断、运动调整动态过程中，使车体始终保持在平衡位置附近，达到一种动态平衡。32 控制系统硬件设计控制系统主芯片选用Atmel公司基于RISC结构8位单片机ATmegal28。它采用低功耗CMOS工艺；具有丰富片上资源，包括4个定时器、4 KB数据SRAM、l28 KB程序Flash、可外扩至64 KBE PROM 与8个1O位ADC通道；拥有UART、SPI、I。C、JTAG接口，方便外部扩展与传感器接入一 。321 传感信息输入电路传感器信号通过放大器引入，通过设置反馈电阻(R1与R2)与输出端电阻(R3与R4)阻值可以使输入电压值在要求范围内。具体硬件电路如图4所示。322 扩展CAN通信电路单片机及电机控制器采用CAN e2,线通信，CAN接口采用Philips公司CAN 物理层与链路层接口芯片SJA1000与PCA82C250。单片机直接控制SJA1000ADOAD7、ALE、INT引脚。SJA1000工作在Intel模式下，MODE脚接高电平，片选脚CS接地，始终处于选通状态。扩展CAN通信电路如图5所示。单片机对SJA1000操作主要是对寄存器操作：一方面，对SJA1000模式寄存器、命令寄存器、状态寄存器、中断寄存器、中断允许寄存器、总线定时寄存器、输出控制寄存器、时钟分频计数器进行设置与检测；另一方面，对收发缓冲区进行读写，从而与CAN设备交换数据。图5 扩展CAN通信电路 控制系统软件设计自平衡机器人控制系统需要实现以下功能：传感器信息处理与机器人姿态信息计算，电机控制PID算法实现与CAN 口数据处理。系统控制流程如图6所示。系统上电后从主函数开始执行。主函数负责初始化控制器io 口、CAN 口、控制周期定时器、AD转换器、各个中断接收数据与发送数据模块等。初始化完毕，控制周期定时器时间到后，传感器信息输入到AD转换器中进行转换，计算机器人倾倒角度与倾倒角速度；然后调用PID自平衡机器人控制系统需要实现以下功能：传感器信息处理与机器人姿态信息计算，电机控制PID算法实现与CAN 口数据处理。系统控制流程如图6所示。系统上电后从主函数开始执行。主函数负责初始化控制器io 口、CAN 口、控制周期定时器、AD转换器、各个中断接收数据与发送数据模块等。初始化完毕，控制周期定时器时间到后，传感器信息输入到AD转换器中进行转换，计算机器人倾倒角度与倾倒角速度；然后调用PID算法计算电机控制数据，并通过CAN 口输出给电机驱动器。速度PID控制算法机器人采用经典PID算法进行平衡控制。控制系统采用典型双闭环控制结构与前馈控制方法。速度环路设计有利于提高电机输出速度精度。前馈控制主要是为了补偿电机转轴及车轮转动轴以及传动系统间隙误差，这些误差是稳定、可以测量。当电机在正反转变换时，控制系统可以检测到变换方向，经过位置误差环节调整后把确定偏差补偿量叠加到驱动主函数系统初始化CANH控制信号输出到电机驱动器器输出端。这样电机转动角度在 图6 系统控制流程原理基础上增加了补偿量，机械误差得以修正。采用PID控制算法公式如下：“(忌)一 (忌一1)+ A“(是)Au(k)一KPP(愚)一e(k一1)+K1P( )+KdP(尼)一28(矗一1)+e(k一2)其中，“(惫)为当前调节器输出量；“( 一1)为上一次调节器输出量；Au(k)为当前控制增量； (愚)为当前控制误差量；为比例系数；KI为积分系数；K 为微分系数。PID控制算法流程如图7所示。图中，e为调试过程中设定误差值。实 验利用该控制方法，对原理样机进行了多次实验。实验中，在机器人正上方水平位置安装陀螺仪，以采集机器人倾倒角度数据。实际测量角度信息随时间变化曲线如图8所示。通过测得数据分析可以看到，机器人大致可以稳定在0。附近，最大偏差为±2。 图8 机器人角度变化曲线基于plc控制系统设计数控机床上、下料机器人是油缸自动生产线上专用机器人, 其要完成卸料与装料动作, 并且要及数控机床、料架协调, 实现生产自动化。机器人为六自由度关节式结构, 由液压驱动, 运行平稳, 工作力矩大。通过限位开关、压力继电器采集信号, 再由PLC 控制液压系统电磁换向阀, 来控制机器人动作与位置。由于可编程控制器具有抗干扰能力强、可靠性高、易于使用与维护等特点, 所以, 以PLC 为核心这套控制系统运行非常稳定。1 控制系统硬件设计机器人操作控制面板如图1 所示。油泵电机启/ 停及外部负载电源通/ 断由总启动按钮与急停按钮控制, 手动调整或自动运行两种操作方式由转换开关(SA1) 选择。选择自动方式时, 按下启动按钮, 机器人自动运行一个周期后停下; 选择手动方式时, 用转换开关(SA2)选择相应运动形式, 由启动( 正向) / 停止( 反向) 按钮分别控制其两个方向运动。机器人电气系统采用限位开关(SQ1SQ8)作为位置检测信号, 从而实现准确定位。为了保证抓取工件时, 手指达到足够夹紧力后才可运动,在夹紧油缸液压回路中安装一压力继电器(SQ9), 作为压力检测信号。要保证机器人能准确地从料架上取到工件, 须有一料架位置正确检测开关(SQ10)与有无工件检测开关(SQ11)。系统输出信号分别接各油缸电磁换向阀电磁铁及原点指示灯。由于机器人为纯开关量逻辑控制, 所需I/O 点数为21/12, 各I/O 设备名称及相应功能。系统PLC 选用三菱公司生产FXON40MR, 系统硬件接线图如图2 所示。为了保护PLC 输出继电器, 在电磁铁两端各并联一阻容吸收电路, 防止在感性负载断开时产生很高感应电动势或浪涌电流对PLC 输出点及内部电源冲击。 机器人开关设计机器人有手动与自动两种工作方式, 由万能转换开关SA1 选择。在手动操作方式下, 各种动作都是用按钮控制来实现, 其控制程序可单独设计, 及自动工作方式控制程序相对独立。因此总程序设计成两段独立部分: 自动操作程序与手动操作程序。当选择手动操作方式时, 输入点X015 接通, 其常闭X016 常闭触点闭合, 则跳过自动程序段。若选择自动操作方式, 则跳过手动程序, 执行自动程序段。手动操作系统设计手动操作主要用于检修调整, 通过按钮对机器人每一步动作进行单独控制。例如, 当选择小臂伸/ 缩运动时, 按下启动按钮, 小臂伸出; 按下停止按钮, 小臂缩回。其它动作以此类推。这样, 其控制较简单, 可按照一般继电器控制系统逻辑设计法来设计。为了安全起见, 程序中需要设置联锁保护。例如, 只有大臂竖立时小臂才能上/ 下摆动, 因此小臂上/ 下摆动用大臂上限条件作为联锁保护。还有小臂处于上限位时, 才允许大臂上/下摆动; 小臂伸出时料架不能转位。另外, 由于大、小臂与手腕、手指运动都采用双线圈三位电磁阀控制, 两个线圈不能同时通电, 因此在其电路中设置互锁环节。自动操作程序设计在正常运行时, 机器人处于自动操作方式。数控机床在加工零件时, 机器人大臂竖立、小臂伸出并处于水平、手腕横移向右、手指松开, 即处于原始位置, 原点指示灯亮。加工完毕后, 按一下启动按钮SB1, 机器人动作顺序为: 原始位置( 大臂竖立、小臂水平且缩回、手腕横移向右、手指松开) 手指夹紧( 抓住卡盘上工件) 手腕横移向左( 从卡盘上卸下工件) 小臂上摆大臂下摆手指松开( 将工件放在料架上) 小臂缩回料架转位小臂伸出手指夹紧( 抓住待加工工件) 大臂上摆( 从料架上取走工件) 小臂下摆手腕横移向右( 把工件装到卡盘上) 手指松开( 原位) 。机器人完成一个自动循环后处于原始位置, 然后数控机床开始加工。待加工完毕, 再按启动按钮, 机器人又重复上述动作。这是一个典型按顺序动作步进控制系统, 可用PLC 步进指令编程。图3 为机器人自动操作程序状态转移图。3 此系统设计特点用可编程控制器设计机器人电气控制系统, 线路简单,系统可靠性高, 功能强, 整个系统运行稳定、精确, 同时还可根据需要对其进行功能扩展。典型机器人自用度极其结构特点并联机器人以其承载能力大、刚度大、精度高与适于在线控制等串联机器人无法企及优点成为机器人学研究热点早期国内外对并联机器人研究主要集中在6自由度与3自由度并联机器人，但是在相当多场合，如飞行模拟器与并联机床等等，需要是多于3自由度而又不必用到复杂6自由度，因此，4自由度并联机器人研究有着广泛应用前景目前，4自由度并联机器人基本上采用是非对称结构，非对称结构会导致非对称工作空间，从而使设计制造与任务规划复杂性增加Fang与Tsai提出了多种结构对称4自由度并联机器人机构，这些机构都是过约束，采用传统机构学理论不能够正确计算这些机构自由度数目及性质应用螺旋理论分析了多自由度并联机器人机构，所提出方法不但能够正确分析机构自由度数目，而且还可以确定各自由度性质，即各自由度所对应运动特性，并为这种机构进一步分析研究与应用提供了理论基础1 4一RRCR并联机器人机构结构特点新型4一RRCR是由4个结构相同支链分别连接动平台与固定平台而组成，每条支链由3个转动副R与1个圆柱副C所组成，并按RRCR顺序自固定平台到动平台依次串联而成，如图1所示图1中，代表第i条支链第J个运动副轴线方向矢量，它们在空间方位上满足s 平行s s 。位于同一平面上，sll与s3l平行，s2l与s4l平行，s Si4与s 5在机构运动过程中始终相交于0点，其中s3与s4分别表示圆柱副C转动副轴线方向矢量与移动副轴线方向矢量，s3与s4都及s 。垂直相交11 基于螺旋理论机构自由度分析螺旋$=(s；s0)由两个矢量对偶组成，也可用PlUcker坐标表示为$=(L，M ，N；P，Q，R)，(L，M ，N)与(P，Q，R)分别代表了矢量s与矢量s0这里，s代表空间一个矢量，称为对偶矢量原部；s0=s0+hs表示了该矢量在空间位置，其中s0= s0×s称为对偶矢量对偶部，是由原点至该空间矢量任一点矢径，h=s·s0(s·s)称为螺旋节距，是原点不变量当h=0时，螺旋退化为线矢量，当h=OO时，螺旋退化为自由矢量，也称为偶量，其在空间只有方向而，并联机器人没有位置概念机构所有运动副都可以用移动副与转动副单独表示或者组合而成，而移动副与转动副则可分别用移动速度偶量与角速度线矢量来表示当两螺旋$与$ 满足$ 。$ =0时，则称$r为$反螺旋，同样$r亦为$ 反螺旋，它们是互逆其中“。”表示两个螺旋作互易积对于运动螺旋来说，它反螺旋为力螺旋，代表着该螺旋所受到力约束对于一个空间 阶螺旋系$ (k=1 ， 6， 个螺旋均线性无关)，对应有6一 阶反螺旋系$ (m=16一 )$ 与$ 之间应满足$ 。$ =0 (1)当给定一个螺旋系以后，通过式(1)可求解出对应反螺旋系当该反螺旋系代表着机器人运动平台约束力螺旋系时，动平台自由度数目为F = 6一C (2)式中，C代表约束力螺旋系秩4一RRCR总共有4条结构相同支链，每条支链由5个运动副组成为研究方便，支链运动螺旋系用矩阵Ti=$ 表示及支链i运动螺旋系对应反螺旋为约束力螺旋，用$ 表示因此，运动平台受到约束力螺旋系用矩阵W =$ l$r2$r3$l4表示通过建立各支链 ，可由式(1)求得对应$ 进而得到w计算C=Rank(W)，由式(2)便可得到动平台F机器人运动螺旋系建立建立如图1所示坐标系，其中坐标原点ol在$ll轴线上， 轴与Y轴在基平面上， 轴及sll位于同一直线方向上，O点坐标为(z，Y， )，矢量，代表矢量OO首先建立支链1各运动副所组成运动螺旋系$tj=(sl ；，l ×sl )这里，(LIj，Ml ，NIj)与 (PIj，Ql ，RlJ)分别表示运动螺旋系$lj轴线方向矢量与轴线线矩矢量；$11过原点，因此rll：0；s12及sl3垂直相交，因此两矢量点积为零；$13与s15均过0点，因此rl3=rl5=r同理可以建立其余3个支链运动螺旋系如下：支链2($2j ) $21=(0，1，0；0，0，R21)；$22=(0，1，0；P22，0，R22)；$23=(L23，0，N23；yN23，zL23，一 23)；$24：(0，0，0；P24，0，R24)；$25=(L25，M25，N25； 25一zM25，zL25一xN25，xM25一 25)支链3($3j) $31=(1，0，0；0，0，R31)；$32=(1，0，0；0，Q32，R32)；$33：(0，M33，N33；sN33，xN33)；$34=(0，0，0；0，Q34，R34)；$35=(L35，M3s，N35；yN35-zm35，zL35一xN35， 5一止35)支链4($4j) $41=(0，1，0；0，0，R41)；$42=(0，1，0；P42，0，R42)；$43=(L43，0，N43；SN43，-xN43+43，-43)；$44：(0，0，0；P44，0，R44)；$45=(L45，M45，N45；yN45-45， 45一xN4545)13 4一RRCR并联机器人反螺旋系建立给定一个螺旋系，其反螺旋都可以由式(1)计算得到根据所建立支链 ，由式(1)便可得到对应$ 令$ =(s ；s )=(s r ×s +h ri$ )=(L M N ；P Q R )取支链1T1，将其坐标代人式(1)中，得可以看出，式(8)系数矩阵为组成支链1运动螺旋系坐标组合，该系数矩阵秩为5，从而可知支链1运动螺旋系之间是彼此线性无关因此，由线性代数理论可知，式(8)解出基础解系为，任一个都可以表示为$ 】：t$ ，(t=1，2，3)同理，分别代入支链2、支链3、支链4运动螺旋系坐标，可以得到4个支链力螺旋基础解系：$b= (1，0，0；0，2，一 )，$b2=(0，1，0；一2，0， )，$b3=(1，0，0；0，2，一 )，$b4=(0，1，0；一2，0，z)对上述力螺旋进行分析，可以得出以下结论：(1)所有力螺旋轴线矢量及所选坐标参数无关，轴线矢量第3项代表螺旋轴线及 轴方向余弦，因为都为0，即均及2轴垂直，都平行于 平面；(2)因为s 及 点积均为0，故所有力螺旋均为线矢量，因此h =0；(3)因为所有力螺旋均满足r× f=s0 故均过点O(z，y，2)；(4)反螺旋系W C=2，这也可以从螺旋理论得到，因为对于任意平面汇交线矢量，其最大线性无关数为2因此，根据式(2)，可以得到动平台运动自由度数目为414 动平台自由度性质分析自由度性质，即该自由度种类(如转动自由度、移动自由度等等)及其进一步描述(如转动自由度转轴描述、移动自由度移动轴描述等等)如前所述，互为反螺旋两个螺旋分别代表物体运动螺旋与力螺旋，则互易积就是力螺旋对运动螺旋所作功互易积为零，则力螺旋对运动螺旋作功为零，此力螺旋必为物体约束反力因此，反螺旋反映了物体被约束运动与物体为约束所允许运动，这样，通过对反螺旋分析就可以知道运动平台哪些运动为约束所允许，哪些运动受到约束(1)平动自由度分析从上面对动平台力螺旋分析可知，动平台上所受到4个力螺旋、$r2、$r3与$ 在同一个及静平台平行平面上，并且相交于O点对于动平台沿2轴移动，因为及xy平面正交，即力螺旋及沿2轴移动运动螺旋是相互垂直，做功为零，因此这个方向移动是为力螺旋约束所允许而沿平行xy平面平动均可以分解为沿相互正交力螺旋$rl，$r2方向，此时力螺旋对这类移动做功不为零，这些运动都将被力螺旋所约束掉，即动平台没有沿平行xy平面平动自由度(2)转动自由度分析 因为螺旋之间线性相关及否及坐标系选择无关n，因此移动坐标系到如图2所示坐标系此时$ 1=(S r1；s Ul )=(1，0，0；0，0，0)，$r2=(sr2；so2)=(0，1，0；0，0，0)假设空间任一线矢量$ ：(Sz；r ×S )=(L ，M ，N ，P ，Q。，R )为动平台某一个转动轴线矢量，且该转动是为力螺旋约束所允许动平台绕轴线$ 转动时，动平台上过原点沿s 1与sr2直线方向上所有点任何位移被约束，即对于$ ，如果r ×S -$rl0或者r ×S ·St20，那么这个转动是被约束因此，对于空间任一能够为力螺旋约束所允许转动轴线$ ，必将满足r ×S ·S r1=0， r ×Sz·Sr2：0 (9)同时，由于$ 为线矢量，故有s： r：×s：=L：P：+M：Qz+N：R：=0，代人各自坐标计算可得P =0， Q =0， N R =0 (10)机器人机械结构设计机器人工作任务要求工作任务要求该机器人在同一平面内三个矩形区域内(如图l所示)任意位置可拿起与放下物体。标准物体为 OII衄，高100mm圆柱体，在平面上立式放置，材料为45钢。机器人结构型式任务要求该机器人在 个固定平面三个区域内拿起与放置物体，所以，可采用水平关节式机器人。有大、小两个臂，腰与肘此，这两个自由度要随时控制。为了方便，采用自身带有减速器伺服电机驱动；而手爪夹蘩松开与上下移动这两个自由度，则可由开关量控翘，采用气缸驱动。考虑到三个可达区域具有一定对称性并且三个区域尺寸相同。故将对称中心0点作为腰转中心。因此，可初步确定该机器人结构型式，如图2所示。臂长确定大臂与小臂长度L1 与L2 是两个关键尺寸。合理取值会降低机器人转动惯量从而提高机器人回转速度，节约能量消耗，又使机器人外形美观、匀称。臂长确定，可以采用优化设计方法。确定优化目标(如能量消耗最小、回转角之与最小等)后，以臂长L1 与L2 为设计变量，再确定并建立约束函数，就可以采用合适优化设计方法进行优化设计。在目标函数取得最小值同时，得到设计变量I与 最优化值。关于该机器人臂长优化设计，本文作者有另文叙述，在此不再赘述。本文采用则是一种比较实际方法，即先由经验与任务要求选取合理尺寸，再进行验证，如果三个区域任意位置都可到达，就取定该尺寸；不满足则进行适当修改，再进行验证 这实际上是一种经验设计法。经验设计法最大优点最选取尺寸往疰比较切合实际，不至于有太大偏离。某一可达区域角点A是最远点OA=两个臂都不能短于500mm2=250mm即1060mm> ，k >250ram。另外当机器人末端达到最远点时机器人肘关节应基本上处在180角，为了保证有一定回旋余地，可比l8 略小，即L十 OA=IWoOrm-n。从节约能耗方面考虑，应使大臂短于小臂，而从任务要求来看，两臂长度应比较接近才合理，故初步取定L ：500ram，L2=600rrma。经过编制计算机程序并上机验证，结果证明该长度值能保证机器人末端达到三个区域任意位置。手爪结构形式及关键尺寸计算为使手爪结构简单实用，又能获得较大夹持力，因而采用了如图3所示双支点结构形式。两半手爪回转中心距离及被夹持物体回转直径相等。从结构上显然可以看出，当 O0 o2=90时，气缸力传递最好，此时在手爪上获得最大转矩M=F·I1：M ·L2。l2长度要尽可能短，为给被夹物体留出足够空间与满足回转轴结构尺寸要求，取 =7Onmi，I1=40mmI3取值主要受0角限制，当0角接近90时R从N转化面来力量最大，此时b为25rm'n。但这是极限值，考虑到实际结构，取I3=30nnn被夹物体质量m=4 x x100x 7、81000=153kg、B=mtetg43arc*in2550=8313。取摩擦系数f=O1假设手爪夹持物体后，向上移动最大加速度mn =5r s2，则可以列出如下公式。rm(G+m )2Nr·f该机器人及手爪在结构上主要有以下特点：腰部回转电机及减速器采用了倒置形式，减小了其在铅垂方向高度，又合理利用了腰部下面空问；大臂及小臂值得注意是，我们似乎在这里看到把摹同控制器处理带有微分环节系统，而该环节具有超前相位特性；实际上，由。z<0，He(s)仍是一个滞后环节，这可以l田它Bode图看出而出现s与s2项只是数学上近似结果主要用来研究采样频率附近及采样频率以下系统特性。4 整单周控瓤丘克变换器将控制器环节加入丘克电路小信号流图，便能得到完整单周控制丘克变换器小信号模型(圉6)。通过类似模型，可以艇释系统谐渡现象见3，而且，使高频段理论更加接近实验结果。图7是对应频率响应。可以看出，单周控制丘克电路具有很好稳定性能。) 截面都采用了倒u字形结构，在下面又增加了一个较薄盖板便于维护，又减轻了臂部重量； 手爪及小臂末端连接采用了上凸盖板形式。整个手爪可以从下面取出与装入，便于手爪快速更换； 换上不同手爪后，可以完成其它工作任务，因此，该机器人具有一定通用性 码垛机器人分布式控制结构微型计算机需要完成各种运算，如轨迹控制插补运算、坐标变换与伺服系统中补偿量计算等。这里包括了矩阵、三角函数等大量实时计算，通常需在515ms内完成，要在一台微型计算机上实现是困难。除了在算法、软件编制上注意采取减少运算时间措施外，在控制方式上，将计算分散于多台微处理器上进行。因此在结构上采用分布式结构两级微型计算机控制系统，如图1所示第一级为主控计算机(1 主CPU)它负责系统监控与作业管理。主要完成系统控制操作台及示教盒控制，显示服务，坐标转换，提前点计算插补计算，自动加减速计算，系统自诊断，IO控制，机器人语言编译等任务。l 主CPU 根据操作者命令与动作程序语句要求进行轨迹规划、插补运算及坐标变换，计算出各轴电机位置设定值序列。同时它还要接收并根据下一级反馈信号与外传感器信号判断任务执行情况与环境状态，然后向下一级各关节位置伺服系统传送一次及设定点相应位置更新值，实现对各关节运动协调与控制作用第二级为伺服控制计算机及交流伺服驱动装置，主要负责机器人运动控制。各从计算机根据主控计算机送来各关节位置指令，进行关节插补，得到新关节量。同时，不断地读取各轴编码器脉冲量，计算机器人现行位置。并用软件方法及给定位置进行比较，对偏差进行PID调节。伺服控制计算机实现对各关节数字位置伺服控制，其输出经DA变换后，作为交流伺服驱动装置速度设定值控制机器人本体运行。两级计算机通讯，由程序控制两级计算机之间并行口，实现两级间通讯。各从计算机用中断方式向主控计算机报告各关节当前位置，主控计算机用查询方式判明各关节是否完成要求位置伺服值，然后再向各从计算机送出各关节新位置设定数据及其它数据。码垛机器人控制器功能机器人在运动时候，有许多外来干扰作用在机器人各个关节处，例如由离心力、哥式力、重力等带来干扰力矩与干扰力。另外，围绕关节轴惯性力矩大小也随时间而变化。由于存在这些外来干扰，如果还是采用固定模式与缺乏抗干扰能力伺服控制系统话，那么机器人系统就很难产生高速与高精度动态响应。为了适应时刻变化对象，必须使伺服系统动作具有某种柔性，这种柔性是通过计算机程序来实现，故称为软伺服(software servo)。软伺服基本原理是采用反馈控制与采样控制方式。在某一时刻，采集机器人位置及速度等信息，然后对这些信息进行运算处理，在下一时刻输出运算处理结果。码垛机器人系统功能框图如图2所示，其中虚线框内内容表示机器人控制器应具备功能。 软伺服实现方法作业点示教通过示教盒对机器人位置进行示教。它把示教整个动作序列与轨迹点位置数据存人存储器，并以程序形式记忆下来。工作时执行上及机器人控制器以串行通信方式进行信息交互。轨迹生成为了生成机器人运动轨迹，在轨迹生成这一部分中求出在某一时刻，安装在机器人各关节处电机旋转角度、速度与加速度等目标值。码垛机器人运动轨迹生成原理如图3所示。在图中把计算方法分成为轨迹计算法1与轨迹计算法2_2。a轨迹计算法1。图3中输入q (位置、速度、时间)是对作业点示教中用机器人语言描述机器人动作进行解释结果，包括用作业环境直角坐标(绝对坐标系)表示目标位置，与向目标位置靠近理想速度，以及接近目标跟踪类型(CP型或PTP型)等。把输入q 从绝对坐标系变换为机器人本身坐标系(基座坐标系)各分量X ，y ，Z ，接着求出用基座坐标系表示随时间变化轨迹。即根据机器人语言形式给出指令速度以及当前位置到目标位置距离，求出每一时刻控制量，形成图2中所示时间序列数据。b轨迹计算法2。这一部分主要进行各种数据变换。在加速与减速时速度变化曲线基础上，把每一个采样时间7 时刻机器人位置设定值、速度与加速度时间序列数据变换为安装在机器人各关节处电机旋转角。旋转角速度与旋转角加速度，即进行坐标逆变换，求逆运动学反解。一般来说，机器人机械系统存在2O50Hz固有振动频率(= )，为了避免及机器人机械系统产生共振，采样时问问隔Tn <Tm10，这样应在15ms范围之内。但是，在必须考虑硬件成本前提下，在15ms时间内，完成浮点运算坐标逆变换与伺服处理是不大可能。所以还需要利用插补运算方法，进行配合来解决及运算速度之间存在矛盾。即对采样时问间隔Ts相邻两点之间轨迹再进行一次分割，把这相邻两点之间轨迹分成若干个点，然后用轨迹计算法2求每一个点目标值，使得机器人运动在这两点之间是平滑，从而使机器人以适当速度与加速度沿着平滑位置曲线轨迹进行移动。上述运算都是用浮点运算法在各从CPU 中进行。伺服控制把轨迹生成部分输出控制量作为指令值，再把这个指令值及位置等传感器送来信号由各从CPU进行比较，用比较后指令值，控制各伺服电机转动，其中应用了软伺服。在软伺服中，对位置及速度控制是同时进行。伺服控制部分接受通过插补运算得到各种目标值，遵循一定原则，形成控制电机电流指令值。在这个过程中必须对当前各个关节转动角度值与角速度值进行检测。用软件方法对设定目标值与从码盘采集当前位置反馈信号进行比较，对偏差进行PID调节，得到控制量经DA转换与初级放大后，作为各关节交流伺服驱动装置速度设定，再将速度设定值及速度反馈值比较，送至速度调节器，同样将得到控制量送到电流环。角度值可用编码器等位置传感器进行检测。角速度值用安装在机器人各个关节电机上测速发电机等速度传感器进行检测，也可通过对位置传感器检测到信号进行微分运算求出角速度值。伺服控制部分各种运算在15ms采样时问问隔内完成，仍是有困难，故还需进行插补运算来配合，然后对各个关节进行控制。34 电流控制电流控制方块接受从伺服系统来电流指令，对实际流过电机电流值进行检测与监控流经电机电流大小，保证电机按照来自伺服控制部分电流指令值进行旋转运动，检测与监控一般都采用模拟方式。码垛机器人四个关节都是使用交流伺服电动机驱动，其交流伺服驱动装置有模拟量速度调节器、带有脉宽调制(PWM)控制电流调节器构成反馈控制内环数字位置调节器为外环组成，如图4虚线框内部分。码垛机器人每次作业前先要整体回零，以校准各关节零位，作为轨迹规划及控制基准。针对码垛机器人回零特点及通讯方式，伺服控制又分零位、位置与终点伺服3种。零位伺服：码垛机器人回零动作是串行。当某关节回零完毕而其它关节尚未回零、或都已回零完毕而等待主控计算机传送位置设定值时，则运行零位伺服程序；位置伺服：位置伺服计算机接受主控计算机传来位置设定值后，则运行位置伺服程序。对每一点位置设定值，伺服3个采样周期，每个伺服有4个点向主控汁算机报告一次当前位置。当一次传送所有设定值都伺服完时，向主控计算机报告并转去执行终点伺服程序；终点伺服是当前位置伺服完成最后一点伺服计算后，将这一点设定值保存起来，并以这个值进行随后伺服动作。码垛机器人采用工业控制计算机作为主控计算机，伺服系统采用8952单片机。实现了PID控制算法，接收、处理多种信号，形成所需控制指令，达到由主控计算机协调各关节运动，准确地跟踪轨迹规划所给定空间轨迹，使速度完全可控，避免出现死区。本控制系统控制算法正确、可行。多传感器上下料机器人控制系统设计在现代化生产线上多传感器空瓶检测机器人，负责对进行灌装前空瓶质量进行检测。由于是在线检测，对检测速度有很高要求，需要控制多个不同传感器，所以对控制系统性能提出了很高要求。根据以上分析，本文提出了一种由工控机进行上层监控与通讯，PLC进行底层控制控制系统来完成多传感器检测机器人控制任务。1 控制系统基本结构系统基本结构如图1。检测机器人运行状态分为调试状态与正常工作状态。检测机器人处于正常工作状态时，控制系统工作流程为PLC通过光电传感器与编码器获取检测对象精确位置，当检测对象到达检测位时，通知图像采集系统启动CCD摄像机对其进行拍摄。将拍摄图像数据传输给专门信息处理系统去处理。得出空瓶质量是否合格结果后通知PLC。PLc再控制击出器在不合格产品到达击出位置时将其击出。在调试运行状态时，系统能在主控计算机监督下，按要求分别对各个光电传感器，图像采集子系统，击出器等进行静态调试，以使各个设备能处于系统需要正常状态。至于检测机器人传送系统则由变频器控制，具体启动，停止等控制以及速度设定由工控机管理。传动系统控制多传感器检测机器人传动系统由交流电机驱动，使用变频器来控制。由于在一般情况下检测机器人传动系统是以恒定速度运行，而且因为使用了光电传感器与编码器来检测对象位置，及传动系统运行速度无关，所以无需复杂控制策略，主要是由工控机按要求启动与停止变频器，并相应调整变频器频率，从而控制传动系统运行速度即可。变频器可以选用西门子MICROMASTER 420，该系列变频器运行可靠、功能完善，易于安装与调试，既可适用于简单电动机控制也可用于更高级电动机控制系统。而且该系列变频器具有串行接口，可以通过485总线及PC机进行通讯。因为工控机上串行接口是232模式，所以工控机为了与变频器通讯，需要使用一个232转485转接器。在通讯时，两者间采用是半双工模式。MICROMASTER 420变频器通讯协议是USS协议。协议中报文结构如下：其中ST×区内容为02hex，代表一条信息开始；LGE区为一个字节，内容表示信息长度；ADR区是一个字节，表示变频器地址；BCC 区内容是校验与，采用异或方式，长度为一个字节。报文中数据区分为两个部分，即PKW 区(参数识别ID一数值区)与PZD区(过程数据区)。PKW区用于对变频器参数处理，本文没有涉及到这方面操作，所以不再详细讨论。下面主要介绍PZD区使用。PZD区一般为两个字长，分为主站到变频器与变频器应答主站两种。其中主站到变频器任务报文第一个字是变频器控制字STVv，第二个字是主设定值HSW。其中s1、字各位含义如表1所示。HSW 字是主频率设定值，用来设定变频器主频率。根据参数P20o9设置，可以采用两种不同方式。当P20o9=0时，数值用16进制方式发送，并规格化为P2000设定频率，即实际输出频率=(HSW )xP2OoO中额定频率16384。而当P2009=1时数值是以绝对十进制数形式发送。从变频器到主站应答报文也是两个字，分别为变频器状态字ZSW 与变频器参数实际值HlW。当变频器接收到主站任务报文后，会将变频器状态经由应答报文告知主站。在实际进行通讯前，需要先设定好变频器几个参数。设定参数P00o3=2或3，以便访问第二级或第三级参数；设定参数P2010， 使变频器波特率及主机波特率相同； 设定参数P2011，规定变频器通讯地址；设定参数P0700与P1Oo0，使主站能够通过USS协议控制变频器及发送主设定值给变频器。在工控机上可以使用MicrosoEActiveX控件MSComm 来编写串口通讯程序。MSComm控件功能强大，但使用起来确并不复杂。MSComm封装了2O多个及通讯有关属性，其中比较常用有如下几个：CommPort属性：设置并返回通信端口号；Seings属性：以字符串形式设置并返回波特率、奇偶校验、数据位、停止位；PolOpen属性：设置并返回通信端El状态，也可以打开与关闭端El；Input属性：返回并删除接收缓冲区数据流；Output属性：向传输缓冲区写一个数据串；CommEvent属性：用于反映通讯中出现事件或错误类型。同时MSComm控件还有一个事件OnComm，当通讯中出现事件或错误时会触发该事件。利用以上属性与事件，可以很方便地编写出根据USS协议及变频器通讯，从而控制变频器运行程序来。3 PLc底层控制