【行人慣性導航】關於行人導航中IMU位姿推導的知識點及相關程式碼

半耳聆聽發表於2021-10-30

IMU姿態慣性推導

  • 最近從事行人慣性導航的研究，本人也是一個小白，其中看了很多文獻，有很多個人思考很費時間的地方，撰寫此隨筆的目的不僅是給自己做一個筆記，也是給各位有需要的仁兄一點個人理解。
  • 本文只關於使用IMU感測器為主的行人導航演算法。
  • 本文為一篇行人慣性導航的入門，主要針對其中重要的涉及的知識點之間的解釋、串聯，包括航向更新、速度更新、位置更新、座標變換原理即程式碼，不深究其推導。要是有什麼講得不對的地方，請給予指正，謝謝。

引言

以IMU為主，其他感測器（多為磁力計、高度計、氣壓計、壓力感測器、肌肉電感測器等）為輔的行人導航研究中，主要有兩種方案（這裡忽略SLAM、鐳射雷達之類的，）。一個是基於航跡推移的方案（SHS，step-and-heading systems），另一個是基於慣性積分的方案（INS，inertial navigation systems）。個人研究的技術方向屬於後者（INS），所以後面將會著重介紹後者的技術細節。

SHS

也就是常見的航跡推移演算法（PDR，Pedestrian Dead Reckoning），根據計算出來的步長和航向，通過三角函式關係遞推下一步的位置。所以該方案的核心就是求步長和航向，同時也是該演算法的兩個主要誤差來源。求步長有諸多步長估計模型，比如Weinberg模型[1]

\[l=k \times (a_{max}-a_{min})^{1/4} \]

或者Scarlet模型[2]

\[l=k \times \cfrac{\cfrac{\sum_{i=1}^Na_i}{N}-a_{min}}{a_{max}-a_{min}} \]

等等，詳細細節可自行了解。但是無論使用哪一個步長模型，這裡面的引數（比如k）都跟個體引數（步頻、步長、腿長等）的不同而不同，即需要對人體進行運動學建模，這也是這種模型最大的一個弊端（個人覺得）[3]。

關於航向修正放在後面（INS）一起總結，因為無論是SHS，還是INS，航向漂移都是兩者的痛點。

INS

基於慣性遞推的技術方案，給出常見的遞推公式[4]便一目瞭然。

\[p_k^n=p_{n-1}^n+v_{k-1}^n\Delta t \\ v_k^n = v_{k-1}^n+\{R(q_{k-1})f^b + g^n\}\Delta t \\ q_{b,k}^n = \Omega(\omega_k \Delta t)q_{b,k-1}^n \]

其中\(n\)、\(b\)分別表示導航座標系和載體座標系，\(p\)、\(v\)、\(q\)分別表示位置、速度、姿態四元數（對應尤拉角，俯仰角pitch、滾轉角raw、航向角yaw）。這裡是一個簡化後的模型，忽略了地球自轉、經緯度等因素的影響，因為這些因素對低精度的慣性器件影響可以忽略不計，具體細節可以參考文獻[7]，這裡不再展開。

INS特點：

  1. 基於慣性遞推的方案相比於航跡推移演算法，其本身的姿態推導不需要為人體運動學建模，適用性更好；
  2. 基於慣性遞推的方案誤差隨時間的立方根次增長，這是其最大的弊端；
  3. 基於慣性遞推的方案一般結合零速修正技術，使用卡爾曼濾波器將兩者進行融合，限制誤差的增長。

關於INS方法的技術點

  • 零速修正
    • 使用零速修正技術，重點是使用什麼檢測器檢測零速條件！
    • 當前有很多零速檢測器，比SHOE、ARED、MBGTD等，其中SHOE公認精度是最好的[6]。
  • 航向修正
    • 無論是SHS還是INS，在航向修正方面所遇到的問題都基本相同，及決方案大致一樣，要麼藉助磁力計，要麼是HDR/iHDR，或者其他的方案。

乾貨

下面所講的內容是個人所經歷的，也是本文的重點。如果看慣性導航參考書[5]的話，你一般都會看到複雜的推導，容易搞蒙，尤其是涉及到座標系的轉換關係，除非死磕硬骨頭才能拿下。其實有一本不錯的講義[8]，本人的大多數困惑都能從上邊找到答案。

  • 姿態更新
    • 首先要明白IMU測的是相對於慣性座標系 \(i\) 的加速度和角速度，不是相對於導航系 \(n\) ，這之間存在轉換。這也是為什麼我早期一直很困惑的原因。具體的推導比較複雜，本文只給出關鍵結果。姿態的更新一般用四元數、旋轉矩陣或旋轉向量表示。本文以旋轉矩陣進行說明。不同表達形式，都有各自的優缺點。旋轉矩陣存在萬向鎖問題，四元數存在交換不可逆誤差等。一般在使用的時候結合使用。

\[C_{b(k)}^{n(k)}=C_{n(k-1)}^{n(k)}C_{i(k-1)}^{n(k-1)}C_{b(k-1)}^{i(k-1)}C_{b(k)}^{b(k-1)}=C_{n(k-1)}^{n(k)}C_{b(k-1)}^{n(k-1)}C_{b(k)}^{b(k-1)} \]

其中，\(C_{b(k)}^{b(k-1)}\) 表示以i系作為參考基準，b系從 \(t_{k-1}\) 時刻到 \(t_{k}\) 時刻的旋轉變化，\(C_{b(k)}^{b(k-1)}\) 由角速度 \(w_{ib}^b\) 確定；\(C_{n(k)}^{n(k-1)}\) 表示以i系作為參考基準，n系從 \(t_{k-1}\) 時刻到 \(t_{k}\) 時刻的旋轉變化，\(C_{n(k-1)}^{n(k)}\) 由計算角速度 \(-w_{in}^n\) 確定；\(C_{n(k-1)}^{n(k-1)}\) 和 \(C_{b(k)}^{n(k)}\) 分別表示 \(t_{k-1}\) 和 \(t_{k}\) 時刻的捷聯姿態矩陣。通常在導航更新週期 \([t_{k-1},t_k]\) 內，可認為速度和位置引起的 \(w_{in}^n\) 計算角速度很小，即可視為常值[8]；通常在更新週期 \([t_{k-1},t_k]\) 內，導航系的變化很緩慢，所以 \(C_{n(k-1)}^{n(k)} \approx I_{3 \times 3}\)[5]。從這個角度應該能更好的理解座標系之間的變化關係。

用四元數表示為：

\[Q(t_k) = Q(t_{k-1})\bigotimes q(\omega_k \Delta t) \\ q(\omega_k \Delta t) = cos\cfrac{\phi}{2} + \cfrac{\overrightarrow{\phi}}{\phi}sin\cfrac{\phi}{2} \]

其中 \(Q\) 為四元數，表示載體的姿態，\(\phi\) 是旋轉向量 \(\overrightarrow{\phi}\) 的模長，四元數、旋轉向量、旋轉矩陣可以互相轉換。這裡所寫的形式和之前的遞推公式沒有什麼區別，只是一個是四元數的左乘，一個是四元數的右乘。為什麼四元數需要結合旋轉向量使用，就是彌補各自的缺點（建議看書，這裡面存在一個理論推導）。

  • 速度更新
    • 速度的更新也存在一個座標系的轉換；
    • 其實只要理解的旋轉矩陣的更新，後面的推導都是在旋轉矩陣更新的基礎上的得到；
    • 關於速度更新，更注意的一點是剔除重力因素的影響。
    • 上面的遞推公式 \(v_k^n = v_{k-1}^n+\{R(q_{k-1})f^b + g^n\}\Delta T\)，其中 \(R(q_{k-1})f^b\) 對所測到的加速度 \(f^b\)（一般稱為比力） 進行座標變換，\(R(q_{k-1})\) 表示對四元數轉換為旋轉矩陣。
  • 位置更新
    • 這就不用說了，一眼明瞭。

相關程式碼（Python）

\[p_k^n=p_{n-1}^n+v_{k-1}^n\Delta T \\ v_k^n = v_{k-1}^n+\{R(q_{k-1})f^b + g^n\}\Delta T \\ q_{b,k}^n = \Omega(\omega_k \Delta t)q_{b,k-1}^n \]

    def nav_eq(self, xin, imu, qin, dt):
        # imu[0:3] acc  加速度
        # imu[3:6] gyro 角速度
        # imu[6:9] attitude 航向
        # update Quaternions
        x_out = np.copy (xin)  # initialize the output
        # 這裡是求 omega，即 wt 的
        omega = np.array ([[0, -imu[3], -imu[4], -imu[5]],
                           [imu[3], 0, imu[5], -imu[4]],
                           [imu[4], -imu[5], 0, imu[3]],
                           [imu[5], imu[4], -imu[3], 0]])

        # 求角速度的二範數，即模長
        # qout輸出旋轉矩陣 $\Omega$
        # 定時增量法，四元數微分方程的畢卡求解法，四元數的更新為了避免其自身的不可交換誤差，會先計算旋轉向量，利用旋轉向量更新四元數，具體參考文獻[8]
        norm_w = LA.norm (imu[3:6])
        if (norm_w * dt != 0):
            q_out = (np.cos (dt * norm_w / 2) * np.identity (4) + (1 / (norm_w)) * np.sin (dt * norm_w / 2) * omega).dot (qin)
        else:
            q_out = qin

        attitude = quat2euler (q_out, 'sxyz')  # update euler angles
        x_out[6:9] = attitude

        Rot_out = quat2mat (q_out)  # 將更新後的四元數轉換成旋轉矩陣
        acc_n = Rot_out.dot (imu[0:3])  # 對所測的加速度進行座標變換
        acc_n = acc_n + np.array ([0, 0, self.config["g"]])  # 剔除重力影響

        x_out[3:6] += dt * acc_n  # 速度更新
        x_out[0:3] += dt * x_out[3:6] + 0.5 * np.power (dt, 2) * acc_n  # 位置更新

        return x_out, q_out, Rot_out

在此感謝大佬的開源pyshoe。如果是小白，強烈推薦這個開源，因為其還有對標論文，可以便看邊上手。同時還需要感謝大佬實驗室OpenSHOE。這兩個開源專案給我的研究帶來了巨大的飛躍，在此重重感謝。

總結

關於IMU的行人導航技術，還有很多研究的熱點，比如初始校準、高程問題、航向問題、多運動模型的問題等等。以上只是個人的理解，如果有什麼不對的地方，請大佬儘管之處，虛心請教。歡迎各位同學留言，互相學習、探討。

參考文獻

[1] Weinberg H. Using the ADXL202 in pedometer and personal navigation applications[J].
[2] Scarlett J. Enhancing the performance of pedometers using a single accelerometer[J].
[3] 潘獻飛, 穆華, 胡小平. 單兵自主導航技術發展綜述[J]. 導航定位與授時, 2018, 5(1):11.
[4] Wagstaff B , Peretroukhin V , Kelly J . Robust Data-Driven Zero-Velocity Detection for Foot-Mounted Inertial Navigation[J]. IEEE Sensors Journal, 2019, PP(99):1-1.
[5] 秦永元. 慣性導航.第2版[M]. 科學出版社, 2014.
[6] Wahlstrm J , Skog I . Fifteen Years of Progress at Zero Velocity: A Review[J]. 2020.
[7] Muhammad I , Kuk C , Seung-Ho B , et al. Drift Reduction in Pedestrian Navigation System by Exploiting Motion Constraints and Magnetic Field[J]. Sensors (Basel, Switzerland), 2016, 16(9).
[8] 捷聯慣導演算法與組合導航原理講義. 嚴恭敏，翁浚 編著.

相關文章