1.一种基于双目视觉的水下机器人自主避障方法,其特征在于:包括如下步骤:步骤1:建立水下机器人自主避障系统,水下机器人自主避障系统包括双目摄像机、摄像机矫正模块、视差图处理模块、三维信息处理模块、运动模块和避障控制模块;
摄像机矫正模块用于对双目摄像机进行标定和校准;
步骤2:摄像机矫正模块获取双目摄像机的内部矩阵和外部矩阵,将双目摄像机的内部矩阵和外部矩阵与一组有助于校正镜头畸变的参数一起计算,根据计算结果对双目摄像机进行校正;
内部矩阵中的元素包括水平焦距fx、垂直焦距fy、倾斜参数s、表示主要点偏移的分量x0和分量y0;
外部矩阵中的元素包括旋转矩阵R和平移向量t;
所述一组有助于校正镜头畸变的参数包括径向畸变参数和切向畸变参数;
步骤3:摄像机矫正模块通过代数方法计算校正一对图像所需修正数据,具体包括如下步骤:步骤A1:根据以下公式获得基本矩阵F:
‑T ‑1
F=K′[t]×RK ;
其中:K和K′均为双目摄像机的内部矩阵,R为旋转矩阵,t为平移向量,运算符[t]×表示执行向量交叉积的矩阵;
步骤A2:通过基本矩阵F的奇异值分解SVD,获得基本矩阵F的左零空间和右零空间,其公式如下:T
F=UDV;
其中U和V是两个正交矩阵,D是一个对角矩阵,定义D的第i对角元为F的第i奇异值,U的第i列和V的第i列分别为基本矩阵F的左奇异向量和右奇异向量;
基本矩阵F的右零空间为V的零奇异值列向量;
T
基本矩阵F的左零空间为奇异值等于零的U行向量;
步骤A3:对于下面的校正步骤,需要找到投影变换H′,该变换将外极e′映射到无穷远T(1,0,0) 处的点;然后计算匹配投影变换H,它将剩余的外极e映射到无穷远,即最小距离:其中,H是投影变换,xi是图像像素点坐标,i是点数;
图像本身的校正通过根据各自的投影变换H和H′对两个图像重新采样来执行;
步骤4:视差图处理模块根据SAD块匹配法和半全局块匹配法,计算双目摄像机拍摄的视差图,通过校正图像的矩阵运算,表示物体三维信息;
步骤5:三维信息处理模块根据双目摄像机的内部矩阵和外部矩阵,计算出视差图中所有具有已知视差的像素对应的三维坐标;
步骤6:双目摄像机所拍摄的连续图像帧经过步骤3到步骤5的方法处理后生成预处理连续图像帧,并发送给运动模块,运动模块对预处理连续图像帧进行特征跟踪,利用三维坐标来估计机器人的运动;
步骤7:在避障控制模块中预先设定多个行为控制策略;
避障控制模块根据步骤6的方法获得到双目摄像机所拍摄到的障碍物图像,并根据障碍物图像选择一个行为控制策略作为避障策略。
2.如权利要求1所述的一种基于双目视觉的水下机器人自主避障方法,其特征在于:在执行步骤2时,参数k1、k2和k3的径向畸变参数用以下公式描述
2 4 6
xr=x(1+k1r+k2r+k3r)
2 4 6
yr=y(1+k1r+k2r+k3r)
2 2 2
r=x+y;
径向畸变参数是由标准透镜产生的;
其中x表示世界坐标系坐标x、y表示世界坐标系坐标y、xr和yr表示径向畸变参数;
切向畸变参数用以下公式描述:
2 2 2 2
xt=x+[2p1xy+p2((x+y) +2x)]
2 2 2 2
yt=y+[2p2xy+p1((x+y) +2x)];
其中:x表示世界坐标系坐标x、y表示世界坐标系坐标y、参数p1和p2在透镜未与传感器平行安装的情况下出现,xt和yt表示切向畸变参数。
3.如权利要求1所述的一种基于双目视觉的水下机器人自主避障方法,其特征在于:在执行步骤4时,具体包括如下步骤:步骤B1:给定一对由像素A和像素B组成的正方形块;
步骤B2:根据一下公式计算SAD块匹配法的相似性度量:其中,A,B表示2类图像目标,像素A和B组成的正方形块,n表示图像像素点数量、i表示行、j表示列,a表示A类图像的灰度值,b表示B类图像的灰度值;
步骤B3:半全局块匹配法利用基于熵的匹配代价获得视差,具体为将以下能量函数最小化:其中,第一项为D的差异的所有像素匹配代价之和,第二项为p的邻域Np中的所有像素q增加一个常数惩罚P1,其差异变化最大为1像素;对于所有较大的差异变化,第三项增加了较大的常数惩罚P2。
4.如权利要求1所述的一种基于双目视觉的水下机器人自主避障方法,其特征在于:在执行步骤5时,具体包括如下步骤:步骤C1:根据以下公式计算深度坐标,即z坐标:其中f是焦距,b是基线,(xlp‑xrp)是视差,即p点的左像素和右像素之间的对应差;
步骤C2:根据以下公式计算横坐标,即x坐标:其中,xp为世界坐标系x轴数据;
步骤C3:根据以下公式计算横坐标,即y坐标:其中,yp为世界坐标系y轴数据;
步骤C4:根据步骤C1到步骤C3的方法,对视差图中所有具有已知视差的像素进行三维坐标计算,得到视差图中所有具有已知视差的像素的三维点云;
步骤C5:根据以下统计过滤器对三维点云进行过滤:Pd={pi∈Praw|||pi‑pj||>μ+dthreshσ};
其中,其中Pd是去噪声点云,μ和σ是最近邻距离的平均值和标准偏差,Praw表示原始点云数据集合,dthresh表示滤波阈值,i、j表示不同的点云数据;
步骤C6:通过点聚类来过滤噪声,去除点数较少或非结构分布的簇。
5.如权利要求1所述的一种基于双目视觉的水下机器人自主避障方法,其特征在于:在执行步骤6时,具体步骤如下:步骤D1:首先从两幅连续图像中提取特征,设定在时刻T0时,所述运动模块获取到的预处理图像帧为(prevL,prevR)图像对,在时刻T1时,所述运动模块获取到的预处理图像帧为(currL,currR)图像对,时刻T0与时刻T1时之间的关系为:T0=T1‑ΔT,其中ΔT为一个时间上的常数;
步骤D2:利用旋转和尺度不变的SURF描述子分别检测(prevL,prevR)图像对和(currL,currR)图像对中的特征,使用从SURF中提取的特征描述符对(prevL,prevR)图像对和(currL,currR)图像对在空间和时间上进行匹配;
步骤D3:利用平方差和SSD法比较(prevL,prevR)图像对和(currL,currR)图像对的特征的范数,估计运动轨迹。