Efficient laser radar inertial positioning method

Abstract

The invention relates to the technical field of mobile robots, and discloses an efficient laser radar inertial positioning method, which comprises the following steps: establishing a kinetic model based on detection data of an inertial sensing unit, and realizing forward evolution of pose estimation; motion compensation is carried out on the radar point cloud obtained through laser radar scanning through reverse evolution, and compensated radar point cloud coordinates are obtained; using the compensated radar point cloud coordinates to construct a detection residual error; performing single-step iterative updating of pose estimation by using the constructed detection residual error to obtain real-time estimation of the pose; according to the method, from the angle of the rear end of the positioning system, the complete estimation of the pose is calculated through single-step iterative updating, so that the positioning efficiency is effectively improved; the invariant extended Kalman filter is used for fusing sensor information to realize positioning, and introduction of linearization errors in the whole pose estimation process is avoided, so that the efficiency is improved, and the positioning precision is ensured.

Description

Technical Field

[0001] The present invention relates to the technical field of mobile robots, and in particular to an efficient laser radar inertial positioning method. Background Art

[0002] LiDAR sensors are very popular in robot positioning and mapping due to their high accuracy, high resolution, strong long-range detection capabilities, and reliability in various environmental conditions. However, they also have some limitations, such as insufficient feature information provided in environments with weak structures and high sensitivity to fast motion distortion, which can lead to reduced positioning performance relying on LiDAR. To address these issues, researchers and engineers in related fields have designed LiDAR inertial positioning systems in combination with inertial sensing units to achieve more accurate and robust robot positioning.

[0003] In the research on LiDAR inertial positioning systems, the positioning system based on tightly coupled filters has attracted extensive attention due to its better real-time performance and the ability to fully utilize the detection information of LiDAR and inertial measurement units. Efficiency and accuracy are the most important indicators for evaluating the performance of almost all positioning systems, including LiDAR inertial positioning systems. For existing LiDAR inertial positioning systems, most of the research work aimed at improving efficiency is concentrated on the front end of the system, including the collection and preprocessing of environmental information, and improving the system positioning efficiency by changing the point cloud spatial data structure and its corresponding radar point insertion and query method or changing the structure of optical radar scanning. For improving the accuracy of the positioning system, the focus is on the state estimation process at the back end of the system, and the estimation error is reduced and the estimation accuracy is improved by increasing the number of iterations in the estimation value update process or the number of sample points representing the target state distribution. However, these methods for improving positioning accuracy often increase the computational complexity while improving the accuracy of pose estimation, resulting in longer calculation time, thereby reducing the efficiency of the system. Summary of the invention

[0004] To solve the above technical problems, the present invention provides a high-efficiency lidar inertial positioning method. From the perspective of the back-end state estimation of the positioning system, an high-efficiency lidar inertial positioning method based on the invariant extended Kalman filter is proposed to achieve the goal of improving the positioning efficiency while ensuring the positioning accuracy.

[0005] In order to solve the above technical problems, the present invention adopts the following technical solutions:

[0006] An efficient laser radar inertial positioning method, comprising:

[0007] A dynamic model is established based on the detection data of the inertial sensor unit to achieve the forward evolution of posture estimation;

[0008] The motion compensation of the radar point cloud obtained by the laser radar scanning is realized through reverse evolution to obtain the coordinates of the compensated radar point cloud;

[0009] Construct detection residuals using compensated radar point cloud coordinates;

[0010] The constructed detection residual is used to perform a single-step iterative update of the pose estimation to obtain a real-time estimate of the pose.

[0011] Furthermore, the dynamic model is established based on the detection data of the inertial sensor unit to realize the forward evolution of the posture estimation, which specifically includes:

[0012] The inertial sensor unit detects data including angular velocity and acceleration;

[0013] The kinetic model is:

[0014] R i+1 =R i exp m ((ω m,i -b ω,i -n ω,i )Δt);

[0015] p i+1 =p i +v i Δt;

[0016] v i+1 =v i +[R i (a m,i -b a,i -n a,i )+g]Δt;

[0017] b ω,i+1 =b ω,i +n bω,i Δt;

[0018] b a,i+1 =b a,i +n ba,i Δt;

[0019] Among them, i represents the sampling time sequence number of the inertial sensor unit, R i ∈SO3 and are the rotation matrix and position vector of the corresponding direction of the i-th sampling moment of the inertial sensor unit, SO3 represents the rotation matrix group, represents the velocity of the inertial sensor unit at the i-th sampling moment; g represents the gravitational acceleration; ω m,i and a m,i is the angular velocity and acceleration detected by the inertial sensor unit at the i-th sampling moment, b ω,i and ba,i They are ω m,i and a m,i The corresponding detection deviation, n ω,i and n a,i They are ω m,i and a m,i The corresponding Gaussian detection noise satisfies: and σ ω I and σ a I represents the covariance matrix of the observation error of the inertial sensor unit corresponding to the angular velocity and acceleration, and I represents the identity matrix; the detection deviation is modeled as a random walk process, in which the Gaussian noise σ bω I and σ ba I represents the covariance of Gaussian noise; Δt is the sampling interval of the inertial sensor unit, exp m (·) represents the matrix exponential, i.e., exp m (a) = exp(A), where matrix A is the antisymmetric matrix corresponding to vector a;

[0020] Define the state at the i-th sampling moment

[0021] The kinetic model can then be expressed as:

[0022] X t+1 =f(X i ,u i ,n i );

[0023] in is the control input at the i-th sampling moment, is the process noise at the i-th sampling moment; the forward evolution law of pose estimation is:

[0024]

[0025] It represents the prior estimate of the pose at the i+1th sampling moment, which is the result of forward evolution; Represents the posterior estimate of the pose at the i-th sampling moment, which is the result after the estimation update.

[0026] Furthermore, the motion compensation of the radar point cloud obtained by laser radar scanning is performed through inverse evolution to obtain the compensated radar point cloud coordinates, which specifically includes:

[0027]

[0028] k = 1, 2, ... is the index of the laser radar scan, each laser radar scan contains n sampling moments, and the nth sampling moment in the kth laser radar scan coincides with the end moment of the kth laser radar scan; represents the estimate of the rotation matrix, represents the bias estimate, I k represents the local coordinate system at the end time of the kth laser radar scan, I j represents the local coordinate system at the jth sampling moment, represents the position of the local coordinate system at the jth sampling time relative to the local coordinate system at the end time of the kth lidar scan, represents the velocity of the local coordinate system at the jth sampling moment relative to the local coordinate system at the end moment of the kth LiDAR scan, and Represents the deviation estimate; at the beginning of reverse evolution, initialize The relative relationship between the body coordinate system of the laser radar at each sampling moment and the local coordinate system at the end of the kth laser radar scan is calculated through reverse evolution. Including relative rotation and relative position; the relative relationship between coordinate systems can be used to realize motion compensation of the radar point cloud obtained by laser radar scanning, that is, the coordinates of the radar point cloud obtained at the jth sampling moment are transformed into the radar coordinate system at the end of the kth laser radar scanning, and the compensated radar point cloud coordinates are obtained:

[0029]

[0030] represents the radar point cloud f j The position in the radar coordinate system at time k, represents the radar point cloud f j The position in the radar coordinate system at the jth sampling moment, is the fixed external parameter between the lidar and the inertial sensor unit, represents the relative direction of the lidar and inertial sensing unit, It represents the relative position of the lidar and the inertial sensor unit, L represents the radar coordinate system, and I represents the local coordinate system.

[0031] Furthermore, constructing the detection residual using the compensated radar point cloud coordinates specifically includes:

[0032] For the corresponding radar point cloud f in the kth lidar scan j The detection residual is constructed as:

[0033]

[0034] is the residual vector consisting of the detection residuals corresponding to all radar point clouds in the kth lidar scan, μ j is the radar point cloud f j Normal vector of the plane, q j are the coordinates of a point on the plane; represents the direction estimate at the end time of the kth lidar scan; represents the position estimate at the end time of the kth lidar scan.

[0035] Furthermore, the single-step iterative update of the pose estimation using the constructed detection residual to obtain a real-time estimation of the pose specifically includes:

[0036]

[0037] in, The posterior pose estimate updated at the end of the k-th laser radar scan, that is, the positioning result at the end of the k-th laser radar scan; is the residual vector consisting of the detection residuals corresponding to all radar point clouds at the end of the k-th laser radar scan; is the covariance matrix of radar Gaussian detection noise; radar observation matrix (·) × It refers to the antisymmetric matrix corresponding to the vector; The estimated covariance calculated for the forward evolution; K k represents the Kalman gain matrix.

[0038] Compared with the prior art, the beneficial technical effects of the present invention are:

[0039] 1. From the perspective of the back-end of the positioning system, the present invention provides an efficient lidar inertial positioning method, which calculates the complete estimate of the position and posture through single-step iterative updates, effectively improving the positioning efficiency.

[0040] 2. The present invention utilizes the invariant extended Kalman filter to fuse sensor information to achieve positioning, avoiding the introduction of linearization errors in the entire posture estimation process, thereby improving efficiency while ensuring positioning accuracy.

[0041] 3. The positioning method proposed in the present invention can realize robot positioning in real time and accurately, and can play an important role in various application fields of mobile robots. BRIEF DESCRIPTION OF THE DRAWINGS

[0042] Figure 1 is a flow chart of an efficient laser radar inertial positioning method according to an embodiment of the present invention;

[0043] Figure 2This is a positioning and mapping effect diagram according to an embodiment of the present invention. DETAILED DESCRIPTION

[0044] Below, embodiments of the present invention will be described with reference to the accompanying drawings. However, it should be understood that these descriptions are exemplary only and are not intended to limit the scope of the present invention. In the following detailed description, for ease of explanation, many specific details are set forth to provide a comprehensive understanding of embodiments of the present invention. However, it is apparent that one or more embodiments may also be implemented without these specific details. In addition, in the following description, descriptions of known structures and technologies are omitted to avoid unnecessary confusion of concepts of the present invention.

[0045] The terms used herein are only for describing specific embodiments and are not intended to limit the present invention. The terms "comprise", "include", etc. used herein indicate the existence of the features, steps, operations and / or components, but do not exclude the existence or addition of one or more other features, steps, operations or components.

[0046] Figure 1 The process of an efficient laser radar inertial positioning method according to an embodiment of the present invention is schematically given.

[0047] like Figure 1 As shown, the laser radar inertial positioning method in the present invention may include steps S101 to S104.

[0048] Step S101, establishing a dynamic model based on inertial sensor unit detection to achieve forward evolution of posture estimation.

[0049] Specifically, based on the angular velocity ω detected by the inertial sensor unit m and acceleration a m , the discrete dynamics model is constructed as:

[0050] R i+1 =R i exp m ((ω m,i -b ω,i -n ω,i )Δt);

[0051] p i+1 =p i +v i Δt;

[0052] v i+1 =v i +[R i (a m,i -b a,i -n a,i )+g]Δt;

[0053] b ω,i+1 =b ω,i +n bω,i Δt;

[0054] b a,i+1 =b a,i +n ba,i Δt;

[0055] Where i is the sampling time of the inertial sensor unit, R∈SO3 and are the rotation matrix and pose vector corresponding to the direction, Indicates the speed of movement. ω and b a is the corresponding detection deviation, n ω and n a is the corresponding Gaussian detection noise, which satisfies and The above detection bias is modeled as a random walk process, where the Gaussian noise Δt is the sampling interval of the inertial sensor unit, exp m (·) represents the matrix exponential, exp m (a) = exp(A), where matrix A is the antisymmetric matrix corresponding to vector a.

[0056] Order Status

[0057] The above kinetic model can be expressed as:

[0058] X i+1 =f(X i ,u i ,u i );

[0059] in is the control input, is the process noise.

[0060] Based on this, the forward evolution law of pose estimation is defined as:

[0061]

[0062] Represents the prior estimate of the pose, which is the result of forward evolution; Represents the posterior estimate of the pose, which is the result after the estimate is updated.

[0063] While performing the forward evolution of the pose estimate, the corresponding error covariance estimate must also be calculated. To this end, the estimated error is defined as

[0064] And according to the Lie index Map it to vector space:

[0065]

[0066] The evolution of the error vector satisfies

[0067]

[0068] in

[0069]

[0070] here,

[0071] Based on the error vector evolution model, the error covariance estimate can be calculated:

[0072]

[0073] in

[0074] Step S102, motion compensation for high-frequency laser radar detection is achieved through reverse updating to avoid motion distortion.

[0075] Specifically, the reverse evolution law is constructed as:

[0076]

[0077] Where k = 1, 2, ... is the index of the LiDAR scan, each LiDAR scan contains n sampling moments, and the nth sampling moment in the kth LiDAR scan coincides with the end moment of the kth LiDAR scan. In the direction update, The relative relationship between the body coordinate system at each sampling moment of the laser radar and the local coordinate system at the end of the kth laser radar scan is calculated through reverse evolution and recorded as Including relative rotation and relative position. The relative relationship between coordinate systems can be used to achieve motion compensation of the radar detection point cloud, that is, the radar point cloud coordinates obtained at the jth radar sampling moment are converted to the radar coordinate system at the end of the kth laser radar scan:

[0078]

[0079] here is the fixed external parameter between the lidar and the inertial sensor unit.

[0080] Step S103, constructing a detection residual using the compensated radar point cloud coordinates for updating the pose estimation.

[0081] Specifically, the detection residual of the jth radar point cloud in the kth lidar scan is constructed as:

[0082]

[0083] ∈ j is the normal vector of the plane to which the radar point cloud belongs, q j is the coordinates of a point on the plane.

[0084] The detection residual can be linearized as

[0085]

[0086] in, Gaussian noise The total number of radar detection points at the end of the kth laser radar scan is recorded as M k , the detection residuals of all points constitute the vector and

[0087]

[0088] in,

[0089] Step S104, using the calculated residual to perform a single-step iterative update of the pose estimation, to obtain a posterior pose estimate, i.e., the corresponding error covariance estimate, which is also commonly referred to as a complete estimate.

[0090] Specifically, using the detection residual of the lidar, the iterative update rule for pose estimation is:

[0091]

[0092] in, is the updated a posteriori pose estimate, that is, the positioning result at the end of the k-th lidar scan; is the covariance matrix of radar Gaussian detection noise. The corresponding error covariance estimation update rule is

[0093]

[0094] Next, the technical solution of the present invention is further described through specific examples.

[0095] A set of data sequences corresponding to a path length of 0.84 km is selected from the public data set M2DGR, and the positioning method of the present invention is run on the sequence to determine the motion trajectory, such as Figure 2 As shown, the motion trajectory can be accurately estimated and the environment map can be constructed based on the trajectory estimation.

[0096] The positioning method of the present invention and other laser radar inertial positioning methods are run simultaneously in the same sequence of the public data set to compare their efficiency and accuracy. As shown in Table 1 and Table 2, the technical solution of the present invention can achieve a positioning accuracy similar to that of other methods with higher efficiency.

[0097] Table 1 System running time comparison table:

[0098]

[0099] In Table 1, sr represents the average running time of the positioning system; ru represents the average total time of residual calculation and estimation update.

[0100] Table 2 Positioning error comparison table:

[0101]

[0102] It is obvious to those skilled in the art that the present invention is not limited to the details of the exemplary embodiments described above, and that the present invention can be implemented in other specific forms without departing from the spirit or essential features of the present invention. Therefore, from any point of view, the embodiments should be regarded as exemplary and non-limiting, and the scope of the present invention is defined by the appended claims rather than the above description, and it is intended that all changes falling within the meaning and scope of the equivalent elements of the claims are included in the present invention, and any reference numerals in the claims should not be regarded as limiting the claims involved.

[0103] In addition, it should be understood that although this specification is described in terms of implementation methods, not every implementation method contains only one independent technical solution. This narrative method of the specification is only for the sake of clarity. Those skilled in the art should regard the specification as a whole. The technical solutions in each embodiment may also be appropriately combined to form other implementation methods that can be understood by those skilled in the art.

Claims

1. An efficient laser radar inertial positioning method, characterized in that: include: A dynamic model is established based on the detection data of the inertial sensor unit to achieve the forward evolution of posture estimation; The motion compensation of the radar point cloud obtained by the laser radar scanning is realized through reverse evolution to obtain the coordinates of the compensated radar point cloud; Construct detection residuals using compensated radar point cloud coordinates; The constructed detection residual is used to perform a single-step iterative update of the pose estimation to obtain a real-time estimate of the pose.

2. The high-efficiency laser radar inertial positioning method according to claim 1, characterized in that: The method of establishing a dynamic model based on the detection data of the inertial sensor unit to realize the forward evolution of the pose estimation specifically includes: The inertial sensor unit detects data including angular velocity and acceleration; The kinetic model is: R i+1 =R i exp m ((ω m,i -b ω,i -n ω,i )Δt); p i+1 =p i +v i Δt; v i+1 =v i +[R i (a m,i -b a,i -n a,i )+g]Δt; b ω,i+1 =b ω,i +n bω,i Δt; b a,i+1 =v a,i +n ba,i Δt; Among them, i represents the sampling time sequence number of the inertial sensor unit, R i ∈SO3 and are the rotation matrix and position vector of the corresponding direction of the i-th sampling moment of the inertial sensor unit, SO3 represents the rotation matrix group, represents the velocity of the inertial sensor unit at the i-th sampling moment; g represents the gravitational acceleration; ω m,i and a m,i is the angular velocity and acceleration detected by the inertial sensor unit at the i-th sampling moment, b ω,i and b a,i They are ω m,i and a m,i The corresponding detection deviation, n ω,i and n a,i They are ω m,i and a m,i The corresponding Gaussian detection noise satisfies: and σ ω I and σ a I represents the covariance matrix of the observation error of the inertial sensor unit corresponding to the angular velocity and acceleration, and I represents the identity matrix; the detection deviation is modeled as a random walk process, in which the Gaussian noise σ bω I and σ ba I represents the covariance of Gaussian noise; Δt is the sampling interval of the inertial sensor unit, exp m (·) represents the matrix exponential, i.e., exp m (a) = exp(A), where matrix A is the antisymmetric matrix corresponding to vector a; Define the state at the i-th sampling moment The kinetic model can then be expressed as: X i+1 =f(X i ,u i ,n i ); in is the control input at the i-th sampling moment, is the process noise at the i-th sampling moment; the forward evolution law of pose estimation is: It represents the prior estimate of the pose at the i+1th sampling moment, which is the result of forward evolution; Represents the posterior estimate of the pose at the i-th sampling moment, which is the result after the estimation update.

3. The high-efficiency laser radar inertial positioning method according to claim 2, characterized in that: The method of performing motion compensation on the radar point cloud obtained by laser radar scanning through reverse evolution to obtain the compensated radar point cloud coordinates specifically includes: k = 1, 2, ... is the index of the laser radar scan, each laser radar scan contains n sampling moments, and the nth sampling moment in the kth laser radar scan coincides with the end moment of the kth laser radar scan; represents the estimate of the rotation matrix, represents the bias estimate, I k represents the local coordinate system at the end time of the kth laser radar scan, I j represents the local coordinate system at the jth sampling moment, represents the position of the local coordinate system at the jth sampling time relative to the local coordinate system at the end time of the kth lidar scan, represents the velocity of the local coordinate system at the jth sampling moment relative to the local coordinate system at the end moment of the kth LiDAR scan, and Represents the deviation estimate; at the beginning of reverse evolution, initialize The relative relationship between the body coordinate system of the laser radar at each sampling moment and the local coordinate system at the end of the kth laser radar scan is calculated through reverse evolution. Including relative rotation and relative position; the relative relationship between coordinate systems can be used to realize motion compensation of the radar point cloud obtained by laser radar scanning, that is, the coordinates of the radar point cloud obtained at the jth sampling moment are transformed into the radar coordinate system at the end of the kth laser radar scanning, and the compensated radar point cloud coordinates are obtained: represents the radar point cloud f j The position in the radar coordinate system at time k, represents the radar point cloud f j The position in the radar coordinate system at the jth sampling moment, is the fixed external parameter between the lidar and the inertial sensor unit, represents the relative direction of the lidar and inertial sensing unit, It represents the relative position of the lidar and the inertial sensor unit, L represents the radar coordinate system, and I represents the local coordinate system.

4. The high-efficiency laser radar inertial positioning method according to claim 3, characterized in that: The method of constructing the detection residual using the compensated radar point cloud coordinates specifically includes: For the corresponding radar point cloud f in the kth lidar scan j The detection residual is constructed as: is the residual vector consisting of the detection residuals corresponding to all radar point clouds in the kth lidar scan, μ j is the radar point cloud f j Normal vector of the plane, q j are the coordinates of a point on the plane; represents the direction estimate at the end time of the kth lidar scan; represents the position estimate at the end time of the kth lidar scan.

5. The high-efficiency laser radar inertial positioning method according to claim 4, characterized in that: The single-step iterative update of the pose estimation using the constructed detection residual to obtain a real-time estimation of the pose specifically includes: in, The posterior pose estimate updated at the end of the k-th laser radar scan, that is, the positioning result at the end of the k-th laser radar scan; is the residual vector consisting of the detection residuals corresponding to all radar point clouds at the end of the k-th laser radar scan; is the covariance matrix of radar Gaussian detection noise; radar observation matrix (·) × It refers to the antisymmetric matrix corresponding to the vector; The estimated covariance calculated for the forward evolution; K k represents the Kalman gain matrix.

Images