EEPW论坛

姿态解算

姿态解算，网上用得最多的就是先求四元数，再转化成角度了。资料显示，相对于另两种旋转表示法（矩阵和欧拉角），四元数具有某些方面的优势，如速度更快、提供平滑插值、有效避免万向锁问题、存储空间较小等等 。

下面列出几个常用的算法，都是用C语言的格式。

1、让四轴飞的匿名四轴版

*************************************************

//默认参数 10 0.008 #define Kp 10.0f // proportional gain governs rate of convergence to accelerometer/magnetometer #define Ki 0.008f // integral gain governs rate of convergence of gyroscope biases #define halfT 0.001f // half the sample period采样周期的一半 float q0 = 1, q1 = 0, q2 = 0, q3 = 0; // quaternion elements representing the estimated orientation float exInt = 0, eyInt = 0, ezInt = 0; // scaled integral error void IMUupdate(float gx, float gy, float gz, float ax, float ay, float az) { float norm; // float hx, hy, hz, bx, bz; float vx, vy, vz;// wx, wy, wz; float ex, ey, ez; // 先把这些用得到的值算好 float q0q0 = q0*q0; float q0q1 = q0*q1; float q0q2 = q0*q2; // float q0q3 = q0*q3; float q1q1 = q1*q1; // float q1q2 = q1*q2; float q1q3 = q1*q3; float q2q2 = q2*q2; float q2q3 = q2*q3; float q3q3 = q3*q3; if(ax*ay*az==0) return; norm = sqrt(ax*ax + ay*ay + az*az); //acc数据归一化 ax = ax / norm; ay = ay / norm; az = az / norm; // estimated direction of gravity and flux (v and w) 估计重力方向和流量/变迁 vx = 2*(q1q3 - q0q2); //四元素中xyz的表示 vy = 2*(q0q1 + q2q3); vz = q0q0 - q1q1 - q2q2 + q3q3 ; // error is sum of cross product between reference direction of fields and direction measured by sensors ex = (ay*vz - az*vy) ; //向量外积在相减得到差分就是误差 ey = (az*vx - ax*vz) ; ez = (ax*vy - ay*vx) ; exInt = exInt + ex * Ki; //对误差进行积分 eyInt = eyInt + ey * Ki; ezInt = ezInt + ez * Ki; // adjusted gyroscope measurements gx = gx + Kp*ex + exInt; //将误差PI后补偿到陀螺仪，即补偿零点漂移 gy = gy + Kp*ey + eyInt; gz = gz + Kp*ez + ezInt; //这里的gz由于没有观测者进行矫正会产生漂移，表现出来的就是积分自增或自减 // integrate quaternion rate and normalise //四元素的微分方程 q0 = q0 + (-q1*gx - q2*gy - q3*gz)*halfT; q1 = q1 + (q0*gx + q2*gz - q3*gy)*halfT; q2 = q2 + (q0*gy - q1*gz + q3*gx)*halfT; q3 = q3 + (q0*gz + q1*gy - q2*gx)*halfT; // normalise quaternion norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3); q0 = q0 / norm; q1 = q1 / norm; q2 = q2 / norm; q3 = q3 / norm; Q_ANGLE.Z = GYRO_I.Z;//atan2(2 * q1 * q2 + 2 * q0 * q3, -2 * q2*q2 - 2 * q3* q3 + 1)* 57.3; // yaw Q_ANGLE.Y = asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3; // pitch Q_ANGLE.X = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3; // roll }

*************************************************

下面两个算法为crazyflie使用的算法

变量定义如下：

//#define MADWICK_QUATERNION_IMU #ifdef MADWICK_QUATERNION_IMU #define BETA_DEF 0.01f // 2 * proportional gain #else // MAHONY_QUATERNION_IMU #define TWO_KP_DEF (2.0f * 0.4f) // 2 * proportional gain #define TWO_KI_DEF (2.0f * 0.001f) // 2 * integral gain #endif #ifdef MADWICK_QUATERNION_IMU float beta = BETA_DEF; // 2 * proportional gain (Kp) #else // MAHONY_QUATERNION_IMU float twoKp = TWO_KP_DEF; // 2 * proportional gain (Kp) float twoKi = TWO_KI_DEF; // 2 * integral gain (Ki) float integralFBx = 0.0f; float integralFBy = 0.0f; float integralFBz = 0.0f; // integral error terms scaled by Ki #endif float q0 = 1.0f; float q1 = 0.0f; float q2 = 0.0f; float q3 = 0.0f; // quaternion of sensor frame relative to auxiliary frame

*************************************************

2、crazyflie的Madgwick's IMU and AHRS algorithms

*************************************************

// Implementation of Madgwick's IMU and AHRS algorithms. // See: http://www.x-io.co.uk/node/8#open_source_ahrs_and_imu_algorithms // // Date Author Notes // 29/09/2011 SOH Madgwick Initial release // 02/10/2011 SOH Madgwick Optimised for reduced CPU load void sensfusion6UpdateQ(float gx, float gy, float gz, float ax, float ay, float az, float dt) { float recipNorm; float s0, s1, s2, s3; float qDot1, qDot2, qDot3, qDot4; float _2q0, _2q1, _2q2, _2q3, _4q0, _4q1, _4q2 ,_8q1, _8q2, q0q0, q1q1, q2q2, q3q3; // Rate of change of quaternion from gyroscope qDot1 = 0.5f * (-q1 * gx - q2 * gy - q3 * gz); qDot2 = 0.5f * (q0 * gx + q2 * gz - q3 * gy); qDot3 = 0.5f * (q0 * gy - q1 * gz + q3 * gx); qDot4 = 0.5f * (q0 * gz + q1 * gy - q2 * gx); // Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation) if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) { // Normalise accelerometer measurement recipNorm = invSqrt(ax * ax + ay * ay + az * az); ax *= recipNorm; ay *= recipNorm; az *= recipNorm; // Auxiliary variables to avoid repeated arithmetic _2q0 = 2.0f * q0; _2q1 = 2.0f * q1; _2q2 = 2.0f * q2; _2q3 = 2.0f * q3; _4q0 = 4.0f * q0; _4q1 = 4.0f * q1; _4q2 = 4.0f * q2; _8q1 = 8.0f * q1; _8q2 = 8.0f * q2; q0q0 = q0 * q0; q1q1 = q1 * q1; q2q2 = q2 * q2; q3q3 = q3 * q3; // Gradient decent algorithm corrective step s0 = _4q0 * q2q2 + _2q2 * ax + _4q0 * q1q1 - _2q1 * ay; s1 = _4q1 * q3q3 - _2q3 * ax + 4.0f * q0q0 * q1 - _2q0 * ay - _4q1 + _8q1 * q1q1 + _8q1 * q2q2 + _4q1 * az; s2 = 4.0f * q0q0 * q2 + _2q0 * ax + _4q2 * q3q3 - _2q3 * ay - _4q2 + _8q2 * q1q1 + _8q2 * q2q2 + _4q2 * az; s3 = 4.0f * q1q1 * q3 - _2q1 * ax + 4.0f * q2q2 * q3 - _2q2 * ay; recipNorm = invSqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3); // normalise step magnitude s0 *= recipNorm; s1 *= recipNorm; s2 *= recipNorm; s3 *= recipNorm; // Apply feedback step qDot1 -= beta * s0; qDot2 -= beta * s1; qDot3 -= beta * s2; qDot4 -= beta * s3; } // Integrate rate of change of quaternion to yield quaternion q0 += qDot1 * dt; q1 += qDot2 * dt; q2 += qDot3 * dt; q3 += qDot4 * dt; // Normalise quaternion recipNorm = invSqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3); q0 *= recipNorm; q1 *= recipNorm; q2 *= recipNorm; q3 *= recipNorm; }

*************************************************

3、crazyflie的Mayhony's AHRS algorithm

*************************************************

// Madgwick's implementation of Mayhony's AHRS algorithm. // See: http://www.x-io.co.uk/node/8#open_source_ahrs_and_imu_algorithms // // Date Author Notes // 29/09/2011 SOH Madgwick Initial release // 02/10/2011 SOH Madgwick Optimised for reduced CPU load void sensfusion6UpdateQ(float gx, float gy, float gz, float ax, float ay, float az, float dt) { float recipNorm; float halfvx, halfvy, halfvz; float halfex, halfey, halfez; float qa, qb, qc; gx = gx * M_PI / 180; gy = gy * M_PI / 180; gz = gz * M_PI / 180; // Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation) if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) { // Normalise accelerometer measurement recipNorm = invSqrt(ax * ax + ay * ay + az * az); ax *= recipNorm; ay *= recipNorm; az *= recipNorm; // Estimated direction of gravity and vector perpendicular to magnetic flux halfvx = q1 * q3 - q0 * q2; halfvy = q0 * q1 + q2 * q3; halfvz = q0 * q0 - 0.5f + q3 * q3; // Error is sum of cross product between estimated and measured direction of gravity halfex = (ay * halfvz - az * halfvy); halfey = (az * halfvx - ax * halfvz); halfez = (ax * halfvy - ay * halfvx); // Compute and apply integral feedback if enabled if(twoKi > 0.0f) { integralFBx += twoKi * halfex * dt; // integral error scaled by Ki integralFBy += twoKi * halfey * dt; integralFBz += twoKi * halfez * dt; gx += integralFBx; // apply integral feedback gy += integralFBy; gz += integralFBz; } else { integralFBx = 0.0f; // prevent integral windup integralFBy = 0.0f; integralFBz = 0.0f; } // Apply proportional feedback gx += twoKp * halfex; gy += twoKp * halfey; gz += twoKp * halfez; } // Integrate rate of change of quaternion gx *= (0.5f * dt); // pre-multiply common factors gy *= (0.5f * dt); gz *= (0.5f * dt); qa = q0; qb = q1; qc = q2; q0 += (-qb * gx - qc * gy - q3 * gz); q1 += (qa * gx + qc * gz - q3 * gy); q2 += (qa * gy - qb * gz + q3 * gx); q3 += (qa * gz + qb * gy - qc * gx); // Normalise quaternion recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3); q0 *= recipNorm; q1 *= recipNorm; q2 *= recipNorm; q3 *= recipNorm; }

*************************************************

匿名的程序在更新四元数的同时更新了姿态角，crazyflie使用单独的函数获得姿态角，同时使用了一个快速求平方根的逆的函数invSqrt.代码如下：

void sensfusion6GetEulerRPY(float* roll, float* pitch, float* yaw) { float gx, gy, gz; // estimated gravity direction gx = 2 * (q1*q3 - q0*q2); gy = 2 * (q0*q1 + q2*q3); gz = q0*q0 - q1*q1 - q2*q2 + q3*q3; *yaw = atan2(2*q1*q2 - 2*q0*q3, 2*q0*q0 + 2*q1*q1 - 1) * 180 / M_PI; *pitch = atan(gx / sqrt(gy*gy + gz*gz)) * 180 / M_PI; *roll = atan(gy / sqrt(gx*gx + gz*gz)) * 180 / M_PI; } //--------------------------------------------------------------------------------------------------- // Fast inverse square-root // See: http://en.wikipedia.org/wiki/Fast_inverse_square_root float invSqrt(float x) { float halfx = 0.5f * x; float y = x; long i = *(long*)&y; i = 0x5f3759df - (i>>1); y = *(float*)&i; y = y * (1.5f - (halfx * y * y)); return y; }

*************************************************

本帖就贴些代码了，下一帖再分析代码。

楼主的帖子就是教程类的帖子典范啊~~

先送上10个积分 以资鼓励

今天总算有点进展了，以前总觉得定时不准，终于查出原因了。在原来的main()函数中(可在《PWM电机驱动》章看到)，在配置了外部系统时钟后，又重新调用了sysInit()函数，该函数仍然是配置时钟，这样，开始配置的时钟不再有效，贴源码：

/* Personal configs */ #include "FreeRTOSConfig.h" /* FreeRtos includes */ #include "FreeRTOS.h" #include "task.h" /* Project includes */ #include "config.h" #include "system.h" #include "nvic.h" #include "usec_time.h" #include "led.h" #include "uart.h" #include "motors.h" #include "adc.h" #include "imu.h" /* ST includes */ #include "stm32f10x.h" /* Private functions */ static void prvClockInit(void); static void prvSetupHardware( void ); int main() { //Low level init: Clock and Interrupt controller prvSetupHardware(); nvicInit(); uartInit(); xTaskCreate(vUartSendTask,(const signed char *)"UartSend", configMINIMAL_STACK_SIZE, NULL, tskIDLE_PRIORITY+3, NULL); ledInit(); xTaskCreate(vLEDTask,(const signed char *)"LED", configMINIMAL_STACK_SIZE, NULL, tskIDLE_PRIORITY+3, NULL ); motorsInit(); // xTaskCreate(motorsTestTask,(const signed char *)"MotorTest",configMINIMAL_STACK_SIZE,NULL,tskIDLE_PRIORITY+3,NULL); adcInit(); // imu6Test(); //Launch the system task that will initialize and start everything // systemLaunch(); // xTaskCreate(vImu6ReadTask,(const signed char *)"Imu6Read", configMINIMAL_STACK_SIZE, NULL, tskIDLE_PRIORITY+3, NULL); //Start the FreeRTOS scheduler vTaskStartScheduler(); //Should never reach this point! while(1); // return 0; } static void prvSetupHardware( void ) { SystemInit(); prvClockInit(); } //Clock configuration static void prvClockInit(void) { ErrorStatus HSEStartUpStatus; RCC_DeInit(); /* Enable HSE */ RCC_HSEConfig(RCC_HSE_ON); /* Wait till HSE is ready */ HSEStartUpStatus = RCC_WaitForHSEStartUp(); if (HSEStartUpStatus == SUCCESS) { /* Enable Prefetch Buffer */ FLASH_PrefetchBufferCmd(FLASH_PrefetchBuffer_Enable); /* Flash 2 wait state */ FLASH_SetLatency(FLASH_Latency_2); /* HCLK = SYSCLK */ RCC_HCLKConfig(RCC_SYSCLK_Div1); /* PCLK2 = HCLK */ RCC_PCLK2Config(RCC_HCLK_Div1); /* PCLK1 = HCLK/2 */ RCC_PCLK1Config(RCC_HCLK_Div2); /* ADCCLK = PCLK2/6 = 72 / 6 = 12 MHz*/ RCC_ADCCLKConfig(RCC_PCLK2_Div6); /* PLLCLK = 16MHz/2 * 9 = 72 MHz */ RCC_PLLConfig(RCC_PLLSource_HSE_Div2, RCC_PLLMul_9); /* Enable PLL */ RCC_PLLCmd(ENABLE); /* Wait till PLL is ready */ while (RCC_GetFlagStatus(RCC_FLAG_PLLRDY) == RESET); /* Select PLL as system clock source */ RCC_SYSCLKConfig(RCC_SYSCLKSource_PLLCLK); /* Wait till PLL is used as system clock source */ while(RCC_GetSYSCLKSource() != 0x08); } else { GPIO_InitTypeDef GPIO_InitStructure; //Cannot start the main oscillator: LED of death... GPIO_InitStructure.GPIO_Pin = LED2_GPIO; GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP; GPIO_InitStructure.GPIO_Speed = GPIO_Speed_10MHz; GPIO_Init(LED2_GPIO_PORT, &GPIO_InitStructure); GPIO_InitStructure.GPIO_Pin = LED3_GPIO; GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP; GPIO_InitStructure.GPIO_Speed = GPIO_Speed_10MHz; GPIO_Init(LED3_GPIO_PORT, &GPIO_InitStructure); GPIO_ResetBits(LED2_GPIO_PORT, LED2); GPIO_ResetBits(LED3_GPIO_PORT, LED3); //Cannot start xtal oscillator! while(1); } }

运行后调用原始函数vTaskDelay()后延时准确了。

重新修改了ADC的程序模块，沿用了crazyflie的程序，修改情况见《ADC电压监测》

前段时间忙了一阵，最近工作不太忙，继续试验。今天调通了MPU6050的程序，但还是不太满意。原来一直想用硬件I2C来实现的，但是一直没成功，今天就退而求其次，用了软件模拟，幸好还算成功。有图有真相：

工程下载地址放到百度网盘了，分享一下：

http://pan.baidu.com/s/1EklYI

不知道是不是违规，违规就请斑竹删了吧。

应该编译之后就能用了吧，Keil 5.10的开发环境，st-link下载器，有兴趣的可以试试。