CN115984504B

CN115984504B - Automatic map updating method and system and storage medium

Info

Publication number: CN115984504B
Application number: CN202310277053.7A
Authority: CN
Inventors: 王冠
Original assignee: Shanghai Xiangong Intelligent Technology Co ltd
Current assignee: Shanghai Xiangong Intelligent Technology Co ltd
Priority date: 2023-03-21
Filing date: 2023-03-21
Publication date: 2023-07-04
Anticipated expiration: 2043-03-21
Also published as: CN115984504A

Abstract

The invention provides a map automatic updating method, a map automatic updating system and a storage medium, wherein the method comprises the following steps: in the navigation map, setting the area where the semi-static object is located as the area to be updated of the map; after the mobile robot goes to the map area to be updated, starting a map building algorithm, and collecting the positioning pose A of the mobile robot in the navigation map and the positioning pose B of the mobile robot in the local map; taking the positioning pose B as a vertex, taking the pose transformation relationship between two adjacent positioning poses B as a bilateral, constructing a graph model to perform graph optimization, and obtaining the positioning pose of the local graph construction to construct a local map; performing time stamp alignment on the positioning pose A and the positioning pose B through linear interpolation compensation, and performing ICP alignment on the acquired track A and the track B to calculate a rotation translation matrix C so as to convert the local map into a navigation map coordinate system according to the matrix C; replacing the area to be updated of the map by the converted local map; thereby reducing the computational resources occupied during map updating.

Description

Automatic map updating method and system and storage medium

Technical Field

The invention relates to a mobile robot positioning navigation technology, in particular to a method, a system and a storage medium for automatically updating a map in a specific area where a semi-static object is located.

Background

At present, the intelligent degree of factories and warehouse logistics fields is higher and higher, the mode of constructing an environment map by utilizing a laser SLAM technology and then performing mobile robot navigation is more and more popular, and the deployment is simple and quick due to low implementation cost.

However, in some factory environments, since objects in the environment often undergo movement changes, the actual environment deviates greatly from a previously scanned laser profile map, so that deviation occurs when the laser map is referred to in the actual operation process of the mobile robot, and the operation route and position of the mobile robot may not be in accordance with expectations.

For this purpose, various map updating techniques have been proposed in the prior art, such as "laser map updating method, robot and clustered robot system" (patent publication No. CN 112629518A), which mainly uses mapping the current laser point cloud onto a map according to the current positioning point, and then determining whether to update the map according to the total deviation between the current scanning point and the map point.

However, when the laser radar with a higher sampling rate is used, the calculation amount of the method is larger, which usually causes calculation at all times when needed, so that the method continuously occupies calculation resources in a large map scene. On the other hand, in the step of confirming whether to update the map, this prior art makes a judgment by comparing the total deviation between the scanning point and the map point, which requires a calculation comparison for each laser point in the current scanning and a point on the map, which is very wasteful of time and calculation resources.

Disclosure of Invention

Therefore, a primary object of the present invention is to provide an automatic map updating method, system and storage medium, so as to reduce the computing resources occupied during map updating.

In order to achieve the above object, according to a first aspect of the present invention, there is provided an automatic map updating method comprising the steps of:

step S100, setting an area where a semi-static object is located as an area to be updated of a map in a navigation map;

step S200, after the mobile robot goes to the area to be updated of the map, starting a mapping algorithm, and collecting all positioning pose A in the navigation map and all positioning pose B in the local map;

step S300, taking each positioning pose B as a vertex, taking the pose transformation relationship between two adjacent positioning poses B as a bilateral, constructing a graph model to perform graph optimization, and obtaining a local graph construction positioning pose to construct a new local map;

step S400, performing time stamp alignment on each positioning pose A and each positioning pose B through linear interpolation compensation, and performing ICP (Iterative Closest Point) alignment on the obtained track a and the obtained track B to calculate a rotation translation matrix C so as to convert a new local map into a navigation map coordinate system according to the matrix C;

and step S500, replacing the map to-be-updated area with the new local map converted in the step S400.

In a possibly preferred embodiment, step S500 further comprises, before:

step S410, the set of the positioning pose A points on the track a is collected

And the set of localization pose B points on track B +.>

Root mean square error calculation

，

When judging

And allowing the execution of step S500 when the preset precision value is satisfied.

In a possibly preferred embodiment, in step S300, each positioning pose B of a key frame in the local map is used as a vertex, and a pose transformation relationship between positioning poses B of two adjacent key frames is used as a bilateral, so as to construct a graph model for graph optimization.

In a possibly preferred embodiment, the key frame in step S500 is that when the mobile robot moves more than 0.5m or rotates more than 20 °, the current localization pose is recorded as localization pose B of the key frame in the local map.

In order to achieve the above object, corresponding to the above method, according to a second aspect of the present invention, there is provided an automatic map updating system comprising:

the storage unit is used for storing a program comprising any one of the map automatic updating method steps, so that the program is timely adjusted and executed by the navigation unit, the data acquisition unit, the map building unit and the processing unit;

the navigation unit is used for setting the area where the semi-static object is located as the area to be updated of the map in the navigation map so as to enable the mobile robot to go to;

the map construction unit is used for starting a map construction algorithm in the area to be updated of the map;

the data acquisition unit is used for acquiring each positioning pose A of the mobile robot in the navigation map and each positioning pose B of the mobile robot in the local map;

the processing unit is used for taking each positioning pose B of the key frame as a vertex, taking the pose transformation relationship between two adjacent positioning poses B of the key frame as a bilateral, constructing a graph model for graph optimization, and obtaining the positioning poses of the local graph construction;

the map building unit is also used for building a new local map according to the local map building positioning pose;

the processing unit is also used for performing time stamp alignment on the positioning pose A and the positioning pose B through linear interpolation compensation, acquiring a track a and a track B, performing ICP alignment to calculate a rotation translation matrix C, and converting a new local map into a navigation map coordinate system according to the matrix C;

and the map building unit is also used for updating the navigation map to-be-updated area of the converted new local map.

In a possibly preferred embodiment, the processing unit is further adapted to: the set of the position pose A points on the track a

And the set of localization pose B points on track B +.>

Root mean square error calculation

，

When (when)

And allowing the map building unit to update the map when the preset precision value is met.

In order to achieve the above object, corresponding to the above method, according to a third aspect of the present invention, there is also provided a computer-readable storage medium having stored thereon a computer program, characterized in that the computer program, when executed by a processor, implements the steps of the map automatic updating method according to any one of the above.

The map scene is divided into three parts skillfully by the automatic map updating method, the system and the storage medium provided by the invention: the static map, the semi-static map and the dynamic map are used for processing only the semi-static map area through analysis, and map updating is conducted in a specific area, so that unnecessary calculation amount in the moving process of the mobile robot is effectively reduced, and meanwhile, the updated area is smaller than the whole map, so that updating precision of the local map can be guaranteed to a certain extent under the reference of the original navigation map.

On the other hand, in a part of the preferred scheme, because the prior art mainly judges whether to update the map by comparing the total deviation between the scanning points and the map points, the calculation and comparison are needed to be carried out on each laser point in the current scanning and the points on the map, so that time is wasted, and the comparison calculation can be completed by recording a plurality of local map track points and navigation map track points and only calculating dozens or even a plurality of points, so that the calculation requirement is greatly reduced.

Drawings

The accompanying drawings, which are included to provide a further understanding of the invention and are incorporated in and constitute a part of this specification, illustrate embodiments of the invention and together with the description serve to explain the invention. In the drawings:

FIG. 1 is a schematic diagram illustrating steps of an automatic map updating method according to the present invention;

FIG. 2 is a schematic diagram of the map automatic update method according to the present invention;

FIG. 3 is a schematic diagram of linear interpolation in the automatic map updating method of the present invention;

FIG. 4 is a schematic diagram of the alignment of track a and track b in the map automatic update method of the present invention;

FIG. 5 is a schematic diagram of an occupied grid map establishment process in the map automatic updating method of the present invention;

fig. 6 is a schematic structural diagram of the map automatic updating system of the present invention.

Description of the embodiments

In order that those skilled in the art can better understand the technical solutions of the present invention, the following description will clearly and completely describe the specific technical solutions of the present invention in conjunction with the embodiments to help those skilled in the art to further understand the present invention. It will be apparent that the embodiments described herein are merely some, but not all embodiments of the invention. It should be noted that embodiments and features of embodiments in this application may be combined with each other by those of ordinary skill in the art without departing from the inventive concept and conflict. All other embodiments, which are derived from the embodiments herein without creative effort for a person skilled in the art, shall fall within the disclosure and the protection scope of the present invention.

Furthermore, the terms "first," "second," "S100," "S200," and the like in the description and in the claims and drawings are used for distinguishing between similar objects and not necessarily for describing a particular sequential or chronological order. It is to be understood that the data so used may be interchanged where appropriate such that the embodiments of the invention described herein may be implemented in sequences other than those described herein. Also, the terms "comprising" and "having" and any variations thereof herein are intended to cover a non-exclusive inclusion. Unless specifically stated or limited otherwise, the terms "disposed," "configured," "mounted," "connected," "coupled" and "connected" are to be construed broadly, e.g., as being either permanently connected, removably connected, or integrally connected; can be mechanically or electrically connected; can be directly connected or indirectly connected through an intermediate medium, and can be communication between two elements. The specific meaning of the terms in this case will be understood by those skilled in the art in view of the specific circumstances and in combination with the prior art.

In the example of the present invention, for industrial scenes, environments are generally classified into three kinds, a static environment, a semi-static environment, and a dynamic environment, wherein the static environment refers to a series of objects such as wall posts that do not move, the dynamic environment refers to objects such as worker vehicles that do not stop moving, and the semi-static environment refers to objects where the position of goods changes, but does not change all the time.

In an industrial scenario, dynamic objects are reduced when automation is implemented, and the static environment is originally the main reference object for positioning, so the patent focuses on map updating of the area where the semi-static objects are located. In which the positioning of the mobile robot is mainly dependent on the stationary object, whereas both the semi-stationary object and the stationary object interfere with the positioning, it is considered that in an automated factory there are fewer, more semi-stationary objects, which are dynamic objects. At present, a laser radar sensor is mainly adopted as a main positioning sensor in the industry, and for a semi-static object processing scheme, a reflecting column is usually adopted as a positioning reference object, or a fixed reference object is additionally added, so that laser can scan more static objects, the former needs to carry out a large number of reflecting column installation on a scene, and the latter needs to install more fixed objects in the scene as the static reference objects.

Therefore, the invention aims to adopt natural contour mapping and navigation, so that a reflection column is not required to be deployed in a scene, and for a semi-static scene, an additional static reference object is not required to be additionally arranged, and map updating is carried out according to a preset scene, thereby the CPU operation resource of the AGV can be effectively reduced, and the local mapping algorithm is only invoked when the AGV enters an area. Finally, the map can be updated in real time while occupying less CPU operation resources, so that the positioning accuracy in the semi-static area is ensured, and the map can be realized without additional operation only by setting an automatic map updating area in the navigation map.

To this end, in a preferred embodiment, as shown in fig. 1 to 5, the present invention provides an automatic map updating method, which includes the steps of:

step S100, in the navigation map, setting the area where the semi-static object is located as the area to be updated of the map.

Specifically, in an example, the step S100 includes:

in step S110, a navigation positioning map needs to be acquired first, so that a scene needs to be mapped, and the mapping scheme adopted in this example is a karto slam algorithm, and after the mapping is completed, a 2D grid map of the scene is obtained.

And step S120, editing the grid map obtained in the step S110 by using a high-level computer software RoboShop of the cursive intelligence, and setting the area where the semi-static object is located as the area where the map is to be updated.

Step S200, after the mobile robot is made to go to the area of the map to be updated, a mapping algorithm is started, and all positioning pose A in the navigation map and all positioning pose B in the local map are collected.

Specifically, after the above work is completed, the mobile robot (AGV for short) can travel to the area to be updated in the map according to the navigation positioning map, and at this time, the mapping unit of the AGV starts the mapping algorithm, reads the laser radar data and the wheel mileage data, and starts to construct the local map. Meanwhile, in the process, all positioning pose A of the AGV in the navigation map and all positioning pose B in the local map are collected.

And step S300, taking each positioning pose B as a vertex, taking the pose transformation relation between two adjacent positioning poses B as a bilateral, constructing a graph model to perform graph optimization, and obtaining the positioning poses of the local graph construction to construct a new local map.

Specifically, after the AGV drives away from the area to be updated, the map updating system of the AGV closes the map building algorithm to perform back-end optimization on the local map so as to build the rasterized local map.

First, in this example, the back-end optimization is Graph optimization, where Graph optimization is a way to represent the optimization problem as a Graph (Graph), where a Graph is composed of several vertices (vertexes) and edges (edges) connecting these nodes. Where the amount to be optimized (pose of the AGV) is represented by vertices and the constraint of the vertices (spatial constraint between two nodes) is represented by edges. The aim is to minimize the errors of prediction and observation by optimizing the pose of each node by constructing a graph.

In the local map construction of this example, not every map construction positioning pose is calculated, in order to save calculation resources, in this example, key frames are used for calculation, and when the mobile robot translates, for example, by more than 0.5m or rotates by more than 20 degrees, one key frame is selected.

And then, taking each positioning pose B of the local map building key frame of each timestamp of the local map as a vertex, taking the pose transformation relation between the positioning poses B of two adjacent key frames in the local map building as a bilateral, and optimizing to finally obtain a new optimized local map building positioning pose to construct a new local map.

Specifically, during the mapping process, the system records the positioning pose (vertex) of the key frame of the local mapping, the positioning pose transformation relationship (bilateral) between the adjacent key frames of the local mapping, and the scan point cloud information corresponding to the key frames. In the optimization process, the amount to be optimized is the positioning pose (vertex) of the key frame in the local map, and the constraint is bilateral. After the optimization is completed, the optimized vertexes and the laser scanning points corresponding to the vertexes can be obtained for the next local map construction.

In this example, the map used is a two-dimensional occupancy grid map. Firstly, an original grid map is constructed according to optimized vertexes, the map is divided into small grids according to the sizes of the grids, each grid in an original state is unknown, then the vertexes are placed into the map, the positions of scanning laser points of the vertexes are set to occupy (black), the middle grids for laser ray propagation are set to be free (white), all vertexes and corresponding laser scanning points are sequentially placed into the grid map, the grid map is updated according to the black and the white until all points are placed into the map, and the rasterized local map can be completed. The process is shown in fig. 5.

Step S400, performing time stamp alignment on the positioning pose A and the positioning pose B through linear interpolation compensation, and performing ICP alignment on the acquired track a and the track B to calculate a rotation translation matrix C so as to convert a new local map into a navigation map coordinate system according to the matrix C.

Specifically, after the local map is built, the problem of how to spell in the navigation map needs to be considered, and because the navigation map is different from the local map coordinate system, in order to obtain the coordinate conversion relationship between the local map and the navigation map, the positioning pose B in the local map and the positioning pose a in the navigation map need to be aligned by time stamps.

Wherein in the example, alignment compensates the pose by adopting a linear interpolation mode, and finally obtainsTwo tracks under the same time stamp, namely a track a corresponding to a positioning pose A in a navigation map and a track B corresponding to a positioning pose B in a local map, wherein points on the tracks correspond to each other one by one, and the principle of linear interpolation is shown in figure 3; assume that the navigation map at time t-1 is positioned in a pose

Is->

Positioning pose of navigation map at time t>

Is->

Positioning pose of local map at s moment +.>

For (+)>

) Satisfy->

Then according to the linear interpolation, the positioning pose of the navigation map at the s moment can be obtained

。

And then aligning the two tracks by adopting an ICP algorithm to obtain a translation and rotation matrix C of the two tracks, and transferring the local map to a navigation map coordinate system according to the matrix C.

Wherein, the time stamp alignment is already made on the result obtained by the linear interpolation, and at the moment, the positioning track points of the navigation map and the map-building positioning track points of the local map are in one-to-one correspondence, as shown in fig. 4, the right track and the points are positioning points on the navigation map, the left track and the points are positioning points on the local map-building, the points on the two tracks are in one-to-one correspondence, the rotation and translation transformation relationship between the two tracks is obtained through the ICP algorithm, namely, the matrix C is obtained,the core idea is that

Through rotation->

And translation->

So that->

Overlapping, and constructing an error function:

-(R*/>

-t)，

solving for errors that can be minimized

：

，

The above formula is a standard least squares expression.

Finally, step S500 is performed to replace the area to be updated with the new local map converted in step S400, so as to complete the local update of the navigation map.

On the other hand, in order to improve the overall map updating accuracy, it is necessary to judge whether the local map satisfies the deviation requirement before the map updating, so in the preferred embodiment, the map automatic updating method further includes, before step S500:

step S410, adopting root mean square error as a criterion for track alignment, and after the track alignment is assumed, collecting the positioning pose A points on the track a

And the set of locating pose B points on track B

Root mean square error calculation

Root mean square error

。

The smaller the RMSE at this time, the smaller the deviation of the two tracks, which in this example is preferably considered to be aligned better when the RMSE is smaller than 0.1m, to allow the step S500 to be performed.

On the other hand, as shown in fig. 6, the present invention also provides an automatic map updating system, corresponding to the above method example, which includes:

Further, in order to improve the overall map updating accuracy, the processing unit is further configured to: the set of the position pose A points on the track a

And the set of localization pose B points on track B +.>

Root mean square error calculation

，

When (when)

In another aspect, the present invention also provides a computer readable storage medium having a computer program stored thereon, corresponding to the above method, wherein the computer program when executed by a processor implements the steps of the map automatic updating method according to any one of the above.

In summary, by the automatic map updating method, the system and the storage medium provided by the invention, map scenes are skillfully divided into three parts: the static map, the semi-static map and the dynamic map are used for processing only the semi-static map area through analysis, and map updating is conducted in a specific area, so that unnecessary calculation amount in the moving process of the mobile robot is effectively reduced, and meanwhile, the updated area is smaller than the whole map, so that updating precision of the local map can be guaranteed to a certain extent under the reference of the original navigation map.

The preferred embodiments of the invention disclosed above are intended only to assist in the explanation of the invention. The preferred embodiments are not exhaustive or to limit the invention to the precise form disclosed. Obviously, many modifications and variations are possible in light of the above teaching. The embodiments were chosen and described in order to best explain the principles of the invention and the practical application, to thereby enable others skilled in the art to best understand and utilize the invention. The invention is to be limited only by the following claims and their full scope and equivalents, and any modifications, equivalents, improvements, etc., which fall within the spirit and principles of the invention are intended to be included within the scope of the invention.

It will be appreciated by those skilled in the art that the system, apparatus and their respective modules provided by the present invention may be implemented entirely by logic programming method steps, in the form of logic gates, switches, application specific integrated circuits, programmable logic controllers, embedded microcontrollers, etc., except for implementing the system, apparatus and their respective modules provided by the present invention in a purely computer readable program code. Therefore, the system, the apparatus, and the respective modules thereof provided by the present invention may be regarded as one hardware component, and the modules included therein for implementing various programs may also be regarded as structures within the hardware component; modules for implementing various functions may also be regarded as being either software programs for implementing the methods or structures within hardware components.

Furthermore, all or part of the steps in implementing the methods of the embodiments described above may be implemented by a program, where the program is stored in a storage medium and includes several instructions for causing a single-chip microcomputer, chip or processor (processor) to perform all or part of the steps in the methods described in the embodiments of the present application. And the aforementioned storage medium includes: a U-disk, a removable hard disk, a Read-Only Memory (ROM), a random access Memory (RAM, random Access Memory), a magnetic disk, or an optical disk, or other various media capable of storing program codes.

In addition, any combination of various embodiments of the present invention may be performed, so long as the concept of the embodiments of the present invention is not violated, and the disclosure of the embodiments of the present invention should also be considered.

Claims

1. An automatic map updating method is characterized by comprising the following steps:

step S400, performing time stamp alignment on each positioning pose A and each positioning pose B through linear interpolation compensation, and performing ICP alignment on an obtained track a and a track B to calculate a rotation translation matrix C so as to convert a new local map into a navigation map coordinate system according to the matrix C;

2. The automatic map updating method according to claim 1, characterized by further comprising, before step S500:

step S410, the set of the positioning pose A points on the track a is collected

And the set of localization pose B points on track B +.>

Root mean square error calculation

，

When judging

3. The automatic map updating method according to claim 1, wherein in step S300, each positioning pose B of a key frame in the local map is adopted as a vertex, a pose transformation relationship between positioning poses B of two adjacent key frames is adopted as a bilateral, and a map model is constructed to perform map optimization.

4. The automatic map updating method according to claim 3, wherein the key frame in step S500 is to record the current localization pose as localization pose B of the key frame in the local map when the mobile robot moves more than 0.5m or rotates more than 20 °.

5. An automatic map updating system, characterized by comprising:

a storage unit for storing a program comprising the steps of the map automatic updating method according to any one of claims 1 to 4, for the navigation unit, the data acquisition unit, the map building unit, the processing unit, and for timely retrieving and executing;

the processing unit is used for taking the keyframe positioning pose B as a vertex, taking the pose transformation relationship between two adjacent keyframe positioning poses B as a bilateral, constructing a graph model for graph optimization, and obtaining a local graph construction positioning pose;

6. The automatic map updating system of claim 5, wherein the processing unit is further configured to: the set of the position pose A points on the track a

And the set of localization pose B points on track B +.>

Root mean square error calculation

，

When (when)

7. A computer-readable storage medium, on which a computer program is stored, characterized in that the computer program, when being executed by a processor, implements the steps of the map automatic updating method of any one of claims 1 to 4.