主從式USV協同定位系統性能分析

于 飛， 范世偉， 李 倩， 郝 強， 姜 畔

(哈爾濱工業大學 電氣工程及自動化學院， 哈爾濱 150001)

主從式USV協同定位系統性能分析

于 飛， 范世偉， 李 倩， 郝 強， 姜 畔

(哈爾濱工業大學 電氣工程及自動化學院， 哈爾濱 150001)

為研究以距離為觀測量的主從式無人水面航行器(unmanned surface vehicle, USV)協同定位系統在不同運動狀態下的定位性能問題，建立一種新的協同坐標系，并在該坐標系中推導主從USV運動模型，以主從USV相對距離為觀測量分析主從式USV協同定位系統的可觀測性及可觀測度. 同時，針對不可觀測路徑，提出通過改變控制輸入的方法以達到提高系統可觀測度的目的. 通過MATLAB仿真實驗與水面艦艇實驗,結果表明:在不同情況下，通過調整控制輸入的方法改變系統的可觀測度是有效的；利用所提出的分析方法可以反映協同定位系統的定位性能,驗證了所得結論及方法的有效性.

無人水面航行器；協同定位；性能分析；控制輸入；可觀測性

無人水面航行器(USV)是近些年伴隨著軍事需求而新興發展的產物. USV通過搭載不同類型的傳感器或設備模塊，可以完成情報搜集、水上監視、無人偵察、反水雷等不同的任務[1]. 單體USV難以完成日益復雜的任務，從而有必要通過多USV協同作業共同完成任務使命. 多USV進行協同作業的首要問題是實現各單體USV精確定位. 文獻[2]研究了基于長基線的多無人艇協同定位算法，將從艇與兩個主艇之間的距離值作為觀測量能顯著提高定位精度；文獻[3]提出一種基于雙水聽器的多自主航行器協同導航方法；文獻[4]提出一種基于Kullback-Leiber距離的單主艇協同導航算法，并通過大量水上試驗驗證了算法有效性；文獻[5-8]則采用相鄰時刻的運動矢徑作為觀測量來實現單主艇協同定位.

協同定位過程的本質是狀態預測，而各狀態的可觀測性及可觀測度與系統定位性能存在著緊密關系，所以分析系統的可觀測性及可觀測度具有重要意義. 文獻[9]對固定單信標定位系統進行了基于李導數的可觀測性分析，表明只要作為量測量的距離矢量方向隨時間一直變化，那么定位系統就是可觀的，即艇位置信息可估；文獻[10]分析了基于距離測量的主從式AUV協同定位系統可觀測性，表明從艇控制輸入均不為零且主從艇不在同一直線上航行時系統可觀，并且分析了不同控制輸入下系統的可觀測性. 值得注意的是，文獻[9-10]雖然使用了非線性可觀測性分析方法，避免了可觀測性條件不充分的問題，但是都沒有進一步分析系統可觀測度，從而缺少對系統定位性能的定量分析. 文獻[11]基于條件數分析了多自主水下航行器協同定位系統的可觀測度，但是并沒有考慮控制輸入對協同定位系統可觀測度的影響. USV改變控制輸入時會影響協同定位系統可觀測度的變化，甚至會使系統從可觀變為不可觀測. 所以，脫離控制輸入單純討論協同定位系統的可觀測性及可觀測度是不合理的. 更為重要的是，上述文獻多是在慣性坐標系中進行系統可觀測性分析，由于在慣性坐標系中主從USV都存在狀態變化，使得主從艇狀態無法解耦，從而導致分析過程較為復雜，甚至會導致可觀測性分析結論片面的問題. 針對以上問題，本文以相對距離為觀測量的主從式USV協同定位系統為研究對象，提出在協同坐標系下分析系統可觀測性及可觀測度，進而定量研究協同定位系統的定位性能. 同時，提出通過在協同坐標系中調整控制輸入的方式改變系統的可觀測性，進而改善系統的定位性能.

1 主從式USV運動學模型

由于USV在水面航行，所以主從式USV協同定位系統的定位問題是二維平面內的定位問題. 在主從式USV協同定位系統中，主USV搭載高精度導航設備，可以實現精確定位，從USV搭載低精度導航設備，需要獲得與主USV間的距離信息并利用數據融合技術來提高自身定位精度. 圖1為基于距離量測的主從式USV協同定位系統，圖中ΣI為二維慣性坐標系，定義USV艏艉向與慣性坐標系的XI軸夾角為航向角，以逆時針方向為航向角正向. 定義ΣCL為二維協同坐標系，以主USV重心為協同坐標系原點，其x軸方向指向主USV航行方向，y軸垂直于x軸并指向主USV左側. 根據以上定義可以看出，協同坐標系跟隨主USV艇體相對慣性坐標系運動，同時主USV相對協同坐標系靜止且始終位于協同坐標系原點. 主USV在慣性坐標系中的位置為(Xm,Ym)，從USV在慣性坐標系中的位置為(X,Y)，以從USV的二維位置坐標作狀態變量，將慣性坐標系中從USV的狀態投影到協同坐標系可得

(1)

式中ψm為主USV在慣性坐標系中的航向角. 進一步，對式(1)兩邊同時求導可以得到

(2)

圖1 主從式USV協同定位系統

(3)

將式(3)代入式(2)中，可得

(4)

進而可以得到從USV在協同坐標系中的速度為

(5)

從USV在協同坐標系中的航向角記為γ，該航向角定義為從USV的速度矢量與主USV的速度矢量之差與協同坐標系x軸之間的夾角. 進一步，可以利用從USV在協同坐標系中的速度v1、v2表示出從USV在協同坐標系中的航向角γ為

(6)

至此，將主從USV的位置、速度及航向角全部投影到協同坐標系中. 在協同坐標系中，主艇的位置始終為坐標系原點，從而可以將其看作是一個固定信標，這樣慣性坐標系中主從USV間的相對運動信息都轉化到協同坐標系中的從USV上. 基于這種建模方式進行系統可觀測性及可觀測度分析可以大大簡化分析過程，而且還充分考慮了主從USV的相對速度大小、相對航向及相對位置對系統可觀測性的影響，使得可觀測性條件的充分性得到保障.

2 協同定位系統性能分析

系統可觀測是其狀態可估的前提，不可觀測系統的定位誤差將隨時間的推移而不斷增長，并最終使協同定位系統失去穩定性[12]. 系統可觀測度是定量的描述系統的可觀測情況，系統可觀測度為0時系統不可觀測，可觀測度越大則系統的可觀測性越好. 所以系統的定位性能與其可觀測性及可觀測度度有著緊密的聯系，可以通過分析其可觀測性及可觀測度來分析系統的性能. 主從式USV協同定位系統觀測量是主從USV間的相對距離，所以觀測方程為

(7)

顯然觀測方程是一個非線性方程，為了更加準確地分析系統可觀測性，本文使用李導數來對其可觀測性進行分析. 本文在第1節建立的基于協同坐標系的主從式USV運動模型基礎上分析系統可觀測性及可觀測度，根據系統可觀測性的李導數判定方法，首先將從USV在協同坐標系中的狀態方程表示成如下連續形式:

(8)

其中

(9)

觀測方程的零階李導數就是方程本身，可以寫作

(10)

求解L0h(x)關于USV狀態量的梯度，得

(11)

進而求得一階李導數的梯度為

(12)

那么可觀測性矩陣可以寫作

(13)

可觀測性矩陣的行列式值為

(14)

通過以上分析可知，當主從式USV協同定位系統滿足以上3種情況時，系統不可觀，將后兩種情況定義為主從式USV協同定位系統的不可觀測路徑. 除此之外，主從式USV協同定位系統都可觀. 但是，系統是否可觀只是從定性角度反映了觀測量是否具備解算出系統狀態量的能力，并不能描述其性能的好壞. 在實際應用中，僅僅明確系統是否可觀往往是不夠的. 對于完全可觀的系統，尚存在可觀測程度的問題. 如果系統可觀測矩陣性態不好，那么即使系統完全可觀，也有可能造成系統方程的病態性. 另外，考慮到單純依靠距離觀測的協同定位系統觀測信息量有限，往往更關心的是系統可觀測性大小的條件性變化，即系統在何種條件下可觀測性強，何種條件下可觀測性弱，只有這樣才能夠在實際應用過程中有目的的去設計合理的協同定位方案(包括隊形編排、航路機動以及協同規則等)，從而保證協同定位性能. 為進一步考察系統狀態量的估計精度，通過矩陣條件數理論對系統可觀測度作進一步分析. 常用的關于p范數‖·‖p的條件數標記為condAp，一般稱condA2為譜條件數. 根據矩陣條件數理論，條件數小的矩陣稱為“良性”矩陣，反之稱為“病態”矩陣. 系統可觀測性矩陣的條件數越大，系統的可觀測程度越差；如果系統可觀測性矩陣的條件數無窮大，則系統不可觀測；反之，系統可觀測矩陣的條件數越接近1，系統的可觀測性最好. 由于矩陣條件數是一個≥1的正數，不妨定義可觀測度D為可觀測矩陣O條件數的倒數，即

D=(condO)-1.

(15)

根據以上分析，D可表示為

(16)

x2+y2+vxsinγ-vycosγ=0,

(17)

x2+y2-vxsinγ+vycosγ=0.

(18)

根據平面幾何知識可知，式(17)表示的是以(-v·sinγ/2,v·cosγ/2)為圓心，v/2為半徑的圓，式(21)表示的是以(v·sinγ/2,-v·cosγ/2)為圓心，v/2為半徑的圓，也就是說，當從USV在協同坐標系中的位置在式(17)、(18)表示的兩個圓中任意一個上時，系統可觀測度最大為1.

為了更加清晰的觀察從艇在協同坐標系中的速度v以及航向角γ對系統可觀測度的影響，設置兩組仿真，第1組仿真中從艇速度v不變，改變從艇航向角γ，觀察不同航向角下可觀測度的分布情況，仿真結果如圖2、3所示；第2組仿真中，從艇航向角γ不變，改變從艇速度v，觀察不同速度下可觀測度的分布情況，仿真結果如圖4所示.

圖2 可觀測度三維圖

圖3 觀測度三維圖

當從USV在協同坐標系中的速度v=20 m/s，航向角γ=0°時，可觀測度隨從USV位置變化如圖2所示. 當從USV在協同坐標系中的速度v=20 m/s，航向角γ=135°時，可觀測度隨從USV位置變化如圖3所示. 當從USV在協同坐標系中的速度v=5 m/s，航向角γ=135°時，可觀測度隨從USV位置變化如圖4所示.

不同學歷學員各項考核成績比較見表2。碩士和學士學位學員各項技能成績比較顯示，腰椎穿刺術、氣管插管術成績比較，差異具有統計學意義[87.0（81.1，92.8） vs.84.3（76.9，87.0），（P=0.047）；87.4（82.5，90.1） vs.81.1（76.2，87.0），（P=0.019）]，其余項目考核成績比較，差異無統計學意義（P＞0.05）。

圖4 觀測度三維圖

上面3組圖中的x軸、y軸坐標表示從USV在協同坐標系中的坐標，D軸表示系統可觀測度. 可以看出，系統的控制輸入對可觀測度有影響，那么針對不可觀測路徑或可觀測度低的路徑，可以通過改變系統控制輸入，使從USV在協同坐標系中的位置位于可觀測度高的區域，從而改善系統定位性能，具體調整方法可根據實際情況和任務需求利用式(19)進行調整. 舉例說明：當任務需要主從USV同時駛向某地時，各USV往往會以大致相同的速度航行. 根據前述分析可知，這種運動方式對應不可觀測路徑(2)的情況，從而導致系統不可觀. 在這種情況下，可以通過改變系統控制輸入達到改善系統可觀性的目的，即選擇主從USV中任意一個進行變速運動，使主從USV間始終存在相對運動，從而使得從USV在協同坐標系中處于較高可觀測度的位置，進而可以改善系統定位性能. 針對不可觀測路徑(3)的情況，由于此時滿足式tanγ=y/x使得系統不可觀測，此時可通過圓形機動來快速提高系統可觀測度，圓形機動軌跡可以減小速度與位移分量比例的相似程度，也就使得tanγ≠y/x，從而達到提高系統可觀測度，改善系統定位性能的目的.

通常情況下，USV的航向根據任務需要確定，為了使系統可觀測度提高，可以在航向不變的情況下改變速度大小，根據式(16)，可以推導出

v=(x2+y2)/|ycosγ-xsinγ|.

(19)

速度越接近式(19)，系統的可觀測度越大. 當速度滿足式(19)時，系統可觀測度為1. 在USV協同定位時，由于實際系統的條件限制并不一定能滿足式(19)，只能使從USV在協同坐標系中的速度盡可能接近式(19)，來提高系統可觀測度.

3 仿真與實驗

3.1 仿真試驗

為驗證本文分析的正確性，首先利用MATLAB進行仿真試驗，試驗主要針對不可觀測路徑進行仿真；在此基礎上，進行調整系統控制輸入使不可觀測路徑變為可觀測路徑的仿真驗證.

3.1.1 不可觀測路徑仿真

當協同定位系統滿足detO=(v1y-v2x)/(x2+y2)=0時，系統不可觀測，對第二節分析中的不可觀測路徑(2)和不可觀測路徑(3)進行仿真.

1) 主從USV相對靜止.主USV初始狀態為(-1,1)，航向為0°，速度為2 m/s；從USV初始狀態為(20,-20)，初始狀態估計值為(120,-120)，航向為0°，速度為2 m/s，其余仿真條件相同，此時主從USV相對靜止，系統不可觀測，仿真結果如圖5所示.

圖5(a)中3條曲線分別為主USV軌跡，從USV真實軌跡以及從USV的EKF濾波軌跡，圖5(b)為從USV定位誤差，從圖中可以看到定位誤差始終在40 m附近，圖5(c)為系統可觀測度隨時間變化曲線，從圖中可以看出系統可觀測度一直為0. 當主從艇相對靜止時，系統不可觀測，濾波軌跡始終與真實軌跡存在較大誤差，與前文理論分析結論一致.

圖5 協同定位系統相對靜止路徑

2) 主從USV連線與x軸夾角為航向角.主從USV連線即從USV與協同坐標系原點連線，當主從USV連線與協同你坐標系x軸夾角為航向角時，根據前文分析可知此時系統不客觀. 主USV初始狀態為(-1,1)，航向為0°，速度為1 m/s；從USV初始狀態為(100,1)，初始狀態估計值為(200,-99)，航向為0°，速度為2 m/s，其余仿真條件與3.1.(1)節相同，仿真結果如圖6所示.

圖6 協同定位系統過協同原點路徑

圖6(a)中3條曲線分別為主USV軌跡，從USV真實軌跡以及從USV的EKF濾波軌跡，圖6(b)為從USV定位誤差，從圖中可以看出始終存在70 m左右的定位誤差，圖6(c)為系統可觀測度，從圖中可以看出可觀測度一直為0. 從仿真結果可以看出，相對運動軌跡經過協同坐標系ΣCL坐標系原點時，系統不可觀測，濾波軌跡與真實軌跡有很大差距，與前文理論分析結論一致.

3.1.2 不可觀測路徑調整控制輸入后仿真

1) 針對主從USV相對靜止路徑.為了驗證改變控制輸入可以提高系統定位性能，前500 s仿真條件與3.1.1條件相同，此時主從USV速度大小相等、方向相同，即主從艇相對靜止，系統不可觀；500 s之后，使從USV停止5 s，運行5 s，且運行速度設置為4 m/s，此時主從USV始終存在相對運動，系統可觀測，仿真結果如圖7所示.

從圖7(c)可以看出系統觀測度在調整控制輸入后有顯著提高，在調整前系統觀測度為0，即系統不可觀；500 s后，從USV速度開始變化，此時由于速度的變化使得主從USV相對位置發生變化，引起可觀測度上下振蕩變化. 從圖7(b)也可以看出在速度調整前系統定位誤差較大，在調整速度后定位誤差逐漸收斂，從而驗證了本文所提方法的有效性.

2) 針對主從USV連線與x軸夾角為航向角路徑.主USV初始狀態為(-1,1)，航向為0°，速度為2 m/s；從USV初始狀態為(100,1)，初始狀態估計值為(200,-99)，速度為2 m/s，前200 s航向為0°，此時由于主從USV連線與協同坐標系x軸夾角與航向角相同，導致系統不可觀測. 為改善系統定位性能，使從艇在200～210 s之間航向為90°，210 s到480 s開始作圓形機動路徑，480 s后航向仍為0°，仿真結果如圖8所示.

從圖8(c)可以看出系統在調整控制輸入后可觀測度有顯著的增加，在前200 s系統沒有調整控制輸入，此時系統的觀測度為0，系統不可觀，從圖8(b)可以看出在速度調整前系統的定位誤差很大；在200 s后系統調整控制輸入，可以看出調整控制輸入后系統可觀測度顯著提高，定位誤差也逐漸減小，驗證了本文所提方法的有效性.

圖7 調整控制輸入的協同定位系統

圖8 調整控制輸入的協同定位系統

3.2 USV水上實驗

為進一步驗證本文所得出的結論在實際系統中的有效性，在無錫太湖水域進行了協同定位湖上驗證性試驗. 主從USV均配備GPS，從USV配備GPS是為了給后續數據處理過程中提供位置基準. 試驗過程中以主艇高精度的實時GPS位置信息為參考基準，基于水聲通信設備實現主從艇間參考信息的傳遞以及相對距離信息的觀測，進而實現對從艇航位推算誤差的協同校正. 從艇的航位推算位置是基于DVL提供的絕對速度信息以及磁羅經提供的航向信息推算得到的. 水聲通信設備是實現航行器協同定位的關鍵，試驗采用水聲通信設備構建水聲通信網絡，實現主從艇相互間相對距離的測量以及參考信息的傳遞. 試驗中GPS定位精度為2.5 m(RMS)，測速精度0.1 m/s，更新頻率為10 Hz，磁羅經方位精度為2°，DVL測速精度為0.1%. 試驗過程中主艇以5 s固定時間間隔向從艇發送自身帶標識符的GPS位置信息以及二者間相對距離量測信息，輔助從艇進行導航誤差的協同校正. 主從USV在水上共進行了40 min試驗，試驗中主從USV的軌跡如圖9所示. 首先主從USV同向航行，前后相距大約100 m，主USV始終朝著東向航行，從USV在前8 min左右向東以大于主USV的速度航行，從USV航行約8 min后轉向，以90°航向角航行，航行約2 min后，與主USV同向航行，航速與主艇大致相同，航行約14 min后，轉向朝著主USV航行約4 min，然后再次與主USV同向航行直至實驗結束，但速度時刻變化.

按照前文分析，前8 min主從USV連線與協同坐標系x軸夾角為從USV在協同坐標系中航向角，則系統不可觀；從第8 min到第10 min是可觀測路徑；從第10 min到第24 min主從USV同向同速航行，主從USV相對靜止，系統不可觀；從第24 min到第28 min從USV滿足可觀測條件；28 min后，由于從USV速度時刻變化，使得其始終可觀測. 從USV定位誤差及觀測度如圖10所示，可以看出試驗結果與本文分析相符，由于實際系統中的噪聲不是仿真實驗中的零均值高斯噪聲，導致從USV在不可觀測時誤差逐漸增大，在第8 min到第10 min時雖然系統可觀測，但是由于觀測性很小，在圖10(b)中只有一個小的尖峰，而且時間很短，所以此時誤差下降不多，在系統可觀測時誤差穩定在10 m左右，驗證了本文分析的正確性.

圖9 水上試驗軌跡

(a)從USV定位誤差

(b)可觀測度

Fig.10 Positioning Error andObservability of follower USV in water experiment

4 結 論

1)兩種不可觀測路徑：從USV在協同坐標系坐標系中速度為零時系統不可觀；協同坐標系中主從USV連線與x軸夾角與航向角相同時系統不可觀測.

2)主從式USV協同定位系統可觀測度可由式(16)表示. 從USV在協同坐標系中速度大小決定較高可觀測度的范圍大小，速度越大，高可觀測度范圍越大，γ決定不可觀測范圍方向.

3)在USV協同定位時，可以根據下式v=(x2+y2)/|ycosγ-xsinγ|來改變USV速度，達到提高系統可觀測度、減小定位誤差的目的.

[1] YAN Rujian , PANG Shuo , SUN Hanbing.Development and missions of unmanned surface vehicle [J].Journal of Marine Science and Application , 2010,9(1):451-457.

[2] 張立川,徐德民,劉明雍,等.基于移動長基線的多AUV協同導航[J].機器人,2009,31(6):581-585,593.

ZHANGLichuan, XU Demin, LIU Mingyong, et al． Cooperative navigation of multiple AUVs using moving long baseline[J]． Robot，2009，31(6) : 581-585，593

[3] 張立川,徐德民,劉明雍,等.基于雙水聽器的多自主水下航行器協同導航方法[J].系統工程與電子技術,2011,33(7):1603-1606.

ZHANGLichuan，XU Demin，LIU Mingyong，et al． Cooperative navigation for multiple autonomous underwater vehicles based on two hydrophones[J].Systems Engineering and Electronics,2011,33(7):1603-1606.

[4] CURCIOO J, LEONARD J, VAGANAY J, et al． Experiments in moving baseline navigation using autonomous surface craft[C]//Proceedings of MTS/IEEE OCEANS． Washington DC: IEEE，2005:730-735．

[5] BAHR A．Cooperative localization for autonomous underwater vehicles [R]． Cambridge: Massachusetts Institute of Technology，2009．

[6] ZHANG Lichuan，XU Demin，LIU Mingyong，et al．Cooperative navigation and location for multiple UUVs[J]． Journal of Marine Science and Application，2009，8 (3):216-221．

[7] 姚堯,徐德民,張立川,等.通信延遲下的多UUV 協同定位—基于航跡預測的實時更新算法[J].機器人,2011,33(1):161-168.

YAO Yao, ZHANG Lichuan, XU Demin, et al. Cooperative localization of multiple UUVs with communication delays—a real-time update method based on path prediction[J].Robot,2011,33(1):161-168.

[8] 李聞白,劉明雍,李虎雄,等.基于單領航者相對位置測量的多AUV協同導航系統定位性能分析[J].自動化學報,2011,37(6):24-73,76.

LI Wenbai, LIU Mingyong, LI Huxiong, et al. Localization performance analysis of cooperative navigation system for multiple AUVs based on relative position measurements with a single leader[J]. Acta Automatica Sinica,2011,37(6):24-73,76.

[9] PAPADOPOULOS G. Underwater vehicle localization using range measurements [D].Cambridge: Massachusetts Institute of Technology, 2010.

[10]房新鵬,嚴衛生,李俊兵.基于距離量測的主從式AUV協同定位系統觀測性研究[J].西北工業大學學報,2012,30(4):547-552.

FANG Xinpeng, YAN Weisheng, LI Junbing, et al. Observability analysis of leader-follower cooperative localization system based on relative position measurements[J].Journal of Northwestern Polytechnical University,2012,30(4):547-552.

[11]馬朋,張福斌,徐德民,等 基于條件數的多自主水下航行器協同定位系統可觀測度分析[J]. 兵工學報,2015,36(1):138-143.

MA Peng, ZHANG Fubin, XU Demin, et al. Observability Analysis of cooperative localization system for MAUV based on condition number[J].ACTA ARMAMENTARII,2015,01:138-143.

[12]GADRE A.Observability analysis in navigation systems with an underwater vehicle application [D].Montgomery: Virginia Polytechnic Institute and State University, 2007.

Performanceanalysisofleader-followerUSVs’cooperativelocalizationsystem

YU Fei, FAN Shiwei, LI Qian, HAO Qiang, JIANG Pan

(School of Electrical Engineeringand Automation, Harbin Institute of Technology, Harbin 150001, China)

This paper studies the performance analysis of the multiple USVs’ cooperative localization system based on the relative distance measurements with a single leader under different maneuver conditions. In order to analyze the performance of the cooperative localization system quantitatively, a new cooperative coordinate is established and motion models of the leader and follower USVs are derived. Furthermore, the USV cooperative localization system’s observability and the degree of observability is analyzed. In addition, aiming at unobservable paths, a method about changing control input is proposed to improve the degree of observability. Through MATLAB simulation experiment and water experiment results show that under different conditions, by adjusting the control input method to change the system observability is effective. And the results show that the analysis method proposed in this paper can reflect the positioning performance of cooperative positioning system, verify the effectiveness of this method.

USV; cooperative localization; performance analysis; control input; observability

10.11918/j.issn.0367-6234.201607062

U666

A

0367-6234(2017)09-0129-07

2016-07-07

國家自然科學基金(51509063)

于 飛(1974—)，男，教授，博士生導師

范世偉， fanshiwei1993@163.com

(編輯魏希柱)