Accurate mapping and path planning control method for mobile robot

Abstract

The invention relates to the field of robot control, in particular to a precise mapping and path planning control method for a mobile robot, and the method comprises the following steps: S1, constructing a 3D map based on an hdlgraphics method; and S2, performing pose updating iteration based on a UKF method, and matching the current laser data with the 3D map through an NDT matching algorithm to obtain an observation value for correction processing. And S3, obtaining an optimal path by adopting a self-adaptive improved ant colony algorithm. According to the method, accurate map construction is carried out on the system on the basis of an hdlgraphslam method, it is considered that the map is kept consistent with a front-end system in 3D laser SLAM, that is, whether the key frame is updated or not is determined by judging the effective matching rate index of the current frame and the key frame, the laser odometer error can be minimized, and the in-situ drifting phenomenon of the odometer pose can be avoided.

Description

Technical Field

[0001] The present invention relates to the field of robot control, and in particular to a precise mapping and path planning control method for mobile robots. Background Art

[0002] Robot navigation technology primarily consists of navigation and positioning technology and path planning technology. Mobile robots utilize various sensors, such as laser rangefinders, ultrasonic sensors, and video cameras, to sense their environment and navigate autonomously. Path planning typically involves planning a route in complex environments based on the robot's sensory perception of the surroundings. This route allows the robot to reach its destination safely and quickly, completing its assigned task during the actual movement. While meeting these requirements, it also requires optimizing the robot's trajectory to meet real-time requirements.

[0003] Most existing two-wheeled mobile robots rely on inertial navigation systems for positioning, mapping, and navigation. However, due to the errors in components of inertial navigation systems, such as gyroscopes and odometers, these errors gradually accumulate over time. This can lead to inaccurate positioning, significant discrepancies between map information and actual conditions, and the inability to execute path planning. Summary of the Invention

[0004] The present invention aims to solve the problems existing in the prior art and provides the following technical solutions:

[0005] The precise mapping and path planning control method for mobile robots includes the following steps:

[0006] S1: Build a 3D map based on the hdl_graph_slam method.

[0007] S2: Perform pose update iteration based on the UKF method, and use the NDT matching algorithm to match the current laser data with the 3D map to obtain observation values for correction processing.

[0008] S3: Adopt the adaptive improved ant colony algorithm to obtain the optimal path.

[0009] As an improvement of the above technical solution, step S1 includes the following steps:

[0010] S101: Detecting a plane based on the point cloud, and fitting the plane using a ground fitting method to obtain a mathematical parameter description of the plane.

[0011] S102: Filter the original point cloud to obtain a sparse point cloud, which is encapsulated as various filter objects in PCL. Several filters constitute a point cloud pre-processing family.

[0012] S103: The original point cloud information is solved by the front-end odometry to obtain the relative pose constraints between the two key frames. The ICP method is used to extract point cloud features and build a LOSS model. Finally, the optimized model is solved and spliced to form a local map.

[0013] S104: Build backend constraints to achieve global graph optimization.

[0014] As an improvement of the above technical solution, the ground fitting method includes the following steps:

[0015] S1011: Construct a representation of the plane equation and the plane normal vector, and calculate the plane normal vector.

[0016] S1012: Fit the ground equation and extract constrained ground points.

[0017] As an improvement to the above technical solution, the constraints include the following formula:

[0018] Ax i +By i +Cz i ∈[-D-δ,-D+δ]

[0019] Among them, A, B, and C represent the three components of the normal vector d, which determine the direction of the plane. D represents the constant term of the plane equation. δ represents the tolerance parameter used to filter ground points. i ,y i , z i Represents the ground centroid coordinates of the i-th ground point.

[0020] As an improvement to the above technical solution, step S104 includes the following steps:

[0021] S1041: Extract key frames, obtain the laser data pose solution between adjacent frames, and then obtain the pose process of the laser radar corresponding to each frame of laser data at the starting time as the laser odometry.

[0022] S1042: Read GPS and IMU information and add them to the probability map, use the distorted laser data of two adjacent frames to perform odometry calculation, and perform motion distortion processing on the current laser frame data based on the calculated posture.

[0023] S1043: Read the ground detection parameters and determine whether to perform matching calculation based on the closed-loop index. If so, calculate the average value of the sum of the Euclidean distances of the nearest neighbor points of the two currently matched laser data.

[0024] S1044: Detect closed loops. If the average value is less than a set threshold, the current matching result is accepted, and the closed loop constraint edge is added to the map for graph optimization and solution and added to the probability map.

[0025] As an improvement to the above technical solution, the closed-loop indicator in step S1043 includes:

[0026] Determine whether the Euclidean distance between two key frames is less than the set threshold.

[0027] Determine whether the robot's travel distance from the last closed-loop keyframe is greater than the set threshold.

[0028] Determine whether the interval from the last closed-loop keyframe is greater than the set threshold.

[0029] If all conditions are met, the matching calculation is performed.

[0030] As an improvement to the above technical solution, the pose update iteration based on the UKF method includes the following steps:

[0031] S21: Construct the motion state model of the system.

[0032] S22: Calculate the prior estimate of the system state model by weighted summation of the predicted values of the Sigma point set, generate a new Sigma point set based on the prior estimate by using an untraceable transformation, and obtain the predicted observation value based on the Sigma point set.

[0033] S23: Perform weighted summation on the predicted observations to obtain the mean and covariance of the system prediction, and calculate the Kalman gain matrix. Finally, calculate the state update and covariance update of the system based on the Kalman gain matrix.

[0034] As an improvement to the above technical solution, the state update and covariance update of the system depend on the following formula:

[0035]

[0036] in, represents the posterior state estimate, is the state estimate in the prediction phase, representing the result based only on the system model and inputs, z k represents the actual observed value, P k+1 represents the posterior covariance matrix, represents the covariance matrix of the predicted observations, K k represents the Kalman gain matrix, which is used to fuse predictions and observations, is the transpose of the gain matrix, represents the mean of the predicted observations, P k represents the prior covariance matrix.

[0037] Beneficial effects of the present invention:

[0038] Based on the hdl_graph_slam method, the system accurately constructs a map, taking into account the consistency with the front-end system in 3D laser SLAM. That is, by judging the effective matching rate index between the current frame and the key frame, it decides whether to update the key frame. This can minimize the error of the laser odometry and avoid the drift of the odometry pose. BRIEF DESCRIPTION OF THE DRAWINGS

[0039] Figure 1 A brief flow chart of the present invention;

[0040] Figure 2 This is a brief flow chart of step S104 of the present invention;

[0041] Figure 3 The figure is a comparison diagram between the present invention and the one before improvement, Figure a is the laser odometer before improvement, and Figure b is the laser odometer after improvement;

[0042] Figure 4 A brief flow chart of the NDT algorithm of the present invention;

[0043] Figure 5 This is a brief flow chart of the adaptive improved ant colony algorithm of the present invention. DETAILED DESCRIPTION

[0044] The following describes the embodiments of the present invention through specific examples. Those skilled in the art will readily understand the other advantages and benefits of the present invention from the disclosure herein. The present invention may also be implemented or applied through various other specific embodiments, and the details in this specification may be modified or altered based on different viewpoints and applications without departing from the spirit of the present invention.

[0045] Most existing two-wheeled mobile robots rely on inertial navigation systems for positioning, mapping, and navigation. However, due to the errors in components of inertial navigation systems, such as gyroscopes and odometers, these errors gradually accumulate over time. This can lead to inaccurate positioning, significant discrepancies between map information and actual conditions, and the inability to execute path planning.

[0046] To resolve this issue, see Figures 1 to 5 , provides a precise mapping and path planning control method for a mobile robot, characterized by comprising the following steps:

[0047] S1: Build a 3D map based on the hdl_graph_slam method.

[0048] The hdl_graph_slam method mainly includes four parts in building 3D scene maps for the system: ground plane detection, point cloud filtering, front-end odometry, and back-end global matching and graph optimization. Specifically, step S1 includes the following steps:

[0049] S101: Detecting a plane based on the point cloud, and fitting the plane using a ground fitting method to obtain a mathematical parameter description of the plane.

[0050] Specifically, the ground fitting method includes the following steps:

[0051] S1011: Construct a representation of the plane equation and the plane normal vector, and calculate the plane normal vector.

[0052] Specifically: take n ground points, calculate the covariance matrix of these n points, and then perform SVD decomposition on it to obtain its various components.

[0053] d=(A,B,C)

[0054] Traverse and add d points close to the ground, A, B, C represent the three components of the normal vector d, and calculate a mean Substitute this mean into the ground equation:

[0055] Right now:

[0056] in, Represents the average coordinate of the ground centroid, which is obtained by calculating the average position of i ground points.

[0057] S1012: Fit the ground equation and extract constrained ground points.

[0058] In step S1011, the mean Because it is the mean of i points, the default is the point closest to the plane where the ground is located.

[0059]

[0060] Right now:

[0061] Where D is the constant term of the plane equation and δ is the tolerance parameter used to filter ground points.

[0062] In the process of filtering ground points for all topics, all points X i =(x i ,y i , z i ) into the above formula to obtain a value that meets the following constraints, including the following formula:

[0063] Ax i +Byi +Cz i ∈[-D-δ,-D+δ]

[0064] Among them, A, B, and C represent the three components of the normal vector d, which determine the direction of the plane. D represents the constant term of the plane equation. δ represents the tolerance parameter used to filter ground points. i ,y i , z i Represents the ground centroid coordinates of the i-th ground point.

[0065] S102: Filter the original point cloud to obtain a sparse point cloud, which is encapsulated as various filter objects in PCL. Several filters constitute a point cloud pre-processing family.

[0066] A relatively complete point cloud pre-processing family is formed by filters with different characteristics. The filtering method needs to be adjusted according to different acquisition methods. In this embodiment, a statistical filter is used.

[0067] S103: The front-end odometry is used to solve the original point cloud information, obtain the relative pose constraints between the two key frames, use the ICP method to extract point cloud features, and build a LOSS model. Finally, the optimization model is solved and stitched to form a local map.

[0068] The specific calculation process includes the following steps:

[0069] Get all the points in the point cloud:

[0070]

[0071] Among them, X, Y are subsets of the original point cloud, and the points selected are the points that can be related to each other in the two point sets, that is, N X =N Y .

[0072] Mapping from Y to X, the equation is:

[0073] X=R*Y+t

[0074] That is:

[0075] XR*Yt=0

[0076] Where X represents the target point cloud, and Y represents the point cloud to be registered. R is the rotation matrix, which rotates point cloud Y to align with X. t is the translation vector, which translates the rotated point cloud Y to coincide with X.

[0077] Since this is a theoretical evaluation, the following LOSS model can be constructed:

[0078]

[0079] Among them, E(R,t) is the registration error function, which represents the average square distance of all corresponding points, x i ∈X represents the i-th point in the target point cloud, y i ∈Y represents the i-th corresponding point in the point cloud to be registered, N y Represents the total number of points in point cloud Y. R represents the rotation matrix, which rotates point cloud Y to align with X. t represents the translation vector, which translates the rotated point cloud Y to coincide with X.

[0080] Iterate using the least squares method:

[0081] x i =R*y i +t

[0082] Right now:

[0083] t=x i -R*y i

[0084] In the above formula, when R is known, t is unique.

[0085] Solve for the displacement matrix t:

[0086]

[0087] Among them, N i Indicates the total number of points in the point cloud.

[0088] Take the derivative of E and set it to 0 to find the extreme value.

[0089]

[0090] Let u x and u y are the centroids of point sets X and Y, respectively, and are expressed as follows:

[0091]

[0092] By introducing the center of mass u x and u y , expand the error function into an expression of relative coordinates. N x and N y Indicates the total number of points in the point cloud X and Y.

[0093] Finally, the SVD method is used to solve the rotation matrix R, and then splicing is performed to realize the construction of the local map.

[0094] S104: Build backend constraints to achieve global graph optimization.

[0095] The step S104 includes the following steps:

[0096] S1041: Extract key frames, obtain the laser data pose solution between adjacent frames, and then obtain the pose process of the laser radar corresponding to each frame of laser data at the starting time as the laser odometry.

[0097] The pose calculation of laser data between adjacent frames in this step can be achieved through matching algorithms such as ICP / NDT. The keyframe method is introduced in the process of implementing the laser odometry. The keyframe is a frame of laser data. If the current laser data is used as the keyframe, the subsequent laser data will be matched with the keyframe. When the pose change value relative to the keyframe obtained by matching a certain frame exceeds the set threshold, the keyframe is replaced with the laser data of the current frame. The advantage of this process is that it can minimize the error of the laser odometry and avoid the phenomenon of drift of the odometry pose in place. The parameters in the calculation process are as follows:

[0098] <param name＝"keyframe_delta_trans"value＝"0.25">

[0099] The keyframe translation change threshold, in meters. If the translation value between the current LiDAR and the LiDAR at the keyframe exceeds this threshold, the keyframe is updated. It is recommended to increase this to 5 for indoor scenes.

[0100] <param name＝"keyframe_delta_angle"value＝"1.0">

[0101] The keyframe angle change threshold is in radians. If the angle between the current LiDAR and the LiDAR at the keyframe is greater than the threshold, the keyframe is updated.

[0102] <param name＝"keyframe_delta_time"value＝"10000.0">

[0103] The key frame time change threshold is in seconds. If the difference between the current laser data sampling time and the key frame sampling time is greater than this threshold, the key frame will be updated.

[0104] <param name＝"max_acceptable_trans"value＝"1.0">

[0105] The threshold for judging whether the current frame of radar data is valid, in meters. That is, if the translation value of the current laser radar and the previous frame's laser radar posture change is greater than the threshold, the current laser frame data is discarded and considered as abnormal data and discarded;

[0106] <param name＝"max_acceptable_angle"value＝"1.0">

[0107] The threshold for judging whether the current frame of radar data is valid, in radians. That is, if the angle between the current laser radar and the previous frame is greater than the threshold, the current laser frame data is discarded and considered as abnormal data.

[0108] S1042: Read GPS and IMU information and add them to the probability map, use the distorted laser data of two adjacent frames to perform odometry calculation, and perform motion distortion processing on the current laser frame data based on the calculated posture.

[0109] The purpose of preprocessing a frame of LiDAR data is to recover the data from noise and motion distortion. This preprocessing allows the data to be restored with the highest possible accuracy. The current logic for processing a frame of distorted LiDAR data is to use high-frequency IMU information (only angular velocity information is used, not linear acceleration information) to perform motion distortion processing. Two adjacent frames of distorted LiDAR data can be used to perform odometry calculations, and the current frame of LiDAR data can be de-distorted based on the calculated pose. The specific parameters are as follows:

[0110] <param name＝"use_distance_filter"value＝"true">

[0111] <param name＝"distance_near_thresh"value＝"0.5">

[0112] <param name＝"distance_far_thresh"value＝"100.0">

[0113] <! --NONE,VOXELGRID,or APPROX_VOXELGRID-->

[0114] <param name＝"downsample_method"value＝"VOXELGRID">

[0115] <param name＝"downsample_resolution"value＝"0.1">

[0116] <! --NONE,RADIUS,or STATISTICAL-->

[0117] <param name＝"outlier_removal_method"value＝"RADIUS">

[0118] <param name＝"statistical_mean_k"value＝"30">

[0119] <param name＝"statistical_stddev"value＝"1.2">

[0120] <param name＝"radius_radius"value＝"0.5">

[0121] <param name＝"radius_min_neighbors"value＝"2">

[0122] S1043: Read the ground detection parameters and determine whether to perform matching calculation based on the closed-loop index. If it is performed, calculate the average value of the sum of the Euclidean distances of the nearest neighbor points of the two currently matched laser data.

[0123] To minimize the cumulative error introduced by the front-end laser odometry system, the robot must be able to effectively identify closed-loop features. The following indicators are usually considered when closing the loop:

[0124] Determine whether the Euclidean distance between two key frames is less than the set threshold;

[0125] Determine whether the robot's travel distance from the last closed-loop keyframe is greater than the set threshold;

[0126] Determine whether the interval from the last closed-loop keyframe is greater than the set threshold;

[0127] If all three of the above conditions are met, the matching calculation is performed.

[0128] The following are some of the more critical parameters:

[0129] <param name＝"distance_thresh"value＝"5.0">

[0130] The Euclidean distance threshold between the corresponding poses of two keyframes in closed-loop matching must be less than the set threshold for matching to be considered.

[0131] <param name＝"accum_distance_thresh"value＝"5.0">

[0132] The cumulative distance threshold between two keyframes in closed-loop matching must be greater than the set threshold to be considered for matching.

[0133] <param name＝"min_edge_interval"value＝"3.0">

[0134] The cumulative walking distance from the last successful closed-loop matching keyframe must be greater than the set threshold for matching to be considered.

[0135] <param name＝"fitness_score_thresh"value＝"1.0">

[0136] Calculate the average Euclidean distance of the nearest neighbor points (calculate the distance between all points in the source point cloud and the nearest neighbor points in the target point cloud, and obtain the nearest neighbor points from the kd-tree) in the results of the two key frames currently matched.

[0137] S1044: Detect closed loops. If the average value is less than a set threshold, the current matching result is accepted, and the closed loop constraint edge is added to the map for graph optimization and solution and added to the probability map.

[0138] The comparison after modeling improvement in this embodiment is as follows Figure 3 shown

[0139] S2: Perform pose update iteration based on the UKF method, and use the NDT matching algorithm to match the current laser data with the 3D map to obtain observation values for correction processing.

[0140] Specifically, the pose update iteration based on the UKF method includes the following steps:

[0141] S21: Construct the motion state model of the system.

[0142] Assume that the motion state model of the system is as follows:

[0143] x k+1 =f(x k ,u k )+w k

[0144] where x k+1 represents the state vector of the system at time k+1, x k represents the state vector of the system at time k, u k represents the control input, w k represents process noise, and f(·) represents the nonlinear state transfer function.

[0145] z k =h(x k )+v k

[0146] Among them, z k represents the observation value at time k, x k represents the state vector of the system at time k, v k represents the observation noise, and h(·) represents the nonlinear observation function.

[0147] S22: Calculate the prior estimate of the system state model by weighted summation of the predicted values of the Sigma point set, generate a new Sigma point set based on the prior estimate by using an untraceable transformation, and obtain the predicted observation value based on the Sigma point set.

[0148] In step S22, a set of sampling points (Sigma) and their corresponding weights are:

[0149]

[0150] in represents the state estimated mean at time k-1, P k-1 represents the state covariance matrix at time k-1, λ represents the scaling parameter, Represents the set of Sigma points, which are used for sampling points of unscented transformation. n is the dimension of the state vector. The variable i represents the index of the Sigma point, which is used to identify different sampling points.

[0151] Calculate the one-step prediction (i.e., prior estimate) of the system state quantity, which is obtained by weighted summation of the predicted values of the Sigma point set.

[0152]

[0153] in represents the prior state estimate, ω (i) Represents the weight of the Sigma point, used for weighted summation, Represents the predicted value of the Sigma point set, and the variable i represents the index of the Sigma point, which is used to identify different sampling points. Indicates the state index of the prediction stage.

[0154]

[0155] Among them, P k represents the prior covariance matrix at time k, represents the prior state estimate, ω (i) Represents the weight of the Sigma point, used for weighted summation, Represents the predicted value of the Sigma point set, Q represents the process noise covariance matrix, n is the dimension of the state vector, and the variable i represents the index of the Sigma point, which is used to identify different sampling points.

[0156] According to the predicted value of one step, an unscented transformation is used here to generate a new Sigma point set.

[0157]

[0158] Substitute the Sigma point set obtained by the above steps into the observation equation to obtain the predicted observation value:

[0159]

[0160] in represents the predicted observation value after being mapped by the observation function, h(·) represents the nonlinear observation function, Represents the Sigma point set.

[0161] S23: Perform weighted summation on the predicted observations to obtain the mean and covariance of the system prediction, and calculate the Kalman gain matrix. Finally, calculate the state update and covariance update of the system based on the Kalman gain matrix.

[0162] The observed predicted values are weighted summed to obtain the mean and covariance of the system prediction

[0163]

[0164] in, represents the mean of the predicted observations, Represents the predicted observation value after mapping by the observation function, ω (i)Indicates the weight of the Sigma point.

[0165]

[0166]

[0167] in, represents the covariance matrix of the predicted observations, R the observation noise covariance matrix, represents the cross-covariance matrix between state and observation, ω (i) represents the weight of the Sigma point, Represents the predicted value of the Sigma point set, represents the mean of the predicted observations, Represents the predicted observation value after mapping through the observation function.

[0168] Calculate the Kalman gain matrix

[0169]

[0170] where K k Represents the Kalman gain matrix, which is used to fuse predictions and observations.

[0171] Finally, the state update and covariance update of the system are calculated:

[0172]

[0173] in, represents the posterior state estimate, z k represents the actual observed value, P k+1 represents the posterior covariance matrix, represents the covariance matrix of the predicted observations, K k represents the Kalman gain matrix, which is used to fuse predictions and observations, represents the mean of the predicted observations, P k represents the prior covariance matrix, is the state estimate in the prediction phase, is the transpose of the gain matrix.

[0174] S3: Adopt the adaptive improved ant colony algorithm to obtain the optimal path.

[0175] Among them, the process of adaptively improving the ant colony algorithm is widely used in the existing technology. This implementation does not make improvements to the alignment solution, so it will not be described in detail. The process of the algorithm is as follows: Figure 5 shown.

[0176] The above embodiments are intended only to illustrate the technical solutions of the present invention and are not intended to limit the same. Anyone skilled in the art may modify or alter the above embodiments without departing from the spirit and scope of the present invention. Therefore, all equivalent modifications or alterations made by one of ordinary skill in the art without departing from the spirit and technical concepts disclosed herein are intended to be covered by the claims of the present invention.

Claims

1. A precise mapping and path planning control method for mobile robots, characterized by: The steps include: S1: Build 3D map based on hdl_graph_slam method; S2: Perform pose update iteration based on the UKF method, and use the NDT matching algorithm to match the current laser data with the 3D map to obtain observation values for correction processing; S3: Adopt the adaptive improved ant colony algorithm to obtain the optimal path.

2. The precise mapping and path planning control method for a mobile robot according to claim 1, characterized in that: The step S1 includes the following steps: S101: Detecting a plane based on the point cloud, and fitting the plane using a ground fitting method to obtain a mathematical parameter description of the plane; S102: Filter the original point cloud to obtain a sparse point cloud, which is encapsulated as various filter objects in PCL. Several filters constitute a point cloud pre-processing family; S103: The front-end odometry is used to solve the original point cloud information, obtain the relative pose constraints between the two key frames, use the ICP method to extract point cloud features, and build a LOSS model. Finally, the optimization model is solved and stitched to form a local map. S104: Build backend constraints to achieve global graph optimization.

3. The precise mapping and path planning control method for a mobile robot according to claim 2, characterized in that: The ground fitting method comprises the following steps: S1011: Construct the representation of the plane equation and the plane normal vector, and calculate the plane normal vector; S1012: Fit the ground equation and extract constrained ground points.

4. The precise mapping and path planning control method for a mobile robot according to claim 3, characterized in that: The constraints include the following: Ax i +By i +Cz i ∈[-D-δ,-D+δ] Among them, A, B, and C represent the three components of the normal vector d, which determine the direction of the plane. D represents the constant term of the plane equation. δ represents the tolerance parameter used to filter ground points. i ,y i , z i Represents the ground centroid coordinates of the i-th ground point.

5. The precise mapping and path planning control method for mobile robots according to claim 1, characterized in that: The step S104 includes the following steps: S1041: Extract key frames, obtain the laser data pose solution between adjacent frames, and then obtain the laser radar pose process corresponding to each frame of laser data at the starting time as the laser odometry; S1042: Read GPS and IMU information and add them to the probability map. Use the distorted laser data of two adjacent frames to perform odometry calculation. De-motion distortion processing is performed on the current laser frame data based on the calculated pose. S1043: Reading ground detection parameters and determining whether to perform matching calculation based on the closed-loop index. If so, calculating the average of the sum of the Euclidean distances of the nearest neighboring points of the two currently matched laser data; S1044: Detect closed loops. If the average value is less than a set threshold, the current matching result is accepted, and the closed loop constraint edge is added to the map for graph optimization and solution and added to the probability map.

6. The precise mapping and path planning control method for mobile robots according to claim 5, characterized in that: The closed-loop indicators in step S1043 include: Determine whether the Euclidean distance between two key frames is less than the set threshold; Determine whether the robot's travel distance from the last closed-loop keyframe is greater than the set threshold; Determine whether the interval from the last closed-loop keyframe is greater than the set threshold; If all conditions are met, the matching calculation is performed.

7. The precise mapping and path planning control method for a mobile robot according to claim 1, characterized in that: The pose update iteration based on the UKF method includes the following steps: S21: Construct the motion state model of the system; S22: Calculate the prior estimate of the system state model by weighted summation of the predicted values of the Sigma point set, generate a new Sigma point set by untraceable transformation according to the prior estimate, and obtain predicted observations according to the Sigma point set; S23: Perform weighted summation on the predicted observations to obtain the mean and covariance of the system prediction, and calculate the Kalman gain matrix. Finally, calculate the state update and covariance update of the system based on the Kalman gain matrix.

8. The precise mapping and path planning control method for a mobile robot according to claim 7, characterized in that: The state update and covariance update of the system depend on the following equations: in, represents the posterior state estimate, is the state estimate in the prediction phase, representing the result based only on the system model and inputs, z k represents the actual observed value, P k+1 represents the posterior covariance matrix, represents the covariance matrix of the predicted observations, K k represents the Kalman gain matrix, which is used to fuse predictions and observations, is the transpose of the gain matrix, represents the mean of the predicted observations, P k represents the prior covariance matrix.

Images