CN110566716A - 基于卡尔曼滤波器的轨道阀阀位测量***及方法 - Google Patents

基于卡尔曼滤波器的轨道阀阀位测量***及方法 Download PDF

Info

Publication number
CN110566716A
CN110566716A CN201910798322.8A CN201910798322A CN110566716A CN 110566716 A CN110566716 A CN 110566716A CN 201910798322 A CN201910798322 A CN 201910798322A CN 110566716 A CN110566716 A CN 110566716A
Authority
CN
China
Prior art keywords
gyroscope
measurement
valve
hall sensor
current
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.)
Granted
Application number
CN201910798322.8A
Other languages
English (en)
Other versions
CN110566716B (zh
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.)
Beijing Institute of Radio Metrology and Measurement
Original Assignee
Beijing Institute of Radio Metrology and Measurement
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 Beijing Institute of Radio Metrology and Measurement filed Critical Beijing Institute of Radio Metrology and Measurement
Priority to CN201910798322.8A priority Critical patent/CN110566716B/zh
Publication of CN110566716A publication Critical patent/CN110566716A/zh
Application granted granted Critical
Publication of CN110566716B publication Critical patent/CN110566716B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Classifications

    • F—MECHANICAL ENGINEERING; LIGHTING; HEATING; WEAPONS; BLASTING
    • F16—ENGINEERING ELEMENTS AND UNITS; GENERAL MEASURES FOR PRODUCING AND MAINTAINING EFFECTIVE FUNCTIONING OF MACHINES OR INSTALLATIONS; THERMAL INSULATION IN GENERAL
    • F16K—VALVES; TAPS; COCKS; ACTUATING-FLOATS; DEVICES FOR VENTING OR AERATING
    • F16K37/00—Special means in or on valves or other cut-off apparatus for indicating or recording operation thereof, or for enabling an alarm to be given
    • F16K37/0075—For recording or indicating the functioning of a valve in combination with test equipment
    • F16K37/0083—For recording or indicating the functioning of a valve in combination with test equipment by measuring valve parameters
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F17/00—Digital computing or data processing equipment or methods, specially adapted for specific functions
    • G06F17/10—Complex mathematical operations
    • G06F17/11—Complex mathematical operations for solving equations, e.g. nonlinear equations, general mathematical optimization problems
    • G06F17/13—Differential equations
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F17/00—Digital computing or data processing equipment or methods, specially adapted for specific functions
    • G06F17/10—Complex mathematical operations
    • G06F17/16—Matrix or vector computation, e.g. matrix-matrix or matrix-vector multiplication, matrix factorization

Landscapes

  • Engineering & Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Mathematical Physics (AREA)
  • Pure & Applied Mathematics (AREA)
  • Computational Mathematics (AREA)
  • Mathematical Optimization (AREA)
  • Mathematical Analysis (AREA)
  • Theoretical Computer Science (AREA)
  • Data Mining & Analysis (AREA)
  • General Engineering & Computer Science (AREA)
  • Algebra (AREA)
  • Databases & Information Systems (AREA)
  • Software Systems (AREA)
  • Computing Systems (AREA)
  • Mechanical Engineering (AREA)
  • Operations Research (AREA)
  • Navigation (AREA)
  • Transmission And Conversion Of Sensor Element Output (AREA)

Abstract

本申请公开了一种基于卡尔曼滤波器的轨道阀阀位测量***及方法，包括：布置于轨道阀上的陀螺仪和霍尔传感器，还包括控制器，所述控制器通过数据采集器与陀螺仪和霍尔传感器相连接；所述数据采集器获取陀螺仪和霍尔传感器的测量数据，控制器构建卡尔曼滤波器，控制所述数据采集器对陀螺仪测量数据进行量测更新，得到更新后的一步预测第一状态向量与一步预测***协方差量；根据预测***协方差量获得更新的卡尔曼滤波器的增益矩阵；根据所述增益矩阵进一步更新当前第一状态向量与当前***协方差量。本发明通过融合霍尔传感器与陀螺仪数据，解决了单一传感器测量精度较差的问题。

Description

基于卡尔曼滤波器的轨道阀阀位测量***及方法
技术领域
本申请涉及阀门领域的阀位检测技术，尤其涉及一种基于卡尔曼滤波器的轨道阀阀位测量***及方法。
背景技术
轨道阀作为单阀座双向密封的球阀综合了闸阀、球阀、截止阀和旋塞阀的优点，在输气管道上广泛应用。轨道阀通常配置有反馈阀门阀位反馈器。反馈器配合现场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。其中，
陀螺输出信号yg,t包含了真实角速率ωt，零偏εg,t以及高斯白噪声νg,t，如下式所示：yg,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)HT[HP(k/k-1)HT+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
通过上述实施过程，确定传感器的误差模型与相关物理量的解算模型，利用卡尔曼滤波器对采样得到的陀螺仪、霍尔传感器测量数据进行数据融合，最终得到轨道阀的旋转角度信息(阀位)。
需要说明的是，术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、商品或者设备不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种过程、方法、商品或者设备所固有的要素。在没有更多限制的情况下，由语句“包括一个......”限定的要素，并不排除在包括所述要素的过程、方法、商品或者设备中还存在另外的相同要素。
以上所述仅为本申请的实施例而已，并不用于限制本申请。对于本领域技术人员来说，本申请可以有各种更改和变化。凡在本申请的精神和原理之内所作的任何修改、等同替换、改进等，均应包含在本申请的权利要求范围之内。

Claims (10)

1.一种基于卡尔曼滤波器的轨道阀阀位测量***，其特征在于，包括：布置于轨道阀上的陀螺仪和霍尔传感器，还包括控制器，所述控制器通过数据采集器与陀螺仪和霍尔传感器相连接；其中，
所述数据采集器获取包括陀螺仪阀门转角、陀螺仪角速度、陀螺仪常值偏差、陀螺仪测量噪声的陀螺仪测量数据，获取包括霍尔传感器阀门转角、霍尔传感器测量噪声的霍尔传感器测量数据；
所述控制器根据陀螺仪测量数据确定出陀螺仪下一个阀门转角预估值；根据霍尔传感器阀门转角、霍尔传感器测量噪声，确定霍尔传感器的阀门转角测量值；以陀螺仪阀门转角作为第一状态向量，以采用霍尔传感器的阀门转角测量值估计得到的陀螺仪常值偏差作为第二状态向量，构建卡尔曼滤波器；
所述控制器基于卡尔曼滤波器，控制所述数据采集器对陀螺仪测量数据进行量测更新，得到更新后的一步预测第一状态向量与一步预测***协方差量；根据预测***协方差量获得更新的卡尔曼滤波器的增益矩阵；根据所述增益矩阵进一步更新当前第一状态向量与当前***协方差量。
2.根据权利要求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)为霍尔传感器测量噪声。
3.根据权利要求1所述的基于卡尔曼滤波器的轨道阀阀位测量***，其特征在于，所述陀螺仪常值偏差b(k)通过下述公式，在采用霍尔传感器的阀门转角测量值z(k)＝θ(k+1)时，可以估计得到的陀螺仪常值偏差b(k)：
θ(k+1)＝θ(k)+(ω(k)-b(k)+w(k))dt
z(k)＝θ(k)+v(k)。
4.根据权利要求1或2所述的基于卡尔曼滤波器的轨道阀阀位测量***，其特征在于，所述卡尔曼滤波器为：
其中，***状态矩阵为：
***量测向量为：H＝[1 0]
状态向量矩阵为：
k表示第k次测量，k-1表示第k-1次测量，
U(k-1)为陀螺仪前一次的角速度，X(k-1)为陀螺仪的前一次阀门转角Z(k)为霍尔传感器测得的转角，W(k)陀螺仪测量噪声，V(k)霍尔传感器测量噪声，T则为***采样周期。
5.一种基于卡尔曼滤波器的轨道阀阀位测量方法，其特征在于，包括：
根据检测计算获得包括陀螺仪阀门转角、陀螺仪角速度、陀螺仪常值偏差、陀螺仪测量噪声的陀螺仪测量数据，确定出陀螺仪下一个阀门转角预估值；根据检测计算获得的霍尔传感器阀门转角、霍尔传感器测量噪声，确定霍尔传感器的阀门转角测量值；以陀螺仪阀门转角作为第一状态向量，以采用霍尔传感器的阀门转角测量值估计得到的陀螺仪常值偏差作为第二状态向量，构建卡尔曼滤波器；
基于卡尔曼滤波器，对陀螺仪测量数据进行量测更新，得到更新后的一步预测第一状态向量与一步预测***协方差量；根据预测***协方差量获得更新的卡尔曼滤波器的增益矩阵；根据所述增益矩阵进一步更新当前第一状态向量与当前***协方差量。
6.根据权利要求5所述的基于卡尔曼滤波器的轨道阀阀位测量方法，其特征在于，所述确定出下一个陀螺仪阀门转角预估值的方式具体为：
θ(k+1)＝θ(k)+(ω(k)-b(k)+w(k))dt
其中，θ(k+1)下一个陀螺仪阀门转角预估值，θ(k)为当前陀螺仪阀门转角，ω(k)为当前陀螺仪角速度，b(k)为当前陀螺仪常值偏差，w(k)为当前陀螺仪测量噪声，dt为测量周期，k表示当前为第k次测量，k+1表示当前为第k+1次测量。
7.根据权利要求5所述的基于卡尔曼滤波器的轨道阀阀位测量方法，其特征在于，所述确定霍尔传感器的阀门转角测量值的方式具体为：
z(k)＝θ(k)+v(k)
其中，k表示当前为第k次测量，z(k)为误差校正后的霍尔传感器的阀门转角测量值，θ(k)为检测到的霍尔传感器真实阀门转角，v(k)为霍尔传感器测量噪声。
8.根据权利要求5所述的基于卡尔曼滤波器的轨道阀阀位测量方法，其特征在于，
所述确定出下一个陀螺仪阀门转角预估值的方式具体为：
θ(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)为霍尔传感器测量噪声。
9.根据权利要求8所述的基于卡尔曼滤波器的轨道阀阀位测量方法，其特征在于，所述陀螺仪常值偏差b(k)通过下述公式，在采用霍尔传感器的阀门转角测量值z(k)＝θ(k+1)时，可以估计得到的陀螺仪常值偏差b(k)：
θ(k+1)＝θ(k)+(ω(k)-b(k)+w(k))dt
z(k)＝θ(k)+v(k)。
10.根据权利要求5或8所述的基于卡尔曼滤波器的轨道阀阀位测量方法，其特征在于，所述卡尔曼滤波器为：
其中，***状态矩阵为：
***量测向量为：H＝[1 0]
状态向量矩阵为：
k表示第k次测量，k-1表示第k-1次测量，
U(k-1)为陀螺仪前一次的角速度，X(k-1)为陀螺仪的前一次阀门转角Z(k)为霍尔传感器测得的转角，W(k)陀螺仪测量噪声，V(k)霍尔传感器测量噪声，T则为***采样周期。
CN201910798322.8A 2019-08-27 2019-08-27 基于卡尔曼滤波器的轨道阀阀位测量***及方法 Active CN110566716B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201910798322.8A CN110566716B (zh) 2019-08-27 2019-08-27 基于卡尔曼滤波器的轨道阀阀位测量***及方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201910798322.8A CN110566716B (zh) 2019-08-27 2019-08-27 基于卡尔曼滤波器的轨道阀阀位测量***及方法

Publications (2)

Publication Number Publication Date
CN110566716A true CN110566716A (zh) 2019-12-13
CN110566716B CN110566716B (zh) 2020-12-11

Family

ID=68776384

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201910798322.8A Active CN110566716B (zh) 2019-08-27 2019-08-27 基于卡尔曼滤波器的轨道阀阀位测量***及方法

Country Status (1)

Country Link
CN (1) CN110566716B (zh)

Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20030028340A1 (en) * 2001-06-26 2003-02-06 Etienne Brunstein Hybrid inertial navigation method and device
CN104477052A (zh) * 2014-12-05 2015-04-01 浙江大学 一种自平衡载人电动独轮车的控制方法
CN105136145A (zh) * 2015-08-11 2015-12-09 哈尔滨工业大学 一种基于卡尔曼滤波的四旋翼无人机姿态数据融合的方法
CN105691532A (zh) * 2016-04-14 2016-06-22 四川大学 一种带有手刹***的双轮自平衡电动车
CN106006384A (zh) * 2016-08-03 2016-10-12 湖南百特随车起重机有限公司 随车起重机六轴陀螺仪自动水平检测及调平***

Patent Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20030028340A1 (en) * 2001-06-26 2003-02-06 Etienne Brunstein Hybrid inertial navigation method and device
CN104477052A (zh) * 2014-12-05 2015-04-01 浙江大学 一种自平衡载人电动独轮车的控制方法
CN105136145A (zh) * 2015-08-11 2015-12-09 哈尔滨工业大学 一种基于卡尔曼滤波的四旋翼无人机姿态数据融合的方法
CN105691532A (zh) * 2016-04-14 2016-06-22 四川大学 一种带有手刹***的双轮自平衡电动车
CN106006384A (zh) * 2016-08-03 2016-10-12 湖南百特随车起重机有限公司 随车起重机六轴陀螺仪自动水平检测及调平***

Also Published As

Publication number Publication date
CN110566716B (zh) 2020-12-11

Similar Documents

Publication Publication Date Title
CN108225308B (zh) 一种基于四元数的扩展卡尔曼滤波算法的姿态解算方法
Chang et al. Optimization-based alignment for strapdown inertial navigation system: Comparison and extension
KR101988786B1 (ko) 관성 항법 장치의 초기 정렬 방법
Fakharian et al. Adaptive Kalman filtering based navigation: An IMU/GPS integration approach
CN104596514B (zh) 加速度计和陀螺仪的实时降噪***和方法
CN101726295A (zh) 考虑加速度补偿和基于无迹卡尔曼滤波的惯性位姿跟踪方法
CN109974697A (zh) 一种基于惯性***的高精度测绘方法
CN111721288B (zh) 一种mems器件零偏修正方法、装置及存储介质
Anjum et al. Sensor data fusion using unscented kalman filter for accurate localization of mobile robots
CN113203429B (zh) 一种陀螺仪温度漂移误差的在线估计及补偿方法
CN106153069B (zh) 自主导航***中的姿态修正装置和方法
Wu et al. An inertial device biases on-line monitoring method in the applications of two rotational inertial navigation systems redundant configuration
CN111272158B (zh) 复杂磁扰动场景mems电子罗盘的动态方位角解算方法
KR20110022953A (ko) 로봇의 각속도 오차 보정 장치 및 방법
WO2017134695A1 (en) System and method for calibrating magnetic sensors in real and finite time
CN111829503A (zh) 一种光纤陀螺阈值测试方法及装置
CN115290080A (zh) 基于无偏有限脉冲响应滤波器的管道机器人定位方法
WO2015057074A1 (en) Adjusted navigation
CN111220151A (zh) 载体系下考虑温度模型的惯性和里程计组合导航方法
WO2016165336A1 (zh) 一种导航的方法和终端
CN110566716B (zh) 基于卡尔曼滤波器的轨道阀阀位测量***及方法
CN103954288B (zh) 一种卫星姿态确定***精度响应关系确定方法
Kwak et al. Improvement of the inertial sensor-based localization for mobile robots using multiple estimation windows filter
CN110736459A (zh) 惯性量匹配对准的角形变测量误差评估方法
Choi et al. Calibration of inertial measurement units using pendulum motion

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
GR01 Patent grant
GR01 Patent grant