阅读说明：本技术 基于卡尔曼滤波器的轨道阀阀位测量系统及方法 (Kalman filter-based rail valve position measuring system and method ) 是由 王悦 倪娜 杨帆 刘伟 甄玉龙 王旭 陈涛 马玉林 张亮 于 2019-08-27 设计创作，主要内容包括：本申请公开了一种基于卡尔曼滤波器的轨道阀阀位测量系统及方法,包括：布置于轨道阀上的陀螺仪和霍尔传感器,还包括控制器,所述控制器通过数据采集器与陀螺仪和霍尔传感器相连接；所述数据采集器获取陀螺仪和霍尔传感器的测量数据,控制器构建卡尔曼滤波器,控制所述数据采集器对陀螺仪测量数据进行量测更新,得到更新后的一步预测第一状态向量与一步预测系统协方差量；根据预测系统协方差量获得更新的卡尔曼滤波器的增益矩阵；根据所述增益矩阵进一步更新当前第一状态向量与当前系统协方差量。本发明通过融合霍尔传感器与陀螺仪数据,解决了单一传感器测量精度较差的问题。(The application discloses track valve position measurement system and method based on Kalman filter, include: the gyroscope and the Hall sensor are arranged on the track valve, and the controller is connected with the gyroscope and the Hall sensor through a data collector; the data acquisition unit acquires measurement data of a gyroscope and a Hall sensor, the controller constructs a Kalman filter, and controls the data acquisition unit to measure and update the measurement data of the gyroscope to obtain updated one-step prediction first state vector and one-step prediction system covariance quantity; obtaining an updated gain matrix of the Kalman filter according to the prediction system covariance quantity; and further updating the current first state vector and the current system covariance amount according to the gain matrix. According to the invention, the problem of poor measurement accuracy of a single sensor is solved by fusing data of the Hall sensor and the gyroscope.)

基于卡尔曼滤波器的轨道阀阀位测量系统及方法

技术领域

本申请涉及阀门领域的阀位检测技术，尤其涉及一种基于卡尔曼滤波器的轨道阀阀位测量系统及方法。

背景技术

轨道阀作为单阀座双向密封的球阀综合了闸阀、球阀、截止阀和旋塞阀的优点，在输气管道上广泛应用。轨道阀通常配置有反馈阀门阀位反馈器。反馈器配合现场PLC(可编程逻辑电路)系统可实现阀门开关状态的远程反馈，为管理阀门的监控系统提供相应的阀门位置信号输出。但受现有轨道阀结构限制，集成的反馈器主要用于供现场操作人员观察，直接与PLC系统结合采样精度有限，而且现场PLC系统与传感器间的线缆敷设受到控制手***作干扰，会对管道上附属设施的装卸造成不良影响。

采用非接触式传感器进行阀门旋转量的采集，目前通用性传感器有陀螺仪、霍尔传感器等。双向霍尔传感器能够对间隔摆放的铁磁物块进行计数，还能辨别增减方向。在轨道阀手轮上安装齿条状圆盘，即可采集阀门的旋转量。然而，在阀门转动过程中，难以避免垂直于旋转面的震动干扰，造成霍尔传感器计数误差。陀螺仪存在温漂、时漂、随机干扰等多种误差，使其难以单独应用于旋转测量系统中。因此，如何实现高效精确的轨道阀阀位测量，成为当前面临的重要技术问题。

发明内容

本申请实施例提供一种基于卡尔曼滤波器的轨道阀阀位测量系统及方法，解决高效精确的轨道阀阀位测量的就技术问题。

本申请的技术方案中提供一种基于卡尔曼滤波器的轨道阀阀位测量系统，包括：布置于轨道阀上的陀螺仪和霍尔传感器，还包括控制器，所述控制器通过数据采集器与陀螺仪和霍尔传感器相连接；其中，

所述数据采集器获取包括陀螺仪阀门转角、陀螺仪角速度、陀螺仪常值偏差、陀螺仪测量噪声的陀螺仪测量数据，获取包括霍尔传感器阀门转角、霍尔传感器测量噪声的霍尔传感器测量数据；所述控制器根据陀螺仪测量数据确定出陀螺仪下一个阀门转角预估值，具体方式为：

θ(k+1)＝θ(k)+(ω(k)-b(k)+w(k))dt

其中，θ(k+1)下一个陀螺仪阀门转角预估值，θ(k)为当前陀螺仪阀门转角，ω(k)为当前陀螺仪角速度，b(k)为当前陀螺仪常值偏差，w(k)为当前陀螺仪测量噪声，dt为测量周期，k表示当前为第k次测量，k+1表示当前为第k+1次测量；

根据霍尔传感器阀门转角、霍尔传感器测量噪声，确定霍尔传感器的阀门转角测量值，具体方式为：

z(k)＝θ(k)+v(k)

其中，k表示当前为第k次测量，z(k)为误差校正后的霍尔传感器的阀门转角测量值，θ(k)为检测到的霍尔传感器真实阀门转角，v(k)为霍尔传感器测量噪声；

以陀螺仪阀门转角作为第一状态向量，以采用霍尔传感器的阀门转角测量值估计得到的陀螺仪常值偏差作为第二状态向量，构建卡尔曼滤波器，所述卡尔曼滤波器为：

其中，系统状态矩阵为：

系统量测向量为：H＝[1 0]

状态向量矩阵为：

k表示第k次测量，k-1表示第k-1次测量，

U(k-1)为陀螺仪前一次的角速度，X(k-1)为陀螺仪的前一次阀门转角

Z(k)为霍尔传感器测得的转角，W(k)陀螺仪测量噪声，V(k)霍尔传感器测量噪声，T则为系统采样周期。

所述陀螺仪常值偏差b(k)通过下述公式，在采用霍尔传感器的阀门转角测量值z(k)＝θ(k+1)时，可以估计得到的陀螺仪常值偏差b(k)：

θ(k+1)＝θ(k)+(ω(k)-b(k)+w(k))dt

z(k)＝θ(k)+v(k)。

所述控制器基于卡尔曼滤波器，控制所述数据采集器对陀螺仪测量数据进行量测更新，得到更新后的一步预测第一状态向量与一步预测系统协方差量；根据预测系统协方差量获得更新的卡尔曼滤波器的增益矩阵；根据所述增益矩阵进一步更新当前第一状态向量与当前系统协方差量。

基于上述的轨道阀阀位测量系统，本申请的技术方案还提供一种基于卡尔曼滤波器的轨道阀阀位测量方法，包括：

根据检测计算获得包括陀螺仪阀门转角、陀螺仪角速度、陀螺仪常值偏差、陀螺仪测量噪声的陀螺仪测量数据，确定出陀螺仪下一个阀门转角预估值；根据检测计算获得的霍尔传感器阀门转角、霍尔传感器测量噪声，确定霍尔传感器的阀门转角测量值；以陀螺仪阀门转角作为第一状态向量，以采用霍尔传感器的阀门转角测量值估计得到的陀螺仪常值偏差作为第二状态向量，构建卡尔曼滤波器；

基于卡尔曼滤波器，对陀螺仪测量数据进行量测更新，得到更新后的一步预测第一状态向量与一步预测系统协方差量；根据预测系统协方差量获得更新的卡尔曼滤波器的增益矩阵；根据所述增益矩阵进一步更新当前第一状态向量与当前系统协方差量。

所述确定出下一个陀螺仪阀门转角预估值的方式具体为：

θ(k+1)＝θ(k)+(ω(k)-b(k)+w(k))dt

其中，θ(k+1)下一个陀螺仪阀门转角预估值，θ(k)为当前陀螺仪阀门转角，ω(k)为当前陀螺仪角速度，b(k)为当前陀螺仪常值偏差，w(k)为当前陀螺仪测量噪声，dt为测量周期，k表示当前为第k次测量，k+1表示当前为第k+1次测量。

所述确定霍尔传感器的阀门转角测量值的方式具体为：

z(k)＝θ(k)+v(k)

其中，k表示当前为第k次测量，z(k)为误差校正后的霍尔传感器的阀门转角测量值，θ(k)为检测到的霍尔传感器真实阀门转角，v(k)为霍尔传感器测量噪声。

所述卡尔曼滤波器为：

其中，系统状态矩阵为：

系统量测向量为：H＝[1 0]

状态向量矩阵为：

k表示第k次测量，k-1表示第k-1次测量，

U(k-1)为陀螺仪前一次的角速度，X(k-1)为陀螺仪的前一次阀门转角

Z(k)为霍尔传感器测得的转角，W(k)陀螺仪测量噪声，V(k)霍尔传感器测量噪声，T则为系统采样周期。

所述陀螺仪常值偏差b(k)通过下述公式，在采用霍尔传感器的阀门转角测量值z(k)＝θ(k+1)时，可以估计得到的陀螺仪常值偏差b(k)：

θ(k+1)＝θ(k)+(ω(k)-b(k)+w(k))dt

z(k)＝θ(k)+v(k)。

本申请实施例采用的上述至少一个技术方案能够达到以下有益效果：

本发明使用多传感器融合滤波方法，融合霍尔传感器与陀螺仪数据，抑制噪声干扰，解决了单一传感器测量精度较差的问题，具体的，通过建立传感器误差模型补偿随机漂移误差，通过使用自适应量测噪声矩阵减小垂直于旋转面的震动干扰对旋转角度解算的影响。本发明的技术方案，实现了低成本，精度高的阀位测量系统，发展了高效精确的阀位测量方法。

附图说明

此处所说明的附图用来提供对本申请的进一步理解，构成本申请的一部分，本申请的示意性实施例及其说明用于解释本申请，并不构成对本申请的不当限定。在附图中：

图1为本申请中的一种基于卡尔曼滤波器的轨道阀阀位测量系统示意图；

具体实施方式

为使本申请的目的、技术方案和优点更加清楚，下面将结合本申请具体实施例及相应的附图对本申请技术方案进行清楚、完整地描述。显然，所描述的实施例仅是本申请一部分实施例，而不是全部的实施例。基于本申请中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本申请保护的范围。

以下结合附图，详细说明本申请各实施例提供的技术方案。

卡尔曼滤波器算法是一种“最优化自回归数据处理算法(optimal recursivedata processing algorithm)”，已经广泛应用超过30年，应用领域包括机器人导航、控制、传感器数据融合、雷达系统、导弹追踪等。近年来更被应用于计算机图像处理，例如头脸识别，图像分割，图像边缘检测等等。卡尔曼滤波器(The Kalman Filter)引入一个离散控制过程的系统。该系统可用一个线性随机微分方程(Linear Stochastic Differenceequation)再加上系统的测量值来描述：

线性方程：X(k)＝A X(k-1)+B U(k)+W(k)

测量值：Z(k)＝H X(k)+V(k)

上述的两个公式中，卡尔曼滤波器假设k时刻的真实状态是从(k-1)时刻的状态演化而来，X(k)是k时刻的系统状态，U(k)是k时刻对系统的控制量。A和B是系统参数，对于多模型系统，它们为矩阵。Z(k)是k时刻的测量值，H是测量系统的参数，对于多测量系统，H为矩阵。W(k)和V(k)分别表示过程和测量的噪声。卡尔曼滤波器的操作包括两个阶段：预测与更新。在预测阶段，滤波器使用上一状态的估计，做出对当前状态的估计。在更新阶段，滤波器利用对当前状态的观测值优化在预测阶段获得的预测值，以获得一个更精确的新估计值。

针对现有技术中，采用单一传感器测量精度较差而无法高效准确进行轨道阀阀位测量的问题，本申请的技术方案中通过采集陀螺仪和霍尔传感器的检测数据，并构建卡尔曼滤波器，基于卡尔曼滤波器进行数据融合而实现了高效准确的轨道阀阀位测量。

本申请的技术方案中提供一种基于卡尔曼滤波器的轨道阀阀位测量系统，如图1所示，包括：布置于轨道阀上的陀螺仪和霍尔传感器，还包括控制器，所述控制器通过数据采集器与陀螺仪和霍尔传感器相连接；其中，

所述数据采集器获取包括陀螺仪阀门转角、陀螺仪角速度、陀螺仪常值偏差、陀螺仪测量噪声的陀螺仪测量数据，获取包括霍尔传感器阀门转角、霍尔传感器测量噪声的霍尔传感器测量数据；所述控制器根据陀螺仪测量数据确定出陀螺仪下一个阀门转角预估值，具体方式为：

θ(k+1)＝θ(k)+(ω(k)-b(k)+w(k))dt

其中，θ(k+1)下一个陀螺仪阀门转角预估值，θ(k)为当前陀螺仪阀门转角，ω(k)为当前陀螺仪角速度，b(k)为当前陀螺仪常值偏差，w(k)为当前陀螺仪测量噪声，dt为测量周期，k表示当前为第k次测量，k+1表示当前为第k+1次测量；

根据霍尔传感器阀门转角、霍尔传感器测量噪声，确定霍尔传感器的阀门转角测量值，具体方式为：

z(k)＝θ(k)+v(k)

其中，k表示当前为第k次测量，z(k)为误差校正后的霍尔传感器的阀门转角测量值，θ(k)为检测到的霍尔传感器真实阀门转角，v(k)为霍尔传感器测量噪声；

以陀螺仪阀门转角作为第一状态向量，以采用霍尔传感器的阀门转角测量值估计得到的陀螺仪常值偏差作为第二状态向量，构建卡尔曼滤波器，所述卡尔曼滤波器为：

其中，系统状态矩阵为：

系统量测向量为：H＝[1 0]

状态向量矩阵为：

k表示第k次测量，k-1表示第k-1次测量，

U(k-1)为陀螺仪前一次的角速度，X(k-1)为陀螺仪的前一次阀门转角

Z(k)为霍尔传感器测得的转角，W(k)陀螺仪测量噪声，V(k)霍尔传感器测量噪声，T则为系统采样周期。

所述陀螺仪常值偏差b(k)通过下述公式，在采用霍尔传感器的阀门转角测量值z(k)＝θ(k+1)时，可以估计得到的陀螺仪常值偏差b(k)：

θ(k+1)＝θ(k)+(ω(k)-b(k)+w(k))dt

z(k)＝θ(k)+v(k)。

所述控制器基于卡尔曼滤波器，控制所述数据采集器对陀螺仪测量数据进行量测更新，得到更新后的一步预测第一状态向量与一步预测系统协方差量；根据预测系统协方差量获得更新的卡尔曼滤波器的增益矩阵；根据所述增益矩阵进一步更新当前第一状态向量与当前系统协方差量。

基于上述的轨道阀阀位测量系统，还可以提供一种基于卡尔曼滤波器的轨道阀阀位测量方法，包括：

根据检测计算获得包括陀螺仪阀门转角、陀螺仪角速度、陀螺仪常值偏差、陀螺仪测量噪声的陀螺仪测量数据，确定出陀螺仪下一个阀门转角预估值；根据检测计算获得的霍尔传感器阀门转角、霍尔传感器测量噪声，确定霍尔传感器的阀门转角测量值；以陀螺仪阀门转角作为第一状态向量，以采用霍尔传感器的阀门转角测量值估计得到的陀螺仪常值偏差作为第二状态向量，构建卡尔曼滤波器；

基于卡尔曼滤波器，对陀螺仪测量数据进行量测更新，得到更新后的一步预测第一状态向量与一步预测系统协方差量；根据预测系统协方差量获得更新的卡尔曼滤波器的增益矩阵；根据所述增益矩阵进一步更新当前第一状态向量与当前系统协方差量。

所述确定出下一个陀螺仪阀门转角预估值的方式具体为：

θ(k+1)＝θ(k)+(ω(k)-b(k)+w(k))dt

其中，θ(k+1)下一个陀螺仪阀门转角预估值，θ(k)为当前陀螺仪阀门转角，ω(k)为当前陀螺仪角速度，b(k)为当前陀螺仪常值偏差，w(k)为当前陀螺仪测量噪声，dt为测量周期，k表示当前为第k次测量，k+1表示当前为第k+1次测量。

所述确定霍尔传感器的阀门转角测量值的方式具体为：

z(k)＝θ(k)+v(k)

其中，k表示当前为第k次测量，z(k)为误差校正后的霍尔传感器的阀门转角测量值，θ(k)为检测到的霍尔传感器真实阀门转角，v(k)为霍尔传感器测量噪声。

所述卡尔曼滤波器为：

其中，系统状态矩阵为：

系统量测向量为：H＝[1 0]

状态向量矩阵为：

k表示第k次测量，k-1表示第k-1次测量，

U(k-1)为陀螺仪前一次的角速度，X(k-1)为陀螺仪的前一次阀门转角

Z(k)为霍尔传感器测得的转角，W(k)陀螺仪测量噪声，V(k)霍尔传感器测量噪声，T则为系统采样周期。

所述陀螺仪常值偏差b(k)通过下述公式，在采用霍尔传感器的阀门转角测量值z(k)＝θ(k+1)时，可以估计得到的陀螺仪常值偏差b(k)：

θ(k+1)＝θ(k)+(ω(k)-b(k)+w(k))dt

z(k)＝θ(k)+v(k)。

上述本申请的阀位测量系统及方法，可以适用于各类手动、电动阀门阀位的解算，同时也适用于旋转多圈的阀门。轨道阀是一种手动、平行多圈旋转阀门，在轨道阀手轮上固装齿条状圆盘，采用霍尔式旋转感知传感器、MEMS惯性感知传感器、MEMS陀螺仪。对于不同的传感器，具有不同的干扰及误差特性，因此首先确定传感器的误差模型与相关物理量的解算模型，利用卡尔曼滤波对采样得到的陀螺仪、霍尔传感器测量数据进行数据融合，得到轨道阀的旋转角度信息(阀位)。

实施例：基于卡尔曼滤波算法的轨道阀阀位测量方案

步骤1：数据采集器采集陀螺仪、霍尔传感器的输出数据

可以采用STM32标准的数据采集器，自动辨别阀门极限旋转角度以及当前角度，无需人来授予极限角度。所述STM32数据采集器快速采集三轴陀螺仪、霍尔传感器输出数据，采样频率1kHZ；同时，在采样间隔中，处理旋转角度数据、更新阀门极限转角，频率为100HZ。其中，

陀螺输出信号y_g,t包含了真实角速率ω_t，零偏ε_g,t以及高斯白噪声ν_g,t，如下式所示：y_g,t＝ω_t+ε_g,t+ν_g,t

经过四元数解算过程，陀螺零偏ε_g,t是引起三维失准角的主要误差源，陀螺零偏属于低频、慢变过程，有ν_g,t为零均值高斯白噪声。

步骤2：陀螺仪能直接测量阀门旋转角速度，并通过积分运算获得角度信息，但是陀螺仪测量模型存在着相应的漂移误差，因此，结合测量模型与轨道阀阀位测量系统，通过实验获得陀螺仪在零输入状态下的误差漂移特性，根据数据拟合模型，结合非线性最小二乘法拟合实验数据，建立陀螺仪误差数学模型。所述陀螺仪可以是MEMS陀螺仪。其中，所述误差模型为：

θ(k+1)＝θ(k)+(ω(k)-b(k)+w(k))dt

其中，θ(k)为阀门转角，ω(k)为陀螺仪角速度，b(k)为陀螺仪常值偏差，

w(k)为陀螺仪测量噪声，dt为测量周期。

步骤3：霍尔传感器可根据初始位置及转动过程中的计数值计算出当前角度，但是难以避免由于垂直于旋转面的震动干扰，对霍尔传感器造成的计数误差。因此，根据卡尔曼滤波器的量测模型，建立利用霍尔传感器进行转角解算的数学模型，其中，转角解算模型为：

z(k)＝θ(k)+v(k)

其中，z(k)为霍尔传感器测量得出的阀门转角，θ(k)为真实阀门转角，v(k为霍尔传感器测量噪声。

步骤4：由于惯性传感器自身的性能和特点，单独使用陀螺仪元件解算阀门旋转角度，会存在严重的误差问题，因此利用MEMS陀螺仪误差模型和轨道阀阀位测量模型构建卡尔曼滤波器。对系统旋转角度测量数据进行数据融合，抑制噪声干扰，补偿误差。根据系统的离散数学模型

得系统状态矩阵：

系统量测向量：

H＝[1 0]

其中，状态U(k)为陀螺仪输出角速度，Z(k)为霍尔传感器测得的转角，W(k)和V(k)分别为陀螺仪测量噪声和霍尔传感器测量噪声，它们的统计特性均为零均值高斯白噪声，用Q和R分别为表示它们噪声方差阵。T则为系统采样周期。

步骤5：利用卡尔曼滤波算法对轨道阀阀位测量数据进行数据融合。其步骤为：

步骤5-1，采样陀螺仪、霍尔传感器转速和转角测量数据：

对陀螺仪测量数据进行量测更新，采样陀螺仪、霍尔传感器转速和转角测量数据，得到更新后的一步预测状态量与一步预测系统协方差量；

步骤5-2，采样数据时间更新过程：

步骤5-3，量测更新过程：

卡尔曼增益：

K(k)＝P(k/k-1)H^T[HP(k/k-1)H^T+R(k)]^-1

更新的状态估计：

X(k)＝X(k/k-1)+K(k)(Z(k)-HX(k/k-1))

更新的协方差估计：

P(k)＝(I-K(k)H)P(k/k-1)(I-K(k)H)^T+K(k)R(k)K(k)^T

通过上述实施过程，确定传感器的误差模型与相关物理量的解算模型，利用卡尔曼滤波器对采样得到的陀螺仪、霍尔传感器测量数据进行数据融合，最终得到轨道阀的旋转角度信息(阀位)。

需要说明的是，术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、商品或者设备不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种过程、方法、商品或者设备所固有的要素。在没有更多限制的情况下，由语句“包括一个......”限定的要素，并不排除在包括所述要素的过程、方法、商品或者设备中还存在另外的相同要素。

以上所述仅为本申请的实施例而已，并不用于限制本申请。对于本领域技术人员来说，本申请可以有各种更改和变化。凡在本申请的精神和原理之内所作的任何修改、等同替换、改进等，均应包含在本申请的权利要求范围之内。