WO2021114764A1

WO2021114764A1 - Map correction method and system based on local map

Info

Publication number: WO2021114764A1
Application number: PCT/CN2020/113932
Authority: WO
Inventors: 林欢; 程敏; 许春山; 毛成林; 王�锋
Original assignee: 亿嘉和科技股份有限公司
Priority date: 2019-12-10
Filing date: 2020-09-08
Publication date: 2021-06-17
Also published as: CN111060135B; CN111060135A

Abstract

A map correction method and system based on a local map. The map correction method comprises the following steps: a robot collects laser data; and perform positioning and navigation in an original Global map by utilizing the collected laser data, wherein the step comprises: calculating a predicted pose of the robot in the original Global map and a confidence score of the predicted pose; generating a local map; and determining whether the positioning is lost, continuing to position and navigate if the positioning is not lost, and correcting the Global map by utilizing the generated local map if the positioning is lost.

Description

Map correction method and system based on local map

This application claims the priority of the Chinese patent application filed with the Chinese Patent Office with application number 201911258908.1 on December 10, 2019. The entire content of the above application is incorporated into this application by reference.

Technical field

This application relates to the field of inspection robots, for example, to a map correction method and system based on a local map.

Background technique

In recent years, with the development of science and technology, 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. With the continuous development and advancement of technology, 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. At present, 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. However, 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. In related technologies, 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.

Summary of the invention

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;

Use the collected laser data to perform positioning and navigation in the original Global map, including:

Calculate the predicted pose and confidence score of the predicted pose of the robot on the original Global map;

Generate a local map;

Determine whether the positioning is lost. If the positioning is not lost, continue positioning and navigation. If the positioning is lost, use the generated local map to modify the Global map.

The use of the local map to modify the original Global map includes:

Calculate the pose of the robot in the local map;

Calculate 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;

According to the matching relationship, 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;

Taking the initial value of the predicted pose as the center, determine each scan angle based on the scan parameters, and use the pose of each scan angle as all the candidate poses of the initial value of the predicted pose;

Calculate the confidence and confidence score of each candidate pose, and select the candidate pose with the highest confidence score as the predicted pose of the robot;

The judging whether the positioning is lost includes:

If the confidence score of the predicted pose is greater than the set threshold, it indicates that the positioning is not lost;

If the confidence score of the predicted pose of consecutive frames is less than the set threshold, it indicates that the positioning is lost.

The generating a local map includes:

Moving forward with a fixed step length through a fixed-length sliding window, divide the collected laser data into segments of continuous data frames;

Calculate the conversion relationship between adjacent poses of the robot according to the pose of the robot when receiving two adjacent frames of laser data;

Use the conversion relationship to splice two adjacent frames of laser data, so as to generate a sub-image from the consecutive frames of laser data in each sliding window. The tail of the previous sub-image and the head of the next sub-image overlap.

The pose of the computing robot in the local map includes:

Using the method of calculating the confidence score of each candidate pose of the predicted pose, obtain the confidence score of each candidate pose of the local predicted pose of the robot in the local map, and select the K positions with the highest confidence score. Pose, calculate the average of the items corresponding to these K poses, and get the pose of the robot in the local map.

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:

Calculate the difference between the pose of the robot in the local map and the pose p _original (x, y, γ) in the original Global map:

Among them, 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;

Calculate the matching relationship R between the local map and the original Global map as:

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:

Overlay the local map with the original Global map according to the matching relationship R;

According to the superposition of the local map and the original Global map, update the confidence of each superimposed grid to complete the correction of the original Global map, including:

Wherein the confidence level _{_{P (x i, y i)}} _new as Global map after correction map coordinates (x _{_i,} y _i) of the _{grid, (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, and 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 );}

Among them, odd _hit represents the state update coefficient of the grid when the pavement point fell last time and this time, and 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:

1. When the working environment of the robot changes, the robot can still locate and navigate accurately, which enhances the adaptability of the robot to the environment.

2. Enhance the stability and safety of the robot's automatic positioning and navigation, greatly reduce the risk of the robot falling off the road, and help the robot to achieve self-rescue and continue to perform tasks.

Description of the drawings

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;

Figure 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.

Detailed ways

The following describes in detail a map correction method and system based on a local map with reference to the accompanying drawings.

A map correction system based on a local map is shown in Figure 1. It 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. In this embodiment, 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. When the robot loses status, 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.

In conjunction with Figure 2, a detailed description of a map correction method based on a local map is given. The specific process is mainly divided into three parts: the positioning and navigation of the robot in the original Global map, the construction of the local map, and the correction of the original Global map. Global map correction is the main content of this application, and the overall process is as follows:

The robot performs positioning and navigation in the original Global map:

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;

Determine each scan angle according to the initial value of the predicted pose and the positioning scan parameters, and use the pose of each scan angle on the map grid as all possible candidate poses;

Calculate the confidence of each possible candidate pose, and select the pose with the highest confidence score as the laser positioning result of the robot. The specific process is as follows:

1. Estimate the initial value of the predicted pose of the robot

Use the odometer and inertial navigation unit to obtain the robot's pose in the map coordinate system at the current moment. Assuming that the robot's pose at time t is p(x _t , y _t , γ _t ), x _t represents the robot at time t In the abscissa of the original Global map, y _t represents the ordinate of the robot in the original Global map _{at time t, and γ t} is the posture of the robot in the original Global map at time t, the previous time point for calculating the pose Is t-1, the corresponding pose is p(x _t-1 , y _t-1 , γ _t-1 ), and the next time point t+1 when the pose is calculated, the next pose of the corresponding position is p( x _t+1 , y _t+1 , γ _t+1 ). The time interval between time points t and t-1 is Δt. Estimate the robot's moving speed 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:

Use 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.

Due to the error of the actual hardware, there will be a deviation between the accurate pose of the robot at time t+1 and the initial value of the predicted pose. _{Next, optimize p(x t+1} , y _t+1 , γ _t+1 ) based on the degree of coincidence between the laser measurement data and the predicted pose initial value corresponding to the map data, and finally obtain the optimal position closest to the accurate pose .

2. Obtain discrete scan data at different scan angles

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, As shown in Figure 3, 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.

Calculate the position of the map grid corresponding to each laser reflection point at each scan angle (that is, calculate the coordinates of each map grid in the map coordinate system) according to the laser data obtained by the positioning scan of the robot, as the discrete scan data of each scan angle. For the discrete scan data of a certain scanning angle, if there are multiple laser reflection points that repeatedly fall on the same map grid position, only the coordinates of the map grid corresponding to one of the laser reflection points in the map coordinate system are taken. As shown in Fig. 4, the gray grid is a situation where multiple laser reflection points fall into the same map grid, and only the coordinates of one laser reflection point in the gray grid is taken to calculate the confidence in the subsequent steps.

3. Confidence estimation

According to the confidence of each map grid corresponding to each candidate pose (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:

Among them, m is the total number of map grids in the discrete scan data of the scan angle corresponding to a certain candidate pose. Set the map coordinates of the nth grid to (x _n , y _n ), and the grid confidence is

The value range is [0, 1].

Calculate the confidence weight ω corresponding to each candidate pose according to the pose difference between each candidate pose and the pose estimation value, the formula is as follows:

Among them, 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, and ω _r is the rotation angle weight. Generally, ω _xy and ω _r take 1, which means that the displacement and the rotation angle have the same weight.

The product of the confidence σ of each candidate pose and the confidence weight ω is used as the confidence score of the current pose, and the formula is as follows:

score=σ·ω

Select the pose with the highest confidence score to update the initial value of the predicted pose as the final predicted pose, that is, as the pose at time t+1 (x _t+1 , y _t+1 , γ _t+1 ), The confidence score corresponding to the predicted pose is score _max .

Second, generate a local map

In the process of positioning, 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:

First, 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).

_{Furthermore, according to the pose p k-1} and p _k when the robot receives two adjacent frames of laser data, solve p _k = p _k-1 r _k-1 +t _{k-1 to} obtain the conversion relationship between adjacent poses of the robot (r _k-1 , t _k-1 ), where p _k-1 represents the pose of the robot when receiving the k-1 frame of laser data, p _k represents the pose of the robot when receiving the k-th frame of laser data, r _{k- 1} represents a conversion relationship _{_{(r k-1, t k}} -1) in the rotation matrix, t _k-1 represents a conversion relationship _{_{(r k-1, t k}} -1) translation variables.

Then, use the obtained conversion relationship to splice the laser data of adjacent frames corresponding to the conversion relationship. The specific splicing formula is: Q _k =Q _k-1 r _k-1 +t _k-1 , where Q _k-1 , Q _k is the k-1th frame of laser data and the kth frame of laser data, respectively.

Finally, 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. OK), use the latest consecutive frames of laser data in the sliding window to generate a sub-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. In this embodiment, for the latest 400 frames of data, 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. Generate sub-image A2, and register the laser data from frames 20 to 40 to generate sub-image A3,..., and finally generate 39 sub-images. Save the generated subgraph as an intermediate file. When a new subgraph is generated, the first generated subgraph in the intermediate file is discarded, and the new subgraph is added. All subgraphs constitute a local map

3. Use the local map to correct the original Global map

When the robot's operating environment changes, the confidence of the original Global map will decrease, and the robot will lose its positioning. At this time, the sub-map stored in the intermediate file needs to be merged with the original Global map. The specific process is as follows:

First, 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. _{If 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.

Then, 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. Pose, p(x′ ₁ ,y′ ₁ ,γ′ ₁ ), p(x′ ₂ ,y′ ₂ ,γ′ ₂ )...p(x′ _K ,y′ _K ,γ′ _K ), K ∈(3,6), take the average of the items corresponding to the selected K poses, and get the pose p(x′,y′,γ′) of the robot in the local map:

The current pose of the robot in the original Global map, the robot pose in the local demand in the original map Global map _original pose p (x, y, γ) the pose difference:

Among them, 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, and γ′ is the robot The posture in the local map;

Find the matching relationship R between the local map and the original Global map as:

According to the matching relationship R, the local map and the original Global map are superimposed.

Finally, according to the superposition of the local map and the original Global map, the confidence of each superimposed grid is updated, and the original Global map is corrected:

Wherein the confidence level _{_{P (x i, y i)}} _new as Global map after correction map coordinates (x _{_i,} y _i) of the _{grid, (x 'i, y'} i) is the i-th partial map 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, and 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 );}

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.

Claims

A map correction method based on a local map includes the following steps:

The robot collects laser data;

Use the collected laser data to perform positioning and navigation in the original Global map, including:

Calculate the predicted pose and confidence score of the predicted pose of the robot on the original Global map;

Generate a local map;

Determine whether the positioning is lost. If the positioning is not lost, continue positioning and navigation. If the positioning is lost, use the generated local map to modify the Global map;

The use of the local map to modify the original Global map includes:

Calculate the pose of the robot in the local map;

Calculate 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;

According to the matching relationship, the local map and the original Global map are superimposed to complete the correction of the original Global map.
The map correction method based on a local map according to claim 1, wherein said calculating the predicted pose and the confidence score of the predicted pose comprises:

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;

Taking the initial value of the predicted pose as the center, determine each scan angle based on the scan parameters, and use the pose of each scan angle as all the candidate poses of the initial value of the predicted pose;

Calculate the confidence and confidence score of each candidate pose, and select the candidate pose with the highest confidence score as the predicted pose of the robot.
The map correction method based on a local map according to claim 2, wherein said determining whether the positioning is lost comprises:

If the confidence score of the predicted pose is greater than the set threshold, it indicates that the positioning is not lost;

If the confidence score of the predicted pose of consecutive frames is less than the set threshold, it indicates that the positioning is lost.
The map correction method based on a local map according to claim 3, wherein said generating a local map comprises:

Moving forward with a fixed step length through a fixed-length sliding window, divide the collected laser data into segments of continuous data frames;

Calculate the conversion relationship between adjacent poses of the robot according to the pose of the robot when receiving two adjacent frames of laser data;

Use the conversion relationship to splice two adjacent frames of laser data, so as to generate a sub-image from the continuous frames of laser data in each sliding window. The tail of the previous sub-image and the head of the next sub-image overlap.
The map correction method based on a local map according to claim 4, wherein the calculating the pose of the robot in the local map comprises:

Using the method of calculating the confidence score of each candidate pose of the predicted pose, obtain the confidence score of each candidate pose of the local predicted pose of the robot in the local map, and select the K positions with the highest confidence score. Pose, calculate the average of the items corresponding to these K poses, and get the pose of the robot in the local map.
The method for map correction based on a local map according to claim 5, wherein said calculating 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 comprises:

Pose p is calculated in the local map (x ', y', γ ') of the original pose p (x, y, γ) the difference between the original position and orientation Global map:

Among them, 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;

Calculate the matching relationship R between the local map and the original Global map as:
The method for map correction based on a local map according to claim 6, wherein the superimposing the local map with the original Global map according to the matching relationship and completing the correction of the original Global map comprises:

Overlay the local map with the original Global map according to the matching relationship R;

According to the superposition of the local map and the original Global map, update the confidence of each superimposed grid to complete the correction of the original Global map, including:

Wherein the confidence level P (x i, y i) _new as Global map after correction map coordinates (x i, y i) of the grid, (x 'i, y' i) is the i-th partial map 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, and 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 );

Among them, odd hit represents the state update coefficient of the grid when the pavement point fell last time and this time, and 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 map correction system based on a local map according to claim 8, wherein the laser acquisition module is a robot equipped with an odometer, an inertial navigation unit and a laser sensor.