CN109443356A - 一种含测量噪声的无人船位置与速度估计结构及设计方法 - Google Patents

Abstract

本发明公开了一种无人船位置和速度状态信息估计结构及设计方法，所述结构包括加速度计单元、陀螺仪单元、磁力计单元、全球定位系统信号采集单元、坐标系转换单元、状态重组单元、比较单元、惯性导航系统数据解算单元和非线性观测器导航滤波算法单元。本发明利用带有测量噪声的无人船位置和速度信息，通过基于非线性观测器的方法进行多传感器信息融合处理，估计出实际的无人船位置和速度信息，既使得所设计的非线性观测器收敛特性更加明确，又方便了参数的调节过程，降低了计算量。本发明考虑了传感器的测量噪声问题，可从带有测量噪声的状态信息中获得实际位置和速度信息，并形成更连续和稳定的导航数据，也使航向角度的估计更加精确。

Description

一种含测量噪声的无人船位置与速度估计结构及设计方法

技术领域

本发明涉及一种含测量噪声的水面无人船位置与速度估计方法，尤其涉及一种针对欠驱动无人船的非线性观测器估计无人船位置和速度状态信息的设计方法，属于水面无人船多传感器信息融合技术领域。

背景技术

水面无人船是无人海洋航行器的一种，具有体积小、成本低、灵活性强和隐蔽性高等优点，可搭载先进的导航控制系统、可靠通信系统、高精度传感器系统和武器系统等不同功能模块，完成海洋开发和海洋运输等任务，在军事和民用领域得到了广泛研究和应用。在无人船的研究内容方面，无人船结构研究、无人船导航制导研究和无人船控制系统研究是主要的三大方向，并得到了较为普遍的关注和研究。无人船导航制导研究主要目的是通过导航系统得到无人船实时的位置和速度状态信息、用于无人船控制算法以及编队算法的研究，无人船位置和速度状态信息估计不仅是无人船导航制导研究的重要内容，而且是实现无人船其它各项功能的关键。海上作业，海洋学研究任务和海上运输都需要获得准确的船舶运动数据来保证其安全和业务开展。通过无人船导航系统得到船舶的位置和速度实时状态信息对于无人船的应用显得尤为重要。因此，根据无人船导航系统设计一种无人船位置和速度实时状态信息估计方法是十分有必要的。

在无人船位置和速度状态信息估计的方法中，国内外已经取得一些研究结果，从结构和设计方面，现有的技术存在以下不足：第一，现有无人船位置和速度估计方法大多是通过卡尔曼滤波器设计，存在计算量大、调参困难和非线性系统收敛性质不明确等缺点；第二，现有其它很多方法都是将卡尔曼滤波器和智能控制算法相结合，具有在普通单片机不易实现的缺点。

随着对水面无人船运动控制和智能水平的要求越来越高，在现实应用中迫切的需要一种能够精确估计无人船位置和速度状态信息的估计方法。

发明内容

为解决现有技术存在的不足，本发明要提出一种含测量噪声的无人船位置与速度估计结构及设计方法，能够利用带测量噪声的无人船传感器信息，包括地球坐标系下的位置坐标和艏摇角、船体坐标系下的纵荡速度和加速度信息，通过非线性观测器估计方法进行数据融合得到无人船的实时位置和速度状态信息。

为了实现上述目的，本发明的技术方案如下：一种无人船位置和速度状态信息估计结构，包括加速度计单元、陀螺仪单元、磁力计单元、全球定位系统信号采集单元、坐标系转换单元、状态重组单元、比较单元、惯性导航系统数据解算单元和非线性观测器导航滤波算法单元，所述的加速度计单元测量无人船的三轴加速度信息；所述的陀螺仪单元测量无人船的加速度信息；所述的磁力计单元测量无人船的磁场信息；所述的加速度计单元、陀螺仪单元和磁力计单元的输出端均与惯性导航系统数据解算单元的输入端相连；所述的惯性导航系统数据解算单元的输出端与非线性观测器导航滤波算法单元的输入端相连；所述的全球定位系统信号采集单元测量无人船的位置和速度信息；全球定位系统信号采集单元的位置信息输出端与坐标系转换单元的输入端相连；所述的坐标系转换单元的输出端与状态重组单元的输入端相连；全球定位系统信号采集单元的速度信息输出端直接与状态重组单元的输入端相连；惯性导航系统数据解算单元的输出端还与状态重组单元的输入端相连；所述的状态重组单元的输出端与比较单元的输入端相连；所述的非线性观测器导航滤波算法单元接收惯性导航系统数据解算单元和比较单元的无人船的位置、速度、加速度和航向信息，并将输出的无人船位置和速度信息的估计值作为比较单元的输入；所述的比较单元输出的位置误差和速度误差输出端均与非线性观测器导航滤波算法单元的输入端相连；非线性观测器导航滤波算法单元的输出端最终输出无人船位置和速度信息的估计值；

所述的加速度计单元、陀螺仪单元、磁力计单元和全球定位系统信号采集单元组成微机械惯性传感器模块。

一种含测量噪声的无人船位置与速度估计结构的设计方法，所述的无人船的运动数学模型基于地球坐标系和载体坐标系建立。描述无人船位置、速度和姿态的运动学方程表示为：

其中，pⁿ＝[N,E,D]^T∈R³分别为无人船在北、东、地方向的位置信息；ν^b＝[u,v,w]^T∈R³分别为无人船纵荡速度、横荡速度和升沉速度信息；a^b＝[a_x,a_y,a_z]^T∈R³分别为无人船在载体坐标系三个方向的加速度信息；ω^b＝[p,q,r]^T∈R³分别为无人船在纵荡速度、横荡速度和升沉速度方向的角速度信息；R为无人船地球坐标系和载体坐标系之间的旋转矩阵。对于任意的三维向量x＝[x₁,x₂,x₃]^T，运算S(x)定义一个斜对称矩阵，如下式所示：

所述的方法包括以下步骤：

A、微机械惯性传感器模块的设计与配置

微机械惯性传感器模块的设计与配置是无人船状态信息的最根本来源。微机械惯性传感器模块包括全球定位系统信号采集单元、加速度计单元、陀螺仪单元和磁力计单元四个部分，分别测量得到无人船在地球坐标系下的原始经度信息、纬度信息、前向速度信息、三轴加速度信息、三轴角速度信息和三轴磁场信息。配置如下所示：

在地球坐标系下，从全球定位系统测量的带有测量噪声的位置信息P_δ表示为：

P_δ＝[x_δ,y_δ]^T；

式中，x_δ代表经度信息，y_δ代表纬度信息；

在无人船载体坐标系下，从全球定位系统测量的带有测量噪声的部分速度信息V_δ表示为：

V_δ＝u_δ；

式中，u_δ代表前向速度信息：

在无人船载体坐标系三个方向下，从加速度计测量的带有测量噪声的加速度信息表示为：

式中，a_xδ代表载体坐标系下北方向的加速度信息，a_yδ代表载体坐标系下东方向的加速度信息，a_zδ代表载体坐标系下地方向的加速度信息；

在无人船载体坐标系下，从陀螺仪测量的带有测量噪声的角速度信息表示为：

式中，p代表载体坐标系下北方向的加速度信息，q代表载体坐标系下东方向的加速度信息，r代表载体坐标系下地方向的加速度信息：；

在无人船载体坐标系下，从磁力计测量的带有测量噪声的磁场信息表示为：

式中，代表载体坐标系下北方向的加速度信息，代表载体坐标系下东方向的加速度信息，代表载体坐标系下地方向的加速度信息；

B、状态重组单元的设计

状态重组单元是将经过转换的微机械惯性传感器模块的信息作为状态变量，在考虑所设计无人船航行环境以及航行速度的属性条件下，将无人船的状态信息进行组合，表示无人船的姿态信息。

状态重组单元的输入分为三个部分：第一部分是全球定位系统信号采集单元输出的位置信息经过坐标系转换得到的地球坐标系下的北东地位置信息；第二部分是全球定位系统信号采集单元直接输出的载体坐标系下的无人船的速度信息；最后一部分是加速度计单元、陀螺仪单元和磁力计单元组成的微机械惯性传感器模块输出的原始信息经过数据解算后得到的航向和角速度信息。

其中，前两部分都是通过全球定位系统信号采集单元得到的原始信息，通过对全球定位系统接收到的位置信息进行解析，提取有效数据存入无人船导航系统的缓冲区。所述的全球定位系统通过NMEA-0183协议输出包含位置信息的不同格式帧，通过接收最小数据量的格式帧获取全球定位系统信息，信息格式如下所示：

$GPRMC,time,status,Lat,N,Lon,E,spd,cog,date,mv,mvE,mode,sum (3)

其中，Lat代表纬度信息、N代表北纬、Lon代表经度信息、E代表东经、spd代表速度信息。第一部分是无人船全球定位系统信号采集单元输出的地球坐标系下的原始经度x_δ和纬度y_δ位置信息：

P_δ＝[x_δ,y_δ]^T

通过坐标系转换单元转换到地球坐标系下的北东地初步导航位置信息：

P_δ＝[N_δ,E_δ]^T，

N_δ和E_δ分别代表无人船相对于设置的坐标原点北向和东向的位置，单位为m。第二部分是全球定位系统信号采集单元直接输出载体坐标系下的无人船的速度信息u_δ，实际上，全球定位系统信号采集单元得到的速度测量值u_δ是合速度但是无人船的横荡速度即侧向速度v相对于纵荡速度即前向速度u很小。因此，对于无人船忽略横荡速度，将合速度当作纵荡速度。最后一部分是通过微机械惯性传感器模块输出的数字量转换为无人船艏摇角度和艏摇角速度信息。

对于低速航行的小型无人船，由于横摇φ和纵摇θ比较小，忽略地球的转动。因此，使用纵荡、横荡和艏摇方向的运动描述无人船在水平面的运动，即忽略垂荡、横摇和纵摇方向的运动，令w＝p＝q＝0。由于横摇和纵摇方向运动的忽略，姿态只与艏摇有关，如式(4)所示，所以通过对上述第一部分得到的无人船北向和东向位置信息、第二部分得到的无人船速度信息和最后一部分得到的无人船艏摇角和艏摇角速度信息进行状态重组，构建出新的状态变量，η和υ分别表示无人船的状态，最终的无人船运动学方程如下：

其中：

为无人船载体坐标系和地球坐标系北东地之间的转移矩阵，υ＝[u,v,r]^T∈R³是选取的新的状态向量。代表地球坐标系北东地无人船位置信号，其中：N表示在地球坐标系北东地的北方向的位置坐标、单位为米，E表示在地球坐标系北东地东方向的位置坐标、单位为米，表示了地球坐标系北东地无人船的艏摇角、单位为度；υ＝[u,v,r]^T∈R³代表无人船载体坐标系下无人船的速度信号，其中u表示无人船载体坐标系下的纵荡速度，v表示无人船载体坐标下的横荡速度，单位为米每秒；r表示无人船载体坐标系下的艏摇角速度，单位为度每秒。

C、比较单元的设计

比较单元的输入信息是状态重组单元输出的带有测量噪声的无人船位置信息和速度信息非线性观测器输出的估计的无人船位置信息和速度信息由下式：

得到比较单元的输出信号和

D、非线性观测器的设计

非线性观测器采用导航滤波算法，其输入信号是比较单元输出的无人船状态信息和以及惯性导航系统数据解算单元输出的加速度信息：

这里选取带有测量噪声的位置信息、速度信息和加速度信息，设计非线性观测器进行导航滤波处理。通过无人船运动学方程(4)设计如下简化的非线性观测器，用于估计无人船位置和速度信息：

式中，表示非线性观测器估计的实际的无人船位置信息，表示非线性观测器估计的实际的无人船速度信息，K₁₁、K₁₂、K₂₁、K₂₂均为非线性观测器的设计增益参数。

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

第一，与现有一般基于卡尔曼滤波或者扩展卡尔曼滤波估计位置和速度信息的方法相比，本发明利用带有测量噪声的无人船位置和速度信息，通过基于非线性观测器的方法进行多传感器信息融合处理，估计出实际的无人船位置和速度信息，既使得所设计的非线性观测器收敛特性更加明确，又方便了参数的调节过程，降低了计算量。

第二，与现有设计的非线性观测器进行速度和位置信息估计的方法相比，本发明设计的非线性观测器考虑了传感器的测量噪声问题，能够在此情况下进行传感器信息融合估计无人船实际的位置和速度信息，可从带有测量噪声的状态信息中获得实际位置和速度信息，并形成更连续和稳定的导航数据，也使航向角度的估计更加精确。

第三，与现有针对采用人工智能算法与卡尔曼滤波结合估计位置和速度状态信息应用于微型智能化小型设备相比，本发明设计的位置和速度估计方法基于全球定位系统和惯性导航系统组合的欠驱动无人船导航系统，采用了非线性观测器作为多传感器信息融合方法，有效整合了多导航传感器信息融合的处理，对于应用在小型单片机等处理器，不仅减小了对单片机计算能力的依赖，更有利于软件的模块化编程，有利于微型智能化设备的应用。

第四，本发明的非线性观测器的设计方法中的无人船运动学方程(4)，由于使用微机械惯性传感器模块限制了速度的测量值只有一维，即纵荡速度，而横荡速度没有实际的测量值，所以该模型转化为欠驱动无人船，即：如果微机械惯性传感器模块提供了横荡速度的测量值，所设计的非线性观测器可用于对全驱动无人水面船进行横荡速度的估计，故设计的非线性观测器可用于欠驱动水面无人船和全驱动水面无人船两种情况。

附图说明

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

图1是含测量噪声的欠驱动无人船位置和速度估计结构示意图。

图2是欠驱动无人船带测量噪声的北方向位置与观测结果仿真图。

图3是欠驱动无人船带测量噪声的东方向位置与观测结果仿真图。

图4是欠驱动无人船带测量噪声的艏摇角与观测结果仿真图。

图5是欠驱动无人船带测量噪声的纵荡速度与观测结果仿真图。

图6是欠驱动无人船带测量噪声的艏摇角速度与观测结果仿真图。

具体实施方式

下面针对一个具体的欠驱动无人船进行位置和速度状态信息观测，并以此为例对本发明的具体实施进行进一步说明。图1为本发明的结构示意图，其中无人船模型被简化状态重组之后如下：

其中：

令无人船在控制力矩下做直线运动，无人船的运动速度为米每秒，无人船的初始位置设为选定的原点北方向-90米和东方向10米的位置，即在原点南方向90米和东方向10米的位置，采用非线性观测器估计无人船位置和速度状态信息系统的结构满足式(4)-(7)，所设计的非线性观测器的增益参数根据赫尔维茨稳定性判据选择如下：

K₁₁＝diag{3.894,3.894,3.894}，K₁₂＝{2.512,2.512,2.512}，K₂₁＝{0.801,0.801,0.801}，K₂₂＝{2.361,2.361,2.361}

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

综上，本发明涉及的含测量噪声的无人船位置和速度估计方法，应用于对水面无人船的位置和速度状态信息估计，在满足对水面无人船的运动控制要求的方面是一种可供同行借鉴参考的标准。根据该标准可以使所设计的非线性观测器不仅适用于全驱动无人船，也适用于欠驱动无人船。

Claims (2)

1.一种无人船位置和速度状态信息估计结构，其特征在于：包括加速度计单元、陀螺仪单元、磁力计单元、全球定位系统信号采集单元、坐标系转换单元、状态重组单元、比较单元、惯性导航系统数据解算单元和非线性观测器导航滤波算法单元，所述的加速度计单元测量无人船的三轴加速度信息；所述的陀螺仪单元测量无人船的加速度信息；所述的磁力计单元测量无人船的磁场信息；所述的加速度计单元、陀螺仪单元和磁力计单元的输出端均与惯性导航系统数据解算单元的输入端相连；所述的惯性导航系统数据解算单元的输出端与非线性观测器导航滤波算法单元的输入端相连；所述的全球定位系统信号采集单元测量无人船的位置和速度信息；全球定位系统信号采集单元的位置信息输出端与坐标系转换单元的输入端相连；所述的坐标系转换单元的输出端与状态重组单元的输入端相连；全球定位系统信号采集单元的速度信息输出端直接与状态重组单元的输入端相连；惯性导航系统数据解算单元的输出端还与状态重组单元的输入端相连；所述的状态重组单元的输出端与比较单元的输入端相连；所述的非线性观测器导航滤波算法单元接收惯性导航系统数据解算单元和比较单元的无人船的位置、速度、加速度和航向信息，并将输出的无人船位置和速度信息的估计值作为比较单元的输入；所述的比较单元输出的位置误差和速度误差输出端均与非线性观测器导航滤波算法单元的输入端相连；非线性观测器导航滤波算法单元的输出端最终输出无人船位置和速度信息的估计值；

所述的加速度计单元、陀螺仪单元、磁力计单元和全球定位系统信号采集单元组成微机械惯性传感器模块。

2.一种含测量噪声的无人船位置与速度估计结构的设计方法，其特征在于：所述的无人船的运动数学模型基于地球坐标系和载体坐标系建立；描述无人船位置、速度和姿态的运动学方程表示为：

其中，pⁿ＝[N,E,D]^T∈R³分别为无人船在北、东、地方向的位置信息；ν^b＝[u,v,w]^T∈R³分别为无人船纵荡速度、横荡速度和升沉速度信息；a^b＝[a_x,a_y,a_z]^T∈R³分别为无人船在载体坐标系三个方向的加速度信息；ω^b＝[p,q,r]^T∈R³分别为无人船在纵荡速度、横荡速度和升沉速度方向的角速度信息；R为无人船地球坐标系和载体坐标系之间的旋转矩阵；对于任意的三维向量x＝[x₁,x₂,x₃]^T，运算S(x)定义一个斜对称矩阵，如下式所示：

所述的方法包括以下步骤：

A、微机械惯性传感器模块的设计与配置

微机械惯性传感器模块的设计与配置是无人船状态信息的最根本来源；微机械惯性传感器模块包括全球定位系统信号采集单元、加速度计单元、陀螺仪单元和磁力计单元四个部分，分别测量得到无人船在地球坐标系下的原始经度信息、纬度信息、前向速度信息、三轴加速度信息、三轴角速度信息和三轴磁场信息；配置如下所示：

在地球坐标系下，从全球定位系统测量的带有测量噪声的位置信息P_δ表示为：

P_δ＝[x_δ,y_δ]^T；

式中，x_δ代表经度信息，y_δ代表纬度信息；

在无人船载体坐标系下，从全球定位系统测量的带有测量噪声的部分速度信息V_δ表示为：

V_δ＝u_δ；

式中，u_δ代表前向速度信息：

在无人船载体坐标系三个方向下，从加速度计测量的带有测量噪声的加速度信息表示为：

式中，a_xδ代表载体坐标系下北方向的加速度信息，a_yδ代表载体坐标系下东方向的加速度信息，a_zδ代表载体坐标系下地方向的加速度信息；

在无人船载体坐标系下，从陀螺仪测量的带有测量噪声的角速度信息表示为：

式中，p代表载体坐标系下北方向的加速度信息，q代表载体坐标系下东方向的加速度信息，r代表载体坐标系下地方向的加速度信息：；

在无人船载体坐标系下，从磁力计测量的带有测量噪声的磁场信息表示为：

式中，代表载体坐标系下北方向的加速度信息，代表载体坐标系下东方向的加速度信息，代表载体坐标系下地方向的加速度信息；

B、状态重组单元的设计

状态重组单元是将经过转换的微机械惯性传感器模块的信息作为状态变量，在考虑所设计无人船航行环境以及航行速度的属性条件下，将无人船的状态信息进行组合，表示无人船的姿态信息；

状态重组单元的输入分为三个部分：第一部分是全球定位系统信号采集单元输出的位置信息经过坐标系转换得到的地球坐标系下的北东地位置信息；第二部分是全球定位系统信号采集单元直接输出的载体坐标系下的无人船的速度信息；最后一部分是加速度计单元、陀螺仪单元和磁力计单元组成的微机械惯性传感器模块输出的原始信息经过数据解算后得到的航向和角速度信息；

其中，前两部分都是通过全球定位系统信号采集单元得到的原始信息，通过对全球定位系统接收到的位置信息进行解析，提取有效数据存入无人船导航系统的缓冲区；所述的全球定位系统通过NMEA-0183协议输出包含位置信息的不同格式帧，通过接收最小数据量的格式帧获取全球定位系统信息，信息格式如下所示：

$GPRMC,time,status,Lat,N,Lon,E,spd,cog,date,mv,mvE,mode,sum (3)

其中，Lat代表纬度信息、N代表北纬、Lon代表经度信息、E代表东经、spd代表速度信息；第一部分是无人船全球定位系统信号采集单元输出的地球坐标系下的原始经度x_δ和纬度y_δ位置信息：

P_δ＝[x_δ,y_δ]^T

通过坐标系转换单元转换到地球坐标系下的北东地初步导航位置信息：

P_δ＝[N_δ,E_δ]^T，

N_δ和E_δ分别代表无人船相对于设置的坐标原点北向和东向的位置，单位为m；第二部分是全球定位系统信号采集单元直接输出载体坐标系下的无人船的速度信息u_δ，实际上，全球定位系统信号采集单元得到的速度测量值u_δ是合速度但是无人船的横荡速度即侧向速度v相对于纵荡速度即前向速度u很小；因此，对于无人船忽略横荡速度，将合速度当作纵荡速度；最后一部分是通过微机械惯性传感器模块输出的数字量转换为无人船艏摇角度和艏摇角速度信息；

对于低速航行的小型无人船，由于横摇φ和纵摇θ比较小，忽略地球的转动；因此，使用纵荡、横荡和艏摇方向的运动描述无人船在水平面的运动，即忽略垂荡、横摇和纵摇方向的运动，令w＝p＝q＝0；由于横摇和纵摇方向运动的忽略，姿态只与艏摇有关，如式(4)所示，所以通过对上述第一部分得到的无人船北向和东向位置信息、第二部分得到的无人船速度信息和最后一部分得到的无人船艏摇角和艏摇角速度信息进行状态重组，构建出新的状态变量，η和υ分别表示无人船的状态，最终的无人船运动学方程如下：

其中：

为无人船载体坐标系和地球坐标系北东地之间的转移矩阵，υ＝[u,v,r]^T∈R³是选取的新的状态向量；代表地球坐标系北东地无人船位置信号，其中：N表示在地球坐标系北东地的北方向的位置坐标、单位为米，E表示在地球坐标系北东地东方向的位置坐标、单位为米，表示了地球坐标系北东地无人船的艏摇角、单位为度；υ＝[u,v,r]^T∈R³代表无人船载体坐标系下无人船的速度信号，其中u表示无人船载体坐标系下的纵荡速度，v表示无人船载体坐标下的横荡速度，单位为米每秒；r表示无人船载体坐标系下的艏摇角速度，单位为度每秒；

C、比较单元的设计

比较单元的输入信息是状态重组单元输出的带有测量噪声的无人船位置信息和速度信息非线性观测器输出的估计的无人船位置信息和速度信息由下式：

得到比较单元的输出信号和

D、非线性观测器的设计

非线性观测器采用导航滤波算法，其输入信号是比较单元输出的无人船状态信息和以及惯性导航系统数据解算单元输出的加速度信息：

这里选取带有测量噪声的位置信息、速度信息和加速度信息，设计非线性观测器进行导航滤波处理；通过无人船运动学方程(4)设计如下简化的非线性观测器，用于估计无人船位置和速度信息：

式中，表示非线性观测器估计的实际的无人船位置信息，表示非线性观测器估计的实际的无人船速度信息，K₁₁、K₁₂、K₂₁、K₂₂均为非线性观测器的设计增益参数。