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

基于卡尔曼滤波和PID控制两轮自平衡车 　　【摘 要】针对两轮自平衡车的稳定和运动过程中的控制问题，我们在信号处理的过程中引入卡尔曼滤波对信号进行处理并且采用传统的PID控制，将控制过程分为三个部分，即站立、直线运动和转向。由于车体运动分为这三个部分，并且这三个部分必须几乎同时控制，所以采用分时控制每一部分的方法，该方法被成功应用于“飞思卡尔”智能车大赛，并且取得良好效果。 　　【关键词】倒立摆系统；自平衡车；卡尔曼滤波；PID控制 　　引言 　　倒立摆系统是控制系统的一个重要的分支和典型的应用。实际上它可以理解成在计算机的控制下，通过对系统各种状态参数的实时分析，使系统在水平方向或垂直方向上的位移和角度（角速度）的偏移量控制在允许的范围以内，从而使系统保持平衡。自平衡车就是以倒立摆系统为工作原理的成品，两轮自平衡智能小车直立行走是要求仿照两轮自平衡电动车的行进模式，让车模以两个后轮驱动进行直立行走。近年来，两轮自平衡电动车以其行走灵活、便利、节能等特点得到了很大的发展。国内外有很多这方面的研究，也有相应的产品。相对于传统的四轮行走的车模竞赛模式，车模直立行走在硬件设计、控制软件开发以及现场调试等方面提出了更高的要求。 　　实物图如下： 　　一、系统构成 　　整个模型车分为两个部分组成，即硬件电路和软件两部分。硬件电路主要由加速度计、陀螺仪、微控制器、编码器、线性CCD、电机驱动电路组成。由微处理器对陀螺仪、滤波电路和加速度计构成的传感器组进行高速A/D采样后，通过卡尔曼滤波器对传感器数据进行补偿和信息融合，得到准确的姿态角度信号，此角度信号再通过PID控制器运算，输出给电子调速器转换成PWM 信号，进而对电机进行控制。系统结构框图如下图所示： 　　二、卡尔曼滤波 　　加速度计用于测量物体的线性加速度，加速度计的输出值与倾角呈非线性关系，随着倾角的增加而表现为正弦函数变化。因此对加速度计的输出进行反正弦函数处理，才能得到其倾角值。测量数据噪声与带宽的平方根成正比，即噪声会随带宽的增加而增加。 　　式中： 为传感器带宽（单位为Hz）。因此在设计卡尔曼滤波器时，首先要确定被测加速度的频率范围，然后再设计滤波器的参数，尽量使滤波器的带宽略高于被测频率，这样不仅有助于滤除高频干扰，而且也有利于降低系统噪声干扰。但是如果要得到精确的倾角值，带宽就需要设置得比较小，而这时加速度计动态响应慢，不适合跟踪动态角度运动，如果期望快速的响应，又会引入较大的噪声。再加上其测量范围的限制，使得单独应用加速度计检测小车倾角并不合适，需要与其他传感器共同使用。 　　陀螺仪的作用是用来测量角速度信号，通过对角速度积分，便能得到角度值。陀螺仪本身极易受噪声干扰，微机械陀螺不能承受较大的震动，同时由于温度变化、不稳定力矩等因素，陀螺仪会产生漂移误差，并会随着时间的推移而累加变大，通过积分会使得误差变得很大。因此，也不能单独使用陀螺仪作为本系统的倾角传感器。 　　（一）卡尔曼滤波过程 　　建立系统的状态方程与测量方程。 　　其中，为车体倾斜的真是角度，为陀螺仪的常值温漂，为包含固定偏差的陀螺仪输出角速度，为陀螺仪测量噪声，为加速度计经处理后得到的角度值，为加速度计的测量噪声。 　　令系统的采样周期为，得到离散系统的状态方程： 　　在进行卡尔曼递归运算过程中，需要知道系统过程噪声协方差阵 以及测量误差的协方差矩阵，对卡尔曼滤波器进行校正。 　　与矩阵的形式如下： 　　式中， 和分别是加速度计和陀螺仪测量的协方差，其数值代表卡尔曼滤波器对其传感器数据的信任程度，值越小，表明信任程度越高.在该系统中陀螺仪的值更为接近准确值，因此取的值小于的值。 　　卡尔曼递归运算的相关算式： 　　为时刻的最优估计值，为对应的协方差。为根据时刻的最优估计得到的 时刻的预测值。为卡尔曼增益。为时刻测量的角度值。这一次得到的最优估计值，作为下一次预测值得参考，这样周而复始反复循环计算这些算式，可以使收敛到最小，这样就是最优值。 　　（二）滤波结果 　　在进行卡尔曼滤波过程中，需要根据实际情况调试与，可以使滤波后的波形平滑，毛刺少，并且与实际波形的相位差小。 　　在小车控制中 　　卡尔曼滤波前后的波形如下（绿色为滤波前，红色为滤波后）： 　　三、PID控制 　　在卡尔曼滤波后和信号处理好后，小车的重点就在控制上面。小车的运动可以分为三个部分，即为站立、直线运动和转向。由于车体运动分为这三个部分，并且这三个部分必须几乎同时控制，所以采用分时控制每一部分的方法，类似CPU时间分片。 　　程序的控制和信号处理都是在中端里面执行，通过设置全局变量来确定执行什么操作。程序设置1毫秒中断，程序进入中断后，全局变量加1，判断全局变量的值，进入不同的执行部分，执行相应的操作。 　　中断服务程序的框架：