CN105136149B

CN105136149B - 一种圆线圈磁场定位装置和方法

Info

Publication number: CN105136149B
Application number: CN201510579142.2A
Authority: CN
Inventors: 吴发林; 梁袁; 付勇; 赵剡
Original assignee: Beihang University
Current assignee: Beihang University
Priority date: 2015-09-11
Filing date: 2015-09-11
Publication date: 2018-04-13
Anticipated expiration: 2035-09-11
Also published as: CN105136149A

Abstract

本发明公开了一种圆线圈磁场定位装置和方法，包括：数据处理及控制单元、D/A转换电路、驱动电路、保护电路、圆线圈、电流检测电路、滤波放大电路、A/D转换电路以及三轴磁力计。首先由数据处理及控制单元控制D/A转换电路按照码分多址的方式产生驱动信号给驱动电路，在保护电路的作用下驱动圆线圈产生低频变化的磁场，同时电流检测电路将通过圆线圈的电流信号经滤波放大和A/D转换电路后给数据处理及控制单元，构成电流控制的反馈回路。三轴磁力计将测量得到的磁场信息传给上位机进行位置和姿态的解算并显示。优点在于：圆线圈的结构简明，架设灵活，磁场利用率与冗余性高，求解算法不受非视距的影响，不需要离线指纹地图，抗干扰能力强。

Description

一种圆线圈磁场定位装置和方法

技术领域

本发明涉及电磁学和室内定位领域，具体涉及一种圆线圈磁场室内定位装置和方法。

背景技术

随着全球定位系统(GPS)应用的普及，人们已经习惯在室外利用GPS来享受定位和导航服务，但GPS在复杂的室内环境中不能提供较高的定位精度，从而不能满足人们对室内定位日益增长的需求。因此，室内定位技术有着广阔的应用前景。现有的室内定位技术主要有红外定位技术，超声波定位技术，超宽带定位技术以及无线局域网络技术等。然而，这些方法常常受到非视距和多路径的影响而无法完成对目标的定位工作。另外，基于地磁信息的定位技术虽然不受视线阻挡的限制，但前期要采集大量的磁场信息并建立相应的指纹库，才能在定位时完成匹配工作。该方法不仅需要存储大量的数据，在环境信息变更时需要更新相应的指纹库，而且还容易受到电磁干扰，所以可靠性较低。

发明内容

本发明提供一种圆线圈磁场室内定位装置和方法，以解决室内定位的非视距问题，并解决利用磁场定位需要构建指纹库以及容易受到电磁干扰的问题。

本发明采取的技术方案为：

一种圆线圈磁场室内定位系统，包括：数据处理及控制单元、D/A转换电路、驱动电路、保护电路、圆线圈、电流检测电路、滤波放大电路、A/D转换电路顺序连接，其中数据处理及控制单元与上位机连接，三轴磁力计也与上位机连接；

数据处理及控制单元，主要由DSP处理器以及SDRAM存储器，FLASH存储器，电源组成，一方面与上位机相连接，由上位机对其进行逻辑功能和数据处理功能的初始设定，另一方面与D/A转换电路和A/D转换电路连接，前者是控制D/A转换电路产生驱动信号，后者是通过A/D转换电路接收通过圆线圈的反馈电流信号；

D/A转换电路，将数据处理及控制单元输出的数字信号转换为模拟信号，并作为驱动信号给驱动电路；

驱动电路，主要由幅度放大、功率放大电路组成，将驱动信号进行放大处理，在保护电路的作用下驱动圆线圈产生低频变化的磁场；

保护电路，由过压保护电路和过流保护电路构成，起静电保护和防止芯片以及圆线圈损坏的作用；

圆线圈，是能够形成圆度、间距比较统一的磁场线圈，并按照一定拓扑结构固定在室内，根据驱动信号产生低频变化的磁场，优选地，将待测空间平面分成多个矩形面，在这些矩形面的四个顶点分别放置圆线圈，且圆线圈平行于同一水平面；

电流检测电路，主要由分流电阻器和电流检测放大器组成，目的是将通过圆线圈的电流值大小反馈给数据处理及控制单元；

滤波放大电路，主要由仪表放大器和自动增益控制电路组成，将电流检测电路得到的电流值进行滤波及幅度放大；

A/D转换电路，负责将滤波放大后的模拟信号转换为供数据处理及控制单元处理的数字信号；

三轴磁力计，用于测量传感器坐标系中三个正交的轴向磁场大小，可是独立的测量设备，或集成在移动终端中的设备；

上位机，主要是PC机或移动终端设备，通过串口或者USB与数据处理及控制单元和三轴磁力计分别相连，上位机利用三轴磁力计的输出数据进行位置和姿态的求解并将求解结果显示在上位机的交互式图形界面上。

本发明所述的数据处理及控制单元工作时，首先由上位机通过串口或者USB将初始的驱动数字信号和电流反馈算法程序下载到FLASH存储器中，供DSP读取，然后通过D/A转换电路将数字信号转换为模拟信号给驱动电路，在保护电路的作用下驱动圆线圈产生低频变化的磁场，且通过圆线圈的电流信号经过电流检测电路以及滤波放大电路和A/D转换电路后，将测量得到的模拟信号转换为数字信号给数据处理及控制单元，构成电流控制的反馈回路，而空间中的三轴磁力计则将测量得到的信号传给上位机进行位置和姿态的解算。

本发明的一种圆线圈磁场定位方法，包括下列步骤：

(1)在空间设定三维坐标系的原点(0,0,0)，建立空间直角坐标系，测量每个圆线圈相对原点的空间三维坐标，并将原点与每个圆线圈的三维坐标输入上位机，优选地，空间直角坐标系的x轴与y轴构成的平面平行于水平面；

(2)基于码分多址CDMA的通信结构，伪随机码选择组合码中的金码，给每个圆线圈分配不同的金码并设定相同的码宽T_c，N个圆线圈需要约个不同的金码，设定金码周期为的M个码片，并将每个圆线圈分配得到的金码和码宽信息输入上位机，N个圆线圈中，第i个圆线圈在空间直角坐标系的位置记为(x_i,y_i,z_i)，i的取值为i＝1,2,3,...,N，分配得到的金码记为C_i＝[c_i(1)c_i(2)c_i(3)…c_i(M)]^T，其中c_i(j)＝±1，j的取值为j＝1,2,3,...,M；

(3)金码中的“1”代表电流大小为I的正向电流，“-1”代表电流大小为I的负向电流，由上位机将分配给每个圆线圈的金码及电流反馈控制程序传给数据处理及控制单元，再由数据处理及控制单元控制D/A转换电路，同时生成驱动信号给每一个驱动电路，最后经过保护电路后周期性地驱动圆线圈，每个圆线圈就在空间中同时产生频率在0Hz～100kHz范围内的低频变化磁场，电流检测电路将通过圆线圈的电流信号经过滤波放大电路放大后，再通过A/D转换电路将模拟信号转换为数字信号给数据处理及控制单元，利用电路反馈控制程序以提高流过圆线圈电流大小的精度；

(4)三轴磁力计在待测空间目标位置测量传感器坐标系中三个正交的轴向磁场大小，并将数据传给上位机进行位置和姿态的解算，最后将求解的位置和姿态信息在上位机上显示出来；

所述位置和姿态的求解算法，其特征是，包含以下步骤：

(1)用矩阵表示待测目标的位置和姿态参数；

待测目标的位置信息由相对空间三维坐标原点(0,0,0)的位置(x,y,z)描述，姿态信息由方位角α，横滚角β，俯仰角γ描述，姿态角可由姿态矩阵T表示，T为正交矩阵；

(2)利用毕奥萨伐尔定律构建圆线圈产生磁场的磁偶极子模型，圆线圈的半径为R，圈数为n，面积为s＝πR²，流过圆线圈的电流大小为I，则位于空间直角坐标系中的待测点P(x,y,z)用球坐标表示为磁通量B为

式(2)中，μ₀＝4π×10^-7H/m，为真空的磁导率，r_e与θ_e分别为点P在球坐标系中r方向上与θ方向上的单位矢量，记第i个圆线圈在P点沿着某一方向v产生的磁通量大小为B_iv；

(3)在一个完整的金码周期内，空间坐标系中N个圆线圈在P点沿着v方向产生的磁通量序列测量值记为：

式(3)中，b_v为P点沿着v方向的磁通量偏差，该偏差包含了地磁场在P点沿着v方向的磁通量大小，M_v是M×1的矩阵，写成矩阵表达式为

M_v＝AX_v (4)

A为M×(N+1)的矩阵，X_v为(N+1)×1的矩阵，即

记M_{v_est}为P点沿着v方向的磁通量序列测量值，利用金码具有的优良自相关和互相关性能，通过最小二乘法，即可识别出P点的磁场是哪些圆线圈产生的磁场叠加产生的，具体计算公式为：

X_{v_est}＝(A^TA)^-1A^TM_{v_est} (6)

通过式(6)计算的结果为X_{v_est}＝[B_{1v_est} B_{2v_est} B_{3v_est} … b_{v_est}]^T，其中B_{iv_est}的值越大，则说明第i个线圈距离P点越近；

(4)通过式(6)找出所有可能位于P点周围的圆线圈，定义磁通量序列测量值和计算值之间的偏差D_v为

D_v＝M_{v_est}-AX_{v_est} (7)

并定义品质因数Q为

式(8)中std(D_v)与std(M_{v_est})分别为D_v与M_{v_est}的标准差，品质因数的大小直接与P点周围最近的3-8个圆线圈的距离相关，Q值越大则说明所选择的圆线圈离P点越近，反之则越远；

(5)在首次进行解算或者在上位机计算能力足够的情况下，首先利用式(6)找出P点周围的圆线圈，再通过穷举的方式依次选择其中的3-8个圆线圈通过式(8)找出Q值最大的圆线圈组合进行之后的解算；

(6)解算前需要判别P点是否受到电磁感应的干扰，P点在空间坐标系中受到电磁感应干扰时沿着v方向产生的磁通量序列测量值记为

式(9)中，是根据C_i中的码片变化而产生序列，具体规则定义为，c_i(j)到c_i(j+1)为“-1”到“+1”时，中的为“+1”，c_i(j)到c_i(j+1)为“+1”到“-1”时，为“-1”，c_i(j)到c_i(j+1)保持不变时，为“0”；

是根据中的码片变化而产生序列，具体规则定义为，到为“-1”到“+1”或“-1”到“0”时，中的为“-1”，到为“+1”到“-1”或“1”到“0”时，为“+1”，到保持不变或为“0”到“+1”或“0”到“-1”时，为“0”；

是根据中的码片变化而产生序列，具体规则定义为，到为“-1”到“+1”或“-1”到“0”时，中的为“-1”，到为“+1”到“-1”或“1”到“0”变化时，为“+1”，到保持不变或为“0”到“+1”或“0”到“-1”变化时，为“0”；

分别为因C_i，中的码片变化而使P点在空间坐标系中沿着v方向受到电磁感应干扰的大小；

M_{v_emi}为M×1的矩阵，写成矩阵表达式为

M_{v_emi}＝A_emiX_{v_emi} (10)

A_emi为M×(4N+1)的矩阵，X_{v_emi}为(4N+1)×1的矩阵，即

同样通过最小二乘法，去识别出P点的磁场是哪些圆线圈产生的磁场叠加产生的，具体计算公式为：

通过式(12)计算的结果为比较式(6)与式(12)的结果，若出现|X_{v_est}(j)-X_{v_emi}(j)|≥1.0mG，则说明P点附近存在电磁感应干扰，应利用式(12)得到的X_{v_emi}进行之后的解算，否则应利用式(6)得到的X_{v_est}进行之后的解算；

(7)在空间直角坐标系中，第i个圆线圈在点P(x,y,z)处沿着三个轴向产生的磁通量大小分别为

式(13)中，k＝μ₀nIs/4π，

(8)第i个圆线圈在点P(x,y,z)处产生的磁场通过三轴磁力计进行测量，而三轴磁力计是以传感器坐标系为基准进行测量，测量得到的三个正交的轴向磁场B_is＝[B_isx,B_isy,B_isz]^T通过方向余弦矩阵转换到空间坐标系，具体计算公式为

式(14)中方向余弦矩阵中的元素满足

(9)在空间直角坐标系中第i个圆线圈在P点产生的磁通量大小为

在球坐标系下可简化为

由于故

|B_i|≈1.5k/r_i ³ (18)

式(17)构成含有N+3个未知数(x,y,z,θ_i)的N个方程组，式(18)构成含有N个未知数r_i的N个方程组，其中B_isx＝B_{ix_est}或B_isx＝B_{ix_emi}，B_isy＝B_{iy_est}或B_isy＝B_{iy_emi}，B_isz＝B_{iz_est}或B_isz＝B_{iz_emi}，取决于步骤(6)得到的结果，首先利用牛顿迭代法求解式(18)组成的方程组，分别求出r_i的估计值再利用作为初值通过牛顿迭代法求解式(17)组成的方程组，得到N+3个未知数(x,y,z,θ_i)的估计值由此得到P点在空间坐标系中的估计位置

(10)利用步骤(9)得到的P点估计位置通过式(13)重新计算P点处的磁通量定义矩阵对E进行奇异值分解，得到

E＝USV^T (19)

由此得矩阵T的估计矩阵为

由此对T中的方位角α，横滚角β，俯仰角γ进行估算

(11)判别P点附近是否存在磁性材料的干扰，定义残差

若不存在两个及以上|R_i|≥0.6mG的情况，则说明P点附近不存在磁性材料干扰的情况，由此解算结束，得到P点估计位置与估计姿态若存在两个及以上|R_i|≥0.6mG的情况，则说明P点附近存在磁性材料的干扰，根据式(2)，将此干扰产生的磁场矢量F_i模型化为单一磁偶极子产生的磁场矢量，如式(23)所示，并假设F_i≈R_i

式(23)中，k_F是与磁性材料的大小、磁导率以及与P点距离相关的常数，记的单位向量为定义磁性材料与三轴磁力计之间的方向向量为u，其单位向量为u_e，θ_Fi为向量与向量u之间的夹角，定义向量n_i，使得

选择|n_i|中最大的两个向量分别记为n_A与n_B，其单位向量分别为n_Ae与n_Be，对应的磁通量分别为与对应的θ_i分别记为θ_A与θ_B，则

u_e＝n_Ae×n_Be (26)

cosθ_Fi与sinθ_Fi通过以下公式计算

通过

求解

由此将k_F与cosθ_Fi及sinθ_Fi带入式(23)即可得到F_i，从B_i中减去F_i后便得到P点减少磁性材料干扰后的磁场即

将替换为B_i带入步骤(8)至步骤(10)重新对位置和姿态进行解算，且解算频率为1Hz，由此解算结束，得到P点估计位置与估计姿态

本发明具有以下优点：

(1)对待测目标的位置和姿态进行解算不受非视距的影响，不需要离线的指纹地图，而且圆线圈的结构简明，架设灵活，功耗低，磁场利用率与冗余性高；

(2)利用码分多址的方式使圆线圈同时产生低频变化的磁场不但能使环境中的干扰有延迟，而且不会像频分多址(FDMA)与时分多址(TDMA)那样随着圆线圈布置数量的增多而使得磁场变化的频率显著增加；

(3)选用最小二乘法进行求解，不但能对编码的圆线圈产生的磁场信号进行去相关的检测，而且对于待测目标附近存在的加性高斯白噪声，最小二乘法还是最优的线性无偏估计；

(4)分别对位置和姿态进行解算，使得两者间的耦合影响小，而且算法还能检测并减小待测目标附近存在的电磁感应干扰与磁性材料的干扰，大大提高了待测目标的位置和姿态求解的精度。

附图说明

图1是本发明的圆线圈磁场定位装置的总体框图；

图2是本发明的圆线圈示意图；

图3是本发明的位置和姿态求解算法的整体流程图；

图4是本发明的待测目标的坐标系OX_sY_sZ_s与空间三维坐标系OXYZ的方位坐标关系；

图5是本发明的圆线圈产生磁场的磁偶极子模型；

图6是本发明的磁性材料干扰化为单一磁偶极子产生磁场矢量的模型。

具体实施方式

下面给出本发明的具体实施方式并结合附图加以说明。

图1是本发明一种圆线圈磁场定位装置的总体框图。数据处理及控制单元100、D/A转换电路101、驱动电路102、保护电路103、圆线圈104、电流检测电路105、滤波放大电路106、A/D转换电路107顺序连接，其中数据处理及控制单元100与上位机108连接，三轴磁力计109也与上位机108连接；

数据处理及控制单元100，主要由DSP处理器以及SDRAM存储器，FLASH存储器、电源组成，一方面与上位机108相连接，由上位机108对其进行逻辑功能和数据处理功能的初始设定，另一方面与D/A转换电路101和A/D转换电路107连接，前者是控制D/A转换电路101产生驱动信号，后者是通过A/D转换电路107接收测量得到的通过圆线圈反馈电流；

D/A转换电路101，将数据处理及控制单元100输出的数字信号转换为模拟信号，并作为驱动信号给驱动电路102；

驱动电路102，主要由幅度放大、功率放大电路组成，将驱动信号进行放大处理，在保护电路103的作用下驱动圆线圈104产生低频变化的磁场；

保护电路103，由过压保护电路和过流保护电路构成，起静电保护和防止芯片以及圆线圈104损坏的作用；

圆线圈104，如图2所示，是能够形成圆度、间距比较统一的磁场线圈，并按照一定拓扑结构固定在室内，由驱动电路102经保护电路103后提供电流，产生低频变化的磁场，优选地，将待测空间平面分成多个矩形面，在这些矩形面的四个顶点分别放置圆线圈104，且圆线圈104平行于同一水平面；

电流检测电路105，主要由分流电阻器和电流检测放大器组成，目的是将通过圆线圈104的电流值大小反馈给数据处理及控制单元100；

滤波放大电路106，主要由仪表放大器和自动增益控制电路组成，将电流检测电路105得到的电流值进行滤波及放大；

A/D转换电路107，负责将滤波放大后的模拟信号转换为供数据处理及控制单元100处理的数字信号；

三轴磁力计109，用于测量传感器坐标系中三个正交的轴向磁场大小，可是独立的测量设备，或集成在移动终端中的设备；

上位机108，主要是PC机或移动终端设备，通过串口或者USB与数据处理及控制单元100和三轴磁力计109分别相连，上位机利用三轴磁力计109的输出数据进行位置和姿态的求解并将求解结果显示在上位机108的交互式图形界面上。

本发明所述的数据处理及控制单元100工作时，首先由上位机108通过串口或者USB将初始的驱动数字信号和电流反馈算法程序下载到FLASH存储器中，供DSP读取，然后通过D/A转换电路101将数字信号转换为模拟信号给驱动电路102，在保护电路103的作用下驱动圆线圈104产生低频变化的磁场，且通过圆线圈104的电流经过电流检测电路105及滤波放大电路106和A/D转换电路107后，将测量得到的模拟信号转换为数字信号给数据处理及控制单元100，构成电流控制的反馈回路，而空间中的三轴磁力计109则将测量得到的信号传给上位机108进行位置和姿态的解算。

本发明的一种圆线圈磁场的定位方法，包括下列步骤：

(1)在空间设定三维坐标系的原点(0,0,0)，建立空间直角坐标系，测量每个圆线圈104相对原点的空间三维坐标，并将原点与每个圆线圈104的三维坐标输入上位机，优选地，空间直角坐标系的x轴与y轴构成的平面平行于水平面；

(2)基于码分多址CDMA的通信结构，伪随机码选择组合码中的金码，给每个圆线圈分配不同的金码并设定相同的码宽T_c，N个圆线圈需要约个不同的金码，设定金码周期为的M个码片，并将每个圆线圈分配得到的金码和码宽信息输入上位机，N个圆线圈中，第i个圆线圈在空间直角坐标系的位置记为(x_i,y_i,z_i)，i的取值为i＝1,2,3,...,N，分配得到的金码记为C_i＝[c_i(1) c_i(2) c_i(3) … c_i(M)]^T，其中c_i(j)＝±1，j的取值为j＝1,2,3,...,M；

(3)金码中的“1”代表电流大小为I的正向电流，“-1”代表电流大小为I的负向电流，由上位机将分配给每个圆线圈的金码及电流反馈控制程序传给数据处理及控制单元，再由数据处理及控制单元控制D/A转换电路，同时生成驱动信号给每一个驱动电路，最后经过保护电路后周期性地驱动圆线圈，每个圆线圈就在空间中同时产生频率在0Hz～100kHz范围内的低频变化磁场，电流检测电路将通过圆线圈的电流信号经过滤波放大电路放大后，再通过A/D转换电路将模拟信号转换为数字信号给数据处理及控制单元，利用电流反馈控制算法以提高流过圆线圈电流大小的精度；

图3示出位置和姿态求解算法的整体流程图，位置和姿态求解算法的具体实现步骤如下：

(1)用矩阵表示待测目标的位置和姿态参数；

待测目标的位置信息由相对空间三维坐标原点(0,0,0)的位置(x,y,z)描述，图4示出了待测目标的坐标系OX_sY_sZ_s与空间三维坐标系OXYZ的方位坐标关系，坐标系OX₀Y₀Z₀以空间三维坐标系OXYZ为基准，绕Z₀轴旋α角得到坐标系OX₁Y₁Z₀，坐标系OX₁Y₁Z₀再绕X₁轴转β角得到坐标系OX₁Y_sZ₁，最后坐标系OX₁Y_sZ₁绕Y_s轴旋转γ角得到待测目标的坐标系OX_sY_sZ_s，待测目标的姿态信息就由方位角α，横滚角β，俯仰角γ描述，姿态角可由姿态矩阵T表示，T为正交矩阵；

(2)图5示出了利用毕奥萨伐尔定律构建圆线圈产生磁场的磁偶极子模型，圆线圈的半径为R，圈数为n，面积为s＝πR²，流过圆线圈的电流大小为I，则位于空间直角坐标系中的待测点P(x,y,z)用球坐标表示为磁通量B为

M_v＝AX_v (4)

A为M×(N+1)的矩阵，X_v为(N+1)×1的矩阵，即

记M_{v_est}为P点沿着v方向的磁通量序列测量值，利用金码具有的优良自相关和互相关性能(相同金码序列之间具有良好的相关性，不同金码序列之间基本不相关)，通过最小二乘法，即可识别出P点的磁场是哪些圆线圈产生的磁场叠加产生的，具体计算公式为：

X_{v_est}＝(A^TA)^-1A^TM_{v_est} (6)

D_v＝M_{v_est}-AX_{v_est} (7)

并定义品质因数Q为

M_{v_emi}为M×1的矩阵，写成矩阵表达式为

M_{v_emi}＝A_emiX_{v_emi} (10)

A_emi为M×(4N+1)的矩阵，X_{v_emi}为(4N+1)×1的矩阵，即

式(13)中，k＝μ₀nIs/4π，

式(14)中方向余弦矩阵中的元素满足

在球坐标系下可简化为

由于故

|B_i|≈1.5k/r_i ³ (18)

式(17)构成含有N+3个未知数(x,y,z,θ_i)的N个方程组，式(18)构成含有N个未知数r_i的N个方程组，其中B_isx＝B_{ix_est}或B_isx＝B_{ix_emi}，B_isy＝B_{iy_est}或B_isy＝B_{iy_emi}，B_isz＝B_{iz_est}或B_isz＝B_{iz_emi}，取决于步骤(6)得到的结果，首先利用牛顿迭代法求解式(18)组成的方程组，分别求出r_i的估计值再利用作为初值通过牛顿迭代法求解式(17)组成的方程组，得到N+3个未知数(x,y,z,θ_i)的估计值由此得到P点在空间坐标系中的估计位置优选地，利用步骤(5)找到的3-8个圆线圈进行解算；

(10)利用步骤(9)得到的P点位置通过式(13)重新计算P点处的磁通量定义矩阵对E进行奇异值分解，得到

E＝USV^T (19)

由此得矩阵T的估计矩阵为

由此对T中的方位角α，横滚角β，俯仰角γ进行估算

(11)判别P点附近是否存在磁性材料的干扰，定义残差

若不存在两个及以上|R_i|≥0.6mG的情况，则说明P点附近不存在磁性材料干扰的情况，由此解算结束，得到P点估计位置与估计姿态若存在两个及以上|R_i|≥0.6mG的情况，则说明P点附近存在磁性材料的干扰，图6示出了此干扰产生的磁场矢量化为单一磁偶极子产生的磁场矢量模型，利用式(2)定义F_i为式(23)，并假设F_i≈R_i

u_e＝n_Ae×n_Be (26)

cosθ_Fi与sinθ_Fi通过以下公式计算

通过

求解

将带入步骤(9)至步骤(10)重新对位置和姿态进行解算，且解算频率为1Hz，由此解算结束，重新得到P点去干扰后的估计位置与估计姿态

Claims

1.一种圆线圈磁场定位装置，其特征在于：数据处理及控制单元、D/A转换电路、驱动电路、保护电路、圆线圈、电流检测电路、滤波放大电路、A/D转换电路顺序连接，其中数据处理及控制单元与上位机连接，三轴磁力计也与上位机连接；

(1)数据处理及控制单元，主要由DSP处理器以及SDRAM存储器，FLASH存储器，电源组成，一方面与上位机相连接，由上位机对其进行逻辑功能和数据处理功能的初始设定，另一方面与D/A转换电路和A/D转换电路连接，前者是控制D/A转换电路产生驱动信号，后者是通过A/D转换电路接收通过圆线圈的反馈电流信号；

(2)D/A转换电路，将数据处理及控制单元输出的数字信号转换为模拟信号，并作为驱动信号给驱动电路；

(3)驱动电路，主要由幅度放大、功率放大电路组成，将驱动信号进行放大处理，在保护电路的作用下驱动圆线圈产生低频变化的磁场；

(4)保护电路，由过压保护电路和过流保护电路构成，起静电保护和防止芯片以及圆线圈损坏的作用；

(5)圆线圈，是能够形成圆度、间距比较统一的磁场线圈，并按照一定拓扑结构固定在室内，根据驱动信号产生低频变化的磁场；

(6)电流检测电路，主要由分流电阻器和电流检测放大器组成，目的是将通过圆线圈的电流大小反馈给数据处理及控制单元；

(7)滤波放大电路，主要由仪表放大器和自动增益控制电路组成，将电流检测电路得到的信号进行滤波及幅度放大；

(8)A/D转换电路，负责将滤波放大后的模拟信号转换为供数据处理及控制单元处理的数字信号；

(9)三轴磁力计，用于测量传感器坐标系中三个正交的轴向磁场大小，可是独立的测量设备，或集成在移动终端中的设备；

(10)上位机，主要是PC机或移动终端设备，通过串口或者USB与数据处理及控制单元和三轴磁力计分别相连，上位机利用三轴磁力计的输出数据进行位置和姿态的求解并将求解结果显示在上位机的交互式图形界面上。

2.根据权利要求1所述的一种圆线圈磁场定位装置，其特征在于：所述的数据处理及控制单元工作时，首先由上位机通过串口或者USB将初始的驱动数字信号和电流反馈控制程序下载到FLASH存储器中，供DSP读取，然后通过D/A转换电路将数字信号转换为模拟信号给驱动电路，在保护电路的作用下驱动圆线圈产生低频变化的磁场，且通过圆线圈的电流信号经过电流检测电路经滤波放大和A/D转换电路后，将测量得到的模拟信号转换为数字信号给数据处理及控制单元，构成电流控制的反馈回路，而空间中的三轴磁力计则将测量得到的信号传给上位机进行位置和姿态的解算并显示。

3.根据权利要求1所述的一种圆线圈磁场定位装置，其特征在于：所述的驱动信号是利用码分多址CDMA的方式，按照金码序列周期性产生的信号。

4.一种用于圆线圈磁场定位的方法，其特征在于包括下列步骤：

(1)在空间设定三维坐标系的原点(0,0,0)，建立空间直角坐标系，测量每个圆线圈相对原点的空间三维坐标，并将原点与每个圆线圈的三维坐标输入上位机；

(2)基于码分多址CDMA的通信结构，伪随机码选择组合码中的金码，给每个圆线圈分配不同的金码并设定相同的码宽T，N个圆线圈需要约个不同的金码，设定金码周期为的M个码片，并将每个圆线圈分配得到的金码和码宽信息输入上位机，N个圆线圈中，第i个圆线圈在空间直角坐标系的位置记为(x_i,y_i,z_i)，i的取值为i＝1,2,3,...,N，分配得到的金码记为C_i＝[c_i(1) c_i(2) c_i(3) … c_i(M)]^T，其中c_i(j)＝±1，j的取值为j＝1,2,3,...,M；

(4)三轴磁力计在待测空间目标位置测量传感器坐标系中三个正交的轴向磁场大小，并将数据传给上位机进行位置和姿态的解算，最后将求解的位置和姿态信息在上位机上显示出来。

5.根据权利要求4所述的一种用于圆线圈磁场定位的方法，其特征在于步骤(4)中所述的位置和姿态的解算算法，具体实现步骤如下：

(1)用矩阵表示待测目标的位置和姿态参数；

待测目标的位置信息由相对空间三维坐标原点(0,0,0)的位置(x,y,z)描述，姿态信息由方位角α，俯仰角β，横滚角γ描述，姿态角可由姿态矩阵T表示，T为正交矩阵；

<mrow> <mtable> <mtr> <mtd> <mrow> <mi>T</mi> <mo>=</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <mrow> <mi>cos</mi> <mi>&gamma;</mi> </mrow> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mrow> <mo>-</mo> <mi>sin</mi> <mi>&gamma;</mi> </mrow> </mtd> </mtr> <mtr> <mtd> <mn>0</mn> </mtd> <mtd> <mn>1</mn> </mtd> <mtd> <mn>0</mn> </mtd> </mtr> <mtr> <mtd> <mrow> <mi>sin</mi> <mi>&gamma;</mi> </mrow> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mrow> <mi>cos</mi> <mi>&gamma;</mi> </mrow> </mtd> </mtr> </mtable> </mfenced> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <mn>1</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> </mtr> <mtr> <mtd> <mn>0</mn> </mtd> <mtd> <mrow> <mi>cos</mi> <mi>&beta;</mi> </mrow> </mtd> <mtd> <mrow> <mi>sin</mi> <mi>&beta;</mi> </mrow> </mtd> </mtr> <mtr> <mtd> <mn>0</mn> </mtd> <mtd> <mrow> <mo>-</mo> <mi>sin</mi> <mi>&beta;</mi> </mrow> </mtd> <mtd> <mrow> <mi>cos</mi> <mi>&beta;</mi> </mrow> </mtd> </mtr> </mtable> </mfenced> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <mrow> <mi>cos</mi> <mi>&alpha;</mi> </mrow> </mtd> <mtd> <mrow> <mi>sin</mi> <mi>&alpha;</mi> </mrow> </mtd> <mtd> <mn>0</mn> </mtd> </mtr> <mtr> <mtd> <mrow> <mo>-</mo> <mi>sin</mi> <mi>&alpha;</mi> </mrow> </mtd> <mtd> <mrow> <mi>cos</mi> <mi>&alpha;</mi> </mrow> </mtd> <mtd> <mn>0</mn> </mtd> </mtr> <mtr> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> </mtr> </mtable> </mfenced> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <mo>=</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <mrow> <mi>cos</mi> <mi>&gamma;</mi> <mi>cos</mi> <mi>&alpha;</mi> <mo>-</mo> <mi>sin</mi> <mi>&gamma;</mi> <mi>sin</mi> <mi>&beta;</mi> <mi>sin</mi> <mi>&alpha;</mi> </mrow> </mtd> <mtd> <mrow> <mi>cos</mi> <mi>&gamma;</mi> <mi>sin</mi> <mi>&alpha;</mi> <mo>+</mo> <mi>sin</mi> <mi>&gamma;</mi> <mi>sin</mi> <mi>&beta;</mi> <mi>cos</mi> <mi>&alpha;</mi> </mrow> </mtd> <mtd> <mrow> <mo>-</mo> <mi>sin</mi> <mi>&gamma;</mi> <mi>cos</mi> <mi>&beta;</mi> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <mo>-</mo> <mi>cos</mi> <mi>&beta;</mi> <mi>sin</mi> <mi>&alpha;</mi> </mrow> </mtd> <mtd> <mrow> <mi>cos</mi> <mi>&beta;</mi> <mi>cos</mi> <mi>&alpha;</mi> </mrow> </mtd> <mtd> <mrow> <mi>sin</mi> <mi>&beta;</mi> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <mi>sin</mi> <mi>&gamma;</mi> <mi>cos</mi> <mi>&alpha;</mi> <mo>+</mo> <mi>cos</mi> <mi>&gamma;</mi> <mi>sin</mi> <mi>&beta;</mi> <mi>sin</mi> <mi>&alpha;</mi> </mrow> </mtd> <mtd> <mrow> <mi>sin</mi> <mi>&gamma;</mi> <mi>sin</mi> <mi>&alpha;</mi> <mo>-</mo> <mi>cos</mi> <mi>&gamma;</mi> <mi>sin</mi> <mi>&beta;</mi> <mi>cos</mi> <mi>&alpha;</mi> </mrow> </mtd> <mtd> <mrow> <mi>cos</mi> <mi>&gamma;</mi> <mi>cos</mi> <mi>&beta;</mi> </mrow> </mtd> </mtr> </mtable> </mfenced> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <mo>=</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <msub> <mi>C</mi> <mn>11</mn> </msub> </mtd> <mtd> <msub> <mi>C</mi> <mn>12</mn> </msub> </mtd> <mtd> <msub> <mi>C</mi> <mn>13</mn> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mi>C</mi> <mn>21</mn> </msub> </mtd> <mtd> <msub> <mi>C</mi> <mn>22</mn> </msub> </mtd> <mtd> <msub> <mi>C</mi> <mn>23</mn> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mi>C</mi> <mn>31</mn> </msub> </mtd> <mtd> <msub> <mi>C</mi> <mn>32</mn> </msub> </mtd> <mtd> <msub> <mi>C</mi> <mn>33</mn> </msub> </mtd> </mtr> </mtable> </mfenced> </mrow> </mtd> </mtr> </mtable> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>1</mn> <mo>)</mo> </mrow> </mrow>

<mrow> <mi>B</mi> <mo>=</mo> <mfrac> <mrow> <msub> <mi>&mu;</mi> <mn>0</mn> </msub> <mi>n</mi> <mi>I</mi> <mi>s</mi> </mrow> <mrow> <mn>4</mn> <msup> <mi>&pi;r</mi> <mn>3</mn> </msup> </mrow> </mfrac> <mrow> <mo>(</mo> <mn>2</mn> <mi>c</mi> <mi>o</mi> <mi>s</mi> <mo>(</mo> <mi>&theta;</mi> <mo>)</mo> <msub> <mi>r</mi> <mi>e</mi> </msub> <mo>+</mo> <mi>s</mi> <mi>i</mi> <mi>n</mi> <mo>(</mo> <mi>&theta;</mi> <mo>)</mo> <msub> <mi>&theta;</mi> <mi>e</mi> </msub> <mo>)</mo> </mrow> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>2</mn> <mo>)</mo> </mrow> </mrow>

<mrow> <msub> <mi>M</mi> <mi>v</mi> </msub> <mo>=</mo> <munderover> <mo>&Sigma;</mo> <mrow> <mi>i</mi> <mo>=</mo> <mn>1</mn> </mrow> <mi>N</mi> </munderover> <msub> <mi>B</mi> <mrow> <mi>i</mi> <mi>v</mi> </mrow> </msub> <msub> <mi>C</mi> <mi>i</mi> </msub> <mo>+</mo> <msub> <mi>b</mi> <mi>v</mi> </msub> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>3</mn> <mo>)</mo> </mrow> </mrow>

M_v＝AX_v (4)

A为M×(N+1)的矩阵，X_v为(N+1)×1的矩阵，即

X_{v_est}＝(A^TA)^-1A^TM_{v_est} (6)

D_v＝M_{v_est}-AX_{v_est} (7)

并定义品质因数Q为

<mrow> <msub> <mi>M</mi> <mrow> <mi>v</mi> <mo>_</mo> <mi>e</mi> <mi>m</mi> <mi>i</mi> </mrow> </msub> <mo>=</mo> <munderover> <mo>&Sigma;</mo> <mrow> <mi>i</mi> <mo>=</mo> <mn>1</mn> </mrow> <mi>N</mi> </munderover> <mrow> <mo>(</mo> <msub> <mi>B</mi> <mrow> <mi>i</mi> <mi>v</mi> </mrow> </msub> <msub> <mi>C</mi> <mi>i</mi> </msub> <mo>+</mo> <msubsup> <mi>E</mi> <mrow> <mi>i</mi> <mi>v</mi> </mrow> <mn>1</mn> </msubsup> <msubsup> <mi>&Delta;C</mi> <mi>i</mi> <mn>1</mn> </msubsup> <mo>+</mo> <msubsup> <mi>E</mi> <mrow> <mi>i</mi> <mi>v</mi> </mrow> <mn>2</mn> </msubsup> <msubsup> <mi>&Delta;C</mi> <mi>i</mi> <mn>2</mn> </msubsup> <mo>+</mo> <msubsup> <mi>E</mi> <mrow> <mi>i</mi> <mi>v</mi> </mrow> <mn>3</mn> </msubsup> <msubsup> <mi>&Delta;C</mi> <mi>i</mi> <mn>3</mn> </msubsup> <mo>)</mo> </mrow> <mo>+</mo> <msub> <mi>b</mi> <mi>v</mi> </msub> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>9</mn> <mo>)</mo> </mrow> </mrow>

M_{v_emi}为M×1的矩阵，写成矩阵表达式为

M_{v_emi}＝A_emiX_{v_emi} (10)

A_emi为M×(4N+1)的矩阵，X_{v_emi}为(4N+1)×1的矩阵，即

<mrow> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <mrow> <msub> <mi>m</mi> <mrow> <mi>v</mi> <mo>_</mo> <mi>m</mi> <mi>e</mi> <mi>i</mi> </mrow> </msub> <mrow> <mo>(</mo> <mn>1</mn> <mo>)</mo> </mrow> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mi>m</mi> <mrow> <mi>v</mi> <mo>_</mo> <mi>m</mi> <mi>e</mi> <mi>i</mi> </mrow> </msub> <mrow> <mo>(</mo> <mn>2</mn> <mo>)</mo> </mrow> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mi>m</mi> <mrow> <mi>v</mi> <mo>_</mo> <mi>m</mi> <mi>e</mi> <mi>i</mi> </mrow> </msub> <mrow> <mo>(</mo> <mn>3</mn> <mo>)</mo> </mrow> </mrow> </mtd> </mtr> <mtr> <mtd> <mo>.</mo> </mtd> </mtr> <mtr> <mtd> <mo>.</mo> </mtd> </mtr> <mtr> <mtd> <mo>.</mo> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mi>m</mi> <mrow> <mi>v</mi> <mo>_</mo> <mi>m</mi> <mi>e</mi> <mi>i</mi> </mrow> </msub> <mrow> <mo>(</mo> <mi>M</mi> <mo>)</mo> </mrow> </mrow> </mtd> </mtr> </mtable> </mfenced> <mo>=</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <mrow> <msub> <mi>c</mi> <mn>1</mn> </msub> <mrow> <mo>(</mo> <mn>1</mn> <mo>)</mo> </mrow> </mrow> </mtd> <mtd> <mrow> <msubsup> <mi>&Delta;c</mi> <mn>1</mn> <mn>1</mn> </msubsup> <mrow> <mo>(</mo> <mn>1</mn> <mo>)</mo> </mrow> </mrow> </mtd> <mtd> <mrow> <msubsup> <mi>&Delta;c</mi> <mn>1</mn> <mn>2</mn> </msubsup> <mrow> <mo>(</mo> <mn>1</mn> <mo>)</mo> </mrow> </mrow> </mtd> <mtd> <mrow> <msubsup> <mi>&Delta;c</mi> <mn>1</mn> <mn>3</mn> </msubsup> <mrow> <mo>(</mo> <mn>1</mn> <mo>)</mo> </mrow> </mrow> </mtd> <mtd> <mrow> <msub> <mi>c</mi> <mn>2</mn> </msub> <mrow> <mo>(</mo> <mn>1</mn> <mo>)</mo> </mrow> </mrow> </mtd> <mtd> <mrow> <msubsup> <mi>&Delta;c</mi> <mn>2</mn> <mn>1</mn> </msubsup> <mrow> <mo>(</mo> <mn>1</mn> <mo>)</mo> </mrow> </mrow> </mtd> <mtd> <mn>...</mn> </mtd> <mtd> <mn>1</mn> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mi>c</mi> <mn>1</mn> </msub> <mrow> <mo>(</mo> <mn>2</mn> <mo>)</mo> </mrow> </mrow> </mtd> <mtd> <mrow> <msubsup> <mi>&Delta;c</mi> <mn>1</mn> <mn>1</mn> </msubsup> <mrow> <mo>(</mo> <mn>2</mn> <mo>)</mo> </mrow> </mrow> </mtd> <mtd> <mrow> <msubsup> <mi>&Delta;c</mi> <mn>1</mn> <mn>2</mn> </msubsup> <mrow> <mo>(</mo> <mn>2</mn> <mo>)</mo> </mrow> </mrow> </mtd> <mtd> <mrow> <msubsup> <mi>&Delta;c</mi> <mn>1</mn> <mn>3</mn> </msubsup> <mrow> <mo>(</mo> <mn>2</mn> <mo>)</mo> </mrow> </mrow> </mtd> <mtd> <mrow> <msub> <mi>c</mi> <mn>2</mn> </msub> <mrow> <mo>(</mo> <mn>2</mn> <mo>)</mo> </mrow> </mrow> </mtd> <mtd> <mrow> <msubsup> <mi>&Delta;c</mi> <mn>2</mn> <mn>1</mn> </msubsup> <mrow> <mo>(</mo> <mn>2</mn> <mo>)</mo> </mrow> </mrow> </mtd> <mtd> <mn>...</mn> </mtd> <mtd> <mn>1</mn> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mi>c</mi> <mn>1</mn> </msub> <mrow> <mo>(</mo> <mn>3</mn> <mo>)</mo> </mrow> </mrow> </mtd> <mtd> <mrow> <msubsup> <mi>&Delta;c</mi> <mn>1</mn> <mn>1</mn> </msubsup> <mrow> <mo>(</mo> <mn>3</mn> <mo>)</mo> </mrow> </mrow> </mtd> <mtd> <mrow> <msubsup> <mi>&Delta;c</mi> <mn>1</mn> <mn>2</mn> </msubsup> <mrow> <mo>(</mo> <mn>3</mn> <mo>)</mo> </mrow> </mrow> </mtd> <mtd> <mrow> <msubsup> <mi>&Delta;c</mi> <mn>1</mn> <mn>3</mn> </msubsup> <mrow> <mo>(</mo> <mn>3</mn> <mo>)</mo> </mrow> </mrow> </mtd> <mtd> <mrow> <msub> <mi>c</mi> <mn>2</mn> </msub> <mrow> <mo>(</mo> <mn>3</mn> <mo>)</mo> </mrow> </mrow> </mtd> <mtd> <mrow> <msubsup> <mi>&Delta;c</mi> <mn>2</mn> <mn>1</mn> </msubsup> <mrow> <mo>(</mo> <mn>3</mn> <mo>)</mo> </mrow> </mrow> </mtd> <mtd> <mn>...</mn> </mtd> <mtd> <mn>1</mn> </mtd> </mtr> <mtr> <mtd> <mo>.</mo> </mtd> <mtd> <mo>.</mo> </mtd> <mtd> <mo>.</mo> </mtd> <mtd> <mo>.</mo> </mtd> <mtd> <mo>.</mo> </mtd> <mtd> <mo>.</mo> </mtd> <mtd> <mo>.</mo> </mtd> <mtd> <mo>.</mo> </mtd> </mtr> <mtr> <mtd> <mo>.</mo> </mtd> <mtd> <mo>.</mo> </mtd> <mtd> <mo>.</mo> </mtd> <mtd> <mo>.</mo> </mtd> <mtd> <mo>.</mo> </mtd> <mtd> <mo>.</mo> </mtd> <mtd> <mo>.</mo> </mtd> <mtd> <mo>.</mo> </mtd> </mtr> <mtr> <mtd> <mo>.</mo> </mtd> <mtd> <mo>.</mo> </mtd> <mtd> <mo>.</mo> </mtd> <mtd> <mo>.</mo> </mtd> <mtd> <mo>.</mo> </mtd> <mtd> <mo>.</mo> </mtd> <mtd> <mo>.</mo> </mtd> <mtd> <mo>.</mo> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mi>c</mi> <mn>1</mn> </msub> <mrow> <mo>(</mo> <mi>M</mi> <mo>)</mo> </mrow> </mrow> </mtd> <mtd> <mrow> <msubsup> <mi>&Delta;c</mi> <mn>1</mn> <mn>1</mn> </msubsup> <mrow> <mo>(</mo> <mi>M</mi> <mo>)</mo> </mrow> </mrow> </mtd> <mtd> <mrow> <msubsup> <mi>&Delta;c</mi> <mn>1</mn> <mn>2</mn> </msubsup> <mrow> <mo>(</mo> <mi>M</mi> <mo>)</mo> </mrow> </mrow> </mtd> <mtd> <mrow> <msubsup> <mi>&Delta;c</mi> <mn>1</mn> <mn>3</mn> </msubsup> <mrow> <mo>(</mo> <mi>M</mi> <mo>)</mo> </mrow> </mrow> </mtd> <mtd> <mrow> <msub> <mi>c</mi> <mn>2</mn> </msub> <mrow> <mo>(</mo> <mi>M</mi> <mo>)</mo> </mrow> </mrow> </mtd> <mtd> <mrow> <msubsup> <mi>&Delta;c</mi> <mn>2</mn> <mn>1</mn> </msubsup> <mrow> <mo>(</mo> <mi>M</mi> <mo>)</mo> </mrow> </mrow> </mtd> <mtd> <mn>...</mn> </mtd> <mtd> <mn>1</mn> </mtd> </mtr> </mtable> </mfenced> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <msub> <mi>B</mi> <mrow> <mn>1</mn> <mi>v</mi> </mrow> </msub> </mtd> </mtr> <mtr> <mtd> <msubsup> <mi>E</mi> <mrow> <mn>1</mn> <mi>v</mi> </mrow> <mn>1</mn> </msubsup> </mtd> </mtr> <mtr> <mtd> <msubsup> <mi>E</mi> <mrow> <mn>1</mn> <mi>v</mi> </mrow> <mn>2</mn> </msubsup> </mtd> </mtr> <mtr> <mtd> <msubsup> <mi>E</mi> <mrow> <mn>1</mn> <mi>v</mi> </mrow> <mn>3</mn> </msubsup> </mtd> </mtr> <mtr> <mtd> <msub> <mi>B</mi> <mrow> <mn>2</mn> <mi>v</mi> </mrow> </msub> </mtd> </mtr> <mtr> <mtd> <msubsup> <mi>E</mi> <mrow> <mn>2</mn> <mi>v</mi> </mrow> <mn>1</mn> </msubsup> </mtd> </mtr> <mtr> <mtd> <mo>.</mo> </mtd> </mtr> <mtr> <mtd> <mo>.</mo> </mtd> </mtr> <mtr> <mtd> <mo>.</mo> </mtd> </mtr> <mtr> <mtd> <msub> <mi>b</mi> <mi>v</mi> </msub> </mtd> </mtr> </mtable> </mfenced> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>11</mn> <mo>)</mo> </mrow> </mrow>

式(13)中，k＝μ₀nIs/4π，

式(14)中方向余弦矩阵中的元素满足

在球坐标系下可简化为

<mrow> <mo>|</mo> <msub> <mi>B</mi> <mi>i</mi> </msub> <mo>|</mo> <mo>=</mo> <mi>k</mi> <msqrt> <mrow> <mn>1</mn> <mo>+</mo> <mn>3</mn> <msup> <mi>cos</mi> <mn>2</mn> </msup> <msub> <mi>&theta;</mi> <mi>i</mi> </msub> </mrow> </msqrt> <mo>/</mo> <msubsup> <mi>r</mi> <mi>i</mi> <mn>3</mn> </msubsup> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>17</mn> <mo>)</mo> </mrow> </mrow>

由于故

式(17)构成含有N+3个未知数(x,y,z,θ_i)的N个方程组，式(18)构成含有N个未知数r_i的N个方程组，其中B_isx＝B_{ix_est}或B_isx＝B_{ix_emi}，B_isy＝B_{iy_est}或B_isy＝B_{iy_emi}，B_isz＝B_{iz_est}或B_isz＝B_{iz_emi}，取决于步骤(6)得到的结果，首先利用牛顿迭代法求解式(18)组成的方程组，分别求出r_i的估计值再利用作为初值通过牛顿迭代法求解式(17)组成的方程组，得到N+3个未知数(x,y,z,θ_i)的估计值由此得到P点在空间坐标系中的估计位置；

(10)利用步骤(9)得到的P点位置(x,y,z)通过式(13)重新计算P点处的磁通量定义矩阵对E进行奇异值分解，得到

E＝USV^T (19)

由此得矩阵T的估计矩阵为

<mrow> <mover> <mi>T</mi> <mo>&OverBar;</mo> </mover> <mo>=</mo> <msup> <mi>UV</mi> <mi>T</mi> </msup> <mo>=</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <msub> <mi>T</mi> <mn>11</mn> </msub> </mtd> <mtd> <msub> <mi>T</mi> <mn>12</mn> </msub> </mtd> <mtd> <msub> <mi>T</mi> <mn>13</mn> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mi>T</mi> <mn>21</mn> </msub> </mtd> <mtd> <msub> <mi>T</mi> <mn>22</mn> </msub> </mtd> <mtd> <msub> <mi>T</mi> <mn>23</mn> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mi>T</mi> <mn>31</mn> </msub> </mtd> <mtd> <msub> <mi>T</mi> <mn>32</mn> </msub> </mtd> <mtd> <msub> <mi>T</mi> <mn>33</mn> </msub> </mtd> </mtr> </mtable> </mfenced> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>20</mn> <mo>)</mo> </mrow> </mrow>

由此对T中的方位角α，俯仰角β，横滚角γ进行估算

<mrow> <mtable> <mtr> <mtd> <mrow> <mover> <mi>&alpha;</mi> <mo>&OverBar;</mo> </mover> <mo>=</mo> <mi>arctan</mi> <mrow> <mo>(</mo> <mo>-</mo> <mfrac> <msub> <mi>T</mi> <mn>21</mn> </msub> <msub> <mi>T</mi> <mn>22</mn> </msub> </mfrac> <mo>)</mo> </mrow> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <mover> <mi>&beta;</mi> <mo>&OverBar;</mo> </mover> <mo>=</mo> <mi>arcsin</mi> <mrow> <mo>(</mo> <msub> <mi>T</mi> <mn>23</mn> </msub> <mo>)</mo> </mrow> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <mover> <mi>&gamma;</mi> <mo>&OverBar;</mo> </mover> <mo>=</mo> <mi>arctan</mi> <mrow> <mo>(</mo> <mo>-</mo> <mfrac> <msub> <mi>T</mi> <mn>13</mn> </msub> <msub> <mi>T</mi> <mn>33</mn> </msub> </mfrac> <mo>)</mo> </mrow> </mrow> </mtd> </mtr> </mtable> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>21</mn> <mo>)</mo> </mrow> </mrow>

(11)判别P点附近是否存在磁性材料的干扰，定义残差

<mrow> <msub> <mi>R</mi> <mi>i</mi> </msub> <mo>=</mo> <mover> <mi>T</mi> <mo>&OverBar;</mo> </mover> <msub> <mi>B</mi> <mrow> <mi>i</mi> <mi>s</mi> </mrow> </msub> <mo>-</mo> <msubsup> <mi>B</mi> <mi>i</mi> <mi>c</mi> </msubsup> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>22</mn> <mo>)</mo> </mrow> </mrow>

<mrow> <msub> <mi>F</mi> <mi>i</mi> </msub> <mo>&ap;</mo> <msub> <mi>k</mi> <mi>F</mi> </msub> <mo>|</mo> <msubsup> <mi>B</mi> <mi>i</mi> <mi>c</mi> </msubsup> <mo>|</mo> <mrow> <mo>(</mo> <mn>2</mn> <msub> <mi>cos&theta;</mi> <mrow> <mi>F</mi> <mi>i</mi> </mrow> </msub> <msub> <mi>r</mi> <mi>e</mi> </msub> <mo>+</mo> <msub> <mi>sin&theta;</mi> <mrow> <mi>F</mi> <mi>i</mi> </mrow> </msub> <msub> <mi>&theta;</mi> <mi>e</mi> </msub> <mo>)</mo> </mrow> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>23</mn> <mo>)</mo> </mrow> </mrow>

<mrow> <mo>|</mo> <msub> <mi>F</mi> <mi>i</mi> </msub> <mo>|</mo> <mo>&ap;</mo> <msub> <mi>k</mi> <mi>F</mi> </msub> <mo>|</mo> <msubsup> <mi>B</mi> <mi>i</mi> <mi>c</mi> </msubsup> <mo>|</mo> <mrow> <mo>(</mo> <mn>1</mn> <mo>+</mo> <mn>3</mn> <msup> <mi>cos</mi> <mn>2</mn> </msup> <msub> <mi>&theta;</mi> <mrow> <mi>F</mi> <mi>i</mi> </mrow> </msub> <mo>)</mo> </mrow> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>24</mn> <mo>)</mo> </mrow> </mrow>

<mrow> <msub> <mi>n</mi> <mi>i</mi> </msub> <mo>=</mo> <msubsup> <mi>B</mi> <mi>i</mi> <mi>c</mi> </msubsup> <mo>&times;</mo> <msub> <mi>R</mi> <mi>i</mi> </msub> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>25</mn> <mo>)</mo> </mrow> </mrow>

u_e＝n_Ae×n_Be (26)

cosθ_Fi与sinθ_Fi通过以下公式计算

<mrow> <mtable> <mtr> <mtd> <mrow> <msub> <mi>cos&theta;</mi> <mrow> <mi>F</mi> <mi>i</mi> </mrow> </msub> <mo>=</mo> <msubsup> <mi>B</mi> <mrow> <mi>i</mi> <mi>e</mi> </mrow> <mi>c</mi> </msubsup> <mo>&CenterDot;</mo> <msub> <mi>u</mi> <mi>e</mi> </msub> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mi>sin&theta;</mi> <mrow> <mi>F</mi> <mi>i</mi> </mrow> </msub> <mo>=</mo> <mrow> <mo>|</mo> <mrow> <msubsup> <mi>B</mi> <mrow> <mi>i</mi> <mi>e</mi> </mrow> <mi>c</mi> </msubsup> <mo>&times;</mo> <msub> <mi>u</mi> <mi>e</mi> </msub> </mrow> <mo>|</mo> </mrow> </mrow> </mtd> </mtr> </mtable> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>27</mn> <mo>)</mo> </mrow> </mrow>

通过

<mrow> <mtable> <mtr> <mtd> <mrow> <mo>|</mo> <msub> <mi>F</mi> <mi>A</mi> </msub> <mo>|</mo> <mo>=</mo> <msub> <mi>k</mi> <mrow> <mi>F</mi> <mi>A</mi> </mrow> </msub> <mo>|</mo> <msubsup> <mi>B</mi> <mi>A</mi> <mi>c</mi> </msubsup> <mo>|</mo> <mrow> <mo>(</mo> <mn>1</mn> <mo>+</mo> <mn>3</mn> <msup> <mi>cos</mi> <mn>2</mn> </msup> <msub> <mi>&theta;</mi> <mi>A</mi> </msub> <mo>)</mo> </mrow> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <mo>|</mo> <msub> <mi>F</mi> <mi>B</mi> </msub> <mo>|</mo> <mo>=</mo> <msub> <mi>k</mi> <mrow> <mi>F</mi> <mi>B</mi> </mrow> </msub> <mo>|</mo> <msubsup> <mi>B</mi> <mi>B</mi> <mi>c</mi> </msubsup> <mo>|</mo> <mrow> <mo>(</mo> <mn>1</mn> <mo>+</mo> <mn>3</mn> <msup> <mi>cos</mi> <mn>2</mn> </msup> <msub> <mi>&theta;</mi> <mi>B</mi> </msub> <mo>)</mo> </mrow> </mrow> </mtd> </mtr> </mtable> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>28</mn> <mo>)</mo> </mrow> </mrow>

求解

将带入步骤(8)至步骤(10)重新对位置和姿态进行解算，且解算频率为1Hz，由此解算结束，重新得到P点去干扰后的估计位置与估计姿态

6.根据权利要求4所述的一种用于圆线圈磁场定位的方法，其特征在于步骤(4)中所述的位置和姿态的解算是独立进行的，相互之间没有耦合影响。

7.根据权利要求4所述的一种用于圆线圈磁场定位的方法，其特征在于步骤(4)中所述的位置和姿态的解算包含对电磁感应干扰与磁性材料干扰的优化。