本帖最后由 对折之内 于 2016-2-27 01:09 编辑
在一些对角度要求不高的场合，要得到角度，只用加速度计就可以了，比如做一个自平衡小车。可是要得到一个更加精确的角度，就需要用到陀螺仪了，比如四旋翼飞行器。陀螺仪测量的是角度的变化率，对这个变化率积分，就可以得到角度值。 我们一般用的是比较常见的卡尔曼滤波（当然还有其他融合方式）。我们先来说说用加速度计和陀螺仪测量倾角的原理，等会儿再说卡尔曼滤波的原理。（以下测量倾角的说明摘抄自文档《MPU6050数据轻松分析》，原理意思懂了就行。） 首先我们知道重力加速度可以分解成x,y,z三个方向的分加速度。而加速度计可以测量某一时刻x,y,z三个方向的加速度值。利用各个方向的分量与重力加速度的比值来计算出小车大致的倾角。其实在自平衡小车上非静止的时候，加速度计测出的结果并不是非常精确。因为大家在高中物理的时候都学过，物体时刻都会受到地球的万有引力作用产生一个向下的重力加速度，而小车在动态时，受电机的作用肯定有一个前进或者后退方向的作用力，而加速度计测出的结果是，重力加速度与小车运动加速度合成得到一个总的加速度在三个方向上的分量。这是加速度计测不准的一个原因。我们这里先理想化加速度计不受外界干扰。 下边我们就开始分析从加速度得到角度的方法。如下图，把加速度计平放，分别画出xyz轴的方向。这三个轴就是我们后边分析所要用到的坐标系。
把mpu6050安装在自平衡车上时也是这样的水平安装在小车底盘上的，假设两个车轮安装时车轴和y轴在一条直线上。那么小车摆动时，参考水平面就是桌面，并且车轴（y轴）与桌面始终是平行的，小车摆动和移动过程中y轴与桌面的夹角是不会发生变化的，一直是0度。发生变化的是x轴与桌面的夹角以及z轴与桌面的夹角，而且桌面与x轴z轴夹角变化度数是一样的。所以我们只需要计算出x轴和z轴中任意一个轴的夹角就可以反映出小车的倾斜的情况了。 为了方便分析，由于y轴与桌面夹角始终不变，我们从y轴的方向俯看下去，那么这个问题就会简化成只有x轴和z轴的二维关系。假设某一时刻小车上加速度计（mpu6050）处于如下状态，下图是我们看到简化后的模型。 在这个图中，y轴已经简化和坐标系的原点o重合在了一起。我们来看看如何计算出小车的倾斜角，也就是与桌面的夹角a。上图g是重力加速度，gx、gz分别是g在x轴和z轴的分量。 由于重力加速度是垂直于水平面的，得到： 角a+角b=90度 X轴与y轴是垂直关系，得到： 角c+角b=90度 于是轻松的就可以得出： 角a=角c 根据力的分解，g、gx、gz三者构成一个长方形，根据平行四边形的原理可以得出： 角c=角d
所以计算出角度d就等效于计算出了x轴与桌面的夹角a。前边已经说过gx是g在x轴的分量，那么根据正弦定理就可以得出： Sind=gx/g 得到这个公式可是还是得不到想要的角度，因为需要计算反正弦，而反正弦在单片机里不是很好计算。
为了得到角度，于是又查了相关资料，原来在角度较小的情况下，角度的正弦与角度对应的弧度成线性关系。先看看下边的图： 这个图x轴是角度，取值范围是0~90度，有三个函数曲线，分别是： Y=sinx 正弦曲线 Y=x*3.14/180 弧度 Y=0.92*x*3.14/180 乘以一个0.92系数的弧度 从图上可以看出，当角度范围是0~29度时： sinx=x*3.14/180 对于自平衡车来说，小车的摆动范围在-29~29度之内，如果超过这个范围，小车姿态也无法调整，所以对于自平衡小车sinx=x*3.14/180基本上是成立的。当然有时候也会担心-29~29度的摇摆范围还是无法满足需求。那可以给上边的公式乘一个系数。得到如下公式： Sinx=k*x*3.14/180 从上边的函数对比图可以看出，当系数取0.92时，角度范围可以扩大到-45~45度。 经过这一系列的分析，终于得到角度换算方法： 由 Sind=gx/g Sind=k*d*3.14/180 得到： gx/g=k*d*3.14/180 那么角度就可以通过如下公式计算出： d=180*gx/(k*g*3.14)
而gx可以从加速度计里读出来，所以这下角度就可以轻松得到了。而之前也说过这个角度不是很精确，但是至少可以反映出角度变化的趋势。不过可以通过卡尔曼滤波等算法把加速度计读出的角度和陀螺仪读出的角度结合起来，使小车的角度更加准确。 通过陀螺仪来测量角度就很简单了，因为陀螺仪读出的是角速度，大家都知道，角速度乘以时间，就是转过的角度。把每次计算出的角度做累加就会等到当前所在位置的角度。先看下图：
假设最初陀螺仪是与桌面平行，单片机每tms读一次陀螺仪的角速度，当读了三次角速度以后z轴转到上图的位置，则在这段时间中转过的角度为x： 角x=角1+角2+角3 假设从陀螺仪读出的角速度为w，那总角度为： X=(w1*t1+w2*t2+w3*t3)/1000 假设经过n次，那么总的角度如下： X=(w1*t1+w2*t2+w3*t3+…+wn*tn)/1000 实际上这就是一个积分过程。
其实这种计算出来的角度也存在一定的误差，而且总的角度是经过多次相加得到的，这样误差就会越积累越大，最终导致计算出的角度与实际角度相差很大。于是也可以使用卡尔曼滤波把加速度计读出的角度结合在一起，使计算出的角度更准确。 接着我们讲一下卡尔曼滤波的原理：
新手平衡小车的卡尔曼滤波算法总结.rar
(30.84 KB, 下载次数: 1538)
，都在这个文档里了，建议打印下来把每个公式，矩阵之类的，推一下。
然后我们就可以得到代码了。 首先硬件连接如下： 代码如下： [mw_shl_code=c,true]// Arduino Wire library is required if I2Cdev I2CDEV_ARDUINO_WIRE implementation
// is used in I2Cdev.h
#include "Wire.h"
// I2Cdev and MPU6050 must be installed as libraries, or else the .cpp/.h files
// for both classes must be in the include path of your project
#include "I2Cdev.h"
#include "MPU6050.h"
// class default I2C address is 0x68
// specific I2C addresses may be passed as a parameter here
// AD0 low = 0x68 (default for InvenSense evaluation board)
// AD0 high = 0x69
MPU6050 accelgyro;
int16_t ax, ay, az;
int16_t gx, gy, gz;
double total_angle=0;
#define LED_PIN 13
/* 把mpu6050放在水平桌面上，分别读取读取2000次，然后求平均值 */
#define AX_ZERO (-1476) /* 加速度计的0偏修正值 */
#define GX_ZERO (-30.5) /* 陀螺仪的0偏修正值 */
void setup() {
// join I2C bus (I2Cdev library doesn't do this automatically)
Wire.begin();
// initialize serial communication
// (38400 chosen because it works as well at 8MHz as it does at 16MHz, but
// it's really up to you depending on your project)
Serial.begin(38400);
// initialize device
Serial.println("Initializing I2C devices...");
accelgyro.initialize();
// verify connection
Serial.println("Testing device connections...");
Serial.println(accelgyro.testConnection() ? "MPU6050 connection successful" : "MPU6050 connection failed");
}
/* 通过卡尔曼滤波得到的最终角度 */
float Angle=0.0;
/*由角速度计算的倾斜角度 */
float Angle_gy=0.0;
float Q_angle=0.9;
float Q_gyro=0.001;
float R_angle=0.5;
float dt=0.01; /* dt为kalman滤波器采样时间; */
char C_0 = 1;
float Q_bias, Angle_err;
float PCt_0=0.0, PCt_1=0.0, E=0.0;
float K_0=0.0, K_1=0.0, t_0=0.0, t_1=0.0;
float Pdot[4] ={0,0,0,0};
float PP[2][2] = { { 1, 0 },{ 0, 1 } };
/* 卡尔曼滤波函数,具体实现可以参考网上资料,也可以使用其它滤波算法 */
void Kalman_Filter(float Accel,float Gyro)
{
Angle+=(Gyro - Q_bias) * dt;
Pdot[0]=Q_angle - PP[0][1] - PP[1][0];
Pdot[1]=- PP[1][1];
Pdot[2]=- PP[1][1];
Pdot[3]=Q_gyro;
PP[0][0] += Pdot[0] * dt;
PP[0][1] += Pdot[1] * dt;
PP[1][0] += Pdot[2] * dt;
PP[1][1] += Pdot[3] * dt;
Angle_err = Accel - Angle;
PCt_0 = C_0 * PP[0][0];
PCt_1 = C_0 * PP[1][0];
E = R_angle + C_0 * PCt_0;
if(E!=0)
{
K_0 = PCt_0 / E;
K_1 = PCt_1 / E;
}
t_0 = PCt_0;
t_1 = C_0 * PP[0][1];
PP[0][0] -= K_0 * t_0;
PP[0][1] -= K_0 * t_1;
PP[1][0] -= K_1 * t_0;
PP[1][1] -= K_1 * t_1;
Angle += K_0 * Angle_err;
Q_bias += K_1 * Angle_err;
}
void loop() {
// read raw accel/gyro measurements from device
double ax_angle=0.0;
double gx_angle=0.0;
unsigned long time=0;
unsigned long mictime=0;
static unsigned long pretime=0;
float gyro=0.0;
if(pretime==0)
{
pretime=millis();
return;
}
mictime=millis();
accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
/* 加速度量程范围设置2g 16384 LSB/g
* 计算公式：
* 前边已经推导过这里再列出来一次
* x是小车倾斜的角度,y是加速度计读出的值
* sinx = 0.92*3.14*x/180 = y/16384
* x=180*y/(0.92*3.14*16384)=
*/
ax -= AX_ZERO;
ax_angle=ax/262;
/* 陀螺仪量程范围设置250 131 LSB//s
* 陀螺仪角度计算公式:
* 小车倾斜角度是gx_angle,陀螺仪读数是y,时间是dt
* gx_angle +=(y/(131*1000))*dt
*/
gy -= GX_ZERO;
time=mictime-pretime;
gyro=gy/131.0;
gx_angle=gyro*time;
gx_angle=gx_angle/1000.0;
total_angle-=gx_angle;
dt=time/1000.0;
Kalman_Filter(ax_angle,gyro);
Serial.print(ax_angle); Serial.print(",");
Serial.print(total_angle); Serial.print(",");
Serial.println(Angle);
pretime=mictime;
}
[/mw_shl_code]
Mpu6050取值范围调整的方法,修改MPU6050库中的mpu6050.cpp,修改initialize函数中标成红色的行。 void MPU6050::initialize() { setClockSource(MPU6050_CLOCK_PLL_XGYRO); setFullScaleGyroRange(MPU6050_GYRO_FS_250); /* 陀螺仪取值范围 */ setFullScaleAccelRange(MPU6050_ACCEL_FS_2);/* 加速度计取值范围 */ setSleepEnabled(false); // thanks to Jack Elston for pointing this oneout! }
下别贴出分析出的图供大家参考,图是使用SericalChart（下一帖子介绍），通过收到的串口数据绘制的。 红色的是陀螺仪读出的角度，绿色的是加速度计读出的角度。 红色是陀螺仪读出的角度，绿色是加速度计读出的角度，黄色是卡尔曼滤波融合后的角度，由于参数调整的不是很好，可以看出黄色曲线有滞后的现象。
另外我还发现另一种滤波方法，介绍如下。 下面我们就来讲解IMU（惯性测量单元），用这个算法来获得一个精确的角度值。 因为他原文的介绍以及代码的实现都是基于他的模块的，所以我们的改一下，此处我基于mpu6050来修改介绍。 首先我们要从mpu6050中把测量得到的3轴加速度和3轴陀螺仪数据读出来。借鉴这篇帖子 [mw_shl_code=c,true]// Arduino Wire library is required if I2Cdev I2CDEV_ARDUINO_WIRE implementation
// is used in I2Cdev.h
#include "Wire.h"
// I2Cdev and MPU6050 must be installed as libraries, or else the .cpp/.h files
// for both classes must be in the include path of your project
#include "I2Cdev.h"
#include "MPU6050.h"
// class default I2C address is 0x68
// specific I2C addresses may be passed as a parameter here
// AD0 low = 0x68 (default for InvenSense evaluation board)
// AD0 high = 0x69
MPU6050 accelgyro;
int16_t ax, ay, az;
int16_t gx, gy, gz;
#define LED_PIN 13
bool blinkState = false;
void setup() {
// join I2C bus (I2Cdev library doesn't do this automatically)
Wire.begin();
// initialize serial communication
// (38400 chosen because it works as well at 8MHz as it does at 16MHz, but
// it's really up to you depending on your project)
Serial.begin(38400);
// initialize device
Serial.println("Initializing I2C devices...");
accelgyro.initialize();
// verify connection
Serial.println("Testing device connections...");
Serial.println(accelgyro.testConnection() ? "MPU6050 connection successful" : "MPU6050 connection failed");
// configure Arduino LED for
pinMode(LED_PIN, OUTPUT);
}
void loop() {
// read raw accel/gyro measurements from device
accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
// these methods (and a few others) are also available
//accelgyro.getAcceleration(&ax, &ay, &az);
//accelgyro.getRotation(&gx, &gy, &gz);
// display tab-separated accel/gyro x/y/z values
Serial.print("a/g:\t");
Serial.print(ax);
Serial.print("\t");
Serial.print(ay);
Serial.print("\t");
Serial.print(az);
Serial.print("\t");
Serial.print(gx);
Serial.print("\t");
Serial.print(gy);
Serial.print("\t");
Serial.println(gz);
// blink LED to indicate activity
blinkState = !blinkState;
digitalWrite(LED_PIN, blinkState);
}
[/mw_shl_code]
输出图像如图： 接下来我们看mpu6050的数据手册： 可以得到加速度和陀螺仪相应的灵敏度。用测得的原始数据除以灵敏度就可以得到以g，度每秒为单位的数据。 程序如下： [mw_shl_code=c,true]#include "Wire.h"
#include "I2Cdev.h"
#include "MPU6050.h"
MPU6050 accelgyro;
int16_t ax, ay, az;
int16_t gx, gy, gz;
bool blinkState = false;
void setup() {
Wire.begin();
Serial.begin(38400);
accelgyro.initialize();
}
void loop() {
accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
Serial.print("a/g:\t");
Serial.print((float)ax/16384); Serial.print("\t");
Serial.print((float)ay/16384); Serial.print("\t");
Serial.print((float)az/16384); Serial.print("\t");
Serial.print((float)gx/131); Serial.print("\t");
Serial.print((float)gy/131); Serial.print("\t");
Serial.println((float)gz/131);
blinkState = !blinkState;
}
[/mw_shl_code]
数据输出如图： 到现在我们得到了以g，度每秒为单位的数据，就可以开始修改算法了。 原始的算法程序在这里，我把每一行的注释都翻译了，结合前面的理论，这个程序很好理解的。 [mw_shl_code=c,true]#define INPUT_COUNT 5 //number of analog inputs 模拟输入的数目
#define VDD 5000.0f //Analog reference voltage in milivolts
#define PI 3.14159265358979f
int an[INPUT_COUNT]; //analog inputs 模拟输入
char firstSample; //marks first sample 第一次采样的标志
struct {
char inpInvert[INPUT_COUNT]; // bits 0..5 invert input 输入的符号 -1 +1
int zeroLevel[INPUT_COUNT]; // 0..2 accelerometer zero level (mV) @ 0 G 加速度调零
// 3..5 gyro zero level (mV) @ 0 deg/s 陀螺仪调零
int inpSens[INPUT_COUNT]; // 0..2 acceleromter input sensitivity (mv/g) 加速度输入灵敏度
// 3..5 gyro input sensitivity (mV/deg/ms) 陀螺仪输入灵敏度
float wGyro; // gyro weight/smooting factor 置信度，表示我们对加速度计和陀螺仪的相信程度
} config;
//记号“W”表示其中一个轴，比如说RwAcc[0],RwAcc[1],RwAcc[2]代表RxAcc,RyAcc,RzAcc
//Notation "w" stands for one of the axes, so for example RwAcc[0],RwAcc[1],RwAcc[2] means RxAcc,RyAcc,RzAcc
//Variables below must be global (their previous value is used in getEstimatedInclination)
//下面的变量必须是全局的，他们先前的值在getEstimatedInclination()函数中被使用
float RwEst[3]; //Rw estimated from combining RwAcc and RwGyro Rw的值是RwAcc和RwGyro估算出来的
unsigned long lastMicros;
//下面的变量不需要是全局的，但是我们定义来调试使用
//Variables below don't need to be global but we expose them for debug purposes
unsigned long interval; //interval since previous analog samples interval
//是我们自先前那个模拟样本以来到现在的间隔时间
float RwAcc[3]; //projection of normalized gravitation force vector on x/y/z axis, as measured by accelerometer
//加速度测量的3个轴的值
float RwGyro[3]; //Rw obtained from last estimated value and gyro movement Rw从上一次的预测值和陀螺仪的值中得到
float Awz[2]; //angles between projection of R on XZ/YZ plane and Z axis (deg)
void setup() {
static int i;
Serial.begin(57600);
//Setup parameters for Acc_Gyro board, see http://www.gadgetgangster.com/213
for(i=0;i<=2;i++){ // X,Y,Z axis
config.zeroLevel = 1650; // Accelerometer zero level (mV) @ 0 G 加速度计调零
config.inpSens = 478; // Accelerometer Sensisitivity mV/g 加速度计灵敏度
}
for(i=3;i<=4;i++){
config.inpSens = 2000; // Gyro Sensitivity mV/deg/ms 陀螺仪灵敏度
config.zeroLevel = 1230; // Gyro Zero Level (mV) @ 0 deg/s 陀螺仪调零
}
config.inpInvert[0] = 1; //Acc X
config.inpInvert[1] = 1; //Acc Y
config.inpInvert[2] = 1; //Acc Z
//Gyro readings are sometimes inverted according to accelerometer coordonate system
//see http://starlino.com/imu_guide.html for discussion
//also see http://www.gadgetgangster.com/213 for graphical diagrams
config.inpInvert[3] = 1; //Gyro X
config.inpInvert[4] = 1; //Gyro Y
config.wGyro = 10;
firstSample = 1; //第一次采样标志
}
void loop() {
getEstimatedInclination();
Serial.print(interval); //microseconds since last sample, please note that printing more data will increase interval
Serial.print(","); //上次样本后经过的时间，请注意串口输出更多的数据就会增大间隔时间
Serial.print(RwAcc[0]); //Inclination X axis (as measured by accelerometer) 加速度测量的x轴的倾向
Serial.print(",");
Serial.print(RwEst[0]); //Inclination X axis (estimated / filtered) 经过滤波处理的x轴
Serial.println("");
}
void getEstimatedInclination(){
static int i,w;
static float tmpf,tmpf2;
static unsigned long newMicros; //new timestamp 新的时间戳
static char signRzGyro;
//get raw adc readings 得到未加工的数据
newMicros = micros(); //save the time when sample is taken 当样本数据被采集的时候，记录下这时的时间
for(i=0;i<INPUT_COUNT;i++) an= analogRead(i); //5个模拟引脚分别读取此时的加速度、陀螺仪读数
//compute interval since last sampling time //计算从上次取值后到现在的时间间隔
interval = newMicros - lastMicros; //please note that overflows are ok, since for example 0x0001 - 0x00FE will be equal to 2
//请注意就算数据溢出了也是ok的，因为0x0001 - 0x00FE的值也是2
lastMicros = newMicros; //save for next loop, please note interval will be invalid in first sample but we don't use it
//把lastMicros更新成newMicros，以便下一次的运算。请注意第一次的时间间隔是无效的，但是我们没有使用
//get accelerometer readings in g, gives us RwAcc vector
for(w=0;w<=2;w++) RwAcc[w] = getInput(w); //经过调零，灵敏度，正负值，把加速度的值变为g
//normalize vector (convert to a vector with same direction and with length 1)
normalize3DVector(RwAcc); //正常化加速度向量，使得3个轴的分量平方和为1，矢量长度为1
if (firstSample)
{
for(w=0;w<=2;w++) RwEst[w] = RwAcc[w]; //initialize with accelerometer readings
} //如果是第一次采样，就把RwEst[5]的值初始化为加速度计的测量值
else
{
//evaluate RwGyro vector
if(abs(RwEst[2]) < 0.1)
{
//Rz is too small and because it is used as reference for computing Axz, Ayz it's error fluctuations will amplify leading to bad results
//Rz太小，因为它是计算Axz、Ayz的参照。它的错误波动将会被放大，导致结果出错。
//in this case skip the gyro data and just use previous estimate
//在这里跳过陀螺仪的数据，只是用先前的估算结果，下面的else语句中的程序不会执行了，直接计算最终的值。
for(w=0;w<=2;w++) RwGyro[w] = RwEst[w];
}
else
{
//get angles between projection of R on ZX/ZY plane and Z axis, based on last RwEst
//在上一次RwEst值的基础上，在R向量在ZX/ZY平面的投影和Z轴之间得到角度值
for(w=0;w<=1;w++)
{
tmpf = getInput(3 + w); //get current gyro rate in deg/ms 得到以deg/ms为单位的角速度
tmpf *= interval / 1000.0f; //get angle change in deg 得到deg为单位的角度改变
Awz[w] = atan2(RwEst[w],RwEst[2]) * 180 / PI; //get angle and convert to degrees 得到角度并且转换为degrees(是弧度么？)
Awz[w] += tmpf; //get updated angle according to gyro movement 根据陀螺仪的值更新角度
}
//estimate sign of RzGyro by looking in what qudrant the angle Axz is,
//RzGyro is pozitive if Axz in range -90 ..90 => cos(Awz) >= 0
signRzGyro = ( cos(Awz[0] * PI / 180) >=0 ) ? 1 : -1; //根据Awz的值来决定RwGyro的值是否是负
//reverse calculation of RwGyro from Awz angles, for formulas deductions see http://starlino.com/imu_guide.html
//根据Awz的角度值来计算RwGyro的值
for(w=0;w<=1;w++){
RwGyro[0] = sin(Awz[0] * PI / 180);
RwGyro[0] /= sqrt( 1 + squared(cos(Awz[0] * PI / 180)) * squared(tan(Awz[1] * PI / 180)) );
RwGyro[1] = sin(Awz[1] * PI / 180);
RwGyro[1] /= sqrt( 1 + squared(cos(Awz[1] * PI / 180)) * squared(tan(Awz[0] * PI / 180)) );
}
RwGyro[2] = signRzGyro * sqrt(1 - squared(RwGyro[0]) - squared(RwGyro[1]));
}
//combine Accelerometer and gyro readings 根据置信度组合加速度和陀螺仪
for(w=0;w<=2;w++) RwEst[w] = (RwAcc[w] + config.wGyro* RwGyro[w]) / (1 + config.wGyro);
normalize3DVector(RwEst);
}
firstSample = 0;
}
void normalize3DVector(float* vector){
static float R;
R = sqrt(vector[0]*vector[0] + vector[1]*vector[1] + vector[2]*vector[2]);
vector[0] /= R;
vector[1] /= R;
vector[2] /= R;
}
float squared(float x){
return x*x;
}
//Convert ADC value for to physical units see http://starlino.com/imu_guide.html for explanation.
//For accelerometer it will return g (acceleration) , applies when xyz = 0..2
//For gyro it will return deg/ms (rate of rotation) , applies when xyz = 3..5
float getInput(char i){
static float tmpf; //temporary variable
tmpf = an * VDD / 1023.0f; //voltage (mV)
tmpf -= config.zeroLevel; //voltage relative to zero level (mV)
tmpf /= config.inpSens; //input sensitivity in mV/G(acc) or mV/deg/ms(gyro)
tmpf *= config.inpInvert; //invert axis value according to configuration
return tmpf;
}[/mw_shl_code]
接下来，我们开始修改这个程序，使得mpu6050也能使用这个算法。 待续。
| 
发表于 2016-2-27 01:09
楼主