卡尔曼滤波模型如图6所示，以陀螺仪测量的角速度作为预测值的控制量，加速度传感器测量的角度作为观测值。公式如下，Z(k)为k时刻加速度积分得到的角度，U(k)为k时刻的角速度，X(k|k)为k时刻经卡尔曼滤波后的角度，X(k|k-1)为系统利用k-1时刻的状态得到的k时刻的预测值，Kg(k)为k时刻的卡尔曼增益。
由于自平衡智能车系统为单输入单输出系统，因此令A、B、H、K不为矩阵，且值都为1。自平衡智能车系统卡尔曼滤波参数有3个，P是卡尔曼系统参数A的协方差，P的初值P(0)的设定需要根据系统效果来调试确定，初值一般在1附近。Q是预测值Z(k)的协方差，R是测量值U(k)的协方差。对Q和R的设定原则是，Q／(Q+R)的值就是卡尔曼增益的收敛值。例如其值为0．2，那么卡尔曼滤波后的增益会向0．2收敛，即如果预测角度值是5°，角度测量值是10°，那么最优化角度为：5+0．2*(10-5)=6°。因此，卡尔曼增益越小，说明预测值越可靠，最优化角度越接近预测值；相反，卡尔曼增益越大，说明测量值越可靠，最优化角度越接近测量值。P／(Q+R)反映收敛的快慢程度，该值设定越小，收敛越快，该值越大，收敛越慢。由于卡尔曼增益收敛总的来说是很快的，因此该值的设置影响较小。
卡尔曼滤波后的波形图7所示，包括加速度计输出的信号、陀螺仪输出的信号和卡尔曼滤波后的波形。实验表明，卡尔曼滤波可以很好地还原信号，并可以克服加速度计及陀螺仪的温漂，能够准确地提供车模的角度信息。
4．2 速度信息的处理
本系统的处理方法是根据光电码盘的脉冲信息，通过单片机的输入捕捉功能实现测速。速度闭环控制方面，采用的则是增量型的PID控制算法。所需公式为：
其中，Δu(k)表示增量，e(t)为系统的控制偏差，Kp为比例增益，Ki=KpTTi表示积分系数，Kd=KpTdT表示微分系数。
Kp——起比例调节作用，系统一旦出现了偏差，比例调节立即产生调节作用以减少偏差。
Ki——使系统消除稳态误差，提高误差度，积分调节的引入会使系统稳定性下降，动态响应变慢。
Kd——反映了系统偏差信号的变化速率，可以预见偏差的变化趋势，能提前对偏差进行控制。
针对车模的电机，结合实际的调试经验，本车模的电机调速采用的方案是PI控制，将对目标速度和当前速度再次进行PI运算，得到电机控制信息并交给外部设备控制模块处理，以达到控制速度的目的。
4．3 软件设计
系统中主要利用主控芯片的A／D转换模块读取陀螺仪和加速度传感器的值，从而通过卡尔曼滤波得到车模的角度信息。由于控制目标是保持车模直立，因此，以直立时车模的角度为控制目标，以所测量的车模角度信息为当前反馈，通过PID控制算法，计算出需要给出的电机速度，通过电机闭环控制使车模产生相应的运动状态以保持车模的直立平衡状态。车模的系统流程图如图8所示。
结语
实验结果表明，本系统方法简单实用，可以很好地实现预期的功能，即实现小车的动态自平衡，采用该自平衡方法的磁导航智能车在第七届“飞思卡尔”杯全国大学生智能车比赛中取得了非常优异的成绩。该方法卡尔曼滤波的应用上，公式(2)～(6)可以进一步简化，同时也可以通过优化各个参数更好地实现车模自平衡，进一步提高车模的稳定性。
上一篇：摄像头智能循迹小车设计与实现解析
