David 的 移动机器人运动控制器的设计
移动机器人运动控制器的设计http://www.ee365.cn 2005-04-26 23:39 eaw 摘    要：本文研究了以TMS320LF2407 DSP为核心的移动机器人运动控制器的设计方案，介绍了主要的系统组成。本系统采用模块化设计，硬件电路简洁，控制可靠。通过步进电机的细分控制，改善了低速性能，以较低的成本满足了移动机器人运动控制的性能要求。 关键词：移动机器人；TMS320LF2407；运动控制；模块设计 引言 运动控制器是移动机器人的执行机构，对系统平稳运行起着重要作用。在机器人运动控制器中，处理器件接受高层控制级的指令，计算和输出多路控制信号，协调各驱动轮，并对系统状态进行监控。因此要求处理器要有高速的运算能力和高度的可靠性。DSP芯片集实时处理和控制器外设于一身，精度高、体积小、运算速度快，同时具有多级流水线操作，容易实现复杂算法，为运动控制器的开发提供了理想的解决方案。 本文提出的移动机器人运动控制器的实现方案，以TI公司的定点DSP芯片TMS320LF2407为核心。利用步进电机作为驱动机构，DSP从上位机接收指令，控制步进电机实现要求的动作。系统以较低的成本满足了控制要求。 运动控制器的设计 机器人移动平台是一个六轮小车，两侧的前轮和中轮采用摇臂式结构，前轮与中轮间距小于车体宽度，易于转向。机器人主控计算机是一台PC机，作用融合各种信息，发出控制指令，进行远程监控；下位机是一台工控机，固定在移动车体上，负责接收主控计算机指令并分配给各子系统，同时把各种检测信息反馈到PC机，两者通过无线网桥进行通信。运动控制器接收下位机传送的指令，计算各个电机的控制量、输出控制脉冲、检测各反馈信号并发送至下位机。六个车轮均为驱动轮，控制时可简化为二轮情形。运动控制器以LF2407为核心，主要模块包括：控制脉冲输出、检测处理、通信接口、复位电路、I/O扩展和必要的外围电路，基本结构如图1所示。 控制脉冲输出 本系统步进电机为两相混合式，步进角为1.8涮浊魑猄H-2H057M。驱动器输入信号有步进脉冲信号CP、方向电平信号DIR、以及脱机信号FREE。OPTO是三路信号的公共阳极。驱动器拨位开关的前3位用于设定细分数。 LF2407利用事件管理器(EVA、EVB)中的定时器中断形成步进脉冲。EVA通过定时器1，EVB通过定时器3分别输出两路步进脉冲信号CP和方向信号DIR到各电机驱动器。控制时，LF2407根据上位机指令计算各驱动电机转速和方向，选择回路内部分频值，确定对应定时器的周期寄存器初值，然后计数器循环计数。当计数器的值与周期寄存器匹配时，即发生定时中断，向对应I/O口输出控制信号。如：车轮直径为30cm，减速箱减速比为1:15，如设定电机驱动器细分数为5，即步进角为0.36绻笮〕抵毕咴硕俣任?5cm/s，则每秒钟各电机所需的步进脉冲数为15×(0.25/(0.3×p)×360/0.36≈3978。LF2407内部时钟为24MHz，如选择时钟分频值为64，则周期寄存器的值为24M/(64×3978)≈94，计数器每次计数到94时，即发生中断，输出一次步进脉冲和方向标志。 为防止给定速度突变使步进电机失步，LF2407在接受到速度指令后首先进行积分平滑，平滑后的结果作为控制要求转换成步进电机的脉冲数值，平滑公式为：       (1) 其中，t为控制的时间周期，与系统积电时间常数有关，需通过实验确定合适的数值。如果平滑后的速度与现有速度相差较大，则由系统取两者平均值作为过渡速度，运行若干周期后再转换到实际给定速度。 此时应注意：(1)设定脉冲频率时要保证CP信号的脉冲宽度不能小于5ms，且为低电平有效；(2)电机换向时，方向信号DIR要在前一个方向的最后一个CP脉冲结束且下一个方向的第一个CP脉冲前发出；(3)OPTO端须外接+5V的VCC，保证给驱动器内部光耦提供8~15mA的驱动电流，如果VCC不是+5V则须加限流电阻。 检测处理模块 检测模块监控系统当前状态，由光电编码器、ADC和其他检测电路组成，把驱动电机的转速、转向和电流及蓄电池电压反馈至LF2407。蓄电池电压通过ADC转换成数字信号，当电池电压低于警戒值时，向上位机发出电池能量不足的警告信息。电机电流利用分流电阻检测，经A/D转换后输入到LF2407中。分流电阻的阻值应能在功放电路达到最大电流时激活过流保护功能。此模块中，ADC的参考电压由外部电压源提供，其模拟供电电压(3.3V)VCCA和模拟地VSSA需与数字供电电压和数字地分开，以提高精度。 本系统在两个后轮安装了光电编码器用于检测电机转速和方向。检测信息反馈到DSP的正交编码脉冲电路(QEP)，通过把检测到的脉冲信息与给定的脉冲数进行比较，分析电机是否出现堵转、失步等故障。当LF2407的QEP被使能(禁止捕获功能)，定时器2对引脚CAP1/QEP1和CAP2/QEP2、定时器4对引脚CAP4/QEP3和CAP5/QEP4上的脉冲数目和频率分别计数，可得到两个电机的角位移和转速，方向检测逻辑通过检测哪个序列领先可确定电机转向。此时正交编码器脉冲作为时钟源，定时器设置为外部时钟、定向增/减计数方式。 通信模块 通信模块涉及到DSP和下位机(工控机)之间的双向通信。主要任务是接收控制指令，向工控机传送运动控制器的当前状态和里程计信息。本模块由串行通信接口(SCI)负责通信转换，利用中断进行控制，其接收器和发送器可以单独或同时工作，并具有独立的中断请求信号和中断矢量。工控机和LF2407之间采用符合RS-232标准的驱动芯片MAX232进行串行通信。由于LF2407采用+3.3V供电，故在MAX232与LF2407之间加入TI公司提供的典型电平匹配电路，整个接口电路简单、可靠，其电路图见图2所示。 LF2407要实现双向通信，必须在初始化时正确设置一系列通信接口控制寄存器，包括操作模式、协议、波特率、字符长度、奇/偶校验位、停止位的位数、中断优先级和使能控制位。通信时，上位机给出控制要求(平移速度和角速度)和数据校验和，等待反馈信号。LF2407以中断方式接收数据、计算校验和，并与接收到的校验和比较。若一致，则回发“0DDH”，表示接收成功；否则，回发“0BBH”，表示数据错误，要求重发。同时，LF2407以中断方式向上位机发送计算结果、电机转速和里程计信息。 复位电路 要使控制系统上电后正常工作，必须对DSP寄存器和I/O端口进行复位。LF2407的复位引脚RS为低电平时DSP终止执行，并使PC=0；当RS为高电平时，从程序0位置开始执行。当看门狗定时器溢出时，在该引脚产生一个系统复位脉冲。图3为复位电路。刚上电时，LF2407处于复位状态，复位时间由R和C确定。为使芯片初始化正确，低电平应保持至少3个时钟周期，同时需满足DSP晶体振荡器的稳定时间。 图3中A点电压 ，其中t为时间常数，且t=RC。假设低电平与高电平分界点为V0=1.5V，则当V=V0时所经过的时间t0为：            (2) 选择R＝200KW，C＝4.7mF，VCC=5.0V，由上式得t0=334ms，采用16.0000MHz晶振可满足系统复位要求。 航迹推算 本系统结合光纤陀螺仪和倾角传感器的角度变换进行航迹推算，上位机建立三维环境坐标系和车体坐标系，包括机器人的位置(x,y,z)和姿态(a,b)，根据LF2407提供的里程计信息进行推算。 在控制中，每个固定时间段(50ms)内，LF2407计算左右前轮角位移平均值(芼)作为车体位移，即：             (3) 车体平面移动时，里程计信息的计算公式为：        (4) 其中，芼为该时段车体的位移；芶、芺分别为左右前轮在一个检测周期内转过的弧度；r为车轮半径；q为光纤陀螺提供的航向角度；f为倾角传感器提供的俯仰角度。 程序设计 本系统控制程序主要有初始化子程序、通信中断子程序、主程序以及脉冲输出中断子程序。主程序调用系统初始化子程序，进行通信接口、I/O、QEP、定时器等部分及必要中间寄存器的初始化，设置中断标志，读取定时器4的计数值。当发生中断时，调用相应中断程序，并修改或重置标志位。 通信中断子程序包括接收中断和发送中断，完成数据的接收和发送。由SCI的中断向量寄存器PIVR的值判断是接收中断还是发送中断，并调用相应服务子程序。接收数据时，要进行奇偶校验，保证数据准确并回发信号，同时对给定值进行积分平滑，然后分别确定三路输出电路的分频值和转向标志，计算各定时器周期寄存器的值，结果放入中间寄存器，并把计数器清零。发送中断时，系统读取蓄电池电压、电机电流、转速和方向等寄存器的值，同时相应寄存器清零，并计算里程计信息和后轮转速和方向，将各结果发送到上位机。 当某定时器中计数器与周期寄存器匹配时，即发生输出中断，相应标志位置位。脉冲输出中断子程序根据标志位读取相应中间寄存器的脉冲值和方向标志，并输出到I/O口，随即计数器清零，重新开始计数，同时标志位清零。为增强程序容错性能，系统使能看门狗定时器。另外，为防止干扰影响控制性能，需增加返回中断服务子程序，此程序只有一条“return”指令，当发生干扰中断时，系统直接返回主程序。 结语 本文简要介绍了移动机器人运动控制器主要部分的实现方案，针对移动机器人的特点和应用背景，以TMS320LF2407作为核心，对六轮摇臂式移动机器人进行运动控制。经过初步实验，机器人运行平稳，能够以较低的成本满足移动机器人运动控制要求。■ 参考文献 1 王文学,孙萍,徐心和. 足球机器人系统结构与关键技术研究[J].控制与决策. 2001,16(2) 2 TMS320F/C24x DSP Controllers Reference Guide [M]. Texas Instruments. 1999. 3 TMS320LF2407 TMS320LF2406 TMS320LF2402 DSP Controllers [M], Revised. Texas Instruments. 2001 4 刘和平 等. TMS320LF24x DSP结构、原理及应用[M]. 北京：北京航空航天大学出版社，2003