卡尔曼滤波和角度测定Word文档格式.docx
《卡尔曼滤波和角度测定Word文档格式.docx》由会员分享，可在线阅读，更多相关《卡尔曼滤波和角度测定Word文档格式.docx（12页珍藏版）》请在冰点文库上搜索。
0
},
{
0,
1
}};
angle;
q_bias;
//偏心、倾向于
rate;
//比率
R_angle
0.5
;
//R倾角
Q_angle
0.001;
//Q倾角
Q_gyro
0.003;
//Q陀螺仪
stateUpdate（const
gyro_m）{
/*状态更新*/
q;
Pdot[4];
q
gyro_m
-
Pdot[0]
P[0][1]
P[1][0];
/*
0,0
*/
Pdot[1]
P[1][1];
0,1
Pdot[2]
1,0
Pdot[3]
Q_gyro;
1,1
rate
angle
+=
*
dt;
P[0][0]
P[1][0]
P[1][1]
return
}
kalmanUpdate（const
incAngle）
angle_m
incAngle;
angle_err
h_0
1;
PHt_0
h_0*P[0][0];
+
h_1*P[0][1]
0*/
PHt_1
h_0*P[1][0];
h_1*P[1][1]
E
+（h_0
PHt_0）;
K_0
/
E;
K_1
Y_0
PHt_0;
/*h_0
P[0][0]*/
Y_1
P[0][1];
-=
Y_0;
Y_1;
angle_err;
q_bias
复制代码
波形显示
测试说明——单片机采集加速度和陀螺仪的信号，并使用上面的kalman滤波，计算出最优倾角，通过串口发送到pc机，pc机运行的串口示波器将相关波形显示出来。
1、蓝色为加速度换算后的角度。
2、黄色为陀螺仪直接积分后的角度。
3、红色为kalman滤波后的角度。
用手指敲小车可以看到加速度模块计算获取的角度震动比较厉害，经过卡尔曼滤波后的波形相对平缓一些。
局部放大看一下曲线还是很优美的哦，哈。
。
波形显示用了园子里xf_z1988的开源波形控件，他的主页是：
RelaxBlog
第二个Arduino小车两轮自平衡
自从做了第一个Arduino小车兴趣大增，于是开始制作第二个Arduino小车，这次我想做得相对复杂一点。
一直对SEGWAY非常着迷，查了些技术资料发现自平衡小车的原理也比较简单：
利用陀螺仪和加速度模块获得小车角度，Arduino对获取的数据进行处理，然后控制电机运转纠正倾斜，从而达到平衡的效果。
先来个视频：
需要准备的硬件有：
1、陀螺仪
我选用的是L3G4200D三轴陀螺仪，其实自平衡小车只用到其中的一轴
2、加速度计
我用的是ADXL345三轴加速度计，自平衡小车也只用到其中两轴
3、Arduino板子
我用的是
我使用的是arduinoduemilanove2009
arduino硬件区别请看这里：
http:
//arduino.cc/en/Main/Hardware
4、L298N电机驱动模块一个
需要带光耦
5、直流减速电机两个
小车轮胎两个
塑料盒子一个
还需要一些杜邦线、电池、螺丝等辅助的东西
有朋友问我这些东西哪里能买到，其实这些材料拜一下淘宝大神就能找到的。
组装过程比较简单，在塑料盒合适的位置打孔，然后用螺丝固定住电路板和电机即可：
制作之前我们需要对陀螺仪+加速度计进行测试，看我们获取的角度数据是否满足要求。
网上常用的方法是使用卡尔曼滤波将陀螺仪和加速度计的数据进行融合而得到一个相对稳定正确的角度值，具体方法在我前面的文章中提到过：
L3G4200D+ADXL345卡尔曼滤波
获取到角度以后需要找到小车的平衡点，也就是无外力作用的时候小车能够立在地面上的角度：
角度差=小车角度-平衡点角度。
用小车角度数据结合当前的倾斜目标值，通过PID运算，得出电机PWM脉宽数据，指挥电机运行即可。
PID算法相对比较简单，而且arduino有现成的PIDlibraries:
//arduino.cc/playground/Code/PIDLibrary
PID:
:
PID（double*
Input,
double*
Output,
Setpoint,
double
Kp,
Ki,
Kd,
int
ControllerDirection）
{
SetOutputLimits（0,
255）;
//default
output
limit
corresponds
to
//the
arduino
pwm
limits
SampleTime
100;
Controller
Sample
Time
is
0.1
seconds
SetControllerDirection（ControllerDirection）;
SetTunings（Kp,
Kd）;
lastTime
millis（）-SampleTime;
inAuto
false;
myOutput
Output;
myInput
Input;
mySetpoint
Setpoint;
}
PIDLIB的参数分别是这样的：
Input
输入值（这里输入卡尔曼融合获取的角度值）
Output
PID计算的结果，供电机驱动的PWM使用
Setpoint
期望值（这里输入小车平衡点的角度值）
Kp、Ki、Kd这是KPI的三个重要参数
这三个参数的详细说明我从网上摘录了一段：
比例（P）控制
比例控制是一种最简单的控制方式。
其控制器的输出与输入误差信号成比例关系。
当仅有比例控制时系统输出存在稳态误差（Steady-stateerror）。
积分（I）控制
在积分控制中，控制器的输出与输入误差信号的积分成正比关系。
对一个自动控制系统，如果在进入稳态后存在稳态误差，则称这个控制系统是有稳态误差的或简称有差系统（SystemwithSteady-stateError）。
为了消除稳态误差，在控制器中必须引入“积分项”。
积分项对误差取决于时间的积分，随着时间的增加，积分项会增大。
这样，即便误差很小，积分项也会随着时间的增加而加大，它推动控制器的输出增大使稳态误差进一步减小，直到等于零。
因此，比例+积分（PI）控制器，可以使系统在进入稳态后无稳态误差。
微分（D）控制
在微分控制中，控制器的输出与输入误差信号的微分（即误差的变化率）成正比关系。
自动控制系统在克服误差的调节过程中可能会出现振荡甚至失稳。
其原因是由于存在有较大惯性组件（环节）或有滞后（delay）组件，具有抑制误差的作用，其变化总是落后于误差的变化。
解决的办法是使抑制误差的作用的变化“超前”，即在误差接近零时，抑制误差的作用就应该是零。
这就是说，在控制器中仅引入“比例”项往往是不够的，比例项的作用仅是放大误差的幅值，而目前需要增加的是“微分项”，它能预测误差变化的趋势，这样，具有比例+微分的控制器，就能够提前使抑制误差的控制作用等于零，甚至为负值，从而避免了被控量的严重超调。
所以对有较大惯性或滞后的被控对象，比例+微分（PD）控制器能改善系统在调节过程中的动态特性。
PID计算相关代码如下：
PID
myPID（&
&
Setpoint,2,5,1,
DIRECT）;
//PID对象声明
setupPID（）;
//PID初始化
....
Kalman_Filter（Adxl_angle,
Gyro_sensor）;
//卡尔曼融合获取angle
Input
myPID.Compute（）;
//PID计算获取
Output
Drive（Output）;
//根据Output驱动电机
void
setupPID（）{
0;
17;
//我的小车自平衡角度为17
myPID.SetSampleTime（100）;
//控制器的采样时间100ms
//myPID.SetOutputLimits（0,
2000）;
myPID.SetMode（AUTOMATIC）;
如果你做完这些小车也能成功站起来了，我的小车抖动得比较厉害，是因为我的直流电机减速太多了（减速比1：
220的单轴电机），而且PID的kp,ki,kd三个参数没调整好。
等有时间换个电机再仔细调整一下参数，最好能做成可以控制前景、后退、转弯的小车。
弄个体积大点的就能骑着上下班了，哈。
这是我们自己整理出来的
kalmanUpdate（double
Acc_Mess,double
Gyro_Media）
Acc_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）;
Acc_Real
}
窗体底端
升级会员 