A LiDAR-IMU coupled odometer method, system, electronic device and vehicle

Abstract

The present invention discloses a LiDAR-IMU coupled odometer method, system, electronic device and vehicle, which belongs to the technical field of high-precision positioning and high-reliability positioning, obtains IMU sampling data and LiDAR sampling data, generates IMU predicted posture data, performs fuzzy processing on the predicted posture data by fuzzy information processing means, uses posture transformation data as the initial posture estimation value of LiDAR point cloud registration, obtains state observation value at time n by LiDAR point cloud registration iterative calculation, fuses posture transformation data and state observation value, and forms LiDAR-IMU coupled odometer. The present invention combines fuzzy processor to generate posture change ratio and variance of posture estimation, forms LIO odometer based on LiDAR point cloud registration technology and EKF fusion technology of feature points, and significantly improves the accuracy and real-time performance of LIO odometer compared with the prior art.

Description

Technical Field

[0001] The present invention relates to an odometer method, system, electronic equipment and vehicle, and in particular to a LiDAR-IMU odometer method, system, electronic equipment and vehicle, belonging to the technical field of high-precision positioning and high-reliability positioning. Background Art

[0002] In autonomous driving technology, accurate positioning technology is one of the core foundations for achieving safe and reliable autonomous driving. It is not only related to whether the vehicle can accurately perceive its position in the environment, but also directly affects the execution of key functions such as path planning and obstacle avoidance, thus determining the safety and reliability of the entire autonomous driving system. Therefore, accurate positioning technology is a prerequisite for ensuring that the vehicle can make correct decisions and navigate smoothly. To achieve this goal, the positioning system of autonomous driving usually relies on LiDAR-IMU coupled odometer (LiDAR-Inertial Odometry, LIO) technology. LIO odometer combines the advantages of IMU track calculation with LiDAR odometer, overcoming the problem that LiDAR odometer relies on good initial pose estimation, especially in the case of sparse features or repeated environments. Therefore, LIO odometer forms a more accurate and robust positioning solution, which is more accurate than traditional GNSS positioning, especially in complex urban environments, tunnels and other places where GNSS signals are weak or invalid, LIO odometer can still provide reliable positioning information.

[0003] Although LIO odometry technology can provide excellent incremental pose estimation, it also faces some challenges, including the problem of IMU information processing. The accuracy and time consumption of IMU information processing have a great impact on the algorithm effect of LIO odometry. IMU is a sensor that includes an accelerometer and a gyroscope, and has a high data update frequency. Based on the system pose state at the previous moment, the track calculation combined with IMU kinematics can provide real-time pose information. LiDAR odometry technology obtains LiDAR point cloud data and realizes incremental pose estimation of the vehicle through point cloud registration between frames. The pose prediction obtained in real time by LIO odometry through IMU track calculation provides a short-term pose prediction for the LiDAR odometry, and serves as the initial estimate of point cloud registration to reduce the search space of the optimization algorithm, thereby improving computational efficiency and real-time performance. However, in the IMU prediction process based on IMU dead reckoning, the IMU measurement and position change are in a double integral relationship. Position change is very sensitive to IMU measurement errors. Especially for some low-cost IMU solutions and LiDAR with low sampling frequency, the position change results of IMU dead reckoning have large errors, affecting the accuracy of LiDAR odometer point cloud registration. In addition, the amount of calculation of IMU integration is large, affecting the real-time performance of the system. Summary of the invention

[0004] The purpose of the present invention is to provide a LiDAR-IMU odometer method, system, electronic device and vehicle, which aims to solve the problems of large errors in position change results calculated by IMU track and large amount of IMU integral calculation, and to solve the shortcomings of the prior art.

[0005] The present invention provides the following scheme:

[0006] A LiDAR-IMU coupled odometer method, applied to a LiDAR-IMU coupled odometer system, comprising:

[0007] Obtain the IMU sampling data and LiDAR sampling data at time n, and use the IMU sampling data in the time period from time n-2 to time n-1 to generate IMU predicted pose data;

[0008] The predicted posture data is fuzzy processed by fuzzy information processing means to generate corresponding posture transformation data ;

[0009] The posture transformation data As the initial pose estimate of LiDAR point cloud registration, the state observation value at time n is obtained through iterative calculation of LiDAR point cloud registration , the posture transformation data and state observations Fusion is performed to form a LiDAR-IMU coupled odometer;

[0010] Furthermore, the acquiring of IMU sampling data and LiDAR sampling data at time n, and using the IMU sampling data in the time period from time n-2 to time n-1 to generate IMU predicted pose data, further includes:

[0011] Assume that the system state at the nth LiDAR sampling moment is: ;in is the Lie algebra of a special Euclidean group, describe The rotating part of Represents the position of the object, Indicates the posture of an object. describe The translation part in , its corresponding Lie group is defined as , The corresponding Lie group is defined as the rotation matrix R, is the position change, is a three-dimensional real vector space, is a six-dimensional real vector space; the IMU sampling data in the time period from time n-2 to time n-1 is used as the posture change benchmark, and the Lie algebra of the posture change benchmark is marked as , the corresponding Lie group is labeled ;

[0012] The pose change ratio and the variance of pose estimation are generated by using the average value and generalized likelihood ratio test of the IMU sampling data in the time period from time n-2 to time n-1;

[0013] The posture change benchmark generates corresponding IMU predicted posture data according to the posture change rate.

[0014] Furthermore, the predicted posture data is fuzzy processed by fuzzy information processing means to generate corresponding posture transformation data , further comprising:

[0015] When fuzzy processing is performed on the position change ratio, the input is the modulus of the average value of the system acceleration. and the model of the reference position transformation , the output is the position change ratio, marked as Position-Ratio;

[0016] When fuzzy processing is performed on the attitude change ratio, the input is the modulus of the average angular velocity of the system. The model of the reference attitude transformation , the output is the posture change ratio, labeled as Posture-Ratio;

[0017] For the fuzzy treatment of the position change variance, the input is the generalized likelihood ratio of the system acceleration measurement and the model of the reference position transformation , the output is the position transformation variance increment, marked as G;

[0018] For the fuzzy processing of attitude transformation variance, the input is the generalized likelihood ratio of the system angular velocity measurement The model of the reference attitude transformation , the output is the attitude transformation variance increment, labeled H.

[0019] Furthermore, the output is a position change ratio, marked as Position-Ratio, specifically:

[0020] Based on Position-Ratio and reference position transformation Get the right arrive The predicted position change between moments satisfies the following formula:

[0021]

[0022] The output is the posture change ratio, denoted as Posture-Ratio, specifically:

[0023] Based on Posture-Ratio and Baseline Posture Transformation Get the right arrive The predicted posture change between moments satisfies the following formula:

[0024]

[0025] The predicted position in the time period from n-1 to n is:

[0026]

[0027] in, Refers to the rotation change Convert to the corresponding rotation matrix through exponential mapping;

[0028] Get the state observation value at time n for:

[0029]

[0030] in, represents the posture transformation matrix at time n-1, represents the change in posture between time n and time n-1, It means to transform the elements in Lie group into the elements in Lie algebra;

[0031] The state observation value Transformed into corresponding pose transformation data , its prediction variance matrix is:

[0032]

[0033] Furthermore, the posture transformation data As the initial pose estimate of LiDAR point cloud registration, the state observation value at time n is obtained through iterative calculation of LiDAR point cloud registration , the posture transformation data and state observations The fusion is performed to form a LiDAR-IMU coupled odometer, which further includes:

[0034] Pose transformation data predicted by IMU pose predictor As the initial pose estimate, through point cloud registration iteration, we obtain The state observation value at time ;

[0035] Receiving LiDAR frames , the LiDAR frame Each point Calculate with the continuous adjacent points in the same scan line beam to get the roughness value of each point , get the LiDAR frame The characteristic points of are calculated as follows:

[0036]

[0037] in: is the set of consecutive adjacent points in the same scan line beam, yes The point in

[0038] Obtain a local feature map and local map feature points corresponding to the local feature map, and convert the LiDAR frame The feature points of the LiDAR frame are matched with the local map feature points to obtain Corresponding pose transformation data .

[0039] Furthermore, the local feature map is defined as: ,in is the local map edge feature set, is the set of local map edge features;

[0040] The LiDAR frame The feature points of the local map are matched with the feature points of the local map to obtain a corresponding matching distance, and the matching distance specifically satisfies the following definition:

[0041]

[0042] in, is the pose transformation data that needs to be estimated for the current frame;

[0043] The following formula is used to perform the feature point matching:

[0044]

[0045] In the above formula, is the pose transformation data that needs to be estimated for the current frame, is the edge feature set of the current frame, is a point in the edge feature set of the current frame; is the current frame plane feature set, It is a point in the plane feature set of the current frame; express The matching distance between adjacent edge feature points in the map, express The matching distance between adjacent planar feature points in the map.

[0046] Furthermore, the local feature map is specifically a set of features in the local map, and the definition of the local feature map satisfies the following formula:

[0047]

[0048] in, and yes and Through the pose transformation to the feature set in the local map coordinate system, is the number of keyframes in the local map, Represents the union operation of sets.

[0049] A LiDAR-IMU coupled odometer system, used to implement the LiDAR-IMU coupled odometer method, comprising:

[0050] The IMU predicted pose data generation module is used to obtain the IMU sampling data and LiDAR sampling data at time n, and generate the IMU predicted pose data using the IMU sampling data in the time period from time n-2 to time n-1;

[0051] The posture transformation data generation module performs fuzzy processing on the predicted posture data through fuzzy information processing means to generate corresponding posture transformation data ;

[0052] LiDAR-IMU coupled odometer fusion module is used to transform the pose data As the initial pose estimate of LiDAR point cloud registration, the state observation value at time n is obtained through iterative calculation of LiDAR point cloud registration , the posture transformation data and state observations Fusion is performed to form a LiDAR-IMU coupled odometer.

[0053] A vehicle comprising the LiDAR-IMU coupled odometer system.

[0054] An electronic device comprises: a processor, a communication interface, a memory and a communication bus, wherein the processor, the communication interface and the memory communicate with each other via the communication bus; a computer program is stored in the memory, and when the computer program is executed by the processor, the processor executes the steps of the method.

[0055] Compared with the prior art, the present invention has the following advantages:

[0056] The present invention obtains the IMU sampling data and LiDAR sampling data at time n, and converts the state change in the previous LiDAR cycle (i.e. arrive The state change between ) as the reference for posture change, using Time has come The average value and generalized likelihood ratio test of IMU samples in the time period of time are combined with the fuzzy processor to generate the pose change ratio and the variance of the pose estimation. On this basis, the LiDAR point cloud registration technology based on feature points and the EKF fusion technology are combined to form the LIO odometer, which significantly improves the accuracy and real-time performance of the LIO odometer compared with the existing technology.

[0057] The present invention applies Lie groups and Lie algebras to describe the posture transformation and posture representation of rigid bodies in three-dimensional space, realizes the accurate posture representation of rotation and translation motion in three-dimensional space, transforms the optimization problem into an unconstrained optimization problem, simplifies the solution method, performs differentiation and small perturbation analysis on the posture, makes the posture update more efficient, and can conveniently establish a perturbation model, providing strong data support for analyzing system motion. BRIEF DESCRIPTION OF THE DRAWINGS

[0058] In order to more clearly illustrate the specific implementation methods of the present invention or the technical solutions in the prior art, the drawings required for use in the specific implementation methods or the description of the prior art will be briefly introduced below. Obviously, the drawings described below are some implementation methods of the present invention. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying creative work.

[0059] Figure 1 is a flow chart of the LiDAR-IMU odometer method (step S1 to step S3).

[0060] Figure 1A It is an architectural diagram of the hardware system of an embodiment of the present application.

[0061] Figure 1B This is a schematic diagram of sensor sampling in an embodiment of the present application.

[0062] Figure 1C It is a schematic diagram of the software division of the embodiment of the present application.

[0063] Figure 1D It is a flow chart of the optimization technical solution of step S1.

[0064] Figure 1EIt is a flow chart of the optimization technical method of step S2.

[0065] Figure 2 This is the architecture diagram of the LiDAR-IMU odometer method.

[0066] Figure 3 It is a structural diagram of an electronic device. DETAILED DESCRIPTION

[0067] The technical solution of the present invention will be described clearly and completely below in conjunction with the accompanying drawings. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.

[0068] The “system” in the embodiments of the present application generally refers to a complex system in which a vehicle is located during an autonomous driving operation. This complex system may include the following subsystems:

[0069] Perception subsystem: Autonomous vehicles collect information about the surrounding environment through onboard sensors such as LiDAR, cameras, and millimeter wave radars. These sensors help the vehicle understand its environment, including the location of obstacles, road signs, and the semantic classification of pedestrian and vehicle detection data. In addition, localization is also part of perception, which determines the specific position of the vehicle relative to the environment.

[0070] Planning subsystem: Once the vehicle understands its environment through the perception system, the planning system needs to decide the vehicle's next action. Planning can be divided into mission planning, behavioral planning, and motion planning. The planning process involves path planning from the starting point to the destination, while avoiding obstacles and optimizing the driving trajectory to ensure the safety and comfort of passengers.

[0071] Control subsystem: After planning the driving path, the vehicle control system needs to accurately execute these planned actions. The actions controlled by the vehicle control system include vehicle speed control, direction control, etc., to ensure that the vehicle can drive safely and accurately according to the planned path.

[0072] Hardware subsystem: The hardware system of an autonomous vehicle includes sensors, computing units, and execution units. Sensors are used to perceive the environment, computing units are used to process data and make decisions, and execution units are responsible for implementing these decisions, such as controlling steering, acceleration, and braking.

[0073] Functional configuration subsystem: The functional configuration of the autonomous driving system is usually divided into different levels, from L0 (no automation) to L5 (full automation). Each level represents a different degree of automation and a set of functions, such as lane keeping assist, adaptive cruise control, conditional autonomous driving, etc.

[0074] In summary, autonomous vehicles are in a highly integrated and collaborative system that uses advanced sensors, powerful computing power, precise control strategies, and complex planning algorithms to enable the vehicle to drive safely without human driver intervention.

[0075] like Figure 1 The LiDAR-IMU coupled odometer method shown in the figure can be applied to the LiDAR-IMU coupled odometer system, including:

[0076] Step S1, obtaining the IMU sampling data and LiDAR sampling data at time n, and using the IMU sampling data in the time period from time n-2 to time n-1 to generate IMU predicted posture data;

[0077] Step S2, fuzzy processing is performed on the predicted posture data by fuzzy information processing means to generate corresponding posture transformation data ;

[0078] Step S3, transforming the posture data As the initial pose estimate of LiDAR point cloud registration, the state observation value at time n is obtained through iterative calculation of LiDAR point cloud registration , the posture transformation data and state observations Fusion is performed to form a LiDAR-IMU coupled odometer.

[0079] The technical solutions provided by steps S1 to S3 realize the collection and prediction of positioning data, and solve the uncertainty problem of data through fuzzy processing and point cloud registration and data fusion, enhance the robustness to noise and outliers, and obtain the state observation value at time n through iterative calculation of the point cloud registration algorithm. Finally, the fuzzy pose transformation data is fused with the state observation value to form a LiDAR-IMU coupled odometer.

[0080] The technical solutions provided in steps S1 to S3 combine the high-frequency data of IMU and the precise ranging capability of LiDAR to achieve high-precision pose estimation in dynamic and complex environments. Fuzzy processing can effectively reduce the impact of data noise, so that the predicted pose can still maintain high reliability under large uncertainties. It can update pose transformation data in real time to meet the high requirements for data real-time in working conditions such as autonomous driving. Finally, by fusing IMU and LiDAR data, the limitations of a single sensor can be overcome to provide more comprehensive environmental perception functions.

[0081] like Figure 1A As shown, the LiDAR-IMU coupled odometer method provided in the embodiment of the present application can be run on a hardware system. A typical hardware system capable of running steps S1 to S3 includes a LiDAR sensor, an IMU sensor, and a main control unit. The specific functions of the hardware system are listed as follows:

[0082] During the operation of the LiDAR sensor, the incremental pose estimation is obtained through point cloud registration between frames. The performance indicators of LiDAR include scanning angle, measurement range, resolution and refresh rate. The LiDAR sensor transmits point cloud information to the main control unit through the TCP\IP bus.

[0083] The IMU sensor consists of an accelerometer and a gyroscope, which are used to measure the acceleration and angular velocity of the vehicle and provide dynamic information about the vehicle's movement. When the LiDAR accuracy decreases or the signal is interrupted, the IMU compensates for the shortcomings of short-term positioning by inferring the position. The accuracy of the IMU has a significant impact on the short-term positioning effect, especially in indoor or complex environments, where the stability and accuracy of the IMU are crucial. The IMU sensor is connected to the embedded collector via the CAN bus, and then the embedded collector forwards the IMU data to the main control unit via the TCP\IP bus. 。

[0084] The main control unit contains a multi-core CPU, GPU, memory, solid-state storage, Ethernet chip, and Ethernet switch chip. It is responsible for real-time processing of data obtained from LiDAR and IMU sensors and executing software algorithms to calculate the vehicle's position and posture. The sampling diagram of LiDAR and IMU is shown in the figure below. Figure 1B shown.

[0085] like Figure 1CAs shown, the software part in the embodiment of the present application includes three parts: an IMU pose predictor based on fuzzy information processing, a LiDAR point cloud alignment, and an EKF fusion. The embodiment of the present application forms an LIO odometer by fusing the LiDAR and IMU sensor information based on the EKF filtering framework. The estimation of the pose by the IMU information is used as a prediction process, and the estimation of the pose by the LiDAR information is used as an observation process. After filtering and fusing the two, the pose estimation of the system is output to realize the positioning technology of autonomous driving.

[0086] like Figure 1D As shown, in step S1, the IMU sampling data and LiDAR sampling data at time n are obtained, and the IMU sampling data in the time period from time n-2 to time n-1 are used to generate IMU predicted pose data, further comprising:

[0087] Step S11, taking the IMU sampling data from time n-2 to time n-1 as the posture change reference (state change amount ), the Lie algebra notation of the pose change benchmark is , the corresponding Lie group is labeled ;

[0088] Step S12, using the average value and generalized likelihood ratio test of the IMU sampling data in the time period from time n-2 to time n-1, generate the posture change ratio and the variance of the posture estimation; in step S12, fuzzy processors are set for posture transformation and rotation transformation respectively.

[0089] Step S13, generating corresponding IMU predicted posture data according to the posture change rate based on the posture change benchmark.

[0090] The innovation focus of step S11 to step S13 is to design an IMU pose predictor based on fuzzy information processing. , The moment state and IMU input sampling data are finally outputted with IMU predicted pose data.

[0091] In the embodiment of the present application, the IMU pose fuzzy predictor mainly consists of three steps, namely: IMU information preprocessing, fuzzy information processor and IMU predicted pose transformation calculation, and the three steps are performed sequentially. On this basis, the LiDAR odometer is combined with the EKF fusion to form a complete LIO odometer.

[0092] IMU information preprocessing:

[0093] Assume that the system state at the nth LiDAR sampling moment is: ;

[0094] in is the Lie algebra of a special Euclidean group, describe The rotating part of Represents the position of the object, Indicates the posture of an object. describe The translation part in , its corresponding Lie group is defined as , The corresponding Lie group is defined as the rotation matrix R, is the position change, is a three-dimensional real vector space, is a six-dimensional real vector space.

[0095] The basic idea of ​​IMU information preprocessing is to convert the state change in the previous LiDAR cycle, that is, arrive The state change between As a reference for posture change. arrive The average value and generalized likelihood ratio test of IMU samples within the time period are combined with the fuzzy processor to generate the pose change ratio and the variance of the pose estimate. The pose change benchmark is generated according to the pose change ratio to generate the IMU predicted pose. Fuzzy processors are set separately for pose transformation and rotation transformation.

[0096] arrive The state change between , taking it as the benchmark, marked as , and its corresponding Lie group is labeled .

[0097] Assumptions At time , the variance matrix of the system state is , the IMU sampling data includes the three-degree-of-freedom linear acceleration and the three-degree-of-freedom angular velocity, let the acceleration measurement value be , the angular velocity measurement is Assumptions arrive The IMU sampling set within the time period is , the number is . Then the average values ​​of acceleration measurement and angular velocity measurement are:

[0098] …… (1)

[0099] …… (2)

[0100] Then the average acceleration and angular velocity of the system are:

[0101] …… (3)

[0102] … (4)

[0103] The variance of the pose transformation is determined by the IMU-based generalized likelihood ratio test (GLRT). The GLRT values ​​of acceleration and angular velocity are calculated and set as and ,but:

[0104] …… (5)

[0105] …… (6)

[0106] like Figure 1E As shown, in step S2, the predicted posture data is fuzzy processed by fuzzy information processing means to generate corresponding posture transformation data , further comprising:

[0107] Step S21, when the position change ratio is fuzzy processed, the input is the model of the average value of the system acceleration. and the model of the reference position transformation , the output is the position change ratio, marked as Position-Ratio;

[0108] Step S22, when the attitude change ratio is fuzzy processed, the input is the model of the average angular velocity of the system. The model of the reference attitude transformation , the output is the posture change ratio, labeled as Posture-Ratio;

[0109] Step S23, for the fuzzy processing means of position transformation variance, the input is the generalized likelihood ratio of the system acceleration measurement and the modulus of the reference position transformation. , the output is the position transformation variance increment, marked as G;

[0110] Step S24, for the fuzzy processing means of attitude transformation variance, the input is the generalized likelihood ratio of the system angular velocity measurement and the model of the reference attitude transformation. , the output is the attitude transformation variance increment, labeled H.

[0111] Steps S21 to S24 use fuzzy information processing methods to refine the process of predicting pose data, and use fuzzy logic to process the predicted pose data of the IMU to generate more accurate pose transformation data. In the optimization technical solution provided in steps S21 to S24, fuzzy logic is used to process the predicted pose data of the IMU to generate more accurate pose transformation data, which makes full use of the fuzzy logic in the fuzzy information processing method to process input data with uncertainty and ambiguity, and provides a more robust output for pose estimation.

[0112] Steps S21 and S22 calculate the position change ratio (Position-Ratio) and posture change ratio (Posture-Ratio) by fuzzy processing the average value of the system acceleration and angular velocity. The position change ratio and posture change ratio reflect the degree of change of the posture from the previous moment n-1 to the current moment n. Then, the uncertainty of the acceleration and angular velocity measurement is quantified by fuzzy processing (steps S23 and S24), and the position change variance increment G and posture change variance H are generated. The variances G and H are used to evaluate the credibility and accuracy of posture changes.

[0113] The optimization technical solution provided in steps S21 to S24 can improve the accuracy of pose estimation and accurately capture slight changes in pose. By introducing fuzzy information processing methods and utilizing fuzzy logic, the system is made less sensitive to sensor noise and external interference, thereby increasing the robustness of pose estimation. The uncertainty of pose changes is quantified by estimating the transformation variance, providing reliable statistical information for subsequent data fusion and decision-making, thereby improving the convergence speed and accuracy of subsequent point cloud registration and providing necessary data support for subsequent LiDAR-IMU data fusion.

[0114] Preferably, in steps S21 to S24, the output is a position change ratio, marked as Position-Ratio, specifically:

[0115] Based on Position-Ratio and Get the right arrive The predicted position change between moments satisfies the following formula:

[0116] …… (7)

[0117] The output is the posture change ratio, denoted as Posture-Ratio, specifically:

[0118] Based on Posture-Ratio and Get the right arrive The predicted posture change between moments satisfies the following formula:

[0119] …… (8)

[0120] The predicted position in the time period from n-1 to n is:

[0121] …… (9)

[0122] Get the state observation value at time n for:

[0123] …… (10)

[0124] The state observation value Transformed into corresponding pose transformation data , its prediction variance matrix is:

[0125] …… (11)

[0126] The optimization technical solutions provided in step S21 to step S24 may correspond to the fuzzy forms shown in Table 1 below:

[0127]

[0128] Table 1

[0129] In Table 1, x (NB, NS, ZO, PS, PB), y (NB, NS, ZO, PS, PB) are input fuzzy sets, z (NB, NB, NS, ZO, PS, PB) is the output fuzzy set, and the choice of the rules of the domain varies according to the specific engineering application.

[0130] In fuzzy information processing, input fuzzy sets and output fuzzy sets are the basic components of fuzzy logic systems. The universe of discourse refers to the set of all possible elements in a fuzzy set. The universe defines the range of values ​​of fuzzy variables. In engineering applications, the choice of the universe usually depends on the needs and characteristics of the specific problem. For example, in the optimization technical solution provided in steps S21 to S24, fuzzy information processing is used to fuzzify the predicted posture data and generate corresponding posture transformation data, which is the need and characteristic of a specific problem in engineering. The application of fuzzy methods in engineering involves converting physical quantities, system logic, control strategies and optimization requirements into the concepts of fuzzy sets, membership functions, fuzzy rules and universes, and realizing the modeling and control of complex systems through fuzzy reasoning mechanisms. This method can handle uncertainty and nonlinear problems and improve the robustness and adaptability of the system.

[0131] Preferably, in step S3, the posture transformation data As the initial pose estimate of LiDAR point cloud registration, the state observation value at time n is obtained through iterative calculation of LiDAR point cloud registration , the posture transformation data and state observations The fusion is performed to form a LiDAR-IMU coupled odometer, which further includes:

[0132] Step S31, using the posture transformation data predicted by the IMU posture predictor As the initial pose estimation, the point cloud registration is iterated to obtain The state observation value at time ;

[0133] Step S32, receiving LiDAR frame , the LiDAR frame Each point Calculate with the continuous adjacent points in the same scan line beam to get the roughness value of each point , get the LiDAR frame The characteristic points of are calculated as follows:

[0134] …… (12)

[0135] in: is the set of consecutive adjacent points in the same scan line beam, yes Then, the edge and plane features are extracted according to the roughness value, where the points with larger roughness are extracted as edge features and represented as a set , while points with smaller roughness are extracted as plane features and represented as a set .

[0136] Step S33, obtaining a local feature map and local map feature points corresponding to the local feature map, and converting the LiDAR frame The feature points of the LiDAR frame are matched with the local map feature points to obtain Corresponding pose transformation data .

[0137] Local feature map:

[0138] The local feature map is a collection of features in the local map, defined as ,in is the local map edge feature set, is a set of local map edge features, which are defined as follows:

[0139] …… (13)

[0140] in, and yes and The feature set is transformed to the local map coordinate system through pose transformation. is the number of keyframes in the local map. In order to reduce the search computation cost, the local feature map is managed by a three-dimensional KD tree.

[0141] Pose estimation:

[0142] Will The feature points extracted from the local map are matched with the corresponding feature points in the local map, and the corresponding matching distance is:

[0143] …… (14)

[0144] in, is the pose transformation that needs to be estimated for the current frame. Therefore, feature point matching can be expressed as the following optimization problem.

[0145] …… (15)

[0146] in, The optimal initial value of It is obtained by predicting the IMU pose predictor. The perturbation model combined with the Gauss-Newton method can be obtained The observed value of the pose transformation .

[0147] Finally, the pose predicted by the IMU pose predictor is transformed based on EKF fusion and LiDAR observation pose transformation The fusion is performed to reduce the impact of IMU measurement error on the LIO odometer.

[0148] It can be seen from the technical solutions provided by steps S1 to S3, as well as the optimization technical solutions corresponding to the different steps, that the embodiment of the present application discloses an IMU pose fuzzy predictor for autonomous driving lidar-inertial navigation coupled positioning technology, which aims to solve the problems existing in the existing IMU pose prediction, namely, the position change is very sensitive to the IMU measurement error, especially for some low-cost IMU solutions and the case of low LiDAR sampling frequency. There is a large error in the position change result calculated by the IMU track, which affects the accuracy of the LiDAR odometer point cloud registration. In addition, the amount of calculation of the IMU integral is large, which affects the real-time performance of the system. The embodiment of the present application calculates the state change amount in the previous LiDAR cycle, i.e. arrive The state change between As a reference for posture change, use arrive The average value and generalized likelihood ratio test of IMU samples within a time period are combined with a fuzzy processor to generate the pose change ratio and the variance of the pose estimation. On this basis, the LiDAR point cloud registration technology based on feature points and the EKF fusion technology are combined to form the LIO odometer. Compared with the prior art, the present invention significantly improves the accuracy and real-time performance of the LIO odometer.

[0149] In the embodiments of the present application, Lie groups and Lie algebras are used in many places to describe the posture transformation and posture representation of a rigid body in three-dimensional space. The following is the principle of Lie groups and Lie algebras:

[0150] Three-dimensional space pose conversion: Three-dimensional space conversion includes rotation and translation, where three-dimensional rotation can be obtained by three-dimensional rotation matrix It means that it forms a special orthogonal group. , the three-dimensional translation can be expressed by the translation vector Represents. Combining rotation and translation to obtain the three-dimensional pose transformation matrix , which constitutes the Special Euclidean Group , the two populations are defined as follows:

[0151]

[0152]

[0153] The two Lie groups mentioned above have corresponding Lie algebras, and there is a mapping relationship between Lie groups and Lie algebras. Lie algebras describe the local properties of Lie groups. The Lie algebra of a special orthogonal group is defined in The vector on , and its mapping relationship with Lie group is:

[0154]

[0155]

[0156] in, Defining the Lie algebra of a special orthogonal group Mapping between and antisymmetric matrices generated from it:

[0157]

[0158] in, , and It is a schematic variable and has no special meaning. , for any antisymmetric matrix, we can find the corresponding vector, so we use To describe and Reverse mapping.

[0159] The Lie algebra of a special Euclidean group is defined in The vector on , which is defined as:

[0160] ;

[0161] in, describe The rotating part in Decompose into module length and direction vector ,Right now . describe The translation part in is related to the translation vector The relationship is:

[0162] ;

[0163] To describe and The mapping relationship between and The meaning of is extended to define Mapping with the following four-dimensional matrix:

[0164] ;

[0165] therefore, and The exponential mapping between can be described as:

[0166] ;

[0167] .

[0168] For the method steps disclosed in the above embodiments, the method steps are described as a series of action combinations for the purpose of simple description, but those skilled in the art should know that the embodiments of the present invention are not limited by the order of the actions described, because according to the embodiments of the present invention, some steps can be performed in other orders or simultaneously. Secondly, those skilled in the art should also know that the embodiments described in the specification are all preferred embodiments, and the actions involved are not necessarily required by the embodiments of the present invention.

[0169] Any process or method description described in a flowchart or other manner may be understood as representing a module, fragment or portion of a code including one or more executable instructions for implementing the steps of a specific logical function or process, and the scope of the preferred embodiments of the present invention includes alternative implementations, in which the order shown or discussed may not be followed, including executing and implementing the functions in a substantially simultaneous manner or in a reverse order according to the functions involved, or executing computer instructions and implementing the corresponding functions according to program structures such as loops and branches, which can be naturally understood by those skilled in the art when implementing the embodiments of the present invention.

[0170] like Figure 2 The LiDAR-IMU coupled odometer system shown is used to implement the LiDAR-IMU coupled odometer method, including:

[0171] The IMU predicted pose data generation module is used to obtain the IMU sampling data and LiDAR sampling data at time n, and generate the IMU predicted pose data using the IMU sampling data in the time period from time n-2 to time n-1;

[0172] The posture transformation data generation module performs fuzzy processing on the predicted posture data through fuzzy information processing means to generate corresponding posture transformation data ;

[0173] LiDAR-IMU coupled odometer fusion module is used to transform the pose data As the initial pose estimate of LiDAR point cloud registration, the state observation value at time n is obtained through iterative calculation of LiDAR point cloud registration , the posture transformation data and state observations Fusion is performed to form a LiDAR-IMU coupled odometer.

[0174] The implementation methods of the system described above are merely illustrative, for example, the various functional modules, units or subsystems in the system may or may not be physically separated, or may or may not be physical units, that is, they may be located in the same place, or they may be distributed to multiple different systems and their subsystems or modules. Those skilled in the art may select some or all of the functional modules, units or subsystems according to actual needs to achieve the purpose of the embodiments of the present invention, and those of ordinary skill in the art may understand and implement the above-mentioned situations without creative work.

[0175] like Figure 3As shown, the embodiment of the present application provides a LiDAR-IMU coupled odometer method and system, and also provides corresponding electronic equipment, storage medium and vehicle:

[0176] A vehicle comprising the LiDAR-IMU coupled odometer system.

[0177] An electronic device includes: a processor, a communication interface, a memory and a communication bus, wherein the processor, the communication interface and the memory communicate with each other via the communication bus; a computer program is stored in the memory, and when the computer program is executed by the processor, the processor executes the steps of a LiDAR-IMU coupled odometer method.

[0178] A computer-readable storage medium stores a computer program executable by an electronic device. When the computer program runs on the electronic device, the electronic device executes the steps of a LiDAR-IMU coupled odometer method.

[0179] The device 600 includes a computing unit 601, which can perform various appropriate actions and processes according to a computer program stored in a read-only memory 602 (ROM) or a computer program loaded from a storage unit 608 into a random access memory 603 (RAM). In the RAM, various programs and data required for the operation of the device 600 can also be stored. The computing unit 601, the ROM, and the RAM are connected to each other via a bus 604. An input / output (I / O) interface 605 is also connected to the bus 604.

[0180] A number of components in the device 600 are connected to the I / O interface 605, including: an input unit 606, such as a keyboard, a mouse, etc.; an output unit 607, such as various types of displays, speakers, etc.; a storage unit 608, such as a disk, an optical disk, etc.; and a communication unit 609, such as a network card, a modem, a wireless communication transceiver, etc. The communication unit 609 allows the device 600 to exchange information / data with other devices through a computer network such as the Internet and / or various telecommunication networks.

[0181] The computing unit 601 may be a variety of general and / or special processing components with processing and computing capabilities. Some examples of the computing unit 601 include, but are not limited to, a central processing unit (CPU), a graphics processing unit (GPU), various dedicated artificial intelligence (AI) computing chips, various computing units running machine learning model algorithms, digital signal processors (DSPs), and any appropriate processors, controllers, microcontrollers, etc. The computing unit 601 performs the various methods and processes described above, such as executing the LiDAR-IMU odometer method. For example, in some embodiments, the LiDAR-IMU odometer method may be implemented as a computer software program, which is tangibly contained in a machine-readable medium, such as a storage unit 608. In some embodiments, part or all of the computer program may be loaded and / or installed on the device 600 via the ROM 602 and / or the communication unit 609. When the computer program is loaded into the RAM 603 and executed by the computing unit 601, one or more steps of the LiDAR-IMU odometer method described above may be performed. Alternatively, in other embodiments, the computing unit 601 may be configured to perform the LiDAR-IMU odometer method in any other suitable manner (eg, by means of firmware).

[0182] Various implementations of the systems and techniques described above herein can be implemented in digital electronic circuit systems, integrated circuit systems, field programmable gate arrays (FPGAs), application specific integrated circuits (ASICs), application specific standard products (ASSPs), systems on chips (SOCs), complex programmable logic devices (CPLDs), computer hardware, firmware, software, and / or combinations thereof. These various implementations can include: being implemented in one or more computer programs that can be executed and / or interpreted on a programmable system including at least one programmable processor, which can be a special purpose or general purpose programmable processor that can receive data and instructions from a storage system, at least one input device, and at least one output device, and transmit data and instructions to the storage system, the at least one input device, and the at least one output device.

[0183] The program code for implementing the method of the present invention may be written in any combination of one or more programming languages. These program codes may be provided to a processor or controller of a general-purpose computer, a special-purpose computer, or other LiDAR-IMU odometer device, so that when the program code is executed by the processor or controller, the functions / operations specified in the flowchart and / or block diagram are implemented. The program code may be executed entirely on the machine, partially on the machine, partially on the machine as a stand-alone software package and partially on a remote machine, or entirely on a remote machine or server.

[0184] In the context of the present invention, a machine-readable medium may be a tangible medium that may contain or store a program for use by or in conjunction with an instruction execution system, device, or equipment. A machine-readable medium may be a machine-readable signal medium or a machine-readable storage medium. A machine-readable medium may include, but is not limited to, electronic, magnetic, optical, electromagnetic, infrared, or semiconductor systems, devices, or equipment, or any suitable combination of the foregoing. A more specific example of a machine-readable storage medium may include an electrical connection based on one or more lines, a portable computer disk, a hard disk, a random access memory (RAM), a read-only memory (ROM), an erasable programmable read-only memory (EPROM or flash memory), an optical fiber, a portable compact disk read-only memory (CD-ROM), an optical storage device, a magnetic storage device, or any suitable combination of the foregoing.

[0185] To provide interaction with a user, the systems and techniques described herein can be implemented on a computer having: a display device (e.g., a CRT (cathode ray tube) or LCD (liquid crystal display) monitor) for displaying information to the user; and a keyboard and pointing device (e.g., a mouse or trackball) through which the user can provide input to the computer. Other types of devices can also be used to provide interaction with the user; for example, the feedback provided to the user can be any form of sensory feedback (e.g., visual feedback, auditory feedback, or tactile feedback); and input from the user can be received in any form (including acoustic input, voice input, or tactile input).

[0186] The systems and techniques described herein may be implemented in a computing system that includes back-end components (e.g., as a data server), or a computing system that includes middleware components (e.g., an application server), or a computing system that includes front-end components (e.g., a user computer with a graphical user interface or a web browser through which a user can interact with implementations of the systems and techniques described herein), or a computing system that includes any combination of such back-end components, middleware components, or front-end components. The components of the system may be interconnected by any form or medium of digital data communication (e.g., a communication network). Examples of communication networks include: a local area network (LAN), a wide area network (WAN), and the Internet.

[0187] A computer system may include a client and a server. The client and the server are generally remote from each other and usually interact through a communication network. The relationship of client and server is generated by computer programs running on respective computers and having a client-server relationship with each other. The server may be a cloud server, a server of a distributed system, or a server combined with a blockchain.

[0188] It should be understood that the various forms of processes shown above can be used to reorder, add or delete steps. For example, the steps described in the present invention can be executed in parallel, sequentially or in different orders, as long as the desired results of the technical solution disclosed in the present invention can be achieved, and this document does not limit this.

[0189] The technical features of the above embodiments may be combined arbitrarily. To make the description concise, not all possible combinations of the technical features in the above embodiments are described. However, as long as there is no contradiction in the combination of these technical features, they should be considered to be within the scope of this specification.

[0190] In addition, those skilled in the art will appreciate that, although some embodiments described herein include certain features included in other embodiments but not other features, the combination of features of different embodiments is meant to be within the scope of the present invention and form different embodiments. For example, any one of the embodiments claimed in the claims can be used in any combination in the embodiments of the present invention.

[0191] In the description of this specification, the description with reference to the terms "one embodiment", "example", "specific example", etc. means that the specific features, structures, materials or characteristics described in conjunction with the embodiment or example are included in at least one embodiment or example of the present invention. In this specification, the schematic representation of the above terms does not necessarily refer to the same embodiment or example. Moreover, the specific features, structures, materials or characteristics described may be combined in any one or more embodiments or examples in a suitable manner.

[0192] In addition, the technical solutions between the various embodiments of the present invention can be combined with each other, but it must be based on the fact that ordinary technicians in the field can implement it. When the combination of technical solutions is contradictory or cannot be implemented, it should be deemed that such combination of technical solutions does not exist and is not within the scope of protection required by the present invention.

[0193] All features disclosed in this specification, or steps in all methods or processes disclosed, except for mutually exclusive features and / or steps, can be combined in any way. Any feature disclosed in this specification, unless otherwise stated, can be replaced by other equivalent or alternative features with similar purposes. That is, unless otherwise stated, each feature is only an example of a series of equivalent or similar features. Throughout the specification, the same reference numerals indicate the same elements.

[0194] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention, rather than to limit it. Although the present invention has been described in detail with reference to the aforementioned embodiments, those skilled in the art should understand that they can still modify the technical solutions described in the aforementioned embodiments, or replace some or all of the technical features therein with equivalents. However, these modifications or replacements do not cause the essence of the corresponding technical solutions to deviate from the scope of the technical solutions of the embodiments of the present invention.

Claims

1. A LiDAR-IMU coupled odometer method, applied to a LiDAR-IMU coupled odometer system, characterized in that: include: Obtain the IMU sampling data and LiDAR sampling data at time n, and use the IMU sampling data in the time period from time n-2 to time n-1 to generate IMU predicted pose data; The predicted posture data is fuzzy processed by fuzzy information processing means to generate corresponding posture transformation data ; The posture transformation data As the initial pose estimate of LiDAR point cloud registration, the state observation value at time n is obtained through iterative calculation of LiDAR point cloud registration , the posture transformation data and state observations Fusion is performed to form a LiDAR-IMU coupled odometer; The pose change ratio and the variance of pose estimation are generated by using the average value and generalized likelihood ratio test of the IMU sampling data in the time period from time n-2 to time n-1; The IMU sampling data in the time period from time n-2 to time n-1 is used as the posture change benchmark, and the posture change benchmark is used to generate the corresponding IMU predicted posture data according to the posture change ratio; The predicted posture data is fuzzy processed by fuzzy information processing means to generate corresponding posture transformation data , further comprising: When fuzzy processing is performed on the position change ratio, the input is the modulus of the average value of the system acceleration. and the model of the reference position transformation , the output is the position change ratio, marked as Position-Ratio; When fuzzy processing the attitude change ratio, the input is the modulus of the average angular velocity of the system. The model of the transformation from the reference attitude , the output is the posture change ratio, labeled as Posture-Ratio; For the fuzzy treatment of the position change variance, the input is the generalized likelihood ratio of the system acceleration measurement and the model of the reference position transformation , the output is the position transformation variance increment, marked as G; For the fuzzy processing of attitude transformation variance, the input is the generalized likelihood ratio of the system angular velocity measurement The model of the transformation from the reference attitude , the output is the attitude transformation variance increment, marked as H; The output is the position change ratio, denoted as Position-Ratio, specifically: Based on Position-Ratio and reference position transformation Get the right arrive The predicted position change between moments satisfies the following formula: ； The output is the posture change ratio, denoted as Posture-Ratio, specifically: Based on Posture-Ratio and Baseline Posture Transformation Get the right arrive The predicted posture change between moments satisfies the following formula: ； The predicted position in the time period from n-1 to n is: ； in, Refers to the rotation change Convert to the corresponding rotation matrix through exponential mapping; Get the state observation value at time n for: ； in, represents the pose transformation matrix at time n-1, represents the change in posture between time n and time n-1, It means to transform the elements in Lie group into the elements in Lie algebra; The state observation value Transformed into corresponding pose transformation data , its prediction variance matrix is: 。 2. The LiDAR-IMU coupled odometer method according to claim 1, characterized in that: The acquiring of IMU sampling data and LiDAR sampling data at time n, and using the IMU sampling data in a time period from time n-2 to time n-1 to generate IMU predicted pose data, further includes: Assume that the system state at the nth LiDAR sampling moment is: ;in is the Lie algebra of a special Euclidean group, describe The rotating part of Represents the position of the object, Indicates the posture of an object. describe The translation part in , its corresponding Lie group is defined as , The corresponding Lie group is defined as the rotation matrix R, is the position change, is a three-dimensional real vector space, is a six-dimensional real vector space; the Lie algebra notation of the pose change benchmark is , the corresponding Lie group is labeled .

3. The LiDAR-IMU coupled odometer method according to claim 1, characterized in that: The posture transformation data As the initial pose estimate of LiDAR point cloud registration, the state observation value at time n is obtained through iterative calculation of LiDAR point cloud registration , the posture transformation data and state observations The fusion is performed to form a LiDAR-IMU coupled odometer, which further includes: Pose transformation data predicted by IMU pose predictor As the initial pose estimate, through point cloud registration iteration, we obtain The state observation value at time ; Receiving LiDAR frames , the LiDAR frame Each point Calculate with the continuous adjacent points in the same scan line beam to get the roughness value of each point , get the LiDAR frame The calculation formula is as follows: ； in: is the set of consecutive adjacent points in the same scan line beam, yes The point in Obtain a local feature map and local map feature points corresponding to the local feature map, and convert the LiDAR frame The feature points of the LiDAR frame are matched with the local map feature points to obtain Corresponding pose transformation data .

4. The LiDAR-IMU coupled odometer method according to claim 3, characterized in that: The local feature map is defined as: ,in is the local map edge feature set, is the set of local map edge features; The LiDAR frame The feature points of the local map are matched with the feature points of the local map to obtain a corresponding matching distance, and the matching distance specifically satisfies the following definition: ； in, is the pose transformation data that needs to be estimated for the current frame; The following formula is used to perform the feature point matching: ； In the above formula, is the pose transformation data that needs to be estimated for the current frame, is the edge feature set of the current frame, is a point in the edge feature set of the current frame; is the current frame plane feature set, It is a point in the plane feature set of the current frame; express The matching distance between adjacent edge feature points in the map, express The matching distance between adjacent planar feature points in the map.

5. The LiDAR-IMU coupled odometer method according to claim 4, characterized in that: The local feature map is specifically a set of features in the local map. The definition of the local feature map satisfies the following formula: ； in, and yes and Through the pose transformation to the feature set in the local map coordinate system, is the number of keyframes in the local map, Represents the union operation of sets.

6. A LiDAR-IMU coupled odometer system, used to implement the LiDAR-IMU coupled odometer method according to any one of claims 1 to 5, characterized in that: include: The IMU predicted pose data generation module is used to obtain the IMU sampling data and LiDAR sampling data at time n, and generate the IMU predicted pose data using the IMU sampling data in the time period from time n-2 to time n-1; The posture transformation data generation module performs fuzzy processing on the predicted posture data through fuzzy information processing means to generate corresponding posture transformation data ; LiDAR-IMU coupled odometer fusion module is used to transform the pose data As the initial pose estimate of LiDAR point cloud registration, the state observation value at time n is obtained through iterative calculation of LiDAR point cloud registration , the posture transformation data and state observations Fusion is performed to form a LiDAR-IMU coupled odometer.

7. A vehicle, characterized in that: The vehicle includes the LiDAR-IMU coupled odometer system described in claim 6.

8. An electronic device, characterized in that: include: A processor, a communication interface, a memory and a communication bus, wherein the processor, the communication interface and the memory communicate with each other via the communication bus; a computer program is stored in the memory, and when the computer program is executed by the processor, the processor executes the steps of the method described in any one of claims 1 to 5.

Images