[发明专利]一种基于激光雷达的水下机器人智能运动控制方法

权利要求书

1.一种基于激光雷达的水下机器人智能运动控制方法，其特征在于，所述方法包括：

步骤S1、在水下工作区域使用激光雷达数据创建3D栅格地图；

步骤S2、向水下机器人的导航模块发送导航目标点；

步骤S3、水下机器人的导航模块根据目标点与当前位置，规划由当前位置到目标点的行走路径；

步骤S4、水下机器人沿规划出的路径运动，导航模块根据激光雷达数据，计算水下机器人运动过程中与障碍物的相对速度，从而根据速度冲突检测方法实时计算水下机器人下一时刻的最优运动速度；

步骤S5、根据最优运动速度控制水下机器人移动，重复执行步骤S4，直到水下机器人到达目标点；

其中，所述步骤S4中的导航模块根据激光雷达数据，计算水下机器人运动过程中与障碍物的相对速度具体为：

步骤S401、取一帧激光数据进行聚类，得出的每一个点集认为是一个障碍物，过滤掉5m以外的点集，同时过滤掉地图上已有障碍的点集，最终得到n_laser1组障碍物点集；

步骤S402、取后一帧激光数据，按照步骤S401得到n_laser2组点集；

步骤S403：定义n_laser1中点集为pts_1i，其中0i＝n_laser1，定义n_laser2中点集为Pts_2j，其中0j＝n_laser2；将n_laser1中各点集中心位置与n_laser2中各点集中心位置依次比较，距离小于阈值e1的认为是两帧激光数据下同一障碍物；

步骤S404：针对同一障碍物，设其在第一帧激光数据下的中心位置为p1(x1,y1)，在第二帧激光数据下的中心位置为p2(x2,y2)，两帧激光数据的时间间隔为dt，则计算出障碍物的移动速度向量Vo＝((x2-x1)/dt,(y2-y1)/dt)；

步骤S405：从里程计上报的信息中获取水下机器人当前速度向量Vr，则水下机器人与障碍物的相对速度Vro＝Vo-Vr；

其中，步骤S4中的实时计算水下机器人下一时刻的运动速度具体为：

步骤S406：按照dwa方法，计算本时刻速度空间下的速度样本；

步骤S407：对每个速度样本，计算dwa得分后，进行速度冲突检测，若检测结果为冲突，则将该样本的得分设置为0；否则，比较该样本得分与最优样本的得分，若该样本得分比最优样本高，则将该样本设为最优样本；

步骤S408：按照步骤S407计算每个速度样本，取最优样本速度值作为水下机器人下一时刻的速度；

其中，步骤S407中的速度冲突检测方法具体为：

步骤S4071：设障碍物点集为pts_2k，其中与水下机器人位置Pr(xr,yr)最近的一点为P(x,y)，过点P作Vo方向的直线L_Vo；

步骤S4072：将L_Vo沿Pr到P的垂线方向向Pr平移，平移距离为水下机器人半径r，得到直线L_Vo’；

步骤S4073：设冲突检测的速度样本V，样本模拟时间为Δt，则模拟最终位置P’＝Pr+V*Δt；

步骤S4074：判断Pr与P’与直线L_Vo’的关系，若Pr与P’在直线L_Vo’同侧，则检测结果为不冲突；反之，检测结果为冲突。