一種用于水下滑翔器的導(dǎo)航系統(tǒng)及姿態(tài)角校正和回溯解耦方法
【專利摘要】本發(fā)明公布了一種用于水下滑翔器的導(dǎo)航系統(tǒng)及姿態(tài)角校正和回溯解耦方法，屬于水下滑翔器導(dǎo)航定位【技術(shù)領(lǐng)域】。該組合導(dǎo)航系統(tǒng)包括數(shù)字信號處理(DSP)模塊、微機(jī)電系統(tǒng)(MEMS)慣性測量單元(IMU)等。由于俯仰或橫滾運動使得姿態(tài)角(航向角、俯仰角、橫滾角)之間的交叉耦合更加明顯，姿態(tài)角之間的交叉耦合導(dǎo)致姿態(tài)角輸出不準(zhǔn)甚至錯誤，進(jìn)而使隨后的速度、位置等其它導(dǎo)航信息解算發(fā)生錯誤，基于回溯思想提出姿態(tài)角回溯解耦方法來消除姿態(tài)角之間的交叉耦合。本系統(tǒng)設(shè)計可以滿足水下滑翔器導(dǎo)航系統(tǒng)低功耗、小體積、長航時的需求，姿態(tài)角回溯解耦方法有效地解決了姿態(tài)角之間的交叉耦合，大大提高了姿態(tài)角精度，達(dá)到了水下滑翔器長航時、低功耗、高精度導(dǎo)航定位的目的。
【專利說明】-種用于水下滑翔器的導(dǎo)航系統(tǒng)及姿態(tài)角校正和回溯解耦 方法

【技術(shù)領(lǐng)域】
[0001] 本發(fā)明涉及一種用于水下滑翔器的導(dǎo)航系統(tǒng)及姿態(tài)角回溯解耦方法，屬于水下滑 翔器導(dǎo)航定位【技術(shù)領(lǐng)域】。

【背景技術(shù)】
[0002] 水下滑翔器是一種使用內(nèi)在制動器通過控制自身浮力和姿態(tài)角來隨水流滑翔的 自主水下載體。水下滑翔器具有結(jié)構(gòu)簡單、功耗較低、能在水下長時間作業(yè)等特點，所以被 用作海洋勘測、海洋數(shù)據(jù)搜集等作業(yè)。近年，水下滑翔器已經(jīng)成為臨海和開放海洋觀測的重 要部分。在水下執(zhí)行的任務(wù)中，精確的位姿信息是至關(guān)重要的，因此水下導(dǎo)航是一個難點也 是重點問題。
[0003] 水下滑翔器的體積小，成本低，所以不能安裝太多高精度水下導(dǎo)航傳感器。對于陸 地車載來說，含有差分校正的全球定位系統(tǒng)（DGPS)能提供高精度的位姿信息，而且成本較 低。但全球定位系統(tǒng)（GPS)信號在水下不能使用。慣性導(dǎo)航系統(tǒng)（INS)可以在短時間內(nèi)提 供精確的位姿信息，且能在沒有GPS的情況下實現(xiàn)自主導(dǎo)航，基于微機(jī)電系統(tǒng)（MEMS)的INS 的優(yōu)勢在使得MEMS IMU在低成本慣性【技術(shù)領(lǐng)域】起到了重要作用。然而，因為陀螺儀和加速 度計自身的固有偏差使得INS的誤差隨時間不斷積累，積累的誤差將會導(dǎo)致姿態(tài)角和位置 的巨大偏差，所以需要其他傳感來補(bǔ)償INS的誤差。
[0004] 傳統(tǒng)的方式采用電子羅盤，這個在抑制INS的位姿漂移能起到一定作用，但僅僅 是電子羅盤，所達(dá)到的效果還是局限的。用航位推算方式（DR)能在保證功耗、成本等不增 加的情況下盡可能達(dá)到所需位姿精度要求。所以采用INS/DR組合方式完成水下滑翔器導(dǎo) 航。
[0005] 對于水下滑翔器，雖然在一定深度下，水流比較平穩(wěn)均勻，滑翔器隨水流滑翔，但 俯仰和橫滾運動是不可避免的，。對于慣性測量單元，由于安裝軸和相應(yīng)的參考軸之間的誤 差會造成姿態(tài)角（航向角、俯仰角和橫滾角）之間的交叉耦合，非零的俯仰角和橫滾角使得 姿態(tài)角的交叉耦合更加明顯，從而造成姿態(tài)角及其它導(dǎo)航信息解算不準(zhǔn)甚至錯誤。俯仰和 橫滾運動在實際中是普遍存在的，滑翔器在一定深度的水中是以較平穩(wěn)的速度隨水滑翔， 但是其特殊的構(gòu)造，必須依靠水的浮力和調(diào)節(jié)自身的俯仰角來形成鋸齒波狀運動，通過這 種運動使得滑翔器向前滑翔。慣性測量單元作為搭載在水下滑翔器的主要導(dǎo)航元件，其導(dǎo) 航信息解算的精度對滑翔器的導(dǎo)航與定位起著至關(guān)重要的作用。因此，低成本、低功耗、長 航時的水下滑翔器導(dǎo)航系統(tǒng)及高精度的位姿估計方法是目前國內(nèi)外研究的重點與難點。


【發(fā)明內(nèi)容】

[0006] 發(fā)明目的：為了克服現(xiàn)有技術(shù)中存在的不足，本發(fā)明提供一種新的用于水下滑翔 器的導(dǎo)航系統(tǒng)及姿態(tài)角回溯解耦方法。
[0007] 為實現(xiàn)上述目的，本發(fā)明采用的技術(shù)方案為：
[0008] -種用于水下滑翔器的導(dǎo)航系統(tǒng)姿態(tài)角校正方法，包括以下步驟：步驟A，根據(jù)陀 螺儀輸出的角速度、加速度計輸出的加速度、磁力計輸出的磁場強(qiáng)度進(jìn)行導(dǎo)航信息解算， 經(jīng)導(dǎo)航解算后得到姿態(tài)角、速度、位置信息的導(dǎo)航數(shù)據(jù)；
[0009] 步驟B，根據(jù)導(dǎo)航解算出的導(dǎo)航數(shù)據(jù)，判斷因俯仰或橫滾運動，使得姿態(tài)角交叉耦 合明顯增大而造成的姿態(tài)角解算錯誤的節(jié)點，用姿態(tài)角交叉耦合造成解算錯誤的前一步解 算數(shù)據(jù)重新計算，得出新的姿態(tài)角、速度、位置信息；
[0010] 步驟C，步驟B中回溯解耦后的姿態(tài)角、速度及位置信息經(jīng)前置濾波器去噪后，一 方面利用誤差方程得到狀態(tài)量，一方面和步驟A中經(jīng)導(dǎo)航解算后得到姿態(tài)角、速度、位置信 息分別作差得到觀測量；將該狀態(tài)量和觀測量進(jìn)入基于卡爾曼的自適應(yīng)濾波算法，進(jìn)行姿 態(tài)角、速度、位置誤差的最優(yōu)估計，所得誤差估計值校正慣導(dǎo)所得的導(dǎo)航信息，最終得到校 正后準(zhǔn)確的姿態(tài)角、速度和位置。
[0011] 所述步驟B中的回溯解耦方法包括以下步驟：
[0012] 步驟B1，根據(jù)導(dǎo)航解算得出的導(dǎo)航數(shù)據(jù)，判斷因俯仰或橫滾運動，使得姿態(tài)角交叉 耦合明顯增大而造成的姿態(tài)角解算錯誤的節(jié)點；
[0013] 步驟B2,用因俯仰或橫滾運動，使得姿態(tài)角交叉耦合明顯增大而造成解算錯誤的 前一步解算數(shù)據(jù)，計算載體坐標(biāo)系相對導(dǎo)航坐標(biāo)系的轉(zhuǎn)動角速率在載體坐標(biāo)系上的投影；
[0014] 步驟B3,用載體坐標(biāo)系相對導(dǎo)航坐標(biāo)系的轉(zhuǎn)動角速率在載體坐標(biāo)系上的投影通過 解算得到新的姿態(tài)角、速度、位置信息。
[0015] 所述步驟Bl中判斷因俯仰或橫滾運動，使得姿態(tài)角交叉耦合明顯增大而造成的 姿態(tài)角解算錯誤的節(jié)點的方法，包括以下步驟：
[0016] 步驟B11，因俯仰或橫滾運動，引起的姿態(tài)角交叉耦合而造成三個失準(zhǔn)角 4>z錯誤，其中4>z分別為載體坐標(biāo)系的三軸偏離導(dǎo)航坐標(biāo)系對應(yīng)的三軸的 偏離角，也即失準(zhǔn)角；
[0017] 步驟B12,將三個失準(zhǔn)角（K，七，叭代入姿態(tài)角校正方程 其中C!:是錯誤解算節(jié)點前一步正確的導(dǎo)航坐標(biāo)系到載體坐標(biāo)系的姿態(tài)角矩陣，

【權(quán)利要求】
1. 一種用于水下滑翔器的導(dǎo)航系統(tǒng)姿態(tài)角校正方法，其特征在于，包括以下步驟： 步驟A，根據(jù)陀螺儀輸出的角速度、加速度計輸出的加速度、磁力計輸出的磁場強(qiáng)度進(jìn) 行導(dǎo)航信息解算，經(jīng)導(dǎo)航解算后得到姿態(tài)角、速度、位置信息的導(dǎo)航數(shù)據(jù)； 步驟B，根據(jù)導(dǎo)航解算出的導(dǎo)航數(shù)據(jù)，判斷因俯仰或橫滾運動，使得姿態(tài)角交叉耦合明 顯增大而造成的姿態(tài)角解算錯誤的節(jié)點，用姿態(tài)角交叉耦合造成解算錯誤的前一步解算數(shù) 據(jù)重新計算，得出新的姿態(tài)角、速度、位置信息； 步驟C，步驟B中回溯解耦后的姿態(tài)角、速度及位置信息經(jīng)前置濾波器去噪后，一方面 利用誤差方程得到狀態(tài)量，一方面和步驟A中經(jīng)導(dǎo)航解算后得到姿態(tài)角、速度、位置信息分 別作差得到觀測量；將該狀態(tài)量和觀測量進(jìn)入基于卡爾曼的自適應(yīng)濾波算法，進(jìn)行姿態(tài)角、 速度、位置誤差的最優(yōu)估計，所得誤差估計值校正慣導(dǎo)所得的導(dǎo)航信息，最終得到校正后準(zhǔn) 確的姿態(tài)角、速度和位置。
2. 根據(jù)權(quán)利要求1所述的一種用于水下滑翔器的導(dǎo)航系統(tǒng)姿態(tài)角校正方法，其特征在 于：所述步驟B中的回溯解耦方法包括以下步驟： 步驟B1，根據(jù)導(dǎo)航解算得出的導(dǎo)航數(shù)據(jù)，判斷因俯仰或橫滾運動，使得姿態(tài)角交叉耦合 明顯增大而造成的姿態(tài)角解算錯誤的節(jié)點； 步驟B2,用因俯仰或橫滾運動，使得姿態(tài)角交叉耦合明顯增大而造成解算錯誤的前一 步解算數(shù)據(jù)，計算載體坐標(biāo)系相對導(dǎo)航坐標(biāo)系的轉(zhuǎn)動角速率在載體坐標(biāo)系上的投影； 步驟B3,用載體坐標(biāo)系相對導(dǎo)航坐標(biāo)系的轉(zhuǎn)動角速率在載體坐標(biāo)系上的投影通過解算 得到新的姿態(tài)角、速度、位置信息。
3. 根據(jù)權(quán)利要求2所述的一種用于水下滑翔器的導(dǎo)航系統(tǒng)姿態(tài)角校正方法，其特征在 于：步驟B1中判斷因俯仰或橫滾運動，使得姿態(tài)角交叉耦合明顯增大而造成的姿態(tài)角解算 錯誤的節(jié)點的方法，包括以下步驟： 步驟B11，因俯仰或橫滾運動，使得姿態(tài)角交叉耦合而造成三個失準(zhǔn)角（K，小y，錯 誤，其中<K，小y，分別為載體坐標(biāo)系的三軸偏離導(dǎo)航坐標(biāo)系對應(yīng)的三軸的偏離角，也即 失準(zhǔn)角； 步驟B12,將三個失準(zhǔn)角（K，小y，代入姿態(tài)角校正方程
其中 < 是錯誤 解算節(jié)點前一步正確的導(dǎo)航坐標(biāo)系到載體坐標(biāo)系的姿態(tài)角矩陣，
為姿態(tài)校正矩陣； 步驟B13,根據(jù)姿態(tài)角校正方程可得出四元數(shù)方程：
步驟B14,判斷式（11)中的根號下的計算結(jié)果是否是負(fù)數(shù)，若是負(fù)數(shù)，則四元數(shù)q(l、 qi、q2、q3錯誤，后續(xù)的導(dǎo)航解算會依次錯誤；從而得出上述式（11)中根號下計算結(jié)果出現(xiàn)負(fù)數(shù) 是因姿態(tài)角交叉耦合造成導(dǎo)航解算錯誤的節(jié)點。
4. 根據(jù)權(quán)利要求3所述的一種用于水下滑翔器的導(dǎo)航系統(tǒng)姿態(tài)角校正方法，其特征在 于：所述步驟B2中載體坐標(biāo)系相對導(dǎo)航坐標(biāo)系的轉(zhuǎn)動角速率在載體坐標(biāo)系上的投影為：
其中，<0^為載體坐標(biāo)系相對導(dǎo)航坐標(biāo)系的轉(zhuǎn)動角速率在載體坐標(biāo)系上的投影為 陀螺儀輸出的角速率；€<.為地球坐標(biāo)系相對于慣性坐標(biāo)系的自轉(zhuǎn)角速率在導(dǎo)航坐標(biāo)系中 的投影；為導(dǎo)航坐標(biāo)系相對于地球坐標(biāo)系的角速率在導(dǎo)航坐標(biāo)系上的投影。
5. 根據(jù)權(quán)利要求4所述的一種用于水下滑翔器的導(dǎo)航系統(tǒng)姿態(tài)角校正方法，其特征在 于：所述步驟B3中用載體坐標(biāo)系相對導(dǎo)航坐標(biāo)系的轉(zhuǎn)動角速率在載體坐標(biāo)系上的投影通 過解算得出新的姿態(tài)角、速度、位置信息的方法，包括以下步驟：
上的分量； 步驟B32,四元數(shù)是由四個元素構(gòu)成，將其定義為zqa+qii+qj+qhq。、 1、q2、q3是實數(shù)，i、j、k是相互正交的單位向量，采用畢卡逼近方法求解四元數(shù)微分方程：
其中，Q(tk+1)、Q(tk)分別代表tk+1、tk時刻的四元數(shù)向量，將式（14)進(jìn)行泰勒級數(shù)展開 得：
(15) 將式（15)寫成三角形式：
其中： A0x、A0y、A0z分別為陀螺的X、y、Z軸在[tktk+1]
采樣時間間隔內(nèi)的角增量；故歸一化后的四元數(shù)： f J
步驟B33,用新的四元數(shù)秦更新姿態(tài)矩陣：
更新三個姿態(tài)角：
用準(zhǔn)確的姿態(tài)變換矩陣^丨代入公式
中，算得比力fn，從而計算出新的速度及 位置。
6. -種基于權(quán)利要求1所述的用于水下滑翔器的導(dǎo)航系統(tǒng)，其特征在于：包括DSP處 理單元、MEMSMU導(dǎo)航元件；所述DSP處理單元包括存儲模塊、回溯解耦模塊、前置濾波去 噪模塊、誤差模塊、觀測量模塊以及基于卡爾曼的自適應(yīng)濾波算法模塊；所述MEMSIMU導(dǎo) 航元件用于采集原始的角速度、加速度及磁場數(shù)據(jù)，所有原始數(shù)據(jù)進(jìn)入DSP處理單元進(jìn)行 解算，得到姿態(tài)角、速度及位置信息，并將得到的姿態(tài)角、速度及位置信息傳送給DSP處理 單元； 所述DSP處理單元用于接收MEMSMU導(dǎo)航元件傳送過來的姿態(tài)角、速度及位置信息， 并將該姿態(tài)角、速度及位置信息傳送給回溯解耦模塊和觀測量模塊； 所述回溯解耦模塊用于根據(jù)回溯解耦方法，將正常解算過程中解算的導(dǎo)航信息存儲起 來，當(dāng)解算過程錯誤判斷后，姿態(tài)角發(fā)生奇異突變，調(diào)用上一次正確解算的導(dǎo)航參數(shù)進(jìn)行重 新解算，得出新的姿態(tài)角、速度及位置信息并對其進(jìn)行更新，同時將更新后的姿態(tài)角、速度 及位置信息發(fā)送給前置濾波去噪模塊；若姿態(tài)角未發(fā)生奇異突變，則將姿態(tài)角、速度及位置 信息發(fā)送給前置濾波去噪模塊； 所述濾波去噪模塊用于接收回溯解耦模塊傳送過來姿態(tài)角、速度及位置信息；同時將 姿態(tài)角、速度和位置信息進(jìn)行濾波去噪，并將濾波去噪后的姿態(tài)角、速度和位置信息分別傳 送給誤差模塊和觀測量模塊； 所述誤差模塊用于根據(jù)濾波去噪模塊傳送的姿態(tài)角、速度和位置信息利用誤差方程求 導(dǎo)航信息誤差得到狀態(tài)量，并將該狀態(tài)量傳送給基于卡爾曼的自適應(yīng)濾波算法模塊； 所述觀測量模塊用于根據(jù)DSP處理單元推送的姿態(tài)角、速度及位置信息與濾波去噪模 塊推送的姿態(tài)角、速度及位置信息作差作為濾波器的觀測量，并將該觀測量傳送給基于卡 爾曼的自適應(yīng)濾波算法模塊； 所述基于卡爾曼的自適應(yīng)濾波算法模塊用于根據(jù)狀態(tài)量和觀測量進(jìn)行姿態(tài)角、速度、 位置誤差的最優(yōu)估計，再用所得誤差的估計值校正慣導(dǎo)所得的姿態(tài)角、速度及位置，最終得 到校正后準(zhǔn)確的姿態(tài)角、速度和位置信息。
7. -種基于權(quán)利要求1所述的用于水下滑翔器的導(dǎo)航系統(tǒng)及姿態(tài)角回溯解耦方法，其 特征在于，包括以下步驟： 步驟一，根據(jù)陀螺儀輸出的角速度、加速度計輸出的加速度、磁力計輸出的磁場強(qiáng)度進(jìn) 行導(dǎo)航信息解算，經(jīng)導(dǎo)航解算后得到姿態(tài)角、速度、位置信息的導(dǎo)航數(shù)據(jù)； 步驟二，根據(jù)導(dǎo)航解算得出的導(dǎo)航數(shù)據(jù)，判斷因俯仰或橫滾運動，引起的姿態(tài)角交叉耦 合而造成的姿態(tài)角解算錯誤的節(jié)點； 步驟三，用因俯仰或橫滾運動，使得姿態(tài)角交叉耦合明顯增大而造成解算錯誤的前一 步解算數(shù)據(jù)，計算載體坐標(biāo)系相對導(dǎo)航坐標(biāo)系的轉(zhuǎn)動角速率在載體坐標(biāo)系上的投影； 步驟四，用載體坐標(biāo)系相對導(dǎo)航坐標(biāo)系的轉(zhuǎn)動角速率在載體坐標(biāo)系上的投影通過解算 得到新的姿態(tài)角、速度、位置信息。
8. 根據(jù)權(quán)利要求1所述7的用于水下滑翔器的導(dǎo)航系統(tǒng)及姿態(tài)角回溯解耦方法，其特 征在于：步驟二中判斷因為俯仰或橫滾運動而使得姿態(tài)角交叉耦合明顯增大導(dǎo)致姿態(tài)角解 算錯誤的節(jié)點的方法，包括以下步驟： 步驟二a，因俯仰或橫滾運動，引起的姿態(tài)角交叉耦合而造成三個失準(zhǔn)角（^，小y， 錯誤，其中小y，分別為載體坐標(biāo)系的三軸偏離導(dǎo)航坐標(biāo)系對應(yīng)的三軸的偏離角，也 即失準(zhǔn)角； 步驟二b，將三個失準(zhǔn)角（K，小y，代入姿態(tài)角校正方程
，其中g(shù)是錯誤 解算節(jié)點前一步正確的導(dǎo)航坐標(biāo)系到載體坐標(biāo)系的姿態(tài)角矩陣，
為姿態(tài)校正矩陣； 步驟二c，根據(jù)姿態(tài)角校正方程可得出四元數(shù)方程：
步驟二d，判斷式（11)中的根號下的計算結(jié)果是否是負(fù)數(shù)，若是負(fù)數(shù)，則四元數(shù)q(l、ql、q2、q3錯誤，后續(xù)的導(dǎo)航解算會依次錯誤，則上述式（11)中根號下計算結(jié)果出現(xiàn)負(fù)數(shù)是因姿 態(tài)角交叉稱合造成導(dǎo)航解算錯誤的節(jié)點。
9. 根據(jù)權(quán)利要求1所述7的一種用于水下滑翔器的導(dǎo)航系統(tǒng)及姿態(tài)角回溯解耦方法， 其特征在于：所述步驟三中載體坐標(biāo)系相對導(dǎo)航坐標(biāo)系的轉(zhuǎn)動角速率在載體坐標(biāo)系上的投 影為：
其中，為載體坐標(biāo)系相對導(dǎo)航坐標(biāo)系的轉(zhuǎn)動角速率在載體坐標(biāo)系上的投影； 為陀螺儀輸出的角速率；為地球坐標(biāo)系相對于慣性坐標(biāo)系的自轉(zhuǎn)角速率在導(dǎo)航坐標(biāo)系 中的投影；為導(dǎo)航坐標(biāo)系相對于地球坐標(biāo)系的角速率在導(dǎo)航坐標(biāo)系上的投影；C;是錯 誤解算節(jié)點前一步正確的導(dǎo)航坐標(biāo)系到載體坐標(biāo)系的姿態(tài)矩陣。
10. 根據(jù)權(quán)利要求1所述7的一種用于水下滑翔器的導(dǎo)航系統(tǒng)及姿態(tài)角回溯解耦方法， 其特征在于：所述步驟四中通過載體坐標(biāo)系相對導(dǎo)航坐標(biāo)系的轉(zhuǎn)動角速率在載體坐標(biāo)系上 的投影導(dǎo)航得出新的姿態(tài)、速度、位置信息的方法，包括以下步驟：
軸上的分量； 步驟四b，四元數(shù)是由四個元素構(gòu)成，將其定義為=qa+qj+qd+q#，， %、qi、q2、q3是實數(shù)，i、j、k是相互正交的單位向量，采用畢卡逼近方法求解四元數(shù)微分方 程：
其中，Q(tk+1)、Q(tk)分別代表tk+1、tk時刻的四元數(shù)向量，將式（14)進(jìn)行泰勒級數(shù)展開 得：
將式（15)寫成三角形式：
其中，
，A0 x、A0 y、A0z分別為陀螺的X、y、Z軸在[tktk+1] 采樣時間間隔內(nèi)的角增量； 故歸一化后的四元數(shù)：
步驟四c，用新的四元數(shù)備更新姿態(tài)矩陣：
更新三個姿態(tài)角：
用準(zhǔn)確的姿態(tài)變換矩陣￡5代入公式
中，算得比力fn，從而計算出新的速度及 位置。
【文檔編號】G01C21/20GK104406592SQ201410712437
【公開日】2015年3月11日 申請日期:2014年11月28日 優(yōu)先權(quán)日:2014年11月28日
【發(fā)明者】陳熙源, 黃浩乾, 周智愷, 呂才平 申請人:東南大學(xué)