李鑫濱 ,袁蕊霞 ,閆 磊 ,韓 松,3

(1.燕山大學河北省工業(yè)計算機控制工程重點實驗室,河北秦皇島 066004;2.東北大學秦皇島分校計算機與通信工程學院,河北秦皇島 066004;3.河北省智能康復及神經調控重點實驗室,河北秦皇島 066004)

1 引言

隨著裝備制造技術發(fā)展,海洋勘探技術成為許多國家的戰(zhàn)略重點[1-2].多AUV(autonomous underwater vehicle)協(xié)作系統(tǒng)由于可以有效的執(zhí)行復雜的海上任務被得到廣泛關注[3-4].準確的定位是實現(xiàn)多AUV協(xié)同操作的技術保證.由于陀螺儀、加速度計等慣性傳感器的測量漂移[5],使得定位誤差累積增長.為了實現(xiàn)AUV集群中更好的定位,基于聲學距離測量及信息共享的協(xié)同定位方法被提出,用于實現(xiàn)定位誤差的協(xié)同修正[6-9].

然而,在復雜的水下環(huán)境中,傳統(tǒng)的協(xié)同定位方法仍然存在一些挑戰(zhàn).在使用距離測量信息時,時間信息是一個重要的信息,輕微時鐘偏差也會導致顯著的性能損失.在水下環(huán)境中,AUV僅依靠自身的本地時鐘信息,但由于本地器件和環(huán)境差異,本地時鐘存在不同的時間傾斜和漂移問題.對于具有時間漂移的協(xié)同定位系統(tǒng),文獻[10]重新推導濾波過程,得到了最優(yōu)狀態(tài)估計,文獻[11]推導出隨機兩步時延濾波器.文獻[12]將時延轉化為測量偏置,重構了測量方程,但其并未解決時鐘傾斜問題.文獻[13]雖然考慮了時鐘傾斜,但其需要有時鐘同步的錨節(jié)點作為輔助,這在沒有錨節(jié)點的AUV協(xié)作系統(tǒng)中難以實現(xiàn).因此,仍需要考慮AUV間存在的異步時鐘問題.

在傳統(tǒng)定位中,由于陀螺儀、加速度計等慣性傳感器的漂移,速度信息往往存在較大偏差.多普勒計程儀可以通過測量相對于水底的相對速度推得自身速度,缺點是其過于昂貴,并且在深海中可能無效.文獻[14]提出從寬帶聲通信信號中提取傳播時間(time of arrival,TOA)和多普勒頻移,將其與慣性測量融合進行定位,然而,其使用的方法需要發(fā)射機的位置固定.文獻[15]提出了一種速度估計和位置預測方法,該方法使用鄰居AUV輔助自身進行速度估計.然而,參考AUV的速度信息可能不準確,直接利用參考AUV的錯誤速度信息會導致較大的估計誤差.因此,如何在不能提供準確參考AUV速度時,充分利用測量信息進行狀態(tài)估計需要深入研究.

其中擴展卡爾曼濾波(extended Kalman filter,EKF)以其計算簡便的優(yōu)勢常用于協(xié)同定位中[12,16-17].然而,EKF對非線性方程進行泰勒展開,由于忽略高階項,導致了建模誤差.因子圖提供了一個全局函數(shù)分解成局部函數(shù)的乘積的自然圖形描述[18-19],將大規(guī)模的全局運算劃分為簡單的局部運算,解決了多源融合應用中實現(xiàn)邊緣后驗概率分布的問題.一些文獻將基于因子圖的高斯消息傳遞算法用于協(xié)同定位[20-21].高斯消息傳遞算法具有自然分布和良好的可伸縮性,這使得高斯消息傳遞算法非常適合于多AUV問題.然而,由于距離測量與位置關系呈非線性,線性化會帶來階段性的誤差.因此,對于AUV協(xié)同定位系統(tǒng)而言,如何降低系統(tǒng)在線性化過程導致的定位誤差,仍然是需要解決的關鍵問題.

綜上所述,在AUV集群系統(tǒng)中協(xié)同定位中的時鐘異步、不準確的速度以及協(xié)同定位中線性化過程帶來的影響仍然是定位的難點.針對上述問題,首先,本文建立了時延與位置的關系,提出了基于泰勒迭代方法來實現(xiàn)AUV時鐘參數(shù)估計,解決了AUV間各不相同的時間傾斜和偏移對距離測量帶來的誤差影響.然后,針對由慣性導航系統(tǒng)測量的速度具有誤差累積的問題,將每個AUV的位置和速度作為變量節(jié)點,建立一個整體的協(xié)同定位與速度估計因子圖模型,通過因子圖的消息傳遞計算因子變量的邊緣分布,得到了位置和速度變量的估計,有效降低了速度和位置測量的誤差.最后,針對測量函數(shù)線性化過程具有較大誤差的問題,提出了根據變量的協(xié)方差矩陣構造一個自適應因子的方法,在因子圖的消息傳遞過程中計算邊緣分布時,用自適應因子調整了變量的置信度,降低了線性化過程帶來的誤差影響.

2 系統(tǒng)建立與問題描述

2.1 系統(tǒng)建立

在協(xié)作系統(tǒng)中包含n個主AUV,m個從AUV,主AUV配備了高精度的慣性導航系統(tǒng)(inertial measurement unit,IMU),從AUV配備了低精度的慣性導航系統(tǒng),主AUV相對于從AUV具有較精確的位置信息.正常情況下,AUV的航向和速度信息用慣性導航系統(tǒng)進行測量,通過壓力傳感器可以準確地獲得深度信息.每個AUV配備了接收機和發(fā)射機,用于測量外部時延信息和多普勒頻移信息.AUV在沒有錨節(jié)點輔助的環(huán)境運動,依靠自身航向、速度和深度信息粗略地獲得航行器的水下位置,然后利用外部測量的時延信息和多普勒頻移信息及其他AUV的位置信息對自身AUV進行位置和速度糾正.系統(tǒng)環(huán)境圖如圖1所示.

圖1 AUV集群協(xié)同定位場景Fig.1 Cooperative localization scenario of AUV swarm

主AUV 的集合為ΩM={1,2,···,n},從AUV的集合為ΩS={n+1,n+2,···,m+n},所有AUV的集合為ΩA={1,2,···,m+n}.運動過程表示為離散測量,記為tk,k=0,1,2,···,K.第i個AUV在tk時刻的位置和速度分別定義為xk,i=[xk,i yk,i zk,i]T和vk,i=[vxk,i vyk,i vzk,i]T.假設AUV運動符合隨機游走模型,AUVi在tk時刻的狀態(tài)xk,i可以從tk-1時刻的狀態(tài)xk-1,i轉移過來,更新過程表示如下:

其中:ts為采樣時間間隔;vk-1,i表示時間tk處的速度;表示時間tk-1處的加速度,由IMU的加速度計測量,即

2)AUVi測量的與AUVj,j ∈ΩA{i}距離為

3)多普勒效應造成的發(fā)射和接收信號的頻率之差稱為多普勒頻移,表示為Δf,用fc表示信號的中心頻率,AUVi測量的與AUVj,j ∈ΩA{i}多普勒尺度因子為:D=Δf/(Δf+fc),多普勒尺度因子與位置和速度變量的對應關系為

AUV間通過接收機與發(fā)射機進行通信,周期性通過信息交互獲取信息,k時刻為完成一個交互周期,需要接收的信息及具體交互過程為:AUV1首先廣播信號給其他AUV,此信號攜帶信息有自身的先驗位置和速度信息ut,1、時間信息t及發(fā)出者信息1,信號包發(fā)出的信息,AUV2,···,AUVn接收信號,AUV2接收到AUV1的信號并進行測量后,立即向除AUV1以外的AUV廣播信號,并且增加接收到的其他AUV的信息包.AUV3向除AUV2以外的AUV廣播信號,其余AUV與AUV3同理,AUVj廣播信號:AUV(j+1),···,AUVn接收信號.當AUVj接收到AUVi廣播的信號,解析信號得到測距信息和多普勒因子測量AUV1在k時得到的全部信息,包含所有AUV的先驗位置信息、測距信息及多普勒頻移信息,這些信息用于算法中計算,得到各個AUV的位置估計,最后把位置估計廣播給各AUV.以上過程為一次交互過程,在一個定位周期內有一次通信交互.完成交互過程后,等待下一個定位周期到達后再次進行交互.

2.2 問題陳述

3 基于因子圖的AUV集群速度估計和協(xié)同定位

本節(jié)為集群中的AUV設計速度估計與位置估計的協(xié)同定位算法,在使用外部測量進行位置估計時,需要先進行時鐘參數(shù)估計,整個算法結構框圖如圖2所示.其中: 黑色虛線框外上方的部分為測量信息,由慣性測量、距離測量和多普勒因子測量組成,為整個結構的輸入;左側黑色虛線框為狀態(tài)的先驗信息,由慣性信息通過航位推算所得,所得結果作為算法結構部分的初值輸入;右側黑色虛線框為算法結構部分,輸入為結構圖左側所得變量的均值與協(xié)方差,輸出為此部分所重新計算的變量均值與協(xié)方差;黑色虛線框外下方的的部分表示算法結構部分的輸出替換掉之前的先驗信息,作為算法結構部分新的輸入,進行迭代計算.

圖2 系統(tǒng)結構框圖Fig.2 System structure diagram

圖2中,算法結構主要分為兩部分,圖中左上紅色虛線框代表第1部分,另一個紅色虛線框代表第2部分.第1部分內容為基于泰勒算法的異步時鐘參數(shù)估計,文章將在第3.1節(jié)進行介紹.第2部分內容為基于因子圖的協(xié)同定位和速度估計,文章將在第3.2節(jié)進行介紹.

3.1 基于泰勒算法的異步時鐘參數(shù)估計

在本小節(jié)中,考慮時鐘異步環(huán)境,將AUV的時鐘等效為具有時鐘傾斜和時鐘偏移的模型.AUVi的本地時鐘表示為

其中:t0表示基準時間;bi表示AUVi時間與基準時間的偏差;ai表示AUVi收到信號時的時鐘傾斜.

AUVj傳輸信號到AUVi的到達時間表示為

則傳播時間可表示為

其中Δτi,j表示測量噪聲.

未知參數(shù)有[ai bi]T,i∈ΩA和[xk,i yk,i]T,i∈ΩS,式(11)左側的測量時延構成測量向量,右側進行泰勒展開,可通過泰勒迭代法進行估計.由τi,j組成的測量向量為T={τi,j},i,j ∈ΩA,待估參數(shù)有[ai bi]T,i ∈ΩA.用f(φ)表示測量和變量的函數(shù)關系,測量模型可表示成如下向量行式:

其 中:φ=[a1···am b1···bm xk,1···xk,m yk,1···yk,m]T;e=[Δτ1,2··· Δτm,m+n-1]T表示測量噪聲向量;T=[τ1,2τ2,1···τ1,m+n τm+n,1···τi,m+n τm+n,i···τm+n-1,m+n τm+n,m+n-1]T.

將式(12)在φ-Δφ處進行泰勒展開,化成參數(shù)φ的線性函數(shù).將f(φ)在φ-Δφ處展開,可得

可構造如下方程:

由已知信息(時間測量和主AUV的位置信息)及未知參數(shù)(從AUV的位置及時鐘參數(shù))根據AUV間位置與傳播時間的關系構成式(14)的方程,通過泰勒迭代算法,可得到集群中每個AUV的時鐘傾斜和時鐘偏移參數(shù)的初始估計

3.2 基于因子圖的協(xié)同定位和速度估計

得到時鐘參數(shù)后,可以利用測距信息計算AUV的位置和速度變量的邊緣概率分布.如第2.2節(jié)所述,由于直接計算邊緣概率分布較復雜,本文采用計算簡便的基于因子圖的消息傳遞算法.AUV可以通過消息傳遞利用外部測量信息來協(xié)同推斷其位置和速度.當分解后的因子圖與網絡拓撲相匹配時,可以使用有效的分布式推理消息傳遞[19],通過消息傳遞算法應用于因子圖,得到了變量的近似邊緣分布(“belief”),即b(x)?p(x|z).在消息傳遞算法中,信念(b(x))和消息(μg→x(x))是重要的傳輸信息.可以將任何變量和函數(shù)的關系用因子圖表示,在本文中,將每個AUV的位置和速度作為變量節(jié)點,將距離測量及多普勒因子測量及前一時刻和后一時刻變量轉移關系作為函數(shù)節(jié)點.以此建立協(xié)同速度和位置估計的因子圖模型,利用因子圖模型描述以慣性測量、距離測量、多普勒因子為觀測量的AUV協(xié)同定位與速度估計過程,以因子圖的信息傳遞用于AUV的位置和速度估計.將式(8)進行中因子分解,分解后的因子圖如圖3所示,圖3中函數(shù)節(jié)點分別為

圖3 因子圖模型Fig.3 Factor graph model

1)在貝葉斯框架下的位置和速度信念.

為求得AUV的位置和速度信念,可通過圖4中因子圖模型及貝葉斯框架得,第i個AUV,i ∈ΩA的位置和速度信念表示為

圖4 時鐘參數(shù)誤差圖Fig.4 Clock parameter error

2)對測量函數(shù)進行泰勒展開.

得到變量的信念在貝葉斯框架下的表示后,本文致力于變量的信念計算.通過消息傳遞,變量間利用函數(shù)關系可以進行信念的更新,達到計算邊緣分布的目的.在計算過程中,測量和變量間轉換關系是一個關鍵點.常規(guī)做法是將其進行線性化處理,鑒于循環(huán)消息傳遞的迭代性質,在時刻k和第L次迭代中,根據估計位置對測量函數(shù)進行一階泰勒展開,即

與距離測量相似,根據之前的估計位置對多普勒因子測量函數(shù)同樣進行一階泰勒展開,即

3)自適應因子.

符號N(x,mx,Vx)表示x服從均值mx,方差為Vx的正態(tài)分布,符號mx表示變量x的均值,符號Vx表示變量x的方差.由于因子圖中的所有信息和信念都是高斯形式,它們可以通過相應的均值和協(xié)方差進行參數(shù)化.由于一階泰勒展開進行線性化會導致近似結果與實際有一定偏差,從而在參數(shù)化的計算過程中引入了誤差.為了抑制線性化的影響,提出了根據變量的協(xié)方差矩陣構造自適應因子β.該自適應因子構造為

其中tr(·)表示矩陣的跡.

在信念的均值和協(xié)方差計算中引入的自適應因子β,β隨當前計算的第i個AUV信念和觀測的第j個AUV信念的協(xié)方差變化,當?shù)趈個AUV信念的協(xié)方差增大時,β也增大,先驗和觀測融合后協(xié)方差變化減小,均值變化也減小;在消息傳遞迭代過程中,隨著信念接近于實際值時,第j個AUV信念的協(xié)方差減小,β也減小,先驗和觀測融合后協(xié)方差變化增大,均值變化也增大.因此,在觀測可信度低時,利用自適應因子β降低均值變化量,在觀測可信度高時,利用自適應因子β提高均值變化量,實現(xiàn)調整線性化過程的影響.

4)位置信念的均值和協(xié)方差計算.

根據文獻[19]的消息傳遞算法,通過參數(shù)化計算,可以得到第i個AUV,i ∈ΩA位置信念的均值和協(xié)方差為

5)速度信念的均值和協(xié)方差計算.

與位置信念參數(shù)化相似,速度信念可做參數(shù)化.第i個AUV,i ∈ΩA速度信念的均值和協(xié)方差為

基于迭代消息傳遞,確定了AUV在時刻k的位置和速度信念,然后通過MMSE得到位置和速度估計.綜合上述步驟,文章將基于因子圖的AUV速度估計和協(xié)同定位算法具體步驟總結為表1所示.

表1 基于因子圖的AUV集群速度估計和協(xié)同定位Table 1 Velocity estimation and cooperative localization of AUV swarm based on factor graph

4 仿真與分析

在本文中,使用MATLAB進行了數(shù)值模擬.在本實驗中,主AUV個數(shù)設置為2個,從AUV個數(shù)設置為4個,初始位置在600×600×50 m3范圍內隨機產生.設每個AUV的x軸、y軸、z軸初始速度分別為1 m/s,0.4 m/s,0.25 m/s,初始加速度為-0.001 m/s2,-0.001 m/s2,-0.00015 m/s2,總共行駛600 s,每10 s進行一次外部信息測量.

為了模擬主AUV和從AUV不同精度的慣性導航設備,每一步(指進行外部測量的10 s)主AUV加速度噪聲設置為N(0,0.0001),每一步從AUV加速度噪聲設置為N(0,0.01).在本文中,設深度信息由深度計提供,深度計誤差服從均值為深度的5%、方差為1的正態(tài)分布,AUV深度運動范圍在[0 m,150 m]之間.根據文獻[12]與文獻[23]所述的測量設備,在600×600×50 m3范圍內將距離測量噪聲設置為N(0,1)較為合理.根據文獻[14]所述多普勒因子誤差,將多普勒因子測量噪聲設為N(0,1×10-3).設2個主AUV,4個從AUV編號依次為1～6,則時鐘參數(shù)設置為:a=[1.05 1.05 1.05 1.05 1.05 1.05],b=[0 0.12-0.1 0.5 0.1-0.02],消息傳遞迭代次數(shù)為5.最后,利用上述信息對AUV的位置和速度進行估計,仿真通過50次蒙特卡羅隨機實驗來計算目標位置和速度估計值的誤差.

1)時鐘參數(shù)估計結果.

本文評估了時鐘參數(shù)估計的均方誤差(mean square error,MSE).首先,本文給出了所估計時鐘參數(shù)與真實值的誤差值.然后,分析了時鐘異步對位置估計的影響.對于時鐘傾斜參數(shù),以估計參數(shù)和實際參數(shù)差值的絕對值作為評價指標;對于時鐘漂移參數(shù),以AUV1 為基準時鐘,計算其余AUV與AUV1的時鐘漂移,并以估計參數(shù)和實際參數(shù)差值的絕對值作為評價指標,即

結果如圖4,時鐘傾斜的誤差均在1.2×10-3以內,時鐘漂移的誤差均在1.0×10-3以內.2)定位和速度估計結果.

本文分析了存在時鐘異步和不存在時鐘異步時,所提算法的位置和速度估計誤差.其中,位置估計結果與文獻[20]的(posterior linearization belief propagation,PLBP)算法、EKF、無跡卡爾曼濾波(unscented Kalman filter,UKF)、航位推算(dead reckoning,DR)做對比,其中PLBP算法中L參數(shù)為5,M參數(shù)為5,在與對比算法深度測量信息一致的條件下,通過仿真對比了x和y方向的位置和速度估計結果.以實際位置(速度)與估計位置(速度)之間的均方根誤差(root mean square error,RMSE)作為評價指標.

首先,文章為了驗證第3.2節(jié)的算法,對于不存在時鐘異步情況下,本文所提算法和DR,EKF,UKF,PLBP進行了對比.AUVs運動軌跡如圖5所示,AUV在區(qū)域內運動,圓標記表示從AUV軌跡的起點,星標記表示主AUV軌跡的起點,圖中藍色曲線代表真實軌跡,紅色曲線代表本文所估計軌跡.

圖5 不存在異步時鐘時的AUV軌跡Fig.5 Trajectories of AUVs when there is no asynchronous clock

圖6為位置誤差結果,在600 s內,DR方法、PLBP,EKF,UKF,本文所提算法平均誤差如表2的情況1所示,將平均誤差以及所提算法較其他算法提升比例進行了總結.因為隨著AUV的運動,誤差會逐漸累積,導致DR方法的結果為沒有修正時AUV的誤差,而EKF,UKF和PLBP有一定的修正作用,本文所提算法引入的速度估計使位置估計時有更準確的先驗位置信息,因此位置誤差更低.

表2 位置平均誤差Table 2 The average error of the position

圖6 不存在異步時鐘時位置誤差Fig.6 Position error when there is no asynchronous clock

為了驗證速度估計部分的效果,對本文方法、最小二乘法(least sqaure,LS)、EKF及DR的速度估計進行了仿真,仿真結果如圖7所示.在600 s內,DR,LS算法,EKF算法.本文所提算法所求速度的平均誤差如表3中情況1所示.本文算法相對于DR,LS算法和EKF算法精度分別提升了96%,89%,53%,上述數(shù)據在表3中情況1進行了總結.這是由于DR方法對速度誤差沒有降低作用,而LS算法依賴于較準確的主AUV速度,使得LS算法對誤差有一定抑制效果;EKF算法不像LS算法需要盡量多的測量方程,并且根據測量及前一時刻狀態(tài)進行濾波,其比LS算法誤差小,而本文的算法不僅將主AUV的速度用于從AUV進行校準,并且通過迭代進行相互校準,因此比EKF算法效果好.由結果可知,本文方法EKF,LS算法均對速度誤差有抑制作用,本文方法效果最明顯.

表3 速度平均誤差Table 3 The average error of the velocity

圖7 不存在異步時鐘時速度誤差Fig.7 Speed error when there is no asynchronous clock

同時,本文比較了存在時鐘異步時,文章的算法和DR,EKF,UKF,PLBP的位置誤差.如圖8所示,在600 s內,DR方法,PLBP,EKF,UKF,本文所提算法平均誤差如表2的情況2所示.5種方法中,本文所提方法誤差最小,比PLBP提升了86%,比EKF提升了74%,比UKF提升了75%,上述數(shù)據在表2的情況2中進行了總結.由于DR不使用測距信息,時鐘異步影響的測距誤差對DR無影響,DR與無時鐘異步時基本一致,EKF,UKF由于沒有考慮時鐘異步,在不準確的測距信息影響下,導致效果變差,而文章考慮了時鐘異步后,效果優(yōu)于其他算法.由于解決了異步時鐘的影響,圖8中本文算法誤差與圖7中不存在異步時鐘時誤差相比基本一致,表明解決時鐘異步問題對定位精度有所提升.對本文方法LS,EKF及DR的速度估計進行了仿真.在600 s內,DR,LS算法、EKF算法,本文所提算法所求速度的平均誤差如表3的情況2所示.由于時鐘異步不影響多普勒頻移信息,表3中情況2結果與不存在時鐘異步時表3中情況1的結果基本一致.

圖8 存在異步時鐘時位置誤差Fig.8 Position error when asynchronous clock exists

3)不同參數(shù)下估計結果.

本節(jié)將進行數(shù)值模擬,以評估如何計算這個迭代次數(shù)的影響,此外,還考慮了各種環(huán)境參數(shù)來驗證算法的性能.本文分析了不同噪聲對算法影響,其中,測距噪聲為零均值,方差為0.5～2.5的高斯噪聲,多普勒因子噪聲為零均值,方差為0.5×10-3～2.5×10-3的高斯噪聲,本實驗對不同噪聲下的結果進行分析,以所有時刻實際位置與估計位置之間的均方根誤差之和作為評價指標,即

圖9為不同測距噪聲下對位置和速度估計影響的實驗,圖9(a)為位置估計誤差,如表6所示,本文所提方法比DR,EKF,UKF,PLBP的位置誤差小.圖9(b)為速度估計誤差,DR,LS,EKF,本文算法平均誤差如表7所示,在不同測距噪聲下速度估計誤差仍然是本文所提方法最小.如表6-7相關數(shù)據所示,本文所提方法適應不同測距噪聲,在不同測距噪聲時仍能有良好效果.

表6 位置平均誤差比較Table 6 The average error of the position m

表7 速度平均誤差比較Table 7 The average error of the velocity m/s

圖9 不同測距噪聲影響Fig.9 Effects of different ranging noise

圖10為不同多普勒因子噪聲下對位置和速度估計影響的實驗,與圖9相似,在不同多普勒因子噪聲下,DR,PLBP,EKF,UKF,本文算法的平均位置估計誤差和平均速度估計誤差在表6-7中進行了總結.位置估計誤差和速度估計誤差中本文所提方法最小.因此,仿真結果證明隨著噪聲變化,本文所提算法的估計誤差呈平穩(wěn)趨勢,所提算法在不同噪聲下有最低的位置和速度誤差.

圖10 不同多普勒因子噪聲影響Fig.10 Effects of different doppler factor noise

5 結論

本文提出了一種基于因子圖的速度估計和協(xié)同定位算法,該方法在AUV協(xié)作系統(tǒng)中利用距離測量和多普勒頻移測量進行誤差修正.首先,文章建立了時鐘傾斜和時延與位置的關系,通過當前的后驗信息利用泰勒算法近似計算時鐘參數(shù).針對由慣性導航系統(tǒng)測量的速度具有誤差累積問題,本文建立了利用距離測量和多普勒頻移測量的因子圖模型,利用因子圖模型描述以距離測量和多普勒因子為觀測量的AUV協(xié)同速度和位置估計過程,提出基于因子圖的速度估計和協(xié)同定位算法,通過因子圖的消息傳遞,計算因子變量的邊緣分布,有效降低了速度和位置測量的誤差.最后,在消息傳遞過程中設計一自適應因子,采用自適應因子來調整觀測的置信度,降低線性化過程帶來的誤差影響.仿真結果表明,該算法估計的時鐘參數(shù)誤差在0.001 以內,在不存在時鐘異步時,該算法比DR,EKF,UKF,PLBP有更低的位置誤差,比DR,LS,EKF有更低的速度誤差.在存在時鐘異步時,該算法通過準確估計時鐘參數(shù),其位置誤差和速度誤差與不存在時鐘異步時基本一致,該算法實現(xiàn)了準確、高效的定位.