CN103499810B - 一种用于电磁定位的装置和方法 - Google Patents

Abstract

本发明涉及了一种交流电磁定位装置和方法，属于电磁定位装置和方法。数据处理及时序控制单元、D/A转换电路、发射驱动电路、过压保护电路、三轴发射源、接收天线、多路选择电路、滤波放大电路、A/D转换电路。首先由数据处理及时序控制单元控制D/A转换电路产生不同频率的正弦信号，经发射驱动电路进行幅度及功率放大后送至三轴发射源，接收天线接收空间辐射的电磁信号，求解算法求解出目标的位置姿态信息并传给上位机显示。优点在于：简化了硬件处理电路，避免现有时分制工作方式所引起的电路复杂、定位速度慢的问题，保证定位实时性，并解决了定位的奇异点问题。

Description

一种用于电磁定位的装置和方法

技术领域

本发明涉及一种交流电磁定位装置和方法，具体涉及一种采用同时发射方案的电磁定位装置和基于四元数的电磁定位方法。

背景技术

近年来，随着各种数据业务的快速发展，人们对定位及导航的需求逐步增加，尤其在航空飞行员驾驶、医疗手术、运动员模拟训练、大型沉浸式游戏等方面，通常需要确定移动终端或者其持有者的位置姿态信息。目前常见的定位方式主要有超声式、光电式和电磁式定位。

光电式定位通过对目标物体上特定光点的跟踪和监视完成定位和捕捉任务。由于其需要对图像进行分析处理，所以计算量比较大，对处理速度要求较高。

超声式定位利用超声波到特定位置的相位差或时间差来实现，但由于超声波的反射、辐射等原因会造成误差，且更新频率较低，而且要求发射器和接收传感器之间没有阻挡，限制了应用范围。

电磁式定位根据电磁感应原理，利用置于磁场中的传感器所获得的数据，基于接收信号与发射信号的耦合关系计算出目标的六个自由度参数。与上述两种定位技术相比，电磁定位技术具有如下优点：

成本低、体积小、重量轻、便携性较好；

不受视线阻挡的限制、活动范围自由灵活，这是很多其它定位方式无法比拟的。例如对手部跟踪定位，因为手可以摇晃、伸缩运动、有时被手臂或身体其它部分遮挡住。这时，其它的定位方式都无法实现定位，只有电磁定位可以实现。

此外，现有的时分制工作方式需在发射端分时发送驱动信号，发射端电路比同时发送方式复杂，并需保留一个空时间段，供接收端采集处理接收数据之用，增加了系统定位的时间，定位速度较慢。本发明采用同时驱动发射源工作的方式，发射端无需分时处理，简化了硬件电路，接收端同时接收数据无需等待，避免了电路复杂、定位速度慢的问题，保证了定位的实时性。

电磁定位系统因其良好的定位效果及不受视线阻挡限制等优点而成为研究的热点。HerbertR.Jones在美国专利5,307,072中描述了一种电磁定位系统，使用非共心补偿技术减少了线圈非共心引起的位置姿态定位误差，但系统采用正弦信号分时驱动发射线圈工作的方式，定位时间较长。杨杰，邹锦，李跃波，刘峰，潘征在题为“六自由度电磁跟踪定位系统的设计研究”论文中，提出了一种基于检波判相电路采集接收信号的电路设计方案，但接收信号处理考虑的不完善，不能应用于实际电路。殷勤，陈彬，汪莹，杨波等人在题为“六自由度电磁跟踪系统位置参数求解算法的改进”的论文中介绍了一种六自由度位置参数的改进算法，引入特征值和特征向量的概念来求解目标参数，但其算法存在奇异点问题，对精度有较大影响。

发明内容

本发明提供一种用于电磁定位装置和方法，以解决现有时分制工作方式存在的电路复杂、实时性差的问题，并且解决了传统矩阵定位算法中存在的奇异点问题，可实现全角度定位。

本发明采取的技术方案是：一种采用同时发射方案的电磁定位系统，其中接收天线、多路选择电路、滤波放大电路、A/D转换电路、数据处理及时序控制单元、D/A转换电路、发射驱动电路、过压保护电路、三轴发射源顺序连接，其中数据处理及时序控制单元也与上位机相连接；

D/A转换电路，与数据处理及时序控制单元相连接，向处理器DSP中写入查表法程序，控制D/A产生连续的正弦驱动信号；

发射驱动电路，主要由幅度放大、功率放大电路组成，将最初的驱动信号进行放大，以驱动发射源工作；

本发明所述的三轴发射源，由三个相互正交并共心的线圈缠绕在骨架上并分别串联相同阻值电阻、不同容值的电容组成，构成品质因数Q较高、带宽较窄的RLC串联谐振电路，电路可被一窄频段内的频率驱动，在三个不同频率正弦信号的同时驱动下工作，每个串联电路在其谐振频率上产生谐振，以此向周围空间辐射稳定且强度较强的电磁波，并且将驱动信号在电阻R上的分压作为参考信号送至接收端；

过压保护电路，防止过压损坏内部芯片并起到静电保护作用；

接收天线，与三轴发射源组成结构类似，由三个相互正交并共心的线圈组成，但与发射源体积大小不同，三轴接收天线不串联电容，通过电磁感应接收三轴发射源辐射的电磁波；

多路选择电路，由多路选择开关构成，负责将接收天线接收的信号及发射端送来的参考信号进行分时选择；

滤波放大电路，由仪表放大器和自动增益控制电路组成，将分时选择后的信号依次进行滤波及幅度放大；

A/D转换电路，将滤波放大后的模拟信号转换为供后续处理的数字量；

数据处理及时序控制单元，主要由核心处理器DSP及SDRAM存储器、FLASH存储器、CPLD等部分组成，一方面与D/A转换电路相连接，控制其产生最初的驱动信号，另一方面与接收处理电路相连接，为多路选择电路提供时序控制信号，对处理后的信号进行采样。

本发明所述的数据处理及时序控制单元工作时，其中的核心处理器DSP和触发器相连，产生方波，可为多路选择电路、滤波放大电路提供控制信号。再控制A/D转换电路将接收模拟信号转换为数字量，用傅里叶变换进行处理，当接收信号包含某一频率的发射信号时，傅里叶变换在此频率点对应较大的幅值，依据三轴发射源每轴发射信号的频率不同提取各信号，拟合成包含9组数据的接收矩阵，再用四元数同时求解算法求解位置姿态参数。

本发明的一种用于电磁定位的方法，包括下列步骤：

(1)由数据处理及时序控制单元控制D/A转换电路产生三路频率不同的正弦驱动信号，三路驱动信号经幅度、功率放大后送至三轴发射源，三轴发射源在定位时安置在固定位置，设其所在位置为坐标原点，用球坐标表示为(0,0,0)，三轴发射源工作时，将经发射驱动电路放大后的连续正弦驱动信号同时加载到三轴发射源，三轴线圈在三个不同频率的正弦信号驱动下工作，驱动信号频率f满足25KHZ≤f≤40KHZ，每个串联谐振电路在其谐振频率上产生谐振，以此向周围空间辐射稳定且强度较强的电磁波；

(2)接受天线通过电磁感应接收空间中的电磁信号，将接收天线接收的信号及发射端送来的参考信号进行分时选择，然后依次对信号进行滤波及幅度放大，再通过A/D转换电路转换为数字量；

(3)将数字量送至数据处理及时序控制单元，利用傅里叶变换依据每轴发射信号频率不同提取各信号，拟合成包含9组数据的接收矩阵Y，最后用四元数同时求解算法计算位置与姿态参数，通过数据接口将求解的位置姿态信息传给上位机显示出来；

所述将数字量送至数据处理及时序控制单元后，需将其做一定处理来提取出三轴发射三轴接收对应的数据，采用数字信号处理中常用的处理方式，首先利用窗函数将连续的采样量划分成小的集合，每个集合代表采样数据的一小段，根据窗内的采样时间为每一个采样集合设定一个权函数，用这个权函数作为系数乘以采样的数字量，其中窗函数采用布莱克曼-哈里斯(Blackman-Harris)窗，加窗函数之后再做离散傅里叶变换，在变换后的频域会有一个较尖锐的响应，当时域采样数据包含某一轴发射信号的频率时，离散傅里叶变换后对应的频率点会有一个相对较大的幅值，由此可以提取出对应的信号。

所述四元数同时求解算法，具体实现步骤如下：

(1)用矩阵表示位置参数及姿态参数；

接收天线相对于三轴发射源(0,0,0)的位置可由距离R、水平角α、仰角β描述，用球坐标表示为(R，α，β)；接收天线姿态可由方位角ω，俯仰角ξ，横滚角描述，将位置参数中的水平角及仰角用位置矩阵P表示，姿态角用姿态矩阵A表示，P、A均为正交矩阵；

$P=\left[\begin{array}{ccc}\cos \mathrm{\α}\cos \mathrm{\β}& \sin \mathrm{\α}\cos \mathrm{\β}& -\sin \mathrm{\β}\\ -\sin \mathrm{\α}& \cos \mathrm{\α}& 0\\ \cos \mathrm{\α}\sin \mathrm{\β}& \sin \mathrm{\α}\sin \mathrm{\β}& \cos \mathrm{\β}\end{array}\right]$

(2)根据位置矩阵及姿态矩阵的旋转变换关系构造矩阵一Q，Q=AP^T，其中T为矩阵的转置运算，结合耦合矩阵H、电磁场耦合系数k表示接收矩阵Y；

Y=kAP^THP=kQHP，H=dg(2,-1,-1)，其中dg表示对角矩阵；

(3)对接收矩阵Y处理得矩阵二U，通过矩阵运算确定电磁场耦合系数k，

U=Y^TY=k²P^TH²P，由矩阵迹的特性得tr表示矩阵的迹；

(4)结合电磁场耦合系数k值，利用信号与距离成反比的关系计算距离R：

设定k₀为k的参考值，R₀为R的参考值，k₀=R₀=1，为已知值，得 $R=R_0{(k_0/k)}^{\left(\frac{1}{3}\right)};$

(5)Y除以电磁场的耦合系数k得归一化信号矩阵M，

M=AP^THP；

(6)用四元数对矩阵进行处理；

位置四元数p、姿态四元数q四元数由一个实数单位1和三个虚数单位i，j，l组成，是包含四个实元的超复数，形式如下：p=p₀+p₁·i+p₂·j+p₃·l，q=q₀+q₁·i+q₂·j+q₃·l，其中p₀、q₀为四元数的标量部分，p₁,p₂,p₃，q₁,q₂,q₃为矢量部分的三个系数，可视为一点的坐标，i，j，l与坐标轴的方向相同，用位置四元数p、姿态四元数q表示接收矩阵：

Y=kQHP=kq^-1[H(p^-1Ip)]q，其中-1为求逆运算，I为单位矩阵；

(7)同样用四元数表示归一化信号矩阵M；

M=q^-1[H(p^-1Ip)]q，I为单位矩阵；

(8)根据算法要求，设定位置误差四元数c和姿态误差四元数e及姿态四元数、位置四元数估算初值q和p来表示姿态四元数q和位置四元数p；

c=c₀+c₁·i+c₂·j+c₃·l，e=e₀+e₁·j+e₂·k+e₃·l，q=eq，p=pc

(9)用设定的姿态四元数估算初值q处理归一化信号矩阵M得矩阵三M₁，矩阵四M₂；

M₁=qMq ^-1=e^-1HPe=(I+2E)HP,E为姿态误差四元数矩阵，

$E=\left[\begin{array}{ccc}0& e_3& -e_2\\ -e_3& 0& e_1\\ e_2& -e_1& 0\end{array}\right]$ 用耦合矩阵H处理矩阵M₁得矩阵M₂，

M₂=H^-1(M₁)=(I+2Z)P，Z由矩阵E变换得到，Z=H^-1EH，对M₂做转置运算得到矩阵五M₃，M₃=(M₂)^T=P^T(I-2Z)=p(I-2Z)p^-1；

(10)用设定的位置四元数估算初值p处理矩阵M₃得到矩阵六M₄，矩阵七M₅；

M₄=p ^-1(M₃)p=c(I-2Z)c^-1=(I-2C)(I-2Z)，C为位置误差四元数矩阵，

$C=\left[\begin{array}{ccc}0& c_3& -c_2\\ -c_3& 0& c_1\\ c_2& -c_1& 0\end{array}\right]$

M₅=(M₄)^T=I+2Z+2C；

(11)将矩阵M₅展开，计算得位置误差四元数c和姿态误差四元数e；

$M_5=\left[\begin{array}{ccc}1& {2c}_3-e_3& {-2c}_2+e_2\\ {-2c}_3+{4e}_3& 1& {2c}_1+{2e}_1\\ {2c}_2-{4e}_2& {-2c}_1-{2e}_1& 1\end{array}\right]$

c₀=1，c₁=0，c₂=-(v₃₁+4v₁₃)/6，c₃=(v₂₁+4v₁₂)/6

e₀=1，e₁=(v₂₃-4v₃₂)/4，e₂=-(v₁₃+v₃₁)/3，e₃=(v₁₂+v₂₁)/3，其中v₃₁，v₁₃，v₂₁，v₁₂，v₂₃，v₃₂分别为M₅中对应行对应列的元素，例如v₃₁为M₅第三行第一列的元素；

(12)计算得新的位置四元数、姿态四元数估算初值p _new，q _new为

(p _new)=pc(q _new)=eq

(13)重复步骤(1)～(12)，用迭代的方法不断计算出新的四元数估算初值p _new，q _new，直至误差四元数小于设定值0.0001，迭代停止，得最终位置四元数p_final，姿态四元数q_final，然后计算得表示姿态角的四元数a，且a=p_finalq_final；

(14)最后由四元数转换欧拉角的公式得各角度；

由此四元数定位算法结束，结合步骤(4)计算的距离R，可得接收天线相对于发射源(0,0,0)的位置坐标(R，α，β)和姿态角ω，ξ，的求解；

本发明具有以下优点：

（1）现有的时分制工作方式需在发射端分时发送驱动信号，发射端电路比同时发送方式复杂，并需保留一个空时间段，供接收端采集处理接收数据之用，增加了系统定位的时间，定位速度较慢。本发明采用同时驱动发射源工作的方式，发射端无需分时处理，简化了硬件电路，接收端同时接收数据无需等待，避免了电路复杂、定位速度慢的问题，保证了定位的实时性。

（2）三轴发射源由三个线圈串联电阻、电容构成品质因数Q较高、带宽较窄的RLC串联谐振电路，三轴线圈在三个不同频率的正弦信号同时驱动下工作，每个串联谐振电路在其谐振频率上产生谐振，以此向周围空间辐射稳定且强度较强的电磁波，可以保证定位范围，并且由于采用连续的正弦信号驱动发射源，根据电磁感应原理可以减少电磁干扰。

（3）信号数据处理及时序控制单元采用高精度浮点DSP作为核心处理器，利用窗函数和傅里叶变换处理A/D转换后的数字量，窗函数和傅里叶变换是数字信号处理常用的有效处理技术，处理器采用高精度浮点运算，可准确有效地提取出对应频率的信号，进而利用定位算法处理信号求解出位置姿态信息，保证定位信息的准确性。

（4）本发明采用四元数同时求解算法进行位置姿态参数的求解，位置和姿态可以同时得到，其中采用向量与四元数的点乘与叉乘运算，可避免传统矩阵算法中复杂的矩阵相乘运算及定位时存在的奇异点问题，此外用四元数代替矩阵来表示位置与姿态参数，减少了所占的系统内存。

附图说明

图1是本发明的总体结构框图；

图2是本发明三轴发射源的组成结构图；

图3是本发明接收天线结构示意图；

图4是本发明多路选择电路原理图；

图5是本发明滤波放大电路原理图；

图6A是本发明数据处理及时序控制单元结构及工作原理图；

图6B是本发明接收信号处理过程框图；

图7是本发明电磁定位中方位坐标示意图；

图8是本发明四元数同时求解算法流程图；

图9是本发明四元数同时求解算法收敛性仿真结果；

图10A是本发明四元数同时求解算法标定的坐标及姿态角示意图；

图10B是本发明四元数同时求解算法坐标及姿态角仿真结果图；

具体实施方式

接收天线、多路选择电路、滤波放大电路、A/D转换电路、数据处理及时序控制单元、D/A转换电路、发射驱动电路、过压保护电路、三轴发射源顺序连接，其中数据处理及时序控制单元也与上位机相连接；

D/A转换电路，与数据处理及时序控制单元相连接，向处理器DSP中写入查表法程序，控制D/A产生连续的正弦驱动信号；

发射驱动电路，主要由幅度放大、功率放大电路组成，将最初的驱动信号进行放大，以驱动发射源工作；

本发明所述的三轴发射源，由三个相互正交并共心的线圈缠绕在骨架上并分别串联相同阻值电阻、不同容值的电容组成，构成品质因数Q较高、带宽较窄的RLC串联谐振电路，电路可被一窄频段内的频率驱动，在三个不同频率正弦信号的同时驱动下工作，每个串联电路在其谐振频率上产生谐振，以此向周围空间辐射稳定且强度较强的电磁波，并且将驱动信号在电阻R上的分压作为参考信号送至接收端；

过压保护电路，防止过压损坏内部芯片并起到静电保护作用；

接收天线，与三轴发射源组成结构类似，由三个相互正交并共心的线圈组成，但与发射源体积大小不同，三轴接收天线不串联电容，通过电磁感应接收三轴发射源辐射的电磁波；

多路选择电路，由多路选择开关构成，负责将接收天线接收的信号及发射端送来的参考信号进行分时选择；

滤波放大电路，由仪表放大器和自动增益控制电路组成，将分时选择后的信号依次进行滤波及幅度放大；

A/D转换电路，将滤波放大后的模拟信号转换为供后续处理的数字量；

数据处理及时序控制单元，主要由核心处理器DSP及SDRAM存储器、FLASH存储器、CPLD等部分组成，一方面与D/A转换电路相连接，控制其产生最初的驱动信号，另一方面与接收处理电路相连接，为多路选择电路提供时序控制信号，对处理后的信号进行采样。

本发明所述的数据处理及时序控制单元工作时，其中的核心处理器DSP和触发器相连，产生方波，可为多路选择电路、滤波放大电路提供控制信号。再控制A/D转换电路将接收模拟信号转换为数字量，用傅里叶变换进行处理，当接收信号包含某一频率的发射信号时，傅里叶变换在此频率点对应较大的幅值，依据三轴发射源每轴发射信号的频率不同提取各信号，拟合成包含9组数据的接收矩阵，再用四元数同时求解算法求解位置姿态参数。

下面结合附图对本发明作进一步描述：

图1是本发明一种交流电磁定位装置的总体结构框图。其中接收天线106、多路选择电路107、滤波放大电路108、A/D转换电路109、数据处理及时序控制单元101、D/A转换电路102、发射驱动电路103、过压保护电路104、三轴发射源105顺序连接，其中数据处理及时序控制单元也与上位机相连接。

系统的定位过程是：D/A转换电路102与数据处理及时序控制单元101相连接，向核心处理器DSP中写入查表法程序，控制D/A转换电路102产生三路频率不同的连续的正弦驱动信号。将驱动信号送至发射驱动电路103，将最初的驱动信号进行放大，以满足驱动发射源工作的要求。再将放大后的驱动信号通过过压保护电路104，以防止电压过大对电路及元器件造成损伤，最后将处理后的驱动信号加载到三轴发射源105上。

三轴发射源105向周围空间辐射电磁波，并将驱动信号在电阻R上的分压作为参考信号送至接收端，接收天线106通过电磁感应接收空间中的电磁信号，将接收信号及发射端送来的参考信号进行分时选择，然后依次对信号进行滤波放大，再通过A/D转换电路109转换为数字量。将数字量送至数据处理及时序控制单元101，利用傅里叶变换依据每轴发射信号频率不同提取各信号，拟合成包含9组数据的接收矩阵Y，最后用四元数同时求解算法计算位置与姿态参数，通过数据接口将求解的位置姿态信息传给上位机显示出来。

图2示出的是发明中所述的三轴发射源组成结构，骨架为一个直径为10cm的带有绕线槽的球体。三轴发射源105由三个相互正交并共心的线圈10501缠绕在骨架10502上并分别串联相同阻值电阻R10503、不同容值的电容C10504组成，构成品质因数Q较高、带宽较窄的RLC串联谐振电路。电路可被一窄频段内的频率驱动，驱动信号频率f满足25KHZ≤f≤40KHZ。三轴发射源在三个不同频率正弦信号的驱动下工作，每个串联电路在其谐振频率上产生谐振，以此向周围空间辐射稳定且强度较强的电磁波，并且将驱动信号在电阻R上的分压作为参考信号送至接收端。由于采用连续的正弦信号进行驱动发射源，根据电磁感应原理可以减少电磁干扰。其中的串联谐振电路的谐振频率由串联的电容C及线圈的电感值决定，谐振频率为：

$f=\frac{1}{2\mathrm{\π}\sqrt{\mathrm{LC}}}$

图3示出的是发明中所述的接收天线106的组成结构，接收天线骨架10601模型为边长1cm的正方体，接收天线组成结构与三轴发射源类似，也由三个相互正交并共心的线圈10602组成，但与三轴发射源体积大小不同，三轴接收天线不串联电容。接收天线通过电磁感应接收发射源辐射的电磁波，且接收信号的强弱程度与接收天线相距三轴发射源的距离有关。

图4示出多路选择电路107原理图。其中多路选择开关为MAX4582，将接收天线每轴线圈的两端对应与MAX4582的输入信号引脚相连，并且将发射端送来的参考信号也送至MAX4582相应的输入引脚。核心处理器DSP结合触发器产生时序控制信号，将此控制信号送入MAX4582控制引脚，分时选择四路输入信号输出。

图5示出滤波放大电路108原理图。将分时选择后的接收信号依次通过滤波放大电路108。滤波放大电路主要由仪表放大器301和自动增益控制电路302组成，同样增益控制信号由核心处理器DSP结合触发器产生。如图5所示，该仪表放大器的增益可以通过改变引脚3与引脚12之间电阻R_G的阻值进行调节。其增益计算公式为：

$G=1+\frac{600}{R_G}$

数据处理及时序控制单元101通过改变加载到自动增益芯片IN1、IN2引脚的控制信号即可改变电阻R_G的阻值，从而控制其增益，以适应当接收天线106距离三轴发射源105较远和较近时均能准确的对信号进行滤波放大，保证系统的稳定性。

其中图6A示出数据处理及时序控制单元101的结构及工作原理，主要由核心处理器DSP及SDRAM存储器201、FLASH存储器202、电源203等部分组成。一方面与D/A转换电路102相连接，控制其产生最初的驱动信号，另一方面与接收处理电路相连接，为多路选择电路107、滤波放大电路108、A/D转换电路109提供时序控制信号。并处理A/D转换后的数字量，利用傅里叶变换提取各信号，拟合成包含9组数据的接收矩阵Y，用四元数算法计算位置与姿态参数，通过数据接口将求解的位置姿态信息传给上位机显示出来。

图6B示出接收信号具体的处理流程。首先信号经分时选择及滤波放大，再经A/D转换电路109将接收模拟信号转换为数字量。然后利用窗函数将连续的采样数字量划分成小的集合，每个集合代表采样数据的一小段，根据窗内的采样时间为每一个采样集合设定一个权函数，用这个权函数作为系数乘以采样的数字量，这是数字信号处理中常用的处理方式。加窗函数之后再做离散傅里叶变换，变换的频域会有一个较尖锐的响应，其中窗函数采用著名的布莱克曼-哈里斯(Blackman-Harris)窗。当时域采样数据包含某一轴发射信号的频率时，离散傅里叶变换后对应的频率点会有一个相对较大的幅值，依据发射源每轴发射信号的频率不同提取各信号，拟合成包含9组数据的接收矩阵。最后在处理器DSP中通过定位算法求解出位置姿态信息，通过数据接口将求解的位置姿态信息传给上位机显示出来；

其中所述的上位机，主要是PC机或移动终端设备，通过串口或者USB口将电磁定位装置与上位机相连，利用OpenGL、OpenInventor、Cosmo3D、Optimizer等多种高级图形库可以将所求出的坐标在终端上显示出来，并将其运动情况以三维立体图形的形式描述，创建交互式3D图形界面。

本发明的一种用于电磁定位的方法，具体包括下列步骤：

(1)由数据处理及时序控制单元控制D/A转换电路产生三路频率不同的正弦驱动信号，三路驱动信号经幅度、功率放大后送至三轴发射源。

图7示出电磁定位中接收天线与三轴发射源的方位坐标关系。三轴发射源在定位时安置在固定位置，设其所在位置为坐标原点，用球坐标表示为(0,0,0)。三轴发射源工作时，将经发射驱动电路放大后的连续正弦驱动信号同时加载到三轴发射源，三轴线圈在三个不同频率的正弦信号驱动下工作，驱动信号频率频率f满足25KHZ≤f≤40KHZ。每个串联谐振电路在其谐振频率上产生谐振，以此向周围空间辐射稳定且强度较强的电磁波；

(2)接受天线通过电磁感应接收空间中的电磁信号，将接收天线接收的信号及发射端送来的参考信号进行分时选择，然后依次对信号进行滤波放大，再通过A/D转换电路转换为数字量；

(3)将数字量送至数据处理及时序控制单元，需将其做一定处理来提取出三轴发射三轴接收对应的数据。首先利用窗函数将连续的采样量划分成小的集合，每个集合代表采样数据的一小段，根据窗内的采样时间为每一个采样集合设定一个权函数，用这个系数乘以采样的数字量，这是数字信号处理中常用的处理方式。加窗函数之后再做离散傅里叶变换，变换的频域会有一个较尖锐的响应，其中窗函数采用著名的布莱克曼-哈里斯(Blackman-Harris)窗。当时域采样数据包含某一轴发射信号的频率时，离散傅里叶变换后对应的频率点会有一个相对较大的幅值，由此可以提取出对应的信号，依据每轴发射信号频率不同提取各信号，拟合成包含9组数据的接收矩阵Y，最后用四元数同时求解算法计算位置与姿态参数，通过数据接口将求解的位置姿态信息传给上位机显示出来。

图8示出四元数同时求解算法的整体流程图。四元数同时求解算法的具体实现步骤如下：

(1)用矩阵表示位置参数及姿态参数；

接收天线相对于三轴发射源(0,0,0)的位置可由距离R、水平角α、仰角β描述，用球坐标表示为(R，α，β)；接收天线姿态可由方位角ω，俯仰角ξ，横滚角描述，将位置参数中的水平角及仰角用位置矩阵P表示，姿态角用姿态矩阵A表示，P、A均为正交矩阵。

$P=\left[\begin{array}{ccc}\cos \mathrm{\α}\cos \mathrm{\β}& \sin \mathrm{\α}\cos \mathrm{\β}& -\sin \mathrm{\β}\\ -\sin \mathrm{\α}& \cos \mathrm{\α}& 0\\ \cos \mathrm{\α}\sin \mathrm{\β}& \sin \mathrm{\α}\sin \mathrm{\β}& \cos \mathrm{\β}\end{array}\right]$

(2)根据位置矩阵及姿态矩阵的旋转变换关系构造矩阵一Q，Q=AP^T，其中T为矩阵的转置运算，结合耦合矩阵H、电磁场耦合系数k表示接收矩阵Y；

Y=kAP^THP=kQHP，H=dg(2,-1,-1)，其中dg表示对角矩阵。

(3)对接收矩阵Y处理得矩阵二U，通过矩阵运算确定电磁场耦合系数k，

U=Y^TY=k²P^TH²P，由矩阵迹的特性得tr表示矩阵的迹。

(4)结合电磁场耦合系数k值，利用信号与距离成反比的关系计算距离R：

设定k₀为k的参考值，R₀为R的参考值，k₀=R₀=1，为已知值，得 $R=R_0{(k_0/k)}^{\left(\frac{1}{3}\right)};$

(5)Y除以电磁场的耦合系数k得归一化信号矩阵M，

M=AP^THP。

(6)用四元数对矩阵进行处理；

位置四元数p、姿态四元数q四元数由一个实数单位1和三个虚数单位i，j，l组成，是包含四个实元的超复数，形式如下：p=p₀+p₁·i+p₂·j+p₃·l，q=q₀+q₁·i+q₂·j+q₃·l，其中p₀、q₀为四元数的标量部分，p₁,p₂,p₃，q₁,q₂,q₃为矢量部分的三个系数，可视为一点的坐标，i，j，l与坐标轴的方向相同，用位置四元数p、姿态四元数q表示接收矩阵：

Y=kQHP=kq^-1[H(p^-1Ip)]q，其中-1为求逆运算，I为单位矩阵。

(7)同样用四元数表示归一化信号矩阵M；

M=q^-1[H(p^-1Ip)]q，I为单位矩阵。

(8)根据算法要求，设定位置误差四元数c和姿态误差四元数e及姿态四元数、位置四元数估算初值q和p来表示四元数姿态四元数q和位置四元数p；

c=c₀+c₁·i+c₂·j+c₃·l，e=e₀+e₁·j+e₂·k+e₃·l，q=eq，p=pc

(9)用设定的姿态四元数估算初值q处理归一化信号矩阵M得矩阵三M₁，矩阵四M₂；

M₁=qMq ^-1=e^-1HPe=(I+2E)HP,E为姿态误差四元数矩阵，

$E=\left[\begin{array}{ccc}0& e_3& -e_2\\ -e_3& 0& e_1\\ e_2& -e_1& 0\end{array}\right]$ 用耦合矩阵H处理矩阵M₁得矩阵M₂，

M₂=H^-1(M₁)=(I+2Z)P，Z由矩阵E变换得到，Z=H^-1EH，对M₂做转置运算得到矩阵五M₃，M₃=(M₂)^T=P^T(I-2Z)=p(I-2Z)p^-1。

(10)用设定的位置四元数估算初值p处理矩阵M₃得到矩阵六M₄，矩阵七M₅；

M₄=p ^-1(M₃)p=c(I-2Z)c^-1=(I-2C)(I-2Z)，C为位置误差四元数矩阵，

$C=\left[\begin{array}{ccc}0& c_3& -c_2\\ -c_3& 0& c_1\\ c_2& -c_1& 0\end{array}\right]$

M₅=(M₄)^T=I+2Z+2C。

(11)将矩阵M₅展开，计算得位置误差四元数c和姿态误差四元数e；

$M_5=\left[\begin{array}{ccc}1& {2c}_3-e_3& {-2c}_2+e_2\\ {-2c}_3+{4e}_3& 1& {2c}_1+{2e}_1\\ {2c}_2-{4e}_2& {-2c}_1-{2e}_1& 1\end{array}\right]$

c₀=1，c₁=0，c₂=-(v₃₁+4v₁₃)/6，c₃=(v₂₁+4v₁₂)/6

e₀=1，e₁=(v₂₃-4v₃₂)/4，e₂=-(v₁₃+v₃₁)/3，e₃=(v₁₂+v₂₁)/3，其中v₃₁，v₁₃，v₂₁，v₁₂，v₂₃，v₃₂分别为M₅中对应行对应列的元素，例如v₃₁为M₅第三行第一列的元素。

(12)计算得新的位置四元数、姿态四元数估算初值p _new，q _new为

(p _new)=pc(q _new)=eq

(13)重复步骤(1)～(12)，用迭代的方法不断计算出新的四元数估算初值p _new，q _new，直至误差四元数小于设定值0.0001，迭代停止，得最终位置四元数p_final，姿态四元数q_final，然后计算得表示姿态角的四元数a，且a=p_finalq_final；

(14)最后由四元数转换欧拉角的公式得各角度；

由此四元数定位算法结束，结合步骤(4)计算的距离R，可得接收天线相对于发射源(0,0,0)的位置坐标(R，α，β)和姿态角ω，ξ，的求解。

下面从仿真实验结果说明本发明中四元数同时求解算法的准确性与可行性。

首先从四元数同时求解算法误差四元数的收敛性来进行说明：

在本发明的具体实施中，实验一设定实验条件为：位置四元数及姿态四元数估算初值分别为p=-0.2+0.2·i+0.4·j+0.4·k，q=0.3+0.3·i+0.4·j+0.35·k，误差精度设为10^-4。图9示出位置误差四元数c、姿态误差四元数e矢量部分随迭代次数的收敛情况。由仿真图可以看出，位置误差四元数矢量部分与姿态误差四元数e矢量部分的收敛情况是同步的，最后都趋近于0。

其次从对目标在空间中位置的定位及姿态的确定来说明：

在本发明的具体实施中，实验二设定的实验条件为：设定八组接收矩阵，并且标定好目标在空间中的位置和姿态角信息，作为仿真结果的参考值，如图10A所示。而图10B示出四元数同时求解算法运算后得到的八组位置坐标和姿态角信息。其中姿态角是以弧度制表示的。由仿真图可以看出，所求姿态角在-0.80(-45°),-1.57(-90°)处定位结果较准确，不存在传统算法中姿态角在-45°，-90°附近定位结果失真很大的奇异点问题。

最后从三种典型的电磁定位算法的运算量方面做了对比，设定的实验条件为：在同一台计算机上运行这三种定位算法，保证算法运行的环境相同。运算时间对比的具体结果如表1所示，可以看出本发明所采用的四元数同时求解算法计算速度较快。

表1三种算法的运行时间对比

Claims (3)

1.一种交流电磁定位装置，接收天线、分时选择电路、滤波放大电路、A/D转换电路、数据处理及时序控制单元、D/A转换电路、发射驱动电路、过压保护电路、三轴发射源顺序连接，其中数据处理及时序控制单元用于与上位机相连接；

(1)D/A转换电路，与数据处理及时序控制单元相连接，向处理器DSP中写入查表法程序，控制D/A产生连续的正弦驱动信号；

(2)发射驱动电路，主要由幅度放大、功率放大电路组成，将最初的驱动信号进行放大，以驱动发射源工作；

(3)过压保护电路，防止过压损坏内部芯片并起到静电保护作用；

(4)三轴发射源，由发射驱动电路驱动，在三个不同频率正弦信号的驱动下工作，向周围空间辐射电磁波，并且将驱动信号在电阻R上的分压作为参考信号送至接收处理单元；

所述三轴发射源由三个相互正交并共心的线圈缠绕在骨架上并分别串联相同阻值电阻R、不同容值的电容C组成，构成品质因数Q较高、带宽较窄的RLC串联谐振电路，该谐振电路可被一窄频段内的频率驱动，每个串联电路在其谐振频率上产生谐振，以此向周围空间辐射稳定且强度较强的电磁波；

(5)接收天线，由三个相互正交并共心的线圈组成，但与三轴发射源体积大小不同，三轴接收天线不串联电容，通过电磁感应接收发射源辐射的电磁波；

(6)多路选择电路，由多路选择开关构成，负责将接收天线接收的信号及发射端送来的参考信号进行分时选择；

(7)滤波放大电路，由仪表放大器和自动增益控制电路组成，将分时选择后的信号依次进行滤波及幅度放大；

(8)A/D转换电路，将滤波放大后的模拟信号转换为供后续处理的数字量；

(9)数据处理及时序控制单元，主要由核心处理器DSP及SDRAM存储器、FLASH存储器、电源部分组成，一方面与D/A转换电路相连接，控制其产生最初的驱动信号，另一方面与接收处理电路相连接，为多路选择电路提供时序控制信号，对处理后的信号进行采样，再通过四元数同时求解算法求解出位置姿态参数，最后通过数据接口将求解的位置姿态信息传给上位机显示出来；

其特征在于：所述的数据处理及时序控制单元的核心处理器DSP和触发器相连，产生方波，可为接收处理电路的多路选择、滤波放大提供控制信号，再控制A/D转换电路将接收模拟信号转换为数字量，用傅里叶变换进行处理，当接收信号包含某一频率的发射信号时，傅里叶变换在此频率点对应较大的幅值，依据三轴发射源每轴发射信号的频率不同提取各信号，拟合成包含9组数据的接收矩阵，再用四元数同时求解算法求解位置姿态参数。

2.一种用于电磁定位的方法，其特征在于包括下列步骤：

(1)由数据处理及时序控制单元控制D/A转换电路产生三路频率不同的正弦驱动信号，三路驱动信号经幅度、功率放大后送至三轴发射源，同时驱动三轴线圈工作，向空中辐射电磁波，并且将驱动信号在电阻R上的分压作为参考信号送至接收处理单元；

所述三轴发射源在定位时被安置在固定位置，设其所在位置为坐标原点，用球坐标表示为(0,0,0)，数据处理及时序控制单元控制D/A转换电路产生三路频率不同的正弦驱动信号，然后三路驱动信号经幅度、功率放大，三轴发射源工作时，将经发射驱动电路放大后的连续正弦驱动信号同时加载到三轴发射源，三轴线圈在三个不同频率的正弦信号驱动下工作，驱动信号频率f满足25KHZ≤f≤40KHZ，每个串联谐振电路在其谐振频率上产生谐振，以此向周围空间辐射稳定且强度较强的电磁波；

(2)接收天线通过电磁感应接收空间中的电磁信号，将接收天线接收的信号及发射端送来的参考信号进行分时选择，然后依次对信号进行滤波放大，再通过A/D转换电路将信号转换为数字量；

(3)将数字量送至数据处理及时序控制单元，利用傅里叶变换依据每轴发射信号频率不同提取各信号，拟合成包含9组数据的接收矩阵Y，最后用四元数同时求解算法计算位置与姿态参数，通过数据接口将求解的位置姿态信息传给上位机显示出来，所述的四元数同时求解算法，具体实现步骤如下：

(a)用矩阵表示位置参数及姿态参数；

接收天线相对于三轴发射源(0,0,0)的位置可由距离R、水平角α、仰角β描述，用球坐标表示为(R，α，β)；接收天线姿态可由方位角ω，俯仰角ξ，横滚角描述，将位置参数中的水平角及仰角用位置矩阵P表示，姿态角用姿态矩阵A表示，P、A均为正交矩阵；

$P=\left[\begin{array}{ccc}cos\mathrm{\α}cos\mathrm{\β}& sin\mathrm{\α}cos\mathrm{\β}& -sin\mathrm{\β}\\ -sin\mathrm{\α}& cos\mathrm{\α}& 0\\ cos\mathrm{\α}sin\mathrm{\β}& sin\mathrm{\α}sin\mathrm{\β}& \cos \mathrm{\β}\end{array}\right]$

(b)根据位置矩阵及姿态矩阵的旋转变换关系构造矩阵一Q，Q＝AP^T，其中T为矩阵的转置运算，结合耦合矩阵H、电磁场耦合系数k表示接收矩阵Y；

Y＝kAP^THP＝kQHP，H＝dg(2,-1,-1)，其中dg表示对角矩阵；

(c)对接收矩阵Y处理得矩阵二U，通过矩阵运算确定电磁场耦合系数k，

U＝Y^TY＝k²P^TH²P，由矩阵迹的特性得tr表示矩阵的迹；

(d)结合电磁场耦合系数k值，利用信号与距离成反比的关系计算距离R：

设定k₀为k的参考值，R₀为R的参考值，k₀＝R₀＝1，为已知值，得 $R=R_0{(k_0/k)}^{\left(\frac{1}{3}\right)};$

(e)Y除以电磁场的耦合系数k得归一化信号矩阵M，

M＝AP^THP；

(f)用四元数对矩阵进行处理；

位置四元数p、姿态四元数q四元数由一个实数单位1和三个虚数单位i，j，l组成，是包含四个实元的超复数，形式如下：p＝p₀+p₁·i+p₂·j+p₃·l，q＝q₀+q₁·i+q₂·j+q₃·l，其中p₀、q₀为四元数的标量部分，p₁,p₂,p₃，q₁,q₂,q₃为矢量部分的三个系数，可视为一点的坐标，i，j，l与坐标轴的方向相同，用位置四元数p、姿态四元数q表示接收矩阵：

Y＝kQHP＝kq^-1[H(p^-1Ip)]q，其中-1为求逆运算，I为单位矩阵；

(g)同样用四元数表示归一化信号矩阵M；

M＝q^-1[H(p^-1Ip)]q，I为单位矩阵；

(h)根据算法要求，设定位置误差四元数c和姿态误差四元数e及姿态四元数、位置四元数估算初值q和p来表示姿态四元数q和位置四元数p；

c＝c₀+c₁·i+c₂·j+c₃·l，e＝e₀+e₁·i+e₂·j+e₃·l，q＝eq，p＝pc

(i)用设定的姿态四元数估算初值q处理归一化信号矩阵M得矩阵三M₁，矩阵四M₂；

M₁＝qMq ^-1＝e^-1HPe＝(I+2E)HP,E为姿态误差四元数矩阵，

$E=\left[\begin{array}{ccc}0& e_3& -e_2\\ -e_3& 0& e_1\\ e_2& -e_1& 0\end{array}\right],$ 用耦合矩阵H处理矩阵M₁得矩阵M₂，

M₂＝H^-1(M₁)＝(I+2Z)P，Z由矩阵E变换得到，Z＝H^-1EH，对M₂做转置运算得到矩阵五M₃，M₃＝(M₂)^T＝P^T(I-2Z)＝p(I-2Z)p^-1；

(j)用设定的位置四元数估算初值p处理矩阵M₃得到矩阵六M₄，矩阵七M₅；

M₄＝p ^-1(M₃)p＝c(I-2Z)c^-1＝(I-2C)(I-2Z)，C为位置误差四元数矩阵，

$C=\left[\begin{array}{ccc}0& c_3& -c_2\\ -c_3& 0& c_1\\ c_2& -c_1& 0\end{array}\right]$

M₅＝(M₄)^T＝I+2Z+2C；

(k)将矩阵M₅展开，计算得位置误差四元数c和姿态误差四元数e；

$M_5=\left[\begin{array}{ccc}1& 2c_3-e_3& -2c_2+e_2\\ -2c_3+4e_3& 1& 2c_1+2e_1\\ 2c_2-4e_2& -2c_1-2e_1& 1\end{array}\right]$

c₀＝1，c₁＝0，c₂＝-(v₃₁+4v₁₃)/6，c₃＝(v₂₁+4v₁₂)/6

e₀＝1，e₁＝(v₂₃-4v₃₂)/4，e₂＝-(v₁₃+v₃₁)/3，e₃＝(v₁₂+v₂₁)/3，其中v₃₁，v₁₃，v₂₁，

v₁₂，v₂₃，v₃₂分别为M₅中对应行对应列的元素；

(l)计算得新的位置四元数、姿态四元数估算初值p _new，q _new为

(p _new)＝pc(q _new)＝eq

(m)重复步骤(a)～(l)，用迭代的方法不断计算出新的四元数估算初值p _new，q _new，直至误差四元数小于设定值0.0001，迭代停止，得最终位置四元数p_final，姿态四元数q_final，然后计算得表示姿态角的四元数a，且a＝p_finalq_final；

(n)最后由四元数转换欧拉角的公式得各角度；

$\left\{\begin{array}{c}\mathrm{\α}=-arctan\frac{2(p_1{p}_2-p_3{p}_0)}{{p_0}^2+{p_1}^2-{p_2}^2+{p_3}^2}\\ \mathrm{\β}=arcsin2(p_1{p}_3+p_0{p}_2)\end{array}\right.$

由此四元数定位算法结束，结合步骤(d)计算的距离R，可得接收天线相对于发射源(0,0,0)的位置坐标(R，α，β)和姿态角ω，ξ，的求解。

3.根据权利要求2所述的一种用于电磁定位的方法，其特征在于步骤(3)中的数字量送至数据处理及时序控制单元后，需将其做一定处理来提取出三轴发射三轴接收对应的数据，采用数字信号处理中常用的处理方式，首先利用窗函数将连续的采样量划分成小的集合，每个集合代表采样数据的一小段，根据窗内的采样时间为每一个采样集合设定一个权函数，用这个权函数作为系数乘以采样的数字量，其中窗函数采用布莱克曼-哈里斯(Blackman-Harris)窗，加窗函数之后再做离散傅里叶变换，在变换后的频域会有一个较尖锐的响应，当时域采样数据包含某一轴发射信号的频率时，离散傅里叶变换后对应的频率点会有一个相对较大的幅值，由此可以提取出对应的信号。