基于航迹推算的移动式机器人定位系统设计

    自工业革命以来机械被广泛运用到各个领域。随着人类社会生产的发展，人们越来越需要高智能、高精度的机器完成各类繁重的、高精度、高风险的作业，于是，完成某些特定任务的机器——机器人逐步发展起来。移动式机器人需完成某些任务，其关键在于定位，即机器人必须知道自己现在的位置和将要“行走”的方位。根据机器人运用的环境不同，定位精度也大不相同，现有的定位技术大致可分为航迹推算、信号灯定位、基于地图的定位、路标定位以及视觉定位等几大类。
    本文阐述的是一种运用多种传感器的航迹推算定位技术。该种定位方式适用于已知航线的行走，能够准确的从出发地快速行进到目的地，具有良好的嵌套性，可以成为某些大型定位系统的基本单元，同时具有抗干扰能力强、维护方便的特点。

1 系统总体设计
1．1 系统的总体结构
    系统采用航迹推算原理对移动式机器人进行定位，航迹推算即利用外部传感器的数据实现机器人实时位置和运动方向的估计，短期定位精度高。航迹推算技术的关键是需测量出机器人单位时间运动的距离以及这段时间内机器人航向的变化。在本设计中，主要采用编码盘和陀螺仪进行航迹推算来实现定位，系统采用模块化设计，由驱动模块、传感器和反馈模块以及控制模块等组成，原理框图如图1所示。

    为提高抗干扰能力，系统各模块之间相互独立使用ISP串行通讯进行数据通信，每个模块都与控制系统构成一个闭环单元，控制系统通过算法整合，从而降低了各个单元之间的干扰和误差的累积。控制系统发出路线的整体行走方案，再由各个闭环单元检测是否按原定计划实施，当出现错误和误差时，系统通过采集的数据即刻做出实时调整，实现快速、准确定位。
1．2 系统的数学模型
    系统的数学模型可以看成是一个平面坐标定位，在平面内任何一个点的速度可以表示为线速度V和角速度V，其坐标为(x，y)。当一个物体在平面内从一点移动到另一点时，只要画出运动轨迹，就可以计算出在该轨迹上任一点的速度和坐标，以进行定位。假设某一运动轨迹方程已知，则需要按其轨迹进行运动定位，其数学模型如图2所示。

   
    式中，Vx(t)和Vy(t)分别表示x轴和y轴在时间t时的速度，S表示t到t+1时刻的行走路径，φ(t)表示t时刻x轴速度与速度的夹角。

2 系统电路设计
2．1 伺服器电路
    伺服器是一种电机驱动器，与普通电机驱动器不同，该驱动器可以与电机连接形成闭环控制单元。伺服器通过实时检测安装在电机上的高分辨率光电编码器的信号计算出电机转速和转角，通过与目标转速和转角进行比较来实时调整电机运动状态。

    如图3所示为伺服器系统硬件电路的信号处理单元。该电路包括了信号整合和信号运算，其中DS26LS32CM实现编码盘信号的整合，即把4路PWM信号整合成2路PWM信号。由于编码盘信号是由A、B两路相位差为90°的PWM组成，通过对DS26LS32CM逻辑使能口的电平控制，可实现输出端在A、B信号超前或滞后90°时输出高电平或低电平，以此判断出电机正反转状态。LM629为PID信号算法芯片，通过对编码盘信号运算和DS26LS32CM输出信号状态分析，芯片可发出信号使电机一直保持在编码盘的某一个栅格处，让电机急停锁死或准确运动所需要的栅格数，实现对机器人移动的准确控制。HCP-2602为高速光耦芯片，其目的是减少控制端与电机之间的信号干扰。

2．2 光电编码器电路
    光电编码器是一种通过光电转换将输出轴上的机械几何位移量转换成脉冲或数字量的传感器，由光栅盘和光电检测装置组成。光栅盘是在一定直径的圆板上等分的开通若干个长方形孔，开孔的数量决定了控制的精度。机器人运动时，光栅盘随电机旋转，经发光二极管等电子元件组成的检测装置检测并输出若干脉冲信号，通过采集脉冲个数后运算即可得出机器人运动路径长度。

    如图4所示为光电编码器的外围采样电路，主要是通过一个与门和一个D触发器来实现对脉冲信号的采集。一般的光电编码器是由A、B、Z三路信号组成，A和B是两个前后相位差为90°的脉冲信号，根据光栅盘的旋转方向，相位差的超前与滞后各不同，因此利用与门将A、B信号整合成一个脉冲信号再利用D触发器判断A、B两者相位差关系。当光栅盘正转时，A超前B 90°，D2发光，同时在DirB处检测到高电平。当光栅盘反转时，A滞后B90°，D1发光，且在DirA处检测到低电平，再通过单片机的处理就能准确判断光栅盘所转过的栅格数，从而计算出机器人运动路径的长度。
   
    式中，P1表示正转PWM脉冲个数，P2表示反转PWM脉冲个数，n表示编码盘栅格数，r表示机器人动力轮半径。
2．3 陀螺仪硬件电路
    设计中采用三轴陀螺仪对加速度进行测量，积分后即可得到所需的角度偏移量，如图5所示为陀螺仪硬件原理图。其工作原理为中央处理单元发送通信数据给陀螺仪使之工作，陀螺仪在机器人发生角度偏移时测量实际偏移量和理论偏移量的差值，将该值传送给中央处理单元，由中央处理单元计算出补偿值后发指令给伺服器，使电机进行误差补偿。

    图5中L3G4200D是一个三轴陀螺仪，可以对x、y、z三个方向的角度进行检测，检测后的数据通过ISP通信传送将端口SCL、SDA角度信号传输给中央处理单元。MMA8451Q是一个速度计，可以实时检测机器人的运行速度，并用相同方式将运行速度信号传输给中央处理单元。之所以在此安装速度计，是为了降低中央处理单元中CPU的计算量，让CPU有更多的空间去处理速度与路径的关系。TPS62112是低噪音的同步降压DC—DC转换器，其内部集成了N型和P型MOSFET，可进行同步整流，为陀螺仪芯片提供良好的电源环境。

2．4 中央处理单元
    系统使用MEGA128单片机作为主控芯片，该单片机具有2个8位定时器和2个16位定时器以及10路可编程PWM，拥有8个10位ADC通道，因此能够实现对多传感器的信号采集处理。

    如图6所示为中央处理单元硬件电路原理图，包括了24 V转5 V的直流稳压电源、晶振电路以及外围拓展口电路。在电源部分使用LM117稳压管，能够很好地控制电源纹波，使单片机稳定工作。在外围拓展口部分使用并行总线结构，能够很方便进行使用和开发。

    如表1所示为中央处理单元中单片机使用的主要端口功能，包括通信与控制信号的发出以及对编码盘的计数。

3 结果与分析
    为验证系统的可行性与稳定性，使用了一个四轮全向轮结构小车进行测试，测试方案如下：在2 m×2 m的平面场地中设定A、B两点，小车在两点间分别多次作直线运动与弧线运动，每次均将实际运行轨迹与理论轨迹进行比较，并分析定位误差。
    (1)直线运行小车直线运行10次，每次运行出发地点A均相同，检测实际运行轨迹与理论轨迹误差在5 mm以内，且小车可以快速定位到C点，而C点在B点误差圆内。
    (2)弧线运行小车弧线运行10次，每次运行出发地点A均相同，检测实际运行轨迹与理论轨迹最大误差为1 cm，且小车可以快速定位到C点，而C点在B点误差圆内。
    试验证明，系统可以完成对任意预设路径的快速定位，基本保证到达目的地，其误差在允许范围之内，且系统充分对CPU资源进行了合理运用，在外围硬件电路设计时考虑了最大程度减小外界对系统的干扰，稳定性较强，达到了预期设计目标。