一种磁罗盘自校正方法技术

本发明专利技术公开了一种磁罗盘自校正方法，涉及惯性导航领域，该方法包括：步骤1：磁罗盘采集数据；步骤2：拟合曲线并计算参数；步骤3：根据参数校正原始数据并计算航向角；步骤4：计算磁场稳定指数σ；步骤5：根据磁场稳定指数σ及给定阈值σ1判别磁场受干扰程度，若磁场较稳定，则更新航向角；步骤6：若磁场受干扰严重，则根据滤波算法滤除干扰较大的NF个数据；步骤7：根据Ns‑NF个水平空间分量

A Self-Correction Method for Magnetic Compass

The invention discloses a magnetic compass self-correction method, which relates to the field of inertial navigation. The method includes: step 1: magnetic compass acquisition data; step 2: fitting curve and calculating parameters; step 3: correcting original data and calculating heading angle according to parameters; step 4: calculating magnetic field stability index_; and step 5: judging magnetic field interference range according to magnetic field stability index_and given threshold_1. Step 6: If the magnetic field is seriously disturbed, then filter out the big interfering NF data according to the filtering algorithm; Step 7: According to the horizontal space components of Ns NF.

【技术实现步骤摘要】
一种磁罗盘自校正方法
本专利技术涉及惯性导航领域，尤其涉及一种磁罗盘自校正方法。
技术介绍
随着无人驾驶概念的兴起，以及物流行业的井喷式发展，关于移动机器人的定位研究一直处于热门阶段。电子磁罗盘虽然存在航向测量精度不高、对磁场环境敏感的缺点，但其成本低、动态响应快、启动时间短、体积小、功耗低，非常适合在精度要求不苛刻的移动机器人上使用。由于磁场干扰的存在以及磁罗盘本身的量测误差，计算航向角的数据源就会产生误差，导致移动机器人出现航向误差，影响定位精度，因此需要对磁罗盘进行校准。目前有一种具有自校正功能的电子罗盘设计。其虽然不需要转动电子罗盘采集磁场信息，但是其磁场采集区域具有局限性，易受干扰，且其缺乏磁场干扰程度度量，无法获知当前磁场干扰程度，可能获得误差较大的航向角数据。
技术实现思路
本专利技术的目的在于克服上述现有技术的缺点和不足，提供一种磁罗盘自校正方法；本专利技术可度量磁场的受干扰程度，且其可以迭代剔除受干扰数据，使得航向角计算更加精确。本专利技术通过下述技术方案实现：一种磁罗盘自校正方法，包括如下步骤：步骤1：磁罗盘绕指定中心以半径R匀速旋转，磁罗盘旋转时每隔一预设周期TS采集转角θk和磁场矢量Bk的两个水平空间分量和共采集Ns(Ns≥5)次；步骤2：根据Ns个水平空间分量和确定拟合曲线参数值；步骤3：根据拟合曲线参数值将水平空间原始分量和分别校正为和则航向角α可由和地磁偏角常数β算出，其中步骤4：计算磁场稳定指数σ；步骤5：根据磁场稳定指数σ及给定阈值σ1判别磁场受干扰程度，若磁场较稳定，则更新航向角；步骤6：若磁场受干扰严重，则根据滤波算法滤除干扰较大的NF个数据；步骤7：根据Ns-NF个水平空间分量和重复2-7步骤，直到磁场稳定指数σ达到给定阈值，若重复次数达到最大次数M仍未得到阈值内的磁场稳定指数σ，则放弃本次数据，取消本次航向角更新。在步骤1之前还包括：磁罗盘载体处于静止或移动状态。步骤4具体包括以下子步骤：步骤4-1：根据校正后的磁场水平空间分量和确立理想正弦曲线，理想正弦曲线的模A和初相φ由下式确定，步骤4-2：对理想正弦曲线进行Ns次采样，得到Ns个采样点Bxr(k)和Byr(k)(0<k≤Ns)，其中采样周期和磁罗盘数据采集周期TS相同，步骤4-3：计算磁场稳定指数σ，σx和σy分别由下式计算得出，步骤6滤波算法包括：a)建立评价函数b)根据Pk的值按照从小到大的顺序将原始水平空间分量和重新排列；c)剔除尾端的NF个数据。本专利技术相对于现有技术，具有如下的优点及效果：本专利技术具有磁场干扰程度度量算法，可度量磁场干扰程度从而判别航向角数据的可靠性。本专利技术具有滤波算法，可迭代剔除受干扰较大的数据，使航向角趋近于稳定值。该磁罗盘若安装于移动机器人车身上，可采集较大区域的磁场数据，避免受到局部较大干扰。附图说明图1为本专利技术磁罗盘自校正方法流程图。图2为自动旋转台示意图。具体实施方式下面结合具体实施例对本专利技术作进一步具体详细描述。实施例自动旋转台固定在机器人车身上，磁罗盘绕转轴以半径R匀速旋转，磁罗盘旋转时每隔一定周期TS采集转角θk和磁场矢量Bk的两个水平空间分量和共采集Ns(Ns≥5)次；根据Ns个水平空间分量首先采用椭圆方程对采样数据进行拟合，一般基于Fitzgibbon改进的直接最小二乘拟合算法，设椭圆方程为：F(a，x)＝a·x＝ax2+bxy+cy2+dx+ey+f＝0,其中a＝[a,b,c,d,e,f]T为系数向量，x＝[x2,xy,y2,x,y,1]。对于椭圆拟合问题，即求取椭圆方程F(a,x)，使得磁场采样数据点Bh到椭圆曲线的距离平方和最小，即由于采用直接最小二乘法会在a＝0处产生奇异，使得F(a,x)≡0。为了保证拟合曲线符合椭圆方程，需要对参数向量施加4ac-b2>0的约束。Fitzgibbon的改进算法提出4ac-b2＝1的约束，则椭圆拟合的最优化问题为其中D为以xi＝[xi2,xiyx,yi2,xi,yi,1]T作为行向量构成的n×6阶矩阵，C为由4ac-b2＝1确定的系数矩阵。D矩阵和C矩阵如下：对约束方程式采用拉格朗日乘子法得到系数向量的方程组，对旋转磁罗盘的采样数据应用上述椭圆拟合算法，得到椭圆的一般方程的系数向量a，并由此得到椭圆的基本参数即校正参数E＝(xc,yc,ax,by,θr)，分别是椭圆中心，半轴长以及椭圆的旋转角。由此，航向角自校正模块可按照如下步骤对旋转磁罗盘的采集数据进行校正：硬铁校正：将椭圆的中心平移至坐标轴的原点，经过硬铁校正后的数据为则有：软铁校正：将硬铁校正后的椭圆旋转-θr,使其轴线与坐标轴重合，经过软铁校正后的磁场数据为则有：比例误差校正：对椭圆的长轴进行压缩，使之校正成一个圆。校正后有：将Bhc3再旋转θr至原位置，得到该过程表达式为：至此，旋转磁罗盘数据已完成椭圆校正过程，对于一个测量数据点磁罗盘航向角α可由和地磁偏角常数β算出，其中根据磁罗盘的航向角α及磁罗盘转角θk，可计算出机器人的航向角。航向角计算出后，并不会立刻更新，而需使用磁场干扰程度判别方法判别地磁场的校正效果。磁场干扰程度判别方法主要包括以下几个步骤：a)根据校正后的磁场水平空间分量和确立理想正弦曲线，理想正弦曲线的模A和初相φ由下式确定，b)对理想正弦曲线进行Ns次采样，得到Ns个采样点Bxr(k)和Byr(k)(0<k≤Ns)，其中采样周期和磁罗盘数据采集周期相同，c)计算磁场稳定指数σ，σx和σy分别由下式计算得出，根据磁场稳定指数σ及给定阈值σ1判别磁场受干扰程度，若磁场较稳定，则更新航向角；若校正效果不理想，表明磁场干扰强烈，则根据滤波算法滤除干扰较大的NF个数据。滤波算法包括：a)建立评价函数b)根据Pk的值按照从小到大的顺序将原始水平空间分量和重新排列；c)剔除尾端的NF个数据；根据Ns-NF个水平空间分量和再次拟合成椭圆并校正，直到磁场稳定指数σ达到给定阈值，若重复次数达到最大次数M仍未得到阈值内的磁场稳定指数σ，则放弃本次数据，取消本次航向角更新。如上所述，便可较好地实现本专利技术。本专利技术的实施方式并不受上述实施例的限制，其他任何未背离本专利技术的精神实质与原理下所作的改变、修饰、替代、组合、简化，均应为等效的置换方式，都包含在本专利技术的保护范围之内。本文档来自技高网...

【技术保护点】
1.一种磁罗盘自校正方法，其特征在于包括如下步骤：步骤1：磁罗盘绕指定中心以半径R匀速旋转，磁罗盘旋转时每隔一预设周期TS采集转角θk和磁场矢量B

【技术特征摘要】
1.一种磁罗盘自校正方法，其特征在于包括如下步骤：步骤1：磁罗盘绕指定中心以半径R匀速旋转，磁罗盘旋转时每隔一预设周期TS采集转角θk和磁场矢量Bk的两个水平空间分量和共采集Ns(Ns≥5)次；步骤2：根据Ns个水平空间分量和确定拟合曲线参数值；步骤3：根据拟合曲线参数值将水平空间原始分量和分别校正为和则航向角α可由和地磁偏角常数β算出，其中步骤4：计算磁场稳定指数σ；步骤5：根据磁场稳定指数σ及给定阈值σ1判别磁场受干扰程度，若磁场较稳定，则更新航向角；步骤6：若磁场受干扰严重，则根据滤波算法滤除干扰较大的NF个数据；步骤7：根据Ns-NF个水平空间分量和重复2-7步骤，直到磁场稳定指数σ达到给定阈值，若重复次数达到最大次数M仍未得到阈值内的磁场稳定指数σ，则放弃本次数据，取消本...

【专利技术属性】
技术研发人员：叶峰，石峰，赖乙宗，谢先治，袁洋，
申请(专利权)人：华南理工大学，
类型：发明
国别省市：广东,44