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

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

Info

Publication number
CN115839726A
CN115839726A CN202310152338.8A CN202310152338A CN115839726A CN 115839726 A CN115839726 A CN 115839726A CN 202310152338 A CN202310152338 A CN 202310152338A CN 115839726 A CN115839726 A CN 115839726A
Authority
CN
China
Prior art keywords
magnetic sensor
transformation matrix
magnetic
angular velocity
error
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.)
Granted
Application number
CN202310152338.8A
Other languages
English (en)
Other versions
CN115839726B (zh
Inventor
王宇
王飞
卢广锋
王凡
王晓飞
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Hunan 2008 Advanced Technology Co ltd
Original Assignee
Hunan 2008 Advanced Technology Co ltd
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by Hunan 2008 Advanced Technology Co ltd filed Critical Hunan 2008 Advanced Technology Co ltd
Priority to CN202310152338.8A priority Critical patent/CN115839726B/zh
Publication of CN115839726A publication Critical patent/CN115839726A/zh
Application granted granted Critical
Publication of CN115839726B publication Critical patent/CN115839726B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Landscapes

  • Gyroscopes (AREA)

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中磁矢量叉积以及参数矩阵的计算函数表达式为：
Figure SMS_1
，
Figure SMS_2
，
上式中，
Figure SMS_5
表示任意第/>
Figure SMS_8
次转动的磁矢量叉积，/>
Figure SMS_11
、/>
Figure SMS_4
和/>
Figure SMS_12
分别为转动前姿态的修正误差后的磁传感器测量值，/>
Figure SMS_14
、/>
Figure SMS_15
和/>
Figure SMS_3
分别为转动后姿态的修正误差后的磁传感器测量值；/>
Figure SMS_7
表示任意第/>
Figure SMS_10
次转动的参数矩阵，/>
Figure SMS_13
、/>
Figure SMS_6
和/>
Figure SMS_9
分别为转动前后的角增量的三轴分量。
可选地，步骤S202中采用最小二乘法估计磁传感器的磁敏感坐标系到角速度传感器的陀螺坐标系之间的姿态变换矩阵的函数表达式为：
Figure SMS_16
，
上式中，
Figure SMS_18
、/>
Figure SMS_21
、/>
Figure SMS_24
、/>
Figure SMS_17
、/>
Figure SMS_22
、/>
Figure SMS_26
、/>
Figure SMS_29
、/>
Figure SMS_19
、/>
Figure SMS_23
分别为姿态变换矩阵/>
Figure SMS_25
的九个参数，/>
Figure SMS_28
表示任意第/>
Figure SMS_20
次转动的参数矩阵，/>
Figure SMS_27
表示任意第/>
Figure SMS_30
次转动得的磁矢量叉积，且姿态变换矩阵的函数表达式为：
Figure SMS_31
。
可选地，步骤S103中使用当前的姿态变换矩阵估计姿态变换矩阵计算误差包括：
S301，使用当前的姿态变换矩阵计算本次迭代的磁矢量叉积；
Figure SMS_32
，/>
Figure SMS_33
，/>
Figure SMS_34
，
上式中，
Figure SMS_48
表示第/>
Figure SMS_36
次迭代的磁矢量叉积，/>
Figure SMS_46
、/>
Figure SMS_43
以及/>
Figure SMS_47
为转换到陀螺坐标系下的本次转动前姿态的修正误差后的磁传感器测量值的三轴分量，/>
Figure SMS_49
、/>
Figure SMS_50
以及/>
Figure SMS_42
为转换到陀螺坐标系下的本次转动后姿态的修正误差后的磁传感器测量值的三轴分量，/>
Figure SMS_45
为当前姿态变换矩阵/>
Figure SMS_37
的转置；/>
Figure SMS_40
、/>
Figure SMS_35
和/>
Figure SMS_39
为本次转动前姿态的修正误差后的磁传感器测量值的三轴分量，/>
Figure SMS_41
、/>
Figure SMS_44
和/>
Figure SMS_38
为本次转动后姿态的修正误差后的磁传感器测量值的三轴分量；
并根据下式计算本次迭代的参数矩阵：
Figure SMS_51
，
上式中，
Figure SMS_52
表示第/>
Figure SMS_53
次迭代的参数矩阵，/>
Figure SMS_54
～/>
Figure SMS_55
为系数；且有：
Figure SMS_56
，
Figure SMS_57
，
上式中，
Figure SMS_60
、/>
Figure SMS_63
、/>
Figure SMS_65
、/>
Figure SMS_58
、/>
Figure SMS_61
、/>
Figure SMS_64
、/>
Figure SMS_66
、/>
Figure SMS_59
、/>
Figure SMS_62
为当前的姿态变换矩阵的参数；
S302，根据所有次迭代的磁矢量叉积和参数矩阵，采用最小二乘法估计根据下式估计姿态变换矩阵计算误差：
Figure SMS_67
，
上式中，
Figure SMS_68
～/>
Figure SMS_69
分别为第1～n+1次迭代的磁矢量叉积，/>
Figure SMS_70
～/>
Figure SMS_71
分别为第1～n+1次迭代的参数矩阵，/>
Figure SMS_72
、/>
Figure SMS_73
和/>
Figure SMS_74
分别为姿态变换矩阵计算误差的三轴分量。
可选地，步骤S104中基于姿态变换矩阵计算误差更新当前的姿态变换矩阵是指将当前的姿态变换矩阵乘以一个基于姿态变换矩阵计算误差得到的变化量
Figure SMS_75
，且有：
Figure SMS_76
，
上式中，
Figure SMS_77
、/>
Figure SMS_78
和/>
Figure SMS_79
分别为姿态变换矩阵计算误差的三轴分量，/>
Figure SMS_80
、/>
Figure SMS_81
和/>
Figure SMS_82
分别表示当前的姿态变换矩阵中的单位向量。
可选地，所述修正误差后的磁传感器测量值的计算函数表达式为：
Figure SMS_83
，
上式中，
Figure SMS_85
为修正误差后的磁传感器测量值，/>
Figure SMS_89
分别为/>
Figure SMS_91
的三轴分量，/>
Figure SMS_84
为灵敏度系数矩阵，/>
Figure SMS_87
为磁传感器测量值，/>
Figure SMS_90
分别为/>
Figure SMS_92
的三轴分量，/>
Figure SMS_86
为磁传感器的零位误差，/>
Figure SMS_88
分别为/>
Figure SMS_93
的三轴分量。
可选地，步骤S101之前还包括估计灵敏度系数矩阵
Figure SMS_94
的步骤：
S401，根据某一点处任意旋转磁传感器时三轴矢量和为该处磁场强度B建立磁场方程：
Figure SMS_95
，
S402，将磁场方程展开：
Figure SMS_96
，
S403，将磁传感器测量值
Figure SMS_97
带入展开后的磁场方程并整理为：
Figure SMS_98
，
S404，将
Figure SMS_99
作为待辨识参数，在同一地点分别按照x、y、z三个轴旋转传感器得到多组磁传感器测量值/>
Figure SMS_100
，将多组磁传感器测量值
Figure SMS_101
利用最小二乘法估计出待辨识参数/>
Figure SMS_102
，并基于待辨识参数/>
Figure SMS_103
得到灵敏度系数矩阵/>
Figure SMS_104
。
此外，本发明还提供一种磁传感器和角速度传感器联合标定的系统，包括相互连接的微处理器和存储器，所述微处理器被编程或配置以执行所述磁传感器和角速度传感器联合标定的方法。
此外，本发明还提供一种计算机可读存储介质，所述计算机可读存储介质中存储有计算机程序，所述计算机程序用于被微处理器编程或配置以执行所述磁传感器和角速度传感器联合标定的方法。
和现有技术相比，本发明主要具有下述优点：
1、本发明采用磁传感器和角速度传感器两者的测量值估计磁传感器的磁敏感坐标系到角速度传感器的陀螺坐标系之间的姿态变换矩阵的初值，从该姿态变换矩阵的初值开始迭代修正磁坐标系到陀螺坐标系之间的误差直至误差小于设定值即可得到最终的姿态变换矩阵，能够实现精确的磁传感器和角速度传感器联合标定；本发明基于分析器件的误差特性，引入陀螺坐标系，基于角速度传感器输出的角速度建立姿态变换矩阵的误差模型以用于实时校正AHRS，可实现 AHRS 的三轴捷联式的 MEMS 惯性/磁传感器的自主校准，基于 MEMS 惯性/磁传感器组合的两步自主校准，能更有效提高传感器的测量精度。
2、本发明的联合标定方法中数据采集与校准同步实现，实时性高。在得到姿态变换矩阵的初值后，控制角速度传感器执行一次转动，利用角速度传感器和磁传感器的测量值计算磁坐标系到陀螺坐标系变换矩阵的估计姿态变换矩阵计算误差，并利用该姿态变换矩阵计算误差实时更新变换矩阵，从而在采集数据的同时接口完成磁坐标系到陀螺坐标系的迭代优化。
3、本发明的方法无需外部设备辅助，成本低。
附图说明
图1为本发明实施例方法的基本流程示意图。
具体实施方式
如图1所示，本实施例磁传感器和角速度传感器联合标定的方法包括：
S101，控制磁传感器和角速度传感器执行多次转动，获取每一次转动前后两个姿态的修正误差后的磁传感器测量值以及角增量，并采用最小二乘法估计磁传感器的磁敏感坐标系到角速度传感器的陀螺坐标系之间的姿态变换矩阵的初值；
S102，对当前的姿态变换矩阵进行正交化处理；
S103，基于当前的姿态变换矩阵，计算每一次转动前后两个姿态的修正误差后的磁传感器测量值以及角增量，估计姿态变换矩阵计算误差；
S104，判断姿态变换矩阵计算误差小于设定值是否成立，若成立则将当前的姿态变换矩阵输出，退出；否则基于姿态变换矩阵计算误差更新当前的姿态变换矩阵，跳转步骤S102继续迭代。
本实施例步骤S101和S103中的转动是指小角度的转动，转动角度不超出角速度传感器的精度的小角度，且该角度越接近角速度传感器的精度越好，因此可以直接取值为角速度传感器的精度，或者在角速度传感器的精度上加以一个裕量或者乘以一个大于1的系数等。其具体可以根据实际需要选择所需的取值。记转动前姿态为姿态1，转动后姿态为姿态2，则有
姿态1的修正误差后的磁传感器测量值为：
Figure SMS_105
，
姿态2的修正误差后的磁传感器测量值为：
Figure SMS_106
，
其中，
Figure SMS_107
为姿态变换矩阵的理想值，/>
Figure SMS_108
、/>
Figure SMS_109
和/>
Figure SMS_110
分别为原始（修正误差前）的磁传感器测量值，/>
Figure SMS_111
为姿态1到姿态2的变换矩阵，可由陀螺的数据计算得到。本实施例方法最终要得到的结果是姿态变换矩阵，但是由于该结果是一个估计值，因此采用符号/>
Figure SMS_112
来表示姿态变换矩阵以便与姿态变换矩阵的理想值/>
Figure SMS_113
进行区分。
将姿态1、2的修正误差后的磁传感器测量值这两个矢量方程两边做矢乘，可得磁矢量叉积的方程：
Figure SMS_114
，
其中，
Figure SMS_115
为单位矩阵。
由于转动是指小角度的转动，因此有；
Figure SMS_116
，
上式中，
Figure SMS_117
、/>
Figure SMS_118
和/>
Figure SMS_119
分别为转动前后的角增量的三轴分量；将上式代入磁矢量叉积的方程可得：
Figure SMS_120
，
记：
Figure SMS_121
，/>
Figure SMS_122
，
则磁矢量叉积的方程可表示为：
Figure SMS_123
，
其中：
Figure SMS_124
为磁矢量叉积，R为参数矩阵，且有：
Figure SMS_125
，
若在上一步的标校过程中，每隔一个小转动取一次转动前后的磁敏感器输出，以及陀螺测得的姿态变换矩阵，则可以获得一系列的磁矢量叉积
Figure SMS_126
、/>
Figure SMS_127
，和一系列方程：
Figure SMS_128
，
因此所求问题即为求向量(
Figure SMS_129
)使得下面的代价函数的值最小：
Figure SMS_130
。
现在已经把问题转化为最小二乘的形式，当可观测性足够时（例如分别绕三个轴各转一周）可以解得九个参数，从而可采用最小二乘法估计磁传感器的磁敏感坐标系到角速度传感器的陀螺坐标系之间的姿态变换矩阵的初值。
本实施例中，步骤S101中采用最小二乘法估计磁传感器的磁敏感坐标系到角速度传感器的陀螺坐标系之间的姿态变换矩阵的初值包括：
S201，分别根据每一次转动前后两个姿态的修正误差后的磁传感器测量值以及角增量计算本次转动的磁矢量叉积以及参数矩阵；
S202，根据每一次转动的磁矢量叉积以及参数矩阵，采用最小二乘法估计磁传感器的磁敏感坐标系到角速度传感器的陀螺坐标系之间的姿态变换矩阵的初值。
本实施例中，步骤S201中磁矢量叉积以及参数矩阵的计算函数表达式为：
Figure SMS_131
，
Figure SMS_132
，
上式中，
Figure SMS_134
表示任意第i次转动的磁矢量叉积，/>
Figure SMS_138
、/>
Figure SMS_141
和/>
Figure SMS_136
分别为转动前姿态的修正误差后的磁传感器测量值，/>
Figure SMS_137
、/>
Figure SMS_140
和/>
Figure SMS_143
分别为转动后姿态的修正误差后的磁传感器测量值；/>
Figure SMS_135
表示任意第/>
Figure SMS_139
次转动的参数矩阵，/>
Figure SMS_142
、/>
Figure SMS_144
和/>
Figure SMS_133
分别为转动前后的角增量的三轴分量。
本实施例中，步骤S202中采用最小二乘法估计磁传感器的磁敏感坐标系到角速度传感器的陀螺坐标系之间的姿态变换矩阵的函数表达式为：
Figure SMS_145
，
上式中，
Figure SMS_147
、/>
Figure SMS_154
、/>
Figure SMS_157
、/>
Figure SMS_149
、/>
Figure SMS_152
、/>
Figure SMS_156
、/>
Figure SMS_159
、/>
Figure SMS_148
、/>
Figure SMS_151
分别为姿态变换矩阵/>
Figure SMS_155
的九个参数，/>
Figure SMS_158
表示任意第/>
Figure SMS_146
次转动的参数矩阵，/>
Figure SMS_150
表示任意第/>
Figure SMS_153
次转动得的磁矢量叉积，且姿态变换矩阵的函数表达式为：
Figure SMS_160
。
由九个参数
Figure SMS_162
、/>
Figure SMS_165
、/>
Figure SMS_168
、/>
Figure SMS_161
、/>
Figure SMS_164
、/>
Figure SMS_167
、/>
Figure SMS_170
、/>
Figure SMS_163
、/>
Figure SMS_166
构成的姿态变换矩阵/>
Figure SMS_169
并不一定满足正交归一条件，因此需要对当前的姿态变换矩阵进行正交化处理。姿态变换矩阵进行正交化处理为公知的现有方法，例如可根据需要采用四元数法或施密特正交化方法等。
本实施例中，步骤S103中使用当前的姿态变换矩阵估计姿态变换矩阵计算误差包括：
S301，使用当前的姿态变换矩阵计算本次迭代的磁矢量叉积；
Figure SMS_171
，/>
Figure SMS_172
，/>
Figure SMS_173
，
上式中，
Figure SMS_185
表示第/>
Figure SMS_175
次迭代的磁矢量叉积，/>
Figure SMS_182
、/>
Figure SMS_177
以及/>
Figure SMS_180
为转换到陀螺坐标系下的本次转动前姿态的修正误差后的磁传感器测量值的三轴分量，/>
Figure SMS_186
、/>
Figure SMS_189
以及/>
Figure SMS_181
为转换到陀螺坐标系下的本次转动后姿态的修正误差后的磁传感器测量值的三轴分量，/>
Figure SMS_184
为当前姿态变换矩阵/>
Figure SMS_176
的转置；/>
Figure SMS_179
、/>
Figure SMS_178
和/>
Figure SMS_183
为本次转动前姿态的修正误差后的磁传感器测量值的三轴分量，/>
Figure SMS_187
、/>
Figure SMS_188
和/>
Figure SMS_174
为本次转动后姿态的修正误差后的磁传感器测量值的三轴分量；
并根据下式计算本次迭代的参数矩阵：
Figure SMS_190
，
上式中，
Figure SMS_191
表示第/>
Figure SMS_192
次迭代的参数矩阵，A 1～A 5为系数；且有：
Figure SMS_193
，
Figure SMS_194
，
上式中，
Figure SMS_195
、/>
Figure SMS_198
、/>
Figure SMS_201
、/>
Figure SMS_197
、/>
Figure SMS_199
、/>
Figure SMS_202
、/>
Figure SMS_203
、/>
Figure SMS_196
、/>
Figure SMS_200
为当前的姿态变换矩阵的参数；
S302，根据所有次迭代的磁矢量叉积和参数矩阵，采用最小二乘法估计根据下式估计姿态变换矩阵计算误差：
Figure SMS_204
，
上式中，
Figure SMS_205
～/>
Figure SMS_206
分别为第1～n+1次迭代的磁矢量叉积，/>
Figure SMS_207
～/>
Figure SMS_208
分别为第1～n+1次迭代的参数矩阵，/>
Figure SMS_209
、/>
Figure SMS_210
和/>
Figure SMS_211
分别为姿态变换矩阵计算误差的三轴分量。
步骤S301中计算本次迭代的磁矢量叉积的函数表达式的推导过程如下：对磁矢量叉积的方程乘以姿态变换矩阵
Figure SMS_212
的转置/>
Figure SMS_213
，则有：
Figure SMS_214
，
由于姿态变换矩阵
Figure SMS_215
为姿态变换矩阵理想值/>
Figure SMS_216
的估计值（粗略值），可令：
Figure SMS_217
，
上式中，
Figure SMS_218
为单位矩阵，/>
Figure SMS_219
为角增量组成的反对称矩阵，则有：
Figure SMS_220
，
代入上式可得：
Figure SMS_221
，
在此基础上，为方便，记：
Figure SMS_222
，/>
Figure SMS_223
，
则可近似有：
Figure SMS_224
，
将：
Figure SMS_225
，/>
Figure SMS_226
，
代入上式并展开，则上式的右侧可表示为
Figure SMS_227
，其中:/>
Figure SMS_228
，
上式中，
Figure SMS_229
为参数矩阵，/>
Figure SMS_230
～/>
Figure SMS_231
为系数；且有：
Figure SMS_232
，
Figure SMS_233
，
上式中，
Figure SMS_234
、/>
Figure SMS_237
、/>
Figure SMS_240
、/>
Figure SMS_236
、/>
Figure SMS_239
、/>
Figure SMS_241
、/>
Figure SMS_242
、/>
Figure SMS_235
、/>
Figure SMS_238
为当前的姿态变换矩阵的参数；从而基于上式，可以通过最小二乘法解上述方程组。
本实施例中，步骤S104中基于姿态变换矩阵计算误差更新当前的姿态变换矩阵是指将当前的姿态变换矩阵乘以一个基于姿态变换矩阵计算误差得到的变化量
Figure SMS_243
，且有：
Figure SMS_244
，
上式中，
Figure SMS_245
、/>
Figure SMS_246
和/>
Figure SMS_247
分别为姿态变换矩阵计算误差的三轴分量，i、j和k分别表示当前的姿态变换矩阵中的单位向量。
本实施例中，修正误差后的磁传感器测量值是指修正磁传感器的非正交误差、灵敏度误差和零位误差。在外界磁干扰下，3轴磁传感器输出模型为：
Figure SMS_248
，
上式中，
Figure SMS_255
为磁传感器测量值，/>
Figure SMS_250
分别为/>
Figure SMS_253
的三轴分量，/>
Figure SMS_252
、/>
Figure SMS_257
、/>
Figure SMS_262
为x、y、z轴的灵敏度系数，/>
Figure SMS_266
(/>
Figure SMS_258
=x,y,z;/>
Figure SMS_263
=x,y,z;/>
Figure SMS_251
≠/>
Figure SMS_254
)为不正交系数，灵敏度系数和不正交系数一起构成了灵敏度矩阵/>
Figure SMS_259
。/>
Figure SMS_261
为修正误差后的磁传感器测量值，/>
Figure SMS_264
分别为/>
Figure SMS_265
的三轴分量，/>
Figure SMS_249
为磁传感器的零位误差，/>
Figure SMS_256
分别为/>
Figure SMS_260
的三轴分量。基于上述输出模型，本实施例中修正误差后的磁传感器测量值的计算函数表达式为：
Figure SMS_267
，/>
上式中，
Figure SMS_269
为修正误差后的磁传感器测量值，/>
Figure SMS_273
分别为/>
Figure SMS_275
的三轴分量，/>
Figure SMS_270
为灵敏度系数矩阵，/>
Figure SMS_272
为磁传感器测量值，/>
Figure SMS_276
分别为/>
Figure SMS_277
的三轴分量，/>
Figure SMS_268
为磁传感器的零位误差，/>
Figure SMS_271
分别为/>
Figure SMS_274
的三轴分量。本实施例中，步骤S101之前还包括估计灵敏度系数矩阵K m 的步骤：
S401，根据某一点处任意旋转磁传感器时三轴矢量和为该处磁场强度
Figure SMS_278
建立磁场方程：
Figure SMS_279
，
S402，将磁场方程展开：
Figure SMS_280
，
S403，将磁传感器测量值
Figure SMS_281
带入展开后的磁场方程并整理为：
Figure SMS_282
，
S404，将
Figure SMS_283
作为待辨识参数，在同一地点分别按照x、y、z三个轴旋转传感器得到多组磁传感器测量值/>
Figure SMS_284
，将多组磁传感器测量值
Figure SMS_285
利用最小二乘法估计出待辨识参数/>
Figure SMS_286
，并基于待辨识参数/>
Figure SMS_287
得到灵敏度系数矩阵/>
Figure SMS_288
，即可消除磁传感器的非正交误差、灵敏度误差和零位误差。
综上所述，由于直接确定磁传感器的磁敏感坐标系到角速度传感器的陀螺坐标系之间的姿态变换矩阵或其对应的四元数都比较困难，本实施例磁传感器和角速度传感器联合标定的方法采用了一种两步标定的方法，第一步为利用包含磁传感器和角速度传感器的惯性测量单元IMU转动做小量近似来求得姿态变换矩阵的初值；第二步为在第一步得到的姿态变换矩阵的初值的基础上，再取惯性测量单元IMU转动的精确值而将姿态变换矩阵的误差做小量近似来求得精确解。本实施例基于分析器件的误差特性，引入陀螺坐标系，基于角速度传感器输出的角速度建立姿态变换矩阵的误差模型以用于实时校正AHRS，可实现AHRS 的三轴捷联式的 MEMS 惯性/磁传感器的自主校准，基于 MEMS 惯性/磁传感器组合的两步自主校准，能更有效提高传感器的测量精度。本实施例的联合标定方法中数据采集与校准同步实现，实时性高。在得到姿态变换矩阵的初值后，控制角速度传感器执行一次转动，利用角速度传感器和磁传感器的测量值计算磁坐标系到陀螺坐标系变换矩阵的估计姿态变换矩阵计算误差，并利用该姿态变换矩阵计算误差实时更新变换矩阵，从而在采集数据的同时接口完成磁坐标系到陀螺坐标系的迭代优化。本实施例的方法无需外部设备辅助，成本低。
此外，本实施例还提供一种磁传感器和角速度传感器联合标定的系统，包括相互连接的微处理器和存储器，该微处理器被编程或配置以执行该磁传感器和角速度传感器联合标定的方法。此外，本实施例还提供一种计算机可读存储介质，该计算机可读存储介质中存储有计算机程序，该计算机程序用于被微处理器编程或配置以执行该磁传感器和角速度传感器联合标定的方法。
本领域内的技术人员应明白，本申请的实施例可提供为方法、系统、或计算机程序产品。因此，本申请可采用完全硬件实施例、完全软件实施例、或结合软件和硬件方面的实施例的形式。而且，本申请可采用在一个或多个其中包含有计算机可用程序代码的计算机可读存储介质（包括但不限于磁盘存储器、CD-ROM、光学存储器等）上实施的计算机程序产品的形式。本申请是参照根据本申请实施例的方法、设备（系统）、和计算机程序产品的流程图和／或方框图来描述的。应理解可由计算机程序指令实现流程图和／或方框图中的每一流程和／或方框、以及流程图和／或方框图中的流程和／或方框的结合。可提供这些计算机程序指令到通用计算机、专用计算机、嵌入式处理机或其他可编程数据处理设备的处理器以产生一个机器，使得通过计算机或其他可编程数据处理设备的处理器执行的指令产生用于实现在流程图一个流程或多个流程和／或方框图一个方框或多个方框中指定的功能的装置。这些计算机程序指令也可存储在能引导计算机或其他可编程数据处理设备以特定方式工作的计算机可读存储器中，使得存储在该计算机可读存储器中的指令产生包括指令装置的制造品，该指令装置实现在流程图一个流程或多个流程和／或方框图一个方框或多个方框中指定的功能。这些计算机程序指令也可装载到计算机或其他可编程数据处理设备上，使得在计算机或其他可编程设备上执行一系列操作步骤以产生计算机实现的处理，从而在计算机或其他可编程设备上执行的指令提供用于实现在流程图一个流程或多个流程和／或方框图一个方框或多个方框中指定的功能的步骤。
以上所述仅是本发明的优选实施方式，本发明的保护范围并不仅局限于上述实施例，凡属于本发明思路下的技术方案均属于本发明的保护范围。应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理前提下的若干改进和润饰，这些改进和润饰也应视为本发明的保护范围。

Claims (10)

1.一种磁传感器和角速度传感器联合标定的方法，其特征在于，包括：
S101，控制磁传感器和角速度传感器执行多次转动，获取每一次转动前后两个姿态的修正误差后的磁传感器测量值以及角增量，并采用最小二乘法估计磁传感器的磁敏感坐标系到角速度传感器的陀螺坐标系之间的姿态变换矩阵的初值；
S102，对当前的姿态变换矩阵进行正交化处理；
S103，基于当前的姿态变换矩阵，利用每一次转动前后两个姿态的修正误差后的磁传感器测量值以及角增量，估计姿态变换矩阵计算误差；
S104，判断姿态变换矩阵计算误差小于设定值是否成立，若成立则将当前的姿态变换矩阵输出，退出；否则基于姿态变换矩阵计算误差更新当前的姿态变换矩阵，跳转步骤S102继续迭代。
2.根据权利要求1所述的磁传感器和角速度传感器联合标定的方法，其特征在于，步骤S101中采用最小二乘法估计磁传感器的磁敏感坐标系到角速度传感器的陀螺坐标系之间的姿态变换矩阵的初值包括：
S201，分别根据每一次转动前后两个姿态的修正误差后的磁传感器测量值以及角增量计算本次转动的磁矢量叉积以及参数矩阵；
S202，根据每一次转动的磁矢量叉积以及参数矩阵，采用最小二乘法估计磁传感器的磁敏感坐标系到角速度传感器的陀螺坐标系之间的姿态变换矩阵的初值。
3.根据权利要求2所述的磁传感器和角速度传感器联合标定的方法，其特征在于，步骤S201中磁矢量叉积以及参数矩阵的计算函数表达式为：
Figure QLYQS_1
，
Figure QLYQS_2
，
上式中，
Figure QLYQS_5
表示任意第
Figure QLYQS_7
次转动的磁矢量叉积，
Figure QLYQS_12
、
Figure QLYQS_6
和
Figure QLYQS_8
分别为转动前姿态的修正误差后的磁传感器测量值，
Figure QLYQS_11
、
Figure QLYQS_15
和
Figure QLYQS_4
分别为转动后姿态的修正误差后的磁传感器测量值；
Figure QLYQS_9
表示任意第
Figure QLYQS_13
次转动的参数矩阵，
Figure QLYQS_14
、
Figure QLYQS_3
和
Figure QLYQS_10
分别为转动前后的角增量的三轴分量。
4.根据权利要求2所述的磁传感器和角速度传感器联合标定的方法，其特征在于，步骤S202中采用最小二乘法估计磁传感器的磁敏感坐标系到角速度传感器的陀螺坐标系之间的姿态变换矩阵的函数表达式为：
Figure QLYQS_16
，
上式中，
Figure QLYQS_17
、
Figure QLYQS_21
、
Figure QLYQS_25
、
Figure QLYQS_19
、
Figure QLYQS_22
、
Figure QLYQS_26
、
Figure QLYQS_29
、
Figure QLYQS_18
、
Figure QLYQS_24
分别为姿态变换矩阵
Figure QLYQS_28
的九个参数，
Figure QLYQS_30
表示任意第
Figure QLYQS_20
次转动的参数矩阵，
Figure QLYQS_23
表示任意第
Figure QLYQS_27
次转动得的磁矢量叉积，且姿态变换矩阵的函数表达式为：
Figure QLYQS_31
。
5.根据权利要求1所述的磁传感器和角速度传感器联合标定的方法，其特征在于，步骤S103中使用当前的姿态变换矩阵估计姿态变换矩阵计算误差包括：
S301，使用当前的姿态变换矩阵计算本次迭代的磁矢量叉积；
Figure QLYQS_32
，
Figure QLYQS_33
，
Figure QLYQS_34
，
上式中，
Figure QLYQS_46
表示第
Figure QLYQS_37
次迭代的磁矢量叉积，
Figure QLYQS_39
、
Figure QLYQS_45
以及
Figure QLYQS_49
为转换到陀螺坐标系下的本次转动前姿态的修正误差后的磁传感器测量值的三轴分量，
Figure QLYQS_48
、
Figure QLYQS_50
以及
Figure QLYQS_40
为转换到陀螺坐标系下的本次转动后姿态的修正误差后的磁传感器测量值的三轴分量，
Figure QLYQS_44
为当前姿态变换矩阵
Figure QLYQS_35
的转置；
Figure QLYQS_42
、
Figure QLYQS_36
和
Figure QLYQS_41
为本次转动前姿态的修正误差后的磁传感器测量值的三轴分量，
Figure QLYQS_43
、
Figure QLYQS_47
和
Figure QLYQS_38
为本次转动后姿态的修正误差后的磁传感器测量值的三轴分量；
并根据下式计算本次迭代的参数矩阵：
Figure QLYQS_51
，
上式中，
Figure QLYQS_52
表示第
Figure QLYQS_53
次迭代的参数矩阵，
Figure QLYQS_54
～
Figure QLYQS_55
为系数；且有：
Figure QLYQS_56
，
Figure QLYQS_57
，
上式中，
Figure QLYQS_60
、
Figure QLYQS_61
、
Figure QLYQS_64
、
Figure QLYQS_58
、
Figure QLYQS_63
、
Figure QLYQS_65
、
Figure QLYQS_66
、
Figure QLYQS_59
、
Figure QLYQS_62
为当前的姿态变换矩阵的参数；
S302，根据所有次迭代的磁矢量叉积和参数矩阵，采用最小二乘法估计根据下式估计姿态变换矩阵计算误差：
Figure QLYQS_67
，
上式中，
Figure QLYQS_68
～
Figure QLYQS_69
分别为第1～n+1次迭代的磁矢量叉积，
Figure QLYQS_70
～
Figure QLYQS_71
分别为第1～n+1次迭代的参数矩阵，
Figure QLYQS_72
、
Figure QLYQS_73
和
Figure QLYQS_74
分别为姿态变换矩阵计算误差的三轴分量。
6.根据权利要求5所述的磁传感器和角速度传感器联合标定的方法，其特征在于，步骤S104中基于姿态变换矩阵计算误差更新当前的姿态变换矩阵是指将当前的姿态变换矩阵乘以一个基于姿态变换矩阵计算误差得到的变化量
Figure QLYQS_75
，且有：
Figure QLYQS_76
，
上式中，
Figure QLYQS_77
、
Figure QLYQS_78
和
Figure QLYQS_79
分别为姿态变换矩阵计算误差的三轴分量，
Figure QLYQS_80
、
Figure QLYQS_81
和
Figure QLYQS_82
分别表示当前的姿态变换矩阵中的单位向量。
7.根据权利要求1所述的磁传感器和角速度传感器联合标定的方法，其特征在于，所述修正误差后的磁传感器测量值的计算函数表达式为：
Figure QLYQS_83
，
上式中，
Figure QLYQS_85
为修正误差后的磁传感器测量值，
Figure QLYQS_87
分别为
Figure QLYQS_90
的三轴分量，
Figure QLYQS_86
为灵敏度系数矩阵，
Figure QLYQS_89
为磁传感器测量值，
Figure QLYQS_91
分别为
Figure QLYQS_93
的三轴分量，
Figure QLYQS_84
为磁传感器的零位误差，
Figure QLYQS_88
分别为
Figure QLYQS_92
的三轴分量。
8.根据权利要求7所述的磁传感器和角速度传感器联合标定的方法，其特征在于，步骤S101之前还包括估计灵敏度系数矩阵
Figure QLYQS_94
的步骤：
S401，根据某一点处任意旋转磁传感器时三轴矢量和为该处磁场强度B建立磁场方程：
Figure QLYQS_95
，
S402，将磁场方程展开：
Figure QLYQS_96
，
S403，将磁传感器测量值
Figure QLYQS_97
带入展开后的磁场方程并整理为：
Figure QLYQS_98
，
S404，将
Figure QLYQS_99
作为待辨识参数，在同一地点分别按照x、y、z三个轴旋转传感器得到多组磁传感器测量值
Figure QLYQS_100
，将多组磁传感器测量值
Figure QLYQS_101
利用最小二乘法估计出待辨识参数
Figure QLYQS_102
，并基于待辨识参数
Figure QLYQS_103
得到灵敏度系数矩阵
Figure QLYQS_104
。
9.一种磁传感器和角速度传感器联合标定的系统，包括相互连接的微处理器和存储器，其特征在于，所述微处理器被编程或配置以执行权利要求1～8中任意一项所述磁传感器和角速度传感器联合标定的方法。
10.一种计算机可读存储介质，所述计算机可读存储介质中存储有计算机程序，其特征在于，所述计算机程序用于被微处理器编程或配置以执行权利要求1～8中任意一项所述磁传感器和角速度传感器联合标定的方法。
CN202310152338.8A 2023-02-23 2023-02-23 磁传感器和角速度传感器联合标定的方法、系统及介质 Active CN115839726B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202310152338.8A CN115839726B (zh) 2023-02-23 2023-02-23 磁传感器和角速度传感器联合标定的方法、系统及介质

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202310152338.8A CN115839726B (zh) 2023-02-23 2023-02-23 磁传感器和角速度传感器联合标定的方法、系统及介质

Publications (2)

Publication Number Publication Date
CN115839726A true CN115839726A (zh) 2023-03-24
CN115839726B CN115839726B (zh) 2023-04-28

Family

ID=85580038

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202310152338.8A Active CN115839726B (zh) 2023-02-23 2023-02-23 磁传感器和角速度传感器联合标定的方法、系统及介质

Country Status (1)

Country Link
CN (1) CN115839726B (zh)

Citations (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20090254294A1 (en) * 2008-03-06 2009-10-08 Texas Instruments Incorporated Processes for more accurately calibrating and operating e-compass for tilt error, circuits, and systems
US20140214364A1 (en) * 2013-01-31 2014-07-31 Yamaha Corporation Method of estimating offset of magnetic sensor
US20150177020A1 (en) * 2012-08-02 2015-06-25 Memsic, Inc. Method and apparatus for data fusion of a three-axis magnetometer and three axis accelerometer
CN105547326A (zh) * 2015-12-08 2016-05-04 上海交通大学 陀螺与磁传感器联合标定方法
CN106197475A (zh) * 2016-06-29 2016-12-07 上海交通大学 基于序贯滤波的陀螺与磁传感器联合标定方法
US20180080768A1 (en) * 2016-02-01 2018-03-22 Limited Liability Company "Topcon Positioning Systems" A method and device for calibration of a three-axis magnetometer
CN110031020A (zh) * 2019-03-28 2019-07-19 广州英卓电子科技有限公司 一种平面磁场校正方法及其装置
CN110174123A (zh) * 2019-05-08 2019-08-27 苏州大学 一种磁传感器实时标定方法
CN112461224A (zh) * 2020-11-10 2021-03-09 武汉大学 一种基于已知姿态角的磁力计标定方法

Patent Citations (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20090254294A1 (en) * 2008-03-06 2009-10-08 Texas Instruments Incorporated Processes for more accurately calibrating and operating e-compass for tilt error, circuits, and systems
US20150177020A1 (en) * 2012-08-02 2015-06-25 Memsic, Inc. Method and apparatus for data fusion of a three-axis magnetometer and three axis accelerometer
US20140214364A1 (en) * 2013-01-31 2014-07-31 Yamaha Corporation Method of estimating offset of magnetic sensor
CN105547326A (zh) * 2015-12-08 2016-05-04 上海交通大学 陀螺与磁传感器联合标定方法
US20180080768A1 (en) * 2016-02-01 2018-03-22 Limited Liability Company "Topcon Positioning Systems" A method and device for calibration of a three-axis magnetometer
CN106197475A (zh) * 2016-06-29 2016-12-07 上海交通大学 基于序贯滤波的陀螺与磁传感器联合标定方法
CN110031020A (zh) * 2019-03-28 2019-07-19 广州英卓电子科技有限公司 一种平面磁场校正方法及其装置
CN110174123A (zh) * 2019-05-08 2019-08-27 苏州大学 一种磁传感器实时标定方法
CN112461224A (zh) * 2020-11-10 2021-03-09 武汉大学 一种基于已知姿态角的磁力计标定方法

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
向甜等: "三轴磁强计与MEMS陀螺的在轨实时标定方法" *
王勇军等: "无人机磁惯导系统中陀螺仪的叉积标定算法" *

Also Published As

Publication number Publication date
CN115839726B (zh) 2023-04-28

Similar Documents

Publication Publication Date Title
CN108827299B (zh) 一种基于改进四元数二阶互补滤波的飞行器姿态解算方法
Wu et al. Generalized linear quaternion complementary filter for attitude estimation from multisensor observations: An optimization approach
Hyyti et al. A DCM Based Attitude Estimation Algorithm for Low-Cost MEMS IMUs.
Kubelka et al. Complementary filtering approach to orientation estimation using inertial sensors only
US9417091B2 (en) System and method for determining and correcting field sensors errors
Guo et al. Novel MARG-sensor orientation estimation algorithm using fast Kalman filter
Li et al. Gradient descent optimization-based self-alignment method for stationary SINS
WO2008120145A1 (en) Method and system for orientation sensing
JP7025215B2 (ja) 測位システム及び測位方法
CN103299247A (zh) 用于磁近场的动态追踪及补偿的设备和方法
CN111189474A (zh) 基于mems的marg传感器的自主校准方法
CN111189442A (zh) 基于cepf的无人机多源导航信息状态预测方法
CN111413651B (zh) 一种磁场总场的补偿方法、装置、系统及存储介质
CN107860382B (zh) 一种在地磁异常情况下应用ahrs测量姿态的方法
Aligia et al. An orientation estimation strategy for low cost IMU using a nonlinear Luenberger observer
Peng et al. Design of an embedded icosahedron mechatronics for robust iterative IMU calibration
CN108592902B (zh) 一种基于多传感器的定位设备及定位方法、系统和机械臂
CN115033844A (zh) 一种无人机状态估计方法、系统、设备及可读存储介质
CN108871319B (zh) 一种基于地球重力场与地磁场序贯修正的姿态解算方法
CN103954288A (zh) 一种卫星姿态确定系统精度响应关系确定方法
Chen et al. High-precision geomagnetic directional technology based on sensor error correction and adaptive hybrid filter
CN110375773B (zh) Mems惯导系统姿态初始化方法
CN112729348A (zh) 一种用于imu系统的姿态自适应校正方法
CN111982126A (zh) 一种全源BeiDou/SINS弹性状态观测器模型设计方法
CN111024071A (zh) Gnss辅助的加速度计和陀螺仪常值漂移估算的导航方法及系统

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