[发明专利]一种无人船多传感器数据融合的导航系统及其设计方法

权利要求书

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、输出单元的设计

输出单元的输入信号为数据融合单元的输出信号，分别为数据融合单元估计得到的位置估计值和速度估计值。