CN107990891A

CN107990891A - 基于长基线和信标在线标定的水下机器人组合导航方法

Info

Publication number: CN107990891A
Application number: CN201610944484.4A
Authority: CN
Inventors: 王轶群; 李波; 刘健; 赵宏宇
Original assignee: Shenyang Institute of Automation of CAS
Current assignee: Shenyang Institute of Automation of CAS
Priority date: 2016-10-26
Filing date: 2016-10-26
Publication date: 2018-05-04
Anticipated expiration: 2036-10-26
Also published as: CN107990891B

Abstract

本发明涉及基于长基线声学定位和信标在线标定的组合导航方法，实现水下机器人的水下导航定位。本发明包括：利用AUV不同时刻的单信标斜距测量值，实现在线标定各个长基线信标位置；建立基于长基线声学定位和惯性导航数据融合的卡尔曼滤波器，计算组合导航的位置估计。本方法能够有效融合长基线定位和惯性导航数据，具有较高的导航定位精度；本方法移植方便，可以适用于各种潜航器水下导航定位。

Description

基于长基线和信标在线标定的水下机器人组合导航方法

技术领域

本发明涉及水下机器人技术领域，尤其涉及一种无人自治水下机器人(简称AUV)的基于长基线声定位和信标在线标定技术的水下机器人组合导航方法，实现AUV水下高精度组合导航定位。

背景技术

在海洋应用中，水下机器人发挥越来越重要的作用。水下机器人分成两类：一种是遥控式有缆水下机器人(简称ROV)，一种是无人自治水下机器人(简称AUV)。ROV需要水面母船支持，同时受到电缆长度的限制，其作业距离有限，一般只有几百米；而AUV自身携带能源，可以远离母船，活动距离达到几十公里甚至上百公里。所以AUV的研究越来越受到各国的重视，AUV的发展代表了未来水下机器人的发展方向。而水下组合导航定位技术是AUV发展的关键和制约的瓶颈。因为水下环境的特殊性，无法像陆地直接使用差分GPS导航定位，所以目前水下导航定位主要有两类：惯性导航数据和水声定位导航数据。惯性导航数据短航程时定位精度高，但随着航程的增加，累积的导航误差降低了系统的导航精度；水声定位导航数据以长基线定位精度最高，长基线定位系统(简称LBL)的基阵长度一般在几千米，需要在海底布设3个以上的信标(一般布设4个信标，其中1个信标是备份信标)，通过测量AUV与信标之间的距离来确定AUV的位置。长基线定位的优点是定位精度较高，并且定位误差不随时间的增加而累积，有效地解决了惯性导航系统的累积导航误差问题，所以现在广泛将长基线数据和惯性导航数据组成组合导航系统，实现高精度水下定位。但是长基线定位的使用也存在一些问题，第一，受到海洋环境干扰，长基线定位常常出现噪点，无法直接用于导航定位；第二，浅水工况下长基线信标标定精度较高，但是深水工况下长基线信标标定精度不是很理想。长基线在使用前需要布放信标和使用母船进行信标位置标定，而深水环境中水声条件复杂，尤其2000米以上，母船标定的长基线信标标定结果随着水深的增加而逐渐下降，影响长基线定位系统的定位精度，无法满足高精度水下定位的要求。在深水工况下，如何将长基线定位数据和惯性导航数据有效数据融合，即抑制长基线定位噪点，又能抑制导航累积误差，实现高精度水下定位，是深水组合导航重点研究的一个难题；如何减少深水工况下长基线信标标定误差对导航系统的影响，也是深水组合导航的亟待解决的技术难题。

发明内容

为了解决滤波长基线定位和抑制导航系统累积误差的问题，同时降低信标标定误差对导航定位的影响，本发明要解决的技术问题是提出一种将基于长基线声学定位和信标在线标定的新型组合导航方法，有效融合长基线定位和惯性导航定位数据，滤波长基线定位扰动对组合导航定位的干扰，降低了导航系统的累积误差，同时在线标定信标位置，降低深水条件下母船标定信标产生的标定误差对导航精度的影响。

本发明为实现上述目的所采用的技术方案是：基于长基线和信标在线标定的水下机器人组合导航方法，包括以下步骤：

利用AUV不同时刻的单信标斜距测量值，在线标定各个信标位置得到各个信标的标定误差；

根据标定误差、通过基于长基线声学定位的量测方程和惯性导航的预测方程估计组合导航的AUV位置。

标定误差通过下式得到

[dx_i,dy_i]^T＝A^-1B

其中，

t时刻、t+1时刻和t+2时刻的AUV位置分别为(x_t,y_t,z_t),(x_t+1,y_t+1,z_t+1)和(x_t+2,y_t+2,z_t+2)，t时刻、t+1时刻和t+2时刻AUV到信标i的斜距分别为d_{i_t},d_{i_t+1},d_{i_t+2}；

t时刻、t+1时刻和t+2时刻AUV到信标i的水平距离分别为r_{i_t},r_{i_t+1},r_{i_t+2}；t时刻、t+1时刻和t+2时刻AUV到信标i的水平距离估计分别为信标i的母船初始标定位置为(x_i,y_i,z_i)。

所述根据标定误差、通过基于长基线声学定位的量测方程和惯性导航的预测方程估计组合导航的位置包括以下步骤：

1)根据标定误差计算t时刻AUV的长基线定位位置Z_t(x_t,y_t)；

2)根据AUV长基线定位位置建立量测方程；

3)建立组合导航的预测方程得到AUV的预测位置X_t(x_t,y_t)；

4)根据AUV的长基线定位位置Z_t(x_t,y_t)和预测位置X_t(x_t,y_t)得到组合导航的AUV位置。

所述根据标定误差计算t时刻AUV的长基线定位位置Z_t(x_t,y_t)，包括以下步骤：[x_t,y_t]^T＝A^-1(R-D)

其中，

t时刻AUV到信标1、2、3的水平距离为r_{1_t},r_{2_t},r_{3_t}；信标i的母船初始标定位置为(x_i,y_i,z_i)；信标1、2、3的位置估计为(x₁+dx₁,y₁+dy₁)，(x₂+dx₂,y₂+dy₂)和(x₃+dx₃,y₃+dy₃)，dx_i,dy_i为标定误差；

上式得到的[x_t,y_t]^T作为t时刻AUV的长基线定位位置Z_t(x_t,y_t)的列向量表示。

所述量测方程为

其中，V_t是t时刻信标的测量噪声；表示t时刻的量测表达式；是t时刻测量时AUV的真实经度和真实纬度；H_t＝[1,1]^T是测量方程线性化近似的雅各比矩阵。

所述预测方程为

其中，u_t,ψ,θ,φ是t时刻AUV的速度、航向角、纵倾角和横滚角，U_t＝[u_t,ψ]是系统输入矩阵；ΔT是从t-1时刻到t时刻的时间间隔；H_t＝[1,1]^T是测量方程的雅各比矩阵；系统输入噪声的方差矩阵为Q_t；x_t-1和y_t-1分别表示t-1时刻AUV在经度和纬度位置；P_t,t-1表示t时刻预测AUV位置的预测方差；P_t-1表示t-1时刻估计AUV位置的估计方差；是欧拉转换矩阵；R_e是地球的短轴半径；e是地球旋转椭球扁率。

所述根据AUV的长基线定位位置Z_t(x_t,y_t)和预测位置X_t(x_t,y_t)得到组合导航的位置估计通过下式得到

其中，t时刻AUV估计位置和分别表示t时刻AUV的估计位置经度和纬度；R_t表示t时刻量测噪声的方差矩阵，h(X_t)＝H_tX_t+V_t。

所述估计组合导航的位置后，通过计算组合导航的位置估计方差来估计导航精度：其中，I是单位矩阵，P_t表示t时刻AUV组合导航的位置估计方差。

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

1.相对于传统的长基线定位和惯性导航数据，本方法使用组合导航算法既能够有效滤波长基线定位噪点对组合导航的干扰，同时有效抑制组合导航的累积导航误差，提高了AUV水下定位精度。

2.传统的母船标定深水长基线信标位置存在随着水深的增加，标定精度逐渐下降的问题。而本方法以母船标定位置为初始量，通过在线标定技术提高信标标定精度，降低信标标定误差对导航精度的影响，提高了AUV的水下定位精度。

3.应用范围广。本发明不但可以应用于AUV水下导航，还可以用于其它潜航器的水下导航。

4.为了有效融合长基线定位数据和惯性导航数据，同时降低信标标定误差对导航定位的影响，本发明将基于长基线的组合导航技术和信标在线标定技术相结合，不但有效滤波长基线定位数据，而且抑制了导航系统的累积误差；在线标定长基线信标位置，降低了标定误差对导航精度的影响，提高了AUV组合导航位置估计的精度。

附图说明

图1是本发明的组成示意图。

具体实施方式

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

本发明的硬件要求是一台AUV，AUV上搭载深度计测量深度，搭载多普勒测量潜器的当前速度，搭载姿态角传感器测量潜器当前的航向角、纵倾角和横滚角，搭载水声测距仪测量AUV到长基线信标的距离，在海底布放4个固定的长基线信标。

如图1所示，AUV在水下作业时，自动将多普勒测量的速度、航向角传感器测量的速度和AUV到信标的距离输入组合导航算法中，实时自动计算AUV的水下位置。

4个锚系在海底的声信标和一台AUV，其中AUV上安装水声测距器、多普勒计程仪、姿态角传感器和深度计，其中姿态角传感器测量AUV当前航向角、纵倾角和横滚角，多普勒计程仪测量AUV当前速度，水声测距器测量AUV到声信标的斜距。

本发明的方法包括两个内容：内容一，利用AUV不同时刻的单信标斜距测量值，在线标定各个信标位置；内容二，建立基于长基线声学定位和惯性导航数据的数据融合的卡尔曼滤波器，计算组合导航的位置估计。

1、利用AUV不同时刻的单信标斜距测量值，在线标定各个信标位置

利用AUV不同时刻的单信标斜距测量值，在线标定各个信标位置的作用是解决母船标定长基线信标标定精度随着水深的增加而标定精度下降的问题。它的思想是将母船标定的初始位置为初始量，使用在线标定算法计算出标定误差，然后获得比母船标定的信标位置精度更高的标定结果。定义信标i(1≤i≤N,N是信标总数目)的母船初始标定位置为(x_i,y_i,z_i)，该变量是已知量；定义信标i的标定误差为(dx_i,dy_i,0)，该变量是待求解变量；定义t时刻、t+1时刻和t+2时刻的AUV位置(x_t,y_t,z_t),(x_t+1,y_t+1,z_t+1)和(x_t+2,y_t+2,z_t+2)，该变量是已知量；定义t时刻、t+1时刻和t+2时刻AUV到信标i的斜距分别为d_{i_t},d_{i_t+1},d_{i_t+2}，该变量是已知量；则t时刻、t+1时刻和t+2时刻信标i到AUV的斜距方程组成的信标i的标定方程组如下：

其解方程为：

[dx_i,dy_i]^T＝A^-1B

其中，

其中，定义t时刻、t+1时刻和t+2时刻AUV到信标i的水平距离r_{i_t},r_{i_t+1},r_{i_t+2}；定义t时刻、t+1时刻和t+2时刻AUV到信标i的水平距离估计那么水平距离和水平距离估计的定义表达式如下：

2、建立基于长基线声学定位和惯性导航数据融合的卡尔曼滤波器

建立基于长基线声学定位和惯性导航数据融合的卡尔曼滤波器的作用是综合利用长基线声学定位数据、多普勒测量的速度、姿态传感器测量的姿态角等惯性导航数据，获得高精度的组合导航位置估计。

第1步，计算t时刻的AUV长基线定位Z_t(x_t,y_t)，长基线定位解算方程如下：

[x_t,y_t]^T＝A^-1(R-D)

其中，

其中，定义t时刻AUV到信标1、2、3的水平距离为r_{1_t},r_{2_t},r_{3_t}，是已知量；信标1、2、3的位置估计为(x₁+dx₁,y₁+dy₁)，(x₂+dx₂,y₂+dy₂)和(x₃+dx₃,y₃+dy₃)，该变量dx_i,dy_i是上一步标定信标位置的计算结果。

第2步，定义t时刻的AUV声学定位Z_t(x_t,y_t)，其中x_t和y_t分别表示t时刻AUV在x方向和y方向的声学定位位置，建立长基线声学定位的量测方程：

其中，V_t是t时刻信标的测量噪声，它是零均值高斯白噪声，量测噪声方差矩阵为R_t，信标噪声的方差矩阵是设备属性；是t时刻的量测表达式，无法获得它的准确表达式，这里是它的线性化近似表达式；是t时刻测量时AUV的真实经度和真实纬度，是未知量；定义H_t＝[1,1]^T是测量方程线性化近似的雅各比矩阵，是常量。

第3步，定义t时刻AUV的预测位置为X_t(x_t,y_t)，其中x_t和y_t分别表示t时刻预测AUV的经度和纬度，建立组合导航的预测方程：

其中，u_t,ψ,θ,φ是t时刻AUV的速度、航向角、纵倾角和横滚角，它们可以分别通过多普勒和姿态传感器测量得到，是已知量；那么定义U_t＝[u_t,ψ]是系统输入矩阵；ΔT是从t-1时刻到t时刻的时间间隔，是已知量；定义H_t＝[1,1]^T是测量方程的雅各比矩阵，是常量；W_t是t时刻的系统输入噪声，它是零均值高斯白噪声，定义系统输入噪声的方差矩阵为Q_t，Q_t是航向角传感器和多普勒的设备属性，所以输入噪声的方差矩阵Q_t是已知量；x_t-1和y_t-1分别表示t-1时刻AUV在经度和纬度位置，是已知量；P_t,t-1表示t时刻预测AUV位置的预测方差，未知量；P_t-1表示t-1时刻估计AUV位置的估计方差，是已知量；是欧拉转换矩阵，将多普勒测量的基于AUV坐标系的速度转换成北-东坐标系速度，是已知量；R_e是地球WGS-84模型的短轴半径，取值为6 378 137米；e是地球WGS-84模型旋转椭球扁率，取值为1/298.257。

通过组合导航的预测方程，计算t时刻AUV的预测位置为X_t(x_t,y_t)；

第4步，计算组合导航的位置估计：定义t时刻AUV估计位置和分别表示t时刻AUV的估计位置经度和纬度；R_t表示t时刻量测噪声的方差矩阵，是设备属性；h(X_t)表示量测表达式h(X_t)＝H_tX_t+V_t；

第5步，计算组合导航的位置估计方差：定义P_t表示t时刻AUV组合导航的位置估计方差，导航精度的估计；其中，I是单位矩阵。P_t越大表示导航精度越低。

Claims

1.基于长基线和信标在线标定的水下机器人组合导航方法，其特征在于，包括以下步骤：

2.根据权利要求1所述的基于长基线和信标在线标定的水下机器人组合导航方法，其特征在于标定误差通过下式得到

[dx_i,dy_i]^T＝A^-1B

其中，

3.根据权利要求1所述的基于长基线和信标在线标定的水下机器人组合导航方法，其特征在于所述根据标定误差、通过基于长基线声学定位的量测方程和惯性导航的预测方程估计组合导航的位置包括以下步骤：

1)根据标定误差计算t时刻AUV的长基线定位位置Z_t(x_t,y_t)；

2)根据AUV长基线定位位置建立量测方程；

3)建立组合导航的预测方程得到AUV的预测位置X_t(x_t,y_t)；

4.根据权利要求3所述的基于长基线和信标在线标定的水下机器人组合导航方法，其特征在于所述根据标定误差计算t时刻AUV的长基线定位位置Z_t(x_t,y_t)，包括以下步骤：[x_t,y_t]^T＝A^-1(R-D)

其中，

5.根据权利要求3所述的基于长基线和信标在线标定的水下机器人组合导航方法，其特征在于所述量测方程为

6.根据权利要求3所述的基于长基线和信标在线标定的水下机器人组合导航方法，其特征在于所述预测方程为

<mfenced open = "{" close = ""> <mtable> <mtr> <mtd> <mrow> <msub> <mi>X</mi> <mi>t</mi> </msub> <mo>=</mo> <msup> <mrow> <mo>&lsqb;</mo> <msub> <mi>x</mi> <mi>t</mi> </msub> <mo>,</mo> <msub> <mi>y</mi> <mi>t</mi> </msub> <mo>&rsqb;</mo> </mrow> <mi>T</mi> </msup> <mo>=</mo> <mi>f</mi> <mrow> <mo>(</mo> <msub> <mi>X</mi> <mrow> <mi>t</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>,</mo> <msub> <mi>U</mi> <mi>t</mi> </msub> <mo>)</mo> </mrow> <mo>+</mo> <msub> <mi>V</mi> <mi>t</mi> </msub> <mo>=</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <msub> <mi>x</mi> <mrow> <mi>t</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mi>y</mi> <mrow> <mi>t</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> </mtd> </mtr> </mtable> </mfenced> <mo>+</mo> <msubsup> <mi>C</mi> <mi>b</mi> <mi>n</mi> </msubsup> <msub> <mi>&Delta;Tu</mi> <mi>t</mi> </msub> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <mfrac> <mn>1</mn> <mrow> <msub> <mi>R</mi> <mi>e</mi> </msub> <mrow> <mo>(</mo> <mn>1</mn> <mo>+</mo> <mi>e</mi> <mi> </mi> <msup> <mi>sin</mi> <mn>2</mn> </msup> <msub> <mi>y</mi> <mrow> <mi>t</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>)</mo> </mrow> </mrow> </mfrac> </mtd> </mtr> <mtr> <mtd> <mfrac> <mn>1</mn> <mrow> <msub> <mi>R</mi> <mi>e</mi> </msub> <mrow> <mo>(</mo> <mn>1</mn> <mo>-</mo> <mn>2</mn> <mi>e</mi> <mo>+</mo> <mn>3</mn> <mi>e</mi> <mi> </mi> <msup> <mi>sin</mi> <mn>2</mn> </msup> <msub> <mi>y</mi> <mrow> <mi>t</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>)</mo> </mrow> </mrow> </mfrac> </mtd> </mtr> </mtable> </mfenced> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mi>H</mi> <mi>t</mi> </msub> <mo>=</mo> <msup> <mrow> <mo>&lsqb;</mo> <mn>1</mn> <mo>,</mo> <mn>1</mn> <mo>&rsqb;</mo> </mrow> <mi>T</mi> </msup> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mi>P</mi> <mrow> <mi>t</mi> <mo>,</mo> <mi>t</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>=</mo> <msub> <mi>P</mi> <mrow> <mi>t</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>+</mo> <msup> <mrow> <mo>&lsqb;</mo> <mi>&Delta;</mi> <mi>T</mi> <mi> </mi> <mi>c</mi> <mi>o</mi> <mi>s</mi> <mi>&psi;</mi> <mo>,</mo> <mo>-</mo> <mi>&Delta;</mi> <mi>T</mi> <mi> </mi> <mi>s</mi> <mi>i</mi> <mi>n</mi> <mi>&psi;</mi> <mo>&rsqb;</mo> </mrow> <mi>T</mi> </msup> <msub> <mi>Q</mi> <mi>t</mi> </msub> <mo>&lsqb;</mo> <mi>&Delta;</mi> <mi>T</mi> <mi> </mi> <mi>c</mi> <mi>o</mi> <mi>s</mi> <mi>&psi;</mi> <mo>,</mo> <mo>-</mo> <mi>&Delta;</mi> <mi>T</mi> <mi> </mi> <mi>s</mi> <mi>i</mi> <mi>n</mi> <mi>&psi;</mi> <mo>&rsqb;</mo> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msubsup> <mi>C</mi> <mi>b</mi> <mi>n</mi> </msubsup> <mo>=</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <mrow> <mi>sin</mi> <mi>&psi;</mi> <mi>c</mi> <mi>o</mi> <mi>s</mi> <mi>&theta;</mi> </mrow> </mtd> <mtd> <mrow> <mi>cos</mi> <mi>&psi;</mi> <mi>c</mi> <mi>o</mi> <mi>s</mi> <mi>&phi;</mi> <mo>+</mo> <mi>sin</mi> <mi>&phi;</mi> <mi>s</mi> <mi>i</mi> <mi>n</mi> <mi>&theta;</mi> <mi>sin</mi> <mi>&psi;</mi> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <mi>cos</mi> <mi>&psi;</mi> <mi>cos</mi> <mi>&theta;</mi> </mrow> </mtd> <mtd> <mrow> <mo>-</mo> <mi>sin</mi> <mi>&psi;</mi> <mi>c</mi> <mi>o</mi> <mi>s</mi> <mi>&phi;</mi> <mo>+</mo> <mi>cos</mi> <mi>&psi;</mi> <mi>sin</mi> <mi>&theta;</mi> <mi>sin</mi> <mi>&phi;</mi> </mrow> </mtd> </mtr> </mtable> </mfenced> </mrow> </mtd> </mtr> </mtable> </mfenced>

7.根据权利要求3所述的基于长基线和信标在线标定的水下机器人组合导航方法，其特征在于所述根据AUV的长基线定位位置Z_t(x_t,y_t)和预测位置X_t(x_t,y_t)得到组合导航的位置估计通过下式得到

8.根据权利要求1所述的基于长基线和信标在线标定的水下机器人组合导航方法，其特征在于所述估计组合导航的位置后，通过计算组合导航的位置估计方差来估计导航精度：其中，I是单位矩阵，P_t表示t时刻AUV组合导航的位置估计方差。