Positioning method, device, and medium based on solid-state laser radar and inertial navigation

Abstract

The application provides a positioning method, device and medium based on a solid-state laser radar and inertial navigation, feature points of the solid-state laser radar are extracted based on geometry and intensity by acquiring measurement values of an inertial sensor, and influence of motion blur on collected data is reduced; in a state propagation process of an iterative error Kalman filtering algorithm, the measurement values of the inertial sensor are taken as propagation error states, a discrete propagation equation is constructed, an iterative residual error vector is calculated by using the feature points of the solid-state laser radar, states in the iterative error Kalman filtering algorithm are updated, Kalman gain in the iterative error Kalman filtering algorithm is adjusted, the pose estimation result of the positioning algorithm is effectively improved, the positioning precision and robustness are improved, finally, parameters of the feature points of the solid-state laser radar and the measurement values of the inertial sensor are synthesized based on the iterative error Kalman filtering algorithm, a global pose is updated, and accurate positioning is realized.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of simultaneous localization and mapping, and in particular to a positioning method, device and medium based on solid-state laser radar and inertial navigation. BACKGROUND

[0002] With the rapid progress of artificial intelligence, big data and computer technology, unmanned driving technology has been widely applied in military and civilian fields. Unmanned driving technology is a comprehensive system of sensors, computers, artificial intelligence, communication, navigation and positioning, pattern recognition, machine vision, intelligent control and other frontier disciplines. Many automatic driving algorithms (such as path planning, map construction and motion control) need to use positioning systems to estimate the position information of objects (such as automatic driving vehicles and unmanned aerial vehicles). A good positioning system can effectively help automatic driving vehicles complete automatic driving functions, or effectively help unmanned aerial vehicles complete automatic flight functions, improve safety, and reliable and accurate real-time positioning is the basis for safe operation and high efficiency of automatic driving vehicles and unmanned aerial vehicles.

[0003] Simultaneous Localization and Mapping (SLAM) is a widely used map-aided positioning method in unmanned driving positioning technology. SLAM using laser radar is called laser SLAM, which is the most stable and mainstream positioning and navigation method. Laser radar is a sensor that can accurately obtain high-definition three-dimensional environmental perception information, and has high stability, and can be directly used for obstacle avoidance or positioning and navigation. Laser radar is mainly divided into mechanical laser radar and solid-state laser radar. Mechanical laser radar has the disadvantages of difficulty in loop detection, low stability, and high price. Compared with mechanical laser radar, solid-state laser radar is cheaper and can greatly reduce costs, but due to the mechanism of the non-repeating scanning model, solid-state laser radar will produce more serious motion blur compared with mechanical laser radar, affecting data acquisition effect. Compared with laser radar, inertial sensors (IMU) can obtain angular velocity and acceleration data in three directions, and are only related to the state of the sensor itself, and are not easily affected by the external environment, and can obtain more accurate pose estimation in the short term. However, IMU has data drift phenomenon, and the error will gradually accumulate and become larger over time, and cannot work alone for a long time.

[0004] Therefore, the development of current SLAM technology mainly focuses on the field of multi-sensor fusion. Due to the motion blur problem of the above-mentioned solid-state laser radar and the data drift problem of IMU, the current multi-sensor fusion algorithm has the shortcomings of insufficient positioning accuracy and robustness. SUMMARY

[0005] The application provides a positioning method, device and medium based on a solid-state laser radar and inertial navigation, to solve the defects of insufficient positioning accuracy and robustness of a multi-sensor fusion algorithm in the prior art, realize the advantages of effectively combining various sensors, and improve the positioning accuracy and robustness.

[0006] The application provides a positioning method based on a solid-state laser radar and inertial navigation, comprising:

[0007] Obtaining a measurement value of an inertial sensor, and extracting feature points of a solid-state laser radar based on geometry and intensity;

[0008] In a state propagation process of an iterative error Kalman filter algorithm, the measurement value of the inertial sensor is taken as a propagation error state, and a discrete propagation equation is constructed;

[0009] An iterative residual error vector is calculated by using the feature points of the solid-state laser radar, and the state in the iterative error Kalman filter algorithm is updated;

[0010] The Kalman gain in the iterative error Kalman filter algorithm is adjusted;

[0011] Based on the iterative error Kalman filter algorithm, the parameters of the feature points of the solid-state laser radar and the measurement value of the inertial sensor are synthesized, and a global pose is updated.

[0012] According to the positioning method based on a solid-state laser radar and inertial navigation provided by the application, the data of an original laser point cloud is obtained from the solid-state laser radar, and is preprocessed, and the time t i ∈[t j ,t j+1 ] is taken as a scanning end time t j+1 , all points in the scanning sampled by the solid-state laser radar are projected to the scanning end time t The i-th point is converted into

[0013]

[0014] Wherein, is the original laser point cloud, W is a world coordinate system, I is an inertial sensor coordinate system, and L is a laser radar coordinate system;

[0015] A local patch is assigned to each candidate point, and geometric plane points are extracted according to the distance of the candidate point relative to the fitting plane, the candidate point being a point in the original laser point cloud; a local patch is assigned to each candidate point A processed point cloud is given The point cloud Perform temporal sorting to determine the direction of the extended patches, for each of the local patches. There exists a fitting plane Π(x,y,z) for each case, and the fitting plane is represented as:

[0016]

[0017] Where, n x n y n z , These are planar parameters.

[0018]

[0019] in, It is the normal vector of the fitted plane. yes The center of mass, N is The number of midpoints, the distance of all points relative to the fitted plane. If all values ​​are less than one-tenth of the average patch size, then the local patch is determined to be... All points in the equation are geometric plane points;

[0020] Geometric edge points are extracted based on the smoothness level, and the intensity difference between two adjacent geometric edge points is calculated. Geometric edge points whose intensity difference exceeds one-tenth of the maximum intensity reading are selected as intensity edge points. For each patch that does not meet the planar function requirements in equation (3), multiple geometric edge points p are extracted in descending order of smoothness. i ∈GeE, point p i p on the same line i+1 The intensity difference ΔI between points is expressed as:

[0021]

[0022] According to the positioning method based on solid-state lidar and inertial navigation provided by the present invention, during the state propagation process of the iterative error Kalman filter algorithm, the measured value of the inertial sensor is used as the propagation error state to construct a discrete propagation equation, specifically including:

[0023] During state propagation, when a new measurement value from the inertial sensor arrives, the propagation error state δx and the error state covariance matrix P are... k and state prior

[0024] The linearized continuous-time model of the inertial sensor error state is as follows:

[0025]

[0026] in, It is a Gaussian noise vector, F t and G t Let be the error state transition matrix and the noise Jacobian matrix at time t. The propagation equation obtained from equation (5) is:

[0027]

[0028]

[0029] Where, Δt=t τ -t τ-1 , t τ and t τ-1 Let w be the continuous inertial sensor time step, and Q represent the covariance matrix of w.

[0030] According to the positioning method based on solid-state lidar and inertial navigation provided by the present invention, the iterative residual vector is calculated using the feature points of the solid-state lidar, and the state in the iterative error Kalman filter algorithm is updated. Specifically, the method includes:

[0031] In the iterative error Kalman filter algorithm, based on prior... The deviation and the residual function f(·) derived from the measurement model link the state update to the optimization problem:

[0032]

[0033] Where ||·|| is the Markov norm, J k Let f(·) be the Jacobian matrix of the measurement noise, and M be the Jacobian matrix of the measurement noise. k To measure the covariance matrix of the noise, the output of f(·) is the superimposed residual vector calculated from point-surface or point-edge pairs; For L k+1 The i-th feature point in the given... Then f(·) corresponds to The error term is described as follows:

[0034]

[0035]

[0036] in, for From L k+1 To L k The transformation point, and This represents the external parameters between the lidar and the IMU;

[0037] Solve equation (8) using the following iterative update:

[0038]

[0039]

[0040] delta x j+1 = delta x j + delta x j (13)

[0041] where delta x j is the correction vector at the jth iteration, H k,j is the Jacobian matrix with respect to delta x j ;

[0042] Initialize the next state

[0043]

[0044] where q0 represents a unit quaternion, and are calculated from and respectively.

[0045] According to the positioning method based on solid-state laser radar and inertial navigation provided by the application, the Kalman gain in the iterative error Kalman filtering algorithm is adjusted, and specifically includes:

[0046] The measurement value of the inertial sensor is used to define a parameter X m·k , and X m·k is divided into X static·k and X dynamic·k according to static and dynamic conditions.

[0047] The Baum-Welch reestimation algorithm is used to model X static·k to obtain a hidden Markov model lambda0.

[0048] The Viterbi algorithm is used to calculate the probability of X m·k generated by lambda0, and is normalized, and the Kalman gain is adjusted according to the calculated probability.

[0049] According to the positioning method based on solid-state laser radar and inertial navigation provided by the application, the measurement value of the inertial sensor is used to define a parameter X m·k , and X m·k is divided into X static·k and X dynamic·k according to static and dynamic conditions, and specifically includes:

[0050] The following formula is defined:​

[0051]

[0052] where the measurement of the inertial sensor at sampling time i is constructed as a m·i ;

[0053] define X m·k immobile in the time interval [k-N+1, k] static·k , define X m·k moving in the time interval [k-N+1, k] dynamic·k ; the Hidden Markov model is symbolizable as λ = (A, B, π), where A = {a ij}, 1≤i,j≤M are the transition probability distributions, B = {b j (X m·k )}, 1≤j≤M, 1≤k≤D are the observation probability distributions, and π = {π i}, 1≤i≤M are the initial state probability distributions;

[0054] The observation probability density function of state s j is defined as:

[0055]

[0056] where F(X m·k , μ jr , Σ jr ), 1≤j≤M, 1≤r≤L are Gaussian functions, μ jr and Σ jr are the mean and covariance of the rth Gaussian function, and c jr is the weight assigned to each Gaussian function.

[0057] According to the positioning method based on solid-state laser radar and inertial navigation provided by the application, the Baum-Welch re-estimation algorithm is used to model X static·k to obtain a Hidden Markov model λ0, which specifically includes:

[0058] S4021, set initial conditions for a c jr , μ jr , Σ jr and π i , and the constraint conditions are c jr ≥ 0, π i ≥ 0, where a represents the transition probability from state i to state j;

[0059] S4022, re-estimate the new model from the current estimated value of the model parameters through equations (18) to (23)

[0060]

[0061]

[0062]

[0063]

[0064]

[0065]

[0066] S4023, Calculation exist In this case, set And return to step S4021, in In this case, set ε = 10 -7 .

[0067] According to the positioning method based on solid-state lidar and inertial navigation provided by the present invention, the Viterbi algorithm is used to calculate λ0 to generate X. m·k The probability is calculated and normalized. The Kalman gain is then adjusted based on the calculated probability, specifically including:

[0068] Modify the Viterbi algorithm:

[0069]

[0070] Where α1(i)=π i b i (X m·1 );

[0071] calculate

[0072]

[0073] Let P(X) static·k |λ0)≈1,P(X dynamic·k |λ0) is close to 0, that is, 0≤P(X) m·k |λ0)≤1, normalized λ0 input:

[0074]

[0075] K k Adjustment mode set to:

[0076]

[0077] wherein, Q is a covariance matrix of process noise in the iterative error Kalman filtering algorithm, and M is a covariance matrix of observation noise in the iterative error Kalman filtering algorithm.

[0078] The application further provides an electronic device, comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor implements the positioning method based on solid-state laser radar and inertial navigation according to any one of the above when executing the program.

[0079] The application further provides a non-transitory computer-readable storage medium having a computer program stored thereon, wherein the computer program is executable on a processor to implement the positioning method based on solid-state laser radar and inertial navigation according to any one of the above.

[0080] The positioning method based on solid-state laser radar and inertial navigation, the device and the medium provided by the application reduce the influence of motion blur on the collected data by obtaining the measurement value of the inertial sensor and extracting feature points of the solid-state laser radar based on geometry and intensity; in the state propagation process of the iterative error Kalman filtering algorithm, the measurement value of the inertial sensor is taken as a propagation error state to construct a discrete propagation equation, the feature points of the solid-state laser radar are used to calculate an iterative residual vector, the state in the iterative error Kalman filtering algorithm is updated, the Kalman gain in the iterative error Kalman filtering algorithm is adjusted, the pose estimation result of the positioning algorithm is effectively improved, the positioning accuracy and robustness are improved, and finally the parameters of the feature points of the solid-state laser radar and the measurement value of the inertial sensor are synthesized based on the iterative error Kalman filtering algorithm to update the global pose, thereby realizing accurate positioning. BRIEF DESCRIPTION OF DRAWINGS

[0081] In order to more clearly illustrate the technical solutions of the present application or the prior art, the following will briefly introduce the drawings needed in the embodiments or prior art description. Obviously, the drawings in the following description are some embodiments of the present application, and those skilled in the art can also obtain other drawings according to these drawings without creative labor.

[0082] Figure 1 is a flowchart of the positioning method based on solid-state laser radar and inertial navigation provided by the application;

[0083] Figure 2 is a comparison diagram of the estimated trajectory and the ground true value of the positioning method based on solid-state laser radar and inertial navigation and the comparative algorithm in the embodiment of the application on the Tree sequence of the data set FR-IOSB;

[0084] Figure 3 is Figure 2 is a schematic diagram of trajectory 1 in

[0085] Figure 4 This is a schematic diagram of the structure of the electronic device provided by the present invention. Detailed Implementation

[0086] To make the objectives, technical solutions, and advantages of this invention clearer, the technical solutions of this invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some, not all, of the embodiments of this invention. All other embodiments obtained by those skilled in the art based on the embodiments of this invention without creative effort are within the scope of protection of this invention.

[0087] The following is combined Figures 1-4 This invention describes a positioning method based on solid-state lidar and inertial navigation.

[0088] like Figure 1 As shown, the positioning method based on solid-state lidar and inertial navigation provided by the present invention includes the following steps:

[0089] S1. Obtain the measurement values ​​from the inertial sensor and extract the feature points of the solid-state lidar based on geometry and intensity;

[0090] S2. In the state propagation process of the iterative error Kalman filter algorithm, the measured value of the inertial sensor is used as the propagation error state, and a discrete propagation equation is constructed.

[0091] S3. Calculate the iterative residual vector using the feature points of the solid-state lidar, and update the state in the iterative error Kalman filter algorithm.

[0092] S4. Adjust the Kalman gain in the iterative error Kalman filter algorithm;

[0093] S5. Based on the iterative error Kalman filter algorithm, the parameters of the feature points of the solid-state lidar and the measurement values ​​of the inertial sensor are combined to update the global pose.

[0094] It should be understood that, such as Figure 1 As shown, there is no specific temporal relationship between steps S2 and S3 and step S4.

[0095] In an optional embodiment of the present application, the inertial sensor includes an accelerometer (or acceleration sensor) and an angular velocity sensor (gyroscope) and their single, double and triple axis combined inertial measurement unit (IMU), and the solid-state laser radar can use the solid-state laser radar Livox Avia, which has the characteristics of long range, high precision, wide viewing angle, light weight and high reliability, and is widely used in surveying and mapping, vehicle-to-everything (V2X), robots and other fields.

[0096] In step S1, the feature points of the solid-state laser radar are extracted based on geometry and intensity, specifically including the following steps:

[0097] S101, acquire the data of the original laser point cloud from the solid-state laser radar and perform preprocessing.

[0098] When the carrier moves, motion blur has a significant impact on the positioning and mapping performance of the solid-state laser radar on the carrier. Compared with the traditional rotating laser radar, the solid-state laser radar produces more serious motion blur due to the mechanism of the non-repeating scanning model. In order to compensate for the motion, the time t i ∈[t j ,t j+1 ] is projected to the end time t j+1 of the scan sampled by the solid-state laser radar. Assuming that is the original laser point cloud, W is the world coordinate system, I is the IMU coordinate system, and L is the laser radar coordinate system, search for the closest transformation matrix (from the coordinate system I to the coordinate system W at time t i ) for each point in the time domain, then the i-th point will be converted to

[0099]

[0100] S102, assign a local patch to each candidate point, and extract geometric plane points according to the distance of the candidate point relative to the fitted plane.

[0101] The candidate point is a point in the original laser point cloud, and a local patch is assigned to each candidate point before the feature extraction process Given a processed point cloud , sort the point cloud in time domain to determine the direction of the extended patch. First, use the nearest neighbor search strategy to ensure that the surrounding points are included in the patch. In addition, if the selected points are not enough, search extra on each scan line to obtain a certain number of points. In order to determine whether the candidate points all belong to plane points, assume that each local patch There exists a fitting plane Π(x,y,z) for each case, which is represented as:

[0102]

[0103] Where, n x n y n z , These are planar parameters, calculated as follows:

[0104]

[0105] in, It is the normal vector of the fitted plane. yes The center of mass, N is The number of midpoints, the distance of all points relative to the fitted plane. If all values ​​are less than one-tenth of the average patch size (assuming the average patch size is 1m), then Determine local patches All points in the array are geometric plane points (GeP).

[0106] S103. Extract geometric edge points based on the smoothness, calculate the intensity difference between two adjacent geometric edge points, and select the geometric edge point whose intensity difference exceeds one-tenth of the maximum intensity reading as the intensity edge point.

[0107] When extracting edge points, not only geometric information is used, but also intensity is used to weigh the difference between candidate points and surrounding points. For each patch that does not meet the requirements of the planar function in equation (3), multiple geometric edge points p with higher smoothness are extracted in descending order of smoothness. i For GeE, the smoothness is defined as follows:

[0108]

[0109] Where k represents the k-th scan. In an optional embodiment of this application, For the distance point p in the k-th scan i The set of the ten most recent points. In an optional embodiment of the invention, in each scan, all points are sorted by their s values, and the points with the highest s values, which make up one-third of the total number of points (rounded to the nearest whole number, for example, the three points with the highest s values ​​in a set of ten points), are selected as geometric edge points.

[0110] The color of the target surface affects the reflection intensity I i The effect depends not only on the color and material of the target object's surface, but also on the incident angle θ. iThis is related to the distance to the solid-state lidar. The normal vector corresponding to each patch is obtained through the process of extracting planar points. Therefore, point p i p on the same line i+1 The intensity difference ΔI between points is expressed as:

[0111]

[0112] Points where the intensity difference ΔI exceeds one-tenth of the maximum intensity reading (i.e., ΔI>25) are intensity edge points (InE).

[0113] In an optional embodiment of the present invention, step S2, during the state propagation process of the iterative error Kalman filter algorithm, uses the measured value of the inertial sensor as the propagation error state to construct a discrete propagation equation, specifically including:

[0114] Let W be a fixed world coordinate system, I k Let L be the inertial sensor coordinate system for the k-th lidar time step. k Let be the lidar coordinate system for the k-th lidar time step. For W relative to I k Location, To describe I k+1 To I k The local state of a relative transformation is defined as follows:

[0115]

[0116]

[0117] in, This is W relative to I k Location, It describes W to I k Rotational unit quaternion. and Indicate I k+1 To I k Translation and rotation, It's about I k speed, b a It is acceleration deviation, b g It's gyroscope bias. Local gravity. (in I k (The symbol in the middle) is also part of the local state.

[0118] To ensure that the state estimation has good properties, this invention employs the error-state representation method for solution. definition The error vector is δx:

[0119] δx:=[δp,δv,δθ,δb a ,δb g ,δg]

[0120] where δ denotes the error term, and δθ is the 3-DOF error angle.

[0121] According to the tradition of the Error State Kalman Filter (ESKF), once δx is solved, the final state can be obtained by injecting δx into the prior state of This is achieved by the operator , which is defined as:

[0122]

[0123] where denotes the quaternion multiplication, and exp: is the mapping of the angle vector to the quaternion rotation.

[0124] During the state propagation, when there is a new measurement of the inertial sensor, the propagation error state δx, the error state covariance matrix P k and the state prior are updated as follows:

[0125]

[0126] where is the Gaussian noise vector, F t and G t are the error state transition matrix and the noise Jacobian matrix at time t, and the propagation equation obtained from equation (5) is:

[0127]

[0128]

[0129] where Δt = t τ -t τ-1 , t τ and t τ-1 are the continuous inertial sensor time steps, and Q denotes the covariance matrix of w, which is calculated discretely during the calibration of the inertial sensor.

[0130] In an optional embodiment of the present application, step S3, the feature point of the solid-state laser radar is used to calculate the iterative residual error vector, and the state in the iterative error Kalman filter algorithm is updated, specifically including:

[0131] In iterative error Kalman filtering, based on prior... The deviation and the residual function f(·) derived from the measurement model link the state update to the optimization problem:

[0132]

[0133] Where ||·|| is the Markov norm, J k Let f(·) be the Jacobian matrix of the measurement noise, and M be the Jacobian matrix of the measurement noise. k The covariance matrix for measuring noise. The output of f(·) is the superimposed residual vector calculated from point-to-surface or point-to-edge pairs; For L k+1 The i-th feature point in the given... Then f(·) corresponds to The error term is described as follows:

[0134]

[0135]

[0136] in, for From L k+1 To L k The transformation point, and This represents the external parameters between the lidar and the IMU. Specifically, in one optional embodiment of this application, This represents the error in angular rotation between the lidar and the inertial sensor. This represents the translational error between the lidar and the inertial sensor.

[0137] Solve equation (8) using the following iterative update:

[0138]

[0139]

[0140] δx j+1 =δx j +Δx j (13)

[0141] Where, Δx j H is the correction vector at the j-th iteration. k,j yes Regarding δx j The Jacobian matrix;

[0142] Initialize the next state

[0143]

[0144] where q0denotes the unit quaternion, and are calculated by and respectively.

[0145] In an optional embodiment of the present application, step S4, adjusting the Kalman gain in the iterative error Kalman filtering algorithm, specifically includes:

[0146] S401, defining a parameter X m·k using the measurement value of the inertial sensor m·k and dividing X static·k into X dynamic·k and X m·k according to static and dynamic conditions.

[0147] where the parameter X m·k is mainly defined using the measurement value of the accelerometer in the inertial sensor.

[0148] Specifically, in step S401, the following formula is first defined:

[0149]

[0150] where the measurement of the inertial sensor at the sampling time i is constructed as a m·i ;

[0151] X m·k that is not moving in the time interval [k-N+1, k] is defined as X static·k , and X m·k that is moving in the time interval is defined as X dynamic·k .

[0152] The Hidden Markov Model (HMM) is symbolized as λ=(A, B, π), where A={φ ij}, 1≤i, j≤M is the transition probability distribution, also known as the transition matrix; B={b j (X m·k )}, 1≤j≤M, 1≤k≤D is the observation probability distribution, also known as the emission matrix; π={π i}, 1≤i≤M is the initial state probability distribution. Considering that X m·k is a continuous variable, a Continuous Hidden Markov Model (CHMM) is adopted.

[0153] The observation probability density function of state s j is defined as the observation probability density function of state s j .

[0154]

[0155] where F(X m·k ,μ jr ,Σ jr ),1≤j≤M,1≤r≤L are Gaussian functions, μ jr and Σ jr are the mean and covariance of the r-th Gaussian function, c jr is the weight assigned to each Gaussian function.

[0156] S402、adopting Baum-Welch re-estimation algorithm to model X static·k to obtain a hidden Markov model λ0.

[0157] Specifically, step S402 includes the following steps:

[0158] S4021、setting initial conditions for c c jr ,μ jr ,Σ jr and π i , with constraints being c jr ≥0, π i ≥0, where denotes the transition probability from state i to state j.

[0159] In step S4021, initial conditions are set for unknown quantities, including c c jr ,μ jr ,Σ jr and π i . Wherein denotes the transition probability from state i to state j. These quantities can be randomly set, but with constraints being c jr ≥0, π i ≥0,

[0160] S4022、re-estimating new model parameters from the current estimated values of model parameters by equations (18) to (23)

[0161]

[0162]

[0163]

[0164]

[0165]

[0166]

[0167] S4023, Calculation exist In this case, set And return to step S4021, in In this case, set ε = 10 -7 .

[0168] S403. Use the Viterbi algorithm to calculate λ0 and generate X. m·k The probability is calculated and normalized, and the Kalman gain is adjusted based on the calculated probability.

[0169] The Viterbi algorithm is a dynamic programming algorithm used to find the Viterbi path—the sequence of hidden states—most likely to produce the sequence of observed events, especially in the context of Markov information sources and Hidden Markov Models (HMMs). Since HMMs are sensitive to changes in the input, a simple modification to Viterbi is made to suppress changes in the HMM output when the carrier is stationary, changes caused by sensor output noise.

[0170] The Viterbi algorithm is modified as follows:

[0171]

[0172] Where α1(i)=π i b i (X m·1 );

[0173] calculate

[0174]

[0175] Let P(X) static·k |λ0)≈1,P(X dynamic·k |λ0) is close to 0, that is, 0≤P(X) m·k |λ00)≤1, normalized λ0 input:

[0176]

[0177] K k Adjustment mode set to:

[0178]

[0179] Wherein, Q is the covariance matrix of the process noise in the iterative error Kalman filtering algorithm, and M is the covariance matrix of the observation noise in the iterative error Kalman filtering algorithm.

[0180] In an optional embodiment of the present application, step S5, based on the iterative error Kalman filtering algorithm, synthesizes the parameters of the feature points of the solid-state laser radar and the measurement values of the inertial sensor, and updates the global pose, specifically comprising:

[0181] After the state update in the iterative error Kalman filtering algorithm, the global pose is updated through the following synthesis steps

[0182]

[0183] In an optional embodiment provided by the present application, the open source dataset FR-IOSB and the ROS unmanned vehicle platform are used to implement the above-mentioned positioning method based on the solid-state laser radar and the inertial navigation. Figure 2 Fig. 1 shows the estimated trajectory and ground truth comparison chart of the positioning method based on the solid-state laser radar and the inertial navigation provided by the present application and the comparative algorithm on the Tree sequence of the dataset FR-IOSB, Figure 2 LI in Fig. 1 indicates the positioning method based on the solid-state laser radar and the inertial navigation provided by the present application, LINS, LeGO-LOAM and LH-LOAM are comparative algorithms. Table 1 is the trajectory average error and angle average error of the positioning method based on the solid-state laser radar and the inertial navigation provided by the present application and the comparative algorithm on the Tree sequence of the dataset FR-IOSB. From Figure 2 As can be seen from Fig. 1 and Table 1, the trajectory of the positioning method based on the solid-state laser radar and the inertial navigation provided by the present application is closer to the ground truth (i.e. Figure 2 in Fig. 1), and has the smallest error in most directions, and the positioning is more accurate. When tested using an unmanned vehicle, the ROS unmanned vehicle is equipped with hardware devices such as a solid-state laser radar, an inertial sensor and a NANO development board, and the unmanned vehicle moves according to the program instructions through remote control on the computer side.

[0184] Table 1: Trajectory average error and angle average error of the positioning method based on the solid-state laser radar and the inertial navigation provided by the present application and the comparative algorithm on the Tree sequence of the dataset FR-IOSB

[0185]

[0186]

[0187] In the motion process of the unmanned vehicle, the solid-state laser radar and the inertial sensor respectively collect laser point cloud, gyroscope and acceleration data, and through preprocessing, feature extraction and pose estimation algorithms, the final unmanned vehicle pose is obtained, and a path trajectory curve is drawn, Figure 3 A path trajectory curve is shown in the figure as an example, that is, trajectory 1 in Table 2. Table 2 is the CPU occupancy and average value of the algorithm and the comparative algorithm of the present application under four different trajectories, and LI in Table 2 represents the algorithm of the present application, and LH-LOAM is the comparative algorithm. As can be seen from the table, the CPU occupancy of the algorithm of the present application is much lower than that of the comparative algorithm under all trajectories.

[0188] Table 2: CPU occupancy and average value of the positioning method based on solid-state laser radar and inertial navigation provided by the present application and the comparative algorithm under four different trajectories

[0189] Trajectory sequence 1 2 3 4 Average LH-LOAM / % 34.568 34.972 35.803 33.967 34.828 LI / % 25.16 25.92 26.70 24.95 25.68

[0190] It should be understood that the above example is only a feasible application mode of the positioning method based on solid-state laser radar and inertial navigation provided by the present application, and is not limited to the method provided by the present application only being used for unmanned vehicle platform. The method provided by the present application can be applied to various carriers, such as various vehicles, unmanned aerial vehicles and the like which need to have automatic driving function.

[0191] The positioning method based on solid-state laser radar and inertial navigation provided by the present application reduces the influence of motion blur on the collected data by obtaining the measurement value of the inertial sensor and extracting the feature points of the solid-state laser radar based on geometry and intensity. In the state propagation process of the iterative error Kalman filtering algorithm, the measurement value of the inertial sensor is taken as the propagation error state to construct a discrete propagation equation, the iterative residual vector is calculated using the feature points of the solid-state laser radar, the state in the iterative error Kalman filtering algorithm is updated, the Kalman gain in the iterative error Kalman filtering algorithm is adjusted, the pose estimation result of the positioning algorithm is effectively improved, the positioning accuracy and robustness are improved, and finally the parameters of the feature points of the solid-state laser radar and the measurement value of the inertial sensor are synthesized based on the iterative error Kalman filtering algorithm to update the global pose and realize accurate positioning.

[0192] Figure 4 An example of an entity structure diagram of an electronic device is shown in FIG. Figure 4As shown, the electronic device can include a processor 410, a communications interface 420, a memory 430, and a communications bus 440, wherein the processor 410, the communications interface 420, and the memory 430 complete mutual communication through the communications bus 440. The processor 410 can invoke a logic instruction in the memory 430 to execute a positioning method based on a solid-state laser radar and inertial navigation, which includes:

[0193] Obtaining a measurement value of an inertial sensor, and extracting a feature point of the solid-state laser radar based on geometry and intensity;

[0194] In a state propagation process of an iterative error Kalman filtering algorithm, the measurement value of the inertial sensor is taken as a propagation error state to construct a discrete propagation equation;

[0195] An iterative residual vector is calculated using the feature point of the solid-state laser radar to update a state in the iterative error Kalman filtering algorithm;

[0196] The Kalman gain in the iterative error Kalman filtering algorithm is adjusted;

[0197] Based on the iterative error Kalman filtering algorithm, parameters of the feature point of the solid-state laser radar and the measurement value of the inertial sensor are synthesized to update a global pose.

[0198] In addition, the logic instruction in the memory 430 described above can be implemented in the form of a software function unit and sold or used as an independent product, and can be stored in a computer readable storage medium. Based on such understanding, the technical solutions of the present application essentially or the part that contributes to the prior art or part of the technical solutions can be embodied in the form of a software product, and the computer software product is stored in a storage medium, including a plurality of instructions to make a computer device (which can be a personal computer, a server, or a network device, etc.) execute all or part of the steps of the method described in various embodiments of the present application. The foregoing storage medium includes: a U disk, a mobile hard disk, a read-only memory (ROM, Read-Only Memory), a random access memory (RAM, Random Access Memory), a magnetic disk or an optical disk, and various media that can store program codes.

[0199] On the other hand, the present application also provides a computer program product, which includes a computer program, the computer program can be stored on a non-transitory computer readable storage medium, and the computer program is executed by a processor, and the computer can execute the positioning method based on the solid-state laser radar and the inertial navigation provided by the above-mentioned method, which includes:

[0200] obtaining a measurement value of an inertial sensor, and extracting feature points of a solid-state laser radar based on geometry and intensity;

[0201] during state propagation of an iterative error Kalman filtering algorithm, taking the measurement value of the inertial sensor as a propagation error state, and constructing a discrete propagation equation;

[0202] calculating an iterative residual vector by using the feature points of the solid-state laser radar, and updating a state in the iterative error Kalman filtering algorithm;

[0203] adjusting a Kalman gain in the iterative error Kalman filtering algorithm;

[0204] based on the iterative error Kalman filtering algorithm, synthesizing parameters of the feature points of the solid-state laser radar and the measurement value of the inertial sensor, and updating a global pose.

[0205] In another aspect, the present application also provides a non-transitory computer readable storage medium having a computer program stored thereon, which, when executed by a processor, implements a positioning method based on a solid-state laser radar and inertial navigation provided by the above method, and the method comprises:

[0206] obtaining a measurement value of an inertial sensor, and extracting feature points of a solid-state laser radar based on geometry and intensity;

[0207] during state propagation of an iterative error Kalman filtering algorithm, taking the measurement value of the inertial sensor as a propagation error state, and constructing a discrete propagation equation;

[0208] calculating an iterative residual vector by using the feature points of the solid-state laser radar, and updating a state in the iterative error Kalman filtering algorithm;

[0209] adjusting a Kalman gain in the iterative error Kalman filtering algorithm;

[0210] based on the iterative error Kalman filtering algorithm, synthesizing parameters of the feature points of the solid-state laser radar and the measurement value of the inertial sensor, and updating a global pose.

[0211] The device embodiments described above are only schematic, wherein the units shown as separate components can or can not be physically separate, and the components shown as units can or can not be physical units, i.e., can be located in one place, or can be distributed on multiple network units. Part or all of the modules can be selected according to actual needs to achieve the purpose of the present embodiment. Those skilled in the art can understand and implement without creative labor.

[0212] Those skilled in the art can clearly understand the implementation of the various embodiments by means of software and necessary general hardware platforms through the description of the above embodiments, and of course, the embodiments can also be implemented by hardware. Based on such understanding, the above technical solutions can be embodied in the form of a software product, and the computer software product can be stored in a computer readable storage medium, such as a ROM / RAM, a magnetic disk, an optical disk, etc., and includes a plurality of instructions for causing a computer device (which can be a personal computer, a server, or a network device, etc.) to execute the method described in each embodiment or some parts of the embodiment.

[0213] Finally, it should be noted that: the above examples are only used to illustrate the technical solutions of the present application, and not to limit them; although the present application has been described in detail with reference to the foregoing examples, those skilled in the art should understand that: it can still modify the technical solutions recorded in the foregoing examples, or make equivalent replacement for some technical features thereof; and these modifications or replacements do not make the essence of the corresponding technical solutions deviate from the spirit and scope of the technical solutions of the embodiments of the present application.

Claims

1. A positioning method based on solid-state lidar and inertial navigation, characterized in that, include: Acquire measurements from inertial sensors and extract feature points from solid-state lidar based on geometry and intensity. In the state propagation process of the iterative error Kalman filter algorithm, the measured value of the inertial sensor is used as the propagation error state to construct a discrete propagation equation; The iterative residual vector is calculated using the feature points of the solid-state lidar, and the state in the iterative error Kalman filter algorithm is updated. Adjust the Kalman gain in the iterative error Kalman filter algorithm; Based on the iterative error Kalman filter algorithm, the parameters of the feature points of the solid-state lidar and the measurement values ​​of the inertial sensor are combined to update the global pose; In the state propagation process of the iterative error Kalman filter algorithm, the measured values ​​of the inertial sensor are used as the propagation error state to construct a discrete propagation equation, specifically including: During state propagation, if a new measurement value from the inertial sensor arrives, the propagation error state... Error state covariance matrix and state prior ; The linearized continuous-time model of the inertial sensor error state is as follows: （5） in, It is a Gaussian noise vector. and for The error state transition matrix and noise Jacobian matrix at time t are given by the propagation equation obtained from equation (5): （6） （7） in, , and For continuous inertial sensor time steps, express The covariance matrix; Specifically, adjusting the Kalman gain in the iterative error Kalman filter algorithm includes: Parameters are defined using the measurements from the inertial sensor. And based on static and dynamic situations, Divided into and ; The Baum-Welch re-estimation algorithm was used to... Modeling is performed to obtain a Hidden Markov Model. ; Calculate using the Viterbi algorithm produce The probability is calculated and normalized, and the Kalman gain is adjusted based on the calculated probability.

2. The positioning method based on solid-state lidar and inertial navigation according to claim 1, characterized in that, Feature points of solid-state lidar are extracted based on geometry and intensity, specifically including: The raw laser point cloud data is acquired from the solid-state lidar and preprocessed, with time... All points sampled by the solid-state lidar described herein are projected to the end time of the scan. Search for the closest transformation matrix for each point in the time domain. , will the Points Convert to , （1） in, The original laser point cloud, Using the world coordinate system, For the inertial sensor coordinate system, For the lidar coordinate system; A local patch is assigned to each candidate point, and geometric plane points are extracted based on the distance of the candidate point relative to the fitted plane. The candidate points are points in the original laser point cloud. A local patch is assigned to each candidate point. Given a processed point cloud For the point cloud Perform temporal sorting to determine the direction of the extended patches, for each of the local patches. There exists a fitting plane for each. The fitting plane is represented as: （2） in, , , , These are planar parameters. （3） in, It is the normal vector of the fitted plane. yes The center of mass, yes The number of midpoints, the distance of all points relative to the fitted plane. If all values ​​are less than one-tenth of the average patch size, then the local patch is determined to be... All points in the equation are geometric plane points; Geometric edge points are extracted based on the smoothness level, and the intensity difference between two adjacent geometric edge points is calculated. Geometric edge points with an intensity difference exceeding one-tenth of the maximum intensity reading are selected as intensity edge points. For each patch that does not meet the planar function requirements in equation (3), multiple geometric edge points are extracted in descending order of smoothness. ,point and on the same straight line Intensity difference between points Represented as: （4）。 3. The positioning method based on solid-state lidar and inertial navigation according to claim 1, characterized in that, The iterative residual vector is calculated using the feature points of the solid-state lidar, and the state in the iterative error Kalman filter algorithm is updated accordingly, specifically including: In the iterative error Kalman filter algorithm, based on prior... The deviation and the residual function derived from the measurement model This connects state updates with optimization problems: （8） in, For Markov normal, for Regarding the Jacobian matrix of measurement noise To measure the covariance matrix of the noise, The output is a superimposed residual vector calculated from point-face or point-edge pairs; for The first in Given feature points, ,but Corresponding to The error term is described as follows: （9） （10） in, for from arrive The transformation point, and This represents the external parameters between the lidar and the IMU; Solve equation (8) using the following iterative update equation: （11） （12） （13） in, This is the correction vector for the j-th iteration. yes about The Jacobian matrix; Initialize the next state : （14） in, Represents a unit quaternion. and respectively by and calculate.

4. The positioning method based on solid-state lidar and inertial navigation according to claim 1, characterized in that, Parameters are defined using the measurements from the inertial sensor. And based on static and dynamic situations, Divided into and Specifically, it includes: Define the following formula: （16） Among them, the inertial sensor in sampling time The measurement at the location is constructed as ; Defined in time interval Immovable inside for Defined as moving within a time interval for Hidden Markov Models can be symbolized as ,in For the transition probability distribution, To observe the probability distribution, The initial probability distribution; state The observation probability density function is defined as: （17） in It is a Gaussian function. and These are the mean and covariance of the r-th Gaussian function. These are the weights assigned to each Gaussian function.

5. The positioning method based on solid-state lidar and inertial navigation according to claim 4, characterized in that, The Baum-Welch re-estimation algorithm was used to... Modeling is performed to obtain a Hidden Markov Model. Specifically, it includes: S4021, to , , , and Set initial conditions and constraints as follows: , , , , , ,in Representing state to state The transition probability; S4022. From the current estimates of the model parameters, re-estimate the new model using equations (18) to (23). , （18） （19） （20） （21） （22） （23）； S4023, Calculation ,exist In this case, set And return to step S4021, in In this case, set , .

6. The positioning method based on solid-state lidar and inertial navigation according to claim 5, characterized in that, Calculate using the Viterbi algorithm produce The probability is calculated and normalized. The Kalman gain is then adjusted based on the calculated probability, specifically including: Modify the Viterbi algorithm: （24） in, ; calculate : （25） make , Close to 0, that is Normalization Input: （26） Adjustment mode set to: （27） in, Let $\mathbf{x}$ be the covariance matrix of the process noise in the iterative error Kalman filter algorithm. Let be the covariance matrix of the observation noise in the iterative error Kalman filter algorithm.

7. An electronic device comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, characterized in that, When the processor executes the program, it implements the positioning method based on solid-state lidar and inertial navigation as described in any one of claims 1 to 6.

8. A non-transitory computer-readable storage medium having a computer program stored thereon, characterized in that, When the computer program is executed by the processor, it implements the positioning method based on solid-state lidar and inertial navigation as described in any one of claims 1 to 6.

Images