基于膠囊磁定位算法穩定性的研究

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

一、膠囊定位系統原型搭建及分析

磁傳感器在定位精度中起至關重要作用。為保證結果準確性和可靠性，需采用靈敏度高、感應范圍廣、抗干擾能力強的傳感器進行實驗。為使結構簡單方便且更好減少能耗。本課題使用數字磁傳感器替代模擬磁傳感器。并且把放大器、多路復用器、模擬-數字轉換器都集成在數字傳感器組成的電路上。整個系統的設置和可編程濾波器都可嵌入到每個數字傳感器上。

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

表1　磁傳感器的比較

對兩種傳感器的穩定性進行測試，兩類傳感器在相同位置相同環境下測量地磁場。經過測試LSM303D相比AK09912傳感器穩定性更好[1]。最終選擇穩定性更好的LSM303D傳感器應用于本系統。

圖1為LSM303D傳感器組成平面傳感器陣列。運用數字磁傳感器后，傳感器陣列結構更加簡潔且可靠性也得到提高。

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

圖1　含有9個數字磁傳感器的平面傳感器陣列

圖2　磁鐵跟蹤系統原型

二、磁定位原型數學模型和算法研究

(一)磁軌跡跟蹤數學模型

圖3　磁偶極子模型

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

(1-1)

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

MT=πδ2LM0

(1-2)

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

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

m2+n2+p2=1

(1-3)

(二)非線性優化算法

本課題使用兩個或多個硬件磁傳感器測量磁鐵周圍磁通強度，并運用已測得數據和已有數學模型，通過非線性優化算法計算磁鐵位置參數和方向參數。通過擴展方程式(2-1)可得如下式子[4]：

(1-4)

(1-5)

(1-6)

(1-7)

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

三、傳感器陣列的校準分析及收斂性研究

為提高軌跡跟蹤系統精確度，每個用來標定校準的傳感器都彼此獨立。因為每個數字磁傳感器靈敏度出廠時都已被標定，所以只需標定每個傳感器位置和方向即可。

為保證校準精度，需要一個高精度校準板。校準板加工精度為0.1mm。校準初始階段，磁鐵被放置在校準板上多個比較特殊位置。磁鐵位置和方向將被輸出和記錄。標定過程中，只闡述第l個校準傳感器，其他傳感器用同樣方法來校準。

(一)傳感器位置校驗

(1-8)

公式中(Bix,Biy,Biz)T為被優化參數。校準位置可以通過將Esp做最小化處理得到。LM算法用來對Esp做最小化處理。

(二)傳感器方向校準

傳感器三個坐標軸不完全平行于跟蹤系統中相應的坐標軸將影響定位精確性。因此本課題采用抽樣數據向量變換法減小誤差。

(1-9)

M為變換矩陣，在跟蹤坐標系系統中把傳感器采集的數據轉換到三個投影坐標軸上的數據。求解矩陣M,定義客觀誤差函數如下[7]：

(1-10)

(三)校準方法的收斂性

傳感器位置和方向通過一次計算不能獲得的其最優解，需進行多次調整。根據最新M矩陣，傳感器可以定位出P點位置。根據最新P點位置可以使傳感器方向轉化成矩陣M。直到迭代中最大值完成或者參數的相對變化足夠小循環將終止。

如迭代不收斂，那么校準結果即為錯。如圖4所示為傳感器位置校準收斂趨勢，經過50次迭代后迭代將收斂。圖5所示為x軸方向傳感器方向校準的收斂性。經過60次迭代后迭代將收斂，相應Y軸和Z軸也有相似收斂趨勢，通過校準，得到傳感器位置和方向更加接近真實值[8]。

圖4　融合傳感器位置的校準

圖5　融合傳感器x軸方向的調整

四、磁定位原型系統的實驗結果分析

磁軌跡跟蹤系統的建立是為評測實時性、魯棒性、位置和方向精確性。

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

圖6　磁軌跡跟蹤系統界面Fig.6　Magnetic trajectory tracking system interface

同時對跟蹤系統的穩定性進行測試。永磁鐵放在坐標為x=30mm,y=30,z=96mm位置，如圖7所示為磁鐵在此位置600s期間的坐標變化。測試結果得出在不同位置誤差都在0.5mm之內。X軸、Y軸、Z軸標準誤差分別為0.094mm，0.113mm和0.048mm。

圖7　在600s內坐標上點的變化

校準板上的孔為永磁鐵移動的軌跡坐標，永磁鐵可以放置在預定準確位置。把磁鐵分別放置在距傳感器陣列高76mm和96mm的校準板上進行定位測試，最終定位值與永磁鐵預定位置值如圖8所示。測試結果中得到軌跡位置值和其預定值基本重合，同時驗證了校驗結果的準確性。

圖8　三維空間實際值和測量值誤差比對

從校驗結果和最后測試穩定性的仿真圖可以看到，在經過校驗優化后的磁定位系統無論是在方向上還是在位置上精度都達到了很高的要求，符合項目要求。