CN115129057B - 基于通行效率评估的机器人分层式自主探索方法 - Google Patents
基于通行效率评估的机器人分层式自主探索方法 Download PDFInfo
- Publication number
- CN115129057B CN115129057B CN202210793053.8A CN202210793053A CN115129057B CN 115129057 B CN115129057 B CN 115129057B CN 202210793053 A CN202210793053 A CN 202210793053A CN 115129057 B CN115129057 B CN 115129057B
- Authority
- CN
- China
- Prior art keywords
- robot
- area
- path
- local
- exploration
- 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
Links
- 238000000034 method Methods 0.000 title claims abstract description 27
- 238000011156 evaluation Methods 0.000 title claims abstract description 25
- 238000012545 processing Methods 0.000 claims abstract description 9
- 230000008569 process Effects 0.000 claims abstract description 6
- 230000008901 benefit Effects 0.000 claims description 20
- 238000005457 optimization Methods 0.000 claims description 11
- 238000003860 storage Methods 0.000 claims description 7
- 239000011159 matrix material Substances 0.000 claims description 6
- 238000001514 detection method Methods 0.000 claims description 5
- 238000010276 construction Methods 0.000 claims description 4
- 238000005520 cutting process Methods 0.000 claims description 3
- 238000000354 decomposition reaction Methods 0.000 claims description 3
- 238000012423 maintenance Methods 0.000 claims description 3
- 230000011218 segmentation Effects 0.000 claims description 3
- 230000006870 function Effects 0.000 description 10
- 238000010586 diagram Methods 0.000 description 9
- 238000004590 computer program Methods 0.000 description 7
- 238000004422 calculation algorithm Methods 0.000 description 4
- 241000282414 Homo sapiens Species 0.000 description 3
- 238000004088 simulation Methods 0.000 description 3
- 238000004364 calculation method Methods 0.000 description 2
- 230000007547 defect Effects 0.000 description 2
- 238000005516 engineering process Methods 0.000 description 2
- 238000012986 modification Methods 0.000 description 2
- 230000004048 modification Effects 0.000 description 2
- 230000004075 alteration Effects 0.000 description 1
- 238000004458 analytical method Methods 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000013461 design Methods 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 230000018109 developmental process Effects 0.000 description 1
- 230000007613 environmental effect Effects 0.000 description 1
- 238000004519 manufacturing process Methods 0.000 description 1
- 230000003287 optical effect Effects 0.000 description 1
- 230000008447 perception Effects 0.000 description 1
- 238000011160 research Methods 0.000 description 1
- 238000013468 resource allocation Methods 0.000 description 1
- 239000000126 substance Substances 0.000 description 1
Classifications
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
- G05D1/0223—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory involving speed control of the vehicle
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
- G05D1/0221—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory involving a learning process
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0276—Control of position or course in two dimensions specially adapted to land vehicles using signals provided by a source external to the vehicle
Landscapes
- Engineering & Computer Science (AREA)
- Aviation & Aerospace Engineering (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
本发明涉及一种基于通行效率评估的机器人分层式自主探索方法，包括以下步骤：步骤S1:获取待规划区域三维点云数据，并将环境区域划分为稀疏区域和稠密区域；步骤S2:基于待规划区域三维点云数据，进行区域通行效率评估；步骤S3:综合评估机器人探索代价和收益，利用稀疏区域点云信息进行自主探索全局规划，生成全局路径；步骤S4:根据机器人实时位置和机器人附近稠密区域的点云信息，进行局部路径规划，生成局部精细路径；步骤S5:连接全局路径和局部路径，生成自主探索有序路径点，并将处理结果存入列表中，以此实现分层式自主探索框架；步骤S6:对机器人探索过程中生成地图进行更新。本发明能够实现移动机器人自主探索未知环境并建立环境地图。
Description
技术领域
本发明涉及工业移动机器人设计领域，具体涉及一种基于通行效率评估的机器人分层式自主探索方法。
背景技术
随着移动机器人技术的发展，机器人探索未知环境成为研究热门，而业界对机器人的自主性探索提出了新的挑战。自主探索是机器人通过自主导航和路径规划逐步的将环境地图建立起来，现在对地图的完整性、地图质量和探索效率的要求越来越高。移动机器人在未知复杂的大规模环境中应用受到很大的限制，其主要原因如下：在危险环境救援等特殊复杂环境下，通常人类无法进入现场获取环境信息，必须依赖移动机器人实现对环境的探测和模型的建立，而环境模型的建立和识别是智能机器人的基础；机器人和人类对周围环境信息的感知方式不同，传统的人类思维地图模型并不适合机器人对空间环境的理解和使用，必须创建适合机器人算法和存储的地图模型；为实现未知复杂的大规模环境的探索遍历，必须制定合理有效的移动机器人自主探索方案。但之前的机器人自主探索技术往往只考虑了导航成本，且导航成本往往用单一的指标来评估，常用欧式距离来作为评估指标，而未考虑到复杂环境下的机器人通行效率问题。之前的算法往往只考虑导航成本和地图完整性，但忽略了地图质量问题。出于计算资源分配的考虑，移动机器人如何用更少的时间和更短的探索路径长度来实现自主构建完整的高质量环境地图更为关键。
发明内容
有鉴于此，本发明的目的在于提供一种基于通行效率评估的机器人分层式自主探索方法，能够克服现有技术的不足，引导机器人自主探索未知环境并建立环境地图。
为实现上述目的，本发明采用如下技术方案：
一种基于通行效率评估的机器人分层式自主探索方法，包括以下步骤：
步骤S1:获取待规划区域三维点云数据，并采用环境检测模块将环境区域划分为稀疏区域和稠密区域；
步骤S2:基于待规划区域三维点云数据，进行区域通行效率评估；
步骤S3:综合评估机器人探索代价和收益，利用稀疏区域点云信息进行自主探索全局规划，生成全局路径；
步骤S4:根据机器人实时位置和机器人附近稠密区域的点云信息，进行局部路径规划，生成局部精细路径；
步骤S5:连接全局路径和局部路径，生成自主探索有序路径点，并将处理结果存入列表中，以此实现分层式自主探索框架；
步骤S6:对机器人探索过程中生成地图进行更新。
进一步的，所述稀疏和稠密区域划分，具体为：将机器人当前位置获取到的三维点云进行网格划分，以机器人当前位置为中心，设定的局部规划范围内且点云数量大于阈值上限的网格为稠密区域，局部规划范围外，点云数量大于阈值下限且小于阈值上限的区域为稀疏区域，稀疏和稠密区域状态划分如公式1所示：
式中Cxy代表划分网格，N(Cxy)代表网格中点云数量，Xthreshold1和Xthreshold2代表人为设定的阈值，Cxy＜Rlocal和Cxy＞Rlocal分别代表网格在局部规划范围内和网格不在局部规划范围内。
进一步的，所述步骤S2具体为：
步骤S21：对待规划区域进行通行性判断，能否通行是根据机器人底盘高度和区域内的点云信息计算得到，；
步骤S22：在区域通行性判断后，对所有可通行区域进行通行效率评估，使用区域内点云起伏变化程度作为通行效率评估指标。
进一步的，所述步骤S21具体为：将区域内的点云进行地面分割，将点云分为地面点云和非地面点云两类，聚类非地面点云，通过非地面点云高度判断机器人是否可通行，是否可通行判断如公式2所示：
式中True和Flase分别代表此区域Cxy可通行和不可通行，pi.z＜hrobot代表点云高度z小于机器人底盘高度hrobot，pi∈Cxy代表点pi在区域Cxy内。
进一步的，所述效率评估，具体为：
对区域内分割出的地面点云，对区域内中每个点云p，搜索出与其最近邻的k个相邻点，拟合出这些点最小二乘意义上的局部平面P，P表示为公式3：
式中为P的法向量，pi为区域内的点，d为P到坐标原点的距离；
计算协方差矩阵M如公式4所示：
式中p0为P的质心；
根据公式(4)中的协方差矩阵M进行特征值分解，求得M的各特征值，若特征值满足λ0≤λ1≤λ2关系，求得P点的表面曲率如公式5所示：
δ作为通行效率评估指标，即其值越大，通行效率越低。
进一步的，所述步骤S3具体为：
骤S31：将探索区域的选择问题转化为代价和收益优化问题，代价函数和收益函数约束如公式6和公式7所示：
Min Cost(Cxy)＝α·Trajectory(Pcurent，Cxy)+β·Ease(δ1，δ2，...，δn)#(6)
Max Gain(Cxy)＝γ·Gain1(Cxy)+η·Gain2(Cxy1，Cxy2，...，Cxyn)#(7)
其中为考虑提高建图质量问题，若预计探索轨迹区域中存在闭环点时，则提高此区域内的收益，如公式8所示：
Gain2(Cxy1，Cxy2，...，Cxyn)
＝α1·Gain1(Cxy1)+α2·Gain1(Cxy2)+...+αn·Gain1(Cxyn)#(8)
式中，Trajectory(Pcurent，Cxy)为轨迹长度代价函数，Ease(δ1，δ2，...δn)为通行效率代价函数，Cxy为待探索区域位置，Pcurent，为机器人当前位置，δ1，δ2....δn为预计轨迹经过的可通行区域得到通行效率指标，其中δ数值越大，代表通行效率越低；Gain1(Cxy)为区域Cxy处的信息收益，Gain2(Cxy1Cxy2...Cxyn)为预期轨迹上的信息收益，Cxy1，Cxy2，...，Cxyn为预期经过的区域，α1，α2，...，αn为预期收益权重参数；α，β，Y，η均为权重参数；
步骤S32：通过将步骤S31得到的代价和收益问题，转化为单目标函数优化问题，优化函数如公式9所示：
Max I(Cxy)＝λ1·Similarity(Cxy1，Cxy2，...，Cxyn)·Gain(Cxy)·e-Cost(Cxy)·λ2#(9)
式中，λ1，λ2分别为收益和代价差异参数；其中Similarity(Cxy1，Cxy2，...，Cxyn)函数是计算预期轨迹中分支之间相似度，即反应了探索方向，相似度越高，则Similarity(Cxy1，Cxy2，...，Cxyn)的值越大；
步骤S33：通过综合评估代价和收益之后，基于优化函数，得到有序的全局路径点，将其存入全局路径列表中。
进一步的，所述步骤S4具体为：
步骤S41：利用随机视点生成器，在局部规划范围内生成视点，；
步骤S42：对局部规划范围内生成的所有视点，综合评估代价和收益，将路径规划问题转换为目标函数优化问题，目标优化函数如公式10所示：
Max P(vb)＝λ1·I(vb)-λ2·Trajectory(v1，v2，...，vn)#(10)
式中，λ1和λ2是用户定义的常量参数，vb为局部规划范围边界上的一个视点，I(vb)为vb视点处机器人探索收益，观测到点云数量为指标，v1，v2，...，vn为从机器人当前位置到vb处预计经过得到视点，Trajectory(v1，v2，...，vn)为机器人到达vb预计轨迹长度；为生成满足机器人运动控制的局部精细路径，对到达vb处预计经过视点进行选择，若到达下一视点不满足机器人最小转弯半径约束条件，则将到达此视点的代价设为无穷大，如公式11所示：
式中，Trcurrent代表从视点vx-1到视点vx所需的转弯半径，Trlimit为机器人允许的最小转弯半径，Length(vx，vx-1)为视点vx-1到视点vx的欧几里得距离。
进一步的，所述步骤S5具体为：基于步骤S3和S4得到的全局路径和局部路径，根据机器人当前位置和人为设定的局部规划范围，对全局路径进行裁剪，将原本处于局部规划范围内全局路径替换为步骤S4生成的局部路径，即完成了全局路径和局部路径的连接，并将结果存入存储路径的容器中，以此实现分层式自主探索框架。
进一步的，所述地图更新分为两种情况，其一是机器人经过非闭环点时，进行日常地图点维护更新，即将点云增量添加至点云容器内，其二是机器人经过闭环点时，进行位姿优化，重新校正地图，并更新网格划分和点云信息。
本发明与现有技术相比具有以下有益效果：
1、本发明对机器人进行通行区域，能够实现较为综合性的评估和更丰富的探索策略；
2、本发明基于通行效率评估的机器人分层式探索效率，其将主要计算资源分配在生成供机器人运动控制的局部精细路径上，，但也考虑机器人自主探索的全局引导，并在全局引导上考虑了建图质量问题，通过闭环点检测和提高闭环点区域收益，使得机器人自主探索建图质量进一步提高，综合考虑各因素，使机器人尽可能使用最少的时间和最短距离建立起高质量环境地图；
3、本发明在评估代价和收益上，考虑了轨迹长度、区域处的通行效率问题和建图质量，算法方面能够根据环境调节各自权重参数，完成不同特点的探索结果。根据各个区域的信息，通过对代价和收益的计算，结合实际环境综合评估最优的路径引导机器人完成自主探索。该算法能够合理分配计算资源，可以提高机器人探索效率和建图质量，降低了计算机配置要求。
附图说明
图1为本发明实施例的方法流程图。
图2为本发明实施例采用的机器人考虑闭环点增益的全局路径规划示意图。
图3为本发明实施例采用的机器人自主探索系统考虑闭环点增益后的整体框架示意图。
图4为本发明实施例采用局部规划视点选取示意图。
图5为本发明实施例采样的机器人自主探索仿真环境。
图6为本发明实施例方法探索仿真环境后生成的三维点云地图。
具体实施方式
下面结合附图及实施例对本发明做进一步说明。
请参照图1，本发明提供一种基于通行效率评估的机器人分层式自主探索方法，具体包括以下步骤：
步骤S1:获取待规划区域三维点云数据，并采用环境检测模块将环境区域划分为稀疏区域和稠密区域；
步骤S2:基于待规划区域三维点云数据，进行区域通行效率评估；
步骤S3:综合评估机器人探索代价和收益，利用稀疏区域点云信息进行自主探索全局规划，生成全局路径；
步骤S4:根据机器人实时位置和机器人附近稠密区域的点云信息，进行局部路径规划，生成局部精细路径；
步骤S5:连接全局路径和局部路径，生成自主探索有序路径点，并将处理结果存入列表中，以此实现分层式自主探索框架；
步骤S6:对机器人探索过程中生成地图进行更新。
在本实施例中，所述稀疏和稠密区域划分，具体为：将机器人当前位置获取到的三维点云进行网格划分，网格形状一般设置为矩形，以机器人当前位置为中心，设定的局部规划范围内且点云数量大于阈值上限的网格为稠密区域(Dense)，局部规划范围外，点云数量大于阈值下限且小于阈值上限的区域为稀疏区域(Spare)，稀疏和稠密区域状态划分如公式1所示：
式中Cxy代表划分网格，N(Cxy)代表网格中点云数量，Xthreshold1和Xthreshold2代表人为设定的阈值，Cxy＜Rlocal和Cxy＞Rlocal分别代表网格在局部规划范围内和网格不在局部规划范围内。
在本实施例中，为了提高复杂环境的通行区域分析准确率，为机器人自主探索提供更完善的评估指标，为机器人自主高效率的探索提供更多可能性，步骤S2具体为：
步骤S21：对待规划区域进行通行性判断，能否通行是根据机器人底盘高度和区域内的点云信息计算得到；
将区域内的点云进行地面分割，将点云分为地面点云和非地面点云两类，聚类非地面点云，通过非地面点云高度判断机器人是否可通行，是否可通行判断如公式2所示：
式中True和Flase分别代表此区域Cxy可通行和不可通行，pi.z＜hrobot代表点云高度z小于机器人底盘高度hrobot，pi∈Cxy代表点pi在区域Cxy内。
步骤S22：在区域通行性判断后，对所有可通行区域进行通行效率评估，使用区域内点云起伏变化程度作为通行效率评估指标，具体为：
对区域内分割出的地面点云，对区域内中每个点云p，搜索出与其最近邻的k个相邻点，拟合出这些点最小二乘意义上的局部平面P，P表示为公式3：
式中为P的法向量，pi为区域内的点，d为P到坐标原点的距离；
计算协方差矩阵M如公式4所示：
式中p0为P的质心；
根据公式(4)中的协方差矩阵M进行特征值分解，求得M的各特征值，若特征值满足λ0≤λ1≤λ2关系，求得P点的表面曲率如公式5所示：
δ作为通行效率评估指标，即其值越大，通行效率越低。
在本实施例中，为了提高机器人自主探索的效率，为机器人自主探索策略添加全局引导，为提高机器人自主探索的建图质量，使用闭环点检测，并提高闭环点所在区域的信息收益。因此步骤S3具体为：综合评估机器人探索代价和收益，利用稀疏区域点云信息进行自主探索全局规划，生成全局路径。其中设机器人当前位置到全局稀疏区域的轨迹长度为T，长度采用欧几里得距离计算，将步骤S2求得的δ作为区域通行效率评估指标，自主探索代价分为轨迹长度和通行效率两方面；自主探索全局规划的收益主要包含待探索区域处信息收益和预计探索轨迹中得到的信息收益；全局区域处信息收益是以机器人为中心，半径为r的圆中得到的高质量点云数量；预计探索轨迹中得到的信息收益用前往全局稀疏区域过程中探索到新的可通行区域高质量点云数量和不可通行区域高质量点云数量总和，其中两种区域收益的权重可根据探索实际环境设置权重参数。
优选的，步骤S3包括以下步骤：
骤S31：将探索区域的选择问题转化为代价和收益优化问题，代价函数和收益函数约束如公式6和公式7所示：
Min Cost(Cxy)＝α·Trajectory(Pcurent，Cxy)+β·Ease(δ1，δ2，...，δn)#(6)
Max Gain(Cxy)＝γ·Gain1(Cxy)+η·Gain2(Cxy1，Cxy2，...，Cxyn)#(7)
其中为考虑提高建图质量问题，若预计探索轨迹区域中存在闭环点时，则提高此区域内的收益，如公式8所示：
Gain2(Cxy1，Cxy2，…，Cxyn)
＝α1·Gain1(Cxy1)+α2·Gain1(Cxy2)+...+αn·Gain1(Cxyn)#(8)
式中，Trajectory(Pcurent，Cxy)为轨迹长度代价函数，Ease(δ1，δ2，...δn)为通行效率代价函数，Cxy为待探索区域位置，Pcurent，为机器人当前位置，δ1，δ2，...δn为预计轨迹经过的可通行区域得到通行效率指标，其中δ数值越大，代表通行效率越低；Gain1(Cxy)为区域Cxy处的信息收益，Gain2(Cxy1Cxy2...Cxyn)为预期轨迹上的信息收益，Cxy1，Cxy2，...，Cxyn为预期经过的区域，α1，α2，...，αn为预期收益权重参数，若预期经过的区域存在闭环点时，提高此区域内的预期收益权重参数；α，β，Y，η均为权重参数，机器人在进行室内环境探索时，一般的认为轨迹长度代价比通行容易度代价更为重要，故此时设置α值大于β值，室外环境则反之；机器人自主探索环境时一般的认为边界点处信息收益重要于预期轨迹上的信息收益，故设置γ值大于η值；
步骤S32：通过将步骤S31得到的代价和收益问题，转化为单目标函数优化问题，优化函数如公式9所示：
Max I(Cxy)＝λ1·Similarity(Cxy1，Cxy2，...，Cxyn)·Gain(Cxy)·e-Cost(Cxy)·λ2#(9)
式中，λ1，λ2分别为收益和代价差异参数，可由用户自定义；其中Similarity(Cxy1，Cxy2，...，Cxyn)函数是计算预期轨迹中分支之间相似度，即反应了探索方向，相似度越高，则Similarity(Cxy1，Cxy2，...，Cxyn)的值越大；
步骤S33：通过综合评估代价和收益之后，基于优化函数，得到有序的全局路径点，将其存入全局路径列表中。
在本实施例中，为生成满足可用于机器人运动控制的局部精细路径，步骤S4具体内容为：利用机器人实时位置和机器人附近的稠密点云信息，进行局部路径规划。
优选的，步骤S4包括以下步骤：
步骤S41：利用随机视点生成器，在局部规划范围内生成视点，；
步骤S42：对局部规划范围内生成的所有视点，综合评估代价和收益，将路径规划问题转换为目标函数优化问题，目标优化函数如公式10所示：
Max P(vb)＝λ1·I(vb)-λ2·Trajectory(v1，v2，...，vn)#(10)
式中，λ3和λ4是用户定义的常量参数，vb为局部规划范围边界上的一个视点，I(vb)为vb视点处机器人探索收益，此处收益主要根据传感器特性及其视野范围，观测到点云数量为指标，v1，v2，...，vn为从机器人当前位置到vb处预计经过得到视点，Trajectory(v1，v2，...，vn)为机器人到达vb预计轨迹长度，以此长度作为局部规划的代价；为生成满足机器人运动控制的局部精细路径，对到达vb处预计经过视点进行选择，若到达下一视点不满足机器人最小转弯半径约束条件，则将到达此视点的代价设为无穷大，如公式11所示：
式中，Trcurrent代表从视点vx-1到视点vx所需的转弯半径，Trlimit为机器人允许的最小转弯半径，Length(vx，vx-1)为视点vx-1到视点vx的欧几里得距离。
正在本实施例中，步骤S5中，为合理分配计算资源，将计算资源主要集中在生成满足机器人运动控制的局部精细路径上，局部规划尽可能提高建图质量，即生成一条能够完成高质量建图的局部路径，且使得机器人自主探索更具目的性和全局引导，使得机器人尽可能使用最少的时间和最短的路径建立高质量环境地图。步骤S5中，连接全局路径和局部路径，生成自主探索有序路径点，并将处理结果存入列表中，以此实现分层式自主探索框架。
优选的，步骤S5具体为：基于步骤S3和S4得到的全局路径和局部路径，根据机器人当前位置和人为设定的局部规划范围，对全局路径进行裁剪，将原本处于局部规划范围内全局路径替换为步骤S4生成的局部路径，即完成了全局路径和局部路径的连接，并将结果存入存储路径的容器中，以此实现分层式自主探索框架。
优选的，在本实施例中，地图更新分为两种情况，其一是机器人经过非闭环点时，进行日常地图点维护更新，即将点云增量添加至点云容器内，其二是机器人经过闭环点时，进行位姿优化，重新校正地图，并更新网格划分和点云信息。
实施例1：
本实施例的基于通行效率评估的机器人分层式自主探索方法应用于工业移动机器人的地图探索领域，其目的在于克服现有技术的不足，引导机器人自主探索未知环境并建立环境地图。具体设置如下：
本实施例采用底盘高度为60mm的机器人，仿真环境的面积大小约为300m2。机器人的速度和角速度分别被设置为0.5m/s和其它相关参数如下：α＝1.5，β＝0.8，γ＝1.0，η＝1.5，λ1＝1.0，λ2＝0.8，λ3＝1.0，λ4＝0.5，Rlocal＝3.0。
本领域内的技术人员应明白，本申请的实施例可提供为方法、系统、或计算机程序产品。因此，本申请可采用完全硬件实施例、完全软件实施例、或结合软件和硬件方面的实施例的形式。而且，本申请可采用在一个或多个其中包含有计算机可用程序代码的计算机可用存储介质(包括但不限于磁盘存储器、CD-ROM、光学存储器等)上实施的计算机程序产品的形式。
本申请是参照根据本申请实施例的方法、设备(系统)、和计算机程序产品的流程图和/或方框图来描述的。应理解可由计算机程序指令实现流程图和/或方框图中的每一流程和/或方框、以及流程图和/或方框图中的流程和/或方框的结合。可提供这些计算机程序指令到通用计算机、专用计算机、嵌入式处理机或其他可编程数据处理设备的处理器以产生一个机器，使得通过计算机或其他可编程数据处理设备的处理器执行的指令产生用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的装置。
这些计算机程序指令也可存储在能引导计算机或其他可编程数据处理设备以特定方式工作的计算机可读存储器中，使得存储在该计算机可读存储器中的指令产生包括指令装置的制造品，该指令装置实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能。
这些计算机程序指令也可装载到计算机或其他可编程数据处理设备上，使得在计算机或其他可编程设备上执行一系列操作步骤以产生计算机实现的处理，从而在计算机或其他可编程设备上执行的指令提供用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的步骤。
以上所述，仅是本发明的较佳实施例而已，并非是对本发明作其它形式的限制，任何熟悉本专业的技术人员可能利用上述揭示的技术内容加以变更或改型为等同变化的等效实施例。但是凡是未脱离本发明技术方案内容,依据本发明的技术实质对以上实施例所作的任何简单修改、等同变化与改型，仍属于本发明技术方案的保护范围。
Claims (6)
1.一种基于通行效率评估的机器人分层式自主探索方法，其特征在于，包括以下步骤：
步骤S1：获取待规划区域三维点云数据，并采用环境检测模块将环境区域划分为稀疏区域和稠密区域；
步骤S2：基于待规划区域三维点云数据，进行区域通行效率评估；
步骤S3：综合评估机器人探索代价和收益，利用稀疏区域点云信息进行自主探索全局规划，生成全局路径；
步骤S4：根据机器人实时位置和机器人附近稠密区域的点云信息，进行局部路径规划，生成局部精细路径；
步骤S5：连接全局路径和局部路径，生成自主探索有序路径点，并将处理结果存入列表中，以此实现分层式自主探索框架；
步骤S6：对机器人探索过程中生成地图进行更新；
所述步骤S3具体为：
骤S31：将探索区域的选择问题转化为代价和收益优化问题，代价函数和收益函数约束如公式6和公式7所示：
Min Cost(Cxy)＝α·Trajectory(Pcurent，Cxy)+β·Ease(δ1，δ2，…，δn) (6)
Max Gain(Cxy)＝γ·Gain1(Cxy)+η·Gain2(Cxy1，Cxy2，…，Cxyn) (7)
其中为考虑提高建图质量问题，若预计探索轨迹区域中存在闭环点时，则提高此区域内的收益，如公式8所示：
Gain2(Cxy1，Cxy2，…，Cxyn)
＝α1·Gain1(Cxy1)+α2·Gain1(Cxy2)+…+αn·Gain1(Cxyn) (8)
式中，Trajectory(Pcurent，Cxy)为轨迹长度代价函数，Ease(δ1，δ2，…δn)为通行效率代价函数，Cxy为待探索区域位置，Pcurent，为机器人当前位置，δ1，δ2，…δn为预计轨迹经过的可通行区域得到通行效率指标，其中δ数值越大，代表通行效率越低；Gain1(Cxy)为区域Cxy处的信息收益，Gain2(Cxy1Cxy2…Cxyn)为预期轨迹上的信息收益，Cxy1，Cxy2，…，Cxyn为预期经过的区域，α1，α2，...，αn为预期收益权重参数；α，β，γ，η均为权重参数；
步骤S32：通过将步骤S31得到的代价和收益问题，转化为单目标函数优化问题，优化函数如公式9所示：
式中，λ1，λ2分别为收益和代价差异参数；其中
Similarity(Cxy1，Cxy2，…，Cxyn)函数是计算预期轨迹中分支之间相似度，即反应了探索方向，相似度越高，则Similarity(Cxy1，Cxy2，…，Cxyn)的值越大；
步骤S33：通过综合评估代价和收益之后，基于优化函数，得到有序的全局路径点，将其存入全局路径列表中；
所述步骤S4具体为：
步骤S41：利用随机视点生成器，在局部规划范围内生成视点；
步骤S42：对局部规划范围内生成的所有视点，综合评估代价和收益，将路径规划问题转换为目标函数优化问题，目标优化函数如公式10所示：
Max P(vb)＝λ1’·I(vb)-λ2’·Trajectory(v1，v2，…，vn) (10)
式中，λ1’和λ2’是用户定义的常量参数，vb为局部规划范围边界上的一个视点，I(vb)为vb视点处机器人探索收益，观测到点云数量为指标，v1，v2，…，vn为从机器人当前位置到vb处预计经过得到视点，Trajectory(v1，v2，…，vn)为机器人到达vb预计轨迹长度；为生成满足机器人运动控制的局部精细路径，对到达vb处预计经过视点进行选择，若到达下一视点不满足机器人最小转弯半径约束条件，则将到达此视点的代价设为无穷大，如公式11所示：
式中，Trcurrent代表从视点vx-1到视点vx所需的转弯半径，Trlimit为机器人允许的最小转弯半径，Length(vx，vx-1)为视点vx-1到视点vx的欧几里得距离；
所述步骤S5具体为：基于步骤S3和S4得到的全局路径和局部路径，根据机器人当前位置和人为设定的局部规划范围，对全局路径进行裁剪，将原本处于局部规划范围内全局路径替换为步骤S4生成的局部路径，即完成了全局路径和局部路径的连接，并将结果存入存储路径的容器中，以此实现分层式自主探索框架。
2.根据权利要求1所述的基于通行效率评估的机器人分层式自主探索方法，其特征在于，所述稀疏和稠密区域划分，具体为：将机器人当前位置获取到的三维点云进行网格划分，以机器人当前位置为中心，设定的局部规划范围内且点云数量大于阈值上限的网格为稠密区域，局部规划范围外，点云数量大于阈值下限且小于阈值上限的区域为稀疏区域，稀疏和稠密区域状态划分如公式1所示：
式中Cxy代表划分网格，N(Cxy)代表网格中点云数量，Xthreshold1和Xthreshold2代表人为设定的阈值，Cxy＜Rlocal和Cxy＞Rlocal分别代表网格在局部规划范围内和网格不在局部规划范围内。
3.根据权利要求1所述的基于通行效率评估的机器人分层式自主探索方法，其特征在于，所述步骤S2具体为：
步骤S21：对待规划区域进行通行性判断，能否通行是根据机器人底盘高度和区域内的点云信息计算得到；
步骤S22：在区域通行性判断后，对所有可通行区域进行通行效率评估，使用区域内点云起伏变化程度作为通行效率评估指标。
4.根据权利要求3所述的基于通行效率评估的机器人分层式自主探索方法，其特征在于，所述步骤S21具体为：将区域内的点云进行地面分割，将点云分为地面点云和非地面点云两类，聚类非地面点云，通过非地面点云高度判断机器人是否可通行，是否可通行判断如公式2所示：
式中True和Flase分别代表此区域Cxy可通行和不可通行，pi.z＜hrobot代表点云高度z小于机器人底盘高度hrobot，pi∈Cxy代表点pi在区域Cxy内。
5.根据权利要求3所述的基于通行效率评估的机器人分层式自主探索方法，其特征在于，所述效率评估，具体为:
对区域内分割出的地面点云，对区域内中每个点云p,搜索出与其最近邻的k个相邻点，拟合出这些点最小二乘意义上的局部平面P，P表示为公式3：
式中为P的法向量，pi为区域内的点，d为P到坐标原点的距离；
计算协方差矩阵M如公式4所示：
式中p0为P的质心；
根据公式(4)中的协方差矩阵M进行特征值分解，求得M的各特征值，若特征值满足λ0≤λ1”≤λ2”关系，求得P点的表面曲率如公式5所示：
δ作为通行效率评估指标，即其值越大，通行效率越低。
6.根据权利要求1所述的基于通行效率评估的机器人分层式自主探索方法，其特征在于，所述地图更新分为两种情况，其一是机器人经过非闭环点时，进行日常地图点维护更新，即将点云增量添加至点云容器内，其二是机器人经过闭环点时，进行位姿优化，重新校正地图，并更新网格划分和点云信息。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210793053.8A CN115129057B (zh) | 2022-07-07 | 2022-07-07 | 基于通行效率评估的机器人分层式自主探索方法 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210793053.8A CN115129057B (zh) | 2022-07-07 | 2022-07-07 | 基于通行效率评估的机器人分层式自主探索方法 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN115129057A CN115129057A (zh) | 2022-09-30 |
| CN115129057B true CN115129057B (zh) | 2024-06-07 |
Family
ID=83381032
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202210793053.8A Active CN115129057B (zh) | 2022-07-07 | 2022-07-07 | 基于通行效率评估的机器人分层式自主探索方法 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN115129057B (zh) |
Citations (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN110221614A (zh) * | 2019-06-14 | 2019-09-10 | 福州大学 | 一种基于快速探索随机树的多机器人地图探索方法 |
| CN113110522A (zh) * | 2021-05-27 | 2021-07-13 | 福州大学 | 一种基于复合式边界检测的机器人自主探索方法 |
| CN114442621A (zh) * | 2022-01-17 | 2022-05-06 | 浙江大学 | 一种基于四足机器人的自主探索和建图系统 |
Family Cites Families (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN110703747B (zh) * | 2019-10-09 | 2021-08-03 | 武汉大学 | 一种基于简化广义Voronoi图的机器人自主探索方法 |
-
2022
- 2022-07-07 CN CN202210793053.8A patent/CN115129057B/zh active Active
Patent Citations (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN110221614A (zh) * | 2019-06-14 | 2019-09-10 | 福州大学 | 一种基于快速探索随机树的多机器人地图探索方法 |
| CN113110522A (zh) * | 2021-05-27 | 2021-07-13 | 福州大学 | 一种基于复合式边界检测的机器人自主探索方法 |
| CN114442621A (zh) * | 2022-01-17 | 2022-05-06 | 浙江大学 | 一种基于四足机器人的自主探索和建图系统 |
Non-Patent Citations (1)
| Title |
|---|
| 一种基于高效边界探索的机器人自主建图方法;刘瑞雪;曾碧;汪明慧;卢智亮;;广东工业大学学报;20200915(第05期);第42-49页 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN115129057A (zh) | 2022-09-30 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| Gao et al. | Gradient-based online safe trajectory generation for quadrotor flight in complex environments | |
| Huang et al. | Adaptive cylinder vector particle swarm optimization with differential evolution for UAV path planning | |
| Batinovic et al. | A multi-resolution frontier-based planner for autonomous 3D exploration | |
| CN107103164B (zh) | 无人机执行多任务的分配方法及装置 | |
| CN108958238B (zh) | 一种基于协变代价函数的机器人点到区路径规划方法 | |
| CN110021072B (zh) | 面向全息测绘的多平台点云智能处理方法 | |
| CN110132291A (zh) | 用于港口的栅格地图生成方法、系统、设备及存储介质 | |
| Xu et al. | Heuristic and random search algorithm in optimization of route planning for Robot’s geomagnetic navigation | |
| CN116954233A (zh) | 一种巡检任务与航线自动匹配方法 | |
| CN115639823A (zh) | 崎岖起伏地形下机器人地形感知与移动控制方法及系统 | |
| CN114859375A (zh) | 基于多机器人协作的自主探索建图系统及探索建图方法 | |
| CN113110455A (zh) | 一种未知初始状态的多机器人协同探索方法、装置及系统 | |
| CN115903888A (zh) | 一种基于天牛群算法的旋翼无人机自主路径规划方法 | |
| Mohamed et al. | Autonomous navigation of agvs in unknown cluttered environments: log-mppi control strategy | |
| CN116880561A (zh) | 基于改进粒子群无人机路径规划安全增强的优化方法及系统 | |
| CN116482711A (zh) | 一种用于着陆区自主选择的局部静态环境感知方法及装置 | |
| CN104793492B (zh) | 一种基于熵和灰关联度的室内环境特征提取方法 | |
| Shi et al. | LiDAR localization at 100 FPS: a map-aided and template descriptor-based global method | |
| CN115129057B (zh) | 基于通行效率评估的机器人分层式自主探索方法 | |
| CN116136417B (zh) | 一种面向越野环境的无人驾驶车辆局部路径规划方法 | |
| CN117406771A (zh) | 一种基于四旋翼无人机的高效自主探索方法、系统及设备 | |
| Guo et al. | Occupancy grid based urban localization using weighted point cloud | |
| CN117289301A (zh) | 一种未知越野场景下空地无人平台协同路径规划方法 | |
| CN117109620A (zh) | 基于采样的车辆行为与环境交互的自动驾驶路径规划方法 | |
| CN114200527B (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 |