机械扫描式成像声呐的运动失真补偿方法.pdf

本发明属于一种图像数据处理方法，尤其涉及一种机械扫描式成像声呐的运动失真补偿方法。其目的在于为实现自主导航与定位的水下机器人提供不失真的和实时的环境目标特征。该方法包括以下步骤：成像声呐获取数据、计算机器人坐标及插值、失真补偿、点特征提取、机器人位姿估计系统处理并反馈，其用于以自主导航与定位为目标的水下自主机器人上，保证机器人能精确地感知它在环境中的位置，从而完成对其自身运动姿态的控制，以实现期望的运动轨迹。

1、  一种机械扫描式成像声呐的运动失真补偿方法，其特征在于该方法包括以下步骤：
(1)利用机械扫描式成像声呐获取数据：成像声呐控制系统每发射一次波束，就会返回一Ping回波数据，计算各项有关障碍物目标的参数，声呐串口返回的数据包括声呐此时的各种状态参数和该波束采用的回波强度值，并建立容量为360°扫描范围的数据缓冲区来保存声呐串口送来的每Ping数据；
(2)计算机器人坐标及插值：通过测量传感器设备采集的数据计算出机器人的位移，并以测量传感器与声呐工作频率的关系对测量传感器采集的数据做插值，当声呐每个Ping到来时取出当前时刻测量传感器计算出的机器人位移值；
(3)失真补偿：以每个Ping为单位，加入当前时刻由测量传感器设备计算出的机器人位移值和旋转角度来校正障碍物目标的位置；
(4)点特征提取：以声呐扫描一周为单位取得一幅失真补偿后的目标特征图像，经过特征提取之后的点特征图像送入数据关联部分和核心的导航算法处理部分进行处理；
(5)机器人位姿估计系统处理并反馈：导航与定位算法的核心部分对机器人的位置进行估计和更新，其输出的机器人位姿反馈到失真校正算法之前，作为一次输入对机器人的位置进行修正，形成一个闭合循环过程。

2、  根据权利要求1所述的机械扫描式成像声呐的运动失真补偿方法，其特征在于：所述的测量传感器为多普勒速度计、数字罗盘、陀螺仪。

3、  根据权利要求1所述的机械扫描式成像声呐的运动失真补偿方法，其特征在于所述的点特征提取方法包括以下步骤：
(1)对每个Ping的声呐数据做阙值分割，剔除表示非障碍物和噪声的Bins；
(2)在经过阈值分割的Bins中寻找强度最大的Bin，以它为中心做稀疏化，凡是在它周围一定距离之内的点都被去掉，只保留该点特征和未被去掉的点；
(3)每个Ping的稀疏化如上进行，当扫描完一周之后，对一周之内剩余的点再做稀疏化，簇拥在一个圆周范围之内的点簇只保留强度最大的点，以此类推，形成最精炼和最具代表性的本周的点特征图像。

机械扫描式成像声呐的运动失真补偿方法
技术领域
本发明属于一种图像数据处理方法，尤其涉及一种机械扫描式成像声呐的运动失真补偿方法。
背景技术
自主式水下机器人(Autonomous Underwater Vehicles，简称AUV)要实现真正的自主导航，很大一部分取决于它们根据所经过路线建立精确地图以及自身定位的能力。这是近年来AUV研究领域的焦点问题，基于此人们提出了很多优秀的导航和定位方法，例如知名的“同时定位与地图构建方法”简称SLAM方法。简单的说，导航的成功一般要包括以下四部分：感知，机器人必须能解释它的传感器信息，提取有意义的数据；定位，机器人必须确定它在环境中的位置；认知，机器人必须决定如何行动以达到目标；运动控制，机器人必须调节它的运动姿态，以实现期望的运动轨迹。不论采用哪种导航方法，获取AUV所在环境的背景信息都是不可或缺的一步，它是实现机器人精确定位以及运动控制的前提。常用的采集环境背景信息的传感器有激光扫描仪、照相机和声呐等。但是在水下环境中，使用激光是不可行的；使用照相机也存在着很多困难，其中一项就是受照明条件的限制。声呐用于水下系统中已经有很长一段时间了，尤其是用于AUV上，目前最常用的是低成本的机械扫描式成像声呐。但是由于声呐角度的不确定性，其提供的距离信息要比激光扫描等设备提供的信息更难以解释；而且，自主导航与定位方法需要机器人周围的环境特征作为输入，经处理后更新机器人的位姿，一般以声呐扫描一周获得的点特征为一个处理周期，声呐每发射一次声波获得一次回波数据，当声波遇到障碍物时反映在回波数据中为高幅度值，经过点特征提取算法提取出需要的点特征位置，声呐扫描一周后就获得了一幅机器人周围环境的特征地图，机器人的位置如果静止不动，按照上述思路可画出其周围环境的2D图像，但实际上机器人是时刻运动的，机械扫描式成像声呐的慢速扫描又需要一定的时间，机器人的位置并非固定在同一位置上，再用同样的方法画出一周的声呐图像就会发生失真，用带有失真的声呐图像供导航方法使用，会导致机器人的定位误差积累越来越大，从而减弱了AUV导航与定位的能力。目前，关于如何补偿机械扫描声呐的运动导致的图像失真的方法非常少见。
发明内容
本发明的目的在于为实现自主导航与定位的水下机器人提供不失真的和实时的环境目标特征，提出了一种机械扫描式成像声呐的运动失真补偿方法。
本发明是采用以下的技术方案实现的：一种机械扫描式成像声呐的运动失真补偿方法，其中该方法包括以下步骤：
(1)利用机械扫描式成像声呐获取数据：成像声呐控制系统每发射一次波束，就会返回一Ping回波数据，计算各项有关障碍物目标的参数，声呐串口返回的数据包括声呐此时的各种状态参数和该波束采用的回波强度值，并建立容量为360°扫描范围的数据缓冲区来保存声呐串口送来的每Ping数据；
(2)计算机器人坐标及插值：通过测量传感器设备采集的数据计算出机器人的位移，并以测量传感器与声呐工作频率的关系对测量传感器采集的数据做插值，当声呐每个Ping到来时取出当前时刻测量传感器计算出的机器人位移值；
(3)失真补偿：以每个Ping为单位，加入当前时刻由测量传感器设备计算出的机器人位移值和旋转角度来校正障碍物目标的位置；
(4)点特征提取：以声呐扫描一周为单位取得一幅失真补偿后的目标特征图像，经过特征提取之后的点特征图像送入数据关联部分和核心的导航算法处理部分进行处理；
(5)机器人位姿估计系统处理并反馈：导航与定位算法的核心部分对机器人的位置进行估计和更新，其输出的机器人位姿反馈到失真校正算法之前，作为一次输入对机器人的位置进行修正，形成一个闭合循环过程。
本发明中，所述的点特征提取方法包括以下步骤：
(1)对每个Ping的声呐数据做阙值分割，剔除表示非障碍物和噪声的Bins；
(2)在经过阈值分割的Bins中寻找强度最大的Bin，以它为中心做稀疏化，凡是在它周围一定距离之内的点都被去掉，只保留该点特征和未被去掉的点；
(3)每个Ping的稀疏化如上进行，当扫描完一周之后，对一周之内剩余的点再做稀疏化，簇拥在一个圆周范围之内的点簇只保留强度最大的点，以此类推，形成最精炼和最具代表性的本周的点特征图像。
所述的测量传感器可以采用多普勒速度计(Doppler Velocity Log，简称DVL)、数字罗盘、陀螺仪等。
本发明以机械扫描式成像声呐为主传感器负责采集机器人周围环境的特征信息和创建环境的声学图像；以DVL、数字罗盘、陀螺仪等为主要的测量传感器负责获取机器人运动的相关数据和辅助定位信息；以实时修正点特征位置为主要方法，能够很好地校正由机器人运动导致的声学图像的失真，为机器人导航定位提供更为精确的目标特征。
本发明的有益效果是：本发明实现了一种机械扫描式成像声呐的运动失真补偿方法，用于以自主导航与定位为目标的水下自主机器人上，保证机器人能精确地感知它在环境中的位置，从而完成对其自身运动姿态的控制，以实现期望的运动轨迹。
附图说明
图1为机器人导航系统的工作流程图；
图2为机械扫描式成像声呐失真补偿算法的流程图；
图3为声呐扫描一个Ping的示意图；
图4为声呐串口返回的一个Ping数据格式；
图5为失真补偿方法原理图。
具体实施方式
本发明中，所述的自主导航方法以SLAM为例，其执行过程大体可以分为三个阶段：预测，观测和更新。首先将机器人姿态和点特征存储在一个独立的状态向量中，然后通过先预测再观测的迭代递推过程来估计系统状态，实现对机器人的定位和特征地图的创建。预测阶段实际上执行的是通常意义下的航位推算方法，造成机器人状态估计误差较大；观测阶段获得特征值，如果第一次观测到该特征，将其添加到状态向量中；如果该特征已存储在状态向量中，更新AUV和地图中已有特征的值。本发明所采用的运动失真补偿方法是在获取到一个Ping的回波数据后，根据此时DVL、数字罗盘、陀螺仪等所测的机器人运动状态数据推算出机器人的位移对各个障碍物点的坐标进行修正，再进行点特征提取，当一周扫描完成所有点特征的位置都得到修正。这时画出的一周点特征图像就是不失真的声呐图像，可供给导航算法作为输入使用。图1体现了本失真补偿方法在整个导航系统中的位置。
该运动失真补偿方法主要包括五个步骤，下面结合图2对该方法做进一步说明：
第一步，利用机械扫描式成像声呐获取数据：本发明中，机械扫描式成像声呐1的发射基阵以步进方式旋转，声呐控制系统每发出一个旋转指令，声呐头的波束指向开始顺时针或逆时针转动一个步进角度，以一定的垂直开角和水平开角向探测区域发射一次声脉冲，并在新的方位上暂停片刻，以接收回波数据。由于暂停时间相对于声呐慢速扫描一周所需时间的比例非常小，因此可以忽略暂停时间对声呐扫描带来的影响。当声波遇到障碍物时反映在回波数据中为高幅度值，如图3所示，声波遇到障碍物2时，对应回波位置为高幅度值。接收到回波后，声呐头再旋转同样的角度，继续如上的过程。声呐串口返回的数据中包括声呐此时的各种状态参数和该波束采样的回波强度值，这些强度值离散成一套灰度值Bins。如图4所示，前45个字节为与声呐设置和扫描目标相关的各种状态参数，除去作为结尾标志的最后一个字节的后面所有字节就是这一Ping回波数据中的灰度值Bins，其大小表示是否扫描到了障碍物。虽然声呐可以检测三维物体的存在，却不能在平面上确定它们的位置，因此声呐产生的是水下环境的二维平面图像。导航定位方法一般以获得一周的声呐图像作为一次完整的输入项，因此，建立了容量为360°扫描范围的数据缓冲区来保存声呐串口送来的每Ping数据，等待接下来的失真补偿时使用。
第二步，利用测量传感器设备计算机器人坐标及插值：AUV上常用的测量传感器设备有加速度计、数字罗盘和陀螺仪等；较先进的AUV采用DVL，它可以直接测出AUV相对于海底的3D速度而无需积分，本实施例中采用DVL作为测量传感器。DVL以一定的采集频率f_d测量一次机器人的运动状态，包括当前时间、机器人在三维坐标系中X、Y、Z方向上相对水流的三个速度和相对海底的三个速度，在三维坐标系中X、Y、Z方向的三个旋转角度，水温等等，并根据每次测量得到的速度值乘以时间得到机器人的位移。由于DVL的采集频率f_d和声呐的工作频率f_s不一样，后者的频率高，因此为了保证每个Ping都能够使用由DVL测量并计算出的机器人位移，需要根据两者频率的关系对DVL数据做插值：即每获得一次DVL数据，按照f_s/f_d的关系由速度乘时间补上相应个数的机器人位移值。这样当声呐每个Ping到来时就可以取一次当前时刻的DVL算出的机器人位移值。
由于通过声呐获取的是平面环境信息，所以机器人的旋转角度只需要Z方向的地磁偏角。同样的，地磁偏角可根据两次数据之间的差值做线性平均，取得机器人旋转角度的插值数据，从而也可以保证声呐每Ping数据都有对应的机器人旋转角度数据，供下一步失真补偿使用。
第三步，失真补偿：在某一时刻从数据缓冲区中获得一个Ping，从如图4所示的声呐数据包中取出各个有用的参数进行计算，其中最重要的是当前Ping的角度。接着加入当前时刻由DVL数据计算出的机器人位移值和旋转角度来校正障碍物目标的位置，原理如图5所示，图中上标B表示全局坐标系，S表示机器人即声呐自身的坐标系，下标R表示机器人的坐标，0表示机器人周围环境中目标特征的坐标，θ_i^S为机器人坐标系中声呐扫描的目标物的角度，由声呐设备提供，θ_BS^B为全局坐标系与机器人坐标系的夹角，该值由DVL等设备提供的地磁偏角获得，失真补偿过程如下所述。
在声呐扫描每一周的起始时刻建立全局坐标系，在全局坐标系下机器人的初始位置为(0，0)，声呐扫描一个Ping的时间后机器人运动到了位置(x_R^B，y_R^B)，该坐标由DVL数据计算出的机器人位移值提供；机器人相比初始时刻的旋转角度为θ_BS^B，该角度由DVL数据计算出的机器人旋转角度提供。此时在机器人自身的坐标系中，距离机器人即声呐ρ_i^r的位置(x_O^S，y_O^S)上测到障碍物目标。没有加入失真补偿的方法产生一周的声呐图像，认为在这一周之内机器人一直固定在(0，0)上而且没有角度的转动，再利用此坐标和距离产生的图像。但是实际上机器人的位置和角度都产生了变化，显然利用此坐标和距离画图肯定会失真。有鉴于此，要将机器人的位移和角度变动考虑进去，将机器人坐标系中的障碍物坐标转化为全局坐标系中的坐标，转化公式如下所述：
xOB(i)=xRB-ρir*sin(θiS+θBSB);]]>
yOB(i)=yRB-ρir*cos(θiS+θBSB);]]>
其中i为每个Bin在这一Ping中的位置。以此类推，声呐每扫描一个Ping，将扫描到的障碍物的坐标根据上述公式算出它的失真补偿后的位置。扫描完一周之后，画出这一周的声呐图像，就是不失真的图像。
第四步，点特征提取：加入校正之后的声呐图像含有的障碍物信息十分庞大，而导航定位需要最精确和最有代表性的特征，因此必须要经过特征提取这一步，一般的提取特征包括点特征、线特征、椭圆特征等。以自主导航为目标的AUV在深海中作业时最常见的障碍物目标是点特征，所以本发明使用点特征提取方法，该方法主要采取以下三个步骤：
(1)对每个Ping的声呐数据做阈值分割，剔除表示非障碍物和噪声的Bins；
(2)在经过阈值分割的Bins中寻找强度最大的Bin，以它为中心做稀疏化，凡是在它周围一定距离之内的点都被去掉，只保留该点特征和未被去掉的点；
(3)每个Ping的稀疏化如上进行，当扫描完一周之后，对一周之内剩余的点再做稀疏化，簇拥在一个圆周范围之内的点簇只保留强度最大的点，以此类推，形成最精炼和最具代表性的本周的点特征图像，
经过特征提取之后的点特征图像就可以送入数据关联部分和核心的导航算法部分进行处理了。
第五步，机器人位姿估计系统处理并反馈：导航定位算法的核心部分就是对机器人的位姿进行估计和更新，因此其输出的机器人位姿可以反馈到失真校正算法之前作为一次输入对机器人的位置进行修正，构成一个循环。