捷聯慣導算法心得

一、四個概念：「地理」座標系、「機體」座標系、他們之間換算公式、換算公式用的係數。

地理座標系：東、北、天，如下簡稱地理。在這個座標系裏有重力永遠是（0,0,1g），地磁永遠是（0,1,x）（地磁的垂直不關心）兩個三維向量。
機體座標系：如下簡稱機體，上面有陀螺、加計、電子羅盤傳感器，三個三維向量。
換算公式：如下簡稱公式，公式就是描述機體姿態的表達方法，通常都是用以地理爲基準，從地理換算到機體的公式，有四元數、歐拉角、方向餘弦矩陣。
換算公式的係數：如下簡稱係數，四元數的q012三、歐拉角的ROLL/PITCH/YAW、餘弦矩陣的9個數。係數就是描述機體姿態的表達方法的具體數值。

姿態，其實就是公式+係數的組合，通常常常用人容易理解的公式「歐拉角」表示，係數就是橫滾xx度俯仰xx度航向xx度。

二、五個數據源：重力、地磁、陀螺、加計、電子羅盤，前兩個來自地理，後三個來自機體。

三、陀螺向量：基於機體，也在機體上積分，由於地理上無參考數據源，因此很獨立，直接在公式的老係數上積分，獲得新系數。
狹義上的捷聯慣導算法，就是指這個陀螺積分公式，也分爲歐拉角、方向餘弦矩陣、四元數，他們的積分算法有增量法、數值積分法（X階龍格-庫塔）等等

四、加計向量、重力向量：加計基於機體，重力基於地理，重力向量（0,0,1g）用公式換算到機體，與機體的加計向量算出偏差。理論上應該沒有偏差，這偏差逆向思惟一下，其實就是換算公式的係數偏差。因此這偏差可用於糾正公式的係數（橫滾、俯仰），也就是姿態。

五、電子羅盤向量、地磁向量：同上，只不過要砍掉地理上的垂直向量，由於無用。只留下地理水平面上的向量。偏差能夠用來糾正公式的係數（航向）。

六、就這樣，係數不停地被陀螺積分更新，也不停地被偏差修正，它和公式所表明的姿態也在不斷更新。
若是積分和修正用四元數算法（由於運算量較少、無奇點偏差），最後用歐拉角輸出控制PID（由於角度比較直觀），那就須要有個四元數係數到歐拉角係數的轉換。經常使用的三種公式，它們之間都有轉換算法。

再搞個直白一點的例子：
機體好似一條船，地理就是那地圖，姿態就是航向（船頭在地圖上的方位），重力和地磁是地圖上的燈塔，陀螺/積分公式是舵手，加計和電子羅盤是瞭望手。
舵手負責估計和把穩航向，他相信本身，原本船向北開的，就必定會一直往北開，以爲轉了90度彎，那就會往東開。
固然若是舵手很牛逼，也許能估計很準確，維持很長時間。不過只信任舵手，確定會迷路，因此通常都有地圖和瞭望手來觀察偏差。
瞭望手根據地圖燈塔方位和船的當前航向，算出燈塔理論上應該在船的X方位。然而看到實際燈塔在船的Y方位，那確定船的當前航向有誤差了，誤差就是ERR=X-Y。
舵手收到瞭望手給的ERR報告，以爲可靠，那就聽個90%*ERR，以爲天氣很差、地圖偏差大，那就聽個10%*ERR，根據這個來糾正估算航向。。



------------------------------------------------------
來點乾貨，注意如下的歐拉角都是這樣的順序：先航向-再俯仰-而後橫滾
公式截圖來自：袁信、鄭鍔的《捷聯式慣性導航原理》，鄧正隆的《慣性技術》。
--------------------------------------------------
根據加計計算初始歐拉角
這個不管歐拉角算法仍是四元數算法仍是方向餘弦矩陣都須要，由於加計和電子羅盤給出歐拉角的描述方式比較方便。
imu.euler.x = atan2(imu.accel.y, imu.accel.z);
imu.euler.y = -asin(imu.accel.x / ACCEL_1G);
ACCEL_1G 爲9.81米/秒^2，accel.xyz的都爲這個單位，算出來的euler.xyz單位是弧度
航向imu.euler.z能夠用電子羅盤計算
--------------------------------------------------
歐拉角微分方程
若是用歐拉角算法，那麼這個公式就夠了，不須要來回轉換。

矩陣上到下三個角度（希臘字母）是roll pitch和yaw，公式最左邊的上面帶點的三個是本次更新後的角度，不帶點的是上個更新週期算出來的角度。
Wx,y,z是roll pitch和yaw方向的三個陀螺在這個週期轉動過的角度，單位爲弧度，計算爲間隔時間T*陀螺角速度,好比0.02秒*0.01弧度/秒=0.0002弧度.


--------------------------------------------------
如下是四元數
--------------------------------------------------
四元數初始化
q0-3爲四元數四個值,用最上面公式根據加計計算出來的歐拉角來初始化

--------------------------------------------------
四元數微分方程
四元數更新算法，一階龍庫法，一樣4個量（入、P1-3）也爲四元數的四個值,即上面的q0-3。
Wx,y,z是三個陀螺的這個週期的角速度，好比歐拉角微分方程中的0.01弧度/秒，T爲更新週期,好比上面的0.02秒。

再來一張，另一本書上的，仔細看和上面是同樣的delta角度，就是上面的角速度*週期，單位爲弧度

--------------------------------------------------
四元數微分方程更新後的規範化
每一個週期更新完四元數，須要對四元數作規範化處理。由於四元數原本就定義爲四維單位向量。
求q0-3的平方和，再開根號算出的向量長度length。而後每一個q0-3除這個length。

--------------------------------------------------
四元數轉歐拉角公式
把四元數轉成了方向餘弦矩陣中的幾個元素，再用這幾個元素轉成了歐拉角
先從四元數q0-3轉成方向餘弦矩陣：

再從方向餘弦矩陣轉成歐拉角


代碼：
        //更新方向餘弦矩陣
        t11=q.q0*q.q0+q.q1*q.q1-q.q2*q.q2-q.q3*q.q3;
        t12=2.0*(q.q1*q.q2+q.q0*q.q3);
        t13=2.0*(q.q1*q.q3-q.q0*q.q2);
        t21=2.0*(q.q1*q.q2-q.q0*q.q3);
        t22=q.q0*q.q0-q.q1*q.q1+q.q2*q.q2-q.q3*q.q3;
        t23=2.0*(q.q2*q.q3+q.q0*q.q1);
        t31=2.0*(q.q1*q.q3+q.q0*q.q2);
        t32=2.0*(q.q2*q.q3-q.q0*q.q1);
        t33=q.q0*q.q0-q.q1*q.q1-q.q2*q.q2+q.q3*q.q3;
        //求出歐拉角
        imu.euler.roll = atan2(t23,t33);
        imu.euler.pitch = -asin(t13);
        imu.euler.yaw = atan2(t12,t11);
        if (imu.euler.yaw < 0){
                imu.euler.yaw += ToRad(360);
        }
----------------------------------------------------
如下代碼摘自網上，很巧妙，附上註釋，有四元數微分，有加計耦合。
沒電子羅盤，其實耦合原理也同樣。

1. //=====================================================================================================
2. // IMU.c
3. // S.O.H. Madgwick
4. // 25th September 2010
5. //=====================================================================================================
6. // Description:
7. //
8. // Quaternion implementation of the 'DCM filter' [Mayhony et al].
9. //
10. // User must define 'halfT' as the (sample period / 2), and the filter gains 'Kp' and 'Ki'.
11. //
12. // Global variables 'q0', 'q1', 'q2', 'q3' are the quaternion elements representing the estimated
13. // orientation.  See my report for an overview of the use of quaternions in this application.
14. //
15. // User must call 'IMUupdate()' every sample period and parse calibrated gyroscope ('gx', 'gy', 'gz')
16. // and accelerometer ('ax', 'ay', 'ay') data.  Gyroscope units are radians/second, accelerometer
17. // units are irrelevant as the vector is normalised.
18. //
19. //=====================================================================================================
20.  
21. //----------------------------------------------------------------------------------------------------
22. // Header files
23.  
24. #include "IMU.h"
25. #include <math.h>
26.  
27. //----------------------------------------------------------------------------------------------------
28. // Definitions
29.  
30. #define Kp 2.0f                        // proportional gain governs rate of convergence to accelerometer/magnetometer
31. #define Ki 0.005f                // integral gain governs rate of convergence of gyroscope biases
32. #define halfT 0.5f                // half the sample period
33.  
34. //---------------------------------------------------------------------------------------------------
35. // Variable definitions
36.  
37. float q0 = 1, q1 = 0, q2 = 0, q3 = 0;        // quaternion elements representing the estimated orientation
38. float exInt = 0, eyInt = 0, ezInt = 0;        // scaled integral error
39.  
40. //====================================================================================================
41. // Function
42. //====================================================================================================
43.  
44. void IMUupdate(float gx, float gy, float gz, float ax, float ay, float az) {
45.         float norm;
46.         float vx, vy, vz;
47.         float ex, ey, ez;         
48.        
49.         // normalise the measurements
50.         norm = sqrt(ax*ax + ay*ay + az*az);      
51.         ax = ax / norm;
52.         ay = ay / norm;
53.         az = az / norm;      
54. 把加計的三維向量轉成單位向量。
55.        
56.  
57.         // estimated direction of gravity
58.         vx = 2*(q1*q3 - q0*q2);
59.         vy = 2*(q0*q1 + q2*q3);
60.         vz = q0*q0 - q1*q1 - q2*q2 + q3*q3;
61.  
62. 這是把四元數換算成《方向餘弦矩陣》中的第三列的三個元素。
63. 根據餘弦矩陣和歐拉角的定義，地理座標系的重力向量，轉到機體座標系，正好是這三個元素。
64. 因此這裏的vx\y\z，其實就是當前的歐拉角（即四元數）的機體座標參照系上，換算出來的重力單位向量。
65.  
66.  
67.         // error is sum of cross product between reference direction of field and direction measured by sensor
68.         ex = (ay*vz - az*vy);
69.         ey = (az*vx - ax*vz);
70.         ez = (ax*vy - ay*vx);
71.  
72. axyz是機體座標參照系上，加速度計測出來的重力向量，也就是實際測出來的重力向量。
73. axyz是測量獲得的重力向量，vxyz是陀螺積分後的姿態來推算出的重力向量，它們都是機體座標參照系上的重力向量。
74. 那它們之間的偏差向量，就是陀螺積分後的姿態和加計測出來的姿態之間的偏差。
75. 向量間的偏差，能夠用向量叉積（也叫向量外積、叉乘）來表示，exyz就是兩個重力向量的叉積。
76. 這個叉積向量仍舊是位於機體座標系上的，而陀螺積分偏差也是在機體座標系，並且叉積的大小與陀螺積分偏差成正比，正好拿來糾正陀螺。（你能夠本身拿東西想象一下）因爲陀螺是對機體直接積分，因此對陀螺的糾正量會直接體如今對機體座標系的糾正。
77.  
78.  
79.  
80.         // integral error scaled integral gain
81.         exInt = exInt + ex*Ki;
82.         eyInt = eyInt + ey*Ki;
83.         ezInt = ezInt + ez*Ki;
84.  
85.         // adjusted gyroscope measurements
86.         gx = gx + Kp*ex + exInt;
87.         gy = gy + Kp*ey + eyInt;
88.         gz = gz + Kp*ez + ezInt;
89.  
90. 用叉積偏差來作PI修正陀螺零偏
91.  
92.  
93.  
94.  
95.         // integrate quaternion rate and normalise
96.         q0 = q0 + (-q1*gx - q2*gy - q3*gz)*halfT;
97.         q1 = q1 + (q0*gx + q2*gz - q3*gy)*halfT;
98.         q2 = q2 + (q0*gy - q1*gz + q3*gx)*halfT;
99.         q3 = q3 + (q0*gz + q1*gy - q2*gx)*halfT;  
100. 四元數微分方程
101.  
102.        
103.  
104.         // normalise quaternion
105.         norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
106.         q0 = q0 / norm;
107.         q1 = q1 / norm;
108.         q2 = q2 / norm;
109.         q3 = q3 / norm;
110. 四元數規範化
111. }
112.  
113. //====================================================================================================
114. // END OF CODE
115. //====================================================================================================

複製代碼