卡尔曼算法在两轮自平衡小车上的应用研究.docx
《卡尔曼算法在两轮自平衡小车上的应用研究.docx》由会员分享，可在线阅读，更多相关《卡尔曼算法在两轮自平衡小车上的应用研究.docx（16页珍藏版）》请在冰豆网上搜索。
卡尔曼算法在两轮自平衡小车上的应用研究
第六届全国大学生创新创业年会学术论文推荐意见表
中央部委所属高校名或省份名：
安徽省学科类别：
工学
学术论文题目
卡尔曼滤波在两轮自平衡小车上的应用研究
项目名称
两轮自平衡代步车的设计
作者姓名
姜辉汪高勇
项目编号
201210879018
已获资助金额
20000
立项时间
2012
项目成员
汪高勇姜辉李椅刚刘阳阳
指导教师
徐朝胜
论文是否正式发表过
否
刊号
期刊名称
期刊期次
论文摘要：
为了使两轮自平衡小车能够在转向、速度控制、抗干扰等稳定性上有比较大的性能提升，本文比较了但传感器和双传感器在不同PWM占空比下的输出波形，并对获得的参数进行期望、方差和采点样的纵向对比。
结果表明：
无论是加速度计还是陀螺仪，都有自己的优势和劣势。
简而言之：
加速度计短时间内不可信，但是长时间内可信。
陀螺仪短时间内可信而长时间内不可信。
车模直立行走常常需要两个传感器来共同工作，因为一个传感器通常不可靠，我们需要在两个传感器中取其精华去其糟粕，这就需要卡尔曼滤波。
学校推荐意见（含论文查重查新结论）：
（单位公章）
年月日
年会专家组评审意见：
专家签名：
年月日
注：
学科类别请按工学、理学、农学、医学、经济学、管理学、法学、哲学、教育学、文学、历史学、艺术学、军事学等学科分类
卡尔曼滤波在两轮自平衡小车上的
应用研究
安徽科技学院姜辉、汪高勇、李椅刚、刘阳阳
徐朝胜、讲师
（机电与车辆工程学院，安徽凤阳233100）
摘要：
为了使两轮自平衡小车能够在转向、速度控制、抗干扰等稳定性上有比较大的性能提升，本文比较了但传感器和双传感器在不同PWM占空比下的输出波形，并对获得的参数进行期望、方差和采点样的纵向对比。
结果表明：
无论是加速度计还是陀螺仪，都有自己的优势和劣势。
简而言之：
加速度计短时间内不可信，但是长时间内可信。
陀螺仪短时间内可信而长时间内不可信。
车模直立行走常常需要两个传感器来共同工作，因为一个传感器通常不可靠，我们需要在两个传感器中取其精华去其糟粕，这就需要卡尔曼滤波。
关键词：
两轮自平衡代步车、MC9S12XS128、卡尔曼滤波
引言
自平衡两轮机器人是一种特殊的轮式移动机器人，其概念是在20世纪80年代末提出来的，是智能机器人领域中一个崭新的研究方向，也是机器人学的一个重要分支，属于轮式机器人中的一种，并且结合了自主移动的思想[1]。
在这个出门就必须要乘车的社会，交通工具已是不可或缺的。
每天大街上穿梭的是各种机动车，可是现在的油价，不是那么容易承受的。
虽然大多数人还是可以承受一辆车的价格的，但是油价和拥挤的街道，这两点使得很多人推迟或是放弃了买车的计划。
电动车在这样的环境下成为一个不错的选择。
可是现在有的电动车有着一定的不足，使得它并不能去撼动传统汽油车的地位。
混合动力的车子也成为了一些汽车厂商的选择，就目前来看，混合动力的车并没有得到消费者的青睐。
卡尔曼滤波器是一个最优化自回归数据处理算法。
对于解决很大部分的问题，它是最优，效率最高甚至是最有用的。
它的广泛应用已经超过30年，包括机器人导航，控制，传感器数据融合甚至在军事方面的雷达系统以及导弹追踪等等。
现在更是被广泛运用在图像处理、图像分析等领域。
自平衡两轮车通过对行驶状态的实时检测，调整两轮的前后运转，来使之找到自身的重心，从而达到平衡。
因此一种可靠的车身姿态就显得尤为重要，在这里我们使用卡尔曼滤波来达到我们的设计要求。
一、两轮平衡车研究概况及意义分析
（一）、研究概况
两轮自平衡代步车是一种两轮直立的电动驱动车，它自己本身不能稳定，必须加以一定的算法控制才能使之平衡。
作为一个被控对象，两轮自平衡代步车的研究重点是其的行走平衡控制技术。
近几年，随着自平衡控制理论的不断发展，以及控制硬件的不断升级，控制处理器的速度越来越快，控制理论越来越合理，关于两轮自平衡代步车的研究得到了飞速的发展。
作者简介：
姜辉（1990-），男，江苏省南通人，电子信息工程，本科3年级，主要从事电子设计研究。
（二）、研究意义
现在，随着科学技术的进去和研究的不断深入，在各个领域所面临的任务和环境也越来越复杂。
很多场合环境艰苦，人类无法触及，这就需要一个灵活的机器人去完成任务，但在这样复杂的环境中，机器人如何灵活快捷的执行任务，成为现在需要研究的一个话题。
两轮自平衡小车就是在这样的一个环境下提出来的。
两轮自平衡小车是一个具有体校小。
灵活多变，转弯半径小等优点，在民用和军用领域有着广泛的应用前景。
所以两轮在平衡小车在理论和实践中都有非常重要的意义。
二、卡尔曼滤波
（一）、卡尔曼滤波简介
卡尔曼滤波实质上就是一种算法，一种最优化自回归数据处理算法。
也叫自回归滤波器。
它能在包含噪声的测量中，预测出动态系统的状态。
对于大部分问题的解决他是最有效率，最优化，甚至是最有用的。
它被广泛应用于工业、导航、甚至是军事上的雷达定位、导弹导航等。
现在更是被广泛运用在图像处理、图像分析等领域。
在生活、工业、军事等上扮演着非正常重要的角色。
（二）、使用卡尔曼滤波的原因
我们参考别的设计之后，决定使用加速度传感器和陀螺仪，但是在使用时我们产生了一些疑问。
在计算角速度时我们可以通过
这样的方法对加速度计进行微分，计算出角速度，通过对陀螺仪
的积分，来获得需要的角度。
如果是这样，用两个传感器有多大必要呢？
对此，我们通过对两个传感器的数据分析采样，观察他输出信号之间的还关系。
由于考虑电机转动产生的震动会对传感器产生比较大的干扰，因此我们测试了不同PWM占空比下传感器输出，并用MATLAB绘制曲线，并进行相关运算。
首先是陀螺仪的数据测试。
PWM20%占空比下，车模直立状态下的陀螺仪输出见图1
图1PWM20%占空比下，车模直立状态下的陀螺仪输出
PWM60%占空比下，车模直立状态下的陀螺仪输出见图2
图2PWM60%占空比下，车模直立状态下的陀螺仪输出
PWM100%占空比下，车模直立状态下的陀螺输出见图3
图3PWM100%占空比下，车模直立状态下的陀螺输出
PWM占空比与陀螺仪方差关系见图4
图4PWM占空比与陀螺仪方差关系
从图像上可以看到，pwm升高，陀螺仪的噪声也会显著增加。
下表是我们通过MATLAB计算获得的一些数据。
此处我们用方差表征传感器噪声大小。
表1陀螺仪参数测试
PWM占空比
最大值
最小值
期望
方差
采样点数
20
71
70
70.0121
0.012
742
30
70
70
70
0
842
40
71
70
70.0201
0.0197
746
50
71
70
70.0199
0.0195
754
60
71
70
70.0199
0.0195
754
70
71
69
70.0471
0.0857
786
80
71
69
70.0455
0.2752
726
90
72
68
70.0587
0.5911
699
100
72
67
70.0676
1.2864
843
之后我们对加速度传感器进行了测试。
PWM100%占空比下，车模直立状态下的加速度计输出见图5
图5PWM100%占空比下，车模直立状态下的加速度计输出
PWM占空比与加速度计方差关系见图6
图6PWM占空比与加速度计方差关系
下表是通过MATLAB计算获得的关于加速度传感器的测试参数
表2加速度参数测试
PWM占空比
最大值
最小值
期望
方差
采样点数
20
80
87
83.1544
0.3116
786
30
78
89
83.304
0.5379
967
40
75
89
83.3189
1.1393
668
50
71
90
83.293
2.5727
789
60
68
91
83.1108
4.3543
977
70
66
93
82.1382
15.7352
867
80
63
97
83.0125
34.4538
921
90
60
98
81.8
27.793
835
100
59
98
83.15
34.3276
937
通过以上数据，我们不难获得以下一些结论：
1、在陀螺仪噪声范围内，加速度噪声早已超出了可接受范围。
2、方差（可以等效理解为噪声大小）随着占空比不断增加。
3、加速度计虽然噪声很大，但是长时间来看，他的数学期望是非常稳定的。
4、陀螺仪的噪声比较小，但是他的误差会对积分造成漂移。
无论是加速度计还是陀螺仪，都有自己的优势和劣势。
简而言之：
加速度计短时间内不可信，但是长时间内可信。
陀螺仪短时间内可信而长时间内不可信。
车模直立行走常常需要两个传感器来共同工作，因为一个传感器通常不可靠，我们需要在两个传感器中取其精华去其糟粕，这就需要卡尔曼滤波。
（三）、卡尔曼滤波原型
由于倾角和倾角角速度之间的倒数关系，我们将系统倾斜真实角度
用作一个状态向量，建立两个方程，分别是状态方程和测量方程。
这里，我们用陀螺仪常值偏差b作为状态向量以此来得到对应的状态方程和观测方程：
式中：
表示的是固定偏差的陀螺仪输出角速度，
代表陀螺仪噪声，
为加速度传感器噪声，
表示的是加速度计经处理后得到的角度值，
是加速度传感器测量噪声，
为陀螺仪漂移误差，假设
和
二者满足正态分布的白色噪声。
令
为系统采样周期，得到离散系统的状态方程和测量方程：
根据k-1时刻的角度值和预测得到的k时刻的角度值得到k时刻的高斯噪声的方差可以估算出k时刻的实际角度。
Q与R矩阵的形式如下：
加速度传感器和陀螺仪测量的协方差分别是
和
，协方差的数值代表着滤波器对传感器的信任度，值越小信任度越高。
在该系统中陀螺仪的值更为接近准确值，因此取
的值小于
的值。
当前状态:
（1）
式中，
，
，X（k|k-1）是利用k预测的结果，X（k-1|k-1）是k-1时刻的最优结果。
则有对应于
的协方差为：
（2）
式中，
是
对应的协方差，
表示
的转置矩阵，
是系统过程的协方差，式子
（1）、
（2）即对系统的状态更新。
则状态k的最优化估算值
：
（3）
其中
为卡尔曼增益：
（4）
这时，已经得到了一个估算值，但为了能够找到最优的角度值，必须还要更新
的协方差：
（5）
其中
为单位阵，对于本系统则有
。
当系统进入k+1状态时，
就是式子
（2）的
。
（3）、（4）、（5）式为卡尔曼滤波器状态更新方程。
计算完成之后需要重复计算，不断循环往复知道找到最优的值。
（四）、卡尔曼滤波参数整定
化简后的卡尔曼滤波主要是对dt以及Kg两个参数进行整定。
需要特别指出的是，这里的dt并不只是一个采样间隔。
由于陀螺仪的输出和加速度计输出的量纲并不相同，所以陀螺仪采样值*dt并不直接反应一个角度，而是与实际角度相差一个系数。
因此此处的dt可以等效理解为dt=采样间隔*比例系数。
dt越大，积分速度越快，卡尔曼输出追随实际角度的情况越好（当然不能太大，不然可能会出现超前相位）。
但是dt越大，陀螺仪漂移造成的影响也就越大。
Kg决定了加速度计的权重。
Kg越大，实际输出的漂移就越小，但是滤波效果的噪声也就越大。
所以dt和kg是一对矛盾，不能太过于极端。
首先是静态整定。
将车模保持在稳定直立状态，让车轮以恒定PWM（80%以上）转动，然后调节参数。
图7
目标是达到上图效果。
红色为卡尔曼滤波输出，蓝色为加速度计直接输出。
如果红色抖动非常厉害，可以适当减小Kg的大小。
接着是动态整定
还是保持车轮恒定PWM旋转，同时，摇摆车身。
图8
大致调节到上图所示的样子。
目前存在一个相位差，这个相位差在后期直立算法的调试中是致命的，必须克服掉。
解决方法，逐渐增加dt即可。
图9
达到上图效果即可认为卡尔曼滤波参数整定完成。
四、滤波算法部分程序
//-------------------------------------------------------
//Kalman滤波
//-------------------------------------------------------
voidKalman_Filter（void）
{
/**/
floatangle_m;
floatgyro_m;
floatangle_,angle_dot_;
staticfloatQ_angle=0.001,Q_gyro=0.003,R_angle=0.5,dt=5*0.001;
staticfloatP[2][2]={
{1,0},
{0,1}
};
staticfloatPdot[4]={0,0,0,0};
staticcharC_0=1;
staticfloatq_bias,angle_err,PCt_0,PCt_1,E,K_0,K_1,t_0,t_1;
angle_+=（gyro_m-q_bias）*dt;
Pdot[0]=Q_angle-P[0][1]-P[1][0];
Pdot[1]=-P[1][1];
Pdot[2]=-P[1][1];
Pdot[3]=Q_gyro;
P[0][0]+=Pdot[0]*dt;
P[0][1]+=Pdot[1]*dt;
P[1][0]+=Pdot[2]*dt;
P[1][1]+=Pdot[3]*dt;
angle_err=angle_m-angle_;
PCt_0=C_0*P[0][0];
PCt_1=C_0*P[1][0];
E=R_angle+C_0*PCt_0;
K_0=PCt_0/E;
K_1=PCt_1/E;
t_0=PCt_0;
t_1=C_0*P[0][1];
P[0][0]-=K_0*t_0;
P[0][1]-=K_0*t_1;
P[1][0]-=K_1*t_0;
P[1][1]-=K_1*t_1;
angle_+=K_0*angle_err;
q_bias+=K_1*angle_err;
angle_dot_=gyro_m-q_bias;
}
参考文献：
[1]李威.两轮自平衡机器人控制系统研制[D].哈尔滨工业大学硕士学位论文，2005,6.
[2]霍亮.两轮自平衡电动车的关键技术研究[D].哈尔滨工程大学硕士学位论文，2010,3.
[3]王晓宇.两轮自平衡机器人的研究[D].哈尔滨工业大学博士学位论文，2007,12.
[4]李红美，李智，高飞.平衡的杰作—赛格威HT两轮平台电动车[J].电器工业，2002（6）:
19-21.
[5]杨慧.自平衡两轮移动机器人的设计[D].兰州大学硕士学位论文，2012,5.
[6]叶聪红.本质不稳定两轮车辅助平衡装置的智能控制[D]西安电子科技大学,2006.
升级会员 