WO2021114764A1 - 基于局部地图的地图修正方法及*** - Google Patents

基于局部地图的地图修正方法及*** Download PDF

Info

Publication number
WO2021114764A1
WO2021114764A1 PCT/CN2020/113932 CN2020113932W WO2021114764A1 WO 2021114764 A1 WO2021114764 A1 WO 2021114764A1 CN 2020113932 W CN2020113932 W CN 2020113932W WO 2021114764 A1 WO2021114764 A1 WO 2021114764A1
Authority
WO
WIPO (PCT)
Prior art keywords
map
pose
robot
local
local map
Prior art date
Application number
PCT/CN2020/113932
Other languages
English (en)
French (fr)
Inventor
林欢
程敏
许春山
毛成林
王�锋
Original Assignee
亿嘉和科技股份有限公司
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 亿嘉和科技股份有限公司 filed Critical 亿嘉和科技股份有限公司
Publication of WO2021114764A1 publication Critical patent/WO2021114764A1/zh

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C25/00—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass

Definitions

  • This application relates to the field of inspection robots, for example, to a map correction method and system based on a local map.
  • inspection robots have become an important aid for equipment inspections in special engineering sites such as substations. Inspection robots can use sensor devices to perceive the surrounding environment to realize the construction of high-precision environmental maps and implement them based on the map. The robot repeats autonomous navigation.
  • 3D lasers are more widely used in robot construction of maps and autonomous positioning and navigation due to the advantages of rich point cloud information and more stable positioning than 2D lasers.
  • the mainstream 3D laser SLAM (real-time positioning and mapping) technology at home and abroad has realized the function of robots in an ideal scene environment to construct maps based on data collected by laser sensors and to locate autonomously based on the maps.
  • the maps constructed by the above technologies are static, but the actual environment may undergo dynamic changes. For example, the seasonal changes of plant growth and withering in the environment, and man-made scene transformations will cause the map and the actual environment to be larger. Does not match.
  • the robot travels in the changed scene area according to the previous map, the positioning accuracy of the robot will be affected, unstable and uncontrollable, and even positioning loss will occur, causing damage to the robot or consuming more manpower to assist the robot. ,Time-consuming.
  • This application provides a map correction method and system based on a local map, which enhances the stability and safety of the robot's automatic positioning and navigation, greatly reduces the risk of the robot falling out of the road, and helps to realize the robot's self-rescue and sustainable execution task.
  • a map correction method based on a local map includes the following steps:
  • the robot collects laser data
  • the use of the local map to modify the original Global map includes:
  • the local map and the original Global map are superimposed to complete the correction of the original Global map.
  • the calculation of the predicted pose and the confidence score of the predicted pose includes:
  • the robot pose corresponding to the first two frames of laser data recorded by the odometer and the inertial navigation unit is used to calculate the robot displacement speed and rotation speed, and the displacement speed, rotation speed and the previous frame pose are used to calculate the initial pose of the next frame.
  • the value is the initial value of the predicted pose
  • the judging whether the positioning is lost includes:
  • the generating a local map includes:
  • the pose of the computing robot in the local map includes:
  • the calculation of the matching relationship between the local map and the original Global map according to the pose of the robot in the original Global map and the pose of the robot in the local map includes:
  • x, y are the abscissa and ordinate of the robot in the original Global map
  • ⁇ is the posture of the robot in the original Global map
  • x′, y′ are the abscissa and ordinate of the robot in the local map
  • ⁇ ′ Is the posture of the robot in the local map
  • the superimposing the local map with the original Global map according to the matching relationship and completing the correction of the original Global map includes:
  • update the confidence of each superimposed grid to complete the correction of the original Global map including:
  • (x 'i, y' i) is the i-th partial map a grid map coordinates
  • P (x 'i, y ' i) is a partial map map coordinates (x 'i, y' i ) of the i-th confidence grid
  • (x 'i, y' i ) is the map coordinates of the i-th grid in the original Global map
  • P(x i , y i ) is the confidence level of the i-th grid in the original Global map whose map coordinates are (x i , y i );
  • odd hit represents the state update coefficient of the grid when the pavement point fell last time and this time
  • odd miss represents the state update coefficient of the grid when the pavement point fell last time and did not fall
  • the value of s and t The value is adjusted according to the sparseness of the ground environment.
  • a map correction system based on a local map including a laser acquisition module, a map generation module, a map correction module, a map scheduling module, and a map database.
  • the laser acquisition module collects laser data and transmits the laser data to the map generation module to generate a local map.
  • the generation module stores the local map generated by it in the map database; when the positioning of the laser acquisition module is lost, the map scheduling module retrieves the latest local map and the original global map from the map database and transmits them to the map correction module, and the map correction module will The latest local map is integrated with the original global map to complete the correction of the original global map.
  • the laser acquisition module is a robot equipped with an odometer, an inertial navigation unit and a laser sensor.
  • the map correction method and system based on local maps provided in this application can realize:
  • the robot can still locate and navigate accurately, which enhances the adaptability of the robot to the environment.
  • Figure 1 is a schematic diagram of a map correction system based on a local map provided by this application;
  • Fig. 2 is a flowchart of a map correction method based on a local map provided by this application;
  • FIG. 3 is a schematic diagram of the scanning window provided by this application.
  • Figure 4 is a schematic diagram of the selection of laser points provided by this application.
  • a map correction system based on a local map includes a laser acquisition module, a map generation module, a map correction module, a map scheduling module, and a map database.
  • the laser acquisition module collects laser data and transmits the laser data to the map generation module. Generate a local map, and the map generation module stores the generated local map in the map database; when the location of the laser acquisition module is lost, the map scheduling module retrieves the latest local map and the original global map from the map database and transmits them to the map correction module ,
  • the map correction module integrates the latest local map with the original global map to complete the correction of the original global map.
  • the laser acquisition module is a robot equipped with an odometer, an inertial navigation unit and a laser sensor.
  • the robot is equipped with an odometer, an inertial navigation unit and a laser sensor to perform tasks under the guidance of the original global map.
  • the laser sensor continuously collects laser data.
  • the generated laser data is used for positioning and a local map can be generated through the map generation module.
  • the generated local map is stored in the map database.
  • the map scheduling module is activated to schedule the newly generated local map and the original global map.
  • the map correction module merges the newly generated local map with the original global map.
  • the original global map was revised.
  • the robot performs positioning and navigation in the original Global map:
  • the robot Before the robot performs the inspection task, it collects the laser data information of the entire job and builds a laser grid map; after that, the robot uses the odometer and the inertial navigation unit to record the robot pose corresponding to the first two frames of laser data to calculate the robot displacement speed and Rotation speed, and predict the initial value of the pose of the next frame through the pose, displacement speed and rotation speed of the previous frame;
  • the time interval between time points t and t-1 is ⁇ t.
  • V(v x , v y , v ⁇ ), v x , v y are linear velocities, and v ⁇ is the angular velocity.
  • the estimation formula is as follows:
  • v x , v y , v ⁇ to predict the robot's pose p(x t+1 , y t+1 , ⁇ t+1 ) at the time point t+1, which is the initial value of the predicted pose, because the laser sensor is Uniform sampling, the time interval between time points t and t+1 is ⁇ t.
  • the robot takes the initial value of the predicted pose as the center, determines different scan angles based on the scan parameters, and uses the pose of each scan angle as all the candidate poses for the initial value of the predicted pose;
  • the scan parameters include displacement scan parameters and angle scan parameters
  • the displacement scan parameter is used to limit the displacement range of the robot during positioning scan, that is, in the map coordinate system, the initial value of the predicted pose is centered, and the front, back, left, and right are deviated by Lcm to form a square with a side length of 2Lcm.
  • the side of the square is parallel or perpendicular to a coordinate axis of the map coordinate system, and the side of the square is the displacement scanning parameter.
  • the angle scan parameter is used to limit the angle range when the robot performs positioning scan, that is, in the map coordinate system, the initial value of the predicted angle ⁇ t of the initial value of the predicted pose is taken as the central angle, and the left and right angles deviate by W degrees.
  • the poses of the robot under different scanning angles constitute all the candidate poses of the predicted poses (x t , y t , ⁇ t ).
  • the positioning scan constituting the candidate pose is a virtual positioning scan, which is a simulation of an actual positioning scan, and does not require actual movement of the robot.
  • the confidence value of the map grid is related to the map construction process, during the positioning process, it is the determined value
  • calculate the confidence of each candidate pose ⁇ the formula is as follows:
  • m is the total number of map grids in the discrete scan data of the scan angle corresponding to a certain candidate pose.
  • x ⁇ is the displacement along the x-axis between each candidate pose and the estimated value of the pose
  • y ⁇ is the displacement along the y-axis between each candidate pose and the estimated value of the pose
  • ⁇ xy is the displacement weight
  • ⁇ r is the rotation angle between the candidate pose and the predicted pose
  • ⁇ r is the rotation angle weight.
  • ⁇ xy and ⁇ r take 1, which means that the displacement and the rotation angle have the same weight.
  • the confidence score corresponding to the predicted pose is score max .
  • the robot moves forward with a certain sliding step through a fixed-length window, and the collected laser data can be divided into segments of continuous data frames, and tile-like data frames are used for each continuous data frame.
  • the mapping method generates a local map, and the specific process is as follows:
  • the robot divides the collected laser data through a total window of M frames of laser data and a sliding window of n frames of laser data, where M ⁇ 400, n ⁇ (10, 20).
  • the tile-based mapping method is used, that is, the tail of the previous sub-image and the head of the next sub-image have overlapping parts (the overlapped part generally accounts for 0.3 to 0.5 in a single sub-image, and the value of the ratio is determined by the sparseness of the map.
  • the form of the sub-map is represented by a grid map.
  • the resolution of the sub-map map is the same as that of the original Global map.
  • a sub-image is generated for every 20 frames of data, that is, the laser data of the first to 20 frames are registered to generate sub-image A1, and the laser data of the 10th to 30th frames are registered.
  • the robot’s fixed position reliability is judged. After real-time laser data and the robot’s regional probability grid map in Global_map are matched in real time according to step 1, the highest matching confidence score score max (score max ⁇ [0, 1]), the higher the score max value, the more accurate the laser data positioning of the new frame.
  • the highest confidence score score max of real-time matching in the navigation and positioning process is greater than the manually set empirical threshold, the positioning has been relatively accurate, and the robot can continue to position and navigate; if it is continuous for several frames (in this example, it is set to 100 frames)
  • the highest confidence score score max of the laser data is lower than the manually set confidence threshold (in this embodiment, the confidence threshold is set to 0.4), it is determined that the positioning is lost, and at this time, the robot is triggered to stop moving and stand by And began to use the local map to repair the original Global map.
  • the local map and the original Global map are registered, and the method in step 1 is used to obtain the confidence scores of each candidate pose of the local predicted pose of the robot in the local map, and the K with the highest confidence score is selected.
  • x, y are the abscissa and ordinate of the robot in the Global map
  • ⁇ is the posture of the robot in the Global map
  • x′, y′ are the abscissa and ordinate of the robot in the local map
  • ⁇ ′ is the robot The posture in the local map
  • the local map and the original Global map are superimposed.
  • the map coordinates of the grids, P(x′ i ,y′ i ) is the confidence of the i-th grid in the local map whose map coordinates are (x′ i ,y′ i ), (x i ,y i ) Is the map coordinates of the i-th grid in the original Global map
  • P(x i , y i ) is the confidence level of the i-th grid in the original Global map whose map coordinates are (x i , y i );
  • odd hit represents the state update coefficient of the grid when the pavement point fell last time and this time
  • odd miss represents the state update coefficient of the grid when the pavement point fell last time and did not fall
  • the value of s and t The value is adjusted according to the sparseness of the ground environment.
  • This application enhances the stability and safety of the robot's automatic positioning and navigation, greatly reduces the risk of the robot falling out of the road, and helps the robot to realize self-rescue and continue to perform tasks.

Landscapes

  • Engineering & Computer Science (AREA)
  • Manufacturing & Machinery (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)

Abstract

一种基于局部地图的地图修正方法及***，地图修正方法包括以下步骤：机器人采集激光数据；利用采集的激光数据在原始Global地图中进行定位导航，包括：计算机器人在原始Global地图的预测位姿和预测位姿的置信度分值；生成局部地图；判断定位是否丢失，若定位没有丢失，则继续定位导航，若定位丢失，则利用生成的局部地图修正Global地图。

Description

基于局部地图的地图修正方法及***
本申请要求在2019年12月10日提交中国专利局、申请号为201911258908.1的中国专利申请的优先权，以上申请的全部内容通过引用结合在本申请中。
技术领域
本申请涉及巡检机器人领域，例如涉及一种基于局部地图的地图修正方法及***。
背景技术
近年来，随着科技的发展，巡检机器人已经成为诸如变电站等特殊工程场所设备巡检的重要辅助，巡检机器人可以利用传感器设备感知周边环境实现对高精度环境地图的构建并根据该地图实现机器人可重复的自主导航。随着技术的不断发展进步，3D激光较2D激光，由于具有点云信息丰富、定位更加稳定的优势，在机器人构建地图与自主定位导航方面得到更加广泛的应用。目前，国内外已有的主流3D激光SLAM(即时定位与建图)技术，已经实现了机器人在理想场景环境中基于激光传感器采集数据构建地图以及根据地图自主定位的功能。然而，上述技术构建的地图是静态的，但实际的环境可能会发生动态的改变，例如环境中随季节变化的植物生长与凋零、人为的场景改造等因素都会造成地图与实际环境存在较大的不吻合。在相关技术中，机器人依据之前的地图在发生变化的场景区域中行进，机器人定位精度会受到影响，不稳定且不可控，乃至发生定位丢失的情况，造成机器人的损坏或耗费较多人力辅助机器人，费时费力。
发明内容
本申请提供一种基于局部地图的地图修正方法及***，增强了机器人自动定位导航的稳定性、安全性，大大降低了机器人跌落到道路之外的风险，有助于实现机器人自救并可持续执行任务。
一种基于局部地图的地图修正方法，包括以下步骤：
机器人采集激光数据；
利用采集的激光数据在原始Global地图中进行定位导航，包括：
计算机器人在原始Global地图的预测位姿和预测位姿的置信度分值；
生成局部地图；
判断定位是否丢失，若定位没有丢失，则继续定位导航，若定位丢失，则利用生成的局部地图修正Global地图。
所述利用局部地图修正原始Global地图包括：
计算机器人在局部地图中的位姿；
根据机器人在原始Global地图中的位姿和机器人在局部地图中的位姿计算局部地图和原始Global地图的匹配关系；
根据匹配关系将局部地图与原始Global地图进行叠加，完成对原始Global地图的修正。
所述计算自身的预测位姿和预测位姿的置信度分值包括：
利用里程计和惯性导航单元记录的前两帧激光数据对应的机器人位姿，计算机器人位移速度和旋转速度，利用所述位移速度、旋转速度和前一帧位姿计算下一帧的位姿初值即预测位姿初值；
以预测位姿初值为中心，基于扫描参数确定各扫描角度，将各扫描角度的位姿作为预测位姿初值的所有的候选位姿；
计算各候选位姿的置信度和置信度分值，选择置信度分值最高的候选位姿作为机器人的预测位姿；
所述判断定位是否丢失包括：
若预测位姿的置信度分值大于设定的阈值，则表明定位未丢失；
若连续数帧的预测位姿的置信度分值小于设定的阈值，则表明定位丢失。
所述生成局部地图包括：
通过固定长度的滑动窗口以固定步长向前移动，将采集到的激光数据划分成一段段连续的数据帧；
根据接收的相邻的两帧激光数据时机器人的位姿计算机器人相邻位姿间的转换关系；
利用转换关系拼接相邻的两帧激光数据，从而将每一个滑动窗口内的连续数帧激光数据生成一张子图，上一张子图的尾部和下一张子图的头部有重叠 部分。
所述计算机器人在局部地图中的位姿包括：
运用计算预测位姿的各候选位姿的置信度分值的方法，求取机器人在局部地图中局部预测位姿的各候选位姿的置信度分值，选取置信度分值最高的K个位姿，对这K个位姿对应的各项求取平均值，得到机器人在局部地图中的位姿。
所述根据机器人在原始Global地图中的位姿和机器人在局部地图中的位姿计算局部地图和原始Global地图的匹配关系包括：
计算机器人在局部地图中的位姿与原始Global地图中位姿p 原(x，y，γ)的位姿差：
Figure PCTCN2020113932-appb-000001
其中，x，y为机器人在原始Global地图中的横坐标和纵坐标，γ为机器人在原始Global地图中的姿态；x′，y′为机器人在局部地图中的横坐标和纵坐标，γ′为机器人在局部地图中的姿态；
计算局部地图与原始Global地图的匹配关系R为：
Figure PCTCN2020113932-appb-000002
所述根据匹配关系将局部地图与原始Global地图进行叠加并完成对原始Global地图的修正包括：
根据匹配关系R将局部地图与原始Global地图进行叠加；
根据局部地图与原始Global地图的叠加情况，更新各个叠加栅格的置信度从而完成对原始Global地图进行修正，包括：
Figure PCTCN2020113932-appb-000003
其中，P(x i，y i)_new为修正后的Global地图中地图坐标为(x i，y i)的栅格的置信度，(x′ i，y′ i)为局部地图中第i个栅格的地图坐标，P(x′ i，y′ i)为局部地图中地图坐标为(x′ i，y′ i)的第i个栅格的置信度，(x′ i，y′ i)为原始Global地图中第i个栅格的地图坐标，P(x i，y i)为原始Global地图中地图坐标为(x i，y i)的第i个栅格的置信度；
Figure PCTCN2020113932-appb-000004
其中，odd hit表示上次和本次都有路面点落入时栅格的状态更新系数，odd miss表示路面点上次落入本次未落入时栅格的状态更新系数；s和t的取值根据地面环境的稀疏程度进行调节。
一种基于局部地图的地图修正***，包括激光采集模块、地图生成模块、地图修正模块、地图调度模块和地图数据库，激光采集模块采集激光数据，将激光数据传输给地图生成模块生成局部地图，地图生成模块将其生成的局部地图存储至地图数据库；当激光采集模块的定位丢失时，地图调度模块从地图数据库调取最新的局部地图与原始global地图将其传输给地图修正模块，地图修正模块将最新的局部地图与原始global地图进行融合，完成对原始global地图的修正。
所述激光采集模块为搭载里程计、惯性导航单元和激光传感器的机器人。
本申请提供的一种基于局部地图的地图修正方法及***可以实现：
1、当机器人的作业环境发生变化时，机器人依旧能够准确的定位导航，增强了机器人对环境的适应性。
2、增强了机器人自动定位导航的稳定性、安全性，大大降低了机器人跌落 到道路之外的风险，有助于实现机器人自救并可持续执行任务。
附图说明
图1为本申请提供的一种基于局部地图的地图修正***原理图；
图2为本申请提供的一种基于局部地图的地图修正方法的流程图；
图3为本申请提供的扫描窗口的示意图；
图4为本申请提供的激光点的选取示意图。
具体实施方式
下面结合附图，对一种基于局部地图的地图修正方法及***进行详细描述。
一种基于局部地图的地图修正***如图1所示，包括激光采集模块、地图生成模块、地图修正模块、地图调度模块和地图数据库，激光采集模块采集激光数据，将激光数据传输给地图生成模块生成局部地图，地图生成模块将其生成的局部地图存储至地图数据库；当激光采集模块的定位丢失时，地图调度模块从地图数据库调取最新的局部地图与原始global地图将其传输给地图修正模块，地图修正模块将最新的局部地图与原始global地图进行融合，完成对原始global地图的修正。本实施例中激光采集模块为搭载里程计、惯性导航单元和激光传感器的机器人。
其中机器人搭载里程计、惯性导航单元和激光传感器在原始global地图的指引下进行作业任务，激光传感器不断采集激光数据，生成的激光数据在用于定位的同时，通过地图生成模块可生成局部地图，生成的局部地图存储于地图数据库中，当机器人发生地位丢失时，启用地图调度模块，调度最新生成的局部地图与原始global地图，地图修正模块将最新生成的局部地图与原始global地图进行融合，对原始global地图进行修正。
结合图2，对一种基于局部地图的地图修正方法进行详细阐述，具体流程主要分为机器人在原始Global地图中的定位导航、局部地图构建、原始Global地图修正三部分，其中局部地图构建、原始Global地图修正是本申请的主要内容，整体流程如下：
机器人在原始Global地图中进行定位导航：
机器人在执行巡检任务之前，会采集整个作业的激光数据信息并构建激光 栅格地图；之后机器人通过里程计和惯性导航单元记录的前两帧激光数据对应的机器人位姿，计算机器人位移速度和旋转速度，并通过前一帧位姿、位移速度和旋转速度预测下一帧的位姿初值；
根据预测位姿初值和定位扫描参数确定各扫描角度，将地图栅格上各扫描角度的位姿作为所有可能的候选位姿；
计算各可能的候选位姿的置信度，选择置信度分值最高的位姿作为机器人的激光定位结果，具体流程如下：
1.估算机器人的预测位姿初值
利用里程计和惯性导航单元获得当前时刻机器人在地图坐标系下的位姿，假定机器人在时间点t的位姿是p(x t，y t，γ t)，x t表示时间点t时机器人在原始Global地图中的横坐标,y t表示时间点t时机器人在原始Global地图中的纵坐标，γ t为时间点t时机器人在原始Global地图中的姿态，前一个计算位姿的时间点为t-1，相应的位姿是p(x t-1，y t-1，γ t-1)，下一个计算位姿的时间点t+1，相应的位下一位姿是p(x t+1，y t+1，γ t+1)。时间点t和t-1之间的时间间隔为Δt，估算机器人的运动速度V(v x，v y，v γ)，v x，v y为线速度，v γ为角速度，估算公式如下：
Figure PCTCN2020113932-appb-000005
使用v x，v y，v γ来预测机器人在时间点t+1时刻的位姿p(x t+1，y t+1，γ t+1)即预测位姿初值，由于激光传感器是均匀采样，时间点t和t+1之间的时间间隔为Δt。
Figure PCTCN2020113932-appb-000006
由于实际硬件的误差，机器人在时间点t+1的准确位姿与该预测位姿初值之间会有偏差。下面通过激光测量数据和预测位姿初值对应地图数据之间符合的程度来优化p(x t+1，y t+1，γ t+1)，最终获得最接近准确位姿的最优位置。
2.获取不同扫描角度的离散扫描数据
机器人以预测位姿初值为中心，基于扫描参数确定不同的扫描角度，将各扫描角度的位姿作为预测位姿初值的所有的候选位姿；扫描参数包括位移扫描 参数和角度扫描参数，如图3所示，位移扫描参数用于限定机器人进行定位扫描时的位移范围，即在地图坐标系中，以预测位姿初值为中心，前后左右各偏离Lcm，形成边长为2Lcm的正方形，所述正方形的边和地图坐标系的一条坐标轴平行或垂直，所述正方形的边即为位移扫描参数。角度扫描参数用于限定机器人进行定位扫描时的角度范围，即在地图坐标系中，以预测位姿初值的预测角度初值γ t为中心角度，左右各偏离W度的角度。不同的扫描角度下的机器人的位姿构成预测位姿(x t，y t，γ t)的所有的候选位姿。所述构成候选位姿的定位扫描为虚拟定位扫描，是一个实际定位扫描的模拟，而不需要机器人实际的运动。
根据机器人的定位扫描得到的激光数据计算各扫描角度下各激光反射点对应的地图栅格位置(即计算每个地图栅格在地图坐标系中的坐标)，作为各扫描角度的离散扫描数据。对某个扫描角度的离散扫描数据而言，若其中有重复落在同一地图栅格位置的多个激光反射点，仅取其中一个激光反射点对应的地图栅格在地图坐标系中的坐标，如图4所示，灰色栅格为多个激光反射点落入同一地图栅格的情况，只取灰色栅格中的一个激光反射点的坐标，用于后续步骤的置信度的计算。
3.置信度估算
根据每个候选位姿对应的各个地图栅格的置信度(地图栅格的置信度值与地图构建过程相关，在定位过程中，为已经确定的值)，计算每个候选位姿的置信度σ，公式如下：
Figure PCTCN2020113932-appb-000007
Figure PCTCN2020113932-appb-000008
其中，m为某个候选位姿对应的扫描角度的离散扫描数据中的地图栅格的总数，设其中第n个栅格的地图坐标为(x n，y n)，该栅格置信度是
Figure PCTCN2020113932-appb-000009
取值范围为[0，1]。
根据每个候选位姿与位姿估计值的位姿差来计算每个候选位姿对应的置信度权重ω，公式如下：
Figure PCTCN2020113932-appb-000010
其中，x Δ是每个候选位姿与位姿估计值之间沿x轴的位移，y Δ是每个候选位姿与位姿估计值之间沿y轴的位移，ω xy是位移权重，Δr是候选位姿与预测位姿之间旋转角度，ω r是旋转角度权重，一般ω xy和ω r取1，表示位移和旋转角度所占权重相同。
将每个候选位姿的置信度σ与置信度权重ω的乘积作为当前位姿的置信度分值，公式如下，
score＝σ·ω
选择置信度分值score最高的位姿更新预测位姿初值，作为最终的预测位姿，即作为t+1时刻的位姿(x t+1，y t+1，γ t+1)，预测位姿对应的置信度分值为score max。
二、生成局部地图
机器人在定位的过程中，通过固定长度的窗口以一定的滑动步长向前移动，可将采集到的激光数据划分成一段段连续的数据帧，针对每段连续的数据帧运用瓦片式的建图方法生成局部地图，具体流程如下：
首先，机器人通过长度为M帧激光数据的总窗口，步长为n帧激光数据的滑动窗口，对采集到的激光数据进行划分，其中M≥400，n∈(10，20)。
进而，根据机器人接收相邻两帧激光数据时的位姿p k-1、p k，求解p k＝p k-1r k-1+t k-1得到机器人相邻位姿间的转换关系(r k-1，t k-1)，其中p k-1表示接收第k-1帧激光数据时机器人的位姿，p k表示接收第k帧激光数据时机器人的位姿，r k-1表示转换关系(r k-1，t k-1)中的旋转矩阵，t k-1表示转换关系(r k-1，t k-1)中的平移变量。
然后，用求得的转换关系拼接与转换关系相对应的相邻帧的激光数据，具体拼接公式为：Q k＝Q k-1r k-1+t k-1，其中Q k-1、Q k分别为第k-1帧激光数据和第k帧激光数据。
最后，采用瓦片式建图方法即上一张子图的尾部和下一张子图的头部有重叠部分(重叠部分在单张子图中的占比一般为0.3～0.5，占比的数值由地图的稀疏程度确定)，利用滑动窗口内最新的连续数帧激光数据生成子图，子图的形式由栅格地图形式表示，子图地图的分辨率与原始Global地图的分辨率相同。本实施例中针对的是最新的400帧数据，将每20帧数据生成一张子图，即将第 1帧到20帧激光数据进行配准生成子图A1，第10帧到30帧激光数据进行配准生成子图A2，第20帧到40帧激光数据进行配准生成子图A3，……，最终将生成39张子图。将生成的子图以中间文件的方式进行保存。当生成一张新的子图后，丢弃中间文件中最先生成的子图，加入新的子图。所有的子图构成局部地图
三、利用局部地图对原始Global地图进行修正
当机器人的作业环境发生变化时，原始Global地图的置信度将会降低，机器人将会发生定位丢失的情况，此时需要将中间文件中存储的子图与原始Global地图进行融合，具体流程如下：
首先，对机器人的定位置信度进行判断，实时的激光数据与Global_map中机器人的区域概率栅格地图按照步骤一进行实时匹配后，得到匹配的最高置信度分值score max(score max∈[0，1])，score max的值越高，说明新一帧的激光数据定位的越准确。若导航定位过程中实时匹配的最高置信度分值score max大于人工设定的经验阈值，则定位一直比较准确，机器人可以一直持续定位导航；若连续若干帧(本实例中设定为100帧)激光数据的最高置信度分值score max低于人工设定的置信度阈值(本实施例中将此置信度阈值设定为0.4)，则判定定位丢失，此时，触发机器人停止移动原地待命的机制，并开始利用局部地图对原始Global地图进行修复。
然后，对局部地图和原始Global地图进行配准，运用步骤一中的方法，求取机器人在局部地图中局部预测位姿的各候选位姿的置信度分值，选取置信度分值最高的K个位姿，p(x′ 1，y′ 1，γ′ 1)，p(x′ 2，y′ 2，γ′ 2)…p(x′ K，y′ K，γ′ K)，K∈(3，6)，对选取的K个位姿对应的各项求取平均值，得到机器人在局部地图中的位姿p(x′，y′，γ′)：
Figure PCTCN2020113932-appb-000011
根据机器人当前在原始Global地图中的位姿，求机器人在局部地图中的位姿与原始Global地图中位姿p 原(x，y，γ)的位姿差：
Figure PCTCN2020113932-appb-000012
其中，x，y为机器人在Global地图中的横坐标和纵坐标，γ为机器人在Global地图中的姿态；x′，y′为机器人在局部地图中的横坐标和纵坐标，γ′为机器人在局部地图中的姿态；
求取局部地图与原始Global地图的匹配关系R为：
Figure PCTCN2020113932-appb-000013
根据匹配关系R将局部地图与原始Global地图进行叠加。
最后，根据局部地图与原始Global地图的叠加情况，更新各个叠加栅格的置信度，对原始Global地图进行修正：
Figure PCTCN2020113932-appb-000014
其中，P(x i，y i)_new为修正后的Global地图中地图坐标为(x i，y i)的栅格的置信度，(x′ i，y′ i)为局部地图中第i个栅格的地图坐标，P(x′ i，y′ i)为局部地图中地图坐标为(x′ i，y′ i)的第i个栅格的置信度，(x i，y i)为原始Global地图中第i个栅格的地图坐标，P(x i，y i)为原始Global地图中地图坐标为(x i，y i)的第i个栅格的置信度；
Figure PCTCN2020113932-appb-000015
其中，odd hit表示上次和本次都有路面点落入时栅格的状态更新系数， odd miss表示路面点上次落入本次未落入时栅格的状态更新系数；s和t的取值根据地面环境的稀疏程度进行调节。
本申请增强了机器人自动定位导航的稳定性、安全性，大大降低了机器人跌落到道路之外的风险，有助于实现机器人自救并可持续执行任务。

Claims (9)

  1. 一种基于局部地图的地图修正方法，包括以下步骤：
    机器人采集激光数据；
    利用采集的激光数据在原始Global地图中进行定位导航，包括：
    计算机器人在原始Global地图的预测位姿和预测位姿的置信度分值；
    生成局部地图；
    判断定位是否丢失，若定位没有丢失，则继续定位导航，若定位丢失，则利用生成的局部地图修正Global地图；
    所述利用局部地图修正原始Global地图包括：
    计算机器人在局部地图中的位姿；
    根据机器人在原始Global地图中的位姿和机器人在局部地图中的位姿计算局部地图和原始Global地图的匹配关系；
    根据匹配关系将局部地图与原始Global地图进行叠加，完成对原始Global地图的修正。
  2. 根据权利要求1所述的基于局部地图的地图修正方法，其中，所述计算自身的预测位姿和预测位姿的置信度分值包括：
    利用里程计和惯性导航单元记录的前两帧激光数据对应的机器人位姿，计算机器人位移速度和旋转速度，利用所述位移速度、旋转速度和前一帧位姿计算下一帧的位姿初值即预测位姿初值；
    以预测位姿初值为中心，基于扫描参数确定各扫描角度，将各扫描角度的位姿作为预测位姿初值的所有的候选位姿；
    计算各候选位姿的置信度和置信度分值，选择置信度分值最高的候选位姿作为机器人的预测位姿。
  3. 根据权利要求2所述的基于局部地图的地图修正方法，其中，所述判断定位是否丢失包括：
    若预测位姿的置信度分值大于设定的阈值，则表明定位未丢失；
    若连续数帧的预测位姿的置信度分值小于设定的阈值，则表明定位丢失。
  4. 根据权利要求3所述的基于局部地图的地图修正方法，其中，所述生成局部地图包括：
    通过固定长度的滑动窗口以固定步长向前移动，将采集到的激光数据划分成一段段连续的数据帧；
    根据接收的相邻的两帧激光数据时机器人的位姿计算机器人相邻位姿间的转换关系；
    利用转换关系拼接相邻的两帧激光数据，从而将每一个滑动窗口内的连续数帧激光数据生成一张子图，上一张子图的尾部和下一张子图的头部有重叠部分。
  5. 根据权利要求4所述的基于局部地图的地图修正方法，其中，所述计算机器人在局部地图中的位姿包括：
    运用计算预测位姿的各候选位姿的置信度分值的方法，求取机器人在局部地图中局部预测位姿的各候选位姿的置信度分值，选取置信度分值最高的K个位姿，对这K个位姿对应的各项求取平均值，得到机器人在局部地图中的位姿。
  6. 根据权利要求5所述的基于局部地图的地图修正方法，其中，所述根据机器人在原始Global地图中的位姿和机器人在局部地图中的位姿计算局部地图和原始Global地图的匹配关系包括：
    计算机器人在局部地图中的位姿p(x′，y′，γ′)与原始Global地图中位姿p 原(x，y，γ)的位姿差：
    Figure PCTCN2020113932-appb-100001
    其中，x，y为机器人在原始Global地图中的横坐标和纵坐标，γ为机器人在原始Global地图中的姿态；x′，y′为机器人在局部地图中的横坐标和纵坐标，γ′为机器人在局部地图中的姿态；
    计算局部地图与原始Global地图的匹配关系R为：
    Figure PCTCN2020113932-appb-100002
    Figure PCTCN2020113932-appb-100003
  7. 根据权利要求6所述的基于局部地图的地图修正方法，其中，所述根据匹配关系将局部地图与原始Global地图进行叠加并完成对原始Global地图的修正包括：
    根据匹配关系R将局部地图与原始Global地图进行叠加；
    根据局部地图与原始Global地图的叠加情况，更新各个叠加栅格的置信度从而完成对原始Global地图进行修正，包括：
    Figure PCTCN2020113932-appb-100004
    其中，P(x i，y i)_new为修正后的Global地图中地图坐标为(x i，y i)的栅格的置信度，(x′ i，y′ i)为局部地图中第i个栅格的地图坐标，P(x′ i，y′ i)为局部地图中地图坐标为(x′ i，y′ i)的第i个栅格的置信度，(x i，y i)为原始Global地图中第i个栅格的地图坐标，P(x i，y i)为原始Global地图中地图坐标为(x i，y i)的第i个栅格的置信度；
    Figure PCTCN2020113932-appb-100005
    其中，odd hit表示上次和本次都有路面点落入时栅格的状态更新系数，odd miss表示路面点上次落入本次未落入时栅格的状态更新系数；s和t的取值根据地面环境的稀疏程度进行调节。
  8. 一种基于局部地图的地图修正***，包括激光采集模块、地图生成模块、地图修正模块、地图调度模块和地图数据库，激光采集模块采集激光数据，将激光数据传输给地图生成模块生成局部地图，地图生成模块将其生成的局部地图存储至地图数据库；当激光采集模块的定位丢失时，地图调度模块从地图数据库调取最新的局部地图与原始global地图将其传输给地图修正模块，地图修 正模块将最新的局部地图与原始global地图进行融合，完成对原始global地图的修正。
  9. 根据权利要求8所述的基于局部地图的地图修正***，其中，所述激光采集模块为搭载里程计、惯性导航单元和激光传感器的机器人。
PCT/CN2020/113932 2019-12-10 2020-09-08 基于局部地图的地图修正方法及*** WO2021114764A1 (zh)

Applications Claiming Priority (2)

Application Number Priority Date Filing Date Title
CN201911258908.1A CN111060135B (zh) 2019-12-10 2019-12-10 一种基于局部地图的地图修正方法及***
CN201911258908.1 2019-12-10

Publications (1)

Publication Number Publication Date
WO2021114764A1 true WO2021114764A1 (zh) 2021-06-17

Family

ID=70300398

Family Applications (1)

Application Number Title Priority Date Filing Date
PCT/CN2020/113932 WO2021114764A1 (zh) 2019-12-10 2020-09-08 基于局部地图的地图修正方法及***

Country Status (2)

Country Link
CN (1) CN111060135B (zh)
WO (1) WO2021114764A1 (zh)

Cited By (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113532439A (zh) * 2021-07-26 2021-10-22 广东电网有限责任公司 输电线路巡检机器人同步定位与地图构建方法及装置
CN113551677A (zh) * 2021-08-16 2021-10-26 河南牧原智能科技有限公司 对机器人进行重定位的方法和相关产品
CN113763548A (zh) * 2021-08-17 2021-12-07 同济大学 基于视觉-激光雷达耦合的贫纹理隧洞建模方法及***
CN113776533A (zh) * 2021-07-29 2021-12-10 北京旷视科技有限公司 可移动设备的重定位方法及装置
CN113932790A (zh) * 2021-09-01 2022-01-14 北京迈格威科技有限公司 一种地图更新方法、装置、***、电子设备及存储介质
CN113984065A (zh) * 2021-10-27 2022-01-28 山东亚历山大智能科技有限公司 一种用于室内机器人的反光板地图生成方法及***
CN114643579A (zh) * 2022-03-29 2022-06-21 深圳优地科技有限公司 一种机器人定位方法、装置、机器人及存储介质

Families Citing this family (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111060135B (zh) * 2019-12-10 2021-12-17 亿嘉和科技股份有限公司 一种基于局部地图的地图修正方法及***
CN111596298B (zh) * 2020-05-13 2022-10-14 北京百度网讯科技有限公司 目标对象的定位方法、装置、设备及存储介质
WO2022007367A1 (en) * 2020-07-09 2022-01-13 Zhejiang Dahua Technology Co., Ltd. Systems and methods for pose determination
CN112013845B (zh) * 2020-08-10 2022-04-22 北京轩宇空间科技有限公司 适应未知动态空间的快速地图更新方法、装置及存储介质
CN112013840B (zh) * 2020-08-19 2022-10-28 安克创新科技股份有限公司 扫地机器人及其地图构建方法和装置
CN112762923A (zh) * 2020-12-31 2021-05-07 合肥科大智能机器人技术有限公司 一种3d点云地图更新方法和***
CN113375683A (zh) * 2021-06-10 2021-09-10 亿嘉和科技股份有限公司 机器人环境地图实时更新方法
CN113532441B (zh) * 2021-08-20 2024-06-14 河南牧原智能科技有限公司 用于对猪舍内载具进行组合导航的方法、装置及存储介质

Citations (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20100121488A1 (en) * 2007-07-23 2010-05-13 Electronics And Telecommunications Research Institute Method and system for creating indoor environment map
CN108007453A (zh) * 2017-12-11 2018-05-08 北京奇虎科技有限公司 基于点云的地图更新方法、装置及电子设备
CN108036793A (zh) * 2017-12-11 2018-05-15 北京奇虎科技有限公司 基于点云的定位方法、装置及电子设备
CN108550318A (zh) * 2018-03-12 2018-09-18 浙江大华技术股份有限公司 一种构建地图的方法及装置
CN109141437A (zh) * 2018-09-30 2019-01-04 中国科学院合肥物质科学研究院 一种机器人全局重定位方法
CN109443351A (zh) * 2019-01-02 2019-03-08 亿嘉和科技股份有限公司 一种稀疏环境下的机器人三维激光定位方法
CN109840448A (zh) * 2017-11-24 2019-06-04 百度在线网络技术（北京）有限公司 用于无人驾驶车辆的信息输出方法和装置
CN111060135A (zh) * 2019-12-10 2020-04-24 亿嘉和科技股份有限公司 一种基于局部地图的地图修正方法及***

Family Cites Families (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP5471626B2 (ja) * 2010-03-09 2014-04-16 ソニー株式会社 情報処理装置、マップ更新方法、プログラム及び情報処理システム
JP5589900B2 (ja) * 2011-03-03 2014-09-17 株式会社豊田中央研究所 局所地図生成装置、グローバル地図生成装置、及びプログラム
CN105258702B (zh) * 2015-10-06 2019-05-07 深圳力子机器人有限公司 一种基于slam导航移动机器人的全局定位方法
JP2017194527A (ja) * 2016-04-19 2017-10-26 トヨタ自動車株式会社 環境地図のデータ構造、環境地図の作成システム及び作成方法、並びに、環境地図の更新システム及び更新方法
CN107144285B (zh) * 2017-05-08 2020-06-26 深圳地平线机器人科技有限公司 位姿信息确定方法、装置和可移动设备
CN107764270A (zh) * 2017-10-19 2018-03-06 武汉工控仪器仪表有限公司 一种激光扫描式室内地图生成和更新装置及方法
CN108344999B (zh) * 2018-01-09 2020-08-11 浙江大学 一种用于水下机器人导航的声呐地图构建与重定位方法
CN109211251B (zh) * 2018-09-21 2022-01-11 北京理工大学 一种基于激光和二维码融合的即时定位与地图构建方法

Patent Citations (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20100121488A1 (en) * 2007-07-23 2010-05-13 Electronics And Telecommunications Research Institute Method and system for creating indoor environment map
CN109840448A (zh) * 2017-11-24 2019-06-04 百度在线网络技术（北京）有限公司 用于无人驾驶车辆的信息输出方法和装置
CN108007453A (zh) * 2017-12-11 2018-05-08 北京奇虎科技有限公司 基于点云的地图更新方法、装置及电子设备
CN108036793A (zh) * 2017-12-11 2018-05-15 北京奇虎科技有限公司 基于点云的定位方法、装置及电子设备
CN108550318A (zh) * 2018-03-12 2018-09-18 浙江大华技术股份有限公司 一种构建地图的方法及装置
CN109141437A (zh) * 2018-09-30 2019-01-04 中国科学院合肥物质科学研究院 一种机器人全局重定位方法
CN109443351A (zh) * 2019-01-02 2019-03-08 亿嘉和科技股份有限公司 一种稀疏环境下的机器人三维激光定位方法
CN111060135A (zh) * 2019-12-10 2020-04-24 亿嘉和科技股份有限公司 一种基于局部地图的地图修正方法及***

Cited By (10)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113532439A (zh) * 2021-07-26 2021-10-22 广东电网有限责任公司 输电线路巡检机器人同步定位与地图构建方法及装置
CN113532439B (zh) * 2021-07-26 2023-08-25 广东电网有限责任公司 输电线路巡检机器人同步定位与地图构建方法及装置
CN113776533A (zh) * 2021-07-29 2021-12-10 北京旷视科技有限公司 可移动设备的重定位方法及装置
CN113551677A (zh) * 2021-08-16 2021-10-26 河南牧原智能科技有限公司 对机器人进行重定位的方法和相关产品
CN113763548A (zh) * 2021-08-17 2021-12-07 同济大学 基于视觉-激光雷达耦合的贫纹理隧洞建模方法及***
CN113763548B (zh) * 2021-08-17 2024-02-27 同济大学 基于视觉-激光雷达耦合的贫纹理隧洞建模方法及***
CN113932790A (zh) * 2021-09-01 2022-01-14 北京迈格威科技有限公司 一种地图更新方法、装置、***、电子设备及存储介质
CN113984065A (zh) * 2021-10-27 2022-01-28 山东亚历山大智能科技有限公司 一种用于室内机器人的反光板地图生成方法及***
CN114643579A (zh) * 2022-03-29 2022-06-21 深圳优地科技有限公司 一种机器人定位方法、装置、机器人及存储介质
CN114643579B (zh) * 2022-03-29 2024-01-16 深圳优地科技有限公司 一种机器人定位方法、装置、机器人及存储介质

Also Published As

Publication number Publication date
CN111060135A (zh) 2020-04-24
CN111060135B (zh) 2021-12-17

Similar Documents

Publication Publication Date Title
WO2021114764A1 (zh) 基于局部地图的地图修正方法及***
CN111207774B (zh) 一种用于激光-imu外参标定的方法及***
US11802769B2 (en) Lane line positioning method and apparatus, and storage medium thereof
CN107239076B (zh) 基于虚拟扫描与测距匹配的agv激光slam方法
CN108253958B (zh) 一种稀疏环境下的机器人实时定位方法
CN106774431B (zh) 一种测绘无人机航线规划方法及装置
US10921461B2 (en) Method and apparatus for determining unmanned vehicle positioning accuracy
KR101976241B1 (ko) 다중로봇의 자기위치인식에 기반한 지도작성 시스템 및 그 방법
CN103914068A (zh) 一种基于栅格地图的服务机器人自主导航方法
CN106441275A (zh) 一种机器人规划路径的更新方法及装置
WO2019136714A1 (zh) 一种基于3d激光的地图构建方法及***
CN110361027A (zh) 基于单线激光雷达与双目相机数据融合的机器人路径规划方法
CN107741234A (zh) 一种基于视觉的离线地图构建及定位方法
CN109828588A (zh) 一种基于多传感器融合的机器人室内路径规划方法
CN109885049A (zh) 一种基于航位推算的激光导引agv自动建图和路径匹配方法
JP5429901B2 (ja) ロボット及び情報処理装置のプログラム
US20230112991A1 (en) Method of high-precision 3d reconstruction of existing railway track lines based on uav multi-view images
CN114526745A (zh) 一种紧耦合激光雷达和惯性里程计的建图方法及***
CN110243380A (zh) 一种基于多传感器数据与角度特征识别的地图匹配方法
CN109813305B (zh) 基于激光slam的无人叉车
CN110530372A (zh) 定位方法、路径确定方法、装置、机器人及存储介质
JP5068840B2 (ja) ロボットのプログラム及び情報処理装置のプログラム
CN108398129B (zh) 用于消防救援中人员位置与地图快速匹配的方法
CN107179533A (zh) 一种多参数机载LiDAR***误差自动检校方法
CN111397599A (zh) 基于三角形匹配算法改进的iccp水下地磁匹配方法

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: 20898608

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: 20898608

Country of ref document: EP

Kind code of ref document: A1