CN113689504B - 基于可描述形状的点云精准定位方法、装置及存储介质 - Google Patents

基于可描述形状的点云精准定位方法、装置及存储介质 Download PDF

Info

Publication number
CN113689504B
CN113689504B CN202111237977.1A CN202111237977A CN113689504B CN 113689504 B CN113689504 B CN 113689504B CN 202111237977 A CN202111237977 A CN 202111237977A CN 113689504 B CN113689504 B CN 113689504B
Authority
CN
China
Prior art keywords
point cloud
fitting
coordinate system
pose
data
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.)
Active
Application number
CN202111237977.1A
Other languages
English (en)
Other versions
CN113689504A (zh
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.)
Shanghai Xiangong Intelligent Technology Co ltd
Original Assignee
Shanghai Xiangong Intelligent Technology 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 Shanghai Xiangong Intelligent Technology Co ltd filed Critical Shanghai Xiangong Intelligent Technology Co ltd
Priority to CN202111237977.1A priority Critical patent/CN113689504B/zh
Publication of CN113689504A publication Critical patent/CN113689504A/zh
Application granted granted Critical
Publication of CN113689504B publication Critical patent/CN113689504B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • 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
    • 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
    • G01S7/00—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00
    • G01S7/48—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00 of systems according to group G01S17/00
    • G01S7/4802—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00 of systems according to group G01S17/00 using analysis of echo signal for target characterisation; Target signature; Target cross-section
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F18/00—Pattern recognition
    • G06F18/20—Analysing
    • G06F18/23—Clustering techniques
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/10—Segmentation; Edge detection
    • G06T7/136—Segmentation; Edge detection involving thresholding
    • 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
    • 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/10032—Satellite or aerial image; Remote sensing
    • G06T2207/10044—Radar image
    • 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/30—Subject of image; Context of image processing
    • G06T2207/30244—Camera pose

Landscapes

  • Engineering & Computer Science (AREA)
  • Theoretical Computer Science (AREA)
  • General Physics & Mathematics (AREA)
  • Physics & Mathematics (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Data Mining & Analysis (AREA)
  • Bioinformatics & Cheminformatics (AREA)
  • Evolutionary Biology (AREA)
  • Evolutionary Computation (AREA)
  • Bioinformatics & Computational Biology (AREA)
  • General Engineering & Computer Science (AREA)
  • Artificial Intelligence (AREA)
  • Life Sciences & Earth Sciences (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Length Measuring Devices By Optical Means (AREA)

Abstract

本发明提供了一种基于可描述形状的点云精准定位方法、装置及存储介质，其中方法步骤包括：S1获取被描述物的第一数据；S2采集场景中的点云数据，做聚类处理；遍历聚类处理结果，以第一数据为参照，做拟合识别；S3若拟合匹配，将第一数据中对应的被描述物转换到相对坐标系下，并设置其在相对坐标系基准处；将拟合匹配目标的点云转换到相对坐标系下；S4将被描述物在相对坐标系中，经第一法则处理后使之与拟合匹配目标基本重叠，以获取第一形变位姿；S5根据第一形变位姿，推算采集器所处绝对坐标系下的检测位姿，进而检测位姿通过外参数转换，获得本体所处绝对坐标系下的本体位姿。籍此实现无需在定位环境中额外设置定位辅助工具，也能实现精准定位。

Description

基于可描述形状的点云精准定位方法、装置及存储介质
技术领域
本发明涉及定位技术领域，尤其涉及一种基于可描述形状的点云精准定位方法、装置及存储介质。
背景技术
本发明所称的机器人是一种能够自动运行的移动机器人，如轮式机器人，其根据工作属性的不同可以分为：清扫机器人、消毒机器人、巡检机器人、搬运机器人等。
现有技术中，机器人在自动运行过程中，定位技术至关重要。一般的机器人定位主要依赖GNSS、UWB、视觉和激光定位等，其中GNSS和UWB技术需要依赖卫星或者基站，而GNSS技术在室内接收不到卫星信号，UWB的精度不能够满足精准定位的需求。
另一方面视觉和激光定位往往依赖于先验地图，由于建图场景和实际场景是有差别的，如建图环境发生变化、存在动态障碍物等，所以依赖于先验地图定位的机器人往往会出现定位偏差的情况，对于需要精准对接的场景，比如和充电桩的对接，或者机器人与线体的对接，往往会由于偏差导致对接失败。
因此为了解决现有技术的缺陷，本领域曾提出以高反的物体，如反光膜，依靠反光膜将空间中的一点作为锚点，以此来进行定位。但弊端在于需要在定位场景中预设置这种高反物体或材料，因此并不方便。为此本领域亟待一种能够不需要依赖此类高反物体，甚至环境中无需增加额外的辅助定位工具的技术。
发明内容
本发明的主要目的在于提供一种基于可描述形状的点云精准定位方法、装置及存储介质，以做到无需在定位环境中额外设置定位辅助工具，也能实现精准定位。
为了实现上述目的，根据本发明的第一个方面，提供了一种基于可描述形状的点云精准定位方法，其步骤包括：
S1获取被描述物的第一数据；
S2采集场景中的点云数据，做聚类处理；遍历聚类处理结果，以第一数据为参照，做拟合识别；
S3若步骤S2拟合识别成功，将第一数据中对应的被描述物转换到相对坐标系下，并设置其在相对坐标系基准处；将拟合识别的点云转换到相对坐标系下；
S4将被描述物在相对坐标系中，经过第一法则处理后使之与拟合识别的点云基本重叠，以获取第一形变位姿；其中所述第一法则处理步骤包括：D1在拟合识别的点云数据中的拟合线段中设定第一特征；D2以第一特征为目标，令被描述物进行位移，以使被描述物的对应特征与拟合识别的点云数据中的第一特征基本重叠；D3记录被描述物的位移为第一形变位姿；其中所述第一特征包括：拟合识别的点云数据中的拟合线段中的预设点、或预设边中的至少一种；其中所述位移包括：旋转或平移中的至少一种；
S5根据第一形变位姿，推算采集器所处绝对坐标系下的检测位姿，进而检测位姿通过外参数转换，获得移动机器人的本体所处绝对坐标系下的本体位姿。
在可能的优选实施方式中，所述第一数据包括被描述物的：长度、角度中的至少一种用来描述形状的参数。
在可能的优选实施方式中，所述步骤S2中的聚类处理步骤包括：根据预设点云间距分割阈值，将待识别的点云分簇，而后再进行聚类处理。
在可能的优选实施方式中，所述步骤S2中拟合识别步骤包括：
C1将待识别的每簇点云的第一处方向上的点与第二处方向上的点进行线段拟合，以定义出拟合线段；
C2判断当前拟合线段与点云簇中离其最远的点的距离是否超出阈值；
C3若超出，则以该当前拟合线段与点云簇中离其最远的点为端点，分段拟合线段；
C4循环步骤C1至C3直至分段完毕。
在可能的优选实施方式中，所述步骤S2中拟合识别步骤还包括：
C5以第一数据为参照，与待识别的点云数据中的拟合线段的长度、及线段之间的夹角做匹配判断，以识别出对应的被描述物。
为了实现上述目的，根据本发明的第二个方面，还提供了一种基于可描述形状的点云精准定位方法，其步骤包括：
S1获取被描述物的第一数据；
S2采集场景中的点云数据，做聚类处理；遍历聚类处理结果，以第一数据为参照，做拟合识别；
S3若步骤S2拟合识别成功，将第一数据中对应的被描述物转换到相对坐标系下，并设置其在相对坐标系基准处；将拟合识别的点云转换到相对坐标系下；
S4将被描述物在相对坐标系中，经过第一法则处理后使之与拟合识别的点云基本重叠，以获取第一形变位姿；其中所述第一法则处理步骤包括：D1在拟合识别的点云数据中的拟合线段中设定第一特征；D2以第一特征为目标，令被描述物进行位移，以使被描述物的对应特征与拟合识别的点云数据中的第一特征基本重叠；D3记录被描述物的位移为第一形变位姿；其中所述第一特征包括：拟合识别的点云数据中的拟合线段中的预设点、或预设边中的至少一种；其中所述位移包括：旋转或平移中的至少一种；
S5将拟合识别的点云，与步骤S4处理后的被描述物做ICP计算，以获取第二形变位姿；
S6根据第二形变位姿，推算采集器所处绝对坐标系下的检测位姿，进而检测位姿通过外参数转换，获得移动机器人的本体所处绝对坐标系下的本体位姿。
在可能的优选实施方式中，所述第一数据包括被描述物的：长度、角度中的至少一种用来描述形状的参数。
在可能的优选实施方式中，所述步骤S2中的聚类处理步骤包括：根据预设点云间距分割阈值，将待识别的点云分簇，而后再进行聚类处理。
在可能的优选实施方式中，所述步骤S2中拟合识别步骤包括：
C1将待识别的每簇点云的第一处方向上的点与第二处方向上的点进行线段拟合，以定义出拟合线段；
C2判断当前拟合线段与点云簇中离其最远的点的距离是否超出阈值；
C3若超出，则以该当前拟合线段与点云簇中离其最远的点为端点，分段拟合线段；
C4循环步骤C1至C3直至分段完毕。
在可能的优选实施方式中，所述步骤S2中拟合识别步骤包括：
C5以第一数据为参照，与待识别的点云数据中的拟合线段的长度、及线段之间的夹角做匹配判断，以识别出对应的被描述物。
在可能的优选实施方式中，所述ICP计算步骤包括：以第一形变位姿为初值，计算拟合识别的点云数据中的拟合线段中的线段及点云的误差函数；并将误差函数相加做最小二乘法。
为了实现上述目的，根据本发明的第三个方面，还提供了一种基于可描述形状的点云精准定位装置，其包括：
存储装置，存储被描述物的第一数据；激光雷达，采集场景中的点云数据；处理单元分别与激光雷达及存储装置连接，以获取点云数据做聚类处理，遍历聚类处理结果，以第一数据为参照，做拟合识别；若拟合识别成功，处理单元将第一数据中对应的被描述物转换到激光雷达坐标系下，并设置其在激光雷达坐标系原点处；而后将拟合识别的点云转换到激光雷达坐标系下；将被描述物在激光雷达坐标系中，经过第一法则处理后使之与拟合识别的点云基本重叠，以获取第一形变位姿，其中所述第一法则处理步骤包括：D1在拟合识别的点云数据中的拟合线段中设定第一特征；D2以第一特征为目标，令被描述物进行位移，以使被描述物的对应特征与拟合识别的点云数据中的第一特征基本重叠；D3记录被描述物的位移为第一形变位姿；其中所述第一特征包括：拟合识别的点云数据中的拟合线段中的预设点、或预设边中的至少一种；其中所述位移包括：旋转或平移中的至少一种；而后将拟合识别的点云，与处理后的被描述物做ICP计算，以获取第二形变位姿，后根据第二形变位姿，推算激光雷达所处世界坐标系下的检测位姿，进而检测位姿通过外参数转换，获得移动机器人的本体所处世界坐标系下的本体位姿。
为了实现上述目的，根据本发明的第四个方面，还提供了一种可读存储介质，其上存储有计算机程序，其中所述计算机程序被处理器执行时实现上述基于可描述形状的点云精准定位方法的步骤。
本发明提供的该基于可描述形状的点云精准定位方法、装置及存储介质，其有益效果包括：在需要精准对位的场景下，机器人可以脱离先验地图定位误差的影响，只依赖于形状物体的识别达到精准对位的效果；不同于传统的点云和点云直接的匹配，本发明直接将实际被描述物的形状配置参数直接带入到算法中进行计算，可以提高最终的定位精度。
此外本发明在实际实施中，无需在定位环境中额外设置定位辅助工具，只需对该场景中已知存在的物体进行描述后转换成数据，就可以达到关键点位的精准定位需求，并且对被描述物的材质没有特殊要求，只需激光能够扫描即可。
附图说明
构成本申请的一部分的附图用来提供对本发明的进一步理解，本发明的示意性实施例及其说明用于解释本发明，并不构成对本发明的不当限定。在附图中：
图1是本发明第一实施例的步骤流程示意图；
图2是本发明第二实施例的步骤流程示意图；
图3是本发明第一、二实施例中示例的拟合匹配目标的点云；
图4至图7是本发明第一、二实施例中点云分段拟合的过程示意图；
图8是本发明第一、二实施例中点云拟合匹配被描述物的示意图；
图9是本发明第一、二实施例中获取第一形变位姿的示意图；
图10是本发明第二实施例中获取第二形变位姿的示意图；
图11是本发明第三实施例的装置结构示意图。
具体实施方式
下面对本发明的具体实施方式进行详细地说明。以下示例将有助于本领域的技术人员进一步理解本发明，但不以任何形式限制本发明。应当指出的是，对本领域的普通技术人员来说，在不脱离本发明构思的前提下，还可以做出若干变形和改进。这些都属于本发明的保护范围。
需要说明的是，在不冲突的情况下，本申请中的实施例及实施例中的特征可以相互组合。下面将参考附图并结合实施例来详细说明本发明。
为了使本领域的技术人员更好的理解本发明方案，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分的实施例，而不是全部的实施例。基于本发明中的实施例，在本领域普通技术人员没有做出创造性劳动前提下所获得的所有其他实施例，都应当属于本发明的保护范围。
需要说明的是，本发明的说明书和权利要求书及上述附图中的术语“第一”、“第二”、“S1”、“S2”、“C1”、“D1”等是用于区别类似的对象，而不必用于描述特定的顺序或先后次序。应该理解这样使用的数据在适当情况下可以互换，以便这里描述的本发明的实施例能够以除了在这里图示或描述的那些以外的顺序实施。此外，术语“包括”和“具有”以及他们的任何变形，意图在于覆盖不排他的包含。此外除非另有明确的规定和限定，术语“设置”、“布设”、“安装”、“相连”、“连接”应做广义理解，例如，可以是固定连接，也可以是可拆卸连接，或一体地连接；可以是机械连接，也可以是电连接；可以是直接相连，也可以通过中间媒介间接相连，可以是两个元件内部的连通。对于本领域的普通技术人员而言，可以根据具体情况，结合现有技术来理解上述术语在本发明中的具体含义。
其中本发明的实施例下，该采集点云数据的采集器，优选以激光雷达为例进行说明，但并未进行限制，任何能够采集到物体点云数据的采集器，皆在本发明的揭露范围中。
（一）
如图1所示，本发明提供的该基于可描述形状的点云精准定位方法其步骤包括：
步骤S1预先测量被描述物的长度、角度，或者其他可以用来描述形状的参数，以记为第一数据。其中本实施例下，该被描述物可以是定位环境空间中，已存在的任意可以被参数描述的物体，也可以是另外添加的物体；籍此获取被描述物的第一数据；
步骤S2激光雷达采集场景中的点云数据，并对每帧点云数据做聚类处理；之后遍历聚类处理结果，以第一数据为参照，做拟合识别。
具体来说，本实施例下该聚类处理步骤包括： 根据预设的点云间的距离作为分割阈值，将点云分为簇，以此对点云进行分割，目的在于确保得到的每个点云簇里，都能包含连续的物体，同时还可以剔除数量过多或者过少的点，以形成筛查，然后再进行聚类处理。此外本实施例中所称聚类算法可以采用欧式聚类，或者其他适用的现有聚类算法本实施例中并不进行限制。
而后，对聚类的结果遍历做线段拟合识别，其步骤包括：
步骤C1将待识别的每簇点云的第一处方向上的点与第二处方向上的点进行线段拟合，以定义出拟合线段；
步骤C2判断当前拟合线段与点云簇中离其最远的点的距离是否超出阈值；
步骤C3若超出，则以该当前拟合线段与点云簇中离其最远的点为端点，分段拟合线段；
步骤C4循环步骤C1至C3直至分段完毕。
具体来说，由于每簇点云的点都是连续的，因此本实施例下，如图4到图7所示，该第一处方向可示例为每簇点云的第一个点，而该第二处方向上的点可示例为，每簇点云的最后一个点，以此两个点作为端点进行线段拟合，以定义出拟合线段；然后再计算这簇点云中，离该拟合线段最远的点，如果该点到拟合线段的距离大于预设阈值，则将该点作为端点去分段拟合线段，如图4到图7所示，然后再继续判断，直至分段完毕。
步骤C5在完成上述步骤C4后，以第一数据为参照，与待识别的点云数据中的拟合线段的长度、及线段之间的夹角做匹配判断，以识别出对应的被描述物。
具体来说，该步骤C5根据拟合线段的长度，以及线段之间的夹角，与第一数据做对比，以此来判断拟合出来的形状，是否有符合的被描述物，如图8所示，该abcde为点云拟合出的形状，ABCDE为设定的形状，当同时满足以下条件，则可认为当前簇点云为被描述物的点云:
Figure DEST_PATH_IMAGE001
Figure DEST_PATH_IMAGE002
其中
Figure DEST_PATH_IMAGE003
等分别代表着线段的长度，
Figure DEST_PATH_IMAGE004
等分别代表这角度，
Figure DEST_PATH_IMAGE005
为设定的阈值。
步骤S3若步骤S2拟合识别成功，则将第一数据中对应的被描述物转换到相对坐标系下，并设置其在相对坐标系基准处；将拟合识别的点云转换到相对坐标系下。
具体来说，本实施例中该相对坐标系可以是，以激光雷达为原点的激光雷达坐标系，从而该被描述物，将被转换到以激光雷达为原点的激光雷达坐标系中，放置在坐标系原点处，之后再将拟合识别的点云转换到相对坐标系下，如图9所示。
步骤S4将被描述物在激光雷达坐标系中，经过第一法则处理后使之与拟合识别的点云基本重叠，以获取第一形变位姿。其中该第一法则处理步骤包括：
步骤D1 在拟合识别的点云数据中的拟合线段中设定第一特征；如该第一特征包括：拟合识别的点云数据中的拟合线段中的预设点、或预设边中的至少一种；而本实施例中以含有预设点和预设边为例进行说明。
步骤D2以第一特征为目标，令被描述物进行位移，以使被描述物的对应特征与拟合识别的点云数据中的第一特征基本重叠；其中该位移包括：旋转或平移中的至少一种，而本实施例中以含有旋转和平移为例进行说明。
步骤D3记录被描述物的旋转和平移数据，记为第一形变位姿。
步骤S5之后根据步骤S4中获取的该第一形变位姿，推算激光雷达所处绝对坐标系下的检测位姿，进而检测位姿通过外参数转换，获得移动机器人本体所处绝对坐标系下的本体位姿。
其中，本实施例中所称的绝对坐标系即世界坐标系，是指移动机器人所处导航环境/场景下的坐标系，也是被描述物所处的坐标系，该世界坐标系可以描述移动机器人及被描述物所处场景中的位置。
进一步的，由于获得了该第一形变位姿，就可根据现有技术，计算出激光雷达所处世界坐标系中的位姿，并记为检测位姿，进而通过激光雷达的外参数来转换，即可得到移动机器人的本体所处世界坐标系下的精确位姿。
可见通过上述方法，无需在定位环境中额外设置定位辅助工具，只需对该场景中已知存在的物体进行描述后转换成数据，就可以达到关键点位的精准定位需求，并且对被描述物的材质没有特殊要求，只需激光能够扫描即可。
（二）
如图2所示，本发明第二个方面提供的该基于可描述形状的点云精准定位方法其步骤包括：
步骤S1预先测量被描述物的长度、角度，或者其他可以用来描述形状的参数，以记为第一数据。其中本实施例下，该被描述物可以是定位环境空间中，已存在的任意可以被参数描述的物体，也可以是另外添加的物体；籍此获取被描述物的第一数据；
步骤S2激光雷达采集场景中的点云数据，并对每帧点云数据做聚类处理；之后遍历聚类处理结果，以第一数据为参照，做拟合识别。
具体来说，本实施例下该聚类处理步骤包括： 根据预设的点云间的距离作为分割阈值，将点云分为簇，以此对点云进行分割，目的在于确保得到的每个点云簇里，都能包含连续的物体，同时还可以剔除数量过多或者过少的点，以形成筛查，然后再进行聚类处理。此外本实施例中所称聚类算法可以采用欧式聚类，或者其他适用的现有聚类算法本实施例中并不进行限制。
而后，对聚类的结果遍历做线段拟合识别，其步骤包括：
步骤C1将待识别的每簇点云的第一处方向上的点与第二处方向上的点进行线段拟合，以定义出拟合线段；
步骤C2判断当前拟合线段与点云簇中离其最远的点的距离是否超出阈值；
步骤C3若超出，则以该当前拟合线段与点云簇中离其最远的点为端点，分段拟合线段；
步骤C4循环步骤C1至C3直至分段完毕。
具体来说，由于每簇点云的点都是连续的，因此本实施例下，如图4到图7所示，该第一处方向可示例为每簇点云的第一个点，而该第二处方向上的点可示例为，每簇点云的最后一个点，以此两个点作为端点进行线段拟合，以定义出拟合线段；然后再计算这簇点云中，离该拟合线段最远的点，如果该点到拟合线段的距离大于预设阈值，则将该点作为端点去分段拟合线段，如图4到图7所示，然后再继续判断，直至分段完毕。
步骤C5在完成上述步骤C4后，以第一数据为参照，与待识别的点云数据中的拟合线段的长度、及线段之间的夹角做匹配判断，以识别出对应的被描述物。
具体来说，该步骤C5根据拟合线段的长度，以及线段之间的夹角，与第一数据做对比，以此来判断拟合出来的形状，是否有符合的被描述物，如图8所示，该abcde为点云拟合出的形状，ABCDE为设定的形状，当同时满足以下条件，则可认为当前簇点云为被描述物的点云:
Figure DEST_PATH_IMAGE006
Figure DEST_PATH_IMAGE007
其中
Figure DEST_PATH_IMAGE008
等分别代表着线段的长度，
Figure DEST_PATH_IMAGE010
等分别代表这角度，
Figure DEST_PATH_IMAGE011
为设定的阈值。
步骤S3若步骤S2拟合识别成功，则将第一数据中对应的被描述物转换到相对坐标系下，并设置其在相对坐标系基准处；将拟合识别的点云转换到相对坐标系下。
具体来说，本实施例中该相对坐标系可以是，以激光雷达为原点的激光雷达坐标系，从而该被描述物，将被转换到以激光雷达为原点的激光雷达坐标系中，放置在坐标系原点处，之后再将拟合识别的点云转换到相对坐标系下，如图9所示。
步骤S4将被描述物在激光雷达坐标系中，经过第一法则处理后使之与拟合识别的点云基本重叠，以获取第一形变位姿。其中该第一法则处理步骤包括：
步骤D1 在拟合识别的点云数据中的拟合线段中设定第一特征；如该第一特征包括：拟合识别的点云数据中的拟合线段中的预设点、或预设边中的至少一种；而本实施例中以含有预设点和预设边为例进行说明。
步骤D2以第一特征为目标，令被描述物进行位移，以使被描述物的对应特征与拟合识别的点云数据中的第一特征基本重叠；其中该位移包括：旋转或平移中的至少一种，而本实施例中以含有旋转和平移为例进行说明。
步骤D3记录被描述物的旋转和平移数据，记为第一形变位姿。
步骤S5将拟合识别的点云，与步骤S4处理后的被描述物做ICP计算，以获取第二形变位姿。其中，所述ICP计算步骤，本实施例中包括：以第一形变位姿为初值，计算拟合识别的点云数据中的拟合线段中的线段及点云的误差函数；并将误差函数相加做最小二乘法。
具体来说，传统的点到点的ICP计算，或者点到线的ICP计算，都是在点云和点云之间进行匹配计算，由于激光点云的跳动造成的误差，容易导致匹配的结果存在误差。
因此为了降低这部分的匹配误差，本实施例中，创新的将步骤S4中经过第一形变位姿调整之后的被描述物，和该拟合识别的点云进行匹配，以第一形变位姿作为第二形变位姿的初值，如图10所示，由于在上述线段拟合中，已经知道了每个点对应的线段，因此这里计算时，需要分别对每个线段以及对应的点，进行误差函数计算，最后将所有的误差函数相加做最小二乘法，籍此又能减少迭代次数，同时还可实现更加准确的匹配。
如图3所示，假设使用图3所示的形状，设线段描述为
Figure DEST_PATH_IMAGE012
，拟合的线段描述为
Figure DEST_PATH_IMAGE013
，对应的拟合点为
Figure DEST_PATH_IMAGE014
，
Figure DEST_PATH_IMAGE015
变换前后的相对关系写为：
Figure DEST_PATH_IMAGE016
Figure DEST_PATH_IMAGE017
则可以将每段的误差函数写为：
Figure DEST_PATH_IMAGE018
那么通过最小化一个误差函数：
Figure DEST_PATH_IMAGE019
Figure DEST_PATH_IMAGE020
其中
Figure DEST_PATH_IMAGE021
为匹配前和匹配后的拟合识别的点云数据中形状的端点集合，
Figure DEST_PATH_IMAGE022
为旋转矩阵，
Figure DEST_PATH_IMAGE023
为平移向量。
Figure DEST_PATH_IMAGE024
和
Figure DEST_PATH_IMAGE025
即为所述的第二形变位姿。从而通过非线性最小二乘法对
Figure DEST_PATH_IMAGE026
进行求解，即可得到最终的第二形变位姿参数。
步骤S6根据第二形变位姿，推算采集器所处绝对坐标系下的检测位姿，进而检测位姿通过外参数转换，获得本体所处绝对坐标系下的本体位姿。
其中，本实施例中所称的绝对坐标系即世界坐标系，是指移动机器人所处导航环境/场景下的坐标系，也是被描述物所处的坐标系，该世界坐标系可以描述移动机器人及被描述物所处场景中的位置。
进一步的，由于获得了该第二形变位姿，就可根据现有技术，计算出激光雷达所处世界坐标系中的位姿，并记为检测位姿，进而通过激光雷达的外参数来转换，即可得到移动机器人的本体所处世界坐标系下的精确位姿。
可见通过上述方法，相比实施例一可以达到更为精确的定位效果，同时也无需在定位环境中额外设置定位辅助工具，只需对该场景中已知存在的物体进行描述后转换成数据，就可以达到关键点位的精准定位需求，并且对被描述物的材质没有特殊要求，只需激光能够扫描即可。
（三）
根据图11所示，本发明的第三个方面，还提供了一种基于可描述形状的点云精准定位装置，其包括：
存储装置，存储被描述物的第一数据；激光雷达，采集场景中的点云数据；处理单元分别与激光雷达及存储装置连接，以获取点云数据做聚类处理，遍历聚类处理结果，以第一数据为参照，做拟合识别；若拟合识别成功，处理单元将第一数据中对应的被描述物转换到相对坐标系下，并设置其在激光雷达坐标系原点处。
而后将拟合识别的点云转换到激光雷达坐标系下；将被描述物在激光雷达坐标系中，经过第一法则处理后使之与拟合识别的点云基本重叠，以获取第一形变位姿；而后将拟合识别的点云，与处理后的被描述物做ICP计算，以获取第二形变位姿，后根据第二形变位姿，推算激光雷达所处世界坐标系下的检测位姿，进而检测位姿通过外参数转换，获得本体所处世界坐标系下的本体位姿。
（四）
本发明的第四个方面，还提供了一种可读存储介质，其上存储有计算机程序，其中所述计算机程序被处理器或实施例三的装置执行时，可实现上述实施例一或实施例二中任一所述基于可描述形状的点云精准定位方法的步骤。
综上所述，本发明提供的该基于可描述形状的点云精准定位方法、装置及存储介质，在需要精准对位的场景下，机器人可以脱离先验地图定位误差的影响，只依赖于形状物体的识别达到精准对位的效果；不同于传统的点云和点云直接的匹配，本发明直接将实际被描述物的形状配置参数直接带入到算法中进行计算，可以提高最终的定位精度。
此外本发明在实际实施中，无需在定位环境中额外设置定位辅助工具，只需对该场景中已知存在的物体进行描述后转换成数据，就可以达到关键点位的精准定位需求，并且对被描述物的材质没有特殊要求，只需激光能够扫描即可。
以上公开的本发明优选实施例只是用于帮助阐述本发明。优选实施例并没有详尽叙述所有的细节，也不限制该发明仅为所述的具体实施方式。显然，根据本说明书的内容，可作很多的修改和变化。本说明书选取并具体描述这些实施例，是为了更好地解释本发明的原理和实际应用，从而使所属技术领域技术人员能很好地理解和利用本发明。本发明仅受权利要求书及其全部范围和等效物的限制，凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。
本领域技术人员可以理解，除了以纯计算机可读程序代码方式实现本发明提供的系统、装置及其各个模块以外，完全可以通过将方法步骤进行逻辑编程来使得本发明提供的系统、装置及其各个模块以逻辑门、开关、专用集成电路、可编程逻辑控制器以及嵌入式微控制器等的形式来实现相同程序。所以，本发明提供的系统、装置及其各个模块可以被认为是一种硬件部件，而对其内包括的用于实现各种程序的模块也可以视为硬件部件内的结构；也可以将用于实现各种功能的模块视为既可以是实现方法的软件程序又可以是硬件部件内的结构。
同时实现上述实施例方法中的全部或部分步骤，是可以通过程序来指令相关的硬件来完成，该程序存储在一个存储介质中，包括若干指令用以使得单片机、芯片或处理器（processor）执行本申请各个实施例所述方法的全部或部分步骤。而前述的存储介质包括：U盘、移动硬盘、只读存储器（ROM，Read-Only Memory）、随机存取存储器（RAM，RandomAccess Memory）、磁碟或者光盘等各种可以存储程序代码的介质。
此外，本发明实施例的各种不同的实施方式之间也可以进行任意组合，只要其不违背本发明实施例的思想，其同样应当视为本发明实施例所公开的内容。

Claims (9)

1.一种基于可描述形状的点云精准定位方法，其特征在于，步骤包括：
S1获取被描述物的第一数据；
S2采集场景中的点云数据，做聚类处理；遍历聚类处理结果，以第一数据为参照，做拟合识别；
S3若步骤S2拟合识别成功，将第一数据中对应的被描述物转换到相对坐标系下，并设置其在相对坐标系基准处；将拟合识别的点云转换到相对坐标系下；
S4将被描述物在相对坐标系中，经过第一法则处理后使之与拟合识别的点云基本重叠，以获取第一形变位姿；其中所述第一法则处理步骤包括：D1在拟合识别的点云数据中的拟合线段中设定第一特征；D2以第一特征为目标，令被描述物进行位移，以使被描述物的对应特征与拟合识别的点云数据中的第一特征基本重叠；D3记录被描述物的位移为第一形变位姿；其中所述第一特征包括：拟合识别的点云数据中的拟合线段中的预设点、或预设边中的至少一种；其中所述位移包括：旋转或平移中的至少一种；
S5根据第一形变位姿，推算采集器所处绝对坐标系下的检测位姿，进而检测位姿通过外参数转换，获得移动机器人的本体所处绝对坐标系下的本体位姿。
2.一种基于可描述形状的点云精准定位方法，其特征在于，步骤包括：
S1获取被描述物的第一数据；
S2采集场景中的点云数据，做聚类处理；遍历聚类处理结果，以第一数据为参照，做拟合识别；
S3若步骤S2拟合识别成功，将第一数据中对应的被描述物转换到相对坐标系下，并设置其在相对坐标系基准处；将拟合识别的点云转换到相对坐标系下；
S4将被描述物在相对坐标系中，经过第一法则处理后使之与拟合识别的点云基本重叠，以获取第一形变位姿；其中所述第一法则处理步骤包括：D1在拟合识别的点云数据中的拟合线段中设定第一特征；D2以第一特征为目标，令被描述物进行位移，以使被描述物的对应特征与拟合识别的点云数据中的第一特征基本重叠；D3记录被描述物的位移为第一形变位姿；其中所述第一特征包括：拟合识别的点云数据中的拟合线段中的预设点、或预设边中的至少一种；其中所述位移包括：旋转或平移中的至少一种；
S5将拟合识别的点云，与步骤S4处理后的被描述物做ICP计算，以获取第二形变位姿；
S6根据第二形变位姿，推算采集器所处绝对坐标系下的检测位姿，进而检测位姿通过外参数转换，获得移动机器人的本体所处绝对坐标系下的本体位姿。
3.根据权利要求2所述的基于可描述形状的点云精准定位方法，其特征在于，所述第一数据包括被描述物的：长度、角度中的至少一种用来描述形状的参数。
4.根据权利要求2所述的基于可描述形状的点云精准定位方法，其特征在于，所述步骤S2中的聚类处理步骤包括：根据预设点云间距分割阈值，将待识别的点云分簇，而后再进行聚类处理。
5.根据权利要求4所述的基于可描述形状的点云精准定位方法，其特征在于，所述步骤S2中拟合识别步骤包括：
C1将待识别的每簇点云的第一处方向上的点与第二处方向上的点进行线段拟合，以定义出拟合线段；
C2判断当前拟合线段与点云簇中离其最远的点的距离是否超出阈值；
C3若超出，则以该当前拟合线段与点云簇中离其最远的点为端点，分段拟合线段；
C4循环步骤C1至C3直至分段完毕。
6.根据权利要求5所述的基于可描述形状的点云精准定位方法，其特征在于，所述步骤S2中拟合识别步骤包括：
C5以第一数据为参照，与待识别的点云数据中的拟合线段的长度、及线段之间的夹角做匹配判断，以识别出对应的被描述物。
7.根据权利要求2所述的基于可描述形状的点云精准定位方法，其特征在于，所述ICP计算步骤包括：以第一形变位姿为初值，计算拟合识别的点云数据中的拟合线段中的线段及点云的误差函数；并将误差函数相加做最小二乘法。
8.一种基于可描述形状的点云精准定位装置，其特征在于包括：
存储装置，存储被描述物的第一数据；
激光雷达，采集场景中的点云数据；
处理单元分别与激光雷达及存储装置连接，以获取点云数据做聚类处理，遍历聚类处理结果，以第一数据为参照，做拟合识别；若拟合识别成功，处理单元将第一数据中对应的被描述物转换到激光雷达坐标系下，并设置其在激光雷达坐标系原点处；而后将拟合识别的点云转换到激光雷达坐标系下；将被描述物在激光雷达坐标系中，经过第一法则处理后使之与拟合识别的点云基本重叠，以获取第一形变位姿，其中所述第一法则处理步骤包括：D1在拟合识别的点云数据中的拟合线段中设定第一特征；D2以第一特征为目标，令被描述物进行位移，以使被描述物的对应特征与拟合识别的点云数据中的第一特征基本重叠；D3记录被描述物的位移为第一形变位姿；其中所述第一特征包括：拟合识别的点云数据中的拟合线段中的预设点、或预设边中的至少一种；其中所述位移包括：旋转或平移中的至少一种；而后将拟合识别的点云，与处理后的被描述物做ICP计算，以获取第二形变位姿，后根据第二形变位姿，推算激光雷达所处世界坐标系下的检测位姿，进而检测位姿通过外参数转换，获得移动机器人的本体所处世界坐标系下的本体位姿。
9.一种可读存储介质，其上存储有计算机程序，其特征在于，所述计算机程序被处理器执行时实现如权利要求1至7中任一项所述的基于可描述形状的点云精准定位方法的步骤。
CN202111237977.1A 2021-10-25 2021-10-25 基于可描述形状的点云精准定位方法、装置及存储介质 Active CN113689504B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202111237977.1A CN113689504B (zh) 2021-10-25 2021-10-25 基于可描述形状的点云精准定位方法、装置及存储介质

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202111237977.1A CN113689504B (zh) 2021-10-25 2021-10-25 基于可描述形状的点云精准定位方法、装置及存储介质

Publications (2)

Publication Number Publication Date
CN113689504A CN113689504A (zh) 2021-11-23
CN113689504B true CN113689504B (zh) 2022-01-25

Family

ID=78587801

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202111237977.1A Active CN113689504B (zh) 2021-10-25 2021-10-25 基于可描述形状的点云精准定位方法、装置及存储介质

Country Status (1)

Country Link
CN (1) CN113689504B (zh)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
EP4345767A1 (en) * 2022-09-28 2024-04-03 Tata Consultancy Services Limited Method and system for ambient intelligence based user interaction

Citations (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111895989A (zh) * 2020-06-24 2020-11-06 浙江大华技术股份有限公司 一种机器人的定位方法、装置和电子设备
CN112327325A (zh) * 2020-09-16 2021-02-05 安徽意欧斯物流机器人有限公司 一种基于特征路标的提升2d-slam精度和稳定性的方法

Family Cites Families (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
KR101907081B1 (ko) * 2011-08-22 2018-10-11 삼성전자주식회사 3차원 점군의 물체 분리 방법
US9098773B2 (en) * 2013-06-27 2015-08-04 Chevron U.S.A. Inc. System and method of detecting objects in scene point cloud
CN105223583B (zh) * 2015-09-10 2017-06-13 清华大学 一种基于三维激光雷达的目标车辆航向角计算方法
KR102063534B1 (ko) * 2017-11-30 2020-01-09 주식회사 모빌테크 라이다를 이용한 지도 생성 방법
CN109541612A (zh) * 2018-11-09 2019-03-29 同济大学 基于单线激光雷达识别柱状物的机器人室内自定位系统
CN109655805B (zh) * 2019-01-25 2021-12-10 南京理工大学 一种基于扫描线段重合长度估计的激光雷达定位方法
CN110793512A (zh) * 2019-09-11 2020-02-14 上海宾通智能科技有限公司 位姿识别方法、装置、电子设备和存储介质
CN112285738B (zh) * 2020-10-23 2023-01-31 中车株洲电力机车研究所有限公司 一种轨道交通车辆的定位方法及其装置
CN113298833A (zh) * 2021-05-20 2021-08-24 山东大学 目标物点云特征线面提取方法及系统

Patent Citations (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111895989A (zh) * 2020-06-24 2020-11-06 浙江大华技术股份有限公司 一种机器人的定位方法、装置和电子设备
CN112327325A (zh) * 2020-09-16 2021-02-05 安徽意欧斯物流机器人有限公司 一种基于特征路标的提升2d-slam精度和稳定性的方法

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
EP4345767A1 (en) * 2022-09-28 2024-04-03 Tata Consultancy Services Limited Method and system for ambient intelligence based user interaction

Also Published As

Publication number Publication date
CN113689504A (zh) 2021-11-23

Similar Documents

Publication Publication Date Title
CN108765489B (zh) 一种基于组合靶标的位姿计算方法、系统、介质及设备
Alenya et al. Robotized plant probing: Leaf segmentation utilizing time-of-flight data
CN104484648A (zh) 基于轮廓识别的机器人可变视角障碍物检测方法
CN111986219B (zh) 一种三维点云与自由曲面模型的匹配方法
CN109933096B (zh) 一种云台伺服控制方法及系统
EP3229208B1 (en) Camera pose estimation
CN115774265B (zh) 用于工业机器人的二维码和激光雷达融合定位方法与装置
CN107818598B (zh) 一种基于视觉矫正的三维点云地图融合方法
CN101359400A (zh) 一种基于视觉的管口空间位置定位方法
CN113689504B (zh) 基于可描述形状的点云精准定位方法、装置及存储介质
JP2011043969A (ja) 画像特徴点抽出方法
KR101844281B1 (ko) 복잡구조물 용접자동화를 위한 광역 환경 인식 방법 및 시스템
US11034028B2 (en) Pose determining method for mobile robot and apparatus and mobile robot thereof
CN114721001A (zh) 一种基于多传感器融合的移动机器人定位方法
Nüchter et al. Skyline-based registration of 3D laser scans
CN113436262A (zh) 一种基于视觉的车辆目标位置和姿态角度的检测方法
Wang et al. Multilayer positioning strategy for tubesheet welding robot based on point cloud model
CN112212851B (zh) 一种位姿确定方法、装置、存储介质及移动机器人
CN204288242U (zh) 基于曲线三维重建的机器人喷涂轨迹提取装置
CN109782771A (zh) 一种果园移动机器人及地头转向方法
CN115556113A (zh) 一种基于机器人坡口切割工作站的坡口切割方法
CN111508067B (zh) 一种基于垂直平面和垂直线的轻量级室内建模方法
CN110455274A (zh) 基于倒角距离形状匹配的agv初始定位方法及定位系统
CN106767719B (zh) 无人机角度的计算方法及气体遥测方法
CN111191513A (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
GR01 Patent grant
GR01 Patent grant