Multi-sensor cross-scene dynamic preferential fusion positioning and mapping method

Abstract

The invention relates to a multi-sensor cross-scene dynamic preferential fusion positioning and mapping method, and the method comprises the steps: obtaining the data of a plurality of sensors, and completing the unification of the time-space relation of the data of the plurality of sensors; processing the data, carrying out loopback detection on image key frame data acquired by a camera, constructing to obtain an I MU pre-integration factor, a visual inertial odometer factor, a laser radar odometer factor, a GPS inertial odometer factor, a UWB factor, a GNSS factor and a loopback detection factor, and adding the factors into a factor graph for optimization; a global positioning pose and a map are obtained; and optimizing the multi-sensor data fusion strategy based on a deep fuzzy neural network. According to the multi-sensor cross-scene dynamic preferential fusion positioning and mapping method provided by the invention, high-precision positioning and navigation of an agricultural robot in different scenes are realized through real-time fusion of various sensor data, and the problems of scene dependence and insufficient precision of an existing single sensor scheme are solved.

Description

Technical Field

[0001] The present invention relates to the technical field of perception and positioning of facility agricultural robots, and provides a multi-sensor cross-scene dynamic optimal fusion positioning and mapping method, specifically a multi-sensor cross-scene dynamic optimal fusion positioning and mapping method for facility agricultural robots. Background Art

[0002] In facility agriculture, agricultural robots, as key tools for improving production efficiency and flexibility, have been widely used in different scenarios such as greenhouses, planting areas, and outdoor operations. With the diversification of task scenarios in facility agriculture, robots face many challenges when operating across different scenarios, especially in terms of positioning and navigation accuracy requirements. The scenarios in facility agriculture usually include indoor, greenhouse planting areas, and outdoor roads, etc. The differences in environmental characteristics in these scenarios bring great difficulties to robot positioning and navigation. To address this challenge, SLAM technology has been widely applied to agricultural robot positioning, and it has achieved adaptation to complex environments by constructing an environmental map and self-positioning in real time. However, existing SLAM technologies mostly rely on the input of a single sensor, resulting in unstable accuracy in different environments. Especially under changing environmental conditions, the limitations of a single sensor become more obvious.

[0003] After retrieval, Chinese invention patent publication No. CN114964234A discloses a multi-sensor loose coupling indoor and outdoor synchronous positioning and mapping method and system. The method includes the steps of preprocessing point cloud data and inertial data to obtain image view data of the point cloud and rough pose data of the vehicle; performing scene recognition, feature extraction, and stitching on the point cloud data to obtain an unoptimized point cloud map; and optimizing the rough pose data and the point cloud map through a backend optimization algorithm and a loop closure detection algorithm to finally achieve synchronous positioning and mapping. When dealing with the feature differences between different scenarios, the accuracy of this existing patent is limited, and it has not been optimized for the information fusion of multi-sensors, so there are certain technical bottlenecks.

[0004] Therefore, although the existing technology has solved the positioning and mapping problems to a certain extent, there are still defects in the insufficient fusion of multi-sensor data. Especially when switching operations among multiple scenarios, it is impossible to automatically select the best sensor data for fusion according to the environmental changes. Summary of the Invention

[0005] To solve at least some of the above problems in the existing technology, the present invention provides a multi-sensor cross-scene dynamic optimal fusion positioning and mapping method.

[0006] The method includes a multi-sensor data acquisition and preprocessing module, an IMU pre-integration module, a visual-inertial odometry module, a lidar-inertial odometry module, a GPS-inertial odometry module, a GNSS module, a UWB module, a loop detection module, a factor graph construction and optimization module, and a multi-sensor fusion and strategy scheduling module.

[0007] The multi-sensor data acquisition and preprocessing module acquires data from various heterogeneous sensors such as lidar, IMU, camera, GPS, GNSS, UWB, etc., and unifies the spatio-temporal relationship of multi-sensor data.

[0008] The IMU pre-integration module calculates the acceleration and angular velocity data of the IMU through integration, obtains the displacement and rotation information of the robot, and adds this information as a constraint to the factor graph to establish an IMU pre-integration factor.

[0009] The visual-inertial odometry module constructs a visual-inertial odometry factor by extracting visual key features such as SURF, matching based on the FLANN algorithm (Fast Approximate Nearest Neighbor algorithm) of visual descriptors and camera motion estimation, and establishing a visual-inertial odometry factor according to the pose estimation result.

[0010] The lidar-inertial odometry module designs point, line, and plane three-dimensional feature detectors to detect the point cloud structure features, constructs the lidar-inertial odometry based on the ICP algorithm (Iterative Closest Point algorithm) and the NDT algorithm (Normal Distribution Transform algorithm) transformation, and introduces the result of the pose transformation as a lidar odometry factor into the factor graph.

[0011] The GPS-inertial odometry module provides position information to constrain the position of the robot, predicts the robot state through IMU data, and establishes a GPS factor based on the pose constraint relationship one.

[0012] The GNSS module constrains the pose of the robot by providing global position information, minimizes the error to correct the pose estimation of the robot, and performs joint optimization in combination with other sensors.

[0013] The UWB module measures the distance between the robot and the UWB base station through wireless communication technology, provides additional constraints for positioning, and optimizes the pose estimation by minimizing the error between multiple UWB base stations with the ranging information of the UWB system.

[0014] The loop detection module uses the DBoW2 loop detection algorithm to perform loop detection on the key frame data of the images collected by the camera. For each new image key frame, it extracts the BRIEF descriptor and matches it with the previously extracted visual descriptors. The image timestamp of the loop candidate returned by DBoW2 is passed to the lidar-inertial system for further verification, and a loop detection factor is established based on the pose constraint relationship four.

[0015] The factor graph construction and optimization module is used to construct the robot pose as a factor graph variable node, and construct factors of variables from IMU pre-integration factors, visual-inertial odometry factors, lidar odometry factors, GPS-inertial odometry factors, UWB factors, GNSS factors and loop detection factors. These factors are used as observation factors and added to the factor graph optimization to obtain the optimal pose solution at different times, and jointly optimized to obtain the global positioning pose and map;

[0016] The multi-sensor fusion and strategy scheduling module establishes an online inference mechanism for multi-sensor confidence based on a deep fuzzy neural network, evaluates the confidence of each sensor in the current scenario, and dynamically adjusts the sensor weights in the factor graph accordingly to optimize the multi-sensor data fusion strategy.

[0017] Among them, the IMU pre-integration factor is added to the factor graph as a constraint, which reflects the movement of the robot in a short time and provides inertial information constraints for pose estimation; the visual-inertial odometry factor combines camera visual information and IMU inertial information, calculates the robot pose transformation by extracting and matching image feature points and combining the movement information of the IMU, and provides visual and inertial fusion movement constraints for the factor graph; the lidar odometry factor is obtained based on lidar point cloud data processing, calculates the pose transformation between adjacent lidar frames to obtain the movement information of the robot, and is used to constrain the pose relationship between state nodes, which is a movement constraint based on lidar data; the GPS-inertial odometry factor uses the position information provided by GPS (Global Positioning System) as an external absolute position constraint to correct the pose estimation of the robot and reduce the cumulative error; the UWB factor is used to more accurately constrain the position of the robot in a local area, especially in an environment with poor GPS signals, and provides additional position constraints; the GNSS factor provides a global position reference for the robot, constrains the pose of the robot, and improves the accuracy of positioning; the loop detection factor is used to correct the cumulative error of the robot pose estimation and make the constructed map more accurate and consistent.

[0018] The present invention provides a multi-sensor cross-scene dynamic optimal fusion positioning and mapping method, which is used for facility agricultural robots, and the method includes:

[0019] S1. Obtain the data of multiple sensors and complete the unification of the spatio-temporal relationship of the data of multiple sensors. The multiple sensors include IMU (Inertial Measurement Unit), camera, lidar, GPS, UWB (Ultra-Wideband), GNSS (Global Navigation Satellite System);

[0020] S2 - S11. Process the data, perform loop detection on the key - frame data of the images collected by the camera, construct the IMU pre - integration factor, visual - inertial odometry factor, lidar odometry factor, GPS - inertial odometry factor, UWB factor, GNSS factor, and loop - detection factor, and add the IMU pre - integration factor, visual - inertial odometry factor, lidar odometry factor, GPS - inertial odometry factor, UWB factor, GNSS factor, and loop - detection factor into the factor graph for optimization to obtain the global - positioning pose and the map;

[0021] S12. Based on the deep fuzzy neural network (DFNN), establish an online inference mechanism for multi - sensor confidence, evaluate the confidence of the multi - sensors in the current scenario, and dynamically adjust the sensor weights in the factor graph according to the confidence to optimize the multi - sensor data - fusion strategy, that is, realize the dynamic optimal fusion positioning and mapping of multi - sensors across scenarios.

[0022] Further, in step S1, static calibration and dynamic online self - calibration are adopted to achieve the unity of the spatio - temporal relationship of multi - sensor data, specifically including:

[0023] S1 - 1. In static calibration, set the coordinate system of one sensor as the reference coordinate system, and the coordinate systems of other sensors are transformed into the reference coordinate system through the rotation matrix R i and the translation vector t i The spatio - temporal transformation between multi - sensors is expressed by the following formula:

[0024]

[0025] where, T i is the transformation matrix of sensor i, R i is the rotation matrix of sensor i, representing the rotation transformation of sensor i relative to the reference coordinate system, and t i is the translation vector of sensor i;

[0026] S1 - 2. In the dynamic online self - calibration stage, the spatio - temporal parameters have obtained initial values through static calibration. Project the motion trajectory obtained by multi - sensor pose estimation into the inertial - measurement unit coordinate system, and use the trajectory - similarity principle to measure the difference between two trajectories through the Frechet distance. Assume there are two trajectories X1 = {x1(t)} and X2 = {x2(t)};

[0027] The Frechet distance is defined as:

[0028]

[0029] where, α(t) is a monotonically increasing mapping, representing the correspondence relationship between trajectories;

[0030] The objective function constructed by the Frechet distance cost is used for the online optimization of the spatio-temporal parameters, and the optimization objective is to minimize the sum of squared errors to obtain the optimal spatio-temporal parameters:

[0031]

[0032] where T is the spatio-temporal parameter to be optimized, and X1(t) and X2(t) are the estimated trajectories from any two sensors respectively; and / or

[0033] The multi-sensors are set to be rigidly connected, and the external measurement method or the true value mapping method is used for the feature registration of the target points, lines, and surfaces to obtain the initial values of the spatio-temporal parameters between each sensor system.

[0034] Further, steps S2 - S11 include:

[0035] S2. Measure the inertial data of the robot through the IMU. The inertial data includes acceleration and angular velocity. The inertial data is integrated to obtain the preliminary pose estimation information of the robot (the calculation result of IMU pre-integration), that is, the displacement and rotation information of the robot;

[0036] S3. Obtain the first-frame lidar point cloud through the lidar, use the preliminary pose estimation information to correct the distortion of the lidar point cloud data to obtain the current lidar point cloud, and register the preliminary pose estimation information with the current lidar frame to correct the spatial position of the current lidar point cloud.

[0037] For the original information collected by the lidar during the scanning process, first correct the distortion of the lidar point cloud data. This is because during the movement of the robot, the self-movement of the lidar when collecting data may cause the data to be distorted. Through the preliminary pose estimation (the pose information of the IMU), this kind of distortion can be corrected, that is, the lidar point cloud is registered with the current lidar frame through the predicted pose of the IMU to correct the spatial position of the point cloud.

[0038] Further, in step S2, if the GPS signal exists, the pose estimation of the IMU will be corrected by the GPS data.

[0039] Further, steps S2 - S11 also include:

[0040] S4. Introduce the preliminary pose estimation information as a constraint into the factor graph to establish an IMU pre-integration factor. The IMU pre-integration factor will further improve the positioning accuracy and reduce the influence brought by sensor errors or cumulative errors, specifically including:

[0041] S4-1. Integrate the acceleration to obtain the displacement increment Δp of the robot within the time interval k, the calculation formula is:

[0042]

[0043] where Δt = t f ―t0 is the time interval, v k is the velocity of the robot at t k , a k is the acceleration of the robot at t k , v0 is the initial velocity of the robot at t0, p0 is the initial position of the robot at t0, and a(t) is the acceleration of the robot at t0;

[0044] S4-2. Integrate the angular velocity to obtain the rotation increment ΔR of the robot k , and the calculation formula is:

[0045]

[0046] where ΔR k is the rotation matrix, representing the rotation increment of the robot from the initial time t0 to the current time, exp(·) is the exponential map of the Lie group, used to describe the rotation transformation, and the angular velocity ω(t) is used to describe the rotation change of the robot during the movement;

[0047] S4-3. Construct the IMU pre-integration factor. The IMU pre-integration factor represents the pose of the robot through the nodes x k and x k+1 . The nodes x k and x k+1 correspond to the poses of the robot at times t k and t k+1 . The constraint expression of the IMU pre-integration factor is:

[0048] f k = [Δp k , ΔR k

[0049] In the factor graph, the constraint of the IMU pre-integration factor will affect all pose variables related to the nodes x k and x k+1 . Therefore, minimize the residual r k of the IMU pre-integration factor to optimize the pose estimation. The residual r k is calculated as follows:

[0050] r k = f k ―f IMU (p k , R k , p k―1 , R k―1 ​)

[0051] Among them, f IMU (·) is used to calculate the IMU pre-integration factor, and estimate the pose change of the robot between two time points based on the IMU data.

[0052] Furthermore, steps S2 - S11 also include:

[0053] S5. Extract significant SURF visual key feature points and visual descriptors, establish a visual-inertial odometry factor based on the FLANN algorithm matching of the visual descriptors and the motion estimation of the camera, and add the visual-inertial odometry factor to the factor graph to optimize the pose estimation, specifically including:

[0054] S5-1. The SURF visual key feature point extraction process is expressed as:

[0055] f = SURF(I) = {(P i , d i )}

[0056] Among them, f is the set of extracted feature points, P i = (x i , y i ) represents the position of the feature point, and d i is the descriptor of this feature point; each feature point consists of the position P i = (x i , y i ) and the descriptor d i , and the descriptor is the gradient information of the area around this feature point.

[0057] S5-2. Use the FLANN algorithm to match the visual descriptors to find the corresponding feature point pairs of the same or similar objects in two frames of images. For each pair of feature point pairs, calculate the Euclidean distance between the visual descriptors to select the best matching pair. The distance metric of the matching process is expressed as:

[0058] d(d i , d j ) = ‖d i ― d j ‖

[0059] Among them, d i and d j are the descriptors of the corresponding feature points P i and P j in two frames of images respectively;

[0060] S5-3. Use the matched feature point pairs to estimate the relative motion of the camera through the essential matrix E or the homography matrix H; the essential matrix E and the rotation matrix R of the camera cameraand translation vector t camera The relationship between them is:

[0061] E = [t camera × R camera

[0062] where, [t] × is the skew-symmetric matrix of translation vector t, R camera is the rotation matrix of the camera, t camera is the translation vector of the camera; the internal parameters of the camera are known, and the essential matrix E or the homography matrix H is calculated by the eight-point method; and / or

[0063] The relationship between the homography matrix H, the rotation matrix R camera of the camera and the translation vector t camera is:

[0064]

[0065] where, K1 and K2 are the camera internal parameter matrices corresponding to two different images, n is the plane normal vector, representing the normal direction of a certain plane in the image, used for matching with the points on the plane, and d is the distance from the plane to the camera, representing the perpendicular distance from the plane to the camera.

[0066] S5-4. For the matched feature point pairs, by calculating the rotation matrix R camera of the camera and the displacement vector t camera to obtain the visual-inertial odometry factor, and the visual-inertial odometry factor provides the relative motion constraint between two frames of images, and the constraint of the visual-inertial odometry factor is expressed as:

[0067] f v = (R camera , t camera )

[0068] where, f v is the observed value of the visual-inertial odometry factor, which includes the rotation matrix R camera of the camera and the displacement vector t camera ;

[0069] S5-5. Add the visual-inertial odometry factor to the factor graph and minimize the residual r v of the visual-inertial odometry factor to optimize the pose estimation, and the residual r v is calculated as follows:

[0070] r v = f v ―f vision (p k , R k , p​k―1 , R k―1 )

[0071] Among them, f vision (·) is a motion model estimated based on vision, representing the relative pose change between two frames of images.

[0072] Furthermore, steps S2 - S11 also include:

[0073] S6. Detect point, line, and plane features in the lidar point cloud through the 3D feature detector Velodyne HDL - 64E, perform point cloud registration on the lidar point cloud data based on the ICP algorithm and the NDT algorithm to obtain a pose transformation, and introduce the result of the pose transformation as a lidar odometry factor into the factor graph to optimize pose estimation, specifically including:

[0074] S6 - 1. Calculate the point curvature to extract point features, extract line features by fitting continuous points in the lidar point cloud through the least squares method, and extract plane features by fitting the plane area in the lidar point cloud through the least squares method; the point curvature is estimated through the covariance matrix of each point in the neighborhood of the lidar point cloud. Assume that each point p i =(x i , y i , z i ) has a neighborhood N i , and its local curvature κ i is given by the following formula:

[0075]

[0076] Among them, λ min is the minimum eigenvalue of point p i in its neighborhood N i , representing the degree of curvature of the lidar point cloud surface at this point p i ; point features refer to points with relatively high local geometric properties (such as curvature). Usually, these points are located at the edges or corners of objects. The extraction of point features depends on calculating the curvature of each point in the point cloud. Points with relatively large local curvature (i.e., λ min is smaller) are usually regarded as corner points.

[0077] S6 - 2. Perform point cloud registration on the lidar point cloud data through the ICP algorithm. Starting from the feature points extracted from the current lidar frame P i and the previous frame P k―1 , for each feature point p i in the current frame, find the closest point p j in the previous frame's point cloud, perform registration based on the distance of the closest point, and calculate the error d(p i , p j ) between all point pairs:

[0078] d(p i ,p j )=‖p i ―p j ‖

[0079] By minimizing the error d(p i ,p j ), the rotation matrix R lidar and the translation vector t lidar are obtained, which represent the pose change of the current frame relative to the previous frame. The pose transformation optimized by the ICP algorithm is expressed as:

[0080] T k =R lidar T k―1 +t lidar

[0081] where t k is the pose of the current frame, t k―1 is the pose of the previous frame, R lidar is the rotational transformation of the lidar coordinate system from the previous moment to the current moment, and t lidar is the translational transformation of the lidar coordinate system from the previous moment to the current moment;

[0082] S6-3. Use the NDT algorithm to model the local area of the lidar point cloud as a Gaussian distribution for matching. Divide the lidar point cloud into multiple voxels, and perform Gaussian distribution modeling on the points within each voxel. Each voxel is represented as a Gaussian distribution Ν(μ,∑), where μ is the centroid of the voxel and ∑ is the covariance matrix. For each point p i in the current frame of lidar point cloud, calculate the distance between this point and the Gaussian distribution of the corresponding voxel in the previous frame of lidar point cloud, and estimate the pose transformation by minimizing the distance between the Gaussian distributions. The distance between the Gaussian distributions is calculated based on the Mahalanobis distance from the point to the Gaussian distribution:

[0083] d(p i ,Ν(μ,∑))=‖∑ ―1 (p i ―μ)‖

[0084] Through optimization iteration, the pose transformation T k of the current frame is obtained;

[0085] S6-4. The lidar inertial odometry factor is added to the factor graph according to the pose transformation obtained by registration. In the factor graph, each lidar inertial odometry factor contains the relative pose transformation between the current frame and the previous frame. The constraint of the pose transformation is expressed as:

[0086] fl = [R lidar , t lidar

[0087] where f l is the observed value of the lidar factor, representing the relative pose transformation between the current frame and the previous frame, and R lidar is the rotation transformation of the lidar coordinate system from the previous moment to the current moment, and t lidar is the translation transformation of the lidar coordinate system from the previous moment to the current moment;

[0088] S6-5. Introduce the lidar inertial odometry factor into the factor graph and minimize the residual r l of the lidar inertial odometry factor to optimize the pose estimation. The residual r l is calculated as follows:

[0089] r l = f l - f lidar (p k , R k , p k―1 , R k―1 )

[0090] where r l is the residual of the lidar inertial odometry factor, and f lidar (·) is the registration model of the lidar data, representing the relative pose change between the lidar data calculated by lidar point cloud registration.

[0091] Furthermore, in step S6-1, when extracting line features, a set of approximately collinear points in the lidar point cloud is usually used. Given a set of points P line = {P i} in the lidar point cloud, a straight line is fitted by the least squares method. Assuming the parameters of the straight line are I = (a, b, c), the straight line equation is:

[0092] ax + by + cz = d

[0093] The least squares objective is to minimize the perpendicular distance from the points to the straight line, and the formula is:

[0094]

[0095] where n is the number of fitting points, and p i = (x i , y i , z i ) is each point in the lidar point cloud.

[0096] Furthermore, in step S6-1, when extracting plane features, assume a set of points P in the lidar point cloud​plane ={P i} comes from a plane, and the plane equation is:

[0097] ax + by + cz = d

[0098] Using the least squares method to fit the plane, the goal is to minimize the perpendicular distance from the points to the plane, and the optimization formula is:

[0099]

[0100] where p i =(x i , y i , z i ) is each point in the laser point cloud, and (a, b, c) obtained after fitting are the plane parameters.

[0101] Furthermore, steps S2 - S11 also include:

[0102] S7. Construct a GPS inertial odometer based on the GPS differential correction technology, construct a GPS inertial odometer factor according to the pose constraint relationship one, and introduce the GPS inertial odometer factor into the factor graph to optimize the pose estimation, specifically including:

[0103] S7 - 1. The GPS differential correction technology improves the positioning accuracy of the GPS by introducing a GPS receiver with a known position as a reference station. The reference station receives satellite signals, calculates its position error, and then transmits the correction information to the GPS receiver of the robot. The known position of the reference station is Δp ref , the received GPS position is p GPS,ref , and the differential correction value is:

[0104] Δp diff = p ref ―p GPS,ref

[0105] The differential correction value will be transmitted to the GPS receiver of the robot. The GPS position received by the robot is p GPS,mobile , after applying the differential correction value, the corrected position of the robot is:

[0106] p GPS,corrected = p GPS,mobile + Δp diff

[0107] S7 - 2. Construct a GPS inertial odometer factor based on the position information received by the robot through GPS and the accurate pose obtained through differential correction. The GPS position of the robot at time t k is p GPS,corrected , and the constraint in the constraint relationship one is expressed as:

[0108] f GPS = p GPS,corrected

[0109] Minimize the residual r of the factor graph to optimize the pose estimation and obtain a more accurate global pose estimation. The residual r GPS , in order to optimize the pose estimation and obtain a more accurate global pose estimation. The residual r GPS is expressed as:

[0110] r GPS = f GPS − f(p k )

[0111] where f(p k ) is the pose estimation obtained during factor graph optimization.

[0112] Furthermore, steps S2 - S11 also include:

[0113] S8. Construct a UWB inertial odometer based on the wireless communication time difference algorithm, construct a UWB factor according to the pose constraint relationship two, and introduce the UWB factor into the factor graph, specifically including:

[0114] S8 - 1. The UWB technology estimates the distance between the robot and the UWB base station by measuring the time difference of signal propagation, and obtains the distance d between the robot and the UWB base station through time difference measurement i :

[0115] d i = ‖p mobile − p UWB,i ‖

[0116] where d i is the distance between the robot and the i-th UWB base station, p mobile is the current position of the robot, and p UWB,i is the position of the UWB base station i;

[0117] Combine the ranging information provided by multiple UWB base stations, and estimate the pose of the robot by minimizing the residual of the distances of all UWB base stations. When there are N base stations, the pose of the robot is estimated by minimizing the following objective function, which represents the ranging error between the robot and all UWB base stations to obtain an accurate pose estimation:

[0118]

[0119] S8 - 2. Construct a UWB inertial odometer factor based on the distance information measured by the UWB base station and the preliminary pose estimation information. p mobile is the current position of the robot (the position of the robot at time t k ), di is the distance between the robot and the \(i\)-th UWB base station, and the constraint in the pose constraint relationship two is expressed as:

[0120] f UWB = ‖p mobile − p UWB,i ‖ − d i

[0121] The UWB inertial odometer factor constrains the ranging error between the robot and the UWB base station. By minimizing the residual \(r\) in the factor graph UWB , the pose estimation is optimized, and the residual is expressed as:

[0122] r UWB = f UWB − f(p k ).

[0123] Furthermore, steps S2 - S11 further include:

[0124] S9. Construct a GNSS inertial odometer based on the GNSS signal. According to the position information provided by the GNSS and the pose estimations of the lidar, IMU, camera, GPS, and UWB, combined with the pose constraint relationship three, construct a GNSS factor and introduce the GNSS factor into the factor graph, specifically including:

[0125] The constraint in the pose constraint relationship three is expressed as:

[0126] f GNSS = [x GNSS , y GNSS , z GNSS T

[0127] where \(f\) GNSS is the observation value of the GNSS factor, representing the position information of the robot obtained through the GNSS system, and \(x\) GNSS , y GNSS , z GNSS represent the longitude, latitude, and altitude information of the robot provided by the GNSS, respectively.

[0128] Furthermore, the GNSS module provides the position data of the robot in the global coordinate system by receiving satellite signals.

[0129] Furthermore, steps S2 - S11 further include:

[0130] ​S10. Use the DBoW2 loop detection algorithm to perform loop detection on the key frame data of the image, extract BRIEF descriptors and match them with the visual descriptors extracted in step S5. Transmit the image timestamps of loop candidates to the lidar inertial system for further verification, and establish loop detection factors based on the pose constraint relationship four. The pose estimation relationship four is the relative pose difference between the current image and the loop candidate image obtained by performing loop detection on the key frame data of the image using the DBoW2 loop detection algorithm. The relative pose difference describes the rotation and displacement from the current image coordinate system to the loop candidate image coordinate system, that is, the change in the spatial position and orientation between these two images. In loop detection, by matching the feature points in the current image and the loop candidate image (i.e., the historical image), the relative pose difference between these two images is calculated. Here, the relative pose difference refers to the rotation matrix and the translation vector, which describe the rotation and displacement from the current image coordinate system to the loop candidate image coordinate system, that is, the change in the spatial position and orientation between these two images. By calculating the relative pose difference, the system can determine whether the robot has returned to the previous position, thereby updating the map, correcting the pose, and correcting possible positioning errors.

[0131] Further, steps S2 - S11 also include:

[0132] S11. Construct the robot pose as a factor graph variable node, and add the IMU pre-integration factor, visual inertial odometry factor, lidar odometry factor, GPS inertial odometry factor, UWB factor, GNSS factor, and loop detection factor as factors into the factor graph for optimization to obtain the global positioning pose and map, specifically including:

[0133] S11 - 1. Based on the Gaussian distribution, construct the error expressions of the motion equation and the observation equation. The error expression of the motion equation is constructed as follows:

[0134]

[0135] where f(X k , u k ) is the motion model that predicts the next state X k based on the current state X k and the control input u k+1 . Ω motion is the covariance matrix of this equation, representing the error distribution during the motion process, and ‖·‖ represents the Euclidean norm of the error;

[0136] The actual measurement value z k obtained through the sensor and the observation value h(X k predicted from the robot state X k) Construct an observation equation based on the differences, and the error expression of the observation equation is constructed as follows:

[0137]

[0138] Where h(X k ) is the predicted observation value obtained through the sensor model based on the robot state X k , z k is the actual observation value directly measured by the sensor, and Ω sensor is the covariance matrix of the observation error, representing the spatial distribution of the measurement error;

[0139] S11-2. Use the nonlinear incremental optimization method of the factor graph to optimize the objective function in the Bayesian network, gradually improve the posterior probability, and finally obtain the optimal solution. The construction of the objective function in the Bayesian network is as follows:

[0140]

[0141] Where X i represents the robot pose state variable at the i-th moment, f motion represents the motion constraint based on IMU pre-integration, f lidar , f vision , f GNSS , f GPS , f UWB represent the radar-lidar odometry factor, visual-inertial odometry factor, GNSS factor, GPS-inertial odometry factor, UWB factor, f loop represents the loop closure detection factor, and Ω motion , Ω lidar , Ω vision , Ω GNSS , Ω GPS , Ω UWB , Ω loop respectively correspond to the covariance matrices of different observation factors, used to describe the spatial distribution of the measurement errors of each sensor, represents the weights of each sensor, and the weights are dynamically adjusted according to the confidence of the sensors to improve the fusion accuracy.

[0142] Further, step S12 specifically includes:

[0143] S12-1. At the first layer of the deep fuzzy neural network, use fuzzy processing to extract the fuzzy features of the multi-sensor input data to convert the multi-sensor data into fuzzy sets, and the conversion is expressed as:

[0144]

[0145] Where Represents the sensor data after fuzzification, Y i is the original data of sensor i, Y is the set of all sensor data, Φ(Y i ) is the fuzzification function, Φ(Y i ) is used to transform the sensor data Y i into a fuzzy set to adapt to the noise and uncertainty of different sensors;

[0146] S12 - 2. In the second layer of the deep fuzzy neural network, a deep inference module based on the Gumbel - Softmax activation function is adopted to perform real - time inference on the sensor signal quality and environmental changes, and dynamically adjust the confidence weights of each sensor, which is expressed as:

[0147] p t = Gumbel―Softmax(ω sensor , temperature)

[0148] where p t is the confidence of the sensor in the current scenario, ω sensor is the weight of the sensor, representing the signal quality of the sensor, and temperature controls the "softening" degree of Gumbel - Softmax;

[0149] S12 - 3. In the third layer of the deep fuzzy neural network, a fully - connected layer is used to process the outputs of the first and second layers of the deep fuzzy neural network, and calculate the final sensor confidence value, which is expressed as:

[0150]

[0151] where, represents the sensor confidence calculated through the neural network, which is the output of the third layer of the deep fuzzy neural network and is used to describe the sensor confidence value of the sensor at the current moment t, h2 is the output of the second layer of the deep fuzzy neural network, and W3 and b3 are the weights and biases of the third layer of the deep fuzzy neural network;

[0152] S12 - 4. Apply the calculated sensor confidence value to the dynamic adjustment of the sensor weights in the factor graph model. By optimizing the confidence of the sensor, the optimal data fusion effect can be achieved in different scenarios. The dynamic adjustment of the sensor weights is expressed as:

[0153]

[0154] where ω sensor (t) represents the weight of the sensor at the current time t, is the sensor confidence calculated in S12-3, and α and β are adjustment factors that control the relationship between confidence and weight;

[0155] S12-5. Continuously train and optimize according to the performance of the sensor in actual applications, and continuously adjust the weight ω of the sensor sensor , and the training is carried out by minimizing the error, and the error function is expressed as:

[0156]

[0157] where is the confidence output by the sensor belief network calculated in S12-3, and p target is the target confidence, and the error function minimizes the prediction error by optimizing the weight ω sensor to optimize the multi-sensor data fusion strategy.

[0158] The deep fuzzy neural network combines the spatio-temporal features and environmental information of multiple sensors, and uses fuzzy processing to convert the input multi-sensor data into a fuzzy set to adapt to the noise and uncertainty of different sensors.

[0159] The present invention has at least the following beneficial effects: 1) The multi-sensor cross-scene dynamic optimal fusion positioning and mapping method proposed by the present invention realizes high-precision positioning and navigation of agricultural robots in different scenarios through real-time fusion of multiple sensor data, overcoming the problems of scene dependence and insufficient accuracy of existing single-sensor solutions; 2) The present invention combines the characteristics of different sensors and adopts static calibration and dynamic online self-calibration methods to solve the problem of spatio-temporal parameter changes caused by high vibration and strong interference in the agricultural environment, realizing high-precision correction of spatio-temporal synchronization between multiple sensors, thereby significantly improving the positioning accuracy and system robustness; 3) The present invention proposes an online inference mechanism for multi-sensor confidence based on a deep fuzzy neural network, realizes the confidence evaluation of each sensor signal at the current scene position, flexibly selects the optimal sensor information source, improves the cross-scene positioning accuracy, and overcomes the problem of accuracy fluctuation caused by performance differences of different sensors in the prior art. BRIEF DESCRIPTION OF THE DRAWINGS

[0160] To further clarify the above and other advantages and features of the embodiments of the present invention, more specific descriptions of the embodiments of the present invention will be presented with reference to the drawings. It can be understood that these drawings only depict typical embodiments of the present invention and will not be considered as limiting its scope. In the drawings, for clarity, the same or corresponding components will be denoted by the same or similar reference numerals.

[0161] Figure 1It shows a schematic flow chart of the multi-sensor cross-scenario dynamic optimal fusion positioning and mapping method in the present invention;

[0162] Figure 2 It shows a schematic diagram of the SLAM model framework for multi-sensor cross-scenario dynamic optimal fusion in an embodiment of the present invention;

[0163] Figure 3 It shows a schematic diagram of the multi-sensor factor graph fusion framework in an embodiment of the present invention;

[0164] Figure 4 It shows a schematic diagram of the multi-sensor confidence online inference model based on a deep fuzzy neural network in an embodiment of the present invention. Detailed implementation manners

[0165] It should be noted that the components in the respective drawings may be exaggeratedly shown for illustrative purposes and are not necessarily to scale correctly.

[0166] In the present invention, each embodiment is only intended to illustrate the solution of the present invention and should not be construed as restrictive.

[0167] In the present invention, unless otherwise specified, the quantifiers "a" and "one" do not exclude the scenario of multiple elements.

[0168] It should also be noted here that in the embodiments of the present invention, for the sake of clarity and simplicity, only a part of the components or assemblies may be shown, but those of ordinary skill in the art can understand that, under the teaching of the present invention, the required components or assemblies can be added according to specific scenario needs.

[0169] It should also be noted here that within the scope of the present invention, the terms "same", "equal", "equals", etc. do not mean that the two values are absolutely equal, but allow a certain reasonable error, that is, the said terms also cover "substantially the same", "substantially equal", "substantially equals".

[0170] It should also be noted here that in the description of the present invention, the orientation or positional relationship indicated by the terms "center", "longitudinal", "transverse", "up", "down", "front", "rear", "left", "right", "vertical", "horizontal", "top", "bottom", "inner", "outer", etc. is the orientation or positional relationship based on the drawings shown, and is only for the convenience of describing the present invention and simplifying the description, rather than explicitly or implicitly indicating that the device or element referred to must have a specific orientation, be constructed and operated in a specific orientation, and therefore cannot be construed as a limitation of the present invention. In addition, the terms "first" and "second" are only used for descriptive purposes and cannot be construed as explicitly or implicitly indicating relative importance.

[0171] In addition, the embodiments of the present invention describe the process steps in a specific order. However, this is only for the convenience of distinguishing each step, rather than limiting the order of each step. In different embodiments of the present invention, the order of each step can be adjusted according to the adjustment of the process. In addition, the numbering of the steps is only for referring to specific steps, rather than limiting the execution order of the steps.

[0172] The following embodiments provide a multi-sensor cross-scenario dynamic optimal fusion positioning and mapping method, which is used for facility agricultural robots. The method specifically includes the following steps:

[0173] S1. Obtain the data of multi-sensors (IMU, camera, lidar, GPS, UWB, GNSS), and complete the unification of the spatio-temporal relationship of the multi-sensor data by using static calibration and dynamic online self-calibration.

[0174] S1-1. In static calibration, set the coordinate system of one sensor as the reference coordinate system, and the coordinate systems of other sensors are transformed into the reference coordinate system through the rotation matrix R i and the translation vector t i . The multi-sensors are set to be rigidly connected, and the external measurement method or the true value mapping method is used for the feature registration of the target points, lines, and surfaces to obtain the initial spatio-temporal parameters between the systems. The spatio-temporal transformation between the multi-sensors is represented by the following formula:

[0175]

[0176] where T i is the transformation matrix of sensor i, R i is the rotation matrix of sensor i, representing the rotational transformation of sensor i relative to the reference coordinate system, and t i is the translation vector of sensor i;

[0177] S1-2. In the dynamic online self-calibration stage, the initial spatio-temporal parameters have been obtained through static calibration. Project the motion trajectory obtained by multi-sensor pose estimation into the inertial measurement unit coordinate system, and use the trajectory similarity principle to measure the difference between the two trajectories through the Frechet distance. Suppose there are two trajectories X1 = {x1(t)} and X2 = {x2(t)}.

[0178] The Frechet distance is defined as:

[0179]

[0180] where α(t) is a monotonically increasing mapping, representing the correspondence between the trajectories;

[0181] The objective function constructed by the Frechet distance cost is used for online optimization of spatio-temporal parameters. The optimization objective is to minimize the sum of squared errors to obtain the optimal spatio-temporal parameters:

[0182]

[0183] where T is the spatio-temporal parameter to be optimized, and X1(t) and X2(t) are the estimated trajectories from any two sensors respectively.

[0184] S2. Measure the inertial data of the robot through the IMU. The inertial data includes acceleration and angular velocity. The inertial data is integrated to obtain the preliminary pose estimation information of the robot, that is, the displacement and rotation information of the robot; if the GPS signal exists, the pose estimation of the IMU will be corrected by the GPS data.

[0185] S3. Obtain the first-frame laser point cloud through the lidar. Use the preliminary pose estimation information to correct the distortion of the lidar point cloud data to obtain the current laser point cloud, and register the preliminary pose estimation information with the current lidar frame to correct the spatial position of the current laser point cloud.

[0186] S4. Introduce the preliminary pose estimation information as a constraint into the factor graph to establish the IMU pre-integration factor. The IMU pre-integration factor will further improve the positioning accuracy and reduce the influence caused by sensor errors or cumulative errors.

[0187] S4-1. Integrate the acceleration to obtain the displacement increment Δp of the robot within the time interval k , and the calculation formula is:

[0188]

[0189] where Δt = t f ―t0 is the time interval, v k is the velocity of the robot at t k a k is the acceleration of the robot at t k v0 is the initial velocity of the robot at t0, p0 is the initial position of the robot at t0, and a(t) is the acceleration of the robot at t0;

[0190] S4-2. Integrate the angular velocity to obtain the rotation increment ΔR of the robot k , and the calculation formula is:

[0191]

[0192] where ΔR kis the rotation matrix, representing the rotational increment of the robot from the initial time t0 to the current time, and exp(·) is the exponential map of the Lie group, used to describe the rotational transformation;

[0193] S4-3. Construct the IMU pre-integration factor, which is represented by nodes x k and x k+1 representing the pose of the robot. The nodes x k and x k+1 correspond to the poses of the robot at times t k and t k+1 . The constraint expression of the IMU pre-integration factor is:

[0194] f k = [Δp k , ΔR k

[0195] Minimize the residual r k of the IMU pre-integration factor to optimize the pose estimation. The residual r k is calculated as follows:

[0196] r k = f k − f IMU (p k , R k , p k―1 , R k―1 )

[0197] where f IMU (·) is used to calculate the IMU pre-integration factor and estimate the pose change of the robot between two time points based on the IMU data.

[0198] S5. Extract significant SURF visual key feature points and visual descriptors, establish a visual-inertial odometry factor based on the FLANN algorithm matching of the visual descriptors and the motion estimation of the camera, and add the visual-inertial odometry factor to the factor graph to optimize the pose estimation.

[0199] S5-1. The SURF visual key feature point extraction process is expressed as:

[0200] f = SURF(I) = {(P i , d i )}

[0201] where f is the set of extracted feature points, P i = (x i , y i ) represents the position of the feature point, and d i is the descriptor of this feature point; each feature point is represented by the position P i = (x​i , y i ), and descriptor d i . The descriptor is the gradient information of the area around the feature point.

[0202] S5-2. Use the FLANN algorithm to match the visual descriptors to find the corresponding feature point pairs of the same or similar objects in two frames of images. For each pair of feature point pairs, calculate the Euclidean distance between the visual descriptors to select the best matching pair. The distance metric in the matching process is expressed as:

[0203] d(d i , d j ) = ‖d i ―d j ‖

[0204] where d i and d j are the descriptors of the corresponding feature points P i and P j in two frames of images, respectively;

[0205] S5-3. Use the matched feature point pairs to estimate the relative motion of the camera through the essential matrix E or the homography matrix H; the relationship between the essential matrix E and the camera rotation matrix R camera and the translation vector t camera is as follows:

[0206] E = [t camera × R camera

[0207] where [t] × is the skew-symmetric matrix of the translation vector t camera , R camera is the camera rotation matrix, representing the rotation relationship between the camera coordinate system and the world coordinate system, and t camera is the camera translation vector, representing the translation of the camera in the world coordinate system; the camera internal parameters are known, and the essential matrix E or the homography matrix H is calculated by the eight-point method; and / or

[0208] The relationship between the homography matrix H and the camera rotation matrix R camera and the translation vector t camera is as follows:

[0209]

[0210] where K1 and K2 are the camera internal parameter matrices corresponding to two different images, n is the plane normal vector, representing the normal direction of a certain plane in the image, used for matching with the points on the plane, and d is the distance from the plane to the camera, representing the perpendicular distance from the plane to the camera.​

[0211] S5-4. For the matched feature point pairs, calculate the rotation matrix R of the camera camera and the displacement vector t camera to obtain the visual-inertial odometry factor, and the constraint of the visual-inertial odometry factor is expressed as:

[0212] f v =(R camera , t camera )

[0213] where f v is the observed value of the visual-inertial odometry factor, which contains the rotation matrix R of the camera camera and the displacement vector t camera ;

[0214] S5-5. Add the visual-inertial odometry factor to the factor graph and minimize the residual r v of the visual-inertial odometry factor to optimize the pose estimation. The residual r v is calculated as follows:

[0215] r v =f v -f vision (p k , R k , p k―1 , R k―1 )

[0216] where f vision (·) is the motion model estimated based on vision, representing the relative pose change between two frames of images.

[0217] S6. Detect the point, line, and plane features in the lidar point cloud through the 3D feature detector Velodyne HDL-64E, perform point cloud registration on the lidar point cloud data based on the ICP algorithm and the NDT algorithm to obtain the pose transformation, and introduce the result of the pose transformation as the lidar odometry factor into the factor graph to optimize the pose estimation.

[0218] S6-1. Calculate the point curvature to extract point features, extract line features by fitting continuous points in the lidar point cloud through the least squares method, and extract plane features by fitting the plane region in the lidar point cloud through the least squares method; the point curvature is estimated by the covariance matrix of each point in the neighborhood of the lidar point cloud. Assume that each point p i =(x i , y i , z i ) in the lidar point cloud has a neighborhood N i , and its local curvature κ i is given by the following formula:

[0219]

[0220] Among them, λ min is the minimum eigenvalue at point p i in its neighborhood N i , representing the curvature of the laser point cloud surface at this point p i ; Point features refer to points with relatively high local geometric properties (such as curvature). Usually, these points are located at the edges or corners of objects. The extraction of point features depends on calculating the curvature of each point in the point cloud. Points with relatively large local curvature (i.e., λ min is smaller) are usually regarded as corner points.

[0221] When extracting line features, a set of approximately collinear points in the laser point cloud is usually used. Given a set of points P line = {P i} in the laser point cloud, a straight line is fitted by the least squares method. Assuming the parameters of the straight line are I = (a, b, c), the straight line equation is:

[0222] ax + by + cz = d

[0223] The goal of the least squares method is to minimize the perpendicular distance from the points to the straight line, and the formula is:

[0224]

[0225] Among them, n is the number of fitting points, and p i = (x i , y i , z i ) is each point in the laser point cloud.

[0226] When extracting plane features, assume that a set of points P plane = {P i} in the laser point cloud comes from a plane, and the plane equation is:

[0227] ax + by + cz = d

[0228] Use the least squares method to fit the plane. The goal is to minimize the perpendicular distance from the points to the plane, and the optimization formula is:

[0229]

[0230] Among them, p i = (x i , y i , z i ) is each point in the laser point cloud, and (a, b, c) obtained after fitting is the plane parameter;

[0231] S6-2. Perform point cloud registration on the point cloud data of the lidar through the ICP algorithm, starting from the feature points extracted from the current lidar frame P i and the previous frame P k―1 . For each feature point p i in the current frame, find the closest point p j in the previous frame's point cloud, perform registration based on the distance of the closest point, and calculate the error d(p i , p j ) between all point pairs:

[0232] d(p i , p j ) = ‖p i - p j ‖

[0233] . By minimizing the error d(p i , p j ), obtain the rotation matrix R lidar and the translation vector t lidar , which represent the pose change of the current frame relative to the previous frame. The pose transformation after ICP algorithm optimization is expressed as:

[0234] T k = R lidar T k―1 + t lidar

[0235] . Among them, T k is the pose of the current frame, T k―1 is the pose of the previous frame, R lidar is the rotation transformation of the lidar coordinate system from the previous moment to the current moment, and t lidar is the translation transformation of the lidar coordinate system from the previous moment to the current moment;

[0236] S6-3. Use the NDT algorithm to model the local area of the laser point cloud as a Gaussian distribution for matching. Divide the laser point cloud into multiple voxels, and perform Gaussian distribution modeling on the points within each voxel. Each voxel is represented as a Gaussian distribution Ν(μ,∑), where μ is the centroid of the voxel and ∑ is the covariance matrix; for each point p i in the current frame of the laser point cloud, calculate the distance between this point and the Gaussian distribution of the corresponding voxel in the previous frame of the laser point cloud, and estimate the pose transformation by minimizing the distance between the Gaussian distributions. The distance between the Gaussian distributions is calculated based on the Mahalanobis distance from the point to the Gaussian distribution:

[0237] d(p i , Ν(μ,∑)) = ‖∑ ―1 (p i - μ)‖

[0238] Through optimization iteration, the pose transformation T of the current frame is obtained k ;

[0239] S6-4. The lidar inertial odometry factor is added to the factor graph according to the pose transformation obtained by registration. In the factor graph, each lidar inertial odometry factor contains the relative pose transformation between the current frame and the previous frame, and the constraint of the pose transformation is expressed as:

[0240] f l =[R lidar ,t lidar

[0241] where f l is the observation value of the lidar factor, representing the relative pose transformation between the current frame and the previous frame, R lidar is the rotation transformation of the lidar coordinate system from the previous moment to the current moment, and t lidar is the translation transformation of the lidar coordinate system from the previous moment to the current moment;

[0242] S6-5. The lidar inertial odometry factor is introduced into the factor graph, and the residual r l of the lidar inertial odometry factor is minimized to optimize the pose estimation. The residual r l is calculated as follows:

[0243] r l =f l ―f lidar (p k ,R k ,p k―1 ,R k―1 )

[0244] where r l is the residual of the lidar inertial odometry factor, and f lidar (·) is the registration model of lidar data, representing the relative pose change between lidar data calculated by lidar point cloud registration.

[0245] S7. Build a GPS inertial odometer based on GPS differential correction technology, construct a GPS inertial odometry factor according to the pose constraint relationship one, and introduce the GPS inertial odometry factor into the factor graph to optimize the pose estimation.

[0246] S7-1. The GPS differential correction technology improves the positioning accuracy of GPS by introducing a GPS receiver with a known position as a reference station. The reference station receives satellite signals, calculates its position error, and then transmits the correction information to the GPS receiver of the robot. The known position of the reference station is Δp ref , and the received GPS position is p GPS,ref ​, the differential correction value is:

[0247] Δp diff =p ref ―p GPS,ref

[0248] The differential correction value is transmitted to the robot's GPS receiver, and the GPS position received by the robot is p GPS,mobile , after applying the differential correction value, the corrected position of the robot is:

[0249] p GPS,corrected =p GPS,mobile +Δp diff

[0250] S7-2. Construct a GPS inertial odometer factor based on the position information received by the robot through GPS and the precise pose obtained through differential correction. At time t of the robot k The GPS position is p GPS,corrected , and the constraint in Constraint Relationship 1 is expressed as:

[0251] f GPS =p GPS,corrected

[0252] Minimize the residual r of the factor graph GPS , to optimize the pose estimation and obtain a more accurate global pose estimation. The residual r GPS is expressed as:

[0253] r GPS =f GPS ―f(p k )

[0254] where f(p k ) is the pose estimation obtained during the optimization of the factor graph.

[0255] S8. Construct a UWB inertial odometer based on the wireless communication time difference algorithm, construct a UWB factor according to the pose constraint relationship two, and introduce the UWB factor into the factor graph.

[0256] S8-1. The UWB technology estimates the distance between the robot and the UWB base station by measuring the time difference of signal propagation. The distance d between the robot and the UWB base station is obtained through the time difference measurement i :

[0257] d i =‖p mobile ―p UWB,i ‖

[0258] where d i is the distance between the robot and the i-th UWB base station, p mobile is the current position of the robot, pUWB,i is the position of UWB base station i;

[0259] Combining the ranging information provided by multiple UWB base stations, the pose of the robot is estimated by minimizing the residuals of the distances of all UWB base stations. When there are N base stations, the pose of the robot is estimated by minimizing the following objective function, which represents the ranging error between the robot and all UWB base stations to obtain an accurate pose estimate:

[0260]

[0261] S8-2. Construct a UWB inertial odometry factor based on the distance information measured by the UWB base station and the preliminary pose estimation information, p mobile is the current position of the robot, d i is the distance between the robot and the i-th UWB base station, and the constraint in the second pose constraint relationship is expressed as:

[0262] f UWB =‖p mobile ― p UWB,i ‖― d i

[0263] The UWB inertial odometry factor constrains the ranging error between the robot and the UWB base station, and optimizes the pose estimate by minimizing the residual r UWB in the factor graph. The residual is expressed as:

[0264] r UWB = f UWB ― f(p k ).

[0265] S9. Construct a GNSS inertial odometry based on the GNSS signal. According to the position information provided by the GNSS and the pose estimates of the lidar, IMU, camera, GPS, and UWB, combined with the third pose constraint relationship, construct a GNSS factor and introduce the GNSS factor into the factor graph.

[0266] The constraint in the third pose constraint relationship is expressed as:

[0267] f GNSS = [x GNSS , y GNSS , z GNSS T

[0268] where f GNSS is the observation value of the GNSS factor, representing the position information of the robot obtained through the GNSS system, x GNSS , y GNSS , z GNSSrespectively represent the longitude, latitude and altitude information of the robot provided by GNSS.

[0269] S10. Use the DBoW2 loop detection algorithm to perform loop detection on the image key-frame data, extract BRIEF descriptors and match them with the visual descriptors extracted in step S5, transfer the image timestamps of the loop candidates to the lidar inertial system for further verification, and establish loop detection factors based on the pose constraint relationship four. The pose estimation relationship four is the relative pose difference between the current image obtained by performing loop detection on the image key-frame data by the DBoW2 loop detection algorithm and the loop candidate image. The relative pose difference describes the rotation and displacement from the current image coordinate system to the loop candidate image coordinate system, that is, the change in the spatial position and orientation between these two images.

[0270] S11. Construct the robot pose as a factor graph variable node, and add the IMU pre-integration factor, visual inertial odometry factor, lidar odometry factor, GPS inertial odometry factor, UWB factor, GNSS factor and loop detection factor as factors into the factor graph for optimization to obtain the global positioning pose and map.

[0271] S11-1. Based on the Gaussian distribution, construct the error expressions of the motion equation and the observation equation. The error expression of the motion equation is constructed as follows:

[0272]

[0273] where, f(X k , u k ) is the motion model that predicts the next state X k based on the current state X k and the control input u k+1 . Ω motion is the covariance matrix of this equation, representing the error distribution during the motion process, and ‖·‖ represents the Euclidean norm of the error;

[0274] Construct the observation equation from the difference between the actual measurement value z k obtained through the sensor and the predicted observation value h(X k ) predicted by the robot state X k . The error expression of the observation equation is constructed as follows:

[0275]

[0276] where, h(X k ) is the predicted observation value obtained through the sensor model based on the robot state X k , z k is the actual observation value directly measured by the sensor, and Ω sensoris the covariance matrix of the observation error, representing the spatial distribution of the measurement error;

[0277] S11-2. Use the nonlinear incremental optimization method of the factor graph to optimize the objective function in the Bayesian network, gradually improve the posterior probability, and finally obtain the optimal solution. The construction of the objective function in the Bayesian network is as follows:

[0278]

[0279] where X i represents the robot pose state variable at the i-th moment, and f motion represents the motion constraint based on IMU pre-integration, and f lidar , f vision , f GNSS , f GPS , f UWB represent the lidar odometry factor, visual-inertial odometry factor, GNSS factor, GPS-inertial odometry factor, UWB factor, and f loop represents the loop closure detection factor, and Ω motion , Ω lidar , Ω vision , Ω GNSS , Ω GPS , Ω UWB , Ω loop correspond to the covariance matrices of different observation factors respectively, used to describe the spatial distribution of the measurement errors of each sensor, represent the weights of each sensor, and the weights are dynamically adjusted according to the confidence of the sensor to improve the fusion accuracy.

[0280] S12. Based on the deep fuzzy neural network, establish an online inference mechanism for the confidence of multi-sensors, evaluate the confidence of multi-sensors in the current scenario, and dynamically adjust the sensor weights in the factor graph according to the confidence to optimize the multi-sensor data fusion strategy, that is, realize the dynamic optimal fusion positioning and mapping of multi-sensors across scenarios. The deep fuzzy neural network combines the spatio-temporal characteristics and environmental information of multi-sensors, and uses fuzzy processing to transform the input multi-sensor data into a fuzzy set to adapt to the noise and uncertainty of different sensors.

[0281] S12-1. In the first layer of the deep fuzzy neural network, use fuzzy processing to extract the fuzzy features of the multi-sensor input data to transform the multi-sensor data into a fuzzy set, and the transformation is expressed as:

[0282]

[0283] where represents the sensor data after fuzzy processing, and Y iis the original data of sensor i, Y is the set of all sensor data, Φ(Y i ) is the fuzzification function, and the role of Φ(Y i ) is to transform the sensor data Y i into a fuzzy set to adapt to the noise and uncertainty of different sensors;

[0284] S12-2. In the second layer of the deep fuzzy neural network, a deep inference module based on the Gumbel-Softmax activation function is adopted to perform real-time inference on the sensor signal quality and environmental changes, and dynamically adjust the confidence weights of each sensor, which is expressed as:

[0285] p t = Gumbel―Softmax(ω sensor , temperature)

[0286] where p t is the confidence of the sensor in the current scenario, ω sensor is the weight of the sensor, representing the signal quality of the sensor, and temperature controls the "softening" degree of Gumbel-Softmax;

[0287] S12-3. In the third layer of the deep fuzzy neural network, a fully connected layer is used to process the outputs of the first and second layers of the deep fuzzy neural network, and calculate the final sensor confidence value, which is expressed as:

[0288]

[0289] where, represents the sensor confidence calculated through the neural network. It is the output of the third layer of the deep fuzzy neural network and is used to describe the sensor confidence value of the sensor at the current moment t. h2 is the output of the second layer of the deep fuzzy neural network, and W3 and b3 are the weights and biases of the third layer of the deep fuzzy neural network;

[0290] S12-4. Apply the calculated sensor confidence value to the dynamic adjustment of the sensor weights in the factor graph model. By optimizing the confidence of the sensor, the optimal data fusion effect can be achieved in different scenarios. The dynamic adjustment of the sensor weights is expressed as:

[0291]

[0292] where ω sensor (t) represents the weight of the sensor at the current time t, is the sensor confidence calculated in S12-3, where α and β are adjustment factors that control the relationship between confidence and weight;

[0293] S12-5. Continuously train and optimize according to the performance of the sensor in actual applications, and continuously adjust the weight ω of the sensor sensor , and the training is carried out by minimizing the error, and the error function is expressed as:

[0294]

[0295] where is the sensor confidence calculated in S12-3, and p target is the target confidence. The error function minimizes the prediction error by optimizing the weight ω sensor to optimize the multi-sensor data fusion strategy.

[0296] Figure 2 shows a schematic diagram of the SLAM model framework for multi-sensor cross-scenario dynamic optimal fusion. Figure 4 shows a schematic diagram of the multi-sensor confidence online inference model based on the deep fuzzy neural network in the embodiment of the present invention. Figure 2 It is divided into two parts: the DFNN on the left and the factor graph on the right. In the DFNN, the original data Y of sensor i in the input layer i , and the fuzzifier fuzzifies the original data Y i to obtain the fuzzified sensor data which converts the exact numerical value into a fuzzy set for subsequent processing; the inference engine performs inference calculations based on the fuzzified data to obtain the confidence p of the sensor in the current scenario t , and combines it with the weight ω of the sensor at the current time t sensor (t) for processing; the defuzzifier defuzzifies the inference result to obtain the final sensor weight ω sensor , which converts the fuzzy result into an exact value; the output layer outputs the final sensor weight ω sensor and the final sensor confidence value In the factor graph on the right, the measurement value of each sensor is associated with the corresponding state node X, and connections are established through different factor functions; the state node X represents the pose of the sensor, and each state node is associated with one or more factors, where the factors represent the relationship between the sensor measurement value and the corresponding state node; the measurement value of the sensor is connected to the state node X through factors, and these factors include the measurement error of the sensor, and the factor function establishes a measurement error model according to the type of the sensor; each factor is associated with a weight factor, and the weight factor reflects the credibility of each sensor. The confidence of each sensor at each moment is calculated by the inference engine in the previous step and adjusted according to the current environment; the entire factor graph is solved by the Gauss-Newton method, and the optimization goal is to minimize the measurement error between all factors. During the graph optimization process, each factor will adjust the estimated value of the state node according to the sensor data and the relationship between sensors to obtain an optimal pose estimation result. Figure 4 In it, the input layer receives data from various sensors, and the data of these sensors enter the system through different modalities (vision, lidar, etc.); the fuzzifier fuzzifies the original data of the sensors; the inference engine uses a deep fuzzy neural network to reason about the fuzzified data, evaluate the confidence of the sensors, and dynamically adjust the confidence according to the input environmental information and historical data; the defuzzifier converts the fuzzified data into a clear credibility value, eliminates the ambiguity, and obtains an accurate sensor confidence evaluation; the output layer outputs the final sensor weight and sensor confidence value.

[0297] Figure 3A schematic diagram of a multi-sensor factor graph fusion framework is shown. State nodes X0, X1, X2, … represent the pose information (position and orientation) of the robot at different time steps. Each state node is connected to factors of multiple sensors, and these factors represent the errors between sensor measurements and state nodes. The optimization goal is to minimize these errors, thereby optimizing the estimation of the robot's pose. The measurement data of different sensors are connected to state nodes through different factors. The factor graph optimization algorithm fuses the data of multiple sensors, combines the weight factors of each sensor, and minimizes the measurement error to finally provide a more accurate estimation of the robot's pose. Among them, the IMU pre-integration factor is added to the factor graph as a constraint, reflecting the motion of the robot in a short period of time and providing inertial information constraints for pose estimation; the visual-inertial odometry factor combines camera visual information and IMU inertial information, calculates the pose transformation of the robot by extracting and matching image feature points and combining the motion information of the IMU, and provides motion constraints for visual and inertial fusion for the factor graph; the lidar odometry factor is obtained based on lidar point cloud data processing, and the motion information of the robot is obtained by calculating the pose transformation between adjacent lidar frames, which is used to constrain the pose relationship between state nodes and is a motion constraint based on lidar data; the GPS-inertial odometry factor uses the position information provided by GPS as an external absolute position constraint to correct the pose estimation of the robot and reduce the cumulative error; the UWB factor (UWB measurement) is used to more precisely constrain the position of the robot in a local area, especially in an environment with poor GPS signals, providing additional position constraints; the GNSS factor (GNSS measurement) provides a global position reference for the robot, constrains the pose of the robot, and improves the accuracy of positioning; the loop detection factor is used to correct the cumulative error of the robot pose estimation and make the constructed map more accurate and consistent.

[0298] The present invention combines the characteristics of different sensors and adopts static calibration and dynamic online self-calibration methods to solve the problem of spatio-temporal parameter changes caused by high vibration and strong interference in the agricultural environment, realizes high-precision calibration of spatio-temporal synchronization between multiple sensors, and thus significantly improves the positioning accuracy and system robustness. In addition, the present invention proposes an online inference mechanism for multi-sensor confidence based on a deep fuzzy neural network to realize the confidence evaluation of each sensor signal at the current scene position, flexibly select the optimal sensor information source, improve the cross-scene positioning accuracy, and overcome the problem of accuracy fluctuation caused by performance differences of different sensors in the prior art. In summary, the multi-sensor cross-scene dynamic optimal fusion positioning and mapping method proposed by the present invention realizes high-precision positioning and navigation of agricultural robots in different scenes through real-time fusion of various sensor data, and overcomes the problems of scene dependence and insufficient accuracy of existing single-sensor solutions.

[0299] Although some embodiments of the present invention have been described in this application document, those skilled in the art can understand that these embodiments are merely shown as examples. Those skilled in the art can conceive of numerous variations, alternatives, and improvements without departing from the scope of the present invention under the teaching of the present invention. The appended claims are intended to define the scope of the present invention and thereby cover the methods and structures within the scope of these claims themselves and their equivalent transformations.

Claims

1. A multi-sensor cross-scenario dynamic optimal fusion positioning and mapping method, which is used for facility agricultural robots, and is characterized in that, The method includes: S1. Obtain the data of multiple sensors, and complete the unification of the spatio-temporal relationship of the data of multiple sensors. The multiple sensors include IMU, camera, lidar, GPS, UWB, and GNSS; S2 - S11. Process the data, perform loop detection on the key frame data of the images collected by the camera, construct the IMU pre-integration factor, visual-inertial odometry factor, lidar odometry factor, GPS-inertial odometry factor, UWB factor, GNSS factor, and loop detection factor, and add the IMU pre-integration factor, visual-inertial odometry factor, lidar odometry factor, GPS-inertial odometry factor, UWB factor, GNSS factor, and loop detection factor to the factor graph for optimization to obtain the global positioning pose and map; S12. Based on the deep fuzzy neural network, establish an online inference mechanism for the confidence of multiple sensors, evaluate the confidence of the multiple sensors in the current scenario, and dynamically adjust the sensor weights in the factor graph according to the confidence to optimize the multi-sensor data fusion strategy, that is, realize the cross-scenario dynamic optimal fusion positioning and mapping of multiple sensors.

2. The multi-sensor cross-scenario dynamic optimal fusion positioning and mapping method according to claim 1, wherein In step S1, static calibration and dynamic online self-calibration are adopted to achieve the unification of the spatio-temporal relationship of multi-sensor data. Specifically, it includes: S1-1. In static calibration, set the coordinate system of one sensor as the reference coordinate system, and the coordinate systems of other sensors are transformed into the reference coordinate system through the rotation matrix R i and the translation vector t i . The spatio-temporal transformation between multiple sensors is expressed by the following formula: where, T i is the transformation matrix of sensor i, R i is the rotation matrix of sensor i, representing the rotational transformation of sensor i relative to the reference coordinate system, and t i is the translation vector of sensor i; S1-2. In the dynamic online self-calibration stage, the spatio-temporal parameters have obtained initial values through static calibration. Project the motion trajectory obtained by multi-sensor pose estimation into the inertial measurement unit coordinate system, and use the trajectory similarity principle to measure the difference between the two trajectories through the Frechet distance. Suppose there are two trajectories X1 = {x1(t)} and X2 = {x2(t)}; The Frechet distance is defined as: where α(t) is a monotonically increasing mapping representing the correspondence between trajectories; The objective function constructed by the Frechet distance cost is used for the online optimization of the spatio-temporal parameters. The optimization goal is to minimize the sum of squared errors to obtain the optimal spatio-temporal parameters: where T is the spatio-temporal parameter to be optimized, and X1(t) and X2(t) are the estimated trajectories from any two sensors respectively; and / or The multiple sensors are set to be rigidly connected, and the external measurement method or the true value mapping method is used for the registration of target point, line, and plane features to obtain the initial values of the spatio-temporal parameters between each system.

3. The multi-sensor cross-scenario dynamic optimal fusion positioning and mapping method according to claim 1, characterized in that Steps S2 - S11 include: S2. Measure the inertial data of the robot through the IMU. The inertial data includes acceleration and angular velocity. The inertial data is integrated to obtain the preliminary pose estimation information of the robot, that is, the displacement and rotation information of the robot; S3. Obtain the first frame of lidar point cloud through the lidar, use the preliminary pose estimation information to correct the distortion of the lidar point cloud data to obtain the current lidar point cloud, and register the preliminary pose estimation information with the current lidar frame to correct the spatial position of the current lidar point cloud.

4. The multi-sensor cross-scenario dynamic optimal fusion positioning and mapping method according to claim 3, characterized in that Steps S2 - S11 also include: S4. Introduce the preliminary pose estimation information as a constraint into the factor graph to establish the IMU pre-integration factor. Specifically, it includes: S4-1. Integrate the acceleration to obtain the displacement increment Δp of the robot within the time interval, and the calculation formula is as follows: j , where the calculation formula is: where, Δt = t f ―t0 is the time interval, v k is the velocity of the robot at t k , a k is the acceleration of the robot at t k , v0 is the initial velocity of the robot at t0, p0 is the initial position of the robot at t0, and a(t) is the acceleration of the robot at t0; S4-2. Integrate the angular velocity to obtain the rotation increment ΔR of the robot k , and the calculation formula is: where, ΔR k is the rotation matrix, representing the rotational increment of the robot from the initial time t0 to the current time, and exp(·) is the exponential map of the Lie group, which is used to describe the rotational transformation; S4-3. Construct the IMU pre-integration factor, which is represented by nodes x k and x k+1 indicating the pose of the robot. The nodes x k and x k+1 correspond to the poses of the robot at times t k and t k+1 respectively. The constraint expression of the IMU pre-integration factor is: f k = [Δp k , ΔR k ​ Minimize the residual r of the IMU pre-integration factor k to optimize the pose estimation. The residual r k is calculated as follows: r k = f k - f IMU (p k , R k , p k―1 , R k―1 ) where, f IMU (·) is used to calculate the IMU pre-integration factor and estimate the pose change of the robot between two time points based on IMU data.

5. The multi-sensor cross-scenario dynamic optimal fusion positioning and mapping method according to claim 1, wherein Steps S2 - S11 also include: S5. Extract SURF visual key feature points and visual descriptors, establish a visual-inertial odometry factor based on the FLANN algorithm matching of the visual descriptors and the motion estimation of the camera, and add the visual-inertial odometry factor to the factor graph to optimize the pose estimation, which specifically includes: S5-1. The process of extracting SURF visual key feature points is expressed as: f = SURF(I) = {(P i , d i )} Among them, f is the set of extracted feature points, and P i =(x i , y i ) represents the position of the feature point, and d i is the descriptor of this feature point; S5-2. Use the FLANN algorithm to match the visual descriptors to find the corresponding feature point pairs of the same or similar objects in two frames of images. For each pair of feature point pairs, calculate the Euclidean distance between the visual descriptors to select the best matching pair. The distance metric of the matching process is expressed as: d(d i ,d j ) = ‖d i ―d j ‖ where d i and d j are the descriptors of the corresponding feature points P i and P j in two frames of images, respectively; S5-3. Estimate the relative motion of the cameras by using the matched feature point pairs through the essential matrix E or the homography matrix H; the relationship between the essential matrix E and the rotation matrix R camera and the translation vector t camera is as follows: E = [t camera × R camera ​ where, [t] × is the translation vector t camera is the skew-symmetric matrix of, R camera is the rotation matrix of the camera, representing the rotation relationship between the camera coordinate system and the world coordinate system, t camera is the translation vector of the camera, representing the translation of the camera in the world coordinate system; the internal parameters of the camera are known, and the essential matrix E or the homography matrix H is calculated by the eight-point method; and / or Homography matrix H and rotation matrix R of the camera camera and translation vector t camera The relationship between them is as follows: Where, K1 and K2 are the camera intrinsic parameter matrices corresponding to two different images, n is the plane normal vector, representing the normal direction of a certain plane in the image, used for matching with the points on the plane, and d is the distance from the plane to the camera, representing the vertical distance from the plane to the camera. S5-4. For the matched feature point pairs, calculate the rotation matrix R of the camera camera and the displacement vector t camera to obtain the visual-inertial odometry factor, and the constraint of the visual-inertial odometry factor is expressed as: f v = (R camera , t camera ) Among them, f v is the observed value of the visual-inertial odometry factor, which includes the rotation matrix R camera of the camera and the displacement vector t camera ; S5-5. Add the visual-inertial odometry factor to the factor graph and minimize the residual r of the visual-inertial odometry factor v to optimize the pose estimation. The residual r v is calculated as follows: r v = f v −f vision (p k , R k , p k―1 , R k―1 ) Among them, f vision (·) is a motion model estimated based on vision, representing the relative pose change between two frames of images.

6. The multi-sensor cross-scenario dynamic optimal fusion positioning and mapping method according to claim 5, wherein Steps S2 - S11 also include: S6. Detect the point, line, and plane features in the lidar point cloud through the 3D feature detector Velodyne HDL-64E, perform point cloud registration on the lidar point cloud data based on the ICP algorithm and the NDT algorithm to obtain the pose transformation, and introduce the result of the pose transformation as the lidar odometry factor into the factor graph to optimize the pose estimation, which specifically includes: S6-1. Calculate the point curvature to extract point features, extract line features by fitting continuous points in the laser point cloud using the least squares method, and extract surface features by fitting the plane region in the laser point cloud using the least squares method; the point curvature is estimated by the covariance matrix of each point in the neighborhood of the laser point cloud. Assume that each point p i =(x i , y i , z i ) has a neighborhood N i , and its local curvature κ i is given by the following formula: Among them, λ min is the minimum eigenvalue of point p i in its neighborhood N i and represents the degree of curvature of the laser point cloud surface at this point p i ; S6-2. Perform point cloud registration on the point cloud data of the lidar through the ICP algorithm, starting from the feature points extracted from the current lidar frame P i and the previous frame P k―1 . For each feature point p i in the current frame, find the closest point p j in the previous frame's point cloud. Perform registration based on the distance of the closest points, and calculate the error d(p i , p j ) between all point pairs: d(p i ,p j )=‖p i ―p j ‖ By minimizing the error d(p i , p j ), the rotation matrix R lidar and the translation vector t lidar are obtained, which represent the pose change of the current frame relative to the previous frame. The pose transformation after ICP algorithm optimization is expressed as: T k = R lidar T k―1 + t lidar Among them, T k is the pose of the current frame, T k―1 is the pose of the previous frame, R lidar is the rotation transformation of the lidar coordinate system from the previous moment to the current moment, t lidar is the translation transformation of the lidar coordinate system from the previous moment to the current moment; S6-3. Use the NDT algorithm to model the local area of the laser point cloud as a Gaussian distribution for matching. Divide the laser point cloud into multiple voxels, and perform Gaussian distribution modeling on the points within each voxel. Each voxel is represented as a Gaussian distribution Ν(μ,∑), where μ is the centroid of the voxel and ∑ is the covariance matrix; for each point p in the current frame of the laser point cloud u , calculate the distance between this point and the Gaussian distribution of the corresponding voxel in the previous frame of the laser point cloud, and estimate the pose transformation by minimizing the distance between the Gaussian distributions. The distance calculation between the Gaussian distributions is based on the Mahalanobis distance from the point to the Gaussian distribution: d(p i , N(μ, Σ)) = ‖Σ ―1 (p i − μ)‖ Through optimization iteration, the pose transformation T of the current frame is obtained k ; S6-4. The lidar-inertial odometry factor is added to the factor graph according to the pose transformation obtained by registration. In the factor graph, each lidar-inertial odometry factor contains the relative pose transformation between the current frame and the previous frame. The constraint of the pose transformation is expressed as: f l = [R lidar , t lidar ​ Among them, f l is the observed value of the lidar factor, representing the relative pose transformation between the current frame and the previous frame. R lidar is the rotation transformation of the lidar coordinate system from the previous moment to the current moment, and t lidar is the translation transformation of the lidar coordinate system from the previous moment to the current moment; S6-5. Introduce the lidar inertial odometry factor into the factor graph and minimize the residual r of the lidar inertial odometry factor l to optimize the pose estimation. The residual r l is calculated as follows: r l = f l - f lidar (p k , R k , p k―1 , R k―1 ) where r l is the residual of the lidar inertial odometry factor, and f lidar (·) is the registration model of lidar data, representing the relative pose change between lidar data calculated through lidar point cloud registration.

7. The multi-sensor cross-scenario dynamic optimal fusion positioning and mapping method according to claim 5, characterized in that Steps S2 - S11 also include: S7. Construct a GPS-inertial odometry based on the GPS differential correction technology, construct a GPS-inertial odometry factor according to the pose constraint relationship one and introduce the GPS-inertial odometry factor into the factor graph to optimize the pose estimation, which specifically includes: S7-1. The GPS differential correction technology introduces a GPS receiver with a known position as a reference station. The reference station receives satellite signals, calculates its position error, and then transmits the correction information to the GPS receiver of the robot. The known position of the reference station is Δp ref , and the received GPS position is p GPS,ref . The differential correction value is: Δp diff = p ref − p GPS,ref The differential correction value is passed to the GPS receiver of the robot, and the GPS position received by the robot is p GPS,mobile , after applying the differential correction value, the corrected position of the robot is: p GPS,corrected = p GPS,mobile + Δp diff S7-2. Construct a GPS inertial odometer factor based on the position information received by the robot through GPS and the precise pose obtained through differential correction. At time t of the robot k the GPS position is p GPS,corrected , and the constraint in Constraint Relationship 1 is expressed as: f GPS = p GPS,corrected Minimize the residual r of the factor graph GPS to optimize the pose estimation and obtain a more accurate global pose estimation. The residual r GPS is expressed as: r GPS = f GPS −f(p k ) Among them, f(p k ) is the pose estimation obtained during factor graph optimization.

8. The multi-sensor cross-scenario dynamic optimal fusion positioning and mapping method according to claim 5, characterized in that Steps S2 - S11 also include: S8. Construct a UWB-inertial odometry based on the wireless communication time difference algorithm, construct a UWB factor according to the pose constraint relationship two and introduce the UWB factor into the factor graph, which specifically includes: S8-1. The UWB technology estimates the distance between the robot and the UWB base station by measuring the time difference of signal propagation, and obtains the distance d between the robot and the UWB base station through the time difference measurement i : d i = || p mobile - p UWB,i || where d i is the distance between the robot and the i-th UWB base station, p mobile is the current position of the robot, and p UWB,i is the position of the UWB base station i; When there are N base stations, the pose of the robot is estimated by minimizing the following objective function to obtain an accurate pose estimation: S8-2. Construct a UWB inertial odometry factor p based on the distance information measured by the UWB base station and the preliminary pose estimation information. mobile p is the current position of the robot, and d i is the distance between the robot and the i-th UWB base station. The constraint in the second pose constraint relationship is expressed as: f UWB = ‖p mobile ―p UWB,i ‖―d i The UWB inertial odometer factors constrain the ranging error between the robot and the UWB base station, and optimize the pose estimation by minimizing the residual r in the factor graph. The residual is expressed as: UWB , r UWB = f UWB - f(p k )。 9. The multi-sensor cross-scenario dynamic optimal fusion positioning and mapping method according to claim 5, wherein Steps S2 - S11 also include: S9. Construct a GNSS-inertial odometry based on the GNSS signal, construct a GNSS factor according to the position information provided by the GNSS and the pose estimations of the lidar, IMU, camera, GPS, and UWB, and combine the pose constraint relationship three, and introduce the GNSS factor into the factor graph, which specifically includes: The constraint in the pose constraint relationship three is expressed as: f GNSS = [x GNSS , y GNSS , z GNSS T ​ Among them, f GNSS is the observation value of the GNSS factor, representing the position information of the robot obtained through the GNSS system, x GNSS , y GNSS , z GNSS respectively represent the longitude, latitude, and altitude information of the robot provided by GNSS.

10. The multi-sensor cross-scenario dynamic optimal fusion positioning and mapping method according to claim 5, characterized in that Steps S2 - S11 also include: S10. Use the DBoW2 loop detection algorithm to perform loop detection on the key frame data of the image, extract BRIEF descriptors and match them with the visual descriptors extracted in step S5. Transmit the image timestamps of the loop candidates to the lidar inertial system for further verification, and establish loop detection factors based on the pose constraint relationship four. The pose estimation relationship four is the relative pose difference between the current image obtained by the DBoW2 loop detection algorithm for loop detection of the key frame data of the image and the loop candidate image. The relative pose difference describes the rotation and displacement from the current image coordinate system to the loop candidate image coordinate system, that is, the change in the spatial position and orientation between these two images.

11. The multi-sensor cross-scenario dynamic optimal fusion positioning and mapping method according to claim 1, wherein Steps S2 - S11 further include: S11. Construct the robot pose as a factor graph variable node, and add the IMU pre-integration factor, visual inertial odometry factor, lidar odometry factor, GPS inertial odometry factor, UWB factor, GNSS factor, and loop detection factor as factors into the factor graph for optimization to obtain the global positioning pose and map. Specifically, it includes: S11 - 1. Based on the Gaussian distribution, construct the error expressions of the motion equation and the observation equation. The error expression of the motion equation is constructed as follows: where f(X k , u k ) is a motion model that predicts the next state X k based on the current state X k and the control input u k+1 , Ω motion is the covariance matrix of this equation, representing the error distribution during the motion process, and ‖·‖ represents the Euclidean norm of the error; The actual measurement value z obtained through the sensor k and the predicted observation value h(X k ) obtained from the robot state X k are used to construct an observation equation, and the observation equation error expression is constructed as follows: where h(X k ) is the predicted observation value obtained from the sensor model based on the robot state X k , z k is the actual observation value directly measured by the sensor, and Ω sensor is the covariance matrix of the observation error, representing the spatial distribution of the measurement error; S11 - 2. Use the non-linear incremental optimization method of the factor graph to optimize the objective function in the Bayesian network, gradually increase the posterior probability, and finally obtain the optimal solution. The construction of the objective function in the Bayesian network is as follows: Among them, X i represents the robot pose state variable at the i-th moment, and f motion represents the motion constraint based on IMU pre-integration. f lidar , f vision , f GNSS , f GPS , f UWB represent the lidar odometry factor, visual-inertial odometry factor, GNSS factor, GPS-inertial odometry factor, UWB factor. f loop represents the loop closure detection factor, and Ω motion , Ω lidar , Ω vision , Ω GNSS , Ω GPS , Ω UWB , Ω loop correspond to the covariance matrices of different observation factors respectively, which are used to describe the spatial distribution of the measurement errors of each sensor. represent the weights of each sensor, and the weights are dynamically adjusted according to the confidence of the sensors to improve the fusion accuracy.

12. The multi-sensor cross-scenario dynamic optimal fusion positioning and mapping method according to claim 1, wherein, Step S12 specifically includes: S12 - 1. In the first layer of the depth fuzzy neural network, use fuzzy processing to extract the fuzzy features of the multi-sensor input data to convert the multi-sensor data into a fuzzy set. The conversion is expressed as: Among them, represents the sensor data after fuzzification, Y i is the original data of sensor i, Y is the set of all sensor data, Φ(Y i ) is the fuzzification function; S12 - 2. In the second layer of the depth fuzzy neural network, adopt a deep inference module based on the Gumbel-Softmax activation function to perform real-time inference on the sensor signal quality and environmental changes, and dynamically adjust the confidence weights of each sensor, expressed as: p t = Gumbel-Softmax(ω sensor , temperature) where p t is the confidence of the sensor in the current scenario, ω sensor is the weight of the sensor, representing the signal quality of the sensor, and temperature controls the "softening" degree of the Gumbel-Softmax; S12 - 3. In the third layer of the depth fuzzy neural network, use a fully connected layer to process the outputs of the first and second layers of the depth fuzzy neural network and calculate the final sensor confidence value, expressed as: wherein, represents the sensor confidence calculated by the neural network, which is the output of the third layer of the deep fuzzy neural network and is used to describe the sensor confidence value of the sensor at the current moment t. h2 is the output of the second layer of the deep fuzzy neural network, and W3 and b3 are the weights and biases of the third layer of the deep fuzzy neural network; S12-4. Apply the calculated sensor confidence value to the dynamic adjustment of the sensor weights in the factor graph model. By optimizing the confidence of the sensors, the optimal data fusion effect can be achieved in different scenarios. The dynamic adjustment of the sensor weights is expressed as: where ω sensor (t) represents the weight of the sensor at the current time t, is the sensor confidence calculated in S12-3, and α and β are adjustment factors that control the relationship between the confidence and the weight; S12-5 Continuously train and optimize according to the performance of the sensor in actual applications, and continuously adjust the weight ω of the sensor sensor , and the training is carried out by minimizing the error, and the error function is expressed as: Among them, is the sensor confidence calculated in S12-3 p target is the target confidence, and the error function optimizes the weight ω sensor to minimize the prediction error and optimize the multi-sensor data fusion strategy.

Images