CN103744085B - 水下机器人五分量测距声纳斜井三维成像系统及成像方法 - Google Patents

Abstract

本发明涉及一种水下机器人五分量测距声纳斜井三维成像系统及成像方法，包括水下机器人、水面光端机、水面上位机，水下机器人上设有4个水平分量测距声纳和1个垂直分量测距声纳，其中4个水平分量测距声纳的相互之间方位差为90度；水下机器人上还设有光纤陀螺仪。通过4个水平分量测距声纳可获得斜井的一帧二维图像序列，水面上位机对一系列二维图像序列进行数据处理，通过对斜井的几何尺寸和4个水平分量测距声纳数据进行处理，使二维图像序列垂直轴向上对齐，通过光纤陀螺仪测得的艏向角度使二维图像序列水平方向对齐，再结合垂直分量测距声纳测得的机器人到水面垂直距离，实时绘制出斜井的二维和三维图像。

Description

水下机器人五分量测距声纳斜井三维成像系统及成像方法

技术领域

本发明涉及一种水下机器人五分量测距声纳斜井三维成像系统及成像方法。

背景技术

近年来，声纳成像在水声领域受到广泛关注和发展，利用三维成像声纳在水质浑浊和光照条件较差的条件下仍可获取水下目标的外形轮廓的细节信息和环境信息。三维成像声纳是一种高分辨率的图像声纳，能够提供非常优秀的图像质量。它以波束形成或声透镜技术为基础，目前主要有两种技术路线：一是采用一维线阵，通过机械平移合成二维面阵；第二种是直接采用二维面阵，在水平、垂直、距离3个方向上直接获得分辨率。把图像声纳装载到各种水下智能机器人（AutomaticUnderwaterVehicle，AUV）、遥控水下机器人（ROV）和水下无人潜器(UUV)上进行水下作业,可对水下堤坝、管道、桥墩和海港等进行探测。借助计算机可视化技术，将探测结果以生动的三维图形图像的方式表现出来，通过缩放、移动、转动和测距等操作从各种视角进行观察和分析，提高水下探测的效率。

水利蓄能电站斜井内管壁探测对研制新型成像声纳和三维成像方法提出了需求，因为对水利蓄能电站斜井探测来说，一方面目前的图形声纳是一维或二维的传感器阵，需要非常多的水声传感器单元，对安装精度的要求很高，硬件制作成本和软件计算成本也都很高；另一方面，因为斜井本身是个圆柱形，水平截面是个椭圆形，传统的三维成像声纳很难获得水平全向的斜井三维图像，必须采用技术手段进行拼接，实现起来较为困难，且实际效果很难保证。另外，水下机器人依靠螺旋桨推进器控制其在水中运动，由于其非线性水动力学特性、水流的随机干扰和差分反馈控制机制等因素的影响，在下潜和水平旋转过程中很难使它保持一致的相对位置。与机器人刚性连接的声纳系统受此影响，采集的二维目标图像切片不能在垂直轴向和水平方向对齐，导致三维绘制扭曲失真。

发明内容

本发明目的在于提供一种水下机器人五分量测距声纳斜井三维成像系统及成像方法，能够实现全面探测，并且能够有效避免探测图像失真。

基于同一发明构思，本发明具有两个独立的技术方案：

1.一种水下机器人五分量测距声纳斜井三维成像系统，包括水下机器人、水面光端机、水面上位机，水下机器人与水面光端机通信连接，水面光端机与水面上位机通信连接，其特征在于：水下机器人上设有4个水平分量测距声纳和1个垂直分量测距声纳，其中4个水平分量测距声纳的相互之间方位差为90度；水下机器人上还设有光纤陀螺仪。

水下机器人上还设有深度计。

水下机器人上设有串口服务器，串口服务器可以是以太网交换机，用于上传水下机器人运动状态、光纤陀螺仪、深度计和五分量测距声纳的数据。

水面光端机可以为快速光纤以太网卡，实现水面上位机和水下机器人通信连接。

水面上位机可以是工业笔记本或台式电脑，负责向下发送命令，接收水下机器人的数据并进行处理，实时绘制二维和三维斜井图像。

光纤陀螺仪可以是数字光纤陀螺，可测量相对机器人初始艏向的相对角度和积分角度。

2.一种利用上述水下机器人五分量测距声纳斜井三维成像系统的成像方法，其特征在于：水下机器人的运动步骤为，

步骤1：初始化机器人位置和校正艏向零度；

步骤2：机器人保持艏向下潜动作，下潜一步长；

步骤3：机器人原地逆时针旋转90度进行扫描动作，利用测距声纳扫描测距；

步骤4：机器人保持艏向下潜动作，再下潜一步长；

步骤5：机器人原地顺时针旋转90度进行扫描动作，利用测距声纳扫描测距；

步骤6：重复步骤2—步骤5，直至机器人下潜结束；

上述步骤3和步骤5中，通过4个水平分量测距声纳可获得斜井的一帧二维图像序列，水面上位机对一系列二维图像序列进行数据处理，通过对斜井的几何尺寸和4个水平分量测距声纳数据进行处理，使二维图像序列垂直轴向上对齐，通过光纤陀螺仪测得的艏向角度使二维图像序列水平方向对齐，再结合垂直分量测距声纳测得的机器人到水面垂直距离，实时绘制出斜井的二维和三维图像。

通过对斜井的几何尺寸和4个水平分量测距声纳数据进行处理，使二维图像序列垂直轴向上对齐，具体通过如下方法实现：

通过4个水平分量测距声纳，同一时刻可获得水下机器人前后左右4个目标距离信息，分别用a,c,b,d表示，对应的斜井内壁的四个顶点A(x₁,y₁)、B(x₂,y₂)、C(x₃,y₃)、D(x₄,y₄)，O(x₀,y₀)为斜井水平椭圆截面中心，x₁,y₁、x₂,y₂、x₃,y₃、x₄,y₄，x₀,y₀为相应的坐标。

根据4个水平分量测距声纳的距离信息和斜井的几何尺寸，计算斜井水平椭圆截面相对机器人位置的坐标,计算公式如下：

$\left\{\begin{array}{c}{x}_0=\frac{\begin{array}{c}{-m}^2({-y}_1^2{y}_3+y_1^2{y}_4+y_1{y}_3^2-y_1{y}_4^2+y_2^2{y}_3-y_2^2{y}_4-y_2{y}_3^2+y_2{y}_4^2)+\\ {-n}^2({-x}_1^2{y}_3+x_1^2{y}_4+x_2^2{y}_3-x_2^2{y}_4+x_3^2{y}_1-x_3^2{y}_2-x_4^2{y}_1+x_4^2{y}_2)\end{array}}{\begin{array}{c}{2n}^2(x_1{y}_3-x_3{y}_1-x_1{y}_4-x_{21}{y}_3+x_3{y}_2+x_4{y}_1+x_2{y}_4-x_4{y}_2)\\ m^2(x_1{y}_3^2-x_1{y}_4^2-x_2{y}_3^2-x_2{y}_4^2+x_3{y}_1^2+x_3{y}_2^2+x_4{y}_1^2\underset{}{-x_4{y}_2^2})+\end{array}}\\ y_0=\frac{n^2(x_1{x}_3^2+x_1{x}_4^2+x_2^2{x}_3+x_2^2{x}_4-x_2{x}_3^2+x_2{x}_4^2)}{{2m}^2(x_1{y}_3-x_3{y}_1-x_1{y}_4-x_2{y}_3-x_3{y}_2+x_4{y}_1+x_2{y}_4-x_4{y}_2)}\end{array}\right.$

其中m,n分别是椭圆的焦距，可根据斜井直径和倾斜角度计算出来。

根据坐标变换，计算斜井内壁对应点相对椭圆中心的坐标，使所有的数据自动在垂直轴向上对齐，计算公式如下：

$\left\{\begin{array}{c}{x}_1^{\′}=x_1-x_0,y_1^{\′}=y_1-y_0\\ x_2^{\′}=x_2-x_0,y_2^{\′}=y_2-y_0\\ x_3^{\′}=x_3-x_0,y_3^{\′}=y_3-y_0\\ x_4^{\′}=x_4-x_0,y_4^{\′}=y_4-y_0\end{array}\right.$

通过光纤陀螺仪测得的艏向角度使二维图像序列水平方向对齐，具体通过如下方法实现：

针对光纤陀螺测得的艏向角，通过向下取整或者曲线拟合的方法获得整数角度值，相同艏向值上下对正，实现二维图像序列水平角度的对齐。

机器人下潜的一步长为一米。

本发明具有的有益效果：

本发明通过机器人下潜和旋转运动，利用五分量测距声纳对目标斜井进行扫描，可以获得目标斜井的二维切片序列并进行全向三维成像，克服传统的图像声纳不能进行全向探测的不足；本发明利用斜井的几何信息、水平四分量测距声纳获得的目标距离信息和光纤陀螺仪的相对航向角信息，不需要参考图像和原始图像，就可以消除由机器人运动偏差导致的图像序列在水平和轴向偏移，避免三维成像的扭曲失真；本发明因只需五个分量的测距声纳，容易安装校准，降低了硬件成本，同时降低了数据量，克服了传统图像声纳传感器数目多和计算量大的缺点。

附图说明

图1为本发明系统的组成框图；

图2为本发明水下机器人运动流程图；

图3为本发明数据处理和成像算法流程图；

图4为水平四分量探测俯视坐标图；

图5为水利蓄能电站斜井剖面图。

具体实施方式

如图5所示，实施例中的水利蓄能电站斜井为高压引水斜井，它是圆柱形并与水平成固定角度，水平截面是一个椭圆形，斜井直径7米和倾角58度。

如图1所示，本发明系统包括水面上位机1，水面光端机2和水下机器人3。水下机器人上设有4个水平分量测距声纳和1个垂直分量测距声纳，即五分量测距声纳34，其中4个水平分量测距声纳的相互之间方位差为90度；水下机器人上还设有光纤陀螺仪33、深度计32、发射机控制板35，电源控制板36，推进器控制板37，其中光纤陀螺仪是数字光纤陀螺，可测量相对机器人初始艏向的相对角度和积分角度。水面上位机和水面光端机之间通过光缆连接，水面光端机2和水下机器人3内部装载的串口服务器31通过以太IP网络相连，用于上传水下机器人运动状态、光纤陀螺仪、深度计和五分量测距声纳的数据。水面光端机可以为快速光纤以太网卡，实现水面上位机和水下机器人通信连接。水面上位机可以是工业笔记本或台式电脑，负责向下发送命令，接收水下机器人的数据并进行处理，实时绘制二维和三维斜井图像。

水面控制机1从底层的基础运动控制系统获得机器人的运动状态，通过光纤将控制指令传给基础运动控制系统,通过D/A卡向水下机器人的推进器发送模拟电压信号，通过驱动六个推进器，控制水下机器人进行平移、下潜和旋转等动作。通过A/D采集卡获得光纤陀螺33、深度计32和五分量测距声纳34的数据信息，并通过串口服务器31和水面光端机2上传到水面上位机1，为斜井目标探测和成像提供数据，水面上位机1负责处理和显示上传得数据。

如图2所示，水下机器人可手控，也可程控，运动步骤如下：

（1）水下机器人放入水中后，启动控制系统，校准艏向零位，使艏向与斜井延伸方向相同；

（2）控制系统根据深度计和五分量声纳测得的机器人与直航段距离值控制机器人垂直下潜，当该距离值为2米时，机器人停止垂直下潜运动；

（3）控制系统根据激光陀螺航向指示，进行水平直航运动，当五分量声纳测得前向距离为3米时，停止前向运送，准备斜井测试运动过程；

（4）根据深度计控制机器人下潜1米，同时根据激光陀螺航向指示保持航向不变；

（5）根据激光陀螺航向指示原地左转90°至+90°（以便实时绘制斜井内壁二维椭圆平面图和三维椭圆线图）；

（6）根据深度计控制机器人下潜1米，同时根据激光陀螺航向指示保持航向不变；

（7）根据激光陀螺航向指示原地右转90°至0°；（以便实时绘制斜井内壁二维椭圆平面图和三维椭圆线图）；

（8）重复（4）至（7），直至深度达到800米；

（9）机器人按原路返回。

上述步骤（5）和（7）中，通过4个水平分量测距声纳可获得斜井的一帧二维图像序列，重复步骤（4）至（7）可获得一系列二维图像系列，五分量测距声纳数据和光纤陀螺仪的艏向角度数据通过以太网和水面光端机上传到水面上位机1进行处理，使二维图像序列在水平和垂直轴向上进行对齐，并实时绘制二维和三维斜井图像。

如图3所示，水面上位机数据处理和斜井内壁成像算法流程主要包括控制进程、二维绘制进程、三维绘制进程、数据处理进程和数据接收进程，其中数据每500ms上传一次。

参考图4所示，以机器人所在位置为圆点建立坐标系，水平四分量测距声纳的相邻两分量互相垂直，同一时刻可获得前后左右四个目标距离信息，分别用a,c,b,d表示，对应的斜井内壁的四个顶点A(x₁,y₁)、B(x₂,y₂)、C(x₃,y₃)、D(x₄,y₄)相对机器人中心的坐标分别为：

$\left\{\begin{array}{cc}{x}_1=a\cos \mathrm{\θ}& y_1=a\sin \mathrm{\θ}\\ x_2=b\sin \mathrm{\θ}& y_2=-b\cos \mathrm{\θ}\\ x_3=-c\cos \mathrm{\θ}& y_3=-c\sin \mathrm{\θ}\\ x_4=-d\sin \mathrm{\θ}& y_4=d\cos \mathrm{\θ}\end{array}\right.---\left(1\right)$

A(x₁,y₁)、B(x₂,y₂)、C(x₃,y₃)、D(x₄,y₄)四个顶点在椭圆上，因此满足式（2）的方程：

$\left\{\begin{array}{c}\frac{{(x_1-x_0)}^2}{m^2}+\frac{{(y_1-y_0)}^2}{n^2}=\frac{{(x_2-x_0)}^2}{m^2}+\frac{{(y_2-y_0)}^2}{n^2}\\ \frac{{(x_3-x_0)}^2}{m^2}+\frac{{(y_3-y_0)}^2}{n^2}=\frac{{(x_4-x_0)}^2}{m^2}+\frac{{(y_4-y_0)}^2}{n^2}\end{array}\right.---\left(2\right)$

其中m,n分别是椭圆的焦距，可根据斜井直径和倾斜角度计算出来。O(x₀,y₀)为斜井水平椭圆截面的中心，根据式(2)计算得出，即：

$\left\{\begin{array}{c}{x}_0=\frac{\begin{array}{c}{-m}^2({-y}_1^2{y}_3+y_1^2{y}_4+y_1{y}_3^2-y_1{y}_4^2+y_2^2{y}_3-y_2^2{y}_4-y_2{y}_3^2+y_2{y}_4^2)+\\ {-n}^2({-x}_1^2{y}_3+x_1^2{y}_4+x_2^2{y}_3-x_2^2{y}_4+x_3^2{y}_1-x_3^2{y}_2-x_4^2{y}_1+x_4^2{y}_2)\end{array}}{\begin{array}{c}{2n}^2(x_1{y}_3-x_3{y}_1-x_1{y}_4-x_{21}{y}_3+x_3{y}_2+x_4{y}_1+x_2{y}_4-x_4{y}_2)\\ m^2(x_1{y}_3^2-x_1{y}_4^2-x_2{y}_3^2-x_2{y}_4^2+x_3{y}_1^2+x_3{y}_2^2+x_4{y}_1^2\underset{}{-x_4{y}_2^2})+\end{array}}\\ y_0=\frac{n^2(x_1{x}_3^2+x_1{x}_4^2+x_2^2{x}_3+x_2^2{x}_4-x_2{x}_3^2+x_2{x}_4^2)}{{2m}^2(x_1{y}_3-x_3{y}_1-x_1{y}_4-x_2{y}_3-x_3{y}_2+x_4{y}_1+x_2{y}_4-x_4{y}_2)}\end{array}\right.---\left(\text{3}\right)$

把式（1）带入式（3），就可计算出椭圆圆点O(x₀,y₀)的坐标，

将坐标原点平移到O(x₀,y₀)点，以便不同层的二维斜井切片图像在垂直轴向上对齐，即把A(x₁,y₁)、B(x₂,y₂)、C(x₃,y₃)、D(x₄,y₄)四点的坐标变为：

$\left\{\begin{array}{c}{x}_1^{\′}=x_1-x_0,y_1^{\′}=y_1-y_0\\ x_2^{\′}=x_2-x_0,y_2^{\′}=y_2-y_0\\ x_3^{\′}=x_3-x_0,y_3^{\′}=y_3-y_0\\ x_4^{\′}=x_4-x_0,y_4^{\′}=y_4-y_0\end{array}\right.---\left(4\right)$

针对光纤陀螺测得的艏向角，通过向下取整或者曲线拟合的方法获得整数角度值，相同艏向值上下对正，实现二维图像序列水平角度的对齐。

用θ表示光纤陀螺艏向角，h(i,j)表示高度分量，用数组f_x(i,j),b_x(i,j),l_x(i,j),r_x(i,j),f_y(i,j),b_y(i,j),l_y(i,j),r_y(i,j)，其中i=0…800,j=0…89,分别用来存储每层扫描的斜井内壁相对于椭圆圆点的坐标,用p表示当前扫描层。

如图3所示，通过水面上位机指令控制水下机器人做下潜和旋转的的动作。做下潜动作时，令Flag=1,i=i+1。做旋转动作时Flag=0，在当前下潜位置，根据光纤陀螺的艏向角控制机器人旋转90度。例如当前层p=i层，在某一时刻，令在该时刻根据式（4）计算斜井内壁的坐标，并用该坐标更新坐标数组，即：

$\left\{\begin{array}{c}{f}_x(i,j)=x_1^{\′},f_y(i,j)=y_1^{\′}\\ l_x(i,j)=x_2^{\′},l_y(i,j)=y_2^{\′}\\ b_x(i,j)=x_3^{\′},b_y(i,j)=y_3^{\′}\\ r_x(i,j)=x_4^{\′},r_y(i,j)=y_4^{\′}\end{array}\right.---\left(5\right)$

如果机器人在第一层扫描，即p=0，只绘制斜井的二维切片图像，通过重绘实时显示当前层已扫描的斜井内壁；如果p>0时，除了实时绘制二维图像以外，还要利用OPENGL等可视化工具，对斜井内壁进行实时的三维绘制。令Δh(i,j)=h(i,j)–h(i-1,j)，从第1层开始，重绘所有相邻两层对应角度的4个相邻顶点的四边形面片，直到当前位置为止，实现斜井的实时三维绘制。绘制时可进行滤波、光照和插值等手段进行渲染来获得比更好的效果。

Claims (3)

1.一种利用水下机器人五分量测距声纳斜井三维成像系统的成像方法，所述水下机器人五分量测距声纳斜井三维成像系统包括水下机器人、水面光端机、水面上位机，水下机器人与水面光端机通信连接，水面光端机与水面上位机通信连接，水下机器人上设有4个水平分量测距声纳和1个垂直分量测距声纳，其中4个水平分量测距声纳相互之间方位差为90度；水下机器人上还设有光纤陀螺仪，其特征在于：水下机器人的运动步骤为：

步骤1：初始化机器人位置和校正艏向零度；

步骤2：机器人保持艏向下潜动作，下潜一步长；

步骤3：机器人原地逆时针旋转90度进行扫描动作，利用测距声纳扫描测距；

步骤4：机器人保持艏向下潜动作，再下潜一步长；

步骤5：机器人原地顺时针旋转90度进行扫描动作，利用测距声纳扫描测距；

步骤6：重复步骤2—步骤5，直至机器人下潜结束；

上述步骤3和步骤5中，通过4个水平分量测距声纳均可获得斜井的一帧二维图像序列，水面上位机对一系列二维图像序列进行数据处理，通过对斜井的几何尺寸和4个水平分量测距声纳数据进行处理，使二维图像序列垂直轴向上对齐，通过光纤陀螺仪测得的艏向角度使二维图像序列水平方向对齐，再结合垂直分量测距声纳测得的机器人到水面垂直距离，实时绘制出斜井的二维和三维图像；

通过对斜井的几何尺寸和4个水平分量测距声纳数据进行处理，使二维图像序列垂直轴向上对齐，具体通过如下方法实现：

通过4个水平分量测距声纳，同一时刻获得水下机器人前后左右4个目标距离信息，分别用a,c,b,d表示，对应的斜井内壁的四个顶点A(x₁,y₁)、B(x₂,y₂)、C(x₃,y₃)、D(x₄,y₄)，O(x₀,y₀)为斜井水平椭圆截面中心，x₁,y₁、x₂,y₂、x₃,y₃、x₄,y₄，x₀,y₀为相应的坐标，

根据4个水平分量测距声纳的距离信息和斜井的几何尺寸，计算斜井水平椭圆截面相对机器人位置的坐标，计算公式如下：

$\left\{\begin{array}{c}{x}_0=\frac{\begin{array}{c}-m^2(-y_1^2{y}_3+y_1^2{y}_4+y_1{y}_3^2-y_1{y}_4^2+y_2^2{y}_3-y_2^2{y}_4-y_2{y}_3^2+y_2{y}_4^2)+\\ -n^2(-x_1^2{y}_3+x_1^2{y}_4-x_2^2{y}_3-x_2^2{y}_4+x_3^2{y}_1-x_3^2{y}_2-x_4^2{y}_1+x_4^2{y}_2)\end{array}}{2n^2(x_1{y}_3-x_3{y}_1-x_1{y}_4-x_{21}{y}_3+x_3{y}_2+x_4{y}_1+x_2{y}_4-x_4{y}_2)}\\ y_0=\frac{\begin{array}{c}{m}^2(x_1{y}_3^2-x_1{y}_4^2-x_2{y}_3^2+x_2{y}_4^2-x_3{y}_1^2+x_3{y}_2^2+x_4{y}_1^2-x_4{y}_2^2)+\\ n^2(x_1{x}_3^2+x_1{x}_4^2+x_2^2{x}_3+x_2^2{x}_4-x_2{x}_3^2+x_2{x}_4^2)\end{array}}{2m^2(x_1{y}_3-x_3{y}_1-x_1{y}_4-x_2{y}_3+x_3{y}_2+x_4{y}_1+x_2{y}_4-x_4{y}_2)}\end{array}\right.$

其中m,n分别是椭圆的焦距，根据斜井直径和倾斜角度计算出来，

根据坐标变换，计算斜井内壁对应点相对椭圆中心的坐标，使所有的数据自动在垂直轴向上对齐，计算公式如下：

$\left\{\begin{array}{cc}{x}_1^{\′}=x_1-x_0,& y_1^{\′}=y_1-y_0\\ x_2^{\′}=x_2-x_0,& y_2^{\′}=y_2-y_0\\ x_3^{\′}=x_3-x_0,& y_3^{\′}=y_3-y_0\\ x_4^{\′}=x_4-x_0,& y_4^{\′}=y_4-y_0\end{array}\right..$

2.根据权利要求1所述的利用水下机器人五分量测距声纳斜井三维成像系统的成像方法，其特征在于：通过光纤陀螺仪测得的艏向角度使二维图像序列水平方向对齐，具体通过如下方法实现：

针对光纤陀螺测得的艏向角，通过向下取整或者曲线拟合的方法获得整数角度值，相同艏向值上下对正，实现二维图像序列水平角度的对齐。

3.根据权利要求2所述的利用水下机器人五分量测距声纳斜井三维成像系统的成像方法，其特征在于：机器人下潜的一步长为一米。