CN112066984B - 姿态角度解算方法、装置、处理设备和存储介质 - Google Patents
姿态角度解算方法、装置、处理设备和存储介质 Download PDFInfo
- Publication number
- CN112066984B CN112066984B CN202010979510.3A CN202010979510A CN112066984B CN 112066984 B CN112066984 B CN 112066984B CN 202010979510 A CN202010979510 A CN 202010979510A CN 112066984 B CN112066984 B CN 112066984B
- Authority
- CN
- China
- Prior art keywords
- attitude
- current
- value
- time
- moment
- 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
Images
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/04—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by terrestrial means
- G01C21/08—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by terrestrial means involving use of the magnetic field of the earth
Landscapes
- Engineering & Computer Science (AREA)
- Remote Sensing (AREA)
- Radar, Positioning & Navigation (AREA)
- Automation & Control Theory (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Life Sciences & Earth Sciences (AREA)
- Environmental & Geological Engineering (AREA)
- General Life Sciences & Earth Sciences (AREA)
- Geology (AREA)
- Gyroscopes (AREA)
Abstract
本申请涉及一种姿态角度解算方法、装置、处理设备和存储介质。所述方法包括:获取陀螺仪采集的当前时刻三轴角速度、加速度传感器采集的当前时刻三轴加速度以及磁场传感器采集的当前时刻三轴磁场强度;对当前时刻三轴角速度进行积分处理,得到下一时刻姿态预测值;根据当前时刻三轴加速度和当前时刻三轴磁场强度,得到当前时刻姿态值;根据下一时刻姿态预测值的方差、当前时刻姿态值的方差和当前时刻姿态值的误差值,得到滤波融合修正系数;基于滤波融合修正系数,对下一时刻姿态预测值进行修正,得到下一时刻姿态修正值;将下一时刻姿态修正值转换成姿态角度,从而提高可姿态角度解算精度,降低了解算误差,避免因震动干扰产生的角度漂移现象。
Description
技术领域
本申请涉及姿态测量技术领域,特别是涉及一种姿态角度解算方法、装置、处理设备和存储介质。
背景技术
近年来,对姿态测量技术的研究主要包括姿态测量组合、姿态解算算法以及姿态测量误差补偿方法,其中,在选定参考坐标系的基础上,姿态解算的目的是确定动坐标系(载体坐标系)与参考坐标系的数学关联,通常采用姿态矩阵表示。为求得姿态矩阵,需要选取合理的姿态解算方法。传统的姿态解算算法有欧拉角法(又称三参数法)、方向余弦法(又称九参数法)以及四元数法(又称四参数法)。但是,在实现过程中发明人发现现有技术至少存在以下技术问题:传统姿态解算算法解算出的姿态角度精度低、误差大、容易受到震动干扰使航向角发生角度漂移。
发明内容
基于此,有必要针对上述技术问题,提供一种能够提高姿态角度解算精度的姿态角度解算方法、装置、处理设备和存储介质。
一种姿态角度解算方法,包括以下步骤:
获取陀螺仪采集的当前时刻三轴角速度、加速度传感器采集的当前时刻三轴加速度以及磁场传感器采集的当前时刻三轴磁场强度;
对当前时刻三轴角速度进行积分处理,得到下一时刻姿态预测值;
根据当前时刻三轴加速度和当前时刻三轴磁场强度,得到当前时刻姿态值;
根据下一时刻姿态预测值的方差、当前时刻姿态值的方差和当前时刻姿态值的误差值,得到滤波融合修正系数;
基于滤波融合修正系数,对下一时刻姿态预测值进行修正,得到下一时刻姿态修正值;
将下一时刻姿态修正值转换成姿态角度。
在其中一个实施例中,对当前时刻三轴角速度进行积分处理,得到下一时刻姿态预测值的步骤中,包括步骤:
在当前时刻三轴角速度上叠加陀螺仪的当前时刻零偏误差,以对当前时刻三轴角速度进行修正;
对修正后的当前时刻三轴角速度进行积分处理,得到下一时刻姿态预测值。
在其中一个实施例中,当前时刻零偏误差为初始时刻的零偏误差时,当前时刻零偏误差为陀螺仪在预设时间段内处于静止状态下的三轴角速度均值。
在其中一个实施例中,根据下一时刻姿态预测值的方差、当前时刻姿态值的方差和当前时刻姿态值的误差值,得到滤波融合修正系数的步骤中,包括步骤:
根据下一时刻姿态预测值的方差、当前时刻姿态值的方差和当前时刻姿态值的误差值,获取下一时刻零偏误差。
在其中一个实施例中,根据当前时刻三轴加速度和当前时刻三轴磁场强度,得到当前时刻姿态值的步骤中,包括步骤:
根据当前时刻三轴加速度,获取在预设空间坐标系下的X轴向角度和Y轴向角度;
当前时刻三轴磁场强度在水平面上的投影,获取在预设空间坐标系下的Z轴向角度;
将X轴向角度、Y轴向角度和Z轴向角度进行四元数转换,获取当前时刻姿态值。
在其中一个实施例中,获取陀螺仪采集的当前时刻三轴角速度、加速度传感器采集的当前时刻三轴加速度以及磁场传感器采集的当前时刻三轴磁场强度步骤:
在预设周期到来时,获取陀螺仪采集的当前时刻三轴角速度、加速度传感器采集的当前时刻三轴加速度以及磁场传感器采集的当前时刻三轴磁场强度。
在其中一个实施例中,获取陀螺仪采集的当前时刻三轴角速度、加速度传感器采集的当前时刻三轴加速度以及磁场传感器采集的当前时刻三轴磁场强度的步骤之前,还包括步骤:
执行初始化程序。
一种姿态角度解算装置,包括:
数据获取模块,用于获取陀螺仪采集的当前时刻三轴角速度、加速度传感器采集的当前时刻三轴加速度以及磁场传感器采集的当前时刻三轴磁场强度;
姿态预测模块,用于对当前时刻三轴角速度进行积分处理,得到下一时刻姿态预测值;
当前姿态获取模块,用于根据当前时刻三轴加速度和当前时刻三轴磁场强度,得到当前时刻姿态值;
修正系数获取模块,用于根据下一时刻姿态预测值的方差、当前时刻姿态值的方差和当前时刻姿态值的误差值,得到滤波融合修正系数;
修正模块,用于基于滤波融合修正系数,对下一时刻姿态预测值进行修正,得到下一时刻姿态修正值;
转换模块,用于将下一时刻姿态修正值转换成姿态角度。
一种处理设备,包括存储器和处理器,存储器存储有计算机程序,处理器执行计算机程序时实现上述方法的步骤。
一种计算机可读存储介质,其上存储有计算机程序,计算机程序被处理器执行时实现上述方法的步骤。
上述技术方案中的一个技术方案具有如下优点和有益效果:
本申请各实施例提供的姿态角度解算方法,包括以下步骤:获取陀螺仪采集的当前时刻三轴角速度、加速度传感器采集的当前时刻三轴加速度以及磁场传感器采集的当前时刻三轴磁场强度;对当前时刻三轴角速度进行积分处理,得到下一时刻姿态预测值;根据当前时刻三轴加速度和当前时刻三轴磁场强度,得到当前时刻姿态值;根据下一时刻姿态预测值的方差、当前时刻姿态值的方差和当前时刻姿态值的误差值,得到滤波融合修正系数;基于滤波融合修正系数,对下一时刻姿态预测值进行修正,得到下一时刻姿态修正值;将下一时刻姿态修正值转换成姿态角度,通过当前时刻三轴角速度预测下一时刻姿态预测值,然后利用获取到滤波融合修正系数对下一时刻姿态预测值进行修正,从而提高可姿态角度解算精度,降低了解算误差,避免因震动干扰产生的角度漂移现象。
附图说明
图1为一个实施例中姿态角度解算方法的流程示意图;
图2为一个实施例中获取下一时刻姿态预测值步骤的流程示意图;
图3为一个实施例中获取当前时刻姿态值步骤的流程示意图;
图4为一个实施例中姿态角度解算装置的结构框图;
图5为一个实施例中处理设备的内部结构图。
具体实施方式
为了使本申请的目的、技术方案及优点更加清楚明白,以下结合附图及实施例,对本申请进行进一步详细说明。应当理解,此处描述的具体实施例仅仅用以解释本申请,并不用于限定本申请。
在一个实施例中,如图1所示,提供了一种姿态角度解算方法,包括以下步骤:
步骤S110,获取陀螺仪采集的当前时刻三轴角速度、加速度传感器采集的当前时刻三轴加速度以及磁场传感器采集的当前时刻三轴磁场强度。
需要说明的是,在各次启动***时,采集数据之前,即在步骤S110之前,执行初始化程序。
具体的,执行初始化程序的步骤中,包括步骤:
执行***初始化程序;执行姿态角初始化程序。
需要说明的是,执行***初始化程序包括单片机外设的初始化、加速度传感器的初始化、陀螺仪的初始化以及磁场传感器的初始化,并配置好量程、带宽等必要的参数。其中,单片机用于执行本申请姿态角度解算方法的步骤。
执行姿态角初始化程序是给各传感器的一个初始角度,该初始角度用于后续的姿态推算。给定的初始角度不需要太准确,后续步骤会逐步修正该初始角度,但初始角度也不能偏差太大,会增加后续步骤的收敛时间。
初始角度包括在预设空间坐标系的初始X轴向角度、初始Y轴向角度以及初始Z轴向角度,初始X轴向角度和初始Y轴向角度的获取过程为:在一个示例中,获取初始角度的具体步骤为:在静止状态下,通过加速度传感器采样加速度值,该加速度值为地球重力加速度,根据该加速度值在预设空间坐标系的各轴上的分量数值,得到初始X轴向角度和初始Y轴向角度。
在另一个示例中,在运动状态下,通过加速度传感器采集的加速度值为重力加速度和运动加速度之和,通过该加速度值获取的初始X轴向角度和初始Y轴向角度会存在偏差,利用陀螺仪采集的三轴角速度对初始X轴向角度和初始Y轴向角度进行滤波,以修正初始X轴向角度和初始Y轴向角度。
初始Z轴向角度的获取过程为:获取磁场传感器的磁场强度,根据初始X轴向角度和初始Y轴向角度和磁场强度,得到该磁场强度在预设坐标系的XY平面(即水平面)的磁场分量,再根据该磁场分量的方向,得到初始Z轴向角度。
为了进一步的提高姿态解算的精确性,在一个实施例中,在预设周期到来时,获取陀螺仪采集的当前时刻三轴角速度、加速度传感器采集的当前时刻三轴加速度以及磁场传感器采集的当前时刻三轴磁场强度。其中,预设周期可依据实际需求而定,在一个示例中,预设周期通过单片机的定时中断***产生固定的时间间隔,在该间隔时间到来后,立即执行数据采样任务,并将数据存储在指定的数据寄存器中。
步骤S120,对当前时刻三轴角速度进行积分处理,得到下一时刻姿态预测值。
需要说明的是,对当前时刻三轴角速度进行积分计算,以对姿态进行下一步预测。卡尔曼滤波算法需要列出***状态的转移方程,对下一时刻姿态进行递推。在一个示例中,下一时刻姿态预测值包括姿态四元数,在另一个示例中,下一时刻姿态预测值还包括陀螺仪的零偏误差,运用运动学方程,结合陀螺仪测量的当前时刻三轴角速度,可以推算出下一时刻姿态预测值。由于陀螺仪的零偏误差只能尽量减小,无法从根本上消除,而状态转移方程实际上就是通过对陀螺仪的当前时刻三轴角速度进行积分运算获得的下一时刻姿态预测值,所以长期以来一定会有累积误差。但由于陀螺仪数据精度相对较高,而且不会受到运动加速度的影响,所以短时间内计算出来的姿态角度是相对比较精确的。而长期漂移项,则需要在后续的步骤中结合三轴加速度和三轴磁场强度获得的当前时刻姿态值进行滤波修正,降低长时间的累积误差。在对姿态进行递推的同时,还要对姿态的方差进行一步预测,估计出最新的姿态误差水平。
为了进一步提高姿态解算精度,在一个实施例中,如图2所示,对当前时刻三轴角速度进行积分处理,得到下一时刻姿态预测值的步骤中,包括步骤:
步骤S210,在当前时刻三轴角速度上叠加陀螺仪的当前时刻零偏误差,以对当前时刻三轴角速度进行修正;
步骤S220,对修正后的当前时刻三轴角速度进行积分处理,得到下一时刻姿态预测值。
需要说明的是,本申请中传感器的修正是在原始数据(即当前时刻三轴角速度、当前时刻三轴加速度和当前时刻三轴磁场强度)的基础上,叠加传感器的零偏误差,通过修正后获得更加精准的传感器原始数据。任何传感器在使用时都会存在零偏误差,该零偏误差可通过校准来获取,并在后续步骤中不断地迭代更新该零偏误差值。
在一个示例中,当前时刻零偏误差为初始时刻的零偏误差时,当前时刻零偏误差为陀螺仪在预设时间段内处于静止状态下的三轴角速度均值。
步骤S130,根据当前时刻三轴加速度和当前时刻三轴磁场强度,得到当前时刻姿态值。
需要说明的是,如图3所示,根据当前时刻三轴加速度和当前时刻三轴磁场强度,得到当前时刻姿态值的步骤中,包括步骤:
步骤S310,根据当前时刻三轴加速度,获取在预设空间坐标系下的X轴向角度和Y轴向角度;
步骤S320,当前时刻三轴磁场强度在水平面上的投影,获取在预设空间坐标系下的Z轴向角度;
步骤S330,将X轴向角度、Y轴向角度和Z轴向角度进行四元数转换,获取当前时刻姿态值。
为了进一步提高姿态解算精度,在根据当前时刻三轴加速度和当前时刻三轴磁场强度,得到当前时刻姿态值的步骤中,还需要对当前时刻三轴加速度和当前时刻三轴磁场强度进行修正。具体的,在一个示例中,通过水平校准法或六面校准法来获得加速度传感器的当前时刻零偏误差和标度因数的误差,在当前时刻三轴加速度上叠加加速度传感器的当前时刻零偏误差,对当前时刻三轴加速度进行修正,利用修成后的当前三轴加速度来获取得到当前时刻姿态值。
在一个示例中,通过绕三轴旋转磁场传感器,将磁场强度拟合出一个球面或者椭球面,球面的球心为磁场传感器的当前时刻零偏误差、球面的半轴为磁场传感器的标度因数,进一步的,在当前时刻三轴磁场强度上叠加磁场传感器的当前时刻零偏误差,对当前时刻三轴磁场强度进行修正,利用修成后的当前三轴磁场强度来获取得到当前时刻姿态值。
步骤S140,根据下一时刻姿态预测值的方差、当前时刻姿态值的方差和当前时刻姿态值的误差值,得到滤波融合修正系数。
需要说明的是,运动加速度越大,当前时刻姿态值计算出来的结果越不准确,该运动加速度就可以作为一个系数叠加到通过方差计算获得的结果中。其中,运动加速度的计算将上一时刻姿态值转换成姿态矩阵,利用该姿态矩阵将重力加速度进行转换,获得重力加速度在预设坐标系下的投影,然后将三轴加速度减去该重力加速度的在各坐标轴上的投影,即可获得加速度传感器的运动加速度。获得滤波融合修正系数后,将该滤波融合修正系数乘以当前时刻姿态值的误差值,获得下一时刻姿态预测值的修正值。
进一步的,根据下一时刻姿态预测值的方差、当前时刻姿态值的方差和当前时刻姿态值的误差值,得到滤波融合修正系数的步骤中,包括步骤:
根据下一时刻姿态预测值的方差、当前时刻姿态值的方差和当前时刻姿态值的误差值,获取下一时刻零偏误差。下一时刻零偏误差用于对下一时刻传感器采集的数据进行修成,具体的,下一时刻零偏误差包括加速度传感器的下一时刻零偏误差、陀螺仪的下一时刻零偏误差以及磁场传感器的下一时刻零偏误差。
步骤S150,基于滤波融合修正系数,对下一时刻姿态预测值进行修正,得到下一时刻姿态修正值。
需要说明的是,在一个示例中,利用卡尔曼滤波算法,基于滤波融合修正系数,对下一时刻姿态预测值进行修正,得到下一时刻姿态修正值。
步骤S160,将下一时刻姿态修正值转换成姿态角度。
需要说明的是,下一时刻姿态修正值是姿态四元数,四元数不能直观地体现出三轴姿态角度,需要通过转换将四元数变换成对应的姿态欧拉角。欧拉角在使用的时候必须预先定义坐标旋转顺序,也就是坐标从惯性系旋转到设备的预设坐标系的时候,按照什么顺序去旋转。一个姿态对应一个姿态四元数,但不同的旋转顺序则可以解算出不同欧拉角,本申请中采用321的旋转顺序来定义欧拉角,也就是先绕Z轴旋转,再绕Y轴旋转,再绕X轴旋转的方式。运用四元数到欧拉角的转换公式就可将下一时刻姿态修正值转换成姿态角度。
进一步的,在获取到姿态角度,还需将姿态角度输出,具体的,数据输出环节将解算出来的数据按照预定义的格式进行打包输出。数据的输出方式可以根据需要选择串口、CAN总线或者以太网方式。数据输出的格式根据数据的内容进行打包,每个数据包有11各字节的数据,包含数据包包头、数据内容和校验值。
本申请各实施例提供的姿态角度解算方法,包括以下步骤:获取陀螺仪采集的当前时刻三轴角速度、加速度传感器采集的当前时刻三轴加速度以及磁场传感器采集的当前时刻三轴磁场强度;对当前时刻三轴角速度进行积分处理,得到下一时刻姿态预测值;根据当前时刻三轴加速度和当前时刻三轴磁场强度,得到当前时刻姿态值;根据下一时刻姿态预测值的方差、当前时刻姿态值的方差和当前时刻姿态值的误差值,得到滤波融合修正系数;基于滤波融合修正系数,对下一时刻姿态预测值进行修正,得到下一时刻姿态修正值;将下一时刻姿态修正值转换成姿态角度,通过当前时刻三轴角速度预测下一时刻姿态预测值,然后利用获取到滤波融合修正系数对下一时刻姿态预测值进行修正,从而提高可姿态角度解算精度,降低了解算误差,避免因震动干扰产生的角度漂移现象。
应该理解的是,虽然图1-3的流程图中的各个步骤按照箭头的指示依次显示,但是这些步骤并不是必然按照箭头指示的顺序依次执行。除非本文中有明确的说明,这些步骤的执行并没有严格的顺序限制,这些步骤可以以其它的顺序执行。而且,图1-3中的至少一部分步骤可以包括多个子步骤或者多个阶段,这些子步骤或者阶段并不必然是在同一时刻执行完成,而是可以在不同的时刻执行,这些子步骤或者阶段的执行顺序也不必然是依次进行,而是可以与其它步骤或者其它步骤的子步骤或者阶段的至少一部分轮流或者交替地执行。
在一个实施例中,如图4所示,提供了一种姿态角度解算装置,包括:
数据获取模块41,用于获取陀螺仪采集的当前时刻三轴角速度、加速度传感器采集的当前时刻三轴加速度以及磁场传感器采集的当前时刻三轴磁场强度;
姿态预测模块42,用于对当前时刻三轴角速度进行积分处理,得到下一时刻姿态预测值;
当前姿态获取模块43,用于根据当前时刻三轴加速度和当前时刻三轴磁场强度,得到当前时刻姿态值;
修正系数获取模块44,用于根据下一时刻姿态预测值的方差、当前时刻姿态值的方差和当前时刻姿态值的误差值,得到滤波融合修正系数;
修正模块45,用于基于滤波融合修正系数,对下一时刻姿态预测值进行修正,得到下一时刻姿态修正值;
转换模块46,用于将下一时刻姿态修正值转换成姿态角度。
关于姿态角度解算装置的具体限定可以参见上文中对于姿态角度解算方法的限定,在此不再赘述。上述姿态角度解算装置中的各个模块可全部或部分通过软件、硬件及其组合来实现。上述各模块可以硬件形式内嵌于或独立于处理设备中的处理器中,也可以以软件形式存储于处理设备中的存储器中,以便于处理器调用执行以上各个模块对应的操作。
在一个实施例中,提供了一种处理设备,该处理设备可以是单片机,其内部结构图可以如图5所示。该处理设备包括通过***总线连接的处理器、存储器、接口和数据库。其中,该处理设备的处理器用于提供计算和控制能力。该处理设备的存储器包括非易失性存储介质、内存储器。该非易失性存储介质存储有操作***、计算机程序和数据库。该内存储器为非易失性存储介质中的操作***和计算机程序的运行提供环境。该处理设备的数据库用于存储三轴加速度、三轴磁场强度和三轴角速度等数据。该处理设备的接口用于连接传感器。该计算机程序被处理器执行时以实现一种姿态角度解算方法。
本领域技术人员可以理解,图5中示出的结构,仅仅是与本申请方案相关的部分结构的框图,并不构成对本申请方案所应用于其上的处理设备的限定,具体的处理设备可以包括比图中所示更多或更少的部件,或者组合某些部件,或者具有不同的部件布置。
在一个实施例中,提供了一种处理设备,包括存储器和处理器,存储器中存储有计算机程序,该处理器执行计算机程序时实现以下步骤:
获取陀螺仪采集的当前时刻三轴角速度、加速度传感器采集的当前时刻三轴加速度以及磁场传感器采集的当前时刻三轴磁场强度;
对当前时刻三轴角速度进行积分处理,得到下一时刻姿态预测值;
根据当前时刻三轴加速度和当前时刻三轴磁场强度,得到当前时刻姿态值;
根据下一时刻姿态预测值的方差、当前时刻姿态值的方差和当前时刻姿态值的误差值,得到滤波融合修正系数;
基于滤波融合修正系数,对下一时刻姿态预测值进行修正,得到下一时刻姿态修正值;
将下一时刻姿态修正值转换成姿态角度。
在一个实施例中,处理器执行计算机程序时还实现以下步骤:
在当前时刻三轴角速度上叠加陀螺仪的当前时刻零偏误差,以对当前时刻三轴角速度进行修正;
对修正后的当前时刻三轴角速度进行积分处理,得到下一时刻姿态预测值。
在一个实施例中,处理器执行计算机程序时还实现以下步骤:
根据当前时刻三轴加速度,获取在预设空间坐标系下的X轴向角度和Y轴向角度;
当前时刻三轴磁场强度在水平面上的投影,获取在预设空间坐标系下的Z轴向角度;
将X轴向角度、Y轴向角度和Z轴向角度进行四元数转换,获取当前时刻姿态值。
在一个实施例中,提供了一种计算机可读存储介质,其上存储有计算机程序,计算机程序被处理器执行时实现以下步骤:
获取陀螺仪采集的当前时刻三轴角速度、加速度传感器采集的当前时刻三轴加速度以及磁场传感器采集的当前时刻三轴磁场强度;
对当前时刻三轴角速度进行积分处理,得到下一时刻姿态预测值;
根据当前时刻三轴加速度和当前时刻三轴磁场强度,得到当前时刻姿态值;
根据下一时刻姿态预测值的方差、当前时刻姿态值的方差和当前时刻姿态值的误差值,得到滤波融合修正系数;
基于滤波融合修正系数,对下一时刻姿态预测值进行修正,得到下一时刻姿态修正值;
将下一时刻姿态修正值转换成姿态角度。
在一个实施例中,计算机程序被处理器执行时还实现以下步骤:
在当前时刻三轴角速度上叠加陀螺仪的当前时刻零偏误差,以对当前时刻三轴角速度进行修正;
对修正后的当前时刻三轴角速度进行积分处理,得到下一时刻姿态预测值。
在一个实施例中,计算机程序被处理器执行时还实现以下步骤:
根据当前时刻三轴加速度,获取在预设空间坐标系下的X轴向角度和Y轴向角度;
当前时刻三轴磁场强度在水平面上的投影,获取在预设空间坐标系下的Z轴向角度;
将X轴向角度、Y轴向角度和Z轴向角度进行四元数转换,获取当前时刻姿态值。
本领域普通技术人员可以理解实现上述实施例方法中的全部或部分流程,是可以通过计算机程序来指令相关的硬件来完成,所述的计算机程序可存储于一非易失性计算机可读取存储介质中,该计算机程序在执行时,可包括如上述各方法的实施例的流程。其中,本申请所提供的各实施例中所使用的对存储器、存储、数据库或其它介质的任何引用,均可包括非易失性和/或易失性存储器。非易失性存储器可包括只读存储器(ROM)、可编程ROM(PROM)、电可编程ROM(EPROM)、电可擦除可编程ROM(EEPROM)或闪存。易失性存储器可包括随机存取存储器(RAM)或者外部高速缓冲存储器。作为说明而非局限,RAM以多种形式可得,诸如静态RAM(SRAM)、动态RAM(DRAM)、同步DRAM(SDRAM)、双数据率SDRAM(DDRSDRAM)、增强型SDRAM(ESDRAM)、同步链路(Synchlink)DRAM(SLDRAM)、存储器总线(Rambus)直接RAM(RDRAM)、直接存储器总线动态RAM(DRDRAM)、以及存储器总线动态RAM(RDRAM)等。
以上实施例的各技术特征可以进行任意的组合,为使描述简洁,未对上述实施例中的各个技术特征所有可能的组合都进行描述,然而,只要这些技术特征的组合不存在矛盾,都应当认为是本说明书记载的范围。
以上所述实施例仅表达了本申请的几种实施方式,其描述较为具体和详细,但并不能因此而理解为对申请专利范围的限制。应当指出的是,对于本领域的普通技术人员来说,在不脱离本申请构思的前提下,还可以做出若干变形和改进,这些都属于本申请的保护范围。因此,本申请专利的保护范围应以所附权利要求为准。
Claims (10)
1.一种姿态角度解算方法,其特征在于,包括以下步骤:
获取陀螺仪采集的当前时刻三轴角速度、加速度传感器采集的当前时刻三轴加速度以及磁场传感器采集的当前时刻三轴磁场强度;
对所述当前时刻三轴角速度进行积分处理,得到下一时刻姿态预测值;
根据所述当前时刻三轴加速度和所述当前时刻三轴磁场强度,得到当前时刻姿态值;
根据所述下一时刻姿态预测值的方差、所述当前时刻姿态值的方差和所述当前时刻姿态值的误差值,得到滤波融合修正系数;
基于所述滤波融合修正系数,对所述下一时刻姿态预测值进行修正,得到下一时刻姿态修正值;所述下一时刻姿态修正值为由所述滤波融合修正系数乘以所述当前时刻姿态值的误差值获得;
将所述下一时刻姿态修正值转换成姿态角度。
2.根据权利要求1所述的姿态角度解算方法,其特征在于,对所述当前时刻三轴角速度进行积分处理,得到下一时刻姿态预测值的步骤中,包括步骤:
在所述当前时刻三轴角速度上叠加所述陀螺仪的当前时刻零偏误差,以对所述当前时刻三轴角速度进行修正;
对修正后的所述当前时刻三轴角速度进行积分处理,得到所述下一时刻姿态预测值。
3.根据权利要求2所述的姿态角度解算方法,其特征在于,所述当前时刻零偏误差为初始时刻的零偏误差时,所述当前时刻零偏误差为所述陀螺仪在预设时间段内处于静止状态下的三轴角速度均值。
4.根据权利要求2所述的姿态角度解算方法,其特征在于,根据所述下一时刻姿态预测值的方差、所述当前时刻姿态值的方差和所述当前时刻姿态值的误差值,得到滤波融合修正系数的步骤中,包括步骤:
根据所述下一时刻姿态预测值的方差、所述当前时刻姿态值的方差和所述当前时刻姿态值的误差值,获取下一时刻零偏误差。
5.根据权利要求1所述的姿态角度解算方法,其特征在于,根据所述当前时刻三轴加速度和所述当前时刻三轴磁场强度,得到当前时刻姿态值的步骤中,包括步骤:
根据所述当前时刻三轴加速度,获取在预设空间坐标系下的X轴向角度和Y轴向角度;
所述当前时刻三轴磁场强度在水平面上的投影,获取在所述预设空间坐标系下的Z轴向角度;
将所述X轴向角度、所述Y轴向角度和所述Z轴向角度进行四元数转换,获取所述当前时刻姿态值。
6.根据权利要求1所述的姿态角度解算方法,其特征在于,获取陀螺仪采集的当前时刻三轴角速度、加速度传感器采集的当前时刻三轴加速度以及磁场传感器采集的当前时刻三轴磁场强度步骤:
在预设周期到来时,获取所述陀螺仪采集的所述当前时刻三轴角速度、所述加速度传感器采集的所述当前时刻三轴加速度以及所述磁场传感器采集的所述当前时刻三轴磁场强度。
7.根据权利要求1至6任意一项所述的姿态角度解算方法,其特征在于,获取陀螺仪采集的当前时刻三轴角速度、加速度传感器采集的当前时刻三轴加速度以及磁场传感器采集的当前时刻三轴磁场强度的步骤之前,还包括步骤:
执行初始化程序。
8.一种姿态角度解算装置,其特征在于,包括:
数据获取模块,用于获取陀螺仪采集的当前时刻三轴角速度、加速度传感器采集的当前时刻三轴加速度以及磁场传感器采集的当前时刻三轴磁场强度;
姿态预测模块,用于对所述当前时刻三轴角速度进行积分处理,得到下一时刻姿态预测值;
当前姿态获取模块,用于根据所述当前时刻三轴加速度和所述当前时刻三轴磁场强度,得到当前时刻姿态值;
修正系数获取模块,用于根据所述下一时刻姿态预测值的方差、所述当前时刻姿态值的方差和所述当前时刻姿态值的误差值,得到滤波融合修正系数;
修正模块,用于基于所述滤波融合修正系数,对所述下一时刻姿态预测值进行修正,得到下一时刻姿态修正值;所述下一时刻姿态修正值为由所述滤波融合修正系数乘以所述当前时刻姿态值的误差值获得;
转换模块,用于将所述下一时刻姿态修正值转换成姿态角度。
9.一种处理设备,包括存储器和处理器,所述存储器存储有计算机程序,其特征在于,所述处理器执行所述计算机程序时实现权利要求1至7中任意一项所述方法的步骤。
10.一种计算机可读存储介质,其上存储有计算机程序,其特征在于,所述计算机程序被处理器执行时实现权利要求1至7中任意一项所述方法的步骤。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202010979510.3A CN112066984B (zh) | 2020-09-17 | 2020-09-17 | 姿态角度解算方法、装置、处理设备和存储介质 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202010979510.3A CN112066984B (zh) | 2020-09-17 | 2020-09-17 | 姿态角度解算方法、装置、处理设备和存储介质 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN112066984A CN112066984A (zh) | 2020-12-11 |
CN112066984B true CN112066984B (zh) | 2022-08-30 |
Family
ID=73681730
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202010979510.3A Active CN112066984B (zh) | 2020-09-17 | 2020-09-17 | 姿态角度解算方法、装置、处理设备和存储介质 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN112066984B (zh) |
Families Citing this family (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN113188505B (zh) * | 2021-04-14 | 2022-09-27 | 湖南三一智能控制设备有限公司 | 姿态角度的测量方法、装置、车辆及智能臂架 |
CN113865571A (zh) * | 2021-08-20 | 2021-12-31 | 无锡宇宁智能科技有限公司 | 提高手机指南针应用精度的方法、装置和可读存储介质 |
CN118276672A (zh) * | 2022-12-30 | 2024-07-02 | 优奈柯恩(北京)科技有限公司 | 用于在头戴显示设备显示画面的方法、装置、设备及介质 |
CN117450992B (zh) * | 2023-09-21 | 2024-05-03 | 中国海洋工程研究院(青岛) | 一种姿态与磨损监测预警方法和*** |
Family Cites Families (12)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN103674009A (zh) * | 2012-09-04 | 2014-03-26 | 百度在线网络技术(北京)有限公司 | 获取移动终端运动轨迹的方法、装置和移动终端 |
CN104764452A (zh) * | 2015-04-23 | 2015-07-08 | 北京理工大学 | 一种基于惯性和光学跟踪***的混合位姿跟踪方法 |
CN105136145A (zh) * | 2015-08-11 | 2015-12-09 | 哈尔滨工业大学 | 一种基于卡尔曼滤波的四旋翼无人机姿态数据融合的方法 |
CN105716610B (zh) * | 2016-01-28 | 2018-09-04 | 北京航空航天大学 | 一种地磁场模型辅助的载体姿态和航向计算方法和*** |
CN106767776A (zh) * | 2016-11-17 | 2017-05-31 | 上海兆芯集成电路有限公司 | 移动设备及求取移动设备姿态的方法 |
CN107272718B (zh) * | 2017-06-19 | 2020-07-24 | 歌尔科技有限公司 | 基于卡尔曼滤波的姿态控制方法及装置 |
CN108534744A (zh) * | 2018-01-30 | 2018-09-14 | 歌尔科技有限公司 | 一种姿态角获取方法、装置和手柄 |
CN108519090B (zh) * | 2018-03-27 | 2021-08-20 | 东南大学—无锡集成电路技术研究所 | 一种基于优化的ukf算法的双通道组合定姿算法的实现方法 |
CN108827299B (zh) * | 2018-03-29 | 2022-04-12 | 南京航空航天大学 | 一种基于改进四元数二阶互补滤波的飞行器姿态解算方法 |
CN109163721B (zh) * | 2018-09-18 | 2020-06-09 | 河北美泰电子科技有限公司 | 姿态测量方法及终端设备 |
CN110146077A (zh) * | 2019-06-21 | 2019-08-20 | 台州知通科技有限公司 | 移动机器人姿态角解算方法 |
CN111551175B (zh) * | 2020-05-27 | 2024-03-15 | 北京计算机技术及应用研究所 | 一种航姿参考***的互补滤波姿态解算方法 |
-
2020
- 2020-09-17 CN CN202010979510.3A patent/CN112066984B/zh active Active
Also Published As
Publication number | Publication date |
---|---|
CN112066984A (zh) | 2020-12-11 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN112066984B (zh) | 姿态角度解算方法、装置、处理设备和存储介质 | |
RU2662458C1 (ru) | Способ прецизионной калибровки системы измерения пространственного положения | |
CN108957495B (zh) | Gnss与mimu组合导航方法、装置及计算机设备 | |
CN108871311B (zh) | 位姿确定方法和装置 | |
US20200286245A1 (en) | Posture estimation method, posture estimation apparatus and computer readable storage medium | |
CN108731664B (zh) | 机器人状态估计方法、装置、计算机设备和存储介质 | |
CN110440827B (zh) | 一种参数误差的标定方法、装置及存储介质 | |
CN112798021B (zh) | 基于激光多普勒测速仪的惯导***行进间初始对准方法 | |
KR101106048B1 (ko) | 센서오차의 작동 중 자동교정 방법과 이를 이용한 관성항법장치 | |
CN111522034B (zh) | 基于惯性导航的定位方法、设备及装置 | |
CN116067370A (zh) | 一种imu姿态解算方法及设备、存储介质 | |
CN110873563B (zh) | 一种云台姿态估计方法及装置 | |
CN104819717B (zh) | 一种基于mems惯性传感器组的多旋翼飞行器姿态检测方法 | |
CN110361003B (zh) | 信息融合方法、装置、计算机设备和计算机可读存储介质 | |
CN115752471A (zh) | 一种传感器数据的处理方法、设备及计算机可读存储介质 | |
CN112428277A (zh) | 位置标定方法、装置、计算机设备和存储介质 | |
CN110160530B (zh) | 一种基于四元数的航天器姿态滤波方法 | |
CN106931965B (zh) | 一种确定终端姿态的方法及装置 | |
CN112525144A (zh) | 一种非线性姿态检测补偿方法及终端 | |
CN109866217B (zh) | 机器人里程定位方法、装置、终端设备及计算机存储介质 | |
CN114147717B (zh) | 机器人运动轨迹估计方法、装置、控制器及存储介质 | |
CN102932686B (zh) | 在遥控器中处理传感器数据的方法 | |
CN115355905A (zh) | 用于足式机器人的状态估计方法及装置、计算机设备 | |
CN114234970A (zh) | 一种运动载体姿态实时测量方法、装置 | |
CN115037703A (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 |