您的位置: 首页  >  文章  >  絮叨---imu

絮叨---imu

分类: 文章 • 2024-10-15 23:45:40

转载： http://www.arduino.cn/forum.php?mod=viewthread&tid=18392&page=1

在一些对角度要求不高的场合，要得到角度，只用加速度计就可以了，比如做一个自平衡小车。可是要得到一个更加精确的角度，就需要用到陀螺仪了，比如四旋翼飞行器。陀螺仪测量的是角度的变化率，对这个变化率积分，就可以得到角度值。

    我们一般用的是比较常见的卡尔曼滤波（当然还有其他融合方式）。我们先来说说用加速度计和陀螺仪测量倾角的原理，等会儿再说卡尔曼滤波的原理。（以下测量倾角的说明摘抄自文档《MPU6050数据轻松分析》，原理意思懂了就行。）
    首先我们知道重力加速度可以分解成x,y,z三个方向的分加速度。而加速度计可以测量某一时刻x,y,z三个方向的加速度值。利用各个方向的分量与重力加速度的比值来计算出小车大致的倾角。其实在自平衡小车上非静止的时候，加速度计测出的结果并不是非常精确。因为大家在高中物理的时候都学过，物体时刻都会受到地球的万有引力作用产生一个向下的重力加速度，而小车在动态时，受电机的作用肯定有一个前进或者后退方向的作用力，而加速度计测出的结果是，重力加速度与小车运动加速度合成得到一个总的加速度在三个方向上的分量。这是加速度计测不准的一个原因。我们这里先理想化加速度计不受外界干扰。
    下边我们就开始分析从加速度得到角度的方法。如下图，把加速度计平放，分别画出xyz轴的方向。这三个轴就是我们后边分析所要用到的坐标系。
絮叨---imu
把mpu6050安装在自平衡车上时也是这样的水平安装在小车底盘上的，假设两个车轮安装时车轴和y轴在一条直线上。那么小车摆动时，参考水平面就是桌面，并且车轴（y轴）与桌面始终是平行的，小车摆动和移动过程中y轴与桌面的夹角是不会发生变化的，一直是0度。发生变化的是x轴与桌面的夹角以及z轴与桌面的夹角，而且桌面与x轴z轴夹角变化度数是一样的。所以我们只需要计算出x轴和z轴中任意一个轴的夹角就可以反映出小车的倾斜的情况了。
絮叨---imu
为了方便分析，由于y轴与桌面夹角始终不变，我们从y轴的方向俯看下去，那么这个问题就会简化成只有x轴和z轴的二维关系。假设某一时刻小车上加速度计（mpu6050）处于如下状态，下图是我们看到简化后的模型。
絮叨---imu
在这个图中，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
得到这个公式可是还是得不到想要的角度，因为需要计算反正弦，而反正弦在单片机里不是很好计算。

为了得到角度，于是又查了相关资料，原来在角度较小的情况下，角度的正弦与角度对应的弧度成线性关系。先看看下边的图：
絮叨---imu
这个图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可以从加速度计里读出来，所以这下角度就可以轻松得到了。而之前也说过这个角度不是很精确，但是至少可以反映出角度变化的趋势。不过可以通过卡尔曼滤波等算法把加速度计读出的角度和陀螺仪读出的角度结合起来，使小车的角度更加准确。
通过陀螺仪来测量角度就很简单了，因为陀螺仪读出的是角速度，大家都知道，角速度乘以时间，就是转过的角度。把每次计算出的角度做累加就会等到当前所在位置的角度。先看下图：
絮叨---imu

假设最初陀螺仪是与桌面平行，单片机每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
实际上这就是一个积分过程。

其实这种计算出来的角度也存在一定的误差，而且总的角度是经过多次相加得到的，这样误差就会越积累越大，最终导致计算出的角度与实际角度相差很大。于是也可以使用卡尔曼滤波把加速度计读出的角度结合在一起，使计算出的角度更准确。
接着我们讲一下卡尔曼滤波的原理：
絮叨---imu新手平衡小车的卡尔曼滤波算法总结.rar(30.84 KB, 下载次数: 683)，都在这个文档里了，建议打印下来把每个公式，矩阵之类的，推一下。
然后我们就可以得到代码了。
首先硬件连接如下：
絮叨---imu
代码如下：
[C] 纯文本查看 复制代码
?
代码
001
002
003
004
005
006
007
008
009
010
011
012
013
014
015
016
017
018
019
020
021
022
023
024
025
026
027
028
029
030
031
032
033
034
035
036
037
038
039
040
041
042
043
044
045
046
047
048
049
050
051
052
053
054
055
056
057
058
059
060
061
062
063
064
065
066
067
068
069
070
071
072
073
074
075
076
077
078
079
080
081
082
083
084
085
086
087
088
089
090
091
092
093
094
095
096
097
098
099
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
// 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;
doubletotal_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;
}
 
voidloop() {
    // read raw accel/gyro measurements from device
 
    doubleax_angle=0.0;
    doublegx_angle=0.0;
    unsignedlongtime=0;
    unsignedlongmictime=0;
    staticunsignedlong pretime=0;
    floatgyro=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;
}

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
红色的是陀螺仪读出的角度，绿色的是加速度计读出的角度。
絮叨---imu
红色是陀螺仪读出的角度，绿色是加速度计读出的角度，黄色是卡尔曼滤波融合后的角度，由于参数调整的不是很好，可以看出黄色曲线有滞后的现象。

另外我还发现另一种滤波方法，介绍如下。
下面我们就来讲解IMU（惯性测量单元），用这个算法来获得一个精确的角度值。
       首先说明这个算法的作者是Kalman filter（卡尔曼.菲尔德）关于算法的详细介绍在这里http://www.starlino.com/imu_guide.html
哈哈，全是英文的看晕了吧。这里有中文翻译的文档。絮叨---imu加速度计和陀螺仪指南.pdf(564 KB, 下载次数: 772)
个人建议把这个文档打印下来，好好研究研究，就几页，绝对会受益匪浅的。（注：你只需要有加速度传感器和陀螺仪就行了，不用非得买文中的那个模块，我才不会告诉你只需要一个mpu6050模块就够了呢）对文档中的理论知识了解过之后，我们接着来讲代码的实现。原文介绍在这里http://www.starlino.com/imu_kalman_arduino.html
因为他原文的介绍以及代码的实现都是基于他的模块的，所以我们的改一下，此处我基于mpu6050来修改介绍。
首先我们要从mpu6050中把测量得到的3轴加速度和3轴陀螺仪数据读出来。借鉴这篇帖子
http://www.geek-workshop.com/forum.php?mod=viewthread&tid=1017，首先讲一下mpu6050和arduino的接线：mpu6050的SDA接A4，SCL接A5，VCC接3.3V，GND接GND。然后刷入下面的代码，打开串口就可以看见mpu6050读取的原始数据了。
程序需要的库，在这儿：絮叨---imuMPU6050.zip(31.47 KB, 下载次数: 213)絮叨---imuI2Cdev.zip(10.85 KB, 下载次数: 150)
[C] 纯文本查看 复制代码
?
代码
001
002
003
004
005
006
007
008
009
010
011
012
013
014
015
016
017
018
019
020
021
022
023
024
025
026
027
028
029
030
031
032
033
034
035
036
037
038
039
040
041
042
043
044
045
046
047
048
049
050
051
052
053
054
055
056
057
058
059
060
061
062
063
064
065
066
067
068
// 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);
}

输出图像如图：
絮叨---imu
接下来我们看mpu6050的数据手册：
絮叨---imu
絮叨---imu
可以得到加速度和陀螺仪相应的灵敏度。用测得的原始数据除以灵敏度就可以得到以g，度每秒为单位的数据。
程序如下：
[C] 纯文本查看 复制代码
?
代码
001
002
003
004
005
006
007
008
009
010
011
012
013
014
015
016
017
018
019
020
021
022
023
024
025
026
027
028
029
030
#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();
 }
 voidloop() {
  
    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;
  
}

数据输出如图：
絮叨---imu
到现在我们得到了以g，度每秒为单位的数据，就可以开始修改算法了。
原始的算法程序在这里，我把每一行的注释都翻译了，结合前面的理论，这个程序很好理解的。
[C] 纯文本查看 复制代码
?
代码
001
002
003
004
005
006
007
008
009
010
011
012
013
014
015
016
017
018
019
020
021
022
023
024
025
026
027
028
029
030
031
032
033
034
035
036
037
038
039
040
041
042
043
044
045
046
047
048
049
050
051
052
053
054
055
056
057
058
059
060
061
062
063
064
065
066
067
068
069
070
071
072
073
074
075
076
077
078
079
080
081
082
083
084
085
086
087
088
089
090
091
092
093
094
095
096
097
098
099
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
#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 {                                       
  charinpInvert[INPUT_COUNT];   // bits 0..5 invert input  输入的符号  -1 +1
  intzeroLevel[INPUT_COUNT];    // 0..2 accelerometer zero level (mV) @ 0 G  加速度调零
                                  // 3..5 gyro zero level (mV) @ 0 deg/s  陀螺仪调零
  intinpSens[INPUT_COUNT];      // 0..2 acceleromter input sensitivity (mv/g) 加速度输入灵敏度
                                  // 3..5 gyro input sensitivity (mV/deg/ms)  陀螺仪输入灵敏度
  floatwGyro;                         // 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 longlastMicros; 
//下面的变量不需要是全局的，但是我们定义来调试使用
//Variables below don't need to be global but we expose them for debug purposes
unsigned longinterval;//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() {
  staticinti;
  Serial.begin(57600);
 
  //Setup parameters for Acc_Gyro board, see [url=http://www.gadgetgangster.com/213]http://www.gadgetgangster.com/213[/url]
  for(i=0;i<=2;i++){                 // X,Y,Z axis
    config.zeroLevel[i] = 1650;     // Accelerometer zero level (mV) @ 0 G  加速度计调零
    config.inpSens[i] = 478;        // Accelerometer Sensisitivity mV/g   加速度计灵敏度
  }       
   
  for(i=3;i<=4;i++){
    config.inpSens[i] = 2000;           // Gyro Sensitivity mV/deg/ms    陀螺仪灵敏度
    config.zeroLevel[i] = 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 [url=http://starlino.com/imu_guide.html]http://starlino.com/imu_guide.html[/url] for discussion
  //also see [url=http://www.gadgetgangster.com/213]http://www.gadgetgangster.com/213[/url] 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(){
  staticinti,w;
  staticfloattmpf,tmpf2; 
  staticunsignedlong newMicros; //new timestamp 新的时间戳
  staticcharsignRzGyro; 
 
  //get raw adc readings  得到未加工的数据
  newMicros = micros();      //save the time when sample is taken 当样本数据被采集的时候，记录下这时的时间
  for(i=0;i<INPUT_COUNT;i++) an[i]= 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  [url=http://starlino.com/imu_guide.html]http://starlino.com/imu_guide.html[/url]
      //根据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;
}
 
voidnormalize3DVector(float* vector){
  staticfloatR; 
  R =sqrt(vector[0]*vector[0] + vector[1]*vector[1] + vector[2]*vector[2]);
  vector[0] /= R;
  vector[1] /= R; 
  vector[2] /= R; 
}
 
floatsquared(floatx){
  returnx*x;
}
 
//Convert ADC value for to physical units see [url=http://starlino.com/imu_guide.html]http://starlino.com/imu_guide.html[/url] 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
floatgetInput(chari){
  staticfloattmpf;               //temporary variable
  tmpf = an[i] * VDD / 1023.0f; //voltage (mV)
  tmpf -= config.zeroLevel[i]; //voltage relative to zero level (mV)
  tmpf /= config.inpSens[i];   //input sensitivity in mV/G(acc) or mV/deg/ms(gyro)
  tmpf *= config.inpInvert[i]; //invert axis value according to configuration
  returntmpf;               
}

相关推荐