CN110986895A - 基于卡尔曼滤波算法判断铅鱼完全入水的方法及系统 - Google Patents

基于卡尔曼滤波算法判断铅鱼完全入水的方法及系统 Download PDF

Info

Publication number
CN110986895A
CN110986895A CN201911237925.7A CN201911237925A CN110986895A CN 110986895 A CN110986895 A CN 110986895A CN 201911237925 A CN201911237925 A CN 201911237925A CN 110986895 A CN110986895 A CN 110986895A
Authority
CN
China
Prior art keywords
pressure sensor
fish
water
lead
moment
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Pending
Application number
CN201911237925.7A
Other languages
English (en)
Inventor
王剑平
张衎
张果
欧阳鑫
杨晓洪
李伟业
张永华
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Kunming University of Science and Technology
Original Assignee
Kunming University of Science and Technology
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Kunming University of Science and Technology filed Critical Kunming University of Science and Technology
Priority to CN201911237925.7A priority Critical patent/CN110986895A/zh
Publication of CN110986895A publication Critical patent/CN110986895A/zh
Pending legal-status Critical Current

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C13/00—Surveying specially adapted to open water, e.g. sea, lake, river or canal
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01D—MEASURING NOT SPECIALLY ADAPTED FOR A SPECIFIC VARIABLE; ARRANGEMENTS FOR MEASURING TWO OR MORE VARIABLES NOT COVERED IN A SINGLE OTHER SUBCLASS; TARIFF METERING APPARATUS; MEASURING OR TESTING NOT OTHERWISE PROVIDED FOR
    • G01D21/00—Measuring or testing not otherwise provided for
    • G01D21/02—Measuring two or more variables by means not covered by a single other subclass

Landscapes

  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Life Sciences & Earth Sciences (AREA)
  • Hydrology & Water Resources (AREA)
  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Navigation (AREA)

Abstract

本发明涉及基于卡尔曼滤波算法判断铅鱼完全入水的方法及系统，属于水文检测装备技术领域。本发明包括步骤：利用卡尔曼滤波算法对铅鱼的姿态数据进行处理得到相对于铅鱼垂直轴的倾斜角；根据压力传感器得到的压力数值计算得到此时压力传感器所在位置的水深值；根据相对于铅鱼垂直轴的倾斜角和压力传感器所在位置的水深值，利用卡尔曼滤波算法修正压力传感器所在位置的水深值；如果修正后所得的水深值大于等于压力传感器与铅鱼自带的转子流速仪的高度差时，且水面开关传感器输出入水信号，则判断铅鱼完全入水。本发明能达到精确检测铅鱼完全入水的目的，从而能在天然河道中更精确实现水深测量及测流过程的准确控制。

Description

基于卡尔曼滤波算法判断铅鱼完全入水的方法及系统
技术领域
本发明涉及基于卡尔曼滤波算法判断铅鱼完全入水的方法及系统，属于水文检测装备技术领域。
背景技术
在测流过程中，最常用的测流工具是铅鱼，准确判断铅鱼完全入水对测流过程和铅鱼的控制具有重要作用。在测流过程中，可通过安装在铅鱼上的转子流速仪对流速进行检测。水面流速是一项重要的数据，由转自流速仪在铅鱼刚好完全入水时测得，因此，精确判断铅鱼是否完全入水对于水面流速的测量至关重要。目前，常见的水面开关为水电导开关，但这种开关不具有自检定的功能，检测的可靠性和使用寿命均受环境条件的制约较大。此外，在天然河道的测流过程中，单信号水面开关极易受波浪或潮湿空气影响发生误导通，引起的入水信号抖动影响对铅鱼水下深度的判断，不能适应天然河道多变的情况。
专利201820088477.3，《一种铅鱼测流的水下信号采集无线通信装置》中使用水压检测信号和入水开关信号双信号判断铅鱼入水的方法，但该专利仅阐述了装置的硬件设计，且通过压力传感器得到的数值同样存在信号的抖动问题，这种方法虽有所改进，但没有从根本上解决精确判断入水的问题。
发明内容
本发明提供了基于卡尔曼滤波算法判断铅鱼完全入水的方法及系统，能更加精确的判断铅鱼是否完全入水，从而提高铅鱼用电效率；
以用于主要解决了以下两个问题：1.因波浪或潮湿空气等因素引起的判断入水信号判断错误；2、自然应用中波浪会使压力传感器的信号抖动，对于压力判断不准而无法准确测量水深。
本发明的技术方案是：基于卡尔曼滤波算法判断铅鱼完全入水的方法，所述方法的具体步骤如下：
Step1、利用三轴陀螺仪配合三轴加速度计测量铅鱼的姿态数据；
Step2、利用卡尔曼滤波算法对铅鱼的姿态数据进行处理得到相对于铅鱼垂直轴的倾斜角，即翻滚角γ和俯仰角β；
Step3、根据压力传感器得到的压力数值计算得到此时压力传感器所在位置的水深值；
Step4、根据相对于铅鱼垂直轴的倾斜角和压力传感器所在位置的水深值，利用卡尔曼滤波算法修正压力传感器所在位置的水深值；
Step5、判断修正后的压力传感器所在位置的水深值是否大于等于压力传感器与铅鱼自带的转子流速仪的高度差；
如果修正后所得的水深值大于等于压力传感器与铅鱼自带的转子流速仪的高度差时，且水面开关传感器输出入水信号，则判断铅鱼完全入水；否则循环Step1-Step5。
进一步地，所述Step2中：
在加速度修正子系统中，利用卡尔曼滤波算法对测量得到铅鱼的姿态数据进行最优估计，得到相对于铅鱼垂直轴的倾斜角，即翻滚角γ和俯仰角β。
进一步地，所述步骤Step2的具体步骤如下：
Step2.1、基于卡尔曼滤波算法建立线性系统状态方程、观测方程；
其中，在加速度修正子系统中，基于卡尔曼滤波算法建立线性系统状态方程、观测方程分别如公式(1)、(2)：
x1(n)＝A1x1(n-1)+w1(n) (1)
z1(n)＝H1(n)x1(n)+v1(n) (2)
x1(n)是加速度修正子系统n时刻的状态向量，采用三轴加速度计坐标(即铅鱼坐标)重力矢量来表示，具体为x1(n)＝[x1,x x1,y x1,z]T，其通过上一时刻的x1(n-1)迭代得到，x1(n-1)是其n-1时刻的状态向量，A1是状态转移矩阵，w1(n)是过程噪声，v1(n)为测量噪声，w1(n)～N(0，Q1)v1(n)～N(o，R1)，服从高斯分布，Q1，R1加速度修正子系统的过程噪声协方差矩阵和测量噪声协方差矩阵，H1(n)是3×3的观测矩阵，z1(n)是三轴加速度计测量向量，即观测目标，是通过三轴加速度计得到的三个方向的加速度数值；
其中，
A1＝I3-ΔtyG(n-1) (3)
yG(n-1)是3×3的三轴陀螺仪测量的斜对称矩阵，I3是3×3的单位矩阵，Δt测量的间隔时间，g为重力矢量；
Figure BDA0002305370970000031
H1(n)＝gI3 (5)
Step2.2、基于Step2.1所建立的利用系统状态方程，利用状态方程中的n-1时刻的状态向量去预测n时刻的状态向量，得到状态向量的预测优化值；
Step2.3、通过状态向量的预测优化值计算得到相对于铅鱼垂直轴的倾斜角，即翻滚角γ和俯仰角β，由公式(6)得到：
Figure BDA0002305370970000032
进一步地，所述步骤Step4的具体步骤如下：
Step4.1、在压力传感器测水位系统中，基于卡尔曼滤波算法建立线性系统状态方程、观测方程；
其中，在压力传感器测水位系统中，基于卡尔曼滤波算法建立线性系统状态方程、观测方程分别如公式(7)、(8)：
x2(n)＝A2x2(n-1)+B2u2(n)+w2(n) (7)
z2(n)＝H2(n)x2(n)+v2(n) (8)
x2(n)是压力传感器测水位系统n时刻的状态向量，x2(n)＝[h(n) v(n)]T，两项分别是所得到的水深值及垂直速度，x2(n-1)是压力传感器测水位系统n-1时刻的状态向量，A2是压力传感器的状态转移矩阵，由式(9)得到，其中Δt表示测量的间隔时间：
Figure BDA0002305370970000033
B2是输入控制矩阵，由式(10)得到：
Figure BDA0002305370970000034
u2(n)＝([0 0 1])·(Rγ，βa+(n-1)) (11)
Figure BDA0002305370970000035
B2u2(n)是由Step2中加速度修正子系统提供的翻滚角γ和俯仰角β乘积作为压力传感器测水位系统中的加速度垂直分量，用于修正压力传感器所得到的压力值，从而准确输出铅鱼的水下高度；w2(n)是2×1的过程噪声向量，a+(n-1)是重力补偿的加速度；加速度修正子系统是X、Y、Z三轴方向的，压力传感器测水位系统是垂直方向的，将加速度修正子系统中的Z轴与压力传感器测水位系统对齐，对齐过程中的旋转矩阵即为Rγ，β；
其中，z2(n)是Step3得到的压力传感器所在位置的水深值，H2(n)是观测矩阵，H2(n)x2(n)得到由修正后压力传感器所测压力数据计算出的水深值，v2(n)为压力传感器的测量噪声；其中，H2(n)＝[1 0]；
Step4.2、基于Step4.1所建立的利用系统状态方程，利用状态方程中的n-1时刻的状态向量去预测n时刻的状态向量，得到状态向量的预测优化值；因为压力传感器测水位系统n时刻的状态向量x2(n)分采用的分别是所得到的水深值及垂直速度，那么状态向量的预测优化值包含了优化后的水深值及垂直速度；
Step4.3、取状态向量的预测优化值中的水深值即为修正后的压力传感器所在位置的水深值。
进一步地，所述步骤Step2.2的具体步骤如下：
基于Step2.1所建立的利用系统状态方程，利用状态方程中的n-1时刻的状态向量去预测n时刻的状态向量，得到状态预测方程：
Figure BDA0002305370970000041
其中，
Figure BDA0002305370970000042
是n-1时刻x1(n)的预测值，
Figure BDA0002305370970000043
是n-1时刻x1(n-1)的预测值；
首先求取卡尔曼增益系数K1(n)及n-1时刻加速度修正子系统预测的n时刻最小均方误差矩阵P1(n|n-1)，其具体过程如公式(14)、(15)、(16)所示，
P1(n|n-1)＝A1P1(n-1|n-1)A1 T+Q1(n) (14)
K1(n)＝P1(n|n-1)H1 T(n)[R1(n)+H1(n)P1(n|n-1)H1 T(n)]-1 (15)
其中P1(n-1|n-1)是n-1时刻的最小均方误差矩阵，Q1(n)和R1(n)为加速度修正子系统的过程和测量噪声协方差矩阵；
Q1(n)＝-Δt2x1(n)∑Gx1(n) (16)
R1(n)＝∑acc+∑A (17)
其中，∑G是三轴陀螺仪测量噪声的协方差，σG 2是陀螺仪测量噪声的方差，∑A是加速度计测量噪声的协方差，σA 2是三轴加速度计测量噪声的方差，可由式(18)、(19)得到；
∑G＝E[nGnG T]＝σG 2I3 (18)
∑A＝E[nAnA T]＝σA 2I3 (19)
然后，通过状态预测方程对状态向量进行预测更新：
Figure BDA0002305370970000051
P1(n|n)＝[I3-K1(n)H1(n)]P1(n|n-1) (21)
其中，P1(n|n)是n时刻加速度修正子系统的最小均方误差矩阵。
进一步地，所述步骤Step4.2的具体步骤如下：
基于Step4.1所建立的利用系统状态方程，利用状态方程中的n-1时刻的状态向量去预测n时刻的状态向量，得到状态预测方程：
Figure BDA0002305370970000052
其中，
Figure BDA0002305370970000053
是n-1时刻x2(n)的预测值，
Figure BDA0002305370970000054
是n-1时刻x2(n-1)的预测值；
首先求取卡尔曼增益系数K2(n)及n-1时刻压力传感器测水位系统预测的n时刻最小均方误差矩阵P2(n|n-1)：
P2(n|n-1)＝A2P2(n-1|n-1)A2 T+Q2(n) (23)
K2(n)＝P2(n|n-1)H2 T(n)[R2+H2(n)P2(n|n-1)H2 T(n)]-1 (24)
其中P2(n-1|n-1)是n-1时刻的最小均方误差矩阵，Q2(n)为压力传感器测水位系统的过程噪声协方差矩阵，R2是压力传感器测水位系统的测量噪声协方差，
Figure BDA0002305370970000055
是大气压力带来的压力传感器的噪声；
Figure BDA0002305370970000056
然后，通过状态预测方程对状态向量进行预测更新：
Figure BDA0002305370970000057
P2(n|n)＝[I3-K2(n)H2(n)]P2(n|n-1) (27)
其中，P2(n|n)是n时刻压力传感器测水位系统的最小均方误差矩阵。
一种基于卡尔曼滤波算法精确判断铅鱼完全入水的系统，包括压力传感器1、加速度修正子系统、压力传感器测水位系统、三轴陀螺仪、三轴加速度计、水面开关传感器、CPU、无线传输模块、上位机；
所述压力传感器1设置在铅鱼底部用于测量其所在位置的压力值；
所述三轴陀螺仪、三轴加速度计用于测量测量铅鱼的姿态数据；
所述水面开关传感器设置在铅鱼的顶部，位于首尾两端，当两个水面开关传感器同时入水，则输出入水入水信号；
所述加速度修正子系统用于利用卡尔曼滤波算法对铅鱼的姿态数据进行处理得到相对于铅鱼垂直轴的倾斜角，即翻滚角γ和俯仰角β；
所述压力传感器测水位系统用于根据压力传感器得到的压力数值计算得到此时压力传感器所在位置的水深值；还用于根据相对于铅鱼垂直轴的倾斜角和压力传感器所在位置的水深值，利用卡尔曼滤波算法修正压力传感器所在位置的水深值；
所述CPU用于判断修正后的压力传感器所在位置的水深值是否大于等于压力传感器与铅鱼自带的转子流速仪的高度差；如果修正后所得的水深值大于等于压力传感器与铅鱼自带的转子流速仪的高度差时，且水面开关传感器输出入水信号，则判断铅鱼完全入水；否则重新获取修正后的压力传感器所在位置的水深值再进行判断；
所述无线传输模块用于把CPU中铅鱼是否完全入水的信号发送给上位机。
本发明的有益效果是：
1、因压力传感器的输出变化是线性的，使用卡尔曼滤波算法可以得到最优化的水深输出值，从而解决了自然应用中波浪会使压力传感器的信号抖动、对于压力判断不准而无法准确测量水深的问题，同时卡尔曼数据融合还可以克服时延问题。
2、本发明为了得到稳定的压力传感器数值，加入三轴姿态传感器测量竖直方向铅鱼的翻滚角和俯仰角，利用卡尔曼滤波算法建立线性系统状态方程、观测方程，去除波浪带来的噪声和干扰，对状态压力传感器水深数值进行最优估计，从而得到更加精确和稳定的铅鱼入水深度，精确检测铅鱼是否完全入水，能达到精确检测铅鱼完全入水的目的，从而能在天然河道中更精确实现水深测量及测流过程的准确控制。
附图说明
图1是本发明铅鱼剖面安装位置示意图；
图2是本发明的流程图。
图1-2中各标号：1-压力传感器，2-三轴姿态修正装置，3-水面开关，h为压力传感器到转子流速仪的高度差。
具体实施方式
实施例1：如图1-2所示，基于卡尔曼滤波算法判断铅鱼完全入水的方法，所述方法的具体步骤如下：
所述方法的具体步骤如下：
Step1、利用三轴陀螺仪配合三轴加速度计测量铅鱼的姿态数据；
Step2、利用卡尔曼滤波算法对铅鱼的姿态数据进行处理得到相对于铅鱼垂直轴的倾斜角，即翻滚角γ和俯仰角β；
Step3、根据压力传感器得到的压力数值计算得到此时压力传感器所在位置的水深值；
Step4、根据相对于铅鱼垂直轴的倾斜角和压力传感器所在位置的水深值，利用卡尔曼滤波算法修正压力传感器所在位置的水深值；
Step5、判断修正后的压力传感器所在位置的水深值是否大于等于压力传感器与铅鱼自带的转子流速仪的高度差；
如果修正后所得的水深值大于等于压力传感器与铅鱼自带的转子流速仪的高度差时，且水面开关传感器输出入水信号，则判断铅鱼完全入水；否则循环Step1-Step5。
进一步地，所述Step2中：
在加速度修正子系统中，利用卡尔曼滤波算法对测量得到铅鱼的姿态数据进行最优估计，得到相对于铅鱼垂直轴的倾斜角，即翻滚角γ和俯仰角β。
进一步地，所述步骤Step2的具体步骤如下：
Step2.1、基于卡尔曼滤波算法建立线性系统状态方程、观测方程；
其中，在加速度修正子系统中，基于卡尔曼滤波算法建立线性系统状态方程、观测方程分别如公式(1)、(2)：
x1(n)＝A1x1(n-1)+w1(n) (1)
z1(n)＝H1(n)x1(n)+v1(n) (2)
x1(n)是加速度修正子系统n时刻的状态向量，采用三轴加速度计坐标(即铅鱼坐标)重力矢量来表示，具体为x1(n)＝[x1,x x1,y x1,z]T，其通过上一时刻的x1(n-1)迭代得到，x1(n-1)是其n-1时刻的状态向量，A1是状态转移矩阵，w1(n)是过程噪声，v1(n)为测量噪声，w1(n)～N(0,Q1)，v1(n)～N(0,R1)服从高斯分布，Q1，R1加速度修正子系统的过程噪声协方差矩阵和测量噪声协方差矩阵，H1(n)是3×3的观测矩阵，z1(n)是三轴加速度计测量向量，即观测目标，是通过三轴加速度计得到的三个方向的加速度数值；
其中，
A1＝I3-ΔtyG(n-1) (3)
yG(n-1)是3×3的三轴陀螺仪测量的斜对称矩阵，I3是3×3的单位矩阵，Δt测量的间隔时间，g为重力矢量；
Figure BDA0002305370970000081
H1(n)＝gI3 (5)
Step2.2、基于Step2.1所建立的利用系统状态方程，利用状态方程中的n-1时刻的状态向量去预测n时刻的状态向量，得到状态向量的预测优化值；
Step2.3、通过状态向量的预测优化值计算得到相对于铅鱼垂直轴的倾斜角，即翻滚角γ和俯仰角β，由公式(6)得到：
Figure BDA0002305370970000082
进一步地，所述步骤Step4的具体步骤如下：
Step4.1、在压力传感器测水位系统中，基于卡尔曼滤波算法建立线性系统状态方程、观测方程；
其中，在压力传感器测水位系统中，基于卡尔曼滤波算法建立线性系统状态方程、观测方程分别如公式(7)、(8)：
x2(n)＝A2x2(n-1)+B2u2(n)+w2(n) (7)
z2(n)＝H2(n)x2(n)+v2(n) (8)
x2(n)是压力传感器测水位系统n时刻的状态向量，x2(n)＝[h(n)v(n)]T，两项分别是所得到的水深值及垂直速度，x2(n-1)是压力传感器测水位系统n-1时刻的状态向量，A2是压力传感器的状态转移矩阵，由式(9)得到，其中Δt表示测量的间隔时间：
Figure BDA0002305370970000091
B2是输入控制矩阵，由式(10)得到：
Figure BDA0002305370970000092
u2(n)＝([0 0 1])·(Rγ，βa+(n-1)) (11)
Figure BDA0002305370970000093
B2u2(n)是由Step2中加速度修正子系统提供的翻滚角γ和俯仰角β乘积作为压力传感器测水位系统中的加速度垂直分量，用于修正压力传感器所得到的压力值，从而准确输出铅鱼的水下高度；w2(n)是2×1的过程噪声向量，a+(n-1)是重力补偿的加速度；加速度修正子系统是X、Y、Z三轴方向的，压力传感器测水位系统是垂直方向的，将加速度修正子系统中的Z轴与压力传感器测水位系统对齐，对齐过程中的旋转矩阵即为Rγ,β；
其中，z2(n)是Step3得到的压力传感器所在位置的水深值，H2(n)是观测矩阵，H2(n)x2(n)得到由修正后压力传感器所测压力数据计算出的水深值，v2(n)为压力传感器的测量噪声；其中，H2(n)＝[1 0]；
Step4.2、基于Step4.1所建立的利用系统状态方程，利用状态方程中的n-1时刻的状态向量去预测n时刻的状态向量，得到状态向量的预测优化值；因为压力传感器测水位系统n时刻的状态向量x2(n)分采用的分别是所得到的水深值及垂直速度，那么状态向量的预测优化值包含了优化后的水深值及垂直速度；
Step4.3、取状态向量的预测优化值中的水深值即为修正后的压力传感器所在位置的水深值。
进一步地，所述步骤Step2.2的具体步骤如下：
基于Step2.1所建立的利用系统状态方程，利用状态方程中的n-1时刻的状态向量去预测n时刻的状态向量，得到状态预测方程：
Figure BDA0002305370970000094
其中，
Figure BDA0002305370970000095
是n-1时刻x1(n)的预测值，
Figure BDA0002305370970000096
是n-1时刻x1(n-1)的预测值；
首先求取卡尔曼增益系数K1(n)及n-1时刻加速度修正子系统预测的n时刻最小均方误差矩阵P1(n|n-1)，其具体过程如公式(14)、(15)、(16)所示，
P1(n|n-1)＝A1P1(n-1|n-1)A1 T+Q1(n) (14)
K1(n)＝P1(n|n-1)H1 T(n)[R1(n)+H1(n)P1(n|n-1)H1 T(n)]-1 (15)
其中P1(n-1|n-1)是n-1时刻的最小均方误差矩阵，Q1(n)和R1(n)为加速度修正子系统的过程和测量噪声协方差矩阵；
Q1(n)＝-Δt2x1(n)ΣGx1(n) (16)
R1(n)＝Σacc+∑A (17)
其中，∑G是三轴陀螺仪测量噪声的协方差，σG 2是陀螺仪测量噪声的方差，∑A是加速度计测量噪声的协方差，σA 2是三轴加速度计测量噪声的方差，可由式(18)、(19)得到；
ΣG＝E[nGnG T]＝σG 2I3 (18)
∑A＝E[nAnA T]＝σA 2I3 (19)
然后，通过状态预测方程对状态向量进行预测更新：
Figure BDA0002305370970000101
P1(n|n)＝[I3-K1(n)H1(n)]P1(n|n-1) (21)
其中，P1(n|n)是n时刻加速度修正子系统的最小均方误差矩阵。
进一步地，所述步骤Step4.2的具体步骤如下：
基于Step4.1所建立的利用系统状态方程，利用状态方程中的n-1时刻的状态向量去预测n时刻的状态向量，得到状态预测方程：
Figure BDA0002305370970000102
其中，
Figure BDA0002305370970000103
是n-1时刻x2(n)的预测值，
Figure BDA0002305370970000104
是n-1时刻x2(n-1)的预测值；
首先求取卡尔曼增益系数K2(n)及n-1时刻压力传感器测水位系统预测的n时刻最小均方误差矩阵P2(n|n-1)：
P2(n|n-1)＝A2P2(n-1|n-1)A2 T+Q2(n) (23)
K2(n)＝P2(n|n-1)H2 T(n)[R2+H2(n)P2(n|n-1)H2 T(n)]-1 (24)
其中P2(n-1|n-1)是n-1时刻的最小均方误差矩阵，Q2(n)为压力传感器测水位系统的过程噪声协方差矩阵，R2是压力传感器测水位系统的测量噪声协方差，
Figure BDA0002305370970000111
是大气压力带来的压力传感器的噪声；
Figure BDA0002305370970000112
然后，通过状态预测方程对状态向量进行预测更新：
Figure BDA0002305370970000113
P2(n|n)＝[I3-K2(n)H2(n)]P2(n|n-1) (27)
其中，P2(n|n)是n时刻压力传感器测水位系统的最小均方误差矩阵。
一种基于卡尔曼滤波算法精确判断铅鱼完全入水的系统，包括压力传感器1、加速度修正子系统、压力传感器测水位系统、三轴陀螺仪、三轴加速度计、水面开关传感器、CPU、无线传输模块、上位机；
所述压力传感器1设置在铅鱼底部用于测量其所在位置的压力值；
所述三轴陀螺仪、三轴加速度计用于测量测量铅鱼的姿态数据；
所述水面开关传感器设置在铅鱼的顶部，位于首尾两端，当两个水面开关传感器同时入水，则输出入水入水信号；
所述加速度修正子系统用于利用卡尔曼滤波算法对铅鱼的姿态数据进行处理得到相对于铅鱼垂直轴的倾斜角，即翻滚角γ和俯仰角β；
所述压力传感器测水位系统用于根据压力传感器得到的压力数值计算得到此时压力传感器所在位置的水深值；还用于根据相对于铅鱼垂直轴的倾斜角和压力传感器所在位置的水深值，利用卡尔曼滤波算法修正压力传感器所在位置的水深值；
所述CPU用于判断修正后的压力传感器所在位置的水深值是否大于等于压力传感器与铅鱼自带的转子流速仪的高度差；如果修正后所得的水深值大于等于压力传感器与铅鱼自带的转子流速仪的高度差时，且水面开关传感器输出入水信号，则判断铅鱼完全入水；否则重新获取修正后的压力传感器所在位置的水深值再进行判断；
所述无线传输模块用于把CPU中铅鱼是否完全入水的信号发送给上位机。
所述水面开关传感器安装在与转子流速仪平行同高度位置，位于铅鱼的首尾各一个。为减少波浪对压力传感器的影响，将压力传感器、三轴陀螺仪和三轴加速度计安装在铅鱼底部垂直中心线偏后位置，如图1所示，与转子流速仪形成竖直高度差h。三轴姿态修正装置包括三轴加速度计和三轴陀螺仪，为使其对铅鱼姿态测量的坐标与铅鱼本身坐标一致，将三轴姿态修正装置安装在铅鱼水平中心线，为此三轴姿态修正装置需要贯穿铅鱼。为避免水中侧流压力传感器冲击造成巨大误差，将压力传感器对称安装在铅鱼底部，与三轴姿态修正装置处于同一垂直线上，同时便于计算。水面开关传感器和压力传感器的信号经加速度修正子系统、压力传感器测水位系统、CPU处理，通过无线传输模块(LoRa天线)传输到上位机。水面开关传感器与修正后的压力传感器所在位置的水深值信号共同确定铅鱼是否入水，当修正后所得的水深值大于等于设定值h，且水面开关传感器输出入水信号，则判断铅鱼完全入水，铅鱼通信电路才开始工作，从而铅鱼用电效率。
上面结合附图对本发明的具体实施方式作了详细说明，但是本发明并不限于上述实施方式，在本领域普通技术人员所具备的知识范围内，还可以在不脱离本发明宗旨的前提下作出各种变化。

Claims (7)

1.基于卡尔曼滤波算法判断铅鱼完全入水的方法，其特征在于：所述方法的具体步骤如下：
Step1、利用三轴陀螺仪配合三轴加速度计测量铅鱼的姿态数据；
Step2、利用卡尔曼滤波算法对铅鱼的姿态数据进行处理得到相对于铅鱼垂直轴的倾斜角，即翻滚角γ和俯仰角β；
Step3、根据压力传感器得到的压力数值计算得到此时压力传感器所在位置的水深值；
Step4、根据相对于铅鱼垂直轴的倾斜角和压力传感器所在位置的水深值，利用卡尔曼滤波算法修正压力传感器所在位置的水深值；
Step5、判断修正后的压力传感器所在位置的水深值是否大于等于压力传感器与铅鱼自带的转子流速仪的高度差；
如果修正后所得的水深值大于等于压力传感器与铅鱼自带的转子流速仪的高度差时，且水面开关传感器输出入水信号，则判断铅鱼完全入水；否则循环Step1-Step5。
2.根据权利要求1所述的基于卡尔曼滤波算法判断铅鱼完全入水的方法，其特征在于：
所述Step2中：
在加速度修正子系统中，利用卡尔曼滤波算法对测量得到铅鱼的姿态数据进行最优估计，得到相对于铅鱼垂直轴的倾斜角，即翻滚角γ和俯仰角β。
3.根据权利要求1所述的基于卡尔曼滤波算法判断铅鱼完全入水的方法，其特征在于：所述步骤Step2的具体步骤如下：
Step2.1、基于卡尔曼滤波算法建立线性系统状态方程、观测方程；
其中，在加速度修正子系统中，基于卡尔曼滤波算法建立线性系统状态方程、观测方程分别如公式(1)、(2)：
x1(n)＝A1x1(n-1)+w1(n) (1)
z1(n)＝H1(n)x1(n)+v1(n) (2)
x1(n)是加速度修正子系统n时刻的状态向量，采用三轴加速度计坐标(即铅鱼坐标)重力矢量来表示，具体为x1(n)＝[x1,x x1,y x1,z]T，其通过上一时刻的x1(n-1)迭代得到，x1(n-1)是其n-1时刻的状态向量，A1是状态转移矩阵，w1(n)是过程噪声，v1(n)为测量噪声，w1(n)～N(0,Q1)，v1(n)～N(0,R1)服从高斯分布，Q1，R1加速度修正子系统的过程噪声协方差矩阵和测量噪声协方差矩阵，H1(n)是3×3的观测矩阵，z1(n)是三轴加速度计测量向量，即观测目标，是通过三轴加速度计得到的三个方向的加速度数值；
其中，
A1＝I3-ΔtyG(n-1) (3)
yG(n-1)是3×3的三轴陀螺仪测量的斜对称矩阵，I3是3×3的单位矩阵，Δt测量的间隔时间，g为重力矢量；
Figure FDA0002305370960000021
H1(n)＝gI3 (5)
Step2.2、基于Step2.1所建立的利用系统状态方程，利用状态方程中的n-1时刻的状态向量去预测n时刻的状态向量，得到状态向量的预测优化值；
Step2.3、通过状态向量的预测优化值计算得到相对于铅鱼垂直轴的倾斜角，即翻滚角γ和俯仰角β，由公式(6)得到：
Figure FDA0002305370960000022
4.根据权利要求1所述的基于卡尔曼滤波算法判断铅鱼完全入水的方法，其特征在于：
所述步骤Step4的具体步骤如下：
Step4.1、在压力传感器测水位系统中，基于卡尔曼滤波算法建立线性系统状态方程、观测方程；
其中，在压力传感器测水位系统中，基于卡尔曼滤波算法建立线性系统状态方程、观测方程分别如公式(7)、(8)：
x2(n)＝A2x2(n-1)+B2u2(n)+w2(n) (7)
z2(n)＝H2(n)x2(n)+v2(n) (8)
x2(n)是压力传感器测水位系统n时刻的状态向量，x2(n)＝[h(n) v(n)]T，两项分别是所得到的水深值及垂直速度，x2(n-1)是压力传感器测水位系统n-1时刻的状态向量，A2是压力传感器的状态转移矩阵，由式(9)得到，其中Δt表示测量的间隔时间：
Figure FDA0002305370960000031
B2是输入控制矩阵，由式(10)得到：
Figure FDA0002305370960000032
u2(n)＝([0 0 1])·(Rγ,βa+(n-1)) (11)
Figure FDA0002305370960000033
B2u2(n)是由Step2中加速度修正子系统提供的翻滚角γ和俯仰角β乘积作为压力传感器测水位系统中的加速度垂直分量，用于修正压力传感器所得到的压力值，从而准确输出铅鱼的水下高度；w2(n)是2×1的过程噪声向量，a+(n-1)是重力补偿的加速度；加速度修正子系统是X、Y、Z三轴方向的，压力传感器测水位系统是垂直方向的，将加速度修正子系统中的Z轴与压力传感器测水位系统对齐，对齐过程中的旋转矩阵即为Rγ,β；
其中，z2(n)是Step3得到的压力传感器所在位置的水深值，H2(n)是观测矩阵，H2(n)x2(n)得到由修正后压力传感器所测压力数据计算出的水深值，v2(n)为压力传感器的测量噪声；其中，H2(n)＝[1 0]；
Step4.2、基于Step4.1所建立的利用系统状态方程，利用状态方程中的n-1时刻的状态向量去预测n时刻的状态向量，得到状态向量的预测优化值；因为压力传感器测水位系统n时刻的状态向量x2(n)分采用的分别是所得到的水深值及垂直速度，那么状态向量的预测优化值包含了优化后的水深值及垂直速度；
Step4.3、取状态向量的预测优化值中的水深值即为修正后的压力传感器所在位置的水深值。
5.根据权利要求3所述的基于卡尔曼滤波算法判断铅鱼完全入水的方法，其特征在于：所述步骤Step2.2的具体步骤如下：
基于Step2.1所建立的利用系统状态方程，利用状态方程中的n-1时刻的状态向量去预测n时刻的状态向量，得到状态预测方程：
Figure FDA0002305370960000041
其中，
Figure FDA0002305370960000042
是n-1时刻x1(n)的预测值，
Figure FDA0002305370960000043
是n-1时刻x1(n-1)的预测值；
首先求取卡尔曼增益系数K1(n)及n-1时刻加速度修正子系统预测的n时刻最小均方误差矩阵P1(n|n-1)，其具体过程如公式(14)、(15)、(16)所示，
P1(n|n-1)＝A1P1(n-1|n-1)A1 T+Q1(n) (14)
K1(n)＝P1(n|n-1)H1 T(n)[R1(n)+H1(n)P1(n|n-1)H1 T(n)]1- (15)
其中P1(n-1|n-1)是n-1时刻的最小均方误差矩阵，Q1(n)和R1(n)为加速度修正子系统的过程和测量噪声协方差矩阵；
Q1(n)＝-Δt2x1(n)∑Gx1(n) (16)
R1(n)＝Σacc+∑A (17)
其中，∑G是三轴陀螺仪测量噪声的协方差，σG 2是陀螺仪测量噪声的方差，∑A是加速度计测量噪声的协方差，σA 2是三轴加速度计测量噪声的方差，可由式(18)、(19)得到；
∑G＝E[nGnG T]＝σG 2I3 (18)
ΣA＝E[nAnA T]＝σA 2I3 (19)
然后，通过状态预测方程对状态向量进行预测更新：
Figure FDA0002305370960000044
P1(n|n)＝[I3-K1(n)H1(n)]P1(n|n-1) (21)
其中，P1(n|n)是n时刻加速度修正子系统的最小均方误差矩阵。
6.根据权利要求4所述的基于卡尔曼滤波算法判断铅鱼完全入水的方法，其特征在于：所述步骤Step4.2的具体步骤如下：
基于Step4.1所建立的利用系统状态方程，利用状态方程中的n-1时刻的状态向量去预测n时刻的状态向量，得到状态预测方程：
Figure FDA0002305370960000045
其中，
Figure FDA0002305370960000046
是n-1时刻x2(n)的预测值，
Figure FDA0002305370960000047
是n-1时刻x2(n-1)的预测值；
首先求取卡尔曼增益系数K2(n)及n-1时刻压力传感器测水位系统预测的n时刻最小均方误差矩阵P2(n|n-1)：
P2(n|n-1)＝A2P2(n-1|n-1)A2 T+Q2(n) (23)
K2(n)＝P2(n|n-1)H2 T(n)[R2+H2(n)P2(n|n-1)H2 T(n)]-1 (24)
其中P2(n-1|n-1)是n-1时刻的最小均方误差矩阵，Q2(n)为压力传感器测水位系统的过程噪声协方差矩阵，R2是压力传感器测水位系统的测量噪声协方差，
Figure FDA0002305370960000051
Figure FDA0002305370960000052
是大气压力带来的压力传感器的噪声；
Figure FDA0002305370960000053
然后，通过状态预测方程对状态向量进行预测更新：
Figure FDA0002305370960000054
P2(n|n)＝[I3-K2(n)H2(n)]P2(n|n-1) (27)
其中，P2(n|n)是n时刻压力传感器测水位系统的最小均方误差矩阵。
7.基于卡尔曼滤波算法判断铅鱼完全入水的系统，其特征在于：包括压力传感器、加速度修正子系统、压力传感器测水位系统、三轴陀螺仪、三轴加速度计、水面开关传感器、CPU、无线传输模块、上位机；
所述压力传感器设置在铅鱼底部用于测量其所在位置的压力值；
所述三轴陀螺仪、三轴加速度计用于测量测量铅鱼的姿态数据；
所述水面开关传感器设置在铅鱼的顶部，位于首尾两端，当两个水面开关传感器同时入水，则输出入水入水信号；
所述加速度修正子系统用于利用卡尔曼滤波算法对铅鱼的姿态数据进行处理得到相对于铅鱼垂直轴的倾斜角，即翻滚角γ和俯仰角β；
所述压力传感器测水位系统用于根据压力传感器得到的压力数值计算得到此时压力传感器所在位置的水深值；还用于根据相对于铅鱼垂直轴的倾斜角和压力传感器所在位置的水深值，利用卡尔曼滤波算法修正压力传感器所在位置的水深值；
所述CPU用于判断修正后的压力传感器所在位置的水深值是否大于等于压力传感器与铅鱼自带的转子流速仪的高度差；如果修正后所得的水深值大于等于压力传感器与铅鱼自带的转子流速仪的高度差时，且水面开关传感器输出入水信号，则判断铅鱼完全入水；否则重新获取修正后的压力传感器所在位置的水深值再进行判断；
所述无线传输模块用于把CPU中铅鱼是否完全入水的信号发送给上位机。
CN201911237925.7A 2019-12-06 2019-12-06 基于卡尔曼滤波算法判断铅鱼完全入水的方法及系统 Pending CN110986895A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201911237925.7A CN110986895A (zh) 2019-12-06 2019-12-06 基于卡尔曼滤波算法判断铅鱼完全入水的方法及系统

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201911237925.7A CN110986895A (zh) 2019-12-06 2019-12-06 基于卡尔曼滤波算法判断铅鱼完全入水的方法及系统

Publications (1)

Publication Number Publication Date
CN110986895A true CN110986895A (zh) 2020-04-10

Family

ID=70090825

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201911237925.7A Pending CN110986895A (zh) 2019-12-06 2019-12-06 基于卡尔曼滤波算法判断铅鱼完全入水的方法及系统

Country Status (1)

Country Link
CN (1) CN110986895A (zh)

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112147712A (zh) * 2020-09-28 2020-12-29 中国海洋大学 一种水下振动源探测装置及方法
CN117346745A (zh) * 2023-12-04 2024-01-05 天宇利水信息技术成都有限公司 一种水文缆道行车位置时空双向修正及自适应方法

Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20130016584A1 (en) * 2011-07-15 2013-01-17 Teledyne Scientific & Imaging Llc Methods and apparatus for obtaining sensor motion and position data from underwater acoustic signals
CN104075715A (zh) * 2014-07-07 2014-10-01 东南大学 一种结合地形和环境特征的水下导航定位方法
CN106780542A (zh) * 2016-12-29 2017-05-31 北京理工大学 一种基于嵌入卡尔曼滤波器的Camshift的机器鱼跟踪方法
CN107076554A (zh) * 2014-10-01 2017-08-18 英特尔公司 用于垂直轨迹确定和自动跳跃检测的方法和系统
CN108844539A (zh) * 2018-05-28 2018-11-20 哈尔滨工程大学 一种用于海浪主动补偿系统的位姿检测系统
CN108880593A (zh) * 2018-06-01 2018-11-23 昆明理工大学 一种基于双入水信号控制铅鱼水下无线通信的电路及其控制方法

Patent Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20130016584A1 (en) * 2011-07-15 2013-01-17 Teledyne Scientific & Imaging Llc Methods and apparatus for obtaining sensor motion and position data from underwater acoustic signals
CN104075715A (zh) * 2014-07-07 2014-10-01 东南大学 一种结合地形和环境特征的水下导航定位方法
CN107076554A (zh) * 2014-10-01 2017-08-18 英特尔公司 用于垂直轨迹确定和自动跳跃检测的方法和系统
CN106780542A (zh) * 2016-12-29 2017-05-31 北京理工大学 一种基于嵌入卡尔曼滤波器的Camshift的机器鱼跟踪方法
CN108844539A (zh) * 2018-05-28 2018-11-20 哈尔滨工程大学 一种用于海浪主动补偿系统的位姿检测系统
CN108880593A (zh) * 2018-06-01 2018-11-23 昆明理工大学 一种基于双入水信号控制铅鱼水下无线通信的电路及其控制方法

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
刘振等: "改进粒子群算法辨识噪声模型的卡尔曼直接转矩控制", 《昆明理工大学学报( 自然科学版)》 *
徐嵩等: "基于卡尔曼滤波算法的智能铅鱼测深系统研究", 《软件导刊》 *

Cited By (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112147712A (zh) * 2020-09-28 2020-12-29 中国海洋大学 一种水下振动源探测装置及方法
CN112147712B (zh) * 2020-09-28 2021-10-01 中国海洋大学 一种水下振动源探测装置及方法
CN117346745A (zh) * 2023-12-04 2024-01-05 天宇利水信息技术成都有限公司 一种水文缆道行车位置时空双向修正及自适应方法
CN117346745B (zh) * 2023-12-04 2024-02-06 天宇利水信息技术成都有限公司 一种水文缆道行车位置时空双向修正及自适应方法

Similar Documents

Publication Publication Date Title
CN106647791B (zh) 三维姿态测控装置、机械设备及三维姿态的测控方法
CN110263075B (zh) Ladcp与usbl组合观测及数据资料处理方法
CN102175887B (zh) 移动式超声波风速风向仪及测量风速风向的方法
CN112923924B (zh) 一种锚泊船舶姿态与位置监测方法及系统
CN110986895A (zh) 基于卡尔曼滤波算法判断铅鱼完全入水的方法及系统
CN110346600B (zh) 一种超声波风速风向测量方法
KR101185875B1 (ko) 시추공형 지진센서의 방위각 및 수평 경사 보정 장치 및 방법
CN109855649B (zh) 一种水下滑翔机平台运动轨迹被动确定方法
CN110082386B (zh) 一种尾翼可调式海底沉积物温度探针控制系统及控制方法
CN110824430A (zh) 基于北斗定位系统的水下定位方法
CN112362057A (zh) 基于零速修正与姿态自观测的惯性行人导航算法
CN108761470B (zh) 一种基于拖缆形态方程解析的目标定位方法
CN110007318B (zh) 风场干扰下基于卡尔曼滤波的单无人机判断gps欺骗的方法
CN113495169B (zh) 风力机风轮前风速超声测量系统
CN110082033B (zh) 一种运动状态下的水上载体重心测量装置和方法
CN207894954U (zh) 一种高水条件下的水面流速检测系统
CN108761420B (zh) 一种基于mems的船用固态导航雷达目标检测的补偿方法
CN115112103B (zh) 一种ladcp与组合惯性导航系统联合观测系统及方法
CN110030991B (zh) 融合陀螺和磁强计的飞行物高速旋转角运动测量方法
CN106123927A (zh) 一种载体加速运动时倾角误差修正方法
CN116413642A (zh) 一种面向运动平台的单点磁梯度定位方法及装置
CN109443333A (zh) 一种陀螺阵列反馈加权融合方法
CN112965091B (zh) 一种农业机器人定位方法及系统
CN110057383B (zh) 一种auv推位导航系统杆臂误差标校方法
CN112881970A (zh) 一种适用于声呐浮标平台的目标测向方法及系统

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
RJ01 Rejection of invention patent application after publication

Application publication date: 20200410

RJ01 Rejection of invention patent application after publication