基于膠囊磁定位算法穩(wěn)定性的研究

(四川理工學(xué)院　四川　自貢　643000)

一、膠囊定位系統(tǒng)原型搭建及分析

磁傳感器在定位精度中起至關(guān)重要作用。為保證結(jié)果準(zhǔn)確性和可靠性，需采用靈敏度高、感應(yīng)范圍廣、抗干擾能力強(qiáng)的傳感器進(jìn)行實(shí)驗(yàn)。為使結(jié)構(gòu)簡(jiǎn)單方便且更好減少能耗。本課題使用數(shù)字磁傳感器替代模擬磁傳感器。并且把放大器、多路復(fù)用器、模擬-數(shù)字轉(zhuǎn)換器都集成在數(shù)字傳感器組成的電路上。整個(gè)系統(tǒng)的設(shè)置和可編程濾波器都可嵌入到每個(gè)數(shù)字傳感器上。

對(duì)各類磁傳感器進(jìn)行對(duì)比選出符合本課題要求的傳感器。表1-1列出可用磁傳感器幾個(gè)主要參數(shù)特性。傳感器之間主要差別為測(cè)量范圍和輸出分辨率。LSM303D傳感器分辨率最高，而AK09912傳感器測(cè)量范圍最大。為更好提高測(cè)量范圍和保證更好精確度，著重考慮傳感器測(cè)量范圍和分辨率兩個(gè)參數(shù)。因此LSM303D和AK09912被作為本課題首選。

表1　磁傳感器的比較

對(duì)兩種傳感器的穩(wěn)定性進(jìn)行測(cè)試，兩類傳感器在相同位置相同環(huán)境下測(cè)量地磁場(chǎng)。經(jīng)過(guò)測(cè)試LSM303D相比AK09912傳感器穩(wěn)定性更好[1]。最終選擇穩(wěn)定性更好的LSM303D傳感器應(yīng)用于本系統(tǒng)。

圖1為L(zhǎng)SM303D傳感器組成平面?zhèn)鞲衅麝嚵小＿\(yùn)用數(shù)字磁傳感器后，傳感器陣列結(jié)構(gòu)更加簡(jiǎn)潔且可靠性也得到提高。

依照模型搭建系統(tǒng)，此系統(tǒng)包含數(shù)字-輸出磁傳感器陣列、一個(gè)ARM控制器、一個(gè)校驗(yàn)板和一臺(tái)計(jì)算機(jī)。如圖2所示，系統(tǒng)中傳感器陣列中包含9個(gè)LSM303D傳感器。九個(gè)傳感器焊接在一塊面積為1.5×1.5m2電路板上。傳感器陣列尺寸在未來(lái)應(yīng)用中可以根據(jù)需求進(jìn)行增加或減少。ARM控制器STM32F103通過(guò)SPI總線讀取磁傳感器采集數(shù)據(jù)，并通過(guò)USB串口轉(zhuǎn)UART接口傳輸?shù)诫娔X。計(jì)算機(jī)進(jìn)行位置計(jì)算，最終把磁鐵位置、方向等信息實(shí)時(shí)顯示在電腦屏幕。傳感器陣列和ARM控制器在整個(gè)過(guò)程中共消耗455mW功耗，而傳感器陣列只消耗45mW[2]。

圖1　含有9個(gè)數(shù)字磁傳感器的平面?zhèn)鞲衅麝嚵?/p>

圖2　磁鐵跟蹤系統(tǒng)原型

二、磁定位原型數(shù)學(xué)模型和算法研究

(一)磁軌跡跟蹤數(shù)學(xué)模型

圖3　磁偶極子模型

如圖3所示，(a,b,c)為磁鐵中心，(xl,yl,zl)為第l個(gè)傳感器位置。H0為磁鐵從南極到北極的某個(gè)向量(m,n,p)T。Pl為從點(diǎn)(a,b,c)到點(diǎn)(xl,yl,zl)向量?？臻g中某點(diǎn)磁通密度Bl可以用如下公式表示[3]：

(1-1)

N為傳感器總數(shù)；μr為媒介相對(duì)滲透率(空氣中，μr≈1)；μ0為空氣磁導(dǎo)率(μ0=4π·10-7T·m/A)；Rl為Pl的長(zhǎng)度，MT為某個(gè)與磁鐵相關(guān)的常數(shù)，其可用如下公式表示：

MT=πδ2LM0

(1-2)

δ表示圓形磁鐵半徑，L表示圓形磁鐵長(zhǎng)度，M0(單位/m)表示磁化磁鐵表面。

當(dāng)圓柱形磁鐵沿主軸旋轉(zhuǎn)時(shí)，Bl將保持不變和對(duì)稱。因此不能獲得旋轉(zhuǎn)角度。H0長(zhǎng)度不受磁鐵方向影響，因此需添加(1-3)作為約束條件：

m2+n2+p2=1

(1-3)

(二)非線性優(yōu)化算法

本課題使用兩個(gè)或多個(gè)硬件磁傳感器測(cè)量磁鐵周圍磁通強(qiáng)度，并運(yùn)用已測(cè)得數(shù)據(jù)和已有數(shù)學(xué)模型，通過(guò)非線性優(yōu)化算法計(jì)算磁鐵位置參數(shù)和方向參數(shù)。通過(guò)擴(kuò)展方程式(2-1)可得如下式子[4]：

(1-4)

(1-5)

(1-6)

(1-7)

求解公式(1-7)最優(yōu)解常用方法有LM算法、Powell算法和DIRECT算法。結(jié)合本系統(tǒng)對(duì)實(shí)時(shí)性和精確性的要求，最終選擇LM算法作為最優(yōu)解算法。LM算法解決多元函數(shù)的方法為非線性函數(shù)迭代的平方和，融合最陡下降全局收斂和快速牛頓法局部收斂的優(yōu)點(diǎn)。

三、傳感器陣列的校準(zhǔn)分析及收斂性研究

為提高軌跡跟蹤系統(tǒng)精確度，每個(gè)用來(lái)標(biāo)定校準(zhǔn)的傳感器都彼此獨(dú)立。因?yàn)槊總€(gè)數(shù)字磁傳感器靈敏度出廠時(shí)都已被標(biāo)定，所以只需標(biāo)定每個(gè)傳感器位置和方向即可。

為保證校準(zhǔn)精度，需要一個(gè)高精度校準(zhǔn)板。校準(zhǔn)板加工精度為0.1mm。校準(zhǔn)初始階段，磁鐵被放置在校準(zhǔn)板上多個(gè)比較特殊位置。磁鐵位置和方向?qū)⒈惠敵龊陀涗?。?biāo)定過(guò)程中，只闡述第l個(gè)校準(zhǔn)傳感器，其他傳感器用同樣方法來(lái)校準(zhǔn)。

(一)傳感器位置校驗(yàn)

(1-8)

公式中(Bix,Biy,Biz)T為被優(yōu)化參數(shù)。校準(zhǔn)位置可以通過(guò)將Esp做最小化處理得到。LM算法用來(lái)對(duì)Esp做最小化處理。

(二)傳感器方向校準(zhǔn)

傳感器三個(gè)坐標(biāo)軸不完全平行于跟蹤系統(tǒng)中相應(yīng)的坐標(biāo)軸將影響定位精確性。因此本課題采用抽樣數(shù)據(jù)向量變換法減小誤差。

(1-9)

M為變換矩陣，在跟蹤坐標(biāo)系系統(tǒng)中把傳感器采集的數(shù)據(jù)轉(zhuǎn)換到三個(gè)投影坐標(biāo)軸上的數(shù)據(jù)。求解矩陣M,定義客觀誤差函數(shù)如下[7]：

(1-10)

(三)校準(zhǔn)方法的收斂性

傳感器位置和方向通過(guò)一次計(jì)算不能獲得的其最優(yōu)解，需進(jìn)行多次調(diào)整。根據(jù)最新M矩陣，傳感器可以定位出P點(diǎn)位置。根據(jù)最新P點(diǎn)位置可以使傳感器方向轉(zhuǎn)化成矩陣M。直到迭代中最大值完成或者參數(shù)的相對(duì)變化足夠小循環(huán)將終止。

如迭代不收斂，那么校準(zhǔn)結(jié)果即為錯(cuò)。如圖4所示為傳感器位置校準(zhǔn)收斂趨勢(shì)，經(jīng)過(guò)50次迭代后迭代將收斂。圖5所示為x軸方向傳感器方向校準(zhǔn)的收斂性。經(jīng)過(guò)60次迭代后迭代將收斂，相應(yīng)Y軸和Z軸也有相似收斂趨勢(shì)，通過(guò)校準(zhǔn)，得到傳感器位置和方向更加接近真實(shí)值[8]。

圖4　融合傳感器位置的校準(zhǔn)

圖5　融合傳感器x軸方向的調(diào)整

四、磁定位原型系統(tǒng)的實(shí)驗(yàn)結(jié)果分析

磁軌跡跟蹤系統(tǒng)的建立是為評(píng)測(cè)實(shí)時(shí)性、魯棒性、位置和方向精確性。

磁鐵以3cm/s速度在離傳感器陣列10cm高度上進(jìn)行移動(dòng)，圖6所示為其軌跡跟蹤。它顯示磁鐵以20HZ頻率更新運(yùn)動(dòng)的2D和3D軌跡數(shù)據(jù)。

圖6　磁軌跡跟蹤系統(tǒng)界面Fig.6　Magnetic trajectory tracking system interface

同時(shí)對(duì)跟蹤系統(tǒng)的穩(wěn)定性進(jìn)行測(cè)試。永磁鐵放在坐標(biāo)為x=30mm,y=30,z=96mm位置，如圖7所示為磁鐵在此位置600s期間的坐標(biāo)變化。測(cè)試結(jié)果得出在不同位置誤差都在0.5mm之內(nèi)。X軸、Y軸、Z軸標(biāo)準(zhǔn)誤差分別為0.094mm，0.113mm和0.048mm。

圖7　在600s內(nèi)坐標(biāo)上點(diǎn)的變化

校準(zhǔn)板上的孔為永磁鐵移動(dòng)的軌跡坐標(biāo)，永磁鐵可以放置在預(yù)定準(zhǔn)確位置。把磁鐵分別放置在距傳感器陣列高76mm和96mm的校準(zhǔn)板上進(jìn)行定位測(cè)試，最終定位值與永磁鐵預(yù)定位置值如圖8所示。測(cè)試結(jié)果中得到軌跡位置值和其預(yù)定值基本重合，同時(shí)驗(yàn)證了校驗(yàn)結(jié)果的準(zhǔn)確性。

圖8　三維空間實(shí)際值和測(cè)量值誤差比對(duì)

從校驗(yàn)結(jié)果和最后測(cè)試穩(wěn)定性的仿真圖可以看到，在經(jīng)過(guò)校驗(yàn)優(yōu)化后的磁定位系統(tǒng)無(wú)論是在方向上還是在位置上精度都達(dá)到了很高的要求，符合項(xiàng)目要求。