CN116595487A - 一种基于动静态切换自适应imu姿态融合方法及*** - Google Patents
一种基于动静态切换自适应imu姿态融合方法及*** Download PDFInfo
- Publication number
- CN116595487A CN116595487A CN202310655412.8A CN202310655412A CN116595487A CN 116595487 A CN116595487 A CN 116595487A CN 202310655412 A CN202310655412 A CN 202310655412A CN 116595487 A CN116595487 A CN 116595487A
- Authority
- CN
- China
- Prior art keywords
- gesture
- algorithm
- value
- static
- dynamic
- 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
Links
- 230000003068 static effect Effects 0.000 title claims abstract description 85
- 238000007500 overflow downdraw method Methods 0.000 title claims abstract description 24
- 230000003044 adaptive effect Effects 0.000 title claims description 5
- 238000004422 calculation algorithm Methods 0.000 claims abstract description 123
- 238000001914 filtration Methods 0.000 claims abstract description 94
- 230000004927 fusion Effects 0.000 claims abstract description 30
- 238000012545 processing Methods 0.000 claims abstract description 30
- 238000000034 method Methods 0.000 claims abstract description 24
- 230000000295 complement effect Effects 0.000 claims abstract description 13
- 238000004590 computer program Methods 0.000 claims description 6
- 230000004044 response Effects 0.000 abstract description 18
- 238000004364 calculation method Methods 0.000 abstract description 10
- 230000001133 acceleration Effects 0.000 description 16
- 239000013598 vector Substances 0.000 description 12
- 239000011159 matrix material Substances 0.000 description 9
- 230000006870 function Effects 0.000 description 8
- 239000004973 liquid crystal related substance Substances 0.000 description 6
- 238000012360 testing method Methods 0.000 description 5
- 238000010586 diagram Methods 0.000 description 4
- 238000011478 gradient descent method Methods 0.000 description 4
- 238000005259 measurement Methods 0.000 description 4
- 230000000704 physical effect Effects 0.000 description 4
- 230000008569 process Effects 0.000 description 4
- 238000012986 modification Methods 0.000 description 3
- 230000004048 modification Effects 0.000 description 3
- NLHHRLWOUZZQLW-UHFFFAOYSA-N Acrylonitrile Chemical compound C=CC#N NLHHRLWOUZZQLW-UHFFFAOYSA-N 0.000 description 2
- 238000006243 chemical reaction Methods 0.000 description 2
- 238000005457 optimization Methods 0.000 description 2
- 238000005070 sampling Methods 0.000 description 2
- 230000009466 transformation Effects 0.000 description 2
- 230000002159 abnormal effect Effects 0.000 description 1
- 238000013459 approach Methods 0.000 description 1
- 238000013475 authorization Methods 0.000 description 1
- FFBHFFJDDLITSX-UHFFFAOYSA-N benzyl N-[2-hydroxy-4-(3-oxomorpholin-4-yl)phenyl]carbamate Chemical compound OC1=C(NC(=O)OCC2=CC=CC=C2)C=CC(=C1)N1CCOCC1=O FFBHFFJDDLITSX-UHFFFAOYSA-N 0.000 description 1
- 238000012937 correction Methods 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 238000013461 design Methods 0.000 description 1
- 238000001514 detection method Methods 0.000 description 1
- 230000005484 gravity Effects 0.000 description 1
- 238000010606 normalization Methods 0.000 description 1
- 238000000053 physical method Methods 0.000 description 1
- -1 quaternion Substances 0.000 description 1
- 238000011160 research Methods 0.000 description 1
- 230000000284 resting effect Effects 0.000 description 1
Classifications
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06F—ELECTRIC DIGITAL DATA PROCESSING
- G06F18/00—Pattern recognition
- G06F18/20—Analysing
- G06F18/25—Fusion techniques
-
- 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
-
- 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/18—Stabilised platforms, e.g. by gyroscope
-
- 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/183—Compensation of inertial measurements, e.g. for temperature effects
- G01C21/188—Compensation of inertial measurements, e.g. for temperature effects for accumulated errors, e.g. by coupling inertial systems with absolute positioning systems
-
- 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
- G06—COMPUTING; CALCULATING OR COUNTING
- G06F—ELECTRIC DIGITAL DATA PROCESSING
- G06F18/00—Pattern recognition
- G06F18/10—Pre-processing; Data cleansing
-
- 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
- Y02D—CLIMATE CHANGE MITIGATION TECHNOLOGIES IN INFORMATION AND COMMUNICATION TECHNOLOGIES [ICT], I.E. INFORMATION AND COMMUNICATION TECHNOLOGIES AIMING AT THE REDUCTION OF THEIR OWN ENERGY USE
- Y02D30/00—Reducing energy consumption in communication networks
- Y02D30/70—Reducing energy consumption in communication networks in wireless communication networks
Landscapes
- Engineering & Computer Science (AREA)
- Remote Sensing (AREA)
- Radar, Positioning & Navigation (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Theoretical Computer Science (AREA)
- Data Mining & Analysis (AREA)
- Bioinformatics & Cheminformatics (AREA)
- Evolutionary Computation (AREA)
- General Engineering & Computer Science (AREA)
- Evolutionary Biology (AREA)
- Computer Vision & Pattern Recognition (AREA)
- Bioinformatics & Computational Biology (AREA)
- Artificial Intelligence (AREA)
- Life Sciences & Earth Sciences (AREA)
- Navigation (AREA)
Abstract
本发明为基于动静态切换自适应IMU姿态融合方法及***，所述的方法包括步骤：S1，姿态融合算法滤波参数初始化，分别设置姿态算法A以及姿态算法B的初始滤波参数；S2，采集并处理IMU数据；S3，根据IMU数据估算运动的剧烈程度M‑magic值；S4，判断IMU数据对应静态M‑magic值或动态M‑magic值，若IMU数据对应静态M‑magic值，使用姿态算法A处理，若IMU数据对应动态M‑magic值，使用姿态算法B处理；S5，输出IMU姿势四元数。本发明在各算法优缺点的基础上，进行优势互补的方式下，将***高动态响应、与静态稳定性分而治之；***静态范围采用姿态算法A，高动态响应采用姿态算法B；从而提高整体***的动态与静态时，稳定性与动态响应达到最优，提高姿态解算的效率和精度。
Description
技术领域
本申请涉及IMU姿态融合领域，具体涉及一种基于动静态切换自适应IMU姿态融合方法及***。
背景技术
惯性导航***基于惯性传感器与姿态解算方法测量物体的姿态信息，以进行相应控制。在国内外学者的研究推动下，姿态融合算法体系已较为成熟。其中广泛采用的传统卡尔曼滤波、扩展卡尔曼滤波、互补滤波融合、梯度下降融合等姿态融合算法。
但其单一算法中作用范围有限，鲁棒性较差，物体动态测量性能受到了很大的限制。卡尔曼滤波具有较好的预测精度，但涉及协方差矩阵的计算、复杂度高、在预测过程中难以建立可靠稳定的状态方程；互补滤波融合算法利用陀螺仪的高频特性与加速度、磁力计的低频特性优势互补的方式，简单高效，其作用范围有限，但易受噪声与加速度、磁场的干扰；梯度下降融合算法基于加速度、磁力计姿态修正角速度测量姿态，测量精度较高，计算复杂度较低，但提高***高动态响应后，静态稳定性较差，同时过大的步长导致发散，过小的步长导致收敛缓慢。
因此，亟需一种将高动态响应、与静态稳定性分而治之的姿态融合方法及***。
发明内容
该发明的目的是克服了上述现有技术中的缺点，提供一种将高动态响应、与静态稳定性分而治之，提高整体***的动态与静态时稳定性与动态响应的基于动静态切换自适应IMU姿态融合方法及***。
为了实现上述目的，该发明具有如下构成：
本申请包括一种基于动静态切换自适应IMU姿态融合方法，包括步骤：
S1，姿态融合算法滤波参数初始化，分别设置姿态算法A以及姿态算法B的初始滤波参数；
S2，采集并处理IMU数据；
S3，根据所述的IMU数据估算运动的剧烈程度M-magic值；
S4，将所述的M-magic值分为静态M-magic值以及动态M-magic值，判断IMU数据对应静态M-magic值或动态M-magic值，若IMU数据对应静态M-magic值，使用姿态算法A处理，若IMU数据对应动态M-magic值，使用姿态算法B处理；
S5，输出IMU姿势四元数。
在优选的姿态融合方法中，所述的步骤S2包括步骤：
S2-1，分别采集IMU 9轴轴向值并进行校准；
S2-2，将校准后的数据通过统一的坐标轴转换处理。
在优选的姿态融合方法中，所述的步骤S4包括步骤：
S4-1，将所述的M-magic值分为静态M-magic值以及动态M-magic值，判断IMU数据对应静态M-magic值或动态M-magic值，若IMU数据对应静态M-magic值，执行步骤S4-1-1，若IMU数据对应动态M-magic值，执行步骤S4-1-1；
S4-1-1，根据所述静态M-magic值和姿态算法A的初始滤波参数，估算新的滤波参数A；根据所述滤波参数A，将校准后的数据IMU数据通过姿态算法A处理；
S4-1-2，根据所述动态M-magic值和姿态算法B的初始滤波参数，估算新的滤波参数B；根据所述滤波参数B，将校准后的数据IMU数据通过姿态算法B处理。
在优选的姿态融合方法中，所述的M-magic值使用浮点数表示，范围为0.0～1.0之间。
在优选的姿态融合方法中，所述的步骤S4包括步骤：
S4-1，反复执行步骤S3，估算出N值，所述的N值为静态M-magic值以及动态M-magic值的分界值；
S4-2，将所述的M-magic值分为静态M-magic值以及动态M-magic值，所述的静态M-magic值范围为[0.0,N)，所述的动态M-magic值范围为[N,1.0]，判断IMU数据对应静态M-magic值或动态M-magic值，若IMU数据对应静态M-magic值，执行步骤S4-1-1，若IMU数据对应动态M-magic值，执行步骤S4-1-1；
S4-1-1，根据所述静态M-magic值和姿态算法A的初始滤波参数，估算新的滤波参数A；根据所述滤波参数A，将校准后的数据IMU数据通过姿态算法A处理；
S4-1-2，根据所述动态M-magic值和姿态算法B的初始滤波参数，估算新的滤波参数B；根据所述滤波参数B，将校准后的数据IMU数据通过姿态算法B处理。
在优选的姿态融合方法中，所述的姿态算法A为互补滤波算法，所述的姿态算法B为梯度下降算法。
在优选的姿态融合方法中，所述的步骤S1之前还包括步骤
S0，对IMU中传感器的9轴进行校准；
在优选的姿态融合方法中，所述的步骤S5之后还包括回到步骤S2重复执行。
本申请还包括一种基于动静态切换自适应IMU姿态融合***，包括9轴IMU传感器、存储器、处理器以及存储在所述存储器上并可在所述处理器上运行的计算机程序，所述的传感器用于采集IMU数据，所述处理器执行所述计算机程序时实现权利要求1至8任一项所述的方法的步骤。
本申请的基于动静态切换自适应IMU姿态融合方法，包括步骤：S1，姿态融合算法滤波参数初始化，分别设置姿态算法A以及姿态算法B的初始滤波参数；S2，采集并处理IMU数据；S3，根据所述的IMU数据估算运动的剧烈程度M-magic值；S4，将所述的M-magic值分为静态M-magic值以及动态M-magic值，判断IMU数据对应静态M-magic值或动态M-magic值，若IMU数据对应静态M-magic值，使用姿态算法A处理，若IMU数据对应动态M-magic值，使用姿态算法B处理；S5，输出IMU姿势四元数。本发明在各算法优缺点的基础上，进行优势互补的方式下，将***高动态响应、与静态稳定性分而治之；***静态范围采用姿态算法A，高动态响应采用姿态算法B；从而提高整体***的动态与静态时，稳定性与动态响应达到最优，提高姿态解算的效率和精度。
本申请的基于动静态切换自适应IMU姿态融合方法还包括根据运动的剧烈程度估算形的滤波参数的技术方案，具体为根据坐标轴转换后的IMU数据，估算当前IMU的当前运动的剧烈程度M-magic，这里使用浮点数表示；0.0～1.0表示IMU从静态到传感器物理最高可运动阈值范围内，该值与IMU物理特性相关，这里将物理特性参数归一到数学维度中。经过反复测试，估算出N，N范围0.0～1.0之间；将上述计算的M-magic值，选择不同的姿态融合算法；将上述计算的M-magic值，与姿态融合算法初始化滤波参数，估算当前滤波参数。根据新的滤波参数，将校准后的数据IMU数据，输入到姿态融合算法中进行融合，输出姿态四元数。使用新的滤波参数A和滤波参数B来分别对IMU数据进行姿态算法A和姿态算法B的处理。本发明将动态响应快与静态稳定性优的算法进行结合使用，来实现IMU姿态动态响应和静态特性稳定性最优化，同时消除动静态切换时产生的转向漂移。
附图说明
图1为优选的本发明第一实施例的姿态融合方法的步骤图；
图2为优选的姿态算法A的工作流程图；
图3为优选的姿态算法B的工作流程图；
图4为优选的本发明第二实施例的姿态融合方法的步骤图。
具体实施方式
下面将结合附图对本公开实施例中的技术方案进行清楚、完整地描述，显而易见地，所描述的实施例仅仅是本发明的部分实施例，而不是全部的实施例。基于本公开实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，也属于本发明保护的范围。
本发明包括一种基于动静态切换自适应IMU姿态融合方法，其中，所述的姿态为空间姿态。IMU姿态解算中的姿态实际上值得是机体坐标系与地理坐标系的旋转关系。其常用描述形式有三种：欧拉角，方向余弦矩阵，四元数，核心理论概述如下：
四元数法IMU姿态解算
坐标系定义：设定导航参考(地理)坐标系n，载体坐标系b。
四元数姿态表示：
四元数由实数和复数组合，一个实数单位和三个虚数单位组成的四维向量，可以用来表示刚体和坐标系在三维立体空间里的姿态方向。
即：
其中，qw是标量部分的四元数系数，其他大小等于坐标系转动角度的一半余弦值；qx、qy、qz表示矢量的部分；是矢量在运动坐标系上转动的角度值。
通过罗德里格旋转公式，归一化的四元数，可以计算出坐标系旋转矩阵：
旋转矩阵中：
如依次绕z，y，x分别旋转一个固定角度，使用Yaw，Pitch，Roll分别表示物体绕，x，y，z的旋转角度，记为可以利用三个四元数依次表示这三次旋转，欧拉角由四元数方法表示为：
通过一阶龙格库塔法求解四元数微分方程，求解出的四元数的更新方程为：
离散化：
如图1所示，本发明的基于动静态切换自适应IMU姿态融合方法，包括步骤
S1，姿态融合算法滤波参数初始化，分别设置姿态算法A以及姿态算法B的初始滤波参数；
S2，采集并处理IMU数据；
S3，根据所述的IMU数据估算运动的剧烈程度M-magic值；
S4，将所述的M-magic值分为静态M-magic值以及动态M-magic值，判断IMU数据对应静态M-magic值或动态M-magic值，若IMU数据对应静态M-magic值，使用姿态算法A处理，若IMU数据对应动态M-magic值，使用姿态算法B处理；
S5，输出IMU姿势四元数。
本发明的通过采集IMU数据估算运动的剧烈程度M-magic值，并将M-magic值分为静态M-magic值和动态M-magic值，将***高动态响应、与静态稳定性分而治之，分别选用适合的姿态算法，静态M-magic值对应的IMU数据选用姿态算法A，动态M-magic值对应的IMU数据选用姿态算法B。从而提高整体***的动态与静态时，稳定性与动态响应达到最优，提高姿态解算的效率和精度。
本发明还包括一种动态计算滤波参数的方法，其中所述的步骤S4包括步骤：
S4-1，将所述的M-magic值分为静态M-magic值以及动态M-magic值，判断IMU数据对应静态M-magic值或动态M-magic值，若IMU数据对应静态M-magic值，执行步骤S4-1-1，若IMU数据对应动态M-magic值，执行步骤S4-1-1；
S4-1-1，根据所述静态M-magic值和姿态算法A的初始滤波参数，估算新的滤波参数A；根据所述滤波参数A，将校准后的数据IMU数据通过姿态算法A处理；
S4-1-2，根据所述动态M-magic值和姿态算法B的初始滤波参数，估算新的滤波参数B；根据所述滤波参数B，将校准后的数据IMU数据通过姿态算法B处理。
在该技术方案中首先对初始化时，分别设置需要设置姿态算法A(静态)滤波参数与姿态算法B(动态)滤波参数，不同的参数匹配，会导致消除转向漂移收敛的时间，与IMU静态时的稳定性。
随后将采集的IMU数据根据静态M-magic值以及动态M-magic值和姿态算法A以及姿态算法B的初始滤波参数，估算新的滤波参数A和滤波参数B。使用新的滤波参数A和滤波参数B来分别对IMU数据进行姿态算法A和姿态算法B的处理。
本发明将动态响应快与静态稳定性优的算法进行结合使用，来实现IMU姿态动态响应和静态特性稳定性最优化，同时消除动静态切换时产生的转向漂移。
关于滤波参数的设定及计算，包括初始滤波参数设定以及新的滤波参数A以及滤波参数B的计算。
初始滤波参数包括姿态算法A的初始滤波参数以及姿态算法B的初始滤波参数。
所述姿态算法A的初始滤波参数设定方式为：
将***独立工作在姿态算法A，IMU静止水平放置时，调整算法滤波参数，使得IMU姿态静态稳定不漂移，记录当前的滤波参数中Kp(权重比例)和Ki(误差积分)值，该参数为静态滤波参数，记为：aKp_min，aKi_min；保持Ki(误差积分)值，加大Kp(权重比例)，使得***静态抖动不漂移，该参数为静态滤波参数，同时计算抖动Aoffset，记为：aKp_max，aKi_max；；
所述姿态算法B的初始滤波参数设定方式为：
将***独立工作在姿态算法B，快速转动IMU传感器(不超过IMU传感器物理测量范围)，调整算法滤波参数，使得IMU姿态四元数稳定流畅无异常抖动即可，记录当前的滤波参数中Kp(权重比例)和Ki(误差积分)值；该参数为动态滤波参数，记为：bkp_max，bKi_max；IMU静止水平放置时，减小滤波参数中Kp(权重比例)，使得IMU姿态静态稳定不漂移,使得抖动Boffset≈Aoffset时，该参数为静态滤波参数，记为：bKp_min，bKi_min(由于本***测试中，姿态算法B中Ki不存在，所以bKi＝0)
所述的滤波参数A和滤波参数B计算时需要M-magic值，本发明IMU当前状态估算(运动检测)与M-magic值的估算方式为：
由于IMU当前状态估算，仅仅计算载体当前运行的剧烈程度，所以本***采用更为简单的方式进行计算,即使用陀螺仪的范数：当ωnorm趋近于0时，载体也越趋近于静止状态，ωnorm越大，载体当前运行状态越剧烈；
关闭滤波参数更新情况下，大于ωnorm时，采用姿态算法B，小于ωnorm时，使用姿态算法A，IMU静止水平放置时，逐步调整ωnorm值，从0逐步增加，将***姿态从剧烈抖动状态，调整到***姿态逐渐稳定，记下当前ωnorm_max值，设置ωnorm_max为该值为M-magic最大值1.0；即:M-magic＝ωnorm/ωnorm_max。
所述滤波参数A和滤波参数B的计算方式为：
(1)滤波参数A：
(2)滤波参数B：
优选地，所述的步骤S1之前还包括步骤S0，对IMU中传感器的9轴进行校准；
对IMU中传感器9轴(加速度、陀螺仪、磁力计)校准；不经过校准的IMU器件，加速度、陀螺仪、磁力计等坐标系各不相同，加入到后续姿态融合算法中，会导致姿态混乱。
优选地，所述的步骤S2包括步骤：
S2-1，分别采集IMU 9轴轴向值并进行校准；分别采集IMU 9轴加速度(Acc)X、Y、Z轴向值，陀螺仪(Gyro)X、Y、Z轴向值，磁力计(Mag)X、Y、Z轴向值，经过步骤S0中标定参数进行校准；
S2-2，将校准后的数据通过统一的坐标轴转换处理，可以是左手坐标系或右手坐标系，具体可根据实际的物理使用场景决定。
随后执行步骤S4，根据坐标轴转换后的IMU数据，估算当前IMU的当前运动的剧烈程度M-magic，优选地，所述的M-magic值使用浮点数表示，范围为0.0～1.0之间。0.0～1.0表示IMU从静态到传感器物理最高可运动阈值范围内，该值与IMU物理特性相关，这里将物理特性参数归一到数学维度中。
优选地，所述的步骤S4包括步骤：
S4-1，反复执行步骤S3，估算出N值，所述的N值为静态M-magic值以及动态M-magic值的分界值；
S4-2，将所述的M-magic值分为静态M-magic值以及动态M-magic值，所述的静态M-magic值范围为[0.0,N)，所述的动态M-magic值范围为[N,1.0]，判断IMU数据对应静态M-magic值或动态M-magic值，若IMU数据对应静态M-magic值，执行步骤S4-1-1，若IMU数据对应动态M-magic值，执行步骤S4-1-1；
S4-1-1，根据所述静态M-magic值和姿态算法A的初始滤波参数，估算新的滤波参数A；根据所述滤波参数A，将校准后的数据IMU数据通过姿态算法A处理；
S4-1-2，根据所述动态M-magic值和姿态算法B的初始滤波参数，估算新的滤波参数B；根据所述滤波参数B，将校准后的数据IMU数据通过姿态算法B处理。
其中S4-1，经过反复测试，估算出N，N范围在0.0～1.0之间。
所述的N值的估算方式为：
(1)开启滤波参数更新情况下，设置N＝1.0；
(2)来回反复检测IMU从运动回到静止时，观察IMU的姿态滞后状态，与***静止时的稳定性。
(3)逐步调整N值从1.0逐步递减，检查(2)的状态，***姿态稳定性不变的情况下，直至IMU的姿态滞后最小，记下当前N值。
优选地，所述的姿态算法A为互补滤波算法，所述的姿态算法B为梯度下降算法。
本***测试过程中，选用互补滤波算法(姿态算法A)与梯度下降算法(姿态算法B)进行测试与实现，但这不表明仅用于该2种算法中；本***还可用于其他卡尔曼滤波、互补滤波、梯度下降等姿态融合算法中，可两两结合使用也可在单一算法中使用该***。
优选地，所述的姿态算法A(互补滤波姿态融合)基本实现方式为：
陀螺仪动态响应特性良好，而计算姿态时会产生累积误差。磁力计和加速度计测量姿态没有累积误差，但动态响应较差。因此它们在频域特性上互补，所以采用互补滤波器融合这三种传感器的数据，提高测量精度和***的动态性能。
如图2所示，姿态算法A整体设计流程，将加速度、磁力计计算出的角度与最终的角度产生的误差实现PI控制，促进误差的修正；修正后和陀螺仪结算出姿态角相加后，在进行积分得到一个新的姿态角。
对载体坐标进行转换到地理坐标系后，可得到载体坐标系下：
重力加速度的三维向量：
磁力计的三维向量：
而由于地磁计在XOY平面上的向量大小相同，设X轴对准北边；
所以by＝0,bz＝hz，即：
对地理坐标进行转换到载体坐标系后：
利用向量间的叉积求解：
加速度向量与陀螺仪向量间的误差
磁力计向量与陀螺仪向量间的误差
设定误差比例系数，积分系数：
如上所述可修正误差，将该误差向量代入到式(4)中，得到角速度微分四元数。
优选地，所述的姿态算法B(梯度下降姿态融合)基本实现方式为：
如图3所示，梯度下降法的原理为，是以陀螺仪的测得值建立一个微分方程，求解该方程，计算得到一组四元数。接着建立加速度计和磁力计的误差函数，使用梯度下降法来求解该误差函数，计算得到一组新的四元数。最后将这两个四元数的融合。
首先建立误差函数，将地理坐标***的重力加速度和磁场强度转化到载体坐标系中，然后减去载体坐标中加速度计和磁力计测得的加速度值和磁场值。
加速度误差函数：
其中，表示地理坐标系到载体坐标系的四元数转换矩阵，gn表示地理坐标系重力加速度[0 0 0 1]，表示载体坐标系加速度计测得的加速度值[0 ax ay az]；
化简后可知：
对上式求导，可得其雅可比矩阵为：
磁力计误差函数：
其中，表示地理坐标系到载体坐标系的四元数转换矩阵，bn表示地理坐标系磁场强度[0 bx 0 bz]，mb表示载体坐标系磁力计测得的磁场值[0 mx my mz]；
化简后可知：
对上式求导，可得其雅可比矩阵为：
加速度计和地磁传感器的误差函数为：
对上式求导，可得其雅可比矩阵为：
由上两式可得误差函数得梯度值为：
根据梯度下降法的迭代方程为：
其中，为梯度下降法所求的梯度四元数，/>为前一次四元数，u为梯度法下降的步长，/>为梯度归一化，表示梯度的方向。
对这梯度四元数与陀螺仪四元数进行融合，以得到比较准确的四元数，从而得到姿态角。
基于梯度下的姿态融合方程为：
如图4所示，为本发明优选的实施例，所述的姿态融合方法的步骤包括：
1.对IMU中加速度、陀螺仪、磁力计进行校准；
2.姿态融合算法滤波参数初始化，对IMU姿态算法中2组滤波参数初始化，分别需要设置姿态算法A(静态)滤波参数与姿态算法B(动态)滤波参数。
3.分别采集IMU 9轴加速度(Acc)X、Y、Z轴向值，陀螺仪(Gyro)X、Y、Z轴向值，磁力计(Mag)X、Y、Z轴向值，经过本流程中1中标定参数进行校准；
4.将校准后的数据通过统一的坐标轴转换处理，可以是左手坐标系或右手坐标系。
5.根据坐标轴转换后的IMU数据，估算当前IMU的当前运动的剧烈程度M-magic，这里使用浮点数表示；范围为0.0～1.0，0.0～1.0表示IMU从静态到传感器物理最高可运动阈值范围内，该值与IMU物理特性相关，这里将物理特性参数归一到数学维度中。此外，经过反复测试，估算出N，N范围0.0～1.0之间；
6.将上述计算的M-magic值，选择不同的姿态融合算法；
若M-magic值＝0并且M-magic值<N，根据M-magic和姿态算法A初始参数，估算新的滤波参数A，使用该滤波参数A对IMU数据通过姿态算法A进行处理；
若M-magic值>＝N并且M-magic值<＝1，根据M-magic和姿态算法B初始参数，估算新的滤波参数B，使用该滤波参数B对IMU数据通过姿态算法B进行处理；
7.根据新的滤波参数，将校准后的数据IMU数据，输入到姿态融合算法中进行融合，输出姿态四元数。
8.根据***设定的采样率，循环处理。回到本流程中的3采样步骤。
本申请还包括一种基于动静态切换自适应IMU姿态融合***，包括9轴IMU传感器、存储器、处理器以及存储在所述存储器上并可在所述处理器上运行的计算机程序，所述的传感器用于采集IMU数据，所述处理器执行所述计算机程序时实现本发明中姿态融合方法中的技术方案。
如本申请和权利要求书中所示，一般说来，术语“包括”与“包含”仅提示包括已明确标识的步骤和元素，而这些步骤和元素不构成一个排它性的罗列，方法或者设备也可能包含其他的步骤或元素。
除非另外具体说明，否则在这些实施例中阐述的部件、相对布置、功能、数值并不限制本发明的范围。同时，显而易见的，为了便于叙述，附图中所示的各个部分的尺寸并不是按照实际的比例关系绘制的。对于相关领域普通技术人员已知的技术、方法和设备暂不做详细描述，但在适当情况下，所述技术、方法和设备应当被视为授权说明书的一部分。在这里示出的和讨论的所有示例中，任何具体值应被解释为仅仅是实例性的，而不是作为限制。因此，步骤性实施例的其他示例可以具有不同的先后顺序。
上面是对本发明的说明，而不应被认为是对其的限制。尽管描述了本发明的若干示例性实施例，但本领域技术人员将容易地理解，在不背离本发明的技术特征的前提下可以对示例性实施例进行许多修改。因此，所有这些修改都意图包含在权利要求书所限定的本发明范围内。应当理解，上面是对本发明的说明，而不应被认为是限于所公开的特定实施例，并且对所公开的实施例以及其他实施例的修改意图包含在所附权利要求书的范围内。本发明由权利要求书及其等效物限定。
Claims (9)
1.一种基于动静态切换自适应IMU姿态融合方法，其特征在于，包括步骤：
S1，姿态融合算法滤波参数初始化，分别设置姿态算法A以及姿态算法B的初始滤波参数；
S2，采集并处理IMU数据；
S3，根据所述的IMU数据估算运动的剧烈程度M-magic值；
S4，将所述的M-magic值分为静态M-magic值以及动态M-magic值，判断IMU数据对应静态M-magic值或动态M-magic值，若IMU数据对应静态M-magic值，使用姿态算法A处理，若IMU数据对应动态M-magic值，使用姿态算法B处理；
S5，输出IMU姿势四元数。
2.根据权利要求1所述的姿态融合方法，其特征在于，所述的步骤S2包括步骤：
S2-1，分别采集IMU9轴轴向值并进行校准；
S2-2，将校准后的数据通过统一的坐标轴转换处理。
3.根据权利要求1所述的姿态融合方法，其特征在于，所述的步骤S4包括步骤：
S4-1，将所述的M-magic值分为静态M-magic值以及动态M-magic值，判断IMU数据对应静态M-magic值或动态M-magic值，若IMU数据对应静态M-magic值，执行步骤S4-1-1，若IMU数据对应动态M-magic值，执行步骤S4-1-1；
S4-1-1，根据所述静态M-magic值和姿态算法A的初始滤波参数，估算新的滤波参数A；根据所述滤波参数A，将校准后的数据IMU数据通过姿态算法A处理；
S4-1-2，根据所述动态M-magic值和姿态算法B的初始滤波参数，估算新的滤波参数B；根据所述滤波参数B，将校准后的数据IMU数据通过姿态算法B处理。
4.根据权利要求3所述的姿态融合方法，其特征在于，所述的M-magic值使用浮点数表示，范围为0.0～1.0之间。
5.根据权利要求4所述的姿态融合方法，其特征在于，所述的步骤S4包括步骤：
S4-1，反复执行步骤S3，估算出N值，所述的N值为静态M-magic值以及动态M-magic值的分界值；
S4-2，将所述的M-magic值分为静态M-magic值以及动态M-magic值，所述的静态M-magic值范围为[0.0,N)，所述的动态M-magic值范围为[N,1.0]，判断IMU数据对应静态M-magic值或动态M-magic值，若IMU数据对应静态M-magic值，执行步骤S4-1-1，若IMU数据对应动态M-magic值，执行步骤S4-1-1；
S4-1-1，根据所述静态M-magic值和姿态算法A的初始滤波参数，估算新的滤波参数A；根据所述滤波参数A，将校准后的数据IMU数据通过姿态算法A处理；
S4-1-2，根据所述动态M-magic值和姿态算法B的初始滤波参数，估算新的滤波参数B；根据所述滤波参数B，将校准后的数据IMU数据通过姿态算法B处理。
6.根据权利要求1或4所述的姿态融合方法，其特征在于，所述的姿态算法A为互补滤波算法，所述的姿态算法B为梯度下降算法。
7.根据权利要求1所述的姿态融合方法，其特征在于，所述的步骤S1之前还包括步骤
S0，对IMU中传感器的9轴进行校准。
8.根据权利要求1所述的姿态融合方法，其特征在于，所述的步骤S5之后还包括回到步骤S2重复执行。
9.一种基于动静态切换自适应IMU姿态融合***，其特征在于，包括9轴IMU传感器、存储器、处理器以及存储在所述存储器上并可在所述处理器上运行的计算机程序，所述的传感器用于采集IMU数据，所述处理器执行所述计算机程序时实现权利要求1至8任一项所述的方法的步骤。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310655412.8A CN116595487A (zh) | 2023-06-05 | 2023-06-05 | 一种基于动静态切换自适应imu姿态融合方法及*** |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310655412.8A CN116595487A (zh) | 2023-06-05 | 2023-06-05 | 一种基于动静态切换自适应imu姿态融合方法及*** |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN116595487A true CN116595487A (zh) | 2023-08-15 |
Family
ID=87606223
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202310655412.8A Pending CN116595487A (zh) | 2023-06-05 | 2023-06-05 | 一种基于动静态切换自适应imu姿态融合方法及*** |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN116595487A (zh) |
-
2023
- 2023-06-05 CN CN202310655412.8A patent/CN116595487A/zh active Pending
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| US10976341B2 (en) | Multi sensor position and orientation measurement system | |
| JP5594940B2 (ja) | 実質的に不変な回転軸を検出するための方法および装置 | |
| JP4615287B2 (ja) | 方位姿勢検出装置 | |
| CN106052685B (zh) | 一种两级分离融合的姿态和航向估计方法 | |
| CN110887481B (zh) | 基于mems惯性传感器的载体动态姿态估计方法 | |
| CN108731676B (zh) | 一种基于惯性导航技术的姿态融合增强测量方法及*** | |
| CN109030867B (zh) | 使用加速度传感器和地磁传感器计算角速度的方法和设备 | |
| CN112611380B (zh) | 基于多imu融合的姿态检测方法及其姿态检测装置 | |
| JP2013064695A (ja) | 状態推定装置、オフセット更新方法およびオフセット更新プログラム | |
| CN109029435B (zh) | 提高惯性-地磁组合动态定姿精度的方法 | |
| CN106403952A (zh) | 一种动中通低成本组合姿态测量方法 | |
| CN110132271B (zh) | 一种自适应卡尔曼滤波姿态估计算法 | |
| CN112070170B (zh) | 一种动态残差阈值自适应四元数粒子滤波姿态解算数据融合方法 | |
| CN107084722B (zh) | 一种用于提高惯性-地磁组合静动态综合性能的方法 | |
| Aligia et al. | An orientation estimation strategy for low cost IMU using a nonlinear Luenberger observer | |
| CN108871319B (zh) | 一种基于地球重力场与地磁场序贯修正的姿态解算方法 | |
| CN112033405B (zh) | 一种室内环境磁异常实时修正与导航方法及装置 | |
| CN110375773B (zh) | Mems惯导***姿态初始化方法 | |
| CN107621261B (zh) | 用于惯性-地磁组合姿态解算的自适应optimal-REQUEST算法 | |
| Cui et al. | Calibration of MEMS accelerometer using kaiser filter and the ellipsoid fitting method | |
| CN114111835B (zh) | 实时磁场标定***和方法 | |
| Chen et al. | High-precision geomagnetic directional technology based on sensor error correction and adaptive hybrid filter | |
| CN114964214B (zh) | 一种航姿参考***的扩展卡尔曼滤波姿态解算方法 | |
| JP2013122384A (ja) | カルマンフィルタ、及び、状態推定装置 | |
| CN109674480B (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 |