基于aukf的深海機(jī)器人長(zhǎng)基線組合導(dǎo)航方法
【專利摘要】本發(fā)明涉及一種基于AUKF的深海機(jī)器人長(zhǎng)基線組合導(dǎo)航方法，利用全球定位系統(tǒng)獲取深海機(jī)器人的初始絕對(duì)位置作為航跡推算的初始點(diǎn)，并采深海機(jī)器人的集初始信息；構(gòu)建無色卡爾曼濾波主濾波器并對(duì)采集到的初始信息進(jìn)行濾波估計(jì)，構(gòu)建無色卡爾曼輔助濾波器，對(duì)主濾波器濾波估計(jì)后信息進(jìn)一步濾波估計(jì)，采用自適應(yīng)無色卡爾曼濾波的方法對(duì)采集到的初始信息進(jìn)行數(shù)據(jù)融合，得出融合后的信息。本發(fā)明提高使用長(zhǎng)基線定位系統(tǒng)的深海機(jī)器人的導(dǎo)航精度，同時(shí)能夠平滑深海機(jī)器人控制系統(tǒng)所需的航向、深度以及載體坐標(biāo)系下的速度信息。
【專利說明】基于AUKF的深海機(jī)器人長(zhǎng)基線組合導(dǎo)航方法

【技術(shù)領(lǐng)域】
[0001] 本發(fā)明涉及海洋工程領(lǐng)域的一種深海機(jī)器人導(dǎo)航技術(shù)，具體地說是一種基于自適 應(yīng)無色卡爾曼濾波的長(zhǎng)基線組合導(dǎo)航方法。

【背景技術(shù)】
[0002] 精確的導(dǎo)航能力是水下機(jī)器人有效作業(yè)和安全回收的關(guān)鍵。導(dǎo)航系統(tǒng)必須提供遠(yuǎn) 距離及長(zhǎng)時(shí)間范圍內(nèi)的精確定位、速度和姿態(tài)信息。但是由于受到水下環(huán)境的復(fù)雜性、機(jī)器 人自身體積、重量、能源以及隱蔽性等因素的影響，實(shí)現(xiàn)高精度的水下機(jī)器人導(dǎo)航仍然是一 項(xiàng)艱巨的任務(wù)。
[0003] 現(xiàn)有的水下導(dǎo)航方法主要是多傳感器組合導(dǎo)航方法，對(duì)多傳感器信息進(jìn)行濾波。 但是針對(duì)深海機(jī)器人的導(dǎo)航問題，在系統(tǒng)噪聲信息未知時(shí)，如果設(shè)置的噪聲參數(shù)與實(shí)際的 噪聲相差過大，就會(huì)導(dǎo)致有偏估計(jì)甚至濾波算法的發(fā)散。海洋環(huán)境復(fù)雜多變，很難對(duì)噪聲做 出較為準(zhǔn)確的先驗(yàn)估計(jì)，因此要設(shè)計(jì)和采用更適合于實(shí)時(shí)估計(jì)系統(tǒng)噪聲信息的濾波算法。 長(zhǎng)基線定位數(shù)據(jù)野值較多、延遲較大，而且某些情況下工作狀態(tài)不正常，因此對(duì)長(zhǎng)基線數(shù)據(jù) 的正確處理對(duì)提高導(dǎo)航精度也至關(guān)重要。


【發(fā)明內(nèi)容】

[0004] 針對(duì)現(xiàn)有技術(shù)的不足，本發(fā)明提供一種基于自適應(yīng)無色卡爾曼濾波的深海機(jī)器人 組合導(dǎo)航方法，目的在于提高使用長(zhǎng)基線定位系統(tǒng)的深海機(jī)器人的導(dǎo)航精度，同時(shí)能夠平 滑深海機(jī)器人控制系統(tǒng)所需的航向、深度以及載體坐標(biāo)系下的速度信息。
[0005] 本發(fā)明為實(shí)現(xiàn)上述目的所采用的技術(shù)方案是：一種基于AUKF的深海機(jī)器人長(zhǎng)基 線組合導(dǎo)航方法，利用全球定位系統(tǒng)獲取深海機(jī)器人的初始絕對(duì)位置作為航跡推算的初始 點(diǎn)，并采集深海機(jī)器人的初始信息；
[0006] 構(gòu)建無色卡爾曼濾波主濾波器并對(duì)采集到的初始信息進(jìn)行濾波估計(jì)，構(gòu)建無色卡 爾曼輔助濾波器，對(duì)主濾波器濾波估計(jì)后信息進(jìn)一步濾波估計(jì)；
[0007] 采用自適應(yīng)無色卡爾曼濾波的方法對(duì)采集到的初始信息進(jìn)行數(shù)據(jù)融合，得出融合 后的信息。
[0008] 所述初始信息包括多普勒計(jì)程儀采集的線速度信息，運(yùn)動(dòng)傳感器采集的姿態(tài)信 息，長(zhǎng)基線聲學(xué)定位裝置采集的位置信息和深度計(jì)采集的深度信息。
[0009] 所述長(zhǎng)基線聲學(xué)定位裝置通過自行解算深海機(jī)器人絕對(duì)位置的方法獲取位置信 息。
[0010] 所述自行解算深海機(jī)器人絕對(duì)位置的方法，步驟如下：
[0011] 長(zhǎng)基線數(shù)據(jù)包協(xié)議解析出深海機(jī)器人到每個(gè)信標(biāo)的距離信息，判斷各信標(biāo)數(shù)據(jù)的 有效性，根據(jù)每個(gè)有效信標(biāo)的絕對(duì)位置信息和深海機(jī)器人到信標(biāo)的距離信息，建立三元二 次方程組：
[0012]

【權(quán)利要求】
1. 一種基于AUKF的深海機(jī)器人長(zhǎng)基線組合導(dǎo)航方法，其特征在于： 利用全球定位系統(tǒng)獲取深海機(jī)器人的初始絕對(duì)位置作為航跡推算的初始點(diǎn)，并采集深 海機(jī)器人的初始信息； 構(gòu)建無色卡爾曼濾波主濾波器并對(duì)采集到的初始信息進(jìn)行濾波估計(jì)，構(gòu)建無色卡爾曼 輔助濾波器，對(duì)主濾波器濾波估計(jì)后信息進(jìn)一步濾波估計(jì)； 采用自適應(yīng)無色卡爾曼濾波的方法對(duì)采集到的初始信息進(jìn)行數(shù)據(jù)融合，得出融合后的 信息。
2. 根據(jù)權(quán)利要求1所述的基于AUKF的深海機(jī)器人長(zhǎng)基線組合導(dǎo)航方法，其特征在于： 所述初始信息包括多普勒計(jì)程儀采集的線速度信息，運(yùn)動(dòng)傳感器采集的姿態(tài)信息，長(zhǎng)基線 聲學(xué)定位裝置采集的位置信息和深度計(jì)采集的深度信息。
3. 根據(jù)權(quán)利要求2所述的基于AUKF的深海機(jī)器人長(zhǎng)基線組合導(dǎo)航方法，其特征在于： 所述長(zhǎng)基線聲學(xué)定位裝置通過自行解算深海機(jī)器人絕對(duì)位置的方法獲取位置信息。
4. 根據(jù)權(quán)利要求3所述的基于AUKF的深海機(jī)器人長(zhǎng)基線組合導(dǎo)航方法，其特征在于： 所述自行解算深海機(jī)器人絕對(duì)位置的方法，步驟如下： 長(zhǎng)基線數(shù)據(jù)包協(xié)議解析出深海機(jī)器人到每個(gè)信標(biāo)的距離信息，判斷各信標(biāo)數(shù)據(jù)的有效 性，根據(jù)每個(gè)有效信標(biāo)的絕對(duì)位置信息和深海機(jī)器人到信標(biāo)的距離信息，建立三元二次方 程組：
上述方程組中（xA，yA，zA)，（xB，y B，zB)，（Xc，yc，zc)分別為各有效信標(biāo)的絕對(duì)位置坐標(biāo)， 14、1^1。分別為深海機(jī)器人到各個(gè)信標(biāo)的距離，解此方程組得（^，2。）為深海機(jī)器人的絕 對(duì)位置坐標(biāo)，得到機(jī)器人絕對(duì)位置坐標(biāo)的兩組解，根據(jù)當(dāng)前時(shí)刻深度計(jì)采集的深度信息判 定兩組解中的有效解，作為觀測(cè)矢量中的東向位置觀測(cè)值和北向位置觀測(cè)值。
5. 根據(jù)權(quán)利要求1所述的基于AUKF的深海機(jī)器人長(zhǎng)基線組合導(dǎo)航方法，其特征在于： 無色卡爾曼主濾波器濾波估計(jì)的實(shí)現(xiàn)過程為： 對(duì)上一時(shí)刻隨機(jī)向量的被估計(jì)量均值和方差經(jīng)非線性無色變換后，得到離散Sigma 點(diǎn)，經(jīng)主濾波器的狀態(tài)方程計(jì)算更新Sigma點(diǎn)，然后計(jì)算出更新Sigma點(diǎn)的狀態(tài)均值和方 差，對(duì)狀態(tài)均值和方差計(jì)算得到一步估計(jì)的狀態(tài)均值和方差；對(duì)一步估計(jì)的狀態(tài)均值和方 差再經(jīng)無色變換和主濾波器的測(cè)量方程計(jì)算更新，得到深海機(jī)器人可觀測(cè)狀態(tài)的均值、方 差及協(xié)方差的估計(jì)值；讀取傳感器采集觀測(cè)信息，并與狀態(tài)均值、方差、觀測(cè)狀態(tài)的估計(jì)值 和增益矩陣計(jì)算估計(jì)，得到系統(tǒng)均值、方差的估算值，并得到新息；根據(jù)主濾波器的新息經(jīng) 計(jì)算得到測(cè)量值作為輔助濾波器的觀測(cè)信號(hào)。
6. 根據(jù)權(quán)利要求1所述的基于AUKF的深海機(jī)器人長(zhǎng)基線組合導(dǎo)航方法，其特征在于： 輔助濾波器對(duì)系統(tǒng)過程噪聲進(jìn)行濾波估計(jì)的實(shí)現(xiàn)過程如下： 上一時(shí)刻估計(jì)得到輔助濾波器的系統(tǒng)噪聲方差矩陣的對(duì)角線元素和方差，用輔助濾波 器的狀態(tài)方程對(duì)對(duì)角線元素和方差計(jì)算更新后得到對(duì)角線元素和方差的一步估計(jì)值；用輔 助濾波器的測(cè)量方程根據(jù)主濾波器的一步估計(jì)的方差、狀態(tài)方差和增益矩陣計(jì)算更新后得 到估計(jì)的觀測(cè)值，將得到的一步估計(jì)值、觀測(cè)值和主濾波器的測(cè)量值計(jì)算估計(jì)得到系統(tǒng)過 程噪聲方差陣的對(duì)角線元素的估算值和方差，當(dāng)有新觀測(cè)信息時(shí)，繼續(xù)下一控制周期主濾 波器的實(shí)現(xiàn)步驟。
7. 根據(jù)權(quán)利要求6所述的基于AUKF的深海機(jī)器人長(zhǎng)基線組合導(dǎo)航方法，其特征在于： 所述輔助濾波器的狀態(tài)方程建立如下： ---
其中講%為輔助濾波器中的系統(tǒng)噪聲，％為系統(tǒng)噪聲方差矩陣角線元素。
8. 根據(jù)權(quán)利要求6所述的基于AUKF的深海機(jī)器人長(zhǎng)基線組合導(dǎo)航方法，其特征在于： 所述輔助濾波器的測(cè)量方程建立如下：
其中，vdiag(_)表示一個(gè)向量，其分量為矩陣（?）的主對(duì)角線元素，系統(tǒng)噪聲方差矩 陣角線元素 A作為系統(tǒng)的觀測(cè)信號(hào)，Pkh為主濾波器狀態(tài)矢量的一步估計(jì)的方差，Pk為主 濾波器狀態(tài)矢量的方差，Kk為主濾波器的增益矩陣，&/,為估計(jì)的觀測(cè)值。
9. 根據(jù)權(quán)利要求5所述的基于AUKF的深海機(jī)器人長(zhǎng)基線組合導(dǎo)航方法，其特征在于： 所述主濾波器的測(cè)量方程根據(jù)長(zhǎng)基線定位數(shù)據(jù)的有效時(shí)刻進(jìn)行切換： 當(dāng)長(zhǎng)基線聲學(xué)定位裝置提供的觀測(cè)值有效時(shí)，測(cè)量方程定義為： Yk = xk+Nn 其中，y= [uy，vy，wy，ry, €y, ny, 4y, ￥y]T為觀測(cè)矢量，其中115\￥5\'\￥ 5\；^分別是前向速 度、側(cè)向速度、垂向速度和轉(zhuǎn)艏角速度的觀測(cè)量，€y、ny、4y、v y是北向位置、東向位置、深 度和艏向角的觀測(cè)量，Nn為觀測(cè)噪聲； 當(dāng)長(zhǎng)基線聲學(xué)定位裝置提供的觀測(cè)值無效時(shí)，測(cè)量方程定義為：
10. 根據(jù)權(quán)利要求9所述的基于AUKF的深海機(jī)器人長(zhǎng)基線組合導(dǎo)航方法，其特征在于： 所述有效時(shí)刻為長(zhǎng)基線數(shù)據(jù)更新并且不是野值的時(shí)刻。
【文檔編號(hào)】G01C21/00GK104280026SQ201310284939
【公開日】2015年1月14日 申請(qǐng)日期:2013年7月8日 優(yōu)先權(quán)日:2013年7月8日
【發(fā)明者】劉開周, 李靜, 郭威, 祝普強(qiáng), 王曉輝 申請(qǐng)人:中國(guó)科學(xué)院沈陽(yáng)自動(dòng)化研究所