CN115308770A - 一种基于拟合图形的动态障碍物检测方法 - Google Patents

一种基于拟合图形的动态障碍物检测方法 Download PDF

Info

Publication number
CN115308770A
CN115308770A CN202210891994.5A CN202210891994A CN115308770A CN 115308770 A CN115308770 A CN 115308770A CN 202210891994 A CN202210891994 A CN 202210891994A CN 115308770 A CN115308770 A CN 115308770A
Authority
CN
China
Prior art keywords
fitting
circle
laser
robot
preset
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Pending
Application number
CN202210891994.5A
Other languages
English (en)
Inventor
周和文
陈肯
肖刚军
蒋林
黄惠保
陈锦杰
余乘风
赵慧
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Wuhan University of Science and Engineering WUSE
Zhuhai Amicro Semiconductor Co Ltd
Original Assignee
Wuhan University of Science and Engineering WUSE
Zhuhai Amicro Semiconductor Co Ltd
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Wuhan University of Science and Engineering WUSE, Zhuhai Amicro Semiconductor Co Ltd filed Critical Wuhan University of Science and Engineering WUSE
Priority to CN202210891994.5A priority Critical patent/CN115308770A/zh
Publication of CN115308770A publication Critical patent/CN115308770A/zh
Pending legal-status Critical Current

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
    • G01S17/88—Lidar systems specially adapted for specific applications
    • G01S17/93—Lidar systems specially adapted for specific applications for anti-collision purposes
    • G01S17/931—Lidar systems specially adapted for specific applications for anti-collision purposes of land vehicles
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T5/00—Image enhancement or restoration
    • G06T5/20—Image enhancement or restoration using local operators
    • G06T5/30—Erosion or dilatation, e.g. thinning
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/60—Analysis of geometric attributes
    • G06T7/66—Analysis of geometric attributes of image moments or centre of gravity
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/70—Determining position or orientation of objects or cameras
    • G06T7/73—Determining position or orientation of objects or cameras using feature-based methods
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06V—IMAGE OR VIDEO RECOGNITION OR UNDERSTANDING
    • G06V10/00—Arrangements for image or video recognition or understanding
    • G06V10/20—Image preprocessing
    • G06V10/28—Quantising the image, e.g. histogram thresholding for discrimination between background and foreground patterns
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T2207/00—Indexing scheme for image analysis or image enhancement
    • G06T2207/10—Image acquisition modality
    • G06T2207/10028—Range image; Depth image; 3D point clouds

Landscapes

  • Engineering & Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Theoretical Computer Science (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Multimedia (AREA)
  • Geometry (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Electromagnetism (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)

Abstract

本发明公开一种基于拟合图形的动态障碍物检测方法，机器人装配有测距传感器，用于获取点云数据；该动态障碍物检测方法包括如下步骤：机器人从点云数据中提取出用于标记障碍物的拟合图形；同时，机器人构建栅格地图；然后机器人将所述拟合图形的中心与栅格地图进行对比，筛选出中心落入栅格地图的可通行区域的拟合图形；然后利用筛选出的拟合图形的中心的坐标变化，来识别该拟合图形所标记的障碍物是否为动态障碍物。

Description

一种基于拟合图形的动态障碍物检测方法
技术领域
本发明属于障碍物的检测识别的技术领域，具体涉及一种基于拟合图形的动态障碍物检测方法。
背景技术
在室内家庭环境中，扫地机器人进行全覆盖清扫的时候容易碰到人、宠物等动态障碍物，其中，本领域技术人员会根据障碍物的状态类型，可以分为动态障碍物和静态障碍物。扫地机器人在避开环境中出现的动态障碍物时，都是将动态障碍物当成瞬时的静态障碍物来处理或者无法检测到动态障碍物，还由于激光的测量精度原因，容易将障碍物分散成一个个散点，这使机器人判断动态物体十分困难，甚至容易产生误判。
发明内容
针对处于运动状态的障碍物检测的问题，本发明公开一种基于拟合图形的动态障碍物检测方法，具体的技术方案如下：
一种动态障碍物检测方法，机器人装配有测距传感器，用于获取点云数据；动态障碍物检测方法包括如下步骤：机器人从点云数据中提取出用于标记障碍物的拟合图形；同时，机器人构建栅格地图；然后机器人将所述拟合图形的中心与栅格地图进行对比，筛选出中心落入栅格地图的可通行区域的拟合图形；然后利用筛选出的拟合图形的中心的坐标变化，然后利用筛选出的拟合图形的中心的坐标变化，来检测该拟合图形所标记的障碍物是否为动态障碍物，以区分静态障碍物与动态障碍物。
进一步地，所述步骤A具体包括：步骤A1、机器人对所获取的点云数据进行分割，得到若干个点集合；步骤A2、对步骤A1获得的点集合进行合并处理，获得拟合线段；步骤A3、对步骤A2获得的拟合线段进行圆拟合操作，获得预设外接圆；再对预设外接圆进行圆合并处理，获得拟合圆；步骤A4、利用点云数据构建栅格地图，再依据可通行性对栅格地图进行二值化处理，再对二值化的栅格地图进行腐蚀处理，然后将经过二值化处理和腐蚀处理后的栅格地图更新为栅格地图；步骤A5、将步骤A3获得的拟合圆的圆心转换到地图上以实现与步骤A4最后更新出的栅格地图的坐标索引信息进行对比，筛选出圆心落入栅格地图的可通行区域的拟合圆，并确定机器人获得所述用于标记障碍物的拟合图形；步骤A6、通过获取不同时刻下的用于标记障碍物的拟合图形的中心，计算出该障碍物的速度，再依据障碍物的速度识别该障碍物是属于静态障碍物还是属于动态障碍物。
进一步地，在所述步骤A1中，机器人通过测距传感器获取的点云数据是激光点云，激光点云包括多个激光点；所述机器人对所获取的点云数据进行分割，得到若干个点集合的方法包括：步骤A11、在机器人搜索激光点的过程中，机器人利用两个激光点的欧式距离的变化，将激光点云分成多个激光点组；步骤A12、机器人利用最小二乘法将每个激光点组内的激光点拟合出一条拟合线段；在每个激光点组内，机器人将距离对应拟合出的拟合线段最远的激光点标记为最远点，当机器人计算出最远点到该激光点组对应拟合出的拟合线段之间的距离大于预设分割距离阈值，则以该最远点为分界将该激光点组分割为两个子集合以使该最远点成为其中一个子集合内的第一个激光点，其中，该激光点组内，机器人将索引值比该最远点小的激光点归入一个子集合，并将索引值比该最远点大的激光点归入另一个子集合；步骤A13、机器人将每个子集合更新为步骤A12所述的激光点组，再执行步骤A12，直至所有的激光点组内对应的最远点与该激光点组对应拟合出的拟合线段之间的距离都小于或等于预设分割距离阈值，然后将把激光点的数量少于预设数量阈值的激光点组剔除，剩余的激光点组是步骤A11所述的若干个点集合，并获得现存的各个激光点组内的激光点拟合出的的拟合线段。
进一步地，在所述步骤A11中，所述机器人利用两个激光点的欧式距离的变化，将激光点云分成多个激光点组的方法包括：步骤A111、机器人计算当前一次搜索到的激光点与上一次搜索到的激光点的欧式距离；若该欧式距离小于预设分组距离阈值，则将当前一次搜索到的激光点与上一次搜索到的激光点归为同一个激光点组；若该欧式距离大于或等于预设分组距离阈值，则将当前一次搜索到的激光点归入一个新的激光点组内，并将当前一次搜索到的激光点标记为该新的激光点组内的第一个激光点；然后执行步骤A112；步骤A112、机器人搜索新的激光点，然后将当前一次搜索到的激光点更新为上一次搜索到的激光点，再执行步骤A111，直至机器人计算完每个激光点与其余任一个激光点的欧式距离并依据对应两个激光点的欧式距离分出激光点组，然后执行步骤A12。
进一步地，在所述步骤A2中，对点集合进行合并处理的方法包括：每当机器人从步骤A13获得的所有拟合线段当中搜索两条拟合线段后，若机器人检测到该两条拟合线段之间最近的端点的距离小于预设轮廓距离阈值，且该两条拟合线段的斜率的差值的绝对值小于预设斜率阈值，且该两条拟合线段的延长线与同一坐标轴的交点的距离小于预设截距阈值时，则确定该两条拟合线段处于一条直线；然后，将该两条拟合线段对应的两个激光点组并为一个新的激光点组，再利用最小二乘法将合并后的激光点组内的激光点拟合为新的拟合线段。
进一步地，对于每条拟合线段，该条拟合线段的两个端点分别配置为该条拟合线段对应的激光点组内的第一个激光点和最后一个激光点；每个激光点组对应一条拟合线段；在同一个激光点组内，第一个激光点和最后一个激光点之间的距离是任意两个激光点之间的距离当中的最大值。
进一步地，在步骤A3中，对拟合线段进行圆拟合操作的方法包括：机器人从所述步骤A2获得的每条拟合线段中获取垂直于该条拟合线段且指向激光坐标系的原点的向量，将该向量与横坐标轴所成的夹角的角度设置为水平偏转角度，并将该向量与纵坐标轴所成的夹角的角度设置为竖直偏转角度；然后将该条拟合线段设置为等边三角形的一条边，然后将该等边三角形的外心设置为与激光坐标系的原点分居该条拟合线段的两侧、或设置为与激光坐标系的原点位于该条拟合线段的同一侧；然后将该条拟合线段的长度与30度的正切函数值的乘积设置为该等边三角形的外接圆的半径；然后将该等边三角形的外接圆的半径的一半与水平偏转角度的余弦函数值的乘积标记为预设横轴偏移坐标量，并确定该预设横轴偏移坐标量是该条拟合线段的中点与该等边三角形的外接圆的圆心在横坐标轴上的坐标偏移量；同时，将该等边三角形的外接圆的半径的一半与竖直偏转角度的余弦函数值的乘积标记为预设纵轴偏移坐标量，并确定该预设纵轴偏移坐标量是该条拟合线段的中点与该等边三角形的外接圆的圆心在纵坐标轴上的坐标偏移量；然后利用预设纵轴偏移坐标量和预设横轴偏移坐标量计算出该等边三角形的外接圆的圆心的坐标位置；在该等边三角形的外接圆的圆心的坐标位置的基础上，将该等边三角形的外接圆的半径扩大一个预设半径增量，获得所述预设外接圆，并确定对该条拟合线段完成圆拟合操作。
进一步地，在步骤A3中，对预设外接圆进行圆合并处理的方法包括：当机器人检测到存在两个预设外接圆为包含关系时，保留该两个预设外接圆当中半径相对大的预设外接圆，同时剔除该两个预设外接圆当中半径相对小的预设外接圆，并将该两个预设外接圆当中半径相对大的预设外接圆设置为拟合圆，以使该拟合圆标记或圈定一个障碍物；当机器人检测到存在两个预设外接圆相交时，将该两个预设外接圆的圆心的连线配置为拟合线段，再将该拟合线段进行前述的圆拟合操作，得到参考外接圆；然后将参考外接圆的半径与前述的圆拟合操作之前的两个预设外接圆中相对大的半径相加，获得参考半径；当检测到该参考半径小于预设容许半径，则保留下该参考外接圆的圆心，再以参考外接圆的圆心为圆心、参考半径为半径设定一个圆形并将该圆形标记为所述拟合圆，以使该拟合圆标记或圈定一个障碍物。
进一步地，所述步骤A4具体包括：利用点云数据表征的障碍物占据的位置信息，为栅格地图的各个栅格赋予相应的像素值，然后将像素值大于预设像素阈值的栅格标记为可通行栅格，并将像素值小于或等于预设像素阈值的栅格标记为不可通行栅格，然后获得二值化的栅格地图，并确定原始的栅格地图经过二值化处理；再按照预设的点云转换误差将二值化的栅格地图内的每个不可通行栅格的关联邻域的可通行栅格标记为不可通行栅格，获得腐蚀处理后的栅格地图；其中，每个不可通行栅格的关联邻域的栅格区域与预设的点云转换误差涉及的栅格区域正相关；然后将先后经过二值化处理和腐蚀处理后的栅格地图更新为所述栅格地图，再将可通行栅格组成的区域标记为栅格地图内的可通行区域，同时将可通行栅格在所述栅格地图内的坐标索引值记录下来；然后执行步骤A5。
进一步地，在所述步骤A5中，机器人将步骤A3获得的拟合圆的圆心从激光坐标系投影到步骤A4最后更新出的栅格地图中，获得相应拟合圆的圆心在栅格地图的全局地图坐标系上的坐标索引值；然后对比步骤A4所记录的坐标索引值与拟合圆的圆心在栅格地图的全局地图坐标系上的坐标索引值；当步骤A3获得的拟合圆的圆心在栅格地图的全局地图坐标系上的坐标索引值等于步骤A4所记录的其中一个坐标索引值时，确定该拟合圆是所述步骤A筛选出的中心落入栅格地图的可通行区域的拟合图形，则筛选出中心落入栅格地图的可通行区域的拟合圆并将该拟合圆标记为所述用于标记障碍物的拟合图形；当步骤A3获得的拟合圆的圆心在栅格地图的全局地图坐标系上的坐标索引值不等于步骤A4所记录的任意一个坐标索引值时，从栅格地图内剔除该拟合圆的圆心。
进一步地，在所述步骤A6中，所述通过获取不同时刻下的所述用于标记障碍物的拟合图形的中心，计算出该障碍物的速度，再依据障碍物的速度识别该障碍物是属于静态障碍物还是属于动态障碍物的方法包括：机器人将步骤A5在第一时刻筛选出的拟合图形的中心的坐标标记为第一中心坐标；然后，机器人将步骤A5在第二时刻筛选出的同一拟合图形的中心的坐标标记为第二中心坐标；机器人将第一时刻与第二时刻的时间差值标记为障碍物运动时间差；机器人将第一中心坐标与第二中心坐标的距离标记为障碍物的运动距离；然后计算障碍物的运动距离与障碍物运动时间差的比值，获得该障碍物的速度；当机器人判断到该障碍物的速度大于预设速度阈值时，确定该障碍物是动态障碍物，再重复执行步骤A1至步骤A6以实现对该动态障碍物的运动状态的跟踪；当机器人判断到该障碍物的速度小于或等于预设速度阈值时，确定该障碍物是静态障碍物。
本发明的有益技术效果在于：本发明对点云数据进行分组、对初步分出的激光点组继续分割、再对每个激光点组拟合出的拟合线段进行圆拟合操作，进而对圆拟合操作出的外接圆合并处理，以获得拟合圆，再通过用于标记同一障碍物的拟合圆的圆心的运动速度判断该拟合圆标记的障碍物是否为动态障碍物，既可以实现拟合圆标记的障碍物的状态类型的检测，又可以构建障碍物的运动状态模型。本发明对最新拟合出来的拟合线段进行合并处理以合并位于同一直线上的各个拟合线段，至少是相邻两个拟合线段或相邻两个激光点组，并将基于这些拟合线段获得的预设外接圆进行圆合并处理，避免存在圆心位置不同或半径不同的两个拟合圆表示同一个障碍物，提高动态障碍物检测的准确度。
本发明将拟合线段进行圆拟合操作，获得预设外接圆，进而经过圆合并处理而获得拟合圆，等效于圆形障碍物，而不是简单地使用拟合线段表示动态障碍物，减少长走廊、墙壁这些静态的障碍物对于检测的干扰。而且，为保证所做出的圆能充分代表障碍物甚至圈定该障碍物，需要将该等边三角形的外接圆的半径扩展一个有限的预设半径增量，同时，实际环境中的动态障碍物的半径都不会太大，为了排除长走廊、墙壁、体积较大的障碍物的影响，设置一个预设容许半径，只有半径小于该预设容许半径的预设外接圆或拟合圆才能保留下来。
附图说明
图1是本发明的一种实施例公开的一种动态障碍物检测方法的流程图。
图2是本发明的一种实施例公开的将激光点云分成两个激光点组的示意图。
图3是本发明的一种实施例公开的对一个激光点组分割的示意图。
图4是本发明的一种实施例公开的对两条拟合线段合并处理的示意图。
图5是本发明的一种实施例公开的等边三角形的外接圆的示意图。
图6是本发明的一种实施例公开的将两个预设外接圆进行圆合并处理为拟合圆的示意图。
具体实施方式
为了便于本领域普通技术人员理解和实施本发明，下面结合附图及实施例对本发明作进一步的详细描述，应当理解，此处所描述的实施示例仅用于说明和解释本发明，并不用于限定本发明。下面通过实施例并结合附图，对本发明的技术方案进一步具体的说明。
本发明公开一种基于拟合图形的动态障碍物检测方法，动态障碍物检测方法的执行主体是一种自动移动的机器人，该机器人的机体上装配测距传感器，用于获取点云数据，这些点云数据是测距传感器发出的调制光信号扫描机器人所处的环境的结果，具体是机器人所处环境内的障碍物反射的信息，反映机器人所探测到的障碍物全部轮廓或部分轮廓的位置信息。优选地，测距传感器是支持360度旋转的激光雷达，获取各个角度方向上的激光点云，即所述点云数据，由旋转角度信息和距离信息组成；一般地，激光雷达每旋转一圈，则获取一帧激光点云。
作为一种实施例，所述动态障碍物检测方法包括：机器人从点云数据中提取出用于标记障碍物的拟合图形，即构建出能够在点云数据中代表该障碍物的图形，优选为包围(或圈住)该障碍物的圆形，减少长走廊、墙壁这些静态的障碍物对于检测的干扰。同时，机器人构建栅格地图，该栅格地图可以简化为由0和1表示的栅格组成的二值化的栅格地图，也可以是概率栅格地图；其中，用于标记障碍物的拟合图形不一定是位于栅格地图内。因此，机器人需要将所述拟合图形的中心与栅格地图进行对比，筛选出中心落入栅格地图的可通行区域的拟合图形，具体地，机器人会先将所述拟合图形的中心转换到所述栅格地图上，并通过图像处理手段去剔除中心处于地图边界上的拟合图形，从而筛选出中心落入栅格地图的可通行区域的拟合图形，获得处于栅格地图内部的可能的动态障碍物的位置信息。
然后机器人利用前述筛选出的拟合图形的中心的坐标变化，来检测该拟合图形所标记的障碍物的状态类型，以准确区分静态障碍物与动态障碍物。其中，拟合图形是配置为障碍物的整体信息，相比离散的点更加有效地识别出障碍物的状态，特别是将拟合图形的中心的坐标变化是配置为障碍物的运动信息；筛选出的拟合图形的中心的坐标变化是表示为用于标记同一障碍物(运动的障碍物)的拟合图形的坐标运动量，在本实施例中，机器人获取到的点云数据一直是反射自同一个障碍物，该障碍物可能处于运动状态下，用于标记该障碍物的拟合图形的中心的坐标在一段时间内持续发生变化，形成该障碍物的运动速度，因此，本实施例通过检测障碍物的运动速度的变化来判断该障碍物是否属于动态障碍物，以提高动态障碍物的检测精度，更好地适用于局部避障，包括在局部区域内或局部路径上避开处于运动状态的障碍物。
作为一种实施例，如图1所示，所述动态障碍物检测方法具体包括：步骤A1、机器人对所获取的点云数据进行分割，得到若干个点集合；其中，点云数据具体是障碍物点云的位置信息，点云数据以激光数据(激光点云)表示时，每一帧激光数据有对应一条激光的角度和激光点的距离，并以极坐标的形式表示出来，在确定机器人自身的位姿后，将激光数据进行三角函数变换后，得到障碍物的坐标位置信息。一般地，通过机器人的激光雷达获取当前帧的激光数据后，去除激光点的距离过远的激光数据，以激光坐标系内的坐标形式保留所有符合阈值范围要求的激光点形成激光点云。其中，激光点的距离越大，激光点云越稀疏。然后，机器人对所获取的点云数据进行分组，再对每组的点云数据作进一步的分割，如此重复，直至每个激光点都被分割到相应一组内，获得若干个点集合。
需要说明的是，每个点集合都能对应拟合出一条拟合线段，即拟合出直线段，且在误差允许的范围内可以将拟合出的直线段的两个端点分别由该点集合内的第一个点和最后一个点表示，该点集合内的第一个点和最后一个点分别是索引值最小的点和索引值最大的点，表现为在该点集合内的距离最远的两个点；所以，每个点集合对应拟合出的直线段可以使用该直线段对应的直线方程、该点集合内的第一个点和最后一个点来共同描述。
步骤A2、机器人对步骤A1获得(由于机器人所获得的所有符合阈值范围要求的激光点都需要进行分组，所以步骤A1获得可以理解为步骤A1最终获得)的点集合进行合并处理，即对步骤A1重复分割出的所有的点集合进行合并处理以获得新的一个点集合，获得拟合线段，包括合并处理出来的点集合对应拟合出的拟合线段、以及不需合并处理的且属于步骤A1分出的点集合对应拟合出的拟合线段；为避免步骤A1对点云数据过度分割而导致拟合出的直线段过多，步骤A2对拟合出直线段依次进行判断与合并，也表现为相应的点集合的合并，得到数量相对少的拟合线段；当存在部分拟合直线段不需进行合并处理，连同步骤A2合并处理出的拟合线段都作为步骤A2最终获得的拟合线段，分别对应一个点集合。
步骤A3、机器人对步骤A2获得的拟合线段进行圆拟合操作，即对经过步骤A2合并处理后的每个点集合和未经过步骤A2合并处理的每个点集合都进行圆拟合操作，获得预设外接圆，以充分标记并圈定对应的障碍物，包括动态障碍物和静态障碍物；但针对可能存在的不同的预设外接圆标记同一个物体的情况，则需要对当前获得的预设外接圆进行圆合并处理，以替换在先获得的预设外接圆，圆合并处理获得的拟合圆作为步骤A3获得的拟合圆。其中，经过圆拟合操作出来的每个预设外接圆都需进行圆合并处理。在实际环境中的动态障碍物的半径都不会太大，为了排除长走廊、墙壁、体积较大的障碍物的影响，本实施例只考虑将拟合线段进行圆拟合操作，获得预设外接圆，进而经过圆合并处理而获得拟合圆，等效于圆形障碍物，而不是简单地使用拟合线段表示动态障碍物，一般地，线段障碍物用于表示墙壁或长走廊。
步骤A4、利用点云数据构建栅格地图，此时的点云数据是需要先转换为地图数据，再依据可通行性对栅格地图进行二值化处理以使每个栅格由二进制0或二进制1表示，再对二值化的栅格地图进行腐蚀处理，然后将经过二值化处理和腐蚀处理后的栅格地图更新为栅格地图，从而以更加统一和便于进行图像计算操作的形式来表示栅格地图。
需要说明的是，步骤A4可以配置为与步骤A1、或步骤A2、或步骤A3同步执行，属于相互独立执行的步骤，即步骤A4构建的栅格地图分别与步骤A1中分割出的点集合、步骤A2中获得的拟合线段和步骤A3当中获得的拟合圆都不关联。在步骤A4中，机器人使用Cartographer算法构建栅格地图，Cartographer算法主要负责处理来自激光雷达、IMU、里程计的数据，并基于这些数据进行地图的构建，用于激光定位导航场景下，Cartographer框架一般划分为前端匹配和后端优化工作。首先前端匹配的过程就是创建局部地图的过程，通过添加一系列的经过点云滤波，以及陀螺仪、里程计数据信息进行位姿融合，再将融合的位姿添加到局部地图当中去，这里的匹配策略是当前扫描出的激光帧图像与预先扫描出的激光帧图像进行位姿匹配，但是当大量的激光帧图像被创建完成时，就会有误差积累，这时引入后端回环检测，进行优化，也就是分枝定界算法，这个算法通过将栅格分为几层深度(通俗来讲就是分了好几层分辨率)，通过先匹配低分辨率再匹配高分辨率，大大的缩短了回环检测的时间。一旦建立了回环，就会进行优化，优化的方式，就是构建位姿图，通过将陀螺仪数据、里程计数据、激光数据、局部地图数据形成各自的约束，建立一个多边形回环，建立环上的节点，通过稀疏位姿图进行优化，优化的方式是建立一个非线性最小二乘方程进行优化，再为Cartographer建好的地图每个栅格赋予对应的像素值，以描述实际环境分布，包括障碍物的分布情况，最终得到原始的栅格地图，以便接受前述的二值化处理和腐蚀处理。
步骤A5、机器人将步骤A3获得的拟合圆(机器人在步骤A3中获得的所有预设外接圆都需要进行圆合并处理，最终获得的拟合圆)的圆心转换到地图上以实现与步骤A4最后更新出的栅格地图的坐标索引信息进行对比，筛选出圆心落入栅格地图的可通行区域的拟合圆，以剔除掉栅格地图边界处的拟合圆，在一些实施例中与栅格地图比较的过程中可以剔除已知障碍物并以圆圈标记新加入的障碍物，并确定机器人获得所述用于标记障碍物的拟合图形，即获得处于栅格地图内部的可能的动态障碍物的位置信息，其中，拟合图形是以拟合圆(圆形)的形式表示。
步骤A6、通过获取不同时刻下的所述用于标记障碍物的拟合图形的中心，计算出该障碍物的速度，再依据障碍物的速度识别该障碍物是属于静态障碍物还是属于动态障碍物。其中，所述用于标记同一障碍物的部分或全部的拟合图形会跟随对应拟合线段的变化而变化，而该拟合线段也会跟随点集合的变化而变化，这里的变化是指点云数据所反映的同一障碍物的位置发生变化以描述出该障碍物的运动状态，所述用于标记同一障碍物的部分或全部的拟合图形在本实施例中可以表示为用于圈定同一障碍物的部分或全部轮廓的拟合圆，在大多情况下是圈定障碍物的全部轮廓的圆形；从而通过步骤A5筛选出的拟合圆的圆心的实时变化来判断出动态障碍物。
综合前述步骤A1至步骤A6，机器人对点云数据进行分组、对初步分出的激光点组继续分割、再对每个激光点组拟合出的拟合线段进行圆拟合操作，进而对圆拟合操作出的外接圆合并处理，以获得拟合圆，再通过用于标记同一障碍物的拟合圆的圆心的运动速度判断该拟合圆标记的障碍物是否为动态障碍物，既可以实现拟合圆标记的障碍物的状态类型的检测，又可以构建障碍物的运动状态模型。本发明对最新拟合出来的拟合线段进行合并处理以合并位于同一直线上的各个拟合线段，至少是相邻两个拟合线段或相邻两个激光点组，并将基于这些拟合线段获得的预设外接圆进行圆合并处理，避免存在圆心位置不同或半径不同的两个拟合圆表示同一个障碍物，提高动态障碍物检测的准确度。
优选地，机器人通过测距传感器获取的点云数据是激光点云，激光点云包括多个激光点，均能在机器人的内存中搜索；这些激光点可以按照障碍物反射位置在机器人的内存中进行排列，则相邻两个激光点是位置相邻的两个激光点，其中一个激光点可视为位于另一个激光点的邻域。在一些实施例中，激光点云也可以按照机器人搜索的时间先后顺序在机器人内存中进行排序。
所述机器人对所获取的点云数据进行分割，得到若干个点集合的方法包括：
步骤A11、在机器人搜索激光点的过程中，机器人利用两个激光点的欧式距离的变化，将激光点云分成多个激光点组；具体地，所述机器人利用两个激光点的欧式距离的变化，将激光点云分成多个激光点组的方法包括：步骤A111、机器人计算当前一次搜索到的激光点与上一次搜索到的激光点的欧式距离；若该欧式距离小于预设分组距离阈值，则将当前一次搜索到的激光点与上一次搜索到的激光点归为同一个激光点组；若该欧式距离大于或等于预设分组距离阈值，则将当前一次搜索到的激光点归入一个新的激光点组内，并将当前一次搜索到的激光点标记为该新的激光点组内的第一个激光点；然后执行步骤A112；步骤A112、机器人搜索新的激光点，然后将当前一次搜索到的激光点更新为上一次搜索到的激光点，再执行步骤A111，直至机器人计算完每个激光点与其余任一个激光点的欧式距离并依据对应两个激光点的欧式距离分出激光点组，然后执行步骤A12。
作为一种实施例，对应到步骤A11，如图2所示，机器人依次计算当前一个激光点
Figure BDA0003767950110000071
与上一个激光点
Figure BDA0003767950110000072
的欧式距离
Figure BDA0003767950110000073
其中，i是激光点在内存空间的排序号；在一些实施例中，当前一个激光点
Figure BDA0003767950110000081
是上一个激光点
Figure BDA0003767950110000082
的邻近范围内的激光点；若欧式距离
Figure BDA0003767950110000083
小于预设分组距离阈值，则将激光点
Figure BDA0003767950110000084
和激光点
Figure BDA0003767950110000085
归为同一组，并标记为同一个激光点组内的相邻两个激光点；否则，以
Figure BDA0003767950110000086
为第一个点创建一个新的激光点组Sj，相对于激光点
Figure BDA0003767950110000087
所处的激光点组Sj-1而言。预设分组距离阈值用于表示预先配置出的同一组激光点内的两个激光点之间所能达到的最大距离、或大于该最大距离。当机器人计算完任两个激光点之间的欧式距离，即计算完每个激光点与其余任一个激光点的欧式距离并依据对应两个激光点的欧式距离分出激光点组后，机器人已经遍历完所有的激光点且不能继续扩展出的新激光点，会得到若干个分好组的激光点组Sj(j＝1,2,...,n,n≤m)，j表示激光点组的组数或组序号；需要说明的是，在图2中，以
Figure BDA0003767950110000088
的形式保留机器人所获取到的激光点或所获取到的激光点当中符合要求的激光点，xi对应的是激光坐标系上的第i个激光点的横坐标，yi对应的是激光坐标系上的第i个激光点的纵坐标，m为激光点的总数量，m也相当于机器人所需搜索的激光点的数量。图2中的激光点当中，激光点组Sj内的激光点相对于原点(ox，oy)在y轴方向上的偏移距离都小于激光点组Sj-1内的激光点相对于原点(ox，oy)在y轴方向上的偏移距离。
步骤A12、机器人利用最小二乘法将每个激光点组内的激光点拟合出一条拟合线段，在本实施例中，每个激光点组对应拟合出一条拟合线段；在每个激光点组内，机器人将距离对应拟合出的拟合线段最远的激光点标记为最远点，当机器人计算出最远点到该激光点组对应拟合出的拟合线段之间的距离大于预设分割距离阈值，则以该最远点为分界将该激光点组分割为两个子集合以使该最远点成为其中一个子集合内的第一个激光点，并确定该激光点组满足分割条件；其中，预设分割距离阈值用于表示同一组激光点内的激光点到其对应的拟合直线的最大距离；该激光点组内，机器人将索引值比该最远点小的激光点归入一个子集合，并将索引值比该最远点大的激光点归入另一个子集合，最远点可以归入前述分割出的两个子集合当中的任一个子集合内，或同时归入前述分割出的两个子集合当中的每个子集合内；在一些实施例中，机器人将搜索时间比该最远点小的激光点早的归入一个子集合，并将搜索时间比最远点晚的激光点归入另一个子集合，机器人是按照位置远近依次进行激光点的搜索/遍历。
步骤A13、机器人将每个子集合更新为步骤A12所述的激光点组，再执行步骤A12，直至所有的激光点组内对应的最远点与该激光点组对应拟合出的拟合线段之间的距离都小于或等于预设分割距离阈值，即所有被分出的激光点组或所述子集合不满足分割条件；然后将把激光点的数量少于预设数量阈值的激光点组剔除，剩余的激光点组是步骤A11所述的若干个点集合，并获得现存的各个激光点组内的激光点拟合出的的拟合线段。预设数量阈值优选为3。
作为一种实施例，如图3所示，机器人先利用最小二乘法在待分割的激光点组Sj中的激光点内拟合出一条拟合线段lm，即激光点组Sj包括激光点，利用最小二乘法实现直线拟合出直线方程Am*x+Bm*y+Cm＝0，由于所求直线不唯一，这里只需要设置Cm不等于0即可，利用最小二乘法确定直线系数Am和Bm，其中，直线系数Am、直线系数Bm和直线系数Cm的具体求解方式，参阅中国发明专利CN113253717A的权利要求2中的步骤5公开的与直线拟合关联的计算公式，在此再赘述。然后，寻找该激光点组中距离直线lm最远的激光点
Figure BDA0003767950110000089
如果激光点
Figure BDA00037679501100000810
到直线段lm的距离dk大于预设分割距离阈值，则将从该激光点
Figure BDA00037679501100000811
处将此激光点组分割为两个子集合Sj、Sj+1，其中，子集合Sj中存在
Figure BDA0003767950110000091
子集合Sj+1中存在
Figure BDA0003767950110000092
而
Figure BDA0003767950110000093
是同时归入子集合Sj和归入子集合Sj+1；在此基础上，依次对每一个子集合重复执行前述的分割处理，直到所有的子集合都不满足分割条件为止；然后机器人把激光点个数少于3的激光点组剔除，则其对应拟合出的拟合线段也不被机器人调用来进行后续的圆拟合操作。在此基础上，可把所有点云数据按照前述实施例设定的规则以若干个点集合的形式分开，且每一个点集合都对应一条拟合出的直线，然后，在本实施例中，以激光点组中的第一个激光点和最后一个激光点以及对应拟合出的直线方程来描述对应的拟合线段，在误差允许的范围内，激光点组中的第一个激光点和最后一个激光点可以视为该激光点组对应拟合出的拟合线段的两个端点。
作为一种实施例，在所述步骤A2中，对点集合进行合并处理的方法包括：每当机器人从步骤A13最终获得的拟合线段当中搜索到两条拟合线段后，检测该两条拟合线段之间的属性差异，包括拟合线段的两个端点、与对应的直线方程的截距及斜率；机器人搜索到的两条拟合线段可以是位置相邻的两条拟合线段，但不一定是相互平行的两条线段；当机器人检测到该两条拟合线段之间最近的端点的距离小于预设轮廓距离阈值，且该两条拟合线段的斜率的差值的绝对值小于预设斜率阈值，且该两条拟合线段的延长线与同一坐标轴的交点的距离(即在激光坐标系内对应的直线方程的截距)小于预设截距阈值时，则确定该两条拟合线段处于一条直线，在一些实施例中可以确定该两条拟合线段表示同一障碍物或同一障碍物的不同部分，否则该两条拟合线段不位于一条直线上以确定该两条拟合线段分别位于相隔开的两个障碍物上；然后，机器人将该两条拟合线段对应的两个激光点组合并为一个新的激光点组，再利用最小二乘法将合并后的激光点组内的激光点拟合为新的拟合线段，则该新的拟合线段可以单独表示一个障碍物。
为避免步骤A1出现对激光点组或前述子集合或点集合过度分割的问题，机器人对步骤A1中所有分割出的点集合及其对应的拟合线段进行合并判断与处理，得到最终的若干条拟合线段；如图4所示，存在位置相邻的两条直线段，对应的直线方程分别表示为：lm：Am*x+Bm*y+Cm＝0，lm+1：Am+1*x+Bm+1*y+Cm+1＝0，其中，直线lm是由子集合Sj内的激光点拟合出来的拟合直线，直线lm+1是由子集合Sj+1内的激光点拟合出来的拟合直线。首先，判断两条直线段是否靠近。本实施例使用d0表示该两条直线段的最近距离，同时使用预设轮廓距离阈值表示该两条直线段处于同一障碍物的距离阈值。如图4所示，直线段lm上的最后一个点与直线段lm+1上的第一个点的距离是直线lm与直线lm+1之间的最近距离，使用d0表示，当d0小于预设轮廓距离阈值时，确定直线lm与直线lm+1处于同一直线上，可以将直线段lm上的最后一个点与直线段lm+1上的第一个点连接成为一条直线段，同时可以将子集合Sj+1内的激光点并入子集合Sj内的激光点当中，组成激光点数量更多的一个子集合Sj，实现对子集合Sj的更新，也合并出一个新的激光点组以避免子集合过度分割，相应地可以拟合出直线lm，毕竟原来的直线段lm与直线段lm+1被视为处于同一直线上。
优选地，对于每条拟合线段，该条拟合线段的两个端点分别配置为该条拟合线段对应的激光点组内的第一个激光点和最后一个激光点；每个激光点组对应一条拟合线段。在同一个激光点组内，第一个激光点和最后一个激光点之间的距离是任意两个激光点之间的距离当中的最大值，使得第一个激光点和最后一个激光点在所处的激光点组内具有代表性，且在两个不同的激光点组之间具有连通意义。
优选地，在前述组成一个新的激光点组或划分一个新子集合的相关步骤中，机器人将最早加入该条拟合线段对应的激光点组的激光点标记为该激光点组内的第一个激光点，同时将最晚加入该条拟合线段对应的激光点组的激光点标记为该激光点组内的最后一个激光点；在一些实施例中，为了便于索引每个激光点组内的激光点，加入同一个激光点组的时间越早的激光点的索引值越小，加入同一个激光点组的时间越晚的激光点的索引值越大。进一步地，对于不同的激光点组也配置相应的组别索引值，越早被创建或划分出的激光点组被配置出的组别索引值越小。
作为一种实施例，在步骤A3中，对拟合线段进行圆拟合操作，获得拟合圆的方法包括：机器人从所述步骤A2获得的每条拟合线段中获取垂直于该条拟合线段且指向激光坐标系的原点的向量，即机器人从步骤A2合并处理出的每条拟合线段以及没被步骤A2合并处理出的每条拟合线段(来源于步骤A112分割出的每个激光点组内对应拟合出拟合线段)中获取垂直于该条拟合线段且指向激光坐标系的原点的向量，将该向量与横坐标轴所成的夹角的角度设置为水平偏转角度，并将该向量与纵坐标轴所成的夹角的角度设置为竖直偏转角度；然后机器人将该条拟合线段设置为等边三角形的一条边，一般是由该条拟合线段对应的激光点组的第一个激光点和最后一个激光点的连线；然后机器人将该等边三角形的外心(该等边三角形的外接圆的圆心)设置为与激光坐标系的原点分居该条拟合线段的两侧、或设置为与激光坐标系的原点位于该条拟合线段的同一侧；然后，由于是处理等边三角形，所以将该条拟合线段的长度与30度的正切函数值的乘积设置为该等边三角形的外接圆的半径；然后将该等边三角形的外接圆的半径的一半与水平偏转角度的余弦函数值的乘积标记为预设横轴偏移坐标量，并确定该预设横轴偏移坐标量是该条拟合线段的中点与该等边三角形的外接圆的圆心在横坐标轴上的坐标偏移量；同时，机器人将该等边三角形的外接圆的半径的一半与竖直偏转角度的余弦函数值的乘积标记为预设纵轴偏移坐标量，并确定该预设纵轴偏移坐标量是该条拟合线段的中点与该等边三角形的外接圆的圆心在纵坐标轴上的坐标偏移量；然后利用预设纵轴偏移坐标量和预设横轴偏移坐标量计算出该等边三角形的外接圆的圆心的坐标位置，即机器人利用预设纵轴偏移坐标量与该条拟合线段的中点的纵坐标相加以获得该等边三角形的外接圆的圆心的纵坐标，同理地，机器人利用预设横轴偏移坐标量与该条拟合线段的中点的横坐标相加以获得该等边三角形的外接圆的圆心的横坐标，在确定出该等边三角形的外接圆的圆心的坐标位置之后，结合该等边三角形的外接圆的半径，机器人可以在激光坐标系内确定出该等边三角形的外接圆在激光坐标系中的覆盖区域。然后，机器人在该等边三角形的外接圆的圆心的坐标位置的基础上，将该等边三角形的外接圆的半径扩大一个预设半径增量，获得所述预设外接圆，并确定对该条拟合线段完成圆拟合操作，则步骤A2获得的每条拟合线段对应一个预设外接圆；其中，设置的预设半径增量相当于在该等边三角形的外接圆的基础上扩展出一个圆形边界以保证包含障碍物。
优选地，实际环境中的动态障碍物的半径都不会太大，为了排除长走廊、墙壁、体积较大的障碍物的影响，设置一个预设容许半径，只有半径小于该预设容许半径的预设外接圆才能保留下来。
作为一种实施例，为充分代表并包裹障碍物，对步骤A2拟合出的所有拟合线段(视为步骤A2合并处理出的激光点组对应拟合出的拟合线段和不需经过合并处理的激光点组对应拟合出的拟合线段(来源于步骤A13拟合出的拟合线段))进行拟合圆的操作；参阅图5，对激光点组或其对应的拟合线段进行圆拟合操作的方法包括：以一条直线段为例，该直线段对应的激光点组中的第一个激光点为
Figure BDA0003767950110000111
最后一个激光点为
Figure BDA0003767950110000112
也可以视为该直线段的两个端点；同时，机器人设置该直线段的指向激光坐标系原点的法向量为
Figure BDA0003767950110000113
如图5所示，机器人将该直线段作为底边，设计出外心(圆心)远离激光坐标系原点原点(ox，oy)的等边三角形，即该等边三角形的外心与激光坐标系的原点分居该条直线段的两侧，则将该等边三角形的外接圆的覆盖区域设置得更远，覆盖比测距传感器的中心更远的位置处的障碍物，毕竟障碍物不是距离机器人比较近。具体地，在该等边三角形的外接圆中，该外接圆的半径为
Figure BDA0003767950110000114
Figure BDA0003767950110000115
该外接圆的圆心的向量表示为
Figure BDA0003767950110000116
原理在于前述实施例中将该等边三角形的外接圆的半径的一半与水平偏转角度的余弦函数值的乘积标记为预设横轴偏移坐标量，并确定该预设横轴偏移坐标量是该条拟合线段的中点与该等边三角形的外接圆的圆心在横坐标轴上的坐标偏移量；同时，机器人将该等边三角形的外接圆的半径的一半与竖直偏转角度的余弦函数值的乘积标记为预设纵轴偏移坐标量，并确定该预设纵轴偏移坐标量是该条拟合线段的中点与该等边三角形的外接圆的圆心在纵坐标轴上的坐标偏移量。进一步地，为保证所做出的圆能充分代表障碍物甚至圈定该障碍物，需要将该等边三角形的外接圆的半径扩展一个预设半径增量renlarge，以r+renlarge表示最终确定的预设外接圆的半径大小，以
Figure BDA0003767950110000117
表示该预设外接圆的中心点坐标。
在前述实施例的基础上，在步骤A3中，对预设外接圆进行圆合并处理的方法包括：当机器人检测到存在两个预设外接圆为包含关系时，保留该两个预设外接圆当中半径相对大的预设外接圆，同时剔除该两个预设外接圆当中半径相对小的预设外接圆，并将该两个预设外接圆当中半径相对大的预设外接圆设置为拟合圆，以使该拟合圆标记或圈定一个障碍物，则可以确定该拟合圆是用于标记哪个障碍物的拟合圆，方便后续计算用于标记同一障碍物的该拟合圆的运动速度，并在机器人的内存中对应标注上识别信息，以确定该拟合圆是用于标记哪个障碍物的拟合圆，方便后续计算用于标记同一障碍物的该拟合圆的运动速度。当机器人检测到存在两个预设外接圆相交时，将该两个预设外接圆的圆心的连线配置为拟合线段，再将该拟合线段进行圆拟合操作，得到参考外接圆；然后将参考外接圆的半径与进行前述圆拟合操作之前的两个预设外接圆中相对大的半径相加，获得参考半径；当检测到该参考半径小于预设容许半径，则保留下该参考外接圆的圆心，具体是记录下该圆心的坐标，并剔除圆拟合操作前的两个预设外接圆和参考外接圆，再以参考外接圆的圆心为圆心、参考半径为半径设定一个圆形并将该圆形标记为所述拟合圆，从而将圆拟合操作前的两个预设外接圆合并为所述拟合圆，实现使用一个拟合圆标记或圈定对应的一个障碍物，并在机器人的内存中对应标注上识别信息，以确定该拟合圆是用于标记哪个障碍物的拟合圆，方便后续计算用于标记同一障碍物的该拟合圆的运动速度；综上，通过步骤A3获得的不同的拟合圆分别标记或圈定对应的障碍物，避免存在不同的圆(包括圆心位置不同的两个拟合圆和/或半径不同的两个拟合圆)表示同一个障碍物。
对应到图6中，对所有预设外接圆进行圆合并处理的过程中，可以根据位置相邻的两个预设外接圆(图6中的圆)的圆心与半径的关系，进行相应的处理：当两个预设外接圆为相交关系的时候，连接该两个预设外接圆的圆心c1、c2，对c1和c2的连线进行前述圆拟合操作，得到一个圆心为c3的新的预设外接圆，被标记为参考外接圆，再将该参考外接圆的半径r3与圆心为c2的预设外接圆的半径r2相加，若相加后的半径r小于预设容许半径，则保留参考外接圆的圆心的坐标c3和半径r作为所述拟合圆的组成要素以覆盖到激光坐标系内。
作为一种实施例，所述步骤A4具体包括：利用点云数据表征的障碍物占据的位置信息，为栅格地图的各个栅格赋予相应的像素值，然后将像素值大于预设像素阈值的栅格标记为可通行栅格，并将像素值小于或等于预设像素阈值的栅格标记为不可通行栅格，然后获得二值化的栅格地图，并确定原始的栅格地图经过二值化处理；具体地，机器人利用机体预存的Cartographer算法建好栅格地图后，确定利用点云数据表征的障碍物占据的位置信息，为栅格地图的各个栅格赋予相应的像素值，从而获取栅格地图的长宽信息以及每一个栅格的像素值，具体将像素值大于128的像素点所在的栅格标记为二进制0，形成可通行栅格，反之将像素点所在的栅格标记为二进制1，形成不可通行栅格，构建一张由二进制0和二进制1构成的二值栅格地图，并更新为所述栅格地图，以简化地图的表示形式和填充的内环境信息。然后机器人按照预设的点云转换误差将二值化的栅格地图内的每个不可通行栅格的关联邻域的可通行栅格标记为不可通行栅格，获得腐蚀处理后的栅格地图；其中，每个不可通行栅格的关联邻域的栅格区域与预设的点云转换误差涉及的栅格区域正相关，优选地，在腐蚀处理的过程中，机器人在栅格地图内，将以每个赋值为二进制1的栅格为中心的二十四邻域内存在的二进制0全部更新为二进制1，即在每个赋值为二进制1的栅格的外围相邻接的两圈栅格内，将二进制0全部更新为二进制1，以完成对该栅格地图的腐蚀处理，其中，二十四邻域覆盖的区域是除了中心之外，行数为5且列数为5的栅格区域，可以记为该次腐蚀处理的有效腐蚀区域，或者理解为腐蚀半径等于两个栅格，具体的腐蚀半径是能够依据机器人所处的环境进行调节以便于在更大的有效区域内识别动态障碍物。然后将先后经过前述二值化处理和前述腐蚀处理后的栅格地图更新为所述栅格地图，再将可通行栅格组成的区域标记为栅格地图内的可通行区域，同时将可通行栅格在所述栅格地图内的坐标索引值记录下来以形成可通行点的集合，实际上是以索引值的形式保存每个栅格在全局地图坐标系下的坐标信息；然后执行步骤A5。
作为一种实施例，在所述步骤A5中，机器人将步骤A3获得的拟合圆的圆心从激光坐标系投影到步骤A4最后更新出的栅格地图中，以实现将所有拟合圆的圆心从激光点的形式转换到地图索引值的形式，获得相应拟合圆的圆心在栅格地图的全局地图坐标系上的坐标索引值，实际上是以索引值的形式保存拟合圆的圆心在全局地图坐标系下的坐标信息。优选地，全局地图坐标系的X轴与激光坐标系的X轴平行或垂直，且两种坐标系在同一坐标轴上的坐标以不同的比例去表示同一障碍物的轮廓尺寸，即同一坐标偏移量所代表的障碍物轮廓线长度是不同。然后对比前述实施例的步骤A4所记录的坐标索引值与拟合圆的圆心在栅格地图的全局地图坐标系上的坐标索引值；当步骤A3获得的拟合圆的圆心在栅格地图的全局地图坐标系上的坐标索引值等于步骤A4所记录的其中一个坐标索引值时，确定该拟合圆是中心落入栅格地图的可通行区域的拟合图形，则机器人可以筛选出中心落入栅格地图的可通行区域的拟合圆并将该拟合圆标记为所述用于标记障碍物的拟合图形；当步骤A3获得的拟合圆的圆心在栅格地图的全局地图坐标系上的坐标索引值不等于步骤A4所记录的任意一个坐标索引值时，从栅格地图内剔除该拟合圆的圆心，去除掉不在该栅格地图的边界框定范围内的位置点，具体是剔除障碍物占据区域与可通行区域的边界位置处圆心，剩余的即为处于地图可通行区域内的可的动态障碍物的位置信息，因为在可通行区域内才能调用动态窗口探测处于运动状态的障碍物及其反射回的点云数据。
作为一种实施例，在所述步骤A6中，所述通过获取不同时刻下的所述用于标记障碍物的拟合图形的中心，计算出该障碍物的速度，再依据障碍物的速度识别该障碍物是属于静态障碍物还是属于动态障碍物的方法包括：机器人将步骤A5在第一时刻筛选出的用于标记障碍物的拟合图形的中心(在标记同一障碍物的前提下，第一时刻产生的拟合圆的圆心)的坐标标记为第一中心坐标；然后，机器人将步骤A5在第二时刻筛选出的用于标记同一障碍物的拟合图形的中心(在标记同一障碍物的前提下，第二时刻产生的拟合圆的圆心)的坐标标记为第二中心坐标；在机器人执行步骤A5之后，从步骤A4最后更新出的栅格地图的坐标索引信息中提取出用于标记同一障碍物的拟合圆的依据是拟合圆与所标记的障碍物的对应关系，且经过执行步骤A3的圆合并处理可克服不同拟合圆标记同一障碍物的问题，其涉及的对应关系可以由步骤A2中对激光点组的合并处理以及步骤A3中的预设外接圆的获取及其合并处理(即圆合并处理)形成。
机器人将第一时刻与第二时刻的时间差值标记为障碍物运动时间差；障碍物运动时间差可以视为记录同一障碍物的特征点(对应于前述实施例的拟合圆的圆心)所经过的相邻时间间隔，具体可以根据实际探测障碍物的体型大小、轻重程度、受摩擦力的大小等其余物理因素共同设定。因此，机器人将第一中心坐标与第二中心坐标的距离标记为障碍物的运动距离。
然后计算障碍物的运动距离与障碍物运动时间差的比值，获得该障碍物的速度，作为该障碍物的瞬时速度。当机器人判断到该障碍物的速度大于预设速度阈值时，确定该障碍物是动态障碍物，再重复执行步骤A1至步骤A6以实现对该障碍物的运动状态的跟踪，包括对用于标记该动态障碍物的拟合圆及其圆心的变化，还包括该拟合圆对应的激光点组、拟合线段的变化，其中，预设速度阈值是配置为区分静态障碍物和动态障碍物的速度阈值，该速度阈值不一定是0，则静态障碍物不一定处于静止状态下，可能在误差允许的范围内运动得比较慢以视为处于静止状态下。具体地，随着机器人采样时间的推进，点云数据会被刷新，进而被步骤A1分割出的激光点组、步骤A2合并处理出的拟合线段以及由步骤A3获得的拟合圆被刷新，尤其是障碍物反射回的若干个点云数据或用于标记该障碍物的拟合圆及其圆心被不断刷新，因此可以使用最小二乘法去对所述障碍物运动时间差内不断刷新的多个点云数据(激光点)，得到拟合直线方程，配置为障碍物的运动状态方程，可以确定该障碍物在第一时刻与第二时刻下的位置信息，具体是在标记同一障碍物的前提下，第一时刻产生的拟合圆的圆心的坐标，以及第二时刻产生的拟合圆的圆心的坐标，然后可以计算出这两个圆心之间的距离，获得所述障碍物的运动距离，进而可以计算所标记的同一障碍物的运动速度。
另外，当机器人判断到该障碍物的速度小于或等于预设速度阈值时，确定该障碍物是静态障碍物，再停止计算该障碍物的速度，也不予以跟踪用于标记该静态障碍物的拟合圆及其对应激光点组的变化情况。
综上，通过筛选出合适的拟合圆来计算获得不同状态类型的障碍物的整体特性及其运动信息，便于提前对障碍物的运动行为进行预测，减少机器人与动态障碍物发生碰撞。
本发明的保护范围并不限于上述的实施例，显然，本领域的技术人员可以对本发明进行各种改动和变形而不脱离本发明的范围和精神。倘若这些改动和变形属于本发明权利要求及其等同技术的范围内，则本发明的意图也包含这些改动和变形在内。

Claims (11)

1.一种基于拟合图形的动态障碍物检测方法，机器人装配有测距传感器，用于获取点云数据；其特征在于，该动态障碍物检测方法包括：
机器人从点云数据中提取出用于标记障碍物的拟合图形；同时，机器人构建栅格地图；然后机器人将所述拟合图形的中心与栅格地图进行对比，筛选出中心落入栅格地图的可通行区域的拟合图形；
然后利用筛选出的拟合图形的中心的坐标变化，来检测该拟合图形所标记的障碍物是否为动态障碍物。
2.根据权利要求1所述动态障碍物检测方法，其特征在于，所述动态障碍物检测方法具体包括：
步骤A1、机器人对所获取的点云数据进行分割，得到若干个点集合；
步骤A2、对步骤A1获得的点集合进行合并处理，获得拟合线段；
步骤A3、对步骤A2获得的拟合线段进行圆拟合操作，获得预设外接圆；再对预设外接圆进行圆合并处理，获得拟合圆；
步骤A4、利用点云数据构建栅格地图，再依据可通行性对栅格地图进行二值化处理，再对二值化的栅格地图进行腐蚀处理，然后将经过二值化处理和腐蚀处理后的栅格地图更新为栅格地图；
步骤A5、将步骤A3获得的拟合圆的圆心转换到地图上以实现与步骤A4最后更新出的栅格地图的坐标索引信息进行对比，筛选出圆心落入栅格地图的可通行区域的拟合圆，并确定机器人获得所述用于标记障碍物的拟合图形；
步骤A6、通过获取不同时刻下的用于标记障碍物的拟合图形的中心，计算出该障碍物的速度，再依据障碍物的速度检测该障碍物是属于静态障碍物还是属于动态障碍物。
3.根据权利要求2所述动态障碍物检测方法，其特征在于，在所述步骤A1中，机器人通过测距传感器获取的点云数据是激光点云，激光点云包括多个激光点；
所述机器人对所获取的点云数据进行分割，得到若干个点集合的方法包括：
步骤A11、在机器人搜索激光点的过程中，机器人利用两个激光点的欧式距离的变化，将激光点云分成多个激光点组；
步骤A12、机器人利用最小二乘法将每个激光点组内的激光点拟合出一条拟合线段；在每个激光点组内，机器人将距离对应拟合出的拟合线段最远的激光点标记为最远点，当机器人计算出最远点到该激光点组对应拟合出的拟合线段之间的距离大于预设分割距离阈值，则以该最远点为分界将该激光点组分割为两个子集合以使该最远点成为其中一个子集合内的第一个激光点，其中，该激光点组内，机器人将索引值比该最远点小的激光点归入一个子集合，并将索引值比该最远点大的激光点归入另一个子集合；
步骤A13、机器人将每个子集合更新为步骤A12所述的激光点组，再执行步骤A12，直至所有的激光点组内对应的最远点与该激光点组对应拟合出的拟合线段之间的距离都小于或等于预设分割距离阈值，然后将把激光点的数量少于预设数量阈值的激光点组剔除，剩余的激光点组是步骤A11所述的若干个点集合，并获得现存的各个激光点组内的激光点拟合出的的拟合线段。
4.根据权利要求3所述动态障碍物检测方法，其特征在于，在所述步骤A11中，所述机器人利用两个激光点的欧式距离的变化，将激光点云分成多个激光点组的方法包括：
步骤A111、机器人计算当前一次搜索到的激光点与上一次搜索到的激光点的欧式距离；若该欧式距离小于预设分组距离阈值，则将当前一次搜索到的激光点与上一次搜索到的激光点归为同一个激光点组；若该欧式距离大于或等于预设分组距离阈值，则将当前一次搜索到的激光点归入一个新的激光点组内，并将当前一次搜索到的激光点标记为该新的激光点组内的第一个激光点；然后执行步骤A112；
步骤A112、机器人搜索新的激光点，然后将当前一次搜索到的激光点更新为上一次搜索到的激光点，再执行步骤A111，直至机器人计算完每个激光点与其余任一个激光点的欧式距离并依据对应两个激光点的欧式距离分出激光点组，然后执行步骤A12。
5.根据权利要求3所述动态障碍物检测方法，其特征在于，在所述步骤A2中，对点集合进行合并处理的方法包括：
每当机器人从步骤A13获得的所有拟合线段当中搜索两条拟合线段后，若机器人检测到该两条拟合线段之间最近的端点的距离小于预设轮廓距离阈值，且该两条拟合线段的斜率的差值的绝对值小于预设斜率阈值，且该两条拟合线段的延长线与同一坐标轴的交点的距离小于预设截距阈值时，则确定该两条拟合线段处于一条直线；然后，将该两条拟合线段对应的两个激光点组并为一个新的激光点组，再利用最小二乘法将合并后的激光点组内的激光点拟合为新的拟合线段。
6.根据权利要求5所述动态障碍物检测方法，其特征在于，对于每条拟合线段，该条拟合线段的两个端点分别配置为该条拟合线段对应的激光点组内的第一个激光点和最后一个激光点；每个激光点组对应一条拟合线段；
在同一个激光点组内，第一个激光点和最后一个激光点之间的距离是任意两个激光点之间的距离当中的最大值。
7.根据权利要求6所述动态障碍物检测方法，其特征在于，在步骤A3中，对拟合线段进行圆拟合操作的方法包括：
机器人从所述步骤A2获得的每条拟合线段中获取垂直于该条拟合线段且指向激光坐标系的原点的向量，将该向量与横坐标轴所成的夹角的角度设置为水平偏转角度，并将该向量与纵坐标轴所成的夹角的角度设置为竖直偏转角度；
然后将该条拟合线段设置为等边三角形的一条边，然后将该等边三角形的外心设置为与激光坐标系的原点分居该条拟合线段的两侧、或设置为与激光坐标系的原点位于该条拟合线段的同一侧；
然后将该条拟合线段的长度与30度的正切函数值的乘积设置为该等边三角形的外接圆的半径；然后将该等边三角形的外接圆的半径的一半与水平偏转角度的余弦函数值的乘积标记为预设横轴偏移坐标量，并确定该预设横轴偏移坐标量是该条拟合线段的中点与该等边三角形的外接圆的圆心在横坐标轴上的坐标偏移量；同时，将该等边三角形的外接圆的半径的一半与竖直偏转角度的余弦函数值的乘积标记为预设纵轴偏移坐标量，并确定该预设纵轴偏移坐标量是该条拟合线段的中点与该等边三角形的外接圆的圆心在纵坐标轴上的坐标偏移量；然后利用预设纵轴偏移坐标量和预设横轴偏移坐标量计算出该等边三角形的外接圆的圆心的坐标位置；
在该等边三角形的外接圆的圆心的坐标位置的基础上，将该等边三角形的外接圆的半径扩大一个预设半径增量，获得所述预设外接圆，并确定对该条拟合线段完成圆拟合操作。
8.根据权利要求7所述动态障碍物检测方法，其特征在于，在步骤A3中，对预设外接圆进行圆合并处理的方法包括：
当机器人检测到存在两个预设外接圆为包含关系时，保留该两个预设外接圆当中半径相对大的预设外接圆，同时剔除该两个预设外接圆当中半径相对小的预设外接圆，并将该两个预设外接圆当中半径相对大的预设外接圆设置为拟合圆，以使该拟合圆标记或圈定一个障碍物；
当机器人检测到存在两个预设外接圆相交时，将该两个预设外接圆的圆心的连线配置为拟合线段，再将该拟合线段进行前述的圆拟合操作，得到参考外接圆；然后将参考外接圆的半径与前述的圆拟合操作之前的两个预设外接圆中相对大的半径相加，获得参考半径；当检测到该参考半径小于预设容许半径，则保留下该参考外接圆的圆心，再以参考外接圆的圆心为圆心、参考半径为半径设定一个圆形并将该圆形标记为所述拟合圆，以使该拟合圆标记或圈定一个障碍物。
9.根据权利要求2所述动态障碍物检测方法，其特征在于，所述步骤A4具体包括：利用点云数据表征的障碍物占据的位置信息，为栅格地图的各个栅格赋予相应的像素值，然后将像素值大于预设像素阈值的栅格标记为可通行栅格，并将像素值小于或等于预设像素阈值的栅格标记为不可通行栅格，然后获得二值化的栅格地图，并确定原始的栅格地图经过二值化处理；
再按照预设的点云转换误差将二值化的栅格地图内的每个不可通行栅格的关联邻域的可通行栅格标记为不可通行栅格，获得腐蚀处理后的栅格地图；其中，每个不可通行栅格的关联邻域的栅格区域与预设的点云转换误差涉及的栅格区域正相关；
然后将先后经过二值化处理和腐蚀处理后的栅格地图更新为所述栅格地图，再将可通行栅格组成的区域标记为栅格地图内的可通行区域，同时将可通行栅格在所述栅格地图内的坐标索引值记录下来；然后执行步骤A5。
10.根据权利要求9所述动态障碍物检测方法，其特征在于，在所述步骤A5中，机器人将步骤A3获得的拟合圆的圆心从激光坐标系投影到步骤A4最后更新出的栅格地图中，获得相应拟合圆的圆心在栅格地图的全局地图坐标系上的坐标索引值；
然后对比步骤A4所记录的坐标索引值与拟合圆的圆心在栅格地图的全局地图坐标系上的坐标索引值；当步骤A3获得的拟合圆的圆心在栅格地图的全局地图坐标系上的坐标索引值等于步骤A4所记录的其中一个坐标索引值时，确定该拟合圆是所述筛选出的中心落入栅格地图的可通行区域的拟合图形，并确定筛选出中心落入栅格地图的可通行区域的拟合圆并将该拟合圆标记为所述用于标记障碍物的拟合图形；当步骤A3获得的拟合圆的圆心在栅格地图的全局地图坐标系上的坐标索引值不等于步骤A4所记录的任意一个坐标索引值时，从栅格地图内剔除该拟合圆的圆心。
11.根据权利要求10所述动态障碍物检测方法，其特征在于，在所述步骤A6中，所述通过获取不同时刻下的所述用于标记障碍物的拟合图形的中心，计算出该障碍物的速度，再依据障碍物的速度识别该障碍物是属于静态障碍物还是属于动态障碍物的方法包括：
机器人将步骤A5在第一时刻筛选出的拟合图形的中心的坐标标记为第一中心坐标；然后，机器人将步骤A5在第二时刻筛选出的同一拟合图形的中心的坐标标记为第二中心坐标；
机器人将第一时刻与第二时刻的时间差值标记为障碍物运动时间差；
机器人将第一中心坐标与第二中心坐标的距离标记为障碍物的运动距离；
然后计算障碍物的运动距离与障碍物运动时间差的比值，获得该障碍物的速度；
当机器人判断到该障碍物的速度大于预设速度阈值时，确定该障碍物是动态障碍物；
当机器人判断到该障碍物的速度小于或等于预设速度阈值时，确定该障碍物是静态障碍物。
CN202210891994.5A 2022-07-27 2022-07-27 一种基于拟合图形的动态障碍物检测方法 Pending CN115308770A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202210891994.5A CN115308770A (zh) 2022-07-27 2022-07-27 一种基于拟合图形的动态障碍物检测方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202210891994.5A CN115308770A (zh) 2022-07-27 2022-07-27 一种基于拟合图形的动态障碍物检测方法

Publications (1)

Publication Number Publication Date
CN115308770A true CN115308770A (zh) 2022-11-08

Family

ID=83858931

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202210891994.5A Pending CN115308770A (zh) 2022-07-27 2022-07-27 一种基于拟合图形的动态障碍物检测方法

Country Status (1)

Country Link
CN (1) CN115308770A (zh)

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115793652A (zh) * 2022-11-30 2023-03-14 上海木蚁机器人科技有限公司 行驶控制方法、装置及电子设备
CN117788593A (zh) * 2024-02-26 2024-03-29 苏州艾吉威机器人有限公司 剔除三维激光数据中动态点的方法、装置、介质及设备

Cited By (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115793652A (zh) * 2022-11-30 2023-03-14 上海木蚁机器人科技有限公司 行驶控制方法、装置及电子设备
CN115793652B (zh) * 2022-11-30 2023-07-14 上海木蚁机器人科技有限公司 行驶控制方法、装置及电子设备
CN117788593A (zh) * 2024-02-26 2024-03-29 苏州艾吉威机器人有限公司 剔除三维激光数据中动态点的方法、装置、介质及设备
CN117788593B (zh) * 2024-02-26 2024-06-04 苏州艾吉威机器人有限公司 剔除三维激光数据中动态点的方法、装置、介质及设备

Similar Documents

Publication Publication Date Title
US11709058B2 (en) Path planning method and device and mobile device
WO2021143778A1 (zh) 一种基于激光雷达的定位方法
CN112526993B (zh) 栅格地图更新方法、装置、机器人及存储介质
CN115308770A (zh) 一种基于拟合图形的动态障碍物检测方法
CN107622499A (zh) 一种基于目标二维轮廓模型的识别与空间定位方法
CN114488194A (zh) 一种智能驾驶车辆结构化道路下目标检测识别方法
CN109001757B (zh) 一种基于2d激光雷达的车位智能检测方法
CN115268443A (zh) 一种机器人避障路径规划方法
CN109584294A (zh) 一种基于激光点云的路面点云提取方法和装置
US20210247771A1 (en) Information processing device
CN112464812A (zh) 一种基于车辆的凹陷类障碍物检测方法
Wang et al. Map-enhanced ego-lane detection in the missing feature scenarios
CN112346463A (zh) 一种基于速度采样的无人车路径规划方法
CN111709988A (zh) 一种物体的特征信息的确定方法、装置、电子设备及存储介质
CN110705385A (zh) 一种障碍物角度的检测方法、装置、设备及介质
CN113822332A (zh) 路沿数据标注方法及相关系统、存储介质
CN114488026B (zh) 基于4d毫米波雷达的地下停车库可通行空间检测方法
CN114565726A (zh) 一种非结构化动态环境下的同时定位与建图方法
CN114022760B (zh) 铁路隧道障碍物监测预警方法、系统、设备及存储介质
US20240200973A1 (en) Method, data processing apparatus and computer program product for generating map data
CN118411507A (zh) 一种具有动态目标的场景的语义地图构建方法及系统
CN114387293A (zh) 道路边缘检测方法、装置、电子设备及车辆
CN110046209B (zh) 一种基于高斯模型的轨迹停止点提取方法
CN115249223A (zh) 动态目标检测方法及装置、存储介质、终端
CN116434181A (zh) 地面点检测方法、装置、电子设备和介质

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination