Positioning method, system and equipment based on laser radar and inertial navigation and medium
Through lidar and inertial navigation technology, combined with data correction and closed-loop detection, a factor diagram is constructed, which solves the shortcomings in accuracy and robustness of traditional smart car positioning methods, and achieves efficient and accurate smart car positioning.
Patent Information
- Application Number
- CN202411238913.7
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- Filing Date
- 2024-09-05
- Publication Date
- 2024-11-22
AI Technical Summary
Traditional intelligent vehicle positioning methods are difficult to meet the high requirements of modern complex application scenarios in terms of efficiency, accuracy and robustness, and cannot provide high-precision and stable positioning results.
Using the positioning method based on lidar and inertial navigation, point cloud data and attitude data are obtained through lidar units and inertial measurement units, data correction and three-dimensional feature hash mapping closed-loop detection, factor map is constructed to optimize position data, and Global point cloud data updates to achieve accurate positioning of smart cars.
Through coordinate conversion and time stamp synchronization of point cloud data and pose data, the spatial and dimensional alignment of sensors can be achieved, data interference is avoided, positioning accuracy is ensured, and scene recognition efficiency in large-scale environments is improved through deep search and factor graph optimization. Accuracy.
Smart Images

Figure CN119001744A_ABST
Abstract
Description
Technical Field
[0001] The present invention relates to the field of laser radar positioning technology, and in particular to a positioning method, system, equipment and medium based on laser radar and inertial navigation. Background Art
[0002] With the rapid development of intelligent technology, the intelligence level of smart cars is constantly improving. Among them, simultaneous positioning and mapping technology, as the core technology for realizing autonomous driving of smart cars, has become increasingly important. However, in the face of complex and changing actual application scenarios, the performance requirements for smart car precise positioning and mapping algorithms in terms of efficiency, accuracy, and robustness are becoming increasingly stringent. Due to its inherent limitations, traditional smart car positioning methods can no longer meet the urgent needs of modern applications for high accuracy and strong stability. Summary of the invention
[0003] The present invention aims at solving the technical problems existing in the prior art and provides a positioning method, system, device and medium based on laser radar and inertial navigation, including:
[0004] Acquire point cloud data and attitude data of the smart car through a laser radar unit and an inertial measurement unit, and perform data correction according to the point cloud data and attitude data to obtain corrected point cloud data;
[0005] According to the corrected point cloud data, a closed-loop detection is performed by using a three-dimensional feature hash mapping closed-loop detection algorithm to obtain accurate position and posture data of the smart car;
[0006] Based on the precise posture data of the smart car, a factor graph is constructed, and the optimized posture data of the smart car is obtained through the factor graph. According to the posture data of the smart car, the global point cloud data is updated to obtain updated point cloud data, and the smart car is positioned according to the updated point cloud data.
[0007] Preferably, the step of acquiring point cloud data and attitude data of the smart car by using a laser radar unit and an inertial measurement unit, and performing data correction according to the point cloud data and attitude data to obtain corrected point cloud data includes:
[0008] Installing a pre-set laser radar unit and an inertial measurement unit on the smart car, collecting point cloud data of the environment around the smart car through the laser radar unit, and collecting posture data of the smart car through the inertial measurement unit;
[0009] Obtain time tags of the laser radar unit and the inertial measurement unit, and construct a time series diagram of data sampling moments of the laser radar unit and the inertial measurement unit based on the time tags with the timestamp as the horizontal coordinate;
[0010] According to the time series diagram, the point cloud data and the posture data are paired with each other at corresponding times to determine the point cloud data and the posture data at the same time;
[0011] Based on the point cloud data and attitude data at the same time, a mapping relationship between the inertial navigation coordinate system and the laser radar coordinate system is established, and according to the mapping relationship, a transformation matrix between the inertial navigation coordinate system and the laser radar coordinate system is set;
[0012] The point cloud data is converted into an inertial navigation coordinate system based on the transformation matrix to obtain converted point cloud data corresponding to the posture data;
[0013] Performing filtering processing on the conversion point cloud data by a low-pass filtering method to obtain effective conversion point cloud data;
[0014] The effective conversion point cloud data is subjected to secondary filtering processing by a voxel filtering method to obtain a point cloud centroid set, and the point cloud centroid set is subjected to point cloud distortion correction by the posture data collected by the inertial measurement unit to obtain corrected point cloud data.
[0015] Preferably, the effective conversion point cloud data calculation formula is as follows:
[0016]
[0017] in,
[0018] Wherein, L represents the effective conversion point cloud data; M represents the filtering parameters; v(x,y) represents the conversion point cloud data with coordinates (x,y); δ(q,h) represents the posture data with coordinates (q,h); q represents the horizontal coordinate value of the posture data; h represents the vertical coordinate value of the posture data; R represents the set of all point cloud data of the driving environment of the smart car; ρ represents the conversion ratio value; K represents the set of posture data of the driving of the smart car; g(x,y) represents the point cloud data of the driving environment of the smart car with coordinates (x,y); w represents the time parameter; ∈ represents the transformation matrix; ζ represents the coordinate system conversion parameter; a v Indicates the point cloud data value corresponding to the posture data at the same time in the time series graph; b k Indicates the posture data value corresponding to the point cloud data at the same time in the time series graph.
[0019] Preferably, the method of performing secondary filtering on the effective conversion point cloud data by a voxel filtering method to obtain a point cloud centroid set, performing point cloud distortion correction on the point cloud centroid set by using the posture data collected by the inertial measurement unit to obtain the corrected point cloud data includes:
[0020] Calculate the maximum point cloud data and the minimum point cloud data in each axis of the effective conversion point cloud data of each frame in the three-dimensional coordinate system by a voxel filtering method;
[0021] Based on the maximum point cloud data and the minimum point cloud data, calculate the side length of the minimum cuboid in the three-dimensional space that can contain all the converted point cloud data points, and calculate the volume of the minimum cuboid according to the side length;
[0022] According to the preset voxel grid size and the volume of the minimum cuboid, the number of all grids in the minimum cuboid is calculated, and all grids in the minimum cuboid are numbered to obtain numbered voxel grids;
[0023] Based on the numbered voxel grids, the centroid of each voxel grid is calculated, and other converted point cloud data in the corresponding grid are replaced by the centroid to obtain a point cloud centroid set;
[0024] Determine the current time k of the centroid of each point cloud in the point cloud centroid set by using a laser radar unit, and obtain the carrier posture of the smart car at time k and k+1 by solving the inertial measurement unit;
[0025] Integrate the carrier posture to obtain the integrated state quantity of the smart car carrier at time k and k+1, and discretize the integrated state quantity by the median integral method to obtain the pre-integrated quantity at time k and k+1;
[0026] Conditionally constraining the integral state quantity and the pre-integrated quantity at time k and k+1 to obtain a pre-integrated error;
[0027] Based on the pre-integrated error, according to the carrier poses at times k and k+1, the point cloud pose corresponding to the current time k is calculated by a linear interpolation method;
[0028] According to the pose of each point cloud centroid at the starting time and the point cloud pose corresponding to each point cloud centroid at the current time k, the point cloud distortion value of each point cloud centroid at the current time k relative to the starting time is calculated;
[0029] The point cloud distortion value is converted through the world coordinate system to obtain the converted point cloud distortion value at the current time k relative to the starting time;
[0030] The point cloud distortion value at the current moment and the converted point cloud distortion value are subtracted to perform point cloud distortion correction and obtain the corrected point cloud data.
[0031] Preferably, the pre-integration error calculation formula is as follows:
[0032]
[0033] in,
[0034] in,
[0035] Where p represents the pre-integration error; A represents the integral state; B represents the pre-integration quantity; μ represents the Jacobian matrix; ζ1 represents the error parameter; k represents the current time; T α represents the carrier position of the smart car; rotation error; β k Represents the state quantity of the point cloud centroid corresponding to time k; β k+1 Represents the point cloud centroid state corresponding to time k+1; α k Represents the measured value of the point cloud centroid corresponding to time k; α k+1 represents the measured value of the point cloud centroid corresponding to time k+1; η represents the discrete factor; represents the measurement noise.
[0036] Preferably, according to the corrected point cloud data, closed-loop detection is performed by a three-dimensional feature hash mapping closed-loop detection algorithm to obtain accurate position and posture data of the smart car, including:
[0037] According to the corrected point cloud data, the point cloud data is converted into two-dimensional key point data by a three-dimensional feature hash mapping closed-loop detection algorithm, and the two-dimensional key point data is stored in a pre-constructed database;
[0038] Based on the two-dimensional key point data, a predetermined point cloud data space is divided to obtain volume units with different target point cloud data;
[0039] Screening the point cloud with significant edge features in the volume unit by using a preset edge detection algorithm to obtain key point cloud data with edge features, and generating aggregated key points according to the key point cloud data;
[0040] Projecting the aggregated key points on a horizontal plane, and constructing a discretized search space on the horizontal plane with the projected current key points as the center of the circle;
[0041] Wherein, the discretized search space is constructed by a preset number of sectors;
[0042] The first sector is aligned with the direction of the line from the current key point to the nearest neighbor key point to determine the descriptor direction;
[0043] Based on the descriptor direction, number the remaining sectors in counterclockwise order and search for the closest keypoint in each sector;
[0044] When there is a nearest neighbor key point in the sector, a descriptor is generated based on the distance between the current key point and the nearest neighbor key point;
[0045] The distance between the current key point and the nearest neighbor key point is used as the value of the corresponding dimension of the descriptor; otherwise, when there is no nearest neighbor key point in the sector, the value of the descriptor dimension is zero;
[0046] According to the descriptor direction, concatenate descriptors with non-zero dimension values to generate a descriptor;
[0047] Using the key of the descriptor as an index, traverse all position point cloud frames in the database, and record the number of times the key of the descriptor appears in each frame;
[0048] When the frequency of a descriptor in a point cloud frame at a certain position in the database exceeds a preset threshold, the position is marked as a potential matching candidate area;
[0049] Based on the potential matching candidate area, according to the set matching point sets of the current frame and the loop frame, the loop closure value is calculated, and the rotation constraint is performed according to the loop closure value to obtain the accurate posture data of the smart car.
[0050] Preferably, the method of constructing a factor graph based on the precise posture data of the smart car, obtaining optimized posture data of the smart car through the factor graph, updating global point cloud data according to the posture data of the smart car to obtain updated point cloud data, and positioning the smart car according to the updated point cloud data includes:
[0051] Dividing the precise position and posture data of the smart car into factor nodes and variable nodes, mapping the values of the variable nodes on preset scalars through local functions corresponding to the factor nodes, and determining the definition relationship of the variable nodes;
[0052] Connecting the factor nodes and the variable nodes through undirected edges based on the definition relationship to construct a factor graph of the smart car;
[0053] Based on the factor graph, the precise position data of the smart car is added to the sliding window, and the maximum a posteriori estimation of the state in the local window is obtained through joint optimization;
[0054] Optimizing the factor graph according to the maximum a posteriori estimation and the optimization factor, obtaining the rotation parameters and the translation parameters of the precise posture data, and obtaining the optimized posture data of the smart car according to the rotation parameters and the translation parameters;
[0055] Projecting the optimized pose data of each frame of the smart car to the global coordinate system to obtain updated point cloud data;
[0056] The updated point cloud data are superimposed to construct a global operation trajectory of the smart car, and a global point cloud map is obtained. The smart car is positioned according to the global point cloud map.
[0057] Based on the same inventive concept, the present invention also provides a positioning system based on laser radar and inertial navigation, comprising:
[0058] A point cloud correction module is used to obtain point cloud data and posture data of the smart car through a laser radar unit and an inertial measurement unit, and perform data correction according to the point cloud data and posture data to obtain corrected point cloud data;
[0059] A posture acquisition module is used to perform closed-loop detection based on the corrected point cloud data through a three-dimensional feature hash mapping closed-loop detection algorithm to obtain accurate posture data of the smart car;
[0060] The intelligent positioning module is used to construct a factor graph based on the precise posture data of the intelligent vehicle, obtain the optimized posture data of the intelligent vehicle through the factor graph, update the global point cloud data according to the posture data of the intelligent vehicle, obtain the updated point cloud data, and locate the intelligent vehicle according to the updated point cloud data.
[0061] Preferably, the point cloud correction module is specifically used for:
[0062] Installing a pre-set laser radar unit and an inertial measurement unit on the smart car, collecting point cloud data of the environment around the smart car through the laser radar unit, and collecting posture data of the smart car through the inertial measurement unit;
[0063] Obtain time tags of the laser radar unit and the inertial measurement unit, and construct a time series diagram of data sampling moments of the laser radar unit and the inertial measurement unit based on the time tags with the timestamp as the horizontal coordinate;
[0064] According to the time series diagram, the point cloud data and the posture data are paired with each other at corresponding times to determine the point cloud data and the posture data at the same time;
[0065] Based on the point cloud data and attitude data at the same time, a mapping relationship between the inertial navigation coordinate system and the laser radar coordinate system is established, and according to the mapping relationship, a transformation matrix between the inertial navigation coordinate system and the laser radar coordinate system is set;
[0066] The point cloud data is converted into an inertial navigation coordinate system based on the transformation matrix to obtain converted point cloud data corresponding to the posture data;
[0067] Performing filtering processing on the conversion point cloud data by a low-pass filtering method to obtain effective conversion point cloud data;
[0068] The effective conversion point cloud data is subjected to secondary filtering processing by a voxel filtering method to obtain a point cloud centroid set, and the point cloud centroid set is subjected to point cloud distortion correction by the posture data collected by the inertial measurement unit to obtain corrected point cloud data.
[0069] Preferably, the calculation formula for effectively converting point cloud data in the point cloud correction module is as follows:
[0070]
[0071] in,
[0072] Wherein, L represents the effective conversion point cloud data; M represents the filtering parameters; v(x,y) represents the conversion point cloud data with coordinates (x,y); δ(q,h) represents the posture data with coordinates (q,h); q represents the horizontal coordinate value of the posture data; h represents the vertical coordinate value of the posture data; R represents the set of all point cloud data of the driving environment of the smart car; ρ represents the conversion ratio value; K represents the set of posture data of the driving of the smart car; g(x,y) represents the point cloud data of the driving environment of the smart car with coordinates (x,y); w represents the time parameter; ∈ represents the transformation matrix; ζ represents the coordinate system conversion parameter; a v Indicates the point cloud data value corresponding to the posture data at the same time in the time series graph; b k Indicates the posture data value corresponding to the point cloud data at the same time in the time series graph.
[0073] Preferably, the point cloud correction module performs secondary filtering processing on the effective conversion point cloud data by a voxel filtering method to obtain a point cloud centroid set, performs point cloud distortion correction on the point cloud centroid set by using the posture data collected by the inertial measurement unit to obtain the corrected point cloud data, including:
[0074] Calculate the maximum point cloud data and the minimum point cloud data in each axis of the effective conversion point cloud data of each frame in the three-dimensional coordinate system by a voxel filtering method;
[0075] Based on the maximum point cloud data and the minimum point cloud data, calculate the side length of the minimum cuboid in the three-dimensional space that can contain all the converted point cloud data points, and calculate the volume of the minimum cuboid according to the side length;
[0076] According to the preset voxel grid size and the volume of the minimum cuboid, the number of all grids in the minimum cuboid is calculated, and all grids in the minimum cuboid are numbered to obtain numbered voxel grids;
[0077] Based on the numbered voxel grids, the centroid of each voxel grid is calculated, and other converted point cloud data in the corresponding grid are replaced by the centroid to obtain a point cloud centroid set;
[0078] Determine the current time k of the centroid of each point cloud in the point cloud centroid set by using a laser radar unit, and obtain the carrier posture of the smart car at time k and k+1 by solving the inertial measurement unit;
[0079] Integrate the carrier posture to obtain the integrated state quantity of the smart car carrier at time k and k+1, and discretize the integrated state quantity by the median integral method to obtain the pre-integrated quantity at time k and k+1;
[0080] Conditionally constraining the integral state quantity and the pre-integrated quantity at time k and k+1 to obtain a pre-integrated error;
[0081] Based on the pre-integrated error, according to the carrier poses at times k and k+1, the point cloud pose corresponding to the current time k is calculated by a linear interpolation method;
[0082] According to the pose of each point cloud centroid at the starting time and the point cloud pose corresponding to each point cloud centroid at the current time k, the point cloud distortion value of each point cloud centroid at the current time k relative to the starting time is calculated;
[0083] The point cloud distortion value is converted through the world coordinate system to obtain the converted point cloud distortion value at the current time k relative to the starting time;
[0084] The point cloud distortion value at the current moment and the converted point cloud distortion value are subtracted to perform point cloud distortion correction and obtain the corrected point cloud data.
[0085] Preferably, the pre-integration error calculation formula in the point cloud correction module is as follows:
[0086]
[0087] in,
[0088] in,
[0089] Where p represents the pre-integration error; A represents the integral state; B represents the pre-integration quantity; μ represents the Jacobian matrix; ζ1 represents the error parameter; k represents the current time; T α represents the carrier position of the smart car; rotation error; β k Represents the state quantity of the point cloud centroid corresponding to time k; β k+1 Represents the point cloud centroid state corresponding to time k+1; α k Represents the measured value of the point cloud centroid corresponding to time k; α k+1 represents the measured value of the point cloud centroid corresponding to time k+1; η represents the discrete factor; represents the measurement noise.
[0090] Preferably, the posture acquisition module is specifically used for:
[0091] According to the corrected point cloud data, the point cloud data is converted into two-dimensional key point data by a three-dimensional feature hash mapping closed-loop detection algorithm, and the two-dimensional key point data is stored in a pre-constructed database;
[0092] Based on the two-dimensional key point data, a predetermined point cloud data space is divided to obtain volume units with different target point cloud data;
[0093] Screening the point cloud with significant edge features in the volume unit by using a preset edge detection algorithm to obtain key point cloud data with edge features, and generating aggregated key points according to the key point cloud data;
[0094] Projecting the aggregated key points on a horizontal plane, and constructing a discretized search space on the horizontal plane with the projected current key points as the center of the circle;
[0095] Wherein, the discretized search space is constructed by a preset number of sectors;
[0096] The first sector is aligned with the direction of the line from the current key point to the nearest neighbor key point to determine the descriptor direction;
[0097] Based on the descriptor direction, number the remaining sectors in counterclockwise order and search for the closest keypoint in each sector;
[0098] When there is a nearest neighbor key point in the sector, a descriptor is generated based on the distance between the current key point and the nearest neighbor key point;
[0099] The distance between the current key point and the nearest neighbor key point is used as the value of the corresponding dimension of the descriptor; otherwise, when there is no nearest neighbor key point in the sector, the value of the descriptor dimension is zero;
[0100] According to the descriptor direction, concatenate descriptors with non-zero dimension values to generate a descriptor;
[0101] Using the key of the descriptor as an index, traverse all position point cloud frames in the database, and record the number of times the key of the descriptor appears in each frame;
[0102] When the frequency of a descriptor in a point cloud frame at a certain position in the database exceeds a preset threshold, the position is marked as a potential matching candidate area;
[0103] Based on the potential matching candidate area, according to the set matching point sets of the current frame and the loop frame, the loop closure value is calculated, and the rotation constraint is performed according to the loop closure value to obtain the accurate posture data of the smart car.
[0104] Preferably, the intelligent positioning module is specifically used for:
[0105] Dividing the precise position and posture data of the smart car into factor nodes and variable nodes, mapping the values of the variable nodes on preset scalars through local functions corresponding to the factor nodes, and determining the definition relationship of the variable nodes;
[0106] Connecting the factor nodes and the variable nodes through undirected edges based on the definition relationship to construct a factor graph of the smart car;
[0107] Based on the factor graph, the precise position data of the smart car is added to the sliding window, and the maximum a posteriori estimation of the state in the local window is obtained through joint optimization;
[0108] Optimizing the factor graph according to the maximum a posteriori estimation and the optimization factor, obtaining the rotation parameters and the translation parameters of the precise posture data, and obtaining the optimized posture data of the smart car according to the rotation parameters and the translation parameters;
[0109] Projecting the optimized pose data of each frame of the smart car to the global coordinate system to obtain updated point cloud data;
[0110] The updated point cloud data are superimposed to construct a global operation trajectory of the smart car, and a global point cloud map is obtained. The smart car is positioned according to the global point cloud map.
[0111] Based on the same inventive concept, the present invention also provides an electronic device, including:
[0112] Memory for storing computer software programs;
[0113] The processor is used to read and execute the computer software program, thereby implementing the positioning method based on laser radar and inertial navigation.
[0114] Based on the same inventive concept, the present invention also provides a non-transitory computer-readable storage medium, in which a computer software program is stored. When the computer software program is executed by a processor, the positioning method based on laser radar and inertial navigation is implemented.
[0115] The beneficial effects of the present invention are as follows: 1. The present invention provides a positioning method, system, device and medium based on laser radar and inertial navigation, including: obtaining point cloud data and posture data of a smart car through a laser radar unit and an inertial measurement unit, performing data correction according to the point cloud data and posture data to obtain corrected point cloud data; performing closed-loop detection through a three-dimensional feature hash mapping closed-loop detection algorithm according to the corrected point cloud data to obtain precise posture data of the smart car; constructing a factor graph based on the precise posture data of the smart car, obtaining optimized posture data of the smart car through the factor graph, performing global point cloud data update according to the posture data of the smart car to obtain updated point cloud data, and positioning the smart car according to the updated point cloud data. The present invention can realize the alignment of each sensor in the time and space dimensions by performing coordinate conversion and timestamp synchronization on the collected point cloud data and posture data, and correcting the data to avoid data interference, thereby ensuring the accuracy of positioning;
[0116] 2. The present invention performs closed-loop detection through a three-dimensional feature hash mapping closed-loop detection algorithm to obtain accurate posture data of the smart car, converts the original three-dimensional lidar data into two-dimensional key points, and stores them in a dedicated database. The algorithm deeply searches historical frames to achieve six-degree-of-freedom closed-loop posture correction, ensuring efficient and accurate recognition of historical scenes in large-scale environments;
[0117] 3. The present invention also solves the optimization calculation pressure caused by map expansion by constructing a factor graph. The factor graph uses the position of the smart car as a node, optimizes the factor graph, and updates the global point cloud data to further improve the positioning accuracy of the smart car and the global map. Figure 1 Consistency. BRIEF DESCRIPTION OF THE DRAWINGS
[0118] Figure 1 The present invention provides a flow chart of a positioning method based on laser radar and inertial navigation;
[0119] Figure 2 This is a schematic diagram of the connection of the positioning system modules based on laser radar and inertial navigation of the present invention.
[0120] Figure 3 This is a schematic diagram of the structure of an electronic device of the present invention;
[0121] Figure 4 A schematic diagram of the structure of a non-transitory computer-readable storage medium according to the present invention;
[0122] In the accompanying drawings, the components represented by the reference numerals are listed as follows:
[0123] 500 - electronic device, 510 - memory, 511 - computer program, 520 - processor, 600 - computer readable storage medium, 611 - computer program. DETAILED DESCRIPTION
[0124] 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 those skilled in the art without creative work are within the scope of protection of this application.
[0125] In the description of this application, the terms "first" and "second" are used for descriptive purposes only and should not be understood as indicating or implying relative importance or implicitly indicating the number of technical features indicated. Thus, a feature defined as "first" or "second" may explicitly or implicitly include one or more of the features. In the description of this application, "plurality" means two or more, unless otherwise clearly and specifically defined.
[0126] In the description of the present application, the term "for example" is used to mean "used as an example, illustration or description". Any embodiment described as "for example" in the present application is not necessarily to be construed as being more preferred or advantageous than other embodiments. The following description is given to enable any technician in the field to implement and use the present invention. In the following description, details are listed for the purpose of explanation. It should be understood that a person of ordinary skill in the art can recognize that the present invention can be implemented without using these specific details. In other examples, well-known structures and processes will not be elaborated in detail to avoid unnecessary details to obscure the description of the present invention. Therefore, the present invention is not intended to be limited to the embodiments shown, but is consistent with the widest scope consistent with the principles and features disclosed in the present application.
[0127] Embodiment 1:
[0128] The flowchart of the positioning method based on laser radar and inertial navigation provided by the present invention is as follows: Figure 1 As shown, including:
[0129] Step 1: Obtain point cloud data and attitude data of the smart car through a laser radar unit and an inertial measurement unit, and perform data correction according to the point cloud data and attitude data to obtain corrected point cloud data;
[0130] Step 2: Based on the corrected point cloud data, a closed-loop detection is performed by using a three-dimensional feature hash mapping closed-loop detection algorithm to obtain accurate position and posture data of the smart car;
[0131] Step 3: Based on the precise posture data of the smart car, a factor graph is constructed, and the optimized posture data of the smart car is obtained through the factor graph. According to the posture data of the smart car, the global point cloud data is updated to obtain updated point cloud data, and the smart car is positioned according to the updated point cloud data.
[0132] Specifically, the step 1 includes:
[0133] Installing a pre-set laser radar unit and an inertial measurement unit on the smart car, collecting point cloud data of the environment around the smart car through the laser radar unit, and collecting posture data of the smart car through the inertial measurement unit;
[0134] Obtain time tags of the laser radar unit and the inertial measurement unit, and construct a time series diagram of data sampling moments of the laser radar unit and the inertial measurement unit based on the time tags with the timestamp as the horizontal coordinate;
[0135] According to the time series diagram, the point cloud data and the posture data are paired with each other at corresponding times to determine the point cloud data and the posture data at the same time;
[0136] Among them, due to the differences in operating frequencies between sensors, different strategies need to be adopted for time alignment to ensure the timing consistency of the data;
[0137] According to the time tags on the two sensor frames in the lidar unit and the inertial measurement unit, and the inherent differences in the data sampling frequency of the sensors in the lidar unit and the inertial measurement unit, a time series diagram reflecting the sampling moment of each sensor is constructed with the timestamp as the horizontal axis;
[0138] When the lidar sensor completes a sampling, the main system immediately searches the data stream of the inertial measurement unit for the corresponding data point closest to the sampling moment. This process ensures that although the two sensors have different sampling rates, the data collected by each can be aligned on the time axis through time pairing.
[0139] Based on the point cloud data and attitude data at the same time, a mapping relationship between the inertial navigation coordinate system and the laser radar coordinate system is established, and according to the mapping relationship, a transformation matrix between the inertial navigation coordinate system and the laser radar coordinate system is set;
[0140] Among them, when converting between the inertial navigation coordinate system and the lidar coordinate system, it is necessary to establish an accurate mapping relationship between them so as to obtain the current state of the point cloud data when the system is running;
[0141] In the inertial navigation coordinate system, a conventional standard is usually adopted, in which the OX axis points to the due north of the earth, the OZ axis points to the due east of the earth, and the OY axis is determined by the right-hand orthogonal criterion, that is, it forms a right-hand coordinate system with the OX and OZ axes;
[0142] Let the transformation matrix between the two coordinate systems be ∈ to represent the transformation from the LiDAR coordinate system to the INS coordinate system. In the system, it is necessary to ensure the coincidence between the two coordinate systems, that is, the point cloud data in the LiDAR coordinate system can completely correspond to the attitude information in the INS coordinate system;
[0143] Considering the rotation operation of the inertial navigation coordinate system, it is assumed that the inertial measurement unit rotates γ, θ, Angle, so that the two coordinate systems coincide;
[0144] The transformation matrix ∈ is expressed as follows:
[0145]
[0146] In the formula, ∈ represents the transformation matrix; γ represents the angle of rotation around the X-axis; θ represents the angle of rotation around the Y-axis; Indicates the rotation angle around the Z axis.
[0147] The point cloud data is converted into an inertial navigation coordinate system based on the transformation matrix to obtain converted point cloud data corresponding to the posture data;
[0148] Performing filtering processing on the conversion point cloud data by a low-pass filtering method to obtain effective conversion point cloud data;
[0149] The effective conversion point cloud data is subjected to secondary filtering processing by a voxel filtering method to obtain a point cloud centroid set, and the point cloud centroid set is subjected to point cloud distortion correction by the posture data collected by the inertial measurement unit to obtain corrected point cloud data.
[0150] The calculation formula of the effective conversion point cloud data is as follows:
[0151]
[0152] in,
[0153] Wherein, L represents the effective conversion point cloud data; M represents the filtering parameters; v(x,y) represents the conversion point cloud data with coordinates (x,y); δ(q,h) represents the posture data with coordinates (q,h); q represents the horizontal coordinate value of the posture data; h represents the vertical coordinate value of the posture data; R represents the set of all point cloud data of the driving environment of the smart car; ρ represents the conversion ratio value; K represents the set of posture data of the driving of the smart car; g(x,y) represents the point cloud data of the driving environment of the smart car with coordinates (x,y); w represents the time parameter; ∈ represents the transformation matrix; ζ represents the coordinate system conversion parameter; a v Indicates the point cloud data value corresponding to the posture data at the same time in the time series graph; b k Indicates the posture data value corresponding to the point cloud data at the same time in the time series graph.
[0154] The method of performing secondary filtering on the effective conversion point cloud data by a voxel filtering method to obtain a point cloud centroid set, and performing point cloud distortion correction on the point cloud centroid set by using the posture data collected by the inertial measurement unit to obtain corrected point cloud data includes:
[0155] Calculate the maximum point cloud data and the minimum point cloud data in each axis of the converted point cloud data in each frame in the three-dimensional coordinate system by a voxel filtering method;
[0156] Based on the maximum point cloud data and the minimum point cloud data, calculate the side length of the minimum cuboid in the three-dimensional space that can contain all the converted point cloud data points, and calculate the volume of the minimum cuboid according to the side length;
[0157] According to the preset voxel grid size and the volume of the minimum cuboid, the number of all grids in the minimum cuboid is calculated, and all grids in the minimum cuboid are numbered to obtain numbered voxel grids;
[0158] The calculation formula for the number of all grids in the minimum cuboid is as follows:
[0159]
[0160] in,
[0161] In the formula, O X Represents the number of voxel grids on the X axis; Y Represents the number of voxel grids on the Y axis; Z Indicates the number of voxel grids on the Z axis; L X Indicates the side length of the smallest cuboid on the X-axis; L Y Indicates the side length of the smallest cuboid on the Y axis; L Z Indicates the side length of the smallest cuboid on the Z axis; X maxIndicates the maximum point cloud data on the X axis; X min Indicates the minimum point cloud data on the X axis; Y max Indicates the maximum point cloud data on the Y axis; min Indicates the minimum point cloud data on the Y axis; Z max Indicates the maximum point cloud data on the Z axis; Z min Indicates the minimum point cloud data on the Z axis; S X Indicates the voxel grid size set on the X axis; S Y Indicates the voxel grid size set on the Y axis; S Z Indicates the voxel grid size set on the Z axis; O indicates the total number of grids.
[0162] The numbering rules are as follows:
[0163]
[0164] In the formula, i, j, and l represent the voxel indexes corresponding to the X, Y, and Z axis directions respectively; X represents the total value of the point cloud data on the X axis; X min Indicates the minimum point cloud data on the X axis; Y indicates the total value of the point cloud data on the Y axis; Y min Indicates the minimum point cloud data on the Y axis; Z indicates the total value of the point cloud data on the Z axis; Z min Indicates the minimum point cloud data on the Z axis; S X Indicates the voxel grid size set on the X axis; S Y Indicates the voxel grid size set on the Y axis; S Z Indicates the voxel grid size set on the Z axis.
[0165] Based on the numbered voxel grids, the centroid of each voxel grid is calculated, and other converted point cloud data in the corresponding grid are replaced by the centroid to obtain a point cloud centroid set;
[0166] Among them, voxel filtering is achieved by dividing the three-dimensional point cloud space into a series of uniform voxel grids, each voxel represents a tiny cubic volume element;
[0167] The sensor in the LiDAR unit is a high-precision 3D sensing device, and the raw point cloud data it collects usually has a huge volume of data;
[0168] However, such data inevitably contain noise points and outliers. These non-ideal components not only significantly increase the computational complexity of the point cloud registration process, but also have a negative impact on the registration accuracy.
[0169] Therefore, the present invention first performs a first noise reduction process on the collected point cloud data by a low-pass filtering method, which can effectively remove part of the noise;
[0170] Then, the point cloud is subjected to secondary filtering by using a voxel filtering method. The present invention uses the calculated point cloud centroid as the representative of the point cloud in each voxel grid to improve the computational efficiency of point cloud registration and to retain the effective point cloud reflecting the geometric characteristics of the real environment to the maximum extent.
[0171] The center of mass position is calculated as follows:
[0172]
[0173] In the formula, O ijl represents the centroid position corresponding to voxel index i, j, l; n represents the total number of all point clouds in the voxel grid; τ q Indicates the grid point corresponding to the qth point cloud; q represents the number of point clouds;
[0174] Determine the current time k of the centroid of each point cloud in the point cloud centroid set by using a laser radar unit, and obtain the carrier posture of the smart car at time k and k+1 by solving the inertial measurement unit;
[0175] Among them, the point cloud data received by the line lidar sensor during a single complete rotation at a constant angular velocity strictly follows the principle of time sequence arrangement;
[0176] The carrier posture corresponding to time is calculated as follows:
[0177]
[0178] Where, T k represents the carrier position corresponding to the current time k; T k+1 Indicates the carrier position corresponding to the next moment k+1; At time k on the X axis; γ k represents the rotation angle of the inertial measurement unit around the X axis at time k; γ k represents the rotation angle of the inertial measurement unit around the Y axis at time k; γ k represents the rotation angle of the inertial measurement unit around the Z axis at time k; γ k+1 represents the rotation angle of the inertial measurement unit around the X axis at time k+1; γ k+1 represents the rotation angle of the inertial measurement unit around the Y axis at time k+1; γ k+1 Indicates the rotation angle of the inertial measurement unit around the Z axis at time k+1; Indicates the position and attitude of the laser radar unit corresponding to the center of mass at time k on the X axis; Indicates the position and attitude of the laser radar unit corresponding to the center of mass at time k on the Y axis; Indicates the position and posture of the laser radar unit corresponding to the center of mass at time k on the Z axis; Indicates the position and posture of the laser radar unit corresponding to the center of mass at time k+1 on the X axis; Indicates the position and posture of the laser radar unit corresponding to the center of mass at time k+1 on the Y axis; Indicates the position and posture of the laser radar unit at the k+1 moment on the Z axis corresponding to the center of mass.
[0179] Integrate the carrier posture to obtain the integrated state quantity of the smart car carrier at time k and k+1, and discretize the integrated state quantity by the median integral method to obtain the pre-integrated quantity at time k and k+1;
[0180] Conditionally constraining the integral state quantity and the pre-integrated quantity at time k and k+1 to obtain a pre-integrated error;
[0181] Based on the pre-integrated error, according to the carrier poses at times k and k+1, the point cloud pose corresponding to the current time k is calculated by a linear interpolation method;
[0182] T c =b f T k+1 +b b T k
[0183] Where, T c represents the point cloud pose; T k represents the carrier position corresponding to the current time k; T k+1 represents the carrier position corresponding to the next moment k+1; b f Indicates the first proportional coefficient of linear interpolation; b b Indicates the second scaling factor for linear interpolation.
[0184] According to the pose of each point cloud centroid at the starting time and the point cloud pose corresponding to each point cloud centroid at the current time k, the point cloud distortion value of each point cloud centroid at the current time k relative to the starting time is calculated;
[0185] The point cloud distortion value is converted through the world coordinate system to obtain the converted point cloud distortion value at the current time k relative to the starting time;
[0186] The point cloud distortion value at the current moment and the converted point cloud distortion value are subtracted to perform point cloud distortion correction and obtain the corrected point cloud data.
[0187] The pre-integration error calculation formula is as follows:
[0188]
[0189] in,
[0190] in,
[0191] Where p represents the pre-integration error; A represents the integral state; B represents the pre-integration quantity; μ represents the Jacobian matrix; ζ1 represents the error parameter; k represents the current time; T α represents the carrier position of the smart car; rotation error; β k Represents the state quantity of the point cloud centroid corresponding to time k; β k+1 Represents the point cloud centroid state corresponding to time k+1; α k Represents the measured value of the point cloud centroid corresponding to time k; α k+1 represents the measured value of the point cloud centroid corresponding to time k+1; η represents the discrete factor; represents the measurement noise.
[0192] Specifically, the step 2 includes:
[0193] According to the corrected point cloud data, the point cloud data is converted into two-dimensional key point data by a three-dimensional feature hash mapping closed-loop detection algorithm, and the two-dimensional key point data is stored in a pre-constructed database;
[0194] Based on the two-dimensional key point data, a predetermined point cloud data space is divided to obtain volume units with different target point cloud data;
[0195] Screening the point cloud with significant edge features in the volume unit by using a preset edge detection algorithm to obtain key point cloud data with edge features, and generating aggregated key points according to the key point cloud data;
[0196] Projecting the aggregated key points on a horizontal plane, and constructing a discretized search space on the horizontal plane with the projected current key points as the center of the circle;
[0197] Wherein, the discretized search space is constructed by a preset number of sectors;
[0198] The first sector is aligned with the direction of the line from the current key point to the nearest neighbor key point to determine the descriptor direction;
[0199] Based on the descriptor direction, number the remaining sectors in counterclockwise order and search for the closest keypoint in each sector;
[0200] When there is a nearest neighbor key point in the sector, a descriptor is generated based on the distance between the current key point and the nearest neighbor key point;
[0201] In the process of descriptor generation, all clustered key points are first projected onto the horizontal plane to eliminate the influence caused by the non-uniform distribution of cluster edge key points in the vertical direction;
[0202] By projecting the key points onto the horizontal plane, the horizontal distribution characteristics between the key points can be better captured, making the descriptor more accurate and robust;
[0203] The distance between the current key point and the nearest neighbor key point is used as the value of the corresponding dimension of the descriptor; otherwise, when there is no nearest neighbor key point in the sector, the value of the descriptor dimension is zero;
[0204] Among them, a distance priority-based strategy is adopted to obtain the value of each dimension of the descriptor. Specifically, the four nearest points are selected and a descriptor is generated for each point;
[0205] The purpose of this is to improve the robustness of the descriptor. In this process, the priority order of these four points is determined according to their distances;
[0206] First, the descriptor generated by the nearest neighbor is given the highest priority and regarded as the primary candidate. Then, the remaining three neighbors are examined in increasing order of distance. The priority is determined by the effectiveness of the descriptor of the previous neighbor.
[0207] Specifically, if the descriptor of the previous neighbor is non-zero, it is given the second highest priority; conversely, if the previous descriptor is zero, the next neighbor is examined in sequence until a non-zero descriptor is encountered, which is given the corresponding priority;
[0208] This process continues until all neighboring points have been traversed, ensuring that each subsequent neighboring point is included in the priority sorting only when the previous neighboring point descriptor is invalid (i.e., the value is zero);
[0209] Finally, the non-zero descriptors with the highest priority are concatenated to form the final fused descriptor;
[0210] According to the descriptor direction, concatenate descriptors with non-zero dimension values to generate a descriptor;
[0211] The generated descriptors can effectively distinguish different feature points, even in areas with similar or repeated textures, and are highly resistant to common interference factors such as noise, occlusion, and perspective changes, ensuring high matching accuracy in various complex scenarios.
[0212] Using the key of the descriptor as an index, traverse all position point cloud frames in the database, and record the number of times the key of the descriptor appears in each frame;
[0213] When the frequency of a descriptor in a point cloud frame at a certain position in the database exceeds a preset threshold, the position is marked as a potential matching candidate area;
[0214] Based on the potential matching candidate area, according to the set matching point sets of the current frame and the loop frame, the loop closure value is calculated, and the rotation constraint is performed according to the loop closure value to obtain the accurate posture data of the smart car.
[0215] Wherein, the cycle closure value calculation formula is as follows:
[0216]
[0217] Where, F represents the cycle closure value; H loop represents the cyclic frame matching point set; H cur represents the matching point set of the current frame; χ represents the total number of potential candidate regions; i represents the number of potential candidate regions; R loop represents the rotation point set of the cyclic frame matching point set; T cue Represents the translation point set of the current frame matching point set.
[0218] Specifically, the step 3 includes:
[0219] Dividing the precise position and posture data of the smart car into factor nodes and variable nodes, mapping the values of the variable nodes on preset scalars through local functions corresponding to the factor nodes, and determining the definition relationship of the variable nodes;
[0220] Connecting the factor nodes and the variable nodes through undirected edges based on the definition relationship to construct a factor graph of the smart car;
[0221] Based on the factor graph, the precise position data of the smart car is added to the sliding window, and the maximum a posteriori estimation of the state in the local window is obtained through joint optimization;
[0222] Optimizing the factor graph according to the maximum a posteriori estimation and the optimization factor, obtaining the rotation parameters and the translation parameters of the precise posture data, and obtaining the optimized posture data of the smart car according to the rotation parameters and the translation parameters;
[0223] Projecting the optimized pose data of each frame of the smart car to the global coordinate system to obtain updated point cloud data;
[0224] The updated point cloud data are superimposed to construct a global operation trajectory of the smart car, and a global point cloud map is obtained. The smart car is positioned according to the global point cloud map.
[0225] Embodiment 2:
[0226] The present invention provides a schematic diagram of the connection of the temperature online monitoring system module for the intelligent power distribution cabinet. Figure 2 As shown, including:
[0227] A point cloud correction module is used to obtain point cloud data and posture data of the smart car through a laser radar unit and an inertial measurement unit, and perform data correction according to the point cloud data and posture data to obtain corrected point cloud data;
[0228] A posture acquisition module is used to perform closed-loop detection based on the corrected point cloud data through a three-dimensional feature hash mapping closed-loop detection algorithm to obtain accurate posture data of the smart car;
[0229] The intelligent positioning module is used to construct a factor graph based on the precise posture data of the intelligent vehicle, obtain the optimized posture data of the intelligent vehicle through the factor graph, update the global point cloud data according to the posture data of the intelligent vehicle, obtain the updated point cloud data, and locate the intelligent vehicle according to the updated point cloud data.
[0230] Specifically, the point cloud correction module is specifically used for:
[0231] Installing a pre-set laser radar unit and an inertial measurement unit on the smart car, collecting point cloud data of the environment around the smart car through the laser radar unit, and collecting posture data of the smart car through the inertial measurement unit;
[0232] Obtain time tags of the laser radar unit and the inertial measurement unit, and construct a time series diagram of data sampling moments of the laser radar unit and the inertial measurement unit based on the time tags with the timestamp as the horizontal coordinate;
[0233] According to the time series diagram, the point cloud data and the posture data are paired with each other at corresponding times to determine the point cloud data and the posture data at the same time;
[0234] Based on the point cloud data and attitude data at the same time, a mapping relationship between the inertial navigation coordinate system and the laser radar coordinate system is established, and according to the mapping relationship, a transformation matrix between the inertial navigation coordinate system and the laser radar coordinate system is set;
[0235] The point cloud data is converted into an inertial navigation coordinate system based on the transformation matrix to obtain converted point cloud data corresponding to the posture data;
[0236] Performing filtering processing on the conversion point cloud data by a low-pass filtering method to obtain effective conversion point cloud data;
[0237] The effective conversion point cloud data is subjected to secondary filtering processing by a voxel filtering method to obtain a point cloud centroid set, and the point cloud centroid set is subjected to point cloud distortion correction by the posture data collected by the inertial measurement unit to obtain corrected point cloud data.
[0238] The calculation formula for effectively converting point cloud data in the point cloud correction module is as follows:
[0239]
[0240] in,
[0241] Wherein, L represents the effective conversion point cloud data; M represents the filtering parameters; v(x,y) represents the conversion point cloud data with coordinates (x,y); δ(q,h) represents the posture data with coordinates (q,h); q represents the horizontal coordinate value of the posture data; h represents the vertical coordinate value of the posture data; R represents the set of all point cloud data of the driving environment of the smart car; ρ represents the conversion ratio value; K represents the set of posture data of the driving of the smart car; g(x,y) represents the point cloud data of the driving environment of the smart car with coordinates (x,y); w represents the time parameter; ∈ represents the transformation matrix; ζ represents the coordinate system conversion parameter; a v Indicates the point cloud data value corresponding to the posture data at the same time in the time series graph; b k Indicates the posture data value corresponding to the point cloud data at the same time in the time series graph.
[0242] The point cloud correction module performs secondary filtering processing on the effective conversion point cloud data by a voxel filtering method to obtain a point cloud centroid set, performs point cloud distortion correction on the point cloud centroid set by using the posture data collected by the inertial measurement unit to obtain the corrected point cloud data, including:
[0243] Calculate the maximum point cloud data and the minimum point cloud data in each axis of the effective conversion point cloud data of each frame in the three-dimensional coordinate system by a voxel filtering method;
[0244] Based on the maximum point cloud data and the minimum point cloud data, calculate the side length of the minimum cuboid in the three-dimensional space that can contain all the converted point cloud data points, and calculate the volume of the minimum cuboid according to the side length;
[0245] According to the preset voxel grid size and the volume of the minimum cuboid, the number of all grids in the minimum cuboid is calculated, and all grids in the minimum cuboid are numbered to obtain numbered voxel grids;
[0246] Based on the numbered voxel grids, the centroid of each voxel grid is calculated, and other converted point cloud data in the corresponding grid are replaced by the centroid to obtain a point cloud centroid set;
[0247] Determine the current time k of the centroid of each point cloud in the point cloud centroid set by using a laser radar unit, and obtain the carrier posture of the smart car at time k and k+1 by solving the inertial measurement unit;
[0248] Integrate the carrier posture to obtain the integrated state quantity of the smart car carrier at time k and k+1, and discretize the integrated state quantity by the median integral method to obtain the pre-integrated quantity at time k and k+1;
[0249] Conditionally constraining the integral state quantity and the pre-integrated quantity at time k and k+1 to obtain a pre-integrated error;
[0250] Based on the pre-integrated error, according to the carrier poses at times k and k+1, the point cloud pose corresponding to the current time k is calculated by a linear interpolation method;
[0251] According to the pose of each point cloud centroid at the starting time and the point cloud pose corresponding to each point cloud centroid at the current time k, the point cloud distortion value of each point cloud centroid at the current time k relative to the starting time is calculated;
[0252] The point cloud distortion value is converted through the world coordinate system to obtain the converted point cloud distortion value at the current time k relative to the starting time;
[0253] The point cloud distortion value at the current moment and the converted point cloud distortion value are subtracted to perform point cloud distortion correction and obtain the corrected point cloud data.
[0254] The pre-integration error calculation formula in the point cloud correction module is as follows:
[0255]
[0256] in,
[0257] in,
[0258] Where p represents the pre-integration error; A represents the integral state; B represents the pre-integration quantity; μ represents the Jacobian matrix; ζ1 represents the error parameter; k represents the current time; T α represents the carrier position of the smart car; rotation error; β k Represents the state quantity of the point cloud centroid corresponding to time k; β k+1 Represents the point cloud centroid state corresponding to time k+1; α k Represents the measured value of the point cloud centroid corresponding to time k; α k+1 represents the measured value of the point cloud centroid corresponding to time k+1; η represents the discrete factor; represents the measurement noise.
[0259] Specifically, the posture acquisition module is specifically used for:
[0260] According to the corrected point cloud data, the point cloud data is converted into two-dimensional key point data by a three-dimensional feature hash mapping closed-loop detection algorithm, and the two-dimensional key point data is stored in a pre-constructed database;
[0261] Based on the two-dimensional key point data, a predetermined point cloud data space is divided to obtain volume units with different target point cloud data;
[0262] Screening the point cloud with significant edge features in the volume unit by using a preset edge detection algorithm to obtain key point cloud data with edge features, and generating aggregated key points according to the key point cloud data;
[0263] Projecting the aggregated key points on a horizontal plane, and constructing a discretized search space on the horizontal plane with the projected current key points as the center of the circle;
[0264] Wherein, the discretized search space is constructed by a preset number of sectors;
[0265] The first sector is aligned with the direction of the line from the current key point to the nearest neighbor key point to determine the descriptor direction;
[0266] Based on the descriptor direction, number the remaining sectors in counterclockwise order and search for the closest keypoint in each sector;
[0267] When there is a nearest neighbor key point in the sector, a descriptor is generated based on the distance between the current key point and the nearest neighbor key point;
[0268] The distance between the current key point and the nearest neighbor key point is used as the value of the corresponding dimension of the descriptor; otherwise, when there is no nearest neighbor key point in the sector, the value of the descriptor dimension is zero;
[0269] According to the descriptor direction, concatenate descriptors with non-zero dimension values to generate a descriptor;
[0270] Using the key of the descriptor as an index, traverse all position point cloud frames in the database, and record the number of times the key of the descriptor appears in each frame;
[0271] When the frequency of a descriptor in a point cloud frame at a certain position in the database exceeds a preset threshold, the position is marked as a potential matching candidate area;
[0272] Based on the potential matching candidate area, according to the set matching point sets of the current frame and the loop frame, the loop closure value is calculated, and the rotation constraint is performed according to the loop closure value to obtain the accurate posture data of the smart car.
[0273] Specifically, the intelligent positioning module is specifically used for:
[0274] Dividing the precise position and posture data of the smart car into factor nodes and variable nodes, mapping the values of the variable nodes on preset scalars through local functions corresponding to the factor nodes, and determining the definition relationship of the variable nodes;
[0275] Connecting the factor nodes and the variable nodes through undirected edges based on the definition relationship to construct a factor graph of the smart car;
[0276] Based on the factor graph, the precise position data of the smart car is added to the sliding window, and the maximum a posteriori estimation of the state in the local window is obtained through joint optimization;
[0277] Optimizing the factor graph according to the maximum a posteriori estimation and the optimization factor, obtaining the rotation parameters and the translation parameters of the precise posture data, and obtaining the optimized posture data of the smart car according to the rotation parameters and the translation parameters;
[0278] Projecting the optimized pose data of each frame of the smart car to the global coordinate system to obtain updated point cloud data;
[0279] The updated point cloud data are superimposed to construct a global operation trajectory of the smart car, and a global point cloud map is obtained. The smart car is positioned according to the global point cloud map.
[0280] See also Figure 3 , Figure 3 Schematic diagram of an electronic device provided by an embodiment of the present invention. Figure 3 As shown, an embodiment of the present invention provides an electronic device 500, including a memory 510, a processor 520, and a computer program 511 stored in the memory 510 and executable on the processor 520. When the processor 520 executes the computer program 511, the following steps are implemented:
[0281] Memory for storing computer software programs;
[0282] The processor is used to read and execute the computer software program, thereby implementing the positioning method based on laser radar and inertial navigation.
[0283] See also Figure 4 , Figure 4 A schematic diagram of an embodiment of a computer-readable storage medium provided in an embodiment of the present invention. Figure 4 As shown, this embodiment provides a computer-readable storage medium 600, on which a computer program 611 is stored. When the computer program 611 is executed by a processor, the following steps are implemented:
[0284] The storage medium stores a computer software program, which, when executed by a processor, implements the positioning method based on laser radar and inertial navigation.
[0285] It should be noted that in the above embodiments, the description of each embodiment has its own emphasis, and for parts that are not described in detail in a certain embodiment, reference can be made to the relevant descriptions of other embodiments.
[0286] Those skilled in the art will appreciate that embodiments of the present invention may be provided as methods, systems, or computer program products. Therefore, the present invention may take the form of a complete hardware embodiment, a complete software embodiment, or an embodiment combining software and hardware. Moreover, the present invention may take the form of a computer program product implemented on one or more computer-usable storage media (including but not limited to disk storage, CD-ROM, optical storage, etc.) containing computer-usable program code.
[0287] The present invention is described with reference to flowcharts and / or block diagrams of methods, devices (systems), and computer program products according to embodiments of the present invention. It should be understood that each process and / or block in the flowchart and / or block diagram, as well as the combination of processes and / or blocks 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 computer, 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.
[0288] 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.
[0289] 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 in the computer or other programmable device. Figure 1 A process or multiple processes and / or boxes Figure 1 The steps for the functions specified in one or more boxes.
[0290] Although the preferred embodiments of the present invention have been described, those skilled in the art may make other changes and modifications to these embodiments once they have learned the basic creative concept. Therefore, the appended claims are intended to be interpreted as including the preferred embodiments and all changes and modifications that fall within the scope of the present invention.
[0291] Obviously, those skilled in the art can make various changes and modifications to the present invention without departing from the spirit and scope of the present invention. Thus, if these modifications and variations of the present invention fall within the scope of the claims of the present invention and their equivalents, the present invention is also intended to include these modifications and variations.
Claims
1. A positioning method based on laser radar and inertial navigation, characterized in that: include: Acquire point cloud data and attitude data of the smart car through a laser radar unit and an inertial measurement unit, and perform data correction according to the point cloud data and attitude data to obtain corrected point cloud data; According to the corrected point cloud data, a closed-loop detection is performed by using a three-dimensional feature hash mapping closed-loop detection algorithm to obtain accurate position and posture data of the smart car; Based on the precise posture data of the smart car, a factor graph is constructed, and the optimized posture data of the smart car is obtained through the factor graph. According to the posture data of the smart car, the global point cloud data is updated to obtain updated point cloud data, and the smart car is positioned according to the updated point cloud data.
2. The positioning method based on laser radar and inertial navigation according to claim 1, characterized in that: The method of obtaining point cloud data and attitude data of the smart car through the laser radar unit and the inertial measurement unit, and performing data correction according to the point cloud data and the attitude data to obtain corrected point cloud data includes: Installing a pre-set laser radar unit and an inertial measurement unit on the smart car, collecting point cloud data of the environment around the smart car through the laser radar unit, and collecting posture data of the smart car through the inertial measurement unit; Obtain time tags of the laser radar unit and the inertial measurement unit, and construct a time series diagram of data sampling moments of the laser radar unit and the inertial measurement unit based on the time tags with the timestamp as the horizontal coordinate; According to the time series diagram, the point cloud data and the posture data are paired with each other at corresponding times to determine the point cloud data and the posture data at the same time; Based on the point cloud data and attitude data at the same time, a mapping relationship between the inertial navigation coordinate system and the laser radar coordinate system is established, and according to the mapping relationship, a transformation matrix between the inertial navigation coordinate system and the laser radar coordinate system is set; The point cloud data is converted into an inertial navigation coordinate system based on the transformation matrix to obtain converted point cloud data corresponding to the posture data; Performing filtering processing on the conversion point cloud data by a low-pass filtering method to obtain effective conversion point cloud data; The effective conversion point cloud data is subjected to secondary filtering processing by a voxel filtering method to obtain a point cloud centroid set, and the point cloud centroid set is subjected to point cloud distortion correction by using the posture data collected by the inertial measurement unit to obtain corrected point cloud data.
3. The positioning method based on laser radar and inertial navigation according to claim 2, characterized in that: The calculation formula of the effective conversion point cloud data is as follows: in, Wherein, L represents the effective conversion point cloud data; M represents the filtering parameters; v(x,y) represents the conversion point cloud data with coordinates (x,y); δ(q,h) represents the posture data with coordinates (q,h); q represents the horizontal coordinate value of the posture data; h represents the vertical coordinate value of the posture data; R represents the set of all point cloud data of the driving environment of the smart car; ρ represents the conversion ratio value; K represents the set of posture data of the driving of the smart car; g(x,y) represents the point cloud data of the driving environment of the smart car with coordinates (x,y); w represents the time parameter; ∈ represents the transformation matrix; ζ represents the coordinate system conversion parameter; a v Indicates the point cloud data value corresponding to the posture data at the same time in the time series graph; b k Indicates the posture data value corresponding to the point cloud data at the same time in the time series graph.
4. The positioning method based on laser radar and inertial navigation according to claim 2, characterized in that: The method of performing secondary filtering on the effective conversion point cloud data by a voxel filtering method to obtain a point cloud centroid set, and performing point cloud distortion correction on the point cloud centroid set by using the posture data collected by the inertial measurement unit to obtain corrected point cloud data includes: Calculate the maximum point cloud data and the minimum point cloud data in each axis of the effective conversion point cloud data of each frame in the three-dimensional coordinate system by a voxel filtering method; Based on the maximum point cloud data and the minimum point cloud data, calculate the side length of the minimum cuboid in the three-dimensional space that can contain all the converted point cloud data points, and calculate the volume of the minimum cuboid according to the side length; According to the preset voxel grid size and the volume of the minimum cuboid, the number of all grids in the minimum cuboid is calculated, and all grids in the minimum cuboid are numbered to obtain numbered voxel grids; Based on the numbered voxel grids, the centroid of each voxel grid is calculated, and other converted point cloud data in the corresponding grid are replaced by the centroid to obtain a point cloud centroid set; Determine the current time k of the centroid of each point cloud in the point cloud centroid set by using a laser radar unit, and obtain the carrier posture of the smart car at time k and k+1 by solving the inertial measurement unit; Integrate the carrier posture to obtain the integrated state quantity of the smart car carrier at time k and k+1, and discretize the integrated state quantity by the median integral method to obtain the pre-integrated quantity at time k and k+1; Conditionally constraining the integral state quantity and the pre-integrated quantity at time k and k+1 to obtain a pre-integrated error; Based on the pre-integrated error, according to the carrier poses at times k and k+1, the point cloud pose corresponding to the current time k is calculated by a linear interpolation method; According to the pose of each point cloud centroid at the starting time and the point cloud pose corresponding to each point cloud centroid at the current time k, the point cloud distortion value of each point cloud centroid at the current time k relative to the starting time is calculated; The point cloud distortion value is converted through the world coordinate system to obtain the converted point cloud distortion value at the current time k relative to the starting time; The point cloud distortion value at the current moment and the converted point cloud distortion value are subtracted to perform point cloud distortion correction and obtain the corrected point cloud data.
5. The positioning method based on laser radar and inertial navigation according to claim 4, characterized in that: The pre-integration error calculation formula is as follows: in, in, Where p represents the pre-integration error; A represents the integral state; B represents the pre-integration quantity; μ represents the Jacobian matrix; ζ1 represents the error parameter; k represents the current time; T α represents the carrier position of the smart car; rotation error; β k Represents the state quantity of the point cloud centroid corresponding to time k; β k+1 Represents the point cloud centroid state corresponding to time k+1; α k Represents the measured value of the point cloud centroid corresponding to time k; α k+1 represents the measured value of the point cloud centroid corresponding to time k+1; η represents the discrete factor; represents the measurement noise.
6. The positioning method based on laser radar and inertial navigation according to claim 1, characterized in that: According to the corrected point cloud data, a closed-loop detection is performed through a three-dimensional feature hash mapping closed-loop detection algorithm to obtain accurate position and posture data of the smart car, including: According to the corrected point cloud data, the point cloud data is converted into two-dimensional key point data by a three-dimensional feature hash mapping closed-loop detection algorithm, and the two-dimensional key point data is stored in a pre-constructed database; Based on the two-dimensional key point data, a predetermined point cloud data space is divided to obtain volume units with different target point cloud data; Screening the point cloud with significant edge features in the volume unit by using a preset edge detection algorithm to obtain key point cloud data with edge features, and generating aggregated key points according to the key point cloud data; Projecting the aggregated key points on a horizontal plane, and constructing a discretized search space on the horizontal plane with the projected current key points as the center of the circle; Wherein, the discretized search space is constructed by a preset number of sectors; The first sector is aligned with the direction of the line from the current key point to the nearest neighbor key point to determine the descriptor direction; Based on the descriptor direction, number the remaining sectors in counterclockwise order and search for the closest keypoint in each sector; When there is a nearest neighbor key point in the sector, a descriptor is generated based on the distance between the current key point and the nearest neighbor key point; The distance between the current key point and the nearest neighbor key point is used as the value of the corresponding dimension of the descriptor; otherwise, when there is no nearest neighbor key point in the sector, the value of the descriptor dimension is zero; According to the descriptor direction, concatenate descriptors with non-zero dimension values to generate a descriptor; Using the key of the descriptor as an index, traverse all position point cloud frames in the database, and record the number of times the key of the descriptor appears in each frame; When the frequency of a descriptor in a point cloud frame at a certain position in the database exceeds a preset threshold, the position is marked as a potential matching candidate area; Based on the potential matching candidate area, according to the set matching point sets of the current frame and the loop frame, the loop closure value is calculated, and the rotation constraint is performed according to the loop closure value to obtain the accurate posture data of the smart car.
7. The positioning method based on laser radar and inertial navigation according to claim 1, characterized in that: The method constructs a factor graph based on the precise posture data of the smart car, obtains optimized posture data of the smart car through the factor graph, updates global point cloud data according to the posture data of the smart car to obtain updated point cloud data, and locates the smart car according to the updated point cloud data, including: Dividing the precise position and posture data of the smart car into factor nodes and variable nodes, mapping the values of the variable nodes on preset scalars through local functions corresponding to the factor nodes, and determining the definition relationship of the variable nodes; Connecting the factor nodes and the variable nodes through undirected edges based on the definition relationship to construct a factor graph of the smart car; Based on the factor graph, the precise position data of the smart car is added to the sliding window, and the maximum a posteriori estimation of the state in the local window is obtained through joint optimization; Optimizing the factor graph according to the maximum a posteriori estimation and the optimization factor, obtaining the rotation parameters and the translation parameters of the precise posture data, and obtaining the optimized posture data of the smart car according to the rotation parameters and the translation parameters; Projecting the optimized pose data of each frame of the smart car to the global coordinate system to obtain updated point cloud data; The updated point cloud data are superimposed to construct a global operation trajectory of the smart car, and a global point cloud map is obtained. The smart car is positioned according to the global point cloud map.
8. A positioning system based on laser radar and inertial navigation, characterized in that: include: A point cloud correction module is used to obtain point cloud data and posture data of the smart car through a laser radar unit and an inertial measurement unit, and perform data correction according to the point cloud data and posture data to obtain corrected point cloud data; A posture acquisition module is used to perform closed-loop detection based on the corrected point cloud data through a three-dimensional feature hash mapping closed-loop detection algorithm to obtain accurate posture data of the smart car; The intelligent positioning module is used to construct a factor graph based on the precise posture data of the intelligent vehicle, obtain the optimized posture data of the intelligent vehicle through the factor graph, update the global point cloud data according to the posture data of the intelligent vehicle, obtain the updated point cloud data, and locate the intelligent vehicle according to the updated point cloud data.
9. An electronic device, characterized in that: include: Memory for storing computer software programs; A processor is used to read and execute the computer software program, thereby implementing the positioning method based on laser radar and inertial navigation as described in any one of claims 1-7.
10. A non-transitory computer-readable storage medium, characterized in that: The storage medium stores a computer software program, which, when executed by a processor, implements the positioning method based on laser radar and inertial navigation as described in any one of claims 1 to 7.
Citation Information
Cited By
Welding quality online detection and optimization method and system
CN120746975A
Welding quality online detection and optimization method and system
CN120746975B
Unmanned aerial vehicle autonomous obstacle avoidance method and system based on laser radar, and storage medium
CN121477942A
