基于交变磁场的六自由度定位方法和系统与流程

本发明涉及近距离磁定位，具体地，涉及一种基于交变磁场的六自由度定位方法和系统。

背景技术：

1、磁场定位技术作为一种高效快速的定位跟踪手段，逐渐成为一个研究热点。近距离磁定位技术目前常用的定位原理有以下两种：

2、一种是利用静态或者准静态电磁场在空间的分布信息，通过磁传感器测量空间一处或者多处的磁场强度，再根据一定的算法反推出目标的位置和姿态信息，由于人体组织磁导率与真空磁导率相近，磁场在人体中不会出现明显畸变，这种方法具有三维空间位置和姿态同时测量、对人身无损伤、无视线问题、操作简便、实时的优点，最大的缺陷是易受铁磁性物质干扰；

3、另一种是利用电磁波(无线电波)的反射性质，从发射端发射电磁脉冲，然后在接收端接收反射回来的信号，继而确定目标的位置和运动状态，这种方法不需要在待测物体内设置额外的定位装置，但是当电磁波频率低时，比如短波，定位精度不高；增加电磁波频率，则会导致波的反射性增强。

技术实现思路

1、针对现有技术中的缺陷，本发明的目的是提供一种基于交变磁场的六自由度定位方法和系统。

2、根据本发明提供的基于交变磁场的六自由度定位方法，包括：

3、步骤1：构建系统空间直角坐标系oxyz，记磁传感器所在空间位置p在系统坐标系的位置为(x,y,z)，然后确定其中一个线圈磁场在oxyz坐标系下的位置，记为q1(x1,y1,z1)；以q1(x1,y1,z1)为原点建立坐标系q1-x1y1z1，称坐标系一；以p为原点的坐标系p-xyz，称坐标系二；

4、坐标系一和坐标系二的坐标轴均与世界坐标系的坐标轴同向平行；

5、p-x1y1z1是以p为原点，三个坐标轴是三轴磁传感器的测量方向，称坐标系三，坐标系三视为坐标系二绕p点旋转得到；

6、p在坐标系一的位置为(x-x1,y-y1,z-z1)，代入线圈磁场解析模型，得到p点处磁源线圈q1的三轴磁感应强度；

7、步骤2：计算磁传感器的理论值，并获取磁传感器实际测量值；

8、步骤3：利用最优化算法求解理论值与实际测量值的差值的最小值，得到磁传感器在p处的位置和姿态参数。

9、优选地，磁场解析模型的输入为空间位置，输出为磁感应强度，表达式为：

10、

11、其中，bx、by、bz分别为x、y、z方向的磁感应强度；μ0为真空中的磁导率，n为线圈匝数，i为通过线圈的电流，r为线圈电阻，r为线圈半径；

12、得到p点处磁源线圈q1的三轴磁感应强度为：

13、

14、其中，表示p点处磁源线圈q1的三轴磁感应强度，b1x、b1y、b1z为矢量b1在x、y、z方向的分量。

15、优选地，从坐标系二到坐标系三，设坐标系二的欧拉角中俯仰角、横滚角、偏航角的角度为α、β、γ，则有：

16、沿x轴的旋转矩阵：

17、沿y轴的旋转矩阵：

18、沿z轴的旋转矩阵：

19、从坐标系二到坐标系三，先绕z轴旋转，再绕y轴旋转，最后绕x轴旋转，最终的旋转矩阵如下：

20、

21、得到坐标系三的三轴磁感应强度，即磁传感器的理论值：

22、

23、其中，表示磁传感器的理论值；b1x′、b1y′、b1z′表示理论计算的磁感应强度b1在坐标系三中，沿三个坐标轴方向的分量。

24、优选地，磁传感器实际测量值为：

25、

26、其中，表示磁传感器实际测量值；b1x”、b1y”、b1z”表示实际测量的磁感应强度b1在坐标系三中，沿三个坐标轴方向的分量。

27、优选地，通过构建误差函：error1＝|b1x′-b1x″|+|b1y′-b1y″|+|b1z′-b1z″|，当所有误差均不存在时，error1等于零，即求得满足磁传感器的理论值与磁传感器实际测量值相等条件的位姿参数；其中，error1表示空间内单个测量点处，理论计算的磁感应强度与实际测量得到磁感应强度的差值；

28、对磁源线圈q2，q3，q4分别进行分析，得error2，error3，error4；

29、令

30、其中，error表示空间内四个测量点处，理论计算的磁感应强度与实际测量得到磁感应强度的差值；

31、利用最优化算法求解目标函数的最小值，得到磁传感器在p处的位置和姿态参数。

32、根据本发明提供的基于交变磁场的六自由度定位系统，包括：

33、模块m1：构建系统空间直角坐标系oxyz，记磁传感器所在空间位置p在系统坐标系的位置为(x,y,z)，然后确定其中一个线圈磁场在oxyz坐标系下的位置，记为q1(x1,y1,z1)；以q1(x1,y1,z1)为原点建立坐标系q1-x1y1z1，称坐标系一；以p为原点的坐标系p-xyz，称坐标系二；

34、坐标系一和坐标系二的坐标轴均与世界坐标系的坐标轴同向平行；

35、p-x1y1z1是以p为原点，三个坐标轴是三轴磁传感器的测量方向，称坐标系三，坐标系三视为坐标系二绕p点旋转得到；

36、p在坐标系一的位置为(x-x1,y-y1,z-z1)，代入线圈磁场解析模型，得到p点处磁源线圈q1的三轴磁感应强度；

37、模块m2：计算磁传感器的理论值，并获取磁传感器实际测量值；

38、模块m3：利用最优化算法求解理论值与实际测量值的差值的最小值，得到磁传感器在p处的位置和姿态参数。

39、优选地，磁场解析模型的输入为空间位置，输出为磁感应强度，表达式为：

40、

41、其中，bx、by、bz分别为x、y、z方向的磁感应强度；μ0为真空中的磁导率，n为线圈匝数，i为通过线圈的电流，r为线圈电阻，r为线圈半径；

42、得到p点处磁源线圈q1的三轴磁感应强度为：

43、

44、其中，表示p点处磁源线圈q1的三轴磁感应强度，b1x、b1y、b1z为矢量b1在x、y、z方向的分量。

45、优选地，从坐标系二到坐标系三，设坐标系二的欧拉角中俯仰角、横滚角、偏航角的角度为α、β、γ，则有：

46、沿x轴的旋转矩阵：

47、沿y轴的旋转矩阵：

48、沿z轴的旋转矩阵：

49、从坐标系二到坐标系三，先绕z轴旋转，再绕y轴旋转，最后绕x轴旋转，最终的旋转矩阵如下：

50、

51、得到坐标系三的三轴磁感应强度，即磁传感器的理论值：

52、

53、其中，表示磁传感器的理论值；b1x′、b1y′、b1z′表示理论计算的磁感应强度b1在坐标系三中，沿三个坐标轴方向的分量。

54、优选地，磁传感器实际测量值为：

55、

56、其中，表示磁传感器实际测量值；b1x”、b1y”、b1z”表示实际测量的磁感应强度b1在坐标系三中，沿三个坐标轴方向的分量。

57、优选地，通过构建误差函：error1＝|b1x′-b1x″|+|b1y′-b1y″|+|b1z′-b1z″|，当所有误差均不存在时，error1等于零，即求得满足磁传感器的理论值与磁传感器实际测量值相等条件的位姿参数；其中，error1表示空间内单个测量点处，理论计算的磁感应强度与实际测量得到磁感应强度的差值；

58、对磁源线圈q2，q3，q4分别进行分析，得error2，error3，error4；

59、令

60、其中，error表示空间内四个测量点处，理论计算的磁感应强度与实际测量得到磁感应强度的差值；

61、利用最优化算法求解目标函数的最小值，得到磁传感器在p处的位置和姿态参数。

62、与现有技术相比，本发明具有如下的有益效果：

63、(1)本发明提出一种基于交变磁场的新型六自由度定位方法，由不同频率的交变线圈产生磁场，通过设置交变磁场的个数，该定位方法可以实现精度更高六自由度定位；

64、(2)由于待测物体与磁传感器固定在一起，磁传感器的增加不会影响系统内磁场分布，因此可以同时实现多目标的定位；

65、(3)利用交变磁场的频域特性，不受铁磁性物质等产生的静态磁场干扰；此外，通过选择合适的磁场频率，还可以有效提高电磁定位的抗干扰特性。