CN104280025A - 基于无色卡尔曼滤波的深海机器人超短基线组合导航方法 - Google Patents

Abstract

本发明涉及一种基于无色卡尔曼滤波的深海机器人超短基线组合导航方法，利用全球定位系统获取深海机器人的初始绝对位置作为航迹推算的初始点，并采集初始信息；采用无色卡尔曼滤波方法对采集到的初始信息进行数据融合，得出融合后的信息。本方法可以提高使用超短基线定位系统的深海机器人的导航精度，同时能够平滑深海机器人控制系统所需的航向、深度以及载体坐标系下的速度信息。

Description

基于无色卡尔曼滤波的深海机器人超短基线组合导航方法

技术领域

本发明涉及海洋工程领域的一种深海机器人导航技术，具体地说是一种基于无色卡尔曼滤波的超短基线组合导航方法。

背景技术

精确的导航能力是水下机器人有效作业和安全回收的关键。导航系统必须提供远距离及长时间范围内的精确定位、速度和姿态信息。但是由于受到水下环境的复杂性、机器人自身体积、重量、能源以及隐蔽性等因素的影响，实现高精度的水下机器人导航仍然是一项艰巨的任务。

现有的水下导航方法主要是多传感器组合导航方法，与陆地组合导航方法无异，采用运动学方程对多传感器信息进行滤波。但是针对深海机器人的导航问题，机器人在水中的运动状态更加复杂多变，声学定位数据有较多野值而且有很大的延迟，采用水中动力学方程才能更精确地刻画机器人在水中的运动状态，对于有延迟的位置信息正确融合对提高导航精度也至关重要。

发明内容

针对现有技术的不足，本发明提供一种基于无色卡尔曼滤波的深海机器人组合导航方法，目的在于提高使用超短基线定位系统的深海机器人的导航精度，同时能够平滑深海机器人控制系统所需的航向、深度以及载体坐标系下的速度信息。

本发明为实现上述目的所采用的技术方案是：

一种基于无色卡尔曼滤波的深海机器人超短基线组合导航方法，利用全球定位系统获取深海机器人的初始绝对位置作为航迹推算的初始点，并采集深海机器人的初始信息；采用无色卡尔曼滤波方法对采集到的初始信息进行数据融合，得出融合后的信息。

所述初始信息包括多普勒计程仪采集的线速度信息，运动传感器采集的姿态信息，超短基线声学定位装置采集的位置信息和深度计采集的深度信息。

所述超短基线声学定位装置通过位移修正方法采集位置信息。

所述位移修正方法为用当前时刻接收到的超短基线声学定位装置提供的位置信息加上当前时刻距上次水声通讯时刻时间段内深海机器人走过的位移作为当前时刻超短基线定位装置提供的位置观测值。

在无色卡尔曼滤波过程中，取深海机器人的动力学方程作为系统的状态方程。

所述状态方程建立如下：

$M\stackrel{\·\·}{q}+C\left(\stackrel{\·}{q}\right)\stackrel{\·}{q}+D\left(\stackrel{\·}{q}\right)\stackrel{\·}{q}+G\left(q\right)=u$

其中，M为6×6维的惯性矩阵，包含离心力和哥氏力的6×6维矩阵，为6×6维的水动力矩阵，G(q)包含重力和浮力的6×1维矩阵，q和u分别为6×1维的状态和电机转矩矢量。

在无色卡尔曼滤波过程中的测量方程选择超短基线数据有效时刻的切换观测方程。

所述有效时刻为超短基线数据有更新并且不是野值的时刻。

所述测量方程定义如下：

当超短基线定位装置提供的观测值有效时为：

y_k＝x_k+Nⁿ

x＝[u^x,v^x,w^x,r^x,ξ^x,η^x,ζ^x,ψ^x]^T为状态矢量，其中u^x、v^x、w^x和r^x分别是前向速度，侧向速度，垂向速度和转艏角速度的状态量，ξ^x、η^x、ζ^x和ψ^x分别是北向位置、东向位置、深度和艏向角的状态量；

y＝[u^y,v^y,w^y,r^y,ξ^y,η^y,ζ^y,ψ^y]^T为观测矢量，其中u^y、v^y、w^y、r^y分别是前向速度、侧向速度、垂向速度和转艏角速度的观测量，ξ^y、η^y、ζ^y、ψ^y是北向位置、东向位置、深度和艏向角的观测量；

Nⁿ为观测噪声；

当超短基线定位装置提供的观测值无效时为：

$y_1=x_1+N_1^n$

$y_2=x_2+N_2^n$

$y_3=x_3+N_3^n$

$y_4=x_4+N_4^n$

$y_5=x_7+N_5^n$

$y_6=x_8+N_6^n$

其中，y＝[u^y,v^y,w^y,r^y,ζ^y,ψ^y]^T为观测矢量。

本发明具有以下有益效果及优点：

1.本发明采用无色卡尔曼滤波作为数据融合方法，计算量小，在过程噪声和测量噪声的先验信息已知的情况下，可以达到较高的估计精度；

2.本发明使用深海机器人动力学模型作为系统的状态方程，更精确地描述深海机器人在水下的运动状态，提高了导航精度；

3.本发明提出了根据超短基线定位数据的有效性切换观测方程，解决了超短基线定位数据野值较多以及超短基线定位装置与其他传感器采集数据更新率不一致的问题；

4.本发明利用上一次水声通讯时刻位置的新息信息更新当前时刻的位置信息，解决了超短基线定位系统的位置观测值有延迟的问题。

附图说明

图1是本发明的组合导航方法的工作原理图；

图2是本发明的无色卡尔曼滤波算法的流程图；

图3是基于无色卡尔曼滤波算法的机器人超短基线组合导航的实施例结果。

具体实施方式

下面结合附图及实施例对本发明做进一步的详细说明。

如图1所示为本发明的组合导航方法的工作原理图：利用多普勒计程仪采集线速度信息，利用超短基线声学定位装置采集平面位置信息，利用深度计采集深度信息，利用运动传感器采集姿态信息，利用数据融合方法对上述信息进行融合，得到深海机器人在海中的三维运动轨迹，同时能够平滑深海机器人控制系统所需的航向以及载体坐标系下的速度信息。

(1)取载体坐标系下的速度矢量和大地坐标系下的位置矢量和艏向角作为滤波器状态矢量，定义为：x＝[u^x,v^x,w^x,r^x,ξ^x,η^x,ζ^x,ψ^x]^T，其中u^x,v^x，w^x，r^x是前向速度，侧向速度，垂向速度和转艏角速度的状态量，ξ^x,η^x,ζ^x，ψ^x是北向位置，东向位置，深度和艏向角的状态量。

取深海机器人的动力学方程作为系统的状态方程，深海机器人的动力学方程如下：

$M\stackrel{\·\·}{q}+C\left(\stackrel{\·}{q}\right)\stackrel{\·}{q}+D\left(\stackrel{\·}{q}\right)\stackrel{\·}{q}+G\left(q\right)=u$

式中,M为6×6维的惯性矩阵，包含离心力和哥氏力的6×6维矩阵，为6×6维的水动力矩阵，G(q)包含重力和浮力的6×1维矩阵，q和u分别为6×1维的状态和电机转矩矢量。

本发明中所采用的深海机器人动力学方程中的矢量为上述矢量中对应前向速度u^x，侧向速度v^x，垂向速度w^x和转艏角速度r^x的四维。

除u^x,v^x，w^x，r^x状态矢量x中的ξ^x,η^x,ζ^x，ψ^x的状态方程如下：

${\mathrm{\ξ}}_{k+1}^x={\mathrm{\ξ}}_k^x+[\sin ({\mathrm{\ψ}}_k^x+N_8^v)\×(u_k^x+N_1^v)+\cos ({\mathrm{\ψ}}_k^x+N_8^v)\×(v_k^x+N_2^v)]\×\mathrm{\Δt}$

${\mathrm{\η}}_{k+1}^x={\mathrm{\η}}_k^x+[\cos ({\mathrm{\ψ}}_k^x+N_8^v)\×(u_k^x+N_1^v)-\sin ({\mathrm{\ψ}}_k^x+N_8^v)\×(v_k^x+N_2^v)]\×\mathrm{\Δt}$

${\mathrm{\ξ}}_{k+1}^x={\mathrm{\ξ}}_k^x+(w_k^x+N_3^v)\×\mathrm{\Δt}$

${\mathrm{\ψ}}_{k+1}^x={\mathrm{\ψ}}_k^x+(r_k^x+N_4^v)\×\mathrm{\Δt}$

N^v为系统噪声，Δt为传感器采样间隔。

(2)所述根据超短基线定位数据的有效性切换观测方程步骤如下：

超短基线数据有效时刻是指超短基线数据更新并且不是野值的时刻，

1.超短基线定位装置提供的观测值有效时：

观测矢量定义为：y＝[u^y,v^y,w^y,r^y,ξ^y,η^y,ζ^y,ψ^y]^T，其中u^y,v^y，w^y，r^y是前向速度，侧向速度，垂向速度和转艏角速度的观测量，ξ^y,η^y,ζ^y，ψ^y是北向位置，东向位置，深度和艏向角的观测量。观测值来自于多普勒计程仪和运动传感器，东向和北向位置观测值来自于超短基线定位装置，深度观测值来自于深度计，艏向角观测值来自于运动传感器。

测量方程定义为：y_k＝x_k+Nⁿ，Nⁿ为观测噪声。

2.超短基线定位装置提供的观测值无效时：

观测矢量定义为：y＝[u^y,v^y,w^y,r^y,ζ^y,ψ^y]^T

测量方程定义为：

$y_1=x_1+N_1^n$

$y_2=x_2+N_2^n$

$y_3=x_3+N_3^n$

$y_4=x_4+N_4^n$

$y_5=x_7+N_5^n$

$y_6=x_8+N_6^n$

(3)由于超短基线定位装置的工作原理基于水声通讯，超短基线定位装置提供的位置观测值是上一次水声通讯时刻机器人的位置，所述利用上一次水声通讯时刻位置的新息信息更新当前时刻的位置信息步骤如下：

用当前时刻接收到的超短基线定位装置提供的位置信息加上当前时刻距上次水声通讯时刻这个时间段内机器人走过的位移作为当前时刻超短基线定位装置提供的位置观测值，经过无色卡尔曼滤波算法得到当前时刻位置的最优估计。

(4)如图2所示为本发明的无色卡尔曼滤波算法的流程图，所述的无色卡尔曼滤波估计算法是以基本的卡尔曼滤波算法为框架，通过非线性的无色变换来进行非线性系统的状态和误差协方差的递推和更新。

无色卡尔曼滤波估计实现过程：

对上一时刻即k-1由被估计量“先验”的均值和方差P_k-1经非线性无色变换得到的一组离散Sigma点χ_k-1，χ_k-1经系统状态方程计算更新后得到离散的Sigma点对计算得到一步估计的扩展状态均值和方差P_k|k-1；对和P_k|k-1再进行无色变化得到一组离散的Sigma点χ_k|k-1，χ_k|k-1经测量方程计算更新后得到离散的Sigma点γ_k|k-1，对γ_k|k-1计算得到深海机器人可观测状态的均值和方差的估计值；所得到的一步估计值和得到的可观测状态的估计值与传感器采集观测值y_k，经计算估计得到系统估算值和P_k。

如图3所示为基于无色卡尔曼滤波算法的机器人超短基线组合导航的实施例结果实施例：

外场实验中，机器人以不超过2m/s的速度在水下航行并上浮，由于航行期间机器人处于深水，所以没有GPS的定位信息。图3给出了利用本发明基于无色卡尔曼滤波的深海机器人超短基线组合导航方法进行机器人导航的结果，并同超短基线设备的定位结果和航迹推算法的导航结果作比较。其中圆圈表示航迹推算法的导航结果，星号表示超短基线设备的定位结果，点表示基于无色卡尔曼滤波的深海机器人超短基线组合导航方法的导航结果。从图中可以明显看出，采用基于无色卡尔曼滤波的深海机器人超短基线组合导航方法估计出的机器人的轨迹可以克服超短基线设备定位值少、野值多和航迹推算法随时间增长的累积误差。此实施例表明本发明基于无色卡尔曼滤波的深海机器人超短基线组合导航方法可显著提高组合导航系统精度。

Claims (9)

1.一种基于无色卡尔曼滤波的深海机器人超短基线组合导航方法，其特征在于：

利用全球定位系统获取深海机器人的初始绝对位置作为航迹推算的初始点，并采集深海机器人的初始信息；

采用无色卡尔曼滤波方法对采集到的初始信息进行数据融合，得出融合后的信息。

2.根据权利要求1所述的基于无色卡尔曼滤波的深海机器人超短基线组合导航方法，其特征在于：所述初始信息包括多普勒计程仪采集的线速度信息，运动传感器采集的姿态信息，超短基线声学定位装置采集的位置信息和深度计采集的深度信息。

3.根据权利要求2所述的基于无色卡尔曼滤波的深海机器人超短基线组合导航方法，其特征在于：所述超短基线声学定位装置通过位移修正方法采集位置信息。

4.根据权利要求3所述的基于无色卡尔曼滤波的深海机器人超短基线组合导航方法，其特征在于：所述位移修正方法为用当前时刻接收到的超短基线声学定位装置提供的位置信息加上当前时刻距上次水声通讯时刻时间段内深海机器人走过的位移作为当前时刻超短基线定位装置提供的位置观测值。

5.根据权利要求1所述的基于无色卡尔曼滤波的深海机器人超短基线组合导航方法，其特征在于：在无色卡尔曼滤波过程中，取深海机器人的动力学方程作为系统的状态方程。

6.根据权利要求5所述的基于无色卡尔曼滤波的深海机器人超短基线组合导航方法，其特征在于：所述状态方程建立如下：

$M\stackrel{\·\·}{q}+C\left(\stackrel{\·}{q}\right)\stackrel{\·}{q}+D\left(\stackrel{\·}{q}\right)\stackrel{\·}{q}+G\left(q\right)=u$

其中，M为6×6维的惯性矩阵，包含离心力和哥氏力的6×6维矩阵，为6×6维的水动力矩阵，G(q)包含重力和浮力的6×1维矩阵，q和u分别为6×1维的状态和电机转矩矢量。

7.根据权利要求1所述的基于无色卡尔曼滤波的深海机器人超短基线组合导航方法，其特征在于：在无色卡尔曼滤波过程中的测量方程选择超短基线数据有效时刻的切换观测方程。

8.根据权利要求7所述的基于无色卡尔曼滤波的深海机器人超短基线组合导航方法，其特征在于：所述有效时刻为超短基线数据有更新并且不是野值的时刻。

9.根据权利要求7所述的基于无色卡尔曼滤波的深海机器人超短基线组合导航方法，其特征在于：所述测量方程定义如下：

当超短基线定位装置提供的观测值有效时为：

y_k＝x_k+Nⁿ

x＝[u^x,v^x,w^x,r^x,ξ^x,η^x,ζ^x,ψ^x]^T为状态矢量，其中u^x、v^x、w^x和r^x分别是前向速度，侧向速度，垂向速度和转艏角速度的状态量，ξ^x、η^x、ζ^x和ψ^x分别是北向位置、东向位置、深度和艏向角的状态量；

y＝[u^y,v^y,w^y,r^y,ξ^y,η^y,ζ^y,ψ^y]^T为观测矢量，其中u^y、v^y、w^y、r^y分别是前向速度、侧向速度、垂向速度和转艏角速度的观测量，ξ^y、η^y、ζ^y、ψ^y是北向位置、东向位置、深度和艏向角的观测量；

Nⁿ为观测噪声；

当超短基线定位装置提供的观测值无效时为：

$y_1=x_1+N_1^n$

$y_2=x_2+N_2^n$

$y_3=x_3+N_3^n$

$y_4=x_4+N_4^n$

$y_5=x_7+N_5^n$

$y_6=x_8+N_6^n$

其中，y＝[u^y,v^y,w^y,r^y,ζ^y,ψ^y]^T为观测矢量。