CN113867340B - 一种地外未知环境超视距全局路径规划系统和方法 - Google Patents
一种地外未知环境超视距全局路径规划系统和方法 Download PDFInfo
- Publication number
- CN113867340B CN113867340B CN202111092918.XA CN202111092918A CN113867340B CN 113867340 B CN113867340 B CN 113867340B CN 202111092918 A CN202111092918 A CN 202111092918A CN 113867340 B CN113867340 B CN 113867340B
- Authority
- CN
- China
- Prior art keywords
- global
- path
- grid
- map
- planning
- 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 44
- 238000001514 detection method Methods 0.000 claims description 9
- 238000005457 optimization Methods 0.000 claims description 9
- 238000004364 calculation method Methods 0.000 claims description 8
- 238000013507 mapping Methods 0.000 claims description 5
- 238000010845 search algorithm Methods 0.000 claims description 3
- 230000004888 barrier function Effects 0.000 claims description 2
- 238000001914 filtration Methods 0.000 claims description 2
- 230000008569 process Effects 0.000 claims description 2
- 230000000007 visual effect Effects 0.000 claims 2
- 238000004458 analytical method Methods 0.000 description 7
- 238000012937 correction Methods 0.000 description 5
- 238000010586 diagram Methods 0.000 description 3
- 230000000694 effects Effects 0.000 description 3
- 239000000523 sample Substances 0.000 description 3
- 230000009194 climbing Effects 0.000 description 2
- 238000013461 design Methods 0.000 description 2
- 238000005516 engineering process Methods 0.000 description 2
- 230000003287 optical effect Effects 0.000 description 2
- 230000008447 perception Effects 0.000 description 2
- PEDCQBHIVMGVHV-UHFFFAOYSA-N Glycerine Chemical compound OCC(O)CO PEDCQBHIVMGVHV-UHFFFAOYSA-N 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000002457 bidirectional effect Effects 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 230000006870 function Effects 0.000 description 1
- 238000009434 installation Methods 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 238000011084 recovery Methods 0.000 description 1
- 238000006467 substitution reaction Methods 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/0231—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means
- G05D1/0246—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means using a video camera in combination with image processing means
- G05D1/0251—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means using a video camera in combination with image processing means extracting 3D information from a plurality of images taken from different locations, e.g. stereo vision
-
- 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/0214—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory in accordance with safety or protection criteria, e.g. avoiding hazardous areas
-
- 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)
- Physics & Mathematics (AREA)
- Aviation & Aerospace Engineering (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Computer Vision & Pattern Recognition (AREA)
- Multimedia (AREA)
- Electromagnetism (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
- Image Processing (AREA)
Abstract
本发明公开了一种地外未知环境超视距全局路径规划方法,本方法基于分区域全局栅格地图和通行代价权值进行超视距路径规划,引入通行代价函数在全局栅格地图上搜索得到最优路径,最后输出可见区域的路径点,在大范围全局路径规划中取得了良好效果,能够支持地外移动探测器/机器人实现高效移动探测。本发明还公开了一种地外未知环境超视距全局路径规划系统,通过前段模块和后端模块的配合,实现本发明地外未知环境超视距全局路径规划方法。
Description
技术领域
本发明涉及计算机技术领域,特别涉及一种移动探测器/机器人的路径规划系统和方法。
背景技术
路径规划是移动机器人安全、高效移动必不可少的功能。目前已有的地面/地外移动探测器/机器人路径规划算法都是针对环境信息已知的路径规划,在环境信息不可知的区域中,因缺少地形信息,路径规划无法执行。
在未来载人登月及火星探测任务中,探测范围和移动速度显著提升,必须解决目标在百米及更远距离的大范围路径规划问题。由于地外探测没有事先已知的高分辨率地图,探测器自身携带的感知敏感器能够恢复的地形范围有限,如我国月球/火星探测器自主三维感知精度在10m处的误差已近20cm,更大范围的地形数据精度更低,路径规划目标点超出有效视距范围,无法执行全局路径规划。
发明内容
本发明的目的在于克服上述缺陷,提供一种地外未知环境超视距全局路径规划方法,通过合理设置有效视距范围外的通行代价值和多阈值设计,建立分区域全局栅格地图和通行性地图,实现全局栅格地图上的路径规划,然后通过保留可见区域的路径点序列作为本次规划的执行结果,既实现了可视范围内路径安全可信,又保证了视距外目标可达,在大范围全局路径规划中取得了良好效果,能够支持高效移动探测。本发明还提供一种地外未知环境超视距全局路径规划系统,通过前段模块和后端模块的配合,实现本发明地外未知环境超视距全局路径规划方法。
为实现上述发明目的,本发明提供如下技术方案:
一种地外未知环境超视距全局路径规划方法,包括以下步骤:
S1根据移动探测器或机器人自带双目相机采集的双目立体视觉图像得到当前规划位置的数字高程图;所述数字高程图的范围对应有效可见区域,有效可见区域外为不可见区域;
S2建立全局栅格地图并将数字高程图映射到全局栅格地图中;所述全局栅格地图中的栅格为正方形;
S3根据数字高程图计算全局栅格地图中有效可见区域范围内各栅格的通行代价,并将全局栅格地图中不可见区域范围内各栅格的通行代价设为常数,根据全局栅格地图中各栅格的通行代价得到全局通行性地图;
S4根据全局通行性地图规划当前规划位置到目标点的最优路径;
S5移动探测器或机器人移动过程中,获取新的数字高程图后,将数字高程图映射到全局栅格地图中,并迭代更新全局栅格地图和全局可通行地图,规划下一规划位置到目标点的最优路径。
进一步的,所述步骤S1中,根据采集的双目立体视觉图像得到数字高程图的方法为:
S11对双目立体视觉图像进行校正、立体匹配和滤波,得到视差图;
S12根据相机模型和参数将视差图转换为相机坐标系下的三维点云;
S13将三维点云转化为世界坐标系下分辨率和精度满足要求的数字高程图。
进一步的,所述步骤S2中,全局栅格地图根据当前位置和全局路径规划目标点位置建立,全局路径规划目标点位于可见区域之外时,全局栅格地图范围≥数字高程图范围。
进一步的,所述步骤S3中,根据数字高程图中的障碍物坡度信息、粗糙度信息及障碍阶跃高程差等信息计算全局栅格地图中可见区域范围内各栅格的通行代价,获得全局通行性地图。
进一步的,所述步骤S3中,所述各栅格的通行代价dgoodness的计算方法为:
dgoodness=1-(1-d)dcert
d=λ1dslope+λ2drough+λ3dstep
其中,dcert为栅格置信度,dslope为坡度代价,drough为粗糙度代价,dstep为阶跃高程差代价,d为三个代价加权和,λ1,λ2,λ3为三个权重,取值范围为[0,1],取值大小根据移动探测器或机器人对不同障碍的通过能力和探测区域地形分布特点确定;各栅格的通行代价值越高,表示通过难度越大。
进一步的,所述步骤S3中,根据全局栅格地图中各栅格的通行代价得到全局通行性地图的方法为:
根据通行代价的值将各栅格状态确定为可通行、未知区域或障碍三种状态;设置第一阈值和第二阈值,且第一阈值大于第二阈值,所述通行代价大于第一阈值时,确定栅格状态为障碍;所述通行代价小于等于第一阈值且大于第二阈值时,确定栅格状态为未知区域;所述通行代价小于等于第二阈值时,确定栅格状态为可通行。
进一步的,所述步骤S3中,不可见区域范围内各栅格的通行代价设为与第二阈值相等的常数。
进一步的,所述步骤S4中,根据全局通行性地图规划当前位置到目标点的最优路径的方法包括:
S41在全局通行性地图中,采用图搜索算法获得全局可达的路径点序列;
S42保留路径点序列中位于有效可视区域内部的路径点;当路径点序列中存在落在有效可视区域边界上的路径点,则将所述路径点保留,当路径点序列中不存在落在有效可视区域边界上的路径点,则在效可视区域或边界的可通行栅格中增加一个路径点;
S43删除有效可视区域外的路径点。
进一步的,所述步骤S41中,根据设置参数和优化条件,采用图搜索算法获得全局通行性地图中的路径点序列,所述设置参数包括航向约束和相关移动性能,优化条件为时间最短、里程最短或安全性最高中的一项或多项组合;
图搜索算法以正方形栅格为图的顶点,以正方形栅格中心坐标点为移动探测器或机器人途经的位置,以栅格通行代价为边的权重。
进一步的,所述步骤S42中,当路径点序列中不存在落在有效可视区域边界上的路径点,则在效可视区域或边界的可通行栅格中增加一个路径点的具体方法为:
连接有效可视区域边界内外的两个路径点,确定连线与边界交点,若交点所在栅格为可通行状态,则将该点输出为路径点,若交点所在栅格为障碍或未知区域,则沿连线向可视区域内搜索距交点最近且所在栅格为可通行状态的点,将其增设为路径点。
一种地外未知环境超视距全局路径规划系统,用于实现上述一种地外未知环境超视距全局路径规划方法,包括前端模块和后端模块;
所述前端模块将移动探测器或机器人的双目立体视觉图像和路径规划目标点坐标发送至后端模块,后端模块根据双目立体视觉图像和路径规划目标点坐标进行路径规划得到最优路径,并将最优路径实时发送至前端模块,前端模块根据最优路径实时控制移动探测器或机器人移动。
本发明与现有技术相比具有如下有益效果:
(1)本发明一种地外未知环境超视距全局路径规划方法,创新性的提出了一种采用不可见区域通行代价设置和多阈值判断通行代价得到超视距全局通行性地图的方法,为不可见区域赋予有效信息,为全局路径规划提供了条件;
(2)本发明一种地外未知环境超视距全局路径规划方法,设计了通行代价的计算公式,采用数字高程图即可得到通行性分析结果,方法简单,准确性高,便于实现;
(3)本发明一种地外未知环境超视距全局路径规划方法,通过在全局通行地图上搜索得到全局最优路径,然后保留可见区域的路径点序列作为本次规划执行结果,既实现了可视范围内路径安全可信,又保证了视距外目标可达,在大范围超视距全局路径规划中取得了良好效果,能够支持高效移动探测。
附图说明
图1为本发明一种地外未知环境超视距全局路径规划系统示意图;
图2为本发明对双目立体视觉图像进行立体匹配的流程图;
图3为本发明数字高程图生成的流程图;
图4为本发明生成数字高程图时使用的相机参数示意图;
图5为本发明通行性分析结果生成的流程图;
图6为本发明栅格分布度计算示意图。
具体实施方式
下面通过对本发明进行详细说明,本发明的特点和优点将随着这些说明而变得更为清楚、明确。
在这里专用的词“示例性”意为“用作例子、实施例或说明性”。这里作为“示例性”所说明的任何实施例不必解释为优于或好于其它实施例。尽管在附图中示出了实施例的各种方面,但是除非特别指出,不必按比例绘制附图。
为解决高效移动探测大范围超视距路径规划问题,本专利提出了一种新的基于分区域全局栅格地图和通行代价权值的超视距路径规划方法,通过合理设置有效视距范围外的通行代价值和多阈值设计,建立分区域全局栅格地图和通行性地图,实现全局栅格地图上的路径规划,然后通过保留可见区域的路径点序列作为本次规划的执行结果,既实现了可视范围内路径安全可信,又保证了视距外目标可达,在大范围全局路径规划中取得了良好效果,能够支持高效移动探测。
本发明一种未知环境中的机器人远距离路径规划系统前端和后端两个部分,前端采集双目图像信息,并将图像信息和路径规划目标点坐标发送至后端,后端根据双目图像信息和路径规划目标点坐标进行路径规划,并发送路径规划结果到前端。具体包括以下步骤:
(1)前端程序获取图像采集单元采集得到的双目图像;
(2)后端程序执行立体匹配,即对双目图像进行立体匹配,计算视差;
(3)后端程序执行三维恢复,即利用视差,根据三角法原理生成稠密点云地图,并计算得到数字高程图,数字高程图范围对应有效可视区域;
(4)后端程序执行通过性分析,即根据数字高程图,计算可视区域中各栅格的通行代价;地图中每个栅格数据有三种状态:可通行、未知区域和障碍。对每种状态设计通行代价权值,可通行、未知区域和障碍物的通行代价权值递增。初始状态下全局地图的每个栅格初始状态均为未知区域。获取更新的数字高程图后,将其映射到全局栅格地图,并迭代更新。
(5)后端程序执行路径规划,即根据各栅格的通行代价在全局栅格地图上搜索出一条从当前位置到路径规划目标点的最优路径;
(6)后端程序将最优路径的路径点序列发送到前端程序。
进一步的,步骤(2)中所述后端程序执行立体匹配的具体步骤包括:
(21)选择相机参数文件,其中包括极线校正参数和立体匹配参数;
(22)根据读入的双目图像和相机参数进行极线校正,得到视差数据;
其中匹配参数有默认值,可将默认值修改为其他值,使用匹配参数进行立体匹配;
(23)后端对视差数据进行滤波生成视差图;
(24)输出视差图结果并保存相应文件。
进一步的,所述步骤(4)中,进行通行性分析的步骤具体包括:
(41)输入数字高程图数据;
(42)输入越障高度、爬坡高度、可通行粗糙度等配置参数;
(43)创建全局栅格地图,全局栅格地图分为有效可见区域和不可见区域,对可见区域利用输入的数字高程图计算通行代价,通行代价过大,超过所设阈值(即可通行代价的最大值)为不可通行区域;不可见区域栅格的通行代价设为常量;
(44)根据通行代价进行可通行性分析,得到可通行区域和障碍识别结果,获得全局通行性地图。
进一步的,所述步骤(43)中,根据数字高程图计算栅格的通行代价,其通行代价由三个因素综合得出:数字高程图上的障碍物坡度、数字高程图上的粗糙度、数字高程图上的障阶跃高程差。
进一步的,步骤(43)中,不可见区域栅格的通行代价的常量设置为可通行代价的最大值。
进一步的,所述步骤(5)中,进行远距离路径规划步骤具体包括:
(51)导入全局通行性地图;
(52)输入航向约束、相关移动性能等必要的设置参数;
(53)将时间最短、里程最短、安全性最高等一个或几个因素作为优化条件;
(54)将当前位置和前端传输的目标点坐标映射到栅格地图中,形成地图上的起止点;
(55)根据起止点、全局栅格地图、约束条件和优化条件进行搜索,得到路径点序列;
(56)对所得路径点序列进行优化处理,具体为,在数字高程图叠加显示通行性地图和全局路径规划的输出路径点序列;
(57)将优化后的路径点序列保存并发送到前端。
进一步的,步骤(53)中,优化条件可选择安全性最高;
进一步的,步骤(55)中,图搜索算法以栅格为图的顶点,以正方形栅格中心坐标点为机器人途经的位置,以栅格通行代价为边的权重。
进一步的,步骤(55)中,图搜索算法可选择hybrid A*算法;
进一步的,步骤(55)中,路径点的优化步骤为:
保留全局路径规划点中位于可视区域内(包含可视区域边界)的点
若可视区域边界没有点,则在可见区域边界内部附近可通行区域增加一个路径点;
将不可见区域的路径点删除。
实施例1
本发明提供一种未知环境中的机器人远距离路径规划系统和方法。
参照图1,图1为本发明机器人的路径规划方法一实施例的流程示意图。在本实施例中,该机器人的路径规划方法包括:
(1)前端模块获取双目图像信息;
(2)后端模块对双目图像进行立体匹配,具体步骤为:对双目图像对进行极线校正和匹配,提取视差信息;参照图2,具体的说,所述双目图像进行立体匹配步骤包括:选择相机参数文件,其中包括极线校正参数和立体匹配参数;根据读入的双目图像和相机参数进行极线校正;后端对视差数据进行滤波生成视差图;输出视差图结果并保存相应文件。
立体匹配选择采用双向半全局匹配算法,方法如下:
对左右图分别生成视差图,然后输入一个保边滤波器(edeg-preservingfilter),可以有效滤除噪声,为每个像素计算更准确的视差。对于一幅输入图像,我们希望目标图像其尽可能近似,与此同时除了在一些边缘梯度变化比较大的地方外应该越平滑越好。
(3)后端模块进行三维恢复构建地图,参照图3,具体步骤为,根据视差信息,再结合相机模型及相机的安装重建场景的三维坐标,生成稠密地形数据,得到三维点云地图,并计算得到数字高程图;
三维重建是指根据两幅图像中对应像素点的坐标,恢复出场景三维几何形状,具体步骤如下:
参照图4,输入相机参数和视差图,其中相机参数包括两个摄像机的光心分别记为Ol和Or,光心之间的连线长度记为基线长度B和相机的焦距f。
三维场景点X(xw,yw,zw)在左右两幅图像中的投影坐标分别设为ml(xl,yl)、mr(xr,yr);
根据相似三角形定理,可以求得以下对应关系:
通过视差d反推出场景点X的三维坐标:
通过对场景所有像素点进行推算得到点云数据。
初始化有关数字高程图(DEM)的结构体并赋初值0;
遍历点云数据中的所有点,将x,y,z轴坐标进行转化;
将z轴所代表的高度值赋值至数字高程图(DEM)对应坐标中;
对数字高程图(DEM)进行插值;
对数字高程图(DEM)进行滤波。
优选地,对数字高程图(DEM)进行插值的方法选择使用设定的配置参数为窗口半径,以反距离加权的方式插值,方法如下:
与未采样点距离最近的若干个点为采样点值贡献最大,其贡献与距离成反比。可用下式表示:
式中Z是估计值,Zi是第i(i=1,2,3…N)个样本,Di是样本离未采样点的距离,p是距离的幂,它显著影响内插的结果,它的选择标准是最小平均绝对误差。
优选地,插值分为高置信度区域,低置信度区域,噪声区域(周围栅格皆为未知,用栅格高度均值插值);
(4)后端模块进行通行性分析,基于三维恢复的地形数据,即数字高程图,结合机器人的位姿和其他信息,对周围环境进行评估分析;参照图5,具体步骤为:输入数字高程图数据,并且显示三维地形;
输入越障高度、爬坡高度、可通行粗糙度等配置参数;
创建栅格地图、栅格地图分为可见区域和不可见区域,对可见区域(输入的数字高程图)计算通行代价,通行代价过大为不可通行区域;
对不可见区域栅格的通行代价设为可通行代价的最大值。
根据通行代价进行可通行性分析,得到可通行区域和障碍识别结果。
具体的说,根据数字高程图计算栅格的通行代价,其通行代价由三个因素综合得出:数字高程图上的障碍物坡度、数字高程图上的粗糙度、数字高程图上的障阶跃高程差。
创建全局栅格地图的方法,具体包括:
数字高程图的分辨率通常较高,如1×1cm分辨率。如果就原始分辨率对数字高程图进行可通过性分析,得到的行进代价地图的分辨率与数字高程图相同。那么,后续的全局路径规划模块进行规划时的搜索空间将会非常大,从而影响规划的效率。因此,在本模块的算法中,设定一个分辨率的变量,用户可根据后续需要自行设定行进代价地图的分辨率。
假设原始数字高程图分辨率为MDEM×NDEM,可通过性地图的分辨率为MGrid×NGrid,那么,栅格的大小为
对于可见区域进行通行代价计算,具体包括:
根据数字高程图对每个栅格计算通行代价。通行代价是用以描述通过一个栅格的难易程度的。每个栅格的通行代价由该栅格以及邻域内的地形特性,包括坡度、粗糙度、以及阶跃高程差所决定。通行代价越低,表明通过该栅格越容易。
对于坡度信息(倾斜度dslope)以栅格(i,j)为中心,考虑邻域窗口内的高度信息。其中邻域窗口的尺寸,一般选取能够包含机器人的外包络轮廓的大小。在计算倾斜度时,首先对窗口内的栅格用最小二乘法拟合平面,然后根据拟合得到的平面计算坡度信息。
对于粗糙度信息(粗糙度drough)在得到栅格邻域内的拟合平面后,粗糙度由邻域内每个栅格到平面的距离的均方根表征。因此,可以根据以下式子计算得到。
其中,di表示第i个栅格到拟合平面的距离,n为栅格总数。
对于阶跃高程差信息(阶跃高程差dstep)一个栅格的阶跃高程差由以该栅格为中心的邻域内最高和最低的高度差得到。
dstep=Hmax-Hmin
对于置信度信息(置信度dc)一个栅格的置信度dc∈[0,1]与窗口内已知栅格的数目和分布有关。使用置信度能够使得以窗口为单位的栅格属性计算更加合理:对于那些已知栅格数目较少或者栅格分布极不平均的窗口,置信度小。这里采用类似生物学中辛普森多样性指数的方法计算栅格分布度。
其中S是窗口中不同分布区域的数目。将窗口分布区域分为4块(S=4),如图6所示(以窗口5*5为例):
Ni是第i个区域含已知栅格数目,Ns是窗口内已知栅格的总数。
栅格的置信度用分布度与已知栅格的比例的乘积计算。
Nknown是窗口内已知栅格总数,Ntotal是窗口内总栅格数目。
一个栅格的通行代价d可由以下式子计算得到。
d=λ1dslope+λ2drough+λ3dstep
考虑置信度dcert后的通行代价计算:
dgoodness=1-(1-d)dcert
(5)后端程序进行远距离路径规划步骤:在以上步骤基础上,基于优化条件给出一条从指定起始点到目标点的最优路径;
(6)后端程序将路径点序列发送到前端程序。
以上结合具体实施方式和范例性实例对本发明进行了详细说明,不过这些说明并不能理解为对本发明的限制。本领域技术人员理解,在不偏离本发明精神和范围的情况下,可以对本发明技术方案及其实施方式进行多种等价替换、修饰或改进,这些均落入本发明的范围内。本发明的保护范围以所附权利要求为准。
本发明说明书中未作详细描述的内容属本领域技术人员的公知技术。
Claims (10)
1.一种地外未知环境超视距全局路径规划方法,其特征在于,包括以下步骤:
S1根据移动探测器或机器人自带双目相机采集的双目立体视觉图像得到当前规划位置的数字高程图;所述数字高程图的范围对应有效可见区域,有效可见区域外为不可见区域;
S2建立全局栅格地图并将数字高程图映射到全局栅格地图中;所述全局栅格地图中的栅格为正方形;
S3根据数字高程图计算全局栅格地图中有效可见区域范围内各栅格的通行代价,并将全局栅格地图中不可见区域范围内各栅格的通行代价设为常数,根据全局栅格地图中各栅格的通行代价得到全局通行性地图;
S4根据全局通行性地图规划当前规划位置到目标点的最优路径;
S5移动探测器或机器人移动过程中,获取新的数字高程图后,将数字高程图映射到全局栅格地图中,并迭代更新全局栅格地图和全局可通行地图,规划下一规划位置到目标点的最优路径;
所述步骤S4中,根据全局通行性地图规划当前规划位置到目标点的最优路径的方法包括:
S41在全局通行性地图中,采用图搜索算法获得全局可达的路径点序列;
S42保留路径点序列中位于有效可视区域内部的路径点;当路径点序列中存在落在有效可视区域边界上的路径点,则将所述路径点保留,当路径点序列中不存在落在有效可视区域边界上的路径点,则在有效可视区域或边界的可通行栅格中增加一个路径点;
S43删除有效可视区域外的路径点。
2.根据权利要求1所述的一种地外未知环境超视距全局路径规划方法,其特征在于,所述步骤S1中,根据采集的双目立体视觉图像得到数字高程图的方法为:
S11对双目立体视觉图像进行校正、立体匹配和滤波,得到视差图;
S12根据相机模型和参数将视差图转换为相机坐标系下的三维点云;
S13将三维点云转化为世界坐标系下分辨率和精度满足要求的数字高程图。
3.根据权利要求1所述的一种地外未知环境超视距全局路径规划方法,其特征在于,所述步骤S2中,全局栅格地图根据当前位置和全局路径规划目标点位置建立,全局路径规划目标点位于可见区域之外时,全局栅格地图范围≥数字高程图范围。
4.根据权利要求1所述的一种地外未知环境超视距全局路径规划方法,其特征在于,所述步骤S3中,根据数字高程图中的障碍物坡度信息、粗糙度信息及障碍阶跃高程差信息计算全局栅格地图中可见区域范围内各栅格的通行代价,获得全局通行性地图。
5.根据权利要求4所述的一种地外未知环境超视距全局路径规划方法,其特征在于,所述步骤S3中,所述各栅格的通行代价dgoodness的计算方法为:
dgoodness=1-(1-d)dcert
d=λ1dslope+λ2drough+λ3dstep
其中,dcert为栅格置信度,dslope为坡度代价,drough为粗糙度代价,dstep为阶跃高程差代价,d为三个代价加权和,λ1,λ2,λ3为三个权重,取值范围为[0,1],取值大小根据移动探测器或机器人对不同障碍的通过能力和探测区域地形分布特点确定;各栅格的通行代价值越高,表示通过难度越大。
6.根据权利要求1所述的一种地外未知环境超视距全局路径规划方法,其特征在于,所述步骤S3中,根据全局栅格地图中各栅格的通行代价得到全局通行性地图的方法为:
根据通行代价的值将各栅格状态确定为可通行、未知区域或障碍三种状态;设置第一阈值和第二阈值,且第一阈值大于第二阈值,所述通行代价大于第一阈值时,确定栅格状态为障碍;所述通行代价小于等于第一阈值且大于第二阈值时,确定栅格状态为未知区域;所述通行代价小于等于第二阈值时,确定栅格状态为可通行。
7.根据权利要求6所述的一种地外未知环境超视距全局路径规划方法,其特征在于,所述步骤S3中,不可见区域范围内各栅格的通行代价设为与第二阈值相等的常数。
8.根据权利要求1所述的一种地外未知环境超视距全局路径规划方法,其特征在于,所述步骤S41中,根据设置参数和优化条件,采用图搜索算法获得全局通行性地图中的路径点序列,所述设置参数包括航向约束和相关移动性能,优化条件为时间最短、里程最短或安全性最高中的一项或多项组合;
图搜索算法以正方形栅格为图的顶点,以正方形栅格中心坐标点为移动探测器或机器人途经的位置,以栅格通行代价为边的权重。
9.根据权利要求1所述的一种地外未知环境超视距全局路径规划方法,其特征在于,所述步骤S42中,当路径点序列中不存在落在有效可视区域边界上的路径点,则在有效可视区域或边界的可通行栅格中增加一个路径点的具体方法为:
连接有效可视区域边界内外的两个路径点,确定连线与边界交点,若交点所在栅格为可通行状态,则将该点输出为路径点,若交点所在栅格为障碍或未知区域,则沿连线向可视区域内搜索距交点最近且所在栅格为可通行状态的点,将其增设为路径点。
10.一种地外未知环境超视距全局路径规划系统,其特征在于,用于实现权利要求1-9任一项所述的一种地外未知环境超视距全局路径规划方法,包括前端模块和后端模块;
所述前端模块将移动探测器或机器人的双目立体视觉图像和路径规划目标点坐标发送至后端模块,后端模块根据双目立体视觉图像和路径规划目标点坐标进行路径规划得到最优路径,并将最优路径实时发送至前端模块,前端模块根据最优路径实时控制移动探测器或机器人移动。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202111092918.XA CN113867340B (zh) | 2021-09-17 | 2021-09-17 | 一种地外未知环境超视距全局路径规划系统和方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202111092918.XA CN113867340B (zh) | 2021-09-17 | 2021-09-17 | 一种地外未知环境超视距全局路径规划系统和方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN113867340A CN113867340A (zh) | 2021-12-31 |
CN113867340B true CN113867340B (zh) | 2024-02-13 |
Family
ID=78996425
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202111092918.XA Active CN113867340B (zh) | 2021-09-17 | 2021-09-17 | 一种地外未知环境超视距全局路径规划系统和方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN113867340B (zh) |
Families Citing this family (3)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN114494329B (zh) * | 2022-04-15 | 2022-07-26 | 之江实验室 | 用于移动机器人在非平面环境自主探索的导引点选取方法 |
CN115435793B (zh) * | 2022-11-08 | 2023-02-17 | 西北工业大学 | 机器人路径规划方法及装置、存储介质、电子设备 |
CN115839717B (zh) * | 2023-02-14 | 2023-05-09 | 电科疆泰(深圳)科技发展有限公司 | 户外搜救最佳路径规划方法、服务器及存储介质 |
Citations (8)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN101308023A (zh) * | 2008-06-05 | 2008-11-19 | 上海交通大学 | 基于异构视觉传感器系统的导航装置和方法 |
KR101096592B1 (ko) * | 2010-09-29 | 2011-12-20 | 국방과학연구소 | 장애물격자지도를 활용하는 무인차량의 자율주행성능 향상 장치 및 방법 |
CN102520721A (zh) * | 2011-12-08 | 2012-06-27 | 北京控制工程研究所 | 一种基于双目立体视觉的巡视探测器自主避障规划方法 |
CN109597415A (zh) * | 2018-12-06 | 2019-04-09 | 山东大学 | 基于月球表面安全着陆区的巡视器路径规划方法及系统 |
CN111982129A (zh) * | 2020-08-24 | 2020-11-24 | 哈尔滨工业大学 | 一种基于月面数字高程地图的综合全局路径规划方法 |
CN112396039A (zh) * | 2021-01-12 | 2021-02-23 | 之江实验室 | 一种基于邻域关系的火星栅格地形地图生成方法 |
CN112904842A (zh) * | 2021-01-13 | 2021-06-04 | 中南大学 | 一种基于代价势场的移动机器人路径规划与优化方法 |
CN113189986A (zh) * | 2021-04-16 | 2021-07-30 | 中国人民解放军国防科技大学 | 一种自主机器人的二阶段自适应行为规划方法及系统 |
-
2021
- 2021-09-17 CN CN202111092918.XA patent/CN113867340B/zh active Active
Patent Citations (8)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN101308023A (zh) * | 2008-06-05 | 2008-11-19 | 上海交通大学 | 基于异构视觉传感器系统的导航装置和方法 |
KR101096592B1 (ko) * | 2010-09-29 | 2011-12-20 | 국방과학연구소 | 장애물격자지도를 활용하는 무인차량의 자율주행성능 향상 장치 및 방법 |
CN102520721A (zh) * | 2011-12-08 | 2012-06-27 | 北京控制工程研究所 | 一种基于双目立体视觉的巡视探测器自主避障规划方法 |
CN109597415A (zh) * | 2018-12-06 | 2019-04-09 | 山东大学 | 基于月球表面安全着陆区的巡视器路径规划方法及系统 |
CN111982129A (zh) * | 2020-08-24 | 2020-11-24 | 哈尔滨工业大学 | 一种基于月面数字高程地图的综合全局路径规划方法 |
CN112396039A (zh) * | 2021-01-12 | 2021-02-23 | 之江实验室 | 一种基于邻域关系的火星栅格地形地图生成方法 |
CN112904842A (zh) * | 2021-01-13 | 2021-06-04 | 中南大学 | 一种基于代价势场的移动机器人路径规划与优化方法 |
CN113189986A (zh) * | 2021-04-16 | 2021-07-30 | 中国人民解放军国防科技大学 | 一种自主机器人的二阶段自适应行为规划方法及系统 |
Non-Patent Citations (4)
Title |
---|
Xiang Liu 等.A Method of Ground-based Navigation Plan for Chang'e-3 Lunar Rover.《IEEE》.2014,全文. * |
基于栅格地图的月球车任务层路径规划及平滑处理;束磊;《CNKI》;全文 * |
基于能量约束的火星车路径规划研究;刘涛 等;《北京邮电大学学报》;全文 * |
复杂地形环境下基于滑移预测的月球车路径规划算法研究;解维治;《CNKI》;全文 * |
Also Published As
Publication number | Publication date |
---|---|
CN113867340A (zh) | 2021-12-31 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN113867340B (zh) | 一种地外未知环境超视距全局路径规划系统和方法 | |
CN110531760B (zh) | 基于曲线拟合和目标点邻域规划的边界探索自主建图方法 | |
CN106802668B (zh) | 基于双目与超声波融合的无人机三维避撞方法及系统 | |
CN105844629B (zh) | 一种大场景城市建筑物立面点云自动分割方法 | |
CN106595659A (zh) | 城市复杂环境下多无人机视觉slam的地图融合方法 | |
CN103869820A (zh) | 一种巡视器地面导航规划控制方法 | |
Fruh et al. | Fast 3D model generation in urban environments | |
Wang et al. | A bayesian approach to building footprint extraction from aerial lidar data | |
CN110298914B (zh) | 一种建立果园中果树冠层特征地图的方法 | |
CN105096386A (zh) | 大范围复杂城市环境几何地图自动生成方法 | |
CN106382943A (zh) | 移动装置以及用于向其发射数据的方法和系统 | |
CN110488871B (zh) | 一种基于二三维一体化的无人机路径动态规划方法 | |
CN115564926B (zh) | 基于影像建筑物结构学习的三维面片模型构建方法 | |
CN110889899A (zh) | 一种数字地表模型的生成方法及装置 | |
CN115639823A (zh) | 崎岖起伏地形下机器人地形感知与移动控制方法及系统 | |
Palanirajan et al. | Efficient flight planning for building façade 3D reconstruction | |
KR101319526B1 (ko) | 이동 로봇을 이용하여 목표물의 위치 정보를 제공하기 위한 방법 | |
CN111006645A (zh) | 基于运动与结构重建的无人机测绘方法 | |
CN113155126B (zh) | 一种基于视觉导航的多机协同目标高精度定位系统及方法 | |
Rothermel et al. | Fast and robust generation of semantic urban terrain models from UAV video streams | |
Siddiqui et al. | An extensible multi-sensor fusion framework for 3d imaging | |
CN111986320A (zh) | 面向智慧城市应用的dem与倾斜摄影模型空间贴合优化算法 | |
CN116380039A (zh) | 一种基于固态激光雷达和点云地图的移动机器人导航系统 | |
CN115790621A (zh) | 高精地图更新方法、装置及电子设备 | |
Rumpler et al. | Rapid 3d city model approximation from publicly available geographic data sources and georeferenced aerial images |
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 |