A map optimization method, device and mobile robot

Abstract

The embodiments of the present application provide a map optimization method, device and mobile robot, which relate to the field of robot technology. The method is applied to a mobile robot equipped with a radar sensor and an image sensor, and includes: in the process of performing SLAM for an environmental scene based on an image sensor, constructing a first key frame set; constructing a first map point set based on the first key frame set; determining the auxiliary posture of each frame in the first key frame set; constructing a first factor graph based on the robot posture and auxiliary posture of each frame in the first key frame set, and the map points in the first map point set; inputting the first factor graph and the first optimization parameter into a graph optimizer to optimize the robot posture of each frame except the specified key frame, and the map points in the first map point set; and updating the currently constructed map using the optimized map points and the robot posture of each frame to improve the accuracy of the constructed map.

Description

Technical Field

[0001] The present application relates to the field of robotics technology, and in particular to a map optimization method, device and mobile robot. Background Art

[0002] Positioning and navigation are the main functions of autonomous mobile service robots (referred to as mobile robots), and Simultaneous Localization And Mapping (SLAM) technology is considered to be the necessary basis for achieving the above functions. The main principle of SLAM technology is to detect the surrounding environment through the sensors carried by the mobile robot, and build an environmental map while estimating the position and posture of the mobile robot.

[0003] In the related art, mobile robots usually use a single sensor to perform the above-mentioned SLAM technology. However, when using a single sensor, there are problems that cannot be overcome by itself. For example, in the process of SLAM based on image sensors, due to the limited field of view, it will be restricted by environmental factors and affect the accuracy of the constructed map.

[0004] Based on this, optimizing the map constructed by the mobile robot in the process of SLAM based on image sensors, thereby improving the accuracy of the map constructed by the mobile robot, has become a technical problem that urgently needs to be solved. Summary of the invention

[0005] The purpose of the embodiments of the present application is to provide a map optimization method, device and mobile robot, so as to optimize the map constructed by the mobile robot in the process of SLAM based on image sensors, thereby improving the accuracy of the map constructed by the mobile robot. The specific technical solution is as follows:

[0006] In a first aspect, an embodiment of the present application provides a map optimization method, which is applied to a mobile robot, wherein the mobile robot is equipped with a radar sensor and an image sensor, and the method includes:

[0007] In the process of performing SLAM for a target environment scene based on the image sensor, in response to constructing a key frame, a first key frame set is constructed; wherein the first key frame set includes a target key frame and a key frame in a co-viewing relationship with the target key frame among the currently existing key frames; the target key frame is the key frame currently constructed;

[0008] Based on the first key frame set, construct a first map point set; wherein the first map point set includes map points corresponding to each key frame in the first key frame set;

[0009] Determine the auxiliary posture corresponding to each key frame in the first key frame set; wherein the auxiliary posture corresponding to each key frame is: the robot posture at a time matching the acquisition time of the key frame obtained after robot positioning processing based on each frame of point cloud data collected by the radar sensor; each frame of point cloud data is data collected by the radar sensor during the target movement process of the mobile robot; the target movement process is the robot movement process required to be executed for the SLAM process;

[0010] Constructing a first factor graph for graph optimization processing based on the robot pose and the corresponding auxiliary pose of each key frame in the first key frame set, and the map points in the first map point set;

[0011] Inputting the first factor graph and the preset first optimization parameters into a predetermined graph optimizer to optimize the robot postures of each key frame in the first key frame set except for a specified key frame, and the map points in the first map point set; wherein the specified key frame is a key frame with the earliest acquisition time in the first key frame set;

[0012] The currently constructed map of the target environment scene is updated using the optimized map points and the robot poses of each key frame.

[0013] In a second aspect, an embodiment of the present application provides a map optimization device, which is applied to a mobile robot, wherein the mobile robot is equipped with a radar sensor and an image sensor, and the device includes:

[0014] A first construction module is used to construct a first key frame set in response to a key frame constructed during a SLAM process for a target environment scene based on the image sensor; wherein the first key frame set includes a target key frame and a key frame in a co-viewing relationship with the target key frame among the currently existing key frames; the target key frame is the key frame currently constructed;

[0015] A second construction module is used to construct a first map point set based on the first key frame set; wherein the first map point set contains map points corresponding to each key frame in the first key frame set;

[0016] A first determination module is used to determine the auxiliary posture corresponding to each key frame in the first key frame set; wherein the auxiliary posture corresponding to each key frame is: the robot posture at a time matching the acquisition time of the key frame obtained after robot positioning processing based on each frame of point cloud data collected by the radar sensor; each frame of point cloud data is data collected by the radar sensor during the target movement process of the mobile robot; the target movement process is the robot movement process required to be executed for the SLAM process;

[0017] a third construction module, configured to construct a first factor graph for graph optimization processing based on the robot pose and the corresponding auxiliary pose of each key frame in the first key frame set, and the map points in the first map point set;

[0018] A first optimization module, configured to input the first factor graph and a preset first optimization parameter into a predetermined graph optimizer, so as to optimize the robot posture of each key frame in the first key frame set except a specified key frame, and the map points in the first map point set; wherein the specified key frame is a key frame with the earliest acquisition time in the first key frame set;

[0019] The first updating module is used to update the currently constructed map of the target environment scene by using the optimized map points and the robot postures of the key frames.

[0020] In a third aspect, an embodiment of the present application provides a mobile robot, comprising:

[0021] Memory, used to store computer programs;

[0022] The processor is used to implement any of the map optimization methods when executing the program stored in the memory.

[0023] In a fourth aspect, an embodiment of the present application provides a computer-readable storage medium, wherein the computer-readable storage medium stores a computer program, and when the computer program is executed by a processor, any of the map optimization methods described above is implemented.

[0024] An embodiment of the present application also provides a computer program product comprising instructions, which, when executed on a computer, enables the computer to execute any of the map optimization methods described above.

[0025] Beneficial effects of the embodiments of the present application:

[0026] As can be seen from the above, a map optimization method provided in an embodiment of the present application is applied to a mobile robot, which is equipped with a radar sensor and an image sensor. When the mobile robot optimizes the map of the target environment scene, it moves according to the target movement process of the robot required to perform the SLAM process, and in the process of the mobile robot moving according to the above target movement process, the image sensor carried performs SLAM for the target environment scene, and at the same time, the radar sensor carried collects each frame of point cloud data for the target environment scene, so that the subsequent mobile robot will use the robot positioning processing based on each frame of point cloud data, which is obtained at a time matching the acquisition time of each key frame, as the auxiliary posture corresponding to the key frame.

[0027] After the mobile robot responds to the key frame being constructed, it can construct a first key frame set based on the target key frame currently constructed and the key frame in the currently existing key frames that has a co-view relationship with the target key frame, and after constructing the first map point set based on the map points corresponding to each key frame in the first key frame set, determine the auxiliary posture corresponding to each key frame in the first key frame set, thereby constructing a first factor graph for graph optimization based on the robot posture and the corresponding auxiliary posture of each key frame in the first key frame set and the map points in the first map point set. In this way, the mobile robot inputs the constructed first factor graph and the preset first optimization parameters into a predetermined graph optimizer to optimize the robot posture of each key frame in the first key frame set except the designated key frame with the earliest acquisition time and the map points in the first map point set by using the auxiliary posture corresponding to each key frame in the first key frame set, thereby updating the currently constructed map of the target environment scene by using the optimized map points and the robot posture of each key frame.

[0028] Based on this, the solution provided in the embodiment of the present application is applied. In the process of SLAM based on image sensors, the mobile robot uses the radar posture collected by the radar sensor as the auxiliary posture of the mobile robot for map optimization. Thereby, the robot posture of each key frame and the corresponding auxiliary posture are jointly optimized to improve the accuracy of the robot posture and map points of each key frame after optimization, and further improve the accuracy of the map constructed by the mobile robot. BRIEF DESCRIPTION OF THE DRAWINGS

[0029] In order to more clearly illustrate the embodiments of the present application or the technical solutions in the prior art, the drawings required for use in the embodiments or the description of the prior art will be briefly introduced below. Obviously, the drawings described below are only some embodiments of the present application, and for ordinary technicians in this field, other embodiments can also be obtained based on these drawings.

[0030] Figure 1 A schematic diagram of the process of the first map optimization method provided in the embodiment of the present application;

[0031] Figure 2 is a schematic diagram of a reprojection error;

[0032] Figure 3 A schematic diagram of vision-radar fusion positioning provided in an embodiment of the present application;

[0033] Figure 4 A schematic diagram of a factor graph provided in an embodiment of the present application;

[0034] Figure 5 A schematic diagram of a flow chart of a second map optimization method provided in an embodiment of the present application;

[0035] Figure 6 It is a flowchart of the fusion positioning method corresponding to different frameworks;

[0036] Figure 7 A schematic diagram of a flow chart of a fusion positioning method corresponding to a semi-tightly coupled framework structure provided in an embodiment of the present application;

[0037] Figure 8 A schematic diagram of a semi-tightly coupled frame structure provided in an embodiment of the present application;

[0038] Fig. 9 A schematic diagram of a fusion positioning process provided in an embodiment of the present application;

[0039] Fig.10 A schematic diagram of the structure of a map optimization device provided in an embodiment of the present application;

[0040] Fig.11 A schematic diagram of the structure of a mobile robot provided in an embodiment of the present application. DETAILED DESCRIPTION

[0041] The following will be combined with the drawings in the embodiments of the present application to clearly and completely describe the technical solutions in the embodiments of the present application. Obviously, the described embodiments are only part of the embodiments of the present application, not all of the embodiments. Based on the embodiments in the present application, all other embodiments obtained by ordinary technicians in this field based on the present application belong to the scope of protection of the present application.

[0042] Below, the professional terms involved in the embodiments of the present application are first introduced:

[0043] SLAM mapping: The process by which a mobile robot uses sensors to build a map of its environment and simultaneously uses that map to infer its position.

[0044] BA (Bundle Adjustment): An optimization technique widely used in computer vision and photogrammetry. It is mainly used to adjust the pose of the image sensor (camera sensor) carried by the mobile robot and the position of the points in the 3D scene to minimize the reprojection error. In other words, the robot pose and map points are put together for optimization and adjustment.

[0045] KF (KeyFrame): A filtered image frame with relevant attribute information such as robot posture and map points.

[0046] ORB-SLAM3: An advanced open source visual simultaneous localization and mapping (SLAM) system capable of handling visual, visual-inertial, and multi-map SLAM for monocular, binocular, and RGB-D (Red Green Blue-Depth Map) cameras.

[0047] Factor graph: A graphical model for handling a large number of variables and constraints, which is widely used in state estimation and perception problems in fields such as robotics. In SLAM, factor graphs are used to describe the position and map point information of a robot in an unknown environment.

[0048] Graph optimization (processing): A state estimation method that represents the variables to be optimized and the observation constraints between the variables as a graph consisting of nodes and edges (i.e., the factor graph in the embodiment of the present application), and then uses numerical iteration to solve the variables to be optimized; wherein, the nodes represent variables, and during the optimization process, it can be chosen whether to adjust them, and if not, the parameters can be fixed; the edges represent residuals, and can be multivariate edges, connecting multiple variables.

[0049] For mobile robots, their main functions are positioning and navigation, and SLAM technology is the necessary basis for realizing the above functions. Among them, the main principle of SLAM technology is to detect the surrounding environment through the sensors carried by the mobile robot, and build an environmental map while estimating the position and posture of the mobile robot. However, in the related technology, mobile robots usually use a single sensor to perform the above SLAM technology. However, when using a single sensor, there are problems that cannot be overcome by themselves. For example, in the process of SLAM based on image sensors, due to the limited field of view, it will be restricted by environmental factors, and in the absence of loop detection correction, long-term positioning will lead to large cumulative errors in the positioning results, affecting the accuracy of the constructed map.

[0050] Based on this, optimizing the map constructed by the mobile robot in the process of SLAM based on image sensors, thereby improving the accuracy of the map constructed by the mobile robot, has become a technical problem that urgently needs to be solved.

[0051] In order to solve the above technical problems, the embodiments of the present application provide a map optimization method, device and mobile robot.

[0052] Among them, the method can be applied to various application scenarios that require map optimization, for example, in a factory, the map of the workshop is optimized for inspection; in a home scenario, the map of the room that a home cleaning robot can enter is optimized, etc. In addition, the method can be applied to a mobile robot, for example, a map optimization device is set on the mobile robot to execute the method using the map optimization device; it can also be applied to other electronic devices that communicate with the mobile robot. The embodiment of the present application does not specifically limit the application scenario and execution subject of the method. For the sake of clarity, the execution subject of this solution will be collectively referred to as a mobile robot in the following.

[0053] A map optimization method provided in an embodiment of the present application is applied to a mobile robot equipped with a radar sensor and an image sensor. The method may include the following steps:

[0054] In the process of performing SLAM for a target environment scene based on the image sensor, in response to constructing a key frame, a first key frame set is constructed; wherein the first key frame set includes a target key frame and a key frame in a co-viewing relationship with the target key frame among the currently existing key frames; the target key frame is the key frame currently constructed;

[0055] Based on the first key frame set, construct a first map point set; wherein the first map point set includes map points corresponding to each key frame in the first key frame set;

[0056] Determine the auxiliary posture corresponding to each key frame in the first key frame set; wherein the auxiliary posture corresponding to each key frame is: the robot posture at a time matching the acquisition time of the key frame obtained after robot positioning processing based on each frame of point cloud data collected by the radar sensor; each frame of point cloud data is data collected by the radar sensor during the target movement process of the mobile robot; the target movement process is the robot movement process required to be executed for the SLAM process;

[0057] Constructing a first factor graph for graph optimization processing based on the robot pose and the corresponding auxiliary pose of each key frame in the first key frame set, and the map points in the first map point set;

[0058] Inputting the first factor graph and the preset first optimization parameters into a predetermined graph optimizer to optimize the robot postures of each key frame in the first key frame set except for a specified key frame, and the map points in the first map point set; wherein the specified key frame is a key frame with the earliest acquisition time in the first key frame set;

[0059] The currently constructed map of the target environment scene is updated using the optimized map points and the robot poses of each key frame.

[0060] As can be seen from the above, a map optimization method provided by an embodiment of the present application is applied to a mobile robot, which is equipped with a radar sensor and an image sensor. When the mobile robot optimizes the map of the target environment scene, it moves according to the target movement process of the robot required to perform the SLAM process, and in the process of the mobile robot moving according to the above target movement process, the image sensor carried performs SLAM for the target environment scene, and at the same time, the radar sensor carried collects each frame of point cloud data for the target environment scene, so that the subsequent mobile robot will perform robot positioning processing based on each frame of point cloud data collected by the radar sensor, and the robot posture at the time matching the collection time of each key frame is used as the auxiliary posture corresponding to the key frame.

[0061] After the mobile robot responds to the key frame being constructed, it can construct a first key frame set based on the target key frame currently constructed and the key frame in the currently existing key frames that has a co-view relationship with the target key frame, and after constructing the first map point set based on the map points corresponding to each key frame in the first key frame set, determine the auxiliary posture corresponding to each key frame in the first key frame set, thereby constructing a first factor graph for graph optimization based on the robot posture and the corresponding auxiliary posture of each key frame in the first key frame set and the map points in the first map point set. In this way, the mobile robot inputs the constructed first factor graph and the preset first optimization parameters into a predetermined graph optimizer to optimize the robot posture of each key frame in the first key frame set except the designated key frame with the earliest acquisition time and the map points in the first map point set by using the auxiliary posture corresponding to each key frame in the first key frame set, thereby updating the currently constructed map of the target environment scene by using the optimized map points and the robot posture of each key frame.

[0062] Based on this, the solution provided in the embodiment of the present application is applied. In the process of SLAM based on image sensors, the mobile robot uses the radar posture collected by the radar sensor as the auxiliary posture of the mobile robot for map optimization. Thereby, the robot posture of each key frame and the corresponding auxiliary posture are jointly optimized to improve the accuracy of the robot posture and map points of each key frame after optimization, and further improve the accuracy of the map constructed by the mobile robot.

[0063] A map optimization method provided in an embodiment of the present application is described in detail below in conjunction with the accompanying drawings.

[0064] Figure 1 A flowchart of a map optimization method provided in an embodiment of the present application is provided. The method is applied to a mobile robot, and the mobile robot is equipped with a radar sensor and an image sensor, such as Figure 1 As shown, the method may include the following steps:

[0065] S101: In a process of performing SLAM for a target environment scene based on an image sensor, in response to constructing a key frame, constructing a first key frame set;

[0066] The first key frame set includes a target key frame and key frames that have a co-viewing relationship with the target key frame among the currently existing key frames; the target key frame is a key frame that is currently constructed.

[0067] In the present application, the mobile robot can perform a SLAM process for the target environment scene based on an image sensor. During the SLAM process, the mobile robot can optimize the map of the target environment scene. That is, the map optimization process provided by the present application can be implemented during the SLAM process for the target environment scene. Thus, in the above process, the mobile robot responds to the key frame obtained by construction, determines the key frame currently constructed as the target key frame, and determines the key frame that has a co-viewing relationship with the above target key frame among the currently existing key frames.

[0068] The so-called co-viewing relationship refers to the association relationship formed between different keyframes because of the common observation of the same feature points. This association relationship is the basis for posture determination and map construction in SLAM technology. In other words, a keyframe that has a co-viewing relationship with a target keyframe refers to a keyframe that can observe the same feature points with the target keyframe. For example, if the target keyframe contains feature point l, then the keyframe that also contains feature point l is a keyframe that has a co-viewing relationship with the target keyframe.

[0069] In this way, the first key frame set is constructed using the determined target key frame and the key frame in the currently existing key frames that has a co-viewing relationship with the target key frame. It should be emphasized that the method of constructing key frames in the process of performing SLAM for the target environment scene based on the image sensor, and the method of determining the key frame in the currently existing key frames that has a co-viewing relationship with the target key frame, are not the invention points of the present application, and therefore are not limited, and any implementation method can be applied to the present application.

[0070] S102: constructing a first map point set based on the first key frame set;

[0071] The first map point set includes map points corresponding to each key frame in the first key frame set.

[0072] Generally, in the SLAM process, map points are the basic elements for constructing a map of the environment scene, that is, by continuously adding and updating map points, the mobile robot can gradually construct a map of the target environment scene. It can be understood that map points can be constructed through key frames or ordinary frames, but map points need to correspond to key frames.

[0073] The purpose of this application is to perform map optimization. The so-called map optimization can be considered as the optimization of map points and the robot posture of the mobile robot. Therefore, after determining the first key frame set, a map point set can be constructed based on the first key frame set to obtain the first map point set used for map optimization.

[0074] S103: Determine the auxiliary posture corresponding to each key frame in the first key frame set;

[0075] Among them, the auxiliary posture corresponding to each key frame is: the robot posture at a time matching the acquisition time of the key frame, obtained after robot positioning processing based on each frame of point cloud data collected by the radar sensor; each frame of point cloud data is the data collected by the radar sensor during the target movement process of the mobile robot; the target movement process is the robot movement process required to perform the SLAM process.

[0076] In this application, when the mobile robot moves according to the target movement process, the radar sensor it carries collects point cloud data for the target environment scene at the same time. In this way, the mobile robot can perform robot positioning processing based on each frame of point cloud data collected by the radar sensor to obtain each robot posture, wherein, in order to distinguish it from the robot posture determined based on the data collected by the image sensor, the robot posture determined based on the data collected by the radar sensor is called the radar posture.

[0077] Considering that the frame rate of the radar sensor when collecting data is different from that of the image sensor when collecting data, and the radar sensor and the image sensor belong to different processes of SLAM, that is, when the radar sensor and the image sensor collect data, the collection time of the point cloud data and image frames collected for the same area is not strictly aligned, thus, the collection time of the radar pose and the collection time of the key frame are not strictly aligned.

[0078] Therefore, after determining the first key frame set, for the robot posture obtained based on the positioning of the radar sensor, according to the acquisition time of each key frame in the first key frame set, the robot posture at the time matching the acquisition time of the key frame is determined as the auxiliary posture of the key frame.

[0079] Optionally, in one implementation, the acquisition time of the robot posture corresponding to each frame of point cloud data acquired by the radar sensor is determined as the first timestamp; the acquisition time of each key frame acquired by the image sensor is determined as the second timestamp. In this way, when determining the auxiliary posture corresponding to each key frame in the first key frame set, for each key frame, a timestamp matching the second timestamp of the key frame is selected from each first timestamp. For example, the first timestamp with the smallest time difference with the second timestamp, so that the robot posture with the selected first timestamp is used as the auxiliary posture of the key frame.

[0080] Optionally, in another implementation, each radar pose may be interpolated according to the timestamp of the key frame collected by the image sensor and the timestamp corresponding to each radar pose, so that each radar pose formed after interpolation may have: a radar pose with a timestamp that is the same as the timestamp of the key frame, or a time difference within a predetermined time range; in this way, the interpolation alignment of each radar pose with the key frame collected by the image sensor may be achieved to generate an interpolated pose, so that each key frame may correspond to an auxiliary pose, that is, a radar pose with the same timestamp or a time difference within a predetermined time range.

[0081] S104: constructing a first factor graph for graph optimization processing based on the robot posture and the corresponding auxiliary posture of each key frame in the first key frame set, and the map points in the first map point set;

[0082] S105: inputting the first factor graph and the preset first optimization parameters into a predetermined graph optimizer to optimize the robot poses of each key frame in the first key frame set except the specified key frame, and the map points in the first map point set;

[0083] The designated key frame is the key frame with the earliest acquisition time in the first key frame set.

[0084] Considering that visual information can use epipolar geometry to constrain the pose transformation between image frames, in this application, by introducing auxiliary poses, joint pose optimization is performed with the robot pose of the key frame itself, and at the same time, the map points corresponding to the key frame are optimized to eliminate the cumulative pose errors generated by the image sensor during long-term positioning. Graph optimization is a state estimation method that represents the variables to be optimized and the observation constraints between the variables as a graph composed of nodes and edges, and then uses numerical iteration to solve the variables to be optimized. In other words, the graph optimization process can be understood as using the fixed parameters in the constructed factor graph to adjust the non-fixed parameters to achieve the optimization process of the non-fixed parameters.

[0085] Therefore, the robot posture of each key frame in the first key frame set, the auxiliary posture corresponding to each key frame, and the map points in the first map node set can be used as parameters for constructing the factor graph, thereby constructing the first factor graph for graph optimization processing. Then, the constructed first factor graph and the first optimization parameter for the mobile robot in this optimization are input into the graph optimizer for processing the factor graph that is predetermined according to the user's own needs, so as to optimize the robot posture of each key frame in the first key frame set except the specified key frame and the map points in the first map point set through the auxiliary posture corresponding to each key frame in the first key frame set.

[0086] Optionally, in one implementation, the first optimization parameter includes: a first type of information used to characterize the optimization target and a second type of information characterizing the solution method; wherein the optimization target is to perform a joint adjustment on the visual residuals related to the image sensor and the posture residuals related to the radar sensor to optimize the robot posture.

[0087] In this implementation, when the graph optimizer is used to tune the parameters that need to be tuned in the input factor graph, the first type of information used to characterize the optimization target and the second type of information characterizing the solution method can be set in advance according to the needs of the graph optimizer. That is, the graph optimizer can determine the target of joint adjustment for the visual residuals related to the image sensor and the pose residuals related to the radar sensor through the first type of information. At the same time, the graph optimizer can determine which solution method to use for joint pose optimization to complete the solution through the second type of information.

[0088] Among them, the above-mentioned solution method can be solved by using algorithms such as Gauss Newton (GN) and Levenberg Marquadt (LM), and this embodiment of the present application does not specifically limit this. The above-mentioned algorithms are usually encapsulated in open source algorithm libraries such as G2O (General Graphic Optimization, a library based on graph optimization) and Ceres (CeresSolver, a C++ library for solving optimization problems). Exemplarily, the embodiment of the present application adopts the G2O library based on graph optimization theory.

[0089] Optionally, in one implementation, the first type of information used to characterize the optimization target is characterized by the following optimization problem:

[0090] ;

[0091] in, is the matrix corresponding to the robot posture of each key frame except the specified key frame after optimization. is the matrix corresponding to the robot posture of each key frame to be optimized except the specified key frame, The matrix representing the visual residual associated with the image sensor, The matrix representing the pose residual associated with the radar sensor, The robot poses of each keyframe except the specified keyframe in order to minimize the visual residual associated with the image sensor and the pose residual associated with the radar sensor.

[0092] In addition, the above Characterization The square of the magnitude of the corresponding vector, Characterization The square of the magnitude of the corresponding vector.

[0093] The matrix corresponding to the visual residuals associated with the image sensor refers to: a matrix obtained by combining the visual residuals corresponding to each key frame except the specified key frame collected by the image sensor; the matrix corresponding to the pose residuals associated with the radar sensor refers to: a matrix obtained by combining the pose residuals corresponding to each key frame except the specified key frame collected by the radar sensor.

[0094] For ease of reading, the specific implementation of the above steps S104 and S105 will be combined with the specific implementation of the following steps A1-A4, and Figure 2-Figure 4 This is just an example and I won’t go into details here.

[0095] S106: Using the optimized map points and the robot poses of the key frames, the currently constructed map of the target environment scene is updated.

[0096] In the present application, after determining the robot posture of each optimized map point and each key frame, the mobile robot can use the optimized map points to update the original map points corresponding to each map point in the currently constructed map, and use the robot posture of each key frame to update the original robot posture of each key frame in the currently constructed map, thereby achieving the update of the currently constructed map of the target environment scene and the optimization of the currently constructed map.

[0097] Considering that the key frames corresponding to each optimized map point are determined based on the co-viewing relationship between each key frame, and the so-called co-viewing relationship refers to the association relationship formed between different key frames because the same feature point is observed together, that is, the area that can be observed by each key frame with a co-viewing relationship, or the feature points that can be observed, have a certain degree of repetition, which makes the redundancy for the same map point in each optimized key frame higher, and then leads to an increase in the number of redundant map points and the number of corresponding key frames in the updated currently constructed map. Therefore, in order to streamline the map data of the updated currently constructed map, it is optional to determine each candidate key frame with a co-viewing relationship after optimization, and the image features corresponding to each candidate key frame, and remove at least one candidate key frame whose image feature matching rate reaches a predetermined matching rate, so as to reduce the redundancy for the same map point in each optimized key frame, and realize data streamlining of the map data of the updated currently constructed map.

[0098] As can be seen from the above, a map optimization method provided by an embodiment of the present application is applied to a mobile robot, which is equipped with a radar sensor and an image sensor. When the mobile robot optimizes the map of the target environment scene, it moves according to the target movement process of the robot required to perform the SLAM process, and in the process of the mobile robot moving according to the above target movement process, the image sensor carried performs SLAM for the target environment scene, and at the same time, the radar sensor carried collects each frame of point cloud data for the target environment scene, so that the subsequent mobile robot will perform robot positioning processing based on each frame of point cloud data collected by the radar sensor, and the robot posture at the time matching the collection time of each key frame is used as the auxiliary posture corresponding to the key frame.

[0099] After the mobile robot responds to the key frame being constructed, it can construct a first key frame set based on the target key frame currently constructed and the key frame in the currently existing key frames that has a co-view relationship with the target key frame, and after constructing the first map point set based on the map points corresponding to each key frame in the first key frame set, determine the auxiliary posture corresponding to each key frame in the first key frame set, thereby constructing a first factor graph for graph optimization based on the robot posture and the corresponding auxiliary posture of each key frame in the first key frame set and the map points in the first map point set. In this way, the mobile robot inputs the constructed first factor graph and the preset first optimization parameters into a predetermined graph optimizer to optimize the robot posture of each key frame in the first key frame set except the designated key frame with the earliest acquisition time and the map points in the first map point set by using the auxiliary posture corresponding to each key frame in the first key frame set, thereby updating the currently constructed map of the target environment scene by using the optimized map points and the robot posture of each key frame.

[0100] Based on this, the solution provided in the embodiment of the present application is applied. In the process of SLAM based on image sensors, the mobile robot uses the radar posture collected by the radar sensor as the auxiliary posture of the mobile robot for map optimization. Thereby, the robot posture of each key frame and the corresponding auxiliary posture are jointly optimized to improve the accuracy of the robot posture and map points of each key frame after optimization, and further improve the accuracy of the map constructed by the mobile robot.

[0101] It should be noted that a map optimization method provided in the embodiment of the present application can be called a map optimization method under a semi-tightly coupled framework structure. Compared with the tightly coupled and loosely coupled framework structures in the related art, in the semi-tightly coupled framework structure provided in the embodiment of the present application, the radar sensor is used as a separate module, so that the mobile robot does not need to strictly rely on the data synchronization accuracy by setting the independence of the radar sensor, and can improve the positioning accuracy of the fusion positioning by fusing the data of the image sensor and the radar sensor, so as to improve the visual positioning function of the mobile robot when the mobile robot has poor data synchronization accuracy, low radar frame rate, and fluctuations in local positioning accuracy, thereby improving the accuracy of the map constructed by the mobile robot. Among them, the detailed introduction of each framework structure will be specifically described in the subsequent embodiments and will not be described in detail here.

[0102] Optionally, in one implementation, the above step S104, based on the robot posture and the corresponding auxiliary posture of each key frame in the first key frame set, and the map points in the first map point set, constructing a first factor graph for graph optimization processing may include the following steps:

[0103] Step A: constructing a first factor graph for graph optimization processing according to a predetermined first construction method;

[0104] The first construction method includes:

[0105] Step A1: The robot posture of each key frame in the first key frame set is used as a first-type vertex in the first factor graph to be constructed, and the vertex representing the robot posture of the specified key frame in each first-type vertex is set as a fixed attribute and the other vertices are set as non-fixed attributes;

[0106] Step A2: taking the auxiliary posture corresponding to each key frame in the first key frame set as the second type of vertex in the first factor graph to be constructed, and setting each of the constructed second type of vertices as a fixed attribute;

[0107] Step A3: each map point in the first map point set is used as a third-type vertex in the first factor graph to be constructed, and each constructed third-type vertex is set as a non-fixed attribute;

[0108] Step A4: for each third-type vertex in the first factor graph to be constructed, using the visual residual corresponding to the key frame related to the third-type vertex as the edge value, setting the edge between the third-type vertex and the first-type vertex representing the robot posture of the related key frame; for each second-type vertex in the first factor graph to be constructed, using the posture residual about the second-type vertex and the related first-type vertex about the radar as the edge value, setting the edge between the second-type vertex and the related first-type vertex;

[0109] Among them, the key frame related to the third type of vertex is the key frame corresponding to the map point represented by the third type of vertex; the first type of vertex related to the second type of vertex is the robot posture possessed by the key frame corresponding to the auxiliary posture represented by the second type of vertex.

[0110] In this implementation, it is considered that in the process of graph optimization, fixed parameters are used to adjust non-fixed parameters, and the variables to be optimized in this application are the robot posture of the key frame itself and the map point corresponding to the key frame. Therefore, when constructing the first factor graph for graph optimization processing, corresponding attribute features for characterizing whether to adjust parameters can be set for the robot posture and corresponding auxiliary posture of each key frame in the first key frame set, and the map points in the first map point set, specifically:

[0111] Since the robot posture of the key frame needs to be adjusted, the robot posture of each key frame in the first key frame set can be used as the first type of vertex in the first factor graph to be constructed. In addition, considering that when adjusting the parameters, the robot postures of other robots that have a co-viewing relationship with the initial robot posture are adjusted based on an initial robot posture, which is the so-called posture propagation. Therefore, the key frame with the earliest acquisition time in the first key frame set can be determined as the designated key frame, and the robot posture of the designated key frame can be used as the initial robot posture, that is, the robot posture of the designated key frame is fixed, so that the vertices in each first type of vertex that represent the robot posture of the designated key frame are set as fixed attributes, and the vertices in each first type of vertex that represent the robot posture of other key frames are set as non-fixed attributes.

[0112] Then, the auxiliary posture corresponding to each key frame in the first key frame set is used as the second type of vertex in the first factor graph to be constructed. In addition, considering that the purpose of introducing the auxiliary posture is to optimize the robot posture of the key frame using the auxiliary posture, that is, the introduced auxiliary posture is fixed. Therefore, each constructed second type of vertex is set as a fixed attribute.

[0113] Afterwards, each map point in the first map point set is used as the third type of location in the first factor graph to be constructed. Since in the process of graph optimization processing of the present application, the variables to be optimized also include map points corresponding to key frames, that is, each map point in the first map point set needs to be parameterized to achieve optimization, therefore, each third type of vertex constructed can be set as a non-fixed attribute.

[0114] Furthermore, considering that the complete factor graph contains not only vertices (nodes) representing variables, but also edges representing residuals. Therefore, after determining various types of vertices, it is necessary to further determine the residual corresponding to the keyframe associated with each vertex as the value of the edge, so as to set the edges between vertices based on the determined value. Specifically:

[0115] (1) For each third-category vertex in the first factor graph to be constructed, determine the visual residual of the key frame corresponding to the map point represented by the third-category vertex as the value of the edge, and then, based on the determined value, set an edge between the third-category vertex and the first-category vertex representing the robot posture of the related key frame.

[0116] The above-mentioned visual residual can also be called visual constraint residual or visual constraint. For the model of image sensors such as micro cameras, an optimization problem can be constructed to solve the camera pose to obtain the robot pose of a mobile robot equipped with the camera. Visual constraints are usually solved by optimization methods such as bundle adjustment using reprojection errors. The essence of the above-mentioned optimization problem is a least squares problem, and accordingly, the optimization goal is to minimize the reprojection error.

[0117] For example, Figure 2 is a schematic diagram of a reprojection error. Figure 2 As shown, It is a map point Located in the image A pair of matching points, and The corresponding projection point The projection error between , and The optical center of the camera is located in the image For a map with n points The image frame, the corresponding plane projection point is , the pixel coordinates are , the camera pose matrix The corresponding Lie algebra is , the projection error can be defined as:

[0118] ;

[0119] The matrix corresponding to the projection error, that is, the matrix corresponding to the visual residual associated with the image sensor in the embodiment of the present application, Represents the number of map points corresponding to the keyframe, Characterization map points, Characterize the intrinsic parameter matrix of the camera, Characterization The depth value corresponding to the pixel point, that is, The component of the map point corresponding to the pixel point in the z-axis direction in the three-dimensional coordinate system, It is the transposed matrix corresponding to the three-dimensional coordinates of the camera pose in the world coordinate system, P'=[X', Y', Z'] T is the transposed matrix corresponding to the three-dimensional coordinates in the camera coordinate system, Characterize the camera pose, The translation vector representing the camera pose in the three-dimensional coordinate system, It refers to a three-dimensional Euclidean transformation and is a representation of the transformation matrix.

[0120] Among them, by constructing the least squares problem to solve , solve The key is to get the Jacobian matrix form of the projection error with respect to the state quantity (camera pose). The above Jacobian matrix is ​​as follows:

[0121] ;

[0122] Above are the intrinsic parameters of the camera. Express A disturbance quantity.

[0123] (2) For each second-class vertex in the first factor graph to be constructed, determine the vertex of the robot posture of the key frame corresponding to the auxiliary posture represented by the second-class vertex as the first-class vertex related to the second-class vertex, and then use the posture residual about the radar of the second-class vertex and the related first-class vertex as the value of the edge, so that, based on the determined value, set the edge between the second-class vertex and the related first-class vertex.

[0124] The above-mentioned radar pose residual can also be called radar constraint residual or radar constraint. Next, radar pose and is the robot pose generated by interpolation alignment. After the external parameter change, the radar pose can be transformed into the camera coordinate system. At the same time, assuming that the two poses differ by a transformation , recorded as the observation value, in an ideal state, the observation value is the unit matrix. Therefore, the radar constraint residual can be defined as the following formula:

[0125] ;

[0126] in, Represents the matrix corresponding to the radar's pose residual, Represents the inverse of the radar pose in the world coordinate system, represents the inverse of the radar pose in the world coordinate system, represents the radar pose in the world coordinate system. Then, and Adding perturbation derivatives, we can get the Jacobian matrix to solve , where the Jacobian formula is as follows:

[0127] ;

[0128] Representation to find the adjoint matrix, Represents the radar pose in the world coordinate system, Characterizing radar pose and The pose conversion value between the poses of Characterization using radar pose and The pose transformation value between them performs the inverse matrix transformation on the inverse of the radar pose in the world coordinate system.

[0129] It should be noted that the above-mentioned calculation process for the visual residual and the radar residual is only an example provided in the present application, and any calculation method for calculating the visual residual and the radar residual existing in the related art can be applied to the present application.

[0130] Optionally, in an embodiment, a map optimization method provided in an embodiment of the present application may further include the following steps:

[0131] Step B: constructing the key frames in the existing key frames that correspond to the map points in the first map point set and do not belong to the first key frame set into a second key frame set;

[0132] The predetermined first construction method in the above step A also includes:

[0133] Step C: Taking the robot posture of each key frame in the second key frame set as a first type of vertex with fixed attributes in the first factor graph to be constructed.

[0134] In this embodiment, after determining each map point in the first map point set as a third-type vertex in the first factor graph to be constructed, and each constructed third-type vertex is set as a non-fixed attribute, the mobile robot can construct the key frames in the existing key frames that correspond to the map points in the first map point set and do not belong to the first key frame set as a second key frame set. Therefore, when constructing the first factor graph to be constructed, the robot posture of each key frame in the second key frame set is used as the first-type vertex with fixed attributes in the first factor graph to be constructed.

[0135] In this way, through the co-viewing relationship between the key frames, the robot posture of the key frames in the existing key frames that correspond to the map points in the first map point set and do not belong to the first key frame set is determined, providing robot posture reference data with a wider range and more data volume for the joint posture optimization process, thereby improving the accuracy of the robot posture and map points of each key frame after optimization.

[0136] For example, Figure 3 As shown, it is a schematic diagram of a visual-radar fusion positioning provided in an embodiment of the present application; wherein, the camera is the image sensor in the embodiment of the present application, the visual constraint is the visual residual associated with the camera in the embodiment of the present application, and the radar constraint is the posture residual associated with the radar in the embodiment of the present application.

[0137] Generally, visual information can be used to constrain the pose transformation between image frames using epipolar geometry. Furthermore, in order to eliminate the cumulative error of the camera pose, the pose information of radar positioning is introduced in the embodiment of the present application to perform joint pose optimization. The optimization variable is determined as , the optimization problem is defined as:

[0138] ;

[0139] in, The matrix corresponding to the variables to be estimated for the joint adjustment of the visual residuals related to the camera and the pose residuals related to the radar is the matrix corresponding to the robot pose of each key frame other than the specified key frame after optimization in the embodiment of the present application. is the matrix corresponding to the robot posture of each key frame to be optimized except the specified key frame, The matrix representing the visual residual associated with the camera, The matrix representing the pose residual associated with the radar, In order to minimize the visual residual associated with the camera and the pose residual associated with the radar, the robot poses of each keyframe other than the specified keyframe are: Characterization The square of the magnitude of the corresponding vector, Characterization The square of the magnitude of the corresponding vector.

[0140] Through the above formula, the joint pose optimization problem to be performed is converted into a general least squares optimization problem. The solution to the least squares optimization problem is already very mature, and is generally solved by methods such as Gauss-Newton, Levenberg-Marquardt, etc., which are encapsulated in open source algorithm libraries such as G2O and Ceres. In this embodiment, the G2O library based on graph optimization theory is used to optimize the variables to be estimated.

[0141] However, using graph optimization theory to optimize the variables to be estimated requires constructing a factor graph, which consists of edges and nodes (various vertices in the embodiments of the present application). Nodes represent variables, and you can choose whether to adjust them during the optimization process. If you don't adjust them, you can fix the parameters. Edges represent residuals, which can be multivariate edges, connecting multiple variables.

[0142] For example, Figure 4 A schematic diagram of a factor graph provided in an embodiment of the present application. Among them, the small black dots represent the third-type vertices constructed by using each map point in the first map point set; the dark dots with the word radar represent the second-type vertices constructed by using the auxiliary posture corresponding to each key frame in the first key frame set; the light-colored dots with the word vision represent the first-type vertices constructed by using the robot posture of each key frame in the first key frame set; the dark squares represent the edges between the third-type vertices and the second-type vertices, which are determined by the visual reprojection residual (visual residual) corresponding to the key frame related to the third-type vertex as the value of the edge; the white squares represent the edges between the second-type vertices and the first-type vertices, which are determined by the radar residual about the second-type vertex and the related first-type vertex as the value of the edge, and the vertices connected by white trapezoids represent that the vertices are fixed.

[0143] When the mobile robot performs local map optimization, it needs to first build a local map. All key frames and corresponding map points that have a co-visual relationship with the current key frame participate in the optimization. At the same time, the visual posture vertices (first-class vertices) of the local map that are far away from the current frame will be fixed, that is, the robot postures of each key frame in the second key frame set in the embodiment of the present application will be fixed. Figure 4 In the factor graph shown, a radar pose vertex (second-type vertex) is added, which is a fixed vertex. A binary edge of radar and visual constraints is added, one end of which connects the visual pose vertex and the other end connects the radar fixed pose vertex (second-type vertex). Among them, the visual residual edge is a binary edge, connecting the third-type vertex and the first-type vertex. Optimizing and adjusting the camera pose can avoid the cumulative drift caused by the visual pose.

[0144] It should be noted that global map optimization is a special case of local map optimization. Its triggering condition is that when the mobile robot detects a loop, global map optimization begins. At this time, all key frames and map points in the global map participate in the joint adjustment, that is, the map optimization process.

[0145] Based on this, optionally, in one embodiment, as Figure 5 As shown, a map optimization method provided in an embodiment of the present application may also include the following steps:

[0146] S501: In a process of performing SLAM based on an image sensor, in response to satisfying a global map optimization condition, constructing each currently existing key frame into a third key frame set, and constructing each currently existing map point into a second map point set;

[0147] S502: Determine the auxiliary posture corresponding to each key frame in the third key frame set;

[0148] S503: constructing a second factor graph for graph optimization processing based on the robot posture and the corresponding auxiliary posture of each key frame in the third key frame set, and the map points in the second map point set;

[0149] S504: inputting the second factor graph and the preset second optimization parameters into the graph optimizer to optimize the robot poses of each key frame in the third key frame set except the initial key frame, and the map points in the second map point set;

[0150] The initial key frame is the key frame with the earliest acquisition time in the third key frame set;

[0151] S505: Using the optimized map points and robot poses of each key frame, map data of the currently existing global map is updated.

[0152] In this embodiment, the mobile robot can determine whether the current SLAM state meets the global map optimization condition during the SLAM process based on the image sensor. If so, the mobile robot responds to the global map optimization condition by constructing each key frame currently existing in the constructed map updated through the above steps S101-S106 as a third key frame set, and constructs each currently existing map point as a second map point set. It can be understood that when the mobile robot optimizes the global map, it is based on all key frames and all map points existing in the global map.

[0153] Afterwards, based on the data categories required for factor graph construction, the mobile robot continues to determine the auxiliary pose corresponding to each key frame in the third key frame set, thereby constructing a second factor graph for graph optimization processing based on the robot pose and the corresponding auxiliary pose of each third key frame in the third key frame set, and the map points in the second map point set.

[0154] Similar to the above step S105, when the mobile robot performs global map optimization, the user is also required to pre-set the second optimization parameters of the graph optimizer for the mobile robot in this optimization according to his own needs. Therefore, after the second factor graph is constructed, the second factor graph and the preset second optimization parameters are input into the graph optimizer to optimize the robot postures of each key frame in the third key frame set except the initial key frame, and the map points in the second map point set, so as to update the map data of the current global map using the optimized map points and the robot postures of each key frame to achieve optimization adjustment of the global map.

[0155] The specific implementation of the above steps S501-S505 is similar to the specific implementation of the above steps S101-S106, and will not be repeated here.

[0156] Optionally, in one implementation, satisfying the global map optimization condition includes: satisfying a correction condition for loop error correction.

[0157] In this implementation, during the process of SLAM based on image sensors, the mobile robot can simultaneously determine whether the current SLAM state meets the correction conditions for loop error correction, that is, by determining whether its current position is the second time the mobile robot has reached that position, it can determine whether the current SLAM state meets the correction conditions for loop error correction.

[0158] If so, it indicates that the current SLAM state satisfies the above correction conditions. At this time, the mobile robot can update the robot postures of the currently existing key frames and the currently existing map points based on the map data of the currently existing global map, thereby updating the map data of the currently existing global map and further optimizing the global map.

[0159] If not, it indicates that the current SLAM state does not meet the above correction conditions. At this time, the mobile robot can continue to optimize the local map, that is, continue to execute the above steps S101-S106, and use the robot postures of the optimized map points and key frames to update the currently constructed map of the target environment scene, or, after updating the currently constructed map of the target environment scene using the robot postures of the optimized map points and key frames, stop the map optimization process.

[0160] Optionally, in one implementation, the above step S503, based on the robot posture and the corresponding auxiliary posture of each key frame in the third key frame set, and the map points in the second map point set, constructing a second factor graph for graph optimization processing may include the following steps:

[0161] Step D: constructing a second factor graph for graph optimization processing according to a predetermined second construction method;

[0162] The predetermined second construction method includes:

[0163] Step D1: The robot posture of each key frame in the third key frame set is used as the first type of vertex in the second factor graph to be constructed, and the vertex representing the robot posture of the initial key frame in each first type of vertex is set as a fixed attribute and the other vertices are set as non-fixed attributes;

[0164] Step D2: taking the auxiliary posture corresponding to each key frame in the third key frame set as the second type of vertex in the second factor graph to be constructed, and each second type of vertex is set as a fixed attribute;

[0165] Step D3: each map point in the second map point set is used as a third-type vertex in the second factor graph to be constructed, and each third-type vertex is set as a non-fixed attribute;

[0166] Step D4: For each third-class vertex in the second factor graph to be constructed, the visual residual corresponding to the key frame related to the third-class vertex is used as the edge value to set the edge between the third-class vertex and the first-class vertex representing the robot posture of the related key frame; for each second-class vertex in the second factor graph to be constructed, the posture residual about the radar of the second-class vertex and the related first-class vertex is used as the edge value to set the edge between the second-class vertex and the related first-class vertex.

[0167] This implementation is the same as the specific implementation of steps A1-A4 above, which is to set the vertices that do not need to be adjusted in the factor graph as fixed attributes, set the vertices that need to be adjusted as non-fixed attributes, and set the edge between the two vertices according to the residual corresponding to the key frame related to the vertex according to each vertex determined. Thus, by using the residual between the two vertices, the posture represented by the vertex with fixed attributes is used to adjust the posture represented by the vertex with non-fixed attributes, or the map point, so as to achieve the optimization of the robot posture of each key frame in the third key frame set, except the initial key frame, and each map point in the second map point set.

[0168] In order to facilitate understanding of a map optimization method provided in an embodiment of the present application, it is introduced below in conjunction with a specific embodiment.

[0169] In the related art, mobile robots usually use a single sensor to perform the SLAM process. However, a single sensor has problems that it cannot overcome. For example, in the process of SLAM based on image sensors such as camera sensors and visual sensors, due to the limited field of view, it will be restricted by environmental factors. In addition, in the absence of loop detection correction, long-term positioning will lead to large cumulative errors in the positioning results, affecting the accuracy of the constructed map. For another example, in the process of SLAM based on radar sensors such as visual radars and 2D radar sensors, since radar sensors can only obtain sparse point cloud data, the local positioning accuracy determined by radar sensors is low, affecting the accuracy of the constructed map. In addition, radar sensors cannot obtain rich texture and color information of the environment.

[0170] The above two sensors also have their own advantages: when collecting data, radar sensors are less affected by environmental factors (lack of dynamic features) due to their 360-degree viewing angle and have better positioning stability; image sensors can directly obtain image data of the environment to achieve pixel-level environmental mapping.

[0171] Therefore, it is expected that the two sensors can be used in combination to complement each other's advantages, so as to improve the accuracy of the mobile robot. Specifically, the position and posture of the mobile robot can be fused and positioned by fusing the data collected by the visual sensor and the 2D radar sensor. In the related art, the fusion positioning of the data collected by the visual sensor and the 2D radar sensor is achieved through a specific software framework structure. Among them, the above-mentioned specific software framework structure includes: a loosely coupled framework structure and a tightly coupled framework structure.

[0172] For example, Figure 6 The figure is a flowchart of the fusion positioning method corresponding to different frameworks.

[0173] In the loosely coupled framework structure, sensor A uses the pose estimation algorithm A2 to process the collected sensor data A1 to obtain the pose A3; sensor B uses the pose estimation algorithm B2 to process the collected sensor data B1 to obtain the pose B3, and then the obtained pose A3 and pose B3 are fused to obtain the corresponding fused pose.

[0174] In other words, the fusion method corresponding to the loosely coupled framework structure is: the fusion method at the level of processing results of raw data from different sensors; among them, the raw data refers to the features extracted from the image obtained by the camera and the point cloud data obtained by the radar; the processing result refers to the posture information obtained by the raw data through the SLAM algorithm.

[0175] Therefore, it can be determined that the fusion mode corresponding to the loosely coupled framework structure has the following characteristics:

[0176] (1) The fusion method is relatively simple to implement, but it is not sensitive to the synchronization accuracy of the data;

[0177] (2) This fusion approach is highly modular in implementation and is convenient for collaborative development;

[0178] (3) The positioning accuracy of the fused pose obtained using this fusion method is low.

[0179] In the tightly coupled framework structure, the sensor data A1 collected by sensor A and the sensor data B1 collected by sensor B are processed using a joint pose estimation algorithm to obtain a fused pose.

[0180] In other words, the fusion method corresponding to the tightly coupled framework structure is: the fusion method at the level of raw data from different sensors.

[0181] Therefore, it can be determined that the fusion mode corresponding to the tightly coupled framework structure has the following characteristics:

[0182] (1) The implementation of the fusion method is relatively complex, but this fusion method is sensitive to the synchronization accuracy of the data;

[0183] (2) This fusion approach has a high degree of coupling during implementation, which is not conducive to collaborative development and maintenance;

[0184] (3) This fusion method has a large data processing space. Therefore, the positioning accuracy of the fused pose obtained using this fusion method is high.

[0185] From the above comparison, it can be seen that the fusion positioning obtained by using the tightly coupled framework structure has higher accuracy. However, when the mobile robot has poor data synchronization accuracy, low radar frame rate, and fluctuating local positioning accuracy, using a tightly coupled framework structure for fusion positioning will reduce the obtained fusion positioning effect.

[0186] Therefore, when the mobile robot has poor data synchronization accuracy, the local positioning accuracy of the radar sensor fluctuates, and the overall positioning consistency is good, since it cannot strictly rely on the positioning determined by the radar sensor, but there is a need to combine the advantages of the radar sensor's overall positioning consistency and optimize the accumulated positioning error generated by the image sensor during the SLAM process, based on the map optimization method provided in the above-mentioned embodiment of the present application, this embodiment designs a semi-tightly coupled framework structure based on ORB-SLAM3, exemplarily, Figure 7 A flow chart of a fusion positioning method corresponding to a semi-tightly coupled framework structure provided in an embodiment of the present application.

[0187] Among them, sensor A processes the collected sensor data A1 using the pose estimation algorithm A2 to obtain the pose A3, and then uses the joint pose estimation algorithm to process the sensor data B1 and the pose A3 collected by sensor B to obtain the fused pose. In other words, the fusion method corresponding to the semi-tightly coupled framework structure provided in this embodiment is: using the original data of one sensor and the processing result of another sensor for fusion.

[0188] Specifically, in a semi-tightly coupled framework structure provided in an embodiment of the present application, radar positioning is used as an independent module (process), and the radar pose determined based on the data collected by the radar sensor (i.e., point cloud data) is used as a correction reference to participate in the joint adjustment of visual reprojection error and visual pose-radar pose error, so as to achieve fusion not only at the pose level but also at the image feature level. In addition, in the above process, the point cloud data collected by the radar sensor is not involved in the fusion.

[0189] In addition, considering that radar sensors and visual sensors (image sensors) often have poor data synchronization accuracy due to differences in data formation principles, formats, transmission time, etc., the above-mentioned semi-tightly coupled framework structure is used to treat radar positioning as a separate module, that is, by setting independence for the radar positioning module, the mobile robot does not need to strictly rely on data synchronization accuracy under the semi-tightly coupled framework structure, and can improve the positioning accuracy of the fused positioning by fusing the data of the two sensors. Therefore, when the mobile robot has poor data synchronization accuracy, low radar frame rate, and fluctuating local positioning accuracy, the visual positioning function of the mobile robot is improved, thereby improving the accuracy of the map constructed by the mobile robot.

[0190] Combine the following Figure 8 The above-mentioned semi-tightly coupled framework structure and a map optimization method provided in an embodiment of the present application are specifically introduced.

[0191] A semi-tightly coupled framework structure provided in this embodiment is mainly divided into: a first part for radar positioning, a second part for front-end fusion and a third part for back-end fusion.

[0192] The first part includes 2D radar point cloud, positioning algorithm, radar pose and interpolation pose, specifically:

[0193] The first part uses a 2D radar sensor as the main radar sensor. The 2D radar sensor outputs a 2D radar point cloud (point cloud data) to the mobile robot, so that the mobile robot can obtain the corresponding radar pose and 2D grid map after robot positioning processing of the received radar point cloud through the positioning algorithm. Then, the obtained radar pose is interpolated and aligned with the visual data collected by the visual camera (i.e., the image frame collected by the image sensor) according to the timestamp to generate an interpolated pose, and the obtained interpolated pose is used as an input parameter and input into the second part for front-end fusion.

[0194] Optionally, the positioning algorithm may be cartographer (a laser SLAM algorithm based on graph optimization), which performs positioning by matching point clouds between frames.

[0195] The second part includes visual RGB (Red Green Blue) images, extraction of ORB (OrientedFAST and Rotated BRIEF, fast feature point extraction and description algorithm) features, pose initialization, relocalization, map creation, local map positioning, new keyframe processing, map point processing, local BA and local keyframe processing, specifically:

[0196] The second part uses a visual camera as the main image sensor and uses the visual camera to obtain a visual RGB image as visual data. Then, the visual camera outputs the visual RGB image to the mobile robot. The second part then performs the following steps:

[0197] Step 1: The mobile robot uses the ORB algorithm to extract ORB features in the image.

[0198] Step 2: The mobile robot determines its own positioning state by combining the extracted ORB features and the interpolated posture, and then initializes the posture based on the determined positioning state, and determines whether to relocate or create a map according to the system state. The specific implementation of the above step "initializes the posture based on the determined positioning state, and determines whether to relocate or create a map according to the system state" will be combined with the following embodiments and Fig. 9 The specific description will not be given here.

[0199] Step 3: The mobile robot can perform local map positioning based on the key frames and map points with posture information already in the map, thereby further optimizing the robot posture of the key frames.

[0200] Step 4: In the new key frame processing step, the mobile robot determines whether to create the new ordinary frame as a new key frame.

[0201] Specifically, a common frame that matches the image features of a previously determined key frame is determined as a new key frame; or, if the time interval between the acquisition time of the acquired common frame and the acquisition time of the determined previous key frame is a predetermined time interval, the common frame is determined as a new key frame.

[0202] Step 5: When processing map points, the mobile robot mainly removes map points of poor quality based on the observation of the map points to reduce the possibility of affecting the accuracy of subsequent pose optimization due to large errors in the map points.

[0203] Specifically, determine the three-dimensional coordinates corresponding to each map point of the currently constructed map, and map the three-dimensional coordinates corresponding to each map point to the plane image to obtain the projection coordinates of the map point; determine whether the coordinate error between the projection coordinates and the actual coordinates corresponding to the map point is greater than a predetermined threshold; if so, remove the map point.

[0204] By determining the coordinate error in the above manner, it is possible to further determine whether the quality of the observation of the corresponding map point is worth retaining the map point, wherein the above observation is used to characterize the sensor data observing the map point, that is, the image frame captured by the image sensor.

[0205] If there are map points whose coordinate errors are greater than a predetermined threshold, it means that there is less sensor data for observing the map point, and the quality of the observation of the map point is poor, so the map point needs to be eliminated; if there are map points whose coordinate errors are not greater than the predetermined threshold, it means that there is more sensor data for observing the map point, and the observation of the map point is good, so it can be retained.

[0206] In this way, by screening the map points of the currently constructed map, the map points with higher accuracy can be retained to improve the accuracy of the subsequent pose optimization, while streamlining the number of map points that need to be processed subsequently, thereby improving the accuracy and efficiency of subsequent map optimization.

[0207] Step 6: The mobile robot enters the stage of local BA, that is, by determining the local optimization range, combining the radar pose corresponding to the key frame in the local optimization range, and optimizing the robot pose and map points of the key frame; wherein, the local optimization range is determined by the common view relationship between the key frames, and the secondary common view area of ​​the current key frame is used as the local optimization range; it should be noted that the secondary common view area of ​​the current key frame is the area corresponding to the second key frame set in the embodiment of the present application. For example, after determining each first frame that has a common view relationship with the current key frame, the second frame that has a common view relationship with each first frame is further determined. At this time, the current key frame, each first frame, and the area corresponding to each second frame are the secondary common view areas of the current key frame.

[0208] The specific steps of the above-mentioned local BA link are as follows:

[0209] Step 6.1: Determine each key frame that has a co-viewing relationship with the current key frame (target key frame) as the key frame set KF_local (first key frame set);

[0210] Step 6.2: Add the map points corresponding to all key frames in KF_local to the local map point set MP_local (the first map point set);

[0211] Step 6.3: Add all KFs associated with MP_local but not belonging to KF_local to the fixed KF set KF_fix (the second key frame set):

[0212] Step 6.4: Initialize the optimizer based on G2O, select the linear optimizer and use the LM method to solve;

[0213] Step 6.5: Construct the KF pose in KF_local (the robot pose of the key frame) as the pose vertex (first-class vertex) of the graph, and only fix the pose of the initial frame (specified key frame);

[0214] Step 6.6: Construct the radar pose corresponding to the KF in KF_local as the pose vertex of the graph (second type of vertex), all fixed;

[0215] Step 6.7: Construct the KF pose in KF_fix as the pose vertex of the graph (first-class vertex) and fix the pose of all frames;

[0216] Step 6.8: Construct the map points in MP_local as vertices of the graph used to represent the map points (third type of vertices), all of which are not fixed;

[0217] Step 6.9: Create edges connecting the map points with the vertices corresponding to all KF poses;

[0218] Step 6.10: Add all the above vertices to the selected linear optimizer, set the optimization parameters of the linear optimizer (the preset first optimization parameters), and start the optimization;

[0219] Step 6.11: Take out the optimization results and use the optimized map points and KF poses to update the currently constructed map.

[0220] Step 7: After obtaining the updated currently constructed map, the mobile robot can process the local key frames in the updated map, that is, in the co-visual map used to store the co-visual relationship between the key frames, detect each key frame that has a co-visual relationship with the current key frame, and according to the redundancy of the map point in the above co-visual map, eliminate other key frames including the map point, that is, eliminate the co-visual key frames including the map point, thereby improving the computational efficiency of subsequent map optimization.

[0221] At this point, the update of the map corresponding to the determined local optimization range is completed, so as to update the currently constructed map.

[0222] The third part includes loop detection, loop correction, global BA and map update, specifically:

[0223] In the third part, the mobile robot performs loop detection based on the bag-of-words model, that is, checking whether there are similar key frames in all the key frames in the updated currently constructed map and whether they meet the loop conditions; if so, loop correction is performed. Before loop correction, the mobile robot stops local mapping and global BA, and calculates the initial pose of the current key frame after loop correction through Sim3 (Similarity Transformation in 3D Space). Then, through pose propagation, the poses of each key frame that has a co-visual relationship with the current key frame after Sim3 optimization, as well as the map points of each key frame, are obtained, and the co-visual relationship between each key frame and the current key frame is updated.

[0224] After that, the mobile robot enters the global BA phase, which combines the radar poses corresponding to all key frames in the map and optimizes the robot poses and all map points in all key frames.

[0225] The specific steps included in the above global BA are as follows:

[0226] Step 8.1: Determine a key frame set KF_all (third key frame set) of all key frames, and a map point set MP_all (second map point set) of all map points;

[0227] Step 8.2: Initialize the optimizer based on G2O, select the linear optimizer and use the LM method to solve;

[0228] Step 8.3: Construct the KF poses in KF_all as the pose vertices (first-class vertices) of the graph, and only fix the pose of the initial frame (initial keyframe);

[0229] Step 8.4: Construct the radar poses corresponding to the KFs in KF_all as the pose vertices of the graph (second-type vertices), all of which are fixed;

[0230] Step 8.5: Construct the map points in MP_all as vertices of the graph used to represent the map points (third type of vertices), all of which are not fixed;

[0231] Step 8.6: Create edges connecting the map points with the vertices corresponding to all KF poses;

[0232] Step 8.7: Add all the above vertices to the selected linear optimizer, set the optimization parameters of the linear optimizer (the preset second optimization parameters), and start the optimization;

[0233] Step 8.8: Take out the optimization results, and use the optimized map points and KF poses to update the map data of the current global map.

[0234] At this point, the update of the map corresponding to the global scope is completed to achieve optimization of the map data of the global map.

[0235] In this embodiment, the radar posture corresponding to the KF posture is introduced to optimize and adjust the KF posture and each map point, thereby improving the visual positioning function of the mobile robot when the data synchronization accuracy of the mobile robot is poor, the frame rate of the radar is low, and the local positioning accuracy fluctuates, thereby improving the accuracy of the map constructed by the mobile robot.

[0236] Combine the following Fig. 9The embodiment of the present application shown is a flow chart of a fusion positioning process, which specifically explains the implementation method of step 2 of the first part above, "initializing the posture based on the determined positioning state, and determining whether to reposition or create a map according to the system state."

[0237] Step M1: In the case of an existing map, the mobile robot determines whether it is performing real-time positioning; if so, execute step M2; otherwise, execute step M8;

[0238] Step M2: The mobile robot continues to determine whether the current speed information is normal, and when the current speed information is normal, performs real-time positioning using the motion model; when the current speed information is abnormal, executes step M10;

[0239] Step M3: Determine whether the real-time positioning using the motion model is successful, if so, execute step M4; otherwise, execute step M10;

[0240] Step M4: Enter the local map positioning phase;

[0241] Step M5: Determine whether the mobile robot successfully locates itself in real time during the local map positioning process; if so, execute step M6; otherwise, execute step M7;

[0242] Step M6: Enter the local mapping module and start creating a local map;

[0243] Step M7: Set the real-time positioning result to just lost, and execute step M9;

[0244] Step M8: Determine whether the real-time positioning result is just lost; if so, execute step M9; otherwise, return to step M1;

[0245] Step M9: Use the radar predicted posture to perform real-time positioning, and determine whether the radar predicted posture is successful in real-time positioning; if successful, execute step M4; otherwise, execute step M11;

[0246] Step M10: Perform real-time positioning using the reference key frame, and determine whether the real-time positioning using the reference key frame is successful; if successful, execute step M4; otherwise, execute step M11;

[0247] Step M11: Determine whether the current existing map is worth saving, if so, execute step M12; otherwise, execute step M1;

[0248] Step M12: Set the real-time positioning result to just lost, and return to step M9.

[0249] Based on the above steps M1-M12, the posture can be initialized based on the determined positioning state (i.e., the real-time positioning result), and different methods can be used to determine whether the real-time positioning is successful, and the obtained real-time positioning result is used as the system state, thereby judging whether to relocate and create a new map based on the determined system state, or to create a local map. Specifically, when the real-time positioning is unsuccessful, it indicates that the current positioning result of the mobile robot is inaccurate. At this time, the mobile robot relocates its own position and creates a new map; when the real-time positioning is successful, it indicates that the current positioning result of the mobile robot is accurate. At this time, the mobile robot can create a local map based on its own positioning result, thereby obtaining the currently constructed map of the target environment scene in the embodiment of the present application.

[0250] Corresponding to the above method embodiment, the embodiment of the present application also provides a map optimization device, which is applied to a mobile robot equipped with a radar sensor and an image sensor.

[0251] Fig.10 A structural diagram of a map optimization device provided in an embodiment of the present application is shown in FIG. Fig.10 As shown, the device comprises:

[0252] The first construction module 1001 is used to construct a first key frame set in response to the key frame constructed during the process of performing SLAM for the target environment scene based on the image sensor; wherein the first key frame set includes a target key frame and a key frame in a co-viewing relationship with the target key frame among the currently existing key frames; the target key frame is the key frame currently constructed;

[0253] A second construction module 1002 is used to construct a first map point set based on the first key frame set; wherein the first map point set includes map points corresponding to each key frame in the first key frame set;

[0254] The first determination module 1003 is used to determine the auxiliary posture corresponding to each key frame in the first key frame set; wherein the auxiliary posture corresponding to each key frame is: the robot posture at a time matching the acquisition time of the key frame obtained after robot positioning processing based on each frame of point cloud data collected by the radar sensor; each frame of point cloud data is data collected by the radar sensor during the target movement process of the mobile robot; the target movement process is the robot movement process required to be executed for the SLAM process;

[0255] A third construction module 1004 is used to construct a first factor graph for graph optimization processing based on the robot posture and the corresponding auxiliary posture of each key frame in the first key frame set, and the map points in the first map point set;

[0256] A first optimization module 1005, configured to input the first factor graph and the preset first optimization parameters into a predetermined graph optimizer, so as to optimize the robot postures of each key frame in the first key frame set except for a designated key frame, and the map points in the first map point set; wherein the designated key frame is the key frame with the earliest acquisition time in the first key frame set;

[0257] The first updating module 1006 is used to update the currently constructed map of the target environment scene by using the optimized map points and the robot postures of the key frames.

[0258] As can be seen from the above, a map optimization method provided by an embodiment of the present application is applied to a mobile robot, which is equipped with a radar sensor and an image sensor. When the mobile robot optimizes the map of the target environment scene, it moves according to the target movement process of the robot required to perform the SLAM process, and in the process of the mobile robot moving according to the above target movement process, the image sensor carried performs SLAM for the target environment scene, and at the same time, the radar sensor carried collects each frame of point cloud data for the target environment scene, so that the subsequent mobile robot will perform robot positioning processing based on each frame of point cloud data collected by the radar sensor, and the robot posture at the time matching the collection time of each key frame is used as the auxiliary posture corresponding to the key frame.

[0259] After the mobile robot responds to the key frame being constructed, it can construct a first key frame set based on the target key frame currently constructed and the key frame in the currently existing key frames that has a co-view relationship with the target key frame, and after constructing the first map point set based on the map points corresponding to each key frame in the first key frame set, determine the auxiliary posture corresponding to each key frame in the first key frame set, thereby constructing a first factor graph for graph optimization based on the robot posture and the corresponding auxiliary posture of each key frame in the first key frame set and the map points in the first map point set. In this way, the mobile robot inputs the constructed first factor graph and the preset first optimization parameters into a predetermined graph optimizer to optimize the robot posture of each key frame in the first key frame set except the designated key frame with the earliest acquisition time and the map points in the first map point set by using the auxiliary posture corresponding to each key frame in the first key frame set, thereby updating the currently constructed map of the target environment scene by using the optimized map points and the robot posture of each key frame.

[0260] Based on this, the solution provided in the embodiment of the present application is applied. In the process of SLAM based on image sensors, the mobile robot uses the radar posture collected by the radar sensor as the auxiliary posture of the mobile robot for map optimization. Thereby, the robot posture of each key frame and the corresponding auxiliary posture are jointly optimized to improve the accuracy of the robot posture and map points of each key frame after optimization, and further improve the accuracy of the map constructed by the mobile robot.

[0261] Optionally, in an implementation manner, the third building module 1004 is specifically configured to:

[0262] Constructing a first factor graph for graph optimization processing according to a predetermined first construction method;

[0263] Wherein, the first construction method includes:

[0264] The robot posture of each key frame in the first key frame set is used as a first-type vertex in the first factor graph to be constructed, and the vertex representing the robot posture of the specified key frame in each first-type vertex is set as a fixed attribute and the other vertices are set as non-fixed attributes;

[0265] The auxiliary posture corresponding to each key frame in the first key frame set is used as a second type of vertex in the first factor graph to be constructed, and each constructed second type of vertex is set as the fixed attribute;

[0266] Using each map point in the first map point set as a third-type vertex in the first factor graph to be constructed, and setting each constructed third-type vertex as the non-fixed attribute;

[0267] For each third-type vertex in the first factor graph to be constructed, the visual residual corresponding to the key frame related to the third-type vertex is used as the edge value to set the edge between the third-type vertex and the first-type vertex representing the robot posture of the related key frame; for each second-type vertex in the first factor graph to be constructed, the radar posture residual about the second-type vertex and the related first-type vertex is used as the edge value to set the edge between the second-type vertex and the related first-type vertex;

[0268] Among them, the key frame related to the third type of vertex is the key frame corresponding to the map point represented by the third type of vertex; the first type of vertex related to the second type of vertex is the robot posture possessed by the key frame corresponding to the auxiliary posture represented by the second type of vertex.

[0269] Optionally, in an implementation manner, the device further includes:

[0270] A fourth construction module, configured to construct key frames in the existing key frames that correspond to map points in the first map point set and do not belong to the first key frame set into a second key frame set;

[0271] The predetermined first construction method also includes:

[0272] The robot posture of each key frame in the second key frame set is used as a first-type vertex with fixed attributes in the first factor graph to be constructed.

[0273] Optionally, in an implementation manner, the device further includes:

[0274] A fifth construction module is used for constructing each currently existing key frame into a third key frame set and each currently existing map point into a second map point set in response to satisfying a global map optimization condition during a process of performing SLAM based on the image sensor;

[0275] A second determination module, used to determine the auxiliary posture corresponding to each key frame in the third key frame set;

[0276] a sixth construction module, configured to construct a second factor graph for graph optimization processing based on the robot posture and the corresponding auxiliary posture of each key frame in the third key frame set, and the map points in the second map point set;

[0277] A second optimization module, configured to input the second factor graph and the preset second optimization parameters into the graph optimizer, so as to optimize the robot postures of each key frame in the third key frame set except the initial key frame, and the map points in the second map point set; wherein the initial key frame is the key frame with the earliest acquisition time in the third key frame set;

[0278] The second updating module is used to update the map data of the currently existing global map by using the robot posture of each optimized map point and each key frame.

[0279] Optionally, in one implementation, satisfying the global map optimization condition includes: satisfying a correction condition for loop error correction.

[0280] Optionally, in an implementation manner, the sixth building block is specifically configured to:

[0281] Constructing a second factor graph for graph optimization processing according to a predetermined second construction method;

[0282] Wherein, the predetermined second construction method includes:

[0283] The robot posture of each key frame in the third key frame set is used as a first-type vertex in the second factor graph to be constructed, and the vertex representing the robot posture of the initial key frame in each first-type vertex is set as a fixed attribute and the other vertices are set as non-fixed attributes;

[0284] The auxiliary posture corresponding to each key frame in the third key frame set is used as a second type of vertex in the second factor graph to be constructed, and each second type of vertex is set to a fixed attribute;

[0285] Using each map point in the second map point set as a third-type vertex in the second factor graph to be constructed, and each third-type vertex is set to have a non-fixed attribute;

[0286] For each third-type vertex in the second factor graph to be constructed, the visual residual corresponding to the key frame related to the third-type vertex is used as the edge value to set the edge between the third-type vertex and the first-type vertex representing the robot posture of the related key frame; for each second-type vertex in the second factor graph to be constructed, the posture residual about the radar of the second-type vertex and the related first-type vertex is used as the edge value to set the edge between the second-type vertex and the related first-type vertex.

[0287] Optionally, in one implementation, the first optimization parameters include: a first type of information used to characterize the optimization target and a second type of information characterizing the solution method; wherein the optimization target is to perform a joint adjustment on the visual residuals related to the image sensor and the posture residuals related to the radar sensor to optimize the robot posture.

[0288] Optionally, in one implementation, the first type of information used to characterize the optimization target is characterized by the following optimization problem:

[0289] ;

[0290] in, is the matrix corresponding to the robot posture of each key frame except the specified key frame after optimization. is the matrix corresponding to the robot posture of each key frame to be optimized except the specified key frame, A matrix representing the visual residual associated with the image sensor, A matrix representing the pose residual associated with the radar sensor, The robot poses of each key frame except the specified key frame when minimizing the visual residual associated with the image sensor and the pose residual associated with the radar sensor.

[0291] The present application also provides a mobile robot, such as Fig.11 As shown, including:

[0292] Memory 1101, used for storing computer programs;

[0293] The processor 1102 is used to implement any map optimization method provided in the above-mentioned embodiments of the present application when executing the program stored in the memory 1101.

[0294] In addition, the mobile robot may further include a communication bus and / or a communication interface, and the processor 1102, the communication interface, and the memory 1101 communicate with each other via the communication bus.

[0295] The communication bus mentioned in the mobile robot can be a Peripheral Component Interconnect (PCI) bus or an Extended Industry Standard Architecture (EISA) bus. The communication bus can be divided into an address bus, a data bus, a control bus, etc. For ease of representation, only one thick line is used in the figure, but it does not mean that there is only one bus or one type of bus.

[0296] The communication interface is used for communication between the mobile robot and other devices.

[0297] The memory may include a random access memory (RAM) or a non-volatile memory (NVM), such as at least one disk memory. Optionally, the memory may also be at least one storage device located away from the aforementioned processor.

[0298] The above-mentioned processor can be a general-purpose processor, including a central processing unit (CPU), a network processor (NP), etc.; it can also be a digital signal processor (DSP), an application specific integrated circuit (ASIC), a field programmable gate array (FPGA) or other programmable logic devices, discrete gate or transistor logic devices, discrete hardware components.

[0299] In another embodiment provided in the present application, a computer-readable storage medium is provided, in which a computer program is stored. When the computer program is executed by a processor, the steps of any of the above-mentioned map optimization methods are implemented.

[0300] In another embodiment provided by the present application, a computer program product including instructions is also provided, which, when executed on a computer, enables the computer to execute any map optimization method in the above embodiments.

[0301] In the above embodiments, it can be implemented in whole or in part by software, hardware, firmware or any combination thereof. When implemented using software, it can be implemented in whole or in part in the form of a computer program product. The computer program product includes one or more computer instructions. When the computer program instructions are loaded and executed on a computer, the process or function described in the embodiment of the present application is generated in whole or in part. The computer may be a general-purpose computer, a special-purpose computer, a computer network, or other programmable device. The computer instructions may be stored in a computer-readable storage medium, or transmitted from one computer-readable storage medium to another computer-readable storage medium. For example, the computer instructions may be transmitted from a website site, computer, server or data center to another website site, computer, server or data center by wired (e.g., coaxial cable, optical fiber, digital subscriber line (DSL)) or wireless (e.g., infrared, wireless, microwave, etc.) means. The computer-readable storage medium may be any available medium that a computer can access or a data storage device such as a server or data center that includes one or more available media integrated. The available medium may be a magnetic medium (e.g., a floppy disk, a hard disk, a tape), an optical medium (e.g., a DVD), or a solid-state hard disk (SSD), etc.

[0302] It should be noted that, in this article, relational terms such as first and second, etc. are only used to distinguish one entity or operation from another entity or operation, and do not necessarily require or imply any such actual relationship or order between these entities or operations. Moreover, the terms "include", "comprise" or any other variants thereof are intended to cover non-exclusive inclusion, so that a process, method, article or device including a series of elements includes not only those elements, but also other elements not explicitly listed, or also includes elements inherent to such process, method, article or device. In the absence of further restrictions, the elements defined by the sentence "comprise a ..." do not exclude the existence of other identical elements in the process, method, article or device including the elements.

[0303] Each embodiment in this specification is described in a related manner, and the same or similar parts between the embodiments can be referred to each other, and each embodiment focuses on the differences from other embodiments. In particular, for the device embodiment, mobile robot embodiment, computer-readable storage medium embodiment, and computer program product embodiment, since they are basically similar to the method embodiment, the description is relatively simple, and the relevant parts can be referred to the partial description of the method embodiment.

[0304] The above description is only a preferred embodiment of the present application and is not intended to limit the protection scope of the present application. Any modification, equivalent replacement, improvement, etc. made within the spirit and principle of the present application are included in the protection scope of the present application.

Claims

1. A map optimization method, characterized in that: Applied to a mobile robot, the mobile robot is equipped with a radar sensor and an image sensor, the method comprises: In the process of performing SLAM for a target environment scene based on the image sensor, in response to constructing a key frame, a first key frame set is constructed; wherein the first key frame set includes a target key frame and a key frame in a co-viewing relationship with the target key frame among the currently existing key frames; the target key frame is the key frame currently constructed; Based on the first key frame set, construct a first map point set; wherein the first map point set includes map points corresponding to each key frame in the first key frame set; Determine the auxiliary posture corresponding to each key frame in the first key frame set; wherein the auxiliary posture corresponding to each key frame is: the robot posture at a time matching the acquisition time of the key frame obtained after robot positioning processing based on each frame of point cloud data collected by the radar sensor; each frame of point cloud data is data collected by the radar sensor during the target movement process of the mobile robot; the target movement process is the robot movement process required to be executed for the SLAM process; Constructing a first factor graph for graph optimization processing based on the robot pose and the corresponding auxiliary pose of each key frame in the first key frame set, and the map points in the first map point set; Inputting the first factor graph and the preset first optimization parameters into a predetermined graph optimizer to optimize the robot postures of each key frame in the first key frame set except for a specified key frame, and the map points in the first map point set; wherein the specified key frame is a key frame with the earliest acquisition time in the first key frame set; The currently constructed map of the target environment scene is updated using the optimized map points and the robot poses of each key frame.

2. The method according to claim 1, characterized in that The step of constructing a first factor graph for graph optimization processing based on the robot posture and the corresponding auxiliary posture of each key frame in the first key frame set and the map points in the first map point set includes: Constructing a first factor graph for graph optimization processing according to a predetermined first construction method; Wherein, the first construction method includes: The robot posture of each key frame in the first key frame set is used as a first-type vertex in the first factor graph to be constructed, and the vertex representing the robot posture of the specified key frame in each first-type vertex is set as a fixed attribute and the other vertices are set as non-fixed attributes; The auxiliary posture corresponding to each key frame in the first key frame set is used as a second type of vertex in the first factor graph to be constructed, and each constructed second type of vertex is set as the fixed attribute; Using each map point in the first map point set as a third-type vertex in the first factor graph to be constructed, and setting each constructed third-type vertex as the non-fixed attribute; For each third-type vertex in the first factor graph to be constructed, the visual residual corresponding to the key frame related to the third-type vertex is used as the edge value to set the edge between the third-type vertex and the first-type vertex representing the robot posture of the related key frame; for each second-type vertex in the first factor graph to be constructed, the radar posture residual about the second-type vertex and the related first-type vertex is used as the edge value to set the edge between the second-type vertex and the related first-type vertex; Among them, the key frame related to the third type of vertex is the key frame corresponding to the map point represented by the third type of vertex; the first type of vertex related to the second type of vertex is the robot posture possessed by the key frame corresponding to the auxiliary posture represented by the second type of vertex.

3. The method according to claim 2, characterized in that The method further comprises: constructing key frames in the existing key frames that correspond to map points in the first map point set and do not belong to the first key frame set as a second key frame set; The predetermined first construction method also includes: The robot posture of each key frame in the second key frame set is used as a first-type vertex with fixed attributes in the first factor graph to be constructed.

4. The method according to claim 1, characterized in that: The method further comprises: In the process of performing SLAM based on the image sensor, in response to satisfying the global map optimization condition, each currently existing key frame is constructed as a third key frame set, and each currently existing map point is constructed as a second map point set; Determine an auxiliary pose corresponding to each key frame in the third key frame set; Constructing a second factor graph for graph optimization processing based on the robot pose and the corresponding auxiliary pose of each key frame in the third key frame set and the map points in the second map point set; Inputting the second factor graph and the preset second optimization parameters into the graph optimizer to optimize the robot poses of each key frame in the third key frame set except the initial key frame, and the map points in the second map point set; wherein the initial key frame is the key frame with the earliest acquisition time in the third key frame set; The map data of the currently existing global map is updated using the optimized map points and robot poses of each key frame.

5. The method according to claim 4, characterized in that The satisfying of the global map optimization condition includes satisfying the correction condition of the loop error correction.

6. The method according to claim 4, characterized in that The step of constructing a second factor graph for graph optimization based on the robot posture and the corresponding auxiliary posture of each key frame in the third key frame set and the map points in the second map point set includes: Constructing a second factor graph for graph optimization processing according to a predetermined second construction method; Wherein, the predetermined second construction method includes: The robot posture of each key frame in the third key frame set is used as a first-type vertex in the second factor graph to be constructed, and the vertex representing the robot posture of the initial key frame in each first-type vertex is set as a fixed attribute and the other vertices are set as non-fixed attributes; The auxiliary posture corresponding to each key frame in the third key frame set is used as a second type of vertex in the second factor graph to be constructed, and each second type of vertex is set to a fixed attribute; Using each map point in the second map point set as a third-type vertex in the second factor graph to be constructed, and each third-type vertex is set to have a non-fixed attribute; For each third-type vertex in the second factor graph to be constructed, the visual residual corresponding to the key frame related to the third-type vertex is used as the edge value to set the edge between the third-type vertex and the first-type vertex representing the robot posture of the related key frame; for each second-type vertex in the second factor graph to be constructed, the posture residual about the radar of the second-type vertex and the related first-type vertex is used as the edge value to set the edge between the second-type vertex and the related first-type vertex.

7. The method according to claim 1, characterized in that The first optimization parameter includes: a first type of information used to characterize the optimization target and a second type of information used to characterize the solution method; wherein the optimization target is to perform a joint adjustment on the visual residuals related to the image sensor and the posture residuals related to the radar sensor to optimize the robot posture.

8. The method according to claim 7, characterized in that The first type of information used to characterize the optimization objective is characterized by the following optimization problem: ; in, is the matrix corresponding to the robot posture of each key frame except the specified key frame after optimization. is the matrix corresponding to the robot posture of each key frame to be optimized except the specified key frame, A matrix representing the visual residual associated with the image sensor, A matrix representing the pose residual associated with the radar sensor, The robot poses of each key frame except the specified key frame when minimizing the visual residual associated with the image sensor and the pose residual associated with the radar sensor.

9. A map optimization device, characterized in that: Applied to a mobile robot, the mobile robot is equipped with a radar sensor and an image sensor, and the device comprises: A first construction module is used to construct a first key frame set in response to a key frame constructed during a SLAM process for a target environment scene based on the image sensor; wherein the first key frame set includes a target key frame and a key frame in a co-viewing relationship with the target key frame among the currently existing key frames; the target key frame is the key frame currently constructed; A second construction module is used to construct a first map point set based on the first key frame set; wherein the first map point set contains map points corresponding to each key frame in the first key frame set; A first determination module is used to determine the auxiliary posture corresponding to each key frame in the first key frame set; wherein the auxiliary posture corresponding to each key frame is: the robot posture at a time matching the acquisition time of the key frame obtained after robot positioning processing based on each frame of point cloud data collected by the radar sensor; each frame of point cloud data is data collected by the radar sensor during the target movement process of the mobile robot; the target movement process is the robot movement process required to be executed for the SLAM process; a third construction module, configured to construct a first factor graph for graph optimization processing based on the robot pose and the corresponding auxiliary pose of each key frame in the first key frame set, and the map points in the first map point set; A first optimization module, configured to input the first factor graph and a preset first optimization parameter into a predetermined graph optimizer, so as to optimize the robot posture of each key frame in the first key frame set except a specified key frame, and the map points in the first map point set; wherein the specified key frame is a key frame with the earliest acquisition time in the first key frame set; The first updating module is used to update the currently constructed map of the target environment scene by using the optimized map points and the robot postures of the key frames.

10. A mobile robot, characterized in that: include: Memory, used to store computer programs; A processor, for implementing any of the methods described in claims 1-8 when executing a program stored in a memory.

11. A computer-readable storage medium, characterized in that: The computer-readable storage medium stores a computer program, and when the computer program is executed by a processor, the method according to any one of claims 1 to 8 is implemented.

Images