A Multi-Sensor Loosely Coupled Indoor-Outdoor Synchronous Localization and Mapping Method and System

Abstract

The present invention discloses a multi-sensor loose-coupled indoor and outdoor simultaneous localization and mapping method and system, which preprocesses point cloud data and inertial data to obtain image view data of the point cloud and rough pose data of an autonomous vehicle; performs scene recognition, feature extraction and stitching processing on the image view data of the point cloud to obtain an unoptimized point cloud map of the surrounding environment; and based on the result of scene recognition, uses a backend optimization algorithm and a loop detection algorithm to optimize the rough pose data of the autonomous vehicle and the unoptimized point cloud map of the surrounding environment, so as to obtain optimized pose data of the autonomous vehicle and a point cloud map of the surrounding environment, thereby realizing the simultaneous localization and mapping of the autonomous vehicle.

Description

Technical Field

[0001] The present invention belongs to the field of driverless vehicles, and particularly relates to a multi-sensor loose-coupling indoor and outdoor synchronous positioning and mapping method and system. Background Art

[0002] Light Detection and Ranging (LiDAR) is an active sensor, which is a radar system that detects the position, speed and other characteristic quantities of a target by emitting a laser beam. Its working principle is to emit a detection signal (laser beam) to the target, and then compare the received signal (target echo) reflected from the target with the emitted signal. After appropriate processing, information such as the distance and height between the LiDAR and the target to be measured can be obtained. An Inertial Measurement Unit (IMU) is a device that measures the three-axis attitude angle (or angular rate) and acceleration of an object. Generally, an IMU includes three-axis gyroscopes and accelerometers in three directions to measure the angular velocity and acceleration of an object in three-dimensional space, and calculate the attitude of the object therefrom. Simultaneous Localization and Mapping (SLAM) technology can enable a carrier to estimate its own pose in real time according to data collected by sensors in an unknown environment and map the environment, providing positioning information for the carrier for autonomous planning and decision-making. The types of sensors used in SLAM technology are diverse, including LiDAR, cameras, IMUs, Global Navigation Satellite System (GNSS), etc. When multiple sensors are installed on a carrier at the same time, through information flow coupling, the advantages of various sensors can be fully utilized. For example, although LiDAR has accurate ranging, its frequency is relatively low, generally 5 - 20 Hz. The IMU acquisition frequency is as high as 100 - 300 Hz, which is suitable for installation on a dynamic platform. Coupling LiDAR and IMU can use the high-frequency IMU to correct LiDAR data and obtain higher accuracy. According to the tightness of coupling, the information flow can be loosely coupled or tightly coupled. In loose coupling, each sensor system is an independent module, collecting data separately, and then fusing the results output by each module. Compared with loose coupling, tight coupling fuses information earlier, uses the raw data of multiple sensors to jointly output a result, and can make more full use of sensor data. The schematic diagrams of loose coupling and tight coupling are as Figure 1 shown.

[0003] Patent CN201910297985.1 provides a method for simultaneous localization and mapping for visual-inertial-laser fusion, mainly related to technical fields such as multi-sensor fusion and SLAM. To solve the problems of low accuracy and easy loss in single-sensor SLAM for localization and mapping, this technology proposes a robust and high-precision SLAM system that fuses vision, inertial, and lidar.

[0004] The first prior art does not refine the method according to the characteristics of the scene. Specifically, its classification of environmental feature points as edge feature points and plane feature points may result in inaccurate classification in different environments.

[0005] The Gaussian-Newton optimization method used in the first prior art still has room for improvement in terms of computational efficiency. Summary of the Invention

[0006] To solve the technical problems existing in the background art, the present invention aims to provide a method and system for indoor and outdoor simultaneous localization and mapping with multi-sensor loose coupling.

[0007] To solve the technical problems, the technical solution of the present invention is as follows:

[0008] A method for indoor and outdoor simultaneous localization and mapping with multi-sensor loose coupling, the method comprising:

[0009] Performing data preprocessing on point cloud data and inertial data to obtain image view data of the point cloud and rough pose data of the autonomous driving vehicle;

[0010] Performing scene recognition, feature extraction, and stitching processing on the image view data of the point cloud to obtain an unoptimized point cloud map of the surrounding environment;

[0011] Based on the result of scene recognition, using a backend optimization algorithm and a loop closure detection algorithm to optimize the rough pose data of the autonomous driving vehicle and the unoptimized point cloud map of the surrounding environment, obtaining optimized pose data of the autonomous driving vehicle and a point cloud map of the surrounding environment, thereby realizing simultaneous localization and mapping of the autonomous driving vehicle.

[0012] Further, point cloud data is collected by a lidar, and inertial data is collected by an inertial measurement unit.

[0013] Further, the preprocessing of the point cloud data specifically includes:

[0014] Performing beam splitting on the point cloud data to obtain a vertical splitting result of the point cloud data;

[0015] Performing meta-region horizontal splitting on the point cloud data to obtain a horizontal splitting result of the point cloud data;

[0016] Reproject the vertical segmentation result and the horizontal segmentation result of the point cloud data based on the positional relationship between the coordinate system of the preset lidar and the vehicle centroid to obtain the image view data of the point cloud.

[0017] Furthermore, the preprocessing of the inertial data specifically includes:

[0018] Eliminate the gravity component of the inertial data to obtain the rough pose data of the autonomous driving vehicle;

[0019]

[0020] where a' t is the true value of the inertial measurement unit data, a t is the measured value of the inertial measurement unit data, R ZYX is the rotation matrix of the inertial measurement unit data, and g is the gravitational acceleration. By integrating a' t the speed and position of the autonomous driving vehicle are obtained.

[0021] Furthermore, the scene recognition, feature extraction, and stitching processing specifically include:

[0022] Use whether the average point cloud depth d a of the image view data of the point cloud exceeds a threshold as a judgment condition to distinguish whether the autonomous driving vehicle is in an indoor or outdoor environment, and obtain the scene recognition result;

[0023]

[0024] where N is the total number of points in a frame of point cloud, and d i is the Euclidean distance of the i-th point relative to the geometric center of the lidar;

[0025] Based on the scene recognition result, use the curvature c near each point of the point cloud in the image view data of the point cloud to divide the scene features into corner features and plane features, and use the iterative closest point algorithm to register the image view data of the point cloud (point clouds of multiple data frames) to obtain an unoptimized point cloud map of the surrounding environment;

[0026]

[0027] Define the curvature near the i-th point p i on the same horizontal line in the image view data of the point cloud, where a is the number of points adjacent to one side of the point p i and p j is the point adjacent to p i ;

[0028] Furthermore, based on the outdoor environment in the scene recognition result, use the backend optimization algorithm for processing, specifically including:

[0029] Based on the outdoor environment, using the Gauss-Newton algorithm in the backend optimization algorithm, optimize the rough pose data of the autonomous driving vehicle and the unoptimized point cloud map of the surrounding environment to obtain the pose data of the autonomous driving vehicle and the point cloud map of the surrounding environment after optimization in the outdoor environment.

[0030] Furthermore, based on the indoor environment in the result of scene recognition, process it using the two-step LM algorithm in the backend optimization algorithm.

[0031] Furthermore, process it using the two-step LM algorithm in the backend optimization algorithm, specifically including:

[0032] Optimize the rough pose data of the autonomous driving vehicle and the unoptimized point cloud map of the surrounding environment using two-step LM;

[0033] For the first-step LM optimization, first calculate the pitch angle θ pitch and the elevation displacement Z that are more affected by rotation in the six-degree-of-freedom pose using the extracted plane features, and use the calculation result of the first-step LM optimization as prior information, that is, the accurate rotation pose information of the autonomous driving vehicle;

[0034] For the second-step LM optimization, calculate the information of the remaining six-degree-of-freedom pose, the yaw angle θ yaw , the longitudinal displacement X, the lateral displacement Y, and the roll angle θ roll to obtain the accurate translational pose information of the autonomous driving vehicle;

[0035] The accurate rotation pose information of the autonomous driving vehicle and the accurate translational pose information of the autonomous driving vehicle are the optimized pose data of the autonomous driving vehicle, and use the optimized pose data of the autonomous driving vehicle to obtain the optimized point cloud map of the surrounding environment.

[0036] Furthermore, use the loop detection algorithm to optimize the point cloud map of the surrounding environment that returns to the same scene, specifically including:

[0037] Calculate the similarity s(P r ,P c ) of two frames of point clouds using the cosine distance of the two frames of point cloud image view data; that is, obtain the optimized point cloud map of the surrounding environment of the autonomous driving vehicle;

[0038]

[0039] where N s is the number of meta-regions, is the vector extracted from the reference point cloud image view data, is the vector extracted from the point cloud image view data to be detected.

[0040] Use the cosine distance to describe the approximation degree of height values in a certain unit area. The smaller the value of s(P r , P c ), the more similar the two frames of point clouds are.

[0041] A multi-sensor loosely coupled indoor and outdoor synchronous positioning and mapping system, the system includes:

[0042] The system includes:

[0043] One or more processors;

[0044] A memory for storing one or more programs;

[0045] When the one or more programs are executed by the one or more processors, the one or more processors are caused to execute a multi-sensor loosely coupled indoor and outdoor synchronous positioning and mapping method as described in any one of the above.

[0046] Compared with the prior art, the advantages of the present invention are:

[0047] Utilize the loose coupling of multi-sensor data of lidar and inertial measurement unit, enabling an autonomous driving vehicle to perform high-precision synchronous positioning and mapping, with an average trajectory error of 0.06 m in outdoor experiments.

[0048] Utilize the average point cloud depth of lidar data frames to distinguish indoor and outdoor environments. When the driving route of the autonomous driving vehicle passes through indoor and outdoor environments, the average trajectory error of the present invention is 0.95 m.

[0049] Utilize a two-step LM optimization method to reduce the computational amount, ensuring the real-time performance of the algorithm while maintaining the above accuracy. BRIEF DESCRIPTION OF THE DRAWINGS

[0050] Figure 1 、Schematic diagrams of loose coupling and tight coupling of the present invention;

[0051] Figure 2 、General flow chart of a multi-sensor loosely coupled indoor and outdoor synchronous positioning and mapping method of the present invention;

[0052] Figure 3 、Outdoor open environment map established by the present invention;

[0053] Figure 4 、Trajectory of outdoor open environment experiment;

[0054] Figure 5 、Average trajectory error of outdoor open environment experiment;

[0055] Figure 6 、Effect diagram of indoor and outdoor switching driving experiment. Detailed implementation manners

[0056] The following describes the detailed implementation manners of the present invention in conjunction with embodiments:

[0057] It should be noted that the structures, ratios, sizes, etc. shown in this specification are only used to cooperate with the content disclosed in the specification for those skilled in this technology to understand and read, and are not used to limit the implementation conditions of the present invention. Any modification of the structure, change of the proportional relationship or adjustment of the size, without affecting the effects that the present invention can produce and the purposes that can be achieved, should still fall within the scope covered by the technical content disclosed in the present invention.

[0058] At the same time, the terms such as "upper", "lower", "left", "right", "middle" and "one" cited in this specification are only for the convenience of clear narration, and are not used to limit the scope of implementation of the present invention. The change or adjustment of their relative relationship, without substantial change in the technical content, should also be regarded as the scope of implementation of the present invention.

[0059] Embodiment 1

[0060] As Figure 1 shown, a multi-sensor loose-coupling indoor and outdoor simultaneous localization and mapping method, the method includes:

[0061] Perform data preprocessing on the point cloud data and inertial data to obtain the image view data of the point cloud and the rough pose data of the autonomous driving vehicle;

[0062] Perform scene recognition, feature extraction and stitching processing on the image view data of the point cloud to obtain an unoptimized point cloud map of the surrounding environment;

[0063] Based on the results of scene recognition, use the backend optimization algorithm and loop detection algorithm to optimize the rough pose data of the autonomous driving vehicle and the unoptimized point cloud map of the surrounding environment, and obtain the optimized pose data of the autonomous driving vehicle and the point cloud map of the surrounding environment, that is, realize the simultaneous localization and mapping of the autonomous driving vehicle.

[0064] Furthermore, collect point cloud data through a lidar, and collect inertial data through an inertial measurement unit.

[0065] Furthermore, the preprocessing of the point cloud data specifically includes:

[0066] Perform beam splitting on the point cloud data to obtain the vertical splitting result of the point cloud data;

[0067] Perform meta-region horizontal splitting on the point cloud data to obtain the horizontal splitting result of the point cloud data;

[0068] Reproject the vertical segmentation result and horizontal segmentation result of the point cloud data based on the positional relationship between the coordinate system of the preset lidar and the vehicle centroid to obtain the image view data of the point cloud.

[0069] Further, the preprocessing of the inertial data specifically includes:

[0070] Eliminate the gravity component of the inertial data to obtain the rough pose data of the autonomous driving vehicle;

[0071]

[0072] where a' t is the true value of the inertial measurement unit data, a t is the measured value of the inertial measurement unit data, R ZYX is the rotation matrix of the inertial measurement unit data, and g is the gravitational acceleration. By integrating a' t the speed and position of the autonomous driving vehicle are obtained.

[0073] Further, the scene recognition, feature extraction, and stitching processing specifically include:

[0074] Use whether the average point cloud depth d a of the image view data of the point cloud exceeds the threshold as a judgment condition to distinguish whether the autonomous driving vehicle is in an indoor or outdoor environment, and obtain the scene recognition result;

[0075]

[0076] where N is the total number of points in a frame of point cloud, and d i is the Euclidean distance of the i-th point relative to the geometric center of the lidar;

[0077] Based on the scene recognition result, use the curvature c near each point of the point cloud in the image view data of the point cloud to divide the scene features into corner features and plane features, and use the iterative closest point algorithm to register the image view data of the point cloud (point clouds of multiple data frames) to obtain an unoptimized point cloud map of the surrounding environment;

[0078]

[0079] Define the curvature near the i-th point p i on the same horizontal line in the image view data of the point cloud as c, where a is the number of points adjacent to one side of the point p i and p j is the point adjacent to p i ;

[0080] Further, based on the outdoor environment in the scene recognition result, use the backend optimization algorithm for processing, specifically including:

[0081] Based on the outdoor environment, using the Gauss-Newton algorithm in the backend optimization algorithm, optimize the rough pose data of the autonomous driving vehicle and the unoptimized point cloud map of the surrounding environment to obtain the pose data of the autonomous driving vehicle and the point cloud map of the surrounding environment optimized in the outdoor environment.

[0082] Furthermore, based on the indoor environment in the result of scene recognition, process it using the two-step LM algorithm in the backend optimization algorithm.

[0083] Furthermore, process it using the two-step LM algorithm in the backend optimization algorithm, specifically including:

[0084] Optimize the rough pose data of the autonomous driving vehicle and the unoptimized point cloud map of the surrounding environment using two-step LM;

[0085] In the first-step LM optimization, first calculate the pitch angle θ that is more affected by rotation in the six-degree-of-freedom pose using the extracted plane features pitch and the elevation displacement Z, and use the calculation result of the first-step LM optimization as the prior information, that is, the accurate rotation pose information of the autonomous driving vehicle;

[0086] In the second-step LM optimization, calculate the information of the remaining six-degree-of-freedom pose, the yaw angle θ yaw , the longitudinal displacement X, the lateral displacement Y, and the roll angle θ roll to obtain the accurate translational pose information of the autonomous driving vehicle;

[0087] The accurate rotation pose information of the autonomous driving vehicle and the accurate translational pose information of the autonomous driving vehicle are the optimized pose data of the autonomous driving vehicle. Use the optimized pose data of the autonomous driving vehicle to obtain the optimized point cloud map of the surrounding environment.

[0088] Furthermore, use the loop detection algorithm to optimize the point cloud map of the surrounding environment that returns to the same scene, specifically including:

[0089] Calculate the similarity degree s(P r , P c ) of two frames of point clouds using the cosine distance of the two-frame point cloud image view data; that is, obtain the optimized point cloud map of the surrounding environment of the autonomous driving vehicle;

[0090]

[0091] where, N s is the number of meta-regions, is the vector extracted from the reference point cloud image view data, is the vector extracted from the point cloud image view data to be detected.

[0092] Use the cosine distance to describe the approximation degree of height values in a certain unit region, s(P r ,P c ). The smaller the value of s(P r ,P c ), the more similar the two frames of point clouds are.

[0093] A multi-sensor loosely coupled indoor and outdoor simultaneous localization and mapping system, the system includes:

[0094] The system includes:

[0095] One or more processors;

[0096] A memory for storing one or more programs;

[0097] When the one or more programs are executed by the one or more processors, the one or more processors are caused to execute a multi-sensor loosely coupled indoor and outdoor simultaneous localization and mapping method as described in any of the above.

[0098] Embodiment 2

[0099] This embodiment proposes a lidar / IMU loosely coupled simultaneous localization and mapping system; as Figure 2 shown.

[0100] The system takes lidar and IMU signals as inputs and the pose and map of an autonomous vehicle in an unknown environment as outputs. It specifically includes multiple modules such as data preprocessing, scene recognition, lidar odometry, backend optimization, loop detection, etc. The different modules will be introduced in sequence below.

[0101] 1) Data preprocessing.

[0102] This module mainly completes the operations of reprojection, beam segmentation, and de-distortion of lidar data. Due to the sparsity of lidar in three-dimensional space, as the distance increases, the laser point distance between the same horizontal beams is enlarged. Therefore, the lidar point cloud is projected onto a two-dimensional plane using an image view to reduce the dimension of the data and improve the storage efficiency of the data. The abscissa after reprojection represents the points within a 12° range of the same horizontal line of the lidar. This range is defined as the unit region s. Then, each frame of point cloud has 30 elements s0 to s 29 ; the ordinate represents the number of lines of the lidar. According to the different lidar models, the number of lines varies, and the number of ordinate elements is also different. Taking the Velodyne VLP-16 lidar as an example, this product has 16 channels in the vertical direction. Therefore, after completing the reprojection, the ordinate has 16 elements. The line number information of some lidars is published together with the original data, usually referred to as the ring channel. For lidars without a ring channel, the line number attribution can be approximately calculated based on the elevation angle of the data points.

[0103] In the reprojected image matrix, the value of each element is the average height of the lidar points within the current meta-region s. After reprojection, the three-dimensional information of the point cloud is retained while simplifying data storage.

[0104] Define P = {P1, P2, …, P n} as a frame of point cloud obtained by the lidar at time i. Since the lidar frequency is not high, the point cloud cannot be obtained at the same time. Assuming the vehicle is moving at a speed of 10 m / s, although in the lidar coordinate system, the starting point and the ending point of a frame of point cloud are connected, in the world coordinate system, there will be a distance of 1 m between these two points. Therefore, it is necessary to use a high-frequency IMU to perform a de-distortion operation on the point cloud. Before processing, it is necessary to first eliminate the gravity component of the IMU data.

[0105]

[0106] where a' t is the true value of the IMU, a t is the measured value of the IMU, R ZYX is the rotation matrix of the IMU, and g is the acceleration due to gravity. By integrating a' t , the speed and position of the autonomous vehicle can be obtained. The specific process of point cloud de-distortion is as follows: record the starting time t i , t i+1 of a frame of point cloud, the poses T i , T i+1 at the starting and ending times. Assume that the autonomous vehicle moves at a constant speed during the acquisition time of a frame of point cloud. Use linear interpolation to transform all the point clouds within this time to the starting time to complete the de-distortion operation.

[0107] 2) Laser odometry with scene recognition

[0108] After completing the data preprocessing, the laser odometry module starts to execute. This module is the core of simultaneous localization and mapping. The main process is as follows: store the point cloud of each frame of the lidar, extract the feature points of each frame of point cloud, solve the pose transformation matrix between the same feature points between frames, and splice multiple frames of point clouds together through this matrix to form an environmental map. Output the real-time pose of the vehicle and the map.

[0109] Define the average point cloud depth d a to distinguish between indoor and outdoor environments,

[0110]

[0111] where N is the total number of points in a frame of point cloud, d iis the Euclidean distance of the i-th point relative to the geometric center of the lidar. After identifying the current environment, start the feature extraction thread to extract feature points for inter-frame point cloud registration. Inspired by reference [2], define the curvature c of the i-th point p on the same horizontal line in the image view data of the point cloud i nearby.

[0112]

[0113] where a is the number of points adjacent to p on one side, and p i is the point adjacent to p j . By calculating c, the point cloud can be divided into corner features and plane features. i

[0114] Since the ground in the indoor environment is paved with tiles and is flatter than the outdoor environment, the two-step LM method is used to optimize the odometer. Similar to reference [3], in the first step of LM optimization, the pitch angle θ pitch and elevation displacement Z, which are more affected by rotation in the six-degree-of-freedom pose, are calculated using the extracted plane features. Taking the results of the first-step LM optimization as prior information, the remaining six-degree-of-freedom pose information, yaw angle θ yaw , longitudinal displacement X, lateral displacement Y, and roll angle θ roll are further calculated.

[0115] For the outdoor environment, it cannot be assumed that the ground is horizontal. Therefore, when d a is greater than the threshold and it is determined that the autonomous vehicle has run into the outdoor environment, the Gauss-Newton optimization method is used for optimization.

[0116] 3) Loop detection

[0117] The purpose of loop detection is to ensure that when the autonomous vehicle drives through the same scene, the established map can be optimized using this scene as a landmark, reducing the map drift error caused by time accumulation.

[0118] The image view projects the three-dimensional point cloud data onto a two-dimensional plane, facilitating loop detection. For the reference point cloud P r and the current point cloud P c , define the similarity degree s(P r , P c ) of two frames of point clouds:

[0119]

[0120] where N s is the number of meta-regions, and the cosine distance is used to describe the approximation degree of height values under a certain meta-region. The smaller the value of s(P r , P c ), the more similar the two frames of point clouds are.

[0121] The above invention has been tested in both outdoor open environments and indoor-outdoor switching driving environments, and the results are not lower than those of existing simultaneous localization and mapping methods.

[0122] The experimental results in the outdoor open environment are as Figures 3 - 5 shown. This scenario is an athletics stadium. The total length of this experiment is 475m. Compared with the reference data of GNSS, the average trajectory error is 6.7cm.

[0123] The experimental results of indoor-outdoor switching driving are as Figure 6 shown. This experiment starts in an indoor room, drives outdoors through a slope at a constant speed of 1m / s, drives around the building for one circle, and then returns indoors via the slope to complete the circuit loop. Compared with the reference data of GNSS, the average trajectory error is 95cm.

[0124] Embodiment 3

[0125] In terms of the optimization method, the quasi-Newton method can be used instead of the Gauss-Newton method to achieve a similar effect.

[0126] In terms of the loop detection method, the point cloud iris recognition method can be used instead. Reference [4] proposed a global descriptor called Lidar Iris applied to lidar point clouds to perform fast and accurate closed-loop detection. After several LoG-Gabor filtrations and threshold operations on the image represented by LiDAR-Iris, a binary feature image of each point cloud can be obtained. Given two frames of point clouds, their similarity can be calculated through the Hamming distance between the binary feature images extracted from these two frames of point clouds.

[0127] References (such as patents / papers / standards)

[0128] [1][Chinese Invention, Chinese Invention Authorization] CN201910297985.1 A Simultaneous Localization and Mapping Method for Vision-Inertial-Laser Fusion

[0129] [2] ZHANG J, SINGH S. LOAM: Lidar Odometry and Mapping in Real-time [R]. 2014.

[0130] [3]SHAN T,ENGLOT B.LeGO-LOAM:Lightweight and Ground-Optimized LidarOdometry and Mapping on Variable Terrain[C / OL] / / 2018IEEE / RSJ InternationalConference on Intelligent Robots and Systems(IROS).Madrid:IEEE,2018:4758-4765.

[0131] [4]WANG Y,SUN Z,XU C Z,et al.LiDAR Iris for Loop-Closure Detection[C / OL] / / 2020IEEE / RSJ International Conference on Intelligent Robots and Systems(IROS).Las Vegas,NV,USA:IEEE,2020:5769-5775.

[0132] The above has described in detail the preferred embodiments of the present invention. However, the present invention is not limited to the above embodiments, and various changes can be made without departing from the spirit of the present invention within the scope of knowledge possessed by those of ordinary skill in the art.

[0133] Many other changes and modifications can be made without departing from the concept and scope of the present invention. It should be understood that the present invention is not limited to specific embodiments, and the scope of the present invention is defined by the appended claims.

Claims

1. A multi-sensor loose-coupling indoor and outdoor synchronous positioning and mapping method, characterized in that, The method includes: Performing data preprocessing on the point cloud data and inertial data to obtain the image view data of the point cloud and the rough pose data of the autonomous driving vehicle; Performing scene recognition, feature extraction, and stitching processing on the image view data of the point cloud to obtain an unoptimized point cloud map of the surrounding environment; Based on the result of scene recognition, using a backend optimization algorithm and a loop detection algorithm to optimize the rough pose data of the autonomous driving vehicle and the unoptimized point cloud map of the surrounding environment, obtaining the optimized pose data of the autonomous driving vehicle and the point cloud map of the surrounding environment, that is, realizing the simultaneous localization and mapping of the autonomous driving vehicle; Among them, the performing scene recognition, feature extraction, and stitching processing specifically includes: Average point cloud depth d of the image view data using the point cloud a Whether it exceeds a threshold is used as a judgment condition to distinguish whether the autonomous vehicle is in an indoor or outdoor environment, and a scene recognition result is obtained; where N is the total number of points in a frame of point cloud, and d i is the Euclidean distance of the i-th point relative to the geometric center of the lidar; Based on the result of scene recognition, using the curvature c near each point of the point cloud in the image view data of the point cloud to divide the scene features into corner features and plane features, and using the iterative closest point algorithm to register the image view data of the point cloud to obtain an unoptimized point cloud map of the surrounding environment; Define the curvature near the \(i\)-th point \(p_i\) on the same horizontal line in the image view data of the point cloud as \(c\), where \(a\) is the point \(p\) i The number of points adjacent on one side, and \(p_j\) is the point i Adjacent to \(p\).

2. A multi-sensor loose-coupled indoor and outdoor synchronous positioning and mapping method according to claim 1, characterized in that, Collecting point cloud data through a lidar and collecting inertial data through an inertial measurement unit.

3. A method for indoor and outdoor synchronous positioning and mapping with multi-sensor loose coupling according to claim 1, characterized in that The preprocessing of the point cloud data specifically includes: Performing beam splitting on the point cloud data to obtain the vertical splitting result of the point cloud data; Performing meta-region horizontal splitting on the point cloud data to obtain the horizontal splitting result of the point cloud data; Using the preset positional relationship between the coordinate system of the lidar and the position of the vehicle centroid to reproject the vertical splitting result and the horizontal splitting result of the point cloud data to obtain the image view data of the point cloud.

4. A multi-sensor loose-coupled indoor and outdoor simultaneous localization and mapping method according to claim 1, characterized in that, The preprocessing of the inertial data specifically includes: Eliminating the gravity component of the inertial data to obtain the rough pose data of the autonomous driving vehicle; where a' t is the true value of the inertial measurement unit data, a t is the measured value of the inertial measurement unit data, R ZYX is the rotation matrix of the inertial measurement unit data, and g is the acceleration due to gravity.

5. A multi-sensor loose-coupled indoor and outdoor synchronous positioning and mapping method according to claim 1, characterized in that Based on the outdoor environment in the result of scene recognition, using a backend optimization algorithm for processing, specifically including: Based on the outdoor environment, using the Gauss-Newton algorithm in the backend optimization algorithm to optimize the rough pose data of the autonomous driving vehicle and the unoptimized point cloud map of the surrounding environment, obtaining the optimized pose data of the autonomous driving vehicle and the point cloud map of the surrounding environment in the outdoor environment.

6. A method for indoor and outdoor synchronous positioning and mapping with multi-sensor loose coupling according to claim 1, characterized in that, Based on the indoor environment in the result of scene recognition, using the two-step LM algorithm in the backend optimization algorithm for processing.

7. A method for indoor and outdoor synchronous positioning and mapping with multi-sensor loose coupling according to claim 6, characterized in that Using the two-step LM algorithm in the backend optimization algorithm for processing, specifically including: Performing two-step LM optimization on the rough pose data of the autonomous driving vehicle and the unoptimized point cloud map of the surrounding environment; The first step of LM optimization is to first calculate the pitch angle θ that is greatly affected by rotation in the six-degree-of-freedom pose using the extracted planar features pitch and the elevation displacement Z. The calculation result of the first step of LM optimization is used as prior information, that is, the accurate rotation pose information of the autonomous vehicle; Step 2: LM optimization, calculate the information of the remaining six degrees of freedom pose, i.e., yaw angle θ yaw , longitudinal displacement X, lateral displacement Y, and barrel roll angle θ roll , to obtain the accurate translational pose information of the autonomous vehicle The accurate rotational pose information and accurate translational pose information of the autonomous driving vehicle are the optimized pose data of the autonomous driving vehicle, and the optimized point cloud map of the surrounding environment is obtained using the optimized pose data of the autonomous driving vehicle.

8. A method for indoor and outdoor synchronous positioning and mapping with multi-sensor loose coupling according to claim 1, characterized in that, Using a loop detection algorithm to optimize the point cloud map of the surrounding environment that returns to the same scene, specifically including: Calculate the similarity degree s(P r , P c ) of two frames of point clouds using the cosine distance of the point cloud image view data of the two frames; that is, obtain the optimized point cloud map of the surrounding environment of the autonomous vehicle; Among them, N s is the number of meta-regions, is a vector extracted from the reference point cloud image view data, is a vector extracted from the point cloud image view data to be detected.

9. A multi-sensor loosely coupled indoor and outdoor simultaneous localization and mapping system, characterized in that, The system includes: The system includes: One or more processors; A memory for storing one or more programs; When the one or more programs are executed by the one or more processors, the one or more processors are caused to execute a multi-sensor loose-coupling indoor-outdoor simultaneous localization and mapping method as described in any one of claims 1-8.

Images