电磁导引车控制算法研究与设计+文献综述(10)
加速度计的噪声信号虽然可以通过数据平滑滤波将其滤除，但是平滑滤波一方面会使得信号无法实时反映车模倾角变化，从而减缓系统对于车模车轮的控制，另一方面也会将车模角速度变化的信息滤掉。上述两方面的滤波效果将使车模无法保持平衡。因此对于车模直立控制所需要的倾角信息需要通过另外一种器件获得，那就是角速度传感器——陀螺仪。
ENC-03陀螺仪如图4.3所示。
图4.3 ENC-03陀螺仪
在车模上安装陀螺仪，可以测量车模倾斜角速度，将角速度信号进行积分便可以得到车模的倾角。由于陀螺仪输出的是车模的角速度，不会受到车体运动的影响，因此该信号中噪声很小。车模的角度又是通过角速度积分而得，这可进一步平滑信号，从而使得角度信号更加稳定。由于从陀螺仪测得的角速度获得角度信息，需要经过积分运算，如果角速度信号存在微小的偏差和漂移，经过积分运算之后，形成累积误差，这个误差会随着时间推移逐步增加，最终导致电路饱和，无法形成正确的角度信号[10]。
为了获得车模准确而平滑的倾角和角速度信号，本文通过卡尔曼滤波算法对陀螺仪和加速度计获得的信号进行处理，这种方法既能将陀螺仪测量的角速度信号中的漂移误差去除，又能将加速度计测量的车模角度值中的噪声信号滤除。
4.2 卡尔曼滤波算法简介
卡尔曼滤波分为两个阶段，分别是预测阶段和更新阶段，假设现在的系统状态是时刻 的状态，那末就可以由系统模型和系统上一时刻的状态预测得到当前状态：
(4.2)
式（4.2）中， 是利用上一状态预测的结果， 是上一时刻的状态经过优化后的结果， 是当前时刻的控制量，当没有控制量时它可以为0。
现在系统已经被更新，但是 的协方差还没有被更新。用 表示协方差，那末：
(4.3)
式（4.3）中， 是 对应的协方差， 是 对应的协方差， 是 的转置矩阵， 是系统的过程激励噪声协方差矩阵。式（4.2）和式（4.3）作为卡尔曼滤波算法的五个公式中的前两个式子，这两个式子是用于系统预测的。
现在就有了当前状态的预测结果，同时也有了当前状态的测量值。结合预测值和测量值，可以得到当前 时刻最优化的估计值 ：
(4.4)
式（4.4）中 是卡尔曼增益。
(4.5)
经过最优化后的 时刻的估计值 通过上述的式子可以得到，但是为了使卡尔曼滤波器不断地运行下去直到系统过程结束，还需要更新 时刻的 的协方差 ：
(4.6)
式（4.6）中， 是单位矩阵，而且对于单个模型和单个测量值来说 ， 就是当系统进入到 时刻的状态后的式（4.3）中的 ，这样，算法就可以递归地运算下去[16]。
卡尔曼滤波器的工作原理图如图4.4所示。
图4.4 卡尔曼滤波器工作原理图
4.3 车模角度和角速度的卡尔曼滤波算法
两轮自平衡智能车的卡尔曼滤波器的设计关键是状态向量的选取，状态向量的选取影响到卡尔曼滤波器整个状态方程的结构，是卡尔曼滤波器设计的关键。考虑到车模倾角与角速度存在着导数关系，因此可以选取车模的倾角作为状态向量的一个元素，然而车模倾角速度却不适合作为状态向量的元素，因为其导数无法给出。一种可行的方法是不直接估计角速度的值，而是用加速度计来估计陀螺仪的零位偏差 ，这样就得到状态向量的另一个元素[29]。