CN115839726B - 磁传感器和角速度传感器联合标定的方法、系统及介质 - Google Patents

Abstract

本发明公开了一种磁传感器和角速度传感器联合标定的方法、系统及介质，本发明方法包括控制磁传感器和角速度传感器执行多次转动，采用最小二乘法估计磁传感器的磁敏感坐标系到角速度传感器的陀螺坐标系之间的姿态变换矩阵的初值；对当前的姿态变换矩阵进行正交化处理；通过获取每一次转动前后两个姿态的修正误差后的磁传感器测量值以及角增量，并使用当前的姿态变换矩阵估计姿态变换矩阵计算误差，若计算误差小于设定值则输出当前的姿态变换矩阵，否则基于姿态变换矩阵计算误差更新当前的姿态变换矩阵，返回继续迭代。本发明能够实现精确的磁传感器和角速度传感器联合标定，且联合标定中数据采集与校准同步实现，实时性高。

Description

磁传感器和角速度传感器联合标定的方法、系统及介质

技术领域

本发明涉及惯性测量、惯性导航技术领域，具体涉及一种磁传感器和角速度传感器联合标定的方法、系统及介质。

背景技术

载体的姿态测量技术在军用、民用等众多领域中被广泛应用。其中，基于微机电系统（Micro-Electro-MechanicalSystem, 简称MEMS）惯性/磁传感器组合，包括 MEMS 三轴加速度传感器、MEMS 三轴陀螺仪传感器和 MEMS 三轴磁力传感器，因低成本、小体积、低功耗的显著优点而备受青睐。MEMS惯性/磁传感器构成的航姿参考系统（AttitudeandHeading Reference System, 简称AHRS），能够方便可靠地提供载体的姿态信息。将多个AHRS安装于人体，形成可穿戴的人体运动捕获系统，通过一个中心控制单元，将采集到的姿态数据无线发送至电脑端，由电脑进行复杂的数据计算处理后得到各个肢体在三维空间的方向角等方位信息，可以实时获取人体运动数据。

因存在固有的原始性数据测量误差，会直接影响 AHRS 对载体姿态的测量精度。为实现三维空间人体运动捕获系统的技术支撑，实现随时随地对人体任意肢体的运动捕获分析，满足更为广泛的应用领域中不同对象的定位或捕获需要，需要有效地提高MEMS 惯性/磁传感器的测量精度，对 MEMS 惯性/磁传感器进行误差校准，改善其性能。 AHRS的校准可具体分为自主校准方法和非自主校准方法两类。自主校准是指无需外部设备（如高精度转台等）辅助的校准方法，具体而言，就是无需外部的参考基准，仅仅依靠算法本身的特性和可利用的内部的姿态参考基准来实现对传感器的误差校准，该方法成本相对较低。而非自主校准则是需要借助外部设备辅助，通过外部提供的姿态参考基准来进行误差校准，该方法成本较高。在传感器误差校准的过程当中，首先需要根据器件的特性分析误差来源，建立误差模型，然后采集足够多有效的数据样本集合，最后通过合适的数学方法确定或者估计出误差模型里面的各项误差参数。

目前，国内外对于惯性传感器和磁传感器的校准多采用高精度转台等精密仪器的外部设备来提供参考基准的非自主校准方法。由于成本较高，这里不再赘述。自主校准方法，主要有椭球拟合法、点积不变法、极大值极小值法等。由于民用和商用其具有低成本的这个显著要求，所以对 MEMS 惯性/磁传感器进行误差校准的方法在达到校准效果的同时也要尽可能满足低成本的要求，这就要求寻找更简捷而有效的校准方法，同时对投入的设备成本也尽可能要低。

传感器的校准误差包括生产阶段的非正交误差、灵敏度误差和零偏误差，各传感器安装阶段的非对准误差和测试阶段的外界干扰产生的误差。现有的自主校准方法，大多只考虑了磁传感器的非正交误差、灵敏度误差和零偏误差和外部环境干扰因素，忽略了磁传感器和惯性传感器之间的非对准误差。此外，考虑非对准误差的公开技术，采用加速度计和磁传感器结合校准，受加速度计精度的影响，校准精度有限。因此，如何同时满足低成本、高精度、实时可靠的要求下实现载体姿态测量误差校准这一项关键技术，仍有待挖掘新的方法。

发明内容

本发明要解决的技术问题：针对现有技术的上述问题，提供一种磁传感器和角速度传感器联合标定的方法、系统及介质，本发明能够实现精确的磁传感器和角速度传感器联合标定，且联合标定中数据采集与校准同步实现，实时性高。

为了解决上述技术问题，本发明采用的技术方案为：

一种磁传感器和角速度传感器联合标定的方法，包括：

S101，控制磁传感器和角速度传感器执行多次转动，获取每一次转动前后两个姿态的修正误差后的磁传感器测量值以及角增量，并采用最小二乘法估计磁传感器的磁敏感坐标系到角速度传感器的陀螺坐标系之间的姿态变换矩阵的初值；

S102，对当前的姿态变换矩阵进行正交化处理；

S103，基于当前的姿态变换矩阵，利用每一次转动前后两个姿态的修正误差后的磁传感器测量值以及角增量，估计姿态变换矩阵计算误差；

S104，判断姿态变换矩阵计算误差小于设定值是否成立，若成立则将当前的姿态变换矩阵输出，退出；否则基于姿态变换矩阵计算误差更新当前的姿态变换矩阵，跳转步骤S102继续迭代。

可选地，步骤S101中采用最小二乘法估计磁传感器的磁敏感坐标系到角速度传感器的陀螺坐标系之间的姿态变换矩阵的初值包括：

S201，分别根据每一次转动前后两个姿态的修正误差后的磁传感器测量值以及角增量计算本次转动的磁矢量叉积以及参数矩阵；

S202，根据每一次转动的磁矢量叉积以及参数矩阵，采用最小二乘法估计磁传感器的磁敏感坐标系到角速度传感器的陀螺坐标系之间的姿态变换矩阵的初值。

可选地，步骤S201中磁矢量叉积以及参数矩阵的计算函数表达式为：

，

，

上式中，表示任意第次转动的磁矢量叉积，、和分别为转动前姿态的修正误差后的磁传感器测量值，、和分别为转动后姿态的修正误差后的磁传感器测量值；表示任意第次转动的参数矩阵，、和分别为转动前后的角增量的三轴分量。

可选地，步骤S202中采用最小二乘法估计磁传感器的磁敏感坐标系到角速度传感器的陀螺坐标系之间的姿态变换矩阵的函数表达式为：

，

上式中，、、、、、、、、分别为姿态变换矩阵的九个参数，表示任意第次转动的参数矩阵，表示任意第次转动得的磁矢量叉积，且姿态变换矩阵的函数表达式为：

。

可选地，步骤S103中使用当前的姿态变换矩阵估计姿态变换矩阵计算误差包括：

S301，使用当前的姿态变换矩阵计算本次迭代的磁矢量叉积；

，，，

上式中，表示第次迭代的磁矢量叉积，、以及为转换到陀螺坐标系下的本次转动前姿态的修正误差后的磁传感器测量值的三轴分量，、以及为转换到陀螺坐标系下的本次转动后姿态的修正误差后的磁传感器测量值的三轴分量，为当前姿态变换矩阵的转置；、和为本次转动前姿态的修正误差后的磁传感器测量值的三轴分量，、和为本次转动后姿态的修正误差后的磁传感器测量值的三轴分量；

并根据下式计算本次迭代的参数矩阵：

，

上式中，表示第次迭代的参数矩阵，～为系数；且有：

，

，

上式中，、、、、、、、、为当前的姿态变换矩阵的参数；

S302，根据所有次迭代的磁矢量叉积和参数矩阵，采用最小二乘法估计根据下式估计姿态变换矩阵计算误差：

，

上式中，～分别为第1～n+1次迭代的磁矢量叉积，～分别为第1～n+1次迭代的参数矩阵，、和分别为姿态变换矩阵计算误差的三轴分量。

可选地，步骤S104中基于姿态变换矩阵计算误差更新当前的姿态变换矩阵是指将当前的姿态变换矩阵乘以一个基于姿态变换矩阵计算误差得到的变化量，且有：

，

上式中，、和分别为姿态变换矩阵计算误差的三轴分量，、和分别表示当前的姿态变换矩阵中的单位向量。

可选地，所述修正误差后的磁传感器测量值的计算函数表达式为：

，

上式中，为修正误差后的磁传感器测量值，分别为的三轴分量，为灵敏度系数矩阵，为磁传感器测量值，分别为的三轴分量，为磁传感器的零位误差，分别为的三轴分量。

可选地，步骤S101之前还包括估计灵敏度系数矩阵的步骤：

S401，根据某一点处任意旋转磁传感器时三轴矢量和为该处磁场强度 B建立磁场方程：

，

S402，将磁场方程展开：

，

S403，将磁传感器测量值带入展开后的磁场方程并整理为：

，

S404，将作为待辨识参数，在同一地点分别按照x、y、z三个轴旋转传感器得到多组磁传感器测量值，将多组磁传感器测量值利用最小二乘法估计出待辨识参数，并基于待辨识参数得到灵敏度系数矩阵。

此外，本发明还提供一种磁传感器和角速度传感器联合标定的系统，包括相互连接的微处理器和存储器，所述微处理器被编程或配置以执行所述磁传感器和角速度传感器联合标定的方法。

此外，本发明还提供一种计算机可读存储介质，所述计算机可读存储介质中存储有计算机程序，所述计算机程序用于被微处理器编程或配置以执行所述磁传感器和角速度传感器联合标定的方法。

和现有技术相比，本发明主要具有下述优点：

1、本发明采用磁传感器和角速度传感器两者的测量值估计磁传感器的磁敏感坐标系到角速度传感器的陀螺坐标系之间的姿态变换矩阵的初值，从该姿态变换矩阵的初值开始迭代修正磁坐标系到陀螺坐标系之间的误差直至误差小于设定值即可得到最终的姿态变换矩阵，能够实现精确的磁传感器和角速度传感器联合标定；本发明基于分析器件的误差特性，引入陀螺坐标系，基于角速度传感器输出的角速度建立姿态变换矩阵的误差模型以用于实时校正AHRS，可实现 AHRS 的三轴捷联式的 MEMS 惯性/磁传感器的自主校准，基于 MEMS 惯性/磁传感器组合的两步自主校准，能更有效提高传感器的测量精度。

2、本发明的联合标定方法中数据采集与校准同步实现，实时性高。在得到姿态变换矩阵的初值后，控制角速度传感器执行一次转动，利用角速度传感器和磁传感器的测量值计算磁坐标系到陀螺坐标系变换矩阵的估计姿态变换矩阵计算误差，并利用该姿态变换矩阵计算误差实时更新变换矩阵，从而在采集数据的同时接口完成磁坐标系到陀螺坐标系的迭代优化。

3、本发明的方法无需外部设备辅助，成本低。

附图说明

图1为本发明实施例方法的基本流程示意图。

具体实施方式

如图1所示，本实施例磁传感器和角速度传感器联合标定的方法包括：

S101，控制磁传感器和角速度传感器执行多次转动，获取每一次转动前后两个姿态的修正误差后的磁传感器测量值以及角增量，并采用最小二乘法估计磁传感器的磁敏感坐标系到角速度传感器的陀螺坐标系之间的姿态变换矩阵的初值；

S102，对当前的姿态变换矩阵进行正交化处理；

S103，基于当前的姿态变换矩阵，计算每一次转动前后两个姿态的修正误差后的磁传感器测量值以及角增量，估计姿态变换矩阵计算误差；

S104，判断姿态变换矩阵计算误差小于设定值是否成立，若成立则将当前的姿态变换矩阵输出，退出；否则基于姿态变换矩阵计算误差更新当前的姿态变换矩阵，跳转步骤S102继续迭代。

本实施例步骤S101和S103中的转动是指小角度的转动，转动角度不超出角速度传感器的精度的小角度，且该角度越接近角速度传感器的精度越好，因此可以直接取值为角速度传感器的精度，或者在角速度传感器的精度上加以一个裕量或者乘以一个大于1的系数等。其具体可以根据实际需要选择所需的取值。记转动前姿态为姿态1，转动后姿态为姿态2，则有

姿态1的修正误差后的磁传感器测量值为：

，

姿态2的修正误差后的磁传感器测量值为：

，

其中，为姿态变换矩阵的理想值，、和分别为原始（修正误差前）的磁传感器测量值，为姿态1到姿态2的变换矩阵，可由陀螺的数据计算得到。本实施例方法最终要得到的结果是姿态变换矩阵，但是由于该结果是一个估计值，因此采用符号来表示姿态变换矩阵以便与姿态变换矩阵的理想值进行区分。

将姿态1、2的修正误差后的磁传感器测量值这两个矢量方程两边做矢乘，可得磁矢量叉积的方程：

，

其中，为单位矩阵。

由于转动是指小角度的转动，因此有；

，

上式中，、和分别为转动前后的角增量的三轴分量；将上式代入磁矢量叉积的方程可得：

，

记：

，，

则磁矢量叉积的方程可表示为：

，

其中：为磁矢量叉积， R为参数矩阵，且有：

，

若在上一步的标校过程中，每隔一个小转动取一次转动前后的磁敏感器输出，以及陀螺测得的姿态变换矩阵，则可以获得一系列的磁矢量叉积、，和一系列方程：

，

因此所求问题即为求向量()使得下面的代价函数的值最小：

。

现在已经把问题转化为最小二乘的形式，当可观测性足够时（例如分别绕三个轴各转一周）可以解得九个参数，从而可采用最小二乘法估计磁传感器的磁敏感坐标系到角速度传感器的陀螺坐标系之间的姿态变换矩阵的初值。

本实施例中，步骤S101中采用最小二乘法估计磁传感器的磁敏感坐标系到角速度传感器的陀螺坐标系之间的姿态变换矩阵的初值包括：

S201，分别根据每一次转动前后两个姿态的修正误差后的磁传感器测量值以及角增量计算本次转动的磁矢量叉积以及参数矩阵；

S202，根据每一次转动的磁矢量叉积以及参数矩阵，采用最小二乘法估计磁传感器的磁敏感坐标系到角速度传感器的陀螺坐标系之间的姿态变换矩阵的初值。

本实施例中，步骤S201中磁矢量叉积以及参数矩阵的计算函数表达式为：

，

，

上式中，表示任意第i次转动的磁矢量叉积，、和分别为转动前姿态的修正误差后的磁传感器测量值，、和分别为转动后姿态的修正误差后的磁传感器测量值；表示任意第次转动的参数矩阵，、和分别为转动前后的角增量的三轴分量。

本实施例中，步骤S202中采用最小二乘法估计磁传感器的磁敏感坐标系到角速度传感器的陀螺坐标系之间的姿态变换矩阵的函数表达式为：

，

上式中，、、、、、、、、分别为姿态变换矩阵的九个参数，表示任意第次转动的参数矩阵，表示任意第次转动得的磁矢量叉积，且姿态变换矩阵的函数表达式为：

。

由九个参数、、、、、、、、构成的姿态变换矩阵并不一定满足正交归一条件，因此需要对当前的姿态变换矩阵进行正交化处理。姿态变换矩阵进行正交化处理为公知的现有方法，例如可根据需要采用四元数法或施密特正交化方法等。

本实施例中，步骤S103中使用当前的姿态变换矩阵估计姿态变换矩阵计算误差包括：

S301，使用当前的姿态变换矩阵计算本次迭代的磁矢量叉积；

，，，

上式中，表示第次迭代的磁矢量叉积，、以及为转换到陀螺坐标系下的本次转动前姿态的修正误差后的磁传感器测量值的三轴分量，、以及为转换到陀螺坐标系下的本次转动后姿态的修正误差后的磁传感器测量值的三轴分量，为当前姿态变换矩阵的转置；、和为本次转动前姿态的修正误差后的磁传感器测量值的三轴分量，、和为本次转动后姿态的修正误差后的磁传感器测量值的三轴分量；

并根据下式计算本次迭代的参数矩阵：

，

上式中，表示第次迭代的参数矩阵， A ₁～ A ₅为系数；且有：

，

，

上式中，、、、、、、、、为当前的姿态变换矩阵的参数；

S302，根据所有次迭代的磁矢量叉积和参数矩阵，采用最小二乘法估计根据下式估计姿态变换矩阵计算误差：

，

上式中，～分别为第1～n+1次迭代的磁矢量叉积，～分别为第1～n+1次迭代的参数矩阵，、和分别为姿态变换矩阵计算误差的三轴分量。

步骤S301中计算本次迭代的磁矢量叉积的函数表达式的推导过程如下：对磁矢量叉积的方程乘以姿态变换矩阵的转置，则有：

，

由于姿态变换矩阵为姿态变换矩阵理想值的估计值（粗略值），可令：

，

上式中，为单位矩阵，为角增量组成的反对称矩阵，则有：

，

代入上式可得：

，

在此基础上，为方便，记：

，，

则可近似有：

，

将：

，，

代入上式并展开，则上式的右侧可表示为，其中:

，

上式中，为参数矩阵，～为系数；且有：

，

，

上式中，、、、、、、、、为当前的姿态变换矩阵的参数；从而基于上式，可以通过最小二乘法解上述方程组。

本实施例中，步骤S104中基于姿态变换矩阵计算误差更新当前的姿态变换矩阵是指将当前的姿态变换矩阵乘以一个基于姿态变换矩阵计算误差得到的变化量，且有：

，

上式中，、和分别为姿态变换矩阵计算误差的三轴分量， i、 j和 k分别表示当前的姿态变换矩阵中的单位向量。

本实施例中，修正误差后的磁传感器测量值是指修正磁传感器的非正交误差、灵敏度误差和零位误差。在外界磁干扰下，3轴磁传感器输出模型为：

，

上式中，为磁传感器测量值，分别为的三轴分量，、、为x、y、z轴的灵敏度系数，(= x, y, z;= x, y, z;≠)为不正交系数，灵敏度系数和不正交系数一起构成了灵敏度矩阵。为修正误差后的磁传感器测量值，分别为的三轴分量，为磁传感器的零位误差，分别为的三轴分量。基于上述输出模型，本实施例中修正误差后的磁传感器测量值的计算函数表达式为：

，

上式中，为修正误差后的磁传感器测量值，分别为的三轴分量，为灵敏度系数矩阵，为磁传感器测量值，分别为的三轴分量，为磁传感器的零位误差，分别为的三轴分量。本实施例中，步骤S101之前还包括估计灵敏度系数矩阵 K _m 的步骤：

S401，根据某一点处任意旋转磁传感器时三轴矢量和为该处磁场强度建立磁场方程：

，

S402，将磁场方程展开：

，

S403，将磁传感器测量值带入展开后的磁场方程并整理为：

，

S404，将作为待辨识参数，在同一地点分别按照x、y、z三个轴旋转传感器得到多组磁传感器测量值，将多组磁传感器测量值利用最小二乘法估计出待辨识参数，并基于待辨识参数得到灵敏度系数矩阵，即可消除磁传感器的非正交误差、灵敏度误差和零位误差。

综上所述，由于直接确定磁传感器的磁敏感坐标系到角速度传感器的陀螺坐标系之间的姿态变换矩阵或其对应的四元数都比较困难，本实施例磁传感器和角速度传感器联合标定的方法采用了一种两步标定的方法，第一步为利用包含磁传感器和角速度传感器的惯性测量单元IMU转动做小量近似来求得姿态变换矩阵的初值；第二步为在第一步得到的姿态变换矩阵的初值的基础上，再取惯性测量单元IMU转动的精确值而将姿态变换矩阵的误差做小量近似来求得精确解。本实施例基于分析器件的误差特性，引入陀螺坐标系，基于角速度传感器输出的角速度建立姿态变换矩阵的误差模型以用于实时校正AHRS，可实现AHRS 的三轴捷联式的 MEMS 惯性/磁传感器的自主校准，基于 MEMS 惯性/磁传感器组合的两步自主校准，能更有效提高传感器的测量精度。本实施例的联合标定方法中数据采集与校准同步实现，实时性高。在得到姿态变换矩阵的初值后，控制角速度传感器执行一次转动，利用角速度传感器和磁传感器的测量值计算磁坐标系到陀螺坐标系变换矩阵的估计姿态变换矩阵计算误差，并利用该姿态变换矩阵计算误差实时更新变换矩阵，从而在采集数据的同时接口完成磁坐标系到陀螺坐标系的迭代优化。本实施例的方法无需外部设备辅助，成本低。

此外，本实施例还提供一种磁传感器和角速度传感器联合标定的系统，包括相互连接的微处理器和存储器，该微处理器被编程或配置以执行该磁传感器和角速度传感器联合标定的方法。此外，本实施例还提供一种计算机可读存储介质，该计算机可读存储介质中存储有计算机程序，该计算机程序用于被微处理器编程或配置以执行该磁传感器和角速度传感器联合标定的方法。

本领域内的技术人员应明白，本申请的实施例可提供为方法、系统、或计算机程序产品。因此，本申请可采用完全硬件实施例、完全软件实施例、或结合软件和硬件方面的实施例的形式。而且，本申请可采用在一个或多个其中包含有计算机可用程序代码的计算机可读存储介质（包括但不限于磁盘存储器、CD-ROM、光学存储器等）上实施的计算机程序产品的形式。本申请是参照根据本申请实施例的方法、设备（系统）、和计算机程序产品的流程图和／或方框图来描述的。应理解可由计算机程序指令实现流程图和／或方框图中的每一流程和／或方框、以及流程图和／或方框图中的流程和／或方框的结合。可提供这些计算机程序指令到通用计算机、专用计算机、嵌入式处理机或其他可编程数据处理设备的处理器以产生一个机器，使得通过计算机或其他可编程数据处理设备的处理器执行的指令产生用于实现在流程图一个流程或多个流程和／或方框图一个方框或多个方框中指定的功能的装置。这些计算机程序指令也可存储在能引导计算机或其他可编程数据处理设备以特定方式工作的计算机可读存储器中，使得存储在该计算机可读存储器中的指令产生包括指令装置的制造品，该指令装置实现在流程图一个流程或多个流程和／或方框图一个方框或多个方框中指定的功能。这些计算机程序指令也可装载到计算机或其他可编程数据处理设备上，使得在计算机或其他可编程设备上执行一系列操作步骤以产生计算机实现的处理，从而在计算机或其他可编程设备上执行的指令提供用于实现在流程图一个流程或多个流程和／或方框图一个方框或多个方框中指定的功能的步骤。

以上所述仅是本发明的优选实施方式，本发明的保护范围并不仅局限于上述实施例，凡属于本发明思路下的技术方案均属于本发明的保护范围。应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理前提下的若干改进和润饰，这些改进和润饰也应视为本发明的保护范围。

Claims (8)

1.一种磁传感器和角速度传感器联合标定的方法，其特征在于，包括：

S101，控制磁传感器和角速度传感器执行多次转动，获取每一次转动前后两个姿态的修正误差后的磁传感器测量值以及角增量，并采用最小二乘法估计磁传感器的磁敏感坐标系到角速度传感器的陀螺坐标系之间的姿态变换矩阵的初值，包括：S201，分别根据每一次转动前后两个姿态的修正误差后的磁传感器测量值以及角增量计算本次转动的磁矢量叉积以及参数矩阵；S202，根据每一次转动的磁矢量叉积以及参数矩阵，采用最小二乘法估计磁传感器的磁敏感坐标系到角速度传感器的陀螺坐标系之间的姿态变换矩阵的初值；

S102，对当前的姿态变换矩阵进行正交化处理；

S103，基于当前的姿态变换矩阵，利用每一次转动前后两个姿态的修正误差后的磁传感器测量值以及角增量，估计姿态变换矩阵计算误差；

S104，判断姿态变换矩阵计算误差小于设定值是否成立，若成立则将当前的姿态变换矩阵输出，退出；否则基于姿态变换矩阵计算误差更新当前的姿态变换矩阵，跳转步骤S102继续迭代；

步骤S103中估计姿态变换矩阵计算误差包括：

S301，使用当前的姿态变换矩阵计算本次迭代的磁矢量叉积；

，，，

上式中，表示第次迭代的磁矢量叉积，、以及为转换到陀螺坐标系下的本次转动前姿态的修正误差后的磁传感器测量值的三轴分量，、以及为转换到陀螺坐标系下的本次转动后姿态的修正误差后的磁传感器测量值的三轴分量，为当前姿态变换矩阵的转置；、和为本次转动前姿态的修正误差后的磁传感器测量值的三轴分量，、和为本次转动后姿态的修正误差后的磁传感器测量值的三轴分量；并根据下式计算本次迭代的参数矩阵：

，

上式中，表示第次迭代的参数矩阵，～为系数；且有：

，

，

上式中，、、、、、、、、为当前的姿态变换矩阵的参数；

S302，根据所有次迭代的磁矢量叉积和参数矩阵，采用最小二乘法估计根据下式估计姿态变换矩阵计算误差：

，

上式中，～分别为第1～n+1次迭代的磁矢量叉积，～分别为第1～n+1次迭代的参数矩阵，、和分别为姿态变换矩阵计算误差的三轴分量。

2.根据权利要求1所述的磁传感器和角速度传感器联合标定的方法，其特征在于，步骤S201中磁矢量叉积以及参数矩阵的计算函数表达式为：

，

，

上式中，表示任意第次转动的磁矢量叉积，、和分别为转动前姿态的修正误差后的磁传感器测量值，、和分别为转动后姿态的修正误差后的磁传感器测量值；表示任意第次转动的参数矩阵，、和分别为转动前后的角增量的三轴分量。

3.根据权利要求1所述的磁传感器和角速度传感器联合标定的方法，其特征在于，步骤S202中采用最小二乘法估计磁传感器的磁敏感坐标系到角速度传感器的陀螺坐标系之间的姿态变换矩阵的函数表达式为：

，

上式中，、、、、、、、、分别为姿态变换矩阵的九个参数，表示任意第次转动的参数矩阵，表示任意第次转动得的磁矢量叉积，且姿态变换矩阵的函数表达式为：

。

4.根据权利要求1所述的磁传感器和角速度传感器联合标定的方法，其特征在于，步骤S104中基于姿态变换矩阵计算误差更新当前的姿态变换矩阵是指将当前的姿态变换矩阵乘以一个基于姿态变换矩阵计算误差得到的变化量，且有：

，

上式中，、和分别为姿态变换矩阵计算误差的三轴分量，、和分别表示当前的姿态变换矩阵中的单位向量。

5.根据权利要求1所述的磁传感器和角速度传感器联合标定的方法，其特征在于，所述修正误差后的磁传感器测量值的计算函数表达式为：

，

上式中，为修正误差后的磁传感器测量值，分别为的三轴分量，为灵敏度系数矩阵，为磁传感器测量值，分别为的三轴分量，为磁传感器的零位误差，分别为的三轴分量。

6.根据权利要求5所述的磁传感器和角速度传感器联合标定的方法，其特征在于，步骤S101之前还包括估计灵敏度系数矩阵的步骤：

S401，根据某一点处任意旋转磁传感器时三轴矢量和为该处磁场强度B建立磁场方程：

，

S402，将磁场方程展开：

，

S403，将磁传感器测量值带入展开后的磁场方程并整理为：

，

S404，将作为待辨识参数，在同一地点分别按照x、y、z三个轴旋转传感器得到多组磁传感器测量值，将多组磁传感器测量值利用最小二乘法估计出待辨识参数，并基于待辨识参数得到灵敏度系数矩阵。

7.一种磁传感器和角速度传感器联合标定的系统，包括相互连接的微处理器和存储器，其特征在于，所述微处理器被编程或配置以执行权利要求1～6中任意一项所述磁传感器和角速度传感器联合标定的方法。

8.一种计算机可读存储介质，所述计算机可读存储介质中存储有计算机程序，其特征在于，所述计算机程序用于被微处理器编程或配置以执行权利要求1～6中任意一项所述磁传感器和角速度传感器联合标定的方法。

Images