關(guān)于MPU6050姿態(tài)解算原理

mpu6050常用作提供飛控運(yùn)行時(shí)的姿態(tài)測(cè)量和計(jì)算，在在姿態(tài)結(jié)算中有幾個(gè)重要的概念，歐拉角、四元數(shù)等。

歐拉角：用來(lái)表征三維空間中運(yùn)動(dòng)物體繞著坐標(biāo)軸旋轉(zhuǎn)的情況。即物體的每時(shí)每秒的姿態(tài)可以由歐拉角表出。

四元數(shù)：超復(fù)數(shù)，q=（q0，q1，q2，q3），q0位實(shí)數(shù)，q1，q2，q3為虛部的實(shí)數(shù)。簡(jiǎn)單的可以理解為四維空間，就是原有的三維空間加入一個(gè)旋轉(zhuǎn)角。而四元數(shù)可以表征歐拉角，并且計(jì)算方便，故采用四元數(shù)來(lái)計(jì)算。在此還要提到加速度和磁力計(jì)補(bǔ)償原理，可以參照http://blog.csdn.net/nemol1990/article/details/21870197？utm_source=tuicool&utm_medium=referral博客中提到的原理與基本概念。在此再啰嗦一下：補(bǔ)償?shù)哪康氖鞘箖蓚€(gè)坐標(biāo)系世界坐標(biāo)系和剛體坐標(biāo)系能夠完全重合，在此基礎(chǔ)上，計(jì)算補(bǔ)償值來(lái)修正旋轉(zhuǎn)矩陣，即四元數(shù)矩陣。最終的結(jié)果是解算出四元數(shù)的姿態(tài)，就是四元數(shù)矩陣的各個(gè)元素的值。按照上述博客中的程序解算四元數(shù)的時(shí)候，用到了Kp和Ki兩個(gè)參數(shù)，兩個(gè)參數(shù)的作用是用來(lái)控制矯正剛體坐標(biāo)系速度的。即調(diào)節(jié)加速度和磁力計(jì)補(bǔ)償?shù)乃俣龋ㄕ{(diào)節(jié)誤差的生成速度，進(jìn)而調(diào)節(jié)剛體坐標(biāo)系和世界坐標(biāo)系的重合度）

加速計(jì)補(bǔ)償?shù)睦斫猓?/p>

由于重力加速度的原因，加速計(jì)只能補(bǔ)償X-Y軸的偏差，即經(jīng)過(guò)解算后的坐標(biāo)中世界坐標(biāo)XOY和剛體坐標(biāo)XOY能夠重合，但是無(wú)法補(bǔ)償航向角。

在此基礎(chǔ)上，利用磁力計(jì)進(jìn)行補(bǔ)償，因?yàn)楫?dāng)?shù)卮艌?chǎng)是一個(gè)橢圓，所以我們把世界坐標(biāo)系的X-AIXS軸對(duì)準(zhǔn)真實(shí)世界的北方，這樣，磁場(chǎng)園只能在XOZ的平面圓里面，所以磁力計(jì)在世界坐標(biāo)系中的分量是［I，0，k］，即在Y軸沒(méi)有分量，因?yàn)?0度的夾角余弦為0.但此時(shí)i和k的值并不確定是多少。在剛體坐標(biāo)系中磁力計(jì)的輸出為［a，b，c，］，旋轉(zhuǎn)到和世界坐標(biāo)系中，計(jì)算出i和k，然后，在經(jīng)過(guò)旋轉(zhuǎn)把其旋轉(zhuǎn)到剛體坐標(biāo)系中，然后計(jì)算誤差修正旋轉(zhuǎn)矩陣，此時(shí)得到精確的旋轉(zhuǎn)矩陣。

mpu6050姿態(tài)解算程序

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