MWCcomputeIMU算法解析修改20130908要点

1、对 MWC 重要函数的理解目录一、原文件的理解二、我对main loop()主程序的分析三、conputeIMU 函数以及它调用的估计姿态函数 getEstimatedAttitude() 函数 的分析四、旋转矩阵求坐标解析五、计算俯仰角度的依据文件3一、原文件的理解void computeIMU () uint8_t axis;static int16_t gyroADCprevious3 = 0,0,0;int16_t gyroADCp3;int16_t gyroADCinter3;static uint32_t timeInterleave = 0;/we separate the 2

2、situations because reading gyro values with a gyro only setup can be acchieved at a higher rate /gyro+nunchuk: we must wait for a quite high delay betwwen 2 reads to get both WM+ and Nunchuk data. It works with 3ms/gyro only: the delay to read 2 consecutive values can be reduced to only 0.65ms#if de

3、fined(NUNCHUCK)/ /定义 NUNCHUCK (一种硬件设备)执行下面的 annexCode();while(micros()-timeInterleave)<INTERLEA VING_DELAY) ; /interleaving delay between 2 consecutive reads timeInterleave=micros();ACC_getADC();getEstimatedAttitude(); / computation time must last less than one interleaving delay while(micros()-t

4、imeInterleave)<INTERLEA VING_DELAY) ; /interleaving delay between 2 consecutive reads timeInterleave=micros();f.NUNCHUKDATA = 1;while(f.NUNCHUKDA TA) ACC_getADC(); / For this interleaving reading, we must have a gyro update at this point (less delay)for (axis = 0; axis < 3; axis+) / empirical,

5、 we take a weighted value of the current and the previous values/ /4 is to average 4 values, note: overflow is not possible for WMP gyro here gyroDataaxis = (gyroADCaxis*3+gyroADCpreviousaxis)/4;gyroADCpreviousaxis = gyroADCaxis;#else#if ACC/ ACC_getADC();/ 获得加计的 ADC 值getEstimatedAttitude();/ 获取估计姿态

6、#endif#if GYROGyro_getADC();#endiffor (axis = 0; axis < 3; axis+) gyroADCpaxis = gyroADCaxis;timeInterleave=micros();annexCode();通过串口向电脑或 GUI发送MWC的实时数据。if (micros()-timeInterleave)>650) annex650_overrun_count+; /运行时间超时 并进行计数 else while(micros()-timeInterleave)<650) ; /empirical, interleavin

7、g delay between 2 consecutive reads /当运行 时间不足650ms时 依据验证 进行2次连续的读内部延时#if GYROGyro_getADC();#endiffor (axis = 0; axis < 3; axis+) /陀螺仪的值求平均 gyroADCinteraxis = gyroADCaxis+gyroADCpaxis; / empirical, we take a weighted value of the current and the previous values /通过试验获得前面的值和当前值的权重gyroDataaxis = (gy

8、roADCinteraxis+gyroADCpreviousaxis)/3; gyroADCpreviousaxis = gyroADCinteraxis/2;if (!ACC) accADCaxis=0;#endif#if defined(GYRO_SMOOTHING)static int16_t gyroSmooth3 = 0,0,0; /静态变量设置第一次运行时进行初始化，以后保留前次值再更新。for (axis = 0; axis < 3; axis+) gyroDataaxis=(int16_t)(int32_t)(int32_t)gyroSmoothaxis*(conf.Sm

9、oothingaxis-1) )+gyroDataaxis+1 ) / conf.Smoothingaxis);/ /陀螺仪数据平滑滤波gyroSmoothaxis = gyroDataaxis;#elif defined(TRI) /假如定义为三脚机static int16_t gyroYawSmooth = 0;gyroDataY AW = (gyroYawSmooth*2+gyroDataYAW)/3; gyroYawSmooth = gyroDataY AW;#endif / */ Simplified IMU based on "Complementary Filter&q

10、uot; /基于互补滤波简化的IMU 计算/ Inspired by 创新来自于 / adapted by 改编来自于 ziss_dm : / The following ideas was used in this project: /后面的网站知识将要用在这个工程里面/ 1) Rotation matrix 旋转矩阵 : /wiki/Rotation_matrix/ 2) Small-angle approximation 小角度近似算法: /wiki/Small-angle_approximation

11、/ 3) C. Hastings approximation for atan2() /antan2 的近似算法/ 4) Optimization tricks 优化 : / / / Currently Magnetometer uses separate CF which is used only/ for heading approximation ./磁力计 用于近似定向/*/*advanced users settings */ 高级用户设置/* Set the Low Pass Filter factor for ACC */

12、/ 设置加速度计的低通滤波因子/* Increasing this value would reduce ACC noise (visible in GUI), but would increase ACC lag time*/* Comment this if you do not want filter at all.*/ / 注释掉这句代码，假如你不需要滤波#ifndef ACC_LPF_FACTOR#define ACC_LPF_FACTOR 100#endif/* Set the Low Pass Filter factor for Magnetometer */ /设置磁力计的低通

13、滤波因子/* Increasing this value would reduce Magnetometer noise (not visible in GUI), but would increase Magnetometer lag time*/* Comment this if you do not want filter at all.*/#ifndef MG_LPF_FACTOR/#define MG_LPF_FACTOR 4#endif/* Set the Gyro Weight for Gyro/Acc complementary filter */设置陀螺仪权重在加速度计值与加

14、速度互补滤波值的权重因子/* Increasing this value would reduce and delay Acc influence on the output of the filter*/#ifndef GYR_CMPF_FACTOR#define GYR_CMPF_FACTOR 400.0f#endif/* Set the Gyro Weight for Gyro/Magnetometer complementary filter */设置陀螺仪权重在加速度计值与磁力计互补滤波值的权重因子/* Increasing this value would reduce and d

15、elay Magnetometer influence on the output of the filter*/#ifndef GYR_CMPFM_FACTOR#define GYR_CMPFM_FACTOR 200.0f#endif/* end of advanced users settings */ 结束高级用户设置#define INV_GYR_CMPF_FACTOR (1.0f / (GYR_CMPF_FACTOR + 1.0f)#define INV_GYR_CMPFM_FACTOR (1.0f / (GYR_CMPFM_FACTOR + 1.0f)#if GYRO#define

16、 GYRO_SCALE (2380 * PI)/(32767.0f / 4.0f ) * 180.0f * 1000000.0f) /should be 2279.44 but 2380 gives better result/ +-2000/sec deg scale/#define GYRO_SCALE (200.0f * PI)/(32768.0f / 5.0f / 4.0f ) * 180.0f * 1000000.0f) * 1.5f)/ +- 200/sec deg scale/ 1.5 is emperical, not sure what it means/ should be

17、 in rad/sec#else#define GYRO_SCALE (1.0f/200e6f)#/ empirical, depends on WMP on IDG datasheet, tied of deg/ms sensibility/ !should be adjusted to the rad/sec#endif/ Small angle approximation#define ssin(val) (val)#define scos(val) 1.0ftypedef struct fp_vector float X;float Y;float Z; t_fp_vector_def

18、;typedef union float A3; t_fp_vector_def V; t_fp_vector;int16_t _atan2(float y, float x)#define fp_is_neg(val) (uint8_t*)&val)3 & 0x80) != 0)float z = y / x;int16_t zi = abs(int16_t(z * 100);int8_t y_neg = fp_is_neg(y);if ( zi < 100 )if (zi > 10) z = z / (1.0f + 0.28f * z * z);if (fp_i

19、s_neg(x) if (y_neg) z -= PI;else z += PI; else z = (PI / 2.0f) - z / (z * z + 0.28f);if (y_neg) z -= PI;z *= (180.0f / PI * 10);return z;/ Rotate Estimated vector(s) with small angle approximation, according to the gyro data/根据陀螺仪的角度值使用旋转矩阵的小角度近似算法 该函数每个循环 2 次被调用，其中一次的调用是求得上 次加速度值延时一段时间后新的一个估计值(修证值)

20、这个值在后面的计算中权重400，而新获取滤波后的加速度值的权重只有1。void rotateV(struct fp_vector *v,float* delta) .fp_vector v_tmp = *v;v->Z -= deltaROLL * v_tmp.X + deltaPITCH * v_tmp.Y;v->X += deltaROLL * v_tmp.Z - deltaYAW * v_tmp.Y;5v->Y += deltaPITCH * v_tmp.Z + deltaYAW* v_tmp.X;#define ACC_LPF_FOR_VELOCITY 10 stati

21、c float accLPFVel3;static t_fp_vector EstG;void getEstimatedAttitude()uint8_t axis;int32_t accMag = 0;#if MAGstatic t_fp_vector EstM;#endif#if defined(MG_LPF_FACTOR)static int16_t mgSmooth3; #endif#if defined(ACC_LPF_FACTOR)static float accLPF3;#endifstatic uint16_t previousT;uint16_t currentT = mic

22、ros();float scale, deltaGyroAngle3;scale = (currentT - previousT) * GYRO_SCALE; previousT = currentT;/ Initializationfor (axis = 0; axis < 3; axis+) /修整平滑滤波 3轴陀螺仪值 和 3轴加速度值 deltaGyroAngleaxis = gyroADCaxis * scale;#if defined(ACC_LPF_FACTOR)accLPFaxis = accLPFaxis * (1.0f - (1.0f/ACC_LPF_FACTOR)

23、+ accADCaxis * (1.0f/ACC_LPF_FACTOR);accLPFVelaxis = accLPFVelaxis * (1.0f - (1.0f/ACC_LPF_FOR_VELOCITY) + accADCaxis * (1.0f/ACC_LPF_FOR_VELOCITY);accSmoothaxis = accLPFaxis;#define ACC_V ALUE accSmoothaxis#else accSmoothaxis = accADCaxis; #define ACC_V ALUE accADCaxis#endif/accMag += (ACC_V ALUE *

24、 10 / (int16_t)acc_1G) * (ACC_V ALUE * 10 / (int16_t)acc_1G);accMag += (int32_t)ACC_V ALUE*ACC_V ALUE ;#if MAG#if defined(MG_LPF_FACTOR) mgSmoothaxis = (mgSmoothaxis * (MG_LPF_FACTOR - 1) + magADCaxis) / MG_LPF_FACTOR; / LPF for Magnetometer values#define MAG_V ALUE mgSmoothaxis#else#define MAG_V AL

25、UE magADCaxis#endif#endifaccMag = accMag*100/(int32_t)acc_1G*acc_1G);rotateV(&EstG .V,deltaGyroAngle);./在上次获得加速度 3轴数据的基础上，到这个时候已经有时间延时， 也发生了角度的变化，通过旋转得到了一个前次基础上估计的加速度向量#if MAGrotateV(&EstM.V,deltaGyroAngle); / 在上次获得电子罗盘计 3 轴数据的基础上，到这个时候已经有时间延 时，也发生了角度的变化，通过旋转得到了一个前次基础上估计的电子罗盘计向量#endifif ( ab

26、s(accSmoothROLL)<acc_25deg && abs(accSmoothPITCH)<acc_25deg && accSmoothYAW>0) f.SMALL_ANGLES_25 = 1; else f.SMALL_ANGLES_25 = 0;/ Apply complimentary filter (Gyro drift correction)/ If accel magnitude >1.4G or <0.6G and ACC vector outside of the limit range => we n

27、eutralize the effect of accelerometers in the angle estimation./ To do that, we just skip filter, as EstV already rotated by Gyroif ( ( 36 < accMag && accMag < 196 ) | f.SMALL_ANGLES_25 )for (axis = 0; axis < 3; axis+) int16_t acc = ACC_V ALUE ;EstG.Aaxis = (EstG.Aaxis * GYR_CMPF_FA

28、CTOR + acc) * INV_GYR_CMPF_FACTOR; /以前数据基础 上估计的加速度计数据(就是旋转矩阵运算后的数据)占400份，而本次循环取得的 acc数据只占1份 总和是 401 份除以 401 就得到了历史上的数据和最新数据的融合与滤波。#if MAGfor (axis = 0; axis < 3; axis+)EstM.Aaxis = (EstM.Aaxis * GYR _CMPFM_FACTOR+ MAG_VALUE) *INV _GYR_CMPFM_FACTOR;#endif/ Attitude of the estimated vectorangleROLL

29、 = _atan2(EstG.V.X , EstG.V.Z) ;. / 得到滚转的角度 见后面的依据文件 anglePITCH = _atan2(EstG.V.Y , EstG.V.Z) ;/得到俯仰的角度 见后面的依据文件#if MAG/ Attitude of the cross product vector GxMheading = _atan2( EstG.V.X * EstM.V.Z - EstG.V.Z * EstM.V.X , EstG.V.Z * EstM.V.Y - EstG.V.Y *EstM.V.Z );/得到四轴的定向角(为何这么计算还不知道，有知道的QQXXXXXXX

30、XX )/因磁偏角原因进行修正heading += MAG_DECLINIATION * 10; /add declination heading = heading /10;if ( heading > 180) heading = heading - 360; else if (heading < -180) heading = heading + 360;#endif15、我对main loop（庄程序的分析三、conputeIMU函数以及它调用的估计姿态函数 getEstimatedAttitude(函数的分析。取、留时间用于陀螺仪积分，获得角度，这是个关键点。运行设置自检

31、程序。调用 Mag_getADC();。调用computeIMU ()的理解。取得加速度计的三轴。运行 getEstimatedAttilude()函数的时序ACC_getADC();获得加速度计的 ADC值(时间段*角速度)获取时间段(此时的时间一以前存储时间) 。记下此次时间作为下次需要的存储时间 。获得陀螺仪的三轴值O O OO O OO O O。本循环前次的加速度计值因为已经了运行体角度变化本循环前次的电子罗盘计值因为已经发生了运行体角度变化故使用旋转函数调整rotateV(struct fp_vector *v,float* delta)目的是获取原来数据基础上的一个带有历史记忆的一

32、份推测值加速度获取原来数据基础上的一个带有历史记忆的一份推测值电子罗盘计的。(将带有历史记忆推测出的加速度计值 份3轴加速度值。(将带有历史记忆推测出的罗盘计值 份3轴罗盘计值3轴向量3轴向量*400 +最新加速度值*1)/401严*400 +最新罗盘计值*1)/401进行滤波计算出进行滤波计算出个可认可的一个可认可的一。依据计算出来的可认可的加速度值计算出俯仰角和旋转角。依据滤波计算出来的加速度3轴值和罗盘3轴值计算出航向角。取得陀螺仪的三轴值Gyro_getADC();获得陀螺仪的ADC值下次循环将使用到 。运行其它代码开始循环(取陀螺仪函数是在积分计算时间开始到结束的中间段调用获取值的)

33、四、旋转矩阵求坐标解析（一）、原程序void rotateV(struct fp_vector *v,float* delta) fp_vector v_tmp = *v; /拷贝一个副本到 v_tmp 中。v->z -= deltaROLL * v_tmp.X + deltaPITCH * v_tmp.Y;/v->Z -= deltaROLL * v_tmp.X + deltaPITCH * v_tmp.Y;改写成下行代码/ v->Z = v->Z - deltaROLL * v_tmp.X - deltaPITCH * v_tmp.Y; 利用 v_tmp.Z=V-&

34、gt;Z，代码简写成下行代码/ v->Z = v_tmp.Z- deltaROLL * v_tmp.X - deltaPITCH * v_tmp.Y; 这就是写出矩阵的原代码。同理v->X、v->Y 也这样变换v->X += deltaROLL * v_tmp.Z - deltaYAW * v_tmp.Y;v->Y += deltaPITCH * v_tmp.Z + deltaYAW* v_tmp.X;（二八写成矩阵形式r 、V >XV >YV >ZL J1 yaw roll yaw1 pitch roll pitch1V_temp.XV_tem

35、p.YV_temp.Z实际可以写成r V >XV >YV >ZL Jr1SYS ySY1SBV_temp.XV_temp.YV_temp.Z（三）、分解三种旋转1 .绕X轴旋转0cX2100XIY20C0se¥14Z20-SBCBZb JJ丿¥12 .绕Y轴旋转yZ2广X2C¥0SvY2二010* 1Y Z. . .Z2SY0Cy丿a丿Y1Y2rr =X2-SV0mY2cv0+Y 2Z2001L丿a丿丿（四）、一种旋转 绕Z轴旋转=yaw-绕X轴旋转0二pitch- 绕Y轴旋转丫二roll厂r>r >X1C ¥-S¥

36、;0Xn绕Z轴转动Y 1SYC ¥0* ynZ 1001Z nL丿Lr、rr >X2i00X1绕X轴转动Y 20C 0S0* Y'1 XZ 20S0C 0Z 1>< >r、rr、XbC 丫0SYX2绕Y轴转动Yb010* Y'2Z b-S丫 0C 丫Z 2C JJL Jrollr >Xb-少Y b-少Z bC J机体坐标系（五）、MWC的余旋矩阵的计算过程rX1C 丫0S 丫Xn绕Y轴转动Y1010* YnZ 1-S丫 0C 丫Z nLr rr、X2i00X1绕X轴转动Y 20C 0S0* 、Y 1 Xyaw1Z 20S0C 0Z 1J

37、>r、rr >XbC Y-SY0X2绕Z轴转动Y bSYC Y0* Yr2Z b001Z 2L JJ< JMWC采用的坐标系< >r、r >XbXnY bY0YY r1Z bZnj JL丿机体坐标系Cbn物理坐标系SY C 0C Y C 0S0CY Sy - SYS 0 C 丫SY Sy +C Y S 0 C 丫C 0 C yJ采用小角度近似算法C 1 SQQ （弧度）rCW C Y + SW SQ Sy - SW C Q Cb n = SW C Y - C W SQ SyC W C Q-C Q S y -S QC W Sy-SW S Q C 门1厂-SWs

38、yASW Sy+CWSQ C ySW 1SQC Q C 丫Sy S Q1JJ17（六）、完成该文档采用过的图形X2r irr =X2C W-SW0X1Y 2SWC W0* y1Z 2001Z 1< JJ< J2013- 10- 14五、计算俯仰角度的依据文件基r加速度传感器的倾角测磧21I A1 = W当K岫与木平豪产生捕初毎P0L初速厦绳r j.吓;hIE梅忸懂角的测在机關人.七行需以及.运动 小车的说渥控制丰*有非車甕的作用“対貯 怜统的好角ft感，垄離犒度薦.并不适合应电子制柞甲便用、通过加谨度传感 也龍阿擢计算出翻体囂稱的耳傭。虽SS只證黑 物肚协衣时的*4驚.怛理由于枷榕

39、抵廉在不M 妾乳运功翊诵僭鬻的境合还迪*its大的症用 呦氛拿丈莓仆蚪如何烧伺甲片机氏疑加I逮廈吟 晶褂进的腐，一，帅連度倩島器介招加谨厦诗谯祁可以灘备辅的M邀厦值倉 卡眾的MhgJC 传感 MMA726OQT6 可扳肌三抑賈廈，1 &u»W 倍号無 出拘電注信号”在1 SqIWT,営号灵敏金为 BOOmv/g,底憐対3? 时，抑遗度的理范EH 为-Igflig,対应的电压范割为0.酣丫劃瓷殆鸣 加逮度怡越II的坐特累如圈1所乐q«»理毎連度丼不能氏播春示制体的H鶴，过公式梢換才能碍劉牺曲勺”设x输占水中直的夬掏均憔唧角P¥舗£水翠面的

40、先曲貝愦澹角 下茴以帽押扁为刮，迸行计篦公式的淮畀. 当舸识It传橹般木平敦存序平预上时.U 钢蜡状恋芈标系如瓯2所示“S 2利曲找滋*壽H此时鴨厲期为d各抽上的弊務加建度偵i曜孑圧 37万方it据區* X上総乖坐梅廉1AO :耨坐如糸柚说到KK轴平航，可番如阳4所 乐的平面坐可專.由此可求帯茬持上的H感加Xx =-1sx5jh p A-oj' 勺 士 1即 c»p由上述等式，可以得到情旳猶的计H烧式：P - *11X1331(.4, ' &) 同理可褂迪粛的计MS tq> = anctan( Ax IAt )=.电MHft计曲于 MMA/PGCXaFN Bf S,手工却 張舎比鞍瞄锁困比，«tStR-Urf品欄壊, K® 5W示.UmAMMA72B0的毎小羸址. 入电氐为列克33可迤.冷Y. 2脚分