基于卡尔曼滤波的两轮自平衡遥控小车设计

路成强，曾 洁，李千振

(大连交通大学电气信息学院，辽宁 大连 116028)

基于卡尔曼滤波的两轮自平衡遥控小车设计

路成强，曾 洁，李千振

(大连交通大学电气信息学院，辽宁 大连 116028)

针对现有两轮自平衡遥控小车系统稳定性不佳、实现功能单一、车体摆动幅度过大等不足，提出了两轮自平衡遥控小车改进方案。采用STM32F103单片机作为主控器，移植实时操作系统μC/OS-II，选择直流电机、传感器以及外围电路，设计了两轮自平衡遥控小车的硬件控制部分。利用MPU6050模块实时获取小车的当前运行姿态，经卡尔曼滤波处理位姿信息后发送给主控器，随后利用线性PID控制算法调节电机转速，结合双闭环控制实现了小车的直立平衡行走。摇杆电位器结合无线通信模块可远程遥控小车，利用超声波传感器和GSM模块实现避障和远程监测功能，通过TFT彩色液晶屏和蓝牙手机客户端实现人机交互功能。试验结果表明，小车系统运行平稳、功能多元、抗干扰能力强，具有很高的准确性和稳定性，实现了自平衡遥控理论的扩展，在机器人领域具有广阔的应用前景。

STM32F103; 卡尔曼滤波; 线性PID; 双闭环控制; 无线通信; 远程遥控

0 引言

随着科技的发展和产业结构转型战略的驱动，两轮小车系统的研究与应用迅猛发展。两轮小车在移动机器人基础上逐渐探索研究，其本质是自主移动机器人，主体结构类似于倒立摆系统[1]。移动机器人技术随着计算机、微电子技术的进步而快速发展，我国关于两轮小车的研究正处于飞速发展的初期阶段，开展此项研究对其构建数学模型，依赖自平衡理论实现控制，将对于提高我国在该领域的科研水平具有重要的理论和现实意义。

本文在对现有两轮小车技术总结分析的基础上，从电子器件选择、硬件结构设计和控制算法应用等方面进行改进，完成了本系统的总体方案。该小车在稳定性和多功能性等方面均有良好的表现。

1 系统总体平衡原理与运动分析

1.1 两轮自平衡小车平衡原理

两轮自平衡小车是一个高度不稳定系统。自平衡原理主要描述小车在运动情况下如何动态地调整姿态使其平衡。理想状态下，小车重力方向和地面对小车支持力方向相反，角度相差180°，φ角度为0°，此时系统受力均衡，小车保持直立平衡状态。但实际情况下，角度不会为0°，小车重力方向和地面对小车支持力方向总是不处于一条直线，合力也不为0。根据牛顿运动定律可知，小车会有前倾或后仰的趋势，导致角度越来越大。当小车呈现后仰的倾向状态，达到一定程度后，如果可以改变小车让它有后退的倾向状态，则使小车后退产生前倾的趋势去及时纠正原来小车前进过程中后仰的倾向状态；当小车呈现前倾的状态，同理即可。两种状态在行进中循环往复，则小车在竖直方向上就会实时保持动态直立平衡状态。

1.2 两轮自平衡小车遥控原理

摇杆电位器结合两个无线通信模块NRF24L01来控制小车在行进过程中一直保持平衡稳定运行状态。例如向前推动控制杆，无线通信模块发射方将模拟量的变化传输到接收方，再通过A/D转换模块转化为数字信号给主控器。主控器依据程序改变速度给定值，速度的反馈值和给定值会产生偏差，导致车体前倾。MPU6050模块实时检测到车体角度变化信息后传送到主控器。主控器通过电机驱动模块驱动电机向前转动产生相应的力矩来维持小车平衡状态，从而实现无线远程遥控功能。

1.3 两轮自平衡小车运动分析

小车在保持动态平衡状态下，完成静止、前进、后退、转弯等复杂动作。小车的两个车轮是维持小车直立行走的动力来源，分别由两个直流电机来驱动两个车轮转动。从控制角度分析，控制对象是两轮小车，控制输入量是两个直流电机的转动速度。两轮小车运动控制系统可以分为三个子系统[2]。

(1)小车平衡控制系统。

小车实时姿态信息作为输入量，保持小车处于动态直立平衡状态是首要任务，而小车平衡状态是通过系统负反馈实现控制两个直流电机正反向转动，来抵消倾斜的趋势。控制小车平衡状态稳定条件有：

① 利用MPU6050传感器精确测量小车倾角和倾角速度的大小。

② 通过控制电机转速实现对车轮的运动控制，以控制车轮的加速度为主要最终目的。

(2)小车速度控制系统。

通过车体倾角和车轮速度共同控制两个直流电机转动速度，在保持自平衡的基础上实现控制小车运动速度。车轮速度控制需要测量两个电机的转速，车体倾角的变化由MPU6050模块直接测得Y轴的角速度和角加速度。小车角度和速度控制系统框图如图1所示。

图1 角度和速度控制系统框图

图1中：K1和K2分别为角度控制中微分控制参数和比例控制参数；K3和K4分别为速度控制中比例控制参数和微分控制参数。

利用微分控制的目的是增加车模角度和速度的稳定性，防止控制超调。

(3)小车方向控制系统。

主控器通过电机驱动模块分别驱动两个直流电机控制两个车轮转动，使得左右车轮产生转动差速来实现小车方向的控制。实际过程中，由于小车负载较大，行进过程存在较大惯性，为防止小车方向控制中过冲现象发生，增加了微分控制环节来对电机差动控制量进行修正；依据小车差动控制方案，小车方向控制还需简单的比例控制；为防止出现积分饱和现象而导致电机的响应速度降低，则将积分控制环节舍弃[3]。

小车运动控制系统框图如图2所示。

图2 运动控制系统总框图

综上所述，小车平衡和方向控制系统是用PD控制算法控制电机的输出电压来控制电机的转速达到控制目的；而小车速度控制系统也是用PD控制算法，但是通过调节小车的倾角大小实现控制目的。

2 系统硬件电路设计方案

2.1 硬件控制系统

两轮自平衡遥控小车的硬件系统包括：主控器、电源模块、显示模块、数据无线通信模块、MPU6050模块、A/D转换模块、全球移动通信系统(global system for mobile communication,GSM)模块、电机驱动模块等。STM32F103RCT6作为系统的主控器，采用超声波传感器和GSM模块实现避障和远程监测功能，被测得的数据利用薄膜晶体管(thin film transistor,TFT)液晶屏实时显示，通过蓝牙模块实现数据无线传输功能。利用GSM模块实现数据短信发送功能，摇杆电位器结合无线通信模块NRF24L01远程遥控小车，最终实现两轮自平衡遥控小车系统整体功能。小车硬件控制系统如图3所示。

图3 硬件控制系统框图

2.2 主控器电路

选择STM32F103RCT6作为主控器，这是一款由意法半导体公司推出的基于ARM Cortex-M3内核的新型32位嵌入式微处理器，最高工作频率可达72 MHz，可以产生多路PWM控制电机。其具有超低功耗、极低成本、高性能、稳定性极强等优点[4]，提高了对工作模式选择的快速回应及对控制系统进行模块化设计，实时完成多项复杂数据的处理。

2.3 数据无线通信电路

文献[15] 为准确估计内蒙河套灌区灌溉水入渗补给地下水量，采用试验研究与数值模拟相结合的方法，分别根据灌水前后地下水位变化和土壤含水率变化计算了灌溉水入渗补给地下水系数，得到作物生育期(最大灌水量183.72 mm)灌溉补给地下水系数为15%，秋浇灌溉(灌水量491.00 mm)补给地下水系数为30%。河套灌区地下水位埋深相对较浅，通过灌水前后的土壤含水率变化情况和数值模拟结果显示：灌水2～4 d补给地下水量达到最大，8～10 d后即完成对地下水的入渗补给，不同灌水量灌溉水入渗规律基本一致，入渗补给量和入渗时间与灌溉水量直接相关。

无线通信模块可实现两轮小车与控制平台系统的实时数据传输，实现对两轮小车的实时监测和控制。第一种情况：无线通信模块采用NRF24L01，它采用通用的四线SPI接口与主控器通信[5]，结合摇杆电位器传输的模拟信号通过A/D转换模块转化为数字信号传递给主控器，然后主控器及时改变速度给定值来达到小车的远程遥控功能。第二种情况：无线通信模块采用HC-05蓝牙模块，通过设置AT指令使其配置为主模块，与手机蓝牙串口助手配对，进行数据无线传输到智能手机上，可将数据导出。

2.4 MPU6050模块电路

姿态采集模块采用MPU6050。该模块是一款整合性的六轴运动处理器。它整合三轴陀螺仪和三轴加速度器数据，以实时获取小车的当前运行姿态；然后通过卡尔曼滤波对测得的Y轴角速度和角加速度滤波和解耦，选定正确的状态变量，从而得到准确稳定的倾角和倾角速度；将位姿信息经外部设备电路传给主控器，避免了陀螺仪与加速度器组合时的轴间差的问题。

2.5 A/D转换电路

采用A/D转换器芯片HX711，它内部集成稳压电源、片内时钟振荡器等其他同类型芯片所需要的外围电路，具有集成度高、响应速度快及抗干扰性强等优点，独特之处是所有控制信号由管脚驱动，无需对芯片内部的寄存器编程操作。输入开关设定通道A，模拟差分输入直接与桥式传感器的差分输出相接，经过128倍增益放大后，差分输出电压传输给主控器。

2.6 GSM短信模块电路

采用型号YD-SIM900A的GSM短信模块，以短消息形式对指定手机进行数据传输。基于GSM模块的短消息功能的串行口，可以用来控制指定的手机快速、可靠地实现个人用户信息处理等功能。该短信系统由控制中心和显示屏组成，控制中心实时将最新接收的信息内容发送到显示屏，构成控制中心与显示屏端的通信链路[6]。主控器通过与GSM短信息有关的AT指令给指定手机发送被测数据信息，实现远程监测小车系统当前运行状态的功能。

2.7 超声波避障电路

(1)

式中：V为小车行驶速度；L为小车行驶距离。

进而主控器判断小车是否在安全距离范围之内，再进一步有效调节控制车轮的转向。

由于超声波是一种声波，其声速与温度有关，如温度每升高1 ℃，传播速度增加约0.6 m/s。综合考虑温度补偿，利用温度传感器DS18B20进行测量，以避免温度因素对测量的误差影响，实现了速度的精准测量。若现场温度为W时，超声波传播速度的计算公式[9]为：

V≈331.5+0.607W

(2)

得到超声波传播速度后，再精确计算超声波模块距离障碍物的距离。

3 系统软件设计方案

3.1 线性PID控制算法

采用线性PID控制算法，保证系统处于闭环控制状态，并准确达到设置的平衡状态。直流电机的速度闭环控制和小车的倾角闭环控制，共同构成双闭环控制系统。PID算法的输入是传感器获取的角度和角速度值的偏差值，输出是电机的PWM占空比值，从而控制电机转速大小，进而调整角度值的偏差，使得角度值趋近系统平衡零点[10]。PID控制器是一种线性控制器，且整个系统中存在误差计算，则采用增量式PID控制算法，其控制公式为：

(3)

式中：Kp为比例系数；TI为积分时间常数；TD为微分时间常数；u0为系统初始输出值；e(k)为k时刻偏差值；u(k)为k时刻PID控制器的输出，T为采样周期。

3.2 卡尔曼滤波融合算法

卡尔曼滤波的基本思想是：采用信号与噪声的状态空间模型，利用前一时刻的估计值和现时刻的观测值来更新对状态变量的估计，从而得出现时刻的估计值[11]。卡尔曼滤波原理如图4所示。

图4 卡尔曼滤波原理图

选用MPU6050作为两轮小车姿态采集传感器，由于传感器采用数字I2C接口读取数据，读取的数据比A/D转换器采集的数据会更精准稳定。MPU6050传感器采集回来的数据具有较大的扰动，小车控制系统不应该直接采用捕获的数据，需经卡尔曼滤波算法进行数据融合处理，最终准确计算小车的倾角θ。已知倾角速度和倾角存在导数关系，θ可作为状态向量；加速度计估计陀螺仪的偏差δ，将δ作为另一状态向量[12]，则有：

(4)

式中：ωgyro为陀螺仪测量值；ωg为陀螺仪的测量噪声；θacce为加速度计测量值转化后的角度；ωa为加速度计的测量噪声；ωa和ωg为相互独立的零均值高斯白噪声。

4 结果分析

4.1 倾角测量与调试

当小车倾斜时，由受力分析可知，重力加速度g在Y轴方向有加速度分量，于是Y轴输出电压不为0，则电压变化量Δu为：

Δu=kgsinθ≈kgθ

(5)

由电压变化量Δu得出小车倾角θ的度数。但小车运动过程中产生的信号有很大干扰，还需要和陀螺仪测量的倾角速度进行积分得出角度后对比校正，最终得出稳定准确的倾角值。多次实际调试，角度控制中比例控制参数等于155，微分控制参数等于0.7。

4.2 速度测量与调试

将计数器安装在电机输出轴的光电编码盘上，在速度控制周期间隔100 ms内读取出光码盘脉冲信号的个数进行累加，从而计算出电机的转动速度。为了减小安装光码盘带来的误差，小车速度采用两个电机速度的平均值。速度控制中包括比例和微分控制参数，而比例和微分定义是指反馈速度而言，如果相对于车模的反馈位置(速度的积分量)，这两个参数可分别对应称作积分和比例控制参数。多次实际调试，速度控制中积分控制参数等于0.18，比例控制参数等于3.2。

5 结束语

本文详细阐述了两轮自平衡遥控小车控制系统的总体设计方案，分析了两轮小车自平衡原理、遥控原理和三个运动子系统，逐步完成了硬件电路和软件程序的设计。采用线性PID控制算法和卡尔曼滤波，实现了小车的远程遥控、监测和避障功能。两轮遥控小车可作为电子设计作品、试验研究模型、高档玩具等，也可适用于狭小空间探索、复杂地形侦查、危险物品运输等恶劣环境。基于控制自平衡理论设计的小车具有非常广阔的应用前景。

[1] 袁帅.两轮自平衡机器人系统的设计[D].北京:北京交通大学,2012.

[2] 綦声波,张玲.“飞思卡尔”杯智能车设计与实践[M].北京:北京航空航天大学出版社,2015.

[3] 杨建民,赵玫,杨洪勇,等.电磁导航智能寻迹小车方向控制算法的分析与应用[J].科学技术与工程,2016,16(33):71-78.

[4] 刘波文,孙岩.嵌入式实时操作系统uCOS-II经典实例—基于STM32处理器[M].2版.北京:北京航空航天大学出版社,2014.

[5] 王元哲,扈宏杰.磁导航智能车系统的设计与实现[J].自动化仪表,2011,32(11):5-8.

[6] 蔡红娟,翟晟,蔡苗.基于STM32的GSM智能家居控制系统设计[J].自动化技术与应用,2013,32(8):37-40.

[7] 朱志强,张潇宇,王林童,等.基于Arduino+LabVIEW的高精度超声波测距系统设计[J].单片机与嵌入式系统应用,2016,16(9):49-51.

[8] 栾禄祥.室内移动机器人机器视觉定位系统的设计[J].自动化仪表,2017,38(2):49-52.

[9] 胡延苏,高昂.超声波测距误差分析及校正研究[J].计算机测量与控制,2015,23(8):2820-2823.

[10]舒英利,寇磊,苑全德,等.双超声波数据融合的两轮机器人平衡行进策略研究[J].自动化仪表,2017,38(2):36-40.

[11]张团善,何颖.卡尔曼滤波在两轮自平衡代步车姿态检测中的应用[J].单片机与嵌入式系统应用,2014,14(5):33-35.

[12]师树恒,曹阳,师素娟.一种直立智能循迹小车的设计[J].自动化仪表,2014,35(2):66-68.

DesignofTwo-WheeledSelf-BalancingRemoteControlCarBasedonKalmanFilter

LU Chengqiang,ZENG Jie,LI Qianzhen

(School of Electrical and Information,Dalian Jiaotong University,Dalian 116028,China)

For the inadequacies of the existing two-wheeled self-balancing remote control car system, such as the poor stability, the single realization function and the large swing range of car body, an improved scheme of self-balancing remote control car is proposed.Firstly, using STM32F103 microcontroller as the main controller, transplanting the real-time operation system μC/OS-II, choosing DC motors, sensors and external circuits, and hardware control part of the two-wheeled self balancing car is designed.Secondly, MPU6050 module is used to obtain the current running posture of the car in real time, and kalman filter is given to deal with the posture information to transmit to the main controller.Then the linear PID controller is employed to adjust the motor speed.And combined with double close-loop controller, the balance of the car walking upright is achieved.Thirdly, the rocker potentiometer combined with wireless communication module can remote control car.Integrating ultrasonic sensors and GSM module, some functions can be realized, for example, obstacle avoiding and remote monitoring.Human-computer interaction function is realized through the TFT color LCD screen and bluetooth mobile phone client.Finally, the experiment results demonstrate that the car runs extremely smoothly and realizes the diversitied function, and the anti-interference ability is strong.It has the features of high accuracy and high stability, and the expansion of self-balancing remote control theory is realized.This possesses wide application prospects in robotics field.

STM32F103; Kalman filter; Linear PID; Double close-loop controller; Wireless communication; Remote control

TH-39; TP249

： A

10.16086/j.cnki.issn1000-0380.201709011

修改稿收到日期:2016-12-09

路成强(1993—)，男，在读硕士研究生，主要从事嵌入式系统开发与应用、智能机器人及控制技术方向的研究。 E-mail:2305999283@qq.com。 曾洁(通信作者)，男，硕士，教授，主要从事嵌入式系统开发与应用、智能交通与车联网技术方向的研究。 E-mail:zyz@djtu.edu.cn。