Indoor robot mapping precision optimization method based on Fast-SLAM

Abstract

The invention provides an indoor robot mapping precision optimization method based on Fast-SLAM, and relates to the technical field of mobile robot autonomous positioning and mapping. According to the method, in a front-end odometer module, a motion constraint relationship between laser point cloud frames is constructed based on a range flow model, and an adaptive iterative weighted least square method is introduced, so that the pose inference performance and the response rate are remarkably improved. In the state estimation module, a self-adaptive estimation Kalman filter based on a measurement residual error is adopted, and the measurement noise covariance is dynamically adjusted according to the measurement residual error, so that the adaptability and the stability of the system in a complex environment are enhanced. In a back-end particle filter module, a pose optimization method combining maximum likelihood estimation and gradient search is adopted, so that the resampling error is effectively reduced, and the mapping efficiency is improved. The algorithm framework has good universality and expandability, is suitable for autonomous navigation tasks of various mobile robot platforms, and is particularly suitable for being applied to environments with complex structures or without external positioning signal support.

Description

Technical Field

[0001] The present invention relates to the technical field of autonomous positioning and mapping of mobile robots, and in particular to a method for optimizing indoor robot mapping accuracy based on Fast-SLAM. Background Art

[0002] With the development of mobile robotics, quadruped robots, due to their superior terrain adaptability, have been widely used in complex scenarios such as industrial inspections, disaster relief, and environmental exploration. Indoor navigation, a fundamental supporting capability, directly impacts the robot's overall performance and the quality of its mission.

[0003] Simultaneous localization and mapping (SLAM) is a key technology for autonomous navigation. Existing LiDAR SLAM methods typically rely on mechanisms such as feature matching and graph optimization to construct environmental maps and estimate poses. However, in real indoor scenes, the presence of numerous dynamic obstacles, weakly textured areas, and sensor noise can easily lead to pose estimation drift and decreased mapping accuracy in traditional SLAM systems. Furthermore, Fast-SLAM, as an efficient particle filter algorithm structure, while offering advantages such as strong parallelism and clear data associations, still faces challenges in practical applications, including particle degradation, large resampling errors, and high computational resource consumption, making it difficult to achieve a balanced balance between accuracy and real-time performance.

[0004] Therefore, there is an urgent need for a SLAM algorithm framework that can operate stably in complex indoor environments, with higher positioning accuracy, stronger robustness and better mapping efficiency, especially an optimization solution suitable for quadruped robot platforms. Summary of the Invention

[0005] Purpose of the invention: To address the issues of positioning accuracy and system robustness of robots in complex environments, the present invention proposes a method for optimizing indoor robot mapping accuracy based on Fast-SLAM.

[0006] The present invention proposes a method for optimizing indoor robot mapping accuracy based on Fast-SLAM, comprising the following steps:

[0007] Collect point cloud data through LiDAR and inertial measurement data through IMU, and align the timestamps of the two types of data;

[0008] Based on the continuous frame point cloud data collected by LiDAR, the range flow model is used to construct the motion constraint relationship between laser point cloud frames;

[0009] Perform state fusion on the point cloud data collected by the lidar and the inertial measurement data collected by the IMU;

[0010] The fused state results are input into the Fast-SLAM particle filter module, and the particle pose is optimized using a combined strategy of maximum likelihood estimation and gradient optimization.

[0011] The map is updated based on the optimized particle poses, and the map accuracy is optimized by reducing the mapping error, ensuring that the mapping results are highly consistent with the actual environment.

[0012] In a further embodiment, the core formula of the range flow model is as follows:

[0013]

[0014] Where θ represents the laser radar scanning angle; R a k represents the rate of change of the laser point cloud measurement with respect to the scanning angle; a Indicates the ratio of discrete number to real angle; r represents the laser radar scanning distance; v x,s (*) represents the linear velocity component of the lidar in the x-direction of the local coordinate system; v y,s (*) represents the linear velocity component of the lidar in the y direction of the local coordinate system; w s (*) represents the angular velocity caused by the rotation of the laser radar around its own coordinate axis; R t Indicates the rate of change of laser point cloud measurement over time.

[0015] In a further embodiment, when constructing the motion constraint relationship between laser point cloud frames using the range flow model, the iterative strategy is dynamically controlled based on the inter-frame residual, specifically including:

[0016] The laser residual is optimized and calculated by adaptive weighted least squares method, and the metric data complexity γ is defined as:

[0017]

[0018] Among them, ρ i Residual is used to reflect the overall level of noise in the data; N represents the number of valid points in the scan frame participating in the residual calculation, which is used to standardize the overall residual;

[0019] According to the data complexity γ, the adaptive rule of the number of IRLS iterations is given as follows:

[0020]

[0021] Where M is the number of iterations; a, b, c, and d are preset constants.

[0022] In a further embodiment, at each iteration, the weight w(γ) of each measurement point is updated according to the current residual dynamics, and the weight update formula is:

[0023]

[0024] Where k represents the adjustment factor that controls the influence of residual on weight.

[0025] In a further embodiment, after each iteration, the pose estimate is updated by weighted least squares, and the design weight matrix A is updated by the updated weight matrix W. w and target vector B w :

[0026] A w =WA,B w =WB

[0027] Use the updated weight matrix A w and target vector B w To solve the new pose estimation of the robot

[0028]

[0029] Where, Represents the updated weight matrix A w The transpose of

[0030] Set the convergence threshold ε, if the estimated update error changes in two consecutive rounds of iterations meet:

[0031] |δ (k) -δ (k-1) |<ε

[0032] It is considered to have converged and the iteration is terminated early.

[0033] In a further embodiment, state fusion is performed on the point cloud data collected by the lidar and the inertial measurement data collected by the IMU, specifically including:

[0034] Define the system state vector as

[0035] Where, is the position vector; is the velocity vector; is the attitude represented by the quaternion; is the gyroscope bias; is the accelerometer bias;

[0036] The extended Kalman filter is used to fuse the point cloud data collected by the lidar and the inertial measurement data collected by the IMU. During the extended Kalman filter fusion process, the system state x changes with time and is affected by the control input u. k and process noise w k The state transfer equation is as follows:

[0037] x k =f(x k-1 ,u k-1 )+w k ,w k ~N(0,Q k )

[0038] Where u k Represents the acceleration and angular velocity measured by the IMU, which is used to update the speed, position, and attitude; Q k represents the process noise covariance; x k-1 Represents the system state vector at the previous moment; u k-1 Indicates the acceleration and angular velocity control input measured by the IMU at the last moment; w k represents the system process noise; N(0,Q k ) means the mean is 0 and the covariance is Q k Gaussian white noise;

[0039] The process noise covariance Q k Describe the impact of IMU measurement noise and bias change noise on state prediction, expressed as:

[0040]

[0041] Where, represents the position noise equation; represents the velocity noise variance; represents the attitude noise variance; represents the accelerometer bias variation noise variance; represents the gyroscope bias variation noise variance; I 3×3 represents the third-order identity matrix;

[0042] The laser radar provides position and attitude observations, and the observation equation is:

[0043] z k =h(x k )+v k ,v k ~N(0,R k )

[0044] Where z k is the laser radar measurement value, including the position p k and posture q k .

[0045] The observation equation about x k Take the partial derivative to get the measured Jacobian matrix H:

[0046]

[0047] Based on the measurement residual R k Adaptive estimation of :

[0048] R k =(1-β)R k-1 +β(s k s k T )

[0049] Where β is the smoothing factor, which is used to control the dynamic adjustment amplitude of the measurement noise; R k-1 represents the measurement noise covariance matrix after the last iteration update; s k Represents the measurement residual vector at the current moment; s k T Represents the transpose of the measurement residual vector at the current moment;

[0050] Through the above recursive process, the Kalman gain matrix K is continuously updated k and the error covariance matrix P k , and then get the fused state result.

[0051] In a further embodiment, the fused state estimation results (position x, y and orientation angle θ) are used as input variables to define the objective function S(x t ) This objective function is used to measure the consistency between the particle state and the observed data or map features, and the following gradient calculation is performed:

[0052]

[0053] Compute using the finite difference method:

[0054]

[0055] Where x represents the position component of the particle in the x direction in the local coordinate system; y represents the position component of the particle in the y direction in the local coordinate system; θ represents the orientation angle of the particle; Δx represents the small perturbation step in the x direction used to estimate the gradient in the finite difference method; Δy represents the small perturbation step in the y direction used to estimate the gradient in the finite difference method; Δθ represents the small perturbation step in the angle θ used to estimate the gradient in the finite difference method.

[0056] In a further embodiment, the learning rate α and the linear step size l are introduced. d and angle step a d As an adjustment factor, and perform pose update:

[0057]

[0058] in is the pose estimate of the Kth iteration;

[0059] During the gradient optimization process, set the termination condition:

[0060]

[0061] Among them, ε is the threshold. When the pose change is less than the preset value, it means that the optimal solution has been found.

[0062] In addition, the present invention also discloses an electronic device, which includes: a processor and a memory storing computer program instructions; when the processor executes the computer program instructions, it implements the above-mentioned Fast-SLAM-based indoor robot mapping accuracy optimization method.

[0063] In addition, the present invention also discloses a computer-readable storage medium, which stores at least one executable instruction. When the executable instruction is run on an electronic device, the electronic device executes the above-mentioned Fast-SLAM-based indoor robot mapping accuracy optimization method.

[0064] Beneficial effects: The present invention proposes a method for optimizing indoor robot mapping accuracy based on Fast-SLAM to solve the problems of insufficient positioning accuracy, low mapping efficiency and weak system robustness of robots in indoor environments. In the front-end odometer module, this method constructs the motion constraint relationship between laser point cloud frames based on the range flow model, and introduces the adaptive iterative weighted least squares method to significantly improve the pose inference performance and response rate. In the state estimation module, an adaptive estimation Kalman filter based on measurement residuals is adopted to dynamically adjust the measurement noise covariance according to the measurement residuals, thereby enhancing the adaptability and stability of the system in complex environments. In the back-end particle filter module, the pose optimization method combining maximum likelihood estimation and gradient search is used to effectively reduce the resampling error and improve mapping efficiency. The algorithm framework has good versatility and scalability, and is suitable for autonomous navigation tasks of various types of mobile robot platforms, especially for applications in environments with complex structures or without external positioning signal support. BRIEF DESCRIPTION OF THE DRAWINGS

[0065] Figure 1 This is a laser radar scanning image in an embodiment of the present invention.

[0066] Figure 2 The following is a flowchart of optimizing the Fast-SLAM algorithm in an embodiment of the present invention. DETAILED DESCRIPTION

[0067] In the following description, numerous specific details are provided to provide a more thorough understanding of the present invention. However, it will be apparent to those skilled in the art that the present invention may be practiced without one or more of these details. In other instances, certain technical features well known in the art have not been described to avoid confusion with the present invention.

[0068] This embodiment proposes a method for optimizing indoor robot mapping accuracy based on Fast-SLAM. To solve the problems of particle degradation, error accumulation and filter divergence in traditional Fast-SLAM, a systematic optimization scheme is proposed from three aspects: front-end odometer, state estimation and particle filtering. The scheme has higher positioning accuracy, better mapping accuracy and better positioning accuracy. Figure 1 consistency and operational stability.

[0069] Front-end odometry optimization: Based on the range flow model, an adaptive iteratively weighted least squares (IRLS) method is proposed. This method dynamically adjusts the number of iterations based on the complexity of the residual error between laser frames. Compared with traditional fixed iteration strategies, this method effectively avoids estimation divergence in high-noise environments and redundant calculations in low-noise environments. While ensuring estimation accuracy, it significantly shortens the computation cycle and meets the real-time requirements of embedded platforms.

[0070] State estimation optimization: An adaptive estimation Kalman filter based on measurement residuals is used. The dynamic adjustment mechanism of the observation noise covariance driven by the measurement residuals can automatically enhance the robustness of the filter and reduce the probability of state estimation divergence when the environmental disturbance is severe or the perception error is large. Compared with the traditional EKF, it has stronger adaptability.

[0071] In terms of particle filter optimization: Maximum likelihood estimation and gradient optimization strategies are used for particle pose optimization, replacing the traditional predictive distribution sampling mechanism, and combined with the effective number of particles to control adaptive resampling, which significantly reduces particle degradation and trajectory jumping phenomena, while improving mapping quality and computational efficiency. It is particularly suitable for embedded quadruped platforms with limited computing resources.

[0072] The indoor robot mapping accuracy optimization method disclosed in this embodiment includes the following implementation steps:

[0073] The first step is to collect lidar point cloud data and IMU inertial measurement data, including three-axis acceleration and angular velocity information, and align the data timestamps through the time synchronization module to eliminate time deviations between multiple sensors.

[0074] In the second step, based on the continuous frame point cloud data collected by the lidar, the range flow model is used to estimate the inter-frame geometric constraints, and the residual is optimized and calculated through the adaptive weighted least squares method (IRLS). The number of iterations is dynamically adjusted, and the error convergence conditions are set. By dynamically controlling the iterative strategy of the inter-frame residual, more efficient motion estimation output is achieved.

[0075] In the third step, based on the IMU predicted pose results and the lidar odometry estimation results, the extended Kalman filter method is used for state fusion. At the same time, an adaptive observation covariance update mechanism driven by measurement residuals is introduced to realize dynamic modeling of observation uncertainty and improve the robustness of state estimation in complex environments.

[0076] In the fourth step, the fused state estimation results are fed into the Fast-SLAM particle filter module, which optimizes the particle pose using a combined maximum likelihood estimation and gradient optimization strategy, replacing traditional sampling methods to improve pose estimation accuracy and reduce sampling errors. Simultaneously, the module calculates the effective number of particles based on the current particle distribution characteristics, adaptively controls resampling timing, and dynamically adjusts the frequency, effectively alleviating particle degradation and improving the consistency of map construction and the stability of system operation.

[0077] The following is a detailed derivation of the core algorithm expressions of each module from three aspects: front-end odometry optimization, state estimation optimization, and particle filter optimization:

[0078] First, regarding the front-end odometry module: the lidar odometry estimates the robot's pose transformation in the local environment by analyzing the geometric changes in the scan data between consecutive frames. The range flow method continuously estimates the robot's translation and rotational motion by establishing a mathematical relationship between the time-dependent range derivative and angular velocity of each laser point. This method can construct inter-frame motion constraints without relying on explicit feature extraction, such as Figure 1 Schematic diagram of the time variation of the scanning points.

[0079] The core formula of the range flow algorithm is as follows:

[0080]

[0081] From formula (1), we can see that the motion speed and rotation of the LiDAR scanning point work together to affect the estimation of the change in posture.

[0082] LiDAR ranging data is susceptible to interference from environmental noise and outliers during acquisition, resulting in reduced pose estimation accuracy, especially in high-frequency updates and complex scenes. Traditional range flow algorithms often use the IRLS method with a fixed number of iterations (set to 5). This wastes resources in low-noise scenarios and can affect estimation stability in high-noise conditions due to insufficient iterations.

[0083] To improve adaptability, an adaptive IRLS strategy is introduced to dynamically adjust the number of iterations according to data complexity and realize early convergence judgment in combination with error changes, thereby improving computational efficiency while ensuring accuracy.

[0084] Therefore, a new metric data complexity γ is defined, which reflects the overall level of noise in the data. Its calculation formula is as follows:

[0085]

[0086] Among them, ρ i represents the residual, which reflects the overall level of noise in the data.

[0087] According to the data complexity γ, the adaptive rule of the number of IRLS iterations is further given as follows:

[0088]

[0089] Where M is the number of iterations, which is set to a=3, b=5, c=6, and d=8.

[0090] At each iteration, the algorithm updates the weight w(γ) of each measurement point according to the current residual dynamics to reduce the impact of noise on the final estimation result. The weight update formula is:

[0091]

[0092] Moreover, after each iteration, the algorithm updates the pose estimate by weighted least squares. Specifically, the design weight matrix A is updated by the updated weight matrix W. w and target vector B w :

[0093] A w =WA,B w =WB(5)

[0094] Then, use the updated weight matrix A w and target vector B w To solve the new pose estimate of the robot:

[0095]

[0096] To reduce redundant iterations, a convergence threshold ε is set. If the estimated update error changes in two consecutive iterations satisfy:

[0097] |δ (k) -δ (k-1) |<ε (7)

[0098] If convergence is considered, the iteration can be terminated early.

[0099] Secondly, for the state estimation module, the extended Kalman filter (EKF) is used to fuse the IMU and lidar data to improve the accuracy and robustness of state estimation in dynamic environments. The specific method is:

[0100] In the prediction step, the system state vector is defined as:

[0101]

[0102] in: is the position vector; is the velocity vector; is the attitude represented by the quaternion; is the gyroscope bias; is the accelerometer bias.

[0103] In the EKF prediction step, the system state changes with time and is affected by the control input u k and process noise w k The state transfer equation is as follows:

[0104] x k =f(x k-1 ,u k-1 )+w k ,w k ~N(0,Q k )(9)

[0105] where u k Represents the acceleration and angular velocity measured by the IMU, which is used to update velocity, position, and attitude.

[0106] We expand it collectively as:

[0107]

[0108] Since the state transfer equation is nonlinear, we directly use the process noise covariance Q k Propagation error covariance:

[0109]

[0110] The process noise covariance matrix Qk describes the impact of IMU measurement noise and bias change noise on state prediction, which can be expressed as:

[0111]

[0112] Next is the update step. The laser odometry provides position and attitude observations, and we get the observation equation as:

[0113] z k =h(x k )+v k ,v k ~N(0,R k )(13)

[0114] where z k is the laser radar measurement value, including the position p k and posture q k .

[0115] Similarly, for the measurement equation with respect to x k Take the partial derivative to get the measured Jacobian matrix H:

[0116]

[0117] In the traditional EKF scheme, the covariance of the observation noise R k Set to a fixed value:

[0118]

[0119] Through the above recursive process, the Kalman gain matrix K is continuously updated k Error covariance matrix p k , and then realize the state estimation of the system.

[0120] In complex environments, measurement noise may change dynamically over time. If it is set to a fixed value, the filter performance may be seriously affected in dynamic environments or when the measurement error is uncertain. Therefore, adaptive estimation based on measurement residuals is adopted, thus abandoning the traditional EFK scheme:

[0121] R k =(1-β)R k-1 +β(s k s k T )

[0122]

[0123] Where β is the smoothing factor, which is used to control the dynamic adjustment amplitude of the measurement noise, and is the measurement residual. When the measurement noise is small, R k Maintaining a small value makes the filtering more dependent on the observed value. When the measurement noise is large, R k Maintaining a larger value makes the filtering more dependent on the observed values and reduces the impact of erroneous measurements.

[0124] Through the above recursive process, the Kalman gain matrix K is continuously updated k and the error covariance matrix P k , and then realize the state estimation of the system.

[0125] Finally, regarding the particle filter module, the Gmapping algorithm, an extension of Fast-SLAM, demonstrates good mapping performance in small environments. It uses scan matching to optimize particle poses and introduces the effective number of particles, Neff, to control the resampling frequency, resulting in a certain degree of robustness. However, while this method improves Fast-SLAM, it still has some shortcomings.

[0126] Therefore, based on the existing Gmapping algorithm, its Fast-SLAM particle filter architecture and Neff indicator design are retained to further optimize Fast-SLAM.

[0127] Gmapping uses a discrete search MLE method, which requires trying candidate points in six directions. The discrete search has high computational complexity, and multiple candidate points need to be calculated in each time step. The step size is fixed, the error is large, and the posture cannot be fine-tuned. It is easy to fall into a local optimal solution, which will cause the trajectory to jump or drift.

[0128] Therefore, in order to reduce the amount of calculation and improve the estimation accuracy, the gradient optimization method is used instead of discrete search to optimize the objective function S(x t ) to calculate the gradient:

[0129]

[0130] Compute using the finite difference method:

[0131]

[0132] In order to take into account the scale differences of different dimensions, the learning rate α and linear step size l are introduced. d and angle step a d As an adjustment factor, and perform pose update:

[0133]

[0134] in is the pose estimate at the Kth iteration.

[0135] During the gradient optimization process, it cannot be iterated indefinitely, and a termination condition needs to be set:

[0136]

[0137] Among them, ε is the threshold. When the pose change is very small, it means that the optimal solution has been found.

[0138] The technical process of the Fast-SLAM-based indoor robot mapping accuracy optimization method disclosed in the above embodiment can be implemented in whole or in part through software, hardware, firmware or any other combination.

[0139] When implemented using hardware, the aforementioned embodiments can all or partly compile the operating logic and computational processes into software and then run them on an electronic device. The electronic device includes a processor, memory, a communication interface, and a communication bus. The processor, memory, and communication interface communicate with each other via the communication bus. The memory is used to store at least one executable instruction that causes the processor to execute the technical process of the Fast-SLAM-based indoor robot mapping accuracy optimization method disclosed in the aforementioned embodiments.

[0140] When implemented using software, the above embodiments can be implemented in whole or in part in the form of a computer program product. The computer program product includes one or more computer instructions or computer programs. If the above method is implemented in the form of a software function module and sold or used as an independent product, it can also be stored in a computer-readable storage medium. Based on this understanding, the technical solution of the embodiment of the present application can be embodied in the form of a software product in essence or in other words, the part that contributes to the relevant technology. The software product is stored in a storage medium and includes several instructions to enable an electronic device (which can be a personal computer, a server, or a network device, etc.) to execute all or part of the methods described in each embodiment of the present application. The aforementioned storage medium includes various media that can store program codes, such as a U disk, a mobile hard disk, a read-only memory (ROM), a magnetic disk or an optical disk. In this way, the embodiment of the present application is not limited to any specific hardware, software or firmware, or any combination of hardware, software and firmware.

[0141] As described above, although the present invention has been shown and described with reference to specific preferred embodiments, it should not be construed as limiting the present invention itself. Various changes may be made to it in form and detail without departing from the spirit and scope of the present invention as defined in the appended claims.

Claims

1. A Fast-SLAM-based indoor robot mapping accuracy optimization method, characterized in that: include: Collect point cloud data through LiDAR and inertial measurement data through IMU, and align the timestamps of the two types of data; Based on the continuous frame point cloud data collected by LiDAR, the range flow model is used to construct the motion constraint relationship between laser point cloud frames; Perform state fusion on the point cloud data collected by the lidar and the inertial measurement data collected by the IMU; The fused state results are input into the Fast-SLAM particle filter module, and the particle pose is optimized using a combined strategy of maximum likelihood estimation and gradient optimization. Update the map based on the optimized particle poses.

2. The method for optimizing indoor robot mapping accuracy based on Fast-SLAM according to claim 1, wherein: The core formula of the range flow model is as follows: Where θ represents the laser radar scanning angle; R a k represents the rate of change of the laser point cloud measurement with respect to the scanning angle; a Indicates the ratio of discrete number to real angle; r represents the laser radar scanning distance; v x,s (*) represents the linear velocity component of the lidar in the x-direction of the local coordinate system; v y,s (*) represents the linear velocity component of the lidar in the y direction of the local coordinate system; w s (*) represents the angular velocity caused by the rotation of the laser radar around its own coordinate axis; R t Indicates the rate of change of laser point cloud measurement over time.

3. The method for optimizing indoor robot mapping accuracy based on Fast-SLAM according to claim 2, wherein: When using the range flow model to construct the motion constraint relationship between laser point cloud frames, the iterative strategy is dynamically controlled based on the inter-frame residual, specifically including: The laser residual is optimized and calculated by adaptive weighted least squares method, and the metric data complexity γ is defined as: Among them, ρ i Residual is used to reflect the overall level of noise in the data; N represents the number of valid points in the scan frame participating in the residual calculation; According to the data complexity γ, the adaptive rule of the number of IRLS iterations is given as follows: Where M is the number of iterations; a, b, c, and d are preset constants.

4. The method for optimizing indoor robot mapping accuracy based on Fast-SLAM according to claim 3, wherein: At each iteration, the weight w(γ) of each measurement point is updated according to the current residual dynamics. The weight update formula is: Where k is the adjustment factor that controls the influence of residual on weight.

5. The method for optimizing indoor robot mapping accuracy based on Fast-SLAM according to claim 3 or 4, characterized in that: After each iteration, the pose estimate is updated by weighted least squares, and the design weight matrix A is updated by the updated weight matrix W. w and target vector B w : A w =WA, B w =WB Use the updated weight matrix A w and target vector B w To solve the new pose estimation of the robot Where, Represents the updated weight matrix A w The transpose of Set the convergence threshold ε, if the estimated update error changes in two consecutive rounds of iterations meet: |d (k) -d (k-1) |<e It is considered to have converged and the iteration is terminated early.

6. The method for optimizing indoor robot mapping accuracy based on Fast-SLAM according to claim 1, wherein: The state fusion of the point cloud data collected by the lidar and the inertial measurement data collected by the IMU includes: Define the system state vector as Where, is the position vector; is the velocity vector; is the attitude represented by the quaternion; is the gyroscope bias; is the accelerometer bias; The extended Kalman filter is used to fuse the point cloud data collected by the lidar and the inertial measurement data collected by the IMU. During the extended Kalman filter fusion process, the system state x changes with time and is affected by the control input u. k and process noise w k The state transfer equation is as follows: x k =f(x k-1 ,u k-1 )+w k ,w k ~N(0,Q k ) Where u k Represents the acceleration and angular velocity measured by the IMU, which is used to update the speed, position, and attitude; Q k represents the process noise covariance; x k-1 Represents the system state vector at the previous moment; u k-1 Indicates the acceleration and angular velocity control input measured by the IMU at the last moment; w k represents the system process noise; N(0,Q k ) means the mean is 0 and the covariance is Q k Gaussian white noise; The process noise covariance Q k Describe the impact of IMU measurement noise and bias change noise on state prediction, expressed as: Where, represents the position noise equation; represents the velocity noise variance; represents the attitude noise variance; represents the accelerometer bias variation noise variance; represents the gyroscope bias variation noise variance; I 3×3 represents the third-order identity matrix; The laser radar provides position and attitude observations, and the observation equation is: z k =h(x k )+v k ,v k ~N(0,R k ) Where z k is the laser radar measurement value, including the position p k and posture q k . The observation equation about x k Take the partial derivative to get the measured Jacobian matrix H: Based on the measurement residual R k Adaptive estimation of : R k =(1-β)R k-1 +β(s k s k T ) Where β is the smoothing factor, which is used to control the dynamic adjustment amplitude of the measurement noise; R k-1 represents the measurement noise covariance matrix after the last iteration update; s k Represents the measurement residual vector at the current moment; s k T Represents the transpose of the measurement residual vector at the current moment; Through the above recursive process, the Kalman gain matrix K is continuously updated k and the error covariance matrix P k , and then get the fused state result.

7. The method for optimizing indoor robot mapping accuracy based on Fast-SLAM according to claim 6, characterized in that: The fused state estimation result is used as the input variable to define the objective function S(x t ), the objective function is used to measure the consistency between the particle state and the observed data or map features, and the gradient calculation is performed as follows: Compute using the finite difference method: Where x represents the position component of the particle in the x direction in the local coordinate system; y represents the position component of the particle in the y direction in the local coordinate system; θ represents the orientation angle of the particle; Δx represents the small perturbation step in the x direction used to estimate the gradient in the finite difference method; Δy represents the small perturbation step in the y direction used to estimate the gradient in the finite difference method; Δθ represents the small perturbation step in the angle θ used to estimate the gradient in the finite difference method.

8. The method for optimizing indoor robot mapping accuracy based on Fast-SLAM according to claim 7, characterized in that: Introducing learning rate α and linear step size l d and angle step a d As an adjustment factor, and perform pose update: in is the pose estimate of the Kth iteration; During the gradient optimization process, set the termination condition: Among them, ε is the threshold. When the pose change is less than the preset value, it means that the optimal solution has been found.

9. An electronic device, characterized in that: The device includes: a processor and a memory storing computer program instructions; when the processor executes the computer program instructions, it implements the indoor robot mapping accuracy optimization method based on Fast-SLAM as described in any one of claims 1 to 7.

10. A computer-readable storage medium, characterized in that The storage medium stores at least one executable instruction, and when the executable instruction is executed on the electronic device, the electronic device executes the indoor robot mapping accuracy optimization method based on Fast-SLAM according to any one of claims 1 to 7.

Images