Multi-sub-image laser radar positioning method, system and terminal

Through the multi-sub-graph lidar positioning method, sub-graphs are constructed using lidar sensors and gyroscopes, scanning matching and closed-loop detection are solved, and the problems of low positioning accuracy and cumulative error in the existing technology are improved, and the work efficiency and stability of the robot are improved. .

CN113514843BActive Publication Date: 2024-11-12SHENZHEN HUA XIN INFORMATION TECH CO LTD
View PDF 0 Cites 0 Cited by

Patent Information

Application Number
CN202110777459.2
Authority / Receiving Office
CN · China
Patent Type
Patents(China)
Current Assignee / Owner
Filing Date
2021-07-09
Publication Date
2024-11-12
Estimated Expiration
2041-07-09

AI Technical Summary

Technical Problem

The existing lidar positioning technology has problems such as low positioning accuracy, cumulative positioning error, and time-consuming robot repositioning, resulting in low robot work efficiency.

Method used

Using the multi-sub-graph lidar positioning method, data is collected through lidar sensors, odometers and gyroscopes, local and global sub-graphs are constructed, scanning matching and closed-loop detection are performed, positioning is optimized, cumulative errors are reduced, and turned on during repositioning. Relocate threads or fuse positioning threads.

Benefits of technology

It improves positioning accuracy, reduces cumulative errors, shortens repositioning time, and enhances robot flexibility and working stability.

✦ Generated by Eureka AI based on patent content.

Smart Images

  • Figure CN113514843B_ABST
    Figure CN113514843B_ABST
Patent Text Reader

Abstract

The multi-subgraph laser radar positioning method, system and terminal of the present invention use data collected by the laser radar sensor, odometer and gyroscope to construct at least two subgraphs, and use the scanning points of the current laser radar to scan and match the built subgraphs. Combining multiple subgraphs can obtain high-precision posture and reduce cumulative errors; use the environmental information scanned by the current laser radar and the established subgraphs to perform closed-loop detection, thereby establishing constraints and eliminating cumulative positioning errors, solving the problem that cumulative errors are difficult to eliminate; and when the robot is disturbed by dynamic objects and causes incorrect positioning, starting a repositioning thread or a fused positioning thread effectively solves the problem that the robot does not know its position for a long time and loses its autonomous behavior ability, increases the flexibility of the robot, and can also effectively solve the situation of positioning errors, increasing the stability of the robot's work and the accuracy of positioning.
Need to check novelty before this filing date? Find Prior Art

Description

Technical Field

[0001] The present invention relates to the field of robots, and in particular to a multi-sub-graph laser radar positioning method, system and terminal. Background Art

[0002] In recent years, with the increasing level of science and technology, artificial intelligence technology has flourished. Mobile robots are an important manifestation of the development of artificial intelligence. Their purpose is to enable robots to have the ability to perceive, understand and act like humans. If robots want to achieve the ability to act autonomously, positioning technology is an important prerequisite. Among them, laser radar is widely used in positioning applications due to its accurate measurement, simple error model and relatively mature theoretical research. The laser radar positioning method has become a technology for commercial applications. At present, some laser radar positioning technologies have problems such as inaccurate positioning, positioning errors, and easy positioning cumulative errors that are difficult to eliminate. In addition, there is also the problem that after the robot's posture is lost, repositioning takes too much time, which leads to the robot's inability to implement path planning and navigation well during work, which in turn reduces the robot's work efficiency. Summary of the invention

[0003] In view of the shortcomings of the prior art mentioned above, the object of the present invention is to provide a multi-sub-graph lidar positioning method, system and terminal, which are used to solve the problems of low positioning accuracy, cumulative positioning error, robot loss of posture during repositioning, and repositioning taking too much time existing in the lidar positioning method used in the prior art.

[0004] To achieve the above-mentioned purpose and other related purposes, the present invention provides a multi-subgraph laser radar positioning method, which is applied to a mobile robot, which is provided with a laser radar sensor, an odometer and a gyroscope. The method comprises: based on the position information of each obstacle in the current environment collected by the laser radar sensor and the map coordinate system constructed with the initial posture information of the robot collected by the odometer and the gyroscope, respectively construct at least two local subgraphs and a global subgraph in the current environment to obtain a two-dimensional map composed of each subgraph; wherein the two-dimensional map comprises: the coordinate position information of each obstacle in the map coordinate system; the position information of the obstacle collected by the laser radar sensor at the current moment is scanned and matched with the coordinate position information of each obstacle corresponding to the global subgraph in the two-dimensional map in the map coordinate system, respectively, to obtain the optimal posture of the robot at the current moment; the current final posture of the robot is obtained according to the optimal posture, and the position information of the obstacle collected by the laser radar sensor at the current moment is inserted into each local subgraph and the global subgraph with the final posture, so as to update the two-dimensional map and the trajectory information of each local subgraph and the global subgraph; upon receiving the control signal that the corresponding obstacle scanning is completed, When the robot receives a command, the updating of the two-dimensional map is stopped, and the latest updated two-dimensional map is saved as the final two-dimensional map; wherein the final two-dimensional map includes: the final coordinate position information of each obstacle in the map coordinate system and the final trajectory information of each subgraph; when the robot restarts, the saved final two-dimensional map is loaded, and the position information of the obstacles collected by the laser radar sensor at the current moment is scanned and matched with the final coordinate position information of each obstacle in the global subgraph in the two-dimensional map in the map coordinate system, so as to obtain the optimal posture of the robot at the current moment and the local subgraph with the current trajectory information and the global subgraph; the local subgraph with the previous trajectory information is used to perform closed-loop detection on the trajectory nodes in the current trajectory information, the completed old local subgraph with the current trajectory information and the trajectory nodes saved in the previous trajectory information, and establish closed-loop constraints; when the number of trajectory nodes corresponding to the local subgraph with the current trajectory information is greater than the optimization threshold, the constraints are calculated, the optimization solution is performed, the global posture of the local subgraph under the optimized current trajectory and the global posture of the trajectory nodes are obtained, and the optimal posture at the current moment is updated to obtain the final posture at the current moment.

[0005] In one embodiment of the present invention, the map coordinate system constructed based on the position information of each obstacle in the current environment collected by the laser radar sensor and the initial posture information of the robot collected by the odometer and the gyroscope respectively obtains at least two local sub-graphs and a global sub-graph in the current environment to obtain a two-dimensional map composed of each sub-graph, including: constructing a map coordinate system based on the initial posture information of the robot collected by the odometer and the gyroscope; converting the position information of each obstacle in the current environment collected by the laser radar sensor into the same robot coordinate system, and then converting it into a world coordinate system to obtain the motion distortion removed position information of each obstacle in the current environment collected by the laser radar sensor; based on the laser radar frame threshold, constructing one or more local sub-graphs in sequence to obtain a two-dimensional map composed of each local sub-graph and a global sub-graph.

[0006] In one embodiment of the present invention, the position information of the obstacle collected by the laser radar sensor at the current moment is scanned and matched with the coordinate position information of each obstacle corresponding to the global sub-graph in the two-dimensional map in the map coordinate system to obtain the optimal posture of the robot at the current moment, including: based on the posture information of the robot collected in real time by the odometer and the gyroscope and / or the final posture of the previous moment, the acceleration and linear velocity of the robot are calculated to obtain the posture change information of the robot; according to the posture change information of the robot, the predicted posture at the current moment is obtained; the predicted posture is used as the central posture of the laser radar scan at the current moment and one or more candidate postures are searched within a set first search range, and the position information of the obstacle scanned by the laser radar at each candidate posture is calculated. The coordinate position information of each obstacle in the local sub-graph on the two-dimensional map in the map coordinate system is scanned and matched to the matching score; based on the matching score of each candidate posture, the initial posture at the current moment is selected, and the initial posture is nonlinearly optimized to obtain the optimal posture at the current moment and the matching score of the optimal posture.

[0007] In one embodiment of the present invention, obtaining the current final posture of the robot according to the optimal posture includes: judging whether inaccurate positioning occurs during the positioning process based on the positioning judgment condition; if inaccurate positioning does not occur, taking the optimal posture as the final posture of the robot; if inaccurate positioning occurs, taking the predicted posture at the current moment as the final posture at the current moment; inserting the position information of the obstacle collected by the lidar sensor at the current moment into each sub-graph with the final posture to update the two-dimensional map and the trajectory information of each sub-graph; wherein the inaccurate positioning condition includes: the matching score of the optimal posture at the current moment is less than a set first threshold, the difference between the optimal posture at the current moment and the optimal posture at the previous moment is greater than a set second threshold, the difference between the optimal posture at the current moment and the predicted posture at the current moment is greater than a set third threshold, and the difference between the predicted posture at the current moment and the optimal posture at the previous moment is less than a set fourth threshold. One or more of the following situations.

[0008] In one embodiment of the present invention, when the robot is restarted, the saved final two-dimensional map is loaded, and the position information of the obstacles collected by the laser radar sensor at the current moment is scanned and matched with the final coordinate position information of each obstacle in the global subgraph in the two-dimensional map in the map coordinate system to obtain the optimal posture of the robot at the current moment and the local subgraph and the global subgraph with current trajectory information. The method includes: when the robot is restarted, the saved final two-dimensional map is loaded, the pure positioning mode is turned on, and the previous trajectory is stopped to create a new trajectory; the center position of the global subgraph in the two-dimensional map is calculated; The center position is set as the center position of the laser radar scan at the current moment, and one or more candidate poses are globally searched, and the position information of the obstacles scanned by the laser radar at each candidate pose is calculated with the coordinate position information of each obstacle in the global sub-map in the map coordinate system for scanning and matching. The initial pose at the current moment is selected based on the matching score of each candidate pose, and the initial pose is nonlinearly optimized to obtain the optimal pose at the current moment and the matching score of the optimal pose. The sub-maps in the final two-dimensional map are updated according to the optimal pose, and the local sub-map and the global sub-map with current trajectory information are obtained.

[0009] In one embodiment of the present invention, the method of using the local subgraph with the previous trajectory information to perform closed-loop detection on the trajectory nodes in the subgraph with the current trajectory information, the completed old local subgraph with the current trajectory information, and the trajectory nodes saved in the previous trajectory information, and establishing a closed-loop constraint includes: checking the number of local subgraphs with the current trajectory information, and when the number of local subgraphs is greater than two, deleting the first constructed subgraph; establishing observation constraints between the two most recently inserted local subgraphs under the current trajectory and the current trajectory nodes; using each local subgraph with the previous trajectory information to perform closed-loop detection on the trajectory nodes in the current trajectory information, and establishing closed-loop constraints; performing closed-loop detection on the old local subgraph in the completed state and all the saved trajectory nodes of the previous trajectory, and establishing closed-loop constraints.

[0010] In one embodiment of the present invention, when the number of trajectory nodes corresponding to the local subgraph with current trajectory information is greater than the optimization threshold, the constraints are calculated, the optimization solution is performed, the global pose of the local subgraph under the optimized current trajectory and the global pose of the trajectory node are obtained, and the optimal pose at the current moment is updated to obtain the final pose at the current moment, including: when the number of trajectory nodes corresponding to the subgraph with current trajectory information is greater than the optimization threshold, the optimization variables are added, the residual is constructed, and the constraints are calculated; the global pose of the local subgraph under the optimized current trajectory and the global pose of the trajectory node are obtained, and they are updated; a transfer matrix is ​​obtained based on the global pose corresponding to the updated local subgraph with current trajectory information, and the optimal pose at the current moment is updated according to the transfer matrix to obtain the final pose at the current moment.

[0011] In one embodiment of the present invention, the trajectory information includes: an index of a current trajectory and / or position information of a trajectory node.

[0012] To achieve the above-mentioned purpose and other related purposes, the present invention provides a multi-subgraph laser radar positioning system, which is applied to a mobile robot and is provided with a laser radar sensor, an odometer and a gyroscope. The system comprises: a mapping module, which is used to construct at least two local subgraphs and a global subgraph in the current environment based on the position information of each obstacle in the current environment collected by the laser radar sensor and a map coordinate system constructed with the initial posture information of the robot collected by the odometer and the gyroscope, so as to obtain a two-dimensional map composed of each subgraph; wherein the two-dimensional map comprises: the coordinate position information of each obstacle in the map coordinate system; an optimal posture acquisition module, connected to the mapping module, for obtaining the optimal posture information of each obstacle in the current environment; The position information of the obstacles collected by the laser radar sensor at the current moment is scanned and matched with the coordinate position information of each obstacle corresponding to the global sub-map in the two-dimensional map in the map coordinate system, so as to obtain the optimal posture of the robot at the current moment; the sub-map updating module is connected to the optimal posture acquisition module, and is used to obtain the current final posture of the robot according to the optimal posture, and the position information of the obstacles collected by the laser radar sensor at the current moment is inserted into each local sub-map and the global sub-map with the final posture, so as to update the two-dimensional map and the trajectory information of each local sub-map and the global sub-map; the final map acquisition module is connected to the sub-map updating module, and is used to receive the corresponding obstacle scanning information. When the control instruction is completed, the updating of the two-dimensional map is stopped, and the most recently updated two-dimensional map is saved as the final two-dimensional map; wherein the final two-dimensional map includes: the final coordinate position information of each obstacle in the map coordinate system and the final trajectory information of each sub-graph; a current trajectory acquisition module is connected to the final map acquisition module, and is used to load the saved final two-dimensional map when the robot is restarted, and scan and match the position information of the obstacles collected by the lidar sensor at the current moment with the final coordinate position information of each obstacle in the global sub-graph in the two-dimensional map in the map coordinate system, so as to obtain the optimal posture of the robot at the current moment and the final trajectory information of each obstacle with the current trajectory. a local subgraph and a global subgraph of the trajectory information; a closed-loop detection module, connected to the current trajectory acquisition module, used to use the local subgraph with the previous trajectory information to perform closed-loop detection on the trajectory nodes in the current trajectory information, the completed old local subgraph with the current trajectory information, and the trajectory nodes saved in the previous trajectory information, and establish closed-loop constraints; an optimization module, connected to the closed-loop detection module, used to start calculating constraints and perform optimization solutions when the number of trajectory nodes corresponding to the local subgraph with the current trajectory information is greater than the optimization threshold, obtain the global pose of the local subgraph under the optimized current trajectory and the global pose of the trajectory nodes, and update the optimal pose at the current moment to obtain the final pose at the current moment.

[0013] To achieve the above-mentioned purpose and other related purposes, the present invention provides a multi-sub-image laser radar positioning terminal, comprising: a memory for storing computer programs; and a processor for executing the multi-sub-image laser radar positioning method.

[0014] As described above, a multi-subgraph lidar positioning method, system and terminal of the present invention have the following beneficial effects: the present invention uses data collected by a lidar sensor, an odometer and a gyroscope to construct at least two subgraphs, and uses the scanning points of the current lidar to scan and match the built subgraphs. Combining multiple subgraphs can obtain high-precision posture and reduce cumulative errors; the environmental information scanned by the current lidar is used for closed-loop detection with the established subgraphs to establish constraints, eliminate cumulative positioning errors, and solve the problem that cumulative errors are difficult to eliminate; and when the robot is disturbed by a dynamic object and causes incorrect positioning, a repositioning thread or a fusion positioning thread is started, which effectively solves the problem that the robot does not know its position for a long time and loses its ability to act autonomously, increases the flexibility of the robot, and can also effectively solve the situation of positioning errors, increasing the stability of the robot's work and the accuracy of positioning. BRIEF DESCRIPTION OF THE DRAWINGS

[0015] Figure 1 Shown is a schematic flow chart of a multi-sub-image lidar positioning method according to an embodiment of the present invention.

[0016] Figure 2 Shown is a schematic diagram of the structure of a multi-sub-image laser radar positioning system in one embodiment of the present invention.

[0017] Figure 3 Shown is a schematic diagram of the structure of a multi-sub-image laser radar positioning terminal in one embodiment of the present invention. DETAILED DESCRIPTION

[0018] The following describes the embodiments of the present invention by specific examples, and those skilled in the art can easily understand other advantages and effects of the present invention from the contents disclosed in this specification. The present invention can also be implemented or applied through other different specific embodiments, and the details in this specification can also be modified or changed in various ways based on different viewpoints and applications without departing from the spirit of the present invention. It should be noted that the following embodiments and features in the embodiments can be combined with each other without conflict.

[0019] It should be noted that in the following description, reference is made to the accompanying drawings, which describe several embodiments of the present invention. It should be understood that other embodiments may also be used, and that mechanical composition, structural, electrical and operational changes may be made without departing from the spirit and scope of the present invention. The following detailed description should not be considered restrictive, and the scope of the embodiments of the present invention is limited only by the claims of the published patents. The terms used herein are only for describing specific embodiments and are not intended to limit the present invention. Spatially related terms, such as "upper", "lower", "left", "right", "below", "below", "lower", "above", "upper", etc., may be used in the text to facilitate the description of the relationship between an element or feature shown in the figure and another element or feature.

[0020] Throughout the specification, when a part is said to be "connected" to another part, this includes not only the case of "direct connection" but also the case of "indirect connection" by placing other elements therebetween. In addition, when a part is said to "include" a certain constituent element, unless otherwise stated, it does not exclude other constituent elements, but means that other constituent elements may be included.

[0021] The terms first, second and third are used to describe various parts, components, regions, layers and / or segments, but are not limited thereto. These terms are only used to distinguish a certain part, component, region, layer or segment from other parts, components, regions, layers or segments. Therefore, the first part, component, region, layer or segment described below may refer to the second part, component, region, layer or segment within the scope of the present invention.

[0022] Furthermore, as used herein, the singular forms "a", "an" and "the" are intended to include the plural forms as well, unless there is an indication to the contrary in the context. It should be further understood that the terms "comprise", "include" indicate the presence of the described features, operations, elements, components, items, kinds, and / or groups, but do not exclude the presence, occurrence or addition of one or more other features, operations, elements, components, items, kinds, and / or groups. The terms "or" and "and / or" used herein are interpreted as inclusive, or mean any one or any combination. Therefore, "A, B or C" or "A, B and / or C" means "any of the following: A; B; C; A and B; A and C; B and C; A, B and C". Exceptions to this definition will only occur when the combination of elements, functions or operations is inherently mutually exclusive in some way.

[0023] A multi-subgraph laser radar positioning method is provided in an embodiment of the present invention. The present invention uses data collected by a laser radar sensor, an odometer and a gyroscope to construct at least two subgraphs, and uses the scanning points of the current laser radar to scan and match the built subgraphs. Combining multiple subgraphs can obtain high-precision posture and reduce cumulative errors; the environmental information scanned by the current laser radar is used to perform closed-loop detection with the established subgraphs, thereby establishing constraints and eliminating cumulative positioning errors, solving the problem that cumulative errors are difficult to eliminate; and when the robot is disturbed by a dynamic object and causes incorrect positioning, a repositioning thread or a fused positioning thread is started, which effectively solves the problem that the robot does not know its position for a long time and loses its autonomous behavior ability, increases the flexibility of the robot, and can also effectively solve the situation of positioning errors, increasing the stability of the robot's work and the accuracy of positioning.

[0024] The following is a detailed description of the embodiments of the present invention with reference to the accompanying drawings so that those skilled in the art can easily implement the present invention. The present invention can be embodied in many different forms and is not limited to the embodiments described herein.

[0025] like Figure 1 As shown, a schematic diagram of the flow chart of the multi-sub-graph laser radar positioning method in an embodiment of the present invention is shown.

[0026] Applied to a mobile robot, the method comprises:

[0027] Step S11: Based on the position information of each obstacle in the current environment collected by the lidar sensor and the map coordinate system constructed with the initial posture information of the robot collected by the odometer and the gyroscope, at least two local sub-graphs and a global sub-graph in the current environment are constructed respectively to obtain a two-dimensional map composed of the sub-graphs.

[0028] Specifically, the two-dimensional map includes: coordinate position information of each obstacle in the map coordinate system.

[0029] Optionally, step S11 includes: constructing a map coordinate system with the initial posture information of the robot collected by the odometer and the gyroscope; converting the position information of each obstacle in the current environment collected by the lidar sensor from the lidar coordinate system to the same robot coordinate system, and then converting it to the world coordinate system to obtain the motion distortion removed position information of each obstacle in the current environment collected by the lidar sensor; based on the lidar frame threshold, constructing one or more local sub-graphs in sequence to obtain a two-dimensional map composed of local sub-graphs and a global sub-graph.

[0030] When the map is built, a global sub-map is created. This global sub-map exists from the beginning to the end of the map building. It is used for scanning and matching to obtain the pose. Other local sub-maps are only used to insert obstacles and update. When the laser radar reaches a certain threshold, the obstacle information of other local sub-maps will no longer be inserted.

[0031] Optionally, the position information of each obstacle in the current environment collected by the laser radar sensor is converted from the laser radar coordinate system to the same robot coordinate system, and then converted to the world coordinate system to obtain the position information of each obstacle in the current environment collected by the laser radar sensor with motion distortion removed, including: calculating the robot's linear velocity and angular velocity by using the robot's latest pose information and the pose information of the previous moment collected in real time by the odometer and gyroscope; for each laser point of the laser radar, obtaining its timestamp, subtracting the timestamp of the first laser point from the timestamp of the current laser point, and obtaining the time interval of the robot's movement. Assuming that the robot is in uniform motion during a laser radar data reception period, calculate the robot pose corresponding to the timestamp of each laser point, establish a coordinate system with the current robot pose, calculate the pose transformation matrix of the robot pose corresponding to the timestamp of the current laser point and the robot pose corresponding to the timestamp of the first laser point, multiply the pose transformation matrix by the current laser point coordinates, and finally convert all laser point coordinates to the same robot coordinate system, thereby completing the removal of laser radar motion distortion.

[0032] Optionally, the number of local subgraphs to be constructed is determined by checking the number of lidar frames in the local subgraph. If the number of lidar frames inserted into the current subgraph is greater than the set lidar threshold, the local subgraph is set to a completed state, and a new local subgraph is created with the current robot's position as the origin.

[0033] Optionally, the origin pose of the local sub-graph is a local pose, that is, a pose before optimization, and the optimized sub-graph pose is called a global pose.

[0034] Step S12: Scan and match the position information of the obstacles collected by the laser radar sensor at the current moment with the coordinate position information of each obstacle corresponding to the global sub-map in the two-dimensional map in the map coordinate system, so as to obtain the optimal posture of the robot at the current moment.

[0035] Optionally, step S12 includes: based on the robot's posture information collected in real time by the odometer and gyroscope and / or the optimal posture at the previous moment, calculating the acceleration and linear velocity of the robot to obtain the robot's posture change information; obtaining the predicted posture at the current moment according to the robot's posture change information; using the predicted posture as the central posture of the lidar scan at the current moment and searching for one or more candidate postures within a set first search range, and calculating the matching score of the position information of the obstacles scanned by the lidar at each candidate posture and the coordinate position information of each obstacle in the global sub-map on the two-dimensional map in the map coordinate system for scanning matching; selecting the initial posture at the current moment based on the matching score of each candidate posture, and then performing nonlinear optimization on the initial posture to obtain the optimal posture at the current moment and the matching score of the optimal posture.

[0036] Optionally, the robot's acceleration and linear velocity are calculated based on the robot's posture information collected in real time by the odometer and gyroscope and / or the optimal posture at the last moment to obtain the robot's posture change information, including: using the robot's optimal posture matched by the laser scan at the last moment, the robot's posture information collected by the odometer and gyroscope at the previous moment, and the robot's posture information collected by the odometer and gyroscope at the latest moment, obtaining the time difference between the two postures, and calculating the robot's linear velocity and angular velocity. Then, according to the robot's linear velocity and angular velocity and the time difference between the current robot moment and the last laser scan match, the robot's posture change information, such as rotation and displacement, is calculated.

[0037] Optionally, the method of obtaining the predicted posture at the current moment according to the posture change information of the robot includes: adding the final posture of the robot at the previous moment to the rotation and displacement of the robot, which is the predicted posture at the current moment.

[0038] Optionally, the method of using the predicted posture as the central posture of the laser radar scan at the current moment and searching one or more candidate postures in a set first search range, and calculating the matching score of scanning and matching the position information of the obstacle scanned by the laser radar at each candidate posture with the coordinate position information of each obstacle in the global sub-map on the two-dimensional map in the map coordinate system includes:

[0039] The predicted posture is used as the central posture of the current laser scanning matching, and a search window is created with the set first search range, and one or more candidate postures are searched in the database consisting of the laser point clouds scanned by the laser radar and their corresponding posture information; it should be noted that before searching, not only the size of the search window can be set, but also the search angle can be set; and under each candidate posture, the corresponding laser point cloud is respectively projected on each grid map, and the number of laser points with an occupation probability value greater than a preset threshold is calculated, and then divided by the total number of laser points, which is the matching score of the current candidate posture; wherein, the preset threshold can be set according to needs.

[0040] Optionally, the initial posture at the current moment is selected based on the matching score of each candidate posture, and the initial posture is then nonlinearly optimized to obtain the optimal posture at the current moment and the matching score of the optimal posture, including: selecting the candidate posture with the highest matching score as the initial posture; using the acquired initial posture and laser point cloud to construct a least squares equation for nonlinear optimization, adding the cost function of the point cloud, the cost function of the translation, and the cost function of the rotation as well as their respective weights for optimization and solution, and the final posture obtained is the optimal posture of the robot and the matching score of the optimal posture.

[0041] Step S13: Obtain the current final posture of the robot according to the optimal posture, and insert the position information of the obstacle collected by the lidar sensor at the current moment into each local sub-map and the global sub-map with the final posture to update the two-dimensional map and the trajectory information of each local sub-map and the global sub-map.

[0042] Optionally, the trajectory information includes: an index of a current trajectory and / or pose information of a trajectory node.

[0043] Optionally, the motion distortion-removed position information of the obstacle points scanned by the laser radar is inserted into each local sub-map and the global sub-map with the final posture, the coordinate position of each obstacle point is updated, and the idle area between the robot and the obstacle point is updated, and the number of laser radar frames is accumulated and counted.

[0044] Optionally, step S13 includes: based on the positioning judgment condition, judging whether inaccurate positioning occurs during the positioning process; if the positioning judgment condition is met, judging that inaccurate positioning occurs during the positioning process; if the positioning judgment condition is not met, judging that no inaccurate positioning occurs during the positioning process; if no inaccurate positioning occurs, taking the optimal posture as the final posture of the robot; if inaccurate positioning occurs, taking the predicted posture at the current moment as the final posture at the current moment; inserting the position information of the obstacle collected by the laser radar sensor at the current moment into each sub-graph with the final posture to update the two-dimensional map and the trajectory information of each sub-graph; wherein the inaccurate positioning condition includes: the matching score of the optimal posture at the current moment is less than the set first threshold, the difference between the optimal posture at the current moment and the optimal posture at the previous moment is greater than the set second threshold, the difference between the optimal posture at the current moment and the predicted posture at the current moment is greater than the set third threshold, and the difference between the predicted posture at the current moment and the optimal posture at the previous moment is less than the set fourth threshold. One or more of the following situations

[0045] It should be noted that the relocation threshold, fusion positioning threshold, second threshold, third threshold and fourth threshold can be set according to specific needs and are not limited in this application.

[0046] Step S14: upon receiving a control instruction indicating that the scanning of the corresponding obstacle is completed, the updating of the two-dimensional map is stopped, and the latest updated two-dimensional map is saved as the final two-dimensional map.

[0047] Specifically, the final two-dimensional map includes: final coordinate position information of each obstacle in the map coordinate system and final trajectory information of each sub-map.

[0048] Optionally, after each subgraph is created, the index of the trajectory, trajectory nodes, and constraint data corresponding to the subgraph are saved.

[0049] Optionally, after the map is built, turn on the pure positioning mode, that is, add a new track, stop updating the map, stop inserting the LiDAR scanned points into the 2D map, and save the map.

[0050] Optionally, when a control instruction corresponding to the completion of scanning all obstacles is received, the updating of each submap is stopped, and the latest updated two-dimensional map is saved as the final two-dimensional map. Preferably, the saved map is in the form of a grid map, and the grid map includes: map boundary information, the number of grids, and the occupancy probability of each grid. It should be noted that the resolution of the map is set according to demand, for example, 0.05m.

[0051] Using a map that is no longer updated for subsequent steps reduces errors and increases accuracy during the positioning process, compared to the prior art in which the map is constantly updated during the repositioning process.

[0052] Step S15: When the robot is restarted, the saved final two-dimensional map is loaded, and the position information of the obstacles collected by the lidar sensor at the current moment is scanned and matched with the final coordinate position information of each obstacle in the two-dimensional map in the map coordinate system to obtain the optimal posture of the robot at the current moment and the local sub-map and the global sub-map with the current trajectory information.

[0053] Optionally, step S15 includes: when the robot is restarted, the saved final two-dimensional map including the local pose of the sub-graph, the global pose, the boundary of the sub-graph and the grid occupancy probability is loaded, and the previous trajectory is stopped to create a new trajectory; the center position of the global sub-graph is calculated, and the center position is used as the central pose of the laser radar scan at the current moment to globally search for one or more candidate poses and calculate the matching score of the position information of the obstacles scanned by the laser radar under each candidate pose and the coordinate position information of each obstacle in each sub-graph on the two-dimensional map in the map coordinate system for scanning matching; based on the matching score of each candidate pose, the initial pose at the current moment is selected, and the initial pose is nonlinearly optimized to obtain the optimal pose at the current moment and the matching score of the optimal pose; when each sub-graph in the final two-dimensional map is updated according to the optimal pose, one or more local sub-graphs and a global sub-graph with current trajectory information are obtained.

[0054] Step S16: using the local subgraph with the previous trajectory information, perform closed-loop detection on the trajectory nodes in the current trajectory information, the completed old local subgraph with the current trajectory information, and the trajectory nodes saved in the previous trajectory information, to establish closed-loop constraints.

[0055] Optionally, step S16 includes: checking the number of subgraphs with current trajectory information, and when the number of local subgraphs is greater than two, deleting the first constructed local subgraph; establishing observation constraints with the two most recently inserted local subgraphs under the current trajectory and the current trajectory nodes; performing closed-loop detection on the trajectory nodes in the current trajectory information using the local subgraphs with the previous trajectory information, and establishing closed-loop constraints; performing closed-loop detection using the old local subgraphs in the completed state and all the saved trajectory nodes of the previous trajectory, and establishing closed-loop constraints.

[0056] The trajectory node is composed of a timestamp, a local pose and a laser point cloud.

[0057] Optionally, step S16 includes: maintaining the latest two local subgraphs related to the optimization, checking the number of local subgraphs on the current trajectory, and when the number of created local subgraphs is greater than two, deleting the oldest local subgraph, and at the same time deleting the trajectory nodes on the local subgraph and the constraint data established with the subgraph; establishing observation constraints on the current trajectory node and the two latest inserted local subgraphs on the new trajectory; performing closed-loop detection on the current trajectory node and each subgraph with the previous trajectory information based on the set trajectory node sampling rate, and when the sampling rate is set to 0.2, performing closed-loop detection every 5 trajectory nodes, and establishing constraints; when the state of the old subgraph in the two latest inserted local subgraphs is the completed state, using the old subgraph and all trajectory nodes saved in the previous trajectory to perform closed-loop detection and establish closed-loop constraints;

[0058] Step S17: When the number of trajectory nodes corresponding to the local subgraph with the current trajectory information is greater than the optimization threshold, start calculating the constraints, perform optimization and solve, obtain the global pose of the local subgraph under the optimized current trajectory and the global pose of the trajectory node, and update the optimal pose at the current moment to obtain the final pose at the current moment.

[0059] Optionally, step S17 includes: when the number of trajectory nodes corresponding to the subgraph with current trajectory information is greater than the optimization threshold, adding optimization variables, wherein the optimization variables include the global pose of the local subgraph and the global pose of the trajectory node; and optimizing each pose information corresponding to the local subgraph with current trajectory information based on the closed-loop constraint to update each pose information corresponding to the local subgraph with current trajectory information; obtaining a transfer matrix based on each pose information corresponding to the updated subgraph with current trajectory information, and updating the optimal pose at the current moment according to the transfer matrix to obtain the final pose at the current moment.

[0060] Optionally, when the number of pose information corresponding to the subgraph with current trajectory information is greater than the optimization threshold, the optimization variables are added, and the pose information corresponding to the subgraph with current trajectory information is optimized based on the closed-loop constraint to update the pose information corresponding to the subgraph with current trajectory information, including: when the number of pose information accumulated by the optimization node counter is greater than the set threshold, the least square equation is constructed to start nonlinear optimization. Adding optimization variables mainly includes the subgraph pose and trajectory node pose in the world coordinate system, using constraints to construct residuals, and calculating constraints; the constraints mainly include one or more of the pose transformation between the subgraph pose and the trajectory node in the subgraph, the pose transformation between the trajectory node pose searched by the closed-loop detection and the subgraph pose that searches for this trajectory node pose, and the pose transformation between trajectory nodes and nodes. When the constraint calculation is completed, only the optimized subgraph global pose and the global pose of the trajectory node under the current trajectory are obtained, and the global pose of the old subgraph and trajectory node is updated to the latest optimized global pose.

[0061] Optionally, the transfer matrix is ​​obtained based on the pose information corresponding to the updated subgraph with the current trajectory information, and the optimal pose at the current moment is updated according to the transfer matrix to obtain the final pose at the current moment, including: calculating the transfer matrix from the global pose of the last old subgraph participating in the optimization to the global pose of the subgraph that has just been optimized, and updating the subsequent trajectory nodes that did not participate in the optimization to the latest global pose with this transfer matrix, and the current robot pose is also updated with this transfer matrix.

[0062] Similar to the principle of the above embodiment, the present invention provides a multi-sub-image laser radar positioning system.

[0063] The following provides specific embodiments in conjunction with the accompanying drawings:

[0064] like Figure 2 A schematic structural diagram of a multi-sub-graph laser radar positioning system in an embodiment of the present invention is shown.

[0065] Applied to a mobile robot, on which a laser radar sensor, an odometer and a gyroscope are provided, the system comprises:

[0066] A mapping module 21 is used to construct at least two local sub-maps and a global sub-map in the current environment based on the position information of each obstacle in the current environment collected by the laser radar sensor and the map coordinate system constructed by the initial posture information of the robot collected by the odometer and the gyroscope, so as to obtain a two-dimensional map composed of each sub-map; wherein the two-dimensional map includes: coordinate position information of each obstacle in the map coordinate system;

[0067] The optimal posture acquisition module 22 is connected to the mapping module 21 and is used to scan and match the position information of the obstacles collected by the laser radar sensor at the current moment with the coordinate position information of each obstacle corresponding to the global sub-map in the two-dimensional map in the map coordinate system, so as to obtain the optimal posture of the robot at the current moment;

[0068] A sub-graph updating module 23, connected to the optimal posture acquisition module 22, is used to obtain the current final posture of the robot according to the optimal posture, and insert the position information of the obstacle collected by the laser radar sensor at the current moment into each local sub-graph and the global sub-graph with the final posture, so as to update the two-dimensional map and the trajectory information of each local sub-graph and the global sub-graph;

[0069] a final map acquisition module, connected to the sub-map update module, for stopping updating the two-dimensional map upon receiving a control instruction indicating that the corresponding obstacle scan is completed, and saving the latest updated two-dimensional map as the final two-dimensional map; wherein the final two-dimensional map includes: the final coordinate position information of each obstacle in the map coordinate system and the final trajectory information of each sub-map;

[0070] A current trajectory acquisition module, connected to the final map acquisition module, is used to load the saved final two-dimensional map when the robot is restarted, and scan and match the position information of the obstacles collected by the lidar sensor at the current moment with the final coordinate position information of each obstacle in the global sub-map in the two-dimensional map in the map coordinate system, so as to obtain the optimal posture of the robot at the current moment and the local sub-map and the global sub-map with the current trajectory information;

[0071] A closed-loop detection module, connected to the current trajectory acquisition module, is used to perform closed-loop detection on the trajectory nodes in the current trajectory information and the completed old local subgraph with the current trajectory information and the trajectory nodes saved in the previous trajectory information by using the local subgraph with the previous trajectory information, and establish a closed-loop constraint;

[0072] The optimization module is connected to the closed-loop detection module and is used to start calculating constraints and performing optimization when the number of trajectory nodes corresponding to the local subgraph with current trajectory information is greater than the optimization threshold, obtain the global pose of the local subgraph under the optimized current trajectory and the global pose of the trajectory node, and update the optimal pose at the current moment to obtain the final pose at the current moment.

[0073] It should be understood that Figure 2The division of each module in the system embodiment is only a division of logical functions. In actual implementation, they can be fully or partially integrated into one physical entity, or physically separated. And these modules can be all implemented in the form of software calling through processing elements; or all implemented in the form of hardware; or some modules can be implemented in the form of software calling through processing elements, and some modules can be implemented in the form of hardware;

[0074] For example, each module may be one or more integrated circuits configured to implement the above method, such as: one or more application specific integrated circuits (ASIC), or one or more digital signal processors (DSP), or one or more field programmable gate arrays (FPGA). For another example, when a module above is implemented in the form of a processing element scheduling program code, the processing element may be a general-purpose processor, such as a central processing unit (CPU) or other processor that can call program code. For another example, these modules may be integrated together and implemented in the form of a system-on-a-chip (SOC).

[0075] Therefore, since the implementation principle of the multi-sub-graph laser radar positioning system has been described in the aforementioned embodiment, it will not be repeated here.

[0076] like Figure 3 A schematic diagram of the structure of the multi-sub-graph laser radar positioning terminal 30 in an embodiment of the present invention is shown.

[0077] The multi-sub-image laser radar positioning terminal 30 includes: a memory 31 and a processor 32. The memory 31 is used to store computer programs; the processor 32 runs the computer program to implement the following Figure 1 The multi-sub-graph laser radar positioning method.

[0078] Optionally, the number of the memory 31 can be one or more, the number of the processor 32 can be one or more, and Figure 3 Take one as an example.

[0079] Optionally, the processor 32 in the multi-sub-image laser radar positioning terminal 30 will perform the following operations: Figure 1 In the above steps, one or more instructions corresponding to the process of the application are loaded into the memory 31, and the processor 32 runs the application stored in the first memory 31, thereby achieving the following Figure 1Various functions in the multi-sub-image lidar localization method.

[0080] Optionally, the memory 31 may include but is not limited to high-speed random access memory and non-volatile memory. For example, one or more disk storage devices, flash memory devices or other non-volatile solid-state storage devices; the processor 32 may include but is not limited to a central processing unit (CPU), a network processor (NP), etc.; it may 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 gates or transistor logic devices, discrete hardware components.

[0081] Optionally, the processor 32 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 gates or transistor logic devices, discrete hardware components.

[0082] The present invention also provides a computer-readable storage medium storing a computer program, wherein the computer program is executed as follows: Figure 1 The multi-sub-graph laser radar positioning method shown. The computer-readable storage medium may include, but is not limited to, a floppy disk, an optical disk, a CD-ROM (read-only disk memory), a magneto-optical disk, a ROM (read-only memory), a RAM (random access memory), an EPROM (erasable programmable read-only memory), an EEPROM (electrically erasable programmable read-only memory), a magnetic card or an optical card, a flash memory, or other types of media / machine-readable media suitable for storing machine-executable instructions. The computer-readable storage medium may be a product that is not connected to a computer device, or a component that is connected to a computer device for use.

[0083] In summary, the multi-sub-graph laser radar positioning method, system and terminal of the present invention use the data collected by the laser radar sensor, odometer and gyroscope to construct at least two sub-graphs, and use the scanning points of the current laser radar to scan and match the built sub-graphs, and combine the multiple sub-graphs to obtain high-precision posture and reduce the cumulative error; use the environmental information scanned by the current laser radar and the established sub-graphs for closed-loop detection, so as to establish constraints, eliminate the cumulative positioning error, and solve the problem that the cumulative error is difficult to eliminate; and when the robot is disturbed by a dynamic object and causes an incorrect positioning, the repositioning thread or the fusion positioning thread is turned on, which effectively solves the problem that the robot does not know its position for a long time and loses the ability to act autonomously, increases the flexibility of the robot, and can also effectively solve the situation of positioning errors, increasing the stability of the robot's work and the accuracy of positioning. Therefore, the present invention effectively overcomes the various shortcomings in the prior art and has a high industrial utilization value.

[0084] The above embodiments are merely illustrative of the principles and effects of the present invention, and are not intended to limit the present invention. Anyone familiar with the technology may modify or change the above embodiments without violating the spirit and scope of the present invention. Therefore, all equivalent modifications or changes made by a person of ordinary skill in the art without departing from the spirit and technical ideas disclosed by the present invention shall still be covered by the claims of the present invention.

Claims

1. A multi-sub-image laser radar positioning method, characterized in that: Applied to a mobile robot, on which a laser radar sensor, an odometer and a gyroscope are provided, the method comprises: Based on the position information of each obstacle in the current environment collected by the laser radar sensor and the map coordinate system constructed with the initial posture information of the robot collected by the odometer and the gyroscope, at least two local sub-graphs and a global sub-graph in the current environment are respectively constructed to obtain a two-dimensional map composed of each sub-graph; wherein the two-dimensional map includes: the coordinate position information of each obstacle in the map coordinate system; this step specifically includes: constructing a map coordinate system with the initial posture information of the robot collected by the odometer and the gyroscope; converting the position information of each obstacle in the current environment collected by the laser radar sensor into the same robot coordinate system, and then converting it into the world coordinate system to obtain the motion distortion removed position information of each obstacle in the current environment collected by the laser radar sensor; based on the laser radar frame threshold, constructing one or more local sub-graphs in sequence to obtain a two-dimensional map composed of each local sub-graph and a global sub-graph; wherein the global sub-graph exists from the beginning to the end of the map construction, and is used to perform scanning matching to obtain the posture; the local sub-graph only inserts obstacles and updates; when the laser radar reaches a certain threshold, the obstacle information is no longer inserted; Scan and match the position information of the obstacles collected by the laser radar sensor at the current moment with the coordinate position information of each obstacle corresponding to the global sub-map in the two-dimensional map in the map coordinate system, so as to obtain the optimal posture of the robot at the current moment; The current final posture of the robot is obtained according to the optimal posture, and the position information of the obstacle collected by the laser radar sensor at the current moment is inserted into each local sub-map and the global sub-map with the final posture, so as to update the two-dimensional map and the trajectory information of each local sub-map and the global sub-map; wherein, this step specifically includes: based on the positioning judgment condition, judging whether inaccurate positioning occurs during the positioning process; if inaccurate positioning does not occur, taking the optimal posture as the final posture of the robot; if inaccurate positioning occurs, taking the predicted posture at the current moment as the final posture at the current moment; inserting the position information of the obstacle collected by the laser radar sensor at the current moment into each sub-map with the final posture, so as to update the two-dimensional map and the trajectory information of each sub-map; When receiving a control instruction indicating that the corresponding obstacle scan is completed, the updating of the two-dimensional map is stopped, and the latest updated two-dimensional map is saved as the final two-dimensional map; wherein the final two-dimensional map includes: the final coordinate position information of each obstacle in the map coordinate system and the final trajectory information of each sub-map; When the robot is restarted, the saved final two-dimensional map is loaded, and the position information of the obstacles collected by the lidar sensor at the current moment is scanned and matched with the final coordinate position information of each obstacle in the global subgraph in the two-dimensional map in the map coordinate system, so as to obtain the optimal posture of the robot at the current moment and the local subgraph and the global subgraph with the current trajectory information; Using the local subgraph with the previous trajectory information, the trajectory nodes in the current trajectory information, the completed old local subgraph with the current trajectory information, and the trajectory nodes saved in the previous trajectory information are closed-loop detected to establish closed-loop constraints; wherein, this step specifically includes: checking the number of local subgraphs with the current trajectory information, and when the number of local subgraphs is greater than two, deleting the first constructed subgraph; establishing observation constraints between the two most recently inserted local subgraphs under the current trajectory and the current trajectory nodes; using each local subgraph with the previous trajectory information to perform closed-loop detection on the trajectory nodes in the current trajectory information, and establish closed-loop constraints; performing closed-loop detection on the old local subgraph in the completed state and all the saved trajectory nodes of the previous trajectory, and establishing closed-loop constraints; When the number of trajectory nodes corresponding to the local subgraph with current trajectory information is greater than the optimization threshold, start calculating constraints, perform optimization and solve, obtain the global pose of the local subgraph under the optimized current trajectory and the global pose of the trajectory node, and update the optimal pose at the current moment to obtain the final pose at the current moment; wherein, this step specifically includes: when the number of trajectory nodes corresponding to the subgraph with current trajectory information is greater than the optimization threshold, add optimization variables, construct residuals, and start calculating constraints; obtain the global pose of the local subgraph under the optimized current trajectory and the global pose of the trajectory node, and update them; obtain a transfer matrix based on the global pose corresponding to the updated local subgraph with current trajectory information, and update the optimal pose at the current moment according to the transfer matrix to obtain the final pose at the current moment; Among them, the method of scanning and matching the position information of the obstacles collected by the laser radar sensor at the current moment with the coordinate position information of each obstacle corresponding to the global sub-map in the two-dimensional map in the map coordinate system to obtain the optimal posture of the robot at the current moment includes: Based on the posture information of the robot collected in real time by the odometer and the gyroscope and / or the optimal posture at the last moment, the acceleration and linear velocity of the robot are calculated to obtain the posture change information of the robot; Obtaining a predicted posture at the current moment according to the posture change information of the robot; The predicted posture is used as the central posture of the laser radar scanning at the current moment and one or more candidate postures are searched in a set first search range, and a matching score of scanning and matching between the position information of the obstacle scanned by the laser radar at each candidate posture and the coordinate position information of each obstacle in the local sub-image on the two-dimensional map in the map coordinate system is calculated; The initial posture at the current moment is selected based on the matching scores of the candidate postures, and the initial posture is then nonlinearly optimized to obtain the optimal posture at the current moment and the matching score of the optimal posture.

2. The multi-sub-image laser radar positioning method according to claim 1, characterized in that: The inaccurate positioning conditions include: the matching score of the optimal posture at the current moment is less than a set first threshold, the difference between the optimal posture at the current moment and the optimal posture at the previous moment is greater than a set second threshold, the difference between the optimal posture at the current moment and the predicted posture at the current moment is greater than a set third threshold, and the difference between the predicted posture at the current moment and the optimal posture at the previous moment is less than a set fourth threshold. One or more of the following situations.

3. The multi-sub-graph laser radar positioning method according to claim 1, characterized in that: When the robot is restarted, the saved final two-dimensional map is loaded, and the position information of the obstacles collected by the laser radar sensor at the current moment is scanned and matched with the final coordinate position information of each obstacle in the global sub-graph in the two-dimensional map in the map coordinate system to obtain the optimal posture of the robot at the current moment and the local sub-graph and the global sub-graph with the current trajectory information, including: When the robot is restarted, the saved final two-dimensional map is loaded, a pure positioning mode is turned on, and the previous trajectory is stopped to create a new trajectory; Calculate the center position of the global sub-graph in the two-dimensional map, and use the center position as the center position of the laser radar scan at the current moment to globally search for one or more candidate positions, and calculate the matching score of scanning and matching between the position information of the obstacle scanned by the laser radar at each candidate position and the coordinate position information of each obstacle in the global sub-graph in the map coordinate system; Selecting an initial posture at the current moment based on the matching scores of each candidate posture, and then performing nonlinear optimization on the initial posture to obtain an optimal posture at the current moment and a matching score of the optimal posture; Each sub-graph in the final two-dimensional map is updated according to the optimal posture, and a local sub-graph and a global sub-graph with current trajectory information are obtained.

4. The multi-sub-graph laser radar positioning method according to claim 1, characterized in that: The trajectory information includes: the index of the current trajectory and / or the position information of the trajectory node.

5. A multi-sub-graph laser radar positioning system, characterized in that: Applied to a mobile robot, on which a laser radar sensor, an odometer and a gyroscope are provided, the system comprises: A mapping module, for constructing at least two local sub-graphs and a global sub-graph in the current environment based on the position information of each obstacle in the current environment collected by the laser radar sensor and the map coordinate system constructed with the initial posture information of the robot collected by the odometer and the gyroscope, so as to obtain a two-dimensional map composed of each sub-graph; wherein the two-dimensional map includes: the coordinate position information of each obstacle in the map coordinate system; the working method of the mapping module includes: constructing a map coordinate system with the initial posture information of the robot collected by the odometer and the gyroscope; converting the position information of each obstacle in the current environment collected by the laser radar sensor into the same robot coordinate system, and then converting it into the world coordinate system, so as to obtain the motion distortion removed position information of each obstacle in the current environment collected by the laser radar sensor; based on the laser radar frame threshold, constructing one or more local sub-graphs in sequence, so as to obtain a two-dimensional map composed of each local sub-graph and a global sub-graph; wherein the global sub-graph exists from the beginning to the end of mapping, and is used for scanning matching to obtain posture; the local sub-graph only inserts obstacles and updates; when the laser radar reaches a certain threshold, the obstacle information is no longer inserted; An optimal posture acquisition module, connected to the mapping module, for scanning and matching the position information of obstacles collected by the laser radar sensor at the current moment with the coordinate position information of each obstacle corresponding to the global sub-map in the two-dimensional map in the map coordinate system, so as to obtain the optimal posture of the robot at the current moment; A sub-graph updating module, connected to the optimal posture acquisition module, is used to obtain the current final posture of the robot according to the optimal posture, and insert the position information of the obstacle collected by the laser radar sensor at the current moment into each local sub-graph and the global sub-graph with the final posture, so as to update the two-dimensional map and the trajectory information of each local sub-graph and the global sub-graph; wherein the working method of the sub-graph updating module includes: based on the positioning judgment condition, judging whether the positioning inaccuracy occurs during the positioning process; if the positioning inaccuracy does not occur, taking the optimal posture as the final posture of the robot; if the positioning inaccuracy occurs, taking the predicted posture at the current moment as the final posture at the current moment; inserting the position information of the obstacle collected by the laser radar sensor at the current moment into each sub-graph with the final posture, so as to update the two-dimensional map and the trajectory information of each sub-graph; a final map acquisition module, connected to the sub-map update module, for stopping updating the two-dimensional map upon receiving a control instruction indicating that the corresponding obstacle scan is completed, and saving the latest updated two-dimensional map as the final two-dimensional map; wherein the final two-dimensional map includes: the final coordinate position information of each obstacle in the map coordinate system and the final trajectory information of each sub-map; A current trajectory acquisition module, connected to the final map acquisition module, is used to load the saved final two-dimensional map when the robot is restarted, and scan and match the position information of the obstacles collected by the lidar sensor at the current moment with the final coordinate position information of each obstacle in the global sub-map in the two-dimensional map in the map coordinate system, so as to obtain the optimal posture of the robot at the current moment and the local sub-map and the global sub-map with the current trajectory information; A closed-loop detection module is connected to the current trajectory acquisition module, and is used to use the local subgraph with the previous trajectory information to perform closed-loop detection on the trajectory nodes in the current trajectory information, the completed old local subgraph with the current trajectory information, and the trajectory nodes saved in the previous trajectory information, and establish closed-loop constraints; wherein the working method of the closed-loop detection module includes: checking the number of local subgraphs with the current trajectory information, and when the number of local subgraphs is greater than two, deleting the first constructed subgraph; establishing observation constraints between the two most recently inserted local subgraphs under the current trajectory and the current trajectory nodes; using each local subgraph with the previous trajectory information to perform closed-loop detection on the trajectory nodes in the current trajectory information, and establish closed-loop constraints; performing closed-loop detection on the old local subgraph in the completed state and all the saved trajectory nodes of the previous trajectory, and establishing closed-loop constraints; An optimization module, connected to the closed-loop detection module, is used to start calculating constraints and performing optimization when the number of trajectory nodes corresponding to the local subgraph with current trajectory information is greater than the optimization threshold, obtain the global pose of the optimized local subgraph under the current trajectory and the global pose of the trajectory node, and update the optimal pose at the current moment to obtain the final pose at the current moment; wherein the working method of the optimization module includes: when the number of trajectory nodes corresponding to the subgraph with current trajectory information is greater than the optimization threshold, add optimization variables, construct residuals, and start calculating constraints; obtain the global pose of the optimized local subgraph under the current trajectory and the global pose of the trajectory node, and update them; obtain a transfer matrix based on the global pose corresponding to the updated local subgraph with current trajectory information, and update the optimal pose at the current moment according to the transfer matrix to obtain the final pose at the current moment; Among them, the method of scanning and matching the position information of the obstacles collected by the laser radar sensor at the current moment with the coordinate position information of each obstacle corresponding to the global sub-map in the two-dimensional map in the map coordinate system to obtain the optimal posture of the robot at the current moment includes: Based on the posture information of the robot collected in real time by the odometer and the gyroscope and / or the optimal posture at the last moment, the acceleration and linear velocity of the robot are calculated to obtain the posture change information of the robot; Obtaining a predicted posture at the current moment according to the posture change information of the robot; The predicted posture is used as the central posture of the laser radar scanning at the current moment and one or more candidate postures are searched in a set first search range, and a matching score of scanning and matching between the position information of the obstacle scanned by the laser radar at each candidate posture and the coordinate position information of each obstacle in the local sub-image on the two-dimensional map in the map coordinate system is calculated; The initial posture at the current moment is selected based on the matching scores of the candidate postures, and the initial posture is then nonlinearly optimized to obtain the optimal posture at the current moment and the matching score of the optimal posture.

6. A multi-sub-image laser radar positioning terminal, characterized in that: include: Memory for storing computer programs; A processor, used to execute the multi-sub-image laser radar positioning method as described in any one of claims 1 to 4.