根據(jù)自己對慣性導(dǎo)航姿態(tài)解算的學(xué)習(xí)，簡單總結(jié)一下自己對慣導(dǎo)解算認識。先畫一張從傳感器數(shù)據(jù)到最終位姿的過程梳理圖。

01平臺式慣性導(dǎo)航系統(tǒng)和捷聯(lián)慣性導(dǎo)航系統(tǒng)

平臺式慣導(dǎo)的系統(tǒng)主體是平臺，平臺始終跟蹤當?shù)氐乃矫?，軸向始終指向東北天方向。利用位于穩(wěn)定平臺上的加速度計測量載體在導(dǎo)航坐標系下的xyz軸的加速度，積分得到位移速度和位置。 缺點：其體積和重量大，而平臺也是一個高精度、復(fù)雜結(jié)構(gòu)的機電控制系統(tǒng)。

捷聯(lián)式慣性導(dǎo)航系統(tǒng) (Strapdown inertial navigation system) Strapdown 具有“捆綁”的意思， 陀螺儀、加速度計與載體“捆綁”。載體的姿態(tài)和航向可以用載體坐標系（b系）相對于導(dǎo)航坐標系（n系）的三個轉(zhuǎn)動角決定。就是說，在捷聯(lián)式慣性導(dǎo)航系統(tǒng)中，直接測得的三個方向的加速度是載體坐標系三個軸方向上的，需要用一個捷聯(lián)矩陣C將其變換到導(dǎo)航坐標系中來。

02坐標系

運載體中三維空間運動包含六個自由度，既有角運動也有線運動。在地球表面附近，運載體的角運動描述一般以當?shù)厮矫婧偷乩肀毕驗閰⒖蓟鶞剩痪€運動的描述通常采用地理經(jīng)度、緯度和高度表示，它是相對于整個地球旋轉(zhuǎn)橢球體而言。

但是，運載體上慣導(dǎo)系統(tǒng)使用的關(guān)系傳感器(陀螺儀和加速度計)從原理上看是相對于慣性空間進行測量的。另外，地球繞其自轉(zhuǎn)軸相對于慣性空間以常值角速率旋轉(zhuǎn)，運載體在地球表面位置變化會引起其相對于地心的角速率。

03

慣性傳感器

以 MEMS 慣性傳感器為例，通過“感觸”地球引力場和自旋角速度（加速度計和陀螺儀）通過數(shù)學(xué)算法（積分）獲得角速度、加速度、速度、位置等基礎(chǔ)參數(shù)。 因為傳感器測量的誤差存在，在積分運算時，勢必將測量誤差引入了積分，隨著時間的累積，積分誤差也一直在累加。因此在整個姿態(tài)解算前，對誤差建模分析與補償尤為重要。

誤差來源

對MEMS 慣性傳感器來說，引起誤差的主要原因有： 1） 慣性器件本身結(jié)構(gòu)的不完善和工藝差異 2） 慣性器件內(nèi)部物理因素變化如溫度、雜散磁場等 3） 外界環(huán)境如溫度變化、外界磁場干擾、載體劇烈變化等

誤差模型

根據(jù)不同的(確定性和隨機)誤差參數(shù)分析，推導(dǎo)出誤差模型（數(shù)學(xué)公式）。

對確定性誤差參數(shù)是可辨識的（這種辨識是指通過一定的標定方法可以補償，基本消除的誤差）。常用六面法借助于轉(zhuǎn)臺標定加速度和陀螺儀。

不需要轉(zhuǎn)臺的標定常用 迭代優(yōu)化方法。`參考文獻：D. Tedaldi, A. Pretto and E. Menegatti, “A Robust and Easy to Implement Method for IMU Calibration without External Equipments”. In: Proceedings of the IEEE International Conference on Robotics andAutomation (ICRA 2014), May 31 - June 7, 2014 Hong Kong, China, Page(s): 3042 - 3049`

對隨機誤差常用 Allan方差分析

04傳感器輸出的原始單位數(shù)據(jù)直接輸出角度3軸加速度計寄存器讀取到的十六進數(shù)據(jù) 和 選擇的測量范圍決定加速度數(shù)據(jù)的測量精度。Eg:加速度計讀到的數(shù)字是1000，那么對應(yīng)的加速度數(shù)據(jù)是1000/2048=0.49g 因此，利用加速度計測量重力向量在感應(yīng)軸上的映射。被測加速度的振幅隨感應(yīng)軸與水平面的夾角α的正弦值的變化而變化，可以推算出三軸的傾斜角度。

利用三角函數(shù)計算俯仰和滾轉(zhuǎn)角

特別要注意：加速度計繞 Z 軸旋轉(zhuǎn)時，各軸的重力分量不會變化。因此,不用推算 yaw 角解算。一般采用磁力計推算。

3軸陀螺儀從寄存器讀取到的十六進數(shù)據(jù) 和 選擇的測量范圍決定陀螺數(shù)據(jù)的測量精度。Eg:當讀取陀螺儀的值是1000，對應(yīng)的角速度計算如下：32767/2000 =1000/x; 即 x = 1000/16.4（°/s） 因此，利用陀螺儀直接輸出角度時，將需要把角度 °/s 換算成弧度。即：1°/s = π/180 = 57.3rad/s。對3個軸轉(zhuǎn)動的角速度積分，可以得到角度。

數(shù)據(jù)融合

通過上述方法直接利用加速度計和陀螺儀是可以輸出角度的。但是往往在具體的應(yīng)用中加速度計和陀螺儀都表現(xiàn)出不同的特性。

由于加速度易受高頻噪聲干擾，因此算得的姿態(tài)角是很不穩(wěn)定的，不能直接參與控制。陀螺儀對振動不敏感，不產(chǎn)生高頻噪聲，但陀螺儀有零偏、積分出的姿態(tài)會飄移。在應(yīng)用中加速度計對高頻振動噪聲比較敏感，陀螺儀對低頻噪聲敏感。

如上對比，因為加速度和陀螺儀都存在各自的不足之處，因此我們需要引入數(shù)據(jù)融合（濾波算法）。 常見的濾波算法：DMP直接輸出角度 > 卡爾曼濾波 > 四元數(shù)解算 > 清華角度濾波 > 一階互補濾波 在項目中常用：EKF、DCM/Mahoney、Madgwick（基于梯度下降法）等算法

`參考文獻：Madgwick S O H, Harrison A J L, Vaidyanathan R. Estimation of IMU and MARG orientation using a gradient descent algorithm[C]//Rehabilitation Robotics (ICORR), 2011 IEEE International Conference on. IEEE, 2011: 1-7.`

位姿表達

在 AHRS 系統(tǒng)下描述的是載體坐標系的軸系相對于導(dǎo)航坐標系的軸系之間的角度關(guān)系（方向）。通常用歐拉角法或者四元數(shù)法表達。常用的方法還有：方向余弦矩陣法、等效旋轉(zhuǎn)矢量法。 在采用歐拉角時需要考慮的問題是：萬向節(jié)死鎖 不同的旋轉(zhuǎn)規(guī)則下死鎖的情況不一樣，在Z Y X 的順規(guī)下，當pitch角度為90或-90的時候，roll和yaw角度會亂飄，這就是遇到了萬向節(jié)的死鎖。

因此，很多做方案的公司都是用四元數(shù)表征傳感器的姿態(tài)。在這個地方 yaw 角是通過地磁傳感器解算的。地磁傳感器測量的是自身和磁北的角度。北東地坐標系和磁北不是同一個方向。 單位四元數(shù)（Unit quaternion）可以用于表示三維空間里的旋轉(zhuǎn)。四元數(shù)表示能夠更方便地給出旋轉(zhuǎn)的轉(zhuǎn)軸與旋轉(zhuǎn)角。

// 繞Y軸旋 eulerAngle.pitch = asinf(2 * q0 * q2 - 2 * q1 * q3) * (180 / PI); // 繞X軸旋轉(zhuǎn) eulerAngle.roll = atan2f(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2 * q2 + 1) * (180 / PI); // 繞Z軸旋轉(zhuǎn) eulerAngle.yaw = atan2f(2 * q1 * q2 + 2 * q0 * q3, -2 * q2 * q2 - 2 * q3 * q3 + 1) * (180 / PI);

05

通過一次二次積分獲得速度和位置

在載體坐標系下通過矩陣轉(zhuǎn)換到導(dǎo)航坐標系后，加速度計包含的重力加速和運動加速度。積分是只針對運動加速度積分。所以，需要把重力加速度給補償?shù)簟?/p>

然后一次積分得到速度，二次積分得到位置。但是積分存在累加誤差，因此在室外的時候通過 GNSS 對速度和位置做修正。

是呢環(huán)保局：郭婷