[发明专利]水下机器人五分量测距声纳斜井三维成像系统及成像方法

权利要求书

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

2.根据权利要求1所述的水下机器人五分量测距声纳斜井三维成像系统，其特征在于：水下机器人上还设有深度计。 

3.根据权利要求2所述的水下机器人五分量测距声纳斜井三维成像系统，其特征在于：水下机器人上设有串口服务器，串口服务器可以是以太网交换机，用于上传水下机器人运动状态、光纤陀螺仪、深度计和五分量测距声纳的数据。 

4.根据权利要求3所述的水下机器人五分量测距声纳斜井三维成像系统，其特征在于：水面光端机可以为快速光纤以太网卡，实现水面上位机和水下机器人通信连接。 

5.根据权利要求4所述的水下机器人五分量测距声纳斜井三维成像系统，其特征在于：水面上位机可以是工业笔记本或台式电脑，负责向下发送命令，接收水下机器人的数据并进行处理，实时绘制二维和三维斜井图像。 

6.根据权利要求5所述的水下机器人五分量测距声纳斜井三维成像系统，其特征在于：光纤陀螺仪可以是数字光纤陀螺，可测量相对机器人初始艏向的相对角度和积分角度。 

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

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

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

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

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

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

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

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

8.根据权利要求7所述的方法，其特征在于：通过对斜井的几何尺寸和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个水平分量测距声纳的距离信息和斜井的几何尺寸，计算斜井水平椭圆截面相对机器人位置的坐标，计算公式如下： 

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

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

。

9.根据权利要求8所述的方法，其特征在于：通过光纤陀螺仪测得的艏向角度使二维图像序列水平方向对齐，具体通过如下方法实现： 

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

10.根据权利要求9所述的方法，其特征在于：机器人下潜的一步长为一米。