CN113624240A - 一种基于磁感应强度与特征矢量的位姿识别方法和装置 - Google Patents

Abstract

本发明涉及传感器定位技术领域，具体涉及一种基于磁感应强度与特征矢量位姿识别方法和装置。本发明所述基于磁感应强度与特征矢量位姿识别方法包括：采用传感器连续获取空间磁场在目标点处的感应磁场矢量；根据感应磁场矢量和目标点处的位置信息确定目标点处的磁感应强度和对应的实际特征矢量；根据磁感应强度、实际特征矢量和位置信息确定传感器的位置解算函数；根据位置解算函数确定传感器的最优位置；根据最优位置确定目标点处的理论特征矢量；根据理论特征矢量和实际特征矢量确定传感器的位姿。在保证位姿精度的同时降低了位姿解算系统的复杂度，实现了磁信标位姿解算系统在火灾场景、地下矿井以及城市管廊等复杂场景应用。

Description

一种基于磁感应强度与特征矢量的位姿识别方法和装置

技术领域

本发明涉及传感器定位技术领域，具体而言，涉及一种基于磁感应强度与特征矢量位姿识别方法和装置。

背景技术

随着人类活动场景复杂度的增加，以及对未知空间探索的需求，复杂场景下高精度定位已经成为当前亟待解决的科学问题之一。复杂场景主要指楼宇、火灾现场、地下管廊、矿井等特征缺失、光线条件不确定、无线电信号不能远距离传播的未知应用场景。在复杂场景中全球卫星定位系统(GNSS)、光学定位系统、声学定位系统由于自身的局限性无法正常工作、惯性导航系统存在累积误差等问题，不能为载体提供准确的定位服务。

低频磁信标具有强穿透力，利用低频磁信标进行定位，定位结果不存在累计误差，不易受干扰，在复杂场景导航定位领域具有很高的研究价值。目前实现低频磁信标定位技术的方法主要围绕总磁感应强度、感应磁场矢量两方面展开，但是一般需要多个传感器或多个信标构建几何结构实现定位，系统成本比较高，复杂度远高于感应磁场矢量定位，但是利用感应磁场矢量进行定位的方式容易受到传感器姿态的影响。

发明内容

本发明解决的问题是利用感应磁场矢量进行定位的方式容易受到传感器姿态的影响。

为解决上述问题，本发明提供一种基于磁感应强度与特征矢量位姿识别方法，包括：

采用传感器连续获取空间磁场在目标点处的感应磁场矢量；

根据所述感应磁场矢量和所述目标点处的位置信息确定所述目标点处的磁感应强度和对应的实际特征矢量；

根据所述磁感应强度、所述实际特征矢量和所述位置信息确定所述传感器的位置解算函数；

根据所述位置解算函数确定所述传感器的最优位置；

根据所述最优位置确定所述目标点处的理论特征矢量；

根据所述理论特征矢量和所述实际特征矢量确定所述传感器的位姿。

可选地，在所述采用传感器连续获取空间磁场在目标点处的感应磁场矢量之前包括：

将三路激励信号分别通入所述三轴磁信标的三个螺线管中，激励所述三轴磁信标产生所述空间磁场。

可选地，在根据所述感应磁场矢量和所述目标点处的位置信息确定所述目标点处的磁感应强度和对应的实际特征矢量之前包括：

根据所述三轴磁信标的位置确定设定坐标系，确定所述目标点相对于所述磁信标的位置信息；

所述根据所述感应磁场矢量和所述目标点处的位置信息确定所述目标点处的磁感应强度和对应的实际特征矢量包括：

根据所述感应磁场矢量和所述位置信息确定所述感应磁场矢量在所述设定坐标系的不同坐标轴下的感应磁场分量；

根据所述感应磁场分量确定所述感应磁场分量在目标点处沿不同所述坐标轴的磁感应强度；

将任意两个所述感应磁场分量叉乘，获得所述实际特征矢量。

可选地，各所述激励信号为初始相位不同且频率不同的正弦信号。

可选地，所述根据所述磁感应强度、所述实际特征矢量和所述位置信息确定所述传感器的位置解算函数包括：

根据所述磁感应强度和所述实际特征矢量确定磁感应强度模值和实际特征矢量模值；

根据所述磁感应强度模值、所述实际特征矢量模值和所述位置关系建立所述传感器的位置解算函数，所述位置解算函数为：

其中，

分别为所述感应磁场分量在目标点处沿不同所述坐标轴的磁感应强度模值，

为沿XY坐标轴的两个所述感应磁场分量叉乘后获得的实际特征矢量模值，

为沿XZ坐标轴的两个所述感应磁场分量叉乘后获得的实际特征矢量模值，

为沿YZ坐标轴的两个所述感应磁场分量叉乘后获得的实际特征矢量模值，m＝μ₀M₁sin(ω₁t)，n＝μ₀M₂sin(ω₂t+60)，p＝μ₀M₃sin(ω₃t+120)；M₁、M₂、M₃分别为所述三轴磁信标不同的螺线管产生的磁矩；μ₀为磁场传播介质因子，ω₁、ω₂、ω₃分别为不同的所述螺线管的激励电流的频率。

可选地，所述根据位置解算函数确定所述传感器的最优位置包括：

根据所述位置解算函数确定迭代方程和所述迭代方程的雅可比矩阵；

获取迭代初始条件、迭代终止条件和信赖域半径；

基于狗腿优化算法，根据所述雅可比矩阵确定所述高斯牛顿迭代法和所述最速下降法下的最快下降的迭代步长；

根据所述信赖域半径、所述高斯牛顿迭代法和所述最速下降法下的最快下降的迭代步长确定所述狗腿法的迭代增量；

根据所述迭代初始条件、迭代终止条件和所述狗腿法的迭代增量对所述迭代方程进行迭代，获得所述传感器的最优位置。

可选地，所述根据所述理论特征矢量和所述实际特征矢量确定所述传感器的位姿包括：

将所述理论特征矢量的逆矩阵和所述实际特征矢量相乘，获得关于所述传感器位姿的方向余弦阵；

根据所述方向余弦阵确定包括所述传感器的翻滚角、俯仰角和偏航角的传感器位姿。

可选地，所述方向余弦阵为：

其中，

为所述理论特征矢量，

为所述实际特征矢量；

所述传感器位姿为：

其中，α表示翻滚角，β表示俯仰角，γ表示偏航角。

相比于现有技术，本发明的所述的基于磁感应强度与特征矢量位姿识别方法的有益效果为：

本发明通过获取到的感应磁场矢量确定磁感应强度和对应的实际特征矢量，进而建立位置解算函数，并通过对位置解算函数的解算获得传感器的最优位置，并根据最优位置确定目标点处的理论特征矢量，进而对传感器的位姿进行确定；本发明通过定位信息实现目标姿态的解算，实现目标位姿信息的获取，在保证位姿精度的同时节省了位姿解算系统成本，降低了位姿解算系统的复杂度，极大地提高了磁信标位姿解算系统在火灾场景、地下矿井以及城市管廊等复杂场景应用的可能性。

本发明还提供一种基于磁感应强度与特征矢量的传感器位姿识别装置，包括：

获取模块，用于连续获取空间磁场在目标点处的感应磁场矢量；

计算模块，用于根据所述感应磁场矢量和所述目标点处的位置信息确定所述目标点处的磁感应强度和对应的实际特征矢量；

模型建立模块，用于根据所述磁感应强度、所述实际特征矢量和所述位置信息确定所述传感器的位置解算函数；

估算模块，用于根据位置解算函数确定所述传感器的最优位置；

所述计算模块还用于根据所述最优位置确定所述传感器的理论特征矢量；

所述计算模块还用于根据所述理论特征矢量和所述实际特征矢量确定所述传感器的位姿。

本发明还提供一种计算机可读存储介质，所述计算机可读存储介质存储有计算机程序，所述计算机程序被处理器读取并运行时，实现如上述所述的基于磁感应强度与特征矢量位姿识别方法。

附图说明

图1为本发明的实施例中的基于磁感应强度与特征矢量位姿识别方法流程图；

图2为本发明的实施例中的基于磁感应强度与特征矢量的传感器位姿识别装置示意图；

图3为本发明的实施例中的目标点和磁信标之间的位置关系示意图。

具体实施方式

为使本发明的上述目的、特征和优点能够更为明显易懂，下面结合附图对本发明的具体实施例做详细的说明。

在本说明书的描述中，参考术语“实施例”、“一个实施例”和“一个实施方式”等的描述意指结合该实施例或实施方式描述的具体特征、结构、材料或者特点包含于本发明的至少一个实施例或示实施方式中。在本说明书中，对上述术语的示意性表述不一定指的是相同的实施例或实施方式。而且，描述的具体特征或者特点可以在任何的一个或多个实施例或实施方式以合适的方式结合。

如图1所示，本发明的实施例提供一种基于磁感应强度与特征矢量位姿识别方法，包括：

步骤S100、采用传感器连续获取空间磁场在目标点P处的感应磁场矢量；

空间磁场可以是通过激励磁信标产生，也可以是空间本身存在的磁场，传感器可以为三轴磁强计上的磁场感应模块，也可以为其他感应磁场采集单元。较佳地，在步骤S100之前包括：将三路激励信号分别通入所述三轴磁信标的三个螺线管中，激励所述三轴磁信标产生所述空间磁场。也就是说，三轴磁信标可以仅有一个，向三轴磁信标的每个螺线管中均通入一个激励信号，从而形成空间磁场。这里，激励信号在通入磁信标之前，可以先经过功率放大器进行放大。

步骤S200、根据所述感应磁场矢量和所述目标点处的位置信息确定所述目标点处的磁感应强度和对应的实际特征矢量；

在步骤S200中，根据所述感应磁场矢量和所述位置信息确定所述感应磁场矢量在所述设定坐标系的不同坐标轴下的感应磁场分量；

各所述激励信号为初始相位不同且频率不同的正弦信号。也就是说，通入三轴磁信标的各个螺线管中的激励信号的初始相位和频率均不同，从而便于对各个螺线管产生的磁场进行区分。各路激励信号的初始相位可以分别为0°、60°和120°，也可以为0°、75°和135°，当然，也可以为其他组合。各路激励信号的频率可以分别为f1、f2、f3。此时，在步骤S100之后，可以通过带通滤波器直接对感应磁场矢量进行过滤，将采集到的感应磁场矢量分离为：

得到三轴正交螺线管对应的感应磁场矢量，

分别为第n次采集到的三路激励信号产生的感应磁场在设定坐标系下的感应磁场矢量。利用三轴螺线管对应轴产生磁场矢量计算三组特征矢量与每个轴的总感应磁场强度进行定位，定位精度不受传感器姿态的影响。

根据所述感应磁场分量确定所述感应磁场分量在目标点处沿不同所述坐标轴的磁感应强度；

这里，如图3所示，根据所述三轴磁信标的位置确定设定坐标系，确定所述目标点相对于所述磁信标的位置信息。所述设定坐标系的X轴可以与第一螺线管的中轴线重合，所述设定坐标系的Y轴可以与第二螺线管的中轴线重合，所述设定坐标系的Z轴可以与第三螺线管的中轴线重合，x、y、z分别是基于所述设定坐标系的坐标信息。这里的位置信息包括磁信标与目标点之间相对于X轴、Y轴和Z轴的偏航角和俯仰角。

根据磁偶极理论，三轴正交螺线管产生的感应磁场矢量可以表示为：

其中，M₁、M₂、M₃分别为第一螺线管、第二螺线管和第三螺线管的磁矩；μ₀为磁场传播介质因子，r为目标点与磁信标之间的距离，ω₁、ω₂、ω₃分别为第一螺线管、第二螺线管和第三螺线管对应的激励电流的频率；θ₁、θ₂、θ₀分别为磁信标与目标点之间相对于X轴、Y轴和Z轴的偏航角，φ₁、φ₂、φ₀分别为磁信标与目标点之间相对于X轴、Y轴和Z轴的俯仰角。

将任意两个所述感应磁场分量叉乘，获得所述实际特征矢量，即

步骤S300、根据所述磁感应强度、所述实际特征矢量和所述位置信息确定所述传感器的位置解算函数；

在S300中，根据所述磁感应强度和所述实际特征矢量确定磁感应强度模值和实际特征矢量模值；

各个坐标轴的磁感应强度模值为：

这里，实际特征矢量的矢量形式可表示为：

实际特征矢量的模值可表示为：

所述位置关系还包括磁信标与目标点之间的相对方位与相对位置，即：

磁感应强度模值也可以表示为：

实际特征矢量模值可表示为：

根据所述磁感应强度模值、所述实际特征矢量模值和所述位置关系建立所述传感器的位置解算函数，所述位置解算函数为：

其中，

分别为所述感应磁场分量在目标点处沿不同所述坐标轴的磁感应强度模值，

为沿XY坐标轴的两个所述感应磁场分量叉乘后获得的实际特征矢量模值，

为沿XZ坐标轴的两个所述感应磁场分量叉乘后获得的实际特征矢量模值，

为沿YZ坐标轴的两个所述感应磁场分量叉乘后获得的实际特征矢量模值，这里，m、n、p并不代表特定的意义，m＝μ₀M₁sin(ω₁t)，n＝μ₀M₂sin(ω₂t+60)，p＝μ₀M₃sin(ω₃t+120)；M₁、M₂、M₃分别为所述三轴磁信标不同的螺线管产生的磁矩；μ₀为磁场传播介质因子，ω₁、ω₂、ω₃分别为不同的所述螺线管的激励电流的频率。

步骤S400、根据位置解算函数确定所述传感器的最优位置；

较佳地，在S400中，根据所述位置解算函数确定迭代方程和所述迭代方程的雅可比矩阵；

这里，迭代方程为：

雅可比矩阵为：

其中，

获取迭代初始条件、迭代终止条件和信赖域半径；

迭代初始条件即给定初始值，f(a₀),a₀＝(x₀，y₀，z₀，m₀,n₀,p₀)；设定信赖域半径Δ₀，设定迭代终止条件ε₁、ε₂、ε₃；

狗腿法是高斯牛顿法与最速下降法的混合形式，通过狗腿法的迭代增量的变化来对高斯牛顿法与最速下降法这两种方法的迭代进行切换。基于狗腿优化算法，根据所述雅可比矩阵确定所述高斯牛顿迭代法和所述最速下降法下的最快下降的迭代步长；

高斯牛顿迭代法下的最快下降的迭代步长为：

最速下降法下的最快下降的迭代步长为：

这里，对梯度

进行求解，如果

则退出迭代，否则继续迭代。如果‖f(a_k)‖≤ε₃，则退出迭代，否则继续迭代；

根据所述信赖域半径、所述高斯牛顿迭代法和所述最速下降法下的最快下降的迭代步长确定所述狗腿法的迭代增量；

其中，

β使得‖h_sd‖＝Δ；

根据所述迭代初始条件、迭代终止条件和所述狗腿法的迭代增量对所述迭代方程进行迭代；

增益比为：

获得所述传感器的最优位置，即

步骤S500、根据所述最优位置确定所述目标点处的理论特征矢量；

步骤S600、根据所述理论特征矢量和所述实际特征矢量确定所述传感器的位姿。

在S600中，将所述理论特征矢量的逆矩阵和所述实际特征矢量相乘，获得关于所述传感器位姿的方向余弦阵；

所述方向余弦阵为：

其中，

为所述理论特征矢量，

为所述实际特征矢量；

根据所述方向余弦阵确定包括所述传感器的翻滚角、俯仰角和偏航角的传感器位姿，所述传感器位姿为：

其中，α表示翻滚角，β表示俯仰角，γ表示偏航角。

仿真验证，对磁感应与特征矢量融合的位姿解算方法进行仿真验证。将本实例与多信标几何差分的方法进行比较。在本实例仿真验证中，激励电流的幅值均为10A、频率分别为10Hz、20Hz、30Hz。在多信标几何差分法仿真验证中，各磁信标由正交的双轴螺线管构成，每个轴通入的电流频率相同，各个信标的位置信息及工作频率如表1所示，激励电流的幅值均为10A。在本方法仿真验证中，设磁信标通入的电流频率分别为10Hz、20Hz、30Hz，幅值均为10A。环境中存在均值为40000nT的常值干扰磁场、幅度为10nT的白噪声。磁信标在整个坐标系中的初始位置(0m 0m 0m)，目标点P所在位置如表2所示。

表1各磁信标位置信息及工作频率

磁信标	坐标位置	工作频率
			1	(0m,0m,0m)	10Hz
2	(5m,0m,0m)	20Hz
			3	(0m,5m,0m)	30Hz

表2目标点坐标

测量点	坐标	姿态角
			P<sub>1</sub>	(2.10m,1.80m,0.80m)	α＝5.83°,β＝27.62°,γ＝18.81°
P<sub>2</sub>	(2.70m,2.40m,1.20m)	α＝54.16°,β＝48.73°,γ＝26.41°
			P<sub>3</sub>	(3.30m,3.00m,1.60m)	α＝12.54°,β＝34.45°,γ＝62.58°
P<sub>4</sub>	(3.90m,3.60m,2.00m)	α＝45.87°,β＝65.94°,γ＝38.43°

利用磁通门传感器在目标位置以1000Hz的采样频率从测量点采集磁场数据，再用本实施例所述的基于磁感应强度与特征矢量位姿识别方法进行位姿估计，则可得到定位结果如表3和表4所示。实例结果表明，同等条件下，与多信标几何差分法相比，本实施例提出的基于磁感应强度与特征矢量位姿识别方法可以有效保证精度，相比于多磁信标差分几何法的测量结果，偏差几乎可以控制在1％以内。

表3位置仿真结果对比

表4姿态仿真结果对比

本发明通过获取到的感应磁场矢量确定磁感应强度和对应的实际特征矢量，进而建立位置解算函数，并通过对位置解算函数的解算获得传感器的最优位置，并根据最优位置确定目标点处的理论特征矢量，进而对传感器的位姿进行确定；本发明通过定位信息实现目标姿态的解算，实现目标位姿信息的获取，在保证位姿精度的同时节省了位姿解算系统成本，降低了位姿解算系统的复杂度，极大的提高了磁信标位姿解算系统在火灾场景、地下矿井以及城市管廊等复杂场景应用的可能性。

如图2所示，本发明实施例还提供一种基于磁感应强度与特征矢量的传感器位姿识别装置，包括：

获取模块，用于连续获取空间磁场在目标点处的感应磁场矢量；

计算模块，用于根据所述感应磁场矢量和所述目标点处的位置信息确定所述目标点处的磁感应强度和对应的实际特征矢量；

模型建立模块，用于根据所述磁感应强度、所述实际特征矢量和所述位置信息确定所述传感器的位置解算函数；

估算模块，用于根据所述位置解算函数确定所述传感器的最优位置；

所述计算模块还用于根据所述最优位置确定所述传感器的理论特征矢量；

所述计算模块还用于根据所述理论特征矢量和所述实际特征矢量确定所述传感器的位姿。

本发明的实施例所提供的基于磁感应强度与特征矢量的传感器位姿识别装置可执行本发明任意实施例所提供的基于磁感应强度与特征矢量位姿识别方法，具备基于磁感应强度与特征矢量位姿识别方法相应的功能模块和有益效果，在此不再赘述。

本发明实施例还提供一种计算机可读存储介质，所述计算机可读存储介质存储有计算机程序，所述计算机程序被处理器读取并运行时，实现如上述所述的基于磁感应强度与特征矢量位姿识别方法。

需要说明的是，本发明实施例所提供的一种包含计算机可执行指令的存储介质,其计算机可执行指令不限于如上所述的方法操作,还可以执行本发明任意实施例所提供的基于磁感应强度与特征矢量位姿识别方法中的相关操作。

本发明实施例的计算机可读存储介质，可以采用一个或多个计算机可读的介质的任意组合。计算机可读介质可以是计算机可读信号介质或者计算机可读存储介质。计算机可读存储介质例如可以是——但不限于——电、磁、光、电磁、红外线、或半导体的系统、装置或器件，或者任意以上的组合。计算机可读存储介质的更具体的例子(非穷举的列表)包括：具有一个或多个导线的电连接、便携式计算机磁盘、硬盘、随机存取存储器(RAM)、只读存储器(ROM)、可擦式可编程只读存储器(EPROM或闪存)、光纤、便携式紧凑磁盘只读存储器(CD-ROM)、光存储器件、磁存储器件、或者上述的任意合适的组合。在本文件中，计算机可读存储介质可以是任何包含或存储程序的有形介质，该程序可以被指令执行系统、装置或者器件使用或者与其结合使用。

计算机可读的信号介质可以包括在基带中或者作为载波一部分传播的数据信号，其中承载了计算机可读的程序代码。这种传播的数据信号可以采用多种形式，包括但不限于电磁信号、光信号或上述的任意合适的组合。计算机可读的信号介质还可以是计算机可读存储介质以外的任何计算机可读介质，该计算机可读介质可以发送、传播或者传输用于由指令执行系统、装置或者器件使用或者与其结合使用的程序。

虽然本公开披露如上，但本公开的保护范围并非仅限于此。本领域技术人员在不脱离本公开的精神和范围的前提下，可进行各种变更与修改，这些变更与修改均将落入本发明的保护范围。

Claims (10)

1.一种基于磁感应强度与特征矢量位姿识别方法，其特征在于，包括：

采用传感器连续获取空间磁场在目标点处的感应磁场矢量；

根据所述感应磁场矢量和所述目标点处的位置信息确定所述目标点处的磁感应强度和对应的实际特征矢量；

根据所述磁感应强度、所述实际特征矢量和所述位置信息确定所述传感器的位置解算函数；

根据所述位置解算函数确定所述传感器的最优位置；

根据所述最优位置确定所述目标点处的理论特征矢量；

根据所述理论特征矢量和所述实际特征矢量确定所述传感器的位姿。

2.根据权利要求1所述的基于磁感应强度与特征矢量位姿识别方法，其特征在于，在所述采用传感器连续获取空间磁场在目标点处的感应磁场矢量之前，还包括：

将三路激励信号分别通入三轴磁信标的三个螺线管中，激励所述三轴磁信标产生所述空间磁场。

3.根据权利要求2所述的基于磁感应强度与特征矢量位姿识别方法，其特征在于，在根据所述感应磁场矢量和所述目标点处的位置信息确定所述目标点处的磁感应强度和对应的实际特征矢量之前，还包括：

根据所述三轴磁信标的位置确定设定坐标系，确定所述目标点相对于所述磁信标的位置信息；

所述根据所述感应磁场矢量和所述目标点处的位置信息确定所述目标点处的磁感应强度和对应的实际特征矢量包括：

根据所述感应磁场矢量和所述位置信息确定所述感应磁场矢量在所述设定坐标系的不同坐标轴下的感应磁场分量；

根据所述感应磁场分量确定所述感应磁场分量在目标点处沿不同所述坐标轴的磁感应强度；

将任意两个所述感应磁场分量叉乘，获得所述实际特征矢量。

4.根据权利要求2所述的基于磁感应强度与特征矢量位姿识别方法，其特征在于，各所述激励信号为初始相位不同且频率不同的正弦信号。

5.根据权利要求3所述的基于磁感应强度与特征矢量位姿识别方法，其特征在于，所述根据所述磁感应强度、所述实际特征矢量和所述位置信息确定所述传感器的位置解算函数包括：

根据所述磁感应强度和所述实际特征矢量确定磁感应强度模值和实际特征矢量模值；

根据所述磁感应强度模值、所述实际特征矢量模值和所述位置关系建立所述传感器的位置解算函数，所述位置解算函数为：

其中，

分别为所述感应磁场分量在目标点处沿不同所述坐标轴的磁感应强度模值，

为沿XY坐标轴的两个所述感应磁场分量叉乘后获得的实际特征矢量模值，

为沿XZ坐标轴的两个所述感应磁场分量叉乘后获得的实际特征矢量模值，

为沿YZ坐标轴的两个所述感应磁场分量叉乘后获得的实际特征矢量模值，m＝μ₀M₁sin(ω₁t)，n＝μ₀M₂sin(ω₂t+60)，p＝μ₀M₃sin(ω₃t+120)；M₁、M₂、M₃分别为所述三轴磁信标不同的螺线管产生的磁矩；μ₀为磁场传播介质因子，ω₁、ω₂、ω₃分别为不同的所述螺线管的激励电流的频率。

6.根据权利要求1-5任一所述的基于磁感应强度与特征矢量位姿识别方法，其特征在于，所述根据所述位置解算函数确定所述传感器的最优位置包括：

根据所述位置解算函数确定迭代方程和所述迭代方程的雅可比矩阵；

获取迭代初始条件、迭代终止条件和信赖域半径；

基于狗腿优化算法，根据所述雅可比矩阵确定高斯牛顿迭代法和最速下降法下的最快下降的迭代步长；

根据所述信赖域半径、所述高斯牛顿迭代法和所述最速下降法下的最快下降的迭代步长确定所述狗腿法的迭代增量；

根据所述迭代初始条件、迭代终止条件和所述狗腿法的迭代增量对所述迭代方程进行迭代，获得所述传感器的最优位置。

7.根据权利要求6所述的基于磁感应强度与特征矢量位姿识别方法，其特征在于，所述根据所述理论特征矢量和所述实际特征矢量确定所述传感器的位姿包括：

将所述理论特征矢量的逆矩阵和所述实际特征矢量相乘，获得关于所述传感器位姿的方向余弦阵；

根据所述方向余弦阵确定包括所述传感器的翻滚角、俯仰角和偏航角的传感器位姿。

8.根据权利要求7所述的基于磁感应强度与特征矢量位姿识别方法，其特征在于，所述方向余弦阵为：

其中，

为所述理论特征矢量，

为所述实际特征矢量；

所述传感器位姿为：

其中，α表示翻滚角，β表示俯仰角，γ表示偏航角。

9.一种基于磁感应强度与特征矢量的传感器位姿识别装置，其特征在于，包括：

获取模块，用于连续获取空间磁场在目标点处的感应磁场矢量；

计算模块，用于根据所述感应磁场矢量和所述目标点处的位置信息确定所述目标点处的磁感应强度和对应的实际特征矢量；

模型建立模块，用于根据所述磁感应强度、所述实际特征矢量和所述位置信息确定所述传感器的位置解算函数；

估算模块，用于根据所述位置解算函数确定所述传感器的最优位置；

所述计算模块还用于根据所述最优位置确定所述传感器的理论特征矢量；

所述计算模块还用于根据所述理论特征矢量和所述实际特征矢量确定所述传感器的位姿。

10.一种计算机可读存储介质，其特征在于，所述计算机可读存储介质存储有计算机程序，所述计算机程序被处理器读取并运行时，实现如权利要求1至8中任一项所述的基于磁感应强度与特征矢量位姿识别方法。

Images