CN110530397A - 一种基于九轴imu的磁场自动标定方法 - Google Patents

Abstract

本发明提供的一种基于九轴IMU的磁场自动标定方法，包括如下步骤：数据采集、数据滤波、姿态解算、磁力计倾斜补偿和模型优化、基于SVD分解的椭圆参数求解和计算航向角。本发明提出了一种基于九轴IMU的磁场自动校准方法，对磁场“硬铁”，“软铁”误差进行校准，改进了最小二乘方法的弊端，自动筛选数据，简化了模型，降低了校准模型计算量，同时确保参数估计的正确性；补偿了磁力计载体平台姿态变化带来的误差；通过对采集数据进行分析，智能触发校准条件，不需要人为参与复杂的空间运动动作，实现磁力计的自动校准。

Description

一种基于九轴IMU的磁场自动标定方法

技术领域

本发明涉及导航定位领域，具体涉及一种基于九轴IMU的磁场自动标定方法。

背景技术

导航***是人工智能蓬勃发展过程中的一项重要技术，在汽车驾驶、航海、航空、智能机器人、行人定位等方面已经得到了广泛的应用，这其中涉及到多种传感器的组合使用，磁力计(电子罗盘)就是其中重要的组成部分，它单独或与其他传感器结合使用，用于导航方向的测量中。磁力计是一种利用霍尔元件测量地球磁场的传感器，其功能与指南针类似。由于地磁的方向除南北极之外基本都是一样的，从南指向北，磁力计就担当了测量该方向的作用，给人类导航活动提供了重要的支撑。

然而实际使用时，利用磁力计导航还存在一些问题，地磁场的强度是比较弱的，大约只有0.5 Gauss，而距离一个普通的手机喇叭2厘米时就会有约4 Gauss的磁场强度，一个手机马达附近的磁场就达到6 Gausss。由此可知，地磁场非常容易受到电子设备的干扰的；不仅如此，带电设备运行过程中产生的电流、环境中的金属，如铁镍钴一类的物质也会对磁场产生干扰，造成磁力计测量误差。一般来说，所谓磁场干扰是指由于具有磁性的或者可以影响局部磁场强度的物质存在时，导致磁传感器所放置的局部区域内的地球磁场发生了偏差的现象。磁场干扰会造成磁力计测量航向角度的误差，甚至导致航向测量完全相反的结果。

考虑到导航环境的磁场干扰因素带来的严重后果，因此在使用磁力计时必须对其附近的磁场进行校准。校准的原理是：将磁力计在空间中进行全方位的旋转，若附近没有磁场干扰，磁力计三轴输出点集R＝{x,y,z}在空间将构成一个以坐标原点(零点)为中心，半径约为0.5 Guass的圆球体。若附近有磁场干扰，零点会偏移，这个误差叫做“硬铁”干扰；同时不再是一个“球体”，而变成一个“椭球体”,造成这个的误差叫做“软铁”干扰。对磁力计进行校准，也就是修正“硬铁”与“软铁”干扰，由于“硬铁”干扰带来的误差远大于“软铁”误差，在很多精度要求不高的应用中只校准“硬铁”误差，即只计算球体的零点偏移量。

目前一般磁场校准的方法有：1.平面校准法；2.立体8字校准法；3.十面校准法；4.最小二乘拟合法。其中方法1-3都是通过固定姿势旋转磁力计实现校准(方法1，将磁力计水平放置，旋转360度；方法2，拿着磁力计，在空间中绕“8”字运动，该方法目前在手机里面广泛用；方法3，用磁力计摆出10个固定的姿势)，通过这些方法，获取磁力计各个轴磁场强度的最大值与最小值，并取算术平均作为磁力计各轴的“零点偏移”；方法4是将磁力计放在磁场强度已知的恒定磁场中，采集磁力计数据，建立误差模型，然后利用最小二乘思想估计磁力计校准参数。现有技术的问题如下：问题一，方法1-3，虽然方便简单，但仅仅能估计“零点偏移”，不能对“软铁”误差校准，在很多导航应用中，会带来较大的误差；方法4需要大量的符合要求的数据，一方面是计算量比较大，收敛较慢，这在一些实时应用方案或低端嵌入式***中是比较难实现的；另一方面如果采集的数据运动跨度不大，比如只是进行了小角度运动，将会导致模型估计失败。问题二，方法1-4都需要人为参与进去，执行特定的动作，一是不够智能，二是在很多实际导航应用中是很难实现的。比如无人车中的磁场校准，很难将车多方位翻转进行校准，如果要做也是非常麻烦与耗时的；又比如行人在室内导航定位的应用中，运动过程中的磁场经常是变化的，在执行任务的时候，不可能经常的人为参与进来进行磁场校准。问题三，磁场校准的目的是为了计算航向，在校准后实际测量航向时，需要水平放置，若磁力计倾斜了，则会造成较大的测量误差。在实际应用中，磁力计载体往往姿态变化是比较大的，这样测量的航向在姿态变化过程中，会引入很大误差。

发明内容

针对现有技术中的缺陷，本发明提出了一种基于九轴IMU的磁场自动校准方法，对磁场“硬铁”，“软铁”误差进行校准，改进了最小二乘方法的弊端，自动筛选数据，简化了模型，降低了校准模型计算量，同时确保参数估计的正确性；补偿了磁力计载体平台姿态变化带来的误差；通过对采集数据进行分析，智能触发校准条件，不需要人为参与复杂的空间运动动作，实现磁力计的自动校准。

本发明提供的一种基于九轴IMU的磁场自动标定方法，包括如下步骤：

S1、数据采集，利用九轴IMU采集载体平台的相关数据，其中所述九轴IMU由三轴加速度计、三轴陀螺仪和三轴磁力计组成；

S2、数据滤波，对三轴加速度计的数据进行低通滤波，对三轴陀螺仪的数据进行带通滤波；

S3、姿态解算，依据三轴加速度计和三轴陀螺仪的数据，计算出载体平台的俯仰角和滚转角；

S4、磁力计倾斜补偿和模型优化，利用步骤S3求得的俯仰角和滚转角，将三轴磁力计的数据由载体坐标系，转换到导航坐标系下，从而将三轴磁力计在空间各方位旋转所围成的椭球图像转换为平面的椭圆；

S5、基于SVD分解的椭圆参数求解，根据椭圆目标优化函数，通过变形及变量变换，将步骤S4得到的椭圆转化为齐次方程组，利用SVD分解，求方程组的解；

S6、计算航向角，步骤S5中求得的方程组的解，得到校准参数，根据校准参数，对磁力计数据修正，然后计算航向角。

可选地，步骤S3中姿态解算的具体步骤如下：

S31、姿态更新，将步骤S1中三轴陀螺仪测量的三轴角速度，代入四元数微分方程求解，根据龙哥库塔法，由陀螺仪积分更新四元数，设三轴陀螺仪测量的数据为g＝(gx,gy,gz)，采样时间为t，则：

其中，w＝sqrt(gx*gx+gy*gy+gz*gz)，q′为更新后的四元数，q＝[q₀ q₁ q₂ q₃]^T为上一时刻的四元数；

S32、误差修正，采用EKF方法对误差进行修正，设修正的四元数向量为err_q，修正后的四元数为q，则修正后真实的四元数为：

q＝q′+err_q (2)；

S33、求解姿态角

根据步骤S32获得修正后的四元数后，求解出所需的姿态，并转化为欧拉角，计算公式为：

pitch＝atan(2q1q2+2q3q0,1-2q0q0-2q1q1)

roll＝asin(-2q0q2+2q3q1)

yaw＝-astan(2q3q2+2q0q1,1-2q1q1-2q2q2) (3)

其中pitch代表俯仰角，roll代表滚转角，yaw代表偏航角。

可选地，步骤S4中磁力计倾斜补偿的具体步骤如下：

设三轴磁力计k时刻的输出为该输出是为载体坐标系的坐标，而根据磁力计航向计算公式：

其中，D为磁偏角，各个地区会有差别，可由查表获得，B_y，B_x分别为地磁场在水平方向上的投影，在导航坐标系内分解到水平y轴及x轴的正交分量，利用步骤S23解算的姿态信息，转换到导航系下,得到B_y，B_x，计算公式为：

可选地，步骤S4中的模型优化的方法如下：

在理想情况下，当磁力计处于理想地磁场中时的输出模型满足：

其中M_MAG(m_x,m_y,m_z)为磁力计测量数据，G为理想地磁场，约为0.5Guass，具体可以通过查表获得，

由于量化因子的误差及零偏存在，因此磁力计的输出模型满足为：

(k_x(m_x+b_x))²+(k_y(m_y+b_y))²+(k_z(m_z+b_z))²＝G² (7)

其中，k_x,k_y,k_z为尺度因子，即之前所说的“软铁”误差，b_x,b_y,b_z为零偏，即“硬铁”误差，

利用姿态解算的俯仰、滚转角进行了倾斜补偿，将磁力计的输出模型简化为：

(k_x′B_x+b_x′)²+(k_y′B_y+b_y′)²＝G′² (8)

令

B_x′＝k_x′B_x+b_x′

B_y′＝k_y′B_y+b_y′ (9)

则可得到如下优化模型：

可选地，步骤S5中基于SVD分解的椭圆参数求解方法如下：

将(10)式通过变形及变量变换可以得到线性齐次方程组：

AgV＝0 (11)

其中

向量V即该齐次方程的解，该解可通过SVD分解的方法求得，需要说明的是向量V并不是方程组(10)的直接解，需要经过如下变换，

求得磁力计校准参数k_x′,k_y′,b_x′,b_y′后，代入(10)中求得校准后的数据。

由上述技术方案可知，本发明的有益效果：本发明提供的一种基于九轴IMU的磁场自动标定方法，包括如下步骤：数据采集、数据滤波、姿态解算、磁力计倾斜补偿和模型优化、基于SVD分解的椭圆参数求解和计算航向角。本发明提出了一种基于九轴IMU的磁场自动校准方法，对磁场“硬铁”，“软铁”误差进行校准，改进了最小二乘方法的弊端，自动筛选数据，简化了模型，降低了校准模型计算量，同时确保参数估计的正确性；补偿了磁力计载体平台姿态变化带来的误差；通过对采集数据进行分析，智能触发校准条件，不需要人为参与复杂的空间运动动作，实现磁力计的自动校准。

附图说明

为了更清楚地说明本发明具体实施方式或现有技术中的技术方案，下面将对具体实施方式或现有技术描述中所需要使用的附图作简单地介绍。在所有附图中，类似的元件或部分一般由类似的附图标记标识。附图中，各元件或部分并不一定按照实际的比例绘制。

图1为本发明的算法框图；

图2为磁力计磁场校准前后数据对比图。

附图标记：

1-校准后样本数据分布的轨迹、2-校准前样本数据分布的轨迹。

具体实施方式

下面将结合附图对本发明技术方案的实施例进行详细的描述。以下实施例仅用于更加清楚地说明本发明的技术方案，因此只作为示例，而不能以此来限制本发明的保护范围。

请参阅图，本实施例提供的一种基于九轴IMU的磁场自动标定方法，其特征在于，包括如下步骤：

S1、数据采集，利用九轴IMU采集载体平台的相关数据，其中所述九轴IMU由三轴加速度计、三轴陀螺仪和三轴磁力计组成；

S2、数据滤波，对三轴加速度计的数据进行低通滤波，对三轴陀螺仪的数据进行带通滤波；

S3、姿态解算，依据三轴加速度计和三轴陀螺仪的数据，计算出载体平台的俯仰角和滚转角；姿态解算的具体步骤如下：

S31、姿态更新，将步骤S1中三轴陀螺仪测量的三轴角速度，代入四元数微分方程求解，根据龙哥库塔法，由陀螺仪积分更新四元数，设三轴陀螺仪测量的数据为g＝(gx,gy,gz)，采样时间为t，则：

其中，w＝sqrt(gx*gx+gy*gy+gz*gz)，q′为更新后的四元数，q＝[q₀ q₁ q₂ q₃]^T为上一时刻的四元数；

S32、误差修正，由于单独的用陀螺仪积分解算的姿态角会漂移，因此需要由加速度计进行修正，修正的方法有互补滤波和卡尔曼滤波法，为了更准确地估计状态，本发明采用EKF方法(扩展卡尔曼滤波)对误差进行修正，设修正的四元数向量为err_q，修正后的四元数为q，则修正后真实的四元数为：

q＝q′+err_q (2)；

S33、求解姿态角

根据步骤S32获得修正后的四元数后，求解出所需的姿态，并转化为欧拉角，计算公式为：

pitch＝atan(2q1q2+2q3q0,1-2q0q0-2q1q1)

roll＝asin(-2q0q2+2q3q1)

yaw＝-astan(2q3q2+2q0q1,1-2q1q1-2q2q2) (3)

其中pitch代表俯仰角，roll代表滚转角，yaw代表偏航角。注意，解算的yaw角是相对角，无绝对初始值，一般初始化为零，在本发明姿态倾斜补偿中没有使用。

S4、磁力计倾斜补偿和模型优化，利用步骤S3求得的俯仰角和滚转角，将三轴磁力计的数据由载体坐标系，转换到导航坐标系下，从而将三轴磁力计在空间各方位旋转所围成的椭球图像转换为平面的椭圆；

磁力计倾斜补偿的具体步骤如下：

设三轴磁力计k时刻的输出为该输出是为载体坐标系的坐标，而根据磁力计航向计算公式：

其中，D为磁偏角，各个地区会有差别，可由查表获得，B_y，B_x分别为地磁场在水平方向上的投影，在导航坐标系内分解到水平y轴及x轴的正交分量，由此可知，磁力计的倾斜补偿方法就是利用步骤S23解算的姿态信息，转换到导航系下,得到B_y，B_x，计算公式为：

由此可知，磁力计的校准问题，通过倾角补偿，可转化为对水平面双轴磁力的校准问题。这样，极大地简化了后续凸优化过程中算法难度及计算量。

模型优化的方法如下：

在理想情况下，当磁力计处于理想地磁场中时的输出模型满足：

其中M_MAG(m_x,m_y,m_z)为磁力计测量数据，G为理想地磁场，约为0.5Guass，具体可以通过查表获得，

实际应用中，由于量化因子的误差及零偏存在，因此磁力计的输出模型满足为：

(k_x(m_x+b_x))²+(k_y(m_y+b_y))²+(k_z(m_z+b_z))²＝G² (7)

其中，k_x,k_y,k_z为尺度因子，即之前所说的“软铁”误差，b_x,b_y,b_z为零偏，即“硬铁”误差，

在本实施例中，利用姿态解算的俯仰、滚转角进行了倾斜补偿，将磁力计的输出模型简化为：

(k_x′B_x+b_x′)²+(k_y′B_y+b_y′)²＝G′² (8)

这样，磁力计的校准，即求k_x′,k_y′,b_x′,b_y′这4个未知量的过程。

令

B_x′＝k_x′B_x+b_x′

B_y′＝k_y′B_y+b_y′ (9)

则可得到如下优化模型：

S5、基于SVD分解的椭圆参数求解，根据椭圆目标优化函数，通过变形及变量变换，将步骤S4得到的椭圆转化为齐次方程组，利用SVD分解，求方程组的解；

基于SVD分解的椭圆参数求解方法如下：

将(10)式通过变形及变量变换可以得到线性齐次方程组：

AgV＝0 (11)

其中

向量V即该齐次方程的解，该解可通过SVD分解的方法求得，需要说明的是向量V并不是方程组(10)的直接解，需要经过如下变换，

求得磁力计校准参数k_x′,k_y′,b_x′,b_y′后，代入(10)中求得校准后的数据。

S6、计算航向角，步骤S5中求得的方程组的解，得到校准参数，根据校准参数，对磁力计数据修正，然后计算航向角。

本发明提出了一种基于九轴IMU的磁场自动校准方法，对磁场“硬铁”，“软铁”误差进行校准，改进了最小二乘方法的弊端，自动筛选数据，简化了模型，降低了校准模型计算量，同时确保参数估计的正确性；补偿了磁力计载体平台姿态变化带来的误差；通过对采集数据进行分析，智能触发校准条件，不需要人为参与复杂的空间运动动作，实现磁力计的自动校准。

最后应说明的是：以上各实施例仅用以说明本发明的技术方案，而非对其限制；尽管参照前述各实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分或者全部技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的范围，其均应涵盖在本发明的权利要求和说明书的范围当中。

Claims (5)

1.一种基于九轴IMU的磁场自动标定方法，其特征在于，包括如下步骤：

S1、数据采集，利用九轴IMU采集载体平台的相关数据，其中所述九轴IMU由三轴加速度计、三轴陀螺仪和三轴磁力计组成；

S2、数据滤波，对三轴加速度计的数据进行低通滤波，对三轴陀螺仪的数据进行带通滤波；

S3、姿态解算，依据三轴加速度计和三轴陀螺仪的数据，计算出载体平台的俯仰角和滚转角；

S4、磁力计倾斜补偿和模型优化，利用步骤S3求得的俯仰角和滚转角，将三轴磁力计的数据由载体坐标系，转换到导航坐标系下，从而将三轴磁力计在空间各方位旋转所围成的椭球图像转换为平面的椭圆；

S5、基于SVD分解的椭圆参数求解，根据椭圆目标优化函数，通过变形及变量变换，将步骤S4得到的椭圆转化为齐次方程组，利用SVD分解，求方程组的解；

S6、计算航向角，步骤S5中求得的方程组的解，得到校准参数，根据校准参数，对磁力计数据修正，然后计算航向角。

2.根据权利要求1所述的一种基于九轴IMU的磁场自动标定方法，其特征在于，步骤S3中姿态解算的具体步骤如下：

S31、姿态更新，将步骤S1中三轴陀螺仪测量的三轴角速度，代入四元数微分方程求解，根据龙哥库塔法，由陀螺仪积分更新四元数，设三轴陀螺仪测量的数据为g＝(gx,gy,gz)，采样时间为t，则：

其中，w＝sqrt(gx*gx+gy*gy+gz*gz)，q′为更新后的四元数，q＝[q₀ q₁ q₂ q₃]^T为上一时刻的四元数；

S32、误差修正，采用EKF方法对误差进行修正，设修正的四元数向量为err_q，修正后的四元数为q，则修正后真实的四元数为：

q＝q′+err_q (2)；

S33、求解姿态角

根据步骤S32获得修正后的四元数后，求解出所需的姿态，并转化为欧拉角，计算公式为：

pitch＝atan(2q1q2+2q3q0,1-2q0q0-2q1q1)

roll＝asin(-2q0q2+2q3q1)

yaw＝-astan(2q3q2+2q0q1,1-2q1q1-2q2q2) (3)

其中pitch代表俯仰角，roll代表滚转角，yaw代表偏航角。

3.根据权利要求1所述的一种基于九轴IMU的磁场自动标定方法，其特征在于，步骤S4中磁力计倾斜补偿的具体步骤如下：

设三轴磁力计k时刻的输出为该输出是为载体坐标系的坐标，而根据磁力计航向计算公式：

其中，D为磁偏角，各个地区会有差别，可由查表获得，B_y，B_x分别为地磁场在水平方向上的投影，在导航坐标系内分解到水平y轴及x轴的正交分量，

利用步骤S23解算的姿态信息，转换到导航系下,得到B_y，B_x，计算公式为：

4.根据权利要求1所述的一种基于九轴IMU的磁场自动标定方法，其特征在于，步骤S4中的模型优化的方法如下：

在理想情况下，当磁力计处于理想地磁场中时的输出模型满足：

其中M_MAG(m_x,m_y,m_z)为磁力计测量数据，G为理想地磁场，约为0.5Guass，具体可以通过查表获得，

由于量化因子的误差及零偏存在，因此磁力计的输出模型满足为：

(k_x(m_x+b_x))²+(k_y(m_y+b_y))²+(k_z(m_z+b_z))²＝G² (7)

其中，k_x,k_y,k_z为尺度因子，即之前所说的“软铁”误差，b_x,b_y,b_z为零偏，即“硬铁”误差，

利用姿态解算的俯仰、滚转角进行了倾斜补偿，将磁力计的输出模型简化为：

(k′_xB_x+b′_x)²+(k′_yB_y+b′_y)²＝G′² (8)

令

B′_x＝k′_xB_x+b′_x

B_y′＝k′_yB_y+b′_y (9)

则可得到如下优化模型：

5.根据权利要求4所述的一种基于九轴IMU的磁场自动标定方法，其特征在于，步骤S5中基于SVD分解的椭圆参数求解方法如下：

将(10)式通过变形及变量变换可以得到线性齐次方程组：

AgV＝0 (11)

其中

V＝[a,b,c,d,e]^T，其中

向量V即该齐次方程的解，该解可通过SVD分解的方法求得，需要说明的是向量V并不是方程组(10)的直接解，需要经过如下变换，

求得磁力计校准参数k′_x,k′_y,b′_x,b′_y后，代入(10)中求得校准后的数据。