Real-time positioning method and device integrating laser radar and IMU sensor information

Abstract

The present invention provides a real-time positioning method and device integrating laser radar and IMU sensor information, with unscented Kalman filtering as the framework, using high-frequency inertial measurement unit data to predict the motion of an unmanned platform, and caching the data; after receiving the laser radar data with a timestamp, the point cloud matching module uses the predicted posture as a priori information, matches based on the normal distribution transformation and the known point cloud map, and obtains delayed global positioning information; uses the positioning information to update the unscented Kalman filter at the time of the laser radar timestamp, calibrates the prediction error of the inertial measurement unit, and uses the cached high-frequency inertial measurement unit data to re-predict based on the positioning information, so as to achieve a low-latency, high-frequency positioning effect. The method can achieve high-frequency, low-latency real-time positioning, bringing better control effects.

Description

Technical Field

[0001] The present invention relates to the field of robot perception and navigation, and in particular to a real-time positioning method and device that integrates laser radar and IMU sensor information. Background Art

[0002] Robot positioning technology is the basic function that enables robots to achieve autonomous navigation and planning, and is of great significance to mobile robot platforms. Robot positioning technology is divided into positioning in known environments or positioning in unknown environments. The positioning problem of positioning in known environments is also called the relocalization problem, that is, the robot determines the position of the robot in the environment when the environment map or measurement data is known. Common robot positioning technologies include satellite positioning, Wifi positioning, or positioning based on vision and laser sensors. Different from satellite positioning or Wifi positioning technologies, positioning technologies based on sensors such as lidar and cameras can determine the position of the robot in the environment based on its own perception data without the help of external measurements, and can help the robot achieve autonomous positioning in an environment without satellite positioning. At present, positioning methods based on laser sensors can achieve good accuracy both indoors and outdoors, and are widely used in the field of mobile robots. However, due to the large amount of data and the characteristics of the lidar sensor itself, the positioning frequency is low, the calculation overhead leads to high latency, and other problems still need to be solved.

[0003] For the problem of robot relocalization based on known maps, there are existing solutions: The literature (S. Pang, D. Kent, X. Cai, H. Al-Qassab, D. Morris and H. Radha, "3D Scan Registration Based Localization for Autonomous Vehicles-A Comparison of NDT and ICP under Realistic Conditions," Vehicular Technology Conference (VTC-Fall), 2018) introduces two positioning methods based on point cloud matching, namely, the positioning method based on the iterative closest point (ICP) and the normal distribution transform (NDT). Compared with ICP matching, which calculates the distance relationship between corresponding points, NDT calculates the normal distribution of the point cloud, has lower requirements for initial values, and has better robustness and positioning accuracy. In the prior art, the disadvantages of the method using NDT are low positioning frequency, positioning delay, and only applicable to low-speed motion. Therefore, the NDT positioning result cannot be directly used in the control link. Summary of the invention

[0004] In order to solve the above technical problems, the present invention proposes a real-time positioning method and device that integrates laser radar and IMU sensor information. The method and device are used to solve the technical problems in the prior art that the NDT positioning frequency is low, the positioning has a delay and is only suitable for low-speed movement, and the NDT positioning results cannot be directly used for the control link.

[0005] According to a first aspect of the present invention, a real-time positioning method integrating laser radar and IMU sensor information is provided, the method comprising the following steps:

[0006] Step S100: In the offline stage, the observation model of the laser radar and the prediction model of the IMU are respectively modeled, wherein the observation model of the laser radar is used to determine the observation of the laser radar on the posture state of the robot; and the prediction model of the IMU is used to determine the predicted posture of the robot according to the measurement data of the IMU sensor;

[0007] Step S101: In the real-time information acquisition phase, the laser radar and IMU sensors respectively acquire information from the initial time; for the time series Time = {t k-m ,t k-m+1 …,t k}, IMU measurement data is obtained once at each time point, and LiDAR observation data is obtained once every m time points. The LiDAR observation data is point cloud data, t k is the current moment, t k-m is the time when the last optimal pose estimation was performed, and the optimal pose estimation result when the last optimal pose estimation was performed is recorded as The laser radar is k Obtain observation data. At this time, the IMU sensor has obtained the k-m to k The predicted pose at each time point in this period {pose2 k-m ,pose2 k-m+1 ,…,pose2 k}; Save the original measurement data obtained by IMU at each time point in the computer memory;

[0008] Step S102: Obtain t k IMU sensor robot predicted pose2 at the corresponding time k , the IMU sensor robot predicts the pose pose2 k As a priori pose, the laser radar for t is obtained based on NDT matching k The pose observation value pose1 at the moment k Based on t kPredicted pose at the moment pose2 k And observation pose1 k , use the unscented Kalman filter to calculate the optimal estimate of the robot's position at that moment

[0009] Step S103: The NDT matching causes a time lag τ, τ=t k' -t k , t k' is the time after NDT matching, and t k' <t k+m , based on the prediction model and the t k The optimal estimated pose at time Predicting the pose of the IMU sensor robot {pose2 k+1 ,…,pose2 k'} to re-predict and obtain the updated IMU sensor robot predicted pose set {pose2' k+1 ,…,pose2' k'};

[0010] After the update is completed, if real-time IMU and lidar measurement data are still being obtained, then go to step S101; if no measurement data is input, the method ends.

[0011] According to a second aspect of the present invention, a real-time positioning device integrating laser radar and IMU sensor information is provided, the device comprising:

[0012] Modeling module: configured to model the observation model of the laser radar and the prediction model of the IMU respectively in the offline stage, wherein the observation model of the laser radar is used to determine the observation of the robot's posture state by the laser radar; the prediction model of the IMU is used to determine the predicted posture of the robot according to the measurement data of the IMU sensor;

[0013] Prediction information acquisition module: configured to acquire information from the initial moment respectively in the real-time information acquisition phase; for the time series Time = {t k-m ,t k-m+1 …,t k}, IMU measurement data is obtained once at each time point, and LiDAR observation data is obtained once every m time points. The LiDAR observation data is point cloud data, t k is the current moment, t k-m is the time when the last optimal pose estimation was performed, and the optimal pose estimation result when the last optimal pose estimation was performed is recorded as The laser radar is kObtain observation data. At this time, the IMU sensor has obtained the k-m to k The predicted pose at each time point in this period {pose2 k-m ,pose2 k-m+1 ,…,pose2 k}; Save the original measurement data obtained by IMU at each time point in the computer memory;

[0014] Optimal estimation determination module: configured to obtain t k IMU sensor robot predicted pose2 at the corresponding time k , the IMU sensor robot predicts the pose pose2 k As a priori pose, the laser radar for t is obtained based on NDT matching k The pose observation value pose1 at the moment k Based on t k Predicted pose at the moment pose2 k And observation pose1 k , use the unscented Kalman filter to calculate the optimal estimate of the robot's position at that moment

[0015] Update module: configured to cause a time lag τ for the NDT matching, τ = t k' -t k , t k' is the time after NDT matching, and t k' <t k+m , based on the prediction model and the t k The optimal estimated pose at time Predicting the pose of the IMU sensor robot {pose2 k+1 ,…,pose2 k’} to re-predict and obtain the updated IMU sensor robot predicted pose set {pose2' k+1 ,…,pose2' k'}; After the update is completed, if the real-time IMU and lidar measurement data are still being obtained at this time, the prediction information acquisition module is triggered; if no measurement data is input, the positioning is completed.

[0016] According to a third aspect of the present invention, a real-time positioning system integrating laser radar and IMU sensor information is provided, comprising:

[0017] A processor, which is used to execute multiple instructions;

[0018] A memory for storing a plurality of instructions;

[0019] Among them, the multiple instructions are used to be stored by the memory, and loaded and executed by the processor to implement the real-time positioning method that integrates the laser radar and IMU sensor information as described above.

[0020] According to a fourth aspect of the present invention, a computer-readable storage medium is provided, wherein a plurality of instructions are stored in the storage medium; the plurality of instructions are used for a processor to load and execute the real-time positioning method that integrates laser radar and IMU sensor information as described above.

[0021] According to the above scheme of the present invention, the present invention uses unscented Kalman filtering as a fusion framework, uses high-frequency IMU to predict posture state, uses accurate posture information obtained by laser point cloud matching as observation to update posture state, and uses posture state prediction as prior information for point cloud matching to improve the accuracy of point cloud matching. At the same time, the method introduces a data cache and re-prediction mechanism to suppress the delay caused by point cloud matching calculation, thereby ensuring the real-time and robustness of robot posture estimation. The present invention proposes a fusion method of laser radar and IMU based on unscented Kalman filtering, and realizes high-frequency and robust posture state estimation by using IMU to predict state and laser radar matching posture to update state; using the state predicted by IMU as prior information for laser radar matching, reduces the nonlinearity of the optimization problem in point cloud matching, improves the accuracy of the matching algorithm and the robustness to high-speed rotation; introduces a data cache and re-prediction mechanism, by caching the IMU data updated at the last time point, for observation data with time lag, the state from the observation data acquisition time to the current time period can be re-predicted, thereby reducing the time lag of the posture estimation result.

[0022] The above description is only an overview of the technical solution of the present invention. In order to more clearly understand the technical means of the present invention and implement it according to the contents of the specification, the following is a detailed description of the preferred embodiments of the present invention in conjunction with the accompanying drawings. BRIEF DESCRIPTION OF THE DRAWINGS

[0023] The accompanying drawings constituting a part of the present invention are used to provide a further understanding of the present invention, and the present invention is described by the following accompanying drawings. In the accompanying drawings:

[0024] Figure 1 A flowchart of a real-time method for fusing laser radar and IMU sensor information according to an embodiment of the present invention;

[0025] Figure 2 It is a schematic diagram of a delay-compensated real-time positioning framework of multi-sensor fusion based on unscented Kalman filtering according to an embodiment of the present invention;

[0026] Figure 3This is a rendering of a method for fusing laser radar information and IMU sensor information according to an embodiment of the present invention;

[0027] Figure 4 A schematic diagram of a flow chart of an NDT matching method according to an embodiment of the present invention;

[0028] Figure 5 A logical diagram of a data cache and delay compensation mechanism according to an embodiment of the present invention;

[0029] Figure 6 This is a schematic diagram of the structure of a real-time positioning device that integrates laser radar and IMU sensor information according to an embodiment of the present invention. DETAILED DESCRIPTION

[0030] First combine Figure 1-3 The real-time method flow of fusing laser radar and IMU sensor information according to one embodiment of the present invention is described, and the method comprises the following steps:

[0031] Step S100: In the offline stage, the observation model of the laser radar and the prediction model of the IMU are respectively modeled, wherein the observation model of the laser radar is used to determine the observation of the robot's posture state by the laser radar according to the point cloud matching result of the laser radar; the prediction model of the IMU is used to determine the predicted posture of the robot according to the measurement data of the IMU sensor;

[0032] Step S101: In the real-time information acquisition phase, the laser radar and IMU sensors respectively acquire information from the initial time; for the time series Time = {t k-m ,t k-m+1 …,t k}, IMU measurement data is obtained once at each time point, and LiDAR observation data is obtained once every m time points. The LiDAR observation data is point cloud data, t k is the current moment, t k-m is the time when the last optimal pose estimation was performed, and the optimal pose estimation result when the last optimal pose estimation was performed is recorded as The laser radar is k Obtain observation data. At this time, the IMU sensor has obtained the k-m to k The predicted pose at each time point in this period {pose2 k-m ,pose2 k-m+1 ,…,pose2 k}; Save the original measurement data obtained by IMU at each time point in the computer memory;

[0033] Step S102: Obtain t kIMU sensor robot predicted pose2 at the corresponding time k , the IMU sensor robot predicts the pose pose2 k As a priori pose, the laser radar for t is obtained based on NDT matching k The pose observation value pose1 at the moment k Based on t k Predicted pose at the moment pose2 k And observation pose1 k , use the unscented Kalman filter to calculate the optimal estimate of the robot's position at that moment

[0034] Step S103: The NDT matching causes a time lag τ, τ=t k' -t k , t k' is the time after NDT matching, and t k' <t k+m , based on the prediction model and the t k The optimal estimated pose at time Predicting the pose of the IMU sensor robot {pose2 k+1 ,…,pose2 k'} to re-predict and obtain the updated IMU sensor robot predicted pose set {pose2' k+1 ,…,pose2' k'};

[0035] After the update is completed, if real-time IMU and lidar measurement data are still being obtained, then go to step S101; if no measurement data is input, the method ends.

[0036] In step S100, the prediction model of the IMU and the observation model of the laser radar are respectively:

[0037]

[0038]

[0039] Among them, x k t k The state variables of the robot obtained by the IMU at this moment, t k The noise of the moment prediction has a mean of 0 and a variance of Gaussian noise; f represents the noise generated by t k-1 The state recursion formula for predicting the state at time k, x k-1 t k-1 The state variables of the robot at time w k-1 tk-1 The prediction noise at the moment;

[0040] y k Indicates t k The laser radar observes the environment at all times. t k The noise observed at each moment has a mean of 0 and a variance of Gaussian noise; h represents the current observation data y k At the current state x k The following expression.

[0041] Furthermore, in step S100, the prediction equation of the IMU and the observation equation of the laser radar are determined, and the state variable x k Including the robot's position p in the map coordinate system k , the robot's speed v in the map coordinate system k , the robot's posture R in the map coordinate system k , the posture is represented by a rotation matrix;

[0042] The prediction equation of the IMU is as follows:

[0043]

[0044] where a k ,ω k They are respectively IMU at t k The robot linear angular velocity and angular velocity measured at all times, p k-1 t k-1 The position of the robot in the map coordinate system at the moment, v k-1 t k-1 The speed of the robot at the time in the map coordinate system, Δt is the interval between two consecutive IMU measurements, R k-1 t k-1 The posture of the robot at the moment in the map coordinate system, g is the gravitational acceleration.

[0045] The observation equation of the laser radar is as follows:

[0046]

[0047] where y k t k Observation data at all times, robot posture observation based on point cloud matching And observation of robot posture Robot speed observation By t k and t k-1 The position difference at time is obtained.

[0048] In view of the optimal estimation method based on probability distribution used in this embodiment, directly using a nonlinear prediction model will cause the estimation method of the subsequent steps to fail. To address this problem, the prediction model is not used to directly predict the state, but an approximate method is used. This embodiment uses 2N+1 points to approximate the distribution of the state, where N represents the dimension of the state. The specific method is to use the unscented Kalman filter prediction algorithm, and the unscented Kalman filter prediction algorithm process is:

[0049] Input: The state mean x at the previous moment k-1 , covariance P k-1 , IMU measurement value a k ,ω k

[0050] According to the mean and covariance of the state at the previous moment, calculate the 2N+1σ points that can approximate the state distribution

[0051]

[0052]

[0053] After calculating the σ point at the previous moment, use the IMU motion model to predict the σ point at the current moment:

[0054]

[0055] Then calculate the mean and covariance of the predicted σ points:

[0056] in

[0057] in is the mean of the predicted state, is the covariance of the predicted state, Corresponding to the above predicted pose pose1 k .

[0058] like Figure 4 As shown in FIG. 1 , in this embodiment, after obtaining the observation data of the laser radar at a certain moment, it is necessary to calculate the position of the current point cloud in the point cloud map by the point cloud registration method, so as to calculate the robot at t k The laser radar pose at this moment is pose1 k .

[0059] Step S102: obtaining t k IMU sensor robot predicted pose2 at the corresponding time k , the IMU sensor robot predicts the pose pose2 kAs a priori pose, the laser radar for t is obtained based on NDT matching k The pose observation value pose1 at the moment k ,include:

[0060] Step S1021: The laser radar obtains t k Observation data at time t k The original 3D point cloud P measured at each moment k , for the original three-dimensional point cloud P k Downsample to obtain a filtered point cloud set x i Indicates the laser point cloud in the current laser radar coordinate system The three-dimensional coordinates under, i = 1…N x , N x is the number of point clouds after filtering; get the point cloud map at t k IMU sensor robot predicted pose2 at the corresponding time k A collection of 3D point clouds within the laser radar measurement range where y j Indicates the point cloud in the point cloud map in the map coordinate system The three-dimensional coordinates under j = 1…N y , N y is the number of point clouds extracted from the point cloud map; the original 3D point cloud refers to the laser radar at t k The point cloud obtained at the moment, the point cloud map refers to a pre-established point cloud map, and the three-dimensional point cloud within the laser radar measurement range is the point cloud of the point cloud map within the laser radar measurement range;

[0061] Step S1022: Determine the probability distribution (μ, ∑) of the point cloud in the point cloud map, where μ is the mean of the point cloud probability distribution and ∑ is the variance of the point cloud probability distribution.

[0062]

[0063] Step S1023: Obtain t k IMU sensor robot predicted pose2 at the corresponding time k , change pose2 k As The initial value of X k Perform a three-dimensional transformation on the point cloud and convert it to the map coordinate system In, remember x′ i For x i In the map coordinate system The three-dimensional coordinates of As the laser radar coordinate system To map coordinate system Three-dimensional coordinate transformation;

[0064] Step S1024: Determine the calculation X k The probability distribution of each point in ;

[0065]

[0066] Among them, g(Y,x′ i ) is used to calculate X k The formula for the probability distribution of each point in ;

[0067] Step S1025: construct the optimization target maxΨ,

[0068]

[0069] in, To convert point x i pass From the laser radar coordinate system F k Transform to map coordinate system The above optimization objective is solved by the standard Gauss-Newton method, and the current optimal transformation is obtained by iteration. Will As the point cloud matching result, and as the robot in t k The laser radar pose at this moment is pose1 k .

[0070] The step S102: based on t k Predicted pose at the moment pose2 k And observation pose1 k , use the unscented Kalman filter to calculate the optimal estimate of the robot's position at that moment That is, the covariance matrix of the predicted pose and the observed pose is calculated respectively. The covariance matrix represents the pose2 k , pose pose1 k The confidence level of the two confidence values ​​is used to determine the optimal pose estimate.

[0071] In this embodiment, t is calculated according to the unscented Kalman filter equation k The optimal estimation of the robot's posture at the moment uses the unscented Kalman filter optimal estimation algorithm, and its algorithm is:

[0072] Algorithm input: the mean of the predicted state at this moment Covariance Point cloud matching pose2 k

[0073] Compute the observations and their means and covariances:

[0074]

[0075] in

[0076] According to the distribution and covariance of the predicted and observed σ points, the cross-covariance of x and y is calculated, and the Kalman gain K is calculated. k :

[0077]

[0078]

[0079] Finally, the final optimal estimate is calculated based on the Kalman gain and covariance

[0080]

[0081]

[0082] Specifically, the input data of the optimal estimate of the unscented Kalman filter - the predicted state mean at the current moment Covariance And point cloud matching pose2 k First, through the observation equation, the mean of the observation can be calculated and variance P y ; Combined with the predicted state, variance and P y Represents the confidence level for the two estimated values. Based on the observed data and the predicted state distribution, the mutual variance P of the two state estimates is calculated. xy And calculate the Kalman gain K k , K k ∈[0,1],K k The larger the value, the higher the confidence of the observed state, and vice versa, the higher the confidence of the predicted state. k From the predicted pose pose2 k And observation pose1 k Determine the optimal estimate And the corresponding covariance here correspond

[0083] The unscented Kalman filter is used as the state estimation method for the fusion of two sensors. Compared with the most commonly used extended Kalman filter, the unscented Kalman filter has a better approximation effect for nonlinear functions and does not require the probability distribution function to be non-Gaussian, so it can obtain better state estimation results.

[0084] In step S103, due to the huge amount of laser radar point cloud data, solving the laser radar position in the map by iterative matching has a large computational overhead, and the resulting time lag will affect the subsequent filtering fusion. Therefore, a data cache and delay compensation mechanism is designed, and its principle is as follows: Figure 5 As shown in the figure. That is, due to the large overhead of NDT matching calculation, the time at this time has been increased by t k Moved to t k' , t k' <t k+m That is, the next frame of LiDAR point cloud has not been observed yet. Therefore, point cloud matching brings a time lag τ, τ = t k' -t k . Due to t k The change of state at the moment is based on the prediction model and the t k The optimal estimated pose at time Predicting the pose of the IMU sensor robot {pose2 k+1 ,…,pose2 k'} to re-predict and obtain the updated IMU sensor robot predicted pose set {pose2' k+1 ,…,pose2' k'};

[0085] After the update is completed, if real-time IMU and lidar measurement data are still being obtained, then go to step S101; if no measurement data is input, the method ends.

[0086] In this embodiment, according to the unscented Kalman filter equation, the point cloud matching result is used At time t k Update the state variables. k ,t k' The predicted state in the time period is affected by the state update, and the IMU data in the memory is called to re-predict the state variables to obtain the updated real-time predicted state, thereby suppressing the point cloud matching result. The impact of the time lag τ on state estimation.

[0087] The embodiment of the present invention further provides a real-time positioning device that integrates laser radar and IMU sensor information, such as Figure 6 As shown, the device comprises:

[0088] Modeling module: configured to model the observation model of the laser radar and the prediction model of the IMU respectively in the offline stage, wherein the observation model of the laser radar is used to determine the observation of the robot's posture state by the laser radar; the prediction model of the IMU is used to determine the predicted posture of the robot according to the measurement data of the IMU sensor;

[0089] Prediction information acquisition module: configured to acquire information from the initial moment respectively in the real-time information acquisition phase; for the time series Time = {t k-m ,t k-m+1 …,t k}, IMU measurement data is obtained once at each time point, and LiDAR observation data is obtained once every m time points. The LiDAR observation data is point cloud data, t k is the current moment, t k-m is the time when the last optimal pose estimation was performed, and the optimal pose estimation result when the last optimal pose estimation was performed is recorded as The laser radar is k Obtain observation data. At this time, the IMU sensor has obtained the k-m to k The predicted pose at each time point in this period {pose2 k-m ,pose2 k-m+1 ,…,pose2 k}; Save the original measurement data obtained by IMU at each time point in the computer memory;

[0090] Optimal estimation determination module: configured to obtain t k IMU sensor robot predicted pose2 at the corresponding time k , the IMU sensor robot predicts the pose pose2 k As a priori pose, the laser radar for t is obtained based on NDT matching k The pose observation value pose1 at the moment k Based on t k Predicted pose at the moment pose2 k And observation pose1 k , use the unscented Kalman filter to calculate the optimal estimate of the robot's position at that moment

[0091] Update module: configured to cause a time lag τ for the NDT matching, τ = t k' -t k , t k' is the time after NDT matching, and t k' <t k+m, based on the prediction model and the t k The optimal estimated pose at time Predicting the pose of the IMU sensor robot {pose2 k+1 ,…,pose2 k'} to re-predict and obtain the updated IMU sensor robot predicted pose set {pose2' k+1 ,…,pose2' k'};

[0092] After the update is completed, if the real-time IMU and lidar measurement data are still being obtained, the prediction information acquisition module is triggered; if no measurement data is input, the positioning is completed.

[0093] The embodiment of the present invention further provides a real-time positioning system integrating laser radar and IMU sensor information, including:

[0094] A processor, which is used to execute multiple instructions;

[0095] A memory for storing a plurality of instructions;

[0096] Among them, the multiple instructions are used to be stored by the memory, and loaded and executed by the processor to implement the real-time positioning method that integrates the laser radar and IMU sensor information as described above.

[0097] An embodiment of the present invention further provides a computer-readable storage medium, in which a plurality of instructions are stored; the plurality of instructions are used by a processor to load and execute the real-time positioning method that integrates laser radar and IMU sensor information as described above.

[0098] It should be noted that, in the absence of conflict, the embodiments of the present invention and the features in the embodiments may be combined with each other.

[0099] In the several embodiments provided by the present invention, it should be understood that the disclosed systems, devices and methods can be implemented in other ways. For example, the device embodiments described above are only schematic. For example, the division of the units is only a logical function division. There may be other division methods in actual implementation. For example, multiple units or components can be combined or integrated into another system, or some features can be ignored or not executed. Another point is that the mutual coupling or direct coupling or communication connection shown or discussed can be an indirect coupling or communication connection through some interfaces, devices or units, which can be electrical, mechanical or other forms.

[0100] The units described as separate components may or may not be physically separated, and the components shown as units may or may not be physical units, that is, they may be located in one place or distributed on multiple network units. Some or all of the units may be selected according to actual needs to achieve the purpose of the solution of this embodiment.

[0101] In addition, each functional unit in each embodiment of the present invention may be integrated into one processing unit, or each unit may exist physically separately, or two or more units may be integrated into one unit. The above-mentioned integrated unit may be implemented in the form of hardware or in the form of hardware plus software functional units.

[0102] The above-mentioned integrated unit implemented in the form of a software functional unit can be stored in a computer-readable storage medium. The above-mentioned software functional unit is stored in a storage medium, including a number of instructions for a computer device (which can be a personal computer, a physical server, or a network cloud server, etc., which needs to be installed with Windows or Windows Server operating system) to perform some steps of the method described in each embodiment of the present invention. The aforementioned storage medium includes: U disk, mobile hard disk, read-only memory (Read-Only Memory, ROM), random access memory (RandomAccess Memory, RAM), disk or optical disk and other media that can store program codes.

[0103] The above description is only a preferred embodiment of the present invention and does not limit the present invention in any form. Any simple modification, equivalent change and modification made to the above embodiment according to the technical essence of the present invention still falls within the scope of the technical solution of the present invention.

Claims

1. A real-time positioning method integrating laser radar and IMU sensor information, characterized in that: The method comprises the following steps: Step S100: In the offline stage, the observation model of the laser radar and the prediction model of the IMU are respectively modeled, wherein the observation model of the laser radar is used to determine the observation of the robot's posture state by the laser radar according to the point cloud matching result of the laser radar; the prediction model of the IMU is used to determine the predicted posture of the robot according to the measurement data of the IMU sensor; Step S101: In the real-time information acquisition phase, the laser radar and IMU sensors respectively acquire information from the initial time; for the time series Time = {t k-m ,t k-m+1 …,t k }, IMU measurement data is obtained once at each time point, and LiDAR observation data is obtained once every m time points. The LiDAR observation data is point cloud data, t k is the current moment, t k-m is the time when the last optimal pose estimation was performed, and the optimal pose estimation result when the last optimal pose estimation was performed is recorded as The laser radar is k Obtain observation data. At this time, the IMU sensor has obtained the k-m to k The predicted pose at each time point in this period {pose2 k-m ,pose2 k-m+1 ,…,pose2 k }; Save the original measurement data obtained by IMU at each time point in the computer memory; Step S102: Obtain t k IMU sensor robot predicted pose2 at the corresponding time k , the IMU sensor robot predicts the pose pose2 k As a priori pose, the laser radar for t is obtained based on normal distribution transform NDT matching. k The pose observation value at the moment pose1 k Based on t k Predicted pose at the moment pose2 k And observation pose1 k , use the unscented Kalman filter to calculate the optimal estimate of the robot's position at that moment Step S103: The NDT matching causes a time lag τ, τ=t k' -t k , t k' is the time after NDT matching, and t k' <t k+m , based on the prediction model and the t k The optimal estimated pose at time Predicting the pose of the IMU sensor robot {pose2 k+1 ,…,pose2 k' } to re-predict and obtain the updated IMU sensor robot predicted pose set {pose2' k+1 ,…,pose2' k' }; After the update is completed, if real-time IMU and lidar measurement data are still being obtained, then go to step S101; if no measurement data is input, the method ends.

2. The method according to claim 1, characterized in that In step S100, the prediction model of the IMU and the observation model of the laser radar are respectively: Among them, x k t k The state variables of the robot obtained by the IMU at this moment, t k The noise of the moment prediction has a mean of 0 and a variance of Gaussian noise; f represents the noise generated by t k-1 The recursive formula for predicting the state at time k is x k-1 t k-1 The state variables of the robot at time w k-1 t k-1 The prediction noise at the moment; y k Indicates t k The laser radar observes the environment at all times. t k The noise observed at each moment has a mean of 0 and a variance of Gaussian noise; h represents the current observation data y k At the current state x k The following expression.

3. The method according to claim 2, characterized in that Step S102: obtaining t k IMU sensor robot predicted pose2 at the corresponding time k , the IMU sensor robot predicts the pose pose2 k As a priori pose, the laser radar for t is obtained based on NDT matching k The pose observation value pose1 at the moment k ,include: Step S1021: The laser radar obtains t k Observation data at time t k The original 3D point cloud P measured at each moment k , for the original three-dimensional point cloud P k Downsample to obtain a filtered point cloud set x i Indicates the laser point cloud in the current laser radar coordinate system The three-dimensional coordinates under, i = 1…N x , N x is the number of point clouds after filtering; get the point cloud map at t k IMU sensor robot predicted pose2 at the corresponding time k A collection of 3D point clouds within the laser radar measurement range where y j Indicates the point cloud in the point cloud map in the map coordinate system The three-dimensional coordinates under j = 1…N y , N y is the number of point clouds extracted from the point cloud map; the original 3D point cloud refers to the laser radar at t k The point cloud obtained at the moment, the point cloud map refers to a pre-established point cloud map, and the three-dimensional point cloud within the laser radar measurement range is the point cloud of the point cloud map within the laser radar measurement range; Step S1022: Determine the probability distribution (μ, ∑) of the point cloud in the point cloud map, where μ is the mean of the point cloud probability distribution and ∑ is the variance of the point cloud probability distribution. Step S1023: Obtain t k IMU sensor robot predicted pose2 at the corresponding time k , change pose2 k As The initial value of X k Perform a three-dimensional transformation on the point cloud and convert it to the map coordinate system In, remember x i ′ is x i In the map coordinate system The three-dimensional coordinates of As the laser radar coordinate system To map coordinate system Three-dimensional coordinate transformation; Step S1024: Determine the calculation X k The probability distribution of each point in ; Among them, g(Y,x i ′) is used to calculate X k The formula for the probability distribution of each point in ; Step S1025: construct the optimization target maxΨ, in, To convert point x i pass From the LiDAR coordinate system Transform to map coordinate system The above optimization objective is solved by the standard Gauss-Newton method, and the current optimal transformation is obtained by iteration. Will As the point cloud matching result, and as the robot in t k The laser radar pose at this moment is pose1 k .

4. The method according to claim 3, characterized in that The step S102: based on t k Predicted pose at the moment pose2 k And observation pose1 k , use the unscented Kalman filter to calculate the optimal estimate of the robot's position at that moment That is, the covariance matrix of the predicted pose and the observed pose is calculated respectively. The covariance matrix represents the pose2 k , pose pose1 k The confidence level of the two confidence values ​​is used to determine the optimal pose estimate.

5. A real-time positioning device integrating laser radar and IMU sensor information, characterized in that: The device comprises: Modeling module: configured to model the observation model of the laser radar and the prediction model of the IMU respectively in the offline stage, wherein the observation model of the laser radar is used to determine the observation of the robot's posture state by the laser radar; the prediction model of the IMU is used to determine the predicted posture of the robot according to the measurement data of the IMU sensor; Prediction information acquisition module: The laser radar and IMU sensors are configured to acquire information from the initial time. k-m ,t k-m+1 …,t k }, IMU measurement data is obtained once at each time point, and LiDAR observation data is obtained once every m time points. The LiDAR observation data is point cloud data, t k is the current moment, t k-m is the time when the last optimal pose estimation was performed, and the optimal pose estimation result when the last optimal pose estimation was performed is recorded as The laser radar is k Obtain observation data. At this time, the IMU sensor has obtained the k-m to k The predicted pose at each time point in this period {pose2 k-m ,pose 2k-m+1 ,…,pose2 k }; Save the original measurement data obtained by IMU at each time point in the computer memory; Optimal estimation determination module: configured to obtain t k IMU sensor robot predicted pose2 at the corresponding time k , the IMU sensor robot predicts the pose pose2 k As a priori pose, the laser radar for t is obtained based on normal distribution transform NDT matching. k The pose observation value pose1 at the moment k Based on t k Predicted pose at the moment pose2 k And observation pose1 k , use the unscented Kalman filter to calculate the optimal estimate of the robot's position at that moment Update module: configured to cause a time lag τ for the NDT matching, τ = t k' -t k , t k' is the time after NDT matching, and t k' <t k+m , based on the prediction model and the t k The optimal estimated pose at time Predicting the pose of the IMU sensor robot {pose2 k+1 ,…,pose2 k' } to re-predict and obtain the updated IMU sensor robot predicted pose set {pose2' k+1 ,…,pose2' k' }; After the update is completed, if the real-time IMU and lidar measurement data are still being obtained at this time, the prediction information acquisition module is triggered; if no measurement data is input, the positioning is completed.

6. A real-time positioning system integrating laser radar and IMU sensor information, comprising: A processor, which is used to execute multiple instructions; A memory for storing a plurality of instructions; The plurality of instructions are used to be stored in the memory and loaded and executed by the processor to execute the method as described in any one of claims 1 to 4.

7. A computer-readable storage medium, wherein a plurality of instructions are stored in the storage medium; the plurality of instructions are used for a processor to load and execute the method according to any one of claims 1 to 4.

Images