浅析IMU代码

IMU的代码的引自https://storage.googleapis.com/google-code-archive-downloads/v2/code.google.com/imumargalgorithm30042010sohm/IMU.zip

复制代码
 1 //=====================================================================================================
 2 // IMU.c
 3 // S.O.H. Madgwick
 4 // 25th September 2010
 5 //=====================================================================================================
 6 // Description:
 7 //
 8 // Quaternion implementation of the 'DCM filter' [Mayhony et al].
 9 //
10 // User must define 'halfT' as the (sample period / 2), and the filter gains 'Kp' and 'Ki'.
11 //
12 // Global variables 'q0', 'q1', 'q2', 'q3' are the quaternion elements representing the estimated
13 // orientation.  See my report for an overview of the use of quaternions in this application.
14 //
15 // User must call 'IMUupdate()' every sample period and parse calibrated gyroscope ('gx', 'gy', 'gz')
16 // and accelerometer ('ax', 'ay', 'ay') data.  Gyroscope units are radians/second, accelerometer 
17 // units are irrelevant as the vector is normalised.
18 //
19 //=====================================================================================================
20 
21 //----------------------------------------------------------------------------------------------------
22 // Header files
23 
24 #include "IMU.h"
25 #include <math.h>
26 
27 //----------------------------------------------------------------------------------------------------
28 // Definitions
29 
30 #define Kp 2.0f            // proportional gain governs rate of convergence to accelerometer/magnetometer
31 #define Ki 0.005f        // integral gain governs rate of convergence of gyroscope biases
32 #define halfT 0.5f        // half the sample period
33 
34 //---------------------------------------------------------------------------------------------------
35 // Variable definitions
36 
37 float q0 = 1, q1 = 0, q2 = 0, q3 = 0;    // quaternion elements representing the estimated orientation
38 float exInt = 0, eyInt = 0, ezInt = 0;    // scaled integral error
39 
40 //====================================================================================================
41 // Function
42 //====================================================================================================
43 
44 void IMUupdate(float gx, float gy, float gz, float ax, float ay, float az) {
45     float norm;
46     float vx, vy, vz;
47     float ex, ey, ez;         
48     
49     // normalise the measurements
50     norm = sqrt(ax*ax + ay*ay + az*az);       
51     ax = ax / norm;
52     ay = ay / norm;
53     az = az / norm;      
54     
55     // estimated direction of gravity
56     vx = 2*(q1*q3 - q0*q2);
57     vy = 2*(q0*q1 + q2*q3);
58     vz = q0*q0 - q1*q1 - q2*q2 + q3*q3;
59     
60     // error is sum of cross product between reference direction of field and direction measured by sensor
61     ex = (ay*vz - az*vy);
62     ey = (az*vx - ax*vz);
63     ez = (ax*vy - ay*vx);
64     
65     // integral error scaled integral gain
66     exInt = exInt + ex*Ki;
67     eyInt = eyInt + ey*Ki;
68     ezInt = ezInt + ez*Ki;
69     
70     // adjusted gyroscope measurements
71     gx = gx + Kp*ex + exInt;
72     gy = gy + Kp*ey + eyInt;
73     gz = gz + Kp*ez + ezInt;
74     
75     // integrate quaternion rate and normalise
76     q0 = q0 + (-q1*gx - q2*gy - q3*gz)*halfT;
77     q1 = q1 + (q0*gx + q2*gz - q3*gy)*halfT;
78     q2 = q2 + (q0*gy - q1*gz + q3*gx)*halfT;
79     q3 = q3 + (q0*gz + q1*gy - q2*gx)*halfT;  
80     
81     // normalise quaternion
82     norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
83     q0 = q0 / norm;
84     q1 = q1 / norm;
85     q2 = q2 / norm;
86     q3 = q3 / norm;
87 }
88 
89 //====================================================================================================
90 // END OF CODE
91 //====================================================================================================
复制代码

 

复制代码
 1 //=====================================================================================================
 2 // IMU.h
 3 // S.O.H. Madgwick
 4 // 25th September 2010
 5 //=====================================================================================================
 6 //
 7 // See IMU.c file for description.
 8 // 
 9 //=====================================================================================================
10 #ifndef IMU_h
11 #define IMU_h
12 
13 //----------------------------------------------------------------------------------------------------
14 // Variable declaration
15 
16 extern float q0, q1, q2, q3;    // quaternion elements representing the estimated orientation
17 
18 //---------------------------------------------------------------------------------------------------
19 // Function declaration
20 
21 void IMUupdate(float gx, float gy, float gz, float ax, float ay, float az);
22 
23 #endif
24 //=====================================================================================================
25 // End of file
26 //=====================================================================================================
复制代码

 

浅析

将加速度的原始数据,归一化,得到单位加速度  
 // normalise the measurements
   norm = sqrt(ax*ax + ay*ay + az*az);       
   ax = ax / norm;
   ay = ay / norm;
   az = az / norm; 

参考https://en.wikipedia.org/wiki/Quaternions_and_spatial_rotation

 

引用AN5022 Quaternion Algebra.pdf

 

或者

出自《整合 GNSS 與 INS 量測資訊的地平面運動軌跡估測》鄭  期  元的论文

下面把四元数换算成方向余弦中的第三行的三个元素。刚好vx,vy,vz 其实就是上一次的欧拉角(四元数)的机体坐标参考系换算出来的重力的单位向量【没弄懂的可以参考上面的资料】。

    // estimated direction of gravity
    vx = 2*(q1*q3 - q0*q2);
    vy = 2*(q0*q1 + q2*q3);
    vz = q0*q0 - q1*q1 - q2*q2 + q3*q3;

 

 

复制代码
ax、ay、az是机体坐标参照系上,加速度计测出来的重力向量,也就是实际测出来的重力向量。
ax、ay、az是测量得到的重力向量,
vx、vy、vz是陀螺积分后的姿态来推算出的重力向量,它们都是机体坐标参照系上的重力向量。
那它们之间的误差向量,就是陀螺积分后的姿态和加计测出来的姿态之间的误差。
向量间的误差,可以用向量叉积(也叫向量外积、叉乘)来表示,exyz就是两个重力向量的叉积。
这个叉积向量仍旧是位于机体坐标系上的,而陀螺积分误差也是在机体坐标系,而且叉积的大小与陀螺积分误差成正比,正好拿来纠正陀螺。
(你可以自己拿东西想象一下)由于陀螺是对机体直接积分,所以对陀螺的纠正量会直接体现在对机体坐标系的纠正。
// error is sum of cross product between reference direction of field and direction measured by sensor ex = (ay*vz - az*vy); ey = (az*vx - ax*vz); ez = (ax*vy - ay*vx);
复制代码

 

 可以参考的文章

 http://blog.csdn.net/qq_21842557/article/details/50993809

http://www.anobbs.com/forum.php?mod=viewthread&tid=1185

展开阅读全文

没有更多推荐了,返回首页