一種基于互補濾波的水下滑翔器節能算法的制作方法
本發明涉及數字信號處理領域，尤其是一種基于互補濾波的水下滑翔器節能方法。
背景技術：
水下滑翔器是用于海洋的科學研究以及海洋資源的開發的一種重要工具。水下滑翔器可搭載多種傳感器，通過長時間在被測區域低速航行，為海洋科學研究提供關鍵基礎數據，還可以對海底地形地貌進行考察，對水下資源進行探測以及對石油管線進行探查等作業任務。
在水下滑翔器攜帶的能源一定的情況下，影響水下滑翔器功耗的主要因素有耗電設備的功率、運行的路徑以及巡航的速度。水下滑翔器在水下工作時，搭載的各種探測設備和作業設備所需要的能源大部分都由自身所攜帶的電源供應，所以能耗決定了水下滑翔器的航行范圍和工作時間，而水下滑翔器體積有限，限制了所能攜帶的電池。除此之外，水下滑翔器的工作環境復雜，更換電池極為不便，因此降低功耗對水下滑翔器具有十分重要的意義。對于水下滑翔器來說，一次下潛所攜帶的能源是有限的，如何優化控制算法，提高能源利用效率也是低功耗水下滑翔器研究的重要問題之一。水下滑翔器的工作深度從幾米到幾千米不等，同時還會受到潮汐、海浪以及海底地貌的影響，這導致了控制模型十分復雜，對控制技術的要求更高。
基于上述水下滑翔器存在功耗的問題，除了關注在有限空間和重量下提供更多的電能，發展具有工作時間長、比能量高的電池以外，通過提高智能控制和算法的效率來提升滑翔機的續航能力更應該得到重視。
技術實現要素：
發明目的：為解決上述技術問題，本發明提供一種基于互補濾波的水下滑翔器節能方法。本發明根據滑翔器的運動特征，選擇性地開啟和關閉陀螺儀和加速度計(imu)。具體來說，根據多普勒計程儀得到的速度信息判斷滑翔器的運動狀態，若屬于高動態，則開啟imu。另一種情況是，當滑翔器下潛或上浮時，即滑翔器作特有的“z”字型運動時，此時也要開啟imu，以便能夠準確到達預定的坐標。當打開imu時，通過互補濾波融合磁力計和imu的信息，以提高導航精度。
技術方案：為實現上述技術效果，本發明提供的技術方案為：
一種基于互補濾波的水下滑翔器節能方法，該方法根據水下滑翔器的運動特征，選擇性地開啟加速度計，在加速度計開啟時，通過閉環濾波算法調整水下滑翔器的導航參數；所述水下滑翔器搭載的導航設備包括：多普勒計程儀、加速度計、陀螺儀和磁力計；該方法包括步驟：
(1)判定滑翔器的運動狀態，包括步驟：
通過多普勒計程儀實時采集水下滑翔器的速度信息；根據速度信息判斷水下滑翔器的運動狀態；當水下滑翔器處于高動態，則開啟加速度計和陀螺儀，并執行步驟(2)；否則，關閉加速度計和陀螺儀，執行步驟(6)；
(2)根據磁力計測量得到的水下滑翔器運行過程中的歸一化地磁值mb和當前姿態矩陣
式中，m表示水下滑翔器所在地的地磁值在導航坐標系下的投影值；
對e1進行增益和積分運算補償，得到e1對應的補償后角速度誤差量為：
式中，kp1為比例增益系數，ki1為積分增益系數；
(3)執行步驟(2)的同時，根據加速度計測量到的水下滑翔器的歸一化加速度值和當前姿態矩陣
式中，
對e2進行增益和積分運算補償，得到e2對應的補償后角速度誤差量為：
式中，kp2為比例增益系數，ki2為積分增益系數；
(4)通過陀螺儀測量得到當前水下滑翔器的角速度ω，根據步驟(2)和步驟(3)得到的δωm和δωf計算角速度的修正值為：ω′＝ω+δωm+δωf；
(5)根據角速度的修正值更新當前的姿態矩陣為：
式中，q0(k)、q1(k)、q2(k)和q3(k)分別為水下滑翔器姿態的四元數在時刻k的離散值；返回步驟(1)；
(6)當水下滑翔器關閉加速度計時，則根據磁力計測得的歸一化地磁值和多普勒計程儀測得的水下滑翔器速度信息推算導航參數。
進一步的，所述步驟(1)中根據速度信息判斷水下滑翔器的運動狀態的方法為：
當水下滑翔器的速度大于預先設定的速度閾值，或水下滑翔器進行上浮、下潛或設定的“z”字運動時，判定水下滑翔器處于高動態；否則，判定水下滑翔器處于非高動態，即正常滑行狀態。
進一步的，所述步驟(5)中根據角速度的修正值更新當前的姿態矩陣的步驟為：
(5-1)將角速度的修正值轉換為矩陣形式為：
式中，ω′x、ω′y、ω′z分別表示角速度的修正值ω′在x軸、y軸和z軸的分量；ωx、ωy、ωz分別表示ω在x軸、y軸和z軸的分量；δωmx、δωmy、δωmx分別表示δωm在x軸、y軸和z軸的分量；δωfx、δωfy、δωfz分別表示δωf在x軸、y軸和z軸的分量；
(5-2)根據角速度的修正值矩陣計算四元數微分方程為：
式中，
(5-3)將四元數微分方程進行離散化，得到：
式中，δt表示單位采樣時間；
(5-4)根據四元數微分方程的離散化方程，計算當前的姿態矩陣為：
進一步的，所述步驟(6)中推算導航參數的方法為：
磁力計在不開啟imu時用于測量水下滑翔器的姿態角；多普勒計程儀用于測量滑翔器的實時速率；根據測得的水下滑翔器姿態角和實時速率計算時刻n水下滑翔器的位置為(xn，yn)，方向角為αn；其中，
式中，x0、y0表示水下滑翔器的初始坐標；θi表示水下滑翔器在時刻i的方向角；vi表示水下滑翔器在時刻i的速率；
有益效果：與現有技術相比，本發明為了提高水下滑翔器的工作時間，選擇性地開啟和關閉imu，這在很大程度上提高了滑翔器的續航能力；另外，采用創新的互補濾波信息融合算法提高了滑翔器導航系統的工作精度，保證了在imu開啟后對沒有imu推算導航的精度提升。
附圖說明
圖1為基于互補濾波的水下滑翔器節能算法的流程。
圖2為基于互補濾波的水下滑翔器節能算法框圖。
具體實施方式
下面結合附圖對本發明作更進一步的說明。
圖2為基于互補濾波的水下滑翔器節能算法框圖，首先由水下滑翔器控制器的指令或多普勒計程儀判斷出水下滑翔器的動態信息，決定是否需要開啟imu，默認初始狀態時關閉imu。然后分別對加速度計和磁力計的角速度測量值、角速度計算值進行叉乘求取誤差，對求得的誤差進行增益和積分補償。對上述補償后的角速度和陀螺輸出的角速度進行求和并帶入四元數微分方程，得到更新的姿態矩陣。把得到的姿態矩陣反帶入加速度計和磁力計的計算公式，再次求取角速度誤差，形成閉環互補濾波。
圖1描述了基于互補濾波的水下滑翔器節能算法的流程，具體步驟為：a)判斷是否打開imu。b)叉乘求取加速度計、磁力計的測量和計算誤差。c)對誤差進行pi補償。d)補償后的誤差和陀螺輸出求和。e)帶入四元數微分方程并得到更新姿態矩陣。f)返回到b步驟進行閉環互補濾波。
本發明的具體流程如下：
(1)判定滑翔器的運動狀態，包括步驟：
通過多普勒計程儀實時采集水下滑翔器的速度信息；根據速度信息判斷水下滑翔器的運動狀態：當水下滑翔器的速度大于預先設定的速度閾值，或水下滑翔器進行上浮、下潛或設定的“z”字運動時，判定水下滑翔器處于高動態；否則，判定水下滑翔器處于非高動態，即正常滑行狀態。
當水下滑翔器處于高動態，則開啟加速度計和陀螺儀，并執行步驟(2)；否則，關閉加速度計和陀螺儀，執行步驟(6)；
(2)根據磁力計測量得到的水下滑翔器運行過程中的歸一化地磁值mb和當前姿態矩陣
式中，m表示水下滑翔器所在地的地磁值在導航坐標系下的投影值；
對e1進行增益和積分運算補償，得到e1對應的補償后角速度誤差量為：
式中，kp1為比例增益系數，ki1為積分增益系數；
(3)執行步驟(2)的同時，根據加速度計測量到的水下滑翔器的歸一化加速度值和當前姿態矩陣
式中，
對e2進行增益和積分運算補償，得到e2對應的補償后角速度誤差量為：
式中，kp2為比例增益系數，ki2為積分增益系數；
(4)通過陀螺儀測量得到當前水下滑翔器的角速度ω，根據步驟(2)和步驟(3)得到的δωm和δωf計算角速度的修正值為：ω′＝ω+δωm+δωf；
(5)根據角速度的修正值更新當前的姿態矩陣，包括步驟：
(5-1)將角速度的修正值轉換為矩陣形式為：
式中，ω′x、ω′y、ω′z分別表示角速度的修正值ω′在x軸、y軸和z軸的分量；ωx、ωy、ωz分別表示ω在x軸、y軸和z軸的分量；δωmx、δωmy、δωmx分別表示δωm在x軸、y軸和z軸的分量；δωfx、δωfy、δωfz分別表示δωf在x軸、y軸和z軸的分量；
(5-2)根據角速度的修正值矩陣計算四元數微分方程為：
式中，
(5-3)將四元數微分方程進行離散化，得到：
式中，δt表示單位采樣時間；
(5-4)根據四元數微分方程的離散化方程，計算當前的姿態矩陣為：
式中，q0(k)、q1(k)、q2(k)和q3(k)分別為水下滑翔器姿態的四元數在時刻k的離散值；返回步驟(1)；
(6)當水下滑翔器關閉加速度計時，則根據磁力計測得的歸一化地磁值和多普勒計程儀測得的水下滑翔器速度信息推算導航參數，步驟為：
對于未開啟imu的情況，需要借助磁力計和多普勒計程儀進行推算導航，定義第n時刻水下滑翔器的位置坐標為(xn，yn)，方向角為θn，則有下公式(a)，其中di為單位時間內的水下滑翔器的位移。
其中，αn為水下滑翔器在時刻n的方向角，ωi為水下滑翔器單位時間的角速度，(x0，y0)為水下滑翔器的初始位置坐標。
磁力計在不開啟imu時用于測量水下滑翔器的姿態角，定文
其中，δt為單位(采樣)時間，vi為水下滑翔器在時刻i的速度，dyi/dxi為水下滑翔器在時刻i的縱軸、橫軸位移比。
以上所述僅是本發明的優選實施方式，應當指出：對于本技術領域的普通技術人員來說，在不脫離本發明原理的前提下，還可以做出若干改進和潤飾，這些改進和潤飾也應視為本發明的保護范圍。
- 還沒有人留言評論。精彩留言會獲得點贊！