基於aukf的深海機器人長基線組合導航方法
2023-09-15 23:51:10
基於aukf的深海機器人長基線組合導航方法
【專利摘要】本發明涉及一種基於AUKF的深海機器人長基線組合導航方法，利用全球定位系統獲取深海機器人的初始絕對位置作為航跡推算的初始點，並採深海機器人的集初始信息；構建無色卡爾曼濾波主濾波器並對採集到的初始信息進行濾波估計，構建無色卡爾曼輔助濾波器，對主濾波器濾波估計後信息進一步濾波估計，採用自適應無色卡爾曼濾波的方法對採集到的初始信息進行數據融合，得出融合後的信息。本發明提高使用長基線定位系統的深海機器人的導航精度，同時能夠平滑深海機器人控制系統所需的航向、深度以及載體坐標系下的速度信息。
【專利說明】基於AUKF的深海機器人長基線組合導航方法
【技術領域】
[0001] 本發明涉及海洋工程領域的一種深海機器人導航技術，具體地說是一種基於自適 應無色卡爾曼濾波的長基線組合導航方法。
【背景技術】
[0002] 精確的導航能力是水下機器人有效作業和安全回收的關鍵。導航系統必須提供遠 距離及長時間範圍內的精確定位、速度和姿態信息。但是由於受到水下環境的複雜性、機器 人自身體積、重量、能源以及隱蔽性等因素的影響，實現高精度的水下機器人導航仍然是一 項艱巨的任務。
[0003] 現有的水下導航方法主要是多傳感器組合導航方法，對多傳感器信息進行濾波。 但是針對深海機器人的導航問題，在系統噪聲信息未知時，如果設置的噪聲參數與實際的 噪聲相差過大，就會導致有偏估計甚至濾波算法的發散。海洋環境複雜多變，很難對噪聲做 出較為準確的先驗估計，因此要設計和採用更適合於實時估計系統噪聲信息的濾波算法。 長基線定位數據野值較多、延遲較大，而且某些情況下工作狀態不正常，因此對長基線數據 的正確處理對提高導航精度也至關重要。
【發明內容】
[0004] 針對現有技術的不足，本發明提供一種基於自適應無色卡爾曼濾波的深海機器人 組合導航方法，目的在於提高使用長基線定位系統的深海機器人的導航精度，同時能夠平 滑深海機器人控制系統所需的航向、深度以及載體坐標系下的速度信息。
[0005] 本發明為實現上述目的所採用的技術方案是：一種基於AUKF的深海機器人長基 線組合導航方法，利用全球定位系統獲取深海機器人的初始絕對位置作為航跡推算的初始 點，並採集深海機器人的初始信息；
[0006] 構建無色卡爾曼濾波主濾波器並對採集到的初始信息進行濾波估計，構建無色卡 爾曼輔助濾波器，對主濾波器濾波估計後信息進一步濾波估計；
[0007] 採用自適應無色卡爾曼濾波的方法對採集到的初始信息進行數據融合，得出融合 後的信息。
[0008] 所述初始信息包括都卜勒計程儀採集的線速度信息，運動傳感器採集的姿態信 息，長基線聲學定位裝置採集的位置信息和深度計採集的深度信息。
[0009] 所述長基線聲學定位裝置通過自行解算深海機器人絕對位置的方法獲取位置信 息。
[0010] 所述自行解算深海機器人絕對位置的方法，步驟如下：
[0011] 長基線數據包協議解析出深海機器人到每個信標的距離信息，判斷各信標數據的 有效性，根據每個有效信標的絕對位置信息和深海機器人到信標的距離信息，建立三元二 次方程組：
[0012]
【權利要求】
1. 一種基於AUKF的深海機器人長基線組合導航方法，其特徵在於： 利用全球定位系統獲取深海機器人的初始絕對位置作為航跡推算的初始點，並採集深 海機器人的初始信息； 構建無色卡爾曼濾波主濾波器並對採集到的初始信息進行濾波估計，構建無色卡爾曼 輔助濾波器，對主濾波器濾波估計後信息進一步濾波估計； 採用自適應無色卡爾曼濾波的方法對採集到的初始信息進行數據融合，得出融合後的 信息。
2. 根據權利要求1所述的基於AUKF的深海機器人長基線組合導航方法，其特徵在於： 所述初始信息包括都卜勒計程儀採集的線速度信息，運動傳感器採集的姿態信息，長基線 聲學定位裝置採集的位置信息和深度計採集的深度信息。
3. 根據權利要求2所述的基於AUKF的深海機器人長基線組合導航方法，其特徵在於： 所述長基線聲學定位裝置通過自行解算深海機器人絕對位置的方法獲取位置信息。
4. 根據權利要求3所述的基於AUKF的深海機器人長基線組合導航方法，其特徵在於： 所述自行解算深海機器人絕對位置的方法，步驟如下： 長基線數據包協議解析出深海機器人到每個信標的距離信息，判斷各信標數據的有效 性，根據每個有效信標的絕對位置信息和深海機器人到信標的距離信息，建立三元二次方 程組：
上述方程組中（xA，yA，zA)，（xB，y B，zB)，（Xc，yc，zc)分別為各有效信標的絕對位置坐標， 14、1^1。分別為深海機器人到各個信標的距離，解此方程組得（^，2。）為深海機器人的絕 對位置坐標，得到機器人絕對位置坐標的兩組解，根據當前時刻深度計採集的深度信息判 定兩組解中的有效解，作為觀測矢量中的東向位置觀測值和北向位置觀測值。
5. 根據權利要求1所述的基於AUKF的深海機器人長基線組合導航方法，其特徵在於： 無色卡爾曼主濾波器濾波估計的實現過程為： 對上一時刻隨機向量的被估計量均值和方差經非線性無色變換後，得到離散Sigma 點，經主濾波器的狀態方程計算更新Sigma點，然後計算出更新Sigma點的狀態均值和方 差，對狀態均值和方差計算得到一步估計的狀態均值和方差；對一步估計的狀態均值和方 差再經無色變換和主濾波器的測量方程計算更新，得到深海機器人可觀測狀態的均值、方 差及協方差的估計值；讀取傳感器採集觀測信息，並與狀態均值、方差、觀測狀態的估計值 和增益矩陣計算估計，得到系統均值、方差的估算值，並得到新息；根據主濾波器的新息經 計算得到測量值作為輔助濾波器的觀測信號。
6. 根據權利要求1所述的基於AUKF的深海機器人長基線組合導航方法，其特徵在於： 輔助濾波器對系統過程噪聲進行濾波估計的實現過程如下： 上一時刻估計得到輔助濾波器的系統噪聲方差矩陣的對角線元素和方差，用輔助濾波 器的狀態方程對對角線元素和方差計算更新後得到對角線元素和方差的一步估計值；用輔 助濾波器的測量方程根據主濾波器的一步估計的方差、狀態方差和增益矩陣計算更新後得 到估計的觀測值，將得到的一步估計值、觀測值和主濾波器的測量值計算估計得到系統過 程噪聲方差陣的對角線元素的估算值和方差，當有新觀測信息時，繼續下一控制周期主濾 波器的實現步驟。
7. 根據權利要求6所述的基於AUKF的深海機器人長基線組合導航方法，其特徵在於： 所述輔助濾波器的狀態方程建立如下： ---
其中講%為輔助濾波器中的系統噪聲，％為系統噪聲方差矩陣角線元素。
8. 根據權利要求6所述的基於AUKF的深海機器人長基線組合導航方法，其特徵在於： 所述輔助濾波器的測量方程建立如下：
其中，vdiag(_)表示一個向量，其分量為矩陣（?）的主對角線元素，系統噪聲方差矩 陣角線元素 A作為系統的觀測信號，Pkh為主濾波器狀態矢量的一步估計的方差，Pk為主 濾波器狀態矢量的方差，Kk為主濾波器的增益矩陣，&/,為估計的觀測值。
9. 根據權利要求5所述的基於AUKF的深海機器人長基線組合導航方法，其特徵在於： 所述主濾波器的測量方程根據長基線定位數據的有效時刻進行切換： 當長基線聲學定位裝置提供的觀測值有效時，測量方程定義為： Yk = xk+Nn 其中，y= [uy，vy，wy，ry, €y, ny, 4y, ￥y]T為觀測矢量，其中115\￥5\'\￥ 5\；^分別是前向速 度、側向速度、垂向速度和轉艏角速度的觀測量，€y、ny、4y、v y是北向位置、東向位置、深 度和艏向角的觀測量，Nn為觀測噪聲； 當長基線聲學定位裝置提供的觀測值無效時，測量方程定義為：
10. 根據權利要求9所述的基於AUKF的深海機器人長基線組合導航方法，其特徵在於： 所述有效時刻為長基線數據更新並且不是野值的時刻。
【文檔編號】G01C21/00GK104280026SQ201310284939
【公開日】2015年1月14日 申請日期:2013年7月8日 優先權日:2013年7月8日
【發明者】劉開周, 李靜, 郭威, 祝普強, 王曉輝 申請人:中國科學院瀋陽自動化研究所