一種多目標(biāo)六自由度電磁定位裝置和方法
【專利摘要】本發(fā)明涉及一種多目標(biāo)六自由度電磁定位裝置和方法，屬于電磁定位裝置和定位方法。裝置由信號(hào)發(fā)射單元、信號(hào)接收單元、控制與數(shù)據(jù)處理單元組成，據(jù)定位需要，選擇信號(hào)接收傳感器的數(shù)量，并固定在定位目標(biāo)上，接收傳感器通過(guò)電磁感應(yīng)，產(chǎn)生接收信號(hào)，控制與數(shù)據(jù)處理單元接收采樣信號(hào)，執(zhí)行電磁定位算法。本發(fā)明解決了現(xiàn)有裝置對(duì)空間中大范圍移動(dòng)的目標(biāo)跟蹤定位能力較差的問(wèn)題，優(yōu)化定位方法，簡(jiǎn)化電路，提高了定位效率，并解決了六自由度求解時(shí)，位置參數(shù)、姿態(tài)參數(shù)求解過(guò)程相互影響造成的誤差的問(wèn)題。
【專利說(shuō)明】
一種多目標(biāo)六自由度電磁定位裝置和方法
技術(shù)領(lǐng)域
[0001] 本發(fā)明涉及一種電磁定位裝置和定位方法，具體涉及一種支持多個(gè)信號(hào)接收傳感 器同時(shí)工作的電磁定位裝置和一種并行求解目標(biāo)位置及姿態(tài)的電磁定位方法。
【背景技術(shù)】
[0002] 隨著科技的不斷發(fā)展，定位技術(shù)在輔助醫(yī)療、人機(jī)交互、運(yùn)動(dòng)檢測(cè)分析等領(lǐng)域應(yīng)用 廣泛。人體運(yùn)動(dòng)過(guò)程中，肢體會(huì)做出旋轉(zhuǎn)、彎曲等動(dòng)作，定位目標(biāo)可能被身體其他部分遮擋， 這些問(wèn)題使研究對(duì)定位技術(shù)有很高的要求。與超聲式、光電式等定位裝置相比，基于六自由 度電磁定位技術(shù)設(shè)計(jì)的裝置具有速度快、成本低、便攜性好等優(yōu)點(diǎn)。六自由度電磁定位裝置 追蹤目標(biāo)的運(yùn)動(dòng)軌跡的同時(shí)，還可以對(duì)目標(biāo)姿態(tài)精確測(cè)量，最重要的是定位不受障礙物遮 擋影響。這些優(yōu)點(diǎn)使多目標(biāo)六自由度電磁定位裝置可以很好的滿足研究需求。
[0003] 電磁定位因?yàn)槠洫?dú)特的優(yōu)點(diǎn)一直受到廣泛關(guān)注。劉越、王涌天等人于2000年提出 了一種旋轉(zhuǎn)矩陣算法，實(shí)現(xiàn)了六自由度電磁定位的參數(shù)計(jì)算，但算法計(jì)算復(fù)雜。李雪麗、王 毅等人在題為"電磁定位系統(tǒng)中的定位算法研究"的文中提出了一種利用四元數(shù)迭代計(jì)算 的方法定位，但算法脫離實(shí)際裝置，定位方法不夠完善。孫曉穎、陳明智等人在題為"一種用 于電磁定位的裝置和方法"的專利中，提出了一種六自由度電磁定位裝置和四元數(shù)同時(shí)求 解目標(biāo)位置參數(shù)及姿態(tài)參數(shù)的算法，解決定位裝置實(shí)時(shí)性差、定位算法中存在的奇點(diǎn)問(wèn)題， 但裝置定位局限于單目標(biāo)，裝置中濾波放大電路設(shè)計(jì)不夠完善，使裝置對(duì)空間中大范圍移 動(dòng)的目標(biāo)跟蹤定位能力較差，算法同時(shí)求解位置參數(shù)、姿態(tài)參數(shù)，計(jì)算結(jié)果相互影響產(chǎn)生誤 差。

【發(fā)明內(nèi)容】

[0004] 本發(fā)明提供一種多目標(biāo)六自由度電磁定位裝置和方法，以解決現(xiàn)有電磁定位裝置 局限于單目標(biāo)、對(duì)空間中大范圍移動(dòng)的目標(biāo)跟蹤定位能力較差的問(wèn)題。采用優(yōu)化定位方法， 簡(jiǎn)化電路，提高了定位效率，并解決了六自由度求解時(shí)，位置參數(shù)、姿態(tài)參數(shù)求解過(guò)程相互 影響造成的誤差的問(wèn)題。
[0005] 本發(fā)明采用的技術(shù)方案是：一種多目標(biāo)六自由度電磁定位裝置，由以下三部分組 成:信號(hào)發(fā)射單元、信號(hào)接收單元、控制與數(shù)據(jù)處理單元；
[0006] (1)所述信號(hào)發(fā)射單元由：信號(hào)發(fā)射傳感器、濾波放大電路一、數(shù)模轉(zhuǎn)換電路順序 連接，控制與數(shù)據(jù)處理單元為信號(hào)發(fā)射單元提供發(fā)射信號(hào)，經(jīng)數(shù)模轉(zhuǎn)換電路、濾波放大電路 一生成驅(qū)動(dòng)信號(hào)，經(jīng)信號(hào)發(fā)射傳感器，形成電磁場(chǎng)；
[0007] (2)所述信號(hào)接收單元由：至少一個(gè)信號(hào)接收傳感器、第一級(jí)時(shí)分復(fù)用電路、濾波 放大電路二、第二級(jí)時(shí)分復(fù)用電路、模數(shù)轉(zhuǎn)換電路順序連接，其中濾波放大電路二的增益是 可調(diào)節(jié)的，信號(hào)接收傳感器感應(yīng)到電磁場(chǎng)產(chǎn)生電壓信號(hào)，信號(hào)經(jīng)多路復(fù)用、濾波放大后，由 模數(shù)轉(zhuǎn)換電路對(duì)其采樣，并傳送給控制與數(shù)據(jù)處理單元；
[0008] (3)所述控制與數(shù)據(jù)處理單元:包括DSP、SDRAM存儲(chǔ)器、FLASH存儲(chǔ)器、USB接口及電 源，DSP讀取FLASH存儲(chǔ)器中的程序，存入DSP內(nèi)置RAM中，一方面產(chǎn)生控制電路的信號(hào)，另一 方面接收采樣信號(hào)并處理，計(jì)算過(guò)程中大容量數(shù)據(jù)會(huì)存儲(chǔ)到SDRAM中，通過(guò)USB接口實(shí)現(xiàn)裝 置與上位機(jī)通信，電源為裝置提供±5V、3.3V電壓。
[0009] 本發(fā)明所述信號(hào)發(fā)射單元中信號(hào)發(fā)射傳感器由正交的三軸環(huán)形線圈組成，控制與 數(shù)據(jù)處理單元為模數(shù)轉(zhuǎn)換電路提供三組發(fā)射信號(hào)，經(jīng)濾波放大電路后，驅(qū)動(dòng)三軸線圈同時(shí) 工作，發(fā)射信號(hào)采用正弦信號(hào)的形式，頻率按照f(shuō)n = knfs的標(biāo)準(zhǔn)設(shè)計(jì)，其中knS固定的整數(shù)， 11=1、2、3，匕為裝置的定位刷新頻率，根據(jù)這種標(biāo)準(zhǔn)生成的發(fā)射信號(hào)，在一個(gè)工作周期內(nèi)呈 三組固定值，故不再需要參考信號(hào)回流，能夠簡(jiǎn)化電路，提高定位效率。
[0010] 本發(fā)明所述信號(hào)接收單元中信號(hào)接收傳感器由正交的三軸環(huán)形線圈組成，裝置可 支持一個(gè)以上信號(hào)接收傳感器同時(shí)工作，當(dāng)置于信號(hào)發(fā)射單元產(chǎn)生的電磁場(chǎng)中，每個(gè)信號(hào) 接收傳感器都能接收到三軸信號(hào)，第一級(jí)時(shí)分復(fù)用電路將傳感器的三軸信號(hào)復(fù)用，經(jīng)濾波 放大電路二后，第二級(jí)時(shí)分復(fù)用電路將多個(gè)傳感器的信號(hào)復(fù)用，經(jīng)模數(shù)轉(zhuǎn)換電路采樣后傳 送給控制與數(shù)據(jù)處理單元。
[0011] 本發(fā)明所述控制與數(shù)據(jù)處理單元，包括：
[0012] (1)控制與數(shù)據(jù)處理單元根據(jù)信號(hào)接收傳感器的數(shù)量，為第一級(jí)時(shí)分復(fù)用電路、第 二級(jí)時(shí)分復(fù)用電路提供控制信號(hào)；
[0013] (2)控制與數(shù)據(jù)處理單元為信號(hào)發(fā)射單元提供發(fā)射信號(hào)，為模數(shù)轉(zhuǎn)換電路提供采 樣信號(hào)，這些信號(hào)及控制信號(hào)均由同一時(shí)鐘信號(hào)生成，保證時(shí)鐘同步；
[0014] (3)控制與數(shù)據(jù)處理單元接收采樣信號(hào)，求解目標(biāo)的六自由度信息，并通過(guò)USB接 口傳送給上位機(jī)程序。
[0015] 本發(fā)明所述控制與數(shù)據(jù)處理單元、濾波放大電路二、第二級(jí)時(shí)分復(fù)用電路、模數(shù)轉(zhuǎn) 換電路構(gòu)成反饋控制系統(tǒng)，用于調(diào)節(jié)濾波放大電路二的增益。
[0016] -種用于多目標(biāo)六自由度電磁定位的方法，包括如下步驟：
[0017] ( - )根據(jù)定位需要，選擇信號(hào)接收傳感器的數(shù)量，并固定在定位目標(biāo)上，將信號(hào)發(fā) 射傳感器位置固定，作為空間的坐標(biāo)原點(diǎn)（〇,〇,〇)，開(kāi)始定位，由控制與數(shù)據(jù)處理單元產(chǎn)生 發(fā)射信號(hào)，經(jīng)數(shù)模轉(zhuǎn)換電路、濾波放大電路，驅(qū)動(dòng)信號(hào)發(fā)射傳感器，以信號(hào)發(fā)射傳感器為中 心，形成電磁場(chǎng)；
[0018] (二)接收傳感器通過(guò)電磁感應(yīng)，產(chǎn)生接收信號(hào)，并通過(guò)第一級(jí)時(shí)分復(fù)用電路將來(lái) 自一個(gè)信號(hào)接收傳感器的多路信號(hào)復(fù)用，信號(hào)經(jīng)濾波放大后，經(jīng)第二級(jí)時(shí)分復(fù)用電路對(duì)多 個(gè)信號(hào)接收傳感器的多路信號(hào)復(fù)用，由模數(shù)轉(zhuǎn)換電路采樣，并傳送給控制與數(shù)據(jù)處理單元， 濾波放大電路二、時(shí)分復(fù)用電路二、模數(shù)轉(zhuǎn)換電路、控制與數(shù)據(jù)處理單元構(gòu)成反饋控制系 統(tǒng)，通過(guò)輸入信號(hào)的能量調(diào)節(jié)濾波放大電路二的增益，保證目標(biāo)在大范圍移動(dòng)時(shí)接收信號(hào) 穩(wěn)定，按照電磁場(chǎng)強(qiáng)度與距離的衰減規(guī)律，將定位空間劃分成若干個(gè)區(qū)域，并為每個(gè)區(qū)域設(shè) 定相應(yīng)的增益等級(jí)，依次為心、Kd s;
[0019] (三)控制與數(shù)據(jù)處理單元接收采樣信號(hào)，執(zhí)行電磁定位算法，步驟如下：
[0020] (1)首先按照時(shí)鐘信號(hào)，從復(fù)用信號(hào)x(n)中將每個(gè)信號(hào)接收傳感器的三軸信號(hào)還 原，組成信號(hào)接收矩陣L x，其中x表示傳感器的編號(hào)，Lx是一個(gè)NX3的矩陣，表示矩陣有3個(gè)陣 元，N個(gè)時(shí)間樣本序列，利用同步解調(diào)的方法，提取接收信號(hào)中三個(gè)頻率的成分，其中解調(diào)信 號(hào)為三組固定值 81、沿、抑，解調(diào)后三組信號(hào)通過(guò)低通濾波、擬合等方法組成接收矩陣3\;
[0021] (2)利用算法將接收矩陣Sx中的位置參數(shù)、姿態(tài)參數(shù)分離，構(gòu)造出兩個(gè)矩陣X x、Yx， 矩陣Xx只包含目標(biāo)的位置參數(shù)，矩陣Yx只包含目標(biāo)的姿態(tài)參數(shù)，參數(shù)分離后可以對(duì)位置參 數(shù)、姿態(tài)參數(shù)并行求解，結(jié)果互不影響；
[0022] (3)利用閉環(huán)、迭代的四元數(shù)算法同時(shí)求解編號(hào)為x的傳感器的位置參數(shù)(x，y，z) 和姿態(tài)參數(shù)(〇，w， (p)，并將結(jié)果通過(guò)USB接口傳送給上位機(jī)程序。
[0023] 本發(fā)明所述一種多目標(biāo)六自由度電磁定位的方法，所述步驟(二）中的反饋控制系 統(tǒng)負(fù)責(zé)調(diào)節(jié)濾波放大電路二的增益；
[0024]復(fù)用信號(hào)經(jīng)濾波放大電路、采樣后，傳送到數(shù)據(jù)處理單元，數(shù)據(jù)處理單元根據(jù)時(shí)鐘 信號(hào)將采樣信號(hào)還原，組成信號(hào)接收矩陣Lx;
[0025] 求Lx的協(xié)方差矩陣T:
[0026] T==LX^
[0027]求解矩陣T的跡，記為tr(T)，進(jìn)行閾值判斷，如果tr(T)的值低于tr(T)min將增益的 等級(jí)提高一級(jí)，如果tr(T)的值高于tr(T)max，則降低增益等級(jí)，該系統(tǒng)增益控制存在一個(gè)定 位周期的延遲；
[0028] 按照電磁場(chǎng)強(qiáng)度與距離的衰減規(guī)律，將定位空間劃分成若干個(gè)區(qū)域，并為每個(gè)區(qū) 域設(shè)定相應(yīng)的增益等級(jí)，依次為心、Kds;
[0029] 實(shí)際上，信號(hào)的能量只與目標(biāo)的距離有關(guān)，所以可以認(rèn)為接收信號(hào)的能量與目標(biāo) 的距離是等價(jià)的，故可以利用tr(T)去計(jì)算目標(biāo)的距離R，R的誤差也能直接反應(yīng)算法對(duì)接收 信號(hào)能量的估計(jì)是否準(zhǔn)確；
[0030] R=R [tr(T )/tr(T)]<'
[0031] 其中Rs和tr(Ts)是對(duì)應(yīng)增益等級(jí)Ks的一組常數(shù)；
[0032] 本發(fā)明所述一種多目標(biāo)六自由度電磁定位的方法，所述步驟(三）中的電磁定位算 法，具體實(shí)現(xiàn)步驟如下：
[0033] 為方便描述，后文省略了矩陣1^、5\4\、￥\中的傳感器編號(hào)1，用1、54、￥表示；
[0034] (1)還原復(fù)用信號(hào)，組建接收矩陣S;
[0035] 控制與數(shù)據(jù)處理單元會(huì)根據(jù)時(shí)鐘信號(hào)，將復(fù)用信號(hào)x(n)還原，并按照順序存入信 號(hào)接收矩陣L中；
[0036] L=[li,l2,l3]T
[0037]對(duì)矩陣L中3個(gè)陣元的信號(hào)做同步解調(diào)，解調(diào)信號(hào)用gm表示，m=l、2、3對(duì)應(yīng)三軸發(fā) 射線圈，按照f(shuō)n = knfs的標(biāo)準(zhǔn)選擇復(fù)用信號(hào)頻率，gm為三組固定值，同步解調(diào)計(jì)算公式如下：
[0038] lmn=lngm
[0039] 經(jīng)同步解調(diào)后，結(jié)果lmn是由m軸產(chǎn)生n軸接收的信號(hào)，lmn組成Lmn，L mn-個(gè)3 X 3 X 1數(shù) 組，對(duì)數(shù)組做低通濾波、擬合處理后得到傳感器對(duì)應(yīng)的接收矩陣S;
[0040] 接收矩陣S與發(fā)射矩陣I的耦合關(guān)系可以利用旋轉(zhuǎn)矩陣來(lái)表示；
[0041] 定位過(guò)程中，信號(hào)耦合關(guān)系可以通過(guò)旋轉(zhuǎn)矩陣的方式描述，位置參數(shù)可以表示為 兩個(gè)旋轉(zhuǎn)矩陣變換和一個(gè)位移的結(jié)果，位移距離R，繞Z軸順時(shí)針旋轉(zhuǎn)角度為a，繞X軸順時(shí)針 旋轉(zhuǎn)角度為0，旋轉(zhuǎn)矩陣P如下所示： cos[3 0 -sin(3 coscx sincx 0
[0042] 0 1 0 -siiia eosa 0 sin (3 0 eosp 0 0 1
[0043]同理，姿態(tài)可以通過(guò)三個(gè)旋轉(zhuǎn)矩陣來(lái)表示，其中繞Z軸順時(shí)針旋轉(zhuǎn)角度為0、表示繞 X軸順時(shí)針旋轉(zhuǎn)角度C0，表示繞Y軸順時(shí)針旋轉(zhuǎn)角度9，旋轉(zhuǎn)矩陣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] 根據(jù)磁偶極子原理可以求得信號(hào)發(fā)射傳感器、信號(hào)接收傳感器的耦合矩陣H，如下 所示： '2 Q 0"
[0046] H= 0 -1 0 0 0 -1
[0047] 綜上所述，若發(fā)射矩陣為單位矩陣I，則接收矩陣S可以用如下公式表示：
[0048] S = kQPtHPI
[0049] 式中k =電磁耦合系數(shù)/衰減因子；
[0050] (2)接收矩陣中位置參數(shù)與姿態(tài)參數(shù)分離；
[0051]求接收矩陣S的協(xié)方差矩陣；
[0052] Sts = k2ptH2p
[0053]求解左側(cè)矩陣和右側(cè)矩陣的跡，可以得出如下結(jié)果：
[0054] tr(StS)=6k2
[0055] 整理得：
[0056] k=[tr(StS)/6](1/2)
[0057] 距離參數(shù)R只與衰減因子k相關(guān)，可以通過(guò)下式求解；
[0058] R = Rs(ks/k)(1/3)
[0059]公式中，ks、Rs是目標(biāo)處于增益等級(jí)Ks區(qū)域時(shí)對(duì)應(yīng)的參數(shù)；
[0060]利用解得的k值計(jì)算S的歸一化矩陣，用M表示，如下所示：
[0061] M = QptHP
[0062] 將矩陣M中的兩類參數(shù)分離出來(lái)，只含有位置參數(shù)的矩陣X可以按如下公式計(jì)算：
[0063] X = PtFP=(MtM-I)/3
[0064] 式中F = dig(l，0,0);
[0065] 只含有姿態(tài)參數(shù)的矩Y以通過(guò)下式求得：
[0066] 丫 = 〇、=]?%-21
[0067] (3)求解目標(biāo)的位置參數(shù)(x，y，z)及姿態(tài)參數(shù)(0,〇)，的；
[0068]四元數(shù)是一種超復(fù)數(shù)，一個(gè)四元數(shù)c由一個(gè)實(shí)數(shù)單位1和三個(gè)虛數(shù)單位i、j、k組成， 形式如下czco+cd+uj+csk，與旋轉(zhuǎn)矩陣相比，四元數(shù)描述目標(biāo)的方向、姿態(tài)更加方便； [0069]下面利用閉環(huán)迭代四元數(shù)算法，求解目標(biāo)的位置參數(shù)及姿態(tài)參數(shù)，兩種參數(shù)的迭 代計(jì)算過(guò)程互不影響，所以可以同時(shí)進(jìn)行；
[0070] 將含有位置參數(shù)的矩陣X四元數(shù)的方式表示，如下所示：
[0071] X=(MtM-I)/3 = p(FP)p_1
[0072] 式中p位置參數(shù)的最佳四元數(shù)估計(jì)值，p = p〇+pii+p2j+p3k;
[0073] 最佳四元數(shù)估計(jì)值與估計(jì)初值的關(guān)系為p = p'c，c為誤差四元數(shù)，可表示為c = l + cii+C2j+C3k，顯然，當(dāng)誤差四元數(shù)矢量部分的值趨近于0時(shí)，估計(jì)初值p'與最佳四元數(shù)估計(jì) 值P非常接近；
[0074] 首先，在X矩陣的左右分別乘上(p'r\p'，求解得到一個(gè)包含誤差四元數(shù)和位置參 數(shù)的矩陣，計(jì)算公式如下所示：
[0075] Xi=(p，)_1Xp，=c(FP)c _1=(l-2C)(FP)
[0076] 公式中矩陣C為： 0. g3 -Cj
[0077] C= -C3 0 Cj :c2 ， 0 _
[0078] 求心矩陣的轉(zhuǎn)置矩陣；
[0079] X； =p'[F(I+2C)](pT'
[0080] 對(duì)X『做和X矩陣同樣的變換；
[0081] (pT!X1ip'=(I-2C)F(1+2C) =F+2FC-2CF-4CFC
[0082]整理上式結(jié)果，由于4CFC的值非常小，故只保留等式右側(cè)的前三項(xiàng)，又因?yàn)槲恢脜?數(shù)不存在沿Y軸旋轉(zhuǎn)的角度，所以誤差四元數(shù)C1 = C2C3,近似為0,公式右側(cè)等式整理如下所 示： I 2ca -2c.2"
[0083] 2c3 0 0 -2c2 0 0
[0084] 通過(guò)上式可以求解出估計(jì)四元數(shù)c的值，判斷誤差四元數(shù)c是否滿足定位精度要 求，滿足則可以認(rèn)為P'就是最佳估計(jì)值，如不滿足，則可以利用誤差四元數(shù)c求解一個(gè)新的 估計(jì)初值，p n?代替P '重復(fù)迭代過(guò)程；
[0085] Pnew = p'c
[0086] 位置參數(shù)a，0與最佳估計(jì)四元數(shù)p的關(guān)系如下所示：
[0087] a = arcsin2(pip3+p〇p2)
[0088] p=-arctan[2( p,p, -p.p, )/(p,；+pf -p； +p；)]
[0089]在算法第一次運(yùn)行時(shí)，初始估計(jì)值選擇ps，ps為目標(biāo)a = 45°、P = 45°時(shí)四元數(shù)理論 值，之后可以選擇上一次計(jì)算的最佳估計(jì)值P作為下一次定位的估計(jì)初值P ' ；
[0090] 至此目標(biāo)的極坐標(biāo)形式的位置參數(shù)(R，a，e)已經(jīng)全部求出，可以通過(guò)如下公式將 目標(biāo)的位置參數(shù)由極坐標(biāo)形式(R，a，0)轉(zhuǎn)換成直角坐標(biāo)形式(x，y，z);
[0091] x = Rcos(a)cos(0)
[0092] y = Rsin(a)cos(0)
[0093] z = Rsin(0)
[0094]利用姿態(tài)角的最佳四元數(shù)估計(jì)值q表示矩陣Y，q = qo+qii+q2j+q3k;
[0095] Y = qMq-1
[0096] 利用估計(jì)初值q'對(duì)矩陣Y進(jìn)行一次變換求解誤差四元數(shù)u，最佳四元數(shù)估計(jì)值與估 計(jì)初值的關(guān)系為q = q'u，其中u為誤差四元數(shù)u=l+uii+U2j+U3k，顯然，當(dāng)誤差四元數(shù)矢量部 分ui、U2、U3的值趨近于0時(shí)，姿態(tài)四元數(shù)估算值q '的值與最佳估計(jì)值q非常接近；
[0097] 與X矩陣的計(jì)算過(guò)程類似，對(duì)Y的變換如下所示：
[0098] (q，）-Wq，=uMu-1= (I_2U)M [0099]其中U為誤差四元數(shù)組成的矩陣； G u3 -u3
[0100] U= -u3 0 u, u2 -u, 0
[0101] 整理上式，可以求解UM為：
[0102] UM=[M-(a，)_1Ya，]/2 = D
[0103] 將上述公式寫(xiě)成如下形式： 0 m1；! m13]「d" d12 d13
[0104] -u3： 0 Uj m21 m22 = d2l. d22 d23 _ % ， 0」Lm3:1 m32 m33」d32
[0105] 從以上9組等式中選擇三組獨(dú)立的等式，寫(xiě)成如下形式： 0 -m31 .m21「4 「.dn
[0106] m32 0 -ml2 u2 = d12 _-m23 i?t3 〇 1叫」
[0107]將上式寫(xiě)成如下形式：
[0108] M'u=d
[0109] 通過(guò)上式可以求解出估計(jì)四元數(shù)u的值，之后判斷四元數(shù)u是否滿足精度要求，如 果誤差四元數(shù)u滿足定位要求，則可以認(rèn)為q'就是最佳估計(jì)值，不滿足精度要求，則可以利 用誤差四元數(shù)U求解一個(gè)新的估計(jì)初值q new，qnew取代之前的估計(jì)初值q'重復(fù)迭代過(guò)程，利用 誤差四元數(shù)計(jì)算估計(jì)初值的公式如下所示：
[0110] qnew=q'u
[0111] 姿態(tài)參數(shù)9、《、9與最佳估計(jì)四元數(shù)q的關(guān)系如下所示：
[0115] 在算法第一次運(yùn)行時(shí)，初始估計(jì)值選擇qs，qs為目標(biāo) 9 = 45°、〇 =45°、tp=45_°__l時(shí)四 元數(shù)理論值，之后可以選擇上一次計(jì)算的最佳估計(jì)值q作為下一次定位的估計(jì)初值q' ；
[0116] 至此通過(guò)多目標(biāo)六自由度電磁定位方法的步驟(三)計(jì)算出每個(gè)信號(hào)接收傳感器 的空間位置(x，y，z)及姿態(tài)角.(〇,w，(p)。
[0117] 本發(fā)明具有以下優(yōu)點(diǎn)：
[0118] (1)現(xiàn)有的電磁定位裝置局限于單目標(biāo)定位，本發(fā)明采用二級(jí)時(shí)分復(fù)用電路對(duì)多 個(gè)傳感器的多路信號(hào)時(shí)分復(fù)用，采用多目標(biāo)六自由度電磁定位算法，使裝置可以同時(shí)對(duì)一 個(gè)以上目標(biāo)跟蹤定位。
[0119] (2)現(xiàn)有的電磁定位裝置，需要引入?yún)⒖夹盘?hào)回流，本發(fā)明采用fn = knfs的標(biāo)準(zhǔn)設(shè) 計(jì)發(fā)射信號(hào)，定位過(guò)程中不再需要參考信號(hào)回流，簡(jiǎn)化信號(hào)接收單元電路，將定位效率提高 33%〇
[0120] (3)現(xiàn)有的電磁定位裝置，對(duì)大范圍移動(dòng)的目標(biāo)跟蹤定位不夠準(zhǔn)確，本發(fā)明采用反 饋控制系統(tǒng)調(diào)節(jié)濾波放大電路二的增益，保證目標(biāo)在大范圍移動(dòng)時(shí)接收信號(hào)穩(wěn)定，利用信 號(hào)接收矩陣L x協(xié)方差矩陣的跡，估計(jì)接收信號(hào)能量的算法，這種算法過(guò)程簡(jiǎn)便，計(jì)算量小， 對(duì)信號(hào)能量估計(jì)準(zhǔn)確。
[0121 ] (4)本發(fā)明提出一種多目標(biāo)六自由度電磁定位算法，將接收矩陣Sx*的位置參數(shù)、 姿態(tài)參數(shù)分離，組成矩陣Xx、Yx，之后對(duì)Xx、Yx分別求解，因?yàn)閄 x只含有位置參數(shù)的信息、Yx中 只含有姿態(tài)參數(shù)的信息，所以這種方法可以避免求解過(guò)程中兩種參數(shù)互相影響產(chǎn)生的誤 差。
[0122] (5)本發(fā)明采用兩種參數(shù)的計(jì)算過(guò)程同時(shí)進(jìn)行，采用閉環(huán)迭代四元數(shù)算法求解目 標(biāo)的位置參數(shù)與姿態(tài)參數(shù)，大幅度提高定位效率。
【附圖說(shuō)明】
[0123] 圖1是多目標(biāo)六自由度電磁定位裝置原理圖；
[0124] 圖2是控制與數(shù)據(jù)處理單元工作原理圖；
[0125] 圖3A是單個(gè)信號(hào)接收傳感器工作時(shí)，時(shí)分復(fù)用電路的控制信號(hào)；
[0126] 圖3B是十六個(gè)信號(hào)接收傳感器同時(shí)工作時(shí)，時(shí)分復(fù)用電路的控制信號(hào)；
[0127] 圖4是反饋控制系統(tǒng)原理圖；
[0128] 圖5是按增益等級(jí)劃分的定位區(qū)域；
[0129] 圖6是多目標(biāo)六自由度電磁定位算法流程圖；
[0130] 圖7是閉環(huán)四元數(shù)迭代算法計(jì)算位置參數(shù)流程圖；
[0131 ]圖8是閉環(huán)四元數(shù)迭代算法計(jì)算姿態(tài)參數(shù)流程圖；
[0132] 圖9是tr(T)估計(jì)接收信號(hào)能量實(shí)驗(yàn)中選取的位置示意圖；
[0133] 圖10是位置一二三的實(shí)驗(yàn)結(jié)果圖；
[0134] 圖11是位置四五六的實(shí)驗(yàn)結(jié)果圖；
[0135] 圖12是位置七八九的實(shí)驗(yàn)結(jié)果圖。
【具體實(shí)施方式】
[0136] -種多目標(biāo)六自由度電磁定位裝置，由以下三部分組成:信號(hào)發(fā)射單元1、信號(hào)接 收單元2、控制與數(shù)據(jù)處理單元3，如圖1所示；
[0137] (1)所述信號(hào)發(fā)射單元1由：信號(hào)發(fā)射傳感器11、濾波放大電路一12、數(shù)模轉(zhuǎn)換電路 13順序連接，控制與數(shù)據(jù)處理單元3為信號(hào)發(fā)射單元1提供發(fā)射信號(hào)，經(jīng)數(shù)模轉(zhuǎn)換電路13、濾 波放大電路一 12生成驅(qū)動(dòng)信號(hào)，經(jīng)信號(hào)發(fā)射傳感器11，形成電磁場(chǎng)；
[0138] (2)所述信號(hào)接收單元2由：至少一個(gè)信號(hào)接收傳感器21、第一級(jí)時(shí)分復(fù)用22電路、 濾波放大電路二23、第二級(jí)時(shí)分復(fù)用電路24、模數(shù)轉(zhuǎn)換電路順序連接25，其中濾波放大電路 二23的增益是可調(diào)節(jié)的，信號(hào)接收傳感器21感應(yīng)到電磁場(chǎng)產(chǎn)生電壓信號(hào)，信號(hào)經(jīng)多路復(fù)用、 濾波放大后，由模數(shù)轉(zhuǎn)換電路25對(duì)其采樣，并傳送給控制與數(shù)據(jù)處理單元3;
[0139] (3)所述控制與數(shù)據(jù)處理單元3,其組成及工作原理如圖2所示:包括DSP301、
[0140] SDRAM 存儲(chǔ)器302、FLASH 存儲(chǔ)器303、USB 接口 304及電源305，DSP301 讀取 FLASH 存儲(chǔ) 器303中的程序，存入DSP301內(nèi)置RAM中，一方面產(chǎn)生控制電路的信號(hào)，另一方面接收采樣信 號(hào)并處理，計(jì)算過(guò)程中大容量數(shù)據(jù)會(huì)存儲(chǔ)到SDRAM302中，通過(guò)USB接口 304實(shí)現(xiàn)裝置與上位 機(jī)通信，電源305為裝置提供±5V、3.3V電壓。其中DSP301選取了德州儀器公司的F28335，這 是一款32位浮點(diǎn)型DSP，TMS320F28335融合了控制外設(shè)的集成功能與MCU的易用性，具有強(qiáng) 大的控制和信號(hào)處理能力及C語(yǔ)言編程效率，能夠?qū)崿F(xiàn)復(fù)雜的算法；
[0141] -種多目標(biāo)六自由度電磁定位裝置中的信號(hào)發(fā)射單元1:所述信號(hào)發(fā)射傳感器11 由正交的三軸環(huán)形線圈組成，控制與數(shù)據(jù)處理單元3為模數(shù)轉(zhuǎn)換電路13提供三組發(fā)射信號(hào)， 經(jīng)濾波放大電路12后，驅(qū)動(dòng)三軸線圈同時(shí)工作，發(fā)射信號(hào)采用正弦信號(hào)的形式，頻率按照f(shuō) n = knfs的標(biāo)準(zhǔn)設(shè)計(jì)，其中1^為固定的整數(shù)，11 = 1、2、3，匕為裝置的定位刷新頻率，根據(jù)這種標(biāo) 準(zhǔn)生成的發(fā)射信號(hào)，在一個(gè)工作周期內(nèi)呈三組固定值，故不再需要參考信號(hào)回流，能夠簡(jiǎn)化 電路，提尚定位效率。
[0142] -種多目標(biāo)六自由度電磁定位裝置中的信號(hào)接收單元2,所述信號(hào)接收傳感器21 由正交的三軸環(huán)形線圈組成，裝置可支持一個(gè)以上信號(hào)接收傳感器同時(shí)工作，當(dāng)置于信號(hào) 發(fā)射單元產(chǎn)生的電磁場(chǎng)中，每個(gè)信號(hào)接收傳感器21都能接收到三軸信號(hào)，第一級(jí)時(shí)分復(fù)用 電路22將傳感器的三軸信號(hào)復(fù)用，經(jīng)濾波放大電路二23后，第二級(jí)時(shí)分復(fù)用電路24將多個(gè) 傳感器的信號(hào)復(fù)用，經(jīng)模數(shù)轉(zhuǎn)換電路25采樣后傳送給控制與數(shù)據(jù)處理單元3。
[0143] -種多目標(biāo)六自由度電磁定位裝置中的控制與數(shù)據(jù)處理單元3,包括：
[0144] (1)控制與數(shù)據(jù)處理單元3根據(jù)信號(hào)接收傳感器21的數(shù)量，為第一級(jí)時(shí)分復(fù)用電路 22、第二級(jí)時(shí)分復(fù)用24電路提供控制信號(hào)，如圖3A、B所示，其中PWM1A、PMW1B為第一級(jí)時(shí)分 復(fù)用電路22的控制信號(hào)，PWM2A、PMW2B為第二級(jí)時(shí)分復(fù)用電路24的控制信號(hào)；
[0145] (2)控制與數(shù)據(jù)處理單元3為信號(hào)發(fā)射單元1提供發(fā)射信號(hào)，為模數(shù)轉(zhuǎn)換電路13提 供采樣信號(hào)，這些信號(hào)及控制信號(hào)均由同一時(shí)鐘信號(hào)生成，保證時(shí)鐘同步；
[0146] (3)控制與數(shù)據(jù)處理單元3接收采樣信號(hào)，求解目標(biāo)的六自由度信息，并通過(guò)USB接 口 304傳送給上位機(jī)程序。
[0147] 所述的多目標(biāo)六自由度電磁定位裝置，其中控制與數(shù)據(jù)處理單元3、濾波放大電路 二23、第二級(jí)時(shí)分復(fù)用電路24、模數(shù)轉(zhuǎn)換電路25構(gòu)成反饋控制系統(tǒng)，如圖4所示，用于控制濾 波放大電路二23的增益，系統(tǒng)有多路輸入信號(hào)X x(t)，多路信號(hào)分別由運(yùn)算放大電路二23放 大kx倍后，經(jīng)第二級(jí)時(shí)分復(fù)用電路24,整合成信號(hào)x(t)，模數(shù)轉(zhuǎn)換電路25采樣后，以x(n)的 形式傳遞給控制與數(shù)據(jù)處理單元3,控制與數(shù)據(jù)處理單元3首先根據(jù)時(shí)鐘信號(hào)，將復(fù)用信號(hào) 還原，并存入對(duì)應(yīng)的接收矩陣L x中，求Lx的協(xié)方差矩陣T，求解矩陣T的跡，記為tr(T)，進(jìn)行閾 值判斷，如果tr(T)的值低于tr(T)min將增益的等級(jí)提高一級(jí)，如果tr(T)的值高于tr(T) max， 則降低增益等級(jí)，該系統(tǒng)增益控制存在一個(gè)定位周期的延遲。
[0148] 按照電磁場(chǎng)強(qiáng)度與距離的衰減規(guī)律，將定位空間劃分成若干個(gè)區(qū)域，并為每個(gè)區(qū) 域設(shè)定相應(yīng)的增益等級(jí)，依次為心、Kds，如圖5所示。
[0149] -種用于多目標(biāo)六自由度電磁定位的方法，過(guò)程如圖6所示，包括如下步驟：
[0150] ( - )根據(jù)定位需要，選擇信號(hào)接收傳感器的數(shù)量，并固定在定位目標(biāo)上，將信號(hào)發(fā) 射傳感器位置固定，作為空間的坐標(biāo)原點(diǎn)(〇,〇,〇)，開(kāi)始定位，由控制與數(shù)據(jù)處理單元3產(chǎn)生 發(fā)射信號(hào)，經(jīng)數(shù)模轉(zhuǎn)換電路13、濾波放大電路12,驅(qū)動(dòng)信號(hào)發(fā)射傳感器11，以信號(hào)發(fā)射傳感 器為中心，形成電磁場(chǎng)；
[0151] (二)接收傳感器21通過(guò)電磁感應(yīng)，產(chǎn)生接收信號(hào)，并通過(guò)第一級(jí)時(shí)分復(fù)用電路22 將來(lái)自一個(gè)信號(hào)接收傳感器21的多路信號(hào)復(fù)用，信號(hào)經(jīng)濾波放大后，經(jīng)第二級(jí)時(shí)分復(fù)用電 路24對(duì)多個(gè)信號(hào)接收傳感器的多路信號(hào)復(fù)用，由模數(shù)轉(zhuǎn)換電路25采樣，并傳送給控制與數(shù) 據(jù)處理單元3,濾波放大電路二23、時(shí)分復(fù)用電路二24、模數(shù)轉(zhuǎn)換電路25、控制與數(shù)據(jù)處理單 元3構(gòu)成反饋控制系統(tǒng)，通過(guò)輸入信號(hào)的能量調(diào)節(jié)濾波放大電路二的增益，保證目標(biāo)在大范 圍移動(dòng)時(shí)接收信號(hào)穩(wěn)定，按照電磁場(chǎng)強(qiáng)度與距離的衰減規(guī)律，將定位空間劃分成若干個(gè)區(qū) 域，并為每個(gè)區(qū)域設(shè)定相應(yīng)的增益等級(jí)，依次為心、Kd s;
[0152](三)控制與數(shù)據(jù)處理單元3接收采樣信號(hào)，執(zhí)行電磁定位算法，過(guò)程如圖6所示，控 制與數(shù)據(jù)處理單元會(huì)根據(jù)時(shí)鐘信號(hào)，將復(fù)用信號(hào)x(n)還原，并按照順序存入信號(hào)接收矩陣 Lx中，從矩陣Lx中提取接收矩陣Sx，之后求解衰減因子k，并求解歸一化矩陣M，將M矩陣中的 位置參數(shù)、姿態(tài)參數(shù)分離，構(gòu)成矩陣X x、矩陣Yx，對(duì)兩矩陣同時(shí)進(jìn)行四元數(shù)迭代計(jì)算，求解目 標(biāo)的位置參數(shù)(x，y，z)姿態(tài)參數(shù)(〇，w， (p)。
[0153] -種多目標(biāo)六自由度電磁定位的方法，步驟(二）中的反饋控制系統(tǒng)負(fù)責(zé)調(diào)節(jié)濾波 放大電路二的增益；
[0154]復(fù)用信號(hào)經(jīng)濾波放大電路、采樣后，傳送到數(shù)據(jù)處理單元，數(shù)據(jù)處理單元根據(jù)時(shí)鐘 信號(hào)將采樣信號(hào)還原，組成信號(hào)接收矩陣Lx;
[0155] 求Lx的協(xié)方差矩陣T:
[0156] T=LxLtx
[0157] 求解矩陣T的跡，記為tr(T)，進(jìn)行閾值判斷，如果tr(T)的值低于tr(T)min將增益的 等級(jí)提高一級(jí)，如果tr(T)的值高于tr(T) max，則降低增益等級(jí)，該系統(tǒng)增益控制存在一個(gè)定 位周期的延遲；
[0158] 按照電磁場(chǎng)強(qiáng)度與距離的衰減規(guī)律，將定位空間劃分成若干個(gè)區(qū)域，并為每個(gè)區(qū) 域設(shè)定相應(yīng)的增益等級(jí)，依次為心、Kds;
[0159] 實(shí)際上，信號(hào)的能量只與目標(biāo)的距離有關(guān)，所以可以認(rèn)為接收信號(hào)的能量與目標(biāo) 的距離是等價(jià)的，故可以利用tr(T)去計(jì)算目標(biāo)的距離R，R的誤差也能直接反應(yīng)算法對(duì)接收 信號(hào)能量的估計(jì)是否準(zhǔn)確；
[0160] [tr(T )/tr(T)]<'
[0161] 其中Rs和tr(Ts)是對(duì)應(yīng)增益等級(jí)Ks的一組常數(shù)；
[0162] -種多目標(biāo)六自由度電磁定位的方法，步驟(三）中的電磁定位算法，具體實(shí)現(xiàn)步 驟如下：
[0163] 為方便描述，后文省略了矩陣1^、5\、乂\、￥\中的傳感器編號(hào)^用1、5、乂、￥表示；
[0164] (1)還原復(fù)用信號(hào)，組建接收矩陣S;
[0165] 控制與數(shù)據(jù)處理單元會(huì)根據(jù)時(shí)鐘信號(hào)，將復(fù)用信號(hào)x(n)還原，并按照順序存入信 號(hào)接收矩陣L中；
[0166] L=[li，l2，l3]T
[0167] 對(duì)矩陣L中3個(gè)陣元的信號(hào)做同步解調(diào)，解調(diào)信號(hào)用gm表示，m=l、2、3對(duì)應(yīng)三軸發(fā) 射線圈，按照f(shuō)n = knfs的標(biāo)準(zhǔn)選擇復(fù)用信號(hào)頻率，gm為三組固定值，同步解調(diào)計(jì)算公式如下：
[0168] lmn=lngm
[0169] 經(jīng)同步解調(diào)后，結(jié)果lmn是由m軸產(chǎn)生n軸接收的信號(hào)，lmn組成L mn，Lmn-個(gè)3 X 3 X 1數(shù) 組，對(duì)數(shù)組做低通濾波、擬合處理后得到傳感器對(duì)應(yīng)的接收矩陣S;
[0170] 接收矩陣S與發(fā)射矩陣I的耦合關(guān)系可以利用旋轉(zhuǎn)矩陣來(lái)表示；
[0171]定位過(guò)程中，信號(hào)耦合關(guān)系可以通過(guò)旋轉(zhuǎn)矩陣的方式描述，位置參數(shù)可以表示為 兩個(gè)旋轉(zhuǎn)矩陣變換和一個(gè)位移的結(jié)果，位移距離R，繞Z軸順時(shí)針旋轉(zhuǎn)角度為a，繞X軸順時(shí)針 旋轉(zhuǎn)角度為0，旋轉(zhuǎn)矩陣P如下所示： cosp 0 -sinj3 cosa sina 0
[0172] P= 0 10 -sina cosa 0 smj3 0 cosp 0 0 1
[0173]同理，姿態(tài)可以通過(guò)三個(gè)旋轉(zhuǎn)矩陣來(lái)表示，其中繞Z軸順時(shí)針旋轉(zhuǎn)角度為0、表示繞 X軸順時(shí)針旋轉(zhuǎn)角度co，表示繞Y軸順時(shí)針旋轉(zhuǎn)角度9，旋轉(zhuǎn)矩陣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] 根據(jù)磁偶極子原理可以求得信號(hào)發(fā)射傳感器、信號(hào)接收傳感器的耦合矩陣H，如下 所示： ~2 〇 0"
[0176] H= 0 -1 0 0 0 4
[0177] 綜上所述，若發(fā)射矩陣為單位矩陣I，則接收矩陣S可以用如下公式表示：
[0178] S = kQPtHPI
[0179] 式中k =電磁耦合系數(shù)/衰減因子；
[0180] (2)接收矩陣中位置參數(shù)與姿態(tài)參數(shù)分離；
[0181 ]求接收矩陣S的協(xié)方差矩陣；
[0182] sts = k2ptH2p
[0183] 求解左側(cè)矩陣和右側(cè)矩陣的跡，可以得出如下結(jié)果：
[0184] tr(StS)=6k2
[0185] 整理得：
[0186] k=[tr(StS)/6](1/2)
[0187] 距離參數(shù)R只與衰減因子k相關(guān)，可以通過(guò)下式求解；
[0188] R = Rs(ks/k)(1/3)
[0189] 公式中，ks、Rs是目標(biāo)處于增益等級(jí)Ks區(qū)域時(shí)對(duì)應(yīng)的參數(shù)；
[0190] 利用解得的k值計(jì)算S的歸一化矩陣，用M表示，如下所示：
[0191] M = QptHP
[0192] 將矩陣M中的兩類參數(shù)分離出來(lái)，只含有位置參數(shù)的矩陣X可以按如下公式計(jì)算：
[0193] X = PtFP=(MtM-I)/3
[0194] 式中 F = dig(l，0,0);
[0195] 只含有姿態(tài)參數(shù)的矩Y以通過(guò)下式求得：
[0196] 丫 = 〇、=]?%-21
[0197] (3)求解目標(biāo)的位置參數(shù)(x，y，z)及姿態(tài)參數(shù)((U〇,q>):
[0198] 四元數(shù)是一種超復(fù)數(shù)，一個(gè)四元數(shù)c由一個(gè)實(shí)數(shù)單位1和三個(gè)虛數(shù)單位i、j、k組成， 形式如下czco+cd+uj+csk，與旋轉(zhuǎn)矩陣相比，四元數(shù)描述目標(biāo)的方向、姿態(tài)更加方便；
[0199] 下面利用閉環(huán)迭代四元數(shù)算法，求解目標(biāo)的位置參數(shù)及姿態(tài)參數(shù)，兩種參數(shù)的迭 代計(jì)算過(guò)程互不影響，所以可以同時(shí)進(jìn)行；
[0200] 求解位置參數(shù)的過(guò)程如圖7所示，首先，在X矩陣的左右分別乘上估計(jì)初值p'和 (P'）-1，求解得到一個(gè)包含誤差四元數(shù)和位置參數(shù)的矩陣Xi，求乂:矩陣的轉(zhuǎn)置矩陣，對(duì)XI做 和X矩陣同樣的變換，可以求解出誤差四元數(shù)c的值，判斷誤差四元數(shù)c是否滿足定位精度要 求，滿足則可以認(rèn)為P'就是最佳估計(jì)值，求解出目標(biāo)的位置參數(shù)，如不滿足，則可以利用誤 差四元數(shù)c求解一個(gè)新的估計(jì)初值p new，pnew代替p'重復(fù)迭代過(guò)程，在算法第一次運(yùn)行時(shí)，初 始估計(jì)值選擇Ps，p s為目標(biāo)a = 45°、0 = 45°時(shí)四元數(shù)理論值，之后可以選擇上一次計(jì)算的最 佳估計(jì)值P作為下一次定位的估計(jì)初值P ' ；
[0201] 首先，在X矩陣的左右分別乘上(p'r\p'，求解得到一個(gè)包含誤差四元數(shù)和位置參 數(shù)的矩陣，計(jì)算公式如下所示：
[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]求心矩陣的轉(zhuǎn)置矩陣；
[0206] X； =p'[F(i+2C)](pT'
[0207] 對(duì)X〗做和X矩陣同樣的變換；
[0208] (PTlX；p'=(l-2C)F(l i-2C) =F K2FC-2CF-4CF(;
[0209]整理上式結(jié)果，由于4CFC的值非常小，故只保留等式右側(cè)的前三項(xiàng)，又因?yàn)槲恢脜?數(shù)不存在沿Y軸旋轉(zhuǎn)的角度，所以誤差四元數(shù)C1 = C2C3,近似為0,公式右側(cè)等式整理如下所 示： 1 2c3 -2c^
[0210] 2c3 0 0 -2c2 0 0
[0211] 通過(guò)上式可以求解出估計(jì)四元數(shù)c的值，判斷誤差四元數(shù)c是否滿足定位精度要 求，滿足則可以認(rèn)為P'就是最佳估計(jì)值，如不滿足，則可以利用誤差四元數(shù)C求解一個(gè)新的 估計(jì)初值，Pn?代替P '重復(fù)迭代過(guò)程；
[0212] pnew = p'c
[0213] 位置參數(shù)a，0與最佳估計(jì)四元數(shù)p的關(guān)系如下所示：
[0214] a = arcsin2(pip3+p〇p2)
[0215] p=-arctan[2(p, p, -p,p(i VXp,1; +pf -p；； +p：)]
[0216]在算法第一次運(yùn)行時(shí)，初始估計(jì)值選擇ps，ps為目標(biāo)a = 45°、0 = 45°時(shí)四元數(shù)理論 值，之后可以選擇上一次計(jì)算的最佳估計(jì)值P作為下一次定位的估計(jì)初值P ' ；
[0217] 至此目標(biāo)的極坐標(biāo)形式的位置參數(shù)(R，a，P)已經(jīng)全部求出，可以通過(guò)如下公式將 目標(biāo)的位置參數(shù)由極坐標(biāo)形式(R，a，0)轉(zhuǎn)換成直角坐標(biāo)形式(x，y，z);
[0218] x = Rcos(a)cos(0)
[0219] y = Rsin(a)cos(0)
[0220] z = Rsin(0)
[0221]利用姿態(tài)角的最佳四元數(shù)估計(jì)值q表示矩陣Y，q = qo+qii+q2j+q3k;
[0222] Y = qMq-1
[0223] 利用估計(jì)初值q'對(duì)矩陣Y進(jìn)行一次變換求解誤差四元數(shù)u，最佳四元數(shù)估計(jì)值與估 計(jì)初值的關(guān)系為q = q'u，其中u為誤差四元數(shù)u=l+uii+U2j+U3k，顯然，當(dāng)誤差四元數(shù)矢量部 分ui、U2、U3的值趨近于0時(shí)，姿態(tài)四元數(shù)估算值q '的值與最佳估計(jì)值q非常接近；
[0224] 求解姿態(tài)參數(shù)的過(guò)程如圖8所示，在Y矩陣的左右分別乘上估計(jì)初值q'和(q'r1，求 解出矩陣D的值，之后利用M矩陣構(gòu)造出等式通過(guò)上式可以求解出估計(jì)四元數(shù)u的 值，判斷四元數(shù)u是否滿足精度要求，如果誤差四元數(shù)u滿足定位要求，則可以認(rèn)為q'就是最 佳估計(jì)值，求出目標(biāo)的姿態(tài)參數(shù)，不滿足精度要求，則可以利用誤差四元數(shù)u求解一個(gè)新的 估計(jì)初值q new，qnew取代之前的估計(jì)初值q '重復(fù)迭代過(guò)程，在算法第一次運(yùn)行時(shí)，初始估計(jì)值 選擇qs，qs為目標(biāo)9 = 45°、《 =45°、cp=45°時(shí)四元數(shù)理論值，之后可以選擇上一次計(jì)算的最 佳估計(jì)值q作為下一次定位的估計(jì)初值q '。
[0225] 詳細(xì)過(guò)程如下：
[0226] 利用姿態(tài)角的最佳四元數(shù)估計(jì)值q表示矩陣Y，q = qo+qii+q2j+q3k;
[0227] Y = qMq-1
[0228] 利用估計(jì)初值q'對(duì)矩陣Y進(jìn)行一次變換求解誤差四元數(shù)u，最佳四元數(shù)估計(jì)值與估 計(jì)初值的關(guān)系為q = q'u，其中u為誤差四元數(shù)u=l+uii+U2j+U3k，顯然，當(dāng)誤差四元數(shù)矢量部 分ui、U2、U3的值趨近于0時(shí)，姿態(tài)四元數(shù)估算值q '的值與最佳估計(jì)值q非常接近；
[0229] 與X矩陣的計(jì)算過(guò)程類似，對(duì)Y的變換如下所示：
[0230] (q，）-Wq，=uMu-1= (I_2U)M [0231 ]其中U為誤差四元數(shù)組成的矩陣； 0 u3 -u2
[0232] U= -Uj 0 ll] u2 -u, 0
[0233] 整理上式，可以求解UM為：
[0234] UM=[M-(a，)_1Ya，]/2 = D
[0235] 將上述公式寫(xiě)成如下形式： '〇 % '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組等式中選擇三組獨(dú)立的等式，寫(xiě)成如下形式： 0 -m3t m2l ux dn
[0238] m32 0 -mI2 u2 = d12 _-m23 m13 0」[u3」Ltl13_
[0239] 將上式寫(xiě)成如下形式：
[0240] M'u=d
[0241] 通過(guò)上式可以求解出估計(jì)四元數(shù)u的值，之后判斷四元數(shù)u是否滿足精度要求，如 果誤差四元數(shù)u滿足定位要求，則可以認(rèn)為q'就是最佳估計(jì)值，不滿足精度要求，則可以利 用誤差四元數(shù)U求解一個(gè)新的估計(jì)初值q new，qnew取代之前的估計(jì)初值q'重復(fù)迭代過(guò)程，利用 誤差四元數(shù)計(jì)算估計(jì)初值的公式如下所示：
[0242] qnew=q'u
[0243] 姿態(tài)參數(shù)0、《與最佳估計(jì)四元數(shù)q的關(guān)系如下所示：
[0247] 在算法第一次運(yùn)行時(shí)，初始估計(jì)值選擇qs，qs為目標(biāo)0 = 45°、《 =45°、cp=45fi時(shí)四 元數(shù)理論值，之后可以選擇上一次計(jì)算的最佳估計(jì)值q作為下一次定位的估計(jì)初值q' ；
[0248] 至此通過(guò)多目標(biāo)六自由度電磁定位方法的步驟(三)計(jì)算出每個(gè)信號(hào)接收傳感器 的空間位置(x，y，z)及姿態(tài)角作典沖)。
[0249] 為驗(yàn)證反饋控制系統(tǒng)中算法對(duì)信號(hào)能量估計(jì)，將信號(hào)接收傳感器置于空間中9個(gè) 不同的位置并進(jìn)行了實(shí)驗(yàn)；
[0250]實(shí)驗(yàn)?zāi)康?br>[0251 ]驗(yàn)證反饋控制系統(tǒng)中算法對(duì)信號(hào)能量估計(jì)是否準(zhǔn)確。
[0252] 實(shí)驗(yàn)原理
[0253]因?yàn)樾盘?hào)的能量只與目標(biāo)的距離有關(guān)，所以可以認(rèn)為接收信號(hào)的能量與目標(biāo)的距 離是等價(jià)的，如果tr(T)可以精確地計(jì)算距離R，則證明利用tr(T)去估計(jì)信號(hào)的能量也會(huì)十 分精確，計(jì)算過(guò)程如下：
[0254] (1)復(fù)用信號(hào)經(jīng)濾波放大電路、采樣后，傳送到數(shù)據(jù)處理單元，數(shù)據(jù)處理單元根據(jù) 時(shí)鐘信號(hào)將采樣信號(hào)還原，組成信號(hào)接收矩陣Lx;
[0255] (2)求Lx的協(xié)方差矩陣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)是對(duì)應(yīng)增益等級(jí)Ks的一組常數(shù)。
[0262] 實(shí)驗(yàn)過(guò)程
[0263] (1)選擇空間中的9個(gè)位置，9個(gè)位置的距離逐漸增加，空間分布如圖9所示，在每個(gè) 位置測(cè)量5次信號(hào)。
[0264] (2)利用原理中的算法計(jì)算該位置的距離R，并與實(shí)際的距離相比較。
[0265] 實(shí)驗(yàn)結(jié)果
[0266] 定位結(jié)果
[0269]圖10是位置一二三的實(shí)驗(yàn)結(jié)果。
[0270]圖11是位置四五六的實(shí)驗(yàn)結(jié)果。
[0271 ]圖12是位置七八九的實(shí)驗(yàn)結(jié)果。
[0272] 結(jié)論
[0273] 9個(gè)位置的45組數(shù)據(jù)測(cè)量的距離R與實(shí)際的距離相比，最大誤差不超過(guò)0.2cm，故可 以認(rèn)為，驗(yàn)證反饋控制系統(tǒng)中算法對(duì)信號(hào)能量估計(jì)是準(zhǔn)確的。
【主權(quán)項(xiàng)】
1. 一種多目標(biāo)六自由度電磁定位裝置，其特征在于包括：信號(hào)發(fā)射單元、信號(hào)接收單 元、控制與數(shù)據(jù)處理單元； (1) 所述信號(hào)發(fā)射單元由：信號(hào)發(fā)射傳感器、濾波放大電路一、數(shù)模轉(zhuǎn)換電路順序連接， 控制與數(shù)據(jù)處理單元為信號(hào)發(fā)射單元提供發(fā)射信號(hào)，經(jīng)數(shù)模轉(zhuǎn)換電路、濾波放大電路一生 成驅(qū)動(dòng)信號(hào)，經(jīng)信號(hào)發(fā)射傳感器，形成電磁場(chǎng)； (2) 所述信號(hào)接收單元由：至少一個(gè)信號(hào)接收傳感器、第一級(jí)時(shí)分復(fù)用電路、濾波放大 電路二、第二級(jí)時(shí)分復(fù)用電路、模數(shù)轉(zhuǎn)換電路順序連接，其中濾波放大電路二的增益是可調(diào) 節(jié)的，信號(hào)接收傳感器感應(yīng)到電磁場(chǎng)產(chǎn)生電壓信號(hào)，信號(hào)經(jīng)多路復(fù)用、濾波放大后，由模數(shù) 轉(zhuǎn)換電路對(duì)其采樣，并傳送給控制與數(shù)據(jù)處理單元； (3) 所述控制與數(shù)據(jù)處理單元:包括DSP、SDRAM存儲(chǔ)器、FLASH存儲(chǔ)器、USB接口及電源， DSP讀取FLASH存儲(chǔ)器中的程序，存入DSP內(nèi)置RAM中，一方面產(chǎn)生控制電路的信號(hào)，另一方面 接收采樣信號(hào)并處理，計(jì)算過(guò)程中大容量數(shù)據(jù)會(huì)存儲(chǔ)到SDRAM中，通過(guò)USB接口實(shí)現(xiàn)裝置與 上位機(jī)通信，電源為裝置提供± 5V、3.3V電壓。2. 根據(jù)權(quán)利要求1所述的一種多目標(biāo)六自由度電磁定位裝置，其特征在于:所述信號(hào)發(fā) 射單元中信號(hào)發(fā)射傳感器由正交的三軸環(huán)形線圈組成，控制與數(shù)據(jù)處理單元為模數(shù)轉(zhuǎn)換電 路提供三組發(fā)射信號(hào)，經(jīng)濾波放大電路后，驅(qū)動(dòng)三軸線圈同時(shí)工作，發(fā)射信號(hào)采用正弦信號(hào) 的形式，頻率按照f(shuō) n = knfs的標(biāo)準(zhǔn)設(shè)計(jì)，其中1^為固定的整數(shù)，11 = 1、2、3，匕為裝置的定位刷 新頻率，根據(jù)這種標(biāo)準(zhǔn)生成的發(fā)射信號(hào)，在一個(gè)工作周期內(nèi)呈三組固定值，不再需要參考信 號(hào)回流。3. 根據(jù)權(quán)利要求1所述的一種多目標(biāo)六自由度電磁定位裝置，其特征在于:所述信號(hào)接 收單元中信號(hào)接收傳感器由正交的三軸環(huán)形線圈組成，裝置可支持一個(gè)以上信號(hào)接收傳感 器同時(shí)工作，當(dāng)置于信號(hào)發(fā)射單元產(chǎn)生的電磁場(chǎng)中，每個(gè)信號(hào)接收傳感器都能接收到三軸 信號(hào)，第一級(jí)時(shí)分復(fù)用電路將傳感器的三軸信號(hào)復(fù)用，經(jīng)濾波放大電路二后，第二級(jí)時(shí)分復(fù) 用電路將多個(gè)傳感器的信號(hào)復(fù)用，經(jīng)模數(shù)轉(zhuǎn)換電路采樣后傳送給控制與數(shù)據(jù)處理單元。4. 根據(jù)權(quán)利要求1所述的一種多目標(biāo)六自由度電磁定位裝置，其特征在于:所述控制與 數(shù)據(jù)處理單元，包括： (1) 控制與數(shù)據(jù)處理單元根據(jù)信號(hào)接收傳感器的數(shù)量，為第一級(jí)時(shí)分復(fù)用電路、第二級(jí) 時(shí)分復(fù)用電路提供控制信號(hào)； (2) 控制與數(shù)據(jù)處理單元為信號(hào)發(fā)射單元提供發(fā)射信號(hào)，為模數(shù)轉(zhuǎn)換電路提供采樣信 號(hào)，這些信號(hào)及控制信號(hào)均由同一時(shí)鐘信號(hào)生成，保證時(shí)鐘同步； (3) 控制與數(shù)據(jù)處理單元接收采樣信號(hào)，求解目標(biāo)的六自由度信息，并通過(guò)USB接口傳 送給上位機(jī)程序。5. 根據(jù)權(quán)利要求1所述的一種多目標(biāo)六自由度電磁定位裝置，其特征在于:所述控制與 數(shù)據(jù)處理單元、濾波放大電路二、第二級(jí)時(shí)分復(fù)用電路、模數(shù)轉(zhuǎn)換電路構(gòu)成反饋控制系統(tǒng)， 用于調(diào)節(jié)濾波放大電路二的增益。6. -種用于多目標(biāo)六自由度電磁定位的方法，其特征在于包括如下步驟： (一)根據(jù)定位需要，選擇信號(hào)接收傳感器的數(shù)量，并固定在定位目標(biāo)上，將信號(hào)發(fā)射傳 感器位置固定，作為空間的坐標(biāo)原點(diǎn)（〇,〇,〇)，開(kāi)始定位，由控制與數(shù)據(jù)處理單元產(chǎn)生發(fā)射 信號(hào)，經(jīng)數(shù)模轉(zhuǎn)換電路、濾波放大電路，驅(qū)動(dòng)信號(hào)發(fā)射傳感器，以信號(hào)發(fā)射傳感器為中心，形 成電磁場(chǎng)； (二) 接收傳感器通過(guò)電磁感應(yīng)，產(chǎn)生接收信號(hào)，并通過(guò)第一級(jí)時(shí)分復(fù)用電路將來(lái)自一 個(gè)信號(hào)接收傳感器的多路信號(hào)復(fù)用，信號(hào)經(jīng)濾波放大后，經(jīng)第二級(jí)時(shí)分復(fù)用電路對(duì)多個(gè)信 號(hào)接收傳感器的多路信號(hào)復(fù)用，由模數(shù)轉(zhuǎn)換電路采樣，并傳送給控制與數(shù)據(jù)處理單元，濾波 放大電路二、時(shí)分復(fù)用電路二、模數(shù)轉(zhuǎn)換電路、控制與數(shù)據(jù)處理單元構(gòu)成反饋控制系統(tǒng)，通 過(guò)輸入信號(hào)的能量調(diào)節(jié)濾波放大電路二的增益，保證目標(biāo)在大范圍移動(dòng)時(shí)接收信號(hào)穩(wěn)定， 按照電磁場(chǎng)強(qiáng)度與距離的衰減規(guī)律，將定位空間劃分成若干個(gè)區(qū)域，并為每個(gè)區(qū)域設(shè)定相 應(yīng)的增益等級(jí)，依次為KhKfKs; (三) 控制與數(shù)據(jù)處理單元接收采樣信號(hào)，執(zhí)行電磁定位算法，步驟如下： (1) 首先按照時(shí)鐘信號(hào)，從復(fù)用信號(hào)X(n)中將每個(gè)信號(hào)接收傳感器的三軸信號(hào)還原，組 成信號(hào)接收矩陣Lx，其中X表不傳感器的編號(hào)，L x是一個(gè)NX 3的矩陣，表不矩陣有3個(gè)陣元，N 個(gè)時(shí)間樣本序列，利用同步解調(diào)的方法，提取接收信號(hào)中三個(gè)頻率的成分，其中解調(diào)信號(hào)為 三組固定值81、沿、抑，解調(diào)后三組信號(hào)通過(guò)低通濾波、擬合等方法組成接收矩陣34 (2) 利用算法將接收矩陣Sx中的位置參數(shù)、姿態(tài)參數(shù)分離，構(gòu)造出兩個(gè)矩陣Xx、Yx，矩陣X x 只包含目標(biāo)的位置參數(shù)，矩陣Υχ只包含目標(biāo)的姿態(tài)參數(shù)，參數(shù)分離后可以對(duì)位置參數(shù)、姿態(tài) 參數(shù)并行求解，結(jié)果互不影響； (3) 利用閉環(huán)、迭代的四元數(shù)算法同時(shí)求解編號(hào)為X的傳感器的位置參數(shù)(x，y，z)和姿 態(tài)參數(shù)((Uwp)，并將結(jié)果通過(guò)USB接口傳送給上位機(jī)程序。7. 根據(jù)權(quán)利要求6所述的一種用于多目標(biāo)六自由度電磁定位的方法，其特征在于所述 步驟(二）中的反饋控制系統(tǒng)負(fù)責(zé)調(diào)節(jié)濾波放大電路二的增益； 復(fù)用信號(hào)經(jīng)濾波放大電路、采樣后，傳送到數(shù)據(jù)處理單元，數(shù)據(jù)處理單元根據(jù)時(shí)鐘信號(hào) 將采樣信號(hào)還原，組成信號(hào)接收矩陣Lx; 求Lx的協(xié)方差矩陣T: 求解矩陣T的跡，記為tr(T)，進(jìn)行閾值判斷，如果tr(T)的值低于tr(T)min將增益的等級(jí) 提高一級(jí)，如果tr(T)的值高于tr(T)max，則降低增益等級(jí)，該系統(tǒng)增益控制存在一個(gè)定位周 期的延遲； 按照電磁場(chǎng)強(qiáng)度與距離的衰減規(guī)律，將定位空間劃分成若干個(gè)區(qū)域，并為每個(gè)區(qū)域設(shè) 定相應(yīng)的增益等級(jí)，依次為h、Kds; 實(shí)際上，信號(hào)的能量只與目標(biāo)的距離有關(guān)，所以可以認(rèn)為接收信號(hào)的能量與目標(biāo)的距 離是等價(jià)的，故可以利用tr(T)去計(jì)算目標(biāo)的距離R，R的誤差也能直接反應(yīng)算法對(duì)接收信號(hào) 能量的估計(jì)是否準(zhǔn)確；其中Rs和tr(Ts)是對(duì)應(yīng)增益等級(jí)Ks的一組常數(shù)。8. 根據(jù)權(quán)利要求6所述的一種用于多目標(biāo)六自由度電磁定位的方法，其特征在于所述 步驟(三)中的電磁定位算法，具體實(shí)現(xiàn)步驟如下： 為方便描述，后文省略了矩陣Lx、Sx、Xx、Yx中的傳感器編號(hào)X，用L、S、X、Y表示； (1)還原復(fù)用信號(hào)，組建接收矩陣S; 控制與數(shù)據(jù)處理單元會(huì)根據(jù)時(shí)鐘信號(hào)，將復(fù)用信號(hào)x(n)還原，并按照順序存入信號(hào)接 收矩陣L中； L= [ll, 12, l3]T 對(duì)矩陣L中3個(gè)陣元的信號(hào)做同步解調(diào)，解調(diào)信號(hào)用gm表示，m=l、2、3對(duì)應(yīng)三軸發(fā)射線 圈，按照f(shuō)n=knfs的標(biāo)準(zhǔn)選擇復(fù)用信號(hào)頻率，gm為三組固定值，同步解調(diào)計(jì)算公式如下： lmn- lngm 經(jīng)同步解調(diào)后，結(jié)果lm是由m軸產(chǎn)生η軸接收的信號(hào)，lmn組成Lmn，Lmn-個(gè)3 X 3 X 1數(shù)組， 對(duì)數(shù)組做低通濾波、擬合處理后得到傳感器對(duì)應(yīng)的接收矩陣S; 接收矩陣S與發(fā)射矩陣I的耦合關(guān)系可以利用旋轉(zhuǎn)矩陣來(lái)表示； 定位過(guò)程中，信號(hào)耦合關(guān)系可以通過(guò)旋轉(zhuǎn)矩陣的方式描述，位置參數(shù)可以表示為兩個(gè) 旋轉(zhuǎn)矩陣變換和一個(gè)位移的結(jié)果，位移距離R，繞Z軸順時(shí)針旋轉(zhuǎn)角度為α，繞X軸順時(shí)針旋轉(zhuǎn) 角度為β，旋轉(zhuǎn)矩陣Ρ如下所示：同理，姿態(tài)可以通過(guò)三個(gè)旋轉(zhuǎn)矩陣來(lái)表示，其中繞Ζ軸順時(shí)針旋轉(zhuǎn)角度為Θ、表示繞X軸 順時(shí)針旋轉(zhuǎn)角度ω，表示繞Υ軸順時(shí)針旋轉(zhuǎn)角度φ，旋轉(zhuǎn)矩陣Q如下所示：根據(jù)磁偶極子原理可以求得信號(hào)發(fā)射傳感器、信號(hào)接收傳感器的耦合矩陣Η，如下所 示：綜上所述，若發(fā)射矩陣為單位矩陣I，則接收矩陣S可以用如下公式表示： S = kQPtHPI 式中k=電磁親合系數(shù)/衰減因子； (2)接收矩陣中位置參數(shù)與姿態(tài)參數(shù)分離； 求接收矩陣S的協(xié)方差矩陣； gts = k2ptH2p 求解左側(cè)矩陣和右側(cè)矩陣的跡，可以得出如下結(jié)果： ^(8^)=6^ 整理得： k=[tr(StS)/6](1/2) 距離參數(shù)R只與衰減因子k相關(guān)，可以通過(guò)下式求解； R=Rs(ks/k)(1/3) 公式中，ks、Rs是目標(biāo)處于增益等級(jí)Ks區(qū)域時(shí)對(duì)應(yīng)的參數(shù)； 利用解得的k值計(jì)算S的歸一化矩陣，用Μ表示，如下所示： M=QPtEP 將矩陣Μ中的兩類參數(shù)分離出來(lái)，只含有位置參數(shù)的矩陣X可以按如下公式計(jì)算： X = PtFP=(MtM-I)/3 式中F = dig(l，0,0); 只含有姿態(tài)參數(shù)的矩Y以通過(guò)下式求得： 丫 = 〇%=]\^-21 (3)求解目標(biāo)的位置參數(shù)(X，y，z)及姿態(tài)參數(shù)(〇,w,(p): 四元數(shù)是一種超復(fù)數(shù)，一個(gè)四元數(shù)c由一個(gè)實(shí)數(shù)單位1和三個(gè)虛數(shù)單位i、j、k組成，形式 如下czco+cd+uj+csk，與旋轉(zhuǎn)矩陣相比，四元數(shù)描述目標(biāo)的方向、姿態(tài)更加方便； 下面利用閉環(huán)迭代四元數(shù)算法，求解目標(biāo)的位置參數(shù)及姿態(tài)參數(shù)，兩種參數(shù)的迭代計(jì) 算過(guò)程互不影響，所以可以同時(shí)進(jìn)行； 將含有位置參數(shù)的矩陣X四元數(shù)的方式表示，如下所示： X=_-I)/3 = p(FP)p-1 式中P位置參數(shù)的最佳四元數(shù)估計(jì)值，P = po+pi i+P2 j+P3k; 最佳四元數(shù)估計(jì)值與估計(jì)初值的關(guān)系為p = p ' c，c為誤差四元數(shù)，可表示為c = l+cii + C2 j+C3k，顯然，當(dāng)誤差四元數(shù)矢量部分的值趨近于0時(shí)，估計(jì)初值p '與最佳四元數(shù)估計(jì)值p非 常接近； 首先，在X矩陣的左右分別乘上(Ρ'Γ^Ρ'，求解得到一個(gè)包含誤差四元數(shù)和位置參數(shù)的 矩陣，計(jì)算公式如下所示： Xi=(p，)_1Xp，=c(FP)c_1=(l-2C)(FP) 公式中矩陣C為：求心矩陣的轉(zhuǎn)置矩陣； X;'=p]F(卜 2Q](p')4 對(duì)X〗做和X矩陣同樣的變換；整理上式結(jié)果，由于4CFC的值非常小，故只保留等式右側(cè)的前三項(xiàng)，又因?yàn)槲恢脜?shù)不 存在沿Y軸旋轉(zhuǎn)的角度，所以誤差四元數(shù)C1 = C2C3,近似為0,公式右側(cè)等式整理如下所示：通過(guò)上式可以求解出估計(jì)四元數(shù)c的值，判斷誤差四元數(shù)c是否滿足定位精度要求，滿 足則可以認(rèn)為P'就是最佳估計(jì)值，如不滿足，則可以利用誤差四元數(shù)c求解一個(gè)新的估計(jì)初 值ρη?，替P '重復(fù)迭代過(guò)程； pnew-p C 位置參數(shù)α，β與最佳估計(jì)四元數(shù)p的關(guān)系如下所示： a = arcsin2(pip3+p〇p2)在算法第一次運(yùn)行時(shí)，初始估計(jì)值選擇ps，ps為目標(biāo)α = 45°、β = 45°時(shí)四元數(shù)理論值，之 后可以選擇上一次計(jì)算的最佳估計(jì)值Ρ作為下一次定位的估計(jì)初值Ρ ' ； 至此目標(biāo)的極坐標(biāo)形式的位置參數(shù)(R，a，i3)已經(jīng)全部求出，可以通過(guò)如下公式將目標(biāo) 的位置參數(shù)由極坐標(biāo)形式(R，α，β)轉(zhuǎn)換成直角坐標(biāo)形式(X，y，z); x = Rcos(a)cos(0) y = Rsin(a)cos(P) z = Rsin(0) 利用姿態(tài)角的最佳四元數(shù)估計(jì)值q表示矩陣Y，q = q〇+qii+q2j+q3k; Y = qMq-1 利用估計(jì)初值q'對(duì)矩陣Y進(jìn)行一次變換求解誤差四元數(shù)u，最佳四元數(shù)估計(jì)值與估計(jì)初 值的關(guān)系為q = q'u，其中u為誤差四元數(shù)u=l+uii+U2j+U3k，顯然，當(dāng)誤差四元數(shù)矢量部分 ui、U2、U3的值趨近于0時(shí)，姿態(tài)四元數(shù)估算值q'的值與最佳估計(jì)值q非常接近； 與X矩陣的計(jì)算過(guò)程類似，對(duì)Y的變換如下所示： (q' )_1Yq' =uMu_1= (I-2U)M 其中U為誤差四元數(shù)組成的矩陣；整理上式，可以求解UM為： UM=[M-(a'）-1Ya']/2 = D 將上述公式寫(xiě)成如下形式：從以上9組等式中選擇三組獨(dú)立的等式，寫(xiě)成如下形式：將上式寫(xiě)成如下形式：通過(guò)上式可以求解出估計(jì)四元數(shù)u的值，之后判斷四元數(shù)u是否滿足精度要求，如果誤 差四元數(shù)u滿足定位要求，則可以認(rèn)為q'就是最佳估計(jì)值，不滿足精度要求，則可以利用誤 差四元數(shù)U求解一個(gè)新的估計(jì)初值q ne3W，qne3W取代之前的估計(jì)初值q '重復(fù)迭代過(guò)程，利用誤差 四元數(shù)計(jì)算估計(jì)初值的公式如下所示： Qnew-Q U 姿態(tài)參數(shù)θ、ω、_φ與最佳估計(jì)四元數(shù)q的關(guān)系如下所示：在算法第一次運(yùn)行時(shí)，初始估計(jì)值選擇qs，qs為目標(biāo)θ = 45°、ω =45°、φ=45:(5時(shí)四元數(shù) 理論值，之后可以選擇上一次計(jì)算的最佳估計(jì)值q作為下一次定位的估計(jì)初值q' ； 至此通過(guò)多目標(biāo)六自由度電磁定位方法的步驟(三)計(jì)算出每個(gè)信號(hào)接收傳感器的空 間位置(X，y，z)及姿態(tài)角(Ο,ω,φ)
【文檔編號(hào)】G01S5/00GK105929362SQ201610236475
【公開(kāi)日】2016年9月7日
【申請(qǐng)日】2016年4月17日
【發(fā)明人】孫曉穎, 劉陽(yáng), 陳建, 劉壯, 劉妍妍
【申請(qǐng)人】吉林大學(xué)