CN109813307A - 一种无人船多传感器数据融合的导航系统及其设计方法 - Google Patents

Abstract

本发明公开了一种无人船多传感器数据融合的导航系统及其设计方法，所述的导航系统包括全球定位系统数据采集单元、惯性导航系统数据采集单元、数据预处理单元、捷联解算单元、数据融合单元和输出单元。本发明采用基于非线性观测器的方法进行无人船的位置和速度状态估计，不仅简化了参数的调节过程，而且更加明确了所设计数据融合方法的收敛特性。本发明所提出的导航系统的数据融合方法不需要速度传感器的测量的速度信息，仅通过对位置和加速度信息的融合就能估计出无人船的位置和速度信息，不仅减少了传感器的使用，降低了成本，而且减少了传感器噪声干扰的多样性。本发明能够将带有测量噪声的测量信息有效整合，获得实际的位置和速度信息。

Description

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

技术领域

本发明涉及无人船导航系统设计领域，尤其涉及一种针对无人船的多传感器数据融合的导航系统及其设计方法。

背景技术

水面无人船导航是研究水面无人船技术的重要内容，在水面无人船的应用中发挥着不可替代的作用。无人船导航就是无人船在外界环境中移动时，利用相关的传感器(雷达、惯性系统、全球卫星导航系统、磁罗盘、视觉传感器、计程仪等)获取无人船的位置、速度和姿态信息，然后确定移动的方向和速度。无人船的姿态信息获取是实现无人船的基本运动以及智能运动的关键，是保证海洋作业、海洋任务等安全进行的必要保证。

导航系统可以分为自主式导航和非自主式导航两种，自主式导航主要指惯性导航，不需要外界的参考信息，而非自主式导航正好相反，需要外界的参考信息，主要包括天文导航和卫星导航等。惯性导航的原理是通过牛顿力学定律对惯性测量器件(加速度计和陀螺仪)进行导航解算，得到载体的姿态。优点是精度高，不易受环境干扰，缺点是误差会随时间积累，不能长期有效导航；非自主式导航应用最多的是全球导航卫星系统，它的基本原理是通过轨道卫星提供的载波、伪距等信息得到载体姿态。优点是不存在积累误差，缺点是易受环境干扰。所以，单一的导航系统已经不能满足人们的需求，组合导航技术应运而生。

现阶段，国内外对于无人船姿态获取的研究主要集中于组合导航技术。组合导航技术就是利用多个传感器的信息应用数据融合算法得到更高精度的载体姿态信息。然而，现有的技术存在以下不足：第一，多种传感器的使用会使得噪声更加多样和复杂，目前通常使用的方法并没有考虑传感器测量数据的噪声干扰问题，不能有效克服多种传感器在不同环境下对系统造成的干扰，将会导致导航数据不够精确，可能造成严重后果；第二，现有无人船导航系统姿态获取的数据融合算法主要集中在与卡尔曼滤波器设计相关的方面，存在设计的导航系统计算量大、参数调节困难和收敛性质不明确等缺点。因此，尽可能减少传感器的使用，并且考虑测量噪声对数据融合的影响，通过基于非线性观测器的多传感器数据融合技术实现无人船导航系统能够获得实时的姿态信息是具有重要的应用价值。

发明内容

为解决上述背景技术中存在的不足，本发明要提出一种不仅可以更好的提高数据的实时性和准确性，而且能够简化参数的调节过程、减小计算量、降低使用成本、明确导航系统收敛性的无人船多传感器数据融合的导航系统及其设计方法。

为了实现上述目的，本发明的技术方案如下：一种无人船多传感器数据融合的导航系统，包括全球定位系统数据采集单元、惯性导航系统数据采集单元、数据预处理单元、捷联解算单元、数据融合单元和输出单元，所述的数据融合单元包括比较单元、比例单元1、加法单元1、比例单元2、旋转单元1、旋转单元2和加法单元2；

所述的全球定位系统数据采集单元采集无人船在地球坐标系下的经度和纬度信息；所述的惯性导航系统数据采集单元采集无人船在载体坐标系下的三轴加速度信息和三轴磁场信息；所述的全球定位系统数据采集单元的输出端和数据预处理单元的输入端相连；所述的惯性导航系统数据采集单元的输出端和捷联解算单元的输入端相连；所述的数据预处理单元的输出端与比较单元的输入端相连；所述的捷联解算单元的输出端分别与数据预处理单元的输入端、旋转单元1的输入端、旋转单元2的输入端和加法单元2的输入端相连；所述的旋转单元2的输入端还与加法单元1的输入端相连；所述的比较单元的输出端分别与与比例单元1的输入端和比例单元2的输入端相连；所述的比例单元1的输出端与加法单元1的输入端相连；所述加法单元1的输出端分别与比较单元的输入端和输出单元的输入端相连；所述的比例单元2的输出端与旋转单元1的输入端相连；所述的旋转单元1的输出端与加法单元2的输入端相连；所述的加法单元2的输出端分别与与旋转单元2的输入端和输出单元的输入端相连。

一种无人船多传感器数据融合的导航系统的设计方法，所述的无人船导航系统的运动数学模型基于地球坐标系和载体坐标系建立，描述无人船位置、速度和姿态的运动学方程表示为：

其中，ρⁿ＝[N,E,ψ]^T∈R³代表无人船位置信息，其中N代表经度信息，E代表纬度信息，ψ代表艏摇角信息；代表无人船速度信息，其中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对位置误差信息缩放，如下式：

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

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

相比现有技术，本发明的有益效果是：

第一，与现有导航系统的数据融合所采用的基于卡尔曼滤波或者扩展卡尔曼滤波等方法估计载体的姿态信息相比，本发明所设计的无人船导航系统的数据融合方法采用基于非线性观测器的方法进行无人船的位置和速度状态估计，不仅简化了参数的调节过程，而且更加明确了所设计数据融合方法的收敛特性。

第二，与现有一般设计的导航系统的数据融合方法采用非线性观测器对测量的位置、速度信息和加速度信息进行融合估计出载体的速度和位置信息的方法相比，本发明所提出的导航系统的数据融合方法不需要速度传感器的测量的速度信息，仅通过对位置和加速度信息的融合就能估计出无人船的位置和速度信息，不仅减少了传感器的使用，降低了成本，而且减少了传感器噪声干扰的多样性。

第三，与现有一般设计的导航系统的数据融合方法采用非线性观测器对测量信息进行融合估计载体的姿态相比，本发明提出的基于非线性观测器的数据融合方法考虑了传感器的测量噪声问题，能够将带有测量噪声的测量信息有效整合，获得实际的位置和速度信息。

第四，本发明设计的导航系统中的数据融合方法只需要使用全球定位系统输出的位置信息和惯性导航系统输出的加速度信息，就能估计出无人船的位置和速度信息，所以可以用于全驱动无人船横荡速度的估计，故所设计的导航系统可用于欠驱动无人船和全驱动无人船两种情况。

第五，综上所述，本发明能够利用无人船配备的多个传感器，得到地球坐标系下带有测量噪声的经度、纬度、艏摇角信息和船体坐标系下带有测量噪声的加速度信息，并通过数据融合最优化的利用得到的无人船实时的位置、加速度和艏摇角的状态信息，不仅可以更好的提高数据的实时性和准确性，而且不需要使用速度传感器就能够得到速度信息，减小了使用成本，使该方法更适合于成本低、小型化的无人船系统。

附图说明

本发明共有附图7张，其中：

图1是无人船多传感器数据融合的导航系统设计方法结构示意图。

图2是无人船带测量噪声的北向位置与估计结果仿真图。

图3是无人船带测量噪声的东向位置与估计结果仿真图。

图4是无人船带测量噪声的艏摇角与估计结果仿真图。

图5是无人船带测量噪声的纵荡速度与估计结果仿真图。

图6是无人船带测量噪声的横荡速度与估计结果仿真图。

图7是无人船带测量噪声的艏摇角速度与估计结果仿真图。

具体实施方式

下面针对一个具体的无人船进行位置和速度状态信息的估计，并以此为例对本发明的具体实施进行进一步说明。图1为本发明的结构示意图，其中无人船模型如下：

其中：

设置控制力矩控制无人船做直线运动，并采用基于非线性观测器的数据融合方法设计的导航系统估计无人船的位置和速度状态信息，设计的导航系统结构满足式(2)-(9)，根据赫尔维茨稳定性判据，所设计导航系统的比例参数选择如下：

K_pp＝diag{3.51,3.51,3.51}，K_vp＝diag{0.67,0.67,0.67}

仿真结果如图2-7所示。图2-4分别表示无人船的北向位置、东向位置和艏摇角信息，图中使用实线、虚线和点划线分别代表无人船不同位置状态信息的实际值、带测量噪声的实际值和导航系统的估计值，可以看出采用基于非线性观测器的数据融合方法设计的导航系统得到的无人船的北向、东向和艏摇角估计值能够对带有测量噪声的实际北向、东向位置值和艏摇角起到滤波的效果，得到与真实值更为接近的估计值；图5-7分别表示无人船纵荡速度、横荡速度和艏摇角速度的速度信息，图中实线和虚线分别代表无人船不同速度状态信息的实际值和导航系统估计值的比较，可以看出采用基于非线性观测器的数据融合方法设计的导航系统能够估计出无人船纵荡速度、横荡速度和艏摇角速度的值；由仿真结果可知，设计的采用基于非线性观测器的数据融合方法设计的导航系统能够从带有测量噪声的无人船位置信息和加速度状态信息中有效估计出无人船的位置和速度信息，满足设计目标。

综上，本发明涉及的无人船多传感器数据融合的导航系统设计方法，应用于对无人船的状态信息估计，在满足对无人船的运动控制要求方面是一种可供同行借鉴参考的标准。根据该标准可以使采用基于非线性观测器的数据融合方法设计的导航系统不仅适用于全驱动无人船，也适用于欠驱动无人船。

本发明不局限于本实施例，任何在本发明披露的技术范围内的等同构思或者改变，均列为本发明的保护范围。

Claims (2)

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

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