Multi-sensor fusion evaluation algorithm and device based on continuous time series

Abstract

According to the multi-sensor fusion evaluation algorithm and device based on the continuous time sequence, modeling is carried out based on continuous time, physical reality is better met, motion modeling with better precision is achieved, intra-frame motion can be accurately described, smoother and more accurate track estimation can be provided, and the method and the device are suitable for being applied to multi-sensor fusion evaluation. Especially, the advantages are obvious in high-speed and high-frequency vibration scenes; meanwhile, according to the technical scheme disclosed by the invention, when LiDAR / Camera matching fails, the constraints of the IMU and the GNSS are seamlessly and continuously applied to the whole track through a continuous time model instead of only acting on a discrete frame, so that error accumulation is greatly inhibited, system collapse is avoided, the robustness of a degraded scene is enhanced, application in specific fields such as surveying and mapping is greatly facilitated, and the method is suitable for popularization and application. And the method has the capability of flexibly adding constraints.

Description

Technical Field

[0001] This disclosure relates to the field of simultaneous localization and mapping, and in particular to a multi-sensor fusion evaluation algorithm and apparatus based on continuous time series. Background Technology

[0002] In the field of Simultaneous Localization and Mapping (SLAM), multi-sensor fusion technology has become a hot topic in current research and application because it can integrate the advantages of different sensors and improve the robustness and accuracy of the system in complex environments.

[0003] Multi-sensor fusion SLAM algorithms based on Extended Kalman Filter (EKF) fundamentally transform the SLAM problem into a state estimation problem. The EKF recursively estimates the robot's pose state and the state of environmental feature points. In terms of sensor fusion, these algorithms typically input data from sensors such as LiDAR, cameras, inertial measurement units (IMUs), and Global Navigation Satellite Systems (GNSS) in a specific timestamp order into the EKF filter for state updates. For example, high-frequency motion information provided by the IMU is used for prediction steps, combined with environmental observation information from LiDAR or cameras and absolute position information from GNSS for update steps, thereby achieving multi-sensor data fusion and completing the robot's localization and map construction.

[0004] Discrete-time multi-sensor fusion SLAM algorithms based on graph optimization model the SLAM problem as a graph structure. Nodes in the graph represent the robot's pose at different discrete time points and feature points in the environment, while edges represent the constraints between nodes provided by sensor measurements. This type of algorithm optimizes the graph, adjusting the state of the nodes to satisfy the constraints of all edges, thereby obtaining the optimal pose estimation and map. In terms of multi-sensor fusion, it processes measurement data from LiDAR, Camera, IMU, and GNSS separately, converting them into corresponding constraint edges added to the graph. Then, it uses nonlinear optimization algorithms (such as the Gauss-Newton method and the Levenberg-Marquardt method) to solve for the optimal state of the graph.

[0005] Although the existing multi-sensor fusion SLAM algorithms based on discrete time series can achieve robot localization and mapping to a certain extent, they still have many insurmountable shortcomings in practical applications, as follows:

[0006] 1. Discrete-time models do not match actual motion characteristics, resulting in limited estimation accuracy.

[0007] Current technologies generally employ discrete-time series models to model SLAM problems. These models discretize the robot's motion into a series of discontinuous time points, estimating and optimizing the robot's pose only at these discrete time points. However, in real-world scenarios, robot motion is a continuous process, with its pose, velocity, acceleration, and other motion states changing continuously over time. Because discrete-time models ignore motion state information between adjacent time points, they cannot accurately characterize the continuous motion characteristics of the robot and are prone to significant pose estimation errors when the time intervals are large or when the robot is performing high-speed, high-dynamic movements. For example, when a robot moves at high speed in a complex environment, discrete-time models struggle to capture instantaneous pose changes during motion, leading to deviations in subsequent map construction and affecting the accuracy of the entire SLAM system.

[0008] 2. The sensor fusion method is loose and struggles to cope with scenarios where matching degrades.

[0009] Most existing multi-sensor fusion technologies employ loosely coupled or semi-tightly coupled approaches, failing to form a truly tightly coupled fusion framework. In loosely coupled and semi-tightly coupled schemes, data from each sensor is often processed individually before simple fusion, failing to fully utilize the information interaction and constraint relationships between different sensors. This loose fusion method makes the system highly dependent on sensor data. In scenarios where GNSS signals are lost or weak, such as tunnels or underground parking garages, or in scenarios lacking obvious environmental features, such as corridors or open halls, leading to degradation in LiDAR and camera matching, the system is prone to problems such as positioning drift and map building disorder due to the failure of one or more sensors or a decrease in measurement accuracy. For example, in a tunnel environment, GNSS signals are completely absent. If the LiDAR cannot perform effective point cloud matching due to the simple structure of the tunnel walls, and the camera cannot obtain reliable visual information due to dim lighting and insufficient environmental features, existing loosely fused SLAM algorithms will struggle to maintain stable and accurate positioning and map building, greatly limiting their applicability in complex environments.

[0010] 3. External parameters and time synchronization errors are difficult to estimate accurately, affecting system accuracy.

[0011] In multi-sensor fusion SLAM systems, extrinsic parameters (including position offset and attitude deviation) and time synchronization errors between sensors play a crucial role in the system's accuracy. Existing technologies typically require offline calibration to determine these extrinsic parameters and time synchronization parameters before system startup, maintaining these parameters constant throughout the SLAM process. However, in practical applications, robot movement can cause slight changes in sensor mounting positions due to vibrations, collisions, etc., leading to deviations in the pre-calibrated extrinsic parameters. Simultaneously, differences in sampling frequencies and data transmission delays among different sensors can cause time synchronization errors to dynamically change over time. Existing discrete-time series-based SLAM algorithms, due to limitations in their model structure, cannot estimate and correct these time-varying extrinsic parameters and time synchronization errors in real time during the SLAM process. This results in decreased spatiotemporal alignment accuracy between sensor data, consequently affecting the localization and mapping accuracy of the entire SLAM system. Summary of the Invention

[0012] This disclosure provides at least one multi-sensor fusion evaluation algorithm and apparatus based on continuous time series to solve at least one of the above-mentioned technical problems.

[0013] According to one aspect of this disclosure, a multi-sensor fusion evaluation algorithm based on continuous time series is provided, comprising:

[0014] The raw point cloud data is preprocessed to obtain point cloud features; the raw image is preprocessed to obtain visualization features; the raw IMU data is preprocessed to obtain short-term pose estimation of the IMU; the raw GNSS data is preprocessed to obtain the robot's absolute position and velocity information; the short-term pose estimation includes the IMU's angular velocity and linear acceleration.

[0015] A radar measurement model is constructed based on the robot's pose, radar extrinsic parameters relative to the robot, and point cloud features; a camera measurement model is constructed based on camera extrinsic parameters, camera intrinsic parameters, visualization features, and the robot's pose; an IMU measurement model is constructed based on IMU angular velocity, linear acceleration, angular velocity bias, linear acceleration bias, IMU measurement noise, gravitational acceleration, and the robot's pose; and a GNSS measurement model is constructed based on the robot's absolute position, velocity information, robot pose, GNSS measurement noise, and synchronization error between GNSS and system time.

[0016] The continuous motion process of the robot is modeled using k-order B-spline curves: within a set time interval, [the following is set]. A uniform or non-uniform pose control point Furthermore, a B-spline basis function is constructed at each pose control point. Using the B-spline basis function and the robot's pose at the pose control point, a spline parameterized representation of the robot's pose state at any time within a set time interval is constructed.

[0017] An optimization objective function is constructed using radar measurement models, camera measurement models, IMU measurement models, GNSS measurement models, environmental map parameters, raw point cloud data, raw images, raw IMU data, raw GNSS data, initial estimates of the robot at pose control points, the robot's actual pose at pose control points, and the spline parameterized representation of the pose state. The environmental map parameters are the coordinates of feature points in the keyframes corresponding to the raw point cloud coordinates or the visual matching factors of the feature points.

[0018] Using the pose of the pose control points and the environmental map parameters as optimization variables, a nonlinear optimization algorithm is used to minimize the optimization objective function, thereby obtaining the robot's optimal pose and optimal environmental map parameters.

[0019] In one possible implementation, the preprocessing of the raw point cloud data to obtain point cloud features includes:

[0020] Perform distortion correction on the raw point cloud data;

[0021] The distortion-reduced point cloud is segmented into ground points and non-ground points using a ground segmentation algorithm;

[0022] Curvature-based feature extraction algorithms are used to extract features from ground point clouds and non-ground point clouds to obtain line feature data and surface feature data for matching.

[0023] In one possible implementation, the preprocessing of the original image to obtain visual features includes:

[0024] The original image is subjected to histogram equalization, noise reduction, and distortion correction.

[0025] FAST feature points in the distortion-corrected image are obtained using a feature extraction algorithm.

[0026] The Lucas Kanade optical flow tracing algorithm from OpenCV is used to perform feature tracking based on the extracted fast feature points, and the visual matching factor of the feature points in the key frame is determined by the tracking information of the fast feature points in adjacent key frames.

[0027] In one possible implementation, the preprocessing of the raw IMU data to obtain the short-term pose estimate of the IMU includes:

[0028] Zero bias compensation and temperature drift correction are performed on the angular velocity and linear acceleration data output by the IMU;

[0029] The temperature drift-corrected data is processed by integral calculation to obtain the short-term pose estimate of the IMU.

[0030] In one possible implementation, the radar measurement model is as follows:

[0031] ;

[0032] Among them, the robot is at all times The position is , Let be the rotation matrix from the world coordinate system to the robot's body coordinate system. The position vector; the robot's extrinsic parameters include position offsets. and attitude rotation PL represents the coordinates of the feature points in the point cloud feature in the radar coordinate system, and PW represents the coordinates of the feature points in the point cloud feature in the world coordinate system.

[0033] The camera measurement model is as follows:

[0034] ;

[0035] Where (u,v) represents the coordinates of a pixel on the camera image plane; K is the camera intrinsic parameter; and the camera extrinsic parameter includes the position offset. and attitude rotation ; This provides the coordinates of feature points in the camera coordinate system for visualization.

[0036] In one possible implementation, the IMU measurement model is as follows:

[0037] ;

[0038] ;

[0039] in, The angular velocity measurement value of the IMU; The linear acceleration measurement value of the IMU; , These are the zero bias of the IMU's angular velocity and linear acceleration, respectively. , To measure noise for the IMU, Let gravitational acceleration vector be the vector; the robot at time t is... The position is , Let be the rotation matrix from the world coordinate system to the robot's body coordinate system. It is a position vector;

[0040] The GNSS measurement model is as follows:

[0041] ;

[0042] ;

[0043] in, For the robot's absolute position, For the robot's absolute speed, This refers to the synchronization error between GNSS and system time. , Noise for GNSS measurements.

[0044] In one possible implementation, the pose state spline parameterization of the robot at any time within a set time interval is as follows:

[0045] ;

[0046] Where i represents the pose control point number, Pi represents the position and attitude components of the robot at pose control point i, and m represents the maximum number of the pose control point. Let k denote the B-spline basis function, k denote the B-spline order, and t denote time.

[0047] In one possible implementation, the optimization objective function is as follows: ;

[0048] Where S represents sensor, LiDar represents radar, Camera represents camera, and M represents environmental map parameters. For sensors At any moment The original measurement data, For sensor-based The measurement model, in the robot pose is Environmental map parameters are Theoretical measurement value at time For robust kernel functions, These are the prior weighting coefficients. Let Pi be the initial estimated value of pose control point i, and let Pi be the actual pose of pose control point i.

[0049] In one possible implementation, the multi-sensor fusion evaluation algorithm based on continuous time series further includes:

[0050] Using the robot's extrinsic parameters, the synchronization error between GNSS and system time, the pose of the pose control point, and the environmental map parameters as optimization variables, the objective function is minimized to obtain the robot's optimal pose, optimal environmental map parameters, optimal robot extrinsic parameters, and optimal synchronization error.

[0051] On the other hand, this disclosure provides a multi-sensor fusion evaluation device based on continuous time series, including:

[0052] The preprocessing module is used to preprocess the raw point cloud data to obtain point cloud features; preprocess the raw image to obtain visualization features; preprocess the raw IMU data to obtain short-term pose estimation of the IMU; and preprocess the raw GNSS data to obtain the robot's absolute position and velocity information; the short-term pose estimation includes the IMU's angular velocity and linear acceleration.

[0053] The measurement model construction module is used to construct radar measurement models based on robot pose, radar extrinsic parameters relative to the robot, and point cloud features; to construct camera measurement models based on camera extrinsic parameters, camera intrinsic parameters, visualization features, and robot pose; to construct IMU measurement models based on IMU angular velocity, linear acceleration, angular velocity bias, linear acceleration bias, IMU measurement noise, gravitational acceleration, and robot pose; and to construct GNSS measurement models based on robot absolute position, velocity information, robot pose, GNSS measurement noise, and synchronization error between GNSS and system time.

[0054] The spline parameterization module is used to model the continuous motion process of a robot using k-order B-spline curves: setting parameters within a defined time interval. A uniform or non-uniform pose control point Furthermore, a B-spline basis function is constructed at each pose control point. Using the B-spline basis function and the robot's pose at the pose control point, a spline parameterized representation of the robot's pose state at any time within a set time interval is constructed.

[0055] The objective function construction module is used to construct an optimization objective function using radar measurement models, camera measurement models, IMU measurement models, GNSS measurement models, environmental map parameters, raw point cloud data, raw images, raw IMU data, raw GNSS data, initial estimates of the robot at pose control points, the robot's actual pose at pose control points, and the spline parameterized representation of the pose state. The environmental map parameters are the coordinates of feature points in the keyframes corresponding to the raw point cloud coordinates or the visual matching factors of the feature points.

[0056] The optimization module is used to minimize the objective function by employing a nonlinear optimization algorithm, with the pose of the pose control points and the environmental map parameters as optimization variables, to obtain the optimal pose of the robot and the optimal environmental map parameters.

[0057] This disclosure discloses a multi-sensor fusion evaluation algorithm and apparatus based on continuous time series. First, it preprocesses the raw point cloud data to obtain point cloud features; preprocesses the raw image to obtain visualization features; preprocesses the raw IMU data to obtain short-term pose estimation of the IMU; and preprocesses the raw GNSS data to obtain the robot's absolute position and velocity information. The short-term pose estimation includes the IMU's angular velocity and linear acceleration. Then, it constructs a radar measurement model based on the robot's pose, radar extrinsic parameters relative to the robot, and point cloud features; it constructs a camera measurement model based on camera extrinsic parameters, camera intrinsic parameters, visualization features, and the robot's pose; it constructs an IMU measurement model based on the IMU's angular velocity, linear acceleration, angular velocity bias, linear acceleration bias, IMU measurement noise, gravitational acceleration, and the robot's pose; it constructs a GNSS measurement model based on the robot's absolute position, velocity information, robot pose, GNSS measurement noise, and the synchronization error between GNSS and system time; finally, it models the robot's continuous motion process using a k-order B-spline curve: setting a time interval... A uniform or non-uniform pose control point A B-spline basis function is constructed at each pose control point. Using the B-spline basis function and the robot's pose at the pose control point, a spline parameterized representation of the robot's pose state at any given time interval is constructed. Then, an optimization objective function is constructed using radar measurement models, camera measurement models, IMU measurement models, GNSS measurement models, environmental map parameters, raw point cloud data, raw images, raw IMU data, raw GNSS data, the robot's initial estimate at the pose control point, the robot's actual pose at the pose control point, and the spline parameterized representation of the pose state. The environmental map parameters are the coordinates of feature points in the keyframe corresponding to the raw point cloud coordinate set or the visual matching factors of the feature points. Finally, using the pose of the pose control point and the environmental map parameters as optimization variables, a nonlinear optimization algorithm is used to minimize the optimization objective function, obtaining the optimal robot pose and optimal environmental map parameters. The above-disclosed scheme is based on continuous-time modeling, which is more in line with physical reality and achieves better motion modeling accuracy. It can accurately describe intra-frame motion and provide smoother and more accurate trajectory estimation, especially in high-speed and high-frequency vibration scenarios. At the same time, when LiDAR / Camera matching fails, the constraints of IMU and GNSS (if available) are seamlessly and continuously applied to the entire trajectory through the continuous-time model, rather than only acting on discrete frames. This greatly suppresses error accumulation, avoids system crashes, enhances robustness in degraded scenarios, and greatly facilitates applications in specific fields such as surveying and mapping, with the ability to flexibly add constraints. In addition, the scheme is based on pose state spline parameterization for modeling, which allows pose to be queried at any continuous time point. Therefore, it can very naturally fuse asynchronous, different frequency sensor data (such as low-frequency GNSS and high-frequency IMU), achieving natural asynchronous sensor fusion. In summary, the technical solution disclosed herein uses continuous-time B-spline functions as the core to construct a tightly coupled multi-sensor fusion scheme, which enables accurate estimation of robot pose, environmental map, sensor extrinsic parameters and time synchronization error, significantly improving the robustness and accuracy of the SLAM system in complex scenarios.

[0058] It should be understood that the description in this section is not intended to identify key or essential features of the embodiments of this disclosure, nor is it intended to limit the scope of this disclosure. Other features of this disclosure will become readily apparent from the following description. Attached Figure Description

[0059] The accompanying drawings are provided to better understand this solution and do not constitute a limitation of this disclosure. Wherein:

[0060] Figure 1 This is a flowchart of a multi-sensor fusion evaluation algorithm based on continuous time series according to an embodiment of the present disclosure;

[0061] Figure 2 This is a schematic diagram of the structure of a multi-sensor fusion evaluation device based on continuous time series according to an embodiment of the present disclosure. Detailed Implementation

[0062] The exemplary embodiments of this disclosure are described below with reference to the accompanying drawings, including various details of the embodiments to aid understanding, and should be considered merely exemplary. Therefore, those skilled in the art will recognize that various changes and modifications can be made to the embodiments described herein without departing from the scope and spirit of this disclosure. Similarly, for clarity and brevity, descriptions of well-known functions and structures are omitted in the following description.

[0063] This disclosure addresses the shortcomings of current SLAM algorithms, such as the discrepancy between discrete-time models and actual motion characteristics, leading to limited estimation accuracy, loose sensor fusion methods, difficulty in handling matching degradation scenarios, and difficulty in accurately estimating extrinsic parameters and time synchronization errors, which affect system accuracy. It proposes a multi-sensor fusion evaluation algorithm and device based on continuous time series. The technical solution of this disclosure uses a continuous-time B-spline function as the core to construct a tightly coupled multi-sensor fusion scheme, achieving accurate estimation of robot pose, environmental map, sensor extrinsic parameters, and time synchronization errors, significantly improving the robustness and accuracy of the SLAM system in complex scenarios.

[0064] The technical solution of this disclosure will be described below through specific embodiments.

[0065] like Figure 1 The diagram shown is a flowchart of the multi-sensor fusion evaluation algorithm based on continuous time series in this embodiment. The execution subject of this embodiment is a computing device, component, system, or apparatus with data processing capabilities. Specifically, the method of this embodiment may include the following steps:

[0066] S110. Preprocess the raw point cloud data to obtain point cloud features; preprocess the raw image to obtain visualization features; preprocess the raw IMU data to obtain short-term pose estimation of the IMU; preprocess the raw GNSS data to obtain the robot's absolute position and velocity information; the short-term pose estimation includes the IMU's angular velocity and linear acceleration.

[0067] Specifically, the original point cloud data is preprocessed to obtain point cloud features, including: performing distortion correction on the original point cloud data; segmenting the distortion-corrected point cloud into ground points and non-ground points using a ground segmentation algorithm; and using a curvature-based feature extraction algorithm to extract features from the ground point cloud and non-ground point cloud to obtain line feature data and surface feature data for matching.

[0068] The original image is preprocessed to obtain visualization features, including histogram equalization, denoising, and distortion correction. FAST feature points in the distortion-corrected image are obtained through feature extraction algorithms. The Lucas Kanade optical flow tracing algorithm of OpenCV is used to track features based on the extracted FAST feature points, and the visual matching factor of the feature points of the key frame is determined by the tracking information of the FAST feature points of adjacent key frames.

[0069] The raw IMU data is preprocessed to obtain the short-term pose estimate of the IMU, including: zero bias compensation and temperature drift correction of the angular velocity and linear acceleration data output by the IMU; and the temperature drift corrected data is processed by integral calculation to obtain the short-term pose estimate of the IMU.

[0070] S120. Construct a radar measurement model based on the robot's pose, radar extrinsic parameters relative to the robot, and point cloud features; construct a camera measurement model based on camera extrinsic parameters, camera intrinsic parameters, visualization features, and the robot's pose; construct an IMU measurement model based on IMU angular velocity, linear acceleration, angular velocity bias, linear acceleration bias, IMU measurement noise, gravitational acceleration, and the robot's pose; construct a GNSS measurement model based on the robot's absolute position, velocity information, robot pose, GNSS measurement noise, and synchronization error between GNSS and system time.

[0071] Specifically, the radar measurement model is as follows:

[0072] ;

[0073] Among them, the robot is at all times The position is , Let be the rotation matrix from the world coordinate system to the robot's body coordinate system. The position vector; the robot's extrinsic parameters include position offsets. and attitude rotation P L P represents the coordinates of the feature points in the point cloud in the radar coordinate system. W This represents the coordinates of the feature points in the point cloud in the world coordinate system.

[0074] The camera measurement model is as follows:

[0075] ;

[0076] Where (u,v) represents the coordinates of a pixel on the camera image plane; K is the camera intrinsic parameter; and the camera extrinsic parameter includes the position offset. and attitude rotation ; This provides the coordinates of feature points in the camera coordinate system for visualization.

[0077] The IMU measurement model is as follows:

[0078] ;

[0079] ;

[0080] in, The angular velocity measurement value of the IMU; The linear acceleration measurement value of the IMU; , These are the zero bias of the IMU's angular velocity and linear acceleration, respectively. , To measure noise for the IMU, Let gravitational acceleration vector be the vector; the robot at time t is... The position is , Let be the rotation matrix from the world coordinate system to the robot's body coordinate system. This is a position vector.

[0081] The GNSS measurement model is as follows:

[0082] ;

[0083] ;

[0084] in, For the robot's absolute position, For the robot's absolute speed, This refers to the synchronization error between GNSS and system time. , Noise for GNSS measurements.

[0085] S130. Model the continuous motion process of the robot using k-order B-spline curves: Set within a set time interval A uniform or non-uniform pose control point Furthermore, a B-spline basis function is constructed at each pose control point. Using the B-spline basis function and the robot's pose at the pose control point, a spline parameterized representation of the robot's pose state at any time within a set time interval is constructed.

[0086] The pose state spline parameterization of the robot at any time within a set time interval is as follows:

[0087] ;

[0088] Where i represents the pose control point number, P iThis represents the position and attitude components of the robot at pose control point i, where m represents the maximum index of the pose control point. Let k denote the B-spline basis function, k denote the B-spline order, and t denote time.

[0089] S140. Construct an optimization objective function using radar measurement model, camera measurement model, IMU measurement model, GNSS measurement model, environmental map parameters, raw point cloud data, raw images, raw IMU data, raw GNSS data, initial estimates of the robot at pose control points, the robot's actual pose at pose control points, and the spline parameterized representation of pose state; the environmental map parameters are the coordinates of feature points in the keyframes corresponding to the raw point cloud coordinate set or the visual matching factors of the feature points.

[0090] The objective function is optimized as follows:

[0091] ;

[0092] Where S represents sensor, LiDar represents radar, Camera represents camera, and M represents environmental map parameters. For sensors At any moment The original measurement data, For sensor-based The measurement model, in the robot pose is Environmental map parameters are Theoretical measurement value at time For robust kernel functions, These are the prior weighting coefficients. Let Pi be the initial estimated value of pose control point i, and let Pi be the actual pose of pose control point i.

[0093] S150. Using the pose of the pose control point and the environmental map parameters as optimization variables, a nonlinear optimization algorithm is used to minimize the optimization objective function to obtain the robot's optimal pose and optimal environmental map parameters.

[0094] In some embodiments, the robot's extrinsic parameters, the synchronization error between GNSS and system time, the pose of the pose control point, and the environmental map parameters can be used as optimization variables to minimize the optimization objective function, thereby obtaining the robot's optimal pose, optimal environmental map parameters, optimal robot extrinsic parameters, and optimal synchronization error.

[0095] The technical solution of this disclosure will be described in detail below through specific embodiments. Specifically, the solution of this embodiment includes the following steps:

[0096] (1) Sensor data preprocessing and spatiotemporal alignment

[0097] LiDAR data, namely the aforementioned raw point cloud data: The raw LiDAR point cloud is subjected to distortion correction processing (based on the angular velocity and linear acceleration data provided by the IMU to compensate for the point cloud distortion caused by robot motion), and ground points and non-ground points are segmented by a ground segmentation algorithm. A curvature-based feature extraction algorithm is used to obtain line feature and surface feature data for matching.

[0098] Camera data, i.e. the aforementioned original images: The images acquired by the camera are preprocessed, including histogram equalization, noise reduction and distortion correction. FAST feature points in the image are obtained through feature extraction algorithms. Feature tracking is performed using OpenCV's LucasKanade optical flow tracking algorithm. Visual matching factors of feature points in key frames are constructed using feature point tracking information from adjacent key frames.

[0099] IMU data, namely the aforementioned raw IMU data: the angular velocity and linear acceleration data output by the IMU are preprocessed with zero bias compensation, temperature drift correction and other methods, and the short-term pose estimation of the IMU (such as attitude angle and position increment) is obtained by integral calculation, which provides high-frequency motion constraints for the B-spline motion model.

[0100] GNSS data, also known as the aforementioned raw GNSS data, involves processing satellite signals received from GNSS (such as point positioning and differential positioning) to obtain the robot's absolute position and velocity information. For scenarios with weak GNSS signals (such as tunnel entrances), Kalman filtering is used to smooth the GNSS data and improve measurement accuracy.

[0101] 2. Construction of Multi-Sensor Measurement Model

[0102] LiDAR Measurement Model: Let the extrinsic parameters of LiDAR relative to the robot body be... (Including position offset) and attitude rotation The robot is always The position is ( Let be the rotation matrix from the world coordinate system to the robot's body coordinate system. (where the position vector is used), then the coordinates of the LiDAR feature points in the world coordinate system are... It can be represented as:

[0103] ;

[0104] in, These are the coordinates of the LiDAR feature points in the LiDAR coordinate system. The LiDAR measurement residual is the difference between the coordinates of the actual measured point cloud feature and the corresponding feature in the map.

[0105] Camera measurement model: Assume the camera's extrinsic parameters are... (Including position offset) and attitude rotation (Camera intrinsic parameters are) (Including parameters such as focal length and principal point coordinates), the coordinates of the 3D visual feature points in the world coordinate system are: Then its pixel coordinates on the camera image plane satisfy:

[0106] ;

[0107] in, The coordinates of the visual feature points in the camera coordinate system are given. The Camera measurement residual is the difference between the actual pixel coordinates and the theoretical pixel coordinates.

[0108] The algorithm optimizes the pose R(t) and t(t) of the carrier system. Specifically, for different sensor data, conversion is required using extrinsic parameters; that is, lidar data needs to be converted to R. Lb and t Lb However, camera data needs to be compensated for R. Cb and t Cb The extrinsic parameters of these two sensors are also values ​​to be estimated, and will be optimized and solved later.

[0109] IMU measurement model: angular velocity measurements from the IMU And linear acceleration measurement The robot's motion state satisfies:

[0110] ;

[0111] ;

[0112] in, , These are the zero bias of the IMU's angular velocity and linear acceleration, respectively. , To measure noise, This is the vector of gravitational acceleration. It is obtained by differentiating the B-spline curve. (Attitude angular velocity) and (Linear acceleration) is compared with the IMU measurement value to obtain the IMU measurement residual.

[0113] GNSS Measurement Model: Absolute Position Provided by GNSS and speed With robot pose The relationship is:

[0114] ;

[0115] ;

[0116] in, This refers to the synchronization error between GNSS and system time. , This refers to GNSS measurement noise. The GNSS measurement residual is the difference between the actual measured position and velocity and the position and velocity output by the B-spline model.

[0117] 3. B-Spline Parameterization

[0118] The continuous motion process of the robot is modeled using k-order B-spline curves, representing the robot's pose (position and attitude), velocity, acceleration, and other motion states as functions of the B-spline curves. Let the time interval be... Define within this time interval A uniform or non-uniform pose control point Construct B-spline basis functions (in For base function index, (The order is B-spline). At any given time, the robot... pose state It can be represented as:

[0119] ;

[0120] in, These are the pose control points for the B-spline, containing position components. and attitude components (using quaternions) (This is indicated by adjusting the pose control points). It can flexibly depict the continuous motion trajectory of a robot, making it highly consistent with the actual motion characteristics.

[0121] 4. Construction of SLAM Optimization Objective Function

[0122] Based on the robot's motion state parameterized by B-spline curves, a SLAM optimization objective function is constructed. This objective function includes sensor measurement constraints and prior constraints, and its expression is:

[0123] ;

[0124] in, For environmental map parameters (for LiDAR point cloud maps), For a 3D point cloud coordinate set; for a visual map, (The set of 3D coordinates of feature points and visual matching factors). For sensors At any moment The original measurement data; For sensors The measurement model represents the position of the robot at... Environmental map is Theoretical measurement value at that time; Robust kernel functions (such as Huber kernel and Cauchy kernel) are used to suppress the impact of outlier measurement data on optimization results; These are the prior weighting coefficients. This is the initial estimate of the pose control points. This term provides prior constraints for optimization and avoids the optimization process from getting trapped in local optima.

[0125] 5. Tightly Coupled Fusion Optimization Solution

[0126] Substitute the measurement residuals of each sensor from step 2 into step 4, using B-spline pose control points. and environmental maps To optimize the variables, a nonlinear optimization algorithm (such as the Levenberg-Marquardt method) is employed to minimize the objective function. During the optimization process, all sensor data are correlated through a unified B-spline motion model, achieving deep fusion of measurement information and forming a tightly coupled SLAM framework. Even in scenarios with degraded matching, such as tunnels, the system maintains stable positioning and map building through the combined effects of high-frequency motion constraints provided by the IMU, absolute position constraints from GNSS (if the signal is available), and local feature constraints from LiDAR / Camera.

[0127] 6. Real-time estimation and optimization:

[0128] In the SLAM optimization process, extrinsic parameters and time synchronization errors are compared with B-spline pose control points and environmental parameters. Figure 1These variables are used as optimization variables and substituted into the objective function for joint optimization. By analyzing the sensitivity of the measurement residuals of each sensor to extrinsic parameters and time synchronization errors, the weights of each constraint term during the optimization process are dynamically adjusted. For example, using the optimized posterior error, the total residuals of LiDAR, Camera, IMU, and GNSS are analyzed, and the weights of the residuals of different sensors in the next optimization are dynamically adjusted proportionally. Initial inertial terms are added to constrain the extrinsic parameters and time synchronization errors to be estimated, ensuring that the extrinsic parameters and time synchronization errors can be accurately estimated. For example, when the robot moves smoothly and the environment is rich in features, the matching residuals of LiDAR and Camera are more sensitive to changes in extrinsic parameters. The accuracy of extrinsic parameter estimation can be improved by strengthening the LiDAR-Camera constraint term. When the robot is in a dynamic motion scene, the motion constraint residuals of IMU and LiDAR are more sensitive to time synchronization errors. The weight of the IMU-LiDAR constraint term can be increased to achieve accurate correction of time synchronization errors. In this way, the algorithm can track and update sensor extrinsic parameters and time synchronization errors in real time during SLAM, eliminate parameter drift caused by equipment vibration and environmental interference, and continuously improve system accuracy.

[0129] Based on the same inventive concept, this disclosure provides a multi-sensor fusion evaluation device based on continuous time series. The steps performed by the components of this device are the same as or similar to the methods described above, therefore, similar details will not be repeated. Figure 2 As shown, the multi-sensor fusion evaluation device based on continuous time series in this embodiment includes:

[0130] The preprocessing module 210 is used to preprocess the raw point cloud data to obtain point cloud features; preprocess the raw image to obtain visualization features; preprocess the raw IMU data to obtain short-term pose estimation of the IMU; and preprocess the raw GNSS data to obtain the robot's absolute position and velocity information; the short-term pose estimation includes the IMU's angular velocity and linear acceleration.

[0131] The measurement model construction module 220 is used to construct a radar measurement model based on the robot's pose, radar extrinsic parameters relative to the robot, and point cloud features; to construct a camera measurement model based on camera extrinsic parameters, camera intrinsic parameters, visualization features, and the robot's pose; to construct an IMU measurement model based on IMU angular velocity, linear acceleration, angular velocity bias, linear acceleration bias, IMU measurement noise, gravitational acceleration, and the robot's pose; and to construct a GNSS measurement model based on the robot's absolute position, velocity information, robot pose, GNSS measurement noise, and synchronization error between GNSS and system time.

[0132] The spline parameterization module 230 is used to model the continuous motion process of the robot using k-order B-spline curves: setting within a set time interval... A uniform or non-uniform pose control point Furthermore, a B-spline basis function is constructed at each pose control point. Using the B-spline basis function and the robot's pose at the pose control point, a spline parameterized representation of the robot's pose state at any time within a set time interval is constructed.

[0133] The objective function construction module 240 is used to construct an optimization objective function using radar measurement models, camera measurement models, IMU measurement models, GNSS measurement models, environmental map parameters, raw point cloud data, raw images, raw IMU data, raw GNSS data, initial estimates of the robot at pose control points, the actual pose of the robot at pose control points, and the spline parameterized representation of the pose state. The environmental map parameters are the coordinates of feature points in the keyframes corresponding to the raw point cloud coordinate set or the visual matching factors of the feature points.

[0134] The optimization module 250 is used to minimize the objective function by using the pose of the pose control point and the environmental map parameters as optimization variables, and to obtain the optimal pose of the robot and the optimal environmental map parameters.

[0135] The various embodiments of the techniques described above can be implemented in digital electronic circuit systems, integrated circuit systems, field-programmable gate arrays (FPGAs), application-specific integrated circuits (ASICs), application-specific standard products (ASSPs), systems-on-a-chip (SoCs), payload-programmable logic devices (CPLDs), computer hardware, firmware, software, and / or combinations thereof. These various embodiments may include: implementations in one or more computer programs that can be executed and / or interpreted on a programmable system including at least one programmable processor, which may be a dedicated or general-purpose programmable processor, capable of receiving data and instructions from a storage system, at least one input device, and at least one output device, and transmitting data and instructions to the storage system, the at least one input device, and the at least one output device.

[0136] The program code used to implement the methods of this disclosure may be written in any combination of one or more programming languages. This program code may be provided to a processor or controller of a general-purpose computer, special-purpose computer, or other programmable data processing apparatus, such that when executed by the processor or controller, the program code causes the functions / operations specified in the flowcharts and / or block diagrams to be implemented. The program code may be executed entirely on a machine, partially on a machine, as a standalone software package partially on a machine and partially on a remote machine, or entirely on a remote machine or server.

[0137] In the context of this disclosure, a machine-readable medium can be a tangible medium that may contain or store a program for use by or in conjunction with an instruction execution system, apparatus, or device. A machine-readable medium can be a machine-readable signal medium or a machine-readable storage medium. A machine-readable medium can be, but is not limited to, electronic, magnetic, optical, electromagnetic, infrared, or semiconductor systems, apparatus, or devices, or any suitable combination of the foregoing. More specific examples of machine-readable storage media include electrical connections based on one or more wires, portable computer disks, hard disks, random access memory (RAM), read-only memory (ROM), erasable programmable read-only memory (EPROM or flash memory), optical fiber, portable compact disk read-only memory (CD-ROM), optical storage devices, magnetic storage devices, or any suitable combination of the foregoing.

[0138] To provide interaction with a user, the systems and techniques described herein can be implemented on a computer having: a display device for displaying information to the user (e.g., a CRT (cathode ray tube) or LCD (liquid crystal display) monitor); and a keyboard and pointing device (e.g., a mouse or trackball) through which the user provides input to the computer. Other types of devices can also be used to provide interaction with the user; for example, feedback provided to the user can be any form of sensory feedback (e.g., visual feedback, auditory feedback, or tactile feedback); and input from the user can be received in any form (including sound input, voice input, or tactile input).

[0139] The systems and technologies described herein can be implemented in computing systems that include backend components (e.g., as a data server), or computing systems that include middleware components (e.g., an application server), or computing systems that include frontend components (e.g., a user computer with a graphical user interface or web browser through which a user can interact with implementations of the systems and technologies described herein), or any combination of such backend, middleware, or frontend components. The components of the system can be interconnected via digital data communication of any form or medium (e.g., a communication network). Examples of communication networks include local area networks (LANs), wide area networks (WANs), and the Internet.

[0140] Computer systems can include clients and servers. Clients and servers are generally located far apart and typically interact via communication networks. Client-server relationships are created by computer programs running on the respective computers and having a client-server relationship with each other. Servers can be cloud servers, servers in distributed systems, or servers incorporating blockchain technology.

[0141] It should be understood that the various forms of processes shown above can be used to rearrange, add, or delete steps. For example, the steps described in this disclosure can be executed in parallel, sequentially, or in different orders, as long as the desired result of the technical solution disclosed in this disclosure can be achieved, and this is not limited herein.

[0142] The specific embodiments described above do not constitute a limitation on the scope of protection of this disclosure. Those skilled in the art should understand that various modifications, combinations, sub-combinations, and substitutions can be made according to design requirements and other factors. Any modifications, equivalent substitutions, and improvements made within the spirit and principles of this disclosure should be included within the scope of protection of this disclosure.

Claims

1. A continuous-time sequence based multi-sensor fusion evaluation algorithm, characterized in that, The application relates to a method for constructing a robot's environment map and pose, and a robot. The method comprises the following steps: Preprocessing original point cloud data to obtain point cloud features; Preprocessing original images to obtain visual features; Preprocessing IMU original data to obtain short-term pose estimation of the IMU; Preprocessing GNSS original data to obtain absolute position and speed information of the robot; the short-term pose estimation comprises angular velocity and linear acceleration of the IMU; The continuous motion process of the robot is modeled using k-order B-spline curves: within a set time interval, [the following is set]. A uniform or non-uniform pose control point Furthermore, a B-spline basis function is constructed at each pose control point. Using the B-spline basis function and the robot's pose at the pose control point, a spline parameterized representation of the robot's pose state at any time within a set time interval is constructed. Constructing a radar measurement model based on the pose of the robot, external parameters of the radar relative to the robot and the point cloud features; constructing a camera measurement model based on camera external parameters, camera internal parameters, visual features and the pose of the robot; constructing an IMU measurement model based on angular velocity, linear acceleration, angular velocity bias, linear acceleration bias, IMU measurement noise, gravitational acceleration and the pose of the robot; and constructing a GNSS measurement model based on absolute position, speed information of the robot, the pose of the robot, GNSS measurement noise and synchronization error between GNSS and system time; Constructing an optimization objective function by using the radar measurement model, the camera measurement model, the IMU measurement model, the GNSS measurement model, environment map parameters, original point cloud data, original images, IMU original data, GNSS original data, initial estimated values of the robot at pose control points, actual poses of the robot at the pose control points and pose state spline parameterization representation; the environment map parameters are coordinates of feature points in a key frame corresponding to the original point cloud coordinates or the original images and visual matching factors of the feature points; 2. The continuous-time sequence based multi-sensor fusion evaluation algorithm according to claim 1, characterized in that, Minimizing and solving the optimization objective function by taking the pose of the pose control points and the environment map parameters as optimization variables and by using a nonlinear optimization algorithm to obtain optimal pose of the robot and optimal environment map parameters. The preprocessing of the original point cloud data to obtain the point cloud features comprises the following steps: Performing distortion removal processing on the original point cloud data; Segmenting the point cloud after the distortion removal processing into ground points and non-ground points by using a ground segmentation algorithm; 3. The continuous-time sequence based multi-sensor fusion evaluation algorithm according to claim 1, characterized in that, Extracting features from the ground point cloud and the non-ground point cloud by using a curvature-based feature extraction algorithm to obtain line feature data and surface feature data for matching. The preprocessing of the original images to obtain the visual features comprises the following steps: Performing histogram equalization processing, noise removal and distortion correction processing on the original images; Obtaining FAST feature points in the images after the distortion correction processing by using a feature extraction algorithm; 4. The continuous-time sequence based multi-sensor fusion evaluation algorithm according to claim 1, characterized in that, Tracking features based on the extracted FAST feature points by using an OpenCV Lucas Kanade optical flow tracking algorithm, and determining feature point visual matching factors of key frames by using tracking information of the FAST feature points of adjacent key frames. The preprocessing of the IMU original data to obtain the short-term pose estimation of the IMU comprises the following steps: Performing zero bias compensation and temperature drift correction on the angular velocity and linear acceleration data output by the IMU; 5. The continuous-time sequence based multi-sensor fusion evaluation algorithm according to claim 1, characterized in that, Processing the data after the temperature drift correction by integral calculation to obtain the short-term pose estimation of the IMU. ； wherein the pose of the robot at time is , is a rotation matrix from the world coordinate system to the robot body coordinate system, is a position vector; the external parameters of the robot include a position offset and an attitude rotation , P L is the coordinate of a feature point in the point cloud feature in the radar coordinate system, and P W represents the coordinate of a feature point in the point cloud feature in the world coordinate system; The radar measurement model is as follows: ； where (u, v) represents the coordinates of the pixel point on the camera image plane; K is the camera intrinsic parameter; the camera extrinsic parameter contains a position offset and a pose rotation ; is the coordinates of the feature point in the camera coordinate system.

6. The continuous-time sequence based multi-sensor fusion evaluation algorithm according to claim 1, characterized in that, The camera measurement model is as follows: ； ； wherein is an angular velocity measurement of the IMU; is a linear acceleration measurement of the IMU; , are angular velocity bias and linear acceleration bias of the IMU, respectively, , is the IMU measurement noise, is the gravity acceleration vector; the pose of the robot at time is , is the rotation matrix from the world frame to the robot body frame, is the position vector; The IMU measurement model is as follows: The GNSS measurement model is as follows: ； ； wherein is the absolute position of the robot, is the absolute velocity of the robot, is the synchronization error of GNSS and system time, , is the GNSS measurement noise.

7. The continuous-time sequence based multi-sensor fusion evaluation algorithm according to claim 1, characterized in that, The pose state spline parameterization representation of the robot at any time in a set time interval is as follows: ； where i denotes the pose control point index, P i denotes the position component and the pose component of the robot at pose control point i, and m denotes the maximum index of the pose control points, denotes the B-spline basis function, k denotes the B-spline order, and t denotes time.

8. The continuous-time sequence based multi-sensor fusion evaluation algorithm according to claim 1, characterized in that, The optimization objective function is as follows: ； where S denotes a sensor, LiDar denotes a radar, Camera denotes a camera, M is an environment map parameter, is a sensor At time of the original measurement data, is a measurement model based on the sensor , the theoretical measurement value when the robot pose is , and the environment map parameter is , is a robust kernel function, is a prior weight coefficient, is the initial estimated value of the pose control point i, and Pi is the actual pose of the pose control point i.

9. The continuous-time sequence based multi-sensor fusion evaluation algorithm according to claim 1, characterized in that, Further comprising: Taking the robot's external parameters, the synchronization error of GNSS and system time, the pose of the pose control point, and the environment map parameters as optimization variables, the optimization objective function is minimized to obtain the robot's optimal pose, optimal environment map parameters, robot's optimal external parameters, and optimal synchronization error.

10. A continuous-time series based multi-sensor fusion evaluation apparatus, characterized by, Comprising: A preprocessing module for preprocessing the original point cloud data to obtain point cloud features; The original image is preprocessed to obtain visual features; The IMU original data is preprocessed to obtain the short-term pose estimation of the IMU; the GNSS original data is preprocessed to obtain the absolute position and velocity information of the robot; the short-term pose estimation includes the angular velocity and linear acceleration of the IMU; A measurement model construction module for constructing a radar measurement model based on the pose of the robot, the external parameters of the radar relative to the robot, and the point cloud features; constructing a camera measurement model based on the camera external parameters, camera internal parameters, visual features, and the pose of the robot; constructing an IMU measurement model based on the angular velocity, linear acceleration, angular velocity bias, linear acceleration bias, IMU measurement noise, gravitational acceleration, and the pose of the robot; constructing a GNSS measurement model based on the absolute position, velocity information of the robot, the pose of the robot, GNSS measurement noise, and the synchronization error of GNSS and system time; The spline parameterization module is used to model the continuous motion process of a robot using k-order B-spline curves: setting parameters within a defined time interval. A uniform or non-uniform pose control point Furthermore, a B-spline basis function is constructed at each pose control point. Using the B-spline basis function and the robot's pose at the pose control point, a spline parameterized representation of the robot's pose state at any time within a set time interval is constructed. A target function construction module for constructing an optimization objective function using the radar measurement model, camera measurement model, IMU measurement model, GNSS measurement model, environment map parameters, original point cloud data, original image, IMU original data, GNSS original data, initial estimated value of the robot at the pose control point, actual pose of the robot at the pose control point, and pose state spline parameterization representation; the environment map parameters are the coordinates of the feature points in the key frame corresponding to the original point cloud coordinate set or the original image, and the visual matching factors of the feature points; An optimization module for taking the pose of the pose control point and the environment map parameters as optimization variables, and minimizing the optimization objective function using a nonlinear optimization algorithm to obtain the robot's optimal pose and optimal environment map parameters.

Images