一種用于水下滑翔器的導航系統及姿態角校正和回溯解耦方法
【專利摘要】本發明公布了一種用于水下滑翔器的導航系統及姿態角校正和回溯解耦方法，屬于水下滑翔器導航定位【技術領域】。該組合導航系統包括數字信號處理(DSP)模塊、微機電系統(MEMS)慣性測量單元(IMU)等。由于俯仰或橫滾運動使得姿態角(航向角、俯仰角、橫滾角)之間的交叉耦合更加明顯，姿態角之間的交叉耦合導致姿態角輸出不準甚至錯誤，進而使隨后的速度、位置等其它導航信息解算發生錯誤，基于回溯思想提出姿態角回溯解耦方法來消除姿態角之間的交叉耦合。本系統設計可以滿足水下滑翔器導航系統低功耗、小體積、長航時的需求，姿態角回溯解耦方法有效地解決了姿態角之間的交叉耦合，大大提高了姿態角精度，達到了水下滑翔器長航時、低功耗、高精度導航定位的目的。
【專利說明】-種用于水下滑翔器的導航系統及姿態角校正和回溯解耦 方法
【技術領域】
[0001] 本發明涉及一種用于水下滑翔器的導航系統及姿態角回溯解耦方法，屬于水下滑 翔器導航定位【技術領域】。
【背景技術】
[0002] 水下滑翔器是一種使用內在制動器通過控制自身浮力和姿態角來隨水流滑翔的 自主水下載體。水下滑翔器具有結構簡單、功耗較低、能在水下長時間作業等特點，所以被 用作海洋勘測、海洋數據搜集等作業。近年，水下滑翔器已經成為臨海和開放海洋觀測的重 要部分。在水下執行的任務中，精確的位姿信息是至關重要的，因此水下導航是一個難點也 是重點問題。
[0003] 水下滑翔器的體積小，成本低，所以不能安裝太多高精度水下導航傳感器。對于陸 地車載來說，含有差分校正的全球定位系統（DGPS)能提供高精度的位姿信息，而且成本較 低。但全球定位系統（GPS)信號在水下不能使用。慣性導航系統（INS)可以在短時間內提 供精確的位姿信息，且能在沒有GPS的情況下實現自主導航，基于微機電系統（MEMS)的INS 的優勢在使得MEMS IMU在低成本慣性【技術領域】起到了重要作用。然而，因為陀螺儀和加速 度計自身的固有偏差使得INS的誤差隨時間不斷積累，積累的誤差將會導致姿態角和位置 的巨大偏差，所以需要其他傳感來補償INS的誤差。
[0004] 傳統的方式采用電子羅盤，這個在抑制INS的位姿漂移能起到一定作用，但僅僅 是電子羅盤，所達到的效果還是局限的。用航位推算方式（DR)能在保證功耗、成本等不增 加的情況下盡可能達到所需位姿精度要求。所以采用INS/DR組合方式完成水下滑翔器導 航。
[0005] 對于水下滑翔器，雖然在一定深度下，水流比較平穩均勻，滑翔器隨水流滑翔，但 俯仰和橫滾運動是不可避免的，。對于慣性測量單元，由于安裝軸和相應的參考軸之間的誤 差會造成姿態角（航向角、俯仰角和橫滾角）之間的交叉耦合，非零的俯仰角和橫滾角使得 姿態角的交叉耦合更加明顯，從而造成姿態角及其它導航信息解算不準甚至錯誤。俯仰和 橫滾運動在實際中是普遍存在的，滑翔器在一定深度的水中是以較平穩的速度隨水滑翔， 但是其特殊的構造，必須依靠水的浮力和調節自身的俯仰角來形成鋸齒波狀運動，通過這 種運動使得滑翔器向前滑翔。慣性測量單元作為搭載在水下滑翔器的主要導航元件，其導 航信息解算的精度對滑翔器的導航與定位起著至關重要的作用。因此，低成本、低功耗、長 航時的水下滑翔器導航系統及高精度的位姿估計方法是目前國內外研究的重點與難點。
【發明內容】
[0006] 發明目的：為了克服現有技術中存在的不足，本發明提供一種新的用于水下滑翔 器的導航系統及姿態角回溯解耦方法。
[0007] 為實現上述目的，本發明采用的技術方案為：
[0008] -種用于水下滑翔器的導航系統姿態角校正方法，包括以下步驟：步驟A，根據陀 螺儀輸出的角速度、加速度計輸出的加速度、磁力計輸出的磁場強度進行導航信息解算， 經導航解算后得到姿態角、速度、位置信息的導航數據；
[0009] 步驟B，根據導航解算出的導航數據，判斷因俯仰或橫滾運動，使得姿態角交叉耦 合明顯增大而造成的姿態角解算錯誤的節點，用姿態角交叉耦合造成解算錯誤的前一步解 算數據重新計算，得出新的姿態角、速度、位置信息；
[0010] 步驟C，步驟B中回溯解耦后的姿態角、速度及位置信息經前置濾波器去噪后，一 方面利用誤差方程得到狀態量，一方面和步驟A中經導航解算后得到姿態角、速度、位置信 息分別作差得到觀測量；將該狀態量和觀測量進入基于卡爾曼的自適應濾波算法，進行姿 態角、速度、位置誤差的最優估計，所得誤差估計值校正慣導所得的導航信息，最終得到校 正后準確的姿態角、速度和位置。
[0011] 所述步驟B中的回溯解耦方法包括以下步驟：
[0012] 步驟B1，根據導航解算得出的導航數據，判斷因俯仰或橫滾運動，使得姿態角交叉 耦合明顯增大而造成的姿態角解算錯誤的節點；
[0013] 步驟B2,用因俯仰或橫滾運動，使得姿態角交叉耦合明顯增大而造成解算錯誤的 前一步解算數據，計算載體坐標系相對導航坐標系的轉動角速率在載體坐標系上的投影；
[0014] 步驟B3,用載體坐標系相對導航坐標系的轉動角速率在載體坐標系上的投影通過 解算得到新的姿態角、速度、位置信息。
[0015] 所述步驟Bl中判斷因俯仰或橫滾運動，使得姿態角交叉耦合明顯增大而造成的 姿態角解算錯誤的節點的方法，包括以下步驟：
[0016] 步驟B11，因俯仰或橫滾運動，引起的姿態角交叉耦合而造成三個失準角 4>z錯誤，其中4>z分別為載體坐標系的三軸偏離導航坐標系對應的三軸的 偏離角，也即失準角；
[0017] 步驟B12,將三個失準角（K，七，叭代入姿態角校正方程 其中C!:是錯誤解算節點前一步正確的導航坐標系到載體坐標系的姿態角矩陣，
【權利要求】
1. 一種用于水下滑翔器的導航系統姿態角校正方法，其特征在于，包括以下步驟： 步驟A，根據陀螺儀輸出的角速度、加速度計輸出的加速度、磁力計輸出的磁場強度進 行導航信息解算，經導航解算后得到姿態角、速度、位置信息的導航數據； 步驟B，根據導航解算出的導航數據，判斷因俯仰或橫滾運動，使得姿態角交叉耦合明 顯增大而造成的姿態角解算錯誤的節點，用姿態角交叉耦合造成解算錯誤的前一步解算數 據重新計算，得出新的姿態角、速度、位置信息； 步驟C，步驟B中回溯解耦后的姿態角、速度及位置信息經前置濾波器去噪后，一方面 利用誤差方程得到狀態量，一方面和步驟A中經導航解算后得到姿態角、速度、位置信息分 別作差得到觀測量；將該狀態量和觀測量進入基于卡爾曼的自適應濾波算法，進行姿態角、 速度、位置誤差的最優估計，所得誤差估計值校正慣導所得的導航信息，最終得到校正后準 確的姿態角、速度和位置。
2. 根據權利要求1所述的一種用于水下滑翔器的導航系統姿態角校正方法，其特征在 于：所述步驟B中的回溯解耦方法包括以下步驟： 步驟B1，根據導航解算得出的導航數據，判斷因俯仰或橫滾運動，使得姿態角交叉耦合 明顯增大而造成的姿態角解算錯誤的節點； 步驟B2,用因俯仰或橫滾運動，使得姿態角交叉耦合明顯增大而造成解算錯誤的前一 步解算數據，計算載體坐標系相對導航坐標系的轉動角速率在載體坐標系上的投影； 步驟B3,用載體坐標系相對導航坐標系的轉動角速率在載體坐標系上的投影通過解算 得到新的姿態角、速度、位置信息。
3. 根據權利要求2所述的一種用于水下滑翔器的導航系統姿態角校正方法，其特征在 于：步驟B1中判斷因俯仰或橫滾運動，使得姿態角交叉耦合明顯增大而造成的姿態角解算 錯誤的節點的方法，包括以下步驟： 步驟B11，因俯仰或橫滾運動，使得姿態角交叉耦合而造成三個失準角（K，小y，錯 誤，其中<K，小y，分別為載體坐標系的三軸偏離導航坐標系對應的三軸的偏離角，也即 失準角； 步驟B12,將三個失準角（K，小y，代入姿態角校正方程
其中 < 是錯誤 解算節點前一步正確的導航坐標系到載體坐標系的姿態角矩陣，
為姿態校正矩陣； 步驟B13,根據姿態角校正方程可得出四元數方程：
步驟B14,判斷式（11)中的根號下的計算結果是否是負數，若是負數，則四元數q(l、 qi、q2、q3錯誤，后續的導航解算會依次錯誤；從而得出上述式（11)中根號下計算結果出現負數 是因姿態角交叉耦合造成導航解算錯誤的節點。
4. 根據權利要求3所述的一種用于水下滑翔器的導航系統姿態角校正方法，其特征在 于：所述步驟B2中載體坐標系相對導航坐標系的轉動角速率在載體坐標系上的投影為：
其中，<0^為載體坐標系相對導航坐標系的轉動角速率在載體坐標系上的投影為 陀螺儀輸出的角速率；€<.為地球坐標系相對于慣性坐標系的自轉角速率在導航坐標系中 的投影；為導航坐標系相對于地球坐標系的角速率在導航坐標系上的投影。
5. 根據權利要求4所述的一種用于水下滑翔器的導航系統姿態角校正方法，其特征在 于：所述步驟B3中用載體坐標系相對導航坐標系的轉動角速率在載體坐標系上的投影通 過解算得出新的姿態角、速度、位置信息的方法，包括以下步驟：
上的分量； 步驟B32,四元數是由四個元素構成，將其定義為zqa+qii+qj+qhq。、 1、q2、q3是實數，i、j、k是相互正交的單位向量，采用畢卡逼近方法求解四元數微分方程：
其中，Q(tk+1)、Q(tk)分別代表tk+1、tk時刻的四元數向量，將式（14)進行泰勒級數展開 得：
(15) 將式（15)寫成三角形式：
其中： A0x、A0y、A0z分別為陀螺的X、y、Z軸在[tktk+1]
采樣時間間隔內的角增量；故歸一化后的四元數： f J
步驟B33,用新的四元數秦更新姿態矩陣：
更新三個姿態角：
用準確的姿態變換矩陣^丨代入公式
中，算得比力fn，從而計算出新的速度及 位置。
6. -種基于權利要求1所述的用于水下滑翔器的導航系統，其特征在于：包括DSP處 理單元、MEMSMU導航元件；所述DSP處理單元包括存儲模塊、回溯解耦模塊、前置濾波去 噪模塊、誤差模塊、觀測量模塊以及基于卡爾曼的自適應濾波算法模塊；所述MEMSIMU導 航元件用于采集原始的角速度、加速度及磁場數據，所有原始數據進入DSP處理單元進行 解算，得到姿態角、速度及位置信息，并將得到的姿態角、速度及位置信息傳送給DSP處理 單元； 所述DSP處理單元用于接收MEMSMU導航元件傳送過來的姿態角、速度及位置信息， 并將該姿態角、速度及位置信息傳送給回溯解耦模塊和觀測量模塊； 所述回溯解耦模塊用于根據回溯解耦方法，將正常解算過程中解算的導航信息存儲起 來，當解算過程錯誤判斷后，姿態角發生奇異突變，調用上一次正確解算的導航參數進行重 新解算，得出新的姿態角、速度及位置信息并對其進行更新，同時將更新后的姿態角、速度 及位置信息發送給前置濾波去噪模塊；若姿態角未發生奇異突變，則將姿態角、速度及位置 信息發送給前置濾波去噪模塊； 所述濾波去噪模塊用于接收回溯解耦模塊傳送過來姿態角、速度及位置信息；同時將 姿態角、速度和位置信息進行濾波去噪，并將濾波去噪后的姿態角、速度和位置信息分別傳 送給誤差模塊和觀測量模塊； 所述誤差模塊用于根據濾波去噪模塊傳送的姿態角、速度和位置信息利用誤差方程求 導航信息誤差得到狀態量，并將該狀態量傳送給基于卡爾曼的自適應濾波算法模塊； 所述觀測量模塊用于根據DSP處理單元推送的姿態角、速度及位置信息與濾波去噪模 塊推送的姿態角、速度及位置信息作差作為濾波器的觀測量，并將該觀測量傳送給基于卡 爾曼的自適應濾波算法模塊； 所述基于卡爾曼的自適應濾波算法模塊用于根據狀態量和觀測量進行姿態角、速度、 位置誤差的最優估計，再用所得誤差的估計值校正慣導所得的姿態角、速度及位置，最終得 到校正后準確的姿態角、速度和位置信息。
7. -種基于權利要求1所述的用于水下滑翔器的導航系統及姿態角回溯解耦方法，其 特征在于，包括以下步驟： 步驟一，根據陀螺儀輸出的角速度、加速度計輸出的加速度、磁力計輸出的磁場強度進 行導航信息解算，經導航解算后得到姿態角、速度、位置信息的導航數據； 步驟二，根據導航解算得出的導航數據，判斷因俯仰或橫滾運動，引起的姿態角交叉耦 合而造成的姿態角解算錯誤的節點； 步驟三，用因俯仰或橫滾運動，使得姿態角交叉耦合明顯增大而造成解算錯誤的前一 步解算數據，計算載體坐標系相對導航坐標系的轉動角速率在載體坐標系上的投影； 步驟四，用載體坐標系相對導航坐標系的轉動角速率在載體坐標系上的投影通過解算 得到新的姿態角、速度、位置信息。
8. 根據權利要求1所述7的用于水下滑翔器的導航系統及姿態角回溯解耦方法，其特 征在于：步驟二中判斷因為俯仰或橫滾運動而使得姿態角交叉耦合明顯增大導致姿態角解 算錯誤的節點的方法，包括以下步驟： 步驟二a，因俯仰或橫滾運動，引起的姿態角交叉耦合而造成三個失準角（^，小y， 錯誤，其中小y，分別為載體坐標系的三軸偏離導航坐標系對應的三軸的偏離角，也 即失準角； 步驟二b，將三個失準角（K，小y，代入姿態角校正方程
，其中g是錯誤 解算節點前一步正確的導航坐標系到載體坐標系的姿態角矩陣，
為姿態校正矩陣； 步驟二c，根據姿態角校正方程可得出四元數方程：
步驟二d，判斷式（11)中的根號下的計算結果是否是負數，若是負數，則四元數q(l、ql、q2、q3錯誤，后續的導航解算會依次錯誤，則上述式（11)中根號下計算結果出現負數是因姿 態角交叉稱合造成導航解算錯誤的節點。
9. 根據權利要求1所述7的一種用于水下滑翔器的導航系統及姿態角回溯解耦方法， 其特征在于：所述步驟三中載體坐標系相對導航坐標系的轉動角速率在載體坐標系上的投 影為：
其中，為載體坐標系相對導航坐標系的轉動角速率在載體坐標系上的投影； 為陀螺儀輸出的角速率；為地球坐標系相對于慣性坐標系的自轉角速率在導航坐標系 中的投影；為導航坐標系相對于地球坐標系的角速率在導航坐標系上的投影；C;是錯 誤解算節點前一步正確的導航坐標系到載體坐標系的姿態矩陣。
10. 根據權利要求1所述7的一種用于水下滑翔器的導航系統及姿態角回溯解耦方法， 其特征在于：所述步驟四中通過載體坐標系相對導航坐標系的轉動角速率在載體坐標系上 的投影導航得出新的姿態、速度、位置信息的方法，包括以下步驟：
軸上的分量； 步驟四b，四元數是由四個元素構成，將其定義為=qa+qj+qd+q#，， %、qi、q2、q3是實數，i、j、k是相互正交的單位向量，采用畢卡逼近方法求解四元數微分方 程：
其中，Q(tk+1)、Q(tk)分別代表tk+1、tk時刻的四元數向量，將式（14)進行泰勒級數展開 得：
將式（15)寫成三角形式：
其中，
，A0 x、A0 y、A0z分別為陀螺的X、y、Z軸在[tktk+1] 采樣時間間隔內的角增量； 故歸一化后的四元數：
步驟四c，用新的四元數備更新姿態矩陣：
更新三個姿態角：
用準確的姿態變換矩陣￡5代入公式
中，算得比力fn，從而計算出新的速度及 位置。
【文檔編號】G01C21/20GK104406592SQ201410712437
【公開日】2015年3月11日 申請日期:2014年11月28日 優先權日:2014年11月28日
【發明者】陳熙源, 黃浩乾, 周智愷, 呂才平 申請人:東南大學