水下機器人慣性導航方法及系統(tǒng)的制作方法
【技術領域】
[0001] 本發(fā)明涉及多功能水下機器人導航技術，尤其涉及一種水下機器人慣性導航方法 及系統(tǒng)。
【背景技術】
[0002] 水下機器人是開發(fā)海洋的手段之一，多數水下機器人只有浮游能力或爬行能 力，能夠實現(xiàn)浮游和爬壁的兩種模式的水下機器人較為少見。模態(tài)切換水下機器人 (Model-ConvertedROV，MC-R0V)，是一種新穎的模塊化、多功能、帶纜遙控水下機器人，可以 在浮游和爬行模態(tài)之間自由切換，被廣泛應用于水下觀察、浮游勘察、海底勘探、水下結構 檢修、海底管線檢測、爬行清污以及水下安裝等作業(yè)，已經成為海洋工程水下結構安全檢測 及維護的關鍵裝備。精確的導航能力是水下機器人進行有效作業(yè)和安全回收的關鍵，為水 下結構物檢測、清污提供必要的技術支撐。水下機器人導航系統(tǒng)必須提供遠距離及長時間 范圍內的精確定位、速度和姿態(tài)信息。但是由于受到水下環(huán)境的復雜性、機器人自身體積、 重量、能源以及隱蔽性等因素的影響，實現(xiàn)高精度的水下導航仍然是一項艱巨的任務。慣性 導航系統(tǒng)是水下導航最主要的導航方式，慣性導航系統(tǒng)是完全依靠自身設備進行導航的一 種無源系統(tǒng)，依據牛頓慣性原理，利用陀螺儀、加速度計這些慣性傳感器敏感元件測量物體 相對慣性空間的線運動和角運動參數，在給定的初始條件下，通過積分輸出載體的姿態(tài)參 數和導航參數，由于其與外界不發(fā)生任何聯(lián)系，不受環(huán)境的干擾影響，從而能夠在相對封閉 空間內進行較高精度的導航，具有隱秘性好的優(yōu)點。實際上，慣性導航也可以理解為一種推 算導航方法。由于慣性導航是通過對加速度二次積分而得到的。而陀螺儀會隨時間不斷漂 移，這些都會導致慣性導航會存在累積誤差，在長時間導航過程中，慣性導航必須要經過校 正處理。
[0003] 目前，捷聯(lián)式慣性導航技術已經成為慣性導航發(fā)展的主流方向。捷聯(lián)式慣性導航 系統(tǒng)（Strap-down Inertial Navigation System，簡寫SINS)是將加速度計和陀螺儀直 接安裝在載體上，在計算機中實時計算姿態(tài)矩陣，即計算出載體坐標系與導航坐標系之間 的關系，從而把載體坐標系的加速度計信息轉換為導航坐標系下的信息，然后進行導航計 算，具有可靠性高、功能強、重量輕、成本低、精度高以及使用靈活等優(yōu)點。陀螺儀的漂移誤 差和加速度計的零值偏值是影響系統(tǒng)精度的最直接的和最重要的因素，因此如何改善慣 性傳感器的性能，提高慣性組件的測量精度，特別是陀螺儀的測量精度，一直是慣性導航 領域研究的重點。中國專利申請?zhí)枮?01310544880. 4的專利文獻公開了一種用于標定 AUV自主導航傳感器參數的裝置和方法，把GPS融入到該系統(tǒng)中作為補償，實時為水下機 器人提供速度信息、姿態(tài)信息和位置信息，該裝置結構復雜，成本高昂。中國專利申請?zhí)枮?201410413791. 0的專利文獻公開了一種水下結構檢測機器人實時導航系統(tǒng)及方法，把磁羅 盤融入到該系統(tǒng)中作為補償，與陀螺儀和加速度計的頻域互補，以校正角速度矢量，算法復 雜，成本較高。
[0004] 姿態(tài)更新算法是捷聯(lián)慣導系統(tǒng)算法的核心，有限轉動的不可交換性是求取姿態(tài)方 程數值解算過程中的一個主要誤差源。1971年，Bortz提出的等效轉動矢量微分方程，有效 的解決了不可交換性誤差的問題。如果利用當前以及前兩次迭代周期內的陀螺儀輸出角增 量并引入航姿算法，能夠明顯提高姿態(tài)算法的精度。在捷聯(lián)慣性系統(tǒng)中，采用畢卡法求解四 元數微分方程時使用了陀螺儀的角增量輸出。角增量雖然微小，但不可視作無窮小，而剛體 做有限轉動時，剛體的空間角位置與旋轉次序有關，即存在不可交換性誤差。等效旋轉矢量 法在利用角增量計算等效旋轉矢量時，對這種不可交換誤差做了適當補償，在姿態(tài)更新周 期內包含的角增量子樣越多，補償就越精確。
[0005] 2009, Johann Borenstein提出了 HDR(啟發(fā)式漂移消減法）算法。最先將其用在 了車載導航的陀螺儀上面。算法的思想是首先判定載體是否嚴格的沿直線運動，若是則說 明陀螺儀的輸出為零，而實際情況下陀螺儀輸出并不為零，則把該值視為隨機漂移對其進 行補償。
[0006] 卡爾曼濾波（Kalman filtering) -種利用線性系統(tǒng)狀態(tài)方程，通過系統(tǒng)輸入輸出 觀測數據，對系統(tǒng)狀態(tài)進行最優(yōu)估計的算法。由于觀測數據中包括系統(tǒng)中的噪聲和干擾的 影響，所以最優(yōu)估計也可看作是濾波過程。

【發(fā)明內容】

[0007] 本發(fā)明的目的在于提供一種水下機器人導航方法及系統(tǒng)，無需借助磁羅盤等慣性 組件的補償作用，就能夠有效補償陀螺儀的漂移誤差，實現(xiàn)高精度導航，降低導航成本。
[0008] 為解決上述問題，本發(fā)明水下機器人慣性導航系統(tǒng)采用的技術方案是：包括由加 速度計和陀螺儀組成的慣性傳感器，還具有誤差補償器和主控制器，誤差補償器由矩陣奇 異值分解模塊和HDR模塊組成，主控制器由姿態(tài)矩陣計算模塊、卡爾曼濾波模塊、速度位置 計算模塊、姿態(tài)計算模塊及控制模塊組成；加速度計測量水下機器人的線加速度并輸出，加 速度計的輸出分別連接速度位置計算模塊和卡爾曼濾波模塊的輸入；陀螺儀測量水下機器 人的角速度并輸出，陀螺儀的輸出分別連接誤差補償器和卡爾曼濾波模塊的輸入；矩陣奇 異值分解模塊的輸出連接姿態(tài)矩陣計算模塊和姿態(tài)計算模塊的輸入，卡爾曼濾波模塊是姿 態(tài)矩陣計算模塊的姿態(tài)矩陣修正模塊；姿態(tài)矩陣計算模塊的輸出分別連接卡爾曼濾波模塊 和姿態(tài)計算模塊的輸入，速度位置計算模塊和姿態(tài)計算模塊的輸出連接控制模塊的輸入， 控制模塊的輸出連接于水下機器人，控制水下機器人的速度、位置和姿態(tài)角；水下機器人上 裝有旋轉調制機構，旋轉調制機構連接于陀螺儀，帶動陀螺儀進行包含兩個相反方向的周 期性運動，旋轉調制機構1的旋轉角度信息傳輸到姿態(tài)計算模塊。
[0009] 本發(fā)明水下機器人慣性導航方法采用的技術方案是：包括以下步驟：
[0010] A :陀螺儀輸出的角速度ωΗ傳輸到誤差補償器和卡爾曼濾波模塊，誤差補償器中 的矩陣奇異值分解模塊先計算旋轉調制機構的合力H，再對合力H進行矩陣奇異值分解，最 后計算安裝誤差矩陣^^ ;
[0011] B、HDR模塊對陀螺儀輸出的角速度CO11進行零值偏移誤差和漂移誤差補償，得到 補償后的角速度W hti;
[0012] C、加速度計輸出的線加速度值a i傳輸給速度位置計算模塊，速度位置計算模塊 根據加速度值a i計算出水下機器人的位置和速度；
[0013] D、將補償后的角速度值c〇hd"傳輸到姿態(tài)矩陣計算模塊，姿態(tài)矩陣計算模塊計算 陀螺儀的旋轉矢量Φ，通過旋轉矢量Φ計算機器人的姿態(tài)四元數q :
[0014] E、卡爾曼濾波模塊根據輸入值采用卡爾曼濾波算法修正，姿態(tài)矩陣計算模塊通過 卡爾曼濾波修正后的四元數向量Xk計算出水下機器人當前時刻的姿態(tài)矩陣;
[0015] F、姿態(tài)矩陣計算模塊將求得的姿態(tài)矩陣Ct傳輸到姿態(tài)計算模塊34,姿態(tài)計算模塊 求取當前時刻真實姿態(tài)信息，將該姿態(tài)信息傳遞給控制模塊控制水下機器人的運動。
[0016] 與現(xiàn)有技術相比，本發(fā)明的技術方案具有以下優(yōu)勢：
[0017] 1、本發(fā)明采用陀螺儀和加速度計這兩種傳感器來構成慣性傳感器，以實現(xiàn)水下機 器人的慣性導航，不僅可以大大減小慣性傳感器的體積，同時還具有成本低和安裝便利等 優(yōu)點。
[0018] 2、本發(fā)明中的慣性傳感器被旋轉調制機構帶動而進行包含兩個相反方向的周期 性翻轉運動，利用兩個相反方向翻轉的相互抵消作用，直接對低精度、高漂移的傳感器進行 有效自補償。
[0019] 3、本發(fā)明根據慣性傳感器與旋轉調制機構的安裝特點，提出矩陣奇異值分解法， 采用矩陣奇異值分解算法補償安裝誤差，同時采用HDR算法設計閉環(huán)網絡，增加雙低通濾 波器去除由于陀螺儀搖擺產生的