[发明专利]一种多目标六自由度电磁定位装置和方法

摘要

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

主权项

1.一种用于多目标六自由度电磁定位的方法，其特征在于包括如下步骤：(一)根据定位需要，选择信号接收传感器的数量，并固定在定位目标上，将信号发射传感器位置固定，作为空间的坐标原点(0,0,0)，开始定位，由控制与数据处理单元产生发射信号，经数模转换电路、滤波放大电路，驱动信号发射传感器，以信号发射传感器为中心，形成电磁场；(二)接收传感器通过电磁感应，产生接收信号，并通过第一级时分复用电路将来自一个信号接收传感器的多路信号复用，信号经滤波放大后，经第二级时分复用电路对多个信号接收传感器的多路信号复用，由模数转换电路采样，并传送给控制与数据处理单元，滤波放大电路二、时分复用电路二、模数转换电路、控制与数据处理单元构成反馈控制系统，通过输入信号的能量调节滤波放大电路二的增益，保证目标在大范围移动时接收信号稳定，按照电磁场强度与距离的衰减规律，将定位空间划分成若干个区域，并为每个区域设定相应的增益等级，依次为K1、K2…Ks；(三)控制与数据处理单元接收采样信号，执行电磁定位算法，步骤如下：(1)首先按照时钟信号，从复用信号x(n)中将每个信号接收传感器的三轴信号还原，组成信号接收矩阵Lx，其中x表示传感器的编号，Lx是一个N×3的矩阵，表示矩阵有3个阵元，N个时间样本序列，利用同步解调的方法，提取接收信号中三个频率的成分，其中解调信号为三组固定值g1、g2、g3，解调后三组信号通过低通滤波、拟合方法组成接收矩阵Sx；(2)利用算法将接收矩阵Sx中的位置参数、姿态参数分离，构造出两个矩阵Xx、Yx，矩阵Xx只包含目标的位置参数，矩阵Yx只包含目标的姿态参数，参数分离后，对位置参数、姿态参数并行求解，结果互不影响；(3)利用闭环、迭代的四元数算法同时求解编号为x的传感器的位置参数(x,y,z)和姿态参数并将结果通过USB接口传送给上位机程序；所述电磁定位算法，具体实现步骤如下：为方便描述，后文省略了矩阵Lx、Sx、Xx、Yx中的传感器编号x，用L、S、X、Y表示；(1)还原复用信号，组建接收矩阵S；控制与数据处理单元会根据时钟信号，将复用信号x(n)还原，并按照顺序存入信号接收矩阵L中；L＝[l1,l2,l3]T对矩阵L中3个阵元的信号做同步解调，解调信号用gm表示，m＝1、2、3对应三轴发射线圈，按照fn＝knfs的标准选择复用信号频率，gm为三组固定值，同步解调计算公式如下：lmn＝lngm经同步解调后，结果lmn是由m轴产生n轴接收的信号，lmn组成Lmn，Lmn一个3×3×l数组，对数组Lmn做低通滤波、拟合处理后得到传感器对应的接收矩阵S；接收矩阵S与发射矩阵I的耦合关系利用旋转矩阵来表示；定位过程中，信号耦合关系通过旋转矩阵的方式描述，位置参数表示为两个旋转矩阵变换和一个位移的结果，位移距离R，绕Z轴顺时针旋转角度为α，绕X轴顺时针旋转角度为β，旋转矩阵P如下所示：同理，姿态通过三个旋转矩阵来表示，其中绕Z轴顺时针旋转角度为θ、表示绕X轴顺时针旋转角度ω，表示绕Y轴顺时针旋转角度旋转矩阵Q如下所示：根据磁偶极子原理求得信号发射传感器、信号接收传感器的耦合矩阵H，如下所示：综上所述，若发射矩阵为单位矩阵I，则接收矩阵S用如下公式表示：S＝kQPtHPI式中k＝电磁耦合系数/衰减因子；(2)接收矩阵中位置参数与姿态参数分离；求接收矩阵S的协方差矩阵；StS＝k2PtH2P求解左侧矩阵和右侧矩阵的迹，得出如下结果：tr(StS)＝6k2整理得：k＝[tr(StS)/6](12)距离参数R只与衰减因子k相关，通过下式求解；R＝Rs(ks/k)(13)公式中，ks、Rs是目标处于增益等级Ks区域时对应的参数；利用解得的k值计算S的归一化矩阵，用M表示，如下所示：M＝QPtHP将矩阵M中的两类参数分离出来，只含有位置参数的矩阵X按如下公式计算：X＝PtFP＝(MtM‑I)/3式中F＝dig(1,0,0)；只含有姿态参数的矩Y以通过下式求得：Y＝QtM＝MtM‑2I(3)求解目标的位置参数(x,y,z)及姿态参数四元数是一种超复数，一个四元数c由一个实数单位1和三个虚数单位i、j、k组成，形式如下c＝c0+c1i+c2j+c3k，与旋转矩阵相比，四元数描述目标的方向、姿态更加方便；下面利用闭环迭代四元数算法，求解目标的位置参数及姿态参数，两种参数的迭代计算过程互不影响，所以同时进行；将含有位置参数的矩阵X四元数的方式表示，如下所示：X＝(MtM‑I)/3＝p(FP)p‑1式中p位置参数的最佳四元数估计值，p＝p0+p1i+p2j+p3k；最佳四元数估计值与估计初值的关系为p＝p'c，c为误差四元数，表示为c＝1+c1i+c2j+c3k，显然，当误差四元数矢量部分的值趋近于0时，估计初值p'与最佳四元数估计值p相等；首先，在X矩阵的左右分别乘上(p')‑1、p'，求解得到一个包含误差四元数和位置参数的矩阵，计算公式如下所示：X1＝(p')‑1Xp'＝c(FP)c‑1＝(1‑2C)(FP)公式中矩阵C为：求X1矩阵的转置矩阵；对做和X矩阵同样的变换；整理上式结果，由于4CFC的值非常小，故只保留等式右侧的前三项，又因为位置参数不存在沿Y轴旋转的角度，所以误差四元数c1＝c2c3、为0，公式右侧等式整理如下所示：通过上式求解出估计四元数c的值，判断误差四元数c是否满足定位精度要求，满足则认为p'就是最佳估计值，如不满足，则利用误差四元数c求解一个新的估计初值pnew，pnew代替p'重复迭代过程；pnew＝p'c位置参数α，β与最佳估计四元数p的关系如下所示：α＝arcsin2(p1p3+p0p2)在算法第一次运行时，初始估计值选择ps，ps为目标α＝45°、β＝45°时四元数理论值，之后选择上一次计算的最佳估计值p作为下一次定位的估计初值p'；至此目标的极坐标形式的位置参数(R,α,β)已经全部求出，通过如下公式将目标的位置参数由极坐标形式(R,α,β)转换成直角坐标形式(x,y,z)；x＝Rcos(α)cos(β)y＝Rsin(α)cos(β)z＝Rsin(β)利用姿态角的最佳四元数估计值q表示矩阵Y，q＝q0+q1i+q2j+q3k；Y＝qMq‑1利用估计初值q'对矩阵Y进行一次变换求解误差四元数u，最佳四元数估计值与估计初值的关系为q＝q'u，其中u为误差四元数u＝1+u1i+u2j+u3k，显然，当误差四元数矢量部分u1、u2、u3的值趋近于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求解一个新的估计初值qnew，qnew取代之前的估计初值q'重复迭代过程，利用误差四元数计算估计初值的公式如下所示：qnew＝q'u姿态参数θ、ω、与最佳估计四元数q的关系如下所示：ω＝arcsin2(q1q3‑q0q2)在算法第一次运行时，初始估计值选择q_s，q_s为目标θ＝45°、ω＝45°、时四元数理论值，之后选择上一次计算的最佳估计值q作为下一次定位的估计初值q'；至此通过多目标六自由度电磁定位方法的步骤(三)计算出每个信号接收传感器的空间位置(x,y,z)及姿态角