[發(fā)明專利]一種基于三維姿態(tài)的智能鉛魚動(dòng)態(tài)水深測(cè)量電路及方法

在審

• 申請(qǐng)?zhí)枺?/td>: 201710609620.9
• 申請(qǐng)日: 2017-07-25
• 公開（公告）號(hào): CN107367268A
• 公開（公告）日: 2017-11-21
• 發(fā)明（設(shè)計(jì)）人: 王劍平;徐嵩;楊曉洪;張果;車國霖
• 申請(qǐng)（專利權(quán)）人: 昆明理工大學(xué)
• 主分類號(hào): G01C13/00
• 分類號(hào): G01C13/00
• 代理公司: 暫無信息
• 代理人: 暫無信息

1.一種基于三維姿態(tài)的智能鉛魚動(dòng)態(tài)水深測(cè)量電路，其特征在于：包括水深壓力傳感器（1）、姿態(tài)傳感器（2）、CPU電路（3）、ZIGBEE電路（4）、控制開關(guān)（5）、天線（6）、上位機(jī)（7）；水深壓力傳感器（1）輸出端與CPU電路（3）輸入端相連，姿態(tài)傳感器（2）輸出端與CPU電路（3）輸入端相連，CPU電路（3）通信端與ZIGBEE電路（4）信號(hào)輸入端相連，控制開關(guān)（5）控制端與CPU電路（3）相連，控制開關(guān)（5）電源輸出端與ZIGBEE電路（4）電源輸入端相連，ZIGBEE電路（4）信號(hào)輸出端接天線（6），與上位機(jī)（7）通信。

2.根據(jù)權(quán)利要求1所述的基于三維姿態(tài)的智能鉛魚動(dòng)態(tài)水深測(cè)量電路，其特征在于：所述水深壓力傳感器（1）采用3.3V直流供電MS5837-30BA數(shù)字式壓力傳感器，工作電流0.6微安，能測(cè)水壓及水溫?cái)?shù)據(jù)。

3.根據(jù)權(quán)利要求1所述的基于三維姿態(tài)的智能鉛魚動(dòng)態(tài)水深測(cè)量電路，其特征在于：所述姿態(tài)傳感器（2）采用3.3V直流供電九軸姿態(tài)傳感器。

4.根據(jù)權(quán)利要求1所述的基于三維姿態(tài)的智能鉛魚動(dòng)態(tài)水深測(cè)量電路，其特征在于：所述CPU電路（3）采用STM32L151系列CPU。

5.根據(jù)權(quán)利要求1所述的基于三維姿態(tài)的智能鉛魚動(dòng)態(tài)水深測(cè)量電路，其特征在于：所述控制開關(guān)（5）采用帶有控制引腳的電壓調(diào)整器芯片。

6.一種利用權(quán)利要求1-5任一項(xiàng)所述電路進(jìn)行智能鉛魚動(dòng)態(tài)水深測(cè)量的方法，其特征在于：首先確定鉛魚工作狀態(tài)；在入水工作狀態(tài)下，水深壓力傳感器（1）采集水壓及水溫?cái)?shù)據(jù)；姿態(tài)傳感器（2）采集水下鉛魚動(dòng)態(tài)姿態(tài)數(shù)據(jù)，通過數(shù)據(jù)結(jié)算融合算法，得到鉛魚當(dāng)前的俯仰角，橫滾角，航向角；將水壓水溫?cái)?shù)據(jù)和三維姿態(tài)角數(shù)據(jù)封裝成數(shù)據(jù)包，存儲(chǔ)在CPU電路（3）內(nèi)部；打開控制開關(guān)（5），將數(shù)據(jù)通過ZIGBEE電路（4）與天線（6）發(fā)送到上位機(jī)（7）；在上位機(jī)（7）中通過三維姿態(tài)角對(duì)動(dòng)態(tài)情況下水深數(shù)據(jù)進(jìn)行修正。

7.根據(jù)權(quán)利要求6所述的智能鉛魚動(dòng)態(tài)水深測(cè)量的方法，其特征在于：所述方法的具體步驟如下：

Step1、智能鉛魚入水觸發(fā)入水開關(guān)，接通裝置內(nèi)置鋰電池，動(dòng)態(tài)水深測(cè)量電路開始工作；

Step2、水深壓力傳感器（1）與姿態(tài)傳感器（2）同時(shí)開始工作，水深壓力傳感器（1）采集水壓、水溫?cái)?shù)據(jù)，姿態(tài)傳感器（2）采集到鉛魚動(dòng)態(tài)姿態(tài)數(shù)據(jù)，再將采集到的數(shù)據(jù)封裝后發(fā)給CPU電路（3）內(nèi)部存儲(chǔ)空間，并通過CPU電路（3）進(jìn)行三維姿態(tài)角解算融合，且采集到的水壓數(shù)據(jù)傳入CPU電路3解算成水深數(shù)據(jù)；

Step3、CPU電路（3）CPU通過IO口打開控制開關(guān)（5），ZIGBEE電路（4）進(jìn)入工作模式，CPU電路（3）將設(shè)定時(shí)間內(nèi)采集到的數(shù)據(jù)包通過ZIGBEE電路（4）與天線（6）實(shí)時(shí)發(fā)送至上位機(jī)（7）存儲(chǔ)；

Step4、上位機(jī)（7）接收到發(fā)送的數(shù)據(jù)包后，將水壓數(shù)據(jù)解算成水深數(shù)據(jù)；通過姿態(tài)角數(shù)據(jù)對(duì)動(dòng)態(tài)情況下水深數(shù)據(jù)進(jìn)行修正，獲得精確的實(shí)時(shí)動(dòng)態(tài)水深數(shù)據(jù)。

8.根據(jù)權(quán)利要求7所述的智能鉛魚動(dòng)態(tài)水深測(cè)量的方法，其特征在于：所述步驟Step2中，三維姿態(tài)角解算融合的具體步驟如下：

Step2.1、CPU電路（3）獲取姿態(tài)傳感器（2）采集的原始三軸加速度數(shù)據(jù)、三軸角速度數(shù)據(jù)、三軸磁強(qiáng)數(shù)據(jù)，進(jìn)行原始數(shù)據(jù)濾波處理；

Step2.2、通過三軸加速度數(shù)據(jù)、三軸角速度數(shù)據(jù)、三軸磁強(qiáng)數(shù)據(jù)分別解算出兩組俯仰角、橫滾角和航向角數(shù)據(jù)；

Step2.3、CPU電路（3）將兩組俯仰角、橫滾角和航向角進(jìn)行數(shù)據(jù)融合，得到精確動(dòng)態(tài)姿態(tài)角度數(shù)據(jù)。

9.根據(jù)權(quán)利要求8所述的智能鉛魚動(dòng)態(tài)水深測(cè)量的方法，其特征在于：所述步驟Step2.1中，原始數(shù)據(jù)濾波處理采用卡爾曼濾波器，步驟Step2.2中，解算姿態(tài)角度采用四元數(shù)法，步驟Step2.3中，姿態(tài)角度數(shù)據(jù)融合采用卡爾曼數(shù)據(jù)融合算法，步驟Step4中，水深數(shù)據(jù)修正采用涌浪濾波算法。