聯絡方式：[email protected]

一、動機

MPU9250包括三軸加速度計、三軸陀螺儀和三軸磁力計。因為加速度/磁力計具有高頻噪聲（需要低通濾波），將加速度/磁力計的訊號看成是音訊訊號，它們的訊號會有很多“毛刺“，也就是說它們的瞬時值不夠精確，解算出來的姿態會震盪，但長期來看姿態方向是對的。而陀螺儀具有低頻噪聲（需要高通濾波），即每個時刻的得到的角速度是比較精確的，使用積分就能得到旋轉角度（姿態），但是積分會累積誤差，因此積分到後面姿態就不對了，也就是漂移現象。 加速度/磁力計和陀螺儀在頻域上的特性互補，可以融合這三種感測器的資料，提高精度和系統的動態特性。
下圖表示的是把感測器翻轉90°一段時間後又翻轉回來時，加速度/磁力計、陀螺儀以及融合感測訊號，所求得的姿態資訊：

然而對於原始的MPU9250來說，在進行姿態融合前需要對感測器進行校正等“預處理”。

二、步驟

如上圖所示，要對MPU9250進行姿態融合/濾波，大體需要三個步驟：1、校正，2、座標轉換，3、濾波。其中前兩步需要離線執行。下面詳解這三個步驟。

1. 感測器校正

按正常的情況來說，加速度感測器在靜止狀態（即只受重力作用），重力在各個姿態下，在三維空間中，向量頂點會落在一個球面上。但在IMＵ中，x,y,z軸的度量單位很大可能不相同，假設各軸之間相互直，各姿態重力點會落在一個橢球面上，橢球的中心也不一定在原點，這其實就是加速度的偏移量。

在磁力計上，由於測量磁場強度，在環境不變的情況下，感測器每個姿態感受磁場強度是相同的，所以不需要靜止狀態，磁力計測量的x,y,z軸值，在沒有偏差且感測器內部x,y,z軸相互垂直的情況下，在三維空間中組成一個圓球面。但是磁力計存在Hard Iron Distortion和Soft Iron Distortion，使得x,y,z軸度量單位不相同，各軸也並非相互垂直，橢球球心也並非[0,0,0]座標。

總的來說加速度/磁力計校正，需要求出橢球的球心（偏移量）、橢球軸長（各軸單位長度；其實磁力計不需要求，後面會解釋。），即橢球擬合。

加速度計校正

• 在離線資料中尋找靜止狀態下的加速度感測器值，假設靜止時的不同姿態共有n個，則得到資料 accData∈Rn×3；
• accData∈Rn×3 作為樣本資料，使用最小二乘法即可求出橢球方程的各個引數，接著根據我師兄部落格的方法可以求出橢球的球心（偏移量）和軸長。

磁力計校正

• 提取離線資料的磁力計感測器的值（不需要靜止狀態），得到樣本資料 magData∈Rm×3 , 假設有m組資料；
• magData∈Rm×3 作為樣本資料，使用最小二乘法
  即可求出橢球方程的各個引數，進一步求出球心（偏移量）和軸長（軸長可忽略）。

陀螺儀校正

陀螺儀不存在橢球的問題，因此可以直接從離線資料中找到靜止狀態下的資料，對資料求均值即可得到偏移量。

2. 磁力計座標系轉換到加速度座標系

磁力計的基座標系和加速度的基座標系（我們以加速度的基座標系作為世界座標系）並不一定是一樣的，比如加速度的基座標系x軸指向東，磁力計的指向北。因此，需要將磁力計座標系轉換到加速度計座標系，才能通過加速度/磁力計解算出正確的姿態。

假設加速度計測量到的重力加速度向量是 ug，磁力計測量到的磁通量是 vm，首先要將 vm 從橢球上投射到正球上，投射方程為 rm=Avm。rm 是投射後的向量，A 為 3×3 的矩陣。

其次求出 rm 轉換到加速度計座標系的旋轉矩陣 R，得到 um=Rrm。而無論任何情況磁通量 um 和重力加速度 ug 的夾角固定， 即 ug⋅um=|ug|⋅|um|cosθ, 因為我們只關心方向不關心大小，所以定值 |ug|⋅|um|cosθ 可歸一化。綜上，有

ugRAvm=1令 B=RA 有ugBvm=1
橢球轉換正球和旋轉變換都已經包括在矩陣 B 裡了，再次使用最小二乘法求出 B 的各個元素即可（對應前面，不必理會磁力計橢球的軸長）。

磁通量在世界座標系下的x軸分量為0，重力加速度方向固定不變和z軸一致，通過向量點乘可以求出歸一化後磁通量在世界座標系下z軸的分量，再用模長公式求出y軸的分量。最後得到磁通量在世界座標系下的向量 wm，用於濾波時的校正。

3. 姿態融合

在使用各種濾波方法（如高低通濾波，EKF，Mahony）做姿態融合時，可以基於尤拉角、方向餘弦和四元數等方法。尤拉角在求解姿態時存在奇點（萬向節鎖），不能用於全姿態解算；方向餘弦可用於全姿態解算但計算量大，不能滿足實時性要求；四元數，計算量小，無奇點，滿足實時解算。

所以這裡使用的是基於四元數姿態融合，濾波方法包括高低通濾波、EKF、Mahony濾波。下面簡單介紹這三種演算法的大致思想：

高低通濾波
兩個方程說明問題

Quaternion(t)=(1−α)Quaternion(t)^+α⋅accMagQuaternion(t)Quaternion(t)^=Quaternion(t−1)gyroSensorQ(t)

Quaternion 是姿態融合後的四元數，gyroSensorQ 是陀螺儀測量到的角速度所代表的瞬時四元數， Quaternion(t)^ 是使用陀螺儀推算 t 時刻的估計姿態，accMagQuaternion 是加速度/磁力計解算出的四元數。使用 α 加權將陀螺儀的估算和加速度/磁力計的解算融合在一起，達到濾波效果。這裡 α=0.1，一般比較小，因為加速度/磁力計有高頻噪聲，需要低通濾波，否則會抖動。

EKF
廣義卡爾曼濾波，主要有兩步：估算、校正。（公式和推導不詳述）。

• 估算：和高低通濾波一樣，使用陀螺儀估算出 t 時刻四元數；
• 校正：計算出估算姿態下的重力加速度和磁通量向量，再用加速度/磁力計測量到的重力加速度和磁通量向量按照EKF校正方程進行校正。

Mahony濾波
主要是使用了PI控制器思想（未深入，理解有偏差歡迎指出）。同樣也分兩步：估算、校正。

• 估算：和前面一樣，使用陀螺儀估算出 t 時刻四元數；
• 校正：計算出估算姿態下的重力加速度和磁通量向量，再用加速度/磁力計測量到的重力加速度和磁通量向量叉乘估算姿態的計算結果，得到的是估算姿態與測量姿態的誤差 e。

  相關推薦

  IMU姿態融合（MPU9250從校正到濾波步驟）

  聯絡方式：[email protected] 一、動機 MPU9250包括三軸加速度計、三軸陀螺儀和三軸磁力計。因為加速度/磁力計具有高頻噪聲（需要低通濾波），將加速度/磁力計的訊號看成是音訊訊號，它們的訊號會有很多“毛刺“，也就是說它們的瞬時

  談一談 MPU6050 姿態融合（轉）

  nat 不同 四軸飛行器 物體 b- float 例子 只需要 原理 姿態角（Euler角）pitch yaw roll飛行器的姿態角並不是指哪個角度，是三個角度的統稱。它們是：俯仰、滾轉、偏航。你可以想象是飛機圍繞XYZ三個軸分別轉動形成的夾角。地面坐標系（earth-s

  IMU姿態融合演算法

  版權宣告：本文為博主原創文章，未經博主允許不得轉載。 博主：shenshikexmu 聯絡方式：[email protected] 高低通濾波，EKF，Mahony濾波 http://v.youku.com/v_show/id_XMjUxMDM5Mjk0MA=

  使用 matlab 數字影象處理（九）—— 去卷積（deconvolution，逆濾波復原）

  分享一下我老師大神的人工智慧教程！零基礎，通俗易懂！http://blog.csdn.net/jiangjunshow 也歡迎大家轉載本篇文章。分享知識，造福人民，實現我們中華民族偉大復興！        

  深度學習網路篇——ZFNet（Part1 從AlexNet到ZFNet）

  一、上回說到的 AlexNet 請各位看官們參見部落格之前的文章： 深度學習網路篇——AlexNet https://blog.csdn.net/weixin_43624538/article/details/83988998 1）AlexNet效能舉世矚目 top-1和top-5

  洛谷4219 BJOI2014大融合（LCT維護子樹資訊）

  題目連結 QWQ 這個題目是LCT維護子樹資訊的經典應用 根據題目資訊來看，對於一個這條邊的兩個端點各自的 s i

  在 2016 年學 JavaScript 是一種什麼樣的體驗？（React從入門到放棄）

  作者：方應杭連結：https://zhuanlan.zhihu.com/p/22782487來源：知乎著作權歸作者所有。商業轉載請聯絡作者獲得授權，非商業轉載請註明出處。注：原文是英文，本文是我翻譯的。有人把我翻譯的內容原文照抄，放到他自己的專欄，搞得有人問我是不是我抄襲了……請支援我的勞動成果，花了兩個小時

  SpringMvc 請求處理流程（如何從tomcat到SpringMvc）

  HttpServletBean 在init方法中， 首先將Servlet配置的引數使用BeanWrapper設定到DispatcherServlet中， 然後呼叫initServletBean 子類通過這個方法進行初始化 FrameworkServle

  Beam編程系列之Java SDK Quickstart（官網的推薦步驟）

  rate start mark http 單獨 org 托管 pipe bucket 　　不多說，直接上幹貨！ https://beam.apache.org/get-started/beam-overview/

  linux系統--C語言程序開發的基本步驟（包含gcc的基本步驟）

  什麽 linu 第一步 源文件 inux 文件 c語言 決定 擴展 1、使用vi或者vim編寫程序文件 2、使用gcc把所有的源文件翻譯成計算機認識的格式（編譯） 3、使用./a.out作為命令執行得到的可執行文件 gcc編譯器的工作步驟： 1、處理所有的預處理指令 2、把

  機器學習-2（機器學習程式的步驟）

  1 收集資料      這一塊你要的知識是如何獲得資料 如果不是自己的生產環境產生的真實資料，你還需要獲得爬蟲的技能，來從外部獲取資料到自己的分析庫。   2 準備輸入資料     這一塊可以理解為資料轉換，把資料格式轉

  Python全棧工程師（42:Web框架Django步驟）

  1.建立Django工程django-admin startproject 工程名2.建立appcd 工程名python manage.py startapp cmdb3.靜態檔案project.settings.pySTATICFILES_DIRS = (         

  MySQL（1）使用前的準備（安裝版的安裝步驟）

  【安裝版MYSQL，和其他軟體一樣】 注意： 1】選擇安裝型別，有“Typical（預設）”、“Complete（完全）”、“Custom（使用者自定義）”三個選項，我們選擇“Custom”， 2】在“Developer Components（開發者部分）”上左鍵單擊，選擇

  Docker應用部署實錄（包含完善Docker安裝步驟）

  Docker應用部署實錄（包含完善Docker安裝步驟） 前言 首先說一下這篇文章的來源。我之前接手的一個IOT專案，需要安裝多箇中控伺服器。中控伺服器需要安裝RabbitMQ，Mysql，多個服務，這些部署，部署人員不怎麼會（畢竟是硬體兼職的），只能讓我給整一個簡單一些方案。我當時就想到了容器化部署，然後整

  IMU校正以及姿態融合

  版權宣告：本文為博主原創文章，未經博主允許不得轉載。 博主：shenshikexmu 聯絡方式：[email protected] 緣起 有緣在簡極科技兼職兩年。接觸了IMU，我去的時候那家公司還是一個要把IMU放進足球的公司，祝願簡極越來越好。IMU校正演算法大概

  導向濾波小結：從導向濾波（guided filter）到快速導向濾波（fast guide filter）的原理，應用及opencv實現程式碼

  1. 導向濾波簡介導向濾波是何凱明在學生時代提出的一個保邊濾波（edge-preserving smoothing）演算法。何凱明在cv圈應該算是名人了，學生時代關於影象去霧的研究就以第一作者的身份獲得Best Paper Award（CVPR 2009），而且今年剛剛又斬獲

  git代碼倉庫遷移（從github到oschina）【轉】

  mit tail name usr done git blank com detail 轉自：http://blog.csdn.net/a5244491/article/details/44807937 版權聲明：本文為博主原創文章，未經博主允許不得轉載。

  MySQL基礎知識（MySQL從入門到精通觀後感）

  alter mes times 值範圍 model 。。 字符編碼 不同的 精通 17/7/9 1.主從式架構（Client-server model）或客戶端-服務器（Client-Server）結構簡稱C/S結構，是一種網絡架構，通常在該網絡架構下軟件分為客戶端和服務器

  600集Python從入門到精通教程（懂中文就能學會）

  元組 單繼承 查詢 字符串 屬性 linux系統 事件 集成 tar 目錄大綱： 本套教程15天 1-3 天內容為Linux基礎命令 4-13 天內容為Python基礎教程 14-15 天內容為 飛機大戰項目演練 視頻概括： 第一階段（1-3天）：

  跟著我從零開始入門FPGA（一周入門XXOO系列）-1、Verilog語法

  部分 同步 比較 跑馬燈 begin 寫代碼 人的 thread get （本連載共七部分，這是第一部分） 作者：McuPlayer2013 （EETOP FPGA版塊版主） 原帖地址：http://bbs.eetop.cn/thread-385362-1-1.htm