A method and device for constructing a map
Through real-time registration and global optimization techniques, the problem of poor map consistency in high-precision map construction is solved, the accuracy and stability of the map are improved, and the impact of sensor calibration accuracy is overcome.
Patent Information
- Application Number
- CN202010087732.4
- Authority / Receiving Office
- CN · China
- Patent Type
- Patents(China)
- Current Assignee / Owner
- Filing Date
- 2020-02-11
- Publication Date
- 2024-11-15
- Estimated Expiration
- 2040-02-11
AI Technical Summary
The existing high-precision map construction methods have poor map consistency and are greatly affected by the calibration accuracy of sensors such as laser radar, resulting in poor local and global consistency of the map.
By obtaining sensor data on movable objects in real time, registering frames and submaps, establishing relative pose relationships, and global optimization is carried out in combination with the initial poses to form a global map, overcoming the cumulative error and external parameter calibration in traditional methods Accuracy dependency.
It improves the local and global consistency of the map, reduces the impact of lidar external parameter calibration accuracy on the results, and enhances the accuracy and stability of map construction.
Smart Images

Figure CN113252051B_ABST
Abstract
Description
Technical Field
[0001] The present application relates to the field of high-precision map technology, and in particular to a map construction method and device. Background Art
[0002] At present, with the development of autonomous driving technology and intelligent robot technology, how to ensure the accurate driving of autonomous driving vehicles and intelligent robots has become a hot issue. In autonomous driving technology, high-precision maps are generally used. Unlike traditional navigation maps, high-precision maps contain a large amount of driving assistance information. The most important information is the accurate three-dimensional representation of the road network, such as intersection layout and road sign location. In addition, high-precision maps also contain a lot of semantic information. The map may report the meaning of different colors on the communication traffic lights. It may indicate the speed limit of the road and the location of the left turn lane. One of the most important features of high-precision maps is accuracy. High-precision maps can enable autonomous driving vehicles to achieve centimeter-level accuracy, which is crucial to ensuring the safety of autonomous driving vehicles.
[0003] Before a high-precision map is used, it needs to be pre-built. There are generally many methods. For example, a high-precision combined navigation system can be used to obtain the position of the autonomous driving vehicle, and a high-precision map can be built by combining the data collected by a multi-line laser radar. Another example is that SLAM technology (Simultaneous Localization and Mapping) can be used to build a high-precision map. However, the above traditional high-precision map construction methods are generally Figure 1 The consistency is poor and it is greatly affected by the calibration accuracy of sensors such as lidar.
[0004] The embodiment of the present application aims to propose a method for constructing a high-precision map that is different from the above-mentioned construction method. Summary of the invention
[0005] The embodiments of the present application provide a map construction method and device, which can overcome the map construction method existing in the prior art. Figure 1 The consistency is poor and it is greatly affected by the calibration accuracy of sensors such as lidar.
[0006] To achieve the above objectives, the embodiments of the present application adopt the following technical solutions:
[0007] According to a first aspect of an embodiment of the present application, a map construction method is provided, comprising:
[0008] Obtaining sensor data collected by a sensor carried by the movable object in real time, and determining each frame of data in a first sub-map with a preset frame length;
[0009] According to each frame data and a plurality of frame data in the first submap, register each frame with the first submap to determine a first relative position relationship between each frame and the first submap; the plurality of frame data are data from the first frame to a frame before each frame in the first submap;
[0010] Determine the second submaps related to each first submap according to the initial poses corresponding to each first submap, align each first submap with the second submap related thereto, establish a first pose constraint relationship between each first submap and the second submap, and establish a second pose constraint relationship between each first submap and the initial poses corresponding to each first submap;
[0011] Performing global optimization according to the first posture constraint relationship and the second posture constraint relationship to determine the sub-map optimized posture of each first sub-map;
[0012] Determine, according to the first relative posture relationship and the submap optimized posture, a frame optimized posture corresponding to each frame data in each first submap;
[0013] Map construction is performed based on the frame optimization posture and each frame data to form a global map.
[0014] According to a second aspect of an embodiment of the present application, a map construction device is provided, comprising:
[0015] A data acquisition unit, used to obtain sensor data collected by a sensor carried by the movable object in real time, and determine each frame of data in the first sub-map with a preset frame length;
[0016] a frame and submap registration unit, configured to register each frame with the first submap according to each frame data and a plurality of frame data in the first submap, and determine a first relative position relationship between each frame and the first submap; the plurality of frame data are data from the first frame to a frame before each frame in the first submap;
[0017] A submap-to-submap registration unit, configured to determine a second submap associated with each first submap according to an initial position and posture corresponding to each first submap, register each first submap with the second submap associated therewith, establish a first position and posture constraint relationship between each first submap and the second submap, and establish a second position and posture constraint relationship between each first submap and the initial position and posture corresponding to each first submap;
[0018] A global optimization unit, configured to perform global optimization according to the first posture constraint relationship and the second posture constraint relationship to determine a sub-map optimized posture of each first sub-map;
[0019] A frame optimized posture determination unit, used to determine the frame optimized posture corresponding to each frame data in each first submap according to the first relative posture relationship and the submap optimized posture;
[0020] The global map construction unit is used to construct a map based on the frame optimization posture and each frame data to form a global map.
[0021] According to a third aspect of the embodiments of the present application, a computer-readable storage medium is provided, comprising a program or an instruction. When the program or the instruction is executed on a computer, the map construction method described in the first aspect is implemented.
[0022] According to a fourth aspect of the embodiments of the present application, a computer program product comprising instructions is provided. When the computer program product is run on a computer, the computer is enabled to execute the map construction method as described in the first aspect above.
[0023] In a fifth aspect of an embodiment of the present application, a chip system is provided, including a processor, wherein the processor is coupled to a memory, wherein the memory stores program instructions, and when the program instructions stored in the memory are executed by the processor, the map construction method described in the first aspect is implemented.
[0024] According to a sixth aspect of an embodiment of the present application, a circuit system is provided, the circuit system comprising a processing circuit, the processing circuit being configured to execute the map construction method as described in the first aspect above.
[0025] A seventh aspect of the embodiments of the present application provides a computer server, comprising a memory, and one or more processors communicatively connected to the memory;
[0026] The memory stores instructions that can be executed by the one or more processors, and the instructions are executed by the one or more processors to enable the one or more processors to implement the map construction method as described in the first aspect above.
[0027] A map construction method and device provided in an embodiment of the present application can obtain sensor data collected by sensors carried on movable objects in real time, and align each frame with the first submap to determine the first relative posture relationship between each frame and the first submap. After determining the second submap related to the first submap, the first submap can be aligned with the second submap, and a posture constraint relationship can be established in combination with the initial posture of the first submap. Therefore, after globally optimizing the posture constraint relationship, the submap optimized posture of each first submap can be determined. Furthermore, based on the above-mentioned first relative posture relationship and the submap optimized posture, the frame optimized posture corresponding to each frame data in each first submap can be determined. Furthermore, based on the frame optimized posture and each frame data, a map can be constructed to form a global map. In this way, the embodiments of the present application can overcome the local shortcomings of the map construction method in the prior art. Figure 1The consistency is poor and it is greatly affected by the calibration accuracy of sensors such as lidar. BRIEF DESCRIPTION OF THE DRAWINGS
[0028] 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. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying creative labor.
[0029] Figure 1 A structural diagram of a map construction method provided in an embodiment of the present application Figure 1 ;
[0030] Figure 2 A schematic diagram of a laser radar installed on a vehicle in an embodiment of the present application;
[0031] Figure 3 A structural diagram of a map construction method provided in an embodiment of the present application Figure 2 ;
[0032] Figure 4 A global posture relationship diagram in one embodiment of the present application;
[0033] Figure 5 Detailed illustration of the global map to be verified obtained by the map car in the embodiment of the present application Figure 1 ;
[0034] Figure 6 Detailed illustration of the global map to be verified obtained by the map car in the embodiment of the present application Figure 2 ;
[0035] Figure 7 Detailed illustration of the global map to be verified obtained by the truck in the embodiment of the present application Figure 1 ;
[0036] Figure 8 Detailed illustration of the global map to be verified obtained by the truck in the embodiment of the present application Figure 2 ;
[0037] Fig. 9 A schematic diagram of the structure of a map construction device provided in an embodiment of the present application. DETAILED DESCRIPTION
[0038] 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 without creative work are within the scope of protection of this application.
[0039] It should be noted that the terms "first", "second", etc. in the specification and claims of the present application and the above-mentioned drawings are used to distinguish similar objects, and are not necessarily used to describe a specific order or sequential order. It should be understood that the data used in this way can be interchanged where appropriate, so that the embodiments of the present application described here. In addition, the terms "including" and "having" and any of their variations are intended to cover non-exclusive inclusions, for example, a process, method, system, product or device comprising a series of steps or units is not necessarily limited to those steps or units clearly listed, but may include other steps or units that are not clearly listed or inherent to these processes, methods, products or devices.
[0040] In order to enable those skilled in the art to better understand the present application, some technical terms appearing in the embodiments of the present application are explained as follows:
[0041] Movable objects: refers to objects such as vehicles, mobile robots, and aircraft that can be used for map collection. Movable objects can be equipped with various types of sensors, such as lidar, cameras, etc.
[0042] GNSS: Global Navigation Satellite System.
[0043] GPS: Global Positioning System.
[0044] IMU: Inertial Measurement Unit, is a device that measures the three-axis attitude angle (or angular rate) and acceleration of an object.
[0045] High-precision maps: Unlike traditional navigation maps, high-precision maps contain a large amount of driving assistance information. The most important information is the accurate three-dimensional representation of the road network, such as intersection layout and road sign location. In addition, high-precision maps also contain a lot of semantic information. The map may report the meaning of different colors on the communication traffic light. It may indicate the speed limit of the road and the location of the left turn lane. One of the most important features of high-precision maps is accuracy. High-precision maps can enable vehicles to achieve centimeter-level accuracy, which is crucial to ensuring the safety of self-driving cars.
[0046] Mapping: Construct a high-precision map describing the current scene based on the estimated real-time position of the vehicle or mobile robot and the data collected by visual sensors such as lidar.
[0047] Pose: A general term for position and orientation, which includes 6 degrees of freedom, including 3 degrees of freedom for position and 3 degrees of freedom for orientation. The 3 degrees of freedom for orientation are usually expressed by pitch, roll, and yaw.
[0048] Pose Estimation: Estimate the real-time pose of a vehicle or robot based on the environmental information perceived by the sensors carried by the vehicle or mobile robot.
[0049] Frame: The measurement data received by the sensor after completing an observation. For example, one frame of camera data is a picture, and one frame of lidar data is a set of laser point clouds.
[0050] Submap: The global map consists of several submaps, each of which contains observation results of multiple consecutive frames.
[0051] Registration: Match the observation results of the same area at different times and locations to obtain the relative posture relationship between the two observation times.
[0052] Graph Optimization: The optimization problem is expressed in the form of a graph, which consists of nodes and edges. The optimization process is to adjust the position of the nodes so that the constraints of the edges are satisfied to the greatest extent.
[0053] KD-Tree (K-dimensional tree): is a data structure that partitions K-dimensional data space. It is mainly used for searching key data in multidimensional space (such as range search and nearest neighbor search). KD-Tree is a special case of binary space partition tree. In computer science, KD-Tree is a data structure that organizes points in K-dimensional Euclidean space.
[0054] NDT: Normal Distributions Transform, also known as the normal distribution transform algorithm, is a registration algorithm that applies a statistical model of three-dimensional points and uses standard optimization techniques to determine the optimal match between two point clouds.
[0055] SVD: Singular Value Decomposition, is an important matrix decomposition in linear algebra. Singular value decomposition is the generalization of eigendecomposition on any matrix. It has important applications in signal processing and other fields.
[0056] NovAtel: A leading supplier of products and technologies in the field of precision global navigation satellite systems (GNSS) and their subsystems. The embodiment of this application represents the integrated navigation system of NovAtel.
[0057] In some embodiments of the present application, the term "vehicle" is broadly interpreted as including any moving object, including, for example, aircraft, ships, spacecraft, automobiles, trucks, vans, semi-trailers, motorcycles, golf carts, off-road vehicles, warehouse transport vehicles or agricultural vehicles and transportation vehicles running on tracks, such as trams or trains and other rail vehicles. The "vehicle" in the present application can generally include: a power system, a sensor system, a control system, peripheral equipment and a computer system. In other embodiments, the vehicle can include more, fewer or different systems.
[0058] Among them, the power system is the system that provides power movement for the vehicle, including: engine / motor, transmission and wheels / tires, and energy unit.
[0059] The control system may include a combination of devices that control the vehicle and its components, such as a steering unit, a throttle, a brake unit.
[0060] Peripherals may be devices that allow the vehicle to interact with external sensors, other vehicles, external computing devices, and / or users, such as wireless communication systems, touch screens, microphones, and / or speakers.
[0061] Based on the vehicle described above, the autonomous driving vehicle is also equipped with a sensor system and an autonomous driving control device.
[0062] The sensor system may include multiple sensors for sensing information about the environment in which the vehicle is located, and one or more actuators for changing the position and / or orientation of the sensors. The sensor system may include any combination of sensors such as a global positioning system sensor, an inertial measurement unit, a radio detection and ranging (RADAR) unit, a camera, a laser rangefinder, a light detection and ranging (LIDAR) unit, and / or an acoustic sensor; the sensor system may also include sensors that monitor the vehicle's internal systems (e.g., an O2 monitor, a fuel gauge, an engine temperature gauge, etc.).
[0063] The automatic driving control device may include a processor and a memory, wherein at least one machine executable instruction is stored in the memory, and the processor executes at least one machine executable instruction to implement functions including a map engine, a positioning module, a perception module, a navigation or path module, and an automatic control module. The map engine and the positioning module are used to provide map information and positioning information. The perception module is used to perceive things in the environment where the vehicle is located based on the information obtained by the sensor system and the map information provided by the map engine. The navigation or path module is used to plan a driving path for the vehicle based on the processing results of the map engine, the positioning module and the perception module. The automatic control module converts the decision information input of the navigation or path module and other modules into a control command output for the vehicle control system, and sends the control command to the corresponding component in the vehicle control system through the vehicle network (for example, a vehicle internal electronic network system implemented by CAN bus, local area interconnection network, multimedia directional system transmission, etc.) to realize automatic control of the vehicle; the automatic control module can also obtain information of each component in the vehicle through the vehicle network.
[0064] Generally speaking, high-precision maps need to be pre-built before they are used. There are generally several ways to do this:
[0065] For example, method 1: a high-precision combined navigation system can be used to obtain the position and posture of the autonomous driving vehicle, and a high-precision map can be constructed by combining the data collected by the multi-line laser radar. That is, multiple laser radar devices on the autonomous driving vehicle are used to scan the surrounding scene in real time to obtain point cloud data, and a combined navigation system composed of high-precision GPS and IMU is used to measure the real-time position and posture of the autonomous driving vehicle. The real-time position and posture of the laser radar is calculated based on the position and posture relationship obtained by the external parameter calibration between the combined navigation system and the laser radar, and then the point cloud data is superimposed according to the position and posture of the laser radar to obtain a global high-precision map.
[0066] Another example is the second method: SLAM technology can be used to build a high-precision map. That is, the laser radar point cloud data collected in the current frame is continuously registered with the laser point cloud data of the previous moment or the previous moments, so as to incrementally estimate the real-time position of the current vehicle and build a global high-precision map.
[0067] At present, there are generally two standards for evaluating the quality of high-precision map construction - local map consistency and global map consistency. Among them, global map consistency means: when each point generated during map construction is compared with the coordinates of the real point, if the error is less than a certain threshold, then the global consistency can be considered good. Local map consistency refers to: the clarity of the point cloud data representing each local area after map construction. If the local map consistency is poor, for example, the isolation guardrails and lane lines on the vehicle road are not clear, and the thickness of the ground is large (when the local map consistency is high, the ground should have no thickness, or the thickness is small). In general, the construction of the map on the road can be evaluated by the thickness of the ground expressed by the point cloud data and the clarity of the roadside markers to evaluate the quality of the local map consistency.
[0068] At present, the construction method of the above-mentioned method 1 is usually used for mapping in large-scale scenarios (such as large-scale city maps and long highway network maps). The construction method of the above-mentioned method 1 has high mapping efficiency and good global consistency of the map, but is limited by GPS measurement, the obtained vehicle posture usually has noise, and the vehicle trajectory is not smooth enough, resulting in poor local consistency of the obtained map. The construction method of the above-mentioned method 2 is usually used for mapping in small-scale scenarios (such as parks and underground parking lots). The local consistency of the map obtained by the above-mentioned method 2 is good, but due to the inevitable cumulative error of laser point cloud registration, and the lack of absolute constraints provided by GPS, the global consistency of the map obtained by the above-mentioned method 2 is poor.
[0069] It can be seen that, combining the above methods 1 and 2, the current map construction faces multiple problems:
[0070] (1) Poor local consistency of the map: The high-precision integrated navigation system provides a relatively accurate global position and posture, but there are inevitable deviations between the data collected multiple times. In some environments (areas with weak GPS signals), the integrated navigation system may even fail to locate the location, so the local consistency of the map will be reduced.
[0071] (2) Poor global consistency of the map: Method 2 uses a laser SLAM algorithm based on laser point cloud registration, which will inevitably result in cumulative errors. Traditional algorithms use loop detection (also known as closed-loop detection, which refers to the robot's ability to recognize that it has reached a certain scene, making the map closed) to reduce the impact of cumulative errors. However, in scenarios with a large span, such as high-speed scenarios, the cumulative error will increase rapidly, and there are very few available closed loops in high-speed scenarios, resulting in a sharp decline in the global consistency of the map.
[0072] (3) Regardless of method 1 or method 2, the mapping method relies on high-precision sensors (lidar) and requires accurate calibration of external parameters between different sensors. (Traditional mapping methods rely more on the calibration of external parameters of sensors, i.e., the positional relationship between GPS and lidar, and the positional relationship between two lidars).
[0073] The present application embodiment aims to propose a method for constructing a high-precision map that is different from the construction methods of the above-mentioned methods 1 and 2, so as to overcome the problems existing in the map construction methods in the prior art. Figure 1 The consistency is poor and it is greatly affected by the calibration accuracy of sensors such as lidar.
[0074] like Figure 1 As shown, the embodiment of the present application provides a map construction method, including:
[0075] Step 101: Obtain sensor data collected by a sensor carried by a movable object in real time, and determine each frame of data in a first submap with a preset frame length.
[0076] Step 102: perform registration of each frame with the first submap based on each frame data and a plurality of frame data in the first submap, and determine a first relative position relationship between each frame and the first submap.
[0077] The plurality of frames of data are data from the first frame to the previous frame of each frame in the first sub-map.
[0078] Step 103: determine the second submaps related to each first submap according to the initial poses corresponding to each first submap, align each first submap with the second submap related thereto, establish a first pose constraint relationship between each first submap and the second submap, and establish a second pose constraint relationship between each first submap and the initial poses corresponding to each first submap.
[0079] Step 104: Perform global optimization based on the first pose constraint relationship and the second pose constraint relationship to determine the sub-map optimized pose of each first sub-map.
[0080] Step 105: Determine the frame optimized pose corresponding to each frame data in each first submap according to the first relative pose relationship and the submap optimized pose.
[0081] Step 106: construct a map based on the frame-optimized posture and frame data to form a global map.
[0082] In order to enable those skilled in the art to better understand the present application, the embodiments of the present application are described in more detail below with reference to the accompanying drawings, examples, etc. It is worth noting that the movable objects in the embodiments of the present application may refer to objects that can be used for map collection, such as vehicles, mobile robots, and aircraft. Various types of sensors may be carried on the movable objects. For example, in one embodiment of the present application, the sensor may be a laser radar, and the corresponding sensor data is laser point cloud data, and each frame of data is each frame of laser point cloud data collected by the laser radar. For example, Figure 2 As shown, a laser radar 21 for sensing the surrounding environment can be set on both sides or on the top of a vehicle 20 (which can be an autonomous driving vehicle or a manned vehicle such as a map collection vehicle). The specific installation of the laser radar 21 on the vehicle 20 will not be repeated here.
[0083] like Figure 3 As shown, the embodiment of the present application provides a map construction method, including:
[0084] Step 301: Control the sensor carried by the movable object to collect data.
[0085] For example, the laser radar carried on the vehicle is controlled to collect each frame of laser point cloud data; another example is to control the integrated navigation system carried on the vehicle to collect the position and posture of the integrated navigation system, which may include position (such as GPS) and attitude (such as pitch angle (Pitch), roll angle (Roll), yaw angle (Yaw)).
[0086] Step 302: Obtain sensor data collected by a sensor carried by the movable object in real time, and determine each frame of data in the first sub-map with a preset frame length.
[0087] For example, when the laser radar carried on the vehicle collects each frame of laser point cloud data, one frame of laser point cloud data refers to the laser radar emitting lasers to the surroundings and collecting laser point cloud data for one week (360°). Here, the submap is a concept relative to the global map. The global map is composed of several submaps, and each submap contains multiple consecutive frames, that is, the frame length can be preset for the submap. For example, the frame length of the first submap is set to A frames, then each first submap forms all the frame data of the first submap after the sensor such as the laser radar collects A frame of laser point cloud data. For example, A can be set to 20, 30, 40, etc. according to demand, but is not limited to this. In addition, for example, the combined navigation system carried on the vehicle can collect the position and posture of the combined navigation system. The one frame of sensor data corresponding to the combined navigation system can refer to the position and posture of the combined navigation system corresponding to the collection of one frame of laser point cloud data.
[0088] Afterwards, the laser point cloud data of each frame collected by the laser radar needs to be preprocessed in the following steps 303 to 305. This is because in some scenes where vehicles are driving fast (such as high-speed scenes), there are problems such as high speed and complex surrounding vehicles, so the original laser point cloud data obtained inevitably has ranging errors and dynamic vehicle point cloud mixing problems. Therefore, it is necessary to perform preprocessing through steps 303 to 305 to facilitate subsequent data processing.
[0089] Step 303: Perform motion compensation on each frame of laser point cloud data to determine the position of the point in each frame of laser point cloud data after motion compensation.
[0090] Here, step 303 can be implemented in the following manner:
[0091] Obtain the laser radar pose at the start and end time of collecting a frame of laser point cloud data (here, the pose between the laser radar and the integrated navigation system can be calibrated in advance, so that the pose of the laser radar at each moment can be obtained when the pose of the integrated navigation system is obtained in real time). Perform timestamp interpolation between the start and end time of collecting a frame of laser point cloud data to obtain the laser radar pose corresponding to the time when each point in a frame of laser point cloud data is collected. According to the laser radar pose corresponding to the time when each point is collected, and the coordinates of each point in the laser radar coordinate system (when the laser radar scans the external environment, the coordinates of each point in the laser radar coordinate system can be directly obtained through the obtained laser point cloud data), determine the position of each point after motion compensation.
[0092] Step 304: Perform dynamic target detection through each frame of laser point cloud data, determine points corresponding to interfering objects from each frame of laser point cloud data, and remove the points corresponding to the interfering objects.
[0093] In general, the interference object can be a pre-set moving object, such as a pre-marked vehicle, pedestrian, animal, etc., but not limited to this. After marking various moving objects on the sample, the neural network is trained through the sample, so that the trained neural network can be used to perform dynamic target detection on each frame of laser point cloud data, thereby determining the points corresponding to the interference object from each frame of laser point cloud data, and then removing the points corresponding to the interference object. The specific process can be shown in the literature: Zhang, Ji, and Sanjiv Singh. "Low-drift and real-time lidar odometry and mapping." Autonomous Robots 41.2 (2017): 401-416., which will not be repeated here.
[0094] Step 305: extract features from each frame of laser point cloud data after motion compensation, dynamic target detection and elimination, and determine feature points that meet a preset feature extraction strategy.
[0095] Here, step 305 can be implemented in the following manner, for example:
[0096] In a frame of laser point cloud data after motion compensation, dynamic target detection and elimination, any point on a line beam and several points within a preset range of any point on the line beam are obtained. Here, since the laser points measured by the laser radar are arranged according to the line beam, several points within its preset range can be found for each laser point according to the line beam, such as several laser points on the left and right sides of the line beam (such as 5 laser points on each side, but not limited to this).
[0097] The curvature at any point is determined based on the coordinates of the arbitrary point in the laser radar coordinate system and the coordinates of several points within a preset range of any point on the line beam in the laser radar coordinate system. For example, the curvature at any point can be determined using the following curvature calculation formula: Where c represents The curvature at a point; They respectively represent the coordinates of the i-th and j-th points on the k-th line in the current frame in the lidar coordinate system, S represents the point set consisting of several points on the left and right sides of the i-th point, and |S| represents the number of points contained in the point set.
[0098] According to the preset curvature threshold, when the curvature of a point is greater than the curvature threshold, the point is taken as a line feature point, and when the curvature of a point is less than the curvature threshold, the point is taken as a plane feature point.
[0099] According to the preset threshold of the number of feature points, the line feature points and plane feature points corresponding to the curvature on both sides of the curvature threshold are eliminated from the curvature threshold until the sum of the number of remaining line feature points and plane feature points is less than or equal to the threshold of the number of feature points, and the remaining line feature points and plane feature points are obtained. That is, the closer the curvature is to the curvature threshold, the less prominent the line feature and plane feature are, and the more likely it is to make a misjudgment. Therefore, it is necessary to eliminate the points whose curvature is close to the curvature threshold, and only retain the points whose curvature is far from the curvature threshold, so as to limit the number of feature points. For example, the line feature points and the surface feature points can be sorted according to the curvature, and when the number of the extracted line feature points and the surface feature points exceeds the threshold of the number of feature points, the line feature points with smaller curvature and the surface feature points with larger curvature are discarded according to the curvature. For example, there are 10,000 points on both sides of the curvature threshold, totaling 20,000 points, and the threshold of the number of feature points is 5,000, then 7,500 points on both sides of the curvature threshold can be eliminated.
[0100] Step 306: Obtain the laser radar pose corresponding to a frame of laser point cloud data collected as the initial value of the registration pose.
[0101] Among them, one frame of laser point cloud data includes the remaining line feature points and plane feature points in the above step 305.
[0102] The step 306 may be implemented in the following manner, for example:
[0103] The combined navigation system position and posture corresponding to a frame of laser point cloud data is obtained through the combined navigation system.
[0104] According to the combined navigation system pose and the pre-obtained external parameter calibration results of the laser radar and the combined navigation system, the laser radar pose corresponding to the laser point cloud data frame is determined. Here, the external parameter calibration results can reflect the corresponding relationship between the combined navigation system pose and the laser radar pose, so that when the combined navigation system pose is obtained, the corresponding laser radar pose can be solved.
[0105] Step 307: Search and obtain a number of neighboring points corresponding to each point based on the data index pre-established for each point using the KD-Tree algorithm.
[0106] The several neighboring points are points in several frames of data in the first submap. For example, the several neighboring points are points from the first frame to the frame before the current frame in the first submap. For example, if 30 frames of data are to be recorded in a first submap and the current frame is the 9th frame, the several neighboring points are points from the 1st frame to the 8th frame in the first submap.
[0107] After step 307 , step 308 and step 309 are executed.
[0108] Step 308: For a line feature point in a frame of laser point cloud data, a line feature point error function is established according to the initial value of the registration pose corresponding to the line feature point and a number of corresponding neighboring points.
[0109] The step 308 may be implemented in the following manner, for example:
[0110] According to the line feature points in a frame of laser point cloud data The corresponding several neighboring points (for example, searching for 5 neighboring points, but not limited to this) are fitted to obtain a straight line (for example, a least square method is used to fit several neighboring points to obtain a straight line), and the line feature points The distance function from the straight line is used as the line feature point error function.
[0111] Among them, the line feature point error function is: in, and are any two points on the straight line, R and t represent the initial values of the registration pose, where R represents the posture in the initial value of the registration pose, and t represents the position in the initial value of the registration pose.
[0112] Step 309: for a surface feature point in a frame of laser point cloud data, a surface feature point error function is established according to the initial value of the registration pose corresponding to the surface feature point and a number of corresponding neighboring points.
[0113] Here, step 309 can be implemented in the following manner, for example:
[0114] According to the surface feature points in a frame of laser point cloud data The corresponding neighboring points (for example, 5 neighboring points are obtained by searching, but not limited to this) are fitted to obtain a plane Ax+By+Cz+D=0 (for example, the plane is obtained according to the neighboring points by using SVD decomposition method), and the surface feature points are The distance function from the plane is used as the surface feature point error function; where k represents the kth line in a frame of laser point cloud data, i represents the ith point, and A, B, C and D represent the parameters of the fitted plane; is the coordinate of the surface feature point in the laser radar coordinate system.
[0115] The error function of the surface feature point is: Among them, R and t represent the initial values of the registration pose, wherein R represents the posture in the initial values of the registration pose, and t represents the position in the initial values of the registration pose; n represents the matrix: n = (A, B, C).
[0116] Step 310 is performed after steps 308 and 309 .
[0117] Step 310: Determine the error function of frame-to-submap registration based on the line feature point error function and the surface feature point error function.
[0118] Here, step 310 may be implemented in the following manner, for example:
[0119] According to the line feature point error function And surface feature point error function Determine the error function for frame-to-submap registration Among them, n line Indicates the number of line feature points in this frame of laser point cloud data, n plane Indicates the number of surface feature points in this frame of laser point cloud data; Represents the line feature point error of the i-th line feature point in a frame of laser point cloud data, Represents the surface feature point error of the i-th surface feature point in a frame of laser point cloud data.
[0120] Step 311, solve the error function of frame-to-submap registration to obtain a first laser radar optimized pose corresponding to a frame of laser point cloud data, and determine a first relative pose relationship between the first laser radar optimized pose and the pose corresponding to the first submap.
[0121] Among them, the corresponding posture of the first submap is the laser radar posture corresponding to the first frame of laser point cloud data of the first submap.
[0122] Here, in step 311, the error function of the frame-to-submap registration is solved to obtain the first laser radar optimized pose corresponding to a frame of laser point cloud data, which can be obtained in the following manner, for example:
[0123] Error function for frame-to-submap registration Perform an iterative solution (for example, using the Gauss-Newton method or the LM algorithm (Levenberg-Marquard) for iterative solution, but not limited to this), and obtain E frame The posture R and position t corresponding to the minimum value are used as the first laser radar optimized posture corresponding to a frame of laser point cloud data.
[0124] In this way, the embodiment of the present application adopts frame-to-submap registration, which can avoid the problem that the frame-to-frame registration method will cause the cumulative error to increase rapidly due to the limited information contained in the frame. In addition, the frame-to-submap registration in the embodiment of the present application adopts a laser point cloud-based registration algorithm, which does not require high accuracy of the initial value of the registration pose, and therefore does not require high accuracy of the external parameter calibration of the laser radar and the combined navigation system. When the calibration accuracy is low, accurate registration results can still be obtained, that is, the first laser radar optimized pose and the first relative pose relationship corresponding to a frame of laser point cloud data.
[0125] Step 312: determine the second submaps related to each first submap according to the initial poses corresponding to each first submap, align each first submap with the second submap related thereto, establish a first pose constraint relationship between each first submap and the second submap, and establish a second pose constraint relationship between each first submap and the initial poses corresponding to each first submap.
[0126] The step 312 can be performed in the following two ways:
[0127] Method 1:
[0128] The second submaps related to each first submap are determined according to the initial positions corresponding to each first submap, including performing the following steps on each first submap:
[0129] According to the initial pose corresponding to the first submap, the previous submap adjacent to the first submap is determined as the second submap; the initial pose corresponding to the first submap is the laser radar pose corresponding to the first frame of laser point cloud data of the first submap. Here, in method 1, the previous submap adjacent to the first submap is considered, and the situation of closed loop detection is not considered.
[0130] In addition, each first submap is registered with its related second submap, and a first pose constraint relationship between each first submap and the second submap is established, including performing the following steps on each first submap:
[0131] According to the initial posture corresponding to the first submap and the initial posture corresponding to the second submap, the NDT algorithm is used to obtain the posture transformation relationship between the first submap and the second submap The initial position corresponding to the second submap is the laser radar position corresponding to the first frame of laser point cloud data of the second submap. Here, the position transformation relationship between the first submap and the second submap is obtained by the NDT algorithm: The method is as follows: first, select a point cloud of a submap for three-dimensional grid division, then calculate a normal distribution for each grid, then downsample the laser points in the other submap, and project all the downsampled points into the three-dimensional grid according to the initial pose transformation provided by the integrated navigation system, and calculate the probability density function of each point at the same time, and calculate the maximum likelihood of the product of all probability density functions, so as to obtain the pose transformation relationship between the two submaps.
[0132] Establish the first pose error function of the first submap and the second submap: in, Represents the posture transformation relationship between the i-th submap and the i+1-th submap. represents the first position error between the ith submap and the i+1th submap, X i represents the initial pose corresponding to the i-th submap, X i+1 represents the initial pose corresponding to the i+1th submap, X i and X i+1 It can be obtained by determining the optimized posture of the first laser radar in the above step 311.
[0133] In addition, establishing a second posture constraint relationship between each first submap and the initial posture corresponding to each first submap includes performing the following steps on each first submap:
[0134] Establish the second pose error function of the first submap and the initial pose corresponding to the first submap: Among them, X i represents the initial pose corresponding to the i-th submap, It indicates that the lidar pose corresponding to the i-th sub-map is determined according to the pose of the integrated navigation system and the pre-obtained extrinsic calibration result of the lidar and the integrated navigation system.
[0135] Method 2:
[0136] The second submaps related to each first submap are determined according to the initial positions corresponding to each first submap, including performing the following steps on each first submap:
[0137] According to the initial posture corresponding to the first submap, the previous submap adjacent to the first submap and the submap with a closed loop with the first submap are determined as the second submap; the distance between the initial posture corresponding to the submap with a closed loop with the first submap and the initial posture corresponding to the first submap is less than or equal to a preset distance threshold; the initial posture corresponding to the first submap is the laser radar posture corresponding to the first frame of laser point cloud data of the first submap; the initial posture corresponding to the submap with a closed loop with the first submap is the laser radar posture corresponding to the first frame of laser point cloud data of the submap with a closed loop with the first submap. Here, in the second method, not only the previous submap adjacent to the first submap is considered, but also the situation of closed loop detection needs to be considered, that is, the submap with a closed loop with the first submap is also regarded as the second submap.
[0138] In addition, each first submap is registered with its related second submap, and a first pose constraint relationship between each first submap and the second submap is established, including performing the following steps on each first submap:
[0139] According to the initial posture corresponding to the first submap and the initial posture corresponding to the second submap, the NDT algorithm is used to obtain the posture transformation relationship between the first submap and the second submap and Among them, the initial pose corresponding to the second submap is the laser radar pose corresponding to the first frame of laser point cloud data of the second submap; is the posture transformation relationship between the adjacent i-th submap and i+1-th submap; It is the posture transformation relationship between the i-th submap and the j-th submap that has a closed loop with the i-th submap.
[0140] Establish the first pose error function of the first submap and the second submap: as well as in, represents the first position error between the ith submap and the i+1th submap, X i represents the initial pose corresponding to the i-th submap, X i+1 Indicates the initial pose corresponding to the i+1th submap; represents the first position error between the i-th submap and the j-th submap, X j represents the initial pose corresponding to the jth submap. Here, X i , X i+1 and X j It can be obtained by determining the optimized posture of the first laser radar in the above step 311.
[0141] In addition, establishing a second posture constraint relationship between each first submap and the initial posture corresponding to each first submap includes performing the following steps on each first submap:
[0142] Establish the second pose error function of the first submap and the initial pose corresponding to the first submap: Among them, X i represents the initial pose corresponding to the i-th submap, It indicates that the lidar pose corresponding to the i-th sub-map is determined according to the pose of the integrated navigation system and the pre-obtained extrinsic calibration result of the lidar and the integrated navigation system.
[0143] Step 313: Perform global optimization based on the first pose constraint relationship and the second pose constraint relationship to determine the submap optimized pose of each first submap.
[0144] Among them, corresponding to the method one and method two in the above-mentioned step 312, the step 313 can also be implemented by the following method one and method two, wherein method one is the subsequent processing process corresponding to method one in step 312, and method two is the subsequent processing process corresponding to method two in step 312.
[0145] Method 1:
[0146] Among them, global optimization is performed according to the first pose constraint relationship and the second pose constraint relationship to determine the submap optimized pose of each first submap, which can be done in the following way:
[0147] According to the first pose error function: and the second pose error function Determine the first global error function:
[0148] Where i represents the i-th submap; n represents the number of submaps; Ω i(i+1) and Ω ii is a preset information matrix, which represents the error confidence.
[0149] The first global error function is iteratively solved (for example, using the Gauss-Newton method for iterative solution) to determine the sub-map optimized position of each first sub-map when E1 is minimized.
[0150] Method 2:
[0151] Among them, global optimization is performed according to the first pose constraint relationship and the second pose constraint relationship to determine the submap optimized pose of each first submap, which can be done in the following way:
[0152] According to the first pose error function: as well as and the second pose error function Determine the second global error function:
[0153]
[0154] Where i represents the i-th submap, j represents the j-th submap; n represents the number of submaps; Ω i(i+1) ,Ω ij and Ω ii is a preset information matrix, which represents the error confidence.
[0155] The second global error function is iteratively solved (for example, iteratively solved using the Gauss-Newton method) to determine the sub-map optimized position of each first sub-map when E2 is minimized.
[0156] The global optimization method here is generally graph optimization. Graph optimization refers to expressing the optimization problem in the form of a graph. The graph consists of nodes and edges. The optimization process is to adjust the position of the node to maximize the satisfaction of the edge constraints. Figure 4 As shown, the second global error function can be represented by a global posture relationship diagram, where the dotted line represents the route of the vehicle, and S represents the submap (for example, when S i When it is the first sub-map, S i-1 and S k is the first submap S i The first submap is the first submap, G represents the second pose constraint relationship between the first submap and the initial pose corresponding to the first submap, and the solid line represents the constraint relationship between the nodes, that is, the first pose error function mentioned above: as well as and the second pose error function
[0157] Step 314: Determine the frame optimized pose of the laser radar corresponding to the collected frame of laser point cloud data corresponding to the submap optimized pose according to the first relative pose relationship between the first laser radar optimized pose and the pose corresponding to the first submap.
[0158] The relative relationship between the first laser radar optimized posture and the corresponding posture of the first sub-map can be obtained from the first relative posture relationship. Therefore, when determining the sub-map optimized posture of each first sub-map, the frame optimized posture of the laser radar corresponding to a frame of laser point cloud data collected corresponding to the sub-map optimized posture can be obtained through the first relative posture relationship.
[0159] Step 315: Map the points in each frame of the laser point cloud to the world coordinate system according to the frame optimized posture and the coordinates of the points in each frame of the laser point cloud in the laser radar coordinate system.
[0160] Step 316: Overlay the points in the laser point cloud mapped to the world coordinate system to form a global map.
[0161] In order to verify the effectiveness and generalization ability of the method proposed in the above steps 301 to 316 in the embodiment of the present application, a mapping experiment was carried out using a map vehicle with high positioning accuracy and a truck with relatively low positioning accuracy in one embodiment of the present application, and the mapping results were analyzed from the global consistency and local consistency of the map. In addition, in order to enhance the persuasiveness of the experimental results, the embodiment of the present application is equipped with a high-precision combined navigation system (such as NovAtel) on the vehicle, and the result output by the combined navigation system (the reference global map finally obtained by positioning the combined navigation system) is used as the true value, and the reference global map is compared with the global map formed in the embodiment of the present application (referred to as the global map to be verified).
[0162] like Figure 5 and Figure 6 As shown, for the global map to be verified obtained by the map car, its road structure is clear, the lane lines and guardrail contours are regular, and the ground thickness is small, so the local consistency of the global map to be verified is good.
[0163] like Figure 7 and Figure 8 As shown, for the global map to be verified obtained by the truck, the road structure is clear, the lane lines and guardrail contours are regular, and the ground thickness is small, so the local consistency of the global map to be verified is good.
[0164] In addition, if Fig. 9 As shown, the embodiment of the present application also provides a map construction device, including:
[0165] The data acquisition unit 41 is used to obtain sensor data collected by a sensor carried by the movable object in real time, and determine each frame of data in the first sub-map with a preset frame length.
[0166] The frame and submap registration unit 42 is used to register each frame with the first submap according to each frame data and a plurality of frame data in the first submap, and determine a first relative position relationship between each frame and the first submap; the plurality of frame data are data from the first frame to the previous frame of each frame in the first submap.
[0167] The submap-to-submap registration unit 43 is used to determine the second submap related to each first submap according to the initial posture corresponding to each first submap, register each first submap with the second submap related thereto, establish a first posture constraint relationship between each first submap and the second submap, and establish a second posture constraint relationship between each first submap and the initial posture corresponding to each first submap.
[0168] The global optimization unit 44 is used to perform global optimization according to the first pose constraint relationship and the second pose constraint relationship to determine the sub-map optimized pose of each first sub-map.
[0169] The frame optimized posture determination unit 45 is used to determine the frame optimized posture corresponding to each frame data in each first submap according to the first relative posture relationship and the submap optimized posture.
[0170] The global map construction unit 46 is used to construct a map according to the frame optimization posture and each frame data to form a global map.
[0171] In addition, the embodiment of the present application also provides a computer-readable storage medium, including a program or an instruction, which, when executed on a computer, implements the above Figure 1 or Figure 3 The map construction method described.
[0172] In addition, the present application also provides a computer program product including instructions, which, when the computer program product is run on a computer, enables the computer to execute the above-mentioned Figure 1 or Figure 3 The map construction method described.
[0173] In addition, the embodiment of the present application also provides a chip system, including a processor, the processor is coupled to a memory, the memory stores program instructions, and when the program instructions stored in the memory are executed by the processor, the above-mentioned Figure 1 or Figure 3 The map construction method described.
[0174] In addition, the present application also provides a circuit system, the circuit system includes a processing circuit, the processing circuit is configured to perform the above Figure 1 or Figure 3 Map construction method.
[0175] In addition, an embodiment of the present application further provides a computer server, comprising a memory, and one or more processors communicatively connected to the memory;
[0176] The memory stores instructions that can be executed by one or more processors, and the instructions are executed by one or more processors to enable one or more processors to implement the above-mentioned Figure 1 or Figure 3 The map construction method described.
[0177] A map construction method and device provided in an embodiment of the present application can obtain sensor data collected by sensors carried on movable objects in real time, and align each frame with the first submap to determine the first relative posture relationship between each frame and the first submap. After determining the second submap related to the first submap, the first submap can be aligned with the second submap, and a posture constraint relationship can be established in combination with the initial posture of the first submap. Therefore, after globally optimizing the posture constraint relationship, the submap optimized posture of each first submap can be determined. Furthermore, based on the above-mentioned first relative posture relationship and the submap optimized posture, the frame optimized posture corresponding to each frame data in each first submap can be determined. Furthermore, based on the frame optimized posture and each frame data, a map can be constructed to form a global map. In this way, the embodiments of the present application can overcome the local shortcomings of the map construction method in the prior art. Figure 1 The consistency is poor and it is greatly affected by the calibration accuracy of sensors such as lidar.
[0178] Those skilled in the art will appreciate that the embodiments of the present application may be provided as methods, systems, or computer program products. Therefore, the present application may adopt the form of a complete hardware embodiment, a complete software embodiment, or an embodiment in combination with software and hardware. Moreover, the present application may adopt the form of a computer program product implemented in one or more computer-usable storage media (including but not limited to disk storage, CD-ROM, optical storage, etc.) that include computer-usable program code.
[0179] The present application is described with reference to the flowcharts and / or block diagrams of the methods, devices (systems), and computer program products according to the embodiments of the present application. It should be understood that each process and / or box in the flowchart and / or block diagram, as well as the combination of the processes and / or boxes in the flowchart and / or block diagram, can be implemented by computer program instructions. These computer program instructions can be provided to a processor of a general-purpose computer, a special-purpose computer, an embedded processor, or other programmable data processing device to generate a machine, so that the instructions executed by the processor of the computer or other programmable data processing device generate instructions for implementing the processes in the flowchart and / or block diagram. Figure 1 A process or multiple processes and / or boxes Figure 1 A device that provides the functions specified in a block or multiple blocks.
[0180] These computer program instructions may also be stored in a computer readable memory capable of directing a computer or other programmable data processing device to operate in a specific manner, so that the instructions stored in the computer readable memory produce an article of manufacture including an instruction device, which implements the process Figure 1 A process or multiple processes and / or boxes Figure 1 A function specified in one or more boxes.
[0181] These computer program instructions can also be loaded onto a computer or other programmable data processing device so that a series of operating steps are executed on the computer or other programmable device to produce a computer-implemented process, thereby providing instructions for implementing the process. Figure 1 A process or multiple processes and / or boxes Figure 1 The steps for the functions specified in one or more boxes.
[0182] Specific embodiments are used in this application to illustrate the principles and implementation methods of this application. The description of the above embodiments is only used to help understand the method and core idea of this application. At the same time, for those skilled in the art, according to the idea of this application, there will be changes in the specific implementation methods and application scope. In summary, the content of this specification should not be understood as a limitation on this application.
Claims
1. A map construction method, characterized in that: include: Obtaining sensor data collected by a sensor carried by the movable object in real time, and determining each frame of data in a first sub-map with a preset frame length; According to each frame data and a plurality of frame data in the first submap, register each frame with the first submap to determine a first relative position relationship between each frame and the first submap; the plurality of frame data are data from the first frame to a frame before each frame in the first submap; Determine a second submap related to each first submap according to the initial pose corresponding to each first submap, align each first submap with the second submap related thereto, establish a first pose constraint relationship between each first submap and the second submap, and establish a second pose constraint relationship between each first submap and the initial pose corresponding to each first submap according to the pose of the integrated navigation system carried by the movable object and the sensor pose corresponding to each first submap determined based on the pre-obtained external parameter calibration result of the sensor and the integrated navigation system; Performing global optimization according to the first posture constraint relationship and the second posture constraint relationship to determine the sub-map optimized posture of each first sub-map; Determine, according to the first relative posture relationship and the submap optimized posture, a frame optimized posture corresponding to each frame data in each first submap; Map construction is performed based on the frame optimization posture and each frame data to form a global map.
2. The map construction method according to claim 1, characterized in that: The sensor is a laser radar, the sensor data is laser point cloud data, and each frame of data is each frame of laser point cloud data collected by the laser radar.
3. The map construction method according to claim 2, characterized in that: After obtaining sensor data collected by a sensor carried by a movable object in real time and determining each frame of data in a first sub-map with a preset frame length, the method includes: Perform motion compensation on each frame of laser point cloud data to determine the position of the points in each frame of laser point cloud data after motion compensation; Perform dynamic target detection through each frame of laser point cloud data, determine the points corresponding to the interfering objects from each frame of laser point cloud data, and remove the points corresponding to the interfering objects; Feature extraction is performed on each frame of laser point cloud data after motion compensation, dynamic target detection and elimination to determine feature points that meet the pre-set feature extraction strategy.
4. The map construction method according to claim 3, characterized in that: The step of performing motion compensation on each frame of laser point cloud data and determining the position of a point in each frame of laser point cloud data after motion compensation includes performing the following steps on each frame of laser point cloud data: Obtain the laser radar pose at the start and end time of collecting a frame of laser point cloud data; Perform timestamp interpolation between the start time and the end time of collecting a frame of laser point cloud data to obtain the laser radar pose corresponding to the time of collecting each point in a frame of laser point cloud data; The motion-compensated position of each point is determined based on the laser radar pose corresponding to the moment of acquisition of each point and the coordinates of each point in the laser radar coordinate system.
5. The map construction method according to claim 3, characterized in that: The interfering object is a pre-set moving object.
6. The map construction method according to claim 3, characterized in that: The feature extraction of each frame of laser point cloud data after motion compensation and dynamic target detection and elimination is performed to determine feature points that meet the pre-set feature extraction strategy, including: Obtain any point on a line beam and a number of points within a preset range of any point on the line beam in a frame of laser point cloud data after motion compensation, dynamic target detection and elimination; Determine the curvature at any point according to the coordinates of the any point in the laser radar coordinate system and the coordinates of a number of points within a preset range of any point on the line beam in the laser radar coordinate system; According to a preset curvature threshold, when the curvature of a point is greater than the curvature threshold, the point is taken as a line feature point, and when the curvature of a point is less than the curvature threshold, the point is taken as a plane feature point; According to a preset feature point number threshold, line feature points and plane feature points corresponding to the curvature on both sides of the curvature threshold are eliminated starting from the curvature threshold until the sum of the number of remaining line feature points and plane feature points is less than or equal to the feature point number threshold, and the remaining line feature points and plane feature points are obtained.
7. The map construction method according to claim 6, characterized in that: According to each frame of data and a plurality of frames of data in the first submap, registering each frame with the first submap to determine a first relative position relationship between each frame and the first submap includes performing the following steps on each frame of laser point cloud data: Obtaining a laser radar pose corresponding to a frame of laser point cloud data collected as an initial value of the registration pose; wherein a frame of laser point cloud data includes the remaining line feature points and plane feature points; According to the data index established for each point in advance using the KD-Tree algorithm, a number of neighboring points corresponding to each point are searched; wherein the number of neighboring points are points in a number of frames of data in the first sub-map; For a line feature point in a frame of laser point cloud data, a line feature point error function is established according to the initial value of the registration pose corresponding to the line feature point and a number of corresponding neighboring points; For a surface feature point in a frame of laser point cloud data, a surface feature point error function is established according to the initial value of the registration pose corresponding to the surface feature point and a number of corresponding neighboring points; Determining an error function for frame-to-submap registration according to the line feature point error function and the surface feature point error function; The error function of the frame-to-submap alignment is solved to obtain a first laser radar optimized pose corresponding to a frame of laser point cloud data, and a first relative pose relationship between the first laser radar optimized pose and the pose corresponding to the first submap is determined; the pose corresponding to the first submap is the laser radar pose corresponding to the first frame of laser point cloud data of the first submap.
8. The map construction method according to claim 7, characterized in that: The step of obtaining the laser radar pose corresponding to a frame of laser point cloud data as the initial value of the registration pose includes: The integrated navigation system position and posture corresponding to collecting a frame of laser point cloud data is obtained through the integrated navigation system; The laser radar pose corresponding to the frame of laser point cloud data is determined according to the pose of the integrated navigation system and the pre-obtained external parameter calibration result of the laser radar and the integrated navigation system.
9. The map construction method according to claim 7, characterized in that: The method of establishing a line feature point error function for a line feature point in a frame of laser point cloud data according to an initial registration pose value corresponding to the line feature point and a number of corresponding neighboring points includes: According to the line feature points in a frame of laser point cloud data The corresponding neighboring points are fitted to obtain a straight line, and the line feature points The distance function from the straight line is used as the line feature point error function; The line feature point error function is: in, and are any two points on the straight line, R and t represent the initial values of the registration pose, where R represents the posture in the initial value of the registration pose, and t represents the position in the initial value of the registration pose.
10. The map construction method according to claim 9, characterized in that: The surface feature point error function is established for a surface feature point in a frame of laser point cloud data according to the initial value of the registration pose corresponding to the surface feature point and a number of corresponding neighboring points, including: According to the surface feature points in a frame of laser point cloud data The corresponding neighboring points are fitted to obtain a plane Ax+By+Cz+D=0, and the surface feature points The distance function from the plane is used as the surface feature point error function; where k represents the kth line in a frame of laser point cloud data, i represents the ith point, and A, B, C and D represent the parameters of the fitted plane; is the coordinate of the surface feature point in the laser radar coordinate system; The surface feature point error function is: Among them, R and t represent the initial values of the registration pose, wherein R represents the posture in the initial values of the registration pose, and t represents the position in the initial values of the registration pose; n represents the matrix: n = (A, B, C).
11. The map construction method according to claim 10, characterized in that: Determining the error function of frame-to-submap registration according to the line feature point error function and the surface feature point error function includes: According to the line feature point error function And surface feature point error function Determine the error function for frame-to-submap registration Among them, n line Indicates the number of line feature points in this frame of laser point cloud data, n plane Indicates the number of surface feature points in this frame of laser point cloud data; Represents the line feature point error of the i-th line feature point in a frame of laser point cloud data, Represents the surface feature point error of the i-th surface feature point in a frame of laser point cloud data.
12. The map construction method according to claim 11, characterized in that: Solving the error function of the frame-to-submap registration to obtain a first laser radar optimized pose corresponding to a frame of laser point cloud data includes: Error function for the frame-to-submap registration Perform iterative solution and obtain E frame The posture R and position t corresponding to the minimum value are used as the first laser radar optimized posture corresponding to a frame of laser point cloud data.
13. The map construction method according to claim 2, characterized in that: The determining of the second submaps related to each first submap according to the initial positions corresponding to each first submap includes performing the following steps on each first submap: According to the initial posture corresponding to the first submap, the previous submap adjacent to the first submap is determined as the second submap; the initial posture corresponding to the first submap is the laser radar posture corresponding to the first frame of laser point cloud data of the first submap.
14. The map construction method according to claim 13, characterized in that: The step of registering each first submap with the second submap related thereto and establishing the first pose constraint relationship between each first submap and the second submap includes performing the following steps on each first submap: According to the initial posture corresponding to the first submap and the initial posture corresponding to the second submap, the NDT algorithm is used to obtain the posture transformation relationship between the first submap and the second submap The initial position and posture corresponding to the second submap is the laser radar position and posture corresponding to the first frame of laser point cloud data of the second submap; Establish the first pose error function of the first submap and the second submap: in, Represents the posture transformation relationship between the i-th submap and the i+1-th submap. represents the first position error between the ith submap and the i+1th submap, X i represents the initial pose corresponding to the i-th submap, X i+1 Represents the initial pose corresponding to the i+1th submap.
15. The map construction method according to claim 14, characterized in that: The step of establishing the second posture constraint relationship between each first submap and the initial posture corresponding to each first submap includes performing the following steps on each first submap: Establish the second pose error function of the first submap and the initial pose corresponding to the first submap: Among them, X i represents the initial pose corresponding to the i-th submap, It indicates that the lidar pose corresponding to the i-th sub-map is determined according to the pose of the integrated navigation system and the pre-obtained extrinsic calibration result of the lidar and the integrated navigation system.
16. The map construction method according to claim 15, characterized in that: The performing global optimization according to the first posture constraint relationship and the second posture constraint relationship to determine the submap optimized posture of each first submap includes: According to the first pose error function: and the second pose error function Determine the first global error function: Where i represents the i-th submap; n represents the number of submaps; Ω i(i+1) and Ω ii is the pre-set information matrix, indicating the error confidence; The first global error function is iteratively solved to determine the submap optimized position of each first submap when E1 is minimum.
17. The map construction method according to claim 2, characterized in that: The determining of the second submaps related to each first submap according to the initial positions corresponding to each first submap includes performing the following steps on each first submap: According to the initial posture corresponding to the first submap, the previous submap adjacent to the first submap and the submap that has a closed loop with the first submap are determined as the second submap; the distance between the initial posture corresponding to the submap that has a closed loop with the first submap and the initial posture corresponding to the first submap is less than or equal to a preset distance threshold; the initial posture corresponding to the first submap is the laser radar posture corresponding to the first frame of laser point cloud data of the first submap; the initial posture corresponding to the submap that has a closed loop with the first submap is the laser radar posture corresponding to the first frame of laser point cloud data of the submap that has a closed loop with the first submap.
18. The map construction method according to claim 17, characterized in that: The step of registering each first submap with the second submap related thereto and establishing the first pose constraint relationship between each first submap and the second submap includes performing the following steps on each first submap: According to the initial posture corresponding to the first submap and the initial posture corresponding to the second submap, the NDT algorithm is used to obtain the posture transformation relationship between the first submap and the second submap and The initial position and posture corresponding to the second submap is the laser radar position and posture corresponding to the first frame of laser point cloud data of the second submap; is the posture transformation relationship between the adjacent i-th submap and i+1-th submap; is the position transformation relationship between the ith submap and the jth submap that has a closed loop with the ith submap; Establish the first pose error function of the first submap and the second submap: as well as in, represents the first position error between the ith submap and the i+1th submap, X i represents the initial pose corresponding to the i-th submap, X i+1 Indicates the initial pose corresponding to the i+1th submap; represents the first position error between the i-th submap and the j-th submap, X j Represents the initial pose corresponding to the j-th submap.
19. The map construction method according to claim 18, characterized in that: The step of establishing the second posture constraint relationship between each first submap and the initial posture corresponding to each first submap includes performing the following steps on each first submap: Establish the second pose error function of the first submap and the initial pose corresponding to the first submap: Among them, X i represents the initial pose corresponding to the i-th submap, It indicates that the lidar pose corresponding to the i-th sub-map is determined according to the pose of the integrated navigation system and the pre-obtained extrinsic calibration result of the lidar and the integrated navigation system.
20. The map construction method according to claim 19, characterized in that: The performing global optimization according to the first posture constraint relationship and the second posture constraint relationship to determine the submap optimized posture of each first submap includes: According to the first pose error function: as well as and the second pose error function Determine the second global error function: Where i represents the i-th submap, j represents the j-th submap; n represents the number of submaps; Ω i(i+1) ,Ω ij and Ω ii is the pre-set information matrix, indicating the error confidence; The second global error function is iteratively solved to determine the submap optimized position of each first submap when E2 is minimized.
21. The map construction method according to claim 7, characterized in that: The step of determining the frame optimized pose corresponding to each frame data in each first submap according to the first relative pose relationship and the submap optimized pose includes: According to the first relative posture relationship between the first laser radar optimized posture and the posture corresponding to the first sub-map, the frame optimized posture of the laser radar corresponding to the collection of a frame of laser point cloud data corresponding to the sub-map optimized posture is determined.
22. The map construction method according to claim 21, characterized in that: The frame data includes the coordinates of the points in each frame of the laser point cloud in the laser radar coordinate system; The step of constructing a map based on the frame-optimized posture and each frame data to form a global map includes: Mapping the points in each frame of the laser point cloud to the world coordinate system according to the frame optimized pose and the coordinates of the points in each frame of the laser point cloud in the laser radar coordinate system; The points in the laser point cloud mapped to the world coordinate system are superimposed to form a global map.
23. A map construction device, characterized in that: include: A data acquisition unit, used to obtain sensor data collected by a sensor carried by the movable object in real time, and determine each frame of data in the first sub-map with a preset frame length; a frame and submap registration unit, configured to register each frame with the first submap according to each frame data and a plurality of frame data in the first submap, and determine a first relative position relationship between each frame and the first submap; the plurality of frame data are data from the first frame to a frame before each frame in the first submap; A submap-to-submap registration unit, configured to determine a second submap associated with each first submap according to an initial posture corresponding to each first submap, register each first submap with the second submap associated therewith, establish a first posture constraint relationship between each first submap and the second submap, and establish a second posture constraint relationship between each first submap and the initial posture corresponding to each first submap according to a posture of an integrated navigation system carried on a movable object and a sensor posture corresponding to each first submap determined based on an external parameter calibration result of the sensor and the integrated navigation system obtained in advance; A global optimization unit, configured to perform global optimization according to the first posture constraint relationship and the second posture constraint relationship to determine a sub-map optimized posture of each first sub-map; a frame optimized posture determination unit, configured to determine a frame optimized posture corresponding to each frame data in each first submap according to the first relative posture relationship and the submap optimized posture; The global map construction unit is used to construct a map based on the frame optimization posture and each frame data to form a global map.
24. A computer-readable storage medium, characterized in that: The method comprises a program or an instruction, which, when executed on a computer, implements the map construction method according to any one of claims 1 to 22.
25. A computer program product comprising instructions, characterized in that When the computer program product is run on a computer, the computer is enabled to execute the map construction method according to any one of claims 1 to 22.
26. A chip system, characterized in that: The invention comprises a processor, the processor is coupled to a memory, the memory stores program instructions, and when the program instructions stored in the memory are executed by the processor, the map construction method according to any one of claims 1 to 22 is implemented.
27. A circuit system, characterized in that: The circuit system comprises a processing circuit configured to execute a map building method according to any one of claims 1 to 22.
28. A computer server, characterized in that: comprising a memory, and one or more processors communicatively connected to the memory; The memory stores instructions that can be executed by the one or more processors, and the instructions are executed by the one or more processors so that the one or more processors implement the map construction method according to any one of claims 1 to 22.
