mpu6050姿态解算原理_mpu6050姿态解算程序

　　关于MPU6050姿态解算原理

　　mpu6050常用作提供飞控运行时的姿态测量和计算，在在姿态结算中有几个重要的概念，欧拉角、四元数等。

　　欧拉角：用来表征三维空间中运动物体绕着坐标轴旋转的情况。即物体的每时每秒的姿态可以由欧拉角表出。

　　四元数：超复数，q=（q0，q1，q2，q3），q0位实数，q1，q2，q3为虚部的实数。简单的可以理解为四维空间，就是原有的三维空间加入一个旋转角。而四元数可以表征欧拉角，并且计算方便，故采用四元数来计算。在此还要提到加速度和磁力计补偿原理，可以参照http://blog.csdn.net/nemol1990/article/details/21870197？utm_source=tuicool&utm_medium=referral博客中提到的原理与基本概念。在此再啰嗦一下：补偿的目的是使两个坐标系世界坐标系和刚体坐标系能够完全重合，在此基础上，计算补偿值来修正旋转矩阵，即四元数矩阵。最终的结果是解算出四元数的姿态，就是四元数矩阵的各个元素的值。按照上述博客中的程序解算四元数的时候，用到了Kp和Ki两个参数，两个参数的作用是用来控制矫正刚体坐标系速度的。即调节加速度和磁力计补偿的速度（调节误差的生成速度，进而调节刚体坐标系和世界坐标系的重合度）

　　加速计补偿的理解：

　　由于重力加速度的原因，加速计只能补偿X-Y轴的偏差，即经过解算后的坐标中世界坐标XOY和刚体坐标XOY能够重合，但是无法补偿航向角。

　　在此基础上，利用磁力计进行补偿，因为当地磁场是一个椭圆，所以我们把世界坐标系的X-AIXS轴对准真实世界的北方，这样，磁场园只能在XOZ的平面圆里面，所以磁力计在世界坐标系中的分量是［I，0，k］，即在Y轴没有分量，因为90度的夹角余弦为0.但此时i和k的值并不确定是多少。在刚体坐标系中磁力计的输出为［a，b，c，］，旋转到和世界坐标系中，计算出i和k，然后，在经过旋转把其旋转到刚体坐标系中，然后计算误差修正旋转矩阵，此时得到精确的旋转矩阵。

　　mpu6050姿态解算程序

　　08.MPU6050\Hardware\ioi2c.c

　　08.MPU6050\Hardware\ioi2c.h

　　08.MPU6050\Hardware\mpu6050.c

　　08.MPU6050\Hardware\mpu6050.h

　　08.MPU6050\Hardware\nrf24l01.c

　　08.MPU6050\Hardware\nrf24l01.h

　　08.MPU6050\Hardware\spi.c

　　08.MPU6050\Hardware\spi.h

　　08.MPU6050\Libraries\CMSIS\CMSISENDUSERLICENCEAGREEMENT.pdf

　　08.MPU6050\Libraries\CMSIS\Device\ST\STM32F4xx\Include\stm32f4xx.h

　　08.MPU6050\Libraries\CMSIS\Device\ST\STM32F4xx\Include\system_stm32f4xx.h

　　08.MPU6050\Libraries\CMSIS\Device\ST\STM32F4xx\Source\Templates\arm\startup_stm32f4xx.s

　　08.MPU6050\Libraries\CMSIS\Device\ST\STM32F4xx\Source\Templates\gcc_ride7\startup_stm32f4xx.s

　　08.MPU6050\Libraries\CMSIS\Device\ST\STM32F4xx\Source\Templates\iar\startup_stm32f4xx.s

　　08.MPU6050\Libraries\CMSIS\Device\ST\STM32F4xx\Source\Templates\system_stm32f4xx.c

　　08.MPU6050\Libraries\CMSIS\Device\ST\STM32F4xx\Source\Templates\TASKING\cstart_thumb2.asm

　　08.MPU6050\Libraries\CMSIS\Device\ST\STM32F4xx\Source\Templates\TrueSTUDIO\startup_stm32f4xx.s

　　08.MPU6050\Libraries\CMSIS\Documentation\CMSIS-SVD_Schema_1_0.xsd

　　08.MPU6050\Libraries\CMSIS\Documentation\CMSIS_Logo_Final.jpg

　　08.MPU6050\Libraries\CMSIS\Include\arm_common_tables.h

　　08.MPU6050\Libraries\CMSIS\Include\arm_math.h

　　08.MPU6050\Libraries\CMSIS\Include\core_cm0.h

　　08.MPU6050\Libraries\CMSIS\Include\core_cm3.h

　　08.MPU6050\Libraries\CMSIS\Include\core_cm4.h

　　08.MPU6050\Libraries\CMSIS\Include\core_cm4_simd.h

　　08.MPU6050\Libraries\CMSIS\Include\core_cmFunc.h

　　08.MPU6050\Libraries\CMSIS\Include\core_cmInstr.h

　　08.MPU6050\Libraries\CMSIS\README.txt

　　08.MPU6050\Libraries\STM32F4xx_StdPeriph_Driver\inc\misc.h

　　08.MPU6050\Libraries\STM32F4xx_StdPeriph_Driver\inc\stm32f4xx_adc.h

　　08.MPU6050\Libraries\STM32F4xx_StdPeriph_Driver\inc\stm32f4xx_can.h

　　08.MPU6050\Libraries\STM32F4xx_StdPeriph_Driver\inc\stm32f4xx_crc.h

　　08.MPU6050\Libraries\STM32F4xx_StdPeriph_Driver\inc\stm32f4xx_cryp.h

　　08.MPU6050\Libraries\STM32F4xx_StdPeriph_Driver\inc\stm32f4xx_dac.h

　　08.MPU6050\Libraries\STM32F4xx_StdPeriph_Driver\inc\stm32f4xx_dbgmcu.h

　　08.MPU6050\Libraries\STM32F4xx_StdPeriph_Driver\inc\stm32f4xx_dcmi.h

　　08.MPU6050\Libraries\STM32F4xx_StdPeriph_Driver\inc\stm32f4xx_dma.h

　　08.MPU6050\Libraries\STM32F4xx_StdPeriph_Driver\inc\stm32f4xx_exti.h

　　08.MPU6050\Libraries\STM32F4xx_StdPeriph_Driver\inc\stm32f4xx_flash.h

　　08.MPU6050\Libraries\STM32F4xx_StdPeriph_Driver\inc\stm32f4xx_fsmc.h

　　08.MPU6050\Libraries\STM32F4xx_StdPeriph_Driver\inc\stm32f4xx_gpio.h

　　08.MPU6050\Libraries\STM32F4xx_StdPeriph_Driver\inc\stm32f4xx_hash.h

　　08.MPU6050\Libraries\STM32F4xx_StdPeriph_Driver\inc\stm32f4xx_i2c.h

　　08.MPU6050\Libraries\STM32F4xx_StdPeriph_Driver\inc\stm32f4xx_iwdg.h

　　08.MPU6050\Libraries\STM32F4xx_StdPeriph_Driver\inc\stm32f4xx_pwr.h

　　08.MPU6050\Libraries\STM32F4xx_StdPeriph_Driver\inc\stm32f4xx_rcc.h

　　08.MPU6050\Libraries\STM32F4xx_StdPeriph_Driver\inc\stm32f4xx_rng.h

　　08.MPU6050\Libraries\STM32F4xx_StdPeriph_Driver\inc\stm32f4xx_rtc.h

　　08.MPU6050\Libraries\STM32F4xx_StdPeriph_Driver\inc\stm32f4xx_sdio.h

　　08.MPU6050\Libraries\STM32F4xx_StdPeriph_Driver\inc\stm32f4xx_spi.h

　　08.MPU6050\Libraries\STM32F4xx_StdPeriph_Driver\inc\stm32f4xx_syscfg.h

　　08.MPU6050\Libraries\STM32F4xx_StdPeriph_Driver\inc\stm32f4xx_tim.h

　　08.MPU6050\Libraries\STM32F4xx_StdPeriph_Driver\inc\stm32f4xx_usart.h

　　08.MPU6050\Libraries\STM32F4xx_StdPeriph_Driver\inc\stm32f4xx_wwdg.h

　　08.MPU6050\Libraries\STM32F4xx_StdPeriph_Driver\src\misc.c

　　08.MPU6050\Libraries\STM32F4xx_StdPeriph_Driver\src\stm32f4xx_adc.c

　　08.MPU6050\Libraries\STM32F4xx_StdPeriph_Driver\src\stm32f4xx_can.c

　　08.MPU6050\Libraries\STM32F4xx_StdPeriph_Driver\src\stm32f4xx_crc.c

　　08.MPU6050\Libraries\STM32F4xx_StdPeriph_Driver\src\stm32f4xx_cryp.c

　　08.MPU6050\Libraries\STM32F4xx_StdPeriph_Driver\src\stm32f4xx_cryp_aes.c

　　08.MPU6050\Libraries\STM32F4xx_StdPeriph_Driver\src\stm32f4xx_cryp_des.c

　　08.MPU6050\Libraries\STM32F4xx_StdPeriph_Driver\src\stm32f4xx_cryp_tdes.c

　　08.MPU6050\Libraries\STM32F4xx_StdPeriph_Driver\src\stm32f4xx_dac.c

　　08.MPU6050\Libraries\STM32F4xx_StdPeriph_Driver\src\stm32f4xx_dbgmcu.c

　　08.MPU6050\Libraries\STM32F4xx_StdPeriph_Driver\src\stm32f4xx_dcmi.c

　　08.MPU6050\Libraries\STM32F4xx_StdPeriph_Driver\src\stm32f4xx_dma.c

　　08.MPU6050\Libraries\STM32F4xx_StdPeriph_Driver\src\stm32f4xx_exti.c

　　08.MPU6050\Libraries\STM32F4xx_StdPeriph_Driver\src\stm32f4xx_flash.c

　　08.MPU6050\Libraries\STM32F4xx_StdPeriph_Driver\src\stm32f4xx_fsmc.c

　　08.MPU6050\Libraries\STM32F4xx_StdPeriph_Driver\src\stm32f4xx_gpio.c

　　08.MPU6050\Libraries\STM32F4xx_StdPeriph_Driver\src\stm32f4xx_hash.c

　　08.MPU6050\Libraries\STM32F4xx_StdPeriph_Driver\src\stm32f4xx_hash_md5.c

　　08.MPU6050\Libraries\STM32F4xx_StdPeriph_Driver\src\stm32f4xx_hash_sha1.c

　　08.MPU6050\Libraries\STM32F4xx_StdPeriph_Driver\src\stm32f4xx_i2c.c

　　08.MPU6050\Libraries\STM32F4xx_StdPeriph_Driver\src\stm32f4xx_iwdg.c

　　08.MPU6050\Libraries\STM32F4xx_StdPeriph_Driver\src\stm32f4xx_pwr.c

　　08.MPU6050\Libraries\STM32F4xx_StdPeriph_Driver\src\stm32f4xx_rcc.c

　　08.MPU6050\Libraries\STM32F4xx_StdPeriph_Driver\src\stm32f4xx_rng.c

　　08.MPU6050\Libraries\STM32F4xx_StdPeriph_Driver\src\stm32f4xx_rtc.c

　　08.MPU6050\Libraries\STM32F4xx_StdPeriph_Driver\src\stm32f4xx_sdio.c

　　08.MPU6050\Libraries\STM32F4xx_StdPeriph_Driver\src\stm32f4xx_spi.c

　　08.MPU6050\Libraries\STM32F4xx_StdPeriph_Driver\src\stm32f4xx_syscfg.c

　　08.MPU6050\Libraries\STM32F4xx_StdPeriph_Driver\src\stm32f4xx_tim.c

　　08.MPU6050\Libraries\STM32F4xx_StdPeriph_Driver\src\stm32f4xx_usart.c

　　08.MPU6050\Libraries\STM32F4xx_StdPeriph_Driver\src\stm32f4xx_wwdg.c

　　08.MPU6050\Output\Obj\STM32F4-TEST.hex

　　08.MPU6050\Project\JLinkSettings.ini

　　08.MPU6050\Project\STM32F4-TEST.uvgui.XiaoWei

　　08.MPU6050\Project\STM32F4-TEST.uvopt

　　08.MPU6050\Project\STM32F4-TEST.uvproj

　　08.MPU6050\System\delay.c

　　08.MPU6050\System\delay.h

　　08.MPU6050\System\led.c

　　08.MPU6050\System\led.h

　　08.MPU6050\System\sys.c

　　08.MPU6050\System\sys.h

　　08.MPU6050\System\timer2.c

　　08.MPU6050\System\timer2.h

　　08.MPU6050\System\uart.c

　　08.MPU6050\System\uart.h