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

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

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

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

02坐標(biāo)系

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

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

03

慣性傳感器

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

誤差來(lái)源

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

誤差模型

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

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

不需要轉(zhuǎn)臺(tái)的標(biāo)定常用 迭代優(yōu)化方法。`參考文獻(xiàn)：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`

對(duì)隨機(jī)誤差常用 Allan方差分析

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

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

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

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

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

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

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

如上對(duì)比，因?yàn)榧铀俣群屯勇輧x都存在各自的不足之處，因此我們需要引入數(shù)據(jù)融合（濾波算法）。 常見(jiàn)的濾波算法：DMP直接輸出角度 > 卡爾曼濾波 > 四元數(shù)解算 > 清華角度濾波 > 一階互補(bǔ)濾波 在項(xiàng)目中常用：EKF、DCM/Mahoney、Madgwick（基于梯度下降法）等算法

`參考文獻(xiàn)：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.`

位姿表達(dá)

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

因此，很多做方案的公司都是用四元數(shù)表征傳感器的姿態(tài)。在這個(gè)地方 yaw 角是通過(guò)地磁傳感器解算的。地磁傳感器測(cè)量的是自身和磁北的角度。北東地坐標(biāo)系和磁北不是同一個(gè)方向。 單位四元數(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

通過(guò)一次二次積分獲得速度和位置

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

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

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