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

一种多目标六自由度电磁定位装置和方法
【专利摘要】本发明涉及一种多目标六自由度电磁定位装置和方法，属于电磁定位装置和定位方法。装置由信号发射单元、信号接收单元、控制与数据处理单元组成，据定位需要，选择信号接收传感器的数量，并固定在定位目标上，接收传感器通过电磁感应，产生接收信号，控制与数据处理单元接收采样信号，执行电磁定位算法。本发明解决了现有装置对空间中大范围移动的目标跟踪定位能力较差的问题，优化定位方法，简化电路，提高了定位效率，并解决了六自由度求解时，位置参数、姿态参数求解过程相互影响造成的误差的问题。
【专利说明】
一种多目标六自由度电磁定位装置和方法
技术领域
[0001] 本发明涉及一种电磁定位装置和定位方法，具体涉及一种支持多个信号接收传感 器同时工作的电磁定位装置和一种并行求解目标位置及姿态的电磁定位方法。
【背景技术】
[0002] 随着科技的不断发展，定位技术在辅助医疗、人机交互、运动检测分析等领域应用 广泛。人体运动过程中，肢体会做出旋转、弯曲等动作，定位目标可能被身体其他部分遮挡， 这些问题使研究对定位技术有很高的要求。与超声式、光电式等定位装置相比，基于六自由 度电磁定位技术设计的装置具有速度快、成本低、便携性好等优点。六自由度电磁定位装置 追踪目标的运动轨迹的同时，还可以对目标姿态精确测量，最重要的是定位不受障碍物遮 挡影响。这些优点使多目标六自由度电磁定位装置可以很好的满足研究需求。
[0003] 电磁定位因为其独特的优点一直受到广泛关注。刘越、王涌天等人于2000年提出 了一种旋转矩阵算法，实现了六自由度电磁定位的参数计算，但算法计算复杂。李雪丽、王 毅等人在题为"电磁定位系统中的定位算法研究"的文中提出了一种利用四元数迭代计算 的方法定位，但算法脱离实际装置，定位方法不够完善。孙晓颖、陈明智等人在题为"一种用 于电磁定位的装置和方法"的专利中，提出了一种六自由度电磁定位装置和四元数同时求 解目标位置参数及姿态参数的算法，解决定位装置实时性差、定位算法中存在的奇点问题， 但装置定位局限于单目标，装置中滤波放大电路设计不够完善，使装置对空间中大范围移 动的目标跟踪定位能力较差，算法同时求解位置参数、姿态参数，计算结果相互影响产生误 差。

【发明内容】

[0004] 本发明提供一种多目标六自由度电磁定位装置和方法，以解决现有电磁定位装置 局限于单目标、对空间中大范围移动的目标跟踪定位能力较差的问题。采用优化定位方法， 简化电路，提高了定位效率，并解决了六自由度求解时，位置参数、姿态参数求解过程相互 影响造成的误差的问题。
[0005] 本发明采用的技术方案是：一种多目标六自由度电磁定位装置，由以下三部分组 成:信号发射单元、信号接收单元、控制与数据处理单元；
[0006] (1)所述信号发射单元由：信号发射传感器、滤波放大电路一、数模转换电路顺序 连接，控制与数据处理单元为信号发射单元提供发射信号，经数模转换电路、滤波放大电路 一生成驱动信号，经信号发射传感器，形成电磁场；
[0007] (2)所述信号接收单元由：至少一个信号接收传感器、第一级时分复用电路、滤波 放大电路二、第二级时分复用电路、模数转换电路顺序连接，其中滤波放大电路二的增益是 可调节的，信号接收传感器感应到电磁场产生电压信号，信号经多路复用、滤波放大后，由 模数转换电路对其采样，并传送给控制与数据处理单元；
[0008] (3)所述控制与数据处理单元:包括DSP、SDRAM存储器、FLASH存储器、USB接口及电 源，DSP读取FLASH存储器中的程序，存入DSP内置RAM中，一方面产生控制电路的信号，另一 方面接收采样信号并处理，计算过程中大容量数据会存储到SDRAM中，通过USB接口实现装 置与上位机通信，电源为装置提供±5V、3.3V电压。
[0009] 本发明所述信号发射单元中信号发射传感器由正交的三轴环形线圈组成，控制与 数据处理单元为模数转换电路提供三组发射信号，经滤波放大电路后，驱动三轴线圈同时 工作，发射信号采用正弦信号的形式，频率按照fn = knfs的标准设计，其中knS固定的整数， 11=1、2、3，匕为装置的定位刷新频率，根据这种标准生成的发射信号，在一个工作周期内呈 三组固定值，故不再需要参考信号回流，能够简化电路，提高定位效率。
[0010] 本发明所述信号接收单元中信号接收传感器由正交的三轴环形线圈组成，装置可 支持一个以上信号接收传感器同时工作，当置于信号发射单元产生的电磁场中，每个信号 接收传感器都能接收到三轴信号，第一级时分复用电路将传感器的三轴信号复用，经滤波 放大电路二后，第二级时分复用电路将多个传感器的信号复用，经模数转换电路采样后传 送给控制与数据处理单元。
[0011] 本发明所述控制与数据处理单元，包括：
[0012] (1)控制与数据处理单元根据信号接收传感器的数量，为第一级时分复用电路、第 二级时分复用电路提供控制信号；
[0013] (2)控制与数据处理单元为信号发射单元提供发射信号，为模数转换电路提供采 样信号，这些信号及控制信号均由同一时钟信号生成，保证时钟同步；
[0014] (3)控制与数据处理单元接收采样信号，求解目标的六自由度信息，并通过USB接 口传送给上位机程序。
[0015] 本发明所述控制与数据处理单元、滤波放大电路二、第二级时分复用电路、模数转 换电路构成反馈控制系统，用于调节滤波放大电路二的增益。
[0016] -种用于多目标六自由度电磁定位的方法，包括如下步骤：
[0017] ( - )根据定位需要，选择信号接收传感器的数量，并固定在定位目标上，将信号发 射传感器位置固定，作为空间的坐标原点（〇,〇,〇)，开始定位，由控制与数据处理单元产生 发射信号，经数模转换电路、滤波放大电路，驱动信号发射传感器，以信号发射传感器为中 心，形成电磁场；
[0018] (二)接收传感器通过电磁感应，产生接收信号，并通过第一级时分复用电路将来 自一个信号接收传感器的多路信号复用，信号经滤波放大后，经第二级时分复用电路对多 个信号接收传感器的多路信号复用，由模数转换电路采样，并传送给控制与数据处理单元， 滤波放大电路二、时分复用电路二、模数转换电路、控制与数据处理单元构成反馈控制系 统，通过输入信号的能量调节滤波放大电路二的增益，保证目标在大范围移动时接收信号 稳定，按照电磁场强度与距离的衰减规律，将定位空间划分成若干个区域，并为每个区域设 定相应的增益等级，依次为心、Kd s;
[0019] (三)控制与数据处理单元接收采样信号，执行电磁定位算法，步骤如下：
[0020] (1)首先按照时钟信号，从复用信号x(n)中将每个信号接收传感器的三轴信号还 原，组成信号接收矩阵L x，其中x表示传感器的编号，Lx是一个NX3的矩阵，表示矩阵有3个阵 元，N个时间样本序列，利用同步解调的方法，提取接收信号中三个频率的成分，其中解调信 号为三组固定值 81、沿、抑，解调后三组信号通过低通滤波、拟合等方法组成接收矩阵3\;
[0021] (2)利用算法将接收矩阵Sx中的位置参数、姿态参数分离，构造出两个矩阵X x、Yx， 矩阵Xx只包含目标的位置参数，矩阵Yx只包含目标的姿态参数，参数分离后可以对位置参 数、姿态参数并行求解，结果互不影响；
[0022] (3)利用闭环、迭代的四元数算法同时求解编号为x的传感器的位置参数(x，y，z) 和姿态参数(〇，w， (p)，并将结果通过USB接口传送给上位机程序。
[0023] 本发明所述一种多目标六自由度电磁定位的方法，所述步骤(二）中的反馈控制系 统负责调节滤波放大电路二的增益；
[0024]复用信号经滤波放大电路、采样后，传送到数据处理单元，数据处理单元根据时钟 信号将采样信号还原，组成信号接收矩阵Lx;
[0025] 求Lx的协方差矩阵T:
[0026] T==LX^
[0027]求解矩阵T的迹，记为tr(T)，进行阈值判断，如果tr(T)的值低于tr(T)min将增益的 等级提高一级，如果tr(T)的值高于tr(T)max，则降低增益等级，该系统增益控制存在一个定 位周期的延迟；
[0028] 按照电磁场强度与距离的衰减规律，将定位空间划分成若干个区域，并为每个区 域设定相应的增益等级，依次为心、Kds;
[0029] 实际上，信号的能量只与目标的距离有关，所以可以认为接收信号的能量与目标 的距离是等价的，故可以利用tr(T)去计算目标的距离R，R的误差也能直接反应算法对接收 信号能量的估计是否准确；
[0030] R=R [tr(T )/tr(T)]<'
[0031] 其中Rs和tr(Ts)是对应增益等级Ks的一组常数；
[0032] 本发明所述一种多目标六自由度电磁定位的方法，所述步骤(三）中的电磁定位算 法，具体实现步骤如下：
[0033] 为方便描述，后文省略了矩阵1^、5\4\、￥\中的传感器编号1，用1、54、￥表示；
[0034] (1)还原复用信号，组建接收矩阵S;
[0035] 控制与数据处理单元会根据时钟信号，将复用信号x(n)还原，并按照顺序存入信 号接收矩阵L中；
[0036] L=[li,l2,l3]T
[0037]对矩阵L中3个阵元的信号做同步解调，解调信号用gm表示，m=l、2、3对应三轴发 射线圈，按照fn = knfs的标准选择复用信号频率，gm为三组固定值，同步解调计算公式如下：
[0038] lmn=lngm
[0039] 经同步解调后，结果lmn是由m轴产生n轴接收的信号，lmn组成Lmn，L mn-个3 X 3 X 1数 组，对数组做低通滤波、拟合处理后得到传感器对应的接收矩阵S;
[0040] 接收矩阵S与发射矩阵I的耦合关系可以利用旋转矩阵来表示；
[0041] 定位过程中，信号耦合关系可以通过旋转矩阵的方式描述，位置参数可以表示为 两个旋转矩阵变换和一个位移的结果，位移距离R，绕Z轴顺时针旋转角度为a，绕X轴顺时针 旋转角度为0，旋转矩阵P如下所示： cos[3 0 -sin(3 coscx sincx 0
[0042] 0 1 0 -siiia eosa 0 sin (3 0 eosp 0 0 1
[0043]同理，姿态可以通过三个旋转矩阵来表示，其中绕Z轴顺时针旋转角度为0、表示绕 X轴顺时针旋转角度C0，表示绕Y轴顺时针旋转角度9，旋转矩阵Q如下所示： 1 0 0 cos(p 0 -sirup coso) siruo 0
[0044] Q= 0 c〇s0 sin0 0 10 -sina) cos? 0 0 -sinl cosB sincp 0 c〇s(p 0 0 1
[0045] 根据磁偶极子原理可以求得信号发射传感器、信号接收传感器的耦合矩阵H，如下 所示： '2 Q 0"
[0046] H= 0 -1 0 0 0 -1
[0047] 综上所述，若发射矩阵为单位矩阵I，则接收矩阵S可以用如下公式表示：
[0048] S = kQPtHPI
[0049] 式中k =电磁耦合系数/衰减因子；
[0050] (2)接收矩阵中位置参数与姿态参数分离；
[0051]求接收矩阵S的协方差矩阵；
[0052] Sts = k2ptH2p
[0053]求解左侧矩阵和右侧矩阵的迹，可以得出如下结果：
[0054] tr(StS)=6k2
[0055] 整理得：
[0056] k=[tr(StS)/6](1/2)
[0057] 距离参数R只与衰减因子k相关，可以通过下式求解；
[0058] R = Rs(ks/k)(1/3)
[0059]公式中，ks、Rs是目标处于增益等级Ks区域时对应的参数；
[0060]利用解得的k值计算S的归一化矩阵，用M表示，如下所示：
[0061] M = QptHP
[0062] 将矩阵M中的两类参数分离出来，只含有位置参数的矩阵X可以按如下公式计算：
[0063] X = PtFP=(MtM-I)/3
[0064] 式中F = dig(l，0,0);
[0065] 只含有姿态参数的矩Y以通过下式求得：
[0066] 丫 = 〇、=]?%-21
[0067] (3)求解目标的位置参数(x，y，z)及姿态参数(0,〇)，的；
[0068]四元数是一种超复数，一个四元数c由一个实数单位1和三个虚数单位i、j、k组成， 形式如下czco+cd+uj+csk，与旋转矩阵相比，四元数描述目标的方向、姿态更加方便； [0069]下面利用闭环迭代四元数算法，求解目标的位置参数及姿态参数，两种参数的迭 代计算过程互不影响，所以可以同时进行；
[0070] 将含有位置参数的矩阵X四元数的方式表示，如下所示：
[0071] X=(MtM-I)/3 = p(FP)p_1
[0072] 式中p位置参数的最佳四元数估计值，p = p〇+pii+p2j+p3k;
[0073] 最佳四元数估计值与估计初值的关系为p = p'c，c为误差四元数，可表示为c = l + cii+C2j+C3k，显然，当误差四元数矢量部分的值趋近于0时，估计初值p'与最佳四元数估计 值P非常接近；
[0074] 首先，在X矩阵的左右分别乘上(p'r\p'，求解得到一个包含误差四元数和位置参 数的矩阵，计算公式如下所示：
[0075] Xi=(p，)_1Xp，=c(FP)c _1=(l-2C)(FP)
[0076] 公式中矩阵C为： 0. g3 -Cj
[0077] C= -C3 0 Cj :c2 ， 0 _
[0078] 求心矩阵的转置矩阵；
[0079] X； =p'[F(I+2C)](pT'
[0080] 对X『做和X矩阵同样的变换；
[0081] (pT!X1ip'=(I-2C)F(1+2C) =F+2FC-2CF-4CFC
[0082]整理上式结果，由于4CFC的值非常小，故只保留等式右侧的前三项，又因为位置参 数不存在沿Y轴旋转的角度，所以误差四元数C1 = C2C3,近似为0,公式右侧等式整理如下所 示： I 2ca -2c.2"
[0083] 2c3 0 0 -2c2 0 0
[0084] 通过上式可以求解出估计四元数c的值，判断误差四元数c是否满足定位精度要 求，满足则可以认为P'就是最佳估计值，如不满足，则可以利用误差四元数c求解一个新的 估计初值，p n?代替P '重复迭代过程；
[0085] Pnew = p'c
[0086] 位置参数a，0与最佳估计四元数p的关系如下所示：
[0087] a = arcsin2(pip3+p〇p2)
[0088] p=-arctan[2( p,p, -p.p, )/(p,；+pf -p； +p；)]
[0089]在算法第一次运行时，初始估计值选择ps，ps为目标a = 45°、P = 45°时四元数理论 值，之后可以选择上一次计算的最佳估计值P作为下一次定位的估计初值P ' ；
[0090] 至此目标的极坐标形式的位置参数(R，a，e)已经全部求出，可以通过如下公式将 目标的位置参数由极坐标形式(R，a，0)转换成直角坐标形式(x，y，z);
[0091] x = Rcos(a)cos(0)
[0092] y = Rsin(a)cos(0)
[0093] z = Rsin(0)
[0094]利用姿态角的最佳四元数估计值q表示矩阵Y，q = qo+qii+q2j+q3k;
[0095] Y = qMq-1
[0096] 利用估计初值q'对矩阵Y进行一次变换求解误差四元数u，最佳四元数估计值与估 计初值的关系为q = q'u，其中u为误差四元数u=l+uii+U2j+U3k，显然，当误差四元数矢量部 分ui、U2、U3的值趋近于0时，姿态四元数估算值q '的值与最佳估计值q非常接近；
[0097] 与X矩阵的计算过程类似，对Y的变换如下所示：
[0098] (q，）-Wq，=uMu-1= (I_2U)M [0099]其中U为误差四元数组成的矩阵； G u3 -u3
[0100] U= -u3 0 u, u2 -u, 0
[0101] 整理上式，可以求解UM为：
[0102] UM=[M-(a，)_1Ya，]/2 = D
[0103] 将上述公式写成如下形式： 0 m1；! m13]「d" d12 d13
[0104] -u3： 0 Uj m21 m22 = d2l. d22 d23 _ % ， 0」Lm3:1 m32 m33」d32
[0105] 从以上9组等式中选择三组独立的等式，写成如下形式： 0 -m31 .m21「4 「.dn
[0106] m32 0 -ml2 u2 = d12 _-m23 i?t3 〇 1叫」
[0107]将上式写成如下形式：
[0108] M'u=d
[0109] 通过上式可以求解出估计四元数u的值，之后判断四元数u是否满足精度要求，如 果误差四元数u满足定位要求，则可以认为q'就是最佳估计值，不满足精度要求，则可以利 用误差四元数U求解一个新的估计初值q new，qnew取代之前的估计初值q'重复迭代过程，利用 误差四元数计算估计初值的公式如下所示：
[0110] qnew=q'u
[0111] 姿态参数9、《、9与最佳估计四元数q的关系如下所示：
[0115] 在算法第一次运行时，初始估计值选择qs，qs为目标 9 = 45°、〇 =45°、tp=45_°__l时四 元数理论值，之后可以选择上一次计算的最佳估计值q作为下一次定位的估计初值q' ；
[0116] 至此通过多目标六自由度电磁定位方法的步骤(三)计算出每个信号接收传感器 的空间位置(x，y，z)及姿态角.(〇,w，(p)。
[0117] 本发明具有以下优点：
[0118] (1)现有的电磁定位装置局限于单目标定位，本发明采用二级时分复用电路对多 个传感器的多路信号时分复用，采用多目标六自由度电磁定位算法，使装置可以同时对一 个以上目标跟踪定位。
[0119] (2)现有的电磁定位装置，需要引入参考信号回流，本发明采用fn = knfs的标准设 计发射信号，定位过程中不再需要参考信号回流，简化信号接收单元电路，将定位效率提高 33%〇
[0120] (3)现有的电磁定位装置，对大范围移动的目标跟踪定位不够准确，本发明采用反 馈控制系统调节滤波放大电路二的增益，保证目标在大范围移动时接收信号稳定，利用信 号接收矩阵L x协方差矩阵的迹，估计接收信号能量的算法，这种算法过程简便，计算量小， 对信号能量估计准确。
[0121 ] (4)本发明提出一种多目标六自由度电磁定位算法，将接收矩阵Sx*的位置参数、 姿态参数分离，组成矩阵Xx、Yx，之后对Xx、Yx分别求解，因为X x只含有位置参数的信息、Yx中 只含有姿态参数的信息，所以这种方法可以避免求解过程中两种参数互相影响产生的误 差。
[0122] (5)本发明采用两种参数的计算过程同时进行，采用闭环迭代四元数算法求解目 标的位置参数与姿态参数，大幅度提高定位效率。
【附图说明】
[0123] 图1是多目标六自由度电磁定位装置原理图；
[0124] 图2是控制与数据处理单元工作原理图；
[0125] 图3A是单个信号接收传感器工作时，时分复用电路的控制信号；
[0126] 图3B是十六个信号接收传感器同时工作时，时分复用电路的控制信号；
[0127] 图4是反馈控制系统原理图；
[0128] 图5是按增益等级划分的定位区域；
[0129] 图6是多目标六自由度电磁定位算法流程图；
[0130] 图7是闭环四元数迭代算法计算位置参数流程图；
[0131 ]图8是闭环四元数迭代算法计算姿态参数流程图；
[0132] 图9是tr(T)估计接收信号能量实验中选取的位置示意图；
[0133] 图10是位置一二三的实验结果图；
[0134] 图11是位置四五六的实验结果图；
[0135] 图12是位置七八九的实验结果图。
【具体实施方式】
[0136] -种多目标六自由度电磁定位装置，由以下三部分组成:信号发射单元1、信号接 收单元2、控制与数据处理单元3，如图1所示；
[0137] (1)所述信号发射单元1由：信号发射传感器11、滤波放大电路一12、数模转换电路 13顺序连接，控制与数据处理单元3为信号发射单元1提供发射信号，经数模转换电路13、滤 波放大电路一 12生成驱动信号，经信号发射传感器11，形成电磁场；
[0138] (2)所述信号接收单元2由：至少一个信号接收传感器21、第一级时分复用22电路、 滤波放大电路二23、第二级时分复用电路24、模数转换电路顺序连接25，其中滤波放大电路 二23的增益是可调节的，信号接收传感器21感应到电磁场产生电压信号，信号经多路复用、 滤波放大后，由模数转换电路25对其采样，并传送给控制与数据处理单元3;
[0139] (3)所述控制与数据处理单元3,其组成及工作原理如图2所示:包括DSP301、
[0140] 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语言编程效率，能够实现复杂的算法；
[0141] -种多目标六自由度电磁定位装置中的信号发射单元1:所述信号发射传感器11 由正交的三轴环形线圈组成，控制与数据处理单元3为模数转换电路13提供三组发射信号， 经滤波放大电路12后，驱动三轴线圈同时工作，发射信号采用正弦信号的形式，频率按照f n = knfs的标准设计，其中1^为固定的整数，11 = 1、2、3，匕为装置的定位刷新频率，根据这种标 准生成的发射信号，在一个工作周期内呈三组固定值，故不再需要参考信号回流，能够简化 电路，提尚定位效率。
[0142] -种多目标六自由度电磁定位装置中的信号接收单元2,所述信号接收传感器21 由正交的三轴环形线圈组成，装置可支持一个以上信号接收传感器同时工作，当置于信号 发射单元产生的电磁场中，每个信号接收传感器21都能接收到三轴信号，第一级时分复用 电路22将传感器的三轴信号复用，经滤波放大电路二23后，第二级时分复用电路24将多个 传感器的信号复用，经模数转换电路25采样后传送给控制与数据处理单元3。
[0143] -种多目标六自由度电磁定位装置中的控制与数据处理单元3,包括：
[0144] (1)控制与数据处理单元3根据信号接收传感器21的数量，为第一级时分复用电路 22、第二级时分复用24电路提供控制信号，如图3A、B所示，其中PWM1A、PMW1B为第一级时分 复用电路22的控制信号，PWM2A、PMW2B为第二级时分复用电路24的控制信号；
[0145] (2)控制与数据处理单元3为信号发射单元1提供发射信号，为模数转换电路13提 供采样信号，这些信号及控制信号均由同一时钟信号生成，保证时钟同步；
[0146] (3)控制与数据处理单元3接收采样信号，求解目标的六自由度信息，并通过USB接 口 304传送给上位机程序。
[0147] 所述的多目标六自由度电磁定位装置，其中控制与数据处理单元3、滤波放大电路 二23、第二级时分复用电路24、模数转换电路25构成反馈控制系统，如图4所示，用于控制滤 波放大电路二23的增益，系统有多路输入信号X x(t)，多路信号分别由运算放大电路二23放 大kx倍后，经第二级时分复用电路24,整合成信号x(t)，模数转换电路25采样后，以x(n)的 形式传递给控制与数据处理单元3,控制与数据处理单元3首先根据时钟信号，将复用信号 还原，并存入对应的接收矩阵L x中，求Lx的协方差矩阵T，求解矩阵T的迹，记为tr(T)，进行阈 值判断，如果tr(T)的值低于tr(T)min将增益的等级提高一级，如果tr(T)的值高于tr(T) max， 则降低增益等级，该系统增益控制存在一个定位周期的延迟。
[0148] 按照电磁场强度与距离的衰减规律，将定位空间划分成若干个区域，并为每个区 域设定相应的增益等级，依次为心、Kds，如图5所示。
[0149] -种用于多目标六自由度电磁定位的方法，过程如图6所示，包括如下步骤：
[0150] ( - )根据定位需要，选择信号接收传感器的数量，并固定在定位目标上，将信号发 射传感器位置固定，作为空间的坐标原点(〇,〇,〇)，开始定位，由控制与数据处理单元3产生 发射信号，经数模转换电路13、滤波放大电路12,驱动信号发射传感器11，以信号发射传感 器为中心，形成电磁场；
[0151] (二)接收传感器21通过电磁感应，产生接收信号，并通过第一级时分复用电路22 将来自一个信号接收传感器21的多路信号复用，信号经滤波放大后，经第二级时分复用电 路24对多个信号接收传感器的多路信号复用，由模数转换电路25采样，并传送给控制与数 据处理单元3,滤波放大电路二23、时分复用电路二24、模数转换电路25、控制与数据处理单 元3构成反馈控制系统，通过输入信号的能量调节滤波放大电路二的增益，保证目标在大范 围移动时接收信号稳定，按照电磁场强度与距离的衰减规律，将定位空间划分成若干个区 域，并为每个区域设定相应的增益等级，依次为心、Kd s;
[0152](三)控制与数据处理单元3接收采样信号，执行电磁定位算法，过程如图6所示，控 制与数据处理单元会根据时钟信号，将复用信号x(n)还原，并按照顺序存入信号接收矩阵 Lx中，从矩阵Lx中提取接收矩阵Sx，之后求解衰减因子k，并求解归一化矩阵M，将M矩阵中的 位置参数、姿态参数分离，构成矩阵X x、矩阵Yx，对两矩阵同时进行四元数迭代计算，求解目 标的位置参数(x，y，z)姿态参数(〇，w， (p)。
[0153] -种多目标六自由度电磁定位的方法，步骤(二）中的反馈控制系统负责调节滤波 放大电路二的增益；
[0154]复用信号经滤波放大电路、采样后，传送到数据处理单元，数据处理单元根据时钟 信号将采样信号还原，组成信号接收矩阵Lx;
[0155] 求Lx的协方差矩阵T:
[0156] T=LxLtx
[0157] 求解矩阵T的迹，记为tr(T)，进行阈值判断，如果tr(T)的值低于tr(T)min将增益的 等级提高一级，如果tr(T)的值高于tr(T) max，则降低增益等级，该系统增益控制存在一个定 位周期的延迟；
[0158] 按照电磁场强度与距离的衰减规律，将定位空间划分成若干个区域，并为每个区 域设定相应的增益等级，依次为心、Kds;
[0159] 实际上，信号的能量只与目标的距离有关，所以可以认为接收信号的能量与目标 的距离是等价的，故可以利用tr(T)去计算目标的距离R，R的误差也能直接反应算法对接收 信号能量的估计是否准确；
[0160] [tr(T )/tr(T)]<'
[0161] 其中Rs和tr(Ts)是对应增益等级Ks的一组常数；
[0162] -种多目标六自由度电磁定位的方法，步骤(三）中的电磁定位算法，具体实现步 骤如下：
[0163] 为方便描述，后文省略了矩阵1^、5\、乂\、￥\中的传感器编号^用1、5、乂、￥表示；
[0164] (1)还原复用信号，组建接收矩阵S;
[0165] 控制与数据处理单元会根据时钟信号，将复用信号x(n)还原，并按照顺序存入信 号接收矩阵L中；
[0166] L=[li，l2，l3]T
[0167] 对矩阵L中3个阵元的信号做同步解调，解调信号用gm表示，m=l、2、3对应三轴发 射线圈，按照fn = knfs的标准选择复用信号频率，gm为三组固定值，同步解调计算公式如下：
[0168] lmn=lngm
[0169] 经同步解调后，结果lmn是由m轴产生n轴接收的信号，lmn组成L mn，Lmn-个3 X 3 X 1数 组，对数组做低通滤波、拟合处理后得到传感器对应的接收矩阵S;
[0170] 接收矩阵S与发射矩阵I的耦合关系可以利用旋转矩阵来表示；
[0171]定位过程中，信号耦合关系可以通过旋转矩阵的方式描述，位置参数可以表示为 两个旋转矩阵变换和一个位移的结果，位移距离R，绕Z轴顺时针旋转角度为a，绕X轴顺时针 旋转角度为0，旋转矩阵P如下所示： cosp 0 -sinj3 cosa sina 0
[0172] P= 0 10 -sina cosa 0 smj3 0 cosp 0 0 1
[0173]同理，姿态可以通过三个旋转矩阵来表示，其中绕Z轴顺时针旋转角度为0、表示绕 X轴顺时针旋转角度co，表示绕Y轴顺时针旋转角度9，旋转矩阵Q如下所示： 1 〇 0 coscp 〇 -sintp coso) sin? 0
[0174] Q= 0 eosO sin0 0 1 0 -sin? msm 0 ：0 -sin0 cosB sirup 0 coscp 0 0 1
[0175] 根据磁偶极子原理可以求得信号发射传感器、信号接收传感器的耦合矩阵H，如下 所示： ~2 〇 0"
[0176] H= 0 -1 0 0 0 4
[0177] 综上所述，若发射矩阵为单位矩阵I，则接收矩阵S可以用如下公式表示：
[0178] S = kQPtHPI
[0179] 式中k =电磁耦合系数/衰减因子；
[0180] (2)接收矩阵中位置参数与姿态参数分离；
[0181 ]求接收矩阵S的协方差矩阵；
[0182] sts = k2ptH2p
[0183] 求解左侧矩阵和右侧矩阵的迹，可以得出如下结果：
[0184] tr(StS)=6k2
[0185] 整理得：
[0186] k=[tr(StS)/6](1/2)
[0187] 距离参数R只与衰减因子k相关，可以通过下式求解；
[0188] R = Rs(ks/k)(1/3)
[0189] 公式中，ks、Rs是目标处于增益等级Ks区域时对应的参数；
[0190] 利用解得的k值计算S的归一化矩阵，用M表示，如下所示：
[0191] M = QptHP
[0192] 将矩阵M中的两类参数分离出来，只含有位置参数的矩阵X可以按如下公式计算：
[0193] X = PtFP=(MtM-I)/3
[0194] 式中 F = dig(l，0,0);
[0195] 只含有姿态参数的矩Y以通过下式求得：
[0196] 丫 = 〇、=]?%-21
[0197] (3)求解目标的位置参数(x，y，z)及姿态参数((U〇,q>):
[0198] 四元数是一种超复数，一个四元数c由一个实数单位1和三个虚数单位i、j、k组成， 形式如下czco+cd+uj+csk，与旋转矩阵相比，四元数描述目标的方向、姿态更加方便；
[0199] 下面利用闭环迭代四元数算法，求解目标的位置参数及姿态参数，两种参数的迭 代计算过程互不影响，所以可以同时进行；
[0200] 求解位置参数的过程如图7所示，首先，在X矩阵的左右分别乘上估计初值p'和 (P'）-1，求解得到一个包含误差四元数和位置参数的矩阵Xi，求乂:矩阵的转置矩阵，对XI做 和X矩阵同样的变换，可以求解出误差四元数c的值，判断误差四元数c是否满足定位精度要 求，满足则可以认为P'就是最佳估计值，求解出目标的位置参数，如不满足，则可以利用误 差四元数c求解一个新的估计初值p new，pnew代替p'重复迭代过程，在算法第一次运行时，初 始估计值选择Ps，p s为目标a = 45°、0 = 45°时四元数理论值，之后可以选择上一次计算的最 佳估计值P作为下一次定位的估计初值P ' ；
[0201] 首先，在X矩阵的左右分别乘上(p'r\p'，求解得到一个包含误差四元数和位置参 数的矩阵，计算公式如下所示：
[0202] Xi=(p，)_1Xp，=c(FP)c _1=(l-2C)(FP)
[0203]公式中矩阵C为： 0 c：j -c,
[0204] C= -c3. 0 c2 -Cj 0
[0205]求心矩阵的转置矩阵；
[0206] X； =p'[F(i+2C)](pT'
[0207] 对X〗做和X矩阵同样的变换；
[0208] (PTlX；p'=(l-2C)F(l i-2C) =F K2FC-2CF-4CF(;
[0209]整理上式结果，由于4CFC的值非常小，故只保留等式右侧的前三项，又因为位置参 数不存在沿Y轴旋转的角度，所以误差四元数C1 = C2C3,近似为0,公式右侧等式整理如下所 示： 1 2c3 -2c^
[0210] 2c3 0 0 -2c2 0 0
[0211] 通过上式可以求解出估计四元数c的值，判断误差四元数c是否满足定位精度要 求，满足则可以认为P'就是最佳估计值，如不满足，则可以利用误差四元数C求解一个新的 估计初值，Pn?代替P '重复迭代过程；
[0212] pnew = p'c
[0213] 位置参数a，0与最佳估计四元数p的关系如下所示：
[0214] a = arcsin2(pip3+p〇p2)
[0215] p=-arctan[2(p, p, -p,p(i VXp,1; +pf -p；； +p：)]
[0216]在算法第一次运行时，初始估计值选择ps，ps为目标a = 45°、0 = 45°时四元数理论 值，之后可以选择上一次计算的最佳估计值P作为下一次定位的估计初值P ' ；
[0217] 至此目标的极坐标形式的位置参数(R，a，P)已经全部求出，可以通过如下公式将 目标的位置参数由极坐标形式(R，a，0)转换成直角坐标形式(x，y，z);
[0218] x = Rcos(a)cos(0)
[0219] y = Rsin(a)cos(0)
[0220] z = Rsin(0)
[0221]利用姿态角的最佳四元数估计值q表示矩阵Y，q = qo+qii+q2j+q3k;
[0222] Y = qMq-1
[0223] 利用估计初值q'对矩阵Y进行一次变换求解误差四元数u，最佳四元数估计值与估 计初值的关系为q = q'u，其中u为误差四元数u=l+uii+U2j+U3k，显然，当误差四元数矢量部 分ui、U2、U3的值趋近于0时，姿态四元数估算值q '的值与最佳估计值q非常接近；
[0224] 求解姿态参数的过程如图8所示，在Y矩阵的左右分别乘上估计初值q'和(q'r1，求 解出矩阵D的值，之后利用M矩阵构造出等式通过上式可以求解出估计四元数u的 值，判断四元数u是否满足精度要求，如果误差四元数u满足定位要求，则可以认为q'就是最 佳估计值，求出目标的姿态参数，不满足精度要求，则可以利用误差四元数u求解一个新的 估计初值q new，qnew取代之前的估计初值q '重复迭代过程，在算法第一次运行时，初始估计值 选择qs，qs为目标9 = 45°、《 =45°、cp=45°时四元数理论值，之后可以选择上一次计算的最 佳估计值q作为下一次定位的估计初值q '。
[0225] 详细过程如下：
[0226] 利用姿态角的最佳四元数估计值q表示矩阵Y，q = qo+qii+q2j+q3k;
[0227] Y = qMq-1
[0228] 利用估计初值q'对矩阵Y进行一次变换求解误差四元数u，最佳四元数估计值与估 计初值的关系为q = q'u，其中u为误差四元数u=l+uii+U2j+U3k，显然，当误差四元数矢量部 分ui、U2、U3的值趋近于0时，姿态四元数估算值q '的值与最佳估计值q非常接近；
[0229] 与X矩阵的计算过程类似，对Y的变换如下所示：
[0230] (q，）-Wq，=uMu-1= (I_2U)M [0231 ]其中U为误差四元数组成的矩阵； 0 u3 -u2
[0232] U= -Uj 0 ll] u2 -u, 0
[0233] 整理上式，可以求解UM为：
[0234] UM=[M-(a，)_1Ya，]/2 = D
[0235] 将上述公式写成如下形式： '〇 % 'Ymn m 12 「du dH ci 13_
[0236] -u3 0 m2J m22 m23 = d21 dn d23 u2 -xi{ 0 nx3i m32 i%3_ _木1 辻尨 d33_
[0237] 从以上9组等式中选择三组独立的等式，写成如下形式： 0 -m3t m2l ux dn
[0238] m32 0 -mI2 u2 = d12 _-m23 m13 0」[u3」Ltl13_
[0239] 将上式写成如下形式：
[0240] M'u=d
[0241] 通过上式可以求解出估计四元数u的值，之后判断四元数u是否满足精度要求，如 果误差四元数u满足定位要求，则可以认为q'就是最佳估计值，不满足精度要求，则可以利 用误差四元数U求解一个新的估计初值q new，qnew取代之前的估计初值q'重复迭代过程，利用 误差四元数计算估计初值的公式如下所示：
[0242] qnew=q'u
[0243] 姿态参数0、《与最佳估计四元数q的关系如下所示：
[0247] 在算法第一次运行时，初始估计值选择qs，qs为目标0 = 45°、《 =45°、cp=45fi时四 元数理论值，之后可以选择上一次计算的最佳估计值q作为下一次定位的估计初值q' ；
[0248] 至此通过多目标六自由度电磁定位方法的步骤(三)计算出每个信号接收传感器 的空间位置(x，y，z)及姿态角作典冲)。
[0249] 为验证反馈控制系统中算法对信号能量估计，将信号接收传感器置于空间中9个 不同的位置并进行了实验；
[0250]实验目的
[0251 ]验证反馈控制系统中算法对信号能量估计是否准确。
[0252] 实验原理
[0253]因为信号的能量只与目标的距离有关，所以可以认为接收信号的能量与目标的距 离是等价的，如果tr(T)可以精确地计算距离R，则证明利用tr(T)去估计信号的能量也会十 分精确，计算过程如下：
[0254] (1)复用信号经滤波放大电路、采样后，传送到数据处理单元，数据处理单元根据 时钟信号将采样信号还原，组成信号接收矩阵Lx;
[0255] (2)求Lx的协方差矩阵tr(T);
[0256] T=LxL^
[0257] (3)求T矩阵的迹；
[0258] tr(T)=Tii+T22+T33
[0259] (4)求解距离R;
[0260] R=R^tr(T)/tr(T)]'-
[0261] 其中Rs和tr(Ts)是对应增益等级Ks的一组常数。
[0262] 实验过程
[0263] (1)选择空间中的9个位置，9个位置的距离逐渐增加，空间分布如图9所示，在每个 位置测量5次信号。
[0264] (2)利用原理中的算法计算该位置的距离R，并与实际的距离相比较。
[0265] 实验结果
[0266] 定位结果
[0269]图10是位置一二三的实验结果。
[0270]图11是位置四五六的实验结果。
[0271 ]图12是位置七八九的实验结果。
[0272] 结论
[0273] 9个位置的45组数据测量的距离R与实际的距离相比，最大误差不超过0.2cm，故可 以认为，验证反馈控制系统中算法对信号能量估计是准确的。
【主权项】
1. 一种多目标六自由度电磁定位装置，其特征在于包括：信号发射单元、信号接收单 元、控制与数据处理单元； (1) 所述信号发射单元由：信号发射传感器、滤波放大电路一、数模转换电路顺序连接， 控制与数据处理单元为信号发射单元提供发射信号，经数模转换电路、滤波放大电路一生 成驱动信号，经信号发射传感器，形成电磁场； (2) 所述信号接收单元由：至少一个信号接收传感器、第一级时分复用电路、滤波放大 电路二、第二级时分复用电路、模数转换电路顺序连接，其中滤波放大电路二的增益是可调 节的，信号接收传感器感应到电磁场产生电压信号，信号经多路复用、滤波放大后，由模数 转换电路对其采样，并传送给控制与数据处理单元； (3) 所述控制与数据处理单元:包括DSP、SDRAM存储器、FLASH存储器、USB接口及电源， DSP读取FLASH存储器中的程序，存入DSP内置RAM中，一方面产生控制电路的信号，另一方面 接收采样信号并处理，计算过程中大容量数据会存储到SDRAM中，通过USB接口实现装置与 上位机通信，电源为装置提供± 5V、3.3V电压。2. 根据权利要求1所述的一种多目标六自由度电磁定位装置，其特征在于:所述信号发 射单元中信号发射传感器由正交的三轴环形线圈组成，控制与数据处理单元为模数转换电 路提供三组发射信号，经滤波放大电路后，驱动三轴线圈同时工作，发射信号采用正弦信号 的形式，频率按照f n = knfs的标准设计，其中1^为固定的整数，11 = 1、2、3，匕为装置的定位刷 新频率，根据这种标准生成的发射信号，在一个工作周期内呈三组固定值，不再需要参考信 号回流。3. 根据权利要求1所述的一种多目标六自由度电磁定位装置，其特征在于:所述信号接 收单元中信号接收传感器由正交的三轴环形线圈组成，装置可支持一个以上信号接收传感 器同时工作，当置于信号发射单元产生的电磁场中，每个信号接收传感器都能接收到三轴 信号，第一级时分复用电路将传感器的三轴信号复用，经滤波放大电路二后，第二级时分复 用电路将多个传感器的信号复用，经模数转换电路采样后传送给控制与数据处理单元。4. 根据权利要求1所述的一种多目标六自由度电磁定位装置，其特征在于:所述控制与 数据处理单元，包括： (1) 控制与数据处理单元根据信号接收传感器的数量，为第一级时分复用电路、第二级 时分复用电路提供控制信号； (2) 控制与数据处理单元为信号发射单元提供发射信号，为模数转换电路提供采样信 号，这些信号及控制信号均由同一时钟信号生成，保证时钟同步； (3) 控制与数据处理单元接收采样信号，求解目标的六自由度信息，并通过USB接口传 送给上位机程序。5. 根据权利要求1所述的一种多目标六自由度电磁定位装置，其特征在于:所述控制与 数据处理单元、滤波放大电路二、第二级时分复用电路、模数转换电路构成反馈控制系统， 用于调节滤波放大电路二的增益。6. -种用于多目标六自由度电磁定位的方法，其特征在于包括如下步骤： (一)根据定位需要，选择信号接收传感器的数量，并固定在定位目标上，将信号发射传 感器位置固定，作为空间的坐标原点（〇,〇,〇)，开始定位，由控制与数据处理单元产生发射 信号，经数模转换电路、滤波放大电路，驱动信号发射传感器，以信号发射传感器为中心，形 成电磁场； (二) 接收传感器通过电磁感应，产生接收信号，并通过第一级时分复用电路将来自一 个信号接收传感器的多路信号复用，信号经滤波放大后，经第二级时分复用电路对多个信 号接收传感器的多路信号复用，由模数转换电路采样，并传送给控制与数据处理单元，滤波 放大电路二、时分复用电路二、模数转换电路、控制与数据处理单元构成反馈控制系统，通 过输入信号的能量调节滤波放大电路二的增益，保证目标在大范围移动时接收信号稳定， 按照电磁场强度与距离的衰减规律，将定位空间划分成若干个区域，并为每个区域设定相 应的增益等级，依次为KhKfKs; (三) 控制与数据处理单元接收采样信号，执行电磁定位算法，步骤如下： (1) 首先按照时钟信号，从复用信号X(n)中将每个信号接收传感器的三轴信号还原，组 成信号接收矩阵Lx，其中X表不传感器的编号，L x是一个NX 3的矩阵，表不矩阵有3个阵元，N 个时间样本序列，利用同步解调的方法，提取接收信号中三个频率的成分，其中解调信号为 三组固定值81、沿、抑，解调后三组信号通过低通滤波、拟合等方法组成接收矩阵34 (2) 利用算法将接收矩阵Sx中的位置参数、姿态参数分离，构造出两个矩阵Xx、Yx，矩阵X x 只包含目标的位置参数，矩阵Υχ只包含目标的姿态参数，参数分离后可以对位置参数、姿态 参数并行求解，结果互不影响； (3) 利用闭环、迭代的四元数算法同时求解编号为X的传感器的位置参数(x，y，z)和姿 态参数((Uwp)，并将结果通过USB接口传送给上位机程序。7. 根据权利要求6所述的一种用于多目标六自由度电磁定位的方法，其特征在于所述 步骤(二）中的反馈控制系统负责调节滤波放大电路二的增益； 复用信号经滤波放大电路、采样后，传送到数据处理单元，数据处理单元根据时钟信号 将采样信号还原，组成信号接收矩阵Lx; 求Lx的协方差矩阵T: 求解矩阵T的迹，记为tr(T)，进行阈值判断，如果tr(T)的值低于tr(T)min将增益的等级 提高一级，如果tr(T)的值高于tr(T)max，则降低增益等级，该系统增益控制存在一个定位周 期的延迟； 按照电磁场强度与距离的衰减规律，将定位空间划分成若干个区域，并为每个区域设 定相应的增益等级，依次为h、Kds; 实际上，信号的能量只与目标的距离有关，所以可以认为接收信号的能量与目标的距 离是等价的，故可以利用tr(T)去计算目标的距离R，R的误差也能直接反应算法对接收信号 能量的估计是否准确；其中Rs和tr(Ts)是对应增益等级Ks的一组常数。8. 根据权利要求6所述的一种用于多目标六自由度电磁定位的方法，其特征在于所述 步骤(三)中的电磁定位算法，具体实现步骤如下： 为方便描述，后文省略了矩阵Lx、Sx、Xx、Yx中的传感器编号X，用L、S、X、Y表示； (1)还原复用信号，组建接收矩阵S; 控制与数据处理单元会根据时钟信号，将复用信号x(n)还原，并按照顺序存入信号接 收矩阵L中； L= [ll, 12, l3]T 对矩阵L中3个阵元的信号做同步解调，解调信号用gm表示，m=l、2、3对应三轴发射线 圈，按照fn=knfs的标准选择复用信号频率，gm为三组固定值，同步解调计算公式如下： lmn- lngm 经同步解调后，结果lm是由m轴产生η轴接收的信号，lmn组成Lmn，Lmn-个3 X 3 X 1数组， 对数组做低通滤波、拟合处理后得到传感器对应的接收矩阵S; 接收矩阵S与发射矩阵I的耦合关系可以利用旋转矩阵来表示； 定位过程中，信号耦合关系可以通过旋转矩阵的方式描述，位置参数可以表示为两个 旋转矩阵变换和一个位移的结果，位移距离R，绕Z轴顺时针旋转角度为α，绕X轴顺时针旋转 角度为β，旋转矩阵Ρ如下所示：同理，姿态可以通过三个旋转矩阵来表示，其中绕Ζ轴顺时针旋转角度为Θ、表示绕X轴 顺时针旋转角度ω，表示绕Υ轴顺时针旋转角度φ，旋转矩阵Q如下所示：根据磁偶极子原理可以求得信号发射传感器、信号接收传感器的耦合矩阵Η，如下所 示：综上所述，若发射矩阵为单位矩阵I，则接收矩阵S可以用如下公式表示： S = kQPtHPI 式中k=电磁親合系数/衰减因子； (2)接收矩阵中位置参数与姿态参数分离； 求接收矩阵S的协方差矩阵； gts = k2ptH2p 求解左侧矩阵和右侧矩阵的迹，可以得出如下结果： ^(8^)=6^ 整理得： k=[tr(StS)/6](1/2) 距离参数R只与衰减因子k相关，可以通过下式求解； R=Rs(ks/k)(1/3) 公式中，ks、Rs是目标处于增益等级Ks区域时对应的参数； 利用解得的k值计算S的归一化矩阵，用Μ表示，如下所示： M=QPtEP 将矩阵Μ中的两类参数分离出来，只含有位置参数的矩阵X可以按如下公式计算： X = PtFP=(MtM-I)/3 式中F = dig(l，0,0); 只含有姿态参数的矩Y以通过下式求得： 丫 = 〇%=]\^-21 (3)求解目标的位置参数(X，y，z)及姿态参数(〇,w,(p): 四元数是一种超复数，一个四元数c由一个实数单位1和三个虚数单位i、j、k组成，形式 如下czco+cd+uj+csk，与旋转矩阵相比，四元数描述目标的方向、姿态更加方便； 下面利用闭环迭代四元数算法，求解目标的位置参数及姿态参数，两种参数的迭代计 算过程互不影响，所以可以同时进行； 将含有位置参数的矩阵X四元数的方式表示，如下所示： X=_-I)/3 = p(FP)p-1 式中P位置参数的最佳四元数估计值，P = po+pi i+P2 j+P3k; 最佳四元数估计值与估计初值的关系为p = p ' c，c为误差四元数，可表示为c = l+cii + C2 j+C3k，显然，当误差四元数矢量部分的值趋近于0时，估计初值p '与最佳四元数估计值p非 常接近； 首先，在X矩阵的左右分别乘上(Ρ'Γ^Ρ'，求解得到一个包含误差四元数和位置参数的 矩阵，计算公式如下所示： Xi=(p，)_1Xp，=c(FP)c_1=(l-2C)(FP) 公式中矩阵C为：求心矩阵的转置矩阵； X;'=p]F(卜 2Q](p')4 对X〗做和X矩阵同样的变换；整理上式结果，由于4CFC的值非常小，故只保留等式右侧的前三项，又因为位置参数不 存在沿Y轴旋转的角度，所以误差四元数C1 = C2C3,近似为0,公式右侧等式整理如下所示：通过上式可以求解出估计四元数c的值，判断误差四元数c是否满足定位精度要求，满 足则可以认为P'就是最佳估计值，如不满足，则可以利用误差四元数c求解一个新的估计初 值ρη?，替P '重复迭代过程； pnew-p C 位置参数α，β与最佳估计四元数p的关系如下所示： a = arcsin2(pip3+p〇p2)在算法第一次运行时，初始估计值选择ps，ps为目标α = 45°、β = 45°时四元数理论值，之 后可以选择上一次计算的最佳估计值Ρ作为下一次定位的估计初值Ρ ' ； 至此目标的极坐标形式的位置参数(R，a，i3)已经全部求出，可以通过如下公式将目标 的位置参数由极坐标形式(R，α，β)转换成直角坐标形式(X，y，z); x = Rcos(a)cos(0) y = Rsin(a)cos(P) z = Rsin(0) 利用姿态角的最佳四元数估计值q表示矩阵Y，q = q〇+qii+q2j+q3k; Y = qMq-1 利用估计初值q'对矩阵Y进行一次变换求解误差四元数u，最佳四元数估计值与估计初 值的关系为q = q'u，其中u为误差四元数u=l+uii+U2j+U3k，显然，当误差四元数矢量部分 ui、U2、U3的值趋近于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 ne3W，qne3W取代之前的估计初值q '重复迭代过程，利用误差 四元数计算估计初值的公式如下所示： Qnew-Q U 姿态参数θ、ω、_φ与最佳估计四元数q的关系如下所示：在算法第一次运行时，初始估计值选择qs，qs为目标θ = 45°、ω =45°、φ=45:(5时四元数 理论值，之后可以选择上一次计算的最佳估计值q作为下一次定位的估计初值q' ； 至此通过多目标六自由度电磁定位方法的步骤(三)计算出每个信号接收传感器的空 间位置(X，y，z)及姿态角(Ο,ω,φ)
【文档编号】G01S5/00GK105929362SQ201610236475
【公开日】2016年9月7日
【申请日】2016年4月17日
【发明人】孙晓颖, 刘阳, 陈建, 刘壮, 刘妍妍
【申请人】吉林大学