摘要：介绍了一种采用数字信号控制器dsPIC33EP16GS502和运动传感器MPU-6050等设计的两轮自平衡小车的控制系统。该系统利用卡尔曼滤波算法对陀螺仪和加速度计的信号进行融合，计算出小车倾角和角速度的最优估计值，采用PID算法对驱动电机的两路PWM信号进行控制，从而实现两轮小车的自平衡控制。文中详细介绍了两轮小车自平衡控制系统硬件和软件的设计。
关键词：卡尔曼滤波；数据融合；自平衡；两轮；dsPIC33EP16GS502
两轮自平衡车系统是一种高度不稳定系统，具有传统的一阶倒立摆的特性，其平衡控制系统利用安装在车体上的陀螺仪和加速度传感器，来检测车体姿态的变化，再通过伺服控制系统驱动电机进行精确地调整，以实现系统的动态稳定[1,2]。本文利用数字信号控制器、加速度传感器等设计了一种两轮自平衡小车的控制系统。
1系统总体设计
两轮自平衡小车的控制系统主要由数字信号控制器dsPIC33EP16GS502、加速度传感器MPU-6050、蓝牙串口模块（型号：DX-BT04）、直流双电机驱动TB6612FNG等组成。系统框图如图1所示。加速度传感器MPU-6050用于车体倾角及小车加速度的测量，蓝牙串口通信模块用于实现手机对小车的控制，安装在两个车轮上的光电检测电路各输出AB两路信号，处理后可以得到小车的速度及方向信息，TB6612FNG用于驱动车轮动作，OLED模块用于数据和状态显示，所有的通信、测量及控制都在数字信号控制器dsPIC33EP16GS502控制下实现，下面介绍主要电路的设计。
2硬件电路设计
2、1控制电路
系统控制电路如图2所示。由数字信号控制器（DSC）dsPIC33EP16GS502实现，这是一种16位数字信号控制器，适合电机控制类应用，其工作时钟高达70MHz，内部有5个16位、输入时钟频率为960MHz的高速PWM模块（每个PWM模块可有两个输出），可以输出高分辨率、高速PWM信号[3]，本设计中用于平衡车车轮的精确控制。此外，该DSC具有I2C接口、SPI接口及UART接口，方便与MPU-6050、OLED显示模块及蓝牙串口模块接口的设计。控制模块的主要功能为：（1）读取MPU-6050内部寄存器，获取三轴加速度传感器和三轴陀螺仪数据，再利用卡尔曼滤波法对加速度传感器输出的数据和陀螺仪输出的数据进行解算，计算出角速度、角速度误差等；（2）与蓝牙串口模块通信，接收手机APP发来的命令，发送当前数据、状态到手机上显示；（3）控制OLED模块显示数据、状态；（4）读取光电传感器对安装在车轮上的光栅编码盘的检测信号，处理后得到小车的速度和运动方向；（5）输出30kHz、15位分辨率（实际范围为0~32000）的PWM信号给电机驱动芯片，从而实现对两个车轮的精确控制。
2、2电机驱动电路
电机驱动电路如图3所示。TB6612FNG是东芝公司推出的一种电机控制专用芯片，其集成度高、驱动能力强[4]，分别控制两个直流电机。设计中使用了DSC的6个I/O脚用于对TB6612的控制，其中4个I/O脚用于电机正转/反转/停止等功能控制，另外2个I/O脚输出两路30kHz的PWM信号，实现电机转速控制。
2、3测量及通信电路
车体倾角、加速度测量及通信电路如图4所示，DSC通过2个I/O脚与MPU-6050相连，以I2C接口的方式读取MPU-6050的测量结果[5]；为实现手机以蓝牙方式对小车进行控制，设计中使用了蓝牙串口模块，该模块与DSC的串行口相连，如图4所示，图中的D3用于指示蓝牙连接状态，按下S1键可以断开蓝牙连接。
3软件设计
系统程序采用模块化编程方式，每个模块实现一定的功能，各模块在主程序调用下协同运作实现小车的平衡控制。系统程序由MPU-6050读写、卡尔曼滤波、车速及运动方向检测、PID控制、蓝牙串口通信及数据显示等模块组成。系统主程序流程图如图5所示。MPU6050的姿态解算采用DMP（DMP是MPU6050内部的运动引擎，全称DigitalMotionProcessor，直接输出四元数）方式，在平衡车实际调试中发现卡尔曼滤波进行数据融合和通过DMP输出的四元数得到的角度最为稳定。平衡小车的卡尔曼滤波器设计关键要素是选取状态向量，平衡小车的车体倾斜角度和车体的倾斜角速度是倒数关系，所以状态向量的元素可以选择为车体的角速度(Q_gyro)、车体的倾角(Q_angle)等来估计陀螺仪的零位偏(Q_bias），卡尔曼滤波采样时间为dt=0005，观查噪声协方差矩阵(R_angle)[6,7]。卡尔曼滤波融合流程图如图6所示。程序设计中每5ms测量一次倾角及加速度的值，这样的测量频率可以取得较好的卡尔曼滤波和互补滤波的效果。系统启动后调用各个模块采集数据，速度环采用PI（比例-积分）控制、转向环P（比例）控制、直立（平衡）环采用PD（比例-微分）控制。数据处理后最终输出两路PWM控制左右两个电机的运动，从而实现小车的控制。在实际调试中，上述各环的参数需要实际测试得出，以直立环PD控制参数确定为例，其确定流程如图7所示。其余各环的控制参数的确定与直立环类似，不再赘述。
4系统调试
平衡小车的调试主要分为三个环节：车体直立调试、小车速度控制、小车转向控制，下面分别介绍。
4、1车体直立调试
平衡小车直立环使用PD（比例微分）控制器，直立控制调试，由此可以得出结论见表1、表2所示。从表1、表2可以看出直立环中应取：kp=500、kd=1�7，根据实践经验，对kp、kd都乘以06，然后取整即得：kp=300、kd=1。
4、2小车速度控制
在平衡小车的速度控制中，采用最常使用的PI（比例积分）控制器，PI控制器是一种线性控制器，是将偏差的比例（P）和积分（I）通过线性组合构成控制量，对被控对象进行控制。根据速度控制调试，由此可以得出结论见表3所示。根据表3，设计中选取kp=80、kd=0.4。
4、3小车转控制
平衡小车转向控制比较简单，设计中仅使用了比例（P）控制。具体而言，就是使用Z轴陀螺仪的数据作为转向速度偏差进行比例控制，目的是保持转向速度为设定值。这种方法算法简单、避免了编码器对车轮滑动无法检测现象、陀螺仪漂移等问题。转向环的作用是使小车行驶的过程中，跟随给定的Z轴角速度，例如若设定的Z轴目标角速度为零，那么小车应该走一个直线，根据转向控制调试，由此可以得出结论见表4所示。根据表4中的数据，设计中转向环控制中，取kp=1。综上所述，各控制环的选择适当的PID参数，然后再进行综合调试，可以使平衡小车能稳定的做自平衡运动。
5结束语
文中介绍了一种硬件上采用数字信号控制器dsPIC33EP16GS502、加速度传感器MPU-6050等，软件上主要采用卡尔曼数据融合算法、PID控制算法的两轮自平衡小车控制系统的设计。实际测试表明该小车在一般的水泥地面上自平衡控制范围约为±15°，施加一定的干扰，小车仍能保持平衡；可以完成通过手机以蓝牙方式发出的前进、倒退、转弯等指令，控制过程稳定、可靠。
作者：乙金林 陈雯雯 张亚炜 沙春芳 单位：盐城师范学院物理与电子工程学院
