[發明專利]一種無人船多傳感器數據融合的導航系統及其設計方法

在審

• 申請號: 201910142668.2
• 申請日: 2019-02-26
• 公開（公告）號: CN109813307A
• 公開（公告）日: 2019-05-28
• 發明（設計）人: 彭周華;張斌;王丹;劉陸;王浩亮;古楠
• 申請（專利權）人: 大連海事大學
• 主分類號: G01C21/16
• 分類號: G01C21/16;G01C21/20
• 代理公司: 大連東方專利代理有限責任公司 21212
• 代理人: 李洪福

1.一種無人船多傳感器數據融合的導航系統，其特征在于：包括全球定位系統數據采集單元、慣性導航系統數據采集單元、數據預處理單元、捷聯解算單元、數據融合單元和輸出單元，所述的數據融合單元包括比較單元、比例單元1、加法單元1、比例單元2、旋轉單元1、旋轉單元2和加法單元2；

所述的全球定位系統數據采集單元采集無人船在地球坐標系下的經度和緯度信息；所述的慣性導航系統數據采集單元采集無人船在載體坐標系下的三軸加速度信息和三軸磁場信息；所述的全球定位系統數據采集單元的輸出端和數據預處理單元的輸入端相連；所述的慣性導航系統數據采集單元的輸出端和捷聯解算單元的輸入端相連；所述的數據預處理單元的輸出端與比較單元的輸入端相連；所述的捷聯解算單元的輸出端分別與數據預處理單元的輸入端、旋轉單元1的輸入端、旋轉單元2的輸入端和加法單元2的輸入端相連；所述的旋轉單元2的輸入端還與加法單元1的輸入端相連；所述的比較單元的輸出端分別與與比例單元1的輸入端和比例單元2的輸入端相連；所述的比例單元1的輸出端與加法單元1的輸入端相連；所述加法單元1的輸出端分別與比較單元的輸入端和輸出單元的輸入端相連；所述的比例單元2的輸出端與旋轉單元1的輸入端相連；所述的旋轉單元1的輸出端與加法單元2的輸入端相連；所述的加法單元2的輸出端分別與與旋轉單元2的輸入端和輸出單元的輸入端相連。

2.一種無人船多傳感器數據融合的導航系統的設計方法，其特征在于：所述的無人船導航系統的運動數學模型基于地球坐標系和載體坐標系建立，描述無人船位置、速度和姿態的運動學方程表示為：

其中，ρⁿ＝[N,E,ψ]^T∈R³代表無人船位置信息，其中N代表經度信息，E代表緯度信息，ψ代表艏搖角信息；θ^b＝[u,v,r]^T∈R³代表無人船速度信息，其中u代表縱蕩速度信息，v代表橫蕩速度信息，r代表艏搖角速度信息；a^b＝[a_x,a_y,a_z]^T∈R³代表無人船加速度信息，a_x代表x軸方向加速度信息，a_y代表y軸方向加速度信息，a_z代表z軸方向加速度信息；T代表無人船地球坐標系和載體坐標系之間的旋轉矩陣；

所述的設計方法包括以下步驟：

A、全球定位系統信號采集單元的設計

全球定位系統信號采集單元提供地球坐標系下的帶有測量噪聲的經度信息和緯度信息用位置信息表示為：

所述位置數據主要依據NEMA-0183協議提取全球定位系統數據包中的經度和緯度信息，全球定位系統信號采集單元的輸出作為數據預處理單元的輸入，進行數據的預處理再進行數據融合；

B、慣性導航系統信號采集單元的設計

慣性導航系統信號采集單元包括加速度計和磁力計，為無人船多傳感器數據融合的導航系統提供在無人船載體坐標系下帶有測量噪聲的三軸加速度計信息和三軸磁場信息

其中：

表示在無人船載體坐標系三個方向下加速度計輸出的帶有測量噪聲的加速度信息，a_xε代表載體坐標系下北向的帶有測量噪聲的加速度信息，a_yε代表載體坐標系下東向的帶有測量噪聲的加速度信息，a_zε代表載體坐標系下地向的帶有測量噪聲的加速度信息；

表示在無人船載體坐標系三個方向下從磁力計測量的帶有測量噪聲的磁場信息，代表載體坐標系下的北向磁場信息，代表載體坐標系下的東向磁場信息，代表載體坐標系下的地向磁場信息；慣性導航系統信號采集單元的輸出作為捷聯解算單元的輸入，將原始的傳感器數據解算為模型構建所需的狀態信息；

C、數據預處理單元的設計

數據預處理單元的輸入是全球定位系統信號采集單元的輸出和捷聯解算單元的輸出，實現對地球坐標系下的位置信息轉換為平面坐標系下的位置信息，所述的平面坐標系采用北東地平面坐標系，并通過狀態重組輸出表示位置的狀態信息北東地坐標系下的位置信息通過N_ε和E_ε表示，其中N_ε代表無人船的位置處于設置的坐標原點的北向位置信息，單位為米；E_ε代表無人船的位置處于設置的坐標原點的東向位置信息，單位為米；

將帶有測量噪聲的北向位置信息N_ε、帶有測量噪聲的東向位置信息E_ε和帶有測量噪聲的捷聯解算單元輸出的艏搖角信息ψ進行狀態組合，得到數據預處理單元輸出的無人船帶有測量噪聲的位置狀態信息表示為：

D、捷聯解算單元的設計

捷聯解算單元的輸入信號是慣性導航系統信號采集單元的輸出信號，實現對慣性導航系統輸出的原始數據的解算，將原始磁力計的信息轉換為無人船的航向信息ψ，并得到無人船從載體坐標系轉換到北東地坐標系下的旋轉矩陣

由于小型無人船的橫搖φ和縱搖θ相對來說比較小，所以忽略地球的轉動；因此，無人船在水面的運動通過使用縱蕩、橫蕩和艏搖方向的運動來描述；由于橫搖和縱搖角度太小，兩個方向的運動忽略后，從載體坐標系轉換到北東地坐標系的姿態矩陣只與艏搖ψ有關；最終的無人船姿態矩陣表示為：

其中，T₁₁＝T₂₂＝cos(ψ)，-T₁₂＝T₂₁＝sin(ψ)，T₁₃＝T₂₃＝T₃₁＝T₃₂＝0，T₃₃＝1；

E、數據融合單元的設計

數據融合單元的輸入包括三部分；第一部分是數據預處理單元輸出的含測量噪聲的位置信息，第二部分是捷聯解算單元輸出的含測量噪聲的加速度信息，第三部分是捷聯解算單元輸出的從載體坐標系轉換到北東地坐標系的姿態矩陣信息；具體的融合方法包括以下步驟：

E1、比較單元設計

比較單元的輸入信號為數據預處理單元的輸出信號和加法單元1輸出的對位置狀態信息的估計信號通過下式：

得到比較單元的輸出信號ρ₀和θ₀；

E2、比例單元2設計

比例單元2的輸入信號為比較單元輸出的無人船位置誤差信息θ₀，通過輸入參數-K_vp對位置誤差信息縮放，如下式：

θ₁＝-K_vpθ₀ (4)

得到比例單元2的輸出信號θ₁；

E3、旋轉單元1設計

旋轉單元1的輸入信號為比例單元2的輸出信號和捷聯解算單元的輸出信號通過從載體坐標系轉換到北東地坐標系的旋轉矩陣對比例單元2輸出的無人船位置誤差信息θ₁進行旋轉變換，如下式：

得到估計無人船速度的部分信息；

E4、加法單元2設計

加法單元2的輸入信號為旋轉單元1的輸出信號和捷聯解算單元的輸出信號通過在加法單元2中求和最終得到無人船速度估計單元的輸出信號如下式：

無人船的速度估計信號作為旋轉單元2的輸入，并作為輸出信號輸出估計的無人船速度信息；

E5、旋轉單元2設計

旋轉單元2的輸入信號來自加法單元2輸出的速度估計信號和捷聯解算單元的輸出信號通過從載體坐標系轉換到北東地坐標系的旋轉矩陣對輸出的速度估計信號進行旋轉，如下式：

得到加法單元1的部分輸入信號ρ₂，即無人船位置估計的第一部分輸入；

E6、比例單元1設計

比例單元1的輸入信號為比較單元輸出的無人船位置誤差信息ρ₀，通過輸入參數-K_pp對位置誤差信息縮放，如下式：

ρ₁＝-K_ppρ₀ (8)

得到比例單元1的輸出信號ρ₁，即無人船位置估計的第二部分輸入；

E7、加法單元1設計

加法單元1的輸入信號為比例單元1的輸出信號ρ₁和旋轉單元2的輸出信號ρ₂，通過在加法單元1中求和最終得到無人船位置估計的輸出信號如下式：

無人船的位置估計信號作為比較單元的一部分輸入，并作為輸出信號輸出估計的無人船位置信息；

F、輸出單元的設計

輸出單元的輸入信號為數據融合單元的輸出信號，分別為數據融合單元估計得到的位置估計值和速度估計值。