樓狄凱 ,張丹 ,梁華庚

(1.浙江工業(yè)大學(xué)信息工程學(xué)院,浙江杭州 310014;2.華中科技大學(xué)同濟(jì)醫(yī)學(xué)院附屬協(xié)和醫(yī)院,湖北武漢 430022)

1 引言

穿刺手術(shù)是現(xiàn)代醫(yī)學(xué)中使用較為普遍的一種微創(chuàng)手術(shù),具有對(duì)患者創(chuàng)傷小,傷口恢復(fù)快的優(yōu)點(diǎn),廣泛應(yīng)用于病灶組織采樣、藥劑注射、近距離放化療等領(lǐng)域,是醫(yī)學(xué)診斷、治療的重要手段.傳統(tǒng)剛性針由于組織變形、針尖受力不均勻等帶來(lái)的較大誤差,在穿刺過(guò)程中難以對(duì)針尖位置進(jìn)行有效的控制,容易造成穿刺失敗[1].穿刺任務(wù)往往需要多次重復(fù)的進(jìn)行才能完成,這會(huì)延長(zhǎng)手術(shù)時(shí)間,增加患者痛苦,影響治療效果.且在一些進(jìn)針過(guò)程中需要避開(kāi)神經(jīng)、血管骨骼等障礙物,傳統(tǒng)剛性針很難滿(mǎn)足這個(gè)要求.針對(duì)傳統(tǒng)穿刺針的缺點(diǎn),Webster III等人[1]提出了柔性針的概念.柔性針采用彈性較好的合金材料,針尖采用非對(duì)稱(chēng)斜角的形式,通過(guò)組織施加在針尖的非對(duì)稱(chēng)力使針尖軌跡發(fā)生偏轉(zhuǎn),通過(guò)控制針尖斜面的朝向控制針尖的偏轉(zhuǎn)方向,從而實(shí)現(xiàn)對(duì)針尖位置的控制.針尖位置的準(zhǔn)確控制對(duì)成功穿刺至關(guān)重要.Webster III等人[1]首先提出了柔性針的概念,根據(jù)柔性針的運(yùn)動(dòng)形式建立了柔性針的“自行車(chē)模型”.Kallem等人[2]將圖像引導(dǎo)與柔性針穿刺系統(tǒng)相結(jié)合,在Webster III等人提出的“自行車(chē)模型”基礎(chǔ)上引入了圖像引導(dǎo)系統(tǒng),通過(guò)基于觀(guān)測(cè)器的反饋控制來(lái)實(shí)現(xiàn)對(duì)柔性針針尖位置的控制,并給出“自行車(chē)模型”的狀態(tài)空間模型.Park 和Kim等人[3]在“自行車(chē)模型”的基礎(chǔ)上將柔性針模型簡(jiǎn)化為“獨(dú)輪車(chē)模型”.Zhao等人[4]以實(shí)際軌跡和誤差分析為依據(jù),對(duì)“獨(dú)輪車(chē)模型”進(jìn)行改進(jìn),提出了帶返程獨(dú)輪車(chē)模型.Abayazid等人[5]以“獨(dú)輪車(chē)模型”為基礎(chǔ)提出了一個(gè)包含軌跡切割角的柔性針運(yùn)動(dòng)學(xué)模型,同時(shí)結(jié)合針與組織之間的相互作用,提出了基于力學(xué)的柔性針模型.

需要指出的是,柔性針在實(shí)際穿刺的過(guò)程中,針尖的位姿往往無(wú)法完全獲取.Benam和Talebi等人[6]提出了一種高增益觀(guān)測(cè)器對(duì)針尖狀態(tài)進(jìn)行估計(jì),并將估計(jì)狀態(tài)用于反饋控制率設(shè)計(jì),通過(guò)高增益觀(guān)測(cè)器和輸出反饋控制可以提高柔性針穿刺的精度.Huo等人[7]將無(wú)跡卡爾曼濾波算法(unscent Kalman filter,UKF)應(yīng)用到針尖位姿的估計(jì)中,并提出了一種基于針尖可達(dá)范圍的控制方法對(duì)柔性針穿刺過(guò)程進(jìn)行控制.Favaro等人[8]提出了一種使用基于2-D運(yùn)動(dòng)學(xué)的擴(kuò)展卡爾曼算法(extend Kalman filter,EKF)估計(jì)針尖的姿態(tài),并通過(guò)實(shí)驗(yàn)驗(yàn)證了該算法的性能.Kaya等人[9]提出了一種基于Gabor濾波器的圖像處理算法對(duì)針尖進(jìn)行估計(jì),并用卡爾曼濾波器降低估計(jì)噪聲,實(shí)現(xiàn)針尖的實(shí)時(shí)跟蹤估計(jì).Agarwal等人[10]在柔性針穿刺過(guò)程中將圖像處理技術(shù)與卡爾曼濾波算法相結(jié)合來(lái)提高算法的估計(jì)精度.Yan等人[11]提出了一種結(jié)合改進(jìn)的壓縮跟蹤算法和自適應(yīng)卡爾曼濾波器的跟蹤算法,以實(shí)現(xiàn)在2D超聲環(huán)境下的柔性針針尖實(shí)時(shí)跟蹤.Mignon等人[12]人提出了一種新的3D超聲柔性針檢測(cè)方法,利用一種新的基于力學(xué)的預(yù)測(cè)模型和卡爾曼濾波器提高已有算法的魯棒性減少檢測(cè)噪聲和反誤檢測(cè).Lapouge等人[13]在“獨(dú)輪車(chē)模型”的基礎(chǔ)上提出了一種多速率無(wú)跡卡爾曼濾波器應(yīng)用于柔性針針尖的轉(zhuǎn)向,實(shí)現(xiàn)來(lái)自3D圖像、機(jī)器人傳感器和術(shù)前彈性成像測(cè)量的異步數(shù)據(jù)融合.Favaro和Secoli等人[14]提出了一種可操縱針全位姿估計(jì)方法,該方法在建立了一種新型的PBN三維運(yùn)動(dòng)學(xué)模型,并通過(guò)擴(kuò)展卡爾曼濾波來(lái)預(yù)測(cè)針在插入過(guò)程中的完全位姿.Zhao等人[15]在“自行車(chē)模型”的基礎(chǔ)上,分別采用自適應(yīng)卡爾曼濾波算法(adaptive Kalman filter,AKF)、無(wú)跡濾波算法和自適應(yīng)無(wú)跡卡爾曼濾波算法(adaptive unscent Kalman filter,AUKF)3種算法對(duì)柔性針的針尖位置進(jìn)行估計(jì),并比較了3種算法的差異.然而在實(shí)際穿測(cè)過(guò)程中,由于活體組織本身的復(fù)雜性使得穿刺過(guò)程存在不確定性[1],同時(shí)由于呼吸作用等影響,超聲成像等量測(cè)設(shè)備工作環(huán)境產(chǎn)生變化,導(dǎo)致目標(biāo)觀(guān)測(cè)噪聲的方差實(shí)時(shí)變化[16].現(xiàn)有的估計(jì)算法并未考慮柔性針實(shí)際穿刺過(guò)程中的不確定性問(wèn)題和超聲成像等設(shè)備存在的量測(cè)噪聲統(tǒng)計(jì)特性不準(zhǔn)確性問(wèn)題,這也是本文的研究動(dòng)機(jī).

本文針對(duì)柔性針穿刺過(guò)程存在的不確定性問(wèn)題以及超聲成像等設(shè)備存在的量測(cè)噪聲統(tǒng)計(jì)特征不準(zhǔn)確性問(wèn)題,提出了一種帶有噪聲估計(jì)器的自適應(yīng)奇異值分解無(wú)跡卡爾曼濾波算法(adaptive singular value decomposition unscented Kalman fliter,ASVD–UKF).該算法采用自適應(yīng)因子實(shí)時(shí)修正動(dòng)力學(xué)模型誤差,通過(guò)奇異值分解抑制系統(tǒng)狀態(tài)協(xié)方差矩陣的負(fù)定性,利用Sage-Husa估計(jì)器在線(xiàn)估計(jì)系統(tǒng)噪聲的統(tǒng)計(jì)特性,提高濾波精度,減小系統(tǒng)狀態(tài)的估計(jì)誤差.并且將新算法應(yīng)用于帶有不定性的柔性針穿刺模型進(jìn)行計(jì)算仿真,結(jié)果證明,新算法能有效改善濾波性能,提高系統(tǒng)狀態(tài)的估計(jì)精度.

2 柔性針模型

Webster III等人首先提出了柔性針穿刺過(guò)程的自行車(chē)模型[1].當(dāng)斜面的柔性針插入組織時(shí),在針尖不對(duì)稱(chēng)力的作用下,針尖軌跡將遵循一個(gè)曲率為κ的圓弧軌跡.如果針的材料屬性預(yù)期穿刺組織的屬性相匹配,針體的軌跡將幾乎完全遵循針尖的軌跡,即針尖的軌跡可以完全代表針體軌跡.

如圖1所示,定義世界坐標(biāo)系A(chǔ)和體坐標(biāo)系B,C,帶斜面的柔性針由兩個(gè)針基的控制輸入驅(qū)動(dòng)u1和u2,即針的進(jìn)給和自轉(zhuǎn).此系統(tǒng)為非完整約束系統(tǒng),針尖在體坐標(biāo)系B中的速度可以表示為

圖1 運(yùn)動(dòng)學(xué)自行車(chē)模型[1]Fig.1 The kinematic bicycle model[1]

其中:u1為柔性針穿刺速度,使針在體坐標(biāo)系z(mì)軸上的移動(dòng),同時(shí)由于針尖斜面的不對(duì)稱(chēng)作用力產(chǎn)生針體關(guān)于x軸的旋轉(zhuǎn);u2為柔性針針基旋轉(zhuǎn)速度,使針體在體坐標(biāo)系z(mì)軸方向上的旋轉(zhuǎn);V1,V2為對(duì)應(yīng)的控制向量,

其中:ei為單位向量,κ為穿刺曲率.定義一組廣義坐標(biāo)q=[x y z α β γ]T,用于表示針尖的姿態(tài),其中x,y,z表示針尖在坐標(biāo)系A(chǔ)中的位置.α,β,γ分別為繞坐標(biāo)系A(chǔ)中x,y,z軸的角度.針尖在世界坐標(biāo)系中的速度可以表示為

其中RT為坐標(biāo)系A(chǔ),B之間的旋轉(zhuǎn)矩陣,針尖的運(yùn)動(dòng)學(xué)模型可以寫(xiě)成如下形式:

當(dāng)針尖穩(wěn)定至y–z平面時(shí),系統(tǒng)狀態(tài)[y z α]對(duì)其他狀態(tài)[x β γ]的動(dòng)態(tài)過(guò)程沒(méi)有影響,柔性針模型可以降階為下述三維非線(xiàn)性模型:

值得注意的是,當(dāng)針體插入活體組織時(shí),由于組織本身并不是均一介質(zhì),具有不定性,這有可能會(huì)使針尖軌跡的曲率發(fā)生變化而并非保持在一個(gè)常數(shù)[1].本文記曲率的變化為?κ,實(shí)際的曲率為

記s=[s1s2s3]T=[x β γ],設(shè)針基插入速度恒定為u1=1,柔性針運(yùn)動(dòng)模型可以表示為

其中s1為針尖的x軸坐標(biāo),可通過(guò)超聲成像設(shè)備直接量測(cè).實(shí)際穿刺過(guò)程中,由于穿刺環(huán)境復(fù)雜性和傳感器噪聲,量測(cè)值將受到量測(cè)噪聲影響而偏離實(shí)際真值.

穿刺模型為單輸入,單輸出的仿射非線(xiàn)性系統(tǒng),對(duì)非線(xiàn)性模型進(jìn)行反饋線(xiàn)性化處理.記z=[z1z2z3]T,

其中:?a1=?κ·κ,?a1=?κ/κ.

設(shè)計(jì)線(xiàn)性反饋控制率如下:

反饋線(xiàn)性化形式的狀態(tài)方程為

其中:

3 自適應(yīng)SVD–UKF算法

本文針對(duì)柔性針穿刺過(guò)程存在的不確定性問(wèn)題以及超聲成像等設(shè)備存在的量測(cè)噪聲統(tǒng)計(jì)特征不準(zhǔn)確性問(wèn)題,提出了一種帶有噪聲估計(jì)器的自適應(yīng)奇異值分解無(wú)跡卡爾曼濾波算法.該算法采用自適應(yīng)因子實(shí)時(shí)修正動(dòng)力學(xué)模型誤差,通過(guò)奇異值分解抑制系統(tǒng)狀態(tài)協(xié)方差矩陣的負(fù)定性,利用Sage-Husa估計(jì)器在線(xiàn)估計(jì)未知系統(tǒng)噪聲的統(tǒng)計(jì)特性,減小狀態(tài)估計(jì)誤差.

3.1 奇異值分解

由于迭代過(guò)程中的截?cái)嗾`差、模型擾動(dòng)等因素,誤差協(xié)方差矩陣Pk容易失去對(duì)稱(chēng)正定性,致使算法無(wú)法進(jìn)行cholesky分解[17].本文采用奇異值分解(singular value decomposition,SVD)替代cholesky分解進(jìn)行Sigma采樣.奇異值分解是一種具有良好數(shù)值穩(wěn)定性的矩陣分解算法,其特點(diǎn)是不需要被分解矩陣是對(duì)稱(chēng)正定矩陣[17].具體如下:

假設(shè)P∈Rm×n(m≥n),則矩陣P的SVD分解可以表示為

其中:U∈Rm×m,V∈Rn×n分別為A的左、右奇異向量,Λ∈Rm×n,S=diag{s1,s2,···,sr},s1≥s2≥···≥sr為P的奇異值.

3.2 自適應(yīng)因子

考慮如下非線(xiàn)性系統(tǒng):

其中:xk為k時(shí)刻的系統(tǒng)狀態(tài)向量,ωk為系統(tǒng)噪聲,yk為觀(guān)測(cè)向量,φk為量測(cè)噪聲,f(·)為狀態(tài)模型,h(·)為觀(guān)測(cè)模型.過(guò)程噪聲和測(cè)量噪聲的統(tǒng)計(jì)特性分別為ωk ～N(0,Q),φk+1～N(0,R).以預(yù)測(cè)殘差為變量,構(gòu)造狀態(tài)模型的誤差判別統(tǒng)計(jì)量及自適應(yīng)因子.預(yù)測(cè)殘差表示為

其中:αk表示自適應(yīng)因子,c為檢驗(yàn)常數(shù),通常情況下1

3.3 次優(yōu)Sage-Husa估計(jì)器

對(duì)于穿刺過(guò)程中觀(guān)測(cè)噪聲的不定性,用Sage-Husa估計(jì)器對(duì)進(jìn)行估計(jì).對(duì)于時(shí)變?cè)肼?應(yīng)強(qiáng)調(diào)新近數(shù)據(jù)的作用,對(duì)于陳舊數(shù)據(jù)的作用應(yīng)該漸漸遺忘和消失,不同于算術(shù)平均,應(yīng)對(duì)和式中的每項(xiàng)乘以不同的加權(quán)系數(shù),采用漸消記憶指數(shù)加權(quán)法實(shí)現(xiàn),次優(yōu)Sage-Husa估計(jì)器與卡爾曼濾波器結(jié)合,可以得到改進(jìn)的適用于帶時(shí)變?cè)肼曁匦缘木€(xiàn)性系統(tǒng)的自適應(yīng)濾波器[16].可由時(shí)變?cè)肼暪烙?jì)器遞推獲得

其中dk?1=(1?b)/(1?bk),b為遺忘因子,通常b的范圍為0.95

3.4 ASVD–UKF算法步驟

2)基于奇異值分解構(gòu)建Sigma 點(diǎn)對(duì)協(xié)方差矩陣Pk?1進(jìn)行奇異值分解

式中:Uk?1,Vk?1列向量分別為方差矩陣Pk?1的左、右奇異向量,Λ為方差矩陣Pk?1的奇異值矩陣.經(jīng)過(guò)SVD分解得到的Sigma點(diǎn)矩陣如下:

3)時(shí)間更新

4)量測(cè)更新

注1近年來(lái),卡爾曼濾波器在許多工程系統(tǒng)中得到了實(shí)際應(yīng)用.例如,Julier等人[18]提出了基于無(wú)跡變換的卡爾曼濾波算法,利用無(wú)跡變換估計(jì)系統(tǒng)狀態(tài)的均值和協(xié)方差,能夠較好的處理非線(xiàn)性系統(tǒng)的濾波問(wèn)題.與擴(kuò)展卡爾曼濾波算法相比,無(wú)跡濾波算法不需要計(jì)算Jacobian矩陣,計(jì)算更為簡(jiǎn)單,估計(jì)精度更高于擴(kuò)展卡爾曼濾波算法[19].但由于無(wú)跡濾波算法可能存在協(xié)方差陣的負(fù)定性,致使濾波病態(tài)問(wèn)題的出現(xiàn).Wang等人[17]提出了一種自適應(yīng)奇異值分解無(wú)跡濾波算法應(yīng)用于車(chē)輛質(zhì)心側(cè)偏角估計(jì),通過(guò)用奇異值分解代替cholesky分解,抑制系統(tǒng)狀態(tài),協(xié)方差矩陣負(fù)定性問(wèn)題,同時(shí)通過(guò)自適應(yīng)因子調(diào)節(jié)動(dòng)力學(xué)模型誤差,提高算法的跟蹤能力.但由于柔性針穿刺環(huán)境的復(fù)雜性,會(huì)導(dǎo)致觀(guān)測(cè)噪聲的方差發(fā)生實(shí)時(shí)變化.卡爾曼濾波器在噪聲環(huán)境變化時(shí)會(huì)出現(xiàn)濾波發(fā)散[16].Shi等人[20]將Sage-Husa估計(jì)器與無(wú)跡濾波算法結(jié)合,提出一種自適應(yīng)的無(wú)跡濾波算法,在線(xiàn)估計(jì)噪聲的統(tǒng)計(jì)特性,減小狀態(tài)估計(jì)誤差.本文將奇異值分解無(wú)跡卡爾曼濾波算法與Sage-Husa估計(jì)器相結(jié)合,應(yīng)用于柔性針的穿刺狀態(tài)估計(jì).相較于現(xiàn)有的UKF,AUKF算法,本文提出的自適應(yīng)的SVD–UKF算法能夠利用自適應(yīng)因子實(shí)時(shí)修正穿刺過(guò)程中曲率不定性帶來(lái)的估計(jì)偏差,同時(shí)用SVD分解替代cholesky分解,抑制穿刺估計(jì)時(shí)濾波病態(tài)問(wèn)題的出現(xiàn),通過(guò)Sage-Husa估計(jì)器減小由于超聲成像系統(tǒng)等設(shè)備帶來(lái)的觀(guān)測(cè)噪聲干擾,改善濾波性能,提高系統(tǒng)狀態(tài)的估計(jì)精度.

4 仿真

本文采用MATLAB軟件進(jìn)行仿真,通過(guò)比較針尖位姿的估計(jì)與真實(shí)位姿驗(yàn)證觀(guān)測(cè)器的估計(jì)精度與魯棒性.為了評(píng)估曲率不定性對(duì)觀(guān)測(cè)器估計(jì)精度的影響,仿真模擬了在不同的曲率偏差下無(wú)跡卡爾曼濾波算法對(duì)位姿估計(jì)的情況.設(shè)定系統(tǒng)初始狀態(tài)為

采樣時(shí)間?T=0.05 s,軌跡曲率κ=1/122.設(shè)定曲率偏差在±2%范圍內(nèi)隨機(jī)波動(dòng),UKF估計(jì)曲線(xiàn)如圖2所示.

圖2 ±2%曲率偏差UKF估計(jì)曲線(xiàn)Fig.2 The UKF estimation with±2%curvature deviation

圖2中紅色虛線(xiàn)和藍(lán)色虛線(xiàn)分別表示沒(méi)有曲率偏差和存在±2%曲率偏差情況下估計(jì)器的跟蹤曲線(xiàn).當(dāng)曲率偏差在±2%范圍內(nèi)隨機(jī)波動(dòng)時(shí),觀(guān)測(cè)曲線(xiàn)難以精確跟蹤實(shí)際的軌跡曲線(xiàn),觀(guān)測(cè)器估計(jì)精度下降.為了精確描述曲率不定性對(duì)估計(jì)精度的影響,仿真分別在曲率偏差為±2%和±5%的情況下進(jìn)行模擬50次,得到如表1所示各情況的數(shù)據(jù)統(tǒng)計(jì)結(jié)果.

表1 不同曲率偏差下的UKF估計(jì)誤差Table 1 The UKF estimation errors under different curvature deviations

從表1可以看出,當(dāng)曲率偏差存在±2%的隨機(jī)波動(dòng)時(shí),UKF算法的平均估計(jì)誤差從0.0538 mm增大到0.1008 mm,估計(jì)誤差增大87.36%.當(dāng)曲率偏差以±5%波動(dòng)時(shí),估計(jì)平均誤差增大至為0.1242 mm,均方根誤差增大至0.207.由于曲率不定性帶來(lái)的偏差將會(huì)降低觀(guān)測(cè)器的估計(jì)精度.

為了評(píng)估噪聲統(tǒng)計(jì)特征不準(zhǔn)確的情況對(duì)濾波算法估計(jì)精度的影響,仿真分別模擬了量測(cè)噪聲在±5%和±10%隨機(jī)偏差情況下,UKF濾波算法的估計(jì)情況.

圖3中紅藍(lán)綠三色虛線(xiàn)分別表示在沒(méi)有噪聲統(tǒng)計(jì)特征偏差情況下UKF估計(jì)器的跟蹤曲線(xiàn)和在±5%噪聲偏差情況下以及在±10%噪聲偏差情況下估計(jì)器的跟蹤曲線(xiàn).從圖3可以看出,當(dāng)噪聲統(tǒng)計(jì)特征存在偏差時(shí),無(wú)跡濾波算法難以精確跟蹤真實(shí)曲線(xiàn),噪聲偏差越大,跟蹤曲線(xiàn)波動(dòng)越大.為了定量描述噪聲統(tǒng)計(jì)特征偏差估計(jì)精度的影響,仿真分別在偏差為±5%和±10%的情況下模擬50次,統(tǒng)計(jì)結(jié)果如表2所示.

圖3 噪聲偏差下UKF 估計(jì)曲線(xiàn)Fig.3 The UKF estimation with measurement noise deviation

表2 不同噪聲偏差下的UKF估計(jì)誤差Table 2 The UKF estimation errors under different measurement noise deviations

從表2可以看出,當(dāng)噪聲存在±5%的偏差隨機(jī)波動(dòng)時(shí),UKF觀(guān)測(cè)器的平均估計(jì)誤差從0.0946 mm增大到0.1422 mm.當(dāng)噪聲偏差以±10%波動(dòng)時(shí),估計(jì)平均誤差為0.1738 mm,均方根誤差增大至0.206.量測(cè)噪聲統(tǒng)計(jì)特征的不準(zhǔn)確將直接影響濾波算法的估計(jì)精度.

將自適應(yīng)SVD–UKF算法應(yīng)用到柔性針穿刺系統(tǒng),選擇Sigma 采樣參數(shù)α=1,β=1,η=0,ρ=1.22,自適應(yīng)參數(shù)b=2,c=2.0.設(shè)定系統(tǒng)噪聲統(tǒng)計(jì)參數(shù),R=0.02以及量測(cè)噪聲統(tǒng)計(jì)特征存在±5%隨機(jī)偏差.將估計(jì)效果與現(xiàn)有的UKF算法和AUKF算法[15]進(jìn)行仿真對(duì)比,結(jié)果如圖4–7所示.

圖4–6分別為UKF估計(jì)器、AUKF估計(jì)器和ASVD–UKF估計(jì)器的狀態(tài)估計(jì)結(jié)果.在曲率存在±5%的隨機(jī)偏差且噪聲統(tǒng)計(jì)特征不完全準(zhǔn)確的情況下,UKF算法估計(jì)曲線(xiàn)存在較大的估計(jì)偏差,無(wú)法精確跟蹤真實(shí)曲線(xiàn).AUKF算法較UKF 算法能更好的跟蹤真值,但依舊無(wú)法修正由于曲率偏差帶來(lái)的估計(jì)誤差.自適應(yīng)SVD–UKF算法較AUKF算法能更快的收斂,更好地跟蹤實(shí)際曲線(xiàn).

圖4 UKF濾波算法跟蹤曲線(xiàn)Fig.4 The estimated result of UKF filter

圖5 AUKF濾波算法跟蹤曲線(xiàn)Fig.5 The estimated result of AUKF filter

圖6 ASVD–UKF濾波算法跟蹤曲線(xiàn)Fig.6 The estimated result of ASVD–UKF estimator

圖7表示各個(gè)濾波算法跟蹤實(shí)際狀態(tài)的誤差曲線(xiàn).自適應(yīng)SVD–UKF從誤差曲線(xiàn)上相較于UKF算法和AUKF算法,誤差波動(dòng)更小且更趨近于真值,能較好的跟蹤真實(shí)軌跡.為了準(zhǔn)確地比較3種濾波器算法,本文采用均方根誤差的形式描述了濾波器的預(yù)測(cè)精度.每個(gè)算法模擬50次,取平均值.各個(gè)濾波算法估計(jì)狀態(tài)與真實(shí)系統(tǒng)最大誤差、平均誤差、均方根誤差如表3所示.

表3 3種估計(jì)算法的估計(jì)誤差Table 3 The estimation errors of three filters

圖7 3種濾波算法估計(jì)誤差Fig.7 The estimation errors of three filters

由仿真結(jié)果可知,在相同初始條件下,ASVDUKF算法的平均誤差和均方誤差明顯小于無(wú)跡濾波算法,與AUKF算法算法相比,平均誤差減小了51.96%,均方誤差減小了82.46%.自適應(yīng)奇異值分解UKF算法通過(guò)自適應(yīng)因子修正由于曲率偏差帶來(lái)的估計(jì)誤差,并利用Sage-Husa估計(jì)器在線(xiàn)估計(jì)系統(tǒng)噪聲的統(tǒng)計(jì)特性,有效提高了算法的估計(jì)精度.

以下討論奇異值分解和cholesky分解在無(wú)跡濾波算法中的區(qū)別.基于cholesky分解的無(wú)跡濾波算法在進(jìn)行Sigma采樣近似時(shí),容易出現(xiàn)協(xié)方差矩陣非正定的情況,致使病態(tài)問(wèn)題出現(xiàn)[17].病態(tài)問(wèn)題的出現(xiàn)將致使濾波算法無(wú)法對(duì)狀態(tài)進(jìn)行估計(jì)跟蹤.在不出現(xiàn)病態(tài)問(wèn)題的情況下,分別使用SVDKF算法和UKF算法對(duì)系統(tǒng)狀態(tài)進(jìn)行跟蹤估計(jì).在不考慮模型不定性和噪聲偏差的前提下,兩算法跟蹤情況如圖8所示.

圖8 SVDKF與UKF跟蹤曲線(xiàn)Fig.8 The estimated result of UKF and SVDKF

從仿真結(jié)果,可以看出SVDKF算法相較于UKF算法,跟蹤曲線(xiàn)更加平滑,能更好的吻合真實(shí)狀態(tài).對(duì)跟蹤效果進(jìn)行數(shù)據(jù)統(tǒng)計(jì),得到如表4所示結(jié)果.

表4 UKF算法與SVDKF算法估計(jì)誤差Table 4 The estimation errors of UKF and SVDKF

從統(tǒng)計(jì)結(jié)果,可以看出SVDKF算法相較于UKF算法具有更小的估計(jì)誤差.以SVD分解代替cholesky分解的濾波算法具有更好的數(shù)值穩(wěn)定性和魯棒性.

5 結(jié)論

柔性針在實(shí)際穿刺過(guò)程中,由于生物組織的非均勻性,會(huì)造成穿刺曲率發(fā)生變化,導(dǎo)致柔性針穿刺過(guò)程存在參數(shù)不確定性問(wèn)題.本文針對(duì)該問(wèn)題建立帶有曲率不定性?κ的柔性針穿刺的狀態(tài)空間模型.同時(shí)針對(duì)柔性針穿刺過(guò)程存在的不定性問(wèn)題以及超聲成像設(shè)備等量測(cè)噪聲統(tǒng)計(jì)特征不準(zhǔn)確性情況,提出了一種帶有噪聲估計(jì)器的自適應(yīng)奇異值分解無(wú)跡卡爾曼濾波算法.特別地,采用自適應(yīng)因子實(shí)時(shí)修正動(dòng)力學(xué)模型誤差,抑制模型中誤差不定性帶來(lái)的影響,用奇異值分解替代cholesky分解,抑制系統(tǒng)狀態(tài)協(xié)方差矩陣的負(fù)定性.同時(shí)利用Sage-Husa估計(jì)器在線(xiàn)估計(jì)噪聲的統(tǒng)計(jì)特性,減小由于設(shè)備存在的量測(cè)噪聲統(tǒng)計(jì)特征不準(zhǔn)確情況帶來(lái)的誤差.最后,將新的自適應(yīng)SVDUKF算法應(yīng)用于帶有曲率不定性的柔性針穿刺模型進(jìn)行計(jì)算仿真,結(jié)果表明,新算法能有效改善濾波性能,提高穿刺狀態(tài)的估計(jì)精度.