CN117128956B - 基于角速度转换的动态倾角获取方法及应用该方法的设备 - Google Patents
基于角速度转换的动态倾角获取方法及应用该方法的设备 Download PDFInfo
- Publication number
- CN117128956B CN117128956B CN202311104900.6A CN202311104900A CN117128956B CN 117128956 B CN117128956 B CN 117128956B CN 202311104900 A CN202311104900 A CN 202311104900A CN 117128956 B CN117128956 B CN 117128956B
- Authority
- CN
- China
- Prior art keywords
- angular velocity
- angle
- time
- quaternion
- gyroscope
- 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.)
- Active
Links
- 238000000034 method Methods 0.000 title claims abstract description 51
- 238000006243 chemical reaction Methods 0.000 title claims abstract description 32
- 238000005259 measurement Methods 0.000 claims abstract description 33
- 238000004364 calculation method Methods 0.000 claims abstract description 13
- 238000001914 filtration Methods 0.000 claims abstract description 12
- 239000011159 matrix material Substances 0.000 claims description 25
- 230000008569 process Effects 0.000 claims description 18
- 230000010354 integration Effects 0.000 claims description 15
- 230000001133 acceleration Effects 0.000 claims description 6
- 229920001983 poloxamer Polymers 0.000 claims description 6
- 238000005070 sampling Methods 0.000 claims description 5
- 238000000354 decomposition reaction Methods 0.000 claims description 4
- 230000007704 transition Effects 0.000 claims description 4
- 239000000654 additive Substances 0.000 claims description 2
- 230000000996 additive effect Effects 0.000 claims description 2
- 238000013016 damping Methods 0.000 claims description 2
- 238000012545 processing Methods 0.000 claims description 2
- 238000003672 processing method Methods 0.000 claims description 2
- 238000005457 optimization Methods 0.000 claims 2
- 230000004927 fusion Effects 0.000 description 8
- 238000012937 correction Methods 0.000 description 4
- 238000004519 manufacturing process Methods 0.000 description 3
- 238000000691 measurement method Methods 0.000 description 3
- 230000003068 static effect Effects 0.000 description 3
- 238000010586 diagram Methods 0.000 description 2
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000008859 change Effects 0.000 description 1
- 230000000295 complement effect Effects 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 238000006073 displacement reaction Methods 0.000 description 1
- 230000009977 dual effect Effects 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 230000036541 health Effects 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 238000007500 overflow downdraw method Methods 0.000 description 1
- 238000012163 sequencing technique Methods 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
- 238000012360 testing method Methods 0.000 description 1
- 230000000007 visual effect Effects 0.000 description 1
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
- G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
- G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
- G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/20—Instruments for performing navigational calculations
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C23/00—Combined instruments indicating more than one navigational value, e.g. for aircraft; Combined measuring devices for measuring two or more variables of movement, e.g. distance, speed or acceleration
-
- Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
- Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
- Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
- Y02T90/00—Enabling technologies or technologies with a potential or indirect contribution to GHG emissions mitigation
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Aviation & Aerospace Engineering (AREA)
- Gyroscopes (AREA)
Abstract
本发明公开一种基于角速度转换的动态倾角方法及应用该方法的设备，方法包括基于陀螺仪获取结构相对于参考坐标系的角速度，并输出到角速度转换器，角速度转换器进行角度解算以获取的转换角度；构建从角速度转换器获取的角度的四元数状态空间表达式，将角速度转换器输出的角度四元数作为卡尔曼滤波器的观测量；根据四元数微分方程构建从陀螺仪获取的角速度的四元数状态空间表达式，将陀螺仪输出的角速度和常值漂移误差作为卡尔曼滤波器的预测值；建立步骤2中的观测量和步骤3中的预测值组成的系统的卡尔曼滤波状态方程和测量方程，解算结构动态倾角，这样规避角速度积分漂移，也避免阶次选取不当引起的角度转换误差，具有较高的测量精度和实用性。
Description
技术领域
本发明涉及动态倾角测量，具体涉及一种基于角速度转换的动态倾角获取方法及应用该方法的设备。
背景技术
动态倾角指一个结构在运动过程中的倾斜角度，它不仅涉及到静态的结构位置，还包括物体运动造成的实时倾斜变化，是反映结构健康状态的重要参数之一。对于海洋平台、海上风机、桥梁和高塔等结构，实时的动态倾角感知可以为结构安全运行提供保障。现有结构动态倾角感知方法主要包括惯性测量法、视觉图像法。其中基于惯性测量的结构动态倾角感知方法依据牛顿运动定律可以获取结构的动态倾角，具备无需外部参照、精度高、体积小、鲁棒性好的优势，并且通过融合多传感器，可以进一步提升倾角测量精度。
动态倾角感知方法中的惯性传感器一般采用加速度传感器，角速度传感器以及它们的单、双或者三轴组合。其中，加速度计测量结构在空间中的加速度信息，进而解算结构的倾斜角度，解算过程无需积分，不存在累计误差问题。但结构运动所产生的加速度会产生很大的干扰信号叠加在上述测量加速度信号上，使得测量信号无法准确反映结构的倾角。基于陀螺仪的姿态测量方法主要依靠陀螺仪的角速度输出，通过积分运算得到角位移，从而获取姿态角，但是由于陀螺仪存在固有零偏以及在解算积分过程中的累计误差，导致其无法单独用于长时间的动态倾角测量。为了解决以上问题，目前可以借助互补滤波、卡尔曼滤波、梯度算法、模糊算法等融合算法对加速度计和陀螺仪的数据进行融合解算，其中卡尔曼算法最为常用，其可以有效融合加速度计的静态优势和陀螺仪的动态优势，较为精确的获得结构的动态倾角信息。然而，此类设备需要加速度计和陀螺仪两类惯性传感器件，不仅增加了硬件成本，而且由于制造工艺的误差，加速度计与陀螺仪的坐标系难以完全重合，由此产生的不重合误差也使动态倾角测量精度下降。
综上，为了准确的测量结构的动态倾角，目前的动态倾角设备需要加速度计和陀螺仪两类惯性传感器进行融合结算，不仅增加了硬件成本，而且由于制造工艺的误差，加速度计与陀螺仪的坐标系难以完全重合，由此产生的不重合误差也会对测量结果造成影响。
因此，如何规避陀螺仪积分解算角度过程中产生的累计误差，发明一种不需要加速度计修正的基于角速度转换的动态倾角设备是本领域亟需解决的问题。
发明内容
为解决现有技术存在的不足，本发明提供了一种基于角速度转换的动态倾角设备，仅需要陀螺仪一种惯性传感器件来测量角速度信息。首先，由陀螺仪获取角速度信息，采用普罗尼信号表征角速度时间序列，并依据频率特征识别并去除序列中的漂移项，去除了固有零偏以及积分累计误差对角度转换的影响，由此在无需角速度计的条件下就可以得到较为精确的计算角度；并且，将从角速度转换器获取的角度转化为四元数状态空间表达式，为卡尔曼滤波器提供精度较高的观测量输入；同时，依据陀螺仪获取的角速度扩展为四元数形式，采用毕卡逼近法求解四元数微分方程，并将获得的结构姿态四元数和陀螺仪常值漂移误差作为卡尔曼滤波器状态变量，可以有效修正普罗尼序列拟合过程中由于阶次选取造成的误差；在上述基础上，采用卡尔曼滤波融合算法，将直接转换角度和积分角度进行信息融合，在规避角速度积分漂移的同时，也避免了阶次选取不当引起的角度转换误差，可以实现结构实时动态倾角的测量，具有较高的测量精度和实用性。
本发明的技术方案为：
一方面，本发明提供了一种基于角速度转换的动态倾角获取方法，基于动态倾角设备，动态倾角设备包括陀螺仪、角速度转换器、卡尔曼滤波器和Can总线接口；动态倾角方法包括如下步骤：S1、通过陀螺仪获取结构相对于参考坐标系的角速度，并输出到角速度转换器，角速度转换器对角速度进行角度解算以获取转换角度；S2、构建从角速度转换器获取的角度的四元数状态空间表达式，将角速度转换器输出的角度的四元数作为卡尔曼滤波器的观测量；S3、根据四元数微分方程构建从陀螺仪获取的角速度的四元数状态空间表达式，将陀螺仪输出的角速度和常值漂移误差作为卡尔曼滤波器的预测值；S4、建立步骤S2中的观测量和步骤S3中的预测值组成的系统的卡尔曼滤波状态方程和测量方程，解算结构动态倾角。
另一方面，本发明提供了一种动态倾角设备，应用上述动态倾角方法，包括陀螺仪、角速度转换器、卡尔曼滤波器和Can总线接口。
本发明所达到的有益效果为：
1、实际应用中，由于存在随机误差、零漂、温漂等误差源，陀螺仪的测量值在静止时一般也不会是零，这将导致在积分过程后，其倾角结果会出现漂移。基于陀螺仪测量角速度存在固有误差以及在解算积分过程中的累计误差的问题，本发明采用普罗尼信号表征角速度时间序列，并依据频率特征识别序列中的漂移项，通过对角速度时间序列中的漂移项的去除，避免了固有误差以及积分累计误差对角度转换的影响，由此在无需角速度计的条件下就可以得到较为精确的计算角度。
2、陀螺仪角度解算过程中涉及到积分运算，由于陀螺仪具有常值漂移，在没有其他辅助传感器对陀螺仪数据进行修正的情况下，直接更新计算角度会产生较大的累积误差，最终造成倾角估计发散。通常，采用加速度计对陀螺仪倾角测量的漂移误差进行修正，但是这种修正方法不仅增加了硬件成本，而且由于制造工艺的误差，加速度计与陀螺仪的坐标系难以完全重合，由此引入的不重合误差也会对测量结果造成影响。本发明将从角速度转换器获取的角度转化为四元数状态空间表达式，以此作为陀螺仪漂移误差的修正参考，可以实现在不需要加速度计的情况下，为后续卡尔曼滤波器提供精度较高的观测量输入。
3、普罗尼序列的拟合对拟合成分数量的选取要求较高。本发明依据陀螺仪获取的角速度扩展为四元数形式，采用毕卡逼近法求解四元数微分方程，并将获得的结构姿态四元数和陀螺仪常值漂移误差作为卡尔曼滤波器状态变量，可以有效修正普罗尼序列拟合过程中由于拟合成分数量选取造成的误差，提高角度求解精度。
4、本发明采用卡尔曼滤波融合算法，基于步骤S2和步骤S3，将直接转换角度和积分角度进行信息融合，在规避角速度积分漂移的同时，也避免了阶次选取不当引起的角度转换误差，仅依靠陀螺仪一种惯性器件即可实现结构实时动态倾角的测量，规避了传统动态倾角设备加速度计和陀螺仪的不重合误差，具有较高的测量精度和实用性。
附图说明
图1为本发明中的动态倾角设备在一个实施例中的结构框示意图。
图2为实施例的本发明动态倾角设备测量的结构倾角与六自由度仪测量倾角的对比。
图中，110、陀螺仪；120、角速度转换器；130、卡尔曼滤波器；140、Can总线接口。
具体实施方式
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
图1为本发明中的动态倾角设备在一个实施例中的结构框示意图。如图1所示，动态倾角设备包括陀螺仪110，角速度转换器120，卡尔曼滤波器130，Can总线接口140。
陀螺仪110为三轴陀螺仪，可以测量结构相对于参考坐标系的角速度，并输出到角速度转换器120和卡尔曼滤波器130。角速度转换器120用于执行角速度转换程序，并将获取的角度输出到卡尔曼滤波器作为观测量。
卡尔曼滤波器130构建从陀螺仪110获取的角速度和从角速度转换器120获取的角度的四元数状态空间表达式，将陀螺仪110角速度和常值漂移误差作为卡尔曼滤波器的预测值，将角速度转换器120角度作为卡尔曼滤波器130的观测量，在此基础上，通过卡尔曼滤波实现多传感器信息的融合，从而实时修正陀螺仪数据中的误差以解算结构动态倾角。
Can总线接口140负责解算角度的输出。
本发明的动态倾角方法，包含以下具体步骤：
1、基于陀螺仪获取结构相对于参考坐标系的角速度，并输出到角速度转换器，角速度转换器对角速度进行角度解算以获取的转换角度；
角速度转换器的初始化：将基于角速度转换的动态倾角设备安装在待测结构上，并通过陀螺仪测量时间长度为Tc(Tc应至少大于一个结构振动周期)的三个方向的角速度的时间序列ωx(t)、ωy(t)和ωz(t)，其中0<t<Tc。
通过直接测量法或组合惯导等方法进行初始标定，对基于角速度转换的动态倾角设备进行初始化标定，并初始化卡尔曼滤波器中的状态量X(0)、观测量Z(0)、协方差矩阵P(0)。
角速度转换器的角度解算如下：
结构的角速度表示为角度对时间的导数：
式中，ωx是结构绕x轴的角速度；
ωy是结构绕y轴的角速度；
ωz是结构绕z轴的角速度；
θ是结构绕x轴的角度(横摇)；
是结构绕y轴的角度(纵摇)；
ψ是结构绕z轴的角度(艏摇)。
(1)通过陀螺仪测量k时刻三个方向的角速度的时间序列，并通过滑动平均法构建角速度时间序列ωx(t)、ωy(t)和ωz(t)，其中kT<t<Tc+kT，T为测量时间步长，可以根据工程场景进行设定。
(2)将三个方向的角速度时间序列分解为普罗尼序列：
其中：
式中，ωx(t)、ωy(t)和ωz(t)分别是x,y,z三个方向的角速度的时间序列；
Nx、Ny和Nz分别是x,y,z三个方向的角速度时间序列的分解成分数量；
o、p和q分别是第o、p和q个成分；
ox、py和qz分别代表x,y,z三个方向的第o、p和q个成分；
t是时间；
i是复数符号；
π是圆周率；
A、β、ξ和f分别代表对应成分的幅值、相位、阻尼和频率；
γ和λ是普罗尼拟合参数。
(3)使用普罗尼序列对式(2)-(4)进行拟合即可得到参数γ和λ，由此可以算出角速度各成分的频率，对求解出的频率进行排序，并去除其中的极小频率成分，即可去除漂移项对角度计算的影响。由此可得到，去值漂移项的角速度普罗尼序列：
式中，ωx1(t)、ωy1(t)和ωz1(t)分别是去除漂移的x,y,z三个方向角速度的时间序列；
t是时间；
Nx1、Ny1和Nz1分别是去除漂移项的x,y,z三个方向角速度时间序列的分解成分数量；
o1、p1和q1分别是第o1、p1和q1个成分；
ox1、py1和qz1分别代表x,y,z三个方向的第o1、p1和q1个成分；
γ和λ是普罗尼拟合参数。
(4)由角速度普罗尼序列，即式(8)～(10)对时间积分得到转换角度时间序列：
上述角度时间序列的最后一个值即为k时刻的角度，记作θc、和ψc，
式中，θc(t)是角度转换器输出的结构绕x轴的角度(横摇)时间序列；
是角度转换器输出的结构绕y轴的角度(纵摇)时间序列；
ψc(t)是角度转换器输出的结构绕z轴的角度(艏摇)时间序列。
综上，步骤1基于陀螺仪测量角速度存在固有零偏以及在解算积分过程中的累计误差的问题，采用普罗尼信号表征角速度时间序列，并依据频率特征识别序列中的漂移项，通过对角速度时间序列中的漂移项的去除，去除了固有零偏以及积分累计误差对角度转换的影响，由此在无需角速度计的条件下就可以得到较为精确的计算角度，规避了传统动态倾角设备加速度计和陀螺仪的不重合误差。
2、构建从角速度转换器获取的角度的四元数状态空间表达式，将角速度转换器输出的角度四元数作为卡尔曼滤波器的观测量；
构建从角速度转换器获取的角度的四元数Qc状态空间表达式：
式中，c为角速度转换器；
qc0、qc1、qc2和qc3是从角速度转换器获取的角度的四元数。
由此可得到卡尔曼滤波器的观测量：
Z(k)＝Qc(k) (15)
其中，Z(k)表示k时刻的观测量；
Qc(k)表示k时刻的四元数矢量。
综上，步骤2将从角速度转换器c获取的角度转化为四元数状态空间表达式，可以在不需要加速度计的前提下，为后续卡尔曼滤波器提供精度较高的观测量输入。
3、根据四元数微分方程构建从陀螺仪获取的角速度的四元数状态空间表达式，将陀螺仪输出的角速度和常值漂移误差作为卡尔曼滤波器的预测值构造卡尔曼滤波器状态矩阵：
(1)构建陀螺仪获取的角速度与姿态四元数Qb的微分关系：
其中，Qb是通过积分获得的结构角度四元数矢量；
Ω是陀螺仪测得的结构角速度矢量，式(15)用矩阵表示为：
式中，qb0、qb1、qb2和qb3是通过积分获得的角度四元数。
(2)采用毕卡逼近法求解四元数微分方程，到四元数解析表达式：
其中，t是t时刻；
T是采样时间；
Qb(t)是t时刻的通过积分获得的结构角度四元数矢量；
Qb(t+T)是t+T时刻的通过积分获得的结构角度四元数矢量。
(3)实时递推处理过程中，当采样时间T取的非常短的时，假设角速度矢量Ω方向不变，在时间T内对陀螺仪角速度矩阵的积分可近似为：
式中，Δθ是时间T内对陀螺仪角度矩阵；
Δθx是时间T内结构绕x轴的角度；
Δθy是时间T内结构绕y轴的角度；
Δθz是时间T内结构绕z轴的角度。
(4)采用一阶毕卡逼近算法对四元数微分方程进行解算可得：
式中，I是单位矩阵。
(5)选取结构姿态四元数Qb和陀螺仪常值漂移εb误差作为卡尔曼滤波器状态变量，则k时刻系统的预测值如下：
式中，Qb为通过积分获得的结构角度四元数矢量；
εb为陀螺仪常值漂移。
综上，步骤3依据陀螺仪获取的角速度扩展为四元数形式，采用毕业逼近法求解四元数微分方程，并将获得的结构姿态四元数和陀螺仪常值漂移误差作为卡尔曼滤波器状态变量，可以有效修正步骤2中普罗尼序列拟合过程中由于阶次选取造成的误差，提高角度求解精度。
4、建立步骤S2中的观测量和步骤S3中的预测值组成的系统的卡尔曼滤波状态方程和测量方程，解算结构动态倾角
由式(15)和(21)可以获得所述卡尔曼滤波器系统中的观测量Z(k)和预测值X(k)，据此构建所述卡尔曼滤波器系统，包括状态方程和量测方程如下：
式中，X(k)表示k时刻的系统预测值；
Z(k)表示k时刻含有加性噪声的观测量；
Q(k-1)为k-1时刻过程噪声；
R(k)为k时刻量测噪声；
Φ(k|k-1)为k时刻状态转移矩阵；
H(k)为k时刻的量测矩阵；
卡尔曼滤波融合：
(1)时间更新
按照扩展卡尔曼滤波的线性化处理方法，将建立的四元数非线性模型的状态方程和量测方程进行线性化，可以得到k时刻的状态转移矩阵：
式中，和/>分别是x、y和z三个方向修正后的角速度。
式中，εbx、εby和εbz分别是x、y和z三个方向陀螺仪的常值漂移；
状态方程进行状态一步预测：
X(k|k-1)＝Φ(k|k-1)X(k-1|k-1) (25)
式中，X(k|k-1)是用k-1时刻的系统预测值，由步骤3获得，X(k-1|k-1)是k-1时刻状态最优的结果；
进一步预测均方误差：
P(k|k-1)＝Φ(k|k-1)P(k-1|k-1)Φ(k|k-1)T+Q(k-1) (26)
式中，P(k|k-1)是X(k|k-1)的协方差；
P(k-1|k-1)是X(k-1|k-1)的协方差；
k-1时刻的过程噪声协方差矩阵Q(k-1)为：
式中，diag表示对角矩阵；
σ为变量的协方差；
(2)量测更新
计算k时刻滤波器增益K(k|k)：
K(k)＝P(k|k-1)H(k)T[H(k)P(k|k-1)H(k)T+R(k)]-1 (28)
其中，k时刻的量测噪声协方差矩阵R(k)为：
估计状态变量：
X(k|k)＝X(k|k-1)+K(k){Z(k)-H(k)X(k|k-1)} (30)
其中，X(k|k)是k时刻状态最优的结果，Z(k)是用k时刻的系统观测值，由步骤2获得，k时刻的量测矩阵H(k)为：
式中，g是重力加速度；
估计均方误差：
P(k|k)＝[I-K(k)H(k)]P(k|k-1)[I-K(k)H(k)]T+K(k)R(k)K(k)T (32)
依据式(25)、(26)、(28)、(30)和(32)对卡尔曼滤波器系统(22)进行迭代计算，即可得到结构的实时动态倾角数据。
综上，步骤4采用卡尔曼滤波融合算法，将直接转换角度和积分角度进行信息融合，在规避角速度积分漂移的同时，也避免了阶次选取不当引起的角度转换误差，可以实现结构实时动态倾角的测量，具有较高的测量精度和实用性。
以下用一个具体实例进行说明，选取漂浮式风机基础进行水池实验研究，实验布置如图2所示。在风机基础迎浪向立柱顶端布置六自由度仪和动态倾角仪，对不同工况下风机基础的动态倾角进行测量，并对两种设备的测试结果进行对比，设备采样频率为200Hz，工况设置如表1所示。
表1水池实验波浪主要参数
按照步骤1将基于角速度转换的动态倾角设备安装在待测结构上，并通过陀螺仪测量时间长度为Tc＝2s的三个方向的角速度的时间序列。按照步骤1对基于角速度转换的动态倾角设备进行初始化标定，并初始化卡尔曼滤波器中的状态量、观测量、协方差矩阵。按照步骤1以时间步长T＝0.005s进行角速度转换器的角度解算获取当下时刻的角度。按照步骤2构建从角速度转换器获取的角度的四元数的状态空间表达式，由此构造卡尔曼滤波器的观测量。按照步骤3计算结构姿态四元数，并将其和陀螺仪常值漂移误差一起构造卡尔曼滤波器状态变量。按照步骤4卡尔曼滤波融合方法，进行迭代计算即可得到结构姿态四元数，进一步转换即可得到结构实时动态倾角。
图2为基于角速度转换的动态倾角设备测量角度与六自由度仪测量角度的对比。由图可知，本发明提出的动态倾角测量设备能准确捕捉漂浮式风机基础的动态倾角，并且测量结果与六自由度仪相一致，由此说明基于角度转换的动态倾角仪应用效果较好，具有较高的可靠性。
综上所述，本发明提出的动态倾角测量设备通过陀螺仪测量角速度信息，并将角速度转换算法和角速度积分算法分别获得的动态倾角进行卡尔曼融合，实现了不需要加速度计修正的动态倾角实时测量，规避了传统动态倾角设备加速度计和陀螺仪的不重合误差，提高了倾角计算精度并降低了硬件成本。
以上所述的本发明实施方式，并不构成对本发明保护范围的限定。任何在本发明的精神和原则之内所作的修改、等同替换和改进等，均应包含在本发明的权利要求保护范围之内。
Claims (4)
1.一种基于角速度转换的动态倾角获取方法，其特征在于，基于动态倾角设备，动态倾角设备包括陀螺仪、角速度转换器、卡尔曼滤波器和Can总线接口；
包括如下步骤：
S1、通过陀螺仪获取结构相对于参考坐标系的角速度，并输出到角速度转换器，角速度转换器对角速度进行角度解算以获取转换角度；
S2、构建从角速度转换器获取的角度的四元数状态空间表达式，将角速度转换器输出的角度的四元数作为卡尔曼滤波器的观测量；
S3、根据四元数微分方程构建从陀螺仪获取的角速度的四元数状态空间表达式，将陀螺仪输出的角速度和常值漂移误差作为卡尔曼滤波器的预测值；
S4、建立步骤S2中的观测量和步骤S3中的预测值组成的系统的卡尔曼滤波状态方程和测量方程，解算结构动态倾角；
步骤S1的角速度转换器的角度解算步骤如下：
结构的角速度表示为角度对时间的导数：
式中，ωx是结构绕x轴的角速度；
ωy是结构绕y轴的角速度；
ωz是结构绕z轴的角速度；
θ是结构绕x轴的角度；
是结构绕y轴的角度；
ψ是结构绕z轴的角度；
将三个方向的角速度时间序列通过普罗尼序列表征为：
其中：
式中，ωx(t)、ωy(t)和ωz(t)分别是x,y,z三个方向的角速度的时间序列；
Nx、Ny和Nz分别是x,y,z三个方向的角速度时间序列的分解成分数量；
o、p和q分别是第o、p和q个成分；
ox、py和qz分别代表x,y,z三个方向的第o、p和q个成分；
t是时间；
i是复数符号；
π是圆周率；
A、β、ξ和f分别代表对应成分的幅值、相位、阻尼和频率；
γ和λ是普罗尼拟合参数；
对式(2)-(4)进行拟合得到参数γ和λ，由此算出角速度各成分的频率；
对求解出的频率进行排序，由于漂移项频率远小于结构的倾角频率，因此去除其中的极小频率成分，以去除漂移项对角度计算的影响，从而得到去除漂移项的角速度普罗尼序列：
式中，ωx1(t)、ωy1(t)和ωz1(t)分别是去除漂移的x,y,z三个方向角速度的时间序列；
Nx1、Ny1和Nz1分别是去除漂移项的x,y,z三个方向角速度时间序列的分解成分数量；
o1、p1和q1分别是第o1、p1和q1个成分；
ox1、py1和qz1分别代表x,y,z三个方向的第o1、p1和q1个成分；
γ和λ是普罗尼拟合参数；
t是时间；
由此，由角速度普罗尼序列对时间积分得到转换角度时间序列如下：
式中，θc(t)是角度转换器输出的结构绕x轴的角度时间序列；
是角度转换器输出的结构绕y轴的角度时间序列；
ψc(t)是角度转换器输出的结构绕z轴的角度时间序列；
步骤S2还包括：
所述构建从角速度转换器获取的角度的四元数矢量Qc可表示为：
式中，c为角速度转换器；
qc0、qc1、qc2和qc3是从角速度转换器c获取的角度的四元数；
由此得到卡尔曼滤波器的观测量：
Z(k)＝Qc(k) (15)
式中，Z(k)表示k时刻的观测量；
Qc(k)表示k时刻的四元数矢量；
步骤S3还包括：
所述陀螺仪获取的角速度与姿态四元数的微分关系为：
式中，Qb是通过积分获得的结构角度四元数矢量；Ω是陀螺仪测得的结构角速度矢量，式(16)用矩阵表示为：
式中，qb0、qb1、qb2和qb3是通过积分获得的结构角度四元数；
采用毕卡逼近法求解四元数微分方程，得到四元数解析表达式：
式中，t是t时刻；
T是采样时间；
Qb(t)是t时刻的通过积分获得的结构角度四元数矢量；
Qb(t+T)是t+T时刻的通过积分获得的结构角度四元数矢量；
在实时递推处理过程中，当采样时间T取的非常短的时，假设角速度矢量Ω方向不变，在时间T内对陀螺仪角速度矩阵的积分近似为：
式中，Δθ是时间T内陀螺仪角度矩阵；
Δθx是时间T内结构绕x轴的角度；
Δθy是时间T内结构绕y轴的角度；
Δθz是时间T内结构绕z轴的角度；
采用一阶毕卡逼近算法对四元数微分方程进行解算可得：
式中，I是单位矩阵；
选取通过积分获得的结构角度四元数矢量Qb和陀螺仪常值漂移εb误差作为卡尔曼滤波器状态变量，则k时刻系统的预测值X(k)如下：
式中，Qb为通过积分获得的结构角度四元数矢量；
εb为陀螺仪常值漂移。
2.根据权利要求1所述的动态倾角获取方法，其特征在于，步骤S4还包括：
由式(15)和式(21)获得所述卡尔曼滤波器系统中的观测量Z(k)和预测值X(k)，据此构建卡尔曼滤波器系统，包括状态方程和量测方程如下：
式中，X(k)表示k时刻的系统预测值；
Z(k)表示k时刻含有加性噪声的观测量；
Q(k-1)为k-1时刻过程噪声；
R(k)为k时刻量测噪声；
Φ(k|k-1)为k时刻状态转移矩阵；
H(k)为k时刻的量测矩阵；
按照扩展卡尔曼滤波的线性化处理方法，将建立的四元数非线性模型的状态方程和量测方程进行线性化，得到k时刻的状态转移矩阵：
式中，和/>分别是x、y和z三个方向修正后的角速度；
式中，εbx、εby和εbz分别是x、y和z三个方向陀螺仪的常值漂移；
k时刻的量测矩阵H(k)为：
式中，g是重力加速度；
k-1时刻的过程噪声协方差矩阵Q(k-1)为：
式中，diag表示对角矩阵；
σ为变量的协方差；
k时刻的量测噪声协方差矩阵R(k)为：
对卡尔曼滤波器系统式(22)按照以下时间更新和量测更新进行迭代计算：
(1)时间更新
状态方程进行状态一步预测：
X(k|k-1)＝Φ(k|k-1)X(k-1|k-1) (28)
式中，X(k|k-1)是用k-1时刻的系统预测值，由步骤S3获得，X(k-1|k-1)是k-1时刻状态最优的结果；
预测均方误差：
P(k|k-1)＝Φ(k|k-1)P(k-1|k-1)Φ(k|k-1)T+Q(k-1) (29)
式中，P(k|k-1)是用X(k|k-1)的协方差，P(k-1|k-1)是用X(k-1|k-1)的协方差；
(2)量测更新
计算k时刻滤波器增益K(k)：
K(k)＝P(k|k-1)H(k)T[H(k)P(k|k-1)H(k)T+R(k)]-1 (30)
估计状态变量：
X(k|k)＝X(k|k-1)+K(k){Z(k)-H(k)X(k|k-1)} (31)
式中，X(k|k)是k时刻状态最优的结果，Z(k)是用k时刻的系统观测值，由步骤S2获得；
估计均方误差：
P(k|k)＝[I-K(k)H(k)]P(k|k-1)[I-K(k)H(k)]T+K(k)R(k)K(k)T (32)
依据式(28)-(32)对卡尔曼滤波器系统式(22)进行迭代计算，即得到结构的实时动态倾角数据。
3.一种动态倾角设备，其特征在于：包括陀螺仪、角速度转换器、卡尔曼滤波器和Can总线接口，使用如权利要求1至2任意一项所述的动态倾角获取方法。
4.根据权利要求3所述的一种动态倾角设备，其特征在于：所述陀螺仪为三轴陀螺仪。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202311104900.6A CN117128956B (zh) | 2023-08-30 | 2023-08-30 | 基于角速度转换的动态倾角获取方法及应用该方法的设备 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202311104900.6A CN117128956B (zh) | 2023-08-30 | 2023-08-30 | 基于角速度转换的动态倾角获取方法及应用该方法的设备 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN117128956A CN117128956A (zh) | 2023-11-28 |
| CN117128956B true CN117128956B (zh) | 2024-03-26 |
Family
ID=88852416
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202311104900.6A Active CN117128956B (zh) | 2023-08-30 | 2023-08-30 | 基于角速度转换的动态倾角获取方法及应用该方法的设备 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN117128956B (zh) |
Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN107478223A (zh) * | 2016-06-08 | 2017-12-15 | 南京理工大学 | 一种基于四元数和卡尔曼滤波的人体姿态解算方法 |
| CN110146077A (zh) * | 2019-06-21 | 2019-08-20 | 台州知通科技有限公司 | 移动机器人姿态角解算方法 |
| CN111341070A (zh) * | 2020-03-02 | 2020-06-26 | 青岛联合创智科技有限公司 | 一种基于陀螺仪和水深传感器的溺水检测方法及标签 |
| CN111426318A (zh) * | 2020-04-22 | 2020-07-17 | 中北大学 | 基于四元数-扩展卡尔曼滤波的低成本ahrs航向角补偿方法 |
| CN112667952A (zh) * | 2020-10-14 | 2021-04-16 | 中国电建集团华东勘测设计研究院有限公司 | 一种结构动态位移非积分重构方法 |
Family Cites Families (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| KR20090123901A (ko) * | 2007-03-23 | 2009-12-02 | 가부시끼가이샤 도시바 | 손 떨림 보정 장치, 촬상 장치, 손 떨림 보정 프로그램을 기록한 컴퓨터로 판독 가능한 기록 매체, 촬상 프로그램을 기록한 컴퓨터로 판독 가능한 기록 매체, 손 떨림 보정 방법, 촬상 방법 |
-
2023
- 2023-08-30 CN CN202311104900.6A patent/CN117128956B/zh active Active
Patent Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN107478223A (zh) * | 2016-06-08 | 2017-12-15 | 南京理工大学 | 一种基于四元数和卡尔曼滤波的人体姿态解算方法 |
| CN110146077A (zh) * | 2019-06-21 | 2019-08-20 | 台州知通科技有限公司 | 移动机器人姿态角解算方法 |
| CN111341070A (zh) * | 2020-03-02 | 2020-06-26 | 青岛联合创智科技有限公司 | 一种基于陀螺仪和水深传感器的溺水检测方法及标签 |
| CN111426318A (zh) * | 2020-04-22 | 2020-07-17 | 中北大学 | 基于四元数-扩展卡尔曼滤波的低成本ahrs航向角补偿方法 |
| CN112667952A (zh) * | 2020-10-14 | 2021-04-16 | 中国电建集团华东勘测设计研究院有限公司 | 一种结构动态位移非积分重构方法 |
Non-Patent Citations (2)
| Title |
|---|
| A dynamic response analysis method with high-order accuracy for fixed offshore structures based on a normalised expression of external loadings;Shujian Gao;Ocean Engineering;正文第1-10页 * |
| 一种基于极值-留数的高背景噪声测试信号降噪方法研究;李颖;振动与冲击;第38卷(第11期);正文第159-165页 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN117128956A (zh) | 2023-11-28 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN107655493B (zh) | 一种光纤陀螺sins六位置系统级标定方法 | |
| JP4876204B2 (ja) | 小型姿勢センサ | |
| CN111551174A (zh) | 基于多传感器惯性导航系统的高动态车辆姿态计算方法及系统 | |
| CN111238535B (zh) | 一种基于因子图的imu误差在线标定方法 | |
| CN111721288B (zh) | 一种mems器件零偏修正方法、装置及存储介质 | |
| CN109540135B (zh) | 水田拖拉机位姿检测和偏航角提取的方法及装置 | |
| WO2022160391A1 (zh) | 磁力计信息辅助的mems陀螺仪标定方法及标定系统 | |
| CN114216456B (zh) | 一种基于imu与机器人本体参数融合的姿态测量方法 | |
| CN106370178B (zh) | 移动终端设备的姿态测量方法及装置 | |
| CN107727114B (zh) | 基于陀螺仪的加速度标定方法及系统、服务终端、存储器 | |
| CN109507706B (zh) | 一种gps信号丢失的预测定位方法 | |
| CN110702110A (zh) | 一种基于无迹卡尔曼滤波的舰船升沉运动测量方法 | |
| CN116147624B (zh) | 一种基于低成本mems航姿参考系统的船舶运动姿态解算方法 | |
| CN107860382B (zh) | 一种在地磁异常情况下应用ahrs测量姿态的方法 | |
| CN113218391A (zh) | 一种基于ewt算法的姿态解算方法 | |
| CN111649747A (zh) | 一种基于imu的自适应ekf姿态测量改进方法 | |
| CN110319833B (zh) | 一种无误差的光纤陀螺捷联惯导系统速度更新方法 | |
| CN113375693B (zh) | 一种地磁航向误差修正方法 | |
| CN111238532B (zh) | 一种适用于晃动基座环境的惯性测量单元标定方法 | |
| CN117128956B (zh) | 基于角速度转换的动态倾角获取方法及应用该方法的设备 | |
| CN116641697A (zh) | 一种动态随钻测量姿态的方法及系统 | |
| CN113959464B (zh) | 一种陀螺仪辅助的加速度计现场校准方法和系统 | |
| CN113701755B (zh) | 一种无高精度陀螺的光学遥感卫星姿态确定方法 | |
| Ben et al. | Real-time heave motion measurement by adaptive band-pass filter based on strapdown INS | |
| CN114459478A (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 | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |