CN105352487B - 一种姿态测量系统的精度校准方法 - Google Patents
一种姿态测量系统的精度校准方法 Download PDFInfo
- Publication number
- CN105352487B CN105352487B CN201510665021.XA CN201510665021A CN105352487B CN 105352487 B CN105352487 B CN 105352487B CN 201510665021 A CN201510665021 A CN 201510665021A CN 105352487 B CN105352487 B CN 105352487B
- Authority
- CN
- China
- Prior art keywords
- matrix
- accelerometer
- calculated
- electronic compass
- data
- 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
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C25/00—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C17/00—Compasses; Devices for ascertaining true or magnetic north for navigation or surveying purposes
- G01C17/38—Testing, calibrating, or compensating of compasses
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C25/00—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass
- G01C25/005—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass initial alignment, calibration or starting-up of inertial devices
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01P—MEASURING LINEAR OR ANGULAR SPEED, ACCELERATION, DECELERATION, OR SHOCK; INDICATING PRESENCE, ABSENCE, OR DIRECTION, OF MOVEMENT
- G01P15/00—Measuring acceleration; Measuring deceleration; Measuring shock, i.e. sudden change of acceleration
- G01P15/18—Measuring acceleration; Measuring deceleration; Measuring shock, i.e. sudden change of acceleration in two or more dimensions
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01P—MEASURING LINEAR OR ANGULAR SPEED, ACCELERATION, DECELERATION, OR SHOCK; INDICATING PRESENCE, ABSENCE, OR DIRECTION, OF MOVEMENT
- G01P21/00—Testing or calibrating of apparatus or devices covered by the preceding groups
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Manufacturing & Machinery (AREA)
- Length Measuring Devices With Unspecified Measuring Means (AREA)
- Navigation (AREA)
Abstract
本发明公开了一种姿态测量系统的精度校准方法,所述姿态测量系统的精度校准方法包括如下步骤:通过椭球拟合模型对姿态测量系统的加速度计的零偏、刻度系数与轴间不正交角进行校准;利用计算出的椭球参数对加速度计原始数据进行补偿;根据补偿后的加速度计数据通过椭球拟合模型对电子罗盘进行准;利用计算出的椭球参数对电子罗盘原始数据进行补偿;根据补偿后的加速度计数据与电子罗盘数据对姿态进行解算,上述方法步骤,校准结果可靠、精度高、校准耗时少。
Description
技术领域
本发明涉及测量技术领域,尤其涉及一种姿态测量系统的精度校准方法。
背景技术
姿态测量系统是一种可以测量物体空间姿态(俯仰角、横滚角、航向角)的一系列设备,在工业上的许多领域有着广泛的应用,近年来,随着硬件成本的不断下降,各种类型的姿态测量系统开始走进千家万户的日常生活之中,以手机为例,目前大部分智能手机都内置了加速度计、陀螺仪和电子罗盘,构成了一个简易的低成本姿态测量系统。然而,目前大部分姿态测量系统中,由于成本的限制,系统内置传感器的精度和稳定性都不高,从而导致了整个姿态测量系统的测量结果不理想。在不改变系统硬件的情况下对传感器进行校准,无疑是提升整个系统测量精度的一种极为实用的方式。因此,研究低成本姿态测量系统的校准方法具有极强的现实应用价值。
传感器的校准与标定一直是传感器技术领域内的一个重要课题,对于姿态测量系统的校准方案,国内外学者也有大量的相关研究。孙伟、付心如等研究了利用速率转台进行MEMS惯导的多位置标定方法;马斌良等学者提出了一种在有角度基准条件下基于傅里叶变换的电子罗盘校准方案;秦伟等学者研究了基于神经网络和UKF(无迹卡尔曼滤波)的姿态测量系统在线标定技术。
目前的研究大多数集中在利用转台等校准器械对姿态测量系统进行校准。然而,校准专用器械价格昂贵,且其中大部分器械操作复杂,导致生产成本较高;另一方面,在实际应用中,随着周围环境的变化和传感器本身的老化,系统内传感器的特性会不断变化,仅仅依靠出厂前的校准在实际应用中难以起到很好的效果。另外,目前一部分关于非器械校准的研究中,其校准方案要么太过简易,对实际测量精度的提升帮助不大;要么过于复杂,对使用人员的相关技能有着较高的要求。
发明内容
鉴于目前姿态校准存在的上述不足,本发明提供一种姿态测量系统的精度校准方法,校准结果可靠、精度高、校准耗时少。
为达到上述目的,本发明的实施例采用如下技术方案:
一种姿态测量系统的精度校准方法,所述姿态测量系统的精度校准方法包括如下步骤:
通过椭球拟合模型对姿态测量系统的加速度计的零偏、刻度系数与轴间不正交角进行校准;
利用计算出的椭球参数对加速度计原始数据进行补偿;
通过椭球拟合模型对电子罗盘进行准;
利用计算出的椭球参数对电子罗盘原始数据进行补偿;
根据补偿后的加速度计数据与电子罗盘数据对姿态进行解算。
依照本发明的一个方面,所述通过椭球拟合模型对姿态测量系统的加速度计的零偏、刻度系数与轴间不正交角进行校准步骤执行前执行以下步骤:对加速度计进行水平校准,消除加速度计原始零偏。
依照本发明的一个方面,所述对加速度计进行水平校准,消除加速度计原始零偏步骤执行后执行以下步骤:采集加速度计在一段时间内的三轴数据,其中,三轴数据标记为[AX0、AY0、AZ0]T,零偏标记为B=[AX0、AY0、AZ0]T–[0 0 g]T。。
依照本发明的一个方面,所述通过椭球拟合模型对姿态测量系统的加速度计的零偏、刻度系数与轴间不正交角进行校准步骤包括以下步骤:
设定拟合椭球参数向量,标记为α=[a b c d e f k l m n]T;
设定校准过程迭代次数为r,将加速度采集的第一个值[AX0、AY0、AZ0]T拓展为10维列向量D1,并计算矩阵S1,其中,D1标记为D1=[Ax1 2 Ay1 2 Az1 2 Ax1Ay1 Ax1Az1 Ay1Az1 Ax1 Ay1Az1 1]T,S1=D1TD1;
若加速度计三轴数据的采集次数未达到r次,则用新采集的加速度计数据DK对矩阵S进行更新,其中,Sk=Sk-1+Dk TDk;
计算出矩阵S后,同时引入椭球限制矩阵C并将矩阵S分块,其中,
将矩阵S分块:
Dk=[D1 D2]
D1=[Ax1 2 Ay1 2 Az1 2]
D2=[Ax1Ay1 Ay1Az1 Ax1Az1 Ax1 Ay1 Az1 1]
通过以下公式求得椭球拟合向量:
C1 -1(S1-S2S4 -1S2 T)α1=λα1
α2=-S4 -1S2 Tα1;
计算出α1后,通过上述公式计算出α2,则椭球拟合向量的所有参数全部被计算出,其中,α=[α1 α2]=[a b c d e f k l m n]T;
再根据实际的物理对应关系,三轴的刻度系数kx,ky,kz、轴间不正交角δxy,δyz,δxz及残余零偏bx,by,bz可以通过以下公式计算得出:
依照本发明的一个方面,所述通过椭球拟合模型对姿态测量系统的加速度计的零偏、刻度系数与轴间不正交角进行校准步骤执行后执行以下步骤:当采集到新的加速度计数据后,可以用椭球参数对新采集到的数据进行修正。
依照本发明的一个方面,所述通过椭球拟合模型对电子罗盘进行准步骤包括如下步骤:
设定拟合椭球参数向量,标记为α=[a b c d e f k l m n]T;
采集电子罗盘在一段时间内的三轴数据,并对采集的数据进行运算防溢处理,其中,三轴数据标记为[HX0 HY0 HZ0]T;
设定校准过程迭代次数为r,将电子罗盘采集的第一个值[HX1 HY1 HZ1]T拓展为10维列向量D1,并计算矩阵S1,其中,D1标记为D1=[Hx1 2 Hy1 2 Hz1 2 Hx1Hy1 Hx1Hz1 Hy1Hz1 Hx1 Hy1Hz1 1]T,S1=D1TD1;
若电子罗盘三轴数据的采集次数未达到r次,则用新采集的电子罗盘数据DK对矩阵S进行更新,其中,Sk=Sk-1+Dk TDk;
计算出矩阵S后,同时引入椭球限制矩阵C并将矩阵S分块,其中,
将矩阵S分块:
Dk=[D1 D2]
D2=[Hx1Hy1 Hx1Hz1 Hy1Hz1 Hx1 Hy1 Hz1 1]
通过以下公式求得椭球拟合向量:
C1 -1(S1-S2S4 -1S2 T)α1=λα1
α2=-S4 -1S2 Tα1;
计算出α1后,通过上述公式计算出α2,则椭球拟合向量的所有参数全部被计算出,其中,α=[α1 α2]=[a b c d e f k l m n]T;
再根据实际的物理对应关系,三轴的刻度系数kx,ky,kz、轴间不正交角δxy,δyz,δxz及残余零偏bx,by,bz可以通过以下公式计算得出:
依照本发明的一个方面,所述通过椭球拟合模型对电子罗盘进行准步骤执行后执行以下步骤:当采集到新的电子罗盘数据后,可以用椭球参数对新采集到的数据进行修正。
依照本发明的一个方面,所述根据补偿后的加速度计数据与电子罗盘数据对姿态进行解算步骤执行后执行以下步骤:电子罗盘在测量过程中利用测量数据对椭球参数进行更新,提高椭球参数的准确性。
依照本发明的一个方面,所述电子罗盘在测量过程中利用测量数据对椭球参数进行更新,提高椭球参数的准确性步骤执行前执行以下步骤:通过统计规则的方式剔除采集的电子罗盘数据中出现跳变的计算点。
本发明实施的优点:通过采用了椭球拟合的方式对姿态测量系统中的电子罗盘和加速度进行校准,可有效地避免传感器偶然输出异常、跳数等问题,使得校准结果的可靠性得到了有效的保证;对椭球拟合算法进行了递推化处理,使得姿态测量系统内部CPU在处理数据时不需要存储传感器采集的过往所有点的数据,而仅需要存储之前递推得到的矩阵和当次的测量结果即可,这使得在相同的硬件环境下,该椭球拟合方法可以处理更多的数值,从而提高椭球拟合的精度。相对应的,采用传统拟合算法时,CPU为了确保运算过程中内存栈不会溢出,只能根据少量的样本点做拟合运算,因而拟合精度有限;对椭球拟合算法中大规模的矩阵运算进行了改进,优化了其中的计算流程,使得椭球拟合的计算量减少90%以上,以Matlab程序为例,按照传统算法,运算一次拟合算法耗时563ms;相对地,采用改进后的算法耗时仅为14ms;采用了利用测量数据进行校准的策略,提高了校准时的容错能力,从而降低了初始校准时的要求,减少了使用人员的操作复杂程度。
附图说明
为了更清楚地说明本发明实施例中的技术方案,下面将对实施例中所需要使用的附图作简单地介绍,显而易见地,下面描述中的附图仅仅是本发明的一些实施例,对于本领域普通技术人员来讲,在不付出创造性劳动的前提下,还可以根据这些附图获得其他的附图。
图1为本发明所述的一种姿态测量系统的精度校准方法的实施例1的方法流程图;
图2为本发明所述的一种姿态测量系统的精度校准方法的实施例2的方法流程图;
图3为本发明所述的一种姿态测量系统的精度校准方法的实施例3的方法流程图;
图4为本发明所述的一种姿态测量系统的精度校准方法的实施例4的方法流程图;
图5为本发明所述的一种姿态测量系统的精度校准方法的实施例5的方法流程图;
图6为本发明所述的一种姿态测量系统的精度校准方法的实施例6的方法流程图;
图7为本发明所述的一种姿态测量系统的精度校准方法的实施例7的方法流程图。
具体实施方式
下面将结合本发明实施例中的附图,对本发明实施例中的技术方案进行清楚、完整地描述,显然,所描述的实施例仅仅是本发明一部分实施例,而不是全部的实施例。基于本发明中的实施例,本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例,都属于本发明保护的范围。
实施例1:
如图1所示,一种姿态测量系统的精度校准方法,所述姿态测量系统的精度校准方法包括如下步骤:
步骤S1:通过椭球拟合模型对姿态测量系统的加速度计的零偏、刻度系数与轴间不正交角进行校准;
所述步骤S1:通过椭球拟合模型对姿态测量系统的加速度计的零偏、刻度系数与轴间不正交角进行校准步骤包括以下步骤:
设定拟合椭球参数向量,标记为α=[a b c d e f k l m n]T;
设定校准过程迭代次数为r,将加速度采集的第一个值[AX0、AY0、AZ0]T拓展为10维列向量D1,并计算矩阵S1,其中,D1标记为D1=[Ax1 2 Ay1 2 Az1 2 Ax1Ay1 Ax1Az1 Ay1Az1 Ax1 Ay1Az1 1]T,S1=D1TD1;
若加速度计三轴数据的采集次数未达到r次,则用新采集的加速度计数据DK对矩阵S进行更新,其中,Sk=Sk-1+Dk TDk;
计算出矩阵S后,同时引入椭球限制矩阵C并将矩阵S分块,其中,
将矩阵S分块:
Dk=[D1 D2]
D1=[Ax1 2 Ay1 2 Az1 2]
D2=[Ax1Ay1 Ay1Az1 Ax1Az1 Ax1 Ay1 Az1 1]
通过以下公式求得椭球拟合向量:
C1 -1(S1-S2S4 -1S2 T)α1=λα1
α2=-S4 -1S2 Tα1;
计算出α1后,通过上述公式计算出α2,则椭球拟合向量的所有参数全部被计算出,其中,α=[α1 α2]=[a b c d e f k l m n]T;
再根据实际的物理对应关系,三轴的刻度系数kx,ky,kz、轴间不正交角δxy,δyz,δxz及残余零偏bx,by,bz可以通过以下公式计算得出:
上述公式中,α1为矩阵C1 -1(S1-S2S4 -1S2 T)最大特征值对应的特征向量,因此在实际计算中,不用将所有特征值和特征向量全部计算出来,甚至不用计算特征值,而只要利用幂法或者反幂法进行计算出最大的特征向量即可,以反幂法为例:
令A=(C1 -1(S1-S2S4 -1S2 T))-1,取一个初始迭代向量u=[1 1 1]T,设置迭代精度ε=1e-6;
对A做LU分解,即A=LU;
解线性方程组:
Ly(k)=u(k-1),Uv(k)=y(k)
mk=max(v(k)),u(k)=v(k)/mk
若|mk-mk-1|<ε,则跳出迭代,停止计算,此时α1=u(k),否则k=k+1,继续计算。
一般而言,对于ε=1e-6,5次以下的迭代计算即可达到此精度。
步骤S2:利用计算出的椭球参数对加速度计原始数据进行补偿。
步骤S3:通过椭球拟合模型对电子罗盘进行准;
所述步骤S3:通过椭球拟合模型对电子罗盘进行准步骤包括如下步骤:
设定拟合椭球参数向量,标记为α=[a b c d e f k l m n]T;
采集电子罗盘在一段时间内的三轴数据,并对采集的数据进行运算防溢处理,其中,三轴数据标记为[HX0 HY0 HZ0]T;
设定校准过程迭代次数为r,将电子罗盘采集的第一个值[HX1 HY1 HZ1]T拓展为10维列向量D1,并计算矩阵S1,其中,D1标记为D1=[Hx1 2 Hy1 2 Hz1 2 Hx1Hy1 Hx1Hz1 Hy1Hz1 Hx1 Hy1Hz1 1]T,S1=D1TD1;
若电子罗盘三轴数据的采集次数未达到r次,则用新采集的电子罗盘数据DK对矩阵S进行更新,其中,Sk=Sk-1+Dk TDk;
计算出矩阵S后,同时引入椭球限制矩阵C并将矩阵S分块,其中,
将矩阵S分块:
Dk=[D1 D2]
D2=[Hx1Hy1 Hx1Hz1 Hy1Hz1 Hx1 Hy1 Hz1 1]
通过以下公式求得椭球拟合向量:
C1 -1(S1-S2S4 -1S2 T)α1=λα1
α2=-S4 -1S2 Tα1;
计算出α1后,通过上述公式计算出α2,则椭球拟合向量的所有参数全部被计算出,其中,α=[α1 α2]=[a b c d e f k l m n]T;
再根据实际的物理对应关系,三轴的刻度系数kx,ky,kz、轴间不正交角δxy,δyz,δxz及残余零偏bx,by,bz可以通过以下公式计算得出:
上述公式中,α1为矩阵C1 -1(S1-S2S4 -1S2 T)最大特征值对应的特征向量,因此在实际计算中,不用将所有特征值和特征向量全部计算出来,甚至不用计算特征值,而只要利用幂法或者反幂法进行计算出最大的特征向量即可,以反幂法为例:
令A=(C1 -1(S1-S2S4 -1S2 T))-1,取一个初始迭代向量u=[1 1 1]T,设置迭代精度ε=1e-6;
对A做LU分解,即A=LU;
解线性方程组:
Ly(k)=u(k-1),Uv(k)=y(k)
mk=max(v(k)),u(k)=v(k)/mk
若|mk-mk-1|<ε,则跳出迭代,停止计算,此时α1=u(k),否则k=k+1,继续计算。
一般而言,对于ε=1e-6,5次以下的迭代计算即可达到此精度。
步骤S4:利用计算出的椭球参数对电子罗盘原始数据进行补偿。
步骤S5:根据补偿后的加速度计数据与电子罗盘数据对姿态进行解算。
实施例2:
如图2所示,一种姿态测量系统的精度校准方法,所述姿态测量系统的精度校准方法包括如下步骤:
步骤S1:对加速度计进行水平校准,消除加速度计原始零偏;
将姿态测量设备通过水平气泡调整到水平位置,对加速度计进行水平校准,去除加速度计的零偏。
步骤S2:通过椭球拟合模型对姿态测量系统的加速度计的零偏、刻度系数与轴间不正交角进行校准;
所述步骤S2:通过椭球拟合模型对姿态测量系统的加速度计的零偏、刻度系数与轴间不正交角进行校准步骤包括以下步骤:
设定拟合椭球参数向量,标记为α=[a b c d e f k l m n]T;
设定校准过程迭代次数为r,将加速度采集的第一个值[AX0、AY0、AZ0]T拓展为10维列向量D1,并计算矩阵S1,其中,D1标记为D1=[Ax1 2 Ay1 2 Az1 2 Ax1Ay1 Ax1Az1 Ay1Az1 Ax1 Ay1Az1 1]T,S1=D1TD1;
若加速度计三轴数据的采集次数未达到r次,则用新采集的加速度计数据DK对矩阵S进行更新,其中,Sk=Sk-1+Dk TDk;
计算出矩阵S后,同时引入椭球限制矩阵C并将矩阵S分块,其中,
将矩阵S分块:
Dk=[D1 D2]
D1=[Ax1 2 Ay1 2 Az1 2]
D2=[Ax1Ay1 Ay1Az1 Ax1Az1 Ax1 Ay1 Az1 1]
通过以下公式求得椭球拟合向量:
C1 -1(S1-S2S4 -1S2 T)α1=λα1
α2=-S4 -1S2 Tα1;
计算出α1后,通过上述公式计算出α2,则椭球拟合向量的所有参数全部被计算出,其中,α=[α1 α2]=[a b c d e f k l m n]T;
再根据实际的物理对应关系,三轴的刻度系数kx,ky,kz、轴间不正交角δxy,δyz,δxz及残余零偏bx,by,bz可以通过以下公式计算得出:
上述公式中,α1为矩阵C1 -1(S1-S2S4 -1S2 T)最大特征值对应的特征向量,因此在实际计算中,不用将所有特征值和特征向量全部计算出来,甚至不用计算特征值,而只要利用幂法或者反幂法进行计算出最大的特征向量即可,以反幂法为例:
令A=(C1 -1(S1-S2S4 -1S2 T))-1,取一个初始迭代向量u=[1 1 1]T,设置迭代精度ε=1e-6;
对A做LU分解,即A=LU;
解线性方程组:
Ly(k)=u(k-1),Uv(k)=y(k)
mk=max(v(k)),u(k)=v(k)/mk
若|mk-mk-1|<ε,则跳出迭代,停止计算,此时α1=u(k),否则k=k+1,继续计算。
一般而言,对于ε=1e-6,5次以下的迭代计算即可达到此精度。
步骤S3:利用计算出的椭球参数对加速度计原始数据进行补偿;
步骤S4:通过椭球拟合模型对电子罗盘进行准;
所述步骤S4:通过椭球拟合模型对电子罗盘进行准步骤包括如下步骤:
设定拟合椭球参数向量,标记为α=[a b c d e f k l m n]T;
采集电子罗盘在一段时间内的三轴数据,并对采集的数据进行运算防溢处理,其中,三轴数据标记为[HX0 HY0 HZ0]T;
设定校准过程迭代次数为r,将电子罗盘采集的第一个值[HX1 HY1 HZ1]T拓展为10维列向量D1,并计算矩阵S1,其中,D1标记为D1=[Hx1 2 Hy1 2 Hz1 2 Hx1Hy1 Hx1Hz1 Hy1Hz1 Hx1 Hy1Hz1 1]T,S1=D1TD1;
若电子罗盘三轴数据的采集次数未达到r次,则用新采集的电子罗盘数据DK对矩阵S进行更新,其中,Sk=Sk-1+Dk TDk;
计算出矩阵S后,同时引入椭球限制矩阵C并将矩阵S分块,其中,
将矩阵S分块:
Dk=[D1 D2]
D2=[Hx1Hy1 Hx1Hz1 Hy1Hz1 Hx1 Hy1 Hz1 1]
通过以下公式求得椭球拟合向量:
C1 -1(S1-S2S4 -1S2 T)α1=λα1
α2=-S4 -1S2 Tα1;
计算出α1后,通过上述公式计算出α2,则椭球拟合向量的所有参数全部被计算出,其中,α=[α1 α2]=[a b c d e f k l m n]T;
再根据实际的物理对应关系,三轴的刻度系数kx,ky,kz、轴间不正交角δxy,δyz,δxz及残余零偏bx,by,bz可以通过以下公式计算得出:
上述公式中,α1为矩阵C1 -1(S1-S2S4 -1S2 T)最大特征值对应的特征向量,因此在实际计算中,不用将所有特征值和特征向量全部计算出来,甚至不用计算特征值,而只要利用幂法或者反幂法进行计算出最大的特征向量即可,以反幂法为例:
令A=(C1 -1(S1-S2S4 -1S2 T))-1,取一个初始迭代向量u=[1 1 1]T,设置迭代精度ε=1e-6;
对A做LU分解,即A=LU;
解线性方程组:
Ly(k)=u(k-1),Uv(k)=y(k)
mk=max(v(k)),u(k)=v(k)/mk
若|mk-mk-1|<ε,则跳出迭代,停止计算,此时α1=u(k),否则k=k+1,继续计算。
一般而言,对于ε=1e-6,5次以下的迭代计算即可达到此精度。
步骤S5:利用计算出的椭球参数对电子罗盘原始数据进行补偿;
步骤S6:根据补偿后的加速度计数据与电子罗盘数据对姿态进行解算。
实施例3:
如图3所示,一种姿态测量系统的精度校准方法,所述姿态测量系统的精度校准方法包括如下步骤:
步骤S1:对加速度计进行水平校准,消除加速度计原始零偏;
将姿态测量设备通过水平气泡调整到水平位置,对加速度计进行水平校准,去除加速度计的零偏。
步骤S2:采集加速度计在一段时间内的三轴数据,其中,三轴数据标记为[AX0、AY0、AZ0]T,零偏标记为B=[AX0、AY0、AZ0]T–[0 0 g]T。
步骤S3:通过椭球拟合模型对姿态测量系统的加速度计的零偏、刻度系数与轴间不正交角进行校准;
所述步骤S3:通过椭球拟合模型对姿态测量系统的加速度计的零偏、刻度系数与轴间不正交角进行校准步骤包括以下步骤:
设定拟合椭球参数向量,标记为α=[a b c d e f k l m n]T;
设定校准过程迭代次数为r,将加速度采集的第一个值[AX0、AY0、AZ0]T拓展为10维列向量D1,并计算矩阵S1,其中,D1标记为D1=[Ax1 2 Ay1 2 Az1 2 Ax1Ay1 Ax1Az1 Ay1Az1 Ax1 Ay1Az1 1]T,S1=D1TD1;
若加速度计三轴数据的采集次数未达到r次,则用新采集的加速度计数据DK对矩阵S进行更新,其中,Sk=Sk-1+Dk TDk;
计算出矩阵S后,同时引入椭球限制矩阵C并将矩阵S分块,其中,
将矩阵S分块:
Dk=[D1 D2]
D1=[Ax1 2 Ay1 2 Az1 2]
D2=[Ax1Ay1 Ay1Az1 Ax1Az1 Ax1 Ay1 Az1 1]
通过以下公式求得椭球拟合向量:
C1 -1(S1-S2S4 -1S2 T)α1=λα1
α2=-S4 -1S2 Tα1;
计算出α1后,通过上述公式计算出α2,则椭球拟合向量的所有参数全部被计算出,其中,α=[α1 α2]=[a b c d e f k l m n]T;
再根据实际的物理对应关系,三轴的刻度系数kx,ky,kz、轴间不正交角δxy,δyz,δxz及残余零偏bx,by,bz可以通过以下公式计算得出:
上述公式中,α1为矩阵C1 -1(S1-S2S4 -1S2 T)最大特征值对应的特征向量,因此在实际计算中,不用将所有特征值和特征向量全部计算出来,甚至不用计算特征值,而只要利用幂法或者反幂法进行计算出最大的特征向量即可,以反幂法为例:
令A=(C1 -1(S1-S2S4 -1S2 T))-1,取一个初始迭代向量u=[1 1 1]T,设置迭代精度ε=1e-6;
对A做LU分解,即A=LU;
解线性方程组:
Ly(k)=u(k-1),Uv(k)=y(k)
mk=max(v(k)),u(k)=v(k)/mk
若|mk-mk-1|<ε,则跳出迭代,停止计算,此时α1=u(k),否则k=k+1,继续计算。
一般而言,对于ε=1e-6,5次以下的迭代计算即可达到此精度。
步骤S4:利用计算出的椭球参数对加速度计原始数据进行补偿;
步骤S5:通过椭球拟合模型对电子罗盘进行准;
所述步骤S5:通过椭球拟合模型对电子罗盘进行准步骤包括如下步骤:
设定拟合椭球参数向量,标记为α=[a b c d e f k l m n]T;
采集电子罗盘在一段时间内的三轴数据,并对采集的数据进行运算防溢处理,其中,三轴数据标记为[HX0 HY0 HZ0]T;
设定校准过程迭代次数为r,将电子罗盘采集的第一个值[HX1 HY1 HZ1]T拓展为10维列向量D1,并计算矩阵S1,其中,D1标记为D1=[Hx1 2 Hy1 2 Hz1 2 Hx1Hy1 Hx1Hz1 Hy1Hz1 Hx1 Hy1Hz1 1]T,S1=D1TD1;
若电子罗盘三轴数据的采集次数未达到r次,则用新采集的电子罗盘数据DK对矩阵S进行更新,其中,Sk=Sk-1+Dk TDk;
计算出矩阵S后,同时引入椭球限制矩阵C并将矩阵S分块,其中,
将矩阵S分块:
Dk=[D1 D2]
D2=[Hx1Hy1 Hx1Hz1 Hy1Hz1 Hx1 Hy1 Hz1 1]
通过以下公式求得椭球拟合向量:
C1 -1(S1-S2S4 -1S2 T)α1=λα1
α2=-S4 -1S2 Tα1;
计算出α1后,通过上述公式计算出α2,则椭球拟合向量的所有参数全部被计算出,其中,α=[α1 α2]=[a b c d e f k l m n]T;
再根据实际的物理对应关系,三轴的刻度系数kx,ky,kz、轴间不正交角δxy,δyz,δxz及残余零偏bx,by,bz可以通过以下公式计算得出:
上述公式中,α1为矩阵C1 -1(S1-S2S4 -1S2 T)最大特征值对应的特征向量,因此在实际计算中,不用将所有特征值和特征向量全部计算出来,甚至不用计算特征值,而只要利用幂法或者反幂法进行计算出最大的特征向量即可,以反幂法为例:
令A=(C1 -1(S1-S2S4 -1S2 T))-1,取一个初始迭代向量u=[1 1 1]T,设置迭代精度ε=1e-6;
对A做LU分解,即A=LU;
解线性方程组:
Ly(k)=u(k-1),Uv(k)=y(k)
mk=max(v(k)),u(k)=v(k)/mk
若|mk-mk-1|<ε,则跳出迭代,停止计算,此时α1=u(k),否则k=k+1,继续计算。
一般而言,对于ε=1e-6,5次以下的迭代计算即可达到此精度。
步骤S6:利用计算出的椭球参数对电子罗盘原始数据进行补偿;
步骤S7:根据补偿后的加速度计数据与电子罗盘数据对姿态进行解算。
实施例4:
如图4所示,一种姿态测量系统的精度校准方法,所述姿态测量系统的精度校准方法包括如下步骤:
步骤S1:对加速度计进行水平校准,消除加速度计原始零偏;
将姿态测量设备通过水平气泡调整到水平位置,对加速度计进行水平校准,去除加速度计的零偏。
步骤S2:采集加速度计在一段时间内的三轴数据,其中,三轴数据标记为[AX0、AY0、AZ0]T,零偏标记为B=[AX0、AY0、AZ0]T–[0 0 g]T。
步骤S3:通过椭球拟合模型对姿态测量系统的加速度计的零偏、刻度系数与轴间不正交角进行校准;
所述步骤S3:通过椭球拟合模型对姿态测量系统的加速度计的零偏、刻度系数与轴间不正交角进行校准步骤包括以下步骤:
设定拟合椭球参数向量,标记为α=[a b c d e f k l m n]T;
设定校准过程迭代次数为r,将加速度采集的第一个值[AX0、AY0、AZ0]T拓展为10维列向量D1,并计算矩阵S1,其中,D1标记为D1=[Ax1 2 Ay1 2 Az1 2 Ax1Ay1 Ax1Az1 Ay1Az1 Ax1 Ay1Az1 1]T,S1=D1TD1;
若加速度计三轴数据的采集次数未达到r次,则用新采集的加速度计数据DK对矩阵S进行更新,其中,Sk=Sk-1+Dk TDk;
计算出矩阵S后,同时引入椭球限制矩阵C并将矩阵S分块,其中,
将矩阵S分块:
Dk=[D1 D2]
D1=[Ax1 2 Ay1 2 Az1 2]
D2=[Ax1Ay1 Ay1Az1 Ax1Az1 Ax1 Ay1 Az1 1]
通过以下公式求得椭球拟合向量:
C1 -1(S1-S2S4 -1S2 T)α1=λα1
α2=-S4 -1S2 Tα1;
计算出α1后,通过上述公式计算出α2,则椭球拟合向量的所有参数全部被计算出,其中,α=[α1 α2]=[a b c d e f k l m n]T;
再根据实际的物理对应关系,三轴的刻度系数kx,ky,kz、轴间不正交角δxy,δyz,δxz及残余零偏bx,by,bz可以通过以下公式计算得出:
上述公式中,α1为矩阵C1 -1(S1-S2S4 -1S2 T)最大特征值对应的特征向量,因此在实际计算中,不用将所有特征值和特征向量全部计算出来,甚至不用计算特征值,而只要利用幂法或者反幂法进行计算出最大的特征向量即可,以反幂法为例:
令A=(C1 -1(S1-S2S4 -1S2 T))-1,取一个初始迭代向量u=[1 1 1]T,设置迭代精度ε=1e-6;
对A做LU分解,即A=LU;
解线性方程组:
Ly(k)=u(k-1),Uv(k)=y(k)
mk=max(v(k)),u(k)=v(k)/mk
若|mk-mk-1|<ε,则跳出迭代,停止计算,此时α1=u(k),否则k=k+1,继续计算。
一般而言,对于ε=1e-6,5次以下的迭代计算即可达到此精度。
步骤S4:当采集到新的加速度计数据后,可以用椭球参数对新采集到的数据进行修正。
步骤S5:利用计算出的椭球参数对加速度计原始数据进行补偿。
步骤S6:通过椭球拟合模型对电子罗盘进行准;
所述步骤S6:通过椭球拟合模型对电子罗盘进行准步骤包括如下步骤:
设定拟合椭球参数向量,标记为α=[a b c d e f k l m n]T;
采集电子罗盘在一段时间内的三轴数据,并对采集的数据进行运算防溢处理,其中,三轴数据标记为[HX0 HY0 HZ0]T;
设定校准过程迭代次数为r,将电子罗盘采集的第一个值[HX1 HY1 HZ1]T拓展为10维列向量D1,并计算矩阵S1,其中,D1标记为D1=[Hx1 2 Hy1 2 Hz1 2 Hx1Hy1 Hx1Hz1 Hy1Hz1 Hx1 Hy1Hz1 1]T,S1=D1TD1;
若电子罗盘三轴数据的采集次数未达到r次,则用新采集的电子罗盘数据DK对矩阵S进行更新,其中,Sk=Sk-1+Dk TDk;
计算出矩阵S后,同时引入椭球限制矩阵C并将矩阵S分块,其中,
将矩阵S分块:
Dk=[D1 D2]
D2=[Hx1Hy1 Hx1Hz1 Hy1Hz1 Hx1 Hy1 Hz1 1]
通过以下公式求得椭球拟合向量:
C1 -1(S1-S2S4 -1S2 T)α1=λα1
α2=-S4 -1S2 Tα1;
计算出α1后,通过上述公式计算出α2,则椭球拟合向量的所有参数全部被计算出,其中,α=[α1 α2]=[a b c d e f k l m n]T;
再根据实际的物理对应关系,三轴的刻度系数kx,ky,kz、轴间不正交角δxy,δyz,δxz及残余零偏bx,by,bz可以通过以下公式计算得出:
上述公式中,α1为矩阵C1 -1(S1-S2S4 -1S2 T)最大特征值对应的特征向量,因此在实际计算中,不用将所有特征值和特征向量全部计算出来,甚至不用计算特征值,而只要利用幂法或者反幂法进行计算出最大的特征向量即可,以反幂法为例:
令A=(C1 -1(S1-S2S4 -1S2 T))-1,取一个初始迭代向量u=[1 1 1]T,设置迭代精度ε=1e-6;
对A做LU分解,即A=LU;
解线性方程组:
Ly(k)=u(k-1),Uv(k)=y(k)
mk=max(v(k)),u(k)=v(k)/mk
若|mk-mk-1|<ε,则跳出迭代,停止计算,此时α1=u(k),否则k=k+1,继续计算。
一般而言,对于ε=1e-6,5次以下的迭代计算即可达到此精度。
步骤S7:利用计算出的椭球参数对电子罗盘原始数据进行补偿;
步骤S8:根据补偿后的加速度计数据与电子罗盘数据对姿态进行解算。
实施例5:
如图5所示,一种姿态测量系统的精度校准方法,所述姿态测量系统的精度校准方法包括如下步骤:
步骤S1:对加速度计进行水平校准,消除加速度计原始零偏;
将姿态测量设备通过水平气泡调整到水平位置,对加速度计进行水平校准,去除加速度计的零偏。
步骤S2:采集加速度计在一段时间内的三轴数据,其中,三轴数据标记为[AX0、AY0、AZ0]T,零偏标记为B=[AX0、AY0、AZ0]T–[0 0 g]T。
步骤S3:通过椭球拟合模型对姿态测量系统的加速度计的零偏、刻度系数与轴间不正交角进行校准;
所述步骤S3:通过椭球拟合模型对姿态测量系统的加速度计的零偏、刻度系数与轴间不正交角进行校准步骤包括以下步骤:
设定拟合椭球参数向量,标记为α=[a b c d e f k l m n]T;
设定校准过程迭代次数为r,将加速度采集的第一个值[AX0、AY0、AZ0]T拓展为10维列向量D1,并计算矩阵S1,其中,D1标记为D1=[Ax1 2 Ay1 2 Az1 2 Ax1Ay1 Ax1Az1 Ay1Az1 Ax1 Ay1Az1 1]T,S1=D1TD1;
若加速度计三轴数据的采集次数未达到r次,则用新采集的加速度计数据DK对矩阵S进行更新,其中,Sk=Sk-1+Dk TDk;
计算出矩阵S后,同时引入椭球限制矩阵C并将矩阵S分块,其中,
将矩阵S分块:
Dk=[D1 D2]
D1=[Ax1 2 Ay1 2 Az1 2]
D2=[Ax1Ay1 Ay1Az1 Ax1Az1 Ax1 Ay1 Az1 1]
通过以下公式求得椭球拟合向量:
C1 -1(S1-S2S4 -1S2 T)α1=λα1
α2=-S4 -1S2 Tα1;
计算出α1后,通过上述公式计算出α2,则椭球拟合向量的所有参数全部被计算出,其中,α=[α1 α2]=[a b c d e f k l m n]T;
再根据实际的物理对应关系,三轴的刻度系数kx,ky,kz、轴间不正交角δxy,δyz,δxz及残余零偏bx,by,bz可以通过以下公式计算得出:
上述公式中,α1为矩阵C1 -1(S1-S2S4 -1S2 T)最大特征值对应的特征向量,因此在实际计算中,不用将所有特征值和特征向量全部计算出来,甚至不用计算特征值,而只要利用幂法或者反幂法进行计算出最大的特征向量即可,以反幂法为例:
令A=(C1 -1(S1-S2S4 -1S2 T))-1,取一个初始迭代向量u=[1 1 1]T,设置迭代精度ε=1e-6;
对A做LU分解,即A=LU;
解线性方程组:
Ly(k)=u(k-1),Uv(k)=y(k)
mk=max(v(k)),u(k)=v(k)/mk
若|mk-mk-1|<ε,则跳出迭代,停止计算,此时α1=u(k),否则k=k+1,继续计算。
一般而言,对于ε=1e-6,5次以下的迭代计算即可达到此精度。
步骤S4:当采集到新的加速度计数据后,可以用椭球参数对新采集到的数据进行修正。
步骤S5:利用计算出的椭球参数对加速度计原始数据进行补偿。
步骤S6:通过椭球拟合模型对电子罗盘进行准;
所述步骤S6:通过椭球拟合模型对电子罗盘进行准步骤包括如下步骤:
设定拟合椭球参数向量,标记为α=[a b c d e f k l m n]T;
采集电子罗盘在一段时间内的三轴数据,并对采集的数据进行运算防溢处理,其中,三轴数据标记为[HX0 HY0 HZ0]T;
设定校准过程迭代次数为r,将电子罗盘采集的第一个值[HX1 HY1 HZ1]T拓展为10维列向量D1,并计算矩阵S1,其中,D1标记为D1=[Hx1 2 Hy1 2 Hz1 2 Hx1Hy1 Hx1Hz1 Hy1Hz1 Hx1 Hy1Hz1 1]T,S1=D1TD1;
若电子罗盘三轴数据的采集次数未达到r次,则用新采集的电子罗盘数据DK对矩阵S进行更新,其中,Sk=Sk-1+Dk TDk;
计算出矩阵S后,同时引入椭球限制矩阵C并将矩阵S分块,其中,
将矩阵S分块:
Dk=[D1 D2]
D2=[Hx1Hy1 Hx1Hz1 Hy1Hz1 Hx1 Hy1 Hz1 1]
通过以下公式求得椭球拟合向量:
C1 -1(S1-S2S4 -1S2 T)α1=λα1
α2=-S4 -1S2 Tα1;
计算出α1后,通过上述公式计算出α2,则椭球拟合向量的所有参数全部被计算出,其中,α=[α1 α2]=[a b c d e f k l m n]T;
再根据实际的物理对应关系,三轴的刻度系数kx,ky,kz、轴间不正交角δxy,δyz,δxz及残余零偏bx,by,bz可以通过以下公式计算得出:
上述公式中,α1为矩阵C1 -1(S1-S2S4 -1S2 T)最大特征值对应的特征向量,因此在实际计算中,不用将所有特征值和特征向量全部计算出来,甚至不用计算特征值,而只要利用幂法或者反幂法进行计算出最大的特征向量即可,以反幂法为例:
令A=(C1 -1(S1-S2S4 -1S2 T))-1,取一个初始迭代向量u=[1 1 1]T,设置迭代精度ε=1e-6;
对A做LU分解,即A=LU;
解线性方程组:
Ly(k)=u(k-1),Uv(k)=y(k)
mk=max(v(k)),u(k)=v(k)/mk
若|mk-mk-1|<ε,则跳出迭代,停止计算,此时α1=u(k),否则k=k+1,继续计算。
一般而言,对于ε=1e-6,5次以下的迭代计算即可达到此精度。
步骤S7:利用计算出的椭球参数对电子罗盘原始数据进行补偿。
步骤S8:当采集到新的电子罗盘数据后,可以用椭球参数对新采集到的数据进行修正。
步骤S9:根据补偿后的加速度计数据与电子罗盘数据对姿态进行解算。
实施例6:
如图6所示,一种姿态测量系统的精度校准方法,所述姿态测量系统的精度校准方法包括如下步骤:
步骤S1:对加速度计进行水平校准,消除加速度计原始零偏;
将姿态测量设备通过水平气泡调整到水平位置,对加速度计进行水平校准,去除加速度计的零偏。
步骤S2:采集加速度计在一段时间内的三轴数据,其中,三轴数据标记为[AX0、AY0、AZ0]T,零偏标记为B=[AX0、AY0、AZ0]T–[0 0 g]T。
步骤S3:通过椭球拟合模型对姿态测量系统的加速度计的零偏、刻度系数与轴间不正交角进行校准;
所述步骤S3:通过椭球拟合模型对姿态测量系统的加速度计的零偏、刻度系数与轴间不正交角进行校准步骤包括以下步骤:
设定拟合椭球参数向量,标记为α=[a b c d e f k l m n]T;
设定校准过程迭代次数为r,将加速度采集的第一个值[AX0、AY0、AZ0]T拓展为10维列向量D1,并计算矩阵S1,其中,D1标记为D1=[Ax1 2 Ay1 2 Az1 2 Ax1Ay1 Ax1Az1 Ay1Az1 Ax1 Ay1Az1 1]T,S1=D1TD1;
若加速度计三轴数据的采集次数未达到r次,则用新采集的加速度计数据DK对矩阵S进行更新,其中,Sk=Sk-1+Dk TDk;
计算出矩阵S后,同时引入椭球限制矩阵C并将矩阵S分块,其中,
将矩阵S分块:
Dk=[D1 D2]
D1=[Ax1 2 Ay1 2 Az1 2]
D2=[Ax1Ay1 Ay1Az1 Ax1Az1 Ax1 Ay1 Az1 1]
通过以下公式求得椭球拟合向量:
C1 -1(S1-S2S4 -1S2 T)α1=λα1
α2=-S4 -1S2 Tα1;
计算出α1后,通过上述公式计算出α2,则椭球拟合向量的所有参数全部被计算出,其中,α=[α1 α2]=[a b c d e f k l m n]T;
再根据实际的物理对应关系,三轴的刻度系数kx,ky,kz、轴间不正交角δxy,δyz,δxz及残余零偏bx,by,bz可以通过以下公式计算得出:
上述公式中,α1为矩阵C1 -1(S1-S2S4 -1S2 T)最大特征值对应的特征向量,因此在实际计算中,不用将所有特征值和特征向量全部计算出来,甚至不用计算特征值,而只要利用幂法或者反幂法进行计算出最大的特征向量即可,以反幂法为例:
令A=(C1 -1(S1-S2S4 -1S2 T))-1,取一个初始迭代向量u=[1 1 1]T,设置迭代精度ε=1e-6;
对A做LU分解,即A=LU;
解线性方程组:
Ly(k)=u(k-1),Uv(k)=y(k)
mk=max(v(k)),u(k)=v(k)/mk
若|mk-mk-1|<ε,则跳出迭代,停止计算,此时α1=u(k),否则k=k+1,继续计算。
一般而言,对于ε=1e-6,5次以下的迭代计算即可达到此精度。
步骤S4:当采集到新的加速度计数据后,可以用椭球参数对新采集到的数据进行修正。
步骤S5:利用计算出的椭球参数对加速度计原始数据进行补偿。
步骤S6:通过椭球拟合模型对电子罗盘进行准;
所述步骤S6:通过椭球拟合模型对电子罗盘进行准步骤包括如下步骤:
设定拟合椭球参数向量,标记为α=[a b c d e f k l m n]T;
采集电子罗盘在一段时间内的三轴数据,并对采集的数据进行运算防溢处理,其中,三轴数据标记为[HX0 HY0 HZ0]T;
设定校准过程迭代次数为r,将电子罗盘采集的第一个值[HX1 HY1 HZ1]T拓展为10维列向量D1,并计算矩阵S1,其中,D1标记为D1=[Hx1 2 Hy1 2 Hz1 2 Hx1Hy1 Hx1Hz1 Hy1Hz1 Hx1 Hy1Hz1 1]T,S1=D1TD1;
若电子罗盘三轴数据的采集次数未达到r次,则用新采集的电子罗盘数据DK对矩阵S进行更新,其中,Sk=Sk-1+Dk TDk;
计算出矩阵S后,同时引入椭球限制矩阵C并将矩阵S分块,其中,
将矩阵S分块:
Dk=[D1 D2]
D2=[Hx1Hy1 Hx1Hz1 Hy1Hz1 Hx1 Hy1 Hz1 1]
通过以下公式求得椭球拟合向量:
C1 -1(S1-S2S4 -1S2 T)α1=λα1
α2=-S4 -1S2 Tα1;
计算出α1后,通过上述公式计算出α2,则椭球拟合向量的所有参数全部被计算出,其中,α=[α1 α2]=[a b c d e f k l m n]T;
再根据实际的物理对应关系,三轴的刻度系数kx,ky,kz、轴间不正交角δxy,δyz,δxz及残余零偏bx,by,bz可以通过以下公式计算得出:
上述公式中,α1为矩阵C1 -1(S1-S2S4 -1S2 T)最大特征值对应的特征向量,因此在实际计算中,不用将所有特征值和特征向量全部计算出来,甚至不用计算特征值,而只要利用幂法或者反幂法进行计算出最大的特征向量即可,以反幂法为例:
令A=(C1 -1(S1-S2S4 -1S2 T))-1,取一个初始迭代向量u=[1 1 1]T,设置迭代精度ε=1e-6;
对A做LU分解,即A=LU;
解线性方程组:
Ly(k)=u(k-1),Uv(k)=y(k)
mk=max(v(k)),u(k)=v(k)/mk
若|mk-mk-1|<ε,则跳出迭代,停止计算,此时α1=u(k),否则k=k+1,继续计算。
一般而言,对于ε=1e-6,5次以下的迭代计算即可达到此精度。
步骤S7:利用计算出的椭球参数对电子罗盘原始数据进行补偿。
步骤S8:当采集到新的电子罗盘数据后,可以用椭球参数对新采集到的数据进行修正。
步骤S9:根据补偿后的加速度计数据与电子罗盘数据对姿态进行解算。
步骤S10:电子罗盘在测量过程中利用测量数据对椭球参数进行更新,提高椭球参数的准确性。
实施例7:
如图7所示,一种姿态测量系统的精度校准方法,所述姿态测量系统的精度校准方法包括如下步骤:
步骤S1:对加速度计进行水平校准,消除加速度计原始零偏;
将姿态测量设备通过水平气泡调整到水平位置,对加速度计进行水平校准,去除加速度计的零偏。
步骤S2:采集加速度计在一段时间内的三轴数据,其中,三轴数据标记为[AX0、AY0、AZ0]T,零偏标记为B=[AX0、AY0、AZ0]T–[0 0 g]T。
步骤S3:通过椭球拟合模型对姿态测量系统的加速度计的零偏、刻度系数与轴间不正交角进行校准;
所述步骤S3:通过椭球拟合模型对姿态测量系统的加速度计的零偏、刻度系数与轴间不正交角进行校准步骤包括以下步骤:
设定拟合椭球参数向量,标记为α=[a b c d e f k l m n]T;
设定校准过程迭代次数为r,将加速度采集的第一个值[AX0、AY0、AZ0]T拓展为10维列向量D1,并计算矩阵S1,其中,D1标记为D1=[Ax1 2 Ay1 2 Az1 2 Ax1Ay1 Ax1Az1 Ay1Az1 Ax1 Ay1Az1 1]T,S1=D1TD1;
若加速度计三轴数据的采集次数未达到r次,则用新采集的加速度计数据DK对矩阵S进行更新,其中,Sk=Sk-1+Dk TDk;
计算出矩阵S后,同时引入椭球限制矩阵C并将矩阵S分块,其中,
将矩阵S分块:
Dk=[D1 D2]
D1=[Ax1 2 Ay1 2 Az1 2]
D2=[Ax1Ay1 Ay1Az1 Ax1Az1 Ax1 Ay1 Az1 1]
通过以下公式求得椭球拟合向量:
C1 -1(S1-S2S4 -1S2 T)α1=λα1
α2=-S4 -1S2 Tα1;
计算出α1后,通过上述公式计算出α2,则椭球拟合向量的所有参数全部被计算出,其中,α=[α1 α2]=[a b c d e f k l m n]T;
再根据实际的物理对应关系,三轴的刻度系数kx,ky,kz、轴间不正交角δxy,δyz,δxz及残余零偏bx,by,bz可以通过以下公式计算得出:
上述公式中,α1为矩阵C1 -1(S1-S2S4 -1S2 T)最大特征值对应的特征向量,因此在实际计算中,不用将所有特征值和特征向量全部计算出来,甚至不用计算特征值,而只要利用幂法或者反幂法进行计算出最大的特征向量即可,以反幂法为例:
令A=(C1 -1(S1-S2S4 -1S2 T))-1,取一个初始迭代向量u=[1 1 1]T,设置迭代精度ε=1e-6;
对A做LU分解,即A=LU;
解线性方程组:
Ly(k)=u(k-1),Uv(k)=y(k)
mk=max(v(k)),u(k)=v(k)/mk
若|mk-mk-1|<ε,则跳出迭代,停止计算,此时α1=u(k),否则k=k+1,继续计算。
一般而言,对于ε=1e-6,5次以下的迭代计算即可达到此精度。
步骤S4:当采集到新的加速度计数据后,可以用椭球拟合参数对新采集到的数据进行修正。
步骤S5:利用计算出的椭球参数对加速度计原始数据进行补偿。
步骤S6:通过椭球拟合模型对电子罗盘进行准;
所述步骤S6:通过椭球拟合模型对电子罗盘进行准步骤包括如下步骤:
设定拟合椭球参数向量,标记为α=[a b c d e f k l m n]T;
采集电子罗盘在一段时间内的三轴数据,并对采集的数据进行运算防溢处理,其中,三轴数据标记为[HX0 HY0 HZ0]T;
设定校准过程迭代次数为r,将电子罗盘采集的第一个值[HX1 HY1 HZ1]T拓展为10维列向量D1,并计算矩阵S1,其中,D1标记为D1=[Hx1 2 Hy1 2 Hz1 2 Hx1Hy1 Hx1Hz1 Hy1Hz1 Hx1 Hy1Hz1 1]T,S1=D1TD1;
若电子罗盘三轴数据的采集次数未达到r次,则用新采集的电子罗盘数据DK对矩阵S进行更新,其中,Sk=Sk-1+Dk TDk;
计算出矩阵S后,同时引入椭球限制矩阵C并将矩阵S分块,其中,
将矩阵S分块:
Dk=[D1 D2]
D2=[Hx1Hy1 Hx1Hz1 Hy1Hz1 Hx1 Hy1 Hz1 1]
通过以下公式求得椭球拟合向量:
C1 -1(S1-S2S4 -1S2 T)α1=λα1
α2=-S4 -1S2 Tα1;
计算出α1后,通过上述公式计算出α2,则椭球拟合向量的所有参数全部被计算出,其中,α=[α1 α2]=[a b c d e f k l m n]T;
再根据实际的物理对应关系,三轴的刻度系数kx,ky,kz、轴间不正交角δxy,δyz,δxz及残余零偏bx,by,bz可以通过以下公式计算得出:
上述公式中,α1为矩阵C1 -1(S1-S2S4 -1S2 T)最大特征值对应的特征向量,因此在实际计算中,不用将所有特征值和特征向量全部计算出来,甚至不用计算特征值,而只要利用幂法或者反幂法进行计算出最大的特征向量即可,以反幂法为例:
令A=(C1 -1(S1-S2S4 -1S2 T))-1,取一个初始迭代向量u=[1 1 1]T,设置迭代精度ε=1e-6;
对A做LU分解,即A=LU;
解线性方程组:
Ly(k)=u(k-1),Uv(k)=y(k)
mk=max(v(k)),u(k)=v(k)/mk
若|mk-mk-1|<ε,则跳出迭代,停止计算,此时α1=u(k),否则k=k+1,继续计算。
一般而言,对于ε=1e-6,5次以下的迭代计算即可达到此精度。
步骤S7:利用计算出的椭球参数对电子罗盘原始数据进行补偿。
步骤S8:当采集到新的电子罗盘数据后,可以用椭球拟数对新采集到的数据进行修正。
步骤S9:根据补偿后的加速度计数据与电子罗盘数据对姿态进行解算。
步骤S10:通过统计规则的方式剔除采集的电子罗盘数据中出现跳变的计算点。
步骤S11:电子罗盘在测量过程中利用测量数据对椭球参数进行更新,提高椭球参数的准确性。
本发明实施的优点:通过采用了椭球拟合的方式对姿态测量系统中的电子罗盘和加速度进行校准,可有效地避免传感器偶然输出异常、跳数等问题,使得校准结果的可靠性得到了有效的保证;对椭球拟合算法进行了递推化处理,使得姿态测量系统内部CPU在处理数据时不需要存储传感器采集的过往所有点的数据,而仅需要存储之前递推得到的矩阵和当次的测量结果即可,这使得在相同的硬件环境下,该椭球拟合方法可以处理更多的数值,从而提高椭球拟合的精度。相对应的,采用传统拟合算法时,CPU为了确保运算过程中内存栈不会溢出,只能根据少量的样本点做拟合运算,因而拟合精度有限;对椭球拟合算法中大规模的矩阵运算进行了改进,优化了其中的计算流程,使得椭球拟合的计算量减少90%以上,以Matlab程序为例,按照传统算法,运算一次拟合算法耗时563ms;相对地,采用改进后的算法耗时仅为14ms;采用了利用测量数据进行校准的策略,提高了校准时的容错能力,从而降低了初始校准时的要求,减少了使用人员的操作复杂程度。
以上所述,仅为本发明的具体实施方式,但本发明的保护范围并不局限于此,任何熟悉本领域技术的技术人员在本发明公开的技术范围内,可轻易想到的变化或替换,都应涵盖在本发明的保护范围之内。因此,本发明的保护范围应以所述权利要求的保护范围为准。
Claims (1)
1.一种姿态测量系统的精度校准方法,其特征在于,所述姿态测量系统的精度校准方法包括如下步骤:
通过椭球拟合模型对姿态测量系统的加速度计的零偏、刻度系数与轴间不正交角进行校准;
所述通过椭球拟合模型对姿态测量系统的加速度计的零偏、刻度系数与轴间不正交角进行校准步骤包括以下步骤:
设定拟合椭球参数向量,标记为α=[a b c d e f k l m n]T;
设定校准过程迭代次数为r,将加速度采集的第一个值[AX0、AY0、AZ0]T拓展为10维列向量D1,并计算矩阵S1,其中,D1标记为 S1=D1TD1;
若加速度计三轴数据的采集次数未达到r次,则用新采集的加速度计数据DK对矩阵S进行更新,其中,Sk=Sk-1+Dk TDk;
计算出更新的矩阵S后,同时引入椭球限制矩阵C并将矩阵S分块,其中,
C2=03×7,C3=07×3,C4=07×7
将矩阵S分块:
Dk=[D1 D2]
D1=[Ax1 2 Ay1 2 Az1 2]
D2=[Ax1Ay1 Ay1Az1 Ax1Az1 Ax1 Ay1 Az1 1]
通过以下公式求得椭球拟合向量:
C1 -1(S1-S2S4 -1S2 T)α1=λα1
α2=-S4 -1S2 Tα1;
计算出α1后,通过上述公式计算出α2,则椭球拟合向量的所有参数全部被计算出,其中,α=[α1 α2]=[a b c d e f k l m n]T;
再根据实际的物理对应关系,三轴的刻度系数kx,ky,kz、轴间不正交角δxy,δyz,δxz及残余零偏bx,by,bz可以通过以下公式计算得出:
采集加速度计在一段时间内的三轴数据,其中,三轴数据标记为[AX0、AY0、AZ0]T,零偏标记为B=[AX0、AY0、AZ0]T–[0 0 g]T;
当采集到新的加速度计数据后,可以用椭球参数对新采集到的数据进行修正;
利用计算出的椭球参数对加速度计原始数据进行补偿;
通过椭球拟合模型对姿态测量系统的电子罗盘进行校准;
所述通过椭球拟合模型对姿态测量系统的电子罗盘进行校准包括以下步骤:
设定拟合椭球参数向量,标记为α=[a b c d e f k l m n]T;
采集电子罗盘在一段时间内的三轴数据,并对采集的数据进行运算防溢处理,其中,三轴数据标记为[HX0 HY0 HZ0]T;
设定校准过程迭代次数为r,将电子罗盘采集的第一个值[HX1 HY1HZ1]T拓展为10维列向量D1,并计算矩阵S1,其中,D1标记为S1=D1TD1;
若电子罗盘三轴数据的采集次数未达到r次,则用新采集的电子罗盘数据DK对矩阵S进行更新,其中,Sk=Sk-1+Dk TDk;
计算出更新的矩阵S后,同时引入椭球限制矩阵C并将矩阵S分块,其中,
C2=03×7,C3=07×3,C4=07×7
将矩阵S分块:
Dk=[D1 D2]
D2=[Hx1Hy1 Hx1Hz1 Hy1Hz1 Hx1 Hy1 Hz1 1]
通过以下公式求得椭球拟合向量:
C1 -1(S1-S2S4 -1S2 T)α1=λα1
α2=-S4 -1S2 Tα1;
计算出α1后,通过上述公式计算出α2,则椭球拟合向量的所有参数全部被计算出,其中,α=[α1 α2]=[a b c d e f k l m n]T;
再根据实际的物理对应关系,三轴的刻度系数kx,ky,kz、轴间不正交角δxy,δyz,δxz及残余零偏bx,by,bz可以通过以下公式计算得出:
利用计算出的椭球参数对电子罗盘原始数据进行补偿;
根据补偿后的加速度计数据与电子罗盘数据对姿态进行解算。
Priority Applications (6)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201510665021.XA CN105352487B (zh) | 2015-10-13 | 2015-10-13 | 一种姿态测量系统的精度校准方法 |
PCT/CN2016/088303 WO2017063386A1 (zh) | 2015-10-13 | 2016-07-04 | 一种姿态测量系统的精度校准方法 |
EP16854769.3A EP3364151B1 (en) | 2015-10-13 | 2016-07-04 | Precision calibration method for attitude measurement system |
RU2017125953A RU2662458C1 (ru) | 2015-10-13 | 2016-07-04 | Способ прецизионной калибровки системы измерения пространственного положения |
KR1020177023778A KR102008597B1 (ko) | 2015-10-13 | 2016-07-04 | 자세 측정 시스템의 정밀 보정 방법 |
US15/542,931 US10605619B2 (en) | 2015-10-13 | 2016-07-04 | Precision calibration method of attitude measuring system |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201510665021.XA CN105352487B (zh) | 2015-10-13 | 2015-10-13 | 一种姿态测量系统的精度校准方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN105352487A CN105352487A (zh) | 2016-02-24 |
CN105352487B true CN105352487B (zh) | 2018-06-15 |
Family
ID=55328497
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201510665021.XA Active CN105352487B (zh) | 2015-10-13 | 2015-10-13 | 一种姿态测量系统的精度校准方法 |
Country Status (6)
Country | Link |
---|---|
US (1) | US10605619B2 (zh) |
EP (1) | EP3364151B1 (zh) |
KR (1) | KR102008597B1 (zh) |
CN (1) | CN105352487B (zh) |
RU (1) | RU2662458C1 (zh) |
WO (1) | WO2017063386A1 (zh) |
Families Citing this family (20)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN105352487B (zh) | 2015-10-13 | 2018-06-15 | 上海华测导航技术股份有限公司 | 一种姿态测量系统的精度校准方法 |
CN106556383B (zh) * | 2016-12-02 | 2019-05-07 | 上海华测导航技术股份有限公司 | 一种rtk倾斜补偿测量精度验证的方法 |
CN106979775B (zh) * | 2017-04-28 | 2019-07-30 | 江苏号百信息服务有限公司 | 一种电子罗盘阶跃时平滑滤波的方法 |
CN107479079A (zh) * | 2017-07-13 | 2017-12-15 | 临沂大学 | 一种基于pdr与led结合的方法 |
CN107656227B (zh) * | 2017-09-21 | 2019-10-11 | 大连理工大学 | 基于Levenberg-Marquardt算法的磁力计校准方法 |
DE102017220867A1 (de) * | 2017-11-22 | 2019-05-23 | Robert Bosch Gmbh | Verfahren zum automatischen Kalibrieren und Verfahren zum Verwenden eines Beschleunigungssensors |
US20190346897A1 (en) * | 2018-05-13 | 2019-11-14 | Sean Joseph Rostami | Introspective Power Method |
CN109188422B (zh) * | 2018-08-08 | 2023-01-06 | 中国航空工业集团公司雷华电子技术研究所 | 一种基于lu分解的卡尔曼滤波目标跟踪方法 |
CN109470277B (zh) * | 2018-12-26 | 2022-09-13 | 湖南航天机电设备与特种材料研究所 | 非正交角度测量装置标定系数的测定方法及系统 |
CN112146678B (zh) * | 2019-06-27 | 2022-10-11 | 华为技术有限公司 | 一种确定校准参数的方法及电子设备 |
CN111765879A (zh) * | 2019-11-29 | 2020-10-13 | 深圳市瑞芬科技有限公司 | 一种三维电子罗盘装置及实用校准方法 |
CN112325901B (zh) * | 2020-09-28 | 2022-09-16 | 中国船舶重工集团公司第七0七研究所 | 一种平台式惯导系泊状态下计算方位陀螺仪标度的方法 |
CN112284366B (zh) * | 2020-10-26 | 2022-04-12 | 中北大学 | 一种基于tg-lstm神经网络的偏振光罗盘航向角误差校正方法 |
CN112284421B (zh) * | 2020-11-02 | 2021-03-19 | 蘑菇车联信息科技有限公司 | 一种imu内参调整方法及相关装置 |
CN112596015A (zh) * | 2020-12-28 | 2021-04-02 | 上海矽睿科技有限公司 | 三轴磁传感器的测试方法及系统 |
CN113124905B (zh) * | 2021-04-27 | 2022-10-28 | 西安电子科技大学 | 一种用于多轴惯性姿态传感器精度评估的自动测量方法 |
CN113985494A (zh) * | 2021-10-13 | 2022-01-28 | 哈尔滨工程大学 | 一种基于无迹卡尔曼算法海底地震计中电子罗盘误差补偿方法 |
CN114264997B (zh) * | 2021-12-14 | 2024-03-22 | 武汉联影生命科学仪器有限公司 | 梯度灵敏度校准方法、装置及磁共振设备 |
CN114485728A (zh) * | 2022-01-04 | 2022-05-13 | 中国煤炭科工集团太原研究院有限公司 | 一种捷联惯导系统现场快速自标定方法 |
CN116224468B (zh) * | 2023-05-10 | 2023-08-22 | 华中光电技术研究所(中国船舶集团有限公司第七一七研究所) | 一种陆地重力仪标定方法及标定系数 |
Family Cites Families (19)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JPH0625671B2 (ja) * | 1987-08-28 | 1994-04-06 | 日本航空電子工業株式会社 | 慣性航法装置 |
RU2098764C1 (ru) * | 1996-05-29 | 1997-12-10 | Русланов Александр Семенович | Способ определения местоположения подвижных объектов и устройство для его реализации |
WO1998048292A2 (en) * | 1997-01-31 | 1998-10-29 | Greenfield Enterprises, Inc. | Navigation system and method |
US7376507B1 (en) * | 2004-05-27 | 2008-05-20 | Sandia Corporation | Geophysics-based method of locating a stationary earth object |
US7275008B2 (en) * | 2005-09-02 | 2007-09-25 | Nokia Corporation | Calibration of 3D field sensors |
US7451549B1 (en) * | 2006-08-09 | 2008-11-18 | Pni Corporation | Automatic calibration of a three-axis magnetic compass |
US8005635B2 (en) * | 2007-08-14 | 2011-08-23 | Ching-Fang Lin | Self-calibrated azimuth and attitude accuracy enhancing method and system (SAAAEMS) |
DE102008042989A1 (de) * | 2008-10-21 | 2010-04-22 | Robert Bosch Gmbh | Elektronischer Kompass |
US8645093B2 (en) * | 2009-11-04 | 2014-02-04 | Qualcomm Incorporated | Calibrating multi-dimensional sensor for offset, sensitivity, and non-orthogonality |
CN102252689A (zh) * | 2010-05-19 | 2011-11-23 | 北京国浩传感器技术研究院(普通合伙) | 一种基于磁传感器的电子罗盘校准方法 |
US20130185018A1 (en) * | 2010-10-01 | 2013-07-18 | Hua Sheng | Apparatuses and Methods for Estimating the Yaw Angle of a Device in a Gravitational Reference System Using Measurements of Motion Sensors and a Magnetometer Attached to the Device |
KR101209571B1 (ko) * | 2010-10-20 | 2012-12-07 | 한국과학기술연구원 | 자동 교정 방법 및 장치 |
US9810549B2 (en) * | 2011-01-06 | 2017-11-07 | University Of Utah Research Foundation | Systems, methods, and apparatus for calibration of and three-dimensional tracking of intermittent motion with an inertial measurement unit |
US9541393B2 (en) * | 2011-06-30 | 2017-01-10 | Qualcomm Incorporated | Reducing power consumption or error of digital compass |
US9683865B2 (en) * | 2012-01-26 | 2017-06-20 | Invensense, Inc. | In-use automatic calibration methodology for sensors in mobile devices |
KR101503046B1 (ko) * | 2013-08-22 | 2015-03-24 | 한국과학기술연구원 | 다축 감지 장치 및 이의 교정 방법 |
CN103776451B (zh) | 2014-03-04 | 2016-11-09 | 哈尔滨工业大学 | 一种基于mems的高精度三维姿态惯性测量系统以及测量方法 |
CN104898681B (zh) * | 2015-05-04 | 2017-07-28 | 浙江工业大学 | 一种采用三阶近似毕卡四元数的四旋翼飞行器姿态获取方法 |
CN105352487B (zh) * | 2015-10-13 | 2018-06-15 | 上海华测导航技术股份有限公司 | 一种姿态测量系统的精度校准方法 |
-
2015
- 2015-10-13 CN CN201510665021.XA patent/CN105352487B/zh active Active
-
2016
- 2016-07-04 EP EP16854769.3A patent/EP3364151B1/en active Active
- 2016-07-04 US US15/542,931 patent/US10605619B2/en active Active
- 2016-07-04 RU RU2017125953A patent/RU2662458C1/ru not_active IP Right Cessation
- 2016-07-04 KR KR1020177023778A patent/KR102008597B1/ko active IP Right Grant
- 2016-07-04 WO PCT/CN2016/088303 patent/WO2017063386A1/zh active Application Filing
Also Published As
Publication number | Publication date |
---|---|
CN105352487A (zh) | 2016-02-24 |
EP3364151B1 (en) | 2020-11-25 |
EP3364151A4 (en) | 2019-06-19 |
RU2662458C1 (ru) | 2018-07-26 |
WO2017063386A1 (zh) | 2017-04-20 |
KR102008597B1 (ko) | 2019-08-07 |
US20180010923A1 (en) | 2018-01-11 |
US10605619B2 (en) | 2020-03-31 |
EP3364151A1 (en) | 2018-08-22 |
KR20170105619A (ko) | 2017-09-19 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN105352487B (zh) | 一种姿态测量系统的精度校准方法 | |
CN108827299B (zh) | 一种基于改进四元数二阶互补滤波的飞行器姿态解算方法 | |
CN104567871B (zh) | 一种基于地磁梯度张量的四元数卡尔曼滤波姿态估计方法 | |
CN109974714A (zh) | 一种Sage-Husa自适应无迹卡尔曼滤波姿态数据融合方法 | |
CN110361683A (zh) | 基于双目标函数粒子群优化的磁力计校正方法 | |
JP5469670B2 (ja) | 地磁気検出装置 | |
CN103808349B (zh) | 矢量传感器的误差校正方法和装置 | |
CN106019207A (zh) | 一种电能计量校准方法 | |
CN106767671B (zh) | 基于三维电子罗盘的地质结构面产状计算方法 | |
CN108645404A (zh) | 一种小型多旋翼无人机姿态解算方法 | |
CN107132155A (zh) | 基于实测大气密度的短时大气模型修正方法 | |
CN107749627A (zh) | 基于改进匹配追踪的智能配电网潮流雅可比矩阵估计方法 | |
CN103472262A (zh) | 量程可调式mems加速度计的参数标定方法 | |
CN107356786B (zh) | 加速度计的校准方法和装置、计算机可读存储介质 | |
CN106092140B (zh) | 一种陀螺仪零偏估计方法 | |
CN112632454A (zh) | 一种基于自适应卡尔曼滤波算法的mems陀螺滤波方法 | |
CN107389092A (zh) | 一种基于磁传感器辅助的陀螺标定方法 | |
CN106501868A (zh) | 三轴地磁传感器实时校正方法 | |
CN106483079A (zh) | 水汽中氢氧稳定同位素比率的测量数据的处理方法和装置 | |
CN105842481A (zh) | 一种惯性导航系统中基于十字叉的三轴加速度计标定方法 | |
CN109737940A (zh) | 一种电子罗盘实时自由校准方法及装置 | |
Dan et al. | Application Of Strongly Tracking Kalman Filter In MEMS Gyroscope Bias Compensation | |
CN109990776B (zh) | 一种姿态测量方法及装置 | |
Zhang et al. | Implementation and complexity analysis of orientation estimation algorithms for human body motion tracking using low-cost sensors | |
CN110823255A (zh) | 一种基于比力观测无需调平引北的系统级自标定方法 |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
C06 | Publication | ||
PB01 | Publication | ||
C10 | Entry into substantive examination | ||
SE01 | Entry into force of request for substantive examination | ||
GR01 | Patent grant | ||
GR01 | Patent grant |