專利名稱：一種基于噪聲矩陣實時修正的船舶動力定位方法
技術(shù)領(lǐng)域：
本發(fā)明涉及船舶動力定位技術(shù)，特別是涉及一種基于噪聲矩陣實時修正的船舶動 力定位方法。
背景技術(shù)：
動力定位Dynamic Positioning是指海上作業(yè)系統(tǒng)比如輪船、深海鉆井平臺等系 統(tǒng)利用其本身具有的動力來克服風(fēng)、波浪、海流等外部環(huán)境干擾，從而使之能夠保持在一定 的位置和方向上，更進(jìn)一步說明是指不借助錨泊系統(tǒng)的作用，而用精密先進(jìn)的儀器來測定 海上作業(yè)系統(tǒng)因為風(fēng)、浪、流作用而發(fā)生的位移和方向變化，同時通過計算機等自動控制系 統(tǒng)對信息進(jìn)行處理和計算并自動控制若干個不同方向的推進(jìn)器的推力或力矩使得海上作 業(yè)系統(tǒng)回復(fù)到設(shè)定位置。此外，如果將定位點以一定速度沿預(yù)定軌跡運動，也可實現(xiàn)航跡保 持。與普通依靠舵實線航跡保持相比，動力定位具有橫向運動的能力，從而為實現(xiàn)更高精度 的航跡保持提供了可能性，隨著人類對海洋的開發(fā)、探索，動力定位技術(shù)顯得越來越重要。在動力定位中，需要進(jìn)行位置和速度的測量，對于位置的測量，一般分為方位和角 度。其中，角度的測量，在動力定位系統(tǒng)中通常采用電羅經(jīng)，而方位的測量通常采用衛(wèi)星導(dǎo) 航系統(tǒng)(GPS)，速率陀螺儀用于測量角速度、橫蕩速度和縱蕩速度(1)電羅經(jīng)，又稱陀螺羅經(jīng)，它能自動、連續(xù)地提供艦船的航向信號，并通過航向 發(fā)送裝置將航向信號傳遞到艦船需要航向信號的各個部位，從而滿足艦船導(dǎo)航和定位的要 求，是艦船必不可少的精密導(dǎo)航設(shè)備，被稱為艦船的“眼睛”。全套設(shè)備由主羅經(jīng)、分羅經(jīng)和 附屬儀器三部分組成，核心部件是主羅經(jīng)內(nèi)的陀螺球。電羅經(jīng)受到其機械結(jié)構(gòu)和工作環(huán)境 等的影響，不可避免的存在誤差，需要進(jìn)行濾波。(2)衛(wèi)星導(dǎo)航系統(tǒng)有兩種商用的衛(wèi)星導(dǎo)航系統(tǒng)，美國的Navstar GPS系統(tǒng)和俄羅 斯的GL0NASS系統(tǒng)。GPS定位是利用一組衛(wèi)星的偽距、星歷、衛(wèi)星發(fā)射時間等觀測量來實現(xiàn) 的，同時還必須知道用戶時鐘差。因此，要獲得地面點的三維坐標(biāo)，必須對4顆衛(wèi)星進(jìn)行測 量。在這一定位過程中，存在著三部分誤差。一部分是對每一個用戶接收機所公有的，例 如，衛(wèi)星鐘誤差、星歷誤差、電離層誤差、對流層誤差等；第二部分為不能由用戶測量或由校 正模型來計算的傳播延遲誤差；第三部分為各用戶接收機所固有的誤差，例如內(nèi)部噪聲、通 道延遲、多徑效應(yīng)等。利用差分GPS定位技術(shù)(DGPS)，除第三部分誤差無法消除外，第一部分誤差完全 可以消除，第二部分誤差大部分可以消除，其主要取決于基準(zhǔn)接收機和用戶接收機的距離。 差分GPS定位已將衛(wèi)星鐘誤差和星歷誤差消除，并將電離層延遲和對流層延遲誤差部分消 除，定位精度大大提高。所以，差分GPS定位技術(shù)(DGPS)在最近幾年中得到了迅速發(fā)展和 廣泛應(yīng)用。根據(jù)差分GPS基準(zhǔn)站發(fā)送的信息方式可將差分GPS定位技術(shù)(DGPS)分為三類， 即位置差分、偽距差分和相位差分。這三類差分方式的工作原理是相同的，即都是由基準(zhǔn) 站發(fā)送改正數(shù)，由用戶站接收并對其測量結(jié)果進(jìn)行改正，以獲得精確的定位結(jié)果。所不同的是，發(fā)送改正數(shù)的具體內(nèi)容不一樣，其差分定位精度也不同。(3)速率陀螺儀。用以直接測定運載器角速率的二自由度陀螺裝置。把均衡陀螺 儀的外環(huán)固定在運載器上并令內(nèi)環(huán)軸垂直于要測量角速率的軸。當(dāng)運載器連同外環(huán)以角速 度繞測量軸旋進(jìn)時，陀螺力矩將迫使內(nèi)環(huán)連同轉(zhuǎn)子一起相對運載器旋進(jìn)。陀螺儀中有彈簧 限制這個相對旋進(jìn)，而內(nèi)環(huán)的旋進(jìn)角正比于彈簧的變形量。由平衡時的內(nèi)環(huán)旋進(jìn)角即可求 得陀螺力矩和運載器的角速率。它在遠(yuǎn)距離測量系統(tǒng)或自動控制、慣性導(dǎo)航平臺中使用較多ο由于差分GPS和速率陀螺儀同樣存在誤差，需要進(jìn)行濾波處理?？柭鼮V波是卡爾曼在1960年提出的從被提取信號有關(guān)的觀測量中通過算法估 計出所需要信號的一種濾波算法。把狀態(tài)空間的概念引入到隨機估計理論中，把信號過程 視為白噪聲作用下的一個線性系統(tǒng)的輸出，用狀態(tài)空間來描述這種輸入一輸出關(guān)系，估計 過程中利用系統(tǒng)狀態(tài)方程，觀測方程和白噪聲激勵的統(tǒng)計特性形成濾波算法，由于所用的 信息都是時域內(nèi)的量，所以不但可以對平穩(wěn)的一維隨機過程進(jìn)行估計，也可以對非平穩(wěn)的、 多維隨機過程進(jìn)行估計?？柭鼮V波作為一種重要的最優(yōu)估計理論被廣泛應(yīng)用到實際工程中，比如慣性導(dǎo) 航、制導(dǎo)系統(tǒng)、全球定位系統(tǒng)、目標(biāo)跟蹤、通信和信號過程、金融，電機等?？柭鼮V波設(shè)計方法簡單易行，隨著其廣泛的應(yīng)用，也暴露出一些問題，尤其是應(yīng) 用到某些具體領(lǐng)域。如它必須在數(shù)字計算機上運行，隨著微型計算機尤其是嵌入式芯片的 普及，人們對卡爾滿濾波數(shù)值的穩(wěn)定性、計算效率、實用性和有效性的要求也越來越高。由 于計算機的字長有限，使計算機舍入誤差和截斷誤差累積、傳遞，造成誤差方差陣失去對稱 正定性，數(shù)值不穩(wěn)定。在卡爾曼濾波的發(fā)展中，針對這個問題提出了平方根濾波、UD分解 濾波、奇異值分解濾波等一系列數(shù)值魯棒的濾波算法。又如傳統(tǒng)的卡爾曼濾波是建立在模 型精確和隨機干擾信號統(tǒng)計特性已知的基礎(chǔ)上。對于一個實際系統(tǒng)，往往存在著模型不確 定或干擾信號統(tǒng)計特性不完全已知，這些不確定因素使得傳統(tǒng)的卡爾曼濾波算法失去最優(yōu) 性，估計精度大大降低，嚴(yán)重時會引起濾波發(fā)散。為此，人們將魯棒控制的思想引入到濾波 中，形成了魯棒濾波理論。在進(jìn)行動力定位系統(tǒng)的相關(guān)研究中，針對動力定位中實時位置采樣普遍采用的卡 爾曼濾波進(jìn)行較為細(xì)致的研究。發(fā)現(xiàn)在傳統(tǒng)的卡爾曼濾波算法中，一般將系統(tǒng)噪聲均看作 高斯白噪聲。由于白噪聲信號的方差恒定，故卡爾曼濾波算法中的系統(tǒng)噪聲矩陣Q可作為 常數(shù)矩陣處理。這樣做的優(yōu)點，是使得系統(tǒng)進(jìn)行卡爾曼濾波迭代時的計算更簡便。通常，可 以根據(jù)海試實驗資料選擇相應(yīng)的初值。但上述做法也存在較大的問題，當(dāng)實際海況比較復(fù) 雜、系統(tǒng)未知擾動較大時，將系統(tǒng)噪聲矩陣Q作為常數(shù)矩陣處理，將嚴(yán)重影響卡爾曼濾波的 精度。另外，受到海試實驗的局限性影響，給定的初始Q矩陣的值可能存在較大誤差，將影 響卡爾曼濾波的精度和快速性。

發(fā)明內(nèi)容
本發(fā)明所要解決的技術(shù)問題就是為了克服上述現(xiàn)有技術(shù)存在的缺陷而提供一種 基于噪聲矩陣實時修正的船舶動力定位方法。本發(fā)明的目的可以通過以下技術(shù)方案來實現(xiàn)一種基于噪聲矩陣實時修正的船舶
4動力定位方法，其特征在于，包括以下步驟由綜合位置測量系統(tǒng)測量船舶本身位置信號，并通過基于噪聲矩陣實時修正的卡 爾曼數(shù)字濾波器濾去上述信號中的噪聲信號，并分離出船舶位置和姿態(tài)的高、低頻信號，并 將低頻信號傳輸給控制系統(tǒng)；由環(huán)境測量傳感器系統(tǒng)測量船舶所處環(huán)境的參數(shù)，將測得的環(huán)境參數(shù)信號傳輸給 控制系統(tǒng)；控制系統(tǒng)根據(jù)濾波后的低頻信號及環(huán)境信號計算船舶定位控制信號；推力分配器將計算出的船舶定位控制信號分配給推力系統(tǒng)的各推力器；各推力器根據(jù)船舶定位控制信號，將船舶推動至預(yù)期的位置。所述的綜合位置測量系統(tǒng)包括處理器、用于測量艏搖角度的電羅經(jīng)、用于測量橫 蕩位置和縱蕩位置的差分衛(wèi)星導(dǎo)航系統(tǒng)(GPS)以及用于測量角速度、橫蕩速度和縱蕩速度 的速率陀螺儀，所述的電羅經(jīng)、差分GPS以及速率陀螺儀均與處理器連接，該處理器采用基 于噪聲矩陣實時修正的卡爾曼數(shù)字濾波器進(jìn)行濾波。所述的基于噪聲矩陣實時修正的卡爾曼數(shù)字濾波器通過以下步驟進(jìn)行濾波(1)給定初始值f(0),P(0)；(2) x{k\k-\) = Jx{k-\) + BU(3) m{k-\) = (HE-ψ)+ [HAX{k-\) +HBU-Z{k)]，通過更新 ω (k_l)來更新 Q(k-l)；(4) P{k\k-\) = AP(k — VyA + EQ(k-\) Τ ；(5) K (k) =P (k I k_l) Ht [HP (k | k_l) HT+R (k) ] ；(6) x(k) = “A:丨A；-1) + K(k)[Z(k)-Hx(k\k-l)]',(7)P(k) = (I-K (k) H) P (k |k-l)；(8)k = k+1，返回到(2)。所述的處理器采用AtmageS處理器。與現(xiàn)有技術(shù)相比，本發(fā)明通過實時計算系統(tǒng)噪聲，將計算結(jié)果應(yīng)用于修正噪聲序 列的方差，使動力定位系統(tǒng)在復(fù)雜海況下，仍能保持較好的定位精度，該方法具有一定普遍 應(yīng)用價值，可應(yīng)用于未知環(huán)境(環(huán)境擾動未知程度大，統(tǒng)計特性未知)或大擾動環(huán)境(環(huán)境 擾動劇烈，對具有卡爾曼濾波的系統(tǒng)影響較大，明顯不能將其看做具有常值方差的白噪聲) 中的卡爾曼濾波。通過實時更新噪聲矩陣，有效克服上述環(huán)境的影響，提高濾波精度。


圖1為本發(fā)明的原理圖；圖2為本發(fā)明的綜合位置測量及處理系統(tǒng)的結(jié)構(gòu)示意圖。
具體實施例方式下面結(jié)合附圖對本發(fā)明作進(jìn)一步說明。如圖1所示，一種基于噪聲矩陣實時修正的船舶動力定位方法，包括以下步驟由綜合位置測量系統(tǒng)測量船舶本身位置信號，并通過基于噪聲矩陣實時修正的卡
5爾曼數(shù)字濾波器濾去上述信號中的噪聲信號，并分離出船舶位置和姿態(tài)的高、低頻信號，并 將低頻信號傳輸給控制系統(tǒng)；由環(huán)境測量傳感器系統(tǒng)測量船舶所處環(huán)境的參數(shù)，將測得的環(huán)境參數(shù)信號傳輸給 控制系統(tǒng)；控制系統(tǒng)根據(jù)濾波后的低頻信號及環(huán)境信號計算船舶定位控制信號；推力分配器將計算出的船舶定位控制信號分配給推力系統(tǒng)的各推力器；各推力器根據(jù)船舶定位控制信號，將船舶推動至預(yù)期的位置。如圖2所示，綜合位置測量系統(tǒng)包括處理器1、用于測量艏搖角度的電羅經(jīng)2、用于 測量橫蕩位置和縱蕩位置的差分GPS3以及用于測量角速度、橫蕩速度和縱蕩速度的陀螺 儀4，所述的電羅經(jīng)2、差分GPS3以及陀螺儀4均與處理器1連接，該處理器1采用基于噪 聲矩陣實時修正的卡爾曼數(shù)字濾波器進(jìn)行濾波，將電羅經(jīng)，差分GPS、陀螺儀測得的數(shù)據(jù)，發(fā) 送到AtmageS處理器中，處理器內(nèi)置已經(jīng)提出的改進(jìn)型卡爾曼濾波算法，經(jīng)行濾波處理。將 濾波后的信號送船舶動力定位系統(tǒng)的控制器，控制船舶有效實現(xiàn)定位；所述的基于噪聲矩 陣實時修正的卡爾曼數(shù)字濾波器通過以下步驟進(jìn)行濾波(1)給定初始值i(0),P(0)；(2) x(k] k-I) =Iiik-Y) +BU(3) (o{k-1) = (H~E-ψ)+ [Η X{k-1) +HBU-Z(k)]，通過更新 ω (k_l)來更新 Q(k-l)；(4)尸(眾丨 A: -1) 二 Jp(k - \)AT + EQ(k - Υ) Γ ；(5) K (k) =P (k I k_l) Ht [HP (k | k_l) HT+R (k) ] ；(6) x(k) ^x{k\h-\) + K(k)[Z(k)-Hx(k\k-l)]；(7)P(k) = (I-K (k) H) P (k |k-l)；(8)k = k+1，返回到(2)。所述的處理器采用AtmageS處理器，此款單片機芯片具有速度快，實時性好，性價 比高的特點，并且其接口等資源滿足需要，適合于進(jìn)行改進(jìn)型的卡爾曼濾波算法的實現(xiàn)。當(dāng)實際海況比較復(fù)雜、系統(tǒng)未知擾動較大時，噪聲矩陣仍作為常值矩陣處理，將成 為桎梏卡爾曼濾波性能的重要因素。針對這種狀況，這里提出一種基于噪聲矩陣實時修正 的卡爾曼濾波方法。該方法通過實時計算系統(tǒng)噪聲，將計算結(jié)果應(yīng)用于修正噪聲序列的方 差。從而提升了卡爾曼濾波在復(fù)雜擾動環(huán)境下(通常這種環(huán)境的擾動不能看做是統(tǒng)計特性 已知的白噪聲)的濾波精度。在動力定位系統(tǒng)的實際研究中發(fā)現(xiàn)，采用此種方法可以使動 力定位系統(tǒng)在復(fù)雜海況下，仍能保持較好的定位精度。系統(tǒng)模型如下X (k) = AX(k-l) +BU+E ω (k_ 1)Z (k) = HX (k) +V (k)其中，Ζ=[Α^，Χ^]為待估測狀態(tài)量，表示三個自由度低頻和高頻的位置及速度狀 態(tài)，U代表推力器產(chǎn)生的控制輸入，Z代表船舶低頻縱蕩、橫蕩位置和艏搖角度，ω為6X1 系統(tǒng)噪聲矩陣，V為3X1量測誤差矩陣。
假設(shè)Z0)為測量值，V (k)為實際輸出值，由于在模型中已假設(shè)系統(tǒng)噪聲和量測 噪聲為加性噪聲，可以得到 =HEa(k-l) + V(k)=m\k-l) + V(k)(1)其中，ω' (k-1) = ψ ω (k_l)ω (k-1)左乘Ψ矩陣，將每個自由度的系統(tǒng)高頻和低頻噪聲相加，作為總的系統(tǒng) 噪聲，從而使6維ω (k-1)矩陣轉(zhuǎn)化為3維ω ‘ (k-Ι)矩陣Z (k) = HX (k) +V (k)為量測方程，將系統(tǒng)方程代入可得Z (k) =H[AX (k-1) +BU+E ω (k-l)]+V(k)(2)由式(1)可得 將式(3)代入(2)式可得 根據(jù)卡爾曼濾波算法，可以用1)來估計X(k-l)，用= ⑷來估計 Z(k)。經(jīng)過合并移項和化簡，更新的系統(tǒng)噪聲可以從式(4)得到
]( 5 )其中，(ΗΕ-Ψ)+為廣義逆矩陣。經(jīng)過上述步驟，即可得到一步更新的系統(tǒng)噪聲值，將當(dāng)前值與之前所有的歷史值 作為一個新的噪聲序列求方差，即可實現(xiàn)對Q矩陣的實時更新。根據(jù)上述方法，將卡爾曼濾波算法修正如下(1)給定初始值1(0)，P(0)；(2) x(k\k-l) = Ax(k-\) + BU ；(3) a(k-l) = (HE-V/)+[HAX(k-l) +HBU-Z(k)]，通過更新 ω (k-1)來更新 Q(k-1)；
(4)P{k\k-\) = AP(k — 1)ΑΓ + EQ(k — l)ET ；
(5)K (k) =P (k I k-1) Ht [HP (k I k-1) HT+R (k) ] ；(6) x(k) = WI A -1) + K(k)[Z(k)-Hx(k\k-l)]；(7)P(k) = (I-K (k) H) P (k I k-1)；(8)k = k+1，返回到(2)。上述方法用c/c++語言編寫成程序，實現(xiàn)基于噪聲矩陣實時修正的船舶動力定位 濾波算法。
本發(fā)明由以下三個主要部分組成測量系統(tǒng)、推力系統(tǒng)、控制系統(tǒng)。測量系統(tǒng)是動 力定位系統(tǒng)的基礎(chǔ)，沒有準(zhǔn)確的位置測量信息，便沒有位置控制的輸入。測量系統(tǒng)分為綜 合位置、速度測量系統(tǒng)以及環(huán)境參數(shù)測量系統(tǒng)，位置、速度測量主要采用差分衛(wèi)星導(dǎo)航系統(tǒng) (DGPS)、陀螺儀、電羅經(jīng)；環(huán)境參數(shù)測量通常稱其為傳感器系統(tǒng)，一般采用風(fēng)傳感器，用來測 量風(fēng)速及攻角。推力系統(tǒng)是動力定位系統(tǒng)的一個子系統(tǒng)，其基本作用是提供控制系統(tǒng)所需要的推 力和力矩，滿足定位的需要，推力系統(tǒng)主要由動力機械、如原動機、發(fā)電機組、輸電設(shè)備、推 進(jìn)器、其它輔助設(shè)備如管道系統(tǒng)等組成；控制系統(tǒng)讀取位置測量系統(tǒng)所得到的位置信號，將其船舶實際位置與預(yù)定的目標(biāo) 值作比較，通過控制規(guī)律運算，得到抵消位置偏差和外界干擾力所需要的推力，然后對推力 器發(fā)出指令，以產(chǎn)生推力使船盡可能靠近所希望的位置。推力分配器是控制系統(tǒng)的一部分， 由于一艘動力定位船擁有多個推進(jìn)器，需要對各推進(jìn)器行推力分配，推力分配單元計算分 配到每個推進(jìn)單元上的力和方向指令。
8
權(quán)利要求
一種基于噪聲矩陣實時修正的船舶動力定位方法，其特征在于，包括以下步驟由綜合位置測量系統(tǒng)測量船舶本身位置信號，并通過基于噪聲矩陣實時修正的卡爾曼數(shù)字濾波器濾去上述信號中的噪聲信號，并分離出船舶位置與姿態(tài)的高、低頻信號，并將低頻信號傳輸給控制系統(tǒng)；由環(huán)境測量傳感器系統(tǒng)測量船舶所處環(huán)境的參數(shù)，將測得的環(huán)境參數(shù)信號傳輸給控制系統(tǒng)；控制系統(tǒng)根據(jù)濾波后的低頻信號及環(huán)境信號計算船舶定位控制信號；推力分配器將計算出的船舶定位控制信號分配給推力系統(tǒng)的各推力器；各推力器根據(jù)船舶定位控制信號，將船舶推動至預(yù)期的位置。
2.根據(jù)權(quán)利要求1所述的一種基于噪聲矩陣實時修正的船舶動力定位方法，其特征在 于，所述的綜合位置測量系統(tǒng)包括處理器、用于測量艏搖角度的電羅經(jīng)、用于測量橫蕩位置 和縱蕩位置的差分衛(wèi)星導(dǎo)航系統(tǒng)(GPS)以及用于測量角速度、橫蕩速度和縱蕩速度的速率 陀螺儀，所述的電羅經(jīng)、差分GPS以及速率陀螺儀均與處理器連接，該處理器采用基于噪聲 矩陣實時修正的卡爾曼數(shù)字濾波器進(jìn)行濾波。
3.根據(jù)權(quán)利要求1或2所述的一種基于噪聲矩陣實時修正的船舶動力定位方法，其特 征在于，所述的基于噪聲矩陣實時修正的卡爾曼數(shù)字濾波器通過以下步驟進(jìn)行濾波(ι)給定初始值i(o),P(o)；(2)x(A:|A:-l) = ]ii(A:-l) + 5C/；(3)w{k -1) = {HE-ψ)+ [HAX(k-1) +HBU-Z(k)],通過更新 ω (k_l)來更新 Q(k-l)；(4)尸(AI A _ 1) = AP(k - 1)AT + EQ(k - \)1T ；(5)K (k) =P (k I k-1) Ht [HP (k I k-1) HT+R (k) ] ；(6)x(k) = x(k\k-l) + K{k)[Z{k)-Hx(k\k-\)]·,(7)P(k)= (I-K (k) H) P (k I k-1)；(8)k= k+1，返回到(2)。
4.根據(jù)權(quán)利要求3所述的一種基于噪聲矩陣實時修正的船舶動力定位方法，其特征在 于，所述的處理器采用AtmageS處理器。
全文摘要
本發(fā)明涉及一種基于噪聲矩陣實時修正的船舶動力定位方法，包括由綜合位置測量系統(tǒng)測量船舶本身位置信號，并通過基于噪聲矩陣實時修正的卡爾曼數(shù)字濾波器濾去上述信號中的噪聲信號，并分離出船舶的高、低頻信號，并將低頻信號傳輸給控制系統(tǒng)；環(huán)境測量傳感器系統(tǒng)測量船舶所處環(huán)境的參數(shù)，將測得的環(huán)境參數(shù)信號傳輸給控制系統(tǒng)；控制系統(tǒng)根據(jù)濾波后的低頻信號及部分環(huán)境信號計算船舶定位控制信號；推力分配器將計算出的船舶定位控制信號分配給推力系統(tǒng)的各推力器；各推力器根據(jù)船舶定位控制信號，將船舶推動至預(yù)期的位置。本發(fā)明通過實時計算系統(tǒng)噪聲，將計算結(jié)果應(yīng)用于修正噪聲序列的方差，使動力定位系統(tǒng)在復(fù)雜海況下，仍能保持較好的定位精度。
文檔編號B63B9/00GK101920762SQ200910052768
公開日2010年12月22日 申請日期2009年6月9日 優(yōu)先權(quán)日2009年6月9日
發(fā)明者姜威, 趙霞, 陳啟軍 申請人:同濟(jì)大學(xué)