基于卡尔曼滤波算法判断铅鱼完全入水的方法及系统专利检索-信号信号处理专利检索查询-专利查询网

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

阅读：412发布：2023-12-22

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

权利要求

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为重力矢量；
H1(n)＝gI3                            (5)
Step2.2、基于Step2.1所建立的利用系统状态方程，利用状态方程中的n-1时刻的状态向量去预测n时刻的状态向量，得到状态向量的预测优化值；
Step2.3、通过状态向量的预测优化值计算得到相对于铅鱼垂直轴的倾斜角，即翻滚角γ和俯仰角β，由公式(6)得到：
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表示测量的间隔时间：
B2是输入控制矩阵，由式(10)得到：
u2(n)＝([0 0 1])·(Rγ,βa+(n-1))     (11)
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时刻的状态向量，得到状态预测方程：
其中，是n-1时刻x1(n)的预测值，是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)A1T+Q1(n)    (14)
K1(n)＝P1(n|n-1)H1T(n)[R1(n)+H1(n)P1(n|n-1)H1T(n)]1-  (15)
其中P1(n-1|n-1)是n-1时刻的最小均方误差矩阵，Q1(n)和R1(n)为加速度修正子系统的过程和测量噪声协方差矩阵；
2
Q1(n)＝-Δtx1(n)∑Gx1(n)      (16)
R1(n)＝Σacc+∑A      (17)
其中，∑G是三轴陀螺仪测量噪声的协方差，σG2是陀螺仪测量噪声的方差，∑A是加速度计测量噪声的协方差，σA2是三轴加速度计测量噪声的方差，可由式(18)、(19)得到；
T 2
∑G＝E[nGnG]＝σGI3      (18)
ΣA＝E[nAnAT]＝σA2I3     (19)
然后，通过状态预测方程对状态向量进行预测更新：
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时刻的状态向量，得到状态预测方程：
其中，是n-1时刻x2(n)的预测值，是n-1时刻x2(n-1)的预测
值；
首先求取卡尔曼增益系数K2(n)及n-1时刻压力传感器测水位系统预测的n时刻最小均方误差矩阵P2(n|n-1)：
P2(n|n-1)＝A2P2(n-1|n-1)A2T+Q2(n)     (23)
K2(n)＝P2(n|n-1)H2T(n)[R2+H2(n)P2(n|n-1)H2T(n)]-1   (24)
其中P2(n-1|n-1)是n-1时刻的最小均方误差矩阵，Q2(n)为压力传感器测水位系统的过程噪声协方差矩阵，R2是压力传感器测水位系统的测量噪声协方差，是大气压力带来的压力传感器的噪声；
然后，通过状态预测方程对状态向量进行预测更新：
P2(n|n)＝[I3-K2(n)H2(n)]P2(n|n-1)      (27)
其中，P2(n|n)是n时刻压力传感器测水位系统的最小均方误差矩阵。
7.基于卡尔曼滤波算法判断铅鱼完全入水的系统，其特征在于：包括压力传感器、加速度修正子系统、压力传感器测水位系统、三轴陀螺仪、三轴加速度计、水面开关传感器、CPU、无线传输模块、上位机；
所述压力传感器设置在铅鱼底部用于测量其所在位置的压力值；
所述三轴陀螺仪、三轴加速度计用于测量测量铅鱼的姿态数据；
所述水面开关传感器设置在铅鱼的顶部，位于首尾两端，当两个水面开关传感器同时入水，则输出入水入水信号；
所述加速度修正子系统用于利用卡尔曼滤波算法对铅鱼的姿态数据进行处理得到相对于铅鱼垂直轴的倾斜角，即翻滚角γ和俯仰角β；
所述压力传感器测水位系统用于根据压力传感器得到的压力数值计算得到此时压力传感器所在位置的水深值；还用于根据相对于铅鱼垂直轴的倾斜角和压力传感器所在位置的水深值，利用卡尔曼滤波算法修正压力传感器所在位置的水深值；
所述CPU用于判断修正后的压力传感器所在位置的水深值是否大于等于压力传感器与铅鱼自带的转子流速仪的高度差；如果修正后所得的水深值大于等于压力传感器与铅鱼自带的转子流速仪的高度差时，且水面开关传感器输出入水信号，则判断铅鱼完全入水；否则重新获取修正后的压力传感器所在位置的水深值再进行判断；
所述无线传输模块用于把CPU中铅鱼是否完全入水的信号发送给上位机。

说明书全文

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

技术领域

[0001] 本发明涉及基于卡尔曼滤波算法判断铅鱼完全入水的方法及系统，属于水文检测装备技术领域。

背景技术

[0002] 在测流过程中，最常用的测流工具是铅鱼，准确判断铅鱼完全入水对测流过程和铅鱼的控制具有重要作用。在测流过程中，可通过安装在铅鱼上的转子流速仪对流速进行检测。水面流速是一项重要的数据，由转自流速仪在铅鱼刚好完全入水时测得，因此，精确判断铅鱼是否完全入水对于水面流速的测量至关重要。目前，常见的水面开关为水电导开关，但这种开关不具有自检定的功能，检测的可靠性和使用寿命均受环境条件的制约较大。此外，在天然河道的测流过程中，单信号水面开关极易受波浪或潮湿空气影响发生误导通，引起的入水信号抖动影响对铅鱼水下深度的判断，不能适应天然河道多变的情况。

[0003] 专利201820088477.3，《一种铅鱼测流的水下信号采集无线通信装置》中使用水压检测信号和入水开关信号双信号判断铅鱼入水的方法，但该专利仅阐述了装置的硬件设计，且通过压力传感器得到的数值同样存在信号的抖动问题，这种方法虽有所改进，但没有从根本上解决精确判断入水的问题。

发明内容

[0004] 本发明提供了基于卡尔曼滤波算法判断铅鱼完全入水的方法及系统，能更加精确的判断铅鱼是否完全入水，从而提高铅鱼用电效率；

[0005] 以用于主要解决了以下两个问题：1.因波浪或潮湿空气等因素引起的判断入水信号判断错误；2、自然应用中波浪会使压力传感器的信号抖动，对于压力判断不准而无法准确测量水深。

[0006] 本发明的技术方案是：基于卡尔曼滤波算法判断铅鱼完全入水的方法，所述方法的具体步骤如下：

[0007] Step1、利用三轴陀螺仪配合三轴加速度计测量铅鱼的姿态数据；

[0008] Step2、利用卡尔曼滤波算法对铅鱼的姿态数据进行处理得到相对于铅鱼垂直轴的倾斜角，即翻滚角γ和俯仰角β；

[0009] Step3、根据压力传感器得到的压力数值计算得到此时压力传感器所在位置的水深值；

[0010] Step4、根据相对于铅鱼垂直轴的倾斜角和压力传感器所在位置的水深值，利用卡尔曼滤波算法修正压力传感器所在位置的水深值；

[0011] Step5、判断修正后的压力传感器所在位置的水深值是否大于等于压力传感器与铅鱼自带的转子流速仪的高度差；

[0012] 如果修正后所得的水深值大于等于压力传感器与铅鱼自带的转子流速仪的高度差时，且水面开关传感器输出入水信号，则判断铅鱼完全入水；否则循环Step1-Step5。

[0013] 进一步地，所述Step2中：

[0014] 在加速度修正子系统中，利用卡尔曼滤波算法对测量得到铅鱼的姿态数据进行最优估计，得到相对于铅鱼垂直轴的倾斜角，即翻滚角γ和俯仰角β。

[0015] 进一步地，所述步骤Step2的具体步骤如下：

[0016] Step2.1、基于卡尔曼滤波算法建立线性系统状态方程、观测方程；

[0017] 其中，在加速度修正子系统中，基于卡尔曼滤波算法建立线性系统状态方程、观测方程分别如公式(1)、(2)：

[0018] x1(n)＝A1x1(n-1)+w1(n) (1)

[0019] z1(n)＝H1(n)x1(n)+v1(n) (2)

[0020] 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)是三轴加速度计测量向量，即观测目标，是通过三轴加速度计得到的三个方向的加速度数值；

[0021] 其中，

[0022] A1＝I3-ΔtyG(n-1) (3)

[0023] yG(n-1)是3×3的三轴陀螺仪测量的斜对称矩阵，I3是3×3的单位矩阵，Δt测量的间隔时间，g为重力矢量；

[0024]

[0025] H1(n)＝gI3 (5)

[0026] Step2.2、基于Step2.1所建立的利用系统状态方程，利用状态方程中的n-1时刻的状态向量去预测n时刻的状态向量，得到状态向量的预测优化值；

[0027] Step2.3、通过状态向量的预测优化值计算得到相对于铅鱼垂直轴的倾斜角，即翻滚角γ和俯仰角β，由公式(6)得到：

[0028]

[0029] 进一步地，所述步骤Step4的具体步骤如下：

[0030] Step4.1、在压力传感器测水位系统中，基于卡尔曼滤波算法建立线性系统状态方程、观测方程；

[0031] 其中，在压力传感器测水位系统中，基于卡尔曼滤波算法建立线性系统状态方程、观测方程分别如公式(7)、(8)：

[0032] x2(n)＝A2x2(n-1)+B2u2(n)+w2(n) (7)

[0033] z2(n)＝H2(n)x2(n)+v2(n) (8)

[0034] x2(n)是压力传感器测水位系统n时刻的状态向量，x2(n)＝[h(n) v(n)]T，两项分别是所得到的水深值及垂直速度，x2(n-1)是压力传感器测水位系统n-1时刻的状态向量，A2是压力传感器的状态转移矩阵，由式(9)得到，其中Δt表示测量的间隔时间：

[0035]

[0036] B2是输入控制矩阵，由式(10)得到：

[0037]

[0038] u2(n)＝([0 0 1])·(Rγ，βa+(n-1)) (11)

[0039]

[0040] B2u2(n)是由Step2中加速度修正子系统提供的翻滚角γ和俯仰角β乘积作为压力传感器测水位系统中的加速度垂直分量，用于修正压力传感器所得到的压力值，从而准确输出铅鱼的水下高度；w2(n)是2×1的过程噪声向量，a+(n-1)是重力补偿的加速度；加速度修正子系统是X、Y、Z三轴方向的，压力传感器测水位系统是垂直方向的，将加速度修正子系统中的Z轴与压力传感器测水位系统对齐，对齐过程中的旋转矩阵即为Rγ，β；

[0041] 其中，z2(n)是Step3得到的压力传感器所在位置的水深值，H2(n)是观测矩阵，H2(n)x2(n)得到由修正后压力传感器所测压力数据计算出的水深值，v2(n)为压力传感器的测量噪声；其中，H2(n)＝[1 0]；

[0042] Step4.2、基于Step4.1所建立的利用系统状态方程，利用状态方程中的n-1时刻的状态向量去预测n时刻的状态向量，得到状态向量的预测优化值；因为压力传感器测水位系统n时刻的状态向量x2(n)分采用的分别是所得到的水深值及垂直速度，那么状态向量的预测优化值包含了优化后的水深值及垂直速度；

[0043] Step4.3、取状态向量的预测优化值中的水深值即为修正后的压力传感器所在位置的水深值。

[0044] 进一步地，所述步骤Step2.2的具体步骤如下：

[0045] 基于Step2.1所建立的利用系统状态方程，利用状态方程中的n-1时刻的状态向量去预测n时刻的状态向量，得到状态预测方程：

[0046]

[0047] 其中，是n-1时刻x1(n)的预测值，是n-1时刻x1(n-1)的预测值；

[0048] 首先求取卡尔曼增益系数K1(n)及n-1时刻加速度修正子系统预测的n时刻最小均方误差矩阵P1(n|n-1)，其具体过程如公式(14)、(15)、(16)所示，

[0049] P1(n|n-1)＝A1P1(n-1|n-1)A1T+Q1(n) (14)

[0050] K1(n)＝P1(n|n-1)H1T(n)[R1(n)+H1(n)P1(n|n-1)H1T(n)]-1 (15)[0051] 其中P1(n-1|n-1)是n-1时刻的最小均方误差矩阵，Q1(n)和R1(n)为加速度修正子系统的过程和测量噪声协方差矩阵；

[0052] Q1(n)＝-Δt2x1(n)∑Gx1(n) (16)

[0053] R1(n)＝∑acc+∑A (17)

[0054] 其中，∑G是三轴陀螺仪测量噪声的协方差，σG2是陀螺仪测量噪声的方差，∑A是加速度计测量噪声的协方差，σA2是三轴加速度计测量噪声的方差，可由式(18)、(19)得到；

[0055] ∑G＝E[nGnGT]＝σG2I3 (18)

[0056] ∑A＝E[nAnAT]＝σA2I3 (19)

[0057] 然后，通过状态预测方程对状态向量进行预测更新：

[0058]

[0059] P1(n|n)＝[I3-K1(n)H1(n)]P1(n|n-1) (21)

[0060] 其中，P1(n|n)是n时刻加速度修正子系统的最小均方误差矩阵。

[0061] 进一步地，所述步骤Step4.2的具体步骤如下：

[0062] 基于Step4.1所建立的利用系统状态方程，利用状态方程中的n-1时刻的状态向量去预测n时刻的状态向量，得到状态预测方程：

[0063]

[0064] 其中，是n-1时刻x2(n)的预测值，是n-1时刻x2(n-1)的预测值；

[0065] 首先求取卡尔曼增益系数K2(n)及n-1时刻压力传感器测水位系统预测的n时刻最小均方误差矩阵P2(n|n-1)：

[0066] P2(n|n-1)＝A2P2(n-1|n-1)A2T+Q2(n) (23)

[0067] K2(n)＝P2(n|n-1)H2T(n)[R2+H2(n)P2(n|n-1)H2T(n)]-1 (24)

[0068] 其中P2(n-1|n-1)是n-1时刻的最小均方误差矩阵，Q2(n)为压力传感器测水位系统的过程噪声协方差矩阵，R2是压力传感器测水位系统的测量噪声协方差，是大气压力带来的压力传感器的噪声；

[0069]

[0070] 然后，通过状态预测方程对状态向量进行预测更新：

[0071]

[0072] P2(n|n)＝[I3-K2(n)H2(n)]P2(n|n-1) (27)

[0073] 其中，P2(n|n)是n时刻压力传感器测水位系统的最小均方误差矩阵。

[0074] 一种基于卡尔曼滤波算法精确判断铅鱼完全入水的系统，包括压力传感器1、加速度修正子系统、压力传感器测水位系统、三轴陀螺仪、三轴加速度计、水面开关传感器、CPU、无线传输模块、上位机；

[0075] 所述压力传感器1设置在铅鱼底部用于测量其所在位置的压力值；

[0076] 所述三轴陀螺仪、三轴加速度计用于测量测量铅鱼的姿态数据；

[0077] 所述水面开关传感器设置在铅鱼的顶部，位于首尾两端，当两个水面开关传感器同时入水，则输出入水入水信号；

[0078] 所述加速度修正子系统用于利用卡尔曼滤波算法对铅鱼的姿态数据进行处理得到相对于铅鱼垂直轴的倾斜角，即翻滚角γ和俯仰角β；

[0079] 所述压力传感器测水位系统用于根据压力传感器得到的压力数值计算得到此时压力传感器所在位置的水深值；还用于根据相对于铅鱼垂直轴的倾斜角和压力传感器所在位置的水深值，利用卡尔曼滤波算法修正压力传感器所在位置的水深值；

[0080] 所述CPU用于判断修正后的压力传感器所在位置的水深值是否大于等于压力传感器与铅鱼自带的转子流速仪的高度差；如果修正后所得的水深值大于等于压力传感器与铅鱼自带的转子流速仪的高度差时，且水面开关传感器输出入水信号，则判断铅鱼完全入水；否则重新获取修正后的压力传感器所在位置的水深值再进行判断；

[0081] 所述无线传输模块用于把CPU中铅鱼是否完全入水的信号发送给上位机。

[0082] 本发明的有益效果是：

[0083] 1、因压力传感器的输出变化是线性的，使用卡尔曼滤波算法可以得到最优化的水深输出值，从而解决了自然应用中波浪会使压力传感器的信号抖动、对于压力判断不准而无法准确测量水深的问题，同时卡尔曼数据融合还可以克服时延问题。

[0084] 2、本发明为了得到稳定的压力传感器数值，加入三轴姿态传感器测量竖直方向铅鱼的翻滚角和俯仰角，利用卡尔曼滤波算法建立线性系统状态方程、观测方程，去除波浪带来的噪声和干扰，对状态压力传感器水深数值进行最优估计，从而得到更加精确和稳定的铅鱼入水深度，精确检测铅鱼是否完全入水，能达到精确检测铅鱼完全入水的目的，从而能在天然河道中更精确实现水深测量及测流过程的准确控制。附图说明

[0085] 图1是本发明铅鱼剖面安装位置示意图；

[0086] 图2是本发明的流程图。

[0087] 图1-2中各标号：1-压力传感器，2-三轴姿态修正装置，3-水面开关，h为压力传感器到转子流速仪的高度差。

具体实施方式

[0088] 实施例1：如图1-2所示，基于卡尔曼滤波算法判断铅鱼完全入水的方法，所述方法的具体步骤如下：

[0089] 所述方法的具体步骤如下：

[0090] Step1、利用三轴陀螺仪配合三轴加速度计测量铅鱼的姿态数据；

[0091] Step2、利用卡尔曼滤波算法对铅鱼的姿态数据进行处理得到相对于铅鱼垂直轴的倾斜角，即翻滚角γ和俯仰角β；

[0092] Step3、根据压力传感器得到的压力数值计算得到此时压力传感器所在位置的水深值；

[0093] Step4、根据相对于铅鱼垂直轴的倾斜角和压力传感器所在位置的水深值，利用卡尔曼滤波算法修正压力传感器所在位置的水深值；

[0094] Step5、判断修正后的压力传感器所在位置的水深值是否大于等于压力传感器与铅鱼自带的转子流速仪的高度差；

[0095] 如果修正后所得的水深值大于等于压力传感器与铅鱼自带的转子流速仪的高度差时，且水面开关传感器输出入水信号，则判断铅鱼完全入水；否则循环Step1-Step5。

[0096] 进一步地，所述Step2中：

[0097] 在加速度修正子系统中，利用卡尔曼滤波算法对测量得到铅鱼的姿态数据进行最优估计，得到相对于铅鱼垂直轴的倾斜角，即翻滚角γ和俯仰角β。

[0098] 进一步地，所述步骤Step2的具体步骤如下：

[0099] Step2.1、基于卡尔曼滤波算法建立线性系统状态方程、观测方程；

[0100] 其中，在加速度修正子系统中，基于卡尔曼滤波算法建立线性系统状态方程、观测方程分别如公式(1)、(2)：

[0101] x1(n)＝A1x1(n-1)+w1(n) (1)

[0102] z1(n)＝H1(n)x1(n)+v1(n) (2)

[0103] x1(n)是加速度修正子系统n时刻的状态向量，采用三轴加速度计坐标(即铅鱼坐T标)重力矢量来表示，具体为x1(n)＝[x1,x x1,y x1,z] ，其通过上一时刻的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)是三轴加速度计测量向量，即观测目标，是通过三轴加速度计得到的三个方向的加速度数值；

[0104] 其中，

[0105] A1＝I3-ΔtyG(n-1) (3)

[0106] yG(n-1)是3×3的三轴陀螺仪测量的斜对称矩阵，I3是3×3的单位矩阵，Δt测量的间隔时间，g为重力矢量；

[0107]

[0108] H1(n)＝gI3 (5)

[0109] Step2.2、基于Step2.1所建立的利用系统状态方程，利用状态方程中的n-1时刻的状态向量去预测n时刻的状态向量，得到状态向量的预测优化值；

[0110] Step2.3、通过状态向量的预测优化值计算得到相对于铅鱼垂直轴的倾斜角，即翻滚角γ和俯仰角β，由公式(6)得到：

[0111]

[0112] 进一步地，所述步骤Step4的具体步骤如下：

[0113] Step4.1、在压力传感器测水位系统中，基于卡尔曼滤波算法建立线性系统状态方程、观测方程；

[0114] 其中，在压力传感器测水位系统中，基于卡尔曼滤波算法建立线性系统状态方程、观测方程分别如公式(7)、(8)：

[0115] x2(n)＝A2x2(n-1)+B2u2(n)+w2(n) (7)

[0116] z2(n)＝H2(n)x2(n)+v2(n) (8)

[0117] x2(n)是压力传感器测水位系统n时刻的状态向量，x2(n)＝[h(n)v(n)]T，两项分别是所得到的水深值及垂直速度，x2(n-1)是压力传感器测水位系统n-1时刻的状态向量，A2是压力传感器的状态转移矩阵，由式(9)得到，其中Δt表示测量的间隔时间：

[0118]

[0119] B2是输入控制矩阵，由式(10)得到：

[0120]

[0121] u2(n)＝([0 0 1])·(Rγ，βa+(n-1)) (11)

[0122]

[0123] B2u2(n)是由Step2中加速度修正子系统提供的翻滚角γ和俯仰角β乘积作为压力传感器测水位系统中的加速度垂直分量，用于修正压力传感器所得到的压力值，从而准确输出铅鱼的水下高度；w2(n)是2×1的过程噪声向量，a+(n-1)是重力补偿的加速度；加速度修正子系统是X、Y、Z三轴方向的，压力传感器测水位系统是垂直方向的，将加速度修正子系统中的Z轴与压力传感器测水位系统对齐，对齐过程中的旋转矩阵即为Rγ,β；

[0124] 其中，z2(n)是Step3得到的压力传感器所在位置的水深值，H2(n)是观测矩阵，H2(n)x2(n)得到由修正后压力传感器所测压力数据计算出的水深值，v2(n)为压力传感器的测量噪声；其中，H2(n)＝[1 0]；

[0125] Step4.2、基于Step4.1所建立的利用系统状态方程，利用状态方程中的n-1时刻的状态向量去预测n时刻的状态向量，得到状态向量的预测优化值；因为压力传感器测水位系统n时刻的状态向量x2(n)分采用的分别是所得到的水深值及垂直速度，那么状态向量的预测优化值包含了优化后的水深值及垂直速度；

[0126] Step4.3、取状态向量的预测优化值中的水深值即为修正后的压力传感器所在位置的水深值。

[0127] 进一步地，所述步骤Step2.2的具体步骤如下：

[0128] 基于Step2.1所建立的利用系统状态方程，利用状态方程中的n-1时刻的状态向量去预测n时刻的状态向量，得到状态预测方程：

[0129]

[0130] 其中，是n-1时刻x1(n)的预测值，是n-1时刻x1(n-1)的预测值；

[0131] 首先求取卡尔曼增益系数K1(n)及n-1时刻加速度修正子系统预测的n时刻最小均方误差矩阵P1(n|n-1)，其具体过程如公式(14)、(15)、(16)所示，

[0132] P1(n|n-1)＝A1P1(n-1|n-1)A1T+Q1(n) (14)

[0133] K1(n)＝P1(n|n-1)H1T(n)[R1(n)+H1(n)P1(n|n-1)H1T(n)]-1 (15)[0134] 其中P1(n-1|n-1)是n-1时刻的最小均方误差矩阵，Q1(n)和R1(n)为加速度修正子系统的过程和测量噪声协方差矩阵；

[0135] Q1(n)＝-Δt2x1(n)ΣGx1(n) (16)

[0136] R1(n)＝Σacc+∑A (17)

[0137] 其中，∑G是三轴陀螺仪测量噪声的协方差，σG2是陀螺仪测量噪声的方差，∑A是加速度计测量噪声的协方差，σA2是三轴加速度计测量噪声的方差，可由式(18)、(19)得到；

[0138] ΣG＝E[nGnGT]＝σG2I3 (18)

[0139] ∑A＝E[nAnAT]＝σA2I3 (19)

[0140] 然后，通过状态预测方程对状态向量进行预测更新：

[0141]

[0142] P1(n|n)＝[I3-K1(n)H1(n)]P1(n|n-1) (21)

[0143] 其中，P1(n|n)是n时刻加速度修正子系统的最小均方误差矩阵。

[0144] 进一步地，所述步骤Step4.2的具体步骤如下：

[0145] 基于Step4.1所建立的利用系统状态方程，利用状态方程中的n-1时刻的状态向量去预测n时刻的状态向量，得到状态预测方程：

[0146]

[0147] 其中，是n-1时刻x2(n)的预测值，是n-1时刻x2(n-1)的预测值；

[0148] 首先求取卡尔曼增益系数K2(n)及n-1时刻压力传感器测水位系统预测的n时刻最小均方误差矩阵P2(n|n-1)：

[0149] P2(n|n-1)＝A2P2(n-1|n-1)A2T+Q2(n) (23)

[0150] K2(n)＝P2(n|n-1)H2T(n)[R2+H2(n)P2(n|n-1)H2T(n)]-1 (24)

[0151] 其中P2(n-1|n-1)是n-1时刻的最小均方误差矩阵，Q2(n)为压力传感器测水位系统的过程噪声协方差矩阵，R2是压力传感器测水位系统的测量噪声协方差，是大气压力带来的压力传感器的噪声；

[0152]

[0153] 然后，通过状态预测方程对状态向量进行预测更新：

[0154]

[0155] P2(n|n)＝[I3-K2(n)H2(n)]P2(n|n-1) (27)

[0156] 其中，P2(n|n)是n时刻压力传感器测水位系统的最小均方误差矩阵。

[0157] 一种基于卡尔曼滤波算法精确判断铅鱼完全入水的系统，包括压力传感器1、加速度修正子系统、压力传感器测水位系统、三轴陀螺仪、三轴加速度计、水面开关传感器、CPU、无线传输模块、上位机；

[0158] 所述压力传感器1设置在铅鱼底部用于测量其所在位置的压力值；

[0159] 所述三轴陀螺仪、三轴加速度计用于测量测量铅鱼的姿态数据；

[0160] 所述水面开关传感器设置在铅鱼的顶部，位于首尾两端，当两个水面开关传感器同时入水，则输出入水入水信号；

[0161] 所述加速度修正子系统用于利用卡尔曼滤波算法对铅鱼的姿态数据进行处理得到相对于铅鱼垂直轴的倾斜角，即翻滚角γ和俯仰角β；

[0162] 所述压力传感器测水位系统用于根据压力传感器得到的压力数值计算得到此时压力传感器所在位置的水深值；还用于根据相对于铅鱼垂直轴的倾斜角和压力传感器所在位置的水深值，利用卡尔曼滤波算法修正压力传感器所在位置的水深值；

[0163] 所述CPU用于判断修正后的压力传感器所在位置的水深值是否大于等于压力传感器与铅鱼自带的转子流速仪的高度差；如果修正后所得的水深值大于等于压力传感器与铅鱼自带的转子流速仪的高度差时，且水面开关传感器输出入水信号，则判断铅鱼完全入水；否则重新获取修正后的压力传感器所在位置的水深值再进行判断；

[0164] 所述无线传输模块用于把CPU中铅鱼是否完全入水的信号发送给上位机。

[0165] 所述水面开关传感器安装在与转子流速仪平行同高度位置，位于铅鱼的首尾各一个。为减少波浪对压力传感器的影响，将压力传感器、三轴陀螺仪和三轴加速度计安装在铅鱼底部垂直中心线偏后位置，如图1所示，与转子流速仪形成竖直高度差h。三轴姿态修正装置包括三轴加速度计和三轴陀螺仪，为使其对铅鱼姿态测量的坐标与铅鱼本身坐标一致，将三轴姿态修正装置安装在铅鱼水平中心线，为此三轴姿态修正装置需要贯穿铅鱼。为避免水中侧流压力传感器冲击造成巨大误差，将压力传感器对称安装在铅鱼底部，与三轴姿态修正装置处于同一垂直线上，同时便于计算。水面开关传感器和压力传感器的信号经加速度修正子系统、压力传感器测水位系统、CPU处理，通过无线传输模块(LoRa天线)传输到上位机。水面开关传感器与修正后的压力传感器所在位置的水深值信号共同确定铅鱼是否入水，当修正后所得的水深值大于等于设定值h，且水面开关传感器输出入水信号，则判断铅鱼完全入水，铅鱼通信电路才开始工作，从而铅鱼用电效率。

[0166] 上面结合附图对本发明的具体实施方式作了详细说明，但是本发明并不限于上述实施方式，在本领域普通技术人员所具备的知识范围内，还可以在不脱离本发明宗旨的前提下作出各种变化。

标题	发布/更新时间	阅读量
超声波输出装置	2020-05-08	573
薄膜天线	2020-05-11	952
用于无线通信中的覆盖增强的公共搜索空间设计	2020-05-08	345
多通道无源互调数字抵消电路	2020-05-08	334
一种被用于无线通信的用户设备、基站中的方法和装置	2020-05-08	68
基站装置、终端装置、无线通信系统及发送时机设定方法	2020-05-08	626
包括路由器、控制器和非发光可变透射设备的系统及其使用方法	2020-05-08	403
新无线电中的连接模式移动性	2020-05-08	505
一种被用于无线通信的用户设备、基站中的方法和装置	2020-05-08	119
无线电力系统的保护和控制	2020-05-11	233

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

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

技术领域

背景技术

发明内容

具体实施方式

该功能需要专业版企业版VIP权限，您可以：