《卡尔曼滤波和角度测定》由会员分享，可在线阅读，更多相关《卡尔曼滤波和角度测定（12页珍藏版）》请在金锄头文库上搜索。
1、小车下面就是L3G4200D + ADXL345两个模块，加速度模块没固定好，板子太小了没地 方打孔，有时间将两个模块焊到一个万能板上应该会容易固定一些。加速度模块角度计算：如果传感器x轴朝下，y轴朝前那竖直方向弧度计算公式为：angle = atan2(y, z) 结果以弧度表示并介于-pi到pi 之间(不包括-pi)如果要换算成具体角度：angle = ata n2(y, z) * (180/3.14)陀螺仪角度计算:anglen * angledt式中angle(n)为陀螺仪采样到第n次的角度值；angle(n-1)为陀螺仪第n-1次采样时的角度值； gyron为陀螺仪的第n次采样得到的
2、瞬时角速率值; dt为运行一遍所用时间；an gle_n += gyro( n) * dt积分计算卡尔曼滤波网上找的kalman滤波，具体代码如下static const float dt = 0.02;static float P22 = 1, 0 , 0, 1 floatangle;floatq_bias; /偏心、倾向于floatrate; / 比率static const float R_angle = 0.5 ; /R彳顷角static const float Q_angle = 0.001; /Q 彳顷角static const float Q_gyro = 0.003; /Q 陀
3、螺仪float stateUpdate(const float gyro_m) /*状态更新*/ float q;q = gyro m-q bias;Pdot0 = Qangle -P01 一 P10;/* 0,0Pdot1=-P11;/* 0,1*/Pdot2=-P11;/*1,0 */Pdot3 = Qgyro;/*1,1 */rate = q;angle += q* dt;P00 +=Pdot0 *dt;P01 +=Pdot1 *dt;P10 +=Pdot2 *dt;P11 +=Pdot3 *dt;float Pdot4;*/return angle; float kalmanUpdat
4、e(const float incAngle) float angle_m = incAngle;float angle_err = angle_m - angle;float h_0 = 1;const float PHt_0 = h_0*P00; /* + h_1*P01 = 0*/const floatPHt_1 = h_0*P10;/* + h_1*P110*/float E = R_angle +(h_0 * PHt_0);float K_0 = PHt_0 / E;float K_1 = PHt_1 / E;float Y_0 = PHt_0; /*h_0 * P00*/float
5、 Y_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;return angle;复制代码波形显示测试说明一一单片机采集加速度和陀螺仪的信号，并使用上面的kalman滤波，计算出最优倾角，通过串口发送到pc机pc机运行的串口示波器将相关波形显示出来。1、蓝色为加速度换算后的角度。2、黄色为陀螺仪直接积分后的角度。3、红色为kalman滤波后的角度。o o o3 2 1o o o o o o o o1 2 3 4
6、 5 6 7-gs-80-90-1000X轴坐标用手指敲小车可以看到加速度模块计算获取的角度震动比较厉害，经过卡尔曼滤波后的波形相对平缓一些。匪I X - TTIjhITR300局部放大看一下曲线还是很优美的哦，哈。波形显示用了园子里xf_z1988的开源波形控件，他的主页是:http:/ Relax Blog 第二个Arduino小车两轮自平衡自从做了第一个Arduino小车兴趣大增，于是开始制作第二个Arduino小车，这次我想做 得相对复杂一点。一直对SFGWAY非常着迷，查了些技术资料发现自平衡小车的原理也比较简 单：利用陀螺仪和加速度模块获得小车角度，Arduino对获取的数据进行处
7、理，然后控制电机 运转纠正倾斜，从而达到平衡的效果。先来个视频：需要准备的硬件有：1、 陀螺仪我选用的是L3G4200D三轴陀螺仪，其实自平衡小车只用到其中的一轴2、加速度计 我用的是ADXL345三轴加速度计，自平衡小车也只用到其中两轴3、Arduino板子 我用的是我使用的是arduino duemilanove 2009 arduino硬件区 另I请看这里：http:/arduino.cc/en/Main/Hardware4、L298N电机驱动模块一个需要带光耦5、直流减速电机两个小车轮胎两个塑料盒子一个还需要一些杜邦线、电池、螺丝等辅助的东西有朋友问我这些东西哪里能买到，其实这些材料拜
8、一下淘宝大神就能找到的。组装过程比较简单，在塑料盒合适的位置打孔，然后用螺丝固定住电路板和电机即可:制作之前我们需要对陀螺仪+加速度计 进行测试，看我们获取的角度数据是否满足要求。 网上常用的方法是使用卡尔曼滤波将陀螺仪和加速度计的数据进行融合而得到一个相对稳定正 确的角度值，具体方法在我前面的文章中提到过：L3G4200D + ADXL345卡尔曼滤波1心a卞小曼真实角度加速度一 角度滤波器J获取到角度以后需要找到小车的平衡点，也就是无外力作用的时候小车能够立在地面上的 角度：角度差=小车角度-平衡点角度。用小车角度数据结合当前的倾斜目标值，通过PID运算，得出电机PWM脉宽数据，指挥 电机
9、运行即可。PID算法相对比较简单，而且arduino有现成的PIDlibraries : http:/arduino.cc/playground/Code/PIDLibraryPID:PID(double* Input, double* Output, double* Setpoint,double Kp, double Ki, double Kd, int ControllerDirection)/default output limit corrPID:SetOutputLimits(0, 255);esponds to/the arduino pwm limits/default Con
10、troller SamplSampleTime = 100;e Time is 0.1 secondsPID:SetControllerDirection(ControllerDirection);PID:SetTunings(Kp, Ki, Kd);lastTime = millis()-SampleTime;inAuto = false;myOutput = Output;myInput = Input;mySetpoint = Setpoint;复制代码PID LIB的参数分别是这样的：In put输入值（这里输入卡尔曼融合获取的角度值）Output PID计算的结果，供电机驱动的PWM
11、使用Setpoi nt期望值（这里输入小车平衡点的角度值）Kp、Ki、Kd这是KPI的三个重要参数这三个参数的详细说明我从网上摘录了一段：比例（P）控制比例控制是一种最简单的控制方式。其控制器的输出与输入误差信号成比例关系。当仅有比 例控制时系统输出存在稳态误差（Steady-state error ）。积分控制在积分控制中，控制器的输出与输入误差信号的积分成正比关系。对一个自动控制系统，如 果在进入稳态后存在稳态误差，则称这个控制系统是有稳态误差的或简称有差系统（System with Steady-state Error）。为了消除稳态误差，在控制器中必须引入积分项。积分项对误 差取决于时
12、间的积分，随着时间的增加，积分项会增大。这样，即便误差很小，积分项也会随着 时间的增加而加大，它推动控制器的输出增大使稳态误差进一步减小，直到等于零。因此，比例 +积分（PI）控制器，可以使系统在进入稳态后无稳态误差。微分（D）控制在微分控制中，控制器的输出与输入误差信号的微分（即误差的变化率）成正比关系。自动 控制系统在克服误差的调节过程中可能会出现振荡甚至失稳。其原因是由于存在有较大惯性组件 （环节）或有滞后（delay）组件，具有抑制误差的作用，其变化总是落后于误差的变化。解决的 办法是使抑制误差的作用的变化超前”，即在误差接近零时，抑制误差的作用就应该是零。这就 是说，在控制器中仅引入
13、 比例”项往往是不够的，比例项的作用仅是放大误差的幅值，而目前需要增加的是微分项，它能预测误差变化的趋势，这样，具有比例+微分的控制器，就能够提 前使抑制误差的控制作用等于零，甚至为负值，从而避免了被控量的严重超调。所以对有较大惯 性或滞后的被控对象，比例+微分(PD)控制器能改善系统在调节过程中的动态特性。PID计算相关代码如下：PID myPID (&Input, &0 utput, & Setpoint,2,5,1, DIRECT); /PID 对象声明setupPID(); /PID 初始化Kalman_Filter(Adxl_angle, Gyro_sensor); /卡尔曼融合获取 angleInput = angle;myPID.Compute(); /PID 计算获取 OutputDrive(Output);/根据 Output 驱动电机void setupPID()Input = 0;Setpoint = 17;/我的小车自平衡角度为17myPID.SetSampleTime(lOO); /控制器的采样时间 100ms /myPID.SetOutputLimits(0, 2000);myPID.SetMode(AUTOMATIC);
