卡尔曼滤波和角度测定.docx

1、卡尔曼滤波和角度测定 小车下面就是L3G4200D + ADXL345 两个模块，加速度模块没固定好，板子太小了没地方打孔，有时间将两个模块焊到一个万能板上应该会容易固定一些。 加速度模块角度计算： 如果传感器 x 轴朝下， y 轴朝前 那竖直方向弧度计算公式为： angle = atan2(y, z) /结果以弧度表示并介于 -pi 到 pi 之间（不包括 -pi） 如果要换算成具体角度： angle = atan2(y, z) * (180/3.14) 陀螺仪角度计算： 式中angle(n)为陀螺仪采样到第n次的角度值； angle(n-1)为陀螺仪第n-1次采样时的角度值； gyron为

2、陀螺仪的第n次采样得到的瞬时角速率值； dt为运行一遍所用时间； angle_n += gyro(n) * dt /积分计算 卡尔曼滤波 网上找的kalman滤波，具体代码如下staticconstfloatdt=0.02;staticfloatP22=1,0,0,1;floatangle;floatq_bias;/偏心、倾向于floatrate;/比率staticconstfloatR_angle=0.5;/R倾角staticconstfloatQ_angle=0.001;/Q倾角staticconstfloatQ_gyro=0.003;/Q陀螺仪floatstateUpdate(const

3、floatgyro_m)/*状态更新*/floatq;floatPdot4;q=gyro_m-q_bias;Pdot0=Q_angle-P01-P10;/*0,0*/Pdot1=-P11;/*0,1*/Pdot2=-P11;/*1,0*/Pdot3=Q_gyro;/*1,1*/rate=q;angle+=q*dt;P00+=Pdot0*dt;P01+=Pdot1*dt;P10+=Pdot2*dt;P11+=Pdot3*dt;returnangle;floatkalmanUpdate(constfloatincAngle)floatangle_m=incAngle;floatangle_err=

4、angle_m-angle;floath_0=1;constfloatPHt_0=h_0*P00;/*+h_1*P01=0*/constfloatPHt_1=h_0*P10;/*+h_1*P11=0*/floatE=R_angle+(h_0*PHt_0);floatK_0=PHt_0/E;floatK_1=PHt_1/E;floatY_0=PHt_0;/*h_0*P00*/floatY_1=h_0*P01;P00-=K_0*Y_0;P01-=K_0*Y_1;P10-=K_1*Y_0;P11-=K_1*Y_1;angle+=K_0*angle_err;q_bias+=K_1*angle_err;

5、returnangle;复制代码 波形显示 测试说明单片机采集加速度和陀螺仪的信号，并使用上面的kalman滤波，计算出最优倾角，通过串口发送到pc机，pc机运行的串口示波器将相关波形显示出来。 1、蓝色为加速度换算后的角度。 2、黄色为陀螺仪直接积分后的角度。 3、红色为kalman滤波后的角度。 用手指敲小车可以看到加速度模块计算获取的角度震动比较厉害，经过卡尔曼滤波后的波形相对平缓一些。 局部放大看一下曲线还是很优美的哦，哈。 波形显示用了园子里xf_z1988的开源波形控件，他的主页是： Relax Blog第二个Arduino小车 两轮自平衡 自从做了第一个Arduino小车兴趣大增

6、，于是开始制作第二个Arduino小车，这次我想做得相对复杂一点。一直对SEGWAY非常着迷，查了些技术资料发现自平衡小车的原理也比较简单：利用陀螺仪和加速度模块获得小车角度，Arduino对获取的数据进行处理，然后控制电机运转纠正倾斜，从而达到平衡的效果。 先来个视频： 需要准备的硬件有： 1、陀螺仪 我选用的是L3G4200D三轴陀螺仪，其实自平衡小车只用到其中的一轴 2、加速度计 我用的是ADXL345三轴加速度计，自平衡小车也只用到其中两轴 3、Arduino板子 我用的是我使用的是 arduino duemilanove 2009 arduino硬件区别请看这里：http:/ardu

7、ino.cc/en/Main/Hardware 4、L298N电机驱动模块 一个 需要带光耦 5、直流减速电机两个 小车轮胎两个 塑料盒子一个 还需要一些杜邦线、 电池、螺丝等辅助的东西 有朋友问我这些东西哪里能买到，其实这些材料拜一下淘宝大神就能找到的。 组装过程比较简单，在塑料盒合适的位置打孔，然后用螺丝固定住电路板和电机即可： 制作之前我们需要对陀螺仪 + 加速度计 进行测试，看我们获取的角度数据是否满足要求。网上常用的方法是使用卡尔曼滤波将陀螺仪和加速度计的数据进行融合而得到一个相对稳定正确的角度值，具体方法在我前面的文章中提到过：L3G4200D + ADXL345 卡尔曼滤波 获取

8、到角度以后需要找到小车的平衡点，也就是无外力作用的时候小车能够立在地面上的角度：角度差 = 小车角度 - 平衡点角度。 用小车角度数据结合当前的倾斜目标值，通过PID运算，得出电机PWM脉宽数据，指挥电机运行即可。 PID算法相对比较简单，而且arduino有现成的PID libraries :http:/arduino.cc/playground/Code/PIDLibraryPID:PID(double*Input,double*Output,double*Setpoint,doubleKp,doubleKi,doubleKd,intControllerDirection)PID:SetO

9、utputLimits(0,255);/defaultoutputlimitcorrespondsto/thearduinopwmlimitsSampleTime=100;/defaultControllerSampleTimeis0.1secondsPID:SetControllerDirection(ControllerDirection);PID:SetTunings(Kp,Ki,Kd);lastTime=millis()-SampleTime;inAuto=false;myOutput=Output;myInput=Input;mySetpoint=Setpoint;复制代码 PID

10、LIB的参数分别是这样的： Input 输入值(这里输入卡尔曼融合获取的角度值) Output PID计算的结果，供电机驱动的PWM使用 Setpoint 期望值（这里输入小车平衡点的角度值） Kp、Ki、Kd 这是KPI的三个重要参数 这三个参数的详细说明我从网上摘录了一段： 比例（P）控制 比例控制是一种最简单的控制方式。其控制器的输出与输入误差信号成比例关系。当仅有比例控制时系统输出存在稳态误差（Steady-state error）。 积分（I）控制 在积分控制中，控制器的输出与输入误差信号的积分成正比关系。对一个自动控制系统，如果在进入稳态后存在稳态误差，则称这个控制系统是有稳态误差

11、的或简称有差系统（System with Steady-state Error）。为了消除稳态误差，在控制器中必须引入“积分项”。积分项对误差取决于时间的积分，随着时间的增加，积分项会增大。这样，即便误差很小，积分项也会随着时间的增加而加大，它推动控制器的输出增大使稳态误差进一步减小，直到等于零。因此，比例+积分(PI)控制器，可以使系统在进入稳态后无稳态误差。 微分（D）控制 在微分控制中，控制器的输出与输入误差信号的微分（即误差的变化率）成正比关系。自动控制系统在克服误差的调节过程中可能会出现振荡甚至失稳。其原因是由于存在有较大惯性组件（环节）或有滞后(delay)组件，具有抑制误差的作用

12、，其变化总是落后于误差的变化。解决的办法是使抑制误差的作用的变化“超前”，即在误差接近零时，抑制误差的作用就应该是零。这就是说，在控制器中仅引入 “比例”项往往是不够的，比例项的作用仅是放大误差的幅值，而目前需要增加的是“微分项”，它能预测误差变化的趋势，这样，具有比例+微分的控制器，就能够提前使抑制误差的控制作用等于零，甚至为负值，从而避免了被控量的严重超调。所以对有较大惯性或滞后的被控对象，比例+微分(PD)控制器能改善系统在调节过程中的动态特性。 PID计算相关代码如下：PIDmyPID(&Input,&Output,&Setpoint,2,5,1,DIRECT);/PID对象声明set

13、upPID();/PID初始化.Kalman_Filter(Adxl_angle,Gyro_sensor);/卡尔曼融合获取angleInput=angle;myPID.Compute();/PID计算获取OutputDrive(Output);/根据Output驱动电机voidsetupPID()Input=0;Setpoint=17;/我的小车自平衡角度为17myPID.SetSampleTime(100);/控制器的采样时间100ms/myPID.SetOutputLimits(0,2000);myPID.SetMode(AUTOMATIC);复制代码 如果你做完这些小车也能成功站起来了

14、，我的小车抖动得比较厉害，是因为我的直流电机减速太多了（减速比1：220的单轴电机），而且PID的kp,ki,kd三个参数没调整好。等有时间换个电机再仔细调整一下参数，最好能做成可以控制前景、后退、转弯的小车。 弄个体积大点的就能骑着上下班了，哈。这是我们自己整理出来的doublekalmanUpdate(doubleAcc_Mess,doubleGyro_Media)doubleAcc_Real=0.0,Acc_Media=0.0,T=0.0,Q=0.5,Kg=0.0,q=0.6;/测量不确定度Q=10，全局变量/最优角度值的偏差L全局变量/q全局变量/T高斯白噪声T/中间变量Acc_Media/T陀螺仪的高斯白噪声/L上一时刻陀螺仪与真实值的真实误差/Q此时刻陀螺仪与真实值的估计误差/Kg高斯增益/Acc_Media=Gyro_Media;/计算瞬时角度值T=sqrt(Q*Q+L*L);Kg=T*T/(T*T+q*q);Acc_Real=Acc_Media+Kg*(Acc_Mess-Acc_Media);L=sqrt(1-Kg)*T*T);returnAcc_Real; 窗体底端