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

权利要求书

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任一所述的基于磁感应强度与特征矢量位姿识别方法，其特征在于，所述根据所述位置解算函数确定所述传感器的最优位置包括：

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

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

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

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

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