一种室内定位航向误差校正方法与流程

本发明属于室内导航定位
技术领域：
，具体涉及一种室内定位航向误差校正方法。
背景技术：
：传统的室外定位技术借助于全球定位系统(GlobalPositioningSystem,GPS)，利用卫星和移动对象之间的直接通信，已可以完成精确的定位。随着生活水平的提高和移动互联网技术的发展，人们对于室内导航定位的需求日益强烈。在大型的购物商场，建筑物内部结构复杂，种类繁多的商铺分布在不同的角落，顾客短时间内无法找到目的商品，此时如果提供室内导航定位服务，为顾客提供商铺的位置和商品信息，可以帮助顾客快速地找到目标商铺。在多层建筑中发生火灾时，迅速撤离并非易事，这时需要室内导航定位服务指导处于危险的人群迅速、准确地撤离至安全地带，防止发生重大的人员伤亡和财产损失。传统的GPS信号由于受到建筑物等的遮挡，无法准确测距，在室内环境下很难完成定位。当前出现了很多室内导航定位技术，取得了不错的效果。导航最重要的功能是提供载体准确的航向角信息，由于加速度的零偏误差和陀螺仪的漂移等，单纯根据惯性测量单元(IMU)输出的航向角并不能满足要求，由加速度和磁力计组成的组合测向设备可以提高航向的准确性和可靠性。但是磁力计在使用过程中会受到外界磁场的干扰，需要相应的校正和补偿。准确实时的磁力计校正和补偿技术是航向输出的关键所在。利用加速度和磁力计的组合测向装置通过测量地球磁场在载体三轴的分量大小确定航向，具有抗干扰、无辐射、体积小和成本低等优点。但是磁力计会引入安装误差、零位误差和标度因数误差和外界磁质干扰等误差。前三种误差不依赖于实际应用环境，一次补偿即可永久使用，但是实际使用环境中的磁质干扰根据强度和方向的不同，需要实时校正和补偿。目前，出现了很多针对磁力计的校正和补偿技术，一些方法综合考虑了磁力计的误差，对误差进行建模，确定模型参数进而完成校正，但是在并不能满足实时高精度的校正要求；另一些方法将环境的磁质干扰建模为一个通用的误差模型，没有充分考虑不同环境之间的差异性。本发明正是在这样的背景下，设计了一种基于非线性模型的室内定位航向误差校正方法，该方法能够根据实际的磁质干扰环境自适应地确定相应的误差模型参数，实时准确地完成校正。本方法操作简单，无需额外的辅助装置，具有自适应程度高、实时性好，普适性强等优点。技术实现要素：本发明提供了一种室内定位航向误差校正方法，该误差校正方法能够自适应地确定航向误差校正模型，准确完成误差校正与精度补偿，解决了室内定位和导航过程中航向校正的问题。综上所述，为达到上述目的，本发明所采用的技术方案如下：一种室内定位航向误差校正方法，该校正方法采用航向测量装置包括加速度计和磁力计，其特征在于，按照如下步骤实现：步骤一：选取校正方向，在校正环境中均匀选取n个校正方向，航向真实值记为步骤二：测量校正方向，利用加速度计和磁力计组合测向装置测量所述步骤一中选取的校正方向，得到相应的航向测量值步骤三：计算航向误差并建立航向误差校正模型，对所述步骤二中的航向测量值和所述步骤一中的航向真实值作差，得到航向误差并把所述航向误差看做所述步骤二中航向测量值的非线性函数，建立参数未知的航向误差校正模型；步骤四：根据预设阈值进行曲线拟合，确定所述步骤三中航向误差校正模型的参数；步骤五：根据所述步骤四中航向误差校正模型来校正实际的航向。进一步，所述步骤三中的非线性函数为：其中为所述航向误差，为所述航向测量值，A,B,…K,M为所述航向误差校正模型的参数，N是代表函数的最高次幂，且N≠0。进一步，所述步骤四中确定航向误差校正模型参数的方法为：1)将所述公式(9)看成所述航向测量值的N次函数，写成方程组形式为：其中代表第i个校正方向的误差，代表第i个校正方向上的航向测量值，A,B,…K,M为方程参数；2)根据自适应拟合法计算N的值；3)利用最小二乘法求解所述公式(9)的函数参数A,B,…K,M。进一步，所述自适应拟合法，其特征在于，具体为：对于某次拟合的N值，N的初值为1，若则N即为所取，否则N＝N+1,继续下一次拟合；其中为利用所述航向真实值求得的拟合函数值，为所述航向误差。进一步，所述利用最小二乘法求解函数参数A,B,…K,M，其特征在于，具体为：根据得到的拟合值N，将公式(10)所述的方程组写成矩阵形式为：根据最小二乘法计算参数A,B,…K,M为：进一步，所述步骤二中航向测量值的测量方法为：取导航坐标系为东北天坐标系(ENU)，载体坐标系为Oxbybzb，其中Oyb沿载体前向方向，Oxb轴垂直于前向朝右，Ozb轴与Oyb轴和Oxb轴构成右手坐标系，利用载体坐标系下三轴加速度值(ax,ay,az)计算载体横滚角roll和俯仰角pitch，如下：roll=arctan(ay/az)pitch=arctan(-axay2+az2)---(1)]]>根据所述公式(1)计算载体坐标系向导航坐标系转换的旋转矩阵：Rbt=1000cosrsinr0-sinrcosrcosθ0-sinθ010sinθ0cosθ=cosθ0-sinθsinθ·sinrcosrcosθ·sinrsinθ·cosr-sinrcosθ·cosr---(2)]]>其中，其中θ＝pitch，r＝roll，均由式(1)计算而得；将载体任意姿态的三轴磁强转换到导航坐标系下的磁场强度MxtMytMzt=Rbt·MxbMybMzb---(3)]]>根据导航坐标系下的磁场强度值计算航向角：进一步，所述步骤三中所述的航向误差校正模型的建立方法为：磁力计所在环境的软磁和硬磁干扰根据磁场的强度和方向各不相同，成为航向输出的主要误差来源。假设周围无磁场干扰，磁力计输出的x轴地磁分量和y轴磁强分量将构成一个正圆；硬磁干扰产生于永久的磁质材料或者被磁化后的金属，它会使磁场强度输出曲线的圆心发生偏移，对最终航向的影响是周期性且非线性的，误差建模为：软磁干扰产生于磁力计附近磁性材料之间的相互作用，它会使磁场强度输出的曲线由正圆变成椭圆，对最终航向的影响也是周期性且非线性的，误差建模为：因此磁力计受到的硬磁和软磁干扰可建模为：根据fhm和fsm的周期性和非线性可知f也具有周期性和非线性。因此，进一步可将航向误差建模为：其中表示航向误差，表示航向测量值，A,B,…K,M为函数的参数，N是代表函数的最高次幂。由上述发明的方案可知，本发明的有益效果在于：1)本发明提出的方法成本低、精度高，普适性强，能够显著提高导航航向精度，进而提高导航定位系统的可靠性。2)本发明提出的方法具有较强的自适应性，可以根据校正环境实时确定航向误差校正模型，准确完成误差校正与精度补偿。3)本发明的装置构成及方案设计简单易行，只利用少许几个航向真实值即可完成误差模型的确定，因而方法的复杂度较低。4)本发明提出的方法与传统方法相比，不是单纯地对磁场强度进行校正，而是对最终的输出航向误差建模，充分考虑了航向误差影响因素，因此校正结果更加可靠。附图说明图1为磁力计环境中硬磁干扰输出曲线图；图2为磁力计环境中硬磁干扰对航向的周期性干扰图；图3为磁力计环境中软磁干扰输出曲线图；图4为磁力计环境中软磁干扰对航向的周期性干扰图；图5为航向校正流程图；图6为误差模型曲线拟合结果图；图7为校正前后误差对比图。具体实施方式下面结合附图和具体的实施例对本发明做进一步详细的说明，以便更好地理解本发明，但本发明的保护范围不限于此。实施例选用的测量装置为MPU6050加速度计和HMC5883磁力计惯性测量模块，两者具有相同的或者平行的坐标轴定义。MPU6050具有±16g的测量量程，稳定性为0.01g，分辨率高达6.1×10-5g，HMC5883磁力计的测量范围为[-8,8]Gauss，可精确到1mgauss，利用杜邦线将模块与开发板连接通过MiniUSB线与上位机通信存储数据。如图5所示，本实施例的实施环境为一典型的室内环境，由于环境中存在大量的电磁材料而具有明显的磁质干扰，一种室内定位航向误差校正方法，该校正方法具体的实施步骤为：步骤一：在0°～360°内每隔30°选定一个校正方向，航向真实值记为步骤二：利用所述测量装置分别测量所述步骤一中选取的校正方向的加速度和磁场强度数据，得到相应的航向测量值根据加速度数据计算载体的横滚角和俯仰角，进而计算载体坐标系向导航坐标系转换的旋转矩阵。由所述的旋转矩阵将载体坐标系下的磁场强度转换到导航坐标系下，根据公式(4)计算航向角。取导航坐标系为东北天坐标系(ENU)，载体坐标系为Oxbybzb，其中Oyb沿载体前向方向，Oxb轴垂直于前向朝右，Ozb轴与Oyb轴和Oxb轴构成右手坐标系，利用载体坐标系下三轴加速度值(ax,ay,az)计算载体横滚角roll和俯仰角pitch，如下：roll=arctan(ay/az)pitch=arctan(-axay2+az2)---(1)]]>根据公式(1)计算载体坐标系向导航坐标系转换的旋转矩阵：Rbt=1000cosrsinr0-sinrcosrcosθ0-sinθ010sinθ0cosθ=cosθ0-sinθsinθ·sinrcosrcosθ·sinrsinθ·cosr-sinrcosθ·cosr---(2)]]>其中，其中θ＝pitch，r＝roll，均由式(1)计算而得。将载体任意姿态的三轴磁强转换到导航坐标系下的磁场强度MxtMytMzt=Rbt·MxbMybMzb---(3)]]>根据导航坐标系下的磁场强度值计算航向角：步骤三：计算航向误差并建立航向误差校正模型，对所述步骤二中得到的航向测量值和所述步骤一中航向真实值作差，得到航向误差并把所述航向误差看做所述步骤二中航向测量值的非线性函数，建立参数未知的航向误差校正模型；航向误差校正模型的建立方法为：磁力计所在环境的软磁和硬磁干扰根据磁场的强度和方向各不相同，成为航向输出的主要误差来源。假设周围无磁场干扰，磁力计输出的x轴地磁分量和y轴磁强分量将构成一个正圆。硬磁干扰产生于永久的磁质材料或者被磁化后的金属，这种干扰会使磁场强度与真实值产生偏差，如图1所示，以X方向磁场分量为横轴，以Y方向磁场分量为纵轴，图中的圆的圆心发生了偏移。而硬磁干扰对航向的影响呈现非线性关系，如图2所示，以航向为横轴，以航向误差为纵轴，磁力计环境中硬磁干扰对最终航向的影响是周期性且非线性的，将其建模为：软磁干扰产生于磁力计附近磁性材料之间的相互作用，它会使磁场强度输出的曲线由正圆变成椭圆，如图3所示，以X方向磁场分量为横轴，以Y方向磁场分量为纵轴，本来应该为圆形的磁强分布变成了椭圆形；图4显示了软磁干扰对航向误差的影响曲线，以航向为横轴，以航向误差为纵轴，软磁干扰对最终航向的影响也是周期性且非线性的，误差建模为：因此磁力计受到的硬磁和软磁干扰可建模为：根据fhm和fsm的周期性和非线性可知f也具有周期性和非线性。因此，进一步可将航向误差建模为：其中表示航向误差，表示航向测量值，A,B,…K,M为函数的参数，N是代表函数的最高次幂。综上所述，所述非线性函数为：其中为所述航向误差，为所述航向测量值，A,B,…K,M为所述航向误差校正模型的参数，N是代表函数的最高次幂，且N≠0。步骤四：根据预设门限阈值进行曲线拟合，确定所述步骤三中航向误差校正模型的参数；1)确定航向误差校正模型参数的方法为：将所述公式(9)看成所述航向测量值的N次函数，写成方程组形式为：其中Δφi,i＝[1:n]代表第i个校正方向的误差，代表第i个校正方向上的航向测量值，A,B,…K,M为方程参数。2)根据自适应拟合法计算N的值；对于某次拟合的N值，N的初值为1，若则N即为所取，否则N＝N+1,继续下一次拟合。其中为利用所述航向真实值求得的拟合函数值，为所述航向误差。3)利用最小二乘法求解所述公式(9)的函数参数A,B,…K,M具体为：根据得到的拟合值N，将所述公式(10)所述的方程组写成矩阵形式为：根据最小二乘法计算参数A,B,…K,M为：如图6所示，以采样航向值为横轴，以航向误差为纵轴作出的实际测量曲线和拟合曲线，拟合得到N＝4，相应的误差模型系数为：Coef＝[-0.0068,0.0944,-0.3545,0.2051,0.2844]。步骤五：根据所述步骤四中航向误差校正模型来校正实际的航向。步骤六：将所述测量装置旋转一周，每隔15°采集一次数据，用于检验方法的正确性；如表1所示，表1显示了本发明所述方法校正前后的误差对比状况。表1航向校正前后误差对比图7为利用所述的方法对航向进行校正前后的误差对比图，以航向采样值为横轴，以航向误差为纵轴，作出的校正前航向误差曲线和校正后航向误差曲线。表2为对航向误差校正前后的平均误差和误差标准差进行统计的数据。表2航向校正前后主要精度指标对比从表2可以看出，利用发明所述的方法对航向校正前，平均误差为12.9°，标准差为15.4°，利用所述方法校正后的平均误差为3.2°，标准差为3.4°。较大地提高了航向精度。要说明的是，上述实施例是对本发明技术方案的说明而非限制，所属
技术领域：
普通技术人员的等同替换或者根据现有技术而做的其它修改，只要没超出本发明技术方案的思路和范围，均应包含在本发明所要求的权利范围之内。当前第1页1 2 3