CN114526730B - 基于磁梯度的运动速度估计方法及装置 - Google Patents

Abstract

本发明公开了一种基于磁梯度的速度估计方法及装置，方法步骤为：触发航姿参考系统并同步引发磁传感器阵列模块对载体进行测量；根据航姿参考系统返回的姿态四元数数据计算旋转矩阵，根据航姿参考系统返回的角速度数据计算角速度反对称阵；采集磁传感器阵列模块中各磁传感器数据，计算得到磁场梯度，同时根据航姿参考系统返回的磁场数据计算磁场矢量；将计算得到的旋转矩阵、角速度反对称阵、磁场梯度及磁场矢量数据输入MCU控制模块中进行计算，得到载体运动速度。本发明根据航姿参考系统获取的姿态信息、角速度信息，以及从磁传感器阵列获得的室内磁场梯度信息估计载体运动速度，修正惯性导航的速度积分漂移，提高室内导航定位系统的精度及鲁棒性。

Description

基于磁梯度的运动速度估计方法及装置

技术领域

本发明涉及室内导航定位技术领域，具体涉及一种基于磁梯度的运动速度估计方法及装置。

背景技术

随着城市化进程的加快，卫星导航定位技术的发展给人们出行生活带来了诸多便利。建筑的复杂化与大型化也成为了城市建筑发展的趋势。然而这两种趋势也为人们带来了问题：容易在建筑中迷失方向。该问题换在室外，可简单地通过GPS组合导航定位系统解决。然而，由于通过电磁波传播的GPS信号容易衰减，在室内复杂环境中使用GPS组合导航定位系统往往效果不佳，需要通过其它手段获得位置信息，这是当前室内导航定位系统需要解决的主要问题之一；同时，与室外环境相比室内空间相对较小，室外导航定位中达到的米级精度在室内则无法满足要求，室内的精确导航定位需要达到分米，甚至厘米级别的精度。如何高精度是当前室内导航定位系统需要解决的另一主要问题。

传统的惯性导航可通过对加速度二次积分得到载体位移，但由于低成本惯性器件精度不高，最终导致较大的累积误差，需要额外的辅助信息源对速度或位置信息进行量测；按辅助信息源获取所使用的传感方式进行分类，领域内常见的室内导航定位技术有基于视觉的室内导航定位、基于无线技术的室内导航定位、基于激光传感器的室内导航定位、基于地磁匹配或磁力信标的室内导航定位和基于行人航位推算的室内导航定位；而这些现有的技术方案均有不同程度的限制；

基于无线技术的室内导航定位，需要在定位范围内安装相应的信号发送或接收设备，从成本及工程量上考虑不占据优势，且对于无信号覆盖的地域则无法使用；

基于视觉和激光的室内导航定位技术，对环境的光照条件、空气质量具有一定要求，光线的明暗变化、空气中烟雾等因素会对定位结果造成影响，且其较为庞大的数据量也对平台的处理能力提出要求，功耗较大也是此种定位方案的一个缺点；

基于地磁匹配的室内导航定位技术，需要先对导航定位范围内的地磁环境进行数据采集并构建数据库，对于没有地磁数据库的范围则无法使用；

基于磁力信标的室内导航定位技术，类似于无线导航定位技术，需要事先布设信标，对没有布设信标的地域无法使用；

基于行人航位推算的室内导航定位，由于航位推算算法基于行人的运动规律，无法在定位目标为行人以外的目标如车载的情况下使用，应用范围具有一定的局限性。

因此，高精度、低功耗、自主工作以及能适应多种场景的室内导航定位系统的研究依然是室内导航定位领域的研究热点，寻找符合上述要求的辅助信息源是研究方向之一。

发明内容

本发明的主要目的在于克服现有技术的缺点与不足，提供一种基于磁梯度的运动速度估计方法及装置，根据航姿参考系统获取的姿态信息和角速度信息，以及从磁传感器阵列获得的室内磁场梯度信息，获取载体的运动速度作为辅助信息源，修正惯性导航的速度积分漂移，提高室内导航定位系统的精度以及鲁棒性。

为了达到上述目的，本发明采用以下技术方案：

本发明一方面提供了一种基于磁梯度的运动速度估计方法，包括以下步骤：

S1、触发航姿参考系统并同步引发磁传感器阵列模块对载体进行测量；根据航姿参考系统返回的姿态四元数数据计算旋转矩阵，根据航姿参考系统返回的角速度数据计算角速度反对称阵；

S2、采集磁传感器阵列模块中各磁传感器数据，计算得到磁场梯度，同时根据航姿参考系统返回的磁场数据计算磁场矢量；

S3、将计算得到的旋转矩阵、角速度反对称阵、磁场梯度及磁场矢量数据输入MCU控制模块中进行计算，得到载体运动速度。

作为优选的技术方案，步骤S1具体为：

S11、配置MCU控制模块中的定时器，触发各磁传感器的测量动作，实现航姿参考系统的数据以固定的频率进行输出；

S12、测量动作触发外部中断引脚电平跳变，引发航姿参考系统测量动作输出引脚上的电平进行高低转换；依次触发磁传感器阵列模块中的各个磁传感器，使磁传感器阵列模块能根据外部中断输入引脚上的电平变换触发各个磁传感器进行测量动作；

S13、根据航姿参考系统测量返回的姿态四元数数据，MCU控制模块计算旋转矩阵，计算公式为：

其中q₀、q₁、q₂、q₃为姿态四元数分量；

S14、根据航姿参考系统测量返回的角速度数据，MCU控制模块计算角速度反对称阵，计算公式为：

其中ω_x、ω_y、ω_z分别为x、y、z轴的角速度量测。

作为优选的技术方案，所述测量动作输出引脚是指MCU控制模块与航姿参考系统之间互联的GPIO引脚；所述外部中断输入引脚是指MCU控制模块与航姿参考系统的测量动作引脚相连的引脚。

作为优选的技术方案，步骤S2具体为：

S21、在航姿参考系统返回数据后，判断MCU控制模块与航姿参考系统之间的SPI通信使能引脚的电平状况，当使能引脚从低电平跳变为高电平，即表明MCU控制模块与航姿参考系统之间的SPI通信数据传输完成，此时立即采集磁传感器阵列模块中各个磁传感器的测量数据；

S22、对各个磁传感器的测量数据进行滤波与补偿；整合滤波及补偿后的各磁传感器数据，分别表示为B_A、B_B、B_C和B_D：

其中，B_Ax、B_Ay、B_Az分别表示磁传感器A在x、y、z轴上的测量数据；B_Bx、B_By、B_Bz分别表示磁传感器B在x、y、z轴上的测量数据；B_Cx、B_Cy、B_Cz分别表示磁传感器C在x、y、z轴上的测量数据；B_Dx、B_Dy、B_Dz分别表示磁传感器D在x、y、z轴上的测量数据；

S23、计算得到磁梯度G，公式为：

其中，L为十字形传感器阵列的基线距离；

S24、整合航姿参考系统返回的磁场数据，得到装置中心点附近的磁场矢量B^b。

作为优选的技术方案，所述SPI通信使能引脚的电平通过将使能引脚与MCU控制模块外部中断输入进行连接，并将使能引脚的中断触发条件设置为上升沿触发来进行判别；所述磁传感器阵列模块上各个磁传感器数据的采集在外部中断的中断处理函数中进行。

作为优选的技术方案，所述步骤S3具体为：

S31、计算磁场矢量对时间的导数：

其中B_x、B_y、B_z为磁场矢量B^b在x、y、z轴的分量；ΔT为时间步长，由传感器测量与数据刷新频率确定；

S32、整合旋转矩阵、角速度反对称阵、磁梯度数据、磁场矢量与磁场矢量对时间的导数，通过克拉默法则求解非齐次线性方程组：

其中V为载体相对惯性空间的运动速度矢量。

本发明另一方面提供了一种基于磁梯度的运动速度估计装置，应用于所述基于磁梯度的运动速度估计方法，装置包括航姿参考系统、磁传感器阵列模块、MCU控制模块、无线通信模块及电源模块；

所述航姿参考系统，用于提供装置的姿态信息以及中心点附近的磁场矢量信息；航姿参考系统通过SPI通信接口以及GPIO接口联接到MCU控制模块；

所述磁传感器阵列模块，用于测量载体中心点附近空间各点的磁场矢量；所述磁传感器阵列模块包含四个按设定方式排布的磁传感器；各传感器均通过各自的SPI通信接口与MCU控制模块通信；

所述MCU控制模块是装置控制模块的核心，负责装置程序的执行与数据运算；MCU控制模块通过GPIO分别与外部的LED指示灯、按键相连，用于装置调试；MCU控制模块通过SW链接到调试接口，用于程序烧录与调试；MCU控制模块通过USART与无线通信模块相连，用于以无线的方式将数据发送至上位机；

所述电源模块，用于产生装置上各器件模块所需的工作电压，为整套装置供电。

作为优选的技术方案，所述磁传感器阵列模块的四个磁传感器以平面十字形的布局排布在所述基于磁梯度的速度估计装置上，平面十字形的布局的中心点与所述基于磁梯度的速度估计装置中点重合。

作为优选的技术方案，所述航姿参考系统布局于所述基于磁梯度的速度估计装置中央，航姿参考系统的中心点与平面十字形的布局中心、基于磁梯度的速度估计装置中心重合，该中心点作为基于磁梯度的速度估计装置运动的参考质心。

作为优选的技术方案，所述航姿参考系统包括至少一个可配置为测量时发生电平跳变的引脚。

本发明与现有技术相比，具有如下优点和有益效果：

本发明提供的一种基于磁梯度的运动速度估计方法及装置，不需额外布置辅助设备，可自主工作，功耗低，不受光线及空气质量的影响，且在多数室内环境如地下停车场、办公室中都可提供高精度的载体运动速度估计，得到的速度信息可用于实现高精度的室内导航定位，具有广阔的市场前景。

附图说明

为了更清楚地说明本申请实施例中的技术方案，下面将对实施例描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本申请的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。

图1为本发明实施例基于磁梯度的运动速度估计方法的流程示意图；

图2为本发明实施例基于磁梯度的运动速度估计装置的结构示意图；

图3为本发明实施例磁传感器阵列模块与航姿参考系统的布局示意图。

具体实施方式

为了使本技术领域的人员更好地理解本申请方案，下面将结合本申请中的实施例及附图，对本发明的技术方案进行清楚、完整地描述，应当理解，附图仅用于示例性说明，不能理解为对本专利的限制。显然，所描述的实施例仅仅是本申请一部分实施例，而不是全部的实施例。基于本申请中的实施例，本领域技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本申请保护的范围。

在本申请中提及“实施例”意味着，结合实施例描述的特定特征、结构或特性可以包含在本申请的至少一个实施例中。在说明书中的各个位置出现该短语并不一定均是指相同的实施例，也不是与其它实施例互斥的独立的或备选的实施例。本领域技术人员显式地和隐式地理解的是，本申请所描述的实施例可以与其它实施例相结合

实施例

如图1所示，本实施例提供了一种基于磁梯度的运动速度估计方法，包括以下步骤：

S1、触发航姿参考系统101并同步引发磁传感器阵列模块107对载体进行测量，根据航姿参考系统101返回的姿态四元数102数据计算旋转矩阵103，并根据航姿参考系统返回的角速度104数据计算角速度反对称阵105；

S2、采集磁传感器阵列模块107中各磁传感器数据，计算得到磁场梯度109，同时根据航姿参考系统101返回的磁场数据计算磁场矢量110；

S3、将计算得到的旋转矩阵103、角速度反对称阵105、磁场梯度109以及磁场矢量110数据输入MCU控制模块中进行计算，得到载体运动速度111。

更具体的，步骤S1包括以下步骤：

S11、配置MCU控制模块中的定时器，触发各磁传感器的测量动作，实现航姿参考系统的数据以固定的频率进行输出；

S12、测量动作触发外部中断引脚电平跳变，引发航姿参考系统测量动作输出引脚上的电平进行高低转换；依次触发磁传感器阵列模块中的各个磁传感器，使磁传感器阵列模块能根据外部中断输入引脚上的电平变换触发各个磁传感器进行测量动作；

S13、根据航姿参考系统测量返回的姿态四元数数据，MCU控制模块计算旋转矩阵，计算公式为：

其中q₀、q₁、q₂、q₃为姿态四元数分量；

S14、根据航姿参考系统测量返回的角速度数据，MCU控制模块计算角速度反对称阵，计算公式为：

其中ω_x、ω_y、ω_z分别为x、y、z轴的角速度量测。

更具体的，步骤S2中采集磁传感器阵列模块107中各磁传感器数据，计算得到磁场梯度109与磁场矢量110，包括以下步骤：

S21、在航姿参考系统101返回数据后，判断MCU控制模块与航姿参考系统101之间的SPI通信使能引脚的电平状况，当使能引脚从低电平跳变为高电平，即表明MCU控制模块与航姿参考系统101之间的SPI通信数据传输完成，MCU控制模块已接收来自航姿参考系统101的姿态四元数102数据与角速度104数据，此时立即采集磁传感器阵列模块107上各个磁传感器数据，包括磁传感器A203、磁传感器B204、磁传感器C205、磁传感器D206的测量数据；

进一步地，SPI通信使能引脚的电平判别可通过将使能引脚接入MCU控制模块外部中断输入，并将使能引脚的中断触发条件设置为上升沿触发，即可实现相应功能；进一步地，磁传感器阵列模块107上各个磁传感器数据的采集在外部中断的中断处理函数中进行；

S22、对各个磁传感器的测量数据进行滤波与补偿，整合滤波及补偿后的各磁传感器数据，分别表示为B_A、B_B、B_C和B_D：

其中，B_Ax、B_Ay、B_Az分别表示磁传感器A在x、y、z轴上的测量数据；B_Bx、B_By、B_Bz分别表示磁传感器B在x、y、z轴上的测量数据；B_Cx、B_Cy、B_Cz分别表示磁传感器C在x、y、z轴上的测量数据；B_Dx、B_Dy、B_Dz分别表示磁传感器D在x、y、z轴上的测量数据；

如图2、图3所示，本实施例各磁传感器分别为磁传感器A203、磁传感器B204、磁传感器C205、磁传感器D206；

S23、计算得到磁梯度G 109，通过下式计算：

其中，L为十字形传感器阵列的基线距离；

如图3所示，本实施例中十字形传感器阵列的基线距离L为10cm；

S23、整合航姿参考系统101返回的磁场数据，得到装置中心点附近的磁场矢量B^b110。

更具体的，步骤S3包括以下步骤：

S31、使用下式计算磁场矢量对时间的导数：

其中B_x、B_y、B_z为磁场矢量B^b 110在x、y、z轴的分量；ΔT为时间步长，由传感器测量与数据刷新频率确定；

在本实施例中时间步长ΔT为0.01s；

S32、整合旋转矩阵103、角速度反对称阵105、磁梯度数据109、磁场矢量110与磁场矢量对时间的导数，通过克拉默法则求解以下非齐次线性方程组：

其中V为载体相对惯性空间的运动速度矢量。

如图2所示，本发明另一实施例提供了一种基于磁梯度的运动速度估计装置，应用于上述基于磁梯度的运动速度估计方法，该装置包括航姿参考系统211、磁传感器阵列模块202、MCU控制模块208、无线通信模块214及电源模块；

航姿参考系统211置于装置中央，用于提供装置的姿态信息以及中心点附近的磁场矢量信息；至少包含一个可配置为测量时发生电平跳变的引脚；航姿参考系统211通过SPI通信接口以及GPIO接口联接到MCU控制模块208；

磁传感器阵列模块202，用于测量载体中心点附近空间各点的磁场矢量；包含四个按一定方式排布的磁传感器，分别为磁传感器A203、磁传感器B204、磁传感器C205、磁传感器D206；各磁传感器均通过各自的SPI通信接口与MCU控制模块208通信；

如图3所示，磁传感器A203、磁传感器B 204、磁传感器C 205、磁传感器D206均为三轴磁传感器，以一种平面十字形的布局排布在装置中，平面十字形的布局的中心点与装置中点重合；布局的基线距离为10cm；航姿参考系统211布局于装置中央，航姿参考系统211的中心点与平面十字形的布局中心及装置中心点重合，该中心点作为装置运动的参考质心；

MCU控制模块208是装置控制模块的核心，负责装置程序的执行与数据运算；MCU控制模块208通过GPIO分别与LED指示灯209、按键210相连，方便调试；MCU控制模块208通过SW链接到调试接口201，用于程序烧录与调试；MCU控制模块208通过USART与无线通信模块214相连，用于以无线的方式将数据发送至上位机；

电源模块，用于产生装置上各器件模块所需的工作电压，为整套装置供电；包括电池213、电源电路212及电池充电电路207。

需要说明的是，对于前述的各方法实施例，为了简便描述，将其都表述为一系列的动作组合，但是本领域技术人员应该知悉，本发明并不受所描述的动作顺序的限制，因为依据本发明，某些步骤可以采用其它顺序或者同时进行。

以上实施例的各技术特征可以进行任意的组合，为使描述简洁，未对上述实施例中的各个技术特征所有可能的组合都进行描述，然而，只要这些技术特征的组合不存在矛盾，都应当认为是本说明书记载的范围。

上述实施例为本发明较佳的实施方式，但本发明的实施方式并不受上述实施例的限制，其他的任何未背离本发明的精神实质与原理下所作的改变、修饰、替代、组合、简化，均应为等效的置换方式，都包含在本发明的保护范围之内。

Claims (7)

1.基于磁梯度的运动速度估计方法，其特征在于，包括以下步骤：

S1、触发航姿参考系统并同步引发磁传感器阵列模块对载体进行测量；根据航姿参考系统返回的姿态四元数数据计算旋转矩阵，根据航姿参考系统返回的角速度数据计算角速度反对称阵；步骤S1具体为：

S11、配置MCU控制模块中的定时器，触发各磁传感器的测量动作，实现航姿参考系统的数据以固定的频率进行输出；

S12、测量动作触发外部中断引脚电平跳变，引发航姿参考系统测量动作输出引脚上的电平进行高低转换；依次触发磁传感器阵列模块中的各个磁传感器，使磁传感器阵列模块能根据外部中断输入引脚上的电平变换触发各个磁传感器进行测量动作；

S13、根据航姿参考系统测量返回的姿态四元数数据，MCU控制模块计算旋转矩阵，计算公式为：

其中q₀、q₁、q₂、q₃为姿态四元数分量；

S14、根据航姿参考系统测量返回的角速度数据，MCU控制模块计算角速度反对称阵，计算公式为：

其中ω_x、ω_y、ω_z分别为x、y、z轴的角速度量测；

S2、采集磁传感器阵列模块中各磁传感器数据，计算得到磁场梯度，同时根据航姿参考系统返回的磁场数据计算磁场矢量；S2具体为：

S21、在航姿参考系统返回数据后，判断MCU控制模块与航姿参考系统之间的SPI通信使能引脚的电平状况，当使能引脚从低电平跳变为高电平，即表明MCU控制模块与航姿参考系统之间的SPI通信数据传输完成，此时立即采集磁传感器阵列模块中各个磁传感器的测量数据；

S22、对各个磁传感器的测量数据进行滤波与补偿；整合滤波及补偿后的各磁传感器数据，分别表示为B_A、B_B、B_C和B_D：

其中，B_Ax、B_Ay、B_Az分别表示磁传感器A在x、y、z轴上的测量数据；B_Bx、B_By、B_Bz分别表示磁传感器B在x、y、z轴上的测量数据；B_Cx、B_Cy、B_Cz分别表示磁传感器C在x、y、z轴上的测量数据；B_Dx、B_Dy、B_Dz分别表示磁传感器D在x、y、z轴上的测量数据；

S23、计算得到磁梯度G，公式为：

其中，L为十字形传感器阵列的基线距离；

S24、整合航姿参考系统返回的磁场数据，得到装置中心点附近的磁场矢量B^b；

S3、将计算得到的旋转矩阵、角速度反对称阵、磁场梯度及磁场矢量数据输入MCU控制模块中进行计算，得到载体运动速度；步骤S3具体为：

S31、计算磁场矢量对时间的导数：

其中B_x、B_y、B_z为磁场矢量B^b在x、y、z轴的分量；ΔT为时间步长，由传感器测量与数据刷新频率确定；

S32、整合旋转矩阵、角速度反对称阵、磁梯度数据、磁场矢量与磁场矢量对时间的导数，通过克拉默法则求解非齐次线性方程组：

其中V为载体相对惯性空间的运动速度矢量。

2.根据权利要求1所述基于磁梯度的运动速度估计方法，其特征在于，所述测量动作输出引脚是指MCU控制模块与航姿参考系统之间互联的GPIO引脚；所述外部中断输入引脚是指MCU控制模块与航姿参考系统的测量动作引脚相连的引脚。

3.根据权利要求1所述基于磁梯度的运动速度估计方法，其特征在于，所述SPI通信使能引脚的电平通过将使能引脚与MCU控制模块外部中断输入进行连接，并将使能引脚的中断触发条件设置为上升沿触发来进行判别；所述磁传感器阵列模块上各个磁传感器数据的采集在外部中断的中断处理函数中进行。

4.基于磁梯度的运动速度估计装置，其特征在于，应用于权利要求1-3任一项所述基于磁梯度的运动速度估计方法，装置包括航姿参考系统、磁传感器阵列模块、MCU控制模块、无线通信模块及电源模块；

所述航姿参考系统，用于提供装置的姿态信息以及中心点附近的磁场矢量信息；航姿参考系统通过SPI通信接口以及GPIO接口联接到MCU控制模块；

所述磁传感器阵列模块，用于测量载体中心点附近空间各点的磁场矢量；所述磁传感器阵列模块包含四个按设定方式排布的磁传感器；各传感器均通过各自的SPI通信接口与MCU控制模块通信；

所述MCU控制模块是装置控制模块的核心，负责装置程序的执行与数据运算；MCU控制模块通过GPIO分别与外部的LED指示灯、按键相连，用于装置调试；MCU控制模块通过SW链接到调试接口，用于程序烧录与调试；MCU控制模块通过USART与无线通信模块相连，用于以无线的方式将数据发送至上位机；

所述电源模块，用于产生装置上各器件模块所需的工作电压，为整套装置供电。

5.根据权利要求4所述基于磁梯度的运动速度估计装置，其特征在于，所述磁传感器阵列模块的四个磁传感器以平面十字形的布局排布在所述基于磁梯度的速度估计装置上，平面十字形的布局的中心点与所述基于磁梯度的速度估计装置中点重合。

6.根据权利要求5所述基于磁梯度的运动速度估计装置，其特征在于，所述航姿参考系统布局于所述基于磁梯度的速度估计装置中央，航姿参考系统的中心点与平面十字形的布局中心、基于磁梯度的速度估计装置中心重合，该中心点作为基于磁梯度的速度估计装置运动的参考质心。

7.根据权利要求4所述基于磁梯度的运动速度估计装置，其特征在于，所述航姿参考系统包括至少一个可配置为测量时发生电平跳变的引脚。

Images