WO2021237667A1 - 一种适用于腿足机器人规划的稠密高度地图构建方法 - Google Patents

一种适用于腿足机器人规划的稠密高度地图构建方法 Download PDF

Info

Publication number
WO2021237667A1
WO2021237667A1 PCT/CN2020/093236 CN2020093236W WO2021237667A1 WO 2021237667 A1 WO2021237667 A1 WO 2021237667A1 CN 2020093236 W CN2020093236 W CN 2020093236W WO 2021237667 A1 WO2021237667 A1 WO 2021237667A1
Authority
WO
WIPO (PCT)
Prior art keywords
map
height
local
lidar
grid
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.)
Ceased
Application number
PCT/CN2020/093236
Other languages
English (en)
French (fr)
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 University
Original Assignee
Zhejiang University
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 University filed Critical Zhejiang University
Priority to PCT/CN2020/093236 priority Critical patent/WO2021237667A1/zh
Publication of WO2021237667A1 publication Critical patent/WO2021237667A1/zh
Anticipated expiration legal-status Critical
Ceased legal-status Critical Current

Links

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

Definitions

  • the invention relates to a dense height map construction technology, in particular to a dense height map construction method suitable for leg and foot robot planning.
  • Mobile robot is a comprehensive system integrating environment perception, dynamic decision-making and planning, behavior control and execution, etc. It is not only widely used in industry, agriculture, medical, service and other industries, but also in urban security, national defense and space detection. Fields and other occasions that are harmful to people and extremely dangerous have been well applied. In recent years, leg-foot robots have attracted worldwide attention and become a research hotspot due to their excellent accessibility in complex terrain. Compared with wheeled and crawler robots, the legged robot not only has the same motion control behaviors as forward, backward, and turning, but it can also achieve a "crossover" movement by controlling the footing position of the forefoot and hindfoot. This kind of operation allows the legged robot to easily pass through terrain such as trenches and low-lying land, and also allows it to climb steps and other complex scenes that traditional wheeled robots cannot pass.
  • the existing map representation methods mainly use two-dimensional grid maps to describe the external environment. Indicates whether there are obstacles in the corresponding place by whether the two-dimensional grid in the map is occupied.
  • This representation method is simple to construct and fast to retrieve data. It is suitable for the path planning of wheeled and crawler robots, but it cannot provide height information of the environmental terrain for the footing planning of the legged robot. Therefore, in order to realize the efficient motion planning of the legged robot, a dense map that can describe the complex terrain that can meet the navigation planning of the legged robot is required.
  • the dense map is a map formed by accumulating the data obtained by the distance sensor in real time according to the pose information of the robot.
  • Dense maps are mainly used in the research of three-dimensional reconstruction and semantic maps, but due to their large amount of data, it brings greater challenges to the map storage and processing on real-time mobile robot systems.
  • the purpose of the present invention is to provide a dense height map construction method suitable for the planning of leg-foot robots, which is of great significance for realizing the long-term efficient and stable operation of the leg-foot mobile robot.
  • a method for constructing a dense height map including the following steps:
  • Step 1 Use lidar to obtain point cloud information of the surrounding environment at a frequency of 10-20 Hz.
  • the point cloud information is mapped to a local height map.
  • the height measurement value is a Gaussian probability distribution, which is approximately Among them, p is the measured height value, and ⁇ p 2 is the variance.
  • p is the measured height value
  • ⁇ p 2 is the variance.
  • Pass height value p single measurement value ⁇ SM obtains the distance Jacobian matrix J s and the rotating Jacobian matrix J ⁇ measured by the lidar, and obtains the variance ⁇ p 2 of the height value p.
  • ⁇ s represents the covariance matrix of the lidar
  • ⁇ ⁇ IS represents the covariance matrix of the lidar with respect to the angle.
  • the latest height value p obtained by the lidar and the variance (p, ⁇ p 2 ) of the height value p are estimated with the grid corresponding to the local height map Perform fusion, update to get the latest local map height And the local map variance ⁇ h 2+ .
  • the local map variance ⁇ h 2+ constitutes the grid covariance, and the specific method is:
  • the covariance matrix at time k is set for each grid of the local height map
  • ⁇ x, min 2 and ⁇ y, min 2 respectively represent the uncertainty of the horizontal plane of the local height map in the x and y directions, Indicates the high degree of uncertainty of the grid.
  • ⁇ r represents the uncertainty of the translational movement of the robot B 1 to B 2
  • ⁇ ⁇ represents the uncertainty of the rotation change of the movement of B 1 to B 2.
  • the local map construction method further includes: when the local map moves, through the robot's positioning estimation system, obtain the displacement X Shift in the X direction of the robot and the displacement Y Shift in the Y direction from the previous moment, and set the local The map resolution is s, and the position offset values X StartIndex and Y StartIndex are updated according to the movement information of the robot to obtain
  • the relative displacement of the local map on the grid is X index_shift and Y index_shift :
  • X startIndex (X startIndex -round(X Shift /s)+L x )%L x (9)
  • Y startIndex (Y startIndex -round(Y Shift /s)+L y )%L y (10)
  • round() is a function operation for rounding decimals to obtain integers
  • % is a remainder operation
  • L x and Ly represent the number of grids in the X and Y directions of the grid map, respectively.
  • the local map construction method further includes:
  • the update of the point cloud information under the local map construction is directly added to the local map.
  • the measured value to the terrain height of all points on the line segment of the lidar projection point on the ground has a height limitation h limitation :
  • h R_P laser radar observation point to the height difference, L R_P lidar horizontal distance of the observation point, L C_P be required to limit the height of the grid to the horizontal distance of the observation point. If the height data of the local height map at the last moment exceeds the corresponding height limit, it means that the object is moving, and the height information of the corresponding point needs to be cleared.
  • the beneficial effect of the present invention is that the method of the present invention uses GPU and multi-threaded parallel computing, so that the entire mapping process can be run in real time.
  • a 2.5D dense map with feasible region information is constructed to facilitate the planning of legged robots. Only ray tracing the grid that is judged as an obstacle, instead of ray tracing all areas of the map, to determine whether the area is the afterimage of a moving obstacle. Using this strategy construction, dynamic obstacles can be effectively dealt with.
  • the invention overcomes the shortcomings of large data volume, large construction calculation volume, and untimely update of the existing dense map, and has the advantages of convenient construction and high construction accuracy, and can complete real-time construction, and can be directly used for the navigation task of leg-foot robots. .
  • Fig. 1 is a schematic flow chart of the method for constructing a dense height map of the present invention.
  • the present invention provides a dense map construction method, including the following steps:
  • Step 1 Use lidar to obtain point cloud information of the surrounding environment at a frequency of 10-20 Hz.
  • the point cloud information is mapped to a local height map.
  • the height measurement value is a Gaussian probability distribution, which is approximately Among them, p is the measured height value, and ⁇ p 2 is the variance.
  • p is the measured height value
  • ⁇ p 2 is the variance.
  • Pass height value p single measurement value ⁇ SM obtains the distance Jacobian matrix J s and the rotating Jacobian matrix J ⁇ measured by the lidar, and obtains the variance ⁇ p 2 of the height value p.
  • ⁇ s represents the covariance matrix of the lidar
  • ⁇ ⁇ IS represents the covariance matrix of the lidar with respect to the angle.
  • the latest height value p obtained by the lidar and the variance (p, ⁇ p 2 ) of the height value p are estimated with the grid corresponding to the local height map Perform fusion, update to get the latest local map height And the local map variance ⁇ h 2+ .
  • the local map variance ⁇ h 2+ constitutes the grid covariance.
  • the local map information needs to be updated according to the change of the posture estimation, including the average height h and the variance value ⁇ p 2 .
  • the variance and average of each grid on the map will be updated based on the uncertainty of the motion and the estimated value of the surrounding grid.
  • the covariance matrix at time K is set for each grid of the local height map
  • ⁇ x, min 2 and ⁇ y, min 2 respectively represent the uncertainty of the horizontal plane of the local height map in the x and y directions, Indicates the high degree of uncertainty of the grid.
  • ⁇ r represents the uncertainty of the translational movement of the robot B 1 to B 2
  • ⁇ ⁇ represents the uncertainty of the rotation change of the movement of B 1 to B 2.
  • the dense grid map with a fixed size and pixels will move along with the movement of the robot's position.
  • the dense map constructed by this method has only two translational motions along the x and y directions.
  • a small part of the data on the edge of the dense map is deleted due to changes in the visualization area, and most of the map area only changes the coordinate position of the stored data but still retains the data content. Therefore, the common practice when the map is moving is to change the storage location of the map reserved data according to the position of the robot, but this is a huge amount of calculation for a large-size raster map.
  • the mutual positional relationship between the retained data on the map has never changed.
  • the local map moves, through the robot's positioning estimation system, the displacement X Shift in the X direction and the displacement Y Shift in the Y direction of the robot from the previous moment are obtained, and the local map resolution is set to s, and according to the robot's movement information Update the position offset values X StartIndex and Y StartIndex to obtain
  • the relative displacement of the local map on the grid is X index_shift and Y index_shift :
  • X startIndex (X startIndex -round(X Shift /s)+L x )%L x (9)
  • Y startIndex (Y startIndex -round(Y Shift /s)+L y )%L y (10)
  • round() is a function operation for rounding decimals to obtain integers
  • % is a remainder operation
  • L x and Ly represent the number of grids in the X and Y directions of the grid map, respectively.
  • the principle is that if a point on the ground can form a light path with the lidar at the current moment, the point is defaulted to the lidar
  • the terrain height of each point on the line segment of the ground projection point will not block the light path, that is, all points on this line segment will have a maximum height limit, that is, for each measurement value of the lidar, the measurement value
  • the terrain height of all points on the line segment to the lidar projection point on the ground has a height limitation h limitation :
  • h R_P laser radar observation point to the height difference, L R_P lidar horizontal distance of the observation point, L C_P be required to limit the height of the grid to the horizontal distance of the observation point. If the height data of the local height map at the last moment exceeds the corresponding height limit, it means that the object is moving, and the height information of the corresponding point needs to be cleared. Therefore, using the ray tracing principle, and according to the real-time point cloud data of the sensor, the height limit of the grid on each light path can be calculated to realize the removal of moving objects on the dense height map.
  • the feasibility of each area of the map is evaluated by geometric structure characteristics.
  • the surface normal vector representing the slope and curvature of the local surface
  • the height difference of the nearby grids representing the roughness of the local surface
  • k represents the number of grids in the adjacent area
  • Q i is a 3*K matrix with the three-dimensional coordinate information of the adjacent K points
  • P i is the three-dimensional vector of the combination of the two-dimensional coordinate and height information of the grid to be evaluated
  • n i [n ix ,n iy ,n iz ] is the normal vector to be solved.
  • the height difference of the grid can be calculated by establishing a grid window with P (x,y) as the center and a size of N*N.
  • the average height of the grid in this window is The height of the grid P (x, y) is The height difference is:

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

本发明公开了一种稠密高度地图构建方法，属于稠密高度地图构建技术领域。该构建方法具体为：本发明使用高斯分布模型估计激光雷达的测量值，并将其映射到局部地图相应的栅格，以进行多帧数据融合，其间根据机器人位姿信息更新地图栅格协方差，并对局部地图进行相应移动，且对动态障碍物进行清除，以构成完整的局部稠密地图。本发明的方法克服了现有稠密地图数据量大，构建计算量大，更新不及时的不足，并且具有构建方便，构建精度高等优点，并且可以完成实时的构建，可以直接用于腿足机器人的导航任务。

Description

一种适用于腿足机器人规划的稠密高度地图构建方法 技术领域
本发明涉及稠密高度地图构建技术，具体地说，涉及一种适用于腿足机器人规划的稠密高度地图构建方法。
背景技术
移动机器人是一个集环境感知、动态决策与规划、行为控制与执行等多功能于一体的综合系统，不仅广泛运用于工业、农业、医疗、服务等行业中，而且在城市安全、国防和空间探测领域等对人有害且极度危险的场合得到了很好的应用。近年来，腿足机器人因其卓越的复杂地形可通行性受到全世界的关注并成为研究的热点。相比于轮式和履带式机器人，腿足机器人不仅仅拥有前进、后退和转向等相同的运动控制行为，它还能通过控制前足和后足的落脚点位置，实现“跨越”的动作，这种操作能让腿足机器人轻易通过壕沟、低洼地等地形，也能让其攀上台阶等传统轮式机器人不能通过的复杂场景。
针对地面机器人的路径规划任务，现有的地图表示方法主要使用二维栅格地图描述外界环境。通过地图中的二维栅格是否被占用来表示对应地点是否存在障碍物。这种表示方法构建简单，数据检索快速，适用于轮式和履带式机器人的路径规划，但其无法提供环境地形的高度信息以便腿足机器人的落脚点规划。因此，为了实现腿足机器人的高效运动规划，需要一种能够满足腿足机器人导航规划的可描述复杂地形的稠密地图。稠密地图是由距离传感器实时获取的数据根据机器人位姿信息进行累计而形成的地图，它解决了距离传感器检测范围小、分辨率低的问题，从而能够完整地描述场景的表面形状。稠密地图主要被应用于三维重构以及语义地图的研究中，但是由于其数据量大的特性给实时移动机器人系统上的地图存储和处理带来了较大的挑战。
结合目前常用的传感器，如何构建一种高效的地图更新系统实现稠密地图的实时存储和计算，如何将稠密地图运用于常用的路径导航，都是目前稠密建图中需要解决的的难题和挑战。
发明内容
为了克服现有技术的不足，本发明的目的在于提供一种适用于腿足机器人规划的稠密高度地图构建方法，对于实现腿足移动机器人的长期高效稳定作业有着重要意义。
本发明是通过以下技术方案来实现的：一种稠密高度地图构建方法，包括以下步骤：
步骤一：使用激光雷达以10-20Hz的频率获取周边环境的点云信息，所述点云信息映射到局部高度图中，在局部高度图中，高度测量值为高斯概率分布，其近似为
Figure PCTCN2020093236-appb-000001
Figure PCTCN2020093236-appb-000002
其中，p为测量的高度值，δ p 2为方差。在激光雷达坐标系S下，获取激光雷达到地形的单个测量值
Figure PCTCN2020093236-appb-000003
将其转换为相应的高度值p，具体为：
Figure PCTCN2020093236-appb-000004
其中，
Figure PCTCN2020093236-appb-000005
表示激光雷达坐标系S转化至全局地图坐标系M的旋转矩阵的逆；
Figure PCTCN2020093236-appb-000006
为在全局地图坐标系M下SM的距离信息；SM为S点到M点的距离信息；P为投影矩阵，取值为[0 0 1]。
通过高度值p、单个测量值
Figure PCTCN2020093236-appb-000007
Φ SM获得激光雷达测量的距离雅可比矩阵J s和旋转雅可比矩阵J Φ，获取所述高度值p的方差δ p 2。
Figure PCTCN2020093236-appb-000008
Figure PCTCN2020093236-appb-000009
Figure PCTCN2020093236-appb-000010
其中，∑ s代表了为激光雷达的协方差矩阵，∑ ΦIS代表了为激光雷达关于角度的协方差矩阵。
最后通过卡尔曼滤波，将激光雷达最新获取的高度值p以及所述高度值p的方差(p,δ p 2)和局部高度地图对应栅格的估计
Figure PCTCN2020093236-appb-000011
进行融合，更新获取最新的局部地图高度
Figure PCTCN2020093236-appb-000012
和局部地图方差δ h 2+。
Figure PCTCN2020093236-appb-000013
Figure PCTCN2020093236-appb-000014
对激光雷达的所有测量值进行上述操作，使所有测量值映射到局部地图对应栅格进行融合，从而构建完整的局部地图。
进一步地，所述局部地图方差δ h 2+组成栅格协方差，具体方法为：
所述局部高度图的每个栅格设置k时刻的协方差矩阵
Figure PCTCN2020093236-appb-000015
Figure PCTCN2020093236-appb-000016
其中，δ x,min 2和δ y,min 2分别表示局部高度图水平面x，y方向不确定性，
Figure PCTCN2020093236-appb-000017
表示该栅格的高度不确定性。
从时刻k至时刻k+1，机器人从B 1移动至B 2，所述局部高度图的协方差矩阵从
Figure PCTCN2020093236-appb-000018
变为
Figure PCTCN2020093236-appb-000019
得到因机器人移动转移而更新的协方差矩阵：
Figure PCTCN2020093236-appb-000020
其中，∑ r表示机器人B 1移动至B 2平移上的不确定度，∑ Φ表示B 1移动至B 2旋转变化上的不确定度。
进一步地，所述局部地图构建方法还包括：当局部地图移动后，通过机器人的定位估计系统，得到机器人与上一时刻X方向上的位移X Shift和Y方向上的位移Y Shift，设定局部地图分辨率为s，并根据机器人的移动信息更新位置偏移值X StartIndex和Y StartIndex，获得
局部地图在栅格上的相对位移为X index_shift和Y index_shift：
X startIndex＝(X startIndex-round(X Shift/s)+L x)％L x(9)
Y startIndex＝(Y startIndex-round(Y Shift/s)+L y)％L y(10)
其中，round()是对小数进行四舍五入获取整数的函数操作,％为求余操作，L x和L y分别表示栅格地图的X和Y方向的栅格数量。当需要查询地图(x,y)栅格信息时，通过下列映射查询(x search，y search)存储位置的数据内容。
x search＝(L x-X startIndex+x)％L x(11)
y search＝(L y-Y startIndex+y)％L y(12)
进一步地，所述局部地图构建方法还包括：
局部地图构建下的点云信息更新直接添加到局部地图中，对于激光雷达的每一个测量值，该测量值到激光雷达在地面投影点的线段上的所有点的地形高度都有高度限制h limitation：
Figure PCTCN2020093236-appb-000021
其中，h R_P为激光雷达到观测点的高度差，L R_P为激光雷达到观测点的水平距离，L C_P为待求高度限制的栅格到观测点的水平距离。若上一时刻局部高度图的高度数据超过相 应的高度限制，则代表物体发生移动，相应点的高度信息需要被清空。
相比现有技术，本发明的有益效果在于：本发明的方法使用GPU和多线程并行计算，让整个建图过程可以实时运行。构建了一种2.5D的带有可行域信息的稠密地图，便于腿足机器人的规划。只对判断为障碍物的栅格进行光线追踪，而不是对地图所有区域进行光线追踪，以判断该区域是不是移动障碍物的残影。使用这种策略构建，动态障碍物可以有效地被处理。本发明克服了现有稠密地图数据量大，构建计算量大，更新不及时的不足，并且具有构建方便，构建精度高等优点，并且可以完成实时的构建，可以直接用于腿足机器人的导航任务。
附图说明
图1为本发明稠密高度地图构建方法流程示意图。
具体实施方法
下面，结合附图以及具体实施方式，对本发明的技术方案做进一步地描述：
如图1，本发明提供了一种稠密地图构建方法，包括以下步骤：
步骤一：使用激光雷达以10-20Hz的频率获取周边环境的点云信息，所述点云信息映射到局部高度图中，在局部高度图中，高度测量值为高斯概率分布，其近似为
Figure PCTCN2020093236-appb-000022
Figure PCTCN2020093236-appb-000023
其中，p为测量的高度值，δ p 2为方差。在激光雷达坐标系S下，获取激光雷达到地形的单个测量值
Figure PCTCN2020093236-appb-000024
将其转换为相应的高度值p，具体为：
Figure PCTCN2020093236-appb-000025
其中，
Figure PCTCN2020093236-appb-000026
表示激光雷达坐标系S转化至全局地图坐标系M的旋转矩阵的逆；
Figure PCTCN2020093236-appb-000027
为在全局地图坐标系M下SM的距离信息；SM为S点到M点的距离信息；P为投影矩阵，取值为[0 0 1]。
通过高度值p、单个测量值
Figure PCTCN2020093236-appb-000028
Φ SM获得激光雷达测量的距离雅可比矩阵J s和旋转雅可比矩阵J Φ，获取所述高度值p的方差δ p 2。
Figure PCTCN2020093236-appb-000029
Figure PCTCN2020093236-appb-000030
Figure PCTCN2020093236-appb-000031
其中，∑ s代表了为激光雷达的协方差矩阵，∑ ΦIS代表了为激光雷达关于角度的协方差矩阵。
最后通过卡尔曼滤波，将激光雷达最新获取的高度值p以及所述高度值p的方差(p,δ p 2)和局部高度地图对应栅格的估计
Figure PCTCN2020093236-appb-000032
进行融合，更新获取最新的局部地图高度
Figure PCTCN2020093236-appb-000033
和局部地图方差δ h 2+。
Figure PCTCN2020093236-appb-000034
Figure PCTCN2020093236-appb-000035
对激光雷达的所有测量值进行上述操作，使所有测量值映射到局部地图对应栅格进行融合，从而构建完整的局部地图。
所述局部地图方差δ h 2+组成栅格协方差，在更新地图每个栅格协方差的方法中，由于基于M坐标系的局部高度图是相对于传感器/机器人的参考系定义的，因此每当机器人相对于惯性坐标系I运动时，需要根据姿态估计的变化来更新局部地图信息，包括平均高度h和方差值δ p 2。地图上每个栅格的方差和平均值会根据运动的不确定性以及周围方格的估计值进行更新。
所述局部高度图的每个栅格设置K时刻的协方差矩阵
Figure PCTCN2020093236-appb-000036
Figure PCTCN2020093236-appb-000037
其中，δ x,min 2和δ y,min 2分别表示局部高度图水平面x，y方向不确定性，
Figure PCTCN2020093236-appb-000038
表示该栅格的高度不确定性。
从时刻k至时刻k+1，机器人从B 1移动至B 2，所述局部高度图的协方差矩阵从
Figure PCTCN2020093236-appb-000039
变为
Figure PCTCN2020093236-appb-000040
得到因机器人移动转移而更新的协方差矩阵：
Figure PCTCN2020093236-appb-000041
其中，∑ r表示机器人B 1移动至B 2平移上的不确定度，∑ Φ表示B 1移动至B 2旋转变化上的不确定度。
在完成稠密地图更新后，固定尺寸和像素的稠密栅格地图会伴随着机器人位置的移动而移动。为简化计算量，本方法构建的稠密地图只有沿x和y方向两种方向的平移运动。在这个过程中，小部分稠密图边缘的数据因为可视化区域的变化而被删除，大部分地图区域仅仅更改存储数据的坐标位置但仍保留数据内容。因此，当地图移动时的通常 做法是将地图保留数据的存储位置根据机器人的位置变化而变化，但这对大尺寸的栅格地图来说是巨大的计算量。每次地图移动后，地图保留数据之间的相互位置关系从未改变。基于此发现，地图移动后，我们不会改变地图数据的存储位置，而是根据机器人的移动信息来更新位置偏移值X StartIndex和Y StartIndex。当需要查询地图不同位置信息时，我们可以使用这两个值来查询所有栅格的数据内容，具体为：
当局部地图移动后，通过机器人的定位估计系统，得到机器人与上一时刻X方向上的位移X Shift和Y方向上的位移Y Shift，设定局部地图分辨率为s，并根据机器人的移动信息更新位置偏移值X StartIndex和Y StartIndex，获得
局部地图在栅格上的相对位移为X index_shift和Y index_shift：
X startIndex＝(X startIndex-round(X Shift/s)+L x)％L x(9)
Y startIndex＝(Y startIndex-round(Y Shift/s)+L y)％L y(10)
其中，round()是对小数进行四舍五入获取整数的函数操作,％为求余操作，L x和L y分别表示栅格地图的X和Y方向的栅格数量。当需要查询地图(x,y)栅格信息时，通过下列映射查询(x search，y search)存储位置的数据内容。
x search＝(L x-X startIndex+x)％L x(11)
y search＝(L y-Y startIndex+y)％L y(12)
局部地图构建下的点云信息更新直接添加到局部地图中，而对于新观测中消失的点无法轻易判断出是否是因为物体移动而导致的。因此应对因物体移动需消去原位置点云的问题，主要通过光线追踪的方法来处理，其原理是当前时刻中若地面上的某点可以和激光雷达形成光路，就默认了该点到激光雷达在地面投影点的线段上的每一个点的地形高度都不会阻挡该条光路，即这条线段上的所有点都会有最大的高度限制，即对于激光雷达的每一个测量值，该测量值到激光雷达在地面投影点的线段上的所有点的地形高度都有高度限制h limitation：
Figure PCTCN2020093236-appb-000042
其中，h R_P为激光雷达到观测点的高度差，L R_P为激光雷达到观测点的水平距离，L C_P为待求高度限制的栅格到观测点的水平距离。若上一时刻局部高度图的高度数据超过相应的高度限制，则代表物体发生移动，相应点的高度信息需要被清空。因此应用光线跟踪原理，并根据传感器实时的点云数据，每条光路上栅格的高度限制可以被计算，以实现稠密高度地图上的移动物体移除。
对比本发明方法构建的稠密高度地图，地图每个区域的可行性通过几何结构特征来评估。对于地图的每个栅格，提取其表面法向量(代表局部面的斜率和曲率)和其附近栅格的高度差(代表局部面的粗糙程度)作为局部区域的几何特征。
为了获得栅格P i处的表面法向量n i，一个经典的方法是以P i为原点，结合临近栅格的坐标和高度信息，拟合一个平面S i＝n ixx+n iyy+n izz。当拟合的平面与所有三维点的距离和最小时，即可求得法向量n i，具体公式如下：
Figure PCTCN2020093236-appb-000043
其中k代表临近区域的栅格数量，Q i是一个3*K的带有临近K个点的三维坐标信息的矩阵，P i是待评估栅格二维坐标和高度信息组合的三维向量，n i＝[n ix,n iy,n iz]是需要求解的法向量。
栅格的高度差可以建立一个以P (x,y)为中心,大小为N*N的栅格窗口进行计算，这个窗口中栅格的平均高度为
Figure PCTCN2020093236-appb-000044
栅格P (x,y)的高度为
Figure PCTCN2020093236-appb-000045
则高度差为：
Figure PCTCN2020093236-appb-000046
将单位法向量转换成相应栅格的坡度信息并结合高度差，使用简单而有效的评估公式：
Figure PCTCN2020093236-appb-000047
获取每个栅格可通行的分数，其中(x,y)表示栅格所在位置。v slope(x,y)和v rough(x,y)为倾斜度和高度差，w s和w r为对应的权重，
Figure PCTCN2020093236-appb-000048
和
Figure PCTCN2020093236-appb-000049
表示该栅格可通行时的对应特征阈值。若最终计算得到可行域分数高于设定可行阈值，则表示该栅格可通行；若该分数低于阈值，表示该部分有障碍物或该地形不适宜通行。

Claims (4)

  1. 一种稠密高度地图构建方法，其特征在于，包括以下步骤：
    步骤一：使用激光雷达以10-20Hz的频率获取周边环境的点云信息，所述点云信息映射到局部高度图中，在局部高度图中，高度测量值为高斯概率分布，其近似为
    Figure PCTCN2020093236-appb-100001
    Figure PCTCN2020093236-appb-100002
    其中，p为测量的高度值，δ p 2为方差。在激光雷达坐标系S下，获取激光雷达到地形的单个测量值
    Figure PCTCN2020093236-appb-100003
    将其转换为相应的高度值p，具体为：
    Figure PCTCN2020093236-appb-100004
    其中，
    Figure PCTCN2020093236-appb-100005
    表示激光雷达坐标系S转化至全局地图坐标系M的旋转矩阵的逆；
    Figure PCTCN2020093236-appb-100006
    为在全局地图坐标系M下SM的距离信息；SM为S点到M点的距离信息；P为投影矩阵，取值为[0 0 1]。
    通过高度值p、单个测量值
    Figure PCTCN2020093236-appb-100007
    Φ SM获得激光雷达测量的距离雅可比矩阵J s和旋转雅可比矩阵J Φ，获取所述高度值p的方差δ p 2。
    Figure PCTCN2020093236-appb-100008
    Figure PCTCN2020093236-appb-100009
    Figure PCTCN2020093236-appb-100010
    其中，∑ s代表了为激光雷达的协方差矩阵，∑ ΦIS代表了为激光雷达关于角度的协方差矩阵。
    最后通过卡尔曼滤波，将激光雷达最新获取的高度值p以及所述高度值p的方差(p,δ p 2)和局部高度地图对应栅格的估计
    Figure PCTCN2020093236-appb-100011
    进行融合，更新获取最新的局部地图高度
    Figure PCTCN2020093236-appb-100012
    和局部地图方差δ h 2+。
    Figure PCTCN2020093236-appb-100013
    Figure PCTCN2020093236-appb-100014
    对激光雷达的所有测量值进行上述操作，使所有测量值映射到局部地图对应栅格进行融合，从而构建完整的局部地图。
  2. 根据权利要求1所述稠密高度地图构建方法，其特征在于，所述局部地图方差δ h 2+组成栅格协方差，具体方法为：
    所述局部高度图的每个栅格设置k时刻的协方差矩阵
    Figure PCTCN2020093236-appb-100015
    Figure PCTCN2020093236-appb-100016
    其中，δ x,min 2和δ y,min 2分别表示局部高度图水平面x，y方向不确定性，
    Figure PCTCN2020093236-appb-100017
    表示该栅格的高度不确定性。
    从时刻k至时刻k+1，机器人从B 1移动至B 2，所述局部高度图的协方差矩阵从
    Figure PCTCN2020093236-appb-100018
    变为
    Figure PCTCN2020093236-appb-100019
    得到因机器人移动转移而更新的协方差矩阵：
    Figure PCTCN2020093236-appb-100020
    其中，∑ r表示机器人B 1移动至B 2平移上的不确定度，∑ Φ表示B 1移动至B 2旋转变化上的不确定度。
  3. 根据权利要求1所述稠密高度地图构建方法，其特征在于，所述局部地图构建方法还包括：当局部地图移动后，通过机器人的定位估计系统，得到机器人与上一时刻X方向上的位移X Shift和Y方向上的位移Y Shift，设定局部地图分辨率为s，并根据机器人的移动信息更新位置偏移值X StartIndex和Y StartIndex，获得
    局部地图在栅格上的相对位移为X index_shift和Y index_shift：
    X startIndex＝(X startIndex-round(X Shift/s)+L x)％L x(9)
    Y startIndex＝(Y startIndex-round(Y Shift/s)+L y)％L y(10)
    其中，round()是对小数进行四舍五入获取整数的函数操作,％为求余操作，L x和L y分别表示栅格地图的X和Y方向的栅格数量。当需要查询地图(x,y)栅格信息时，通过下列映射查询(x search，y search)存储位置的数据内容。
    x search＝(L x-X startIndex+x)％L x(11)
    y search＝(L y-Y startIndex+y)％L y(12)
  4. 根据权利要求1所述稠密高度地图构建方法，其特征在于，所述局部地图构建方法还包括：
    局部地图构建下的点云信息更新直接添加到局部地图中，对于激光雷达的每一个测量值，该测量值到激光雷达在地面投影点的线段上的所有点的地形高度都有高度限制h limitation：
    Figure PCTCN2020093236-appb-100021
    其中，h R_P为激光雷达到观测点的高度差，L R_P为激光雷达到观测点的水平距离，L C_P为待求高度限制的栅格到观测点的水平距离。若上一时刻局部高度图的高度数据超过相应的高度限制，则代表物体发生移动，相应点的高度信息需要被清空。
PCT/CN2020/093236 2020-05-29 2020-05-29 一种适用于腿足机器人规划的稠密高度地图构建方法 Ceased WO2021237667A1 (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
PCT/CN2020/093236 WO2021237667A1 (zh) 2020-05-29 2020-05-29 一种适用于腿足机器人规划的稠密高度地图构建方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
PCT/CN2020/093236 WO2021237667A1 (zh) 2020-05-29 2020-05-29 一种适用于腿足机器人规划的稠密高度地图构建方法

Publications (1)

Publication Number Publication Date
WO2021237667A1 true WO2021237667A1 (zh) 2021-12-02

Family

ID=78745466

Family Applications (1)

Application Number Title Priority Date Filing Date
PCT/CN2020/093236 Ceased WO2021237667A1 (zh) 2020-05-29 2020-05-29 一种适用于腿足机器人规划的稠密高度地图构建方法

Country Status (1)

Country Link
WO (1) WO2021237667A1 (zh)

Cited By (24)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114638934A (zh) * 2022-03-07 2022-06-17 南京航空航天大学 一种3D激光slam建图中动态障碍物的后处理方法
CN114791734A (zh) * 2022-04-29 2022-07-26 北京理工大学 一种基于语义信息串联的履带车辆的控制方法及系统
CN114993292A (zh) * 2022-06-20 2022-09-02 合肥井松智能科技股份有限公司 一种薄墙壁分割方法及基于其的薄墙壁误匹配优化方法
CN115060266A (zh) * 2022-05-27 2022-09-16 华南农业大学 一种用于农田建图的定位系统
CN115128657A (zh) * 2022-08-09 2022-09-30 中国人民解放军63653部队 一种大尺度空间内多自由度精确测量装置及测量方法
CN115143964A (zh) * 2022-07-05 2022-10-04 中国科学技术大学 一种基于2.5d代价地图的四足机器人自主导航方法
CN115290102A (zh) * 2022-08-03 2022-11-04 九识(苏州)智能科技有限公司 一种基于多元统计信息的大范围场景全局定位方法
CN115388893A (zh) * 2022-08-25 2022-11-25 西安电子科技大学芜湖研究院 基于遗传的滤波slam算法的移动机器人建图方法
CN115601519A (zh) * 2022-11-07 2023-01-13 南京理工大学（Cn） 面向低通信带宽下远程操控的操控端建图方法
CN115619953A (zh) * 2022-10-31 2023-01-17 山东大学 一种面向崎岖地形的移动机器人地形建图方法及系统
CN115619980A (zh) * 2022-11-23 2023-01-17 青岛小优智能科技有限公司 一种基于点云的带束层搭接瑕疵检测方法
CN115717897A (zh) * 2022-11-22 2023-02-28 广州高新兴机器人有限公司 一种点云地图动态物体滤除方法、装置、介质及机器人
CN115861481A (zh) * 2022-12-23 2023-03-28 哈尔滨工业大学 一种基于激光惯性实时动态对象去除的slam系统
CN116202509A (zh) * 2023-03-17 2023-06-02 南京航空航天大学 一种面向室内多层建筑的可通行地图生成方法
CN116358517A (zh) * 2023-02-24 2023-06-30 杭州宇树科技有限公司 一种用于机器人的高度地图构建方法、系统以及存储介质
CN116448118A (zh) * 2023-04-17 2023-07-18 深圳市华辰信科电子有限公司 一种扫地机器人的工作路径优化方法和装置
CN116540206A (zh) * 2023-05-17 2023-08-04 成都理工大学 一种足式机器人高程估计方法、装置及系统
CN116698016A (zh) * 2023-08-04 2023-09-05 西安交通大学 复杂狭窄空间微型雷达阵列建图系统及方法
CN116878488A (zh) * 2023-09-07 2023-10-13 湘江实验室 一种建图方法、装置、存储介质及电子装置
CN116908860A (zh) * 2023-07-20 2023-10-20 深圳玑之智能科技有限公司 机器人定位更新方法、装置、电子设备及可读存储介质
CN117518195A (zh) * 2023-11-28 2024-02-06 湖南大学 基于图稀疏性维护的机器人长周期slam方法和系统
CN117689922A (zh) * 2022-09-05 2024-03-12 科沃斯机器人股份有限公司 行进环境的障碍物识别方法、设备及可读存储介质
CN121121678A (zh) * 2025-09-01 2025-12-12 北京理工大学 一种运动不确定性条件下的越野地形可通行性估计方法
CN121848412A (zh) * 2026-03-18 2026-04-14 呼伦贝尔职业技术学院 一种基于智能感知的化工应急机械手控制系统及方法

Citations (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20170263046A1 (en) * 2016-03-08 2017-09-14 Nvidia Corporation Perceptually-based foveated rendering using a contrast-enhancing filter
CN110221616A (zh) * 2019-06-25 2019-09-10 清华大学苏州汽车研究院（吴江） 一种地图生成的方法、装置、设备及介质

Patent Citations (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20170263046A1 (en) * 2016-03-08 2017-09-14 Nvidia Corporation Perceptually-based foveated rendering using a contrast-enhancing filter
CN110221616A (zh) * 2019-06-25 2019-09-10 清华大学苏州汽车研究院（吴江） 一种地图生成的方法、装置、设备及介质

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
FANKHAUSER PETER, BLOESCH MICHAEL, HUTTER MARCO: "Probabilistic Terrain Mapping for Mobile Robots With Uncertain Localization", IEEE ROBOTICS AND AUTOMATION LETTERS, vol. 3, no. 4, 1 October 2018 (2018-10-01), pages 3019 - 3026, XP055872229, DOI: 10.1109/LRA.2018.2849506 *
ZHOU, BO ET AL.: "Real-time 3D Outdoor Environment Modeling for Mobile Robot with a Laser Scanner", ROBOT, vol. 34, no. 3, 31 May 2012 (2012-05-31), pages 321 - 328,336, XP055872237, ISSN: 1002-0446 *

Cited By (34)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114638934A (zh) * 2022-03-07 2022-06-17 南京航空航天大学 一种3D激光slam建图中动态障碍物的后处理方法
CN114791734A (zh) * 2022-04-29 2022-07-26 北京理工大学 一种基于语义信息串联的履带车辆的控制方法及系统
CN115060266A (zh) * 2022-05-27 2022-09-16 华南农业大学 一种用于农田建图的定位系统
CN115060266B (zh) * 2022-05-27 2025-05-20 华南农业大学 一种用于农田建图的定位系统
CN114993292A (zh) * 2022-06-20 2022-09-02 合肥井松智能科技股份有限公司 一种薄墙壁分割方法及基于其的薄墙壁误匹配优化方法
CN114993292B (zh) * 2022-06-20 2024-04-30 合肥井松智能科技股份有限公司 一种薄墙壁分割方法及基于其的薄墙壁误匹配优化方法
CN115143964A (zh) * 2022-07-05 2022-10-04 中国科学技术大学 一种基于2.5d代价地图的四足机器人自主导航方法
CN115143964B (zh) * 2022-07-05 2024-05-10 中国科学技术大学 一种基于2.5d代价地图的四足机器人自主导航方法
CN115290102A (zh) * 2022-08-03 2022-11-04 九识(苏州)智能科技有限公司 一种基于多元统计信息的大范围场景全局定位方法
CN115128657A (zh) * 2022-08-09 2022-09-30 中国人民解放军63653部队 一种大尺度空间内多自由度精确测量装置及测量方法
CN115388893A (zh) * 2022-08-25 2022-11-25 西安电子科技大学芜湖研究院 基于遗传的滤波slam算法的移动机器人建图方法
CN115388893B (zh) * 2022-08-25 2024-05-14 西安电子科技大学芜湖研究院 基于遗传的滤波slam算法的移动机器人建图方法
CN117689922A (zh) * 2022-09-05 2024-03-12 科沃斯机器人股份有限公司 行进环境的障碍物识别方法、设备及可读存储介质
CN115619953A (zh) * 2022-10-31 2023-01-17 山东大学 一种面向崎岖地形的移动机器人地形建图方法及系统
CN115601519A (zh) * 2022-11-07 2023-01-13 南京理工大学（Cn） 面向低通信带宽下远程操控的操控端建图方法
CN115601519B (zh) * 2022-11-07 2024-04-05 南京理工大学 面向低通信带宽下远程操控的操控端建图方法
CN115717897A (zh) * 2022-11-22 2023-02-28 广州高新兴机器人有限公司 一种点云地图动态物体滤除方法、装置、介质及机器人
CN115619980A (zh) * 2022-11-23 2023-01-17 青岛小优智能科技有限公司 一种基于点云的带束层搭接瑕疵检测方法
CN115861481A (zh) * 2022-12-23 2023-03-28 哈尔滨工业大学 一种基于激光惯性实时动态对象去除的slam系统
CN116358517B (zh) * 2023-02-24 2024-02-23 杭州宇树科技有限公司 一种用于机器人的高度地图构建方法、系统以及存储介质
WO2024174440A1 (zh) * 2023-02-24 2024-08-29 杭州宇树科技有限公司 一种用于机器人的高度地图构建方法、系统以及存储介质
CN116358517A (zh) * 2023-02-24 2023-06-30 杭州宇树科技有限公司 一种用于机器人的高度地图构建方法、系统以及存储介质
CN116202509A (zh) * 2023-03-17 2023-06-02 南京航空航天大学 一种面向室内多层建筑的可通行地图生成方法
CN116448118B (zh) * 2023-04-17 2023-10-31 深圳市华辰信科电子有限公司 一种扫地机器人的工作路径优化方法和装置
CN116448118A (zh) * 2023-04-17 2023-07-18 深圳市华辰信科电子有限公司 一种扫地机器人的工作路径优化方法和装置
CN116540206A (zh) * 2023-05-17 2023-08-04 成都理工大学 一种足式机器人高程估计方法、装置及系统
CN116908860A (zh) * 2023-07-20 2023-10-20 深圳玑之智能科技有限公司 机器人定位更新方法、装置、电子设备及可读存储介质
CN116698016A (zh) * 2023-08-04 2023-09-05 西安交通大学 复杂狭窄空间微型雷达阵列建图系统及方法
CN116698016B (zh) * 2023-08-04 2023-10-20 西安交通大学 复杂狭窄空间微型雷达阵列建图系统及方法
CN116878488A (zh) * 2023-09-07 2023-10-13 湘江实验室 一种建图方法、装置、存储介质及电子装置
CN116878488B (zh) * 2023-09-07 2023-11-28 湘江实验室 一种建图方法、装置、存储介质及电子装置
CN117518195A (zh) * 2023-11-28 2024-02-06 湖南大学 基于图稀疏性维护的机器人长周期slam方法和系统
CN121121678A (zh) * 2025-09-01 2025-12-12 北京理工大学 一种运动不确定性条件下的越野地形可通行性估计方法
CN121848412A (zh) * 2026-03-18 2026-04-14 呼伦贝尔职业技术学院 一种基于智能感知的化工应急机械手控制系统及方法

Similar Documents

Publication Publication Date Title
CN111596665B (zh) 一种适用于腿足机器人规划的稠密高度地图构建方法
Miki et al. Elevation mapping for locomotion and navigation using gpu
CN115639823B (zh) 崎岖起伏地形下机器人地形感知与移动控制方法及系统
US12198364B2 (en) Computer vision systems and methods for detecting and modeling features of structures in images
CN114659513B (zh) 一种面向非结构化道路的点云地图构建与维护方法
CN103984037B (zh) 基于视觉的移动机器人障碍物检测方法和装置
CN112859859A (zh) 一种基于三维障碍物体素对象映射的动态栅格地图更新方法
CN115496792A (zh) 基于人在环优化的点云语义slam的方法、系统及装置
CN110084272A (zh) 一种聚类地图创建方法及基于聚类地图和位置描述子匹配的重定位方法
CN106949893A (zh) 一种三维避障的室内机器人导航方法和系统
CN113515128B (zh) 一种无人车实时路径规划方法及存储介质
CN107036594A (zh) 智能电站巡检智能体的定位与多粒度环境感知技术
CN113885510B (zh) 一种四足机器人避障及领航员跟随方法及系统
CN111721279A (zh) 一种适用于输电巡检工作的末端路径导航方法
CN112907625B (zh) 应用于四足仿生机器人的目标跟随方法及系统
Liu et al. Real-time 6d lidar slam in large scale natural terrains for ugv
CN111862214B (zh) 计算机设备定位方法、装置、计算机设备和存储介质
CN113282088A (zh) 工程车的无人驾驶方法、装置、设备、存储介质及工程车
CN116660916B (zh) 一种用于果园移动机器人的定位方法、建图方法及电子设备
CN119245629B (zh) 一种面向腿足机器人的可通行地图构建方法
Pang et al. Low-cost and high-accuracy LiDAR SLAM for large outdoor scenarios
CN110515382A (zh) 一种智能设备及其定位方法
Pauls et al. Automatic mapping of tailored landmark representations for automated driving and map learning
CN116358580A (zh) 一种基于履带车的路径规划方法及系统
Fan et al. A fire protection robot system based on SLAM localization and fire source identification

Legal Events

Date Code Title Description
121 Ep: the epo has been informed by wipo that ep was designated in this application

Ref document number: 20937503

Country of ref document: EP

Kind code of ref document: A1

NENP Non-entry into the national phase

Ref country code: DE

122 Ep: pct application non-entry in european phase

Ref document number: 20937503

Country of ref document: EP

Kind code of ref document: A1