EKF-based slam front-end strategy with multi-source information fusion

Abstract

The present application is directed to the problem of scan matching relying on initial value in laser radar front-end subgraph construction, and proposes a multi-source information fusion algorithm based on extended Kalman filter. The EKF is fused with the wheel encoder and IMU pre-integration, and the obtained mileage pose (position + attitude) is used to obtain the fused pose and covariance matrix, and the fused pose is subjected to quaternion spherical interpolation to remove the laser radar motion distortion. The present application also improves the matching strategy between frames and subgraphs by using the strategy of scan matching between the current laser radar data frame and the related local map, and optimizes the iteration time and improves the positioning accuracy by using the fused pose as the initial value of laser radar iteration. Finally, the effectiveness of the method is verified through simulation on the gazebo platform.

Description

TECHNICAL FIELD

[0001] The application belongs to the field of Simultaneous Localization and Mapping (SLAM), and particularly relates to a SLAM front-end strategy based on multi-source information fusion of EKF. BACKGROUND

[0002] In the field of Simultaneous Localization and Mapping (SLAM), the framework of a SLAM system mainly contains two parts: front-end and back-end.

[0003] In laser SLAM, the front-end mainly proposes the current scanning point to estimate the current pose under the condition of knowing the pose of the previous frame, and the back-end mainly uses an optimization method based on a filter or graph optimization for optimization. Subgraph scan matching is a subgraph-based SLAM front-end method. It divides the entire map into multiple subgraphs, and each subgraph has an independent coordinate system. In subgraph scan matching, the pose information of each subgraph is obtained by image matching of the images of each subgraph. The weighted average of the pose estimation results of each point is obtained, that is, the pose estimation result of each point. Since the estimation of the initial pose has a great influence on the matching effect in subgraph scan matching, the subgraph scan matching is too dependent on the initial value. Therefore, how to handle this problem has become the main direction of current engineering research. SUMMARY

[0004] The application belongs to the field of Simultaneous Localization and Mapping (SLAM), and particularly relates to a SLAM front-end strategy based on multi-source information fusion of EKF.

[0005] In order to ensure that the front-end matching does not depend on the initial value, the application proposes a SLAM front-end strategy based on multi-source information fusion of EKF

[0006] The work of the application at least includes: first: in view of the problem that the initial value is dependent on the SLAM front-end subgraph scanning matching, an EKFMSF method fusing a wheel edge encoder and an IMU is proposed, and the fused pose is taken as the initial value of the laser radar scanning matching. Second: in view of the problem that the motion distortion occurs in the motion process of the single-line low-frequency laser radar, the fused pose is subjected to spherical interpolation of the laser points of the laser radar based on the quaternion, and then the relative pose obtained by each laser point is taken as compensation for the motion distortion; third: in view of the problem that more feature points are required for the laser radar interframe matching, the matching strategy of the frame and the subgraph matching is improved. Finally, a probability grid map is constructed, and the simulation experiment verifies the effectiveness of the EKFMSF-SLAM algorithm.

[0007] The technical scheme concretely adopted by the application for solving the technical problems is:

[0008] A SLAM front-end strategy based on EKF multi-source information fusion, characterized in that:

[0009] In view of the problem that the initial value is dependent on the laser radar front-end subgraph construction scanning matching, a multi-source information fusion algorithm based on extended Kalman filtering is adopted, and the mileage data calculated by the IMU and the wheel edge encoder are fused, so that the fused pose is taken as the initial value of the laser radar scanning matching.

[0010] The fused pose is subjected to spherical interpolation of the laser points of the laser radar based on the quaternion, and then the relative pose obtained by each laser point is taken as compensation for the motion distortion.

[0011] Further, the matching strategy between the frame and the subgraph matching is improved by using the strategy of the current laser radar data frame and the relevant scanning matching of the local map, and the fused pose is taken as the initial value of the laser radar iteration, so as to optimize the iteration time and improve the positioning accuracy.

[0012] Further, the multi-source information fusion algorithm based on the extended Kalman filtering considers that the established map is a two-dimensional grid map, the coordinates z in the state equation z k ={x,y,z,roll,pitch,yaw} and the rotation roll around the x axis and the rotation pitch around the y axis are all 0, so as to be simplified as z k ={x,y,0,0,0,yaw};Therefore, the fusion of the data of the encoder and the IMU two sensors obtained from the unmanned vehicle can obtain two Gaussian linear system observation equations;And the motion equation is unique in the motion process of the unmanned vehicle, and the corresponding motion equation is only one, so as to obtain the state equation of the system.

[0013] In the loose coupling mode, the initial state of the unmanned vehicle is 0, when the observation data of one of the encoder and the IMU arrives, the other one is linearly interpolated to obtain time-aligned pose observation data, and then the data of the two are fused to output the optimal pose estimation;

[0014] The Kalman formula is used to fuse the two observation data, and the standard deviation of the fused observation estimation is minimized as the target to determine the Kalman gain coefficient.

[0015] Further, the obtained fused pose is subjected to spherical interpolation of a quaternion to remove the motion distortion of the laser radar, and the specific steps are as follows:

[0016] The translation matrix is used to represent the displacement of the unmanned vehicle, the quaternion is used to represent the rotation change of the unmanned vehicle, and the spherical linear interpolation is used for the quaternion;

[0017]

[0018] Wherein, p corresponds to the time t1, q corresponds to the time t2, t' corresponds to the time t', and r corresponds to the time t'; the spherical linear interpolation of the obtained quaternion is performed;

[0019] Therefore, the proportion coefficient t satisfies:

[0020]

[0021] The angle between the unit quaternions p and q is θ, the angle between p and r is tθ, and the angle between q and r is (1-t)θ;

[0022] In the above formula, Slerp represents the spherical linear interpolation of the quaternion, and according to the linear interpolation of the formula, the relative pose of each laser point with respect to the starting laser point is obtained, and then the pose transformation is performed.

[0023] Further, the specific steps of the pose transformation are as follows:

[0024] 1) Obtain the laser radar observation scan data, including the starting time and the ending time of the laser radar, and calculate the time corresponding to each point by linear calculation;

[0025] 2) Solve the relative time of each point corresponding to the time with respect to the starting time, and linearly interpolate the fused pose formula to obtain the pose increment;

[0026] 3) Add the pose increment of each point to the laser radar observation to remove the motion distortion, and finally convert the laser radar observation to the world coordinate system.

[0027] Further, the current laser radar data frame and the local map are matched by using the correlation scanning matching CSM strategy;

[0028] It runs based on a probability grid map, each grid maintains a log form of occupancy probability; for the new input laser frame, all points in the frame data are projected onto the grid map through a predicted pose, thus each laser point will fall into a grid, and the sum of the log probability values of the grid where the laser point is located is the score of the current pose, representing the credibility of this pose, and finally the highest score candidate pose is obtained as the optimal result output.

[0029] Further, the pose information of the wheel odometer and the IMU is fused, and each laser radar point of the current frame of the laser radar is interpolated, and the corresponding pose transformation of each laser point is completed to remove motion distortion, and the pose transformation is used as the initial value of the laser radar scan matching iteration, and then the laser radar is matched through the CSM based on the front end odometer to construct the EKF MSF-SLAM local subgraph:

[0030] Probability is used to describe whether there is an obstacle in the grid, and the greater the occupancy probability, the higher the possibility of the existence of obstacles in the grid:

[0031] After mathematical modeling, a number of laser radar observations are defined as a subgraph, and the coordinates in the subgraph are real world coordinates, and then the map is discretized, and the discrete grid length is r; the position of the point (x, y) in the map in the grid map is (x / r, y / r);

[0032] At this time, the state of the unmanned vehicle is (x, y, θ), θ is the heading angle, the distance between the unmanned vehicle and the laser radar is l, the observation distance of the laser radar to the obstacle is d, and the angle is α, so the global position of the current observed obstacle is (x o ,y o ):

[0033]

[0034] The position of the obstacle in the grid is: At this time, it is considered that the grid is occupied by obstacles.

[0035] Further, the coordinate system transformation includes: conversion from the laser radar coordinate system to the odometer coordinate system, conversion from the fusion frame to the IMU coordinate system, and finally conversion from the IMU coordinate system to the unmanned vehicle base coordinate system.

[0036] Compared to existing technologies, this invention and its preferred scheme address the issue of initial value dependence in the scanning matching of LiDAR front-end submap construction by proposing an Extended Kalman filter multi-source fusion (EKFMSF) algorithm. The EKF algorithm integrates the wheel-side encoder and IMU pre-integration, obtaining the odometer pose (position + attitude) based on this, and then acquiring the fused pose and covariance matrix. Quaternion spherical interpolation is then used to remove LiDAR motion distortion. Furthermore, to address the issue of numerous feature points required for LiDAR frame matching, this invention improves the matching strategy between frames and submaps by using a correlative scan matching (CSM) strategy between the current LiDAR data frame and the local map. Simultaneously, using the fused pose as the initial value for LiDAR iteration optimizes the iteration time and improves positioning accuracy. Finally, simulations on the Gazebo platform verify the effectiveness of the method. Attached Figure Description

[0037] The present invention will be further described in detail below with reference to the accompanying drawings and specific embodiments:

[0038] Figure 1 This is a schematic diagram of the motion model of the Ackerman steering trolley according to an embodiment of the present invention;

[0039] Figure 2 This is a schematic diagram of the EKF fusion framework according to an embodiment of the present invention;

[0040] Figure 3 This is a schematic diagram of motion distortion of a linear motion lidar according to an embodiment of the present invention;

[0041] Figure 4 This is a schematic diagram of laser radar distortion according to an embodiment of the present invention;

[0042] Figure 5 This is a schematic diagram of unit quaternion spherical interpolation according to an embodiment of the present invention;

[0043] Figure 6 This is a schematic diagram of coordinate transformation according to an embodiment of the present invention;

[0044] Figure 7 This is a flowchart illustrating the point cloud matching process according to an embodiment of the present invention;

[0045] Figure 8 This is a schematic diagram illustrating the constraints between various quantities in the CSM according to an embodiment of the present invention;

[0046] Figure 9 This is a schematic diagram of the grid map state according to an embodiment of the present invention;

[0047] Figure 10A scanning update schematic diagram for an embodiment of the present application;

[0048] Figure 11 A motion distortion removal experimental result schematic diagram for an embodiment of the present application;

[0049] Wherein, the left graph is a linear motion and the right graph is a rotational motion;

[0050] Figure 12 A trajectory comparison graph for an embodiment of the present application;

[0051] Figure 13 An XY direction trajectory comparison graph for an embodiment of the present application;

[0052] Figure 14 A partial enlarged view for an embodiment of the present application;

[0053] Wherein, a) is a partial enlarged view of Figure 12 , and b) is a partial enlarged view of Figure 13 ;

[0054] Figure 15 A pose comparison graph for an embodiment of the present application;

[0055] Figure 16 A partial pose enlarged comparison graph for an embodiment of the present application;

[0056] Wherein, the upper graph is a simulation environment and the lower graph is a laser radar initial observation;

[0057] Figure 17 A simulation environment and laser radar initial observation graph for an embodiment of the present application;

[0058] Wherein, a) is an EKFMSF-SLAM multiple subgraph result, and b) is a partial detail enlargement;

[0059] Figure 18 An EKFMSF-SLAM multiple subgraph result and partial detail enlargement schematic diagram for an embodiment of the present application;

[0060] Figure 19 An IMU pre-integration pose absolute pose error schematic diagram for an embodiment of the present application;

[0061] Figure 20 An odometry estimated pose absolute pose error schematic diagram for an embodiment of the present application;

[0062] Figure 21 A fused pose absolute pose error schematic diagram for an embodiment of the present application;

[0063] Figure 22 A method flowchart for an embodiment of the present application. DETAILED DESCRIPTION

[0064] For the features and advantages of the patent to be more clearly understood, the following detailed description is made with reference to the accompanying drawings, wherein:

[0065] It should be noted that the following detailed description is exemplary in nature and is intended to provide further description of the application. Unless otherwise defined, all technical and scientific terms used herein have the same meaning as commonly understood by one of ordinary skill in the art to which this application belongs.

[0066] It is to be understood that the terminology used herein is for the purpose of describing particular embodiments only and is not intended to be limiting of example embodiments in accordance with the present application. As used herein, the singular forms "a", "an" and "the" are intended to include the plural forms as well, unless the context clearly indicates otherwise. It will be further understood that the terms "comprises" and / or "comprising," when used in this specification, specify the presence of stated features, steps, operations, elements, components, and / or groups thereof, but do not preclude the presence or addition of one or more other features, steps, operations, elements, components, and / or groups thereof.

[0067] As shown in the following, the following provides a step-by-step embodiment to make a detailed introduction to the present application solution: Figure 22

[0068] Step 1: Collecting unmanned vehicle data through multiple sensors and performing data preprocessing;

[0069] Collecting information of laser radar, inertial measurement unit (IMU) and wheel encoder sensor from vehicle-mounted sensors. The vehicle model used in the embodiment of the present application is an Ackermann steering dolly, as shown in Figure 1 .

[0070] In the figure, α is the turning angle of the inner wheel of the unmanned vehicle steering wheel, β is the turning angle of the outer wheel of the unmanned vehicle steering wheel, θ is the heading angle of the single vehicle model, L is the wheelbase of the unmanned vehicle, K is the wheel track of the unmanned vehicle, and R is the turning radius of the unmanned vehicle. Because the single vehicle model and the Ackermann model can be considered consistent in terms of motion mechanism, the Ackermann model is compared with the single vehicle model, that is, the steering angle of the front wheel is replaced by the average steering angle of the two front wheels, and the rest is consistent with the single vehicle kinematic model, so the Ackermann model can be simplified into a single vehicle model. The single vehicle model is shown by the dashed line in the figure, and the Ackermann steering mechanism can be more conveniently understood by analyzing the single vehicle model.

[0071] According to Figure 1 , the core formula of Ackermann geometry is:

[0072]

[0073] From the geometric relationship, we can get:

[0074]

[0075]

[0076] ​

[0077] The relationship between the steering angle of the single-wheel model and the steering angle of the Ackerman model can be obtained by combining the above geometric core formulae as follows:

[0078]

[0079]

[0080] Suppose that the steering inner wheel of the unmanned vehicle has a turning radius of R L , the steering outer wheel has a turning radius of R R , and the relationship between them is as follows:

[0081]

[0082]

[0083] Suppose that the steering inner wheel of the unmanned vehicle has a speed of v L , the steering outer wheel has a speed of v R , the linear speed of the unmanned vehicle is v, and the angular speed is w. The following can be obtained:

[0084]

[0085] Therefore, the relationship between the speed of the single-wheel model and the speed of the Ackerman model is as follows:

[0086]

[0087]

[0088] The Ackerman model can thus be simplified into a single-wheel model, and the expressions for the linear speed of the single-wheel model in the x-axis and y-axis directions relative to the world reference system and the angular speed ω relative to point O are obtained as follows, with the center of the front wheel of the single-wheel model as the origin:

[0089]

[0090] The above formulae on the left side of the equal sign represent derivatives. The motion state formula of the unmanned vehicle from t to t+1 is as follows:

[0091]

[0092] where δ is the steering angle of the simplified single-wheel model of the unmanned vehicle, and δ' is the derivative of δ. The above formulae can thus be used to represent the displacement of the X-axis and the displacement of the Y-axis of the wheel encoder of the unmanned vehicle in the world coordinate system, as well as the recursive formulae for the heading angle and the front axle steering angle, thus completing the conversion from the Ackerman steering model to the single-wheel model.

[0093] Step two: Derive the error recursive formula from the data obtained from step one.

[0094] acceleration of the IMU and angular velocity The continuous observation model is:

[0095]

[0096] In the formula, subscript t represents the result in the IMU coordinate system, a is the actual acceleration, b a represents the IMU accelerometer bias, w is the actual measured angular velocity, b w is the gyroscope bias, i is a time between the t time and the t+1 time, and therefore the state value PVQ is based on the discrete form of the median integral:

[0097]

[0098] In the formula, p, v, and q represent the pose, velocity, and quaternion of the unmanned vehicle, respectively, and δt represents the first derivative of time. According to the IMU pre-integration formula:

[0099]

[0100] Discretizing the integration process obtains the pre-integration formula based on the median theorem, which is and :

[0101]

[0102] In the formula, w is the world coordinate system, b k is the world coordinate system, is the rotation matrix from coordinate w to b k , and the results of and are the relative pose changes at the i+1 time relative to b k , and the initial states and are 0, is the unit quaternion, i represents a time between the k time and the k+1 time, δt is the time interval between i and i+1, p, v, and q represent the pose, velocity, and quaternion of the unmanned vehicle, respectively, g represents the gravitational acceleration, represents quaternion multiplication. According to the formula, the subsequent state is not affected by the state of b k , and therefore when it is used as a variable in nonlinear optimization, the repeated transmission of the previous time state can be avoided, thereby avoiding the repeated integration process of solving the subsequent state.

[0103] Since the value of the above formula is integrated with error, and the IMU is affected by the bias b a of the accelerometer, the bias b w of the gyroscope, the noise n a and n w , which are all the sources of error, the error is analyzed. The acceleration bias b a and the gyroscope bias b w are modeled as random walks, and their derivatives are Gaussian:

[0104]

[0105] Assuming that the noise n a and n w comply with Gaussian noise, we get:

[0106]

[0107] Since the quaternion q is over-parameterized, its error term is distributed around its own mean:

[0108]

[0109] Therefore, the error term is δb at , δb wt

[0110] The derivatives of these error terms are calculated, where According to the definition, we get:

[0111]

[0112] For , considering the noise n a and n w , we get:

[0113]

[0114] In the formula, [·] × represents an antisymmetric matrix.

[0115] For the solution of , we first solve and its derivative Without considering noise, the formula is as follows:

[0116]

[0117] Where:

[0118]

[0119] Consider the noise n a and n w The true measurement value is :

[0120]

[0121] According to the derivative property, we have

[0122]

[0123] That is, the equation is obtained:

[0124]

[0125] Combining the above formula, the expression of is:

[0126]

[0127] From the above, the linearization recursive equation of the error term at time t is:

[0128]

[0129] After simplification, it can be written as: In the formula, F t is 15x15, is 15x1, G t is 15x12, and n t is 12x1.

[0130] Similarly, the discrete form of the error is obtained:

[0131] For short, δz k+1 = Fδz k + Vn

[0132] In the formula, F t is 15x15, is 15x1, V 15×18 is 15x18, and n is 18x1.

[0133] Step three: use the SLAM front-end strategy of multi-source information fusion based on extended Kalman filter (EKFMSF) to fuse the mileage data calculated by IMU and wheel encoder, and obtain the fused pose;

[0134] The relationship between the two conditions is described by Bayes' theorem, and its core formula is:

[0135]

[0136] ​The formula is used in the state estimation of unmanned vehicle X = {x0,...,x N} represents the pose of the vehicle, and Z = {z0,...,z N} represents the observation of the current vehicle, wherein the observation is at the pose x0 to x N There are road signs y1,...,y M , so the motion model and observation model of the unmanned vehicle can be represented as:

[0137]

[0138] Since the current pose and the road sign points are variables to be estimated, let x k be all unknown quantities at time k, which contains the pose of the unmanned vehicle at the current time and m road sign points, and can be written as:

[0139] x k = {x k ,y1,...,y m}

[0140] At the same time, all observations at time k are recorded as z k , so the formula can be simplified as:

[0141]

[0142] The data from 0 to k is used to estimate the current state distribution P(x k | x0, u 1:k , z 1:k ), wherein 1: k represents data from 1 to k, and the above core formula can be obtained:

[0143] P(x k | x0, u 1:k , z 1:k ) ∝ P(z k | x k ) P(x k | x0, u 1:k , z 1:k-1 )

[0144] The left side of the above formula is the likelihood probability, and the right side is the prior probability. The likelihood can be obtained from the observation equation, and the prior probability can be derived based on the past state. From this, the Bayesian method of unmanned vehicle state estimation is completed.

[0145] The classical Kalman filtering method assumes Markov property, i.e. the current state is only related to the state at the last time, and the motion equation and the observation equation are linear Gaussian systems, so the state equation is:

[0146]

[0147] Based on the Markov assumption, the prior probability is expressed as x k-1 The conditional probability expansion at each time point yields:

[0148] P(x k |x0,u 1:k ,z 1:k-1 )=∫P(x k |x k-1 ,x0,u 1:k ,z 1:k-1 )P(x k-1 |x0,u 1:k ,z 1:k-1 )dx k-1

[0149] =∫P(x k |x k-1 ,u k )P(x k-1 |x0,u 1:k-1 ,z 1:k-1 )dx k-1

[0150] The second equation in the above formula is because x k Time and x k-1 Since the previous states are irrelevant, the first probability can be simplified to only depend on x. k-1 and u k The relevant form, and the state at time k-1 is independent of the input at time k, so the second probability can remove u. k .

[0151] Assume the noise follows a Gaussian distribution with zero mean:

[0152] w k ~N(0,R),v k ~N(0,Q)

[0153] The first step in Kalman filtering is prediction, which involves determining, based on the equations of motion, that the prior probability distribution is also a Gaussian distribution.

[0154]

[0155] remember in Represents the prior probability. This represents the posterior probability. According to the observation equation, we can obtain:

[0156] P(z k |x k )=N(C k x k Q)

[0157] According to the above completed Bayesian method of state estimation of the unmanned vehicle, assuming that the unmanned vehicle pose obeys a Gaussian distribution

[0158]

[0159] Because both sides of the formula are Gaussian distributions, only the exponential part needs to be compared, and the expansion of the exponential part can obtain

[0160]

[0161] In order to solve and , the expansion of both sides is carried out, and the quadratic coefficient of x k is compared to obtain

[0162]

[0163] The above formula gives the calculation process of the covariance, in order to facilitate the subsequent calculation, an intermediate variable is defined, which is the Kalman gain K:

[0164]

[0165] According to the definition, the above formula is multiplied by on both sides to obtain

[0166]

[0167] Therefore, it can be obtained that

[0168]

[0169] Then the coefficient of the first term of x k in the above expansion formula of the exponential part is compared to obtain

[0170]

[0171] The coefficient is taken and transposed to obtain

[0172]

[0173] The above formula is multiplied by on both sides, and the expression is substituted to obtain the posterior mean as

[0174]

[0175] From this, the optimal unbiased estimation of the linear system composed of the Kalman filter is derived, but the motion equation and the observation equation of the actual SLAM process are nonlinear functions, in order to solve the problem of the nonlinear system, the extended Kalman filter is adopted, and the working point x k ​The motion equation and the observation equation are first-order Taylor expanded in the vicinity, so that the nonlinear equation is approximated into a linear Gaussian system by Taylor expansion, and the linearization formula is:

[0176]

[0177] Record Therefore, the above posterior probability equation becomes:

[0178]

[0179] In combination with the motion equation of the unmanned vehicle, the prior expectation and covariance of the prediction step can be changed to:

[0180]

[0181] In combination with the observation equation of the unmanned vehicle, the posterior probability can be obtained:

[0182]

[0183] Similarly, the Kalman gain at this time is:

[0184]

[0185] Therefore, the posterior form of the extended Kalman filter is:

[0186]

[0187] From this, the maximum posterior probability of the SLAM single linear Gaussian approximation is given by formula derivation. Since the map to be established by the embodiment of the application is a two-dimensional grid map, the coordinates z in the state equation z k ={x, y, z, roll, pitch, yaw} are all 0, and the rotation roll around the x-axis and the rotation pitch around the y-axis are 0, so the state equation can be simplified to z k ={x, y, 0, 0, 0, yaw}. Thus, the data of the two sensors of the encoder and the IMU obtained from the unmanned vehicle are fused, and two Gaussian linear system observation equations are obtained; and the motion equation has only one because the motion process of the unmanned vehicle is unique, so the state equation of the system is:

[0188]

[0189] The specific fusion process flowchart is shown in Figure 2 The application adopts a loose coupling manner, the initial state of the unmanned vehicle is predicted to be 0, then one of the observation data of the encoder and the IMU arrives, the other one is linearly interpolated to obtain time-aligned pose observation data, then the data of the two are fused, and finally the optimal pose estimation is output.

[0190] The specific fusion process is that two sensor data are assumed to conform to Gaussian distribution, that is:

[0191]

[0192] Based on the Kalman formula, the fused observation estimation can be obtained as:

[0193] z k =z1(k)+K k (z2(k)-z1(k))

[0194] Now, the Kalman gain K is required to be solved k so that the standard deviation is minimized, that is, the variance is minimized, and the variance of the fusion system is: 2 =Var(z1(k)+K k (z2(k)-z1(k)))=Var((1-K k )z1(k)+K k z2(k))

[0195] In the formula, Var represents variance, and since the two sensors of the encoder and the IMU are independent of each other, it can be obtained that:

[0196] The formula is derived with respect to K k , and the derivative is set to zero to find the extreme value, so that K k is obtained when the variance is minimized:

[0197]

[0198] In the formula, K k is the Kalman gain, which is equivalent to adjusting the weight of the two sensors, when it takes a value equal to 0, the observation is only z1(k), when it takes a value equal to 1, the observation is only z2(k), so its range is [0, 1], and the analysis of the variance formula can obtain that the variance after fusion is smaller than any one of the two variances, which will make the pose after fusion more accurate.

[0199] Step four: performing spherical interpolation of the quaternion on the fused pose obtained in step three to remove the motion distortion of the laser radar;

[0200] In addition to the dependence on the initial value, the laser radar odometer also has motion distortion in the observation data of the laser radar itself. The motion distortion is that the mechanical laser radar realizes 360-degree environmental scanning through an electric motor, for example, the laser radar used in the present application is 10HZ, that is, there is a time interval of 0.1s between the first laser point and the last point generated by one revolution of the laser radar, for example, Figure 3As shown, the left circle point represents the farthest position that the laser radar can reach when the unmanned vehicle is stationary, at which time the starting point and the end point of the scan can be better closed; the right side is the farthest position that a circle of laser produced by the laser radar can reach when the unmanned vehicle is in straight line motion (no rotation), at which time the starting point and the end point of the scan will be distorted, because the position from the starting point to the end of a circle scan in the figure has changed, that is, the origin of the laser radar coordinate system corresponding to the laser point has changed.

[0201] The motion distortion of the laser radar point cloud is essentially that the coordinate system of each point in a frame is different. When the unmanned vehicle has rotation in addition to straight line motion, the observation of the same straight line object will produce a large error, such as Figure 4 As shown, p1-p3 in the upper figure represent three position points scanned by the laser radar in turn, at which time the unmanned vehicle is stationary and the three points are collinear in the real world. However, due to the "violent" motion of the unmanned vehicle within a frame of time, such as Figure 4 As shown in the middle part, the radar itself scans the three points at three different actual postures respectively. Therefore, in the finally obtained point cloud (the lowermost), the three point coordinates are actually in different coordinate systems, so that the three observation points are not only no longer collinear, but also have serious errors with their real positions.

[0202] In summary, the laser radar motion distortion is that the motion of the unmanned vehicle causes the laser radar coordinate system of each laser radar point to change. If the coordinate change relationship of all laser points with the initial point can be solved, then the laser radar motion distortion problem can be solved.

[0203] The pose after step three fusion is taken as the pose change relationship of each laser point of the laser radar, and since the fused pose can reach 100HZ, the angular resolution of the laser radar used in the application is 0.225°, that is, 1600 laser points are generated in one rotation, so the fused data and the laser radar points are different in time, and an interpolation method needs to be used for estimation.

[0204] The most commonly used rotation representation methods are quaternions, Euler angles and rotation matrices, but Euler angles have a gimbal lock problem, that is, in the Z-Y-X order, if the rotation pitch around the Y axis is positive or negative 90 degrees, the third and first rotations will be around the same axis, causing the system to lose one degree of freedom, so Euler angles are not suitable for interpolation and iteration. Quaternions only need to store four values, while rotation matrices need to create a 3x3 matrix, which requires the storage of nine elements at the same time, so quaternions save more storage space than rotation matrices and are more convenient for interpolation. Therefore, in this embodiment, the translation matrix is used to represent the displacement of the unmanned vehicle, and the quaternion is used to represent the rotation change of the unmanned vehicle, and the translation matrix can be linearly interpolated in the Euclidean space. However, the interpolation vector of the quaternion must ensure that its motion trajectory is on the unit sphere, so the spherical linear interpolation is adopted for the quaternion.

[0205] The unit quaternion has three imaginary parts and one real part, and can represent the rotation of a three-dimensional space, and the expression is:

[0206] q = w + i x + j y + k z

[0207] The unit quaternion satisfies the following properties:

[0208] || q || = 1 q -1 = q * q = cos (θ) + u sin (θ)

[0209] In the formula, u is the rotation axis of the quaternion, and θ is the rotation angle of the quaternion around the u axis; through the above formula, the mutual conversion relationship between the unit quaternion and the rotation vector can be derived as:

[0210]

[0211] log (q) = log (uθ) = uθ

[0212] As shown in Figure 5 , let p correspond to time t1, q correspond to time t2, and the quaternion corresponding to time t' is r, which is the spherical linear interpolation of the obtained quaternion.

[0213] Therefore, the proportionality coefficient t satisfies:

[0214]

[0215] Assume that the angle between the unit quaternions p and q is θ, the angle between p and r is tθ, and the angle between q and r is (1-t)θ. According to the general interpolation formula, the expression of r is as follows:

[0216] r (t) = a (t) p + b (t) q

[0217] At the same time:

[0218] cos θ = p q, cos(t θ) = p r, cos[(1-t) θ] = q r

[0219] Multiplying both sides of the difference formula by p and q respectively, we can get:

[0220]

[0221] Solving the equations, we can get:

[0222] Thus we have:

[0223]

[0224] In the formula, Slerp (Spherical linear interpolation) represents the spherical linear interpolation of quaternions. According to the linear interpolation of the formula, the relative pose of each laser point with respect to the starting laser point can be obtained, and then the pose transformation is performed. The specific steps are as follows:

[0225] 1) Obtain the laser radar observation scan data, i.e. the start time and end time of the laser radar, and calculate the time interval by linear interpolation

[0226] 1600 points correspond to the time of each point.

[0227] 2) Solve the time interval of these points with respect to the starting time t , and perform linear interpolation on the fusion pose formula to obtain the pose increment.

[0228] 3) Add the pose increment of each point to the laser radar observation, remove the motion distortion, and finally convert the laser radar observation to the world coordinate system.

[0229] The coordinate system transformation involved in step three is shown in Figure 6 , which mainly includes the conversion from the laser radar coordinate system to the odometry coordinate system, the conversion from the odometry coordinate system to the IMU coordinate system by the fusion frame, and finally the conversion from the IMU coordinate system to the base coordinate system of the unmanned vehicle.

[0230] Step five: analyze the laser odometry principle, and adopt the current laser radar data frame and local map related scan matching (CSM) strategy to perform frame and subgraph matching strategy;

[0231] The lidar emits a laser beam from a laser source, which is reflected by a mirror and then out of the lidar. At the same time, a servo motor drives the mirror to rotate. When the laser beam encounters an obstacle, it is reflected back to the receiver. The time difference between transmission and reception is calculated by an optical encoder. Multiplying this time difference by the speed of light gives twice the distance between the lidar and the obstacle. By rotating the lidar once, the relative position of the surrounding obstacles with respect to the lidar can be obtained. Based on the relative angle of the lidar, the orientation of the obstacle with respect to the lidar can also be obtained. Thus, the pose of the obstacle with respect to the lidar coordinate system is obtained.

[0232] The lidar ranging formula L is:

[0233]

[0234] In the formula, c is the speed of light, approximately 3 × 10^8 m / s, Δ t This is the time difference between laser emission and reception.

[0235] After obtaining the relative poses of obstacles around the lidar with respect to the lidar, a point cloud matching algorithm is used, such as... Figure 7 As shown, point cloud matching first extracts key points and performs feature description, then matches the feature points of two point clouds and removes some mismatched feature points. Next, it uses an inter-frame matching algorithm or matches radar frames with local maps to obtain R and t and constructs the least squares of the error. Finally, it converges or reaches the number of iterations to obtain the pose difference before and after the lidar, i.e., the mileage data.

[0236] Traditional inter-frame matching involves matching two LiDAR frames to obtain their relative pose. The most typical data matching method is the Iterative ClosestPoint (ICP) algorithm. Its characteristics are: finding corresponding points between the source point cloud and the object point cloud, constructing a transformation matrix based on the corresponding points, converting the source point cloud into the coordinates of the object point cloud using this matrix, estimating the deviation between the converted source point cloud and the object point cloud, and repeatedly iterating if the deviation value is greater than a set threshold until it is less than a certain deviation value.

[0237] Traditional matching algorithms, such as ICP point cloud inter-frame matching, treat the most recent point in the previous frame as the corresponding point at different times. This approach is prone to getting trapped in local optima, and as the autonomous vehicle moves, the algorithm accumulates previous errors, leading to significant relative pose errors in large scenes. Therefore, this invention employs a strategy of correlative scan matching (CSM) between the current LiDAR data frame and the local map, which can reduce error accumulation.

[0238] Unlike the ICP point cloud matching algorithm, the CSM algorithm matches not the points of the previous frame, but the subgraph composed of the laser radar point cloud in the previous period. The algorithm runs based on a probability grid map, and each grid maintains a log-form occupancy probability. For the newly incoming laser frame, all points in the frame data are projected onto the grid map through a predicted pose. In this way, each laser point will fall into a grid, and the sum of the log probability values of the grid where the laser point is located is the score of the current pose, representing the credibility of the pose. Finally, the highest scoring candidate pose is obtained as the optimal result output.

[0239] The mathematical modeling process of this problem is as follows: as shown in Figure 8 , the position of the unmanned vehicle is initialized at position x i The observation of the environment by the unmanned vehicle is z i At the last adjacent position x i-1 , the observation of the environment by the unmanned vehicle is z i-1 From x i-1 to x i , the initial pose change of the unmanned vehicle can also be calculated by the wheel side odometry or IMU, denoted as u. If x i-1 and z i-1 are known, u and z i are also obtained by observation. To reverse the accurate pose x i of the unmanned vehicle, the probability p(x i |x i-1 , u, z i-1 , z i ) needs to be maximized. This probability is not a probability value but a joint probability distribution of three variables. When the probability is maximized, it is the optimal pose estimation of the unmanned vehicle.

[0240] The map established by the method is a two-dimensional grid map, so the pose x i and x i-1 of the unmanned vehicle are three-dimensional vectors, including plane coordinates x, y and heading angle yaw. U describes the change of the adjacent pose, which is also a three-dimensional vector. Z i and z i-1 are the observations of the laser radar, which are a set of two-dimensional coordinates.

[0241] The following derivation replaces z i-1 with m. This representation is more general and can represent any previous laser radar data or subgraph frame composed of several frames. This replacement has no effect on mathematical processing, because whether z i-1 or z i-1 +z i+2 +..., they are all represented in the form of a grid map.

[0242] from Figure 8 The current pose x can be obtained from the middle. i Subject to two constraints:

[0243] x i-1 +u≈x i

[0244] z i |x i ~m

[0245] The first expression in the above formula represents the pose x at the previous moment. i-1 Adding the observation estimate u gives the pose x at the current moment. i The second expression represents the observation z. i It occurs in pose x i Therefore, the current observation should conform to the known local map m. Based on these two constraints, the following observation equation can be obtained:

[0246] p(x i |x i-1 ,u,m,z i )∝p(z i |x i ,m)p(x i |x i-1 ,u)

[0247] In the formula, ∝ signifies proportionality, meaning that solving the overall probability distribution problem is equivalent to finding the one that achieves the global maximum. In the above formula, p(x) i |x i-1 The motion model (μ, σ) conforms to a standard Gaussian distribution N(μ, σ). 2 Therefore, solving for the maximum likelihood estimate is sufficient. Specifically:

[0248]

[0249] In the formula, f(x) is the probability density distribution function, μ is the mean, and σ is the standard deviation. 2 The variance is calculated using the following formula:

[0250]

[0251]

[0252] The joint probability density equation is:

[0253]

[0254] Therefore, the maximum likelihood function and its logarithmic form are:

[0255]

[0256]

[0257] Solving the maximum value of the formula is to solve the extreme value problem of binary function, partial derivative and then let it be zero, where the larger one is the one we want, here no longer.

[0258] The above observation equation formula p(z i ∣x i , m) is the observation model, the probability distribution model is not Gaussian distribution, the specific calculation is: assuming that the probability distribution of each laser point position in the hypothesis is independent of each other, then:

[0259]

[0260] Taking the logarithm of both sides of the above formula, the multiplication calculation is converted to addition calculation, so:

[0261]

[0262] The above formula is to project the current observation into the subgraph according to the pose, add the probability values of all hit grids, different projection poses have different scores, and finally output the pose with the maximum score S as the optimal estimation of the current pose, that is:

[0263]

[0264] From this, we get the laser radar odometry based on CSM, by regarding the continuous laser radar frame as a subgraph and matching the current radar frame with the subgraph, the error accumulation caused by inter-frame matching is solved, and the error of solving the current pose is effectively reduced.

[0265] Step six: construct a probability grid map, and propose a new multi-fusion local subgraph scheme through mathematical modeling;

[0266] The above steps have fused the wheel edge odometry and IMU pose information, and interpolated based on the laser radar points of the current laser radar frame, and completed the motion distortion of the laser points through the corresponding pose transformation, and based on this pose transformation as the initial value of laser radar scan matching iteration, and then the laser radar gets the front end odometry through CSM matching, and then the EKFMSF-SLAM local subgraph will be constructed.

[0267] One map point usually has two states, one is an obstacle, represented by 1. The other is free, represented by 0, but this value cannot be directly linked to the new observation. For example, the new observation may be the same as or different from the previous value. In order to unify the representation and establish a link between the new lidar observation, the present application adopts the form of a probability grid map as shown in Figure 9 , that is, the probability is used to describe whether there is an obstacle in the grid. The greater the occupancy probability, the higher the possibility of the existence of an obstacle in the grid.

[0268] For each grid of the occupancy grid map, p(s=1) is used to represent that the grid is in a free state, and Free is used in the formula. Another occupied state is represented by p(s=0), and Occupied is used in the formula. The present application uses the probability ratio of free and occupied to represent the state of the grid, and the formula is as follows:

[0269]

[0270] In the above formula, Odd represents the probability. When new data arrives, that is, the current observation data z of the lidar arrives, the probability state of the grid needs to be updated, and the new state is:

[0271]

[0272] According to the Bayes formula, the following two formulas are obtained:

[0273]

[0274]

[0275] After substituting the above two formulas into Odd(s|z), the following formula can be obtained:

[0276]

[0277] Taking the logarithm of the above formula, the following formula can be obtained:

[0278]

[0279] At this time, the term containing the measurement value in the grid is only The measurement value has two states: free (gridFree) and occupied (gridOccupied), and the expression is:

[0280]

[0281] At this time, if the state S of the grid s is expressed by logOdd(s), then the update rule is further simplified as:

[0282]

[0283] where S + represents the state of the grid s after updating, and S - represents the state before updating. The probabilities of the states of the grid, free and occupied, are both set to 0.5, so the initial state S init of the grid is:

[0284]

[0285] At this time, through mathematical modeling, updating the state of a grid only needs to add a probability to the original basis, as shown in the following formula:

[0286]

[0287] After mathematical modeling, the present application defines 100 frames of laser radar observation as a subgraph, as shown in the following formula: Figure 10 The coordinates in the graph are real-world coordinates, and then the map is discretized, and the length of the discrete grid is r=0.05m, so the position of the point (x, y) in the grid map is (x / r, y / r).

[0288] At this time, the state of the unmanned vehicle is (x, y, θ), the distance between the unmanned vehicle and the laser radar is l, the observation distance of the laser radar to the obstacle is d, and the angle is α, so the global position of the current observed obstacle is (x o ,y o ):

[0289]

[0290] The position of the obstacle in the grid is: At this time, it is considered that the grid is occupied by the obstacle.

[0291] Step seven: through real vehicle experiments, the superiority of the multi-element fusion motion distortion removal effect proposed in step five is proved, and the construction of the SLAM front-end probability grid subgraph and the simulation verification of the EKFMSF-SLAM algorithm are completed through the Gazebo simulation platform;

[0292] Firstly, in order to verify the effect of multi-element fusion motion distortion removal, the present application carries out real vehicle experiments in an indoor corridor environment. In order to better verify the robustness of the algorithm proposed in step five, a certain period of arbitrary walking and continuous rotation are carried out, and the motion speed is 1m / s, and the experimental results are shown in the following formula: Figure 11

[0293] ​The straight line part in the figure represents the trajectory of the experimental trolley, the white points are the real-time observation point cloud of the laser radar, and the black points are the point cloud after the laser radar motion distortion is removed through multi-source fusion. From the experimental result figure a, it can be seen that in the fast straight line motion, the distortion of the obstacles close to the trolley is less, and the distortion of the obstacles far from the trolley is more serious, but the main error is a rotation from the accurate pose, the main reason is that the trolley has no rotation, and the relative pose of the distant obstacle to the trolley is large, so the error is larger; Figure b is the continuous rotation of the trolley in a small range, which causes serious distortion. Compared with straight line motion, in addition to a rotation, there is also a large translation error in some laser radar scanning positions. The experimental results show that in the two cases prone to distortion, the multi-source fusion method proposed in this method achieves good motion distortion removal and restores the real laser radar observation to a certain extent.

[0294] Secondly, in order to verify the effectiveness of the fusion algorithm in step three, the method carries out experiments in the Gazebo simulation environment, takes the actual pose of the trolley in the simulation environment as the true value, and compares the obtained odometer estimated pose, IMU pre-integrated estimated pose, EKFMSF fused pose trajectory with the true pose as shown in Figures 12 to 16

[0295] Figure 14 (a) From top to bottom, the fusion trajectory, the true value of the trajectory pose, the IMU pre-integrated trajectory and the odometer estimated trajectory are sequentially arranged. Figure 14 (b) From top to bottom, the IMU pre-integrated trajectory, the odometer estimated trajectory, the true value of the trajectory pose and the fusion trajectory are sequentially arranged. Figure 16 From left to right, the true value of the trajectory pose, the fusion trajectory, the IMU pre-integrated trajectory and the odometer estimated trajectory are sequentially arranged. From the above experimental results, it can be seen that there is a large error between the odometer estimated pose and the true value pose, and the reason for this situation is that the trolley has a slipping phenomenon in the simulation environment; The error of the IMU in this environment is much smaller than that of the odometer, but the EKFMSF fusion odometer proposed in this method is closer to the true pose trajectory than the odometer estimated pose and the IMU pre-integrated estimated pose before fusion in X direction, Y direction and heading angle.

[0296] Finally, in order to verify the effectiveness of the multi-element fusion local subgraph scheme, the method carries out simulation experiments in the Gazebo simulation environment as shown in Figure 17 (a). The simulation environment is a 10x10m square map, the middle obstacle is a 1x1x1m cube, the laser radar scanning radius of the simulation trolley is set to 10 meters, and the final experimental results are displayed in Rviz.

[0297] Figure 17 ​B is a frame of observation of the laser radar when the trolley is initialized, and the circle-shaped point cloud represents the global position of a laser point after colliding with an obstacle and returning, which is calculated and presented by coordinate transformation. The grid between the circle point and the laser radar is updated to the idle state, and the position of the square grid is updated to the occupied state. Further experiments can obtain Figure 19 .

[0298] Figure 18 (a) is the result of the trolley starting to move and the multiple subgraphs being overlapped and output, Figure 18 (b) is a local enlarged view, and it can be seen that after enlargement, each grid is a square, and there are multiple obstacle position estimates in the square. Each estimate can be considered as an output result of a subgraph. It can be seen from the result that as the trolley moves and the number of subgraphs increases, serious overlap appears in the map, but no large distortion appears in each subgraph, which verifies the effectiveness of the local map construction of the method. The reason for the serious overlap between different subgraphs is that the continuous construction of the subgraph will continuously accumulate errors, and the generated errors will not only cause distortion, but also affect the matching accuracy, thereby causing large differences in the poses of multiple subgraphs. The output of the result of each subgraph causes the overlap problem.

[0299] The absolute pose error analysis of the odometer estimated pose, the IMU pre-integrated pose and the fused pose respectively obtains Figures 19 to 21 The statistics of each error obtains the following table:

[0300]

[0301]

[0302] From the table results, it can be obtained that the fusion strategy of the EKFMSF-SLAM proposed in the method is more accurate than the data before fusion. The error of the overall estimated data and the true value is controlled within 2 cm, and whether the average value or the standard deviation is far less than the estimated pose of a single sensor.

[0303] The above description is only a preferred embodiment of the present application, and is not intended to limit the present application in other forms. Any skilled person in the art can modify or change the above disclosed technical content to obtain equivalent embodiments. However, any simple modification, equivalent change and modification made on the basis of the technical essence of the present application to the above embodiments still belongs to the protection scope of the present application.

[0304] The patent is not limited to the above best mode, and anyone can derive other various forms of EKF-based multi-source information fusion SLAM front-end strategies based on the inspiration of the patent. Any equivalent changes and modifications made within the scope of the patent application should be covered by the patent.

Claims

1. A SLAM front-end strategy based on EKF multi-source information fusion, characterized in that: To address the issue of scanning matching relying on initial values ​​in the construction of LiDAR front-end submaps, a multi-source information fusion algorithm based on extended Kalman filtering is adopted. The mileage data calculated by the IMU and wheel-side encoder are fused, and the fused pose is used as the initial value for LiDAR scanning matching. Quaternion spherical interpolation is performed by fusing pose based on laser points from LiDAR, and then the relative pose obtained for each laser point is used as compensation to remove motion distortion.

2. The SLAM front-end strategy based on EKF multi-source information fusion according to claim 1, characterized in that: The matching strategy between frames and sub-maps is improved by using a strategy that matches the current LiDAR data frames with local map related scans; and the pose is fused as the initial value for LiDAR iteration to optimize the iteration time and improve positioning accuracy.

3. The SLAM front-end strategy based on EKF multi-source information fusion according to claim 1, characterized in that: The multi-source information fusion algorithm based on extended Kalman filtering considers the established map to be a two-dimensional raster map, for the state equation z k In the equation {x,y,z,roll,pitch,yaw}, the coordinate z, as well as the rotations roll and pitch about the x-axis, are all set to 0 to simplify to z. k ={x,y,0,0,0,yaw}; Therefore, by fusing the data from the encoder and IMU sensors obtained from the autonomous vehicle, two Gaussian linear system observation equations can be obtained; and since the motion process of the autonomous vehicle is unique, there is only one corresponding motion equation, thus obtaining the system's state equation; Then, a loosely coupled approach is adopted. In the initial state, the predicted pose of the autonomous vehicle is 0. When the observation data of one of the encoder and IMU arrives, the other distorted linear interpolation obtains the time-aligned pose observation data. The two data are then fused to output the optimal pose estimate. In this method, the Kalman formula is used to fuse two observation data, and the Kalman gain coefficient is determined with the goal of minimizing the standard deviation of the fused observation estimate.

4. The SLAM front-end strategy based on EKF multi-source information fusion according to claim 3, characterized in that: The obtained fused pose is then subjected to quaternion spherical interpolation to remove motion distortion from the lidar, specifically as follows: The displacement of the unmanned vehicle is represented by a translation matrix, the rotation of the unmanned vehicle is represented by quaternions, and spherical linear interpolation is applied to the quaternions. Let time t1 correspond to p, time t2 correspond to q, and the quaternion corresponding to time t′ be . rq This corresponds to the obtained quaternion spherical linear interpolation; Therefore, the proportionality constant t satisfies: The angle between unit quaternions p and q is θ, the angle between p and r is tθ, and the angle between q and r is (1-t)θ; In the above formula, Slerp represents quaternion spherical linear interpolation. The relative pose of each laser point with respect to the starting laser point is obtained by linear interpolation according to this formula, and then pose transformation is performed.

5. The SLAM front-end strategy based on EKF multi-source information fusion according to claim 4, characterized in that: The specific steps of the pose transformation are as follows: 1) Obtain lidar observation scan data, including lidar start time and end time, and calculate the time corresponding to each point linearly; 2) Solve for the relative time of each point with respect to the starting point, and perform linear interpolation on the fused pose formula to obtain the pose increment; 3) Add the pose increments of each point to the lidar observation, perform motion distortion correction, and finally convert the lidar observation to the world coordinate system.

6. The SLAM front-end strategy based on EKF multi-source information fusion according to claim 5, characterized in that: The strategy of correlation scanning and matching (CSM) between the current lidar data frame and the local map is adopted to match the frame and the sub-map. It operates based on a probabilistic grid map, where each grid maintains a logarithmic occupancy probability. For a new input laser frame, all points in the frame data are projected onto the grid map through a predicted pose. Thus, each laser point will fall into a grid. The sum of the logarithmic probabilities of the grids where the laser point is located is the score of the current pose, representing the confidence level of the pose. Finally, the candidate pose with the highest score is calculated and output as the optimal result.

7. The SLAM front-end strategy based on EKF multi-source information fusion according to claim 6, characterized in that: The pose information of the wheel-side odometer and IMU is fused, and interpolation is performed on each LiDAR point in the current frame to perform corresponding pose transformations on each LiDAR point to remove motion distortion. This pose transformation is used as the initial value for the LiDAR scanning matching iteration. Then, based on the front-end odometer obtained by the LiDAR through CSM matching, an EKFMSF-SLAM local sub-graph is constructed: The presence of obstacles in a grid cell is described using probability; a higher probability of occupancy indicates a greater likelihood that an obstacle exists in the grid cell. For each grid cell occupying a grid map, p(s=1) represents the cell as being in an idle state, denoted as Free in the formula. The other state, occupied, is represented by p(s=0), denoted as Occupied in the formula. The grid state is represented by the ratio of the probability of being idle to that of being occupied, as shown in the following formula: Where Odd represents probability. When new data arrives, i.e., when the current observation data z~{0,1} of the lidar arrives, the probability state of the grid needs to be updated. The new state is: Based on Bayes' theorem, the following two equations are derived: Substituting Odd(s|z) into the equation, we get: Taking the logarithm of both sides yields: At this point, only the grid cells contain the measured values. This measurement value has two states: free grid (gridFree) and occupied grid (gridOccupied), and the corresponding expressions are: At this point, let logOdd(s) represent the state of raster s, then the update rule is further simplified to: Among them, S + S represents the state of raster s after the state is updated. - This represents the state before the update; the probability of the grid being idle and occupied is set to 0.5, so the initial state of the grid is S. init for: At this point, updating the state of a grid simply requires adding a probability to the original state, as shown in the following formula: After mathematical modeling, several frames of LiDAR observations are defined as a submap, where the coordinates are real-world coordinates. Then, the map is discretized, with each discrete grid cell having a side length of r. g The position of a point (x, y) on the map within the raster map is (x / r). g y / r g ); At this moment, the autonomous vehicle's state is (x, y, β), where β is the heading angle, the distance between the autonomous vehicle and the lidar is l, the lidar's observation range for the obstacle is d, and the included angle is α. Therefore, the currently observed obstacle's global position is (x, y, β). o ,y o )for: Therefore, the position of the obstacle map in the grid is: At this point, it is assumed that the grid is occupied by an obstacle.

8. The SLAM front-end strategy based on EKF multi-source information fusion according to claim 1, characterized in that... ； The coordinate system transformations employed include: conversion from the lidar coordinate system to the odometer coordinate system, conversion from the odometer coordinate system to the IMU coordinate system using the fusion framework, and finally conversion from the IMU coordinate system to the autonomous vehicle's base coordinate system.

Images