一种姿态角的融合解算方法和装置制造方法及图纸

一种姿态角的融合解算方法和装置，姿态角的融合解算方法包括：根据陀螺仪的测量数据解算出第一姿态角；根据加速度计的测量数据和磁力计的测量数据解算出第二姿态角；根据互补滤波原理对第一姿态角和第二姿态角进行融合计算，得到第三姿态角；以第三姿态角为第一状态变量且互补滤波后获得的第二姿态角为观测量，利用扩展卡尔曼滤波计算得到融合姿态角。由于将互补滤波和卡尔曼滤波相结合来解算姿态角，弥补了互补滤波和卡尔曼滤波各自的缺陷，既能够提高动态条件下姿态角的跟随性，又能够达到较快的收敛速度。

A fusion method and device for attitude angle fusion

A fusion method and device for calculating attitude angle include: calculating the first attitude angle according to the measured data of gyroscope; calculating the second attitude angle according to the measured data of accelerometer and magnetometer; calculating the first attitude angle and the second attitude angle according to the complementary filtering principle. The third attitude angle is obtained by fusion calculation, and the fusion attitude angle is calculated by extended Kalman filter with the third attitude angle as the first state variable and the second attitude angle obtained by complementary filtering as the observation. The combination of complementary filter and Kalman filter can compensate for the disadvantages of complementary filter and Kalman filter. It can not only improve the follow-up of attitude angle under dynamic conditions, but also achieve faster convergence speed.

【技术实现步骤摘要】
一种姿态角的融合解算方法和装置
本专利技术涉及姿态角测量
，具体涉及一种姿态角的融合解算方法和装置。
技术介绍
姿态角主要是指物体的俯仰角、偏航角和翻滚角，可用来表征物体的姿态，姿态角度的测量在控制、机械、通信、航空航天等
得到了广泛的应用，其主要利用惯性测量单元(Inertialmeasurementunit，IMU)进行测量。一般而言，IMU包含加速度计和陀螺仪，加速度计用于检测物体在载体坐标系中独立三轴的加速度信号，而陀螺仪用于检测物体相对于地理坐标系的角速度信号，通过测量物体在三维空间中的角速度和加速度，并以此解算出物体的姿态。在具体利用IMU解算姿态角信息时，陀螺仪自身的工作原理决定了其长时间的积分会导致误差的累积，使测量值随时间而漂移，导致姿态角解算的精度降低，因而需要利用外部信息对陀螺仪的漂移进行修正。磁力计是一种直接感测地磁场信息的传感器，其没有累积误差，工作较为稳定，因而可用来修正陀螺仪的漂移。目前，在进行姿态角解算时，主要利用的是互补滤波或者卡尔曼滤波，但利用互补滤波解算出的姿态角的跟随性较差，而通过卡尔曼滤波器解算姿态角时的收敛性又较慢，在动态条件下采用这两种方法进行姿态角的解算时都不利于姿态角的更新。因此，针对IMU和磁力计构成的姿态角测量单元，现有的姿态角解算方法还无法实现在提高动态条件下姿态的跟随性的同时，又能使姿态角达到较快的收敛速度。
技术实现思路
本申请提供一种姿态角的融合解算方法和装置，以实现在姿态角的解算过程中，既提高动态条件下姿态的跟随性，又使姿态角达到较快的收敛速度。根据第一方面，一种实施例中提供一种姿态角的融合解算方法，包括：根据陀螺仪的测量数据解算出第一姿态角；根据加速度计的测量数据和磁力计的测量数据解算出第二姿态角；根据互补滤波原理对所述第一姿态角和所述第二姿态角进行融合计算，得到第三姿态角；以所述第三姿态角为第一状态变量且互补滤波后获得的所述第二姿态角为观测量，利用扩展卡尔曼滤波计算得到融合姿态角。根据第二方面，一种实施例中提供一种姿态角融合解算的装置，包括：陀螺仪，用于检测物体相对于地理坐标系的角速度信号并输出；加速度计，用于检测物体在载体坐标系中三个轴的加速度信号并输出；磁力计，用于检测地磁场的强度和方向信息并输出；数据处理器，用于执行如上述方案所述的姿态角的融合解算方法。依据上述实施例的姿态角的融合解算方法和装置，首先根据陀螺仪的测量数据解算出第一姿态角，且根据加速度计的测量数据和磁力计的测量数据解算出第二姿态角；接着根据互补滤波原理对第一姿态角和第二姿态角进行融合计算，得到第三姿态角；然后以第三姿态角为状态变量且互补滤波后获得的第二姿态角为观测量，利用扩展卡尔曼滤波计算得到融合姿态角，从而将互补滤波和卡尔曼滤波相结合，融合解算出姿态角，弥补了互补滤波和卡尔曼滤波各自的缺陷，既能够提高动态条件下姿态角的跟随性，又能够达到较快的收敛速度。附图说明图1为一种实施例中姿态角的融合解算装置的结构示意图；图2为一种实施例中数据处理器的结构示意图；图3为一种具体实施例中姿态角的融合解算方法的流程图；图4为一种具体实施例中计算融合姿态角的流程图；图5为一种具体实施例中利用卡尔曼滤波算法求取融合姿态角的流程图；图6为另一种实施例中数据处理器的结构示意图；图7为一种具体实施例中对磁力计进行校准的流程图；图8为一种具体实施例中利用卡尔曼滤波迭代对磁力计的测量数据进行校准的流程图。具体实施方式下面通过具体实施方式结合附图对本专利技术作进一步详细说明。在某些情况下，本申请相关的一些操作并没有在说明书中显示或者描述，这是为了避免本申请的核心部分被过多的描述所淹没，而对于本领域技术人员而言，详细描述这些相关操作并不是必要的，他们根据说明书中的描述以及本领域的一般技术知识即可完整了解相关操作。另外，说明书中所描述的特点、操作或者特征可以以任意适当的方式结合形成各种实施方式。同时，方法描述中的各步骤或者动作也可以按照本领域技术人员所能显而易见的方式进行顺序调换或调整。姿态角是指物体的俯仰角、偏航角和翻滚角，在本文中，若无特殊说明，这三个角均分别用Φ、θ和Ψ表示，带有下标的Φ、θ和Ψ也分别代表这三个角。在本专利技术实施例中，先根据互补滤波原理将陀螺仪解算出的第一姿态角和加速度计与磁力计解算出第二姿态角进行融合计算，得到第三姿态角，再以第三姿态角为状态变量建立卡尔曼滤波的状态方程，以互补滤波后获得的第二姿态角为观测量进行姿态角更新，以此对互补滤波和卡尔曼滤波进行信息融合来解算姿态角。实施例一：图1为本专利技术一种实施例中姿态角的融合解算装置的结构示意图，如图1所示，该装置包括陀螺仪1、加速度计2、磁力计3和数据处理器4。陀螺仪1用于检测物体相对于地理坐标系的角速度信号并输出；加速度计2用于检测物体在载体坐标系中三个轴的加速度信号并输出；磁力计3用于检测地磁场的强度和方向信息并输出；数据处理器4用于获取陀螺仪1、加速度计2和磁力计3输出的信号，并对这些信号进行放大、去除噪声、计算等处理。在本专利技术的一种实施例中，数据处理器4的结构示意图如图2所示，包括第一计算模块41、第二计算模块42和第三计算模块43。第一计算模块41用于根据陀螺仪的测量数据解算出第一姿态角，且根据加速度计的测量数据和磁力计的测量数据解算出第二姿态角；第二计算模块42用于根据互补滤波原理对第一计算模块41解算出的第一姿态角和第二姿态角进行融合计算，得到第三姿态角；第三计算模块43用于以第二计算模块42融合计算出的第三姿态角为状态变量，且以互补滤波后获得的第二姿态角为观测量，利用扩展卡尔曼滤波计算得到融合姿态角。在一具体实施例中，图3示出了姿态角的融合解算方法的流程图，如图3所示，该方法包括如下步骤：步骤S11：解算第一姿态角和第二姿态角。第一计算模块41根据陀螺仪三轴方向上的测量数据解算出第一姿态角Φg、θg和Ψg，且根据加速度计三轴方向上的测量数据和磁力计三轴方向上的测量数据解算出第二姿态角Φam、θam和Ψam。第一计算模块41在解算出第一姿态角和第二姿态角之后，由第二计算模块42根据互补滤波原理对该第一姿态角和第二姿态角进行融合计算而得到第三姿态角，其具体步骤包括步骤S12和步骤S13，具体为：步骤S12：计算权值F。第二计算模块42根据加速度计的测量数据、磁力计的测量数据和地球的当地磁场值，利用公式(1)计算出权值F，其中的公式(1)为：在(1)式中，MG为地球的当地磁场值，ax、ay、az分别为加速度计的三个轴归一化后的测量数据，mx、my、mz分别为磁力计的三个轴的测量数据。步骤S13：计算第三姿态角。第二计算模块42根据第一姿态角Φg、θg和Ψg、第二姿态角Φam、θam和Ψam以及由公式(1)计算出的F，利用公式(2)计算出第三姿态角Φ、θ和Ψ，其中公式(2)为：由公式(2)可知，第三姿态角是在第一姿态角的基础上加上第二姿态角和权值F的乘积得到的，这样，第二姿态角和权值F的乘积便可看作是第一姿态角的修正项，而第一姿态角是根据陀螺仪的测量数据解算出的姿态角，第二姿态角是根据加速度计和磁力计的测量数据解算出的姿态角，由此便可实现磁力计对陀螺仪漂移的修正，提高了姿态角解算的精度。另一方面，在实时动态条本文档来自技高网...

【技术保护点】
1.一种姿态角的融合解算方法，其特征在于，包括：根据陀螺仪的测量数据解算出第一姿态角；根据加速度计的测量数据和磁力计的测量数据解算出第二姿态角；根据互补滤波原理对所述第一姿态角和所述第二姿态角进行融合计算，得到第三姿态角；以所述第三姿态角为第一状态变量且互补滤波后获得的所述第二姿态角为观测量，利用扩展卡尔曼滤波计算得到融合姿态角。

【技术特征摘要】
1.一种姿态角的融合解算方法，其特征在于，包括：根据陀螺仪的测量数据解算出第一姿态角；根据加速度计的测量数据和磁力计的测量数据解算出第二姿态角；根据互补滤波原理对所述第一姿态角和所述第二姿态角进行融合计算，得到第三姿态角；以所述第三姿态角为第一状态变量且互补滤波后获得的所述第二姿态角为观测量，利用扩展卡尔曼滤波计算得到融合姿态角。2.如权利要求1所述的方法，其特征在于，在所述根据加速度计的测量数据和磁力计的测量数据解算出第二姿态角之前，所述方法还包括：以磁力计的零偏为第二状态变量，利用卡尔曼滤波迭代对磁力计的测量数据进行校准，得到磁力计校准后的测量数据。3.如权利要求2所述的方法，其特征在于，所述以磁力计的零偏为第二状态变量，利用卡尔曼滤波迭代对磁力计的测量数据进行校准，得到磁力计校准后的测量数据，包括：以磁力计的零偏为第二状态变量，建立磁力计的零偏状态方程为x＝f(x)+w，其中，所述x＝[bxbybz]T为第二状态变量，所述f(x)＝0为所述x的微分值，所述w为磁力计零偏的随机噪声，所述bx、by、bz分别为磁力计在x、y、z三轴方向上的零偏，T代表转置；对所述零偏状态方程进行离散化处理，得到卡尔曼滤波的状态转移矩阵A；建立磁力计零偏的观测方程为y＝(Mbx-bx)2+(Mby-by)2+(Mbz-bz)2，其中，所述y为当地磁场值，所述Mbx、Mby、Mbz为磁力计三轴的实际测量输出值；对所述观测方程进行离散化处理，得到量测方程；计算所述量测方程的雅可比矩阵，得到第一量测矩阵H1k；根据所述第二状态变量x、所述状态转移矩阵A、所述磁力计零偏的观测方程和所述第一量测矩阵H1k，利用卡尔曼滤波迭代对磁力计的测量数据进行校准，得到磁力计校准后的测量数据。4.如权利要求3所述的方法，其特征在于，所述根据所述第二状态变量x、所述状态转移矩阵A、所述磁力计零偏的观测方程和所述第一量测矩阵H1k，利用卡尔曼滤波迭代对磁力计的测量数据进行校准，得到磁力计校准后的测量数据，包括：通过状态方程得到x在k时刻的状态预测值，其中，为所述状态预测值，为k-1时刻的最优状态值，W1k-1为k-1时刻的状态噪声；根据公式Z1k|k-1＝H1k*X1k-1+V1k-1计算所述x的估算值，其中，Z1k|k-1为所述估算值，X1k-1为所述x在k-1时刻的状态，V1k-1为k-1时刻的估测噪声；计算滤波增益矩阵K1k和误差协方差矩阵P1k|k-1，计算公式分别为：P1k|k-1＝E1k|k-1*P1k-1*E1Tk|k-1+Q1k-1K1k＝P1k|k-1*H1kT*(H1k*P1k|k-1*H1kT+R1k)-1其中，P1k|k-1为对应的误差协方差矩阵，E1k|k-1为状态方程系数，E1Tk|k-1为E1k|k-1的转置，Q1k-1为系统噪声协方差矩阵，H1kT为H1k的转置，R1k为测量噪声协方差矩阵；根据公式计算k时刻磁力计的零偏值其中，yk为k时刻的观测值；根据公式P1k＝(I1-K1k*H1k)*P1k|k-1计算k时刻的误差协方差矩阵P1k，其中，I1为单位矩阵；从k时刻磁力计的测量数据中减去所述得到k时刻磁力计校准后的测量数据。5.如权利要求2至4任一项所述的方法，其特征在于，所述根据加速度计的测量数据和磁力计的测量数据解算出第二姿态角，包括：根据加速度计的测量数据和磁...

【专利技术属性】
技术研发人员：时广轶，谢偰伟，国林旗，
申请(专利权)人：北京大学深圳研究生院，
类型：发明
国别省市：广东,44