CN114494329B - 用于移动机器人在非平面环境自主探索的导引点选取方法 - Google Patents

用于移动机器人在非平面环境自主探索的导引点选取方法 Download PDF

Info

Publication number
CN114494329B
CN114494329B CN202210393401.2A CN202210393401A CN114494329B CN 114494329 B CN114494329 B CN 114494329B CN 202210393401 A CN202210393401 A CN 202210393401A CN 114494329 B CN114494329 B CN 114494329B
Authority
CN
China
Prior art keywords
grid
guide point
robot
guide
point
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
CN202210393401.2A
Other languages
English (en)
Other versions
CN114494329A (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.)
Zhejiang Lab
Original Assignee
Zhejiang Lab
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 Zhejiang Lab filed Critical Zhejiang Lab
Priority to CN202210393401.2A priority Critical patent/CN114494329B/zh
Publication of CN114494329A publication Critical patent/CN114494329A/zh
Application granted granted Critical
Publication of CN114494329B publication Critical patent/CN114494329B/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/20—Analysis of motion
    • 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
    • G06Q—INFORMATION AND COMMUNICATION TECHNOLOGY [ICT] SPECIALLY ADAPTED FOR ADMINISTRATIVE, COMMERCIAL, FINANCIAL, MANAGERIAL OR SUPERVISORY PURPOSES; SYSTEMS OR METHODS SPECIALLY ADAPTED FOR ADMINISTRATIVE, COMMERCIAL, FINANCIAL, MANAGERIAL OR SUPERVISORY PURPOSES, NOT OTHERWISE PROVIDED FOR
    • G06Q10/00—Administration; Management
    • G06Q10/04—Forecasting or optimisation specially adapted for administrative or management purposes, e.g. linear programming or "cutting stock problem"
    • G06Q10/047—Optimisation of routes or paths, e.g. travelling salesman problem
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T17/00—Three dimensional [3D] modelling, e.g. data description of 3D objects
    • G06T17/05—Geographic models

Abstract

本发明公开了一种用于移动机器人在非平面环境自主探索的导引点选取方法，包括以下步骤：1）按顺序获取3D激光雷达采集的激光帧数据并确定运动轨迹；2）根据激光帧与其对应位姿构建机器人坐标系下的高程地图；3）根据高程地图计算初始导引点集合以及每个栅格的坡度与粗糙度信息；4）对初始导引点集合进行mean‑shift聚类以及计算到聚类点的路径5）根据聚类导引点以及地图几何信息生成最优导引点。本发明的自主导航的导引点选取方法充分考虑了移动机器人的运动特性，导引点的选取充分考虑了时间成本，空间成本以及安全三大要素，可用于移动机器人在非平面环境下的自主快速且安全的探索。

Description

用于移动机器人在非平面环境自主探索的导引点选取方法
技术领域
本发明涉及计算机视觉技术领域，特别是涉及一种用于移动机器人在非平面环境自主探索的导引点选取方法。
背景技术
近年来，随着机器人与人工智能的大力发展，自主移动机器人越来越受到工程界和学术界的关注。机器人具备自主智能功能的首要关键技术是机器人具备在未知环境中的自主探索功能，并利用传感器在未知环境中进行定位与建图。但是传统建图方式大多为人为控制机器人移动，针对大而复杂的场景将耗费巨大的时间、人力与物力。目前学术界与工业界也涌现了大量针对室内场景的自主探索方法。
针对目前绝大多数自主探索机器人主要应用在2D平面场景，其原理主要以地面为标准面去评价前方区域的可通行性，当遇到可通过的坡或坑以及颠簸路面时，它们的可通行区域的评价方式会认为该区域不可通行，使得机器人无法探索更多的未知区域甚至错误的认为前方区域为障碍物。
发明内容
有鉴于此，本发明提供一种用于移动机器人在非平面环境自主探索的导引点选取方法，结合了移动机器人的运动特性，考虑了环境的坡度与粗糙度信息，同时考虑了运动时间成本、空间成本以及信息增益因素，可拓展用于移动机器人在非平面环境下的自主快速且安全的探索。
本发明采用如下技术方案：
一种用于移动机器人在非平面环境自主探索的导引点选取方法，包括以下步骤：
步骤一：按顺序获取3D激光雷达采集的激光帧数据并确定运动轨迹，得到包含运动轨迹的激光帧序列；将第一帧激光雷达作为初始帧，选取一定数量的连续的激光雷达帧构建局部子地图；且所述局部子地图的第一帧与最后一帧的位姿的运动平移距离大于预设距离或旋转角度大于预设角度；
步骤二：根据激光帧与其对应位姿构建机器人坐标系下的高程地图，且该高程地图的每个栅格信息要包含栅格状态；所述栅格状态为占据、空闲与未知中的任一种；
步骤三：根据高程地图计算初始导引点集合以及每个栅格的坡度与粗糙度信息；
步骤四：对初始导引点集合进行mean-shift聚类，得到聚类导引点，并计算当前的机器人中心到每个聚类导引点的路径；
步骤五：根据聚类导引点以及地图几何信息生成最优导引点。
进一步地，所述步骤二具体通过如下的子步骤来实现：
创建以机器人中心为基点，机器人朝向前左上分别对应X-Y-Z构建坐标系；构建以机器人中心为中心的高程地图，该高程地图的每个栅格包含的信息为
Figure 118516DEST_PATH_IMAGE001
；其中，
Figure 7974DEST_PATH_IMAGE002
为第c个栅格的中心点坐标，h c 为落在该栅格的所有3D点的最大的z值，s c 为该栅格的状态，其状态包括占据、空闲与未知三种，分别用occ，free，un表示；所述高程地图的边长的上限为机器人使用传感器的最大感知距离，下限为所述预设距离。
进一步地，所述步骤三具体通过如下的子步骤来实现：
（1）遍历步骤二中高程地图
Figure 214965DEST_PATH_IMAGE003
的每一个栅格，取出以该栅格为中心的4邻域与8邻域，若该栅格状态为空闲且其4邻域中至少有一个的状态为未知，则该栅格被标记为初始候选导引点，最终得到初始候选导引点集合
Figure 531677DEST_PATH_IMAGE004
；
其中，
Figure 925749DEST_PATH_IMAGE005
，其中， W为整个高程地图的X方向的长度，H为整个高程地图的Y方向的长度，r为每个栅格的分辨率，c表示第c个栅格，
Figure 618898DEST_PATH_IMAGE006
；
（2）将每个栅格与其8邻域组成的共9个点的集合用于PCA主成分解，将其中分解的第三个奇异值对应的反余弦值作为坡度信息，用k c 表示；同时以9个点集合计算该栅格的平均高度h mean ，并通过下式粗糙度r c ：
Figure 414816DEST_PATH_IMAGE007
其中，abs()为取绝对值；
（3）更新每个栅格包含的信息
Figure 168008DEST_PATH_IMAGE008
。
进一步地，所述步骤四中，采用D *路径规划算法计算当前的机器人中心到每个聚类导引点的路径。
进一步地，所述步骤五具体通过如下的子步骤来实现：
遍历聚类导引点与其对应路径的集合中的二元组，通过下式计算每个聚类导引点的最终收益，并将收益最大的聚类导引点作为最优导引点：
Figure 314956DEST_PATH_IMAGE009
其中
Figure 811796DEST_PATH_IMAGE010
为三个控制参数，
Figure 462221DEST_PATH_IMAGE011
为导引点
Figure 386314DEST_PATH_IMAGE012
的信息增益，其定义为以
Figure 754979DEST_PATH_IMAGE012
为中心的栅格中状态为未知的格子的个数；
Figure 789931DEST_PATH_IMAGE013
为机器人中心与聚类导引点的欧式距离，
Figure 826020DEST_PATH_IMAGE014
为
Figure 655436DEST_PATH_IMAGE012
对应的路径
Figure 774046DEST_PATH_IMAGE015
中所有点对应栅格的坡度
Figure 612689DEST_PATH_IMAGE016
与粗糙度
Figure 972126DEST_PATH_IMAGE017
之和平均，
Figure 503601DEST_PATH_IMAGE018
；
Figure 581279DEST_PATH_IMAGE019
为机器人当前位置,
Figure 489192DEST_PATH_IMAGE020
。
进一步地，所述高程地图的每个栅格的大小为
Figure 968715DEST_PATH_IMAGE021
，分辨率为
Figure 405512DEST_PATH_IMAGE022
。
进一步地，所述信息增益
Figure 970486DEST_PATH_IMAGE011
定义为以
Figure 416511DEST_PATH_IMAGE012
为中心，长宽度为40个栅格中状态为未知的格子的个数。
进一步地，所述步骤一中的预设距离为5m，预设角度为30°。
一种用于移动机器人在非平面环境自主探索的导引点选取装置，该装置包括一个或者多个处理器，用于实现上述的导引点选取方法。
一种计算机可读存储介质，其上存储有程序，该程序被处理器执行时，实现上述的导引点选取方法。
与现有技术相比，本发明的有益效果是：
（1）本发明的方法在导引点的选取方法上结合了机器人的移动特性，方法不仅仅适用于纯地面环境，也可以用于户外3D环境。
（2）该方法充分考虑了局部地图的几何信息，通过计算局部环境的坡度与粗糙度信息，对潜在导引点进行聚类并计算增益，最终充分考虑了下一个导引点的时间、空间与信息增益等要素，克服了传统移动机器人针对环境平面化建模的问题，对环境进行更细致的建模，使得机器人能够在非平面环境进行快速且有效的探索。
附图说明
图1为本发明用于移动机器人在非平面环境自主探索的导引点选取方法的流程图。
图2为本发明计算坡度与粗糙度示意图；其中，图2中的（a）为坡度计算示意图，图2中的（b）为粗糙度计算示意图。
图3为本发明搭建坡度与障碍物仿真环境示意图；其中，图3中的（a）表示环境主视图，图3中的（b）表示环境右视图。
图4为本发明方法与传统基于地面分割方法生成栅格对比示意图；其中，图4中的（a）为传统基于平面分割方法栅格地图，图4中的（b）为本发明方法生成的栅格地图。
图5为本发明方法选择导引点的示意图。
图6为本发明的装置的实施例的示意图。
具体实施方式
下面结合附图和具体实施方式进一步说明本发明的原理和方案，显然，所描述的实施例仅仅是本发明一部分实施例，此处所描述的具体实施例仅仅用于解释相关发明，而非对该发明的限定。
图1是本发明用于移动机器人在非平面环境自主探索的导引点选取方法的流程示意图，所述自主探索的导引点选取方法包括以下步骤：
步骤一：按顺序获取3D激光雷达采集的激光帧数据并确定运动轨迹，得到包含运动轨迹的激光帧序列
Figure 750540DEST_PATH_IMAGE023
，其中，
Figure 358239DEST_PATH_IMAGE024
表示第k帧对应的雷达数据，
Figure 676088DEST_PATH_IMAGE025
为第k帧雷达相对于世界坐标系的位姿，
Figure 394645DEST_PATH_IMAGE026
为激光雷达与机体坐标系的外参，机体坐标系以机器人中心为基点，机器人朝向前左上分别对应X-Y-Z构建坐标系，N为雷达的帧数；将第一帧激光雷达作为初始帧，选取
Figure 848760DEST_PATH_IMAGE027
个激光雷达帧构建局部子地图
Figure 892940DEST_PATH_IMAGE028
，其中
Figure 432505DEST_PATH_IMAGE029
表示第
Figure 220333DEST_PATH_IMAGE029
个子地图，
Figure 528954DEST_PATH_IMAGE030
为
Figure 475526DEST_PATH_IMAGE031
个激光雷达根据其位姿拼接并转换到机体坐标系的点云集合，且局部子地图的第一帧
Figure 502388DEST_PATH_IMAGE032
与最后一帧的位姿
Figure 828327DEST_PATH_IMAGE033
运动平移距离大于预设距离或旋转角度大于预设角度。
在该实施例中，预设距离和角度的选择为可配置参数，但是距离的设置一般会考虑到使用传感器的种类、安装方式、安装高度及其观测有效距离，本实施例中以16线激光雷达，水平安装且离地高度为0.7m，由于16线激光雷达的垂直观测视场角为正负15度，而本发明的选取导引点的方式需要保证地图的稠密性，同时为了保证计算资源以及机器人运行稳定，将预设距离设定为5m，预设角度设定为30°。
步骤二：根据激光帧与其对应位姿构建机器人坐标系下的高程地图，且该高程地图的每个栅格信息要包含栅格状态；所述栅格状态为占据、空闲与未知中的任一种。
针对上述得到的第
Figure 991455DEST_PATH_IMAGE034
个局部子地图
Figure 111858DEST_PATH_IMAGE035
，其地图的表达形式为3D点云
Figure 891595DEST_PATH_IMAGE036
，根据上述
Figure 755646DEST_PATH_IMAGE036
构建以机器人为中心的高程地图。所述高程地图的边长的选择具有一定的约束，其中边长的上限为传感器最大感知距离，例如本实施例中采用激光雷达，一般激光雷达的感知距离为100m，所以在本实施例中，其上限为100m，而下限则与机器人尺寸，机器人平均行驶速度相关，一般取平均速度下10s行驶的距离作为边长的一半，本实施例中机器人平均行驶速度为0.5m/s，于是其下限为10m。栅格分辨率的选取与机器人尺寸，以及避障碍精度相关，一般在0.05-0.2m之间选取。依据该选取原则，构建
Figure 773281DEST_PATH_IMAGE021
，栅格分辨率为
Figure 595743DEST_PATH_IMAGE022
的高程地图
Figure 331618DEST_PATH_IMAGE037
,每个栅格包含的信息为
Figure 999360DEST_PATH_IMAGE038
，
Figure 871501DEST_PATH_IMAGE039
为第c个栅格的中心点，h c 为落在该栅格的所有3D点
Figure 864864DEST_PATH_IMAGE040
值最大的点，s c 为该栅格的状态，其状态共有三种占据，空闲与未知，分别用occ，free，un表示。其中占据表示该栅格已被当前传感器所探测到，且该位置是有物体存在的；空闲表示该栅格也被探测到，但是该栅格的位置是不存在实际物体的；未知则表示该栅格并没有被传感器所观测到，未知栅格的数量能在一定程度上反应探索增益。
步骤三：根据高程地图计算初始导引点集合以及每个栅格的坡度与粗糙度信息。
遍历步骤二中高程地图
Figure 88035DEST_PATH_IMAGE041
的每一个栅格，以第c个栅格为例，取出以它为中心的4邻域与8邻域，若该栅格状态为空闲且其4邻域中至少有一个的状态为未知，则该点被标记为初始候选导引点，最终得到初始候选导引点集合
Figure 293889DEST_PATH_IMAGE042
；第c个栅格与其8邻域组成的共9个点
Figure 551695DEST_PATH_IMAGE043
集合用于PCA主成分解，其中分解的第三个奇异值对应的反余弦值为坡度信息，用
Figure 450381DEST_PATH_IMAGE044
表示，同时以9个点集合
Figure 895268DEST_PATH_IMAGE045
计算该栅格的平均高度
Figure 904813DEST_PATH_IMAGE046
，其中粗糙度信息定义为
Figure 279775DEST_PATH_IMAGE047
，abs()为取绝对值。更新此时每个栅格所包含的信息为
Figure 83783DEST_PATH_IMAGE048
,其具体计算每个栅格的坡度信息与粗糙度信息见图2。图2中的（a）中，θ为坡度值，n为过中心点且与平面垂直的法向量，z为与Z轴平行的向量，nz为向量n在z方向上的投影，因此，坡度值θ为向量n与nz的夹角。
步骤四：对初始导引点集合进行mean-shift聚类得到聚类导引点，并计算当前的机器人中心到每个聚类导引点的路径。
步骤三中的初始候选导引点集合
Figure 547125DEST_PATH_IMAGE049
数量多，且呈现分块和密集状态，需要对导引点进行聚类，选择合适的导引点，经过mean-shift聚类之后的候选导引点集合为
Figure 94781DEST_PATH_IMAGE042
；针对聚类之后的导引点
Figure 61600DEST_PATH_IMAGE050
，利用
Figure 302089DEST_PATH_IMAGE051
路径规划算法计算对应的路径
Figure 987148DEST_PATH_IMAGE052
，此时得到导引点与路径的集合
Figure 338495DEST_PATH_IMAGE053
。
步骤五：根据聚类导引点以及地图几何信息生成最优导引点。
步骤五包含以下子步骤：
遍历聚类导引点与路径的集合中的二元组，其中机器人当前位置为
Figure 159820DEST_PATH_IMAGE054
,计算每个聚类导引点的最终收益，其中收益函数表达式为：
Figure 836789DEST_PATH_IMAGE055
其中
Figure 743565DEST_PATH_IMAGE056
为三个控制参数，
Figure 633024DEST_PATH_IMAGE057
为导引点
Figure 840014DEST_PATH_IMAGE058
的信息增益，其定义为以
Figure 156726DEST_PATH_IMAGE058
为中心，长宽度为40个栅格中状态为未知的格子的状态的个数，
Figure 550798DEST_PATH_IMAGE059
为机器人中心与导引点的欧式距离，
Figure 243948DEST_PATH_IMAGE060
为
Figure 39866DEST_PATH_IMAGE058
对应的路径
Figure 58637DEST_PATH_IMAGE061
中所有点对应栅格的坡度
Figure 940006DEST_PATH_IMAGE062
与粗糙度
Figure 168337DEST_PATH_IMAGE063
之和平均；最终最优的导引点为
Figure 818761DEST_PATH_IMAGE064
，对应路径为
Figure 8434DEST_PATH_IMAGE061
。
Figure 377099DEST_PATH_IMAGE056
三个控制参数不同的设置将会影响最终最优导引点的选取，其中
Figure 412051DEST_PATH_IMAGE065
为正数，其数值越大，说明最优导引点越偏向导引点的信息增益；
Figure 182561DEST_PATH_IMAGE066
为正数，其数值越大，说明对导引点的欧式距离惩罚越大，即机器人倾向选择距离当前位置近的导引点；
Figure 277556DEST_PATH_IMAGE067
为正数，其数值越大，说明对到达该导引点的困难程度惩罚越大，即机器人倾向选择能够以较为平缓的方式到达的导引点作为最优导引点。
将本发明的方法在搭建的仿真环境中进行验证实验，如图3所示，搭建的环境包含竖直障碍物与一个坡度为10度的坡度，该实验一方面用于验证本发明的所有步骤流程，另一方面突出说明本发明能够区别于传统以平面分割的方式作为选取导引点的方法。根据步骤三每个栅格的坡度信息，按照传统基于平面假设的方法，所搭建的10度坡将被认为是障碍物，机器人将不再探索，具体如图4所示，其中，base_link为机器人中心，本发明的方法通过对局部环境进行更为细致的建模，计算局部坡度信息，在分析中将被认为是可通行区域，机器人可以通行该区域并进行下一步探索。
图5为本发明的方法在楼道环境进行自主探索的一次导引点选择示意图，
Figure 133516DEST_PATH_IMAGE068
和
Figure 706580DEST_PATH_IMAGE069
分别为通过步骤四聚类之后的候选导引点，
Figure 597176DEST_PATH_IMAGE070
和
Figure 128651DEST_PATH_IMAGE071
为两个候选导引点的信息增益，其中图5中网格为0.5m的分辨率，因此按照计算
Figure 206328DEST_PATH_IMAGE070
=600，
Figure 848662DEST_PATH_IMAGE071
=800；针对欧氏距离项
Figure 328185DEST_PATH_IMAGE072
=7.43，
Figure 764983DEST_PATH_IMAGE073
=7.52。沿两个导引点方向的坡度与粗糙度之和平均相等。所以该项忽略，取
Figure 329956DEST_PATH_IMAGE074
；最终计算可得
Figure 510402DEST_PATH_IMAGE075
=2.28，
Figure 110011DEST_PATH_IMAGE076
=3.01，由于
Figure 714780DEST_PATH_IMAGE077
,因此在图5实例中选择导引点2作为下一个导引点。
因此，本发明的用于移动机器人在非平面环境自主探索导引点选取方法一方面能够解决传统基于地面分割的方法错误的将缓坡认为是障碍物，另一方面在导引点的选取上充分考虑了机器人的运动特性、时间空间成本以及导引点的信息增益，使得导引点的选取更精确。
与前述用于移动机器人在非平面环境自主探索导引点选取方法的实施例相对应，本发明还提供了用于移动机器人在非平面环境自主探索导引点选取装置的实施例。
参见图6，本发明实施例提供的用于移动机器人在非平面环境自主探索导引点选取装置，包括一个或多个处理器，用于实现上述实施例中的用于移动机器人在非平面环境自主探索导引点选取方法。
本发明用于移动机器人在非平面环境自主探索导引点选取装置的实施例可以应用在任意具备数据处理能力的设备上，该任意具备数据处理能力的设备可以为诸如计算机等设备或置。装置实施例可以通过软件实现，也可以通过硬件或者软硬件结合的方式实现。
Figure 32629DEST_PATH_IMAGE078
以软件实现为例，作为一个逻辑意义上的装置，是通过其所在任意具备数据处理能力的设备的处理器将非易失性存储器中对应的计算机程序指令读取到内存中运行形成的。从硬件层面而言，如图6所示，为本发明用于移动机器人在非平面环境自主探索导引点选取装置所在任意具备数据处理能力的设备的一种硬件结构图，除了图6所示的处理器、内存、网络接口、以及非易失性存储器之外，实施例中装置所在的任意具备数据处理能力的设备通常根据该任意具备数据处理能力的设备的实际功能，还可以包括其他硬件，对此不再赘述。
上述装置中各个单元的功能和作用的实现过程具体详见上述方法中对应步骤的实现过程，在此不再赘述。
对于装置实施例而言，由于其基本对应于方法实施例，所以相关之处参见方法实施例的部分说明即可。以上所描述的装置实施例仅仅是示意性的，其中所述作为分离部件说明的单元可以是或者也可以不是物理上分开的，作为单元显示的部件可以是或者也可以不是物理单元，即可以位于一个地方，或者也可以分布到多个网络单元上。可以根据实际的需要选择其中的部分或者全部模块来实现本发明方案的目的。本领域普通技术人员在不付出创造性劳动的情况下，即可以理解并实施。
本发明实施例还提供一种计算机可读存储介质，其上存储有程序，该程序被处理器执行时，实现上述实施例中的用于移动机器人在非平面环境自主探索导引点选取方法。所述计算机可读存储介质可以是前述任一实施例所述的任意具备数据处理能力的设备的内部存储单元，例如硬盘或内存。所述计算机可读存储介质也可以是外部存储设备，例如所述设备上配备的插接式硬盘、智能存储卡(SmartMedia card, SMC)、SD卡、闪存卡(Flashcard)等。进一步的，所述计算机可读存储介质还可以既包括任意具备数据处理能力的设备的内部存储单元也包括外部存储设备。所述计算机可读存储介质用于存储所述计算仉程序以及所述任意具备数据处理能力的设备所需的其他程序和数据，还可以用于暂时地存储己经输出或者将要输出的数据。
以上，仅为本发明较佳的具体实施方式；但本发明的保护范围并不局限于此。任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，根据本发明的技术方案及其改进构思加以等同替换或改变，都应涵盖在本发明的保护范围内。

Claims (9)

1.一种用于移动机器人在非平面环境自主探索的导引点选取方法，其特征在于：包括以下步骤：
步骤一：按顺序获取3D激光雷达采集的激光帧数据并确定运动轨迹，得到包含运动轨迹的激光帧序列；将第一帧激光雷达作为初始帧，选取一定数量的连续的激光雷达帧构建局部子地图；且所述局部子地图的第一帧与最后一帧的位姿的运动平移距离大于预设距离或旋转角度大于预设角度；
步骤二：根据激光帧与其对应位姿构建机器人坐标系下的高程地图，且该高程地图的每个栅格信息要包含栅格状态；所述栅格状态为占据、空闲与未知中的任一种；
步骤三：根据高程地图计算初始导引点集合以及每个栅格的坡度与粗糙度信息；
步骤四：对初始导引点集合进行mean-shift聚类，得到聚类导引点，并计算当前的机器人中心到每个聚类导引点的路径；
步骤五：根据聚类导引点以及地图几何信息生成最优导引点；
所述步骤五具体通过如下的子步骤来实现：
遍历聚类导引点与其对应路径的集合中的二元组，通过下式计算每个聚类导引点的最终收益，并将收益最大的聚类导引点作为最优导引点：
Figure 403443DEST_PATH_IMAGE001
其中，
Figure 386180DEST_PATH_IMAGE002
为三个控制参数，
Figure 638301DEST_PATH_IMAGE003
为导引点
Figure 711299DEST_PATH_IMAGE004
的信息增益，其定义为以
Figure 521998DEST_PATH_IMAGE004
为中心的栅格中状态为未知的格子的个数；
Figure 165600DEST_PATH_IMAGE005
为机器人中心与聚类导引点的欧式距离，
Figure 673942DEST_PATH_IMAGE006
为
Figure 601447DEST_PATH_IMAGE004
对应的路径
Figure 786309DEST_PATH_IMAGE007
中所有点对应栅格的坡度
Figure 166475DEST_PATH_IMAGE008
与粗糙度
Figure 947349DEST_PATH_IMAGE009
之和求平均，
Figure 480093DEST_PATH_IMAGE010
；
Figure 321010DEST_PATH_IMAGE011
为机器人当前位置,
Figure 922892DEST_PATH_IMAGE012
。
2.根据权利要求1所述的用于移动机器人在非平面环境自主探索的导引点选取方法，其特征在于，所述步骤二具体通过如下的子步骤来实现：
创建以机器人中心为基点，机器人朝向前左上分别对应X-Y-Z构建坐标系；构建以机器人中心为中心的高程地图，该高程地图的每个栅格包含的信息为
Figure 282024DEST_PATH_IMAGE013
；其中，
Figure 918542DEST_PATH_IMAGE014
为第c个栅格的中心点坐标，h c 为落在该栅格的所有3D点的最大的z值，s c 为该栅格的状态，其状态包括占据、空闲与未知三种，分别用occ，free，un表示；所述高程地图的边长的上限为机器人使用传感器的最大感知距离，下限为所述预设距离。
3.根据权利要求2所述的用于移动机器人在非平面环境自主探索的导引点选取方法，其特征在于，所述步骤三具体通过如下的子步骤来实现：
（1）遍历步骤二中高程地图
Figure 930360DEST_PATH_IMAGE015
的每一个栅格，取出以该栅格为中心的4邻域与8邻域，若该栅格状态为空闲且其4邻域中至少有一个的状态为未知，则该栅格被标记为初始候选导引点，最终得到初始候选导引点集合
Figure 770271DEST_PATH_IMAGE016
；
其中，
Figure 424107DEST_PATH_IMAGE017
，其中， W为整个高程地图的X方向的长度，H为整个高程地图的Y方向的长度，r为每个栅格的分辨率，c表示第c个栅格，
Figure 649552DEST_PATH_IMAGE018
；
（2）将每个栅格与其8邻域组成的共9个点的集合用于PCA主成分解，将其中分解的第三个奇异值对应的反余弦值作为坡度信息，用k c 表示；同时以9个点集合计算该栅格的平均高度h mean ，并通过下式计算粗糙度r c ：
Figure 81539DEST_PATH_IMAGE019
其中，abs()为取绝对值；
（3）更新每个栅格包含的信息
Figure 923593DEST_PATH_IMAGE020
。
4.根据权利要求1所述的用于移动机器人在非平面环境自主探索的导引点选取方法，其特征在于，所述步骤四中，采用D *路径规划算法计算当前的机器人中心到每个聚类导引点的路径。
5.根据权利要求2所述的用于移动机器人在非平面环境自主探索的导引点选取方法，其特征在于，所述高程地图的每个栅格的大小为
Figure 115540DEST_PATH_IMAGE021
，分辨率为
Figure 211803DEST_PATH_IMAGE022
。
6.根据权利要求1所述的用于移动机器人在非平面环境自主探索的导引点选取方法，其特征在于，所述信息增益
Figure 831003DEST_PATH_IMAGE003
定义为以
Figure 894774DEST_PATH_IMAGE004
为中心，长宽度为40个栅格中状态为未知的格子的个数。
7.根据权利要求1所述的用于移动机器人在非平面环境自主探索的导引点选取方法，其特征在于，所述步骤一中的预设距离为5m，预设角度为30°。
8.一种用于移动机器人在非平面环境自主探索的导引点选取装置，其特征在于，该装置包括一个或者多个处理器，用于实现权利要求1-7中任一项所述的导引点选取方法。
9.一种计算机可读存储介质，其上存储有程序，该程序被处理器执行时，实现权利要求1-7中任一项所述的导引点选取方法。
CN202210393401.2A 2022-04-15 2022-04-15 用于移动机器人在非平面环境自主探索的导引点选取方法 Active CN114494329B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202210393401.2A CN114494329B (zh) 2022-04-15 2022-04-15 用于移动机器人在非平面环境自主探索的导引点选取方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202210393401.2A CN114494329B (zh) 2022-04-15 2022-04-15 用于移动机器人在非平面环境自主探索的导引点选取方法

Publications (2)

Publication Number Publication Date
CN114494329A CN114494329A (zh) 2022-05-13
CN114494329B true CN114494329B (zh) 2022-07-26

Family

ID=81489367

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202210393401.2A Active CN114494329B (zh) 2022-04-15 2022-04-15 用于移动机器人在非平面环境自主探索的导引点选取方法

Country Status (1)

Country Link
CN (1) CN114494329B (zh)

Families Citing this family (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115014362B (zh) * 2022-08-09 2022-11-15 之江实验室 一种基于合成单元的牛耕式全覆盖路径规划方法和装置
CN116203972B (zh) * 2023-05-05 2023-08-18 北京理工大学 一种未知环境探索路径规划方法、系统、设备及介质

Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2004100148A (ja) * 2002-09-04 2004-04-02 Mitsubishi Research Institute Inc 道路線形設計方法、装置およびプログラム
CN103869820A (zh) * 2014-03-18 2014-06-18 北京控制工程研究所 一种巡视器地面导航规划控制方法
CN105843222A (zh) * 2016-03-18 2016-08-10 北京航空航天大学 一种六轮/腿机器人复合运动路径规划方法
CN110221614A (zh) * 2019-06-14 2019-09-10 福州大学 一种基于快速探索随机树的多机器人地图探索方法
CN113867340A (zh) * 2021-09-17 2021-12-31 北京控制工程研究所 一种地外未知环境超视距全局路径规划系统和方法

Family Cites Families (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CA2238127A1 (en) * 1998-05-15 1999-11-15 Terry Bergan Truck traffic monitoring and warning systems and vehicle ramp advisory system
CN102183895B (zh) * 2011-03-24 2012-10-03 杭州电子科技大学 一种新型的排水管网水力学系统建模与控制方法
FR2976355B1 (fr) * 2011-06-09 2013-06-21 Jean Luc Desbordes Dispositif de mesure de vitesse et de position d'un vehicule se deplacant le long d'une voie de guidage, procede et produit programme d'ordinateur correspondant.
CN115516516A (zh) * 2020-03-04 2022-12-23 奇跃公司 用于根据室内场景的3d扫描的高效平面图生成的系统和方法

Patent Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2004100148A (ja) * 2002-09-04 2004-04-02 Mitsubishi Research Institute Inc 道路線形設計方法、装置およびプログラム
CN103869820A (zh) * 2014-03-18 2014-06-18 北京控制工程研究所 一种巡视器地面导航规划控制方法
CN105843222A (zh) * 2016-03-18 2016-08-10 北京航空航天大学 一种六轮/腿机器人复合运动路径规划方法
CN110221614A (zh) * 2019-06-14 2019-09-10 福州大学 一种基于快速探索随机树的多机器人地图探索方法
CN113867340A (zh) * 2021-09-17 2021-12-31 北京控制工程研究所 一种地外未知环境超视距全局路径规划系统和方法

Non-Patent Citations (3)

* Cited by examiner, † Cited by third party
Title
Aerial remote sensing in agriculture：a practical approach to area coverage and path planning for fleets of mini aerial robots;barrientos cruz et al.;《journal of field robotics》;20161231;第28卷;第1-10页 *
Autonomously traversing obstacles-metrics for path planning of reconfigurable robots on rough terrain;brunner m.b brueggemann et al.;《international conference on informatics in control》;20151231;第1-10页 *
一种基于高程图的危险区识别算法;耿殿伍等;《哈尔滨工程大学学报》;20061231(第06期);第946-949页 *

Also Published As

Publication number Publication date
CN114494329A (zh) 2022-05-13

Similar Documents

Publication Publication Date Title
US11816907B2 (en) Systems and methods for extracting information about objects from scene information
CN105843223B (zh) 一种基于空间词袋模型的移动机器人三维建图与避障方法
CN106599108B (zh) 一种三维环境中多模态环境地图构建方法
CN114494329B (zh) 用于移动机器人在非平面环境自主探索的导引点选取方法
Li et al. Efficient laser-based 3D SLAM for coal mine rescue robots
CN110472585B (zh) 一种基于惯导姿态轨迹信息辅助的vi-slam闭环检测方法
Huang et al. A fast point cloud ground segmentation approach based on coarse-to-fine Markov random field
Ding et al. Vehicle pose and shape estimation through multiple monocular vision
CN110986945B (zh) 基于语义高度地图的局部导航方法和系统
GB2610410A (en) Incremental dense 3-D mapping with semantics
Wiesmann et al. Locndf: Neural distance field mapping for robot localization
Wang et al. 3D-LIDAR based branch estimation and intersection location for autonomous vehicles
An et al. Multilevel ground segmentation for 3-D point clouds of outdoor scenes based on shape analysis
CN114387576A (zh) 一种车道线识别方法、系统、介质、设备及信息处理终端
Liu et al. Comparison of 2D image models in segmentation performance for 3D laser point clouds
Hu et al. A combined clustering and image mapping based point cloud segmentation for 3D object detection
CN115855086A (zh) 基于自旋转的室内场景自主重建方法、系统及介质
Nabbe et al. Opportunistic use of vision to push back the path-planning horizon
Li et al. Efficient laser-based 3D SLAM in real time for coal mine rescue robots
Zhang et al. An improvement algorithm for OctoMap based on RGB-D SLAM
Yang et al. An optimization-based selection approach of landing sites for swarm unmanned aerial vehicles in unknown environments
Muravyev et al. Evaluation of Topological Mapping Methods in Indoor Environments
Kuo et al. A hybrid approach to RBPF based SLAM with grid mapping enhanced by line matching
Hong et al. Design and Implementation of Path Planning for Wheel-Track Hybrid Mobile Robot
Andert et al. A fast and small 3-d obstacle model for autonomous applications

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