1.一种基于视觉图像和激光雷达的水下机器人导航避障方法,其特征在于,所述方法包括:步骤S1、在水下工作区域使用激光雷达数据创建3D栅格地图;
步骤S2、向水下机器人的导航模块发送导航目标点;
步骤S3、水下机器人的导航模块根据目标点与当前位置,规划由当前位置到目标点的行走路径;
步骤S4、水下机器人沿规划出的路径运动,导航模块根据视觉图像和激光雷达数据,计算水下机器人运动过程中与障碍物的相对速度,从而根据速度冲突检测方法实时计算水下机器人下一时刻的最优运动速度;
步骤S5、根据最优运动速度控制水下机器人移动,重复执行步骤S4,直到水下机器人到达目标点。
2.根据权利要求1所述的一种基于视觉图像和激光雷达的水下机器人导航避障方法,其特征在于,其中,步骤S4中的导航模块根据视觉图像和激光雷达数据,计算水下机器人运动过程中与障碍物的相对速度具体为:步骤S401、通过图像数据识障碍物,取第一张视觉图像数据,并通过深度信息确定障碍物在地图坐标系下的中心位置坐标,同时与激光数据匹配,提取图像计算出的障碍物中心位置附近的点作为一组障碍物点集,最终得到nlaser1组障碍物点集;
步骤S402、取下一张视觉图像数据,按照步骤S401得到nlaser2组点集;
步骤S403:定义nlaser1中点集为pts1i,其中0
0
步骤S404:针对同一障碍物,设其在第一张视觉图像数据下的中心位置为p1(x1,y1),在第二张视觉图像数据下的中心位置为p2(x2,y2),两张视觉图像数据的时间间隔为dt,则计算出障碍物的移动速度向量Vo=((x2-x1)/dt,(y2-y1)/dt);
步骤S405:从里程计上报的信息中获取水下机器人当前速度向量Vr,则水下机器人与障碍物的相对速度Vro=Vo-Vr。
3.根据权利要求1所述的一种基于视觉图像和激光雷达的水下机器人导航避障方法,其特征在于,其中,步骤S4中的实时计算水下机器人下一时刻的运动速度具体为:步骤S406:按照dwa方法,计算本时刻速度空间下的速度样本;
步骤S407:对每个速度样本,计算dwa得分后,进行速度冲突检测,若检测结果为冲突,则将该样本的得分设置为0;否则,比较该样本得分与最优样本的得分,若该样本得分比最优样本高,则将该样本设为最优样本;
步骤S408:按照步骤S407计算每个速度样本,取最优样本速度值作为水下机器人下一时刻的速度。
4.根据权利要求3所述的一种基于视觉图像和激光雷达的水下机器人导航避障方法,其特征在于,其中,步骤S407中的速度冲突检测方法具体为:步骤S4071:设障碍物点集为pts2k,其中与水下机器人位置Pr(xr,yr)最近的一点为P(x,y),过点P作Vo方向的直线LVo;
步骤S4072:将LVo沿Pr到P的垂线方向向Pr平移,平移距离为水下机器人半径r,得到直线LVo’;
步骤S4073:设冲突检测的速度样本V,样本模拟时间为Δt,则模拟最终位置P’=Pr+V*Δt;
步骤S4074:判断Pr与P’与直线LVo’的关系,若Pr与P’在直线LVo’同侧,则检测结果为不冲突;反之,检测结果为冲突。