CN104280026A - 基于aukf的深海机器人长基线组合导航方法 - Google Patents

Abstract

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

Description

基于AUKF的深海机器人长基线组合导航方法

技术领域

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

背景技术

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

现有的水下导航方法主要是多传感器组合导航方法，对多传感器信息进行滤波。但是针对深海机器人的导航问题，在***噪声信息未知时，如果设置的噪声参数与实际的噪声相差过大，就会导致有偏估计甚至滤波算法的发散。海洋环境复杂多变，很难对噪声做出较为准确的先验估计，因此要设计和采用更适合于实时估计***噪声信息的滤波算法。长基线定位数据野值较多、延迟较大，而且某些情况下工作状态不正常，因此对长基线数据的正确处理对提高导航精度也至关重要。

发明内容

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

本发明为实现上述目的所采用的技术方案是：一种基于AUKF的深海机器人长基线组合导航方法，利用全球定位***获取深海机器人的初始绝对位置作为航迹推算的初始点，并采集深海机器人的初始信息；

构建无色卡尔曼滤波主滤波器并对采集到的初始信息进行滤波估计，构建无色卡尔曼辅助滤波器，对主滤波器滤波估计后信息进一步滤波估计；

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

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

所述长基线声学定位装置通过自行解算深海机器人绝对位置的方法获取位置信息。

所述自行解算深海机器人绝对位置的方法，步骤如下：

长基线数据包协议解析出深海机器人到每个信标的距离信息，判断各信标数据的有效性，根据每个有效信标的绝对位置信息和深海机器人到信标的距离信息，建立三元二次方程组：

${(x_o-x_A)}^2+{(y_o-y_A)}^2+{(z_o-z_A)}^2=l_A^2$

${(x_o-x_B)}^2+{(y_o-y_B)}^2+{(z_o-z_B)}^2=l_B^2$

${(x_o-x_C)}^2+{(y_o-y_C)}^2+{(z_o-z_C)}^2=l_C^2$

上述方程组中(x_A,y_A,z_A),(x_B,y_B,z_B),(x_C,y_C,z_C)分别为各有效信标的绝对位置坐标，l_A、l_B、l_C分别为深海机器人到各个信标的距离，解此方程组得(x_o,y_o,z_o)为深海机器人的绝对位置坐标，得到机器人绝对位置坐标的两组解，根据当前时刻深度计采集的深度信息判定两组解中的有效解，作为观测矢量中的东向位置观测值和北向位置观测值。

无色卡尔曼主滤波器滤波估计的实现过程为：

对上一时刻随机向量的被估计量均值和方差经非线性无色变换后，得到离散Sigma点，经主滤波器的状态方程计算更新Sigma点，然后计算出更新Sigma点的状态均值和方差，对状态均值和方差计算得到一步估计的状态均值和方差；对一步估计的状态均值和方差再经无色变换和主滤波器的测量方程计算更新，得到深海机器人可观测状态的均值、方差及协方差的估计值；读取传感器采集观测信息，并与状态均值、方差、观测状态的估计值和增益矩阵计算估计，得到***均值、方差的估算值，并得到新息；根据主滤波器的新息经计算得到测量值作为辅助滤波器的观测信号。

辅助滤波器对***过程噪声进行滤波估计的实现过程如下：

上一时刻估计得到辅助滤波器的***噪声方差矩阵的对角线元素和方差，用辅助滤波器的状态方程对对角线元素和方差计算更新后得到对角线元素和方差的一步估计值；用辅助滤波器的测量方程根据主滤波器的一步估计的方差、状态方差和增益矩阵计算更新后得到估计的观测值，将得到的一步估计值、观测值和主滤波器的测量值计算估计得到***过程噪声方差阵的对角线元素的估算值和方差，当有新观测信息时，继续下一控制周期主滤波器的实现步骤。

所述辅助滤波器的状态方程建立如下：

${\stackrel{\‾}{q}}_k={\stackrel{\‾}{q}}_{k-1}+w_{q_k}$

其中为辅助滤波器中的***噪声，为***噪声方差矩阵角线元素。

所述辅助滤波器的测量方程建立如下：

$\stackrel{\‾}{S}{q}_k=g\left({\stackrel{\‾}{q}}_k\right)=\mathrm{vdiag}\left[{\left(K_k^T{K}_k\right)}^{-1}{K}_k^T(P_{k|k-1}-P_k)K_k{\left(K_k^T{K}_k\right)}^{-1}\right]$

其中，vdiag(·)表示一个向量，其分量为矩阵(·)的主对角线元素，***噪声方差矩阵角线元素作为***的观测信号，P_k|k-1为主滤波器状态矢量的一步估计的方差，P_k为主滤波器状态矢量的方差，K_k为主滤波器的增益矩阵，为估计的观测值。

所述主滤波器的测量方程根据长基线定位数据的有效时刻进行切换：

当长基线声学定位装置提供的观测值有效时，测量方程定义为：

y_k＝x_k+Nⁿ

其中，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.本发明导航精度高，由于AUKF算法中的辅助滤波器可以对***的噪声信息进行实时估计，且能够较快的收敛，解决了复杂多变的海洋环境以及深海机器人执行任务而引起的***噪声难以获得较为准确的先验信息进而导致有偏估计甚至滤波发散的问题。

附图说明

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

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

图3是基于AUKF的深海机器人长基线组合导航的实施例结果。

具体实施方式

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

一种基于AUKF的深海机器人组合导航方法，主要包括以下步骤：

利用全球定位***获取深海机器人的初始绝对位置信息作为航迹推算的初始点，利用多普勒计程仪采集线速度信息，利用运动传感器采集姿态信息，利用长基线声学定位装置和深度计采集位置信息；采用自适应无色卡尔曼滤波方法对上述信息进行数据融合，取载体坐标系下的速度矢量和大地坐标系下的位置矢量和艏向角作为滤波器状态矢量，取各传感器采集的信息作为滤波器观测矢量，某些情况下长基线定位***不能提供较为准确的定位信息时，根据信标距离信息自行解算机器人位置作为位置信息的观测值，选择长基线数据有效时刻切换观测方程。滤波结果被深海机器人控制***与导航***直接使用。

进一步地，如果长基线定位***由于水中声速值设置不准确等原因引起定位误差较大，本发明还涉及一种根据长基线数据包中的机器人距水下信标(不少于3个)的距离信息自行解算机器人绝对位置的方法，步骤如下：

每个接收到长基线数据包的时刻，根据长基线数据协议解析出机器人到每个信标（水下布放应不少于3个信标）的距离信息，由数据包协议判断各信标数据的有效性，根据每个有效信标的绝对位置信息和机器人到信标的距离信息，建立对应的球相交的三元二次方程组，解此方程组，得到机器人绝对坐标的两组解，根据当前时刻深度计采集的深度信息判定两组解中的有效解。

所述采用自适应无色卡尔曼滤波方法步骤如下：

在深海机器人运动学模型的基础上，取载体坐标系下的速度矢量、大地坐标系下的位置矢量和艏向角作为滤波器状态矢量，建立***状态方程；

AUKF方法的主滤波器选择标准UKF滤波器，对***状态传递和更新，实时估计出***的状态信息，得到***估算的结果值均值和方差P_k；

依据主滤波器计算估计的新息信息ξ_k计算出辅助滤波器的测量值Sq_k；建立辅助滤波器的状态方程和测量方程,依据测量值Sq_k作为***的观测信号，采用AUKF的辅助滤波器KF对***的噪声信息进行实时估计，得到***噪声协方差阵Q的对角线元素和方差的估算值，这里的噪声信息包括过程噪声v_k和测量噪声n_k，它们的协方差矩阵分别为Q^v和Qⁿ。

进一步地，由于长基线声学定位装置与多普勒计程仪、运动传感器、深度计的数据更新率不一致，长基线声学定位装置的野值较多，选择长基线数据有效时刻切换观测方程。

如图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分别是北向位置、东向位置、深度和艏向角的状态量。

状态方程定义为：

$u_{k+1}^x=u_k^x+N_1^v$

$v_{k+1}^x=v_k^x+N_2^v$

$w_{k+1}^x=w_k^x+N_3^v$

$r_{k+1}^x=r_k^x+N_4^v$

${\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}+N_5^v$

${\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}+N_6^v$

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

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

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

(2)观测方程根据长基线定位数据的有效性进行切换，长基线数据有效时刻是指长基线数据更新并且不是野值的时刻。

(3)AUKF的主滤波器选用UKF对状态方程进行滤波估计。***噪声分布的变化规律是未知的，此时通常将其视为噪声驱动的不相关随机漂移量，故选择KF作为AUKF的辅助滤波器。

(4)AUKF算法中辅助滤波器状态方程的构建：

由深海机器人***噪声方差矩阵Q对角线元素可将辅助滤波器的状态方程表示为：

${\stackrel{\‾}{q}}_k=f_q\left({\stackrel{\‾}{q}}_{k-1}\right)+w_{q_k}$

其中为辅助滤波器中的***噪声，是零均值高斯白噪声为零均值高斯白噪声；

由于的变化规律未知，可将其视为噪声驱动的不相关的随机漂移向量，故将辅助滤波器的状态方程定义为：

${\stackrel{\‾}{q}}_k={\stackrel{\‾}{q}}_{k-1}+w_{q_k}.$

(5)AUKF算法中辅助滤波器测量方程的构建：

辅助滤波器是以主滤波器的新息ξ_k方差矩阵的对角线元素作为***的观测信号，依据主滤波器状态矢量的一步估计的方差P_k|k-1，主滤波器状态矢量的方差P_k，主滤波器的增益矩阵K_k组成辅助滤波器的观测方程定义为：

$\stackrel{\‾}{S}{q}_k=g\left({\stackrel{\‾}{q}}_k\right)=\mathrm{vdiag}\left[{\left(K_k^T{K}_k\right)}^{-1}{K}_k^T(P_{k|k-1}-P_k)K_k{\left(K_k^T{K}_k\right)}^{-1}\right]$

其中vdiag为取中括号内矩阵的主对角线元素组成的一个向量。

如图2所示为自适应无色卡尔曼滤波算法的实现流程。自适应无色卡尔曼滤波为主滤波器和辅助滤波器相结合的滤波实现过程。

主滤波器滤波估计的实现过程：

对上一时刻随机向量的被估计量均值和方差P_k-1经非线性无色变换后，得到的一组离散Sigma点χ_k-1,经***状态方程计算更新Sigma点,然后计算出更新Sigma点的状态均值和方差P_k|k-1，对状态均值和方差P_k|k-1计算得到一步估计的状态均值和方差；对一步估计的状态均值和方差再经无色变换和用测量方程计算更新,得到深海机器人可观测状态的均值其方差及协方差的估计值；读取传感器采集可观测信息y_k同状态均值和方差P_k|k-1和可观测状态的估计值及增益矩阵K_k经计算估计，得到***均值方差P_k的估算值，从中得到新息ξ_k；依主滤波器的新息ξ_k经计算得到测量值Sq_k作为辅助滤波器的观测信号，然后接续辅助滤波器程序步骤。

辅助滤波器KF对***过程噪声进行滤波估计的实现过程：

上一时刻估计得到辅助滤波器的***噪声方差矩阵的对角线元素和其方差用辅助滤波器的状态方程对和计算更新后得到对角线元素和其方差的一步估计值；用辅助滤波器的测量方程依主滤波器的一步估计的方差P_k|k-1、状态方差P_k和增益矩阵K_k经计算更新后得到估计的观测值所得到的一步估计值和估计的观测值同主滤波器的测量值Sq_k经计算估计得到***过程噪声方差阵Q^v的对角线元素的估算值和其方差，当有新观测信息y_k时，接续下一控制周期主滤波器程序步骤。

某些情况下长基线定位***不能提供较为准确的定位信息，所述采用根据长基线数据包中的机器人距水下信标(不少于3个)的距离信息自行解算机器人绝对位置的方法步骤如下：

每个接收到长基线数据包的时刻，根据长基线数据协议解析出机器人到每个信标（水下布放应不少于3个信标）的距离信息，由数据包协议判断各信标数据的有效性，根据每个有效信标的绝对位置信息和机器人到信标的距离信息，建立对应的球相交的三元二次方程组：

${(x_o-x_A)}^2+{(y_o-y_A)}^2+{(z_o-z_A)}^2=l_A^2$

${(x_o-x_B)}^2+{(y_o-y_B)}^2+{(z_o-z_B)}^2=l_B^2$

${(x_o-x_C)}^2+{(y_o-y_C)}^2+{(z_o-z_C)}^2=l_C^2$

上述方程组中(x_A,y_A,z_A),(x_B,y_B,z_B),(x_C,y_C,z_C)为各有效信标的绝对位置坐标，l_Al_Bl_C为机器人到各个信标的距离，解此方程组得(x_o,y_o,z_o)为机器人的绝对位置坐标。得到机器人绝对坐标的两组解，根据当前时刻深度计采集的深度信息判定两组解中的有效解，此解作为观测矢量中的东向和北向位置观测值。

如图3为基于AUKF的深海机器人长基线组合导航的实施例结果：

外场实验中，机器人以不超过1.5m/s的速度在水面航行。图3给出了利用本发明基于自适应无色卡尔曼滤波的深海机器人长基线组合导航方法进行机器人导航的结果，并同长基线设备的定位结果和航迹推算法的导航结果作比较。其中实线表示GPS获取的绝对位置，点划线表示航迹推算法的导航结果，点表示长基线设备的定位结果，虚线表示基于自适应无色卡尔曼滤波的深海机器人长基线组合导航方法的导航结果。从图中可以明显看出，采用基于自适应无色卡尔曼滤波的深海机器人长基线组合导航方法估计出的机器人的轨迹可以克服长基线设备某些情况下的定位不准和航迹推算法随时间增长的累积误差，且更接近GPS获取的绝对位置。此实施例表明本发明基于自适应无色卡尔曼滤波的深海机器人长基线组合导航方法可显著提高组合导航***精度。

融合后的信息包括：平面位置信息、深度信息、姿态信息、线速度信息、角速度信息。

Claims (10)

1.一种基于AUKF的深海机器人长基线组合导航方法，其特征在于：

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

构建无色卡尔曼滤波主滤波器并对采集到的初始信息进行滤波估计，构建无色卡尔曼辅助滤波器，对主滤波器滤波估计后信息进一步滤波估计；

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

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

3.根据权利要求2所述的基于AUKF的深海机器人长基线组合导航方法，其特征在于：所述长基线声学定位装置通过自行解算深海机器人绝对位置的方法获取位置信息。

4.根据权利要求3所述的基于AUKF的深海机器人长基线组合导航方法，其特征在于：所述自行解算深海机器人绝对位置的方法，步骤如下：

长基线数据包协议解析出深海机器人到每个信标的距离信息，判断各信标数据的有效性，根据每个有效信标的绝对位置信息和深海机器人到信标的距离信息，建立三元二次方程组：

${(x_o-x_A)}^2+{(y_o-y_A)}^2+{(z_o-z_A)}^2=l_A^2$

${(x_o-x_B)}^2+{(y_o-y_B)}^2+{(z_o-z_B)}^2=l_B^2$

${(x_o-x_C)}^2+{(y_o-y_C)}^2+{(z_o-z_C)}^2=l_C^2$

上述方程组中(x_A,y_A,z_A),(x_B,y_B,z_B),(x_C,y_C,z_C)分别为各有效信标的绝对位置坐标，l_A、l_B、l_C分别为深海机器人到各个信标的距离，解此方程组得(x_o,y_o,z_o)为深海机器人的绝对位置坐标，得到机器人绝对位置坐标的两组解，根据当前时刻深度计采集的深度信息判定两组解中的有效解，作为观测矢量中的东向位置观测值和北向位置观测值。

5.根据权利要求1所述的基于AUKF的深海机器人长基线组合导航方法，其特征在于：无色卡尔曼主滤波器滤波估计的实现过程为：

对上一时刻随机向量的被估计量均值和方差经非线性无色变换后，得到离散Sigma点，经主滤波器的状态方程计算更新Sigma点，然后计算出更新Sigma点的状态均值和方差，对状态均值和方差计算得到一步估计的状态均值和方差；对一步估计的状态均值和方差再经无色变换和主滤波器的测量方程计算更新，得到深海机器人可观测状态的均值、方差及协方差的估计值；读取传感器采集观测信息，并与状态均值、方差、观测状态的估计值和增益矩阵计算估计，得到***均值、方差的估算值，并得到新息；根据主滤波器的新息经计算得到测量值作为辅助滤波器的观测信号。

6.根据权利要求1所述的基于AUKF的深海机器人长基线组合导航方法，其特征在于：辅助滤波器对***过程噪声进行滤波估计的实现过程如下：

上一时刻估计得到辅助滤波器的***噪声方差矩阵的对角线元素和方差，用辅助滤波器的状态方程对对角线元素和方差计算更新后得到对角线元素和方差的一步估计值；用辅助滤波器的测量方程根据主滤波器的一步估计的方差、状态方差和增益矩阵计算更新后得到估计的观测值，将得到的一步估计值、观测值和主滤波器的测量值计算估计得到***过程噪声方差阵的对角线元素的估算值和方差，当有新观测信息时，继续下一控制周期主滤波器的实现步骤。

7.根据权利要求6所述的基于AUKF的深海机器人长基线组合导航方法，其特征在于：所述辅助滤波器的状态方程建立如下：

${\stackrel{\‾}{q}}_k={\stackrel{\‾}{q}}_{k-1}+w_{q_k}$

其中为辅助滤波器中的***噪声，为***噪声方差矩阵角线元素。

8.根据权利要求6所述的基于AUKF的深海机器人长基线组合导航方法，其特征在于：所述辅助滤波器的测量方程建立如下：

$\stackrel{\‾}{S}{q}_k=g\left({\stackrel{\‾}{q}}_k\right)=\mathrm{vdiag}\left[{\left(K_k^T{K}_k\right)}^{-1}{K}_k^T(P_{k|k-1}-P_k)K_k{\left(K_k^T{K}_k\right)}^{-1}\right]$

其中，vdiag(·)表示一个向量，其分量为矩阵(·)的主对角线元素，***噪声方差矩阵角线元素作为***的观测信号，P_k|k-1为主滤波器状态矢量的一步估计的方差，P_k为主滤波器状态矢量的方差，K_k为主滤波器的增益矩阵，为估计的观测值。

9.根据权利要求5所述的基于AUKF的深海机器人长基线组合导航方法，其特征在于：所述主滤波器的测量方程根据长基线定位数据的有效时刻进行切换：

当长基线声学定位装置提供的观测值有效时，测量方程定义为：

y_k＝x_k+Nⁿ

其中，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为观测矢量。

10.根据权利要求9所述的基于AUKF的深海机器人长基线组合导航方法，其特征在于：所述有效时刻为长基线数据更新并且不是野值的时刻。