本發(fā)明涉及數(shù)字信號(hào)處理領(lǐng)域，尤其是一種基于互補(bǔ)濾波的水下滑翔器節(jié)能方法。

背景技術(shù)：

水下滑翔器是用于海洋的科學(xué)研究以及海洋資源的開發(fā)的一種重要工具。水下滑翔器可搭載多種傳感器，通過長時(shí)間在被測(cè)區(qū)域低速航行，為海洋科學(xué)研究提供關(guān)鍵基礎(chǔ)數(shù)據(jù)，還可以對(duì)海底地形地貌進(jìn)行考察，對(duì)水下資源進(jìn)行探測(cè)以及對(duì)石油管線進(jìn)行探查等作業(yè)任務(wù)。

在水下滑翔器攜帶的能源一定的情況下，影響水下滑翔器功耗的主要因素有耗電設(shè)備的功率、運(yùn)行的路徑以及巡航的速度。水下滑翔器在水下工作時(shí)，搭載的各種探測(cè)設(shè)備和作業(yè)設(shè)備所需要的能源大部分都由自身所攜帶的電源供應(yīng)，所以能耗決定了水下滑翔器的航行范圍和工作時(shí)間，而水下滑翔器體積有限，限制了所能攜帶的電池。除此之外，水下滑翔器的工作環(huán)境復(fù)雜，更換電池極為不便，因此降低功耗對(duì)水下滑翔器具有十分重要的意義。對(duì)于水下滑翔器來說，一次下潛所攜帶的能源是有限的，如何優(yōu)化控制算法，提高能源利用效率也是低功耗水下滑翔器研究的重要問題之一。水下滑翔器的工作深度從幾米到幾千米不等，同時(shí)還會(huì)受到潮汐、海浪以及海底地貌的影響，這導(dǎo)致了控制模型十分復(fù)雜，對(duì)控制技術(shù)的要求更高。

基于上述水下滑翔器存在功耗的問題，除了關(guān)注在有限空間和重量下提供更多的電能，發(fā)展具有工作時(shí)間長、比能量高的電池以外，通過提高智能控制和算法的效率來提升滑翔機(jī)的續(xù)航能力更應(yīng)該得到重視。

技術(shù)實(shí)現(xiàn)要素：

發(fā)明目的：為解決上述技術(shù)問題，本發(fā)明提供一種基于互補(bǔ)濾波的水下滑翔器節(jié)能方法。本發(fā)明根據(jù)滑翔器的運(yùn)動(dòng)特征，選擇性地開啟和關(guān)閉陀螺儀和加速度計(jì)(imu)。具體來說，根據(jù)多普勒計(jì)程儀得到的速度信息判斷滑翔器的運(yùn)動(dòng)狀態(tài)，若屬于高動(dòng)態(tài)，則開啟imu。另一種情況是，當(dāng)滑翔器下潛或上浮時(shí)，即滑翔器作特有的“z”字型運(yùn)動(dòng)時(shí)，此時(shí)也要開啟imu，以便能夠準(zhǔn)確到達(dá)預(yù)定的坐標(biāo)。當(dāng)打開imu時(shí)，通過互補(bǔ)濾波融合磁力計(jì)和imu的信息，以提高導(dǎo)航精度。

技術(shù)方案：為實(shí)現(xiàn)上述技術(shù)效果，本發(fā)明提供的技術(shù)方案為：

一種基于互補(bǔ)濾波的水下滑翔器節(jié)能方法，該方法根據(jù)水下滑翔器的運(yùn)動(dòng)特征，選擇性地開啟加速度計(jì)，在加速度計(jì)開啟時(shí)，通過閉環(huán)濾波算法調(diào)整水下滑翔器的導(dǎo)航參數(shù)；所述水下滑翔器搭載的導(dǎo)航設(shè)備包括：多普勒計(jì)程儀、加速度計(jì)、陀螺儀和磁力計(jì)；該方法包括步驟：

(1)判定滑翔器的運(yùn)動(dòng)狀態(tài)，包括步驟：

通過多普勒計(jì)程儀實(shí)時(shí)采集水下滑翔器的速度信息；根據(jù)速度信息判斷水下滑翔器的運(yùn)動(dòng)狀態(tài)；當(dāng)水下滑翔器處于高動(dòng)態(tài)，則開啟加速度計(jì)和陀螺儀，并執(zhí)行步驟(2)；否則，關(guān)閉加速度計(jì)和陀螺儀，執(zhí)行步驟(6)；

(2)根據(jù)磁力計(jì)測(cè)量得到的水下滑翔器運(yùn)行過程中的歸一化地磁值mb和當(dāng)前姿態(tài)矩陣計(jì)算磁力計(jì)反饋的水下滑翔器的角速度誤差e1；

式中，m表示水下滑翔器所在地的地磁值在導(dǎo)航坐標(biāo)系下的投影值；

對(duì)e1進(jìn)行增益和積分運(yùn)算補(bǔ)償，得到e1對(duì)應(yīng)的補(bǔ)償后角速度誤差量為：

式中，kp1為比例增益系數(shù)，ki1為積分增益系數(shù)；

(3)執(zhí)行步驟(2)的同時(shí)，根據(jù)加速度計(jì)測(cè)量到的水下滑翔器的歸一化加速度值和當(dāng)前姿態(tài)矩陣計(jì)算加速度計(jì)反饋的水下滑翔器的角速度誤差e2；

式中，為水下滑翔器所在地的重力加速度在航坐標(biāo)系下的投影值；

對(duì)e2進(jìn)行增益和積分運(yùn)算補(bǔ)償，得到e2對(duì)應(yīng)的補(bǔ)償后角速度誤差量為：

式中，kp2為比例增益系數(shù)，ki2為積分增益系數(shù)；

(4)通過陀螺儀測(cè)量得到當(dāng)前水下滑翔器的角速度ω，根據(jù)步驟(2)和步驟(3)得到的δωm和δωf計(jì)算角速度的修正值為：ω′＝ω+δωm+δωf；

(5)根據(jù)角速度的修正值更新當(dāng)前的姿態(tài)矩陣為：

式中，q0(k)、q1(k)、q2(k)和q3(k)分別為水下滑翔器姿態(tài)的四元數(shù)在時(shí)刻k的離散值；返回步驟(1)；

(6)當(dāng)水下滑翔器關(guān)閉加速度計(jì)時(shí)，則根據(jù)磁力計(jì)測(cè)得的歸一化地磁值和多普勒計(jì)程儀測(cè)得的水下滑翔器速度信息推算導(dǎo)航參數(shù)。

進(jìn)一步的，所述步驟(1)中根據(jù)速度信息判斷水下滑翔器的運(yùn)動(dòng)狀態(tài)的方法為：

當(dāng)水下滑翔器的速度大于預(yù)先設(shè)定的速度閾值，或水下滑翔器進(jìn)行上浮、下潛或設(shè)定的“z”字運(yùn)動(dòng)時(shí)，判定水下滑翔器處于高動(dòng)態(tài)；否則，判定水下滑翔器處于非高動(dòng)態(tài)，即正?；袪顟B(tài)。

進(jìn)一步的，所述步驟(5)中根據(jù)角速度的修正值更新當(dāng)前的姿態(tài)矩陣的步驟為：

(5-1)將角速度的修正值轉(zhuǎn)換為矩陣形式為：

式中，ω′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)根據(jù)角速度的修正值矩陣計(jì)算四元數(shù)微分方程為：

式中，分別表示四元數(shù)q0、q1、q2、q3的導(dǎo)數(shù)；

(5-3)將四元數(shù)微分方程進(jìn)行離散化，得到：

式中，δt表示單位采樣時(shí)間；

(5-4)根據(jù)四元數(shù)微分方程的離散化方程，計(jì)算當(dāng)前的姿態(tài)矩陣為：

進(jìn)一步的，所述步驟(6)中推算導(dǎo)航參數(shù)的方法為：

磁力計(jì)在不開啟imu時(shí)用于測(cè)量水下滑翔器的姿態(tài)角；多普勒計(jì)程儀用于測(cè)量滑翔器的實(shí)時(shí)速率；根據(jù)測(cè)得的水下滑翔器姿態(tài)角和實(shí)時(shí)速率計(jì)算時(shí)刻n水下滑翔器的位置為(xn，yn)，方向角為αn；其中，

式中，x0、y0表示水下滑翔器的初始坐標(biāo)；θi表示水下滑翔器在時(shí)刻i的方向角；vi表示水下滑翔器在時(shí)刻i的速率；表示水下滑翔器在時(shí)刻i的姿態(tài)角；dyn/dxn表示水下滑翔器在時(shí)刻n的x軸、y軸的位移之比。

有益效果：與現(xiàn)有技術(shù)相比，本發(fā)明為了提高水下滑翔器的工作時(shí)間，選擇性地開啟和關(guān)閉imu，這在很大程度上提高了滑翔器的續(xù)航能力；另外，采用創(chuàng)新的互補(bǔ)濾波信息融合算法提高了滑翔器導(dǎo)航系統(tǒng)的工作精度，保證了在imu開啟后對(duì)沒有imu推算導(dǎo)航的精度提升。

附圖說明

圖1為基于互補(bǔ)濾波的水下滑翔器節(jié)能算法的流程。

圖2為基于互補(bǔ)濾波的水下滑翔器節(jié)能算法框圖。

具體實(shí)施方式

下面結(jié)合附圖對(duì)本發(fā)明作更進(jìn)一步的說明。

圖2為基于互補(bǔ)濾波的水下滑翔器節(jié)能算法框圖，首先由水下滑翔器控制器的指令或多普勒計(jì)程儀判斷出水下滑翔器的動(dòng)態(tài)信息，決定是否需要開啟imu，默認(rèn)初始狀態(tài)時(shí)關(guān)閉imu。然后分別對(duì)加速度計(jì)和磁力計(jì)的角速度測(cè)量值、角速度計(jì)算值進(jìn)行叉乘求取誤差，對(duì)求得的誤差進(jìn)行增益和積分補(bǔ)償。對(duì)上述補(bǔ)償后的角速度和陀螺輸出的角速度進(jìn)行求和并帶入四元數(shù)微分方程，得到更新的姿態(tài)矩陣。把得到的姿態(tài)矩陣反帶入加速度計(jì)和磁力計(jì)的計(jì)算公式，再次求取角速度誤差，形成閉環(huán)互補(bǔ)濾波。

圖1描述了基于互補(bǔ)濾波的水下滑翔器節(jié)能算法的流程，具體步驟為：a)判斷是否打開imu。b)叉乘求取加速度計(jì)、磁力計(jì)的測(cè)量和計(jì)算誤差。c)對(duì)誤差進(jìn)行pi補(bǔ)償。d)補(bǔ)償后的誤差和陀螺輸出求和。e)帶入四元數(shù)微分方程并得到更新姿態(tài)矩陣。f)返回到b步驟進(jìn)行閉環(huán)互補(bǔ)濾波。

本發(fā)明的具體流程如下：

(1)判定滑翔器的運(yùn)動(dòng)狀態(tài)，包括步驟：

通過多普勒計(jì)程儀實(shí)時(shí)采集水下滑翔器的速度信息；根據(jù)速度信息判斷水下滑翔器的運(yùn)動(dòng)狀態(tài)：當(dāng)水下滑翔器的速度大于預(yù)先設(shè)定的速度閾值，或水下滑翔器進(jìn)行上浮、下潛或設(shè)定的“z”字運(yùn)動(dòng)時(shí)，判定水下滑翔器處于高動(dòng)態(tài)；否則，判定水下滑翔器處于非高動(dòng)態(tài)，即正?；袪顟B(tài)。

當(dāng)水下滑翔器處于高動(dòng)態(tài)，則開啟加速度計(jì)和陀螺儀，并執(zhí)行步驟(2)；否則，關(guān)閉加速度計(jì)和陀螺儀，執(zhí)行步驟(6)；

(2)根據(jù)磁力計(jì)測(cè)量得到的水下滑翔器運(yùn)行過程中的歸一化地磁值mb和當(dāng)前姿態(tài)矩陣計(jì)算磁力計(jì)反饋的水下滑翔器的角速度誤差e1；

式中，m表示水下滑翔器所在地的地磁值在導(dǎo)航坐標(biāo)系下的投影值；

對(duì)e1進(jìn)行增益和積分運(yùn)算補(bǔ)償，得到e1對(duì)應(yīng)的補(bǔ)償后角速度誤差量為：

式中，kp1為比例增益系數(shù)，ki1為積分增益系數(shù)；

(3)執(zhí)行步驟(2)的同時(shí)，根據(jù)加速度計(jì)測(cè)量到的水下滑翔器的歸一化加速度值和當(dāng)前姿態(tài)矩陣計(jì)算加速度計(jì)反饋的水下滑翔器的角速度誤差e2；

式中，為水下滑翔器所在地的重力加速度在導(dǎo)航坐標(biāo)系下的投影值；

對(duì)e2進(jìn)行增益和積分運(yùn)算補(bǔ)償，得到e2對(duì)應(yīng)的補(bǔ)償后角速度誤差量為：

式中，kp2為比例增益系數(shù)，ki2為積分增益系數(shù)；

(4)通過陀螺儀測(cè)量得到當(dāng)前水下滑翔器的角速度ω，根據(jù)步驟(2)和步驟(3)得到的δωm和δωf計(jì)算角速度的修正值為：ω′＝ω+δωm+δωf；

(5)根據(jù)角速度的修正值更新當(dāng)前的姿態(tài)矩陣，包括步驟：

(5-1)將角速度的修正值轉(zhuǎn)換為矩陣形式為：

式中，ω′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)根據(jù)角速度的修正值矩陣計(jì)算四元數(shù)微分方程為：

式中，分別表示四元數(shù)q0、q1、q2、q3的導(dǎo)數(shù)；

(5-3)將四元數(shù)微分方程進(jìn)行離散化，得到：

式中，δt表示單位采樣時(shí)間；

(5-4)根據(jù)四元數(shù)微分方程的離散化方程，計(jì)算當(dāng)前的姿態(tài)矩陣為：

式中，q0(k)、q1(k)、q2(k)和q3(k)分別為水下滑翔器姿態(tài)的四元數(shù)在時(shí)刻k的離散值；返回步驟(1)；

(6)當(dāng)水下滑翔器關(guān)閉加速度計(jì)時(shí)，則根據(jù)磁力計(jì)測(cè)得的歸一化地磁值和多普勒計(jì)程儀測(cè)得的水下滑翔器速度信息推算導(dǎo)航參數(shù)，步驟為：

對(duì)于未開啟imu的情況，需要借助磁力計(jì)和多普勒計(jì)程儀進(jìn)行推算導(dǎo)航，定義第n時(shí)刻水下滑翔器的位置坐標(biāo)為(xn，yn)，方向角為θn，則有下公式(a)，其中di為單位時(shí)間內(nèi)的水下滑翔器的位移。

其中，αn為水下滑翔器在時(shí)刻n的方向角，ωi為水下滑翔器單位時(shí)間的角速度，(x0，y0)為水下滑翔器的初始位置坐標(biāo)。

磁力計(jì)在不開啟imu時(shí)用于測(cè)量水下滑翔器的姿態(tài)角，定文表示水下滑翔器在時(shí)刻i的姿態(tài)角，且有多普勒計(jì)程儀用于測(cè)量滑翔器的實(shí)時(shí)速率vi，則有di＝vi·δt。因此ti時(shí)刻水下滑翔器的位置和航向角αi由公式(b)所示。

其中，δt為單位(采樣)時(shí)間，vi為水下滑翔器在時(shí)刻i的速度，dyi/dxi為水下滑翔器在時(shí)刻i的縱軸、橫軸位移比。

以上所述僅是本發(fā)明的優(yōu)選實(shí)施方式，應(yīng)當(dāng)指出：對(duì)于本技術(shù)領(lǐng)域的普通技術(shù)人員來說，在不脫離本發(fā)明原理的前提下，還可以做出若干改進(jìn)和潤飾，這些改進(jìn)和潤飾也應(yīng)視為本發(fā)明的保護(hù)范圍。