基于卡尔曼滤波和PID控制的两轮自平衡车

张圣军

【摘 要】针对两轮自平衡车的稳定和运动过程中的控制问题，我们在信号处理的过程中引入卡尔曼滤波对信号进行处理并且采用传统的PID控制，将控制过程分为三个部分，即站立、直线运动和转向。由于车体运动分为这三个部分，并且这三个部分必须几乎同时控制，所以采用分时控制每一部分的方法，该方法被成功应用于“飞思卡尔”智能车大赛，并且取得良好效果。

【关键词】倒立摆系统；自平衡车；卡尔曼滤波；PID控制

引言

倒立摆系统是控制系统的一个重要的分支和典型的应用。实际上它可以理解成在计算机的控制下，通过对系统各种状态参数的实时分析，使系统在水平方向或垂直方向上的位移和角度（角速度）的偏移量控制在允许的范围以内，从而使系统保持平衡。自平衡车就是以倒立摆系统为工作原理的成品，两轮自平衡智能小车直立行走是要求仿照两轮自平衡电动车的行进模式，让车模以两个后轮驱动进行直立行走。近年来，两轮自平衡电动车以其行走灵活、便利、节能等特点得到了很大的发展。国内外有很多这方面的研究，也有相应的产品。相对于传统的四轮行走的车模竞赛模式，车模直立行走在硬件设计、控制软件开发以及现场调试等方面提出了更高的要求。

实物图如下：

一、系统构成

整个模型车分为两个部分组成，即硬件电路和软件两部分。硬件电路主要由加速度计、陀螺仪、微控制器、编码器、线性CCD、电机驱动电路组成。由微处理器对陀螺仪、滤波电路和加速度计构成的传感器组进行高速A/D采样后，通过卡尔曼滤波器对传感器数据进行补偿和信息融合，得到准确的姿态角度信号，此角度信号再通过PID控制器运算，输出给电子调速器转换成PWM 信号，进而对电机进行控制。系统结构框图如下图所示：

二、卡尔曼滤波

加速度计用于测量物体的线性加速度，加速度计的输出值与倾角呈非线性关系，随着倾角的增加而表现为正弦函数变化。因此对加速度计的输出进行反正弦函数处理，才能得到其倾角值。测量数据噪声与带宽的平方根成正比，即噪声会随带宽的增加而增加。

式中： 为传感器带宽（单位为Hz）。因此在设计卡尔曼滤波器时，首先要确定被测加速度的频率范围，然后再设计滤波器的参数，尽量使滤波器的带宽略高于被测频率，这样不仅有助于滤除高频干扰，而且也有利于降低系统噪声干扰。但是如果要得到精确的倾角值，带宽就需要设置得比较小，而这时加速度计动态响应慢，不适合跟踪动态角度运动，如果期望快速的响应，又会引入较大的噪声。再加上其测量范围的限制，使得单独应用加速度计检测小车倾角并不合适，需要与其他传感器共同使用。

陀螺仪的作用是用来测量角速度信号，通过对角速度积分，便能得到角度值。陀螺仪本身极易受噪声干扰，微机械陀螺不能承受较大的震动，同时由于温度变化、不稳定力矩等因素，陀螺仪会产生漂移误差，并会随着时间的推移而累加变大，通过积分会使得误差变得很大。因此，也不能单独使用陀螺仪作为本系统的倾角传感器。

（一）卡尔曼滤波过程

建立系统的状态方程与测量方程。

其中，为车体倾斜的真是角度，为陀螺仪的常值温漂，为包含固定偏差的陀螺仪输出角速度，为陀螺仪测量噪声，为加速度计经处理后得到的角度值，为加速度计的测量噪声。

令系统的采样周期为，得到离散系统的状态方程：

在进行卡尔曼递归运算过程中，需要知道系统过程噪声协方差阵 以及测量误差的协方差矩阵，对卡尔曼滤波器进行校正。

与矩阵的形式如下：

式中， 和分别是加速度计和陀螺仪测量的协方差，其数值代表卡尔曼滤波器对其传感器数据的信任程度，值越小，表明信任程度越高.在该系统中陀螺仪的值更为接近准确值，因此取的值小于的值。

卡尔曼递归运算的相关算式：

为时刻的最优估计值，为对应的协方差。为根据时刻的最优估计得到的 时刻的预测值。为卡尔曼增益。为时刻测量的角度值。这一次得到的最优估计值，作为下一次预测值得参考，这样周而复始反复循环计算这些算式，可以使收敛到最小，这样就是最优值。

（二）滤波结果

在进行卡尔曼滤波过程中，需要根据实际情况调试与，可以使滤波后的波形平滑，毛刺少，并且与实际波形的相位差小。

在小车控制中

卡尔曼滤波前后的波形如下（绿色为滤波前，红色为滤波后）：

三、PID控制

在卡尔曼滤波后和信号处理好后，小车的重点就在控制上面。小车的运动可以分为三个部分，即为站立、直线运动和转向。由于车体运动分为这三个部分，并且这三个部分必须几乎同时控制，所以采用分时控制每一部分的方法，类似CPU时间分片。

程序的控制和信号处理都是在中端里面执行，通过设置全局变量来确定执行什么操作。程序设置1毫秒中断，程序进入中断后，全局变量加1，判断全局变量的值，进入不同的执行部分，执行相应的操作。

中断服务程序的框架：

其中，除了上述基本框架外，在每次进入中断后，首先执行卡尔曼滤波程序，并且执行单片机AD采样。根据我们实际获得的经验，卡尔曼滤波是比较耗时间的，并且AD采样同样也是很消耗时间，如果在中断执行的总时间超过1毫秒，这将会影响卡尔曼滤波的效果，因此在这个方面，必须考虑AD所开的通道数目，当通道数目过多，将会导致一次中断服务程序执行的时间超过1毫秒。这时可以选择增加中断定时时间，进而也就需要改变卡尔曼的 。

在控制中，我们采用传统PID控制。我们对直立控制采用PD控制，这主要原因是在直立控制中的主要要求就是快速性，因而舍弃积分单元。对于速度控制采用PID控制，积分单元消除误差。对于方向控制采用PD控制。

在直立控制过程中，P越大，车体抗干扰能力越强，但过大会导致车体振荡，这时可以通过加大微分参数消除振荡。当P达到一定程度时，车体会抖动并不是振荡，这时调节D并不能消除抖动。车体抖动的主要原因来源于卡尔曼滤波后的信号精度，当精度比较低时，P过大，会直接放大信号的幅度，进而表现为车体的抖动。

在做车的速度控制过程中，速度控制的周期会影响车体的运行的稳定性。在我们实际测试中当速度控制周期定为100毫秒时，低速运行车体比较稳定。但当车速度给定为1.3m/s时，车体在转弯后，恢复平稳比较困难。设置速度控制周期为200毫秒后，效果改善明显。

在方向控制中，我们使用分段PID控制。这主要原因是车体在直道运行时，稍微偏离中心线对车体运行没有影响，也就是在知道上方向控制不需要太灵敏，太灵敏不利于车在直道的稳定和进入弯道的稳定。相反，在弯道就需要方向控制的灵敏，灵敏度越高越好，但前提是保证车体的直立基本稳定。

在实际中，车体陀螺仪传感器的安装水平程度对车体运行是否稳定影响很大，安装越水平越好，越利于车的提速。安装不水平会出现车载转弯的过程中加速和减速的现象。

参考文献：

[1] 百度百科.自平衡电动车[DB/OL]. http：//baike.baidu.com/view/2362793.htm.2013-8-26/2013-8-26.

[2] 冯志勇，曾翰，张力，赵艺欣，黄伟.基于陀螺仪及加速度计信号融合的姿态角度测量.[J]西南师范大学学报（自然科学版），2011（8）：137-141.

[3] 何友，王国宏，彭应宁编著.多传感器信息融合及应用[M].2版.北京：电子工业出版社，2007：12-23.