CN112363498B - 一种基于激光雷达的水下机器人智能运动控制方法 - Google Patents

Abstract

本发明公开了一种基于激光雷达的水下机器人智能运动控制方法，其通过在水下工作区域使用激光雷达数据创建3D栅格地图；向水下机器人的导航模块发送导航目标点；导航模块根据目标点与当前位置，规划由当前位置到目标点的行走路径；水下机器人沿规划出的路径运动，导航模块根据激光雷达数据，计算水下机器人运动过程中与障碍物的相对速度，从而根据速度冲突检测方法实时计算水下机器人下一时刻的最优运动速度；根据最优运动速度控制水下机器人移动，重复前述步骤，直到水下机器人到达目标点。本发明通过使用激光雷达，考虑相对运动速度因素设定安全速度边界，降低水下机器人与障碍物发生运动冲突的概率，提高水下机器人运动的安全性与流畅性。

Description

一种基于激光雷达的水下机器人智能运动控制方法

技术领域

本申请涉及水下机器人领域，特别涉及一种基于激光雷达的水下机器人智能运动控制方法。

背景技术

现如今在水下机器人领域，水下机器人运动过程中，会遇到障碍物出现在前进路线上或路线附近。传统的dwa方法，在水下机器人当前的速度空间中只考虑水下机器人目标点方向、水下机器人与路径的距离、水下机器人与障碍物的距离等因素计算每个速度样本的得分，选择得分最高的速度样本作为下一时刻需要执行的速度。当水下机器人与障碍物间有相对移动时，机器前一时刻计算出的速度并不适合本时刻障碍物的状态，导致水下机器人需要急停或重算路径，影响水下机器人运动流畅性，甚至影响机器或人的安全。

发明内容

本发明提供了一种基于激光雷达的水下机器人智能运动控制方法，目的在于降低水下机器人与障碍物发生运动冲突的概率，提高水下机器人运动的安全性与流畅性。

为了解决上述问题或至少部分地解决上述技术问题，在本申请的一个实施例中，提供了一种基于激光雷达的水下机器人智能运动控制方法，其特征在于，所述方法包括：

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

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

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

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

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

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

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

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

步骤S403：定义n_laser1中点集为pts_1i，其中0<i<＝n_laser1，定义n_laser2中点集为Pts_2j，其中0<j<＝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’同侧，则检测结果为不冲突；反之，检测结果为冲突。

本发明公开了一种基于激光雷达的水下机器人智能运动控制方法，其通过在水下工作区域使用激光雷达数据创建3D栅格地图；向水下机器人的导航模块发送导航目标点；导航模块根据目标点与当前位置，规划由当前位置到目标点的行走路径；水下机器人沿规划出的路径运动，导航模块根据激光雷达数据，计算水下机器人运动过程中与障碍物的相对速度，从而根据速度冲突检测方法实时计算水下机器人下一时刻的最优运动速度；根据最优运动速度控制水下机器人移动，重复前述步骤，直到水下机器人到达目标点。本发明通过使用激光雷达，考虑相对运动速度因素设定安全速度边界，降低水下机器人与障碍物发生运动冲突的概率，提高水下机器人运动的安全性与流畅性。

附图说明

为了更清楚地说明本申请实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单介绍。显而易见地，下面描述中的附图仅用于示意本申请的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图中未提及的技术特征、连接关系乃至方法步骤。

图1是本发明一实施例的一种基于激光雷达的水下机器人智能运动控制方法的流程示意图；

图2是本发明一实施例的最优运动速度的流程示意图；

图3是本发明另一实施例的速度冲突检测的流程示意图；

具体实施例

为使本申请实施例的目的、技术方案和优点更加清楚，下面将结合本申请实施例中的附图，对本申请实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本申请一部分实施例，而不是全部的实施例。基于本申请中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本申请保护的范围。

在本申请实施例中使用的术语是仅仅出于描述特定实施例的目的，而非旨在限制本申请。在本申请实施例和所附权利要求书中所使用的单数形式的“一种”、“所述”和“该”也旨在包括多数形式，除非上下文清楚地表示其他含义，“多种”一般包含至少两种，但是不排除包含至少一种的情况。

取决于语境，如在此所使用的词语“如果”、“若”可以被解释成为“在……时”或“当……时”或“响应于确定”或“响应于监测”。类似地，取决于语境，短语“如果确定”或“如果监测(陈述的条件或事件)”可以被解释成为“当确定时”或“响应于确定”或“当监测(陈述的条件或事件)时”或“响应于监测(陈述的条件或事件)”。

还需要说明的是，术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的商品或者系统不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种商品或者系统所固有的要素。在没有更多限制的情况下，由语句“包括一个……”限定的要素，并不排除在包括所述要素的商品或者系统中还存在另外的相同要素。

现如今在水下机器人领域，水下机器人运动过程中，会遇到障碍物出现在前进路线上或路线附近。传统的dwa方法，在水下机器人当前的速度空间中只考虑水下机器人目标点方向、水下机器人与路径的距离、水下机器人与障碍物的距离等因素计算每个速度样本的得分，选择得分最高的速度样本作为下一时刻需要执行的速度。当水下机器人与障碍物间有相对移动时，机器前一时刻计算出的速度并不适合本时刻障碍物的状态，导致水下机器人需要急停或重算路径，影响水下机器人运动流畅性，甚至影响机器或人的安全。

本发明中的水下机器人，指的是可以在水底地面建立三维地图的水下爬行机器人，不限于水下爬行作业机器人、水下爆破机器人、水下挖沙机器人、水下特种作业机器人等。通过设置在水下机器人上的激光雷达，实时感知周围的地面数据并返回该数据到水下机器人的建图模块，建立水底地面地图，从而根据该地图进行路径行走。激光雷达原理是向目标探测物发送探测信号(激光束)，然后将目标发射回来的信号(目标回波)与发射信号进行比较，进行适当处理后，便可获取目标的相关信息，例如，目标距离、方位、高度、速度、姿态、甚至形状等参数，从而对目标进行探测、跟踪和识别。让水下机器人识别物体、行走、避障等，那首先需要为水下机器人提供激光雷达技术，帮助水下机器人进行地图绘制、确定自身定位及感知周边环境，以及对周边的物体进行定位。激光雷达配合SLAM算法，可以让水下机器人在水底下实现智能行走，行走过程中进行地图绘制，以及后续的导航、避障。

在本发明的一实施例中，如图1所示，其示出了一种基于激光雷达的水下机器人智能运动控制方法，所述方法包括：

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

水下机器人预先根据控制其行走时采集的激光雷达数据，建立工作区域内的3D栅格地图。该栅格地图用于水下机器人行走导航的定位和目标点识别。

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

根据工作人员设置的作业目标点，水下机器人识别3D栅格地图上的该作业目标点作为导航目标点。

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

根据工作人员设置的作业目标点，水下机器人识别3D栅格地图上的该作业目标点作为导航目标点，结合水下机器人自身的定位，规划出自身定位位置与导航目标点的最优导航路径。

当具有多个目标点时，路径规划场景定义为：给定n个目标点，在保证水下机器人最终到达第n个点的前提下，可按任意次序遍历其余n-1个点。设总路径代价值为f(C)，代价值用路径经过栅格的中心连线距离和表示，设d(c_i,c_i+1)表示第i个目标点与第i+1个目标点之间路径经过栅格的中心连线距离，则总路径代价可表示为

遗传算法是模拟自然选择和生物进化过程的计算模型，通过模拟自然进化过程搜索最优解，包括编码、选择、交叉、变异四步主要运算过程。其中，编码运算是指将空间问题转换至遗传空间，按一定结构的基因序列组成种群中的不同个体；选择运算是对于种群中的个体，根据适应度不同选出适应能力较强的个体；交叉运算是指对个体中的子序列按一定规则进行交叉进化；变异运算指将个体序列中某基因座上的基因值作变动。基于遗传算法的移动水下机器人多目标点路径规划算法实现步骤如下：

读取目标点信息，最后一个点为水下机器人执行完导航任务后停止的最终目标点，故该点顺序须在处理过程中保持不变；

将所有目标点，按照与起始点之间的距离排序，距离最小目标点的排在最前面，作为遗传算法序列1，根据路径长度计算序列1的总代价值f(C₁)；

将所有目标点，按照距前一个到达点的距离排序，第一次计算取距起始点最近的一个目标点，作为遗传算法序列2，计算序列2代价值f(C₂)；

将序列1与序列2进行交叉运算，运算结果作为序列3与序列4，计算序列3与序列4的代价值f(C₃)、f(C₄)，交叉方法如下：

1)找出两个序列中的差异段，即序列中相同位置不同目标点所在的位置；

2)找出差异段中的相同连续子段，即差异段中连续2个或2个以上基因次序相同，即为1个相同连续子段，该步的目的是在交叉过程中保留适应能力较强的连续子段；

3)将连续子段对应的位置进行交叉，假设得到序列w₁w₂w₃...w_ku₁u₂w_k+3...w_n，序列中缺少基因w_k+1,w_k+2，同时序列中两次出现了u₁,u₂基因，此时，按照u₁,u₂的出现顺序使用w_k+1,w_k+2依次替换交叉位置以外u₁,u₂相同的基因。

在序列3与序列4中随机选定变异基因，变异后使用未出现基因代替变异基因以外的重复基因，生成的新序列分别作为序列5与序列6，计算序列5与序列6的总代价值f(C₅)、f(C₆)。

比较f(C₁)～f(C₆)的大小，选择总代价值最小的序列作为最终解算结果，确定目标点次序，形成水下机器人行走的最优导航路径。

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

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

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

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

步骤S403：定义n_laser1中点集为pts_1i，其中0<i<＝n_laser1，定义n_laser2中点集为Pts_2j，其中0<j<＝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。

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

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

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

步骤S408：按照步骤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’同侧，则检测结果为不冲突；反之，检测结果为冲突。

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

本发明公开了一种基于激光雷达的水下机器人智能运动控制方法，其通过在水下工作区域使用激光雷达数据创建3D栅格地图；向水下机器人的导航模块发送导航目标点；导航模块根据目标点与当前位置，规划由当前位置到目标点的行走路径；水下机器人沿规划出的路径运动，导航模块根据激光雷达数据，计算水下机器人运动过程中与障碍物的相对速度，从而根据速度冲突检测方法实时计算水下机器人下一时刻的最优运动速度；根据最优运动速度控制水下机器人移动，重复前述步骤，直到水下机器人到达目标点。本发明通过使用激光雷达，考虑相对运动速度因素设定安全速度边界，降低水下机器人与障碍物发生运动冲突的概率，提高水下机器人运动的安全性与流畅性。

还需要说明的是，术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的商品或者系统不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种商品或者系统所固有的要素。在没有更多限制的情况下，由语句“包括一个……”限定的要素，并不排除在包括所述要素的商品或者系统中还存在另外的相同要素。

上述说明示出并描述了本发明的若干优选实施例，但如前所述，应当理解本发明并非局限于本文所披露的形式，不应看作是对其他实施例的排除，而可用于各种其他组合、修改和环境，并能够在本文所述发明构想范围内，通过上述教导或相关领域的技术或知识进行改动。而本领域人员所进行的改动和变化不脱离本发明的精神和范围，则都应在本发明所附权利要求的保护范围内。

Claims (1)

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

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

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

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

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

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

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

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

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

步骤S403：定义n_laser1中点集为pts_1i，其中0<i<＝n_laser1，定义n_laser2中点集为Pts_2j，其中0<j<＝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’同侧，则检测结果为不冲突；反之，检测结果为冲突。

Images