CN105929362B - 一种多目标六自由度电磁定位装置和方法 - Google Patents

Abstract

本发明涉及一种多目标六自由度电磁定位装置和方法，属于电磁定位装置和定位方法。装置由信号发射单元、信号接收单元、控制与数据处理单元组成，据定位需要，选择信号接收传感器的数量，并固定在定位目标上，接收传感器通过电磁感应，产生接收信号，控制与数据处理单元接收采样信号，执行电磁定位算法。本发明解决了现有装置对空间中大范围移动的目标跟踪定位能力较差的问题，优化定位方法，简化电路，提高了定位效率，并解决了六自由度求解时，位置参数、姿态参数求解过程相互影响造成的误差的问题。

Description

一种多目标六自由度电磁定位装置和方法

技术领域

本发明涉及一种电磁定位装置和定位方法，具体涉及一种支持多个信号接收传感器同时工作的电磁定位装置和一种并行求解目标位置及姿态的电磁定位方法。

背景技术

随着科技的不断发展，定位技术在辅助医疗、人机交互、运动检测分析等领域应用广泛。人体运动过程中，肢体会做出旋转、弯曲等动作，定位目标可能被身体其他部分遮挡，这些问题使研究对定位技术有很高的要求。与超声式、光电式等定位装置相比，基于六自由度电磁定位技术设计的装置具有速度快、成本低、便携性好等优点。六自由度电磁定位装置追踪目标的运动轨迹的同时，还可以对目标姿态精确测量，最重要的是定位不受障碍物遮挡影响。这些优点使多目标六自由度电磁定位装置可以很好的满足研究需求。

电磁定位因为其独特的优点一直受到广泛关注。刘越、王涌天等人于2000年提出了一种旋转矩阵算法，实现了六自由度电磁定位的参数计算，但算法计算复杂。李雪丽、王毅等人在题为“电磁定位系统中的定位算法研究”的文中提出了一种利用四元数迭代计算的方法定位，但算法脱离实际装置，定位方法不够完善。孙晓颖、陈明智等人在题为“一种用于电磁定位的装置和方法”的专利中，提出了一种六自由度电磁定位装置和四元数同时求解目标位置参数及姿态参数的算法，解决定位装置实时性差、定位算法中存在的奇点问题，但装置定位局限于单目标，装置中滤波放大电路设计不够完善，使装置对空间中大范围移动的目标跟踪定位能力较差，算法同时求解位置参数、姿态参数，计算结果相互影响产生误差。

发明内容

本发明提供一种多目标六自由度电磁定位装置和方法，以解决现有电磁定位装置局限于单目标、对空间中大范围移动的目标跟踪定位能力较差的问题。采用优化定位方法，简化电路，提高了定位效率，并解决了六自由度求解时，位置参数、姿态参数求解过程相互影响造成的误差的问题。

本发明采用的技术方案是：一种多目标六自由度电磁定位装置，由以下三部分组成：信号发射单元、信号接收单元、控制与数据处理单元；

(1)所述信号发射单元由：信号发射传感器、滤波放大电路一、数模转换电路顺序连接，控制与数据处理单元为信号发射单元提供发射信号，经数模转换电路、滤波放大电路一生成驱动信号，经信号发射传感器，形成电磁场；

(2)所述信号接收单元由：至少一个信号接收传感器、第一级时分复用电路、滤波放大电路二、第二级时分复用电路、模数转换电路顺序连接，其中滤波放大电路二的增益是可调节的，信号接收传感器感应到电磁场产生电压信号，信号经多路复用、滤波放大后，由模数转换电路对其采样，并传送给控制与数据处理单元；

(3)所述控制与数据处理单元：包括DSP、SDRAM存储器、FLASH存储器、USB接口及电源，DSP读取FLASH存储器中的程序，存入DSP内置RAM中，一方面产生控制电路的信号，另一方面接收采样信号并处理，计算过程中大容量数据会存储到SDRAM中，通过USB接口实现装置与上位机通信，电源为装置提供±5V、3.3V电压。

本发明所述信号发射单元中信号发射传感器由正交的三轴环形线圈组成，控制与数据处理单元为模数转换电路提供三组发射信号，经滤波放大电路后，驱动三轴线圈同时工作，发射信号采用正弦信号的形式，频率按照f_n＝k_nf_s的标准设计，其中k_n为固定的整数，n＝1、2、3，f_s为装置的定位刷新频率，根据这种标准生成的发射信号，在一个工作周期内呈三组固定值，故不再需要参考信号回流，能够简化电路，提高定位效率。

本发明所述信号接收单元中信号接收传感器由正交的三轴环形线圈组成，装置可支持一个以上信号接收传感器同时工作，当置于信号发射单元产生的电磁场中，每个信号接收传感器都能接收到三轴信号，第一级时分复用电路将传感器的三轴信号复用，经滤波放大电路二后，第二级时分复用电路将多个传感器的信号复用，经模数转换电路采样后传送给控制与数据处理单元。

本发明所述控制与数据处理单元，包括：

(1)控制与数据处理单元根据信号接收传感器的数量，为第一级时分复用电路、第二级时分复用电路提供控制信号；

(2)控制与数据处理单元为信号发射单元提供发射信号，为模数转换电路提供采样信号，这些信号及控制信号均由同一时钟信号生成，保证时钟同步；

(3)控制与数据处理单元接收采样信号，求解目标的六自由度信息，并通过USB接口传送给上位机程序。

本发明所述控制与数据处理单元、滤波放大电路二、第二级时分复用电路、模数转换电路构成反馈控制系统，用于调节滤波放大电路二的增益。

一种用于多目标六自由度电磁定位的方法，包括如下步骤：

(一)根据定位需要，选择信号接收传感器的数量，并固定在定位目标上，将信号发射传感器位置固定，作为空间的坐标原点(0,0,0)，开始定位，由控制与数据处理单元产生发射信号，经数模转换电路、滤波放大电路，驱动信号发射传感器，以信号发射传感器为中心，形成电磁场；

(二)接收传感器通过电磁感应，产生接收信号，并通过第一级时分复用电路将来自一个信号接收传感器的多路信号复用，信号经滤波放大后，经第二级时分复用电路对多个信号接收传感器的多路信号复用，由模数转换电路采样，并传送给控制与数据处理单元，滤波放大电路二、时分复用电路二、模数转换电路、控制与数据处理单元构成反馈控制系统，通过输入信号的能量调节滤波放大电路二的增益，保证目标在大范围移动时接收信号稳定，按照电磁场强度与距离的衰减规律，将定位空间划分成若干个区域，并为每个区域设定相应的增益等级，依次为K₁、K₂…K_s；

(三)控制与数据处理单元接收采样信号，执行电磁定位算法，步骤如下：

(1)首先按照时钟信号，从复用信号x(n)中将每个信号接收传感器的三轴信号还原，组成信号接收矩阵L_x，其中x表示传感器的编号，L_x是一个N×3的矩阵，表示矩阵有3个阵元，N个时间样本序列，利用同步解调的方法，提取接收信号中三个频率的成分，其中解调信号为三组固定值g₁、g₂、g₃，解调后三组信号通过低通滤波、拟合等方法组成接收矩阵S_x；

(2)利用算法将接收矩阵S_x中的位置参数、姿态参数分离，构造出两个矩阵X_x、Y_x，矩阵X_x只包含目标的位置参数，矩阵Y_x只包含目标的姿态参数，参数分离后可以对位置参数、姿态参数并行求解，结果互不影响；

(3)利用闭环、迭代的四元数算法同时求解编号为x的传感器的位置参数(x,y,z)和姿态参数并将结果通过USB接口传送给上位机程序。

本发明所述一种多目标六自由度电磁定位的方法，所述步骤(二)中的反馈控制系统负责调节滤波放大电路二的增益；

复用信号经滤波放大电路、采样后，传送到数据处理单元，数据处理单元根据时钟信号将采样信号还原，组成信号接收矩阵L_x；

求L_x的协方差矩阵T：

求解矩阵T的迹，记为tr(T)，进行阈值判断，如果tr(T)的值低于tr(T)_min将增益的等级提高一级，如果tr(T)的值高于tr(T)_max，则降低增益等级，该系统增益控制存在一个定位周期的延迟；

按照电磁场强度与距离的衰减规律，将定位空间划分成若干个区域，并为每个区域设定相应的增益等级，依次为K₁、K₂…K_s；

实际上，信号的能量只与目标的距离有关，所以可以认为接收信号的能量与目标的距离是等价的，故可以利用tr(T)去计算目标的距离R，R的误差也能直接反应算法对接收信号能量的估计是否准确；

其中R_s和tr(T_s)是对应增益等级K_s的一组常数；

本发明所述一种多目标六自由度电磁定位的方法，所述步骤(三)中的电磁定位算法，具体实现步骤如下：

为方便描述，后文省略了矩阵L_x、S_x、X_x、Y_x中的传感器编号x，用L、S、X、Y表示；

(1)还原复用信号，组建接收矩阵S；

控制与数据处理单元会根据时钟信号，将复用信号x(n)还原，并按照顺序存入信号接收矩阵L中；

L＝[l₁,l₂,l₃]^T

对矩阵L中3个阵元的信号做同步解调，解调信号用g_m表示，m＝1、2、3对应三轴发射线圈，按照f_n＝k_nf_s的标准选择复用信号频率，g_m为三组固定值，同步解调计算公式如下：

l_mn＝l_ng_m

经同步解调后，结果l_mn是由m轴产生n轴接收的信号，l_mn组成L_mn，L_mn一个3×3×l数组，对数组L_mn做低通滤波、拟合处理后得到传感器对应的接收矩阵S；

接收矩阵S与发射矩阵I的耦合关系可以利用旋转矩阵来表示；

定位过程中，信号耦合关系可以通过旋转矩阵的方式描述，位置参数可以表示为两个旋转矩阵变换和一个位移的结果，位移距离R，绕Z轴顺时针旋转角度为α，绕X轴顺时针旋转角度为β，旋转矩阵P如下所示：

同理，姿态可以通过三个旋转矩阵来表示，其中绕Z轴顺时针旋转角度为θ、表示绕X轴顺时针旋转角度ω，表示绕Y轴顺时针旋转角度旋转矩阵Q如下所示：

根据磁偶极子原理可以求得信号发射传感器、信号接收传感器的耦合矩阵H，如下所示：

综上所述，若发射矩阵为单位矩阵I，则接收矩阵S可以用如下公式表示：

S＝kQP^tHPI

式中k＝电磁耦合系数/衰减因子；

(2)接收矩阵中位置参数与姿态参数分离；

求接收矩阵S的协方差矩阵；

S^tS＝k²P^tH²P

求解左侧矩阵和右侧矩阵的迹，可以得出如下结果：

tr(S^tS)＝6k²

整理得：

k＝[tr(S^tS)/6]^(1/2)

距离参数R只与衰减因子k相关，可以通过下式求解；

R＝R_s(k_s/k)^(1/3)

公式中，k_s、R_s是目标处于增益等级K_s区域时对应的参数；

利用解得的k值计算S的归一化矩阵，用M表示，如下所示：

M＝QP^tHP

将矩阵M中的两类参数分离出来，只含有位置参数的矩阵X可以按如下公式计算：

X＝P^tFP＝(M^tM-I)/3

式中F＝dig(1,0,0)；

只含有姿态参数的矩Y以通过下式求得：

Y＝Q^tM＝M^tM-2I

(3)求解目标的位置参数(x,y,z)及姿态参数

四元数是一种超复数，一个四元数c由一个实数单位1和三个虚数单位i、j、k组成，形式如下c＝c₀+c₁i+c₂j+c₃k，与旋转矩阵相比，四元数描述目标的方向、姿态更加方便；

下面利用闭环迭代四元数算法，求解目标的位置参数及姿态参数，两种参数的迭代计算过程互不影响，所以可以同时进行；

将含有位置参数的矩阵X四元数的方式表示，如下所示：

X＝(M^tM-I)/3＝p(FP)p^-1

式中p位置参数的最佳四元数估计值，p＝p₀+p₁i+p₂j+p₃k；

最佳四元数估计值与估计初值的关系为p＝p'c，c为误差四元数，可表示为c＝1+c₁i+c₂j+c₃k，显然，当误差四元数矢量部分的值趋近于0时，估计初值p'与最佳四元数估计值p非常接近；

首先，在X矩阵的左右分别乘上(p')^-1、p'，求解得到一个包含误差四元数和位置参数的矩阵，计算公式如下所示：

X₁＝(p')^-1Xp'＝c(FP)c^-1＝(1-2C)(FP)

公式中矩阵C为：

求X₁矩阵的转置矩阵；

对做和X矩阵同样的变换；

整理上式结果，由于4CFC的值非常小，故只保留等式右侧的前三项，又因为位置参数不存在沿Y轴旋转的角度，所以误差四元数c₁＝c₂c₃，近似为0，公式右侧等式整理如下所示：

通过上式可以求解出估计四元数c的值，判断误差四元数c是否满足定位精度要求，满足则可以认为p'就是最佳估计值，如不满足，则可以利用误差四元数c求解一个新的估计初值p_new，p_new代替p'重复迭代过程；

p_new＝p'c

位置参数α，β与最佳估计四元数p的关系如下所示：

α＝arcsin2(p₁p₃+p₀p₂)

在算法第一次运行时，初始估计值选择p_s，p_s为目标α＝45°、β＝45°时四元数理论值，之后可以选择上一次计算的最佳估计值p作为下一次定位的估计初值p'；

至此目标的极坐标形式的位置参数(R,α,β)已经全部求出，可以通过如下公式将目标的位置参数由极坐标形式(R,α,β)转换成直角坐标形式(x,y,z)；

x＝Rcos(α)cos(β)

y＝Rsin(α)cos(β)

z＝Rsin(β)

利用姿态角的最佳四元数估计值q表示矩阵Y，q＝q₀+q₁i+q₂j+q₃k；

Y＝qMq^-1

利用估计初值q'对矩阵Y进行一次变换求解误差四元数u，最佳四元数估计值与估计初值的关系为q＝q'u，其中u为误差四元数u＝1+u₁i+u₂j+u₃k，显然，当误差四元数矢量部分u₁、u₂、u₃的值趋近于0时，姿态四元数估算值q'的值与最佳估计值q非常接近；

与X矩阵的计算过程类似，对Y的变换如下所示：

(q')^-1Yq'＝uMu^-1＝(I-2U)M

其中U为误差四元数组成的矩阵；

整理上式，可以求解UM为：

UM＝[M-(a')^-1Ya']/2＝D

将上述公式写成如下形式：

从以上9组等式中选择三组独立的等式，写成如下形式：

将上式写成如下形式：

通过上式可以求解出估计四元数u的值,之后判断四元数u是否满足精度要求，如果误差四元数u满足定位要求，则可以认为q'就是最佳估计值，不满足精度要求，则可以利用误差四元数u求解一个新的估计初值q_new，q_new取代之前的估计初值q'重复迭代过程，利用误差四元数计算估计初值的公式如下所示：

q_new＝q'u

姿态参数θ、ω、与最佳估计四元数q的关系如下所示：

ω＝arcsin2(q₁q₃-q₀q₂)

在算法第一次运行时，初始估计值选择q_s，q_s为目标θ＝45°、ω＝45°、时四元数理论值，之后可以选择上一次计算的最佳估计值q作为下一次定位的估计初值q'；

至此通过多目标六自由度电磁定位方法的步骤(三)计算出每个信号接收传感器的空间位置(x,y,z)及姿态角

本发明具有以下优点：

(1)现有的电磁定位装置局限于单目标定位，本发明采用二级时分复用电路对多个传感器的多路信号时分复用，采用多目标六自由度电磁定位算法，使装置可以同时对一个以上目标跟踪定位。

(2)现有的电磁定位装置，需要引入参考信号回流，本发明采用f_n＝k_nf_s的标准设计发射信号，定位过程中不再需要参考信号回流，简化信号接收单元电路，将定位效率提高33％。

(3)现有的电磁定位装置，对大范围移动的目标跟踪定位不够准确，本发明采用反馈控制系统调节滤波放大电路二的增益，保证目标在大范围移动时接收信号稳定，利用信号接收矩阵L_x协方差矩阵的迹，估计接收信号能量的算法，这种算法过程简便，计算量小，对信号能量估计准确。

(4)本发明提出一种多目标六自由度电磁定位算法，将接收矩阵S_x中的位置参数、姿态参数分离，组成矩阵X_x、Y_x，之后对X_x、Y_x分别求解，因为X_x只含有位置参数的信息、Y_x中只含有姿态参数的信息，所以这种方法可以避免求解过程中两种参数互相影响产生的误差。

(5)本发明采用两种参数的计算过程同时进行，采用闭环迭代四元数算法求解目标的位置参数与姿态参数，大幅度提高定位效率。

附图说明

图1是多目标六自由度电磁定位装置原理图；

图2是控制与数据处理单元工作原理图；

图3A是单个信号接收传感器工作时，时分复用电路的控制信号；

图3B是十六个信号接收传感器同时工作时，时分复用电路的控制信号；

图4是反馈控制系统原理图；

图5是按增益等级划分的定位区域；

图6是多目标六自由度电磁定位算法流程图；

图7是闭环四元数迭代算法计算位置参数流程图；

图8是闭环四元数迭代算法计算姿态参数流程图；

图9是tr(T)估计接收信号能量实验中选取的位置示意图；

图10是位置一二三的实验结果图；

图11是位置四五六的实验结果图；

图12是位置七八九的实验结果图。

具体实施方式

一种多目标六自由度电磁定位装置，由以下三部分组成：信号发射单元1、信号接收单元2、控制与数据处理单元3，如图1所示；

(1)所述信号发射单元1由：信号发射传感器11、滤波放大电路一12、数模转换电路13顺序连接，控制与数据处理单元3为信号发射单元1提供发射信号，经数模转换电路13、滤波放大电路一12生成驱动信号，经信号发射传感器11，形成电磁场；

(2)所述信号接收单元2由：至少一个信号接收传感器21、第一级时分复用22电路、滤波放大电路二23、第二级时分复用电路24、模数转换电路顺序连接25，其中滤波放大电路二23的增益是可调节的，信号接收传感器21感应到电磁场产生电压信号，信号经多路复用、滤波放大后，由模数转换电路25对其采样，并传送给控制与数据处理单元3；

(3)所述控制与数据处理单元3，其组成及工作原理如图2所示：包括DSP301、

SDRAM存储器302、FLASH存储器303、USB接口304及电源305，DSP301读取FLASH存储器303中的程序，存入DSP301内置RAM中，一方面产生控制电路的信号，另一方面接收采样信号并处理，计算过程中大容量数据会存储到SDRAM302中，通过USB接口304实现装置与上位机通信，电源305为装置提供±5V、3.3V电压。其中DSP301选取了德州仪器公司的F28335,这是一款32位浮点型DSP，TMS320F28335融合了控制外设的集成功能与MCU的易用性，具有强大的控制和信号处理能力及C语言编程效率，能够实现复杂的算法；

一种多目标六自由度电磁定位装置中的信号发射单元1：所述信号发射传感器11由正交的三轴环形线圈组成，控制与数据处理单元3为模数转换电路13提供三组发射信号，经滤波放大电路12后，驱动三轴线圈同时工作，发射信号采用正弦信号的形式，频率按照f_n＝k_nf_s的标准设计，其中k_n为固定的整数，n＝1、2、3，f_s为装置的定位刷新频率，根据这种标准生成的发射信号，在一个工作周期内呈三组固定值，故不再需要参考信号回流，能够简化电路，提高定位效率。

一种多目标六自由度电磁定位装置中的信号接收单元2，所述信号接收传感器21由正交的三轴环形线圈组成，装置可支持一个以上信号接收传感器同时工作,当置于信号发射单元产生的电磁场中，每个信号接收传感器21都能接收到三轴信号，第一级时分复用电路22将传感器的三轴信号复用，经滤波放大电路二23后，第二级时分复用电路24将多个传感器的信号复用，经模数转换电路25采样后传送给控制与数据处理单元3。

一种多目标六自由度电磁定位装置中的控制与数据处理单元3，包括：

(1)控制与数据处理单元3根据信号接收传感器21的数量，为第一级时分复用电路22、第二级时分复用24电路提供控制信号,如图3A、B所示，其中PWM1A、PMW1B为第一级时分复用电路22的控制信号，PWM2A、PMW2B为第二级时分复用电路24的控制信号；

(2)控制与数据处理单元3为信号发射单元1提供发射信号，为模数转换电路13提供采样信号，这些信号及控制信号均由同一时钟信号生成，保证时钟同步；

(3)控制与数据处理单元3接收采样信号，求解目标的六自由度信息，并通过USB接口304传送给上位机程序。

所述的多目标六自由度电磁定位装置，其中控制与数据处理单元3、滤波放大电路二23、第二级时分复用电路24、模数转换电路25构成反馈控制系统，如图4所示，用于控制滤波放大电路二23的增益，系统有多路输入信号X_x(t),多路信号分别由运算放大电路二23放大k_x倍后，经第二级时分复用电路24，整合成信号x(t),模数转换电路25采样后，以x(n)的形式传递给控制与数据处理单元3，控制与数据处理单元3首先根据时钟信号，将复用信号还原，并存入对应的接收矩阵L_x中，求L_x的协方差矩阵T，求解矩阵T的迹，记为tr(T)，进行阈值判断，如果tr(T)的值低于tr(T)_min将增益的等级提高一级，如果tr(T)的值高于tr(T)_max，则降低增益等级，该系统增益控制存在一个定位周期的延迟。

按照电磁场强度与距离的衰减规律，将定位空间划分成若干个区域，并为每个区域设定相应的增益等级，依次为K₁、K₂…K_s，如图5所示。

一种用于多目标六自由度电磁定位的方法，过程如图6所示，包括如下步骤：

(一)根据定位需要，选择信号接收传感器的数量，并固定在定位目标上，将信号发射传感器位置固定，作为空间的坐标原点(0,0,0)，开始定位，由控制与数据处理单元3产生发射信号，经数模转换电路13、滤波放大电路12，驱动信号发射传感器11，以信号发射传感器为中心，形成电磁场；

(二)接收传感器21通过电磁感应，产生接收信号，并通过第一级时分复用电路22将来自一个信号接收传感器21的多路信号复用，信号经滤波放大后，经第二级时分复用电路24对多个信号接收传感器的多路信号复用，由模数转换电路25采样，并传送给控制与数据处理单元3，滤波放大电路二23、时分复用电路二24、模数转换电路25、控制与数据处理单元3构成反馈控制系统，通过输入信号的能量调节滤波放大电路二的增益，保证目标在大范围移动时接收信号稳定，按照电磁场强度与距离的衰减规律，将定位空间划分成若干个区域，并为每个区域设定相应的增益等级，依次为K₁、K₂…K_s；

(三)控制与数据处理单元3接收采样信号，执行电磁定位算法，过程如图6所示，控制与数据处理单元会根据时钟信号，将复用信号x(n)还原，并按照顺序存入信号接收矩阵L_x中，从矩阵L_x中提取接收矩阵S_x，之后求解衰减因子k，并求解归一化矩阵M，将M矩阵中的位置参数、姿态参数分离，构成矩阵X_x、矩阵Y_x，对两矩阵同时进行四元数迭代计算，求解目标的位置参数(x,y,z)姿态参数

一种多目标六自由度电磁定位的方法，步骤(二)中的反馈控制系统负责调节滤波放大电路二的增益；

复用信号经滤波放大电路、采样后，传送到数据处理单元，数据处理单元根据时钟信号将采样信号还原，组成信号接收矩阵L_x；

求L_x的协方差矩阵T：

求解矩阵T的迹，记为tr(T)，进行阈值判断，如果tr(T)的值低于tr(T)_min将增益的等级提高一级，如果tr(T)的值高于tr(T)_max，则降低增益等级，该系统增益控制存在一个定位周期的延迟；

按照电磁场强度与距离的衰减规律，将定位空间划分成若干个区域，并为每个区域设定相应的增益等级，依次为K₁、K₂…K_s；

实际上，信号的能量只与目标的距离有关，所以可以认为接收信号的能量与目标的距离是等价的，故可以利用tr(T)去计算目标的距离R，R的误差也能直接反应算法对接收信号能量的估计是否准确；

其中R_s和tr(T_s)是对应增益等级K_s的一组常数；

一种多目标六自由度电磁定位的方法，步骤(三)中的电磁定位算法，具体实现步骤如下：

为方便描述，后文省略了矩阵L_x、S_x、X_x、Y_x中的传感器编号x，用L、S、X、Y表示；

(1)还原复用信号，组建接收矩阵S；

控制与数据处理单元会根据时钟信号，将复用信号x(n)还原，并按照顺序存入信号接收矩阵L中；

L＝[l₁,l₂,l₃]^T

对矩阵L中3个阵元的信号做同步解调，解调信号用g_m表示，m＝1、2、3对应三轴发射线圈，按照f_n＝k_nf_s的标准选择复用信号频率，g_m为三组固定值，同步解调计算公式如下：

l_mn＝l_ng_m

经同步解调后，结果l_mn是由m轴产生n轴接收的信号，l_mn组成L_mn，L_mn一个3×3×l数组，对数组L_mn做低通滤波、拟合处理后得到传感器对应的接收矩阵S；

接收矩阵S与发射矩阵I的耦合关系可以利用旋转矩阵来表示；

定位过程中，信号耦合关系可以通过旋转矩阵的方式描述，位置参数可以表示为两个旋转矩阵变换和一个位移的结果，位移距离R，绕Z轴顺时针旋转角度为α，绕X轴顺时针旋转角度为β，旋转矩阵P如下所示：

同理，姿态可以通过三个旋转矩阵来表示，其中绕Z轴顺时针旋转角度为θ、表示绕X轴顺时针旋转角度ω，表示绕Y轴顺时针旋转角度旋转矩阵Q如下所示：

根据磁偶极子原理可以求得信号发射传感器、信号接收传感器的耦合矩阵H，如下所示：

综上所述，若发射矩阵为单位矩阵I，则接收矩阵S可以用如下公式表示：

S＝kQP^tHPI

式中k＝电磁耦合系数/衰减因子；

(2)接收矩阵中位置参数与姿态参数分离；

求接收矩阵S的协方差矩阵；

S^tS＝k²P^tH²P

求解左侧矩阵和右侧矩阵的迹，可以得出如下结果：

tr(S^tS)＝6k²

整理得：

k＝[tr(S^tS)/6]^(1/2)

距离参数R只与衰减因子k相关，可以通过下式求解；

R＝R_s(k_s/k)^(1/3)

公式中，k_s、R_s是目标处于增益等级K_s区域时对应的参数；

利用解得的k值计算S的归一化矩阵，用M表示，如下所示：

M＝QP^tHP

将矩阵M中的两类参数分离出来，只含有位置参数的矩阵X可以按如下公式计算：

X＝P^tFP＝(M^tM-I)/3

式中F＝dig(1,0,0)；

只含有姿态参数的矩Y以通过下式求得：

Y＝Q^tM＝M^tM-2I

(3)求解目标的位置参数(x,y,z)及姿态参数

四元数是一种超复数，一个四元数c由一个实数单位1和三个虚数单位i、j、k组成，形式如下c＝c₀+c₁i+c₂j+c₃k，与旋转矩阵相比，四元数描述目标的方向、姿态更加方便；

下面利用闭环迭代四元数算法，求解目标的位置参数及姿态参数，两种参数的迭代计算过程互不影响，所以可以同时进行；

求解位置参数的过程如图7所示，首先，在X矩阵的左右分别乘上估计初值p'和(p')-¹，求解得到一个包含误差四元数和位置参数的矩阵X₁，求X₁矩阵的转置矩阵，对做和X矩阵同样的变换，可以求解出误差四元数c的值，判断误差四元数c是否满足定位精度要求，满足则可以认为p'就是最佳估计值，求解出目标的位置参数，如不满足，则可以利用误差四元数c求解一个新的估计初值p_new，p_new代替p'重复迭代过程，在算法第一次运行时，初始估计值选择p_s，p_s为目标α＝45°、β＝45°时四元数理论值，之后可以选择上一次计算的最佳估计值p作为下一次定位的估计初值p'；

首先，在X矩阵的左右分别乘上(p')^-1、p'，求解得到一个包含误差四元数和位置参数的矩阵，计算公式如下所示：

X₁＝(p')^-1Xp'＝c(FP)c^-1＝(1-2C)(FP)

公式中矩阵C为：

求X₁矩阵的转置矩阵；

对做和X矩阵同样的变换；

整理上式结果，由于4CFC的值非常小，故只保留等式右侧的前三项，又因为位置参数不存在沿Y轴旋转的角度，所以误差四元数c₁＝c₂c₃，近似为0，公式右侧等式整理如下所示：

通过上式可以求解出估计四元数c的值，判断误差四元数c是否满足定位精度要求，满足则可以认为p'就是最佳估计值，如不满足，则可以利用误差四元数c求解一个新的估计初值p_new，p_new代替p'重复迭代过程；

p_new＝p'c

位置参数α，β与最佳估计四元数p的关系如下所示：

α＝arcsin2(p₁p₃+p₀p₂)

在算法第一次运行时，初始估计值选择p_s，p_s为目标α＝45°、β＝45°时四元数理论值，之后可以选择上一次计算的最佳估计值p作为下一次定位的估计初值p'；

至此目标的极坐标形式的位置参数(R,α,β)已经全部求出，可以通过如下公式将目标的位置参数由极坐标形式(R,α,β)转换成直角坐标形式(x,y,z)；

x＝Rcos(α)cos(β)

y＝Rsin(α)cos(β)

z＝Rsin(β)

利用姿态角的最佳四元数估计值q表示矩阵Y，q＝q₀+q₁i+q₂j+q₃k；

Y＝qMq^-1

利用估计初值q'对矩阵Y进行一次变换求解误差四元数u，最佳四元数估计值与估计初值的关系为q＝q'u，其中u为误差四元数u＝1+u₁i+u₂j+u₃k，显然，当误差四元数矢量部分u₁、u₂、u₃的值趋近于0时，姿态四元数估算值q'的值与最佳估计值q非常接近；

求解姿态参数的过程如图8所示,在Y矩阵的左右分别乘上估计初值q'和(q')^-1，求解出矩阵D的值，之后利用M矩阵构造出等式通过上式可以求解出估计四元数u的值,判断四元数u是否满足精度要求，如果误差四元数u满足定位要求，则可以认为q'就是最佳估计值，求出目标的姿态参数，不满足精度要求，则可以利用误差四元数u求解一个新的估计初值q_new，q_new取代之前的估计初值q'重复迭代过程，在算法第一次运行时，初始估计值选择q_s，q_s为目标θ＝45°、ω＝45°、时四元数理论值，之后可以选择上一次计算的最佳估计值q作为下一次定位的估计初值q'。

详细过程如下：

利用姿态角的最佳四元数估计值q表示矩阵Y，q＝q₀+q₁i+q₂j+q₃k；

Y＝qMq^-1

利用估计初值q'对矩阵Y进行一次变换求解误差四元数u，最佳四元数估计值与估计初值的关系为q＝q'u，其中u为误差四元数u＝1+u₁i+u₂j+u₃k，显然，当误差四元数矢量部分u₁、u₂、u₃的值趋近于0时，姿态四元数估算值q'的值与最佳估计值q非常接近；

与X矩阵的计算过程类似，对Y的变换如下所示：

(q')^-1Yq'＝uMu^-1＝(I-2U)M

其中U为误差四元数组成的矩阵；

整理上式，可以求解UM为：

UM＝[M-(a')^-1Ya']/2＝D

将上述公式写成如下形式：

从以上9组等式中选择三组独立的等式，写成如下形式：

将上式写成如下形式：

通过上式可以求解出估计四元数u的值,之后判断四元数u是否满足精度要求，如果误差四元数u满足定位要求，则可以认为q'就是最佳估计值，不满足精度要求，则可以利用误差四元数u求解一个新的估计初值q_new，q_new取代之前的估计初值q'重复迭代过程，利用误差四元数计算估计初值的公式如下所示：

q_new＝q'u

姿态参数θ、ω、与最佳估计四元数q的关系如下所示：

ω＝arcsin2(q₁q₃-q₀q₂)

在算法第一次运行时，初始估计值选择q_s，q_s为目标θ＝45°、ω＝45°、时四元数理论值，之后可以选择上一次计算的最佳估计值q作为下一次定位的估计初值q'；

至此通过多目标六自由度电磁定位方法的步骤(三)计算出每个信号接收传感器的空间位置(x,y,z)及姿态角

为验证反馈控制系统中算法对信号能量估计,将信号接收传感器置于空间中9个不同的位置并进行了实验；

实验目的

验证反馈控制系统中算法对信号能量估计是否准确。

实验原理

因为信号的能量只与目标的距离有关，所以可以认为接收信号的能量与目标的距离是等价的，如果tr(T)可以精确地计算距离R，则证明利用tr(T)去估计信号的能量也会十分精确，计算过程如下：

(1)复用信号经滤波放大电路、采样后，传送到数据处理单元，数据处理单元根据时钟信号将采样信号还原，组成信号接收矩阵L_x；

(2)求L_x的协方差矩阵tr(T)；

(3)求T矩阵的迹；

tr(T)＝T₁₁+T₂₂+T₃₃

(4)求解距离R；

其中R_s和tr(T_s)是对应增益等级K_s的一组常数。

实验过程

(1)选择空间中的9个位置，9个位置的距离逐渐增加，空间分布如图9所示,在每个位置测量5次信号。

(2)利用原理中的算法计算该位置的距离R，并与实际的距离相比较。

实验结果

定位结果

图10是位置一二三的实验结果。

图11是位置四五六的实验结果。

图12是位置七八九的实验结果。

结论

9个位置的45组数据测量的距离R与实际的距离相比，最大误差不超过0.2cm，故可以认为，验证反馈控制系统中算法对信号能量估计是准确的。

Claims (2)

1.一种用于多目标六自由度电磁定位的方法，其特征在于包括如下步骤：

(一)根据定位需要，选择信号接收传感器的数量，并固定在定位目标上，将信号发射传感器位置固定，作为空间的坐标原点(0,0,0)，开始定位，由控制与数据处理单元产生发射信号，经数模转换电路、滤波放大电路，驱动信号发射传感器，以信号发射传感器为中心，形成电磁场；

(二)接收传感器通过电磁感应，产生接收信号，并通过第一级时分复用电路将来自一个信号接收传感器的多路信号复用，信号经滤波放大后，经第二级时分复用电路对多个信号接收传感器的多路信号复用，由模数转换电路采样，并传送给控制与数据处理单元，滤波放大电路二、时分复用电路二、模数转换电路、控制与数据处理单元构成反馈控制系统，通过输入信号的能量调节滤波放大电路二的增益，保证目标在大范围移动时接收信号稳定，按照电磁场强度与距离的衰减规律，将定位空间划分成若干个区域，并为每个区域设定相应的增益等级，依次为K₁、K₂…K_s；

(三)控制与数据处理单元接收采样信号，执行电磁定位算法，步骤如下：

(1)首先按照时钟信号，从复用信号x(n)中将每个信号接收传感器的三轴信号还原，组成信号接收矩阵L_x，其中x表示传感器的编号，L_x是一个N×3的矩阵，表示矩阵有3个阵元，N个时间样本序列，利用同步解调的方法，提取接收信号中三个频率的成分，其中解调信号为三组固定值g₁、g₂、g₃，解调后三组信号通过低通滤波、拟合方法组成接收矩阵S_x；

(2)利用算法将接收矩阵S_x中的位置参数、姿态参数分离，构造出两个矩阵X_x、Y_x，矩阵X_x只包含目标的位置参数，矩阵Y_x只包含目标的姿态参数，参数分离后，对位置参数、姿态参数并行求解，结果互不影响；

(3)利用闭环、迭代的四元数算法同时求解编号为x的传感器的位置参数(x,y,z)和姿态参数并将结果通过USB接口传送给上位机程序；

所述电磁定位算法，具体实现步骤如下：

为方便描述，后文省略了矩阵L_x、S_x、X_x、Y_x中的传感器编号x，用L、S、X、Y表示；

(1)还原复用信号，组建接收矩阵S；

控制与数据处理单元会根据时钟信号，将复用信号x(n)还原，并按照顺序存入信号接收矩阵L中；

L＝[l₁,l₂,l₃]^T

对矩阵L中3个阵元的信号做同步解调，解调信号用g_m表示，m＝1、2、3对应三轴发射线圈，按照f_n＝k_nf_s的标准选择复用信号频率，g_m为三组固定值，同步解调计算公式如下：

l_mn＝l_ng_m

经同步解调后，结果l_mn是由m轴产生n轴接收的信号，l_mn组成L_mn，L_mn一个3×3×l数组，对数组L_mn做低通滤波、拟合处理后得到传感器对应的接收矩阵S；

接收矩阵S与发射矩阵I的耦合关系利用旋转矩阵来表示；

定位过程中，信号耦合关系通过旋转矩阵的方式描述，位置参数表示为两个旋转矩阵变换和一个位移的结果，位移距离R，绕Z轴顺时针旋转角度为α，绕X轴顺时针旋转角度为β，旋转矩阵P如下所示：

同理，姿态通过三个旋转矩阵来表示，其中绕Z轴顺时针旋转角度为θ、表示绕X轴顺时针旋转角度ω，表示绕Y轴顺时针旋转角度旋转矩阵Q如下所示：

根据磁偶极子原理求得信号发射传感器、信号接收传感器的耦合矩阵H，如下所示：

综上所述，若发射矩阵为单位矩阵I，则接收矩阵S用如下公式表示：

S＝kQP^tHPI

式中k＝电磁耦合系数/衰减因子；

(2)接收矩阵中位置参数与姿态参数分离；

求接收矩阵S的协方差矩阵；

S^tS＝k²P^tH²P

求解左侧矩阵和右侧矩阵的迹，得出如下结果：

tr(S^tS)＝6k²

整理得：

k＝[tr(S^tS)/6]⁽¹²⁾

距离参数R只与衰减因子k相关，通过下式求解；

R＝R_s(k_s/k)⁽¹³⁾

公式中，k_s、R_s是目标处于增益等级K_s区域时对应的参数；

利用解得的k值计算S的归一化矩阵，用M表示，如下所示：

M＝QP^tHP

将矩阵M中的两类参数分离出来，只含有位置参数的矩阵X按如下公式计算：

X＝P^tFP＝(M^tM-I)/3

式中F＝dig(1,0,0)；

只含有姿态参数的矩Y以通过下式求得：

Y＝Q^tM＝M^tM-2I

(3)求解目标的位置参数(x,y,z)及姿态参数

四元数是一种超复数，一个四元数c由一个实数单位1和三个虚数单位i、j、k组成，形式如下c＝c₀+c₁i+c₂j+c₃k，与旋转矩阵相比，四元数描述目标的方向、姿态更加方便；

下面利用闭环迭代四元数算法，求解目标的位置参数及姿态参数，两种参数的迭代计算过程互不影响，所以同时进行；

将含有位置参数的矩阵X四元数的方式表示，如下所示：

X＝(M^tM-I)/3＝p(FP)p^-1

式中p位置参数的最佳四元数估计值，p＝p₀+p₁i+p₂j+p₃k；

最佳四元数估计值与估计初值的关系为p＝p'c，c为误差四元数，表示为c＝1+c₁i+c₂j+c₃k，显然，当误差四元数矢量部分的值趋近于0时，估计初值p'与最佳四元数估计值p相等；

首先，在X矩阵的左右分别乘上(p')^-1、p'，求解得到一个包含误差四元数和位置参数的矩阵，计算公式如下所示：

X₁＝(p')^-1Xp'＝c(FP)c^-1＝(1-2C)(FP)

公式中矩阵C为：

求X₁矩阵的转置矩阵；

对做和X矩阵同样的变换；

整理上式结果，由于4CFC的值非常小，故只保留等式右侧的前三项，又因为位置参数不存在沿Y轴旋转的角度，所以误差四元数c₁＝c₂c₃、为0，公式右侧等式整理如下所示：

通过上式求解出估计四元数c的值，判断误差四元数c是否满足定位精度要求，满足则认为p'就是最佳估计值，如不满足，则利用误差四元数c求解一个新的估计初值p_new，p_new代替p'重复迭代过程；

p_new＝p'c

位置参数α，β与最佳估计四元数p的关系如下所示：

α＝arcsin2(p₁p₃+p₀p₂)

在算法第一次运行时，初始估计值选择p_s，p_s为目标α＝45°、β＝45°时四元数理论值，之后选择上一次计算的最佳估计值p作为下一次定位的估计初值p'；

至此目标的极坐标形式的位置参数(R,α,β)已经全部求出，通过如下公式将目标的位置参数由极坐标形式(R,α,β)转换成直角坐标形式(x,y,z)；

x＝Rcos(α)cos(β)

y＝Rsin(α)cos(β)

z＝Rsin(β)

利用姿态角的最佳四元数估计值q表示矩阵Y，q＝q₀+q₁i+q₂j+q₃k；

Y＝qMq^-1

利用估计初值q'对矩阵Y进行一次变换求解误差四元数u，最佳四元数估计值与估计初值的关系为q＝q'u，其中u为误差四元数u＝1+u₁i+u₂j+u₃k，显然，当误差四元数矢量部分u₁、u₂、u₃的值趋近于0时，姿态四元数估算值q'的值与最佳估计值q相等；

对Y的变换如下所示：

(q')^-1Yq'＝uMu^-1＝(I-2U)M

其中U为误差四元数组成的矩阵；

整理上式，求解UM为：

UM＝[M-(a')^-1Ya']/2＝D

将上述公式写成如下形式：

从以上9组等式中选择三组独立的等式，写成如下形式：

将上式写成如下形式：

通过上式求解出估计四元数u的值,之后判断四元数u是否满足精度要求，如果误差四元数u满足定位要求，则认为q'就是最佳估计值，不满足精度要求，则利用误差四元数u求解一个新的估计初值q_new，q_new取代之前的估计初值q'重复迭代过程，利用误差四元数计算估计初值的公式如下所示：

q_new＝q'u

姿态参数θ、ω、与最佳估计四元数q的关系如下所示：

ω＝arcsin2(q₁q₃-q₀q₂)

在算法第一次运行时，初始估计值选择q_s，q_s为目标θ＝45°、ω＝45°、时四元数理论值，之后选择上一次计算的最佳估计值q作为下一次定位的估计初值q'；

至此通过多目标六自由度电磁定位方法的步骤(三)计算出每个信号接收传感器的空间位置(x,y,z)及姿态角

2.根据权利要求1所述的一种用于多目标六自由度电磁定位的方法，其特征在于所述步骤(二)中的反馈控制系统负责调节滤波放大电路二的增益；

复用信号经滤波放大电路、采样后，传送到数据处理单元，数据处理单元根据时钟信号将采样信号还原，组成信号接收矩阵L_x；

求L_x的协方差矩阵T：

求解矩阵T的迹，记为tr(T)，进行阈值判断，如果tr(T)的值低于tr(T)_min将增益的等级提高一级，如果tr(T)的值高于tr(T)_max，则降低增益等级，系统增益控制存在一个定位周期的延迟；

按照电磁场强度与距离的衰减规律，将定位空间划分成若干个区域，并为每个区域设定相应的增益等级，依次为K₁、K₂…K_s；

实际上，信号的能量只与目标的距离有关，所以认为接收信号的能量与目标的距离是等价的，故利用tr(T)去计算目标的距离R，R的误差也能直接反应算法对接收信号能量的估计是否准确；

其中R_s和tr(T_s)是对应增益等级K_s的一组常数。