具有惯性传感装置的四轮全向驱动小车设计

赖海清， 张 祺， 林凯伟， 林耀灿， 吴海生

（广东工业大学 自动化学院，广州510006）

近年来四轮全向驱动小车在机器人领域的应用很广泛，其优点是可以向360°任一方向运动。但一般控制方式没有考虑小车自身的惯性，且4个电动机的协调控制比较困难［1］，导致小车运动性能较差，特别是高速运动时轨迹跟踪较为困难。因此，本文设计一种具有惯性传感装置的四轮全向驱动小车，以提高小车高速运动时轨迹跟踪的准确性［2］。

小车主要由运动控制系统惯性传感装置组成。运动控制系统通过发出命令控制4个轮子的转速，并同时采集来自惯性传感装置的数据进行反馈控制。

1 全向驱动小车机械结构

小车的总体结构从上往下分为2层：下层如图1所示，为小车车架，上层为电路板。4组全向驱动轮各成90°分布在安装在下层底板上，四轮驱动采用Faulbaher公司的空心杯减速伺服有刷直流电动机。该电动机自带编码器和减速器，输出功率为70W。

图1 全向驱动小车机械结构图

2 系统硬件设计

整个系统硬件结构如图2所示，包括小车驱动控制、系统电源、过流保护装置和惯性传感装置。

2.1 驱动控制单元

图2 双DSP协调控制原理图

小车驱动控制处理器选用TI公司的数字信号处理器（Digital Signal Processon，DSP）TMS320F2812［3］，它是基于TMS320C2XX内核的定点DSP。器件上集成了多种先进的外设，为电动机及其他控制领域应用的实现提供了良好的平台，使控制系统的电路设计大为简化［4］。同时采用双DSP控制，一个DSP芯片为主处理器，另外一个DSP芯片为从处理器，主从DSP分别处理2路的电动机码盘信号和输出电动机驱动信号，克服了单DSP要处理4个电动机信号及其他外围事件负担重、实时性难以保证的缺点。

每片TMS320F2812有12路PWM输出，同时对2个伺服电动机进行控制［5］。采用如图3所示的H桥可逆PWM驱动系统，选用半桥驱动器IR2101（图3中U1和U2）作为H桥上下臂驱动功率管。其中IR2101是专为驱动半桥逆变器中同桥臂的2个MOSFET或IGBT而设计的高压、高速驱动器［6］。

图3 电机驱动电路图

2.2 系统电源

小车采用24V的锂电池进行供电，由于电动机的驱动电路工作在开关状态会产生高频噪声，为保证电路的正常工作，在电路中采用了电容及共轭线圈进行滤波对这些噪声进行了处理。同时，在控制电路和驱动电路之间通过高速光耦进行隔离，从而减少了驱动电路对控制电路的干扰［7］。

2.3 过流保护装置

在系统的保护方面，为防止电动机出现堵转时产生的过电流损坏器件，加入了过流保护装置。在电动机两端串入采样电阻（图3中R137，R139），在采样电阻两端接入一个差分电路，当流过采样电阻的电流超过预设值时，使电压比较器输出电流信号，使继电器动作，切掉电源供应。

2.4 惯性传感装置

惯性传感装置由MENS加速度传感器模块、电子罗盘模块、陀螺仪模块和液晶显示模块组成［8］，如图4所示。陀螺仪随着积分误差的累积会产生漂移，只能工作在相对较短的时间尺度内。加速度计虽然不会产生累积误差［9］，在一定程度上能纠正陀螺仪的累积误差，但当系统在三维的空间做变速运动时，因它不能区分重力加速度和外力加速度而会导致输出不准确。本设计采用电子罗盘航向角清除陀螺仪航向角的累计误差。电子罗盘的航向角为绝对值输出，与陀螺仪融合后能得到更稳定的航向角。整合加速度传感器、电子罗盘和陀螺仪，并均衡的利用三者各自的优点，实现一个所谓的惯性传感单元［10］。

图4 惯性传感装置结构框图

加速度传感器是采用ADI公司的三轴数字加速度传感器ADXL345，其最大可感知16g的加速度，感应精度可达3.9mg／LSB，倾角测量典型误差小于1°，可通过I2C数字接口访问。因此，采用加速度模块ADXL345便能测量到较精确的加速度值和倾角。电子罗盘则采用霍尼韦尔公司的HMC5883L模块，该模块自带有自动消磁和自动校准库功能，能很好地校正因外界磁场引起的偏差。此外，小车有时是在三维空间中运动，故选用三轴陀螺仪ITG－3200模块来对X，Y，Z3轴进行测量。而所选传感器内部都有温度传感器，故都不需要另外对温度进行补偿。由于这些传感器都带有I2C接口，故可以分别通过I2C总线与TMS320F28335的I2C接口进行通讯［11］。TMS320 F28335通过I2C总线驱动各个传感器工作，并采集各个传感器的数据，同时对采集回来的数据进行处理和发送。发送数据则是通过TMS320F28335上的CAN总线接口与小车主控制器TMS320F2812上的CAN总线接口通讯以及通过异步串口与PC机进行通信。

3 四轮全向驱动小车的软件设计

图5为具有惯性传感装置的四轮全向驱动小车的程序流程图。

图5 程序流程图

四轮全向小车工作空间为1平面，建立固连在这个平面的绝对坐标系xw－yw，以及固连于车体并且原点与机器人中心重合的局部坐标系xm－ym。如图1所示，其中θ为xm和xw的夹角，δ为轮子与ym的夹角［12］。

小车的运动方程为

式中，L为四轮全向小车中心到轮子中心的距离；vi为轮子i提供的驱动方向的速度。根据结构代入具体的数据，进一步可得全向小车的运动模型：

所以小车开始时，根据四个轮子的转速运动模型（式（2））规划路径，并设定小车4个轮子的速度v1，v2，v3，v4进行行驶；同时启用惯性传感装置对小车的加速度、位置和目标进行采集，根据惯性回馈计算出速度变化量 Δv1，Δv2，Δv3，Δv4，并对数据进行处理，把处理好的数据发送给小车主控制器，小车主控制器根据惯性参数改变4个轮子的转速（设小车的4个轮子的电动机分别为1，2，3，4，要改变的转速分别为v1＋Δv1，v2＋Δv2，v3＋v3，v4＋Δv4）来达到期望轨迹。而轮子的转速是靠主控制器根据v1＋Δv1，v2＋Δv2，v3＋Δv3，v4＋Δv4来控制各电机的比例积分微分控制器 （Proportinal Band－Integral－Derivative Controller，PID），通过控制PID来改变脉宽调制器（Pulse－Width Modulation，PWM）输出，从而改变4个轮子的转速。惯性参数每更新一次都要重新对路径进行规划调整，并不断地重复上述操作，从而达到小车在高速运动时轨迹跟踪的准确性和快速性。

软件设计主要包括：惯性传感装置里的数据采集程序、数据处理程序、数据通信程序和数据显示程序以及主控制器里的电机协调控制程序［13］。电机协调控制算法采用的是增量式数字PID控制算法。

4 增量式数字PID控制算法

数字PID控制算法表达式为

式中k为采样序号，k＝0，1，2，…；uk为第k次采样时刻的PID控制器输出值；ek为第k－1次采样时刻输入的偏差值；kp为比例系数；kD为微分系数；u0为进行PID控制器的初值。

增量式数字PID控制算法是控制器的输出仅为控制量的增量Δvk。增量式数字PID控制算法可由式（3）推导出。由式（3）可得到PID控制器在第k－1个采样时刻的输出值为

将式（3）与式（4）相减并整理得到增量式数字PID控制算法公式为

5 结 语

本文研究了基于四轮全向驱动小车的双微处理器控制系统，实现了以TMS320F2812为主处理器的双DSP协调控制4个电机；同时加入了加速度计、电子罗盘、陀螺仪这些惯性传感装置来实现小车自身的定位和小车的运动状态跟踪，有效地提高了小车高速运动的性能和小车高速运动轨迹跟踪的准确性。该设计功耗低、体积小、实时性好，在全自主机器人、工业生产及其他相关领域具有一定的参考价值。

［1］孙文焕，程善美，王晓翔，等.多电机协调控制的发展［J］电气传动，1999（6）：3－6.

［2］冯延蓬，仵 博，蔡学军.一种基于多智能体系统的分布式规划系统［J］.深圳职业技术学院学报，2009，8（5）：18－21.

［3］徐勇明，张培仁.基于F2812的足球机器人底层控制［J］.Automation and Instrumentation，2004（6）：10－14.

［4］李 磊，叶 涛，谭 民，等.移动机器人技术研究现状与未来［J］.机器人，2002，24（5）：475－480.

［5］Lowery T F，Petro D W.Application consideration for PWM inerter－fed low－voltage induction motors［J］.Industry Applications，IEEE Trans，1994，30（2）：1532－1538.

［6］张奇志.IR2101带有欠压封锁的半桥MOSFET驱动广州：［J］.集成电路应用，2002（5）：64－65，58.

［7］付根平.全自主足球机器人运动控制系统的研制［D］.广州：广东工业大学，2009：64－65，58.

［8］秦 勇，臧希喆，王晓宇，等.基于MENS惯性传感器的机器人姿态检测系统的研究［J］.传感技术学报2007，20（2）：298－301.

［9］刘晶红，朱志强，沈宏海，等.加速度在控制系统扰动补偿中的应用［J］.光学精密工程，2009，17（9）：2191－2197.

［10］Pasolini F.MEMS加速度计、陀螺仪和地磁感应计推动创新型消费电子应用发展［J］.中国集成电路（CIC），2010（139）：72－75.

［11］Texas Instrument Inc.TMS320F28335，TMS320F283 34，TMS320F28332，TMS320F28235，TMS32F28234，TMS28232Digital Signal Controller（DSCs）Data Manual［EB／OL］.（2009－04－）［2012－06］.http：／／focus.ti.com.cn／cn／docs／prod／folers／print／tms320f28335.html.

［12］Arai T，Pagello E，Parker L E.Advance in multirobot system［J］.IEEE Transaction on Robotics and Automation，2002，18（5）：655－661.

［13］刘陵顺，高艳丽，张树团，等.TMS320F28335DSP原理及开发编程［M］.北京：北京航空航天大学出版社，2011：324－349，208－418.