CN109001787A

CN109001787A - 一种姿态角解算与定位的方法及其融合传感器

Info

Publication number: CN109001787A
Application number: CN201810517173.9A
Authority: CN
Inventors: 时广轶; 黄亚华
Original assignee: Peking University Shenzhen Graduate School
Current assignee: Peking University Shenzhen Graduate School
Priority date: 2018-05-25
Filing date: 2018-05-25
Publication date: 2018-12-14
Anticipated expiration: 2038-05-25
Also published as: CN109001787B

Abstract

一种姿态角解算与定位的方法及其融合传感器，该融合传感器包括多个IMU传感器、一个磁力计和GPS；该方法包括：校正磁力计、加速度计和陀螺仪；分别利用冗余信息融合算法对各加速度计和各陀螺仪的测量数据进行数据融合，得到加速度信息和角速度信息；利用扩展卡尔曼滤波算法融合加速度信息、角速度信息和磁力计的磁场信息得到融合姿态角；以融合姿态角辅助定位信息得到位置信息。由于使用多冗余IMU传感器进行数据测量和数据融合计算，且利用扩展卡尔曼滤波算法融合各数据得到融合姿态角，在静态和动态条件下都能获得较高精度的姿态角；同时利用该融合姿态角辅助GPS得到补正的位置信息，使位置信息更精确，提高了姿态角解算与定位的稳定性和可靠性。

Description

一种姿态角解算与定位的方法及其融合传感器

技术领域

本发明涉及组合导航技术领域，具体涉及一种姿态角解算与定位的方法及其融合传感器。

背景技术

上世纪70年代起，导航技术获得了迅猛的发展，随着信息技术和经济建设的发展，交通运输、工业、农业、测绘等领域对导航技术提出了更加多样性和更高的要求。

目前，在导航技术的定姿与定位方面，应用最为广泛的是组合导航技术。组合导航是指利用GPS(Global Positioning System，全球定位***)、无线电导航、天文导航、卫星导航等***中的一个或几个与INS(Inertial Navigation System，惯性导航***)组合在一起，形成的综合导航***。INS是以陀螺仪和加速度计为敏感器件的导航参数解算***，该***根据陀螺仪的输出建立导航坐标系，根据加速度计输出解算出运载体在导航坐标系中的速度和位置。

在现有的组合导航技术中，GPS和INS的组合是最常见的方式之一。但现有的GPS和INS的组合导航技术极大地依赖于GPS信号，而GPS信号的隐蔽性极差，信号易被干扰和遮蔽，从而导致导航精度的极大下降；并且，GPS接收机的工作受飞行器的机动的影响，当飞行器的机动超过GPS接收机的动态范围时，接收机会失锁而不能捕获和跟踪卫星信号，从而无法进行工作，这样，在对可靠性要求较高的场合，GPS导航会受到限制。同时，在现有的GPS和INS的组合导航技术中，INS是以陀螺仪和加速度计为敏感器件，利用互补滤波融合算法进行姿态角解算的，计算复杂度较高，且在动态条件下获得的姿态角的误差较大。

因此，利用现有的组合导航技术进行姿态角的解算和定位时，获得的姿态角信息和位置信息的精度不高，定姿与定位的稳定性和可靠性较低。

发明内容

本申请提供一种姿态角解算与定位的方法及其融合传感器，以获得高精度与高稳定性的姿态角信息和位置信息，提高姿态角解算与定位的稳定性和可靠性。

根据第一方面，一种实施例中提供一种姿态角解算与定位的方法，应用于姿态角解算与定位的融合传感器中，所述融合传感器包含至少两个IMU传感器、一个磁力计和GPS，一个所述IMU传感器包含一个加速度计和一个陀螺仪，所述方法包括：

获取各加速度计和各陀螺仪的测量数据以及磁力计的磁场信息和GPS的定位信息；

分别根据各加速度计的测量数据之间的置信度和各陀螺仪的测量数据之间的置信度，利用冗余信息融合算法分别对各加速度计的测量数据和各陀螺仪的测量数据进行数据融合，得到加速度信息和角速度信息；

以所述角速度信息为状态变量且所述加速度信息和所述磁场信息为观测量，利用扩展卡尔曼滤波算法计算得到融合姿态角；

根据所述加速度信息、所述角速度信息、所述融合姿态角和所述定位信息，利用扩展卡尔曼滤波算法计算出位置信息。

根据第二方面，一种实施例中提供一种姿态角解算与定位的融合传感器，包括：至少两个IMU传感器、一个磁力计、GPS和数据处理器，一个IMU传感器包含一个加速度计和一个陀螺仪，所述至少两个IMU传感器采用双面反轴型方式或单轴多传感器方式连接；

所述双面反轴型方式为2f个IMU传感器被分为两组，分别安装于平板两侧，且两侧的IMU传感器一轴反向、另外两轴同向，所述f为大于等于1的整数；

所述单轴多传感器方式为各正交轴上分别安装多个IMU传感器，且各IMU传感器的各轴向相同；

所述加速度计用于测量载体的加速度；

所述陀螺仪用于测量载体的角速度；

所述磁力计用于测量磁场信息；

所述GPS用于测量定位信息；

所述数据处理器用于获取各加速度计和各陀螺仪的测量数据以及磁力计的磁场信息和GPS的定位信息，分别根据各加速度计的测量数据之间的置信度和各陀螺仪的测量数据之间的置信度，利用冗余信息融合算法分别对各加速度计的测量数据和各陀螺仪的测量数据进行数据融合，得到加速度信息和角速度信息，以所述角速度信息为状态量且所述加速度信息和所述磁场信息为观测量，利用扩展卡尔曼滤波算法计算得到融合姿态角，根据所述加速度信息、所述角速度信息、所述融合姿态角和所述定位信息，利用扩展卡尔曼滤波算法计算出位置信息。

依据上述实施例的姿态角解算与定位的方法及其融合传感器，通过多个IMU传感器的冗余信息融合算法进行加速度信息和角速度信息的计算，使得计算出的加速度信息和角速度信息更加准确，在此基础，以磁力计作为辅助，利用扩展卡尔曼滤波算法融合该加速度信息、角速度信息以及磁力计的磁场信息得到融合姿态角，从而极大地提高了***的精度与稳定性，在静态和动态条件下都能获得较高精度的融合姿态角；同时，最终的位置信息是由较高精度的融合姿态角辅助GPS的定位信息得到的，对GPS的定位信息进行了补正，使得得到的位置信息更加精确；由此便可获得高精度与高稳定性的姿态角信息和位置信息，提高了姿态角解算与定位的稳定性和可靠性。

附图说明

图1为本发明实施例中姿态角解算与定位的融合传感器的结构示意图；

图2为本发明实施例中IMU传感器通过双面反轴型方式连接的结构示意图；

图3为本发明实施例中IMU传感器通过单轴多传感器方式连接的结构示意图；

图4为本发明实施例中姿态角解算与定位的方法的流程图；

图5为本发明实施例中采用Allan方差法修正陀螺仪零偏误差的方法的流程图；

图6为本发明实施例中利用冗余信息融合算法对加速度计的测量数据进行数据融合的方法的流程图；

图7为本发明实施例中计算融合姿态角的方法的流程图；

图8为本发明实施例中利用扩展卡尔曼滤波递推方程计算融合姿态角的流程图；

图9为本发明实施例中利用扩展卡尔曼滤波算法计算位置信息的流程图。

具体实施方式

下面通过具体实施方式结合附图对本发明作进一步详细说明。

在本发明实施例中，采用多冗余(至少两个)IMU传感器测量载体的加速度和角速度，并利用冗余信息融合算法分别对各加速度计和各陀螺仪的测量数据进行数据融合，得到融合的加速度信息和角速度信息；接着以磁力计作为辅助，利用扩展卡尔曼滤波算法对该加速度信息、角速度信息以及磁力计测得的磁场信息进行信息融合，得到融合姿态角；然后使用该融合姿态角辅助GPS测得的定位信息，利用扩展卡尔曼滤波算法计算得到位置信息。

本发明实施例提供一种姿态角解算与定位的融合传感器，请参考图1，图1示出了该姿态角解算与定位的融合传感器的结构示意图，如图1所示，该姿态角解算与定位的融合传感器包括N个IMU传感器1、一个磁力计2、GPS 3和数据处理器4，其中，N为大于或等于2的整数；其中的IMU传感器1包含一个加速度计11和一个陀螺仪12，加速度计11用于测量载体的加速度，陀螺仪12用于测量载体的角速度，N个IMU传感器1采用双面反轴型方式连接，或者采用单轴多传感器方式连接；磁力计2用于测量磁场信息，GPS 3用于测量定位信息，数据处理器4用于对IMU传感器1、磁力计2和GPS 3检测到的信号进行滤波、放大、计算、分析等处理；在本实施例中，数据处理器4包括获取模块41、数据融合模块42和计算模块43，其中，获取模块41用于获取各加速度计11和各陀螺仪12的测量数据以及磁力计2的磁场信息和GPS3的定位信息；数据融合模块42用于根据各加速度计11的测量数据之间的置信度，利用冗余信息融合算法对各加速度计11的测量数据进行数据融合，得到加速度信息，根据各陀螺仪12的测量数据之间的置信度，利用冗余信息融合算法对各陀螺仪12的测量数据进行数据融合，得到角速度信息；计算模块43用于以角速度信息为状态量且加速度信息和磁场信息为观测量，利用扩展卡尔曼滤波算法计算得到融合姿态角，根据该融合姿态角、加速度信息、角速度信息和定位信息，利用扩展卡尔曼滤波算法计算出位置信息。较优的，数据处理器4还包括修正模块，该修正模块用于对磁力计2和加速度计11进行误差补偿，并对陀螺仪12进行零偏修正，使得磁力计2、加速度计11和陀螺仪12在进行数据测量时不受自身误差的影响，测量的数据更加准确。

具体的，图2示出了IMU传感器通过双面反轴型方式连接的结构示意图，如图2所示，N＝2f个相同的IMU传感器构成冗余传感器***，其中的f为大于等于1的整数，该***为平板型结构，N个IMU传感器被分为两组，每组f个，分别安装在平板的两侧，这两侧的IMU传感器的一轴方向相反，另外两轴的方向相同，比如，在图2中，A侧的所有IMU传感器的坐标方向相同，B侧的所有IMU传感器的坐标方向也相同，而分别安装在A侧和B侧的IMU传感器，其z轴方向相反，而x轴和y轴的方向分别相同。

图3示出了IMU传感器通过单轴多传感器方式连接的结构示意图，如图3所示，N＝a+b+c个相同的IMU传感器构成冗余传感器***，这里的N为大于或等于2的整数；这N个相同的IMU传感器被分为a个、b个和c个共三组，每组IMU传感器的数量可根据需要进行设置，三组IMU传感器被分别安装在冗余传感器***的三个正交轴上，即在冗余传感器***的三个正交轴X、Y和Z上分别安装a个、b个和c个IMU传感器，并且，这N个IMU传感器的各轴向均相同，比如各轴向均与xyz坐标系的对应轴向相同。

基于上述姿态角解算与定位的融合传感器，图4示出了本发明实施例中姿态角解算与定位的方法的流程图，如图4所示，该方法可以包括如下步骤：

步骤S11：误差补偿。

数据处理器4中的修正模块对磁力计2和各加速度计11进行误差补偿。具体的，对磁力计2进行误差补偿可以采用十二面校准的方法，也可以采用椭球拟合的方法；对加速度计11进行误差补偿可以采用卡尔曼滤波的方法，即建立以加速度误差为状态变量的量测方程，经过卡尔曼滤波器的最优估计输出校正信息以补偿加速度计11的误差。

步骤S12：零偏修正。

数据处理器4中的修正模块同时对各陀螺仪12进行零偏修正，具体的，可以通过Allan方差法来修正陀螺仪12的零偏误差。在本实施例中，陀螺仪12可以选择微机械(MicroElectro Mechanical systems，MEMS)陀螺仪，MEMS陀螺仪的噪声可以分为5个独立部分，分别为量化噪声、角度随机游走、偏差不稳定性、速率随机游走和速率斜坡，这5个独立噪声的大小分别由量化噪声系数Q、角度随机游走系数N、偏差不稳定性系数B、速率随机游走系数K及速率斜坡系数R表征。由于这5个噪声相互独立，所以存在如下的关系式(1)：

其中，σ²(τ)为陀螺仪总噪声，即零偏误差，为量化噪声，角度随机游走，为偏差不稳定性，为速率随机游走，为速率斜坡。

由此，可以定义Allan的均方差为这样，σ(τ)的双对数曲线便可以清楚地描述出陀螺仪的各种误差成分和不同误差项。具体的，图5示出了采用Allan方差法修正陀螺仪零偏误差的方法，如图5所示，该方法可以包括如下的步骤S121～步骤S127，具体为：

步骤S121：采集静态漂移数据。

在采用Allan方差法进行陀螺仪零偏误差的修正时，采样率取陀螺仪带宽的3～6倍，这样能够保证处理数据的准确性，由于陀螺仪的带宽大约为100Hz，所以将采样率设为500Hz。采集数据时，将陀螺仪安装在具有良好隔振效果的高精度转台上，然后连续采集陀螺仪的静态漂移数据，采集时间可以根据需要确定，比如连续采集1小时。

步骤S122：平均化处理静态漂移数据。

采集完陀螺仪的静态漂移数据后，为了提高后期运算速度可将采集的数据进行1s平均化处理，剔除预热时间段的数据，比如剔除0.1h的预热数据，从而得到待处理静态漂移数据。

步骤S123：求均值。

得到待处理静态漂移数据后，根据预处理参数对该待处理静态漂移数据进行分组，然后求取各组待处理静态漂移数据的均值，其中的预处理参数可以包括采样周期、标度因子等。

步骤S124：计算Allan均方差。

获得待处理静态漂移数据的均值之后，根据该均值计算Allan方差，然后对Allan方差进行开方运算，得到Allan均方差。

步骤S125：绘制Allan均方差的双对数曲线。

步骤S126：绘制拟合曲线。

得到Allan均方差的双对数曲线之后，对该双对数曲线进行拟合，绘制出该双对数曲线的拟合曲线。

步骤S127：求陀螺仪的零偏误差。

由绘制出的双对数曲线的拟合曲线求取陀螺仪的误差系数，即上述的量化噪声系数Q、角度随机游走系数N、偏差不稳定性系数B、速率随机游走系数K和速率斜坡系数R，根据这些误差系数得到和从而根据关系式(1)计算得到陀螺仪的总噪声σ²(τ)，即得到陀螺仪的零偏误差。

步骤S128：修正陀螺仪的零偏误差。

从陀螺仪的测量数据中减去得到的陀螺仪的零偏误差，从而对陀螺仪的零偏误差进行修正，得到修正后的陀螺仪的测量数据。

实际应用中，上述对陀螺仪的静态漂移数据的处理也可以在MATLAB平台中进行，通过编写的Allan方差数据处理算法计算得到陀螺仪的零偏误差。

步骤S13：获取测量数据。

获取模块41获取各加速度计的测量数据、各陀螺仪的测量数据、磁力计测量的磁场信息和GPS测量的定位信息。

步骤S14：计算加速度信息和角速度信息。

获取模块41获取到各加速度计的测量数据、各陀螺仪的测量数据、磁力计的磁场信息以及GPS的定位信息之后，由数据融合模块42根据各加速度计的测量数据之间的置信度，利用冗余信息融合算法对各加速度计的测量数据进行数据融合，得到加速度信息，同时根据各陀螺仪的测量数据之间的置信度，利用冗余信息融合算法对各陀螺仪的测量数据进行数据融合，得到角速度信息。

在利用冗余信息融合算法分别对各加速度计的测量数据和各陀螺仪的测量数据进行数据融合时，充分利用模糊集合理论中的隶属度函数范围确定的优点，由此定义了一种模糊型指数置信函数，以量化处理同类型传感器中各个传感器的相互置信度，并通过置信矩阵来度量各传感器输出数据的综合置信度，合理地分配同类型传感器中各个传感器的测量数据在数据融合过程中所占的权重，得到数据融合的最终表达式，实现对多冗余传感器的测量数据的数据融合，从而得到加速度信息和角速度信息。

具体的，设多个相同的传感器测量同一参数，第i个传感器和第j个传感器测得的数据分别为c_i和c_j，当c_i的真实性越高时，c_i被其余数据所信任的程度就越高。所谓c_i被c_j信任的程度，是指从c_j来看c_i为真实数据的可能程度，多冗余传感器的测量数据间的这种信任程度被称为置信度。为了对多冗余传感器的测量数据间的信任程度进行进一步地统一量化处理，可以定义一个置信度函数k_ij，用来表示c_i被c_j信任的程度。同时，设定一个上限值S，且S>0，当|c_i﹣c_j|的值超过该上限值S时，可认为这两个数据已经不再相互信任，则可以得到置信度函数k_ij的表达式为：

根据表达式(2)，若k_ij＝0，则认为第i个传感器与第j个传感器相互不信任；若k_ij＝1，则认为第i个传感器信任第j个传感器。当一个传感器不被其他传感器信任，或只被少数传感器信任时，该传感器的测量数据在进行数据融合时即被剔除，这样，在对各传感器的测量数据进行数据融合时便可以自动剔除掉异常数据，参与数据融合的测量数据将是可靠的数据。

由此，通过冗余信息融合算法对多个传感器进行数据处理，增加了目标特征矢量的维数，使得整个测量***获得了任何单个传感器所不能获得的独立特征信息，克服了单个传感器的不确定性和局限性，显著提高了***的性能。

在该理论的基础上，图6示出了利用冗余信息融合算法对N个加速度计的测量数据进行数据融合的方法的流程图，如图6所示，可以包括步骤S141～步骤S143，具体为：

步骤S141：建立置信度矩阵K。

N个加速度计同时测量载体的加速度，根据各加速度计的测量数据之间的置信度函数建立置信度矩阵K，K的表达式为：

其中，n＝N，k_ij为各加速度计的测量数据之间的置信度函数，代表第i个加速度计被第j个加速度计信任的程度，i和j为整数。

步骤142：计算权重矩阵E。

用E_i表示第i个加速度计的测量数据a_i在数据融合过程中所占的权重，E_i的大小反映了其它加速度计的测量数据对第i个加速度计的测量数据a_i的综合信任程度。在信任度矩阵K中，置信度函数k_ij仅仅表示测量数据a_j对a_i的信任程度，并不能反映所有的N个加速度计的测量数据对a_i的信任程度，而a_i的真实程度实际上应该由k_i1，k_i2，…，k_in综合来体现，这时，其他加速度计的测量数据对第i个加速度计的测量数据a_i的信任程度可表示为：

E_i＝m₁k_i1+m₂k_i2+…+m_nk_in (3)

其中，m₁，m₂，…，m_n为一组非负数，用矩阵的形式可以表示为M＝[m₁,m₂,…,m_n]^T，同样的，将E_i表示成矩阵的形式为E＝[e₁,e₂,…,e_n]^T，这样，(3)式可以用矩阵的形式表示为：

E＝KM

又因为k_ij≥0，所以置信度矩阵K是一个非负矩阵，并且该对称矩阵K存在最大模特征值λ(λ>0)，该特征值对应的特征向量即为M的解，由此便可以通过置信度矩阵K和非负向量M得到权重矩阵E。

步骤S143：计算加速度信息。

在得到权重矩阵E＝[e₁,e₂,…,e_n]^T后，根据数据融合公式(4)，利用权重矩阵E对加速度计的测量数据进行加权求和，从而得到加速度信息a，其中的数据融合公式(4)为：

同理，利用冗余信息融合算法对N个陀螺仪的测量数据进行数据融合的过程与利用冗余信息融合算法对N个加速度计的测量数据进行数据融合的过程相同，即先根据各陀螺仪的测量数据之间的置信度函数建立置信度矩阵其中，n＝N，k′_ij为各陀螺仪的测量数据之间的置信度函数，代表第i个陀螺仪被第j个陀螺仪信任的程度，i和j为整数；再根据矩阵乘法公式E′＝K′M′计算权重矩阵E′＝[e′₁，e′₂…e′_i…，e′_n]^T，其中的M′＝[m′₁，m′₂…m′_j…，m′_n]^T为置信度矩阵K′的最大模特征值对应的特征向量，且M′为非负向量；然后根据数据融合公式利用权重矩阵E′对陀螺仪的测量数据进行加权求和得到角速度信息ω，其中的ω_i为第i个陀螺仪的测量数据，e′_i代表ω_i在数据融合中所占的权重。

步骤S15：计算融合姿态角。

在得到加速度信息和角速度信息之后，计算模块43以该角速度信息为状态量且以加速度信息和磁力计测得的磁场信息为观测量，利用扩展卡尔曼滤波算法计算得到融合姿态角，具体的，图7示出了计算融合姿态角的过程，如图7所示，该过程可以包括步骤S151～步骤S155，具体为：

步骤S151：计算初始姿态角。

在利用扩展卡尔曼滤波算法计算融合姿态角之前，必须先在没有滤波方法的情况下得到姿态角，即直接基于加速度计测量的加速度信息和磁力计测量的磁场信息计算出初始姿态角。

具体的，由三轴加速度计和磁力计直接进行的姿态角解算是在载体坐标系中进行的，因此需要将由加速度计和磁力计计算出的姿态量转换至导航坐标系中；将载体坐标系定义为b，导航坐标系定义为d，则该转换由从b到d的转换矩阵实现，其以X-Y-Z轴的顺序为标准进行旋转，得到的转换矩阵可以表示为：

其中，θ和ψ分别代表俯仰角、横滚角和偏航角，为得到的转换矩阵。

当载体处于静止或均匀的线性运动状态时，除了重力加速之外，它具有微小的加速度，通过加速度计的加速度信息和磁力计的磁场信息可以计算出该转换矩阵其计算公式为：

其中，g为重力加速度，a_x、a_y和a_z为加速度计测得的加速度信息，其分别代表加速度计的三轴加速度。

由公式(6)计算出的结合公式(5)可得到初始俯仰角和初始横滚角θ₀的计算公式分别为：

θ₀＝arctan(a_x/g)

进一步的，根据计算出的θ₀和磁力计测得的磁场信息，利用公式一和公式二计算出初始偏航角Ψ₀，其中的公式一为：

其中，m_x、m_y和m_z为磁力计测得的磁场信息，分别代表磁力计的三轴的磁场值。

公式二为：Ψ₀＝arctan(H_y/H_x)。

由此便可以计算出初始姿态角θ₀和Ψ₀。

步骤S152：计算初始姿态四元素。

根据计算出的初始姿态角θ₀和Ψ₀，利用姿态角与四元素的对应关系计算得到初始时刻的姿态四元素q，其中，定义q＝q₀+q₁i+q₂j+q₃k，q₀、q₁、q₂和q₃代表四元素。

在姿态角的解算中，为了避免欧拉角奇异的问题，通常使用四元数来完成姿态角的解算，根据单位旋转四元素q＝q₀+q₁i+q₂j+q₃k，从b到d的转换矩阵可以表示为：

将该式与公式(5)进行对应便可得到姿态角与四元素的对应关系，从而利用该对应关系计算得到四元素q₀、q₁、q₂和q₃，进而得到初始时刻的姿态四元素q。

步骤S153：更新姿态四元素。

在利用姿态四元素进行姿态角的解算时，需要对姿态四元素进行不断更新，其可通过基于陀螺仪数据的四元数微分方程进行更新。具体的，可先根据陀螺仪测得的角速度信息建立角速度矩阵W，W的表达式为：

其中，ω_x、ω_y和ω_z为陀螺仪的角速度信息，分别代表陀螺仪的三轴的角速度。

然后，利用更新方程对姿态四元素进行更新，得到更新后的姿态四元素q′。

步骤S154：建立观测方程。

以不断更新的姿态四元素q′作为状态变量X，且以冗余加速度计测得的加速度信息和磁力计测得的磁场信息为观测量Z，建立观测方程，建立的观测方程为：

Z＝H(X)+V

其中，观测量Z＝[a_x a_y a_z Ψ]^T，g为重力加速度，a_x、a_y和a_z为加速度信息，其分别代表加速度计的三轴加速度，Ψ为根据磁场信息计算出的航向角，V为测量噪声。

对该观测方程进行离散化处理，得到离散化的观测方程为：

Z(k)＝H(k)X(k)+V(k)

其中，X(k)为k时刻的状态值，V(k)为k时刻的测量噪声，为H(X)的雅可比矩阵，表示k时刻的H(X(k))对X(k)求偏导。

步骤S155：计算融合姿态角。

根据状态变量X和离散化的观测方程Z(k)，利用扩展卡尔曼滤波递推方程计算得到融合姿态角，具体的，图8示出了利用扩展卡尔曼滤波递推方程计算融合姿态角的流程图，如图8所示，可以包括如下步骤：

步骤SA1：状态一步预测。

根据公式进行状态预测，得到k时刻的状态预测值其中，F(k-1)为k-1时刻的状态转移矩阵，X(k-1)为k-1时刻的状态值，U(k-1)为k-1时刻的状态噪声。其中，k时刻的状态转移矩阵可以表示为：

其中，ω_x(k)、ω_y(k)和ω_z(k)为k时刻陀螺仪的角速度信息。

步骤SA2：误差协方差预测。

根据公式进行误差协方差的估计，得到误差协方差矩阵的预测值其中，P(k-1)为k-1时刻的误差协方差矩阵，F^T(k-1)为F(k-1)的转置，Q(k-1)为k-1时刻的***噪声协方差矩阵。

步骤SA3：计算滤波增益矩阵。

根据公式计算滤波增益矩阵K(k)，其中，H^T(k)为H(k)的转置，R(k)为k时刻的测量噪声协方差。

步骤SA4：计算融合姿态角。

先根据公式计算k时刻的状态值X(k)，得到k时刻的姿态四元素，然后根据姿态四元素与姿态角的对应关系计算得到k时刻的融合姿态角。

步骤SA5：更新误差协方差。

根据公式对误差协方差进行更新，得到k时刻的误差协方差矩阵，以用于下一时刻的更新计算，其中A＝I-K(k)H(k)，I为单位矩阵。

如此便可在动态条件下不断对融合姿态角进行更新，得到动态条件下的融合姿态角。

步骤S16：融合姿态角辅助计算位置信息。

计算模块43根据计算出的融合姿态角、加速度信息、角速度信息和GPS测得的定位信息，利用扩展卡尔曼滤波算法计算出位置信息。具体的，图9示出了计算位置信息的过程，如图9所示，该过程可以包括如下的步骤S161～步骤S165：

步骤S161：确定状态变量。

计算模块43首先从加速度信息中获取到三个轴向的速度分量υ_x、υ_y和υ_z，然后以该速度分量、GPS的定位信息和以及融合传感器在三个轴向的参数误差和建立状态变量X′，其中的和分别代表经度、纬度和高度，建立的状态变量为

步骤S162：建立状态转移矩阵。

计算模块43在获取到三个轴向的速度分量后，还建立状态转移矩阵其中的F_N为包含速度分量、融合姿态角和定位信息的***矩阵，F_S为单位矩阵，F_M为由陀螺仪和加速度计的输出频率组成的矩阵。

步骤S163：计算载体定位信息。

计算模块43根据数据融合后得到的加速度信息和角速度信息计算出载体定位信息。具体的，计算模块43对加速度信息进行积分，可得到载体运动距离，同时对角速度信息进行二重积分，可以得到载体运动方向，这样，计算模块43便可根据载体运动距离和载体运动方向获得载体定位信息L_t、λ_t和h_t，即得到载体的经度、纬度和高度。

步骤S164：建立观测方程。

在计算出载体定位信息L_t、λ_t和h_t后，计算模块43首先根据计算出的载体定位信息和GPS测得的定位信息和利用公式建立位置观测方程Z′_p(t)，其中，R_M为椭球子午圈曲率半径，R_N为卯酉子午圈曲率半径；接着，通过GPS获取载体的速度信息υ_Gx、υ_Gy和υ_Gz，再根据速度分量υ_x、υ_y和υ_z与载体的速度信息υ_Gx、υ_Gy和υ_Gz之间的差值建立速度观测方程Z′_v(t)，建立的速度观测方程Z′_v(t)为

在建立好位置观测方程Z′_p(t)和速度观测方程Z′_v(t)之后，将Z′_p(t)和Z′_v(t)结合在一起可得到总的观测方程Z′(t)，具体为：

其中，X′(t)为状态量X′对应的状态方程，V′(t)为测量噪声。

对观测方程Z′(t)进行离散化处理，得到离散化的观测方程Z′(k)，Z′(k)可表示为：

Z′(k)＝H′(k)X′(k)+V′(k)

其中，H′(k)为雅可比矩阵，X′(k)和V′(k)分别为k时刻的状态值和测量噪声。

步骤S165：计算位置信息。

根据状态变量X′、状态转移矩阵F′和离散化的观测方程Z′(k)，利用扩展卡尔曼滤波递推方程计算得到位置信息。具体的，以X′为状态变量、F′为状态转移矩阵且Z′(k)为离散化的观测方程，在利用扩展卡尔曼滤波递推方程计算融合姿态角时，其过程与利用扩展卡尔曼滤波递推方程计算得到融合姿态角的过程相同，可参见步骤SA1～步骤SA5，这里不再赘述。

如此便可在动态条件下不断对位置信息进行更新，得到动态条件下的位置信息。而且，在计算位置信息的过程中，以计算出的高精度与高稳定性的融合姿态角对GPS测得的定位信息进行了补正，因此，最终得到的位置信息也具有较高的精度和较高的稳定性。

本发明实施例提供的姿态角解算与定位的方法及其融合传感器，采用了多个IMU传感器进行加速度和角速度的测量，增加了目标特征矢量的维数，使得整个测量***获得了任何单个传感器所不能获得的独立特征信息。进一步的，在姿态角的解算过程中，首先对磁力计和各加速度计进行误差补偿，并对各陀螺仪进行零偏修正，以避免磁力计、加速度计和陀螺仪自身误差对测量数据的影响；再通过冗余信息融合算法对多个IMU传感器的测量数据进行协调、互补和组合，融合得到最终的加速度信息和角速度信息，自动剔除了测量中的异常数据，克服了单个传感器的不确定性和局限性，极大提高了姿态角解算的精度和稳定性；然后利用扩展卡尔曼滤波算法融合该加速度信息、角速度信息以及磁力计获得的磁场信息，摒弃了传统互补滤波融合算法与梯度下降法，计算的复杂度相对较低，在静态和动态条件下都能获得较高精度的姿态角，大幅降低了姿态角的误差；最后由获得的融合姿态角辅助GPS的定位信息计算得到最终的位置信息，对GPS的定位信息进行了补偿，使得得到的位置信息更加精确，稳定性更好。因此，本发明实施例提供的姿态角解算与定位的方法及其融合传感器在姿态角的解算过程中能够获得高精度与高稳定性的姿态角信息和位置信息，提高了姿态角解算与定位的稳定性和可靠性。

本领域技术人员可以理解，上述实施方式中各种方法的全部或部分功能可以通过硬件的方式实现，也可以通过计算机程序的方式实现。当上述实施方式中全部或部分功能通过计算机程序的方式实现时，该程序可以存储于一计算机可读存储介质中，存储介质可以包括：只读存储器、随机存储器、磁盘、光盘、硬盘等，通过计算机执行该程序以实现上述功能。例如，将程序存储在设备的存储器中，当通过处理器执行存储器中程序，即可实现上述全部或部分功能。另外，当上述实施方式中全部或部分功能通过计算机程序的方式实现时，该程序也可以存储在服务器、另一计算机、磁盘、光盘、闪存盘或移动硬盘等存储介质中，通过下载或复制保存到本地设备的存储器中，或对本地设备的***进行版本更新，当通过处理器执行存储器中的程序时，即可实现上述实施方式中全部或部分功能。

以上应用了具体个例对本发明进行阐述，只是用于帮助理解本发明，并不用以限制本发明。对于本发明所属技术领域的技术人员，依据本发明的思想，还可以做出若干简单推演、变形或替换。

Claims

1.一种姿态角解算与定位的方法，其特征在于，应用于姿态角解算与定位的融合传感器中，所述融合传感器包含至少两个IMU传感器、一个磁力计和GPS，一个所述IMU传感器包含一个加速度计和一个陀螺仪，所述方法包括：

以所述角速度信息为状态量且所述加速度信息和所述磁场信息为观测量，利用扩展卡尔曼滤波算法计算得到融合姿态角；

2.如权利要求1所述的方法，其特征在于，所述在获取各加速度计和各陀螺仪的测量数据以及磁力计的磁场信息和GPS的定位信息之前，所述方法还包括：

对磁力计和加速度计进行误差补偿，并对陀螺仪进行零偏修正。

3.如权利要求2所述的方法，其特征在于，所述对陀螺仪进行零偏修正，包括：

采集陀螺仪的静态漂移数据；

对所述静态漂移数据进行1s平均化处理，剔除预热时间段的数据，得到待处理静态漂移数据；

根据预处理参数对所述待处理静态漂移数据进行分组，求取各组待处理静态漂移数据的均值，所述预处理参数包括采样周期和标度因子；

根据所述均值计算Allan方差；

根据所述Allan方差计算Allan均方差；

绘制所述Allan均方差的双对数曲线；

对所述双对数曲线进行拟合，得到拟合曲线；

根据所述拟合曲线求取陀螺仪的误差系数，所述误差系数包括量化噪声系数、角度随机游走系数、偏差不稳定性系数、速率随机游走系数和速率斜坡系数；

根据所述陀螺仪的误差系数计算陀螺仪的总噪声，得到陀螺仪的零偏误差；

从陀螺仪的测量数据中减去所述零偏误差以修正陀螺仪的零偏误差。

4.如权利要求1所述的方法，其特征在于，所述冗余信息融合算法包括：

根据矩阵乘法公式E＝KM计算权重矩阵E＝[e₁，e₂…e_i…，e_n]^T，其中，所述为根据各测量数据之间的置信度函数建立的置信度矩阵，k_ij为各测量数据之间的置信度函数，代表第i个测量数据被第j个测量数据信任的程度，n代表测量数据的个数，i和j为整数，所述M＝[m₁，m₂…m_j…，m_n]^T为置信度矩阵K的最大模特征值对应的特征向量，且M为非负向量；

根据数据融合公式，利用权重矩阵E对测量数据进行加权求和，得到融合数据a；

所述数据融合公式为：

其中，a_i为第i个测量数据，e_i代表a_i在数据融合中所占的权重。

5.如权利要求1所述的方法，其特征在于，所述以所述角速度信息为状态量且所述加速度信息和所述磁场信息为观测量，利用扩展卡尔曼滤波算法计算得到融合姿态角，包括：

根据所述加速度信息和所述磁场信息计算出初始姿态角；

根据所述初始姿态角，利用姿态角与四元素的对应关系计算得到初始时刻的姿态四元素q，其中，q＝q₀+q₁i+q₂j+q₃k，q₀、q₁、q₂和q₃代表四元素；

根据所述角速度信息建立角速度矩阵W；

根据所述角速度矩阵W，利用更新方程对姿态四元素进行更新，得到更新后的姿态四元素q′；

以q′作为状态变量X，且加速度信息和磁场信息为观测量Z，建立观测方程；

对所述观测方程进行离散化处理，得到离散化的观测方程Z(k)；

根据所述X和Z(k)，利用扩展卡尔曼滤波递推方程计算得到融合姿态角；

所述角速度矩阵W为其中，ω_x、ω_y和ω_z为所述角速度信息，分别代表陀螺仪的三轴的角速度；

所述观测方程为Z＝H(X)+V，其中，观测量Z＝[a_x a_y a_z Ψ]^T，g为重力加速度，a_x、a_y和a_z为所述加速度信息，其分别代表加速度计的三轴加速度，Ψ为根据所述磁场信息计算出的航向角，V为测量噪声；

所述离散化的观测方程为Z(k)＝H(k)X(k)+V(k)，其中，X(k)为k时刻的状态值，V(k)为k时刻的测量噪声，为H(X)的雅可比矩阵，表示k时刻的H(X(k))对X(k)求偏导。

6.如权利要求5所述的方法，其特征在于，所述根据所述加速度信息和所述磁场信息计算出初始姿态角，包括：

根据和θ₀＝arctan(a_x/g)分别计算出初始俯仰角和初始横滚角θ₀；

根据所述θ₀和磁场信息，利用公式一和公式二计算出初始偏航角Ψ₀；

所述公式一为：其中，m_x、m_y和m_z为所述磁场信息，分别代表磁力计的三轴的磁场值；

所述公式二为：Ψ₀＝arctan(H_y/H_x)。

7.如权利要求5所述的方法，其特征在于，所述根据所述X和所述Z(k)，利用扩展卡尔曼滤波递推方程计算得到融合姿态角，包括：

根据公式进行状态预测，得到k时刻的状态预测值其中，F(k-1)为k-1时刻的状态转移矩阵，X(k-1)为k-1时刻的状态值，U(k-1)为k-1时刻的状态噪声；

根据公式进行误差协方差的估计，得到误差协方差矩阵的预测值其中，P(k-1)为k-1时刻的误差协方差矩阵，F^T(k-1)为F(k-1)的转置，Q(k-1)为k-1时刻的***噪声协方差矩阵；

根据公式计算滤波增益矩阵K(k)，其中，H^T(k)为H(k)的转置，R(k)为k时刻的测量噪声协方差；

根据公式计算k时刻的状态值X(k)，得到k时刻的姿态四元素；

根据姿态四元素与姿态角的对应关系计算得到k时刻的融合姿态角；

根据公式对误差协方差进行更新，得到k时刻的误差协方差矩阵，其中A＝I-K(k)H(k)，I为单位矩阵。

8.如权利要求1所述的方法，其特征在于，所述根据所述加速度信息、所述角速度信息、所述融合姿态角和所述定位信息，利用扩展卡尔曼滤波算法计算得到位置信息，包括：

获取所述加速度信息中的速度分量，以所述速度分量、所述定位信息和融合传感器的参数误差作为状态变量X′；

根据公式一建立状态转移矩阵F′；

根据所述加速度信息和所述角速度信息计算出载体定位信息；

根据所述载体定位信息和GPS的定位信息，利用公式二建立位置观测方程Z′_p(t)；

通过GPS获取载体的速度信息，并根据所述速度分量和所述速度信息的差值建立速度观测方程Z′_v(t)；

根据所述Z′_p(t)和所述Z′_v(t)，利用公式三得到观测方程Z′(t)；

对所述Z′(t)进行离散化处理，得到离散化的观测方程Z′(k)；

根据所述状态变量X′、所述状态转移矩阵F′和所述离散化的观测方程Z′(k)，利用扩展卡尔曼滤波递推方程计算得到位置信息；

所述其中，υ_x、υ_y和υ_z为所述速度分量，和为所述定位信息，分别代表经度、纬度和高度，▽_x、▽_y和▽_z为所述融合传感器的参数误差；

所述公式一为：其中，F_N为包含所述速度分量、所述融合姿态角和所述定位信息的***矩阵，所述F_S为单位矩阵，所述F_M为由陀螺仪和加速度计的输出频率组成的矩阵；

所述公式二为：其中，L_t、λ_t和h_t为所述载体定位信息，R_M为椭球子午圈曲率半径，R_N为卯酉子午圈曲率半径，L为载体初始纬度；

所述公式三为：其中，V′(t)为测量噪声；

所述Z′(k)＝H′(k)X′(k)+V′(k)，其中，H′(k)为雅可比矩阵，X′(k)和V′(k)分别为k时刻的状态值和测量噪声。

9.如权利要求8所述的方法，其特征在于，所述根据所述加速度信息和所述角速度信息计算出载体定位信息，包括：

对所述加速度信息进行积分，得到载体运动距离；

对所述角速度信息进行二重积分，得到载体运动方向；

根据所述载体运动距离和所述载体运动方向获得载体定位信息。

10.一种姿态角解算与定位的融合传感器，其特征在于，包括：至少两个IMU传感器、一个磁力计、GPS和数据处理器，一个IMU传感器包含一个加速度计和一个陀螺仪，所述至少两个IMU传感器采用双面反轴型方式或单轴多传感器方式连接；

所述加速度计用于测量载体的加速度；

所述陀螺仪用于测量载体的角速度；

所述磁力计用于测量磁场信息；

所述GPS用于测量定位信息；