CN111596665B - 一种适用于腿足机器人规划的稠密高度地图构建方法 - Google Patents
一种适用于腿足机器人规划的稠密高度地图构建方法 Download PDFInfo
- Publication number
- CN111596665B CN111596665B CN202010475455.4A CN202010475455A CN111596665B CN 111596665 B CN111596665 B CN 111596665B CN 202010475455 A CN202010475455 A CN 202010475455A CN 111596665 B CN111596665 B CN 111596665B
- Authority
- CN
- China
- Prior art keywords
- map
- height
- local
- grid
- robot
- 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
- 238000010276 construction Methods 0.000 title claims abstract description 27
- 238000000034 method Methods 0.000 claims abstract description 20
- 238000005259 measurement Methods 0.000 claims abstract description 17
- 238000009826 distribution Methods 0.000 claims abstract description 4
- 230000004927 fusion Effects 0.000 claims abstract description 4
- 239000011159 matrix material Substances 0.000 claims description 28
- 238000006073 displacement reaction Methods 0.000 claims description 9
- 238000013507 mapping Methods 0.000 claims description 7
- 239000004576 sand Substances 0.000 claims description 4
- 238000001914 filtration Methods 0.000 claims description 3
- 238000012546 transfer Methods 0.000 claims description 3
- 238000013519 translation Methods 0.000 claims description 3
- 230000007547 defect Effects 0.000 abstract description 3
- 230000008859 change Effects 0.000 description 5
- 230000006870 function Effects 0.000 description 4
- 230000006399 behavior Effects 0.000 description 2
- 238000001514 detection method Methods 0.000 description 2
- 230000014759 maintenance of location Effects 0.000 description 2
- 230000008569 process Effects 0.000 description 2
- 238000011160 research Methods 0.000 description 2
- 206010047571 Visual impairment Diseases 0.000 description 1
- 230000009471 action Effects 0.000 description 1
- 238000013459 approach Methods 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000007123 defense Effects 0.000 description 1
- 238000005516 engineering process Methods 0.000 description 1
- 230000007613 environmental effect Effects 0.000 description 1
- 238000011156 evaluation Methods 0.000 description 1
- 230000007774 longterm Effects 0.000 description 1
- 230000008447 perception Effects 0.000 description 1
- 238000012545 processing Methods 0.000 description 1
- 238000012800 visualization Methods 0.000 description 1
Images
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/0257—Control of position or course in two dimensions specially adapted to land vehicles using a radar
-
- 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)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Aviation & Aerospace Engineering (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
本发明公开了一种稠密高度地图构建方法,属于稠密高度地图构建技术领域。该构建方法具体为:本发明使用高斯分布模型估计激光雷达的测量值,并将其映射到局部地图相应的栅格,以进行多帧数据融合,其间根据机器人位姿信息更新地图栅格协方差,并对局部地图进行相应移动,且对动态障碍物进行清除,以构成完整的局部稠密地图。本发明的方法克服了现有稠密地图数据量大,构建计算量大,更新不及时的不足,并且具有构建方便,构建精度高等优点,并且可以完成实时的构建,可以直接用于腿足机器人的导航任务。
Description
技术领域
本发明涉及稠密高度地图构建技术,具体地说,涉及一种适用于腿足机器人规划的稠密高度地图构建方法。
背景技术
移动机器人是一个集环境感知、动态决策与规划、行为控制与执行等多功能于一体的综合系统,不仅广泛运用于工业、农业、医疗、服务等行业中,而且在城市安全、国防和空间探测领域等对人有害且极度危险的场合得到了很好的应用。近年来,腿足机器人因其卓越的复杂地形可通行性受到全世界的关注并成为研究的热点。相比于轮式和履带式机器人,腿足机器人不仅仅拥有前进、后退和转向等相同的运动控制行为,它还能通过控制前足和后足的落脚点位置,实现“跨越”的动作,这种操作能让腿足机器人轻易通过壕沟、低洼地等地形,也能让其攀上台阶等传统轮式机器人不能通过的复杂场景。
针对地面机器人的路径规划任务,现有的地图表示方法主要使用二维栅格地图描述外界环境。通过地图中的二维栅格是否被占用来表示对应地点是否存在障碍物。这种表示方法构建简单,数据检索快速,适用于轮式和履带式机器人的路径规划,但其无法提供环境地形的高度信息以便腿足机器人的落脚点规划。因此,为了实现腿足机器人的高效运动规划,需要一种能够满足腿足机器人导航规划的可描述复杂地形的稠密地图。稠密地图是由距离传感器实时获取的数据根据机器人位姿信息进行累计而形成的地图,它解决了距离传感器检测范围小、分辨率低的问题,从而能够完整地描述场景的表面形状。稠密地图主要被应用于三维重构以及语义地图的研究中,但是由于其数据量大的特性给实时移动机器人系统上的地图存储和处理带来了较大的挑战。
结合目前常用的传感器,如何构建一种高效的地图更新系统实现稠密地图的实时存储和计算,如何将稠密地图运用于常用的路径导航,都是目前稠密建图中需要解决的的难题和挑战。
发明内容
为了克服现有技术的不足,本发明的目的在于提供一种适用于腿足机器人规划的稠密高度地图构建方法,对于实现腿足移动机器人的长期高效稳定作业有着重要意义。
本发明是通过以下技术方案来实现的:一种稠密高度地图构建方法,包括以下步骤:
步骤一:使用激光雷达以10-20Hz的频率获取周边环境的点云信息,所述点云信息映射到局部高度图中,在局部高度图中,高度测量值为高斯概率分布,其近似为 其中,p为测量的高度值,δp 2为方差。在激光雷达坐标系S下,获取激光雷达到地形的单个测量值将其转换为相应的高度值p,具体为:
其中,∑s代表了为激光雷达的协方差矩阵,∑ΦIS代表了为激光雷达关于角度的协方差矩阵。
对激光雷达的所有测量值进行上述操作,使所有测量值映射到局部地图对应栅格进行融合,从而构建完整的局部地图。
进一步地,所述局部地图方差δh 2+组成栅格协方差,具体方法为:
其中,∑r表示机器人B1移动至B2平移上的不确定度,∑Φ表示B1移动至B2旋转变化上的不确定度。
进一步地,所述局部地图构建方法还包括:当局部地图移动后,通过机器人的定位估计系统,得到机器人与上一时刻X方向上的位移XShift和Y方向上的位移YShift,设定局部地图分辨率为s,并根据机器人的移动信息更新位置偏移值XStartIndex和YStartIndex,获得
局部地图在栅格上的相对位移为Xindex_shift和Yindex_shift:
XstartIndex=(XstartIndex-round(XShift/s)+Lx)%Lx(9)
YstartIndex=(YstartIndex-round(YShift/s)+Ly)%Ly(10)
其中,round()是对小数进行四舍五入获取整数的函数操作,%为求余操作,Lx和Ly分别表示栅格地图的X和Y方向的栅格数量。当需要查询地图(x,y)栅格信息时,通过下列映射查询(xsearch,ysearch)存储位置的数据内容。
xsearch=(Lx-XstartIndex+x)%Lx (11)
ysearch=(Ly-YstartIndex+y)%Ly (12)
进一步地,所述局部地图构建方法还包括:
局部地图构建下的点云信息更新直接添加到局部地图中,对于激光雷达的每一个测量值,该测量值到激光雷达在地面投影点的线段上的所有点的地形高度都有高度限制hlimitation:
其中,hR_P为激光雷达到观测点的高度差,LR_P为激光雷达到观测点的水平距离,LC_P为待求高度限制的栅格到观测点的水平距离。若上一时刻局部高度图的高度数据超过相应的高度限制,则代表物体发生移动,相应点的高度信息需要被清空。
相比现有技术,本发明的有益效果在于:本发明的方法使用GPU和多线程并行计算,让整个建图过程可以实时运行。构建了一种2.5D的带有可行域信息的稠密地图,便于腿足机器人的规划。只对判断为障碍物的栅格进行光线追踪,而不是对地图所有区域进行光线追踪,以判断该区域是不是移动障碍物的残影。使用这种策略构建,动态障碍物可以有效地被处理。本发明克服了现有稠密地图数据量大,构建计算量大,更新不及时的不足,并且具有构建方便,构建精度高等优点,并且可以完成实时的构建,可以直接用于腿足机器人的导航任务。
附图说明
图1为本发明稠密高度地图构建方法流程示意图。
具体实施方法
下面,结合附图以及具体实施方式,对本发明的技术方案做进一步地描述:
如图1,本发明提供了一种稠密地图构建方法,包括以下步骤:
步骤一:使用激光雷达以10-20Hz的频率获取周边环境的点云信息,所述点云信息映射到局部高度图中,在局部高度图中,高度测量值为高斯概率分布,其近似为 其中,p为测量的高度值,δp 2为方差。在激光雷达坐标系S下,获取激光雷达到地形的单个测量值将其转换为相应的高度值p,具体为:
其中,∑s代表了为激光雷达的协方差矩阵,∑ΦIS代表了为激光雷达关于角度的协方差矩阵。
对激光雷达的所有测量值进行上述操作,使所有测量值映射到局部地图对应栅格进行融合,从而构建完整的局部地图。
所述局部地图方差δh 2+组成栅格协方差,在更新地图每个栅格协方差的方法中,由于基于M坐标系的局部高度图是相对于传感器/机器人的参考系定义的,因此每当机器人相对于惯性坐标系I运动时,需要根据姿态估计的变化来更新局部地图信息,包括平均高度h和方差值δp 2。地图上每个栅格的方差和平均值会根据运动的不确定性以及周围方格的估计值进行更新。
其中,∑r表示机器人B1移动至B2平移上的不确定度,∑Φ表示B1移动至B2旋转变化上的不确定度。
在完成稠密地图更新后,固定尺寸和像素的稠密栅格地图会伴随着机器人位置的移动而移动。为简化计算量,本方法构建的稠密地图只有沿x和y方向两种方向的平移运动。在这个过程中,小部分稠密图边缘的数据因为可视化区域的变化而被删除,大部分地图区域仅仅更改存储数据的坐标位置但仍保留数据内容。因此,当地图移动时的通常做法是将地图保留数据的存储位置根据机器人的位置变化而变化,但这对大尺寸的栅格地图来说是巨大的计算量。每次地图移动后,地图保留数据之间的相互位置关系从未改变。基于此发现,地图移动后,我们不会改变地图数据的存储位置,而是根据机器人的移动信息来更新位置偏移值XStartIndex和YStartIndex。当需要查询地图不同位置信息时,我们可以使用这两个值来查询所有栅格的数据内容,具体为:
当局部地图移动后,通过机器人的定位估计系统,得到机器人与上一时刻X方向上的位移XShift和Y方向上的位移YShift,设定局部地图分辨率为s,并根据机器人的移动信息更新位置偏移值XStartIndex和YStartIndex,获得
局部地图在栅格上的相对位移为Xindex_shift和Yindex_shift:
XstartIndex=(XstartIndex-round(XShift/s)+Lx)%Lx (9)
YstartIndex=(YstartIndex-round(YShift/s)+Ly)%Ly (10)
其中,round()是对小数进行四舍五入获取整数的函数操作,%为求余操作,Lx和Ly分别表示栅格地图的X和Y方向的栅格数量。当需要查询地图(x,y)栅格信息时,通过下列映射查询(xsearch,ysearch)存储位置的数据内容。
xsearch=(Lx-XstartIndex+x)%Lx (11)
ysearch=(Ly-YstartIndex+y)%Ly (12)
局部地图构建下的点云信息更新直接添加到局部地图中,而对于新观测中消失的点无法轻易判断出是否是因为物体移动而导致的。因此应对因物体移动需消去原位置点云的问题,主要通过光线追踪的方法来处理,其原理是当前时刻中若地面上的某点可以和激光雷达形成光路,就默认了该点到激光雷达在地面投影点的线段上的每一个点的地形高度都不会阻挡该条光路,即这条线段上的所有点都会有最大的高度限制,即对于激光雷达的每一个测量值,该测量值到激光雷达在地面投影点的线段上的所有点的地形高度都有高度限制hlimitation:
其中,hR_P为激光雷达到观测点的高度差,LR_P为激光雷达到观测点的水平距离,LC_P为待求高度限制的栅格到观测点的水平距离。若上一时刻局部高度图的高度数据超过相应的高度限制,则代表物体发生移动,相应点的高度信息需要被清空。因此应用光线跟踪原理,并根据传感器实时的点云数据,每条光路上栅格的高度限制可以被计算,以实现稠密高度地图上的移动物体移除。
对比本发明方法构建的稠密高度地图,地图每个区域的可行性通过几何结构特征来评估。对于地图的每个栅格,提取其表面法向量(代表局部面的斜率和曲率)和其附近栅格的高度差(代表局部面的粗糙程度)作为局部区域的几何特征。
为了获得栅格Pi处的表面法向量ni,一个经典的方法是以Pi为原点,结合临近栅格的坐标和高度信息,拟合一个平面Si=nixx+niyy+nizz。当拟合的平面与所有三维点的距离和最小时,即可求得法向量ni,具体公式如下:
其中k代表临近区域的栅格数量,Qi是一个3*K的带有临近K个点的三维坐标信息的矩阵,Pi是待评估栅格二维坐标和高度信息组合的三维向量,ni=[nix,niy,niz]是需要求解的法向量。
将单位法向量转换成相应栅格的坡度信息并结合高度差,使用简单而有效的评估公式:
Claims (4)
1.一种稠密高度地图构建方法,其特征在于,包括以下步骤:
使用激光雷达以10-20Hz的频率获取周边环境的点云信息,所述点云信息映射到局部高度图中,在局部高度图中,高度测量值为高斯概率分布,其近似为其中,p为测量的高度值,δp 2为方差;在激光雷达坐标系S下,获取激光雷达到地形的单个测量值将其转换为相应的高度值p,具体为:
其中,∑s代表了为激光雷达的协方差矩阵,∑ΦIS代表了为激光雷达关于角度的协方差矩阵;
对激光雷达的所有测量值进行上述操作,使所有测量值映射到局部地图对应栅格进行融合,从而构建完整的局部地图。
3.根据权利要求1所述稠密高度地图构建方法,其特征在于,局部地图构建方法还包括:当局部地图移动后,通过机器人的定位估计系统,得到机器人与上一时刻X方向上的位移XShift和Y方向上的位移YShift,设定局部地图分辨率为s,并根据机器人的移动信息更新位置偏移值XStartIndex和YStartIndex,获得
局部地图在栅格上的相对位移为Xindex_shift和Yindex_shift:
XstartIndex=(XstartIndex-round(XShift/s)+Lx)%Lx (9)
YstartIndex=(YstartIndex-round(YShift/s)+Ly)%Ly (10)
其中,round()是对小数进行四舍五入获取整数的函数操作,%为求余操作,Lx和Ly分别表示栅格地图的X和Y方向的栅格数量;当需要查询地图(x,y)栅格信息时,通过下列映射查询(xsearch,ysearch)存储位置的数据内容;
xsearch=(Lx-XstartIndex+x)%Lx (11)
ysearch=(Ly-YstartIndex+y)%Ly (12)。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202010475455.4A CN111596665B (zh) | 2020-05-29 | 2020-05-29 | 一种适用于腿足机器人规划的稠密高度地图构建方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202010475455.4A CN111596665B (zh) | 2020-05-29 | 2020-05-29 | 一种适用于腿足机器人规划的稠密高度地图构建方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN111596665A CN111596665A (zh) | 2020-08-28 |
CN111596665B true CN111596665B (zh) | 2021-06-22 |
Family
ID=72191421
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202010475455.4A Active CN111596665B (zh) | 2020-05-29 | 2020-05-29 | 一种适用于腿足机器人规划的稠密高度地图构建方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN111596665B (zh) |
Families Citing this family (7)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN113406659A (zh) * | 2021-05-28 | 2021-09-17 | 浙江大学 | 一种基于激光雷达信息的移动机器人位置重识别方法 |
CN113340304B (zh) * | 2021-06-03 | 2023-02-17 | 青岛慧拓智能机器有限公司 | 坡度提取方法及装置 |
CN115509216B (zh) * | 2021-06-21 | 2024-10-15 | 广州视源电子科技股份有限公司 | 路径规划方法、装置、计算机设备和存储介质 |
CN115503850B (zh) * | 2022-11-17 | 2023-03-31 | 国网智能科技股份有限公司 | 足式机器人足端触地检测方法及系统 |
CN115771163A (zh) * | 2022-11-17 | 2023-03-10 | 国网智能科技股份有限公司 | 足式机器人系统及方法 |
CN116358517B (zh) * | 2023-02-24 | 2024-02-23 | 杭州宇树科技有限公司 | 一种用于机器人的高度地图构建方法、系统以及存储介质 |
CN116147642B (zh) * | 2023-04-12 | 2023-08-29 | 中国科学技术大学 | 融合地形与力的四足机器人可达性地图构建方法及系统 |
Family Cites Families (6)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US10837773B2 (en) * | 2016-12-30 | 2020-11-17 | DeepMap Inc. | Detection of vertical structures based on LiDAR scanner data for high-definition maps for autonomous vehicles |
CN106875482B (zh) * | 2017-01-13 | 2020-04-28 | 浙江大学 | 一种同时定位与稠密三维重建方法 |
CN108648274B (zh) * | 2018-05-10 | 2020-05-22 | 华南理工大学 | 一种视觉slam的认知点云地图创建系统 |
CN109991636B (zh) * | 2019-03-25 | 2024-09-13 | 启明信息技术股份有限公司 | 基于gps、imu以及双目视觉的地图构建方法及系统 |
CN110675483B (zh) * | 2019-07-17 | 2022-09-09 | 电子科技大学 | 基于稠密视觉slam的无人机三维地图快速重建方法 |
CN110658530B (zh) * | 2019-08-01 | 2024-02-23 | 北京联合大学 | 一种基于双激光雷达数据融合的地图构建方法、系统及地图 |
-
2020
- 2020-05-29 CN CN202010475455.4A patent/CN111596665B/zh active Active
Also Published As
Publication number | Publication date |
---|---|
CN111596665A (zh) | 2020-08-28 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN111596665B (zh) | 一种适用于腿足机器人规划的稠密高度地图构建方法 | |
WO2021237667A1 (zh) | 一种适用于腿足机器人规划的稠密高度地图构建方法 | |
CN111220993B (zh) | 目标场景定位方法、装置、计算机设备和存储介质 | |
CN109509210B (zh) | 障碍物跟踪方法和装置 | |
CN104764457B (zh) | 一种用于无人车的城市环境构图方法 | |
Weng et al. | Pole-based real-time localization for autonomous driving in congested urban scenarios | |
CN108763287A (zh) | 大规模可通行区域驾驶地图的构建方法及其无人驾驶应用方法 | |
CN110488818B (zh) | 一种基于激光雷达的机器人定位方法、装置和机器人 | |
CN113515128B (zh) | 一种无人车实时路径规划方法及存储介质 | |
CN111862214B (zh) | 计算机设备定位方法、装置、计算机设备和存储介质 | |
CN115639823B (zh) | 崎岖起伏地形下机器人地形感知与移动控制方法及系统 | |
Aldibaja et al. | LIDAR-data accumulation strategy to generate high definition maps for autonomous vehicles | |
CN114758063A (zh) | 基于八叉树结构的局部障碍物栅格地图构建方法及系统 | |
CN112907625A (zh) | 应用于四足仿生机器人的目标跟随方法及系统 | |
Pang et al. | Low-cost and high-accuracy LIDAR SLAM for large outdoor scenarios | |
CN113960614A (zh) | 一种基于帧-地图匹配的高程图构建方法 | |
CN116879870A (zh) | 一种适用于低线束3d激光雷达的动态障碍物去除方法 | |
CN117490683A (zh) | 一种井下隧道多传感器融合算法的定位建图方法 | |
CN115049910A (zh) | 一种基于双目视觉里程计的足式机器人建图与导航方法 | |
CN117705123A (zh) | 一种轨迹规划方法、装置、设备及存储介质 | |
CN116147653B (zh) | 一种面向无人驾驶车辆的三维参考路径规划方法 | |
Pauls et al. | Automatic mapping of tailored landmark representations for automated driving and map learning | |
CN112505723B (zh) | 一种基于导航点选择的三维地图重建方法 | |
JP2023173688A (ja) | 移動体、事前地図生成方法、位置推定システム、位置推定方法、事前地図生成方法、およびプログラム | |
CN113340310A (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 | ||
TR01 | Transfer of patent right | ||
TR01 | Transfer of patent right |
Effective date of registration: 20240624 Address after: Building 5, 5th Floor, No. 309 Liuhe Road, Binjiang District, Hangzhou City, Zhejiang Province, 310000 Patentee after: Supcon Group Co.,Ltd. Country or region after: China Address before: 310058 Yuhang Tang Road, Xihu District, Hangzhou, Zhejiang 866 Patentee before: ZHEJIANG University Country or region before: China |