Data fusion method and device, storage medium and computer device

Abstract

The application relates to the technical field of multi-sensor fusion, and in particular to a data fusion method and device, a storage medium and a computer device. After a global clock signal triggers laser radar, a vision sensor and an inertial measurement unit to synchronously collect original point cloud data, target image data and original attitude data, the time stamp corresponding to each visual key frame in the original point cloud data or the target image data is determined. A plurality of subintervals and virtual attitude data of each subinterval are determined based on the time distribution of each time stamp, each virtual attitude data is pre-integrated, each virtual attitude data is corrected by using each pre-integrated quantity, and then target attitude data is obtained. In the process, the total residual error in the multi-sensor fusion is minimized as an optimization target, the relative motion amount is calculated in a local coordinate system through pre-integration, the optimization problem can be decoupled from the initial state solution, the zero offset of the accelerometer and the gyroscope can be compensated, and the dynamic error can be reduced.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of multi-sensor fusion, and particularly to a data fusion method and device, a storage medium and a computer device. BACKGROUND

[0002] At present, in the field of multi-sensor fusion, robots and unmanned aerial vehicles are gradually replacing manual execution of high-risk, high-frequency or high-precision tasks. For example, in the substation inspection scene, a quadruped robot carries an infrared camera and a laser radar, autonomously navigates and detects equipment temperature and abnormal sound; in the warehouse logistics scene, a group of unmanned aerial vehicles cooperatively count high-position shelves, and AGV (Automated Guided Vehicle) robots sort goods; in the disaster rescue scene, unmanned aerial vehicles search for survivors, and ground robots enter collapsed buildings to deliver supplies. These scenes rely on autonomous perception, real-time decision-making and precise control of robots or unmanned aerial vehicles.

[0003] When performing high-precision operations, it is generally necessary to first collect environmental data, and then perform related decision-making according to the environmental data. In this process, since the data sampling frequency of the inertial measurement unit (usually 1 kHz) is much higher than the data sampling frequency of the vision or laser radar (such as a camera 30 Hz), it is necessary to align the data collected by the inertial measurement unit with the time stamp of the low-frequency sensor data through data fusion. However, the existing data fusion algorithm has a large error and cannot meet the high-precision positioning requirement. SUMMARY

[0004] The present application aims to at least solve one of the above technical defects, in particular the technical defect that the existing data fusion algorithm has a large error and cannot meet the high-precision positioning requirement.

[0005] The present application provides a data fusion method, which comprises:

[0006] triggering the laser radar, the vision sensor and the inertial measurement unit to synchronously collect original point cloud data, target image data and original attitude data through a global clock signal;

[0007] determining the time stamp corresponding to each vision key frame in the original point cloud data or the target image data, and interpolating the original attitude data according to each time stamp to obtain virtual attitude data at each time stamp;

[0008] determining a plurality of subintervals and virtual attitude data of each subinterval based on the time distribution of each time stamp, and obtaining a pre-integral quantity of each subinterval after pre-integrating the virtual attitude data of each subinterval;

[0009] The virtual attitude data of each sub-interval is corrected by using the pre-integration quantity of each sub-interval, and target attitude data is obtained.

[0010] Optionally, when the original attitude data meets normal interpolation conditions, the original attitude data is interpolated according to each timestamp to obtain virtual attitude data at each timestamp, including:

[0011] The quaternions of adjacent time points corresponding to each timestamp in the original attitude data are obtained.

[0012] The quaternions of adjacent time points corresponding to each timestamp in the original attitude data are obtained.

[0013] Optionally, when the original attitude data meets emergency interpolation conditions, the original attitude data is interpolated according to each timestamp to obtain virtual attitude data at each timestamp, including:

[0014] The original attitude data is interpolated at each timestamp by using a preset emergency interpolation mode to obtain virtual attitude data at each timestamp.

[0015] Optionally, after the virtual attitude data of each sub-interval is pre-integrated, a pre-integration quantity of each sub-interval is obtained, including:

[0016] The virtual attitude data of each sub-interval is converted to a key frame coordinate system to obtain converted virtual attitude data.

[0017] The converted virtual attitude data in each sub-interval is pre-integrated to obtain a pre-integration quantity of each sub-interval.

[0018] Optionally, before the virtual attitude data of each sub-interval is corrected by using the pre-integration quantity of each sub-interval, it further includes:

[0019] When the virtual attitude data meets Lie group-Lie algebra optimization conditions, Lie algebra optimization is performed on the virtual attitude data of each sub-interval to make the interpolation path meet maximum steering angle and acceleration constraints, and interpolation parameters are dynamically adjusted according to semantic information of each visual key frame to obtain optimized virtual attitude data.

[0020] Optionally, the Lie algebra optimization is performed on the virtual attitude data of each sub-interval to make the interpolation path meet maximum steering angle and acceleration constraints, and interpolation parameters are dynamically adjusted according to semantic information of each visual key frame to obtain optimized virtual attitude data, including:

[0021] The virtual attitude data of each sub-interval is interpolated by using a Lie algebra space linear interpolation method, and in the interpolation process, the interpolation weight is dynamically adjusted according to the semantic information of each visual key frame, each virtual attitude data is optimized by using a pre-integration-Kalman filter architecture, the interpolation path is adjusted by using an acceleration perception Bézier spline path, and the noise in each virtual attitude data is processed in different frequency bands by using an asymmetric noise suppression strategy, so as to obtain the optimized virtual attitude data.

[0022] Optionally, the virtual attitude data of each sub-interval is corrected by using the pre-integration quantity of each sub-interval to obtain target attitude data, including:

[0023] A target function with the minimum overall residual error in multi-sensor fusion as an optimization objective is constructed based on the pre-integration quantity of each sub-interval, and an optimization result is obtained after the target function is optimized and solved.

[0024] The virtual attitude data of each sub-interval is corrected according to the optimization result to obtain target attitude data.

[0025] The application also provides a data fusion device, including:

[0026] A data acquisition module is configured to trigger the laser radar, the visual sensor and the inertial measurement unit to synchronously collect original point cloud data, target image data and original attitude data through a global clock signal;

[0027] A data interpolation module is configured to determine time stamps corresponding to each visual key frame in the original point cloud data or the target image data, and interpolate the original attitude data according to each time stamp to obtain virtual attitude data at each time stamp;

[0028] A pre-integration module is configured to determine a plurality of sub-intervals and virtual attitude data of each sub-interval based on the time distribution of each time stamp, and obtain a pre-integration quantity of each sub-interval after pre-integrating the virtual attitude data of each sub-interval;

[0029] A data correction module is configured to correct the virtual attitude data of each sub-interval by using the pre-integration quantity of each sub-interval to obtain target attitude data.

[0030] The application also provides a computer readable storage medium, the computer readable storage medium stores computer readable instructions, and the computer readable instructions are executed by one or more processors to make the one or more processors execute the steps of the data fusion method in any one of the above embodiments.

[0031] The application also provides a computer device, including one or more processors and a memory.

[0032] The memory stores computer-readable instructions that, when executed by the one or more processors, perform the steps of the data fusion method of any of the above embodiments.

[0033] From the above technical solutions, the embodiments of the present application have the following advantages:

[0034] The data fusion method, device, storage medium and computer device provided by the present application can first determine the timestamps corresponding to each visual key frame in the original point cloud data or target image data after triggering the laser radar, visual sensor and inertial measurement unit to synchronously collect original point cloud data, target image data and original attitude data by the global clock signal. Then, in order to reduce the calculation amount of state estimation, the present application can determine a plurality of sub-intervals and virtual attitude data of each sub-interval based on the time distribution of each timestamp, and pre-integrate the virtual attitude data of each sub-interval to obtain a pre-integration amount of each sub-interval. In this way, the pre-integration amount of each sub-interval can be used to correct the virtual attitude data of each sub-interval, and then the target attitude data is obtained. In this process, the present application takes the minimum overall residual error in multi-sensor fusion as the optimization target, and calculates the relative motion amount in the local coordinate system through pre-integration, which can decouple the optimization problem from the initial state, and can compensate for the zero offset of the accelerometer and gyroscope, thereby reducing the dynamic error and meeting the high-precision positioning requirement. BRIEF DESCRIPTION OF DRAWINGS

[0035] In order to more clearly illustrate the technical solutions in the embodiments of the present application or the prior art, the drawings needed to be used in the embodiments or prior art description will be briefly introduced as follows. Obviously, the drawings in the following description are only some embodiments of the present application, and other drawings can also be obtained by those skilled in the art without creative labor.

[0036] Figure 1 A flowchart of a data fusion method provided by an embodiment of the present application;

[0037] Figure 2 A process diagram of aligning the timestamps of the original attitude data provided by an embodiment of the present application;

[0038] Figure 3 A structural diagram of a data fusion device provided by an embodiment of the present application;

[0039] Figure 4 An internal structure diagram of a computer device provided by an embodiment of the present application. DETAILED DESCRIPTION

[0040] With reference to the drawings and the embodiments of the present application, the technical solutions in the embodiments of the present application will be described clearly and completely. Obviously, the described embodiments are only part of the embodiments of the present application, rather than all the embodiments of the present application. Based on the embodiments in the present application, all other embodiments obtained by a person of ordinary skill in the art without creative work fall within the protection scope of the present application.

[0041] In one embodiment, as shown in Figure 1 Figure 1 a flowchart of a data fusion method provided by an embodiment of the present application; the present application provides a data fusion method, which can include:

[0042] S110: Triggering laser radar, visual sensor and inertial measurement unit to synchronously collect original point cloud data, target image data and original attitude data through a global clock signal.

[0043] In this step, in order to ensure the timestamp alignment of multi-source data, the present application can trigger laser radar, visual sensor and inertial measurement unit to synchronously collect original point cloud data, target image data and original attitude data through a global clock signal, so as to eliminate clock drift and clock offset between different devices, make the timestamps of all sensors based on the same time reference, and correct the error caused by sensor scanning delay. In addition, since the data collection frequencies of different sensors are also different, for example, the sampling rate of inertial measurement unit (IMU) is extremely high (usually 100Hz-1kHz), which is much higher than that of camera (such as 30Hz) or radar (such as 10Hz). Therefore, after synchronously collecting original data, the present application can also align the timestamps of original attitude data with original point cloud data or target image data, so as to obtain target attitude data.

[0044] Among them, the global clock signal of the present application refers to a unified time reference signal generated by a main clock source in a multi-sensor or multi-device system, which is used to synchronize the clocks of all sub-devices (such as cameras, radars, IMUs, etc.), so as to ensure that their timestamps are strictly aligned. The global clock signal of the present application can be synchronized by hardware (high precision) or software, which can be set according to actual conditions, and is not limited here.

[0045] ​Further, the application can generate a unified timestamp of the multiple sensors after synchronization through a time synchronization model, and generate a global clock signal according to the unified timestamp; of course, the application can also generate a unified clock signal through a specific chip, such as an FPGA or other special-purpose chip, and then input the unified clock signal into a pre-configured time synchronization model to generate a unified timestamp of the multiple sensors after synchronization through the time synchronization model, and then generate a global clock signal according to the unified timestamp. In this way, the time synchronization error of the multiple sensors can be controlled within 100 μs through hardware delay compensation and dynamic angular velocity compensation, and the stability of the fusion algorithm can be ensured.

[0046] S120: Determine the timestamps corresponding to each visual key frame in the original point cloud data or target image data, and interpolate the original attitude data according to each timestamp to obtain virtual attitude data at each timestamp.

[0047] S130: Determine a plurality of sub-intervals and virtual attitude data of each sub-interval based on the time distribution of each timestamp, and obtain a pre-integral quantity of each sub-interval after pre-integrating the virtual attitude data of each sub-interval.

[0048] S140: Correct the virtual attitude data of each sub-interval by using the pre-integral quantity of each sub-interval to obtain target attitude data.

[0049] In the above steps S120-S140, since the data sampling frequency (usually 1 kHz) of the IMU in the application is much higher than that of the vision or lidar (such as a camera 30 Hz), the application can align the timestamps of the original attitude data and the low-frequency sensor data through interpolation.

[0050] For example, at the moment of camera exposure, the application can realize the time consistency of the multiple sensor data by interpolating the original attitude data measured by the IMU. The interpolation method can be selected according to the actual situation, for example, the application can select SLERP (Spherical Linear Interpolation, Spherical Linear Interpolation of Quaternions), or select Lie algebra space linear interpolation method, or select other interpolation methods, which can be selected according to the actual situation, and is not limited herein.

[0051] When the application selects the corresponding interpolation mode, the timestamps corresponding to each visual key frame in the original point cloud data or the target image data can be determined first, and the original attitude data is interpolated according to each timestamp and the selected interpolation mode to obtain virtual attitude data at each timestamp. Then, in order to reduce the calculation amount of state estimation, the application can determine a plurality of sub-intervals and virtual attitude data of each sub-interval based on the time distribution of each timestamp, and pre-integrate the virtual attitude data of each sub-interval to obtain a pre-integration quantity of each sub-interval. In this way, the pre-integration quantity of each sub-interval can be used to correct the virtual attitude data of each sub-interval, and then the target attitude data is obtained. In this process, the relative motion quantity is calculated in the local coordinate system through pre-integration, which can decouple the optimization problem from the initial state solution, and can compensate for the zero offset of the accelerometer and the gyroscope, and reduce the dynamic error.

[0052] In the application, the timestamps corresponding to each visual key frame are determined by the image content dynamics and system requirements. For example, when the feature matching rate between consecutive images is lower than a preset matching rate threshold (such as 60%), a new key frame can be triggered; motion saliency can also be detected: the motion energy of the image region is calculated by the optical flow method, and if it exceeds the threshold, it is marked as a key frame; time interval can also be forced to insert: if it exceeds the preset maximum interval (such as 0.5 seconds), a key frame is forced to be generated to prevent positioning loss; time stamp alignment can also be used: the key frame timestamp strictly corresponds to the camera exposure time, and hardware synchronization (such as FPGA trigger) is used to ensure that it is consistent with the time reference of the IMU data.

[0053] Further, the pre-integration quantity calculated by the application includes phase rotation, relative velocity and relative displacement, so when the application can act the relative rotation on the virtual attitude data, then correct the position according to the relative displacement, and correct the speed according to the relative velocity, and take the corrected position and attitude as the motion compensation result.

[0054] In the above embodiments, after the laser radar, the visual sensor and the inertial measurement unit synchronously collect the raw point cloud data, the target image data and the raw attitude data triggered by the global clock signal, the time stamp corresponding to each visual key frame in the raw point cloud data or the target image data can be determined first. Then, in order to reduce the calculation amount of state estimation, the application can determine a plurality of subintervals and virtual attitude data of each subinterval based on the time distribution of each time stamp, and pre-integrate the virtual attitude data of each subinterval to obtain a pre-integration quantity of each subinterval. In this way, the pre-integration quantity of each subinterval can be used to correct the virtual attitude data of each subinterval, and then the target attitude data is obtained. In this process, the application takes the minimum overall residual error in multi-sensor fusion as the optimization objective, and calculates the relative motion quantity in the local coordinate system through pre-integration, which can decouple the optimization problem from the initial state, compensate for the accelerometer and gyroscope zero offset, and reduce the dynamic error, thereby meeting the high-precision positioning requirement.

[0055] In one embodiment, when the raw attitude data meets the normal interpolation condition, the interpolation of the raw attitude data according to each time stamp in S120 to obtain the virtual attitude data at each time stamp can include:

[0056] S121: Obtain the quaternion of the adjacent time corresponding to each time stamp in the raw attitude data.

[0057] S122: Interpolate between the quaternions of the adjacent time corresponding to each time stamp using the spherical linear interpolation method of quaternion to obtain the virtual attitude data at each time stamp.

[0058] In this embodiment, in order to ensure the shortest path interpolation of rotational motion and avoid the gimbal lock problem that may occur in Euler angle interpolation, the attitude change of the robot in continuous motion can be more truly reflected. The application can select the spherical linear interpolation method of quaternion to interpolate the raw attitude data when the raw attitude data meets the normal interpolation condition. The normal interpolation condition here can be that the angular velocity value in the raw attitude data is less than the maximum angular velocity value, or other interpolation conditions, which can be set according to actual conditions and is not limited herein.

[0059] Specifically, when the original pose data of the present application satisfies the normal interpolation condition, the quaternion of the adjacent time corresponding to each timestamp in the original pose data can be obtained, and then the spherical linear interpolation method of quaternion is used to interpolate between the quaternions of the adjacent time corresponding to each timestamp, so that the virtual pose data under each timestamp can be obtained. In this process, the present application can also perform normalization on the interpolated quaternion and correct the integral initial condition. The quaternion here refers to a simple hypercomplex number, and the complex number is composed of a real number plus an imaginary unit i, where i2= -1, and similarly, the quaternion is composed of a real number plus three imaginary units i, j and k, that is, a number like , where a, b, c and d are all real numbers.

[0060] In a specific implementation, the present application can use SLERP to generate virtual pose data between the adjacent time and , where , and is the timestamp corresponding to one of the visual key frames. Assuming that the visual key frame in the present application is triggered at , then:

[0061]

[0062] , where is the virtual pose data under the timestamp , q is the quaternion at the time , and q is the quaternion at the time . The difference between this method and the original method is:

[0063] 1. Traditional linear interpolation: directly linearly weights the four components of the quaternion, resulting in distortion of the rotation axis.

[0064] 2. SLERP optimized interpolation: interpolation along the geodesic line of the quaternion sphere, maintaining the continuity of the angular velocity, and can reduce the drift error caused by direct integration, improving the short-term accuracy of pose estimation.

[0065] Further, the above interpolation method of the present application can be used to dynamically adjust the interpolation step according to the sensor data frequency, for example, when the robot is moving at high speed, a denser interpolation step (such as interpolation once every 0.5ms) is adopted to avoid motion blur; it can also be used for hardware acceleration, for example, implementing parallel SLERP calculation on FPGA or GPU, taking advantage of the vectorization characteristics of quaternion operations (such as simultaneously calculating multiple sets of quaternion interpolation), reducing the interpolation delay to the microsecond level; it can also be used for visual-IMU tight coupling, in visual-inertial odometry (VIO), the SLERP interpolated IMU data can be used for matching with image feature points, and the pose can be optimized jointly; or for motion compensation in dynamic environment, when there is motion distortion in laser radar scanning, the high-precision interpolated IMU data can be used to correct the point cloud coordinates in real time.

[0066] Through the above optimization, the SLERP interpolation can control the IMU pose interpolation error within 0.01 rad (corresponding to a position error of about ±1cm) while ensuring computational efficiency, meeting the high-precision positioning requirements. Moreover, the present application can reduce the trajectory error by 20% through SLERP interpolation + pre-integration mode for timestamp alignment, especially in high-speed motion scenarios, and by decoupling the state quantity through pre-integration, the optimization iteration number can be reduced by 40%, thereby meeting the real-time SLAM (Simultaneous Localization and Mapping) requirements.

[0067] In one embodiment, when the original pose data meets the emergency interpolation condition, the interpolation of the original pose data according to each timestamp in S120 to obtain the virtual pose data at each timestamp can include:

[0068] The original pose data is interpolated at each timestamp using a preset emergency interpolation mode to obtain the virtual pose data at each timestamp.

[0069] In this embodiment, in order to ensure the robustness of the algorithm under extreme working conditions, the present application can set a fault recovery mechanism, which can use the emergency interpolation mode for interpolation when the original pose data meets the emergency interpolation condition. The emergency interpolation condition here can be that the angular velocity value of the original pose data is greater than or equal to the maximum angular velocity value, or other interpolation conditions, which can be set according to actual conditions and are not limited herein.

[0070] When the original pose data of the present application meets the emergency interpolation condition, the present application can interpolate the original pose data at each timestamp using a preset emergency interpolation mode, and then obtain the virtual pose data at each timestamp. For example, the calculation formula of the emergency interpolation mode of the present application can be:

[0071]

[0072] in, The virtual pose data at time t, for Quaternion at time, for Quaternion at time, For time delay, and These are two adjacent moments.

[0073] In one embodiment, the pre-integration of the virtual pose data of each sub-interval in S130 to obtain the pre-integrated value of each sub-interval may include:

[0074] S131: Transform the virtual pose data of each sub-interval to the keyframe coordinate system to obtain the transformed virtual pose data.

[0075] S132: Pre-integrate the transformed virtual attitude data in each sub-interval to obtain the pre-integrated value for each sub-interval.

[0076] In this embodiment, when pre-integrating the virtual pose data of each sub-interval, the virtual pose data of each sub-interval can be first converted to the keyframe coordinate system, and then the converted virtual pose data in each sub-interval can be pre-integrated to obtain the pre-integrated amount of each sub-interval.

[0077] For example, the process of pre-integrating the virtual pose data for each sub-interval in this application is as follows:

[0078] 1. Subinterval partitioning:

[0079] The segmentation rules can be determined based on the temporal distribution of visual keyframes, and the specific strategy is as follows:

[0080] Visual keyframe-driven partitioning:

[0081] Timestamp of each visual keyframe The corresponding endpoint of a sub-interval, i.e., the sub-interval is:

[0082]

[0083] Here, n represents the total number of visual keyframes, and the length of each sub-interval may vary due to uneven distribution of keyframes. This partitioning method ensures that each sub-interval contains at least one complete visual keyframe, providing an accurate time reference for subsequent pose data interpolation and pre-integration.

[0084] Dynamic adjustment mechanism:

[0085] If the amount of virtual attitude data in a certain sub-interval is too small (e.g., <10 sampling points), it will be merged with the adjacent sub-interval to avoid the accumulation of integration error.

[0086] 2. Data alignment:

[0087] Transform the virtual attitude data (angular velocity, acceleration) within the sub-interval to the keyframe coordinate system.

[0088] 3. Pre-integration calculation: Pre-integrate the virtual attitude data within each sub-interval:

[0089]

[0090] Where i and j are the time indices of adjacent virtual pose data; This indicates gyroscope bias, which affects angular velocity measurement. This indicates accelerometer bias, which affects acceleration measurement. Let be the angular velocity at time k. Let k be the acceleration at time k. Let i be the relative rotation between time i and time j. Let be the relative velocity between time i and time j. Let be the relative displacement between time i and time j; all three are pre-integral quantities. Let i be the relative rotation between time i and time k. Let be the relative velocity between time i and time k. Let be the relative displacement between time i and time k. The sampling time interval of the IMU is the time difference between time k and time k+1, where k is the IMU measurement index, ranging from i to j-1.

[0091] By following the steps above, the pre-integral value of each sub-interval can be obtained. By using the pre-integral value of each sub-interval to correct the virtual attitude data of each sub-interval, the target attitude data can be obtained.

[0092] In one embodiment, before correcting the virtual pose data of each sub-interval using the pre-integral values ​​of each sub-interval in step S140, the process may further include:

[0093] S230: When the virtual pose data satisfies the Lie group-Lie algebra optimization condition, Lie algebra optimization is performed on the virtual pose data of each sub-interval to make the interpolation path conform to the maximum steering angle and acceleration limit, and the interpolation parameters are dynamically adjusted according to the semantic information of each visual keyframe to obtain the optimized virtual pose data.

[0094] In this embodiment, interpolating the original posture data using SLERP may result in high-frequency noise, asynchrony, or data loss, which could lead to inaccurate interpolation results, especially during dynamic motion or rapid rotation. Therefore, this application addresses the shortcomings of traditional interpolation methods by proposing an interpolation optimization method. This method combines an adaptive kinematic constraint interpolation algorithm based on Lie group-Lie algebra optimization. This algorithm achieves breakthrough improvements in three aspects: interpolation path optimization, dynamic parameter adjustment, and noise suppression. Specifically, when optimizing the interpolation path, the shortest feasible path satisfying robot dynamics can be generated based on Lie algebra parameterization; when adjusting dynamic parameters, the interpolation weights can be adjusted in real time according to environmental dynamics (such as obstacle velocities); and when suppressing noise, IMU noise can be modeled using the covariance matrix to optimize the anti-interference capability of the interpolation path.

[0095] It is understandable that the Lie group-Lie algebra optimization condition here refers to the optimization condition when the virtual posture data contains high-frequency noise, is out of sync, or has lost data, and the robot's motion state (such as rapid rotation, dynamic acceleration) exceeds the applicable range of conventional interpolation models, causing the direct interpolation result to fail to meet kinematic constraints (such as maximum steering angle, acceleration limits) or semantic rationality (such as obstacle avoidance trajectory needing to conform to the logic of the actual scene). When the virtual posture data meets this optimization condition, rotation and translation can be decoupled through Lie algebra parameterization to construct an optimization objective function that satisfies the robot's dynamic constraints. By combining the semantic information of visual keyframes (such as obstacle category, motion direction) to dynamically adjust interpolation parameters (such as weight allocation, step size control), the optimized virtual posture data that is both kinematically feasible and environmentally adaptable can be generated.

[0096] In the above process, this application can use traditional SLERP for basic interpolation to quickly generate an initial pose sequence; then, it uses the algorithm to optimize the kinematic constraints of the initial path, correcting interpolation points that do not conform to physical laws or pose a collision risk; next, it integrates semantic information from visual keyframes (such as the position of dynamic obstacles) to dynamically adjust the interpolation parameters. Furthermore, in static / low-dynamic environments, this application can prioritize the use of SLERP to reduce computational overhead, while in high-dynamic / complex scenarios, it can improve path safety and accuracy through Lie group-Lie algebra optimization.

[0097] In one embodiment, S230 performs Lie algebra optimization on the virtual pose data of each sub-interval to ensure that the interpolation path conforms to the maximum steering angle and acceleration limits, and dynamically adjusts the interpolation parameters based on the semantic information of each visual keyframe to obtain optimized virtual pose data, which may include:

[0098] The virtual pose data of each sub-interval is interpolated using the Lie algebra space linear interpolation method. During the interpolation process, the interpolation weights are dynamically adjusted according to the semantic information of each visual keyframe. The virtual pose data is optimized using a pre-integration-Kalman joint filtering architecture. The interpolation path is adjusted using an acceleration-aware Bézier spline path. Finally, an asymmetric noise suppression strategy is used to process the noise in each virtual pose data in frequency bands, resulting in optimized virtual pose data.

[0099] In this embodiment, when performing Lie algebra optimization on the virtual pose data of each sub-interval, this application can use the Lie algebra space linear interpolation method for interpolation. Compared with traditional methods, the Lie algebra space linear interpolation method can better handle the interpolation problem of rotation data and avoid issues such as rotation axis distortion. During the interpolation process, considering that different visual keyframes represent different environmental information in the actual scene—for example, some visual keyframes correspond to static obstacle regions, while others correspond to dynamic obstacle regions—it is necessary to dynamically adjust the interpolation weights based on the semantic information of each visual keyframe. For example, for visual keyframes corresponding to dynamic obstacle regions, the interpolation weights can be appropriately increased to more accurately reflect the robot's pose changes in that region.

[0100] Meanwhile, this application can employ a pre-integration-Kalman joint filtering architecture to optimize various virtual attitude data. Pre-integration can calculate relative motion in the local coordinate system, decoupling the optimization problem from the initial state, compensating for the zero bias of the accelerometer and gyroscope, and reducing dynamic errors; Kalman filtering can further filter the data, reduce noise interference, and improve the accuracy and stability of the data. By combining the two, virtual attitude data can be optimized more effectively.

[0101] Furthermore, this application can also adjust the interpolation path using an acceleration-sensing Bézier spline path. The acceleration-sensing Bézier spline path can smooth the interpolation path based on the robot's acceleration information during motion, making the interpolation path more consistent with the robot's actual motion trajectory. This avoids sudden path changes that do not conform to physical laws and ensures that the interpolation path meets the maximum steering angle and acceleration limits.

[0102] Furthermore, this application can also employ an asymmetric noise suppression strategy to process noise in each virtual pose data segment by frequency band. Since different frequency bands have different noise characteristics, the asymmetric noise suppression strategy can use different suppression methods for different frequency bands, more effectively removing noise and improving the quality of the virtual pose data. After the above series of operations, the optimized virtual pose data is finally obtained.

[0103] Understandably, during the interpolation optimization process described above, the semantic information of each visual keyframe in this application can reflect dynamic changes in the environment in real time, such as obstacle positions, obstacle speeds, road surface smoothness, and the robot's own motion state. Therefore, dynamically adjusting the interpolation weights ensures that the robot's motion posture in complex environments is more accurate and safer. For example, when the robot rapidly traverses a dynamic obstacle region, by increasing the interpolation weights of the corresponding visual keyframes in that region, combined with the optimization of the pre-integration-Kalman joint filtering architecture and the smoothing of the acceleration-aware Bézier spline path, the robot can more accurately predict and respond to the movement of obstacles, thereby avoiding collisions. Simultaneously, the frequency-segmented processing of the asymmetric noise suppression strategy further reduces the impact of noise on posture estimation, especially in high-frequency motion scenarios, effectively improving the anti-interference capability of posture data. Finally, the virtual posture data optimized by Lie algebra not only satisfies the constraints of robot dynamics but also significantly improves the safety and accuracy of path planning, providing reliable data support for real-time SLAM and autonomous navigation.

[0104] The following example illustrates the process of a substation inspection robot traversing an equipment area, demonstrating step-by-step how this application uses Lie algebra space linear interpolation to interpolate the virtual attitude data for each sub-interval:

[0105] Scene 1:

[0106] This scenario describes a robot inspecting along a preset path. When navigating around a transformer, it needs to make a sharp 90° turn (angular velocity suddenly increasing to 6 rad / s). Simultaneously, the IMU experiences high-frequency noise due to electromagnetic interference, and the vision system temporarily malfunctions due to metallic reflections. The interpolation process at this point is broken down as follows:

[0107] 1. Pre-integral-Kalman joint filter architecture:

[0108] When the raw attitude data arrives (angular velocity ω = 6.2 rad / s, including high-frequency noise σ = 0.3 rad / s²):

[0109] The pre-integration stage uses the following formula:

[0110]

[0111] The Kalman update phase uses the following formula:

[0112]

[0113]

[0114] in, This is the angular velocity increment after pre-integration correction. The original angular velocity measurement at time t. For the k-th sampling time, For the (k+1)th sampling time, for Time and The time interval of time, The state update value at time t (the optimal estimate after fusion of observations). The predicted state values ​​from time t-1 to time t. For acceleration compensation coefficient, Here is the Kalman gain matrix. It is a visual aid observation (automatically switches to historical trajectory prediction when it fails). This is the kinematic model prediction function for visual failure. For noise covariance, For observation models, For the transpose of the observation model, Let be the prediction error covariance from time t-1 to time t.

[0115] The effects of using the pre-integral-Kalman joint filter architecture include:

[0116] (1) High-frequency noise is filtered by a sliding window: sliding window size N =5, filter out outliers exceeding 3σ.

[0117] (2) The low-frequency drift is compensated by the prediction model, and the effective angular velocity is corrected to 5.8 rad / s.

[0118] 2. Lie group-Li algebra mapping optimization:

[0119] The filtered pose is then transformed into Lie algebra space. The rigid body transformation matrix is ​​given by the following formula:

[0120]

[0121] in, For Lie algebra elements (6-dimensional vectors, including translation and rotation components). This is the inverse operation of the exponential mapping from Lie groups to Lie algebras. It is a special three-dimensional Euclidean group (including rotations and translations in three-dimensional space). For three-dimensional special Euclidean Lie algebras (and) (Corresponding). Through this mapping, the rotation and translation coupling problem, which is difficult to handle directly in Lie group space, is transformed into a relatively independent vector operation problem in Lie algebra space. In Lie algebra space, the filtered pose is further optimized. By utilizing the linear property of Lie algebra, the robot's posture changes can be described more accurately, avoiding undesirable phenomena such as axis distortion during rotation processing.

[0122] Through the above optimizations, it can be achieved that The rotation and translation hybrid operations in the group are decoupled into linear space operations, and the path distortion of quaternion spherical interpolation during abrupt changes in angular velocity is avoided (traditional methods produce an error of 0.78°).

[0123] 3. Dynamic weighted interpolation function:

[0124] The interpolation weights are adjusted based on the corrected angular velocity ω = 5.8 rad / s, using the following formula:

[0125]

[0126]

[0127] Where k is the sensitivity coefficient. For real-time angular velocity, For the maximum angular velocity, To minimize the angular velocity, the interpolation weights automatically and smoothly transition to the nonlinear region as the angular velocity increases. When the sensitivity coefficient k = 1.9, e -k When the value is 0.15, the dynamic weight w(t) = 1 / (1 + 0.15 × 5.8) = 0.534. This interpolation weight ensures that the interpolation process follows the linear region for 53.4% ​​of the time and enters the nonlinear correction region for 46.6%. Therefore, when When the speed is >5 rad / s, this application can automatically enhance the weight of the nonlinear term and suppress high-speed rotational overshoot. Compared with the fixed weight, the peak angular velocity error of the dynamic weight can be reduced by 62%.

[0128] 4. Acceleration-aware Bézier path generation:

[0129] The control points for a cubic Bézier curve are constructed using the following formula:

[0130]

[0131]

[0132] in, Let be the coordinates of the curve at parameter t. Let be the weight coefficient for the i-th control point. These are cubic Bernstein basis functions. These are geometric angle parameters (such as the angle between lines connecting adjacent control points) used to adjust the local curvature of the curve. Angular acceleration (second derivative). The dynamic rate of change of the curve is controlled by time or parameter intervals (such as the time difference between keyframes). This is the weighting function. To generate a path with continuous and smooth acceleration, this weighting function reduces the weights in regions of high angular acceleration to make the curve smoother, and increases the weights in regions of low angular acceleration to allow for more flexible shape changes. The specific weighting function can be shown below:

[0133]

[0134] in, For curvature smoothing function, For acceleration sensitivity coefficient, For time scaling function, The normalization constant is in the denominator. The term indicates that the weight decreases as angular acceleration increases, thereby suppressing rapid changes in direction. This formula is only one form of the weight calculation function in this application. This application can also be designed based on acceleration constraints, or designed as an exponential decay form, as long as it can meet the requirement of continuous acceleration and smooth path generation.

[0135] The control point weight coefficients calculated using the above formula can further determine the specific shape of the cubic Bézier curve. During curve generation, by considering angular acceleration information, the curve can better adapt to dynamic changes during robot motion. For example, when the robot makes a sharp turn, the angular acceleration is large. In this case, by reducing the weight of control points in the corresponding region, the curve becomes smoother in that region, avoiding abrupt path changes and ensuring the stability of the robot's motion posture. Simultaneously, increasing the weight in regions with smaller angular acceleration allows for more flexible shape changes in the curve, better conforming to the robot's actual trajectory. The resulting acceleration-aware Bézier spline path can effectively guide the robot to move along a path that conforms to kinematic constraints and the requirements of the actual scenario, providing a reliable foundation for subsequent path optimization and autonomous navigation.

[0136] 5. Asymmetric noise suppression strategy:

[0137] High-frequency processing:

[0138]

[0139] Low-frequency processing:

[0140]

[0141] in, The output signal after high-frequency noise suppression (such as filtered angular velocity or acceleration) is represented by the subscript h, indicating the result after high-frequency processing. For the original signal in Time step measurements (such as angular velocity data from an IMU). For each data point within the sliding window The weights are given by where i is the index number of the data point within the sliding window. Let j represent the low-frequency noise parameters to be optimized (such as zero bias and drift), j be the index number of the original data point, and N be the total number of the original data points. This represents the actual measurement value of the j-th raw data point (such as the raw measurement value of an IMU). For the j-th reference value or ideal value, This is a projection function, whose purpose is to map the data processed by the transformation model from the original measurement space or state space to the observation space or error assessment space, so as to correlate it with the reference value. Comparison, specifically , This refers to the original measurement space or state space. For the observation space or error evaluation space, the specific form of the projection function can be flexibly designed according to different actual application scenarios and measurement models. For example, in robot posture estimation, it can be designed as a function that maps rotation matrices or quaternions to Euler angles or angular velocity spaces. Let be the transformation function, representing the low-frequency noise model. This is a robust kernel function used to suppress the effects of outliers.

[0142] As can be seen from the above strategy, this application can suppress the 0.3 rad / s² noise caused by electromagnetic pulses through 5-frame sliding window filtering; after visual recovery, the cumulative drift can also be corrected through reprojection error optimization.

[0143] Full-process effect verification:

[0144] 1. Sharp turn phase (t=1.2-1.5s):

[0145] Bézier path curvature is reduced by 40%, effectively avoiding positioning failure caused by mechanical vibration;

[0146] Dynamic weights adaptively shorten the interpolation period to 0.8ms (from 1.2ms).

[0147] 2. Noise interference stage (t=1.6-2.0s):

[0148] High-frequency filtering eliminates 83% of electromagnetic noise;

[0149] Once the vision system recovered, the reprojection optimization corrected the positional drift from 12cm to 3cm.

[0150] 3. Steady-state phase (t>2.0s):

[0151] The sensitivity coefficient k automatically recovers to 2.52 (to improve interpolation efficiency at low angular velocities).

[0152] The computation latency was reduced to 0.09ms through FPGA acceleration.

[0153] Through multi-stage optimization, the above algorithm achieves sub-centimeter-level attitude interpolation accuracy in the complex electromagnetic environment and dynamic motion scenarios of substations, thereby meeting the stringent requirements of power equipment inspection.

[0154] Scene 2:

[0155] This scenario describes a robot inspecting along a preset path when a sudden equipment displacement triggers an emergency obstacle avoidance command. The robot needs to complete a smooth interpolation from its initial pose to the target pose within 0.5 seconds, while simultaneously suppressing IMU vibration noise and visual tracking delay. The specific interpolation process is broken down as follows:

[0156] 1. Optimizing interpolation paths using Lie group-Lie algebra mappings:

[0157] Step 1.1 Transform the pose to Lie algebra space:

[0158]

[0159] Step 1.2 Construct the acceleration compensation interpolator:

[0160]

[0161] in, For dynamic interpolation weights, This is the acceleration compensation coefficient (calibrated based on the substation ground friction coefficient). For IMU in Angular acceleration measured at time t, These are the Lie algebra coordinates corresponding to the initial pose. Let be the initial pose transformation matrix. Let Lie algebra coordinates be the target pose. Let be the target pose transformation matrix. Let t be the Lie algebra coordinates of the interpolation path, and t be the current time.

[0162] Through the above optimization process, rigid motion can be converted to linear space, avoiding path distortion in quaternion spherical interpolation.

[0163] 2. Dynamic weighted interpolation function:

[0164] Real-time calculation of angular velocity sensitivity, the formula is as follows:

[0165]

[0166] in, for Angular velocity sensitivity at any moment For IMU in Angular velocity measured at any time This is the sensitivity coefficient (calculated based on the robot's inertia matrix). This is the critical angular velocity (above which a nonlinear transition is initiated).

[0167] In this application, when obstacle avoidance is triggered, it is detected that... ,but It automatically adjusts to an S-shaped curve to avoid overshooting in traditional linear interpolation.

[0168] 3. Pre-integral-Kalman joint filter architecture:

[0169] Pre-integration phase:

[0170]

[0171] in, To remove the bias angular velocity, The original angular velocity measurement at time t. To achieve zero bias in the gyroscope, Measuring white noise for a gyroscope, second-order term It can compensate for the effects of angular acceleration. For rotation increment matrix, For matrix exponential mapping, For IMU in Angular acceleration measured at time t, For the k-th IMU measurement time, This is the (k+1)th IMU measurement time.

[0172] Kalman Update:

[0173]

[0174]

[0175] in, Let Kalman gain be the value at the k-th observation. Let be the prior estimation error covariance matrix for the k-th observation. Let H be the transpose of the observation matrix, and R be the observation noise covariance matrix. This is the prior state estimate for the k-th observation. This is the posterior state estimate for the k-th observation. This represents the actual observation value at the k-th time, such as the pixel coordinates of feature points from vision, or measurements from other sensors. is the observation model function, which predicts the observation value based on the current state, and k is the index number of the observation number.

[0176] The purpose of the above architecture is to integrate IMU and visual data to suppress IMU noise caused by strong electromagnetic interference in substations.

[0177] 4. Acceleration-sensing Bézier spline path:

[0178] Constructing a cubic Bézier curve:

[0179]

[0180]

[0181] ,

[0182] in, Let be the coordinates of the Bézier curve at parameter t, and s be the normalization parameter. As the starting point of the curve, , These are the control points in the middle of the curve, controlling the direction and degree of curvature of the curve. The endpoint of the curve, The total time to traverse the entire curve is given by t, which is either a time parameter or a path parameter. Angular acceleration, It is the reciprocal of angular acceleration. The initial linear velocity, The termination linear velocity. When detected At that time, the control points are automatically adjusted to reduce the path curvature by 37%.

[0183] 5. Asymmetric noise suppression strategy:

[0184] High-frequency noise processing:

[0185]

[0186] in, For the output signal after high-frequency noise suppression (e.g., filtered angular velocity), represents the smoothed estimate at time k. Here, i is the index of the current time, and i is the index of the time within the sliding window. To adjust the sliding window size, These are Gaussian weighting coefficients. Let i be the original measurement value at time i. The standard deviation of the Gaussian kernel. This can suppress motor vibration noise.

[0187] Low-frequency drift correction:

[0188]

[0189] in, Let be the low-frequency drift parameter vector to be optimized. The total number of data points participating in the optimization. For the data point index participating in the optimization, This is an intrinsic parameter matrix (such as camera intrinsic parameters or sensor calibration matrix). In order to be with the first The transformation matrix associated with each data point. For the first The coordinates of each data point in the world coordinate system For the first The actual observed values ​​of each data point This is a projection function, typically representing a transformation from homogeneous coordinates to non-homogeneous coordinates. These are the predicted observations.

[0190] The above suppression strategies can suppress high-frequency vibration (50Hz) and temperature drift (<0.1Hz) of motors in the substation environment, respectively.

[0191] The complete workflow is as follows:

[0192] 1. Load the initial pose during initialization;

[0193] 2. Upon receiving an obstacle avoidance command, initiate pre-integrator accumulation;

[0194] 3. Execute the following process every 5ms:

[0195] Kalman filtering is used to fuse IMU / visual data;

[0196] according to Calculate dynamic weights ;

[0197] Bézier control points are dynamically updated;

[0198] generate And convert back ;

[0199] 4. When At that time, complete from arrive The smooth motion, that is, after 0.5 seconds of real-time interpolation and filtering control, the robot's actual pose coincides with (or is close enough to) the target pose, and the obstacle avoidance path interpolation is completed.

[0200] After verifying the effectiveness of the above workflow, the algorithm can reduce the maximum angular velocity error from 2.1 rad / s to 0.8 rad / s and the average visual reprojection error from 12.3 pixels to 4.7 pixels in this scenario, with a calculation time of 0.15ms, which meets the real-time requirements of substation inspection.

[0201] This application effectively solves the dynamic interpolation problem in complex electromagnetic environments through multi-constraint joint optimization within the framework of Lie group theory, providing a high-precision motion control scheme for power line inspection robots. Experiments show that in drastic motion scenarios with angular velocities >5 rad / s, this algorithm reduces pose drift error by 60% compared to traditional methods.

[0202] In one embodiment, such as Figure 2 As shown, Figure 2 This is a schematic diagram illustrating the process of timestamp alignment of the original attitude data provided in the embodiments of this application; S140 uses the pre-integral values ​​of each sub-interval to correct the virtual attitude data of each sub-interval to obtain the target attitude data, which may include:

[0203] S141: Construct an objective function based on the pre-integrated quantities of each sub-interval, with the goal of minimizing the overall residual during multi-sensor fusion. Optimize the objective function to obtain the optimization result.

[0204] S142: Based on the optimization results, the virtual pose data of each sub-interval is corrected to obtain the target pose data.

[0205] In this embodiment, when the virtual pose data satisfies the Lie group-Lie algebra optimization conditions, this application can perform Lie algebra optimization on the virtual pose data of each sub-interval so that the interpolation path meets the maximum steering angle and acceleration limits, and dynamically adjust the interpolation parameters according to the semantic information of each visual keyframe to obtain the optimized virtual pose data.

[0206] Next, this application can construct an objective function based on the pre-integrated quantities of each sub-interval, with the goal of minimizing the overall residual during multi-sensor fusion. After optimizing and solving the objective function, the optimization result is obtained. Then, the virtual attitude data of each sub-interval is corrected based on the optimization result to obtain the target attitude data.

[0207] In one specific implementation, this application can calculate the covariance matrix for each subinterval's pre-integral value. The covariance matrix can be obtained from the IMU noise parameters (gyroscope noise, accelerometer noise) through the error dynamics equation, as shown in the following formula:

[0208]

[0209] in, Here is the state transition matrix. This is the noise intensity matrix.

[0210] Next, in the optimization problem, this application can use the covariance matrix as the Mahalanobis distance weight and optimize the Mahalanobis distance weight. The objective function during optimization can adopt the least squares framework, as follows:

[0211]

[0212]

[0213] in, , , Let these represent the rotational residual, velocity residual, and displacement residual within the k-th subinterval, respectively. The information matrix of the residuals (the inverse of the covariance matrix) reflects the uncertainty of the pre-integral, where n is the number of subintervals and k is the subinterval index. This is a pre-integral model; the output is the predicted relative motion. As a reflection of Li Que, For kinematic constraints, Here, is the regularization coefficient, balancing the weights of the pre-integral residuals and kinematic constraints; x is the summation index variable. The rotation increment is obtained by pre-integration of the IMU. The velocity increment obtained by pre-integration of the IMU, Let k be the pose or trajectory at time k.

[0214] This application can combine the above two formulas into a unified objective function, as follows:

[0215]

[0216] in, To balance the weights of the Mahalanobis distance-weighted residuals, The weights of the kinematic constraint terms are to be balanced (which can be determined experimentally or through covariance adaptation). Let be the pose sequence to be optimized, i.e., the pose transformation matrix of the robot at each time k. For the pre-integrated residual, This is the sum of the rotational / velocity / displacement residuals weighted by Mahalanobis distance. This provides kinematic constraints. This not only ensures that the optimized pose matches the relative motion measured by the IMU by minimizing the weighted sum of squares of the pre-integrated residuals, but also, considering noise distribution, allows for the joint optimization of the pre-integrated residuals and kinematic constraints. This guarantees both the geometric consistency of the IMU data and prevents the optimization results from violating physical laws.

[0217] When this application is optimized using the above formula, the weight of high covariance intervals (such as segments with violent motion) can be reduced to suppress the influence of noise, while the weight of low covariance intervals (such as static segments) can be increased to enhance the constraint effect. In addition, if the interpolation error of a certain sub-interval is too large (such as the covariance exceeding the threshold), re-interpolation or keyframe insertion can be triggered.

[0218] The data fusion apparatus provided in the embodiments of this application is described below. The data fusion apparatus described below and the data fusion method described above can be referred to and correspond to each other.

[0219] In one embodiment, such as Figure 3 As shown, Figure 3 This is a schematic diagram of a data fusion device provided in an embodiment of this application; this application also provides a data fusion device, which may include a data acquisition module 210, a data interpolation module 220, a pre-integration module 230, and a data correction module 240, specifically including the following:

[0220] The data acquisition module 210 is used to trigger the lidar, vision sensor and inertial measurement unit to synchronously acquire raw point cloud data, target image data and raw attitude data through a global clock signal.

[0221] The data interpolation module 220 is used to determine the timestamps corresponding to each visual keyframe in the original point cloud data or the target image data, and to interpolate the original pose data according to each timestamp to obtain virtual pose data at each timestamp.

[0222] The pre-integration module 230 is used to determine multiple sub-intervals and virtual attitude data of each sub-interval based on the time distribution of each timestamp, and to obtain the pre-integrated quantity of each sub-interval after pre-integrating the virtual attitude data of each sub-interval.

[0223] The data correction module 240 is used to correct the virtual attitude data of each sub-interval using the pre-integrated quantity of each sub-interval, so as to obtain the target attitude data.

[0224] In the above embodiments, after triggering the LiDAR, visual sensor, and inertial measurement unit to synchronously acquire raw point cloud data, target image data, and raw attitude data via a global clock signal, the timestamps corresponding to each visual keyframe in the raw point cloud data or target image data can be determined first. Then, to reduce the computational load of state estimation, this application can determine multiple sub-intervals and virtual attitude data for each sub-interval based on the time distribution of each timestamp, and pre-integrate the virtual attitude data of each sub-interval to obtain the pre-integrated quantity for each sub-interval. This allows the pre-integrated quantity of each sub-interval to be used to correct the virtual attitude data of each sub-interval, thereby obtaining the target attitude data. In this process, this application aims to minimize the overall residual during multi-sensor fusion. By calculating the relative motion in the local coordinate system through pre-integration, the optimization problem can be decoupled from the initial state, and the zero bias of the accelerometer and gyroscope can be compensated, thereby reducing dynamic errors and meeting the requirements for high-precision positioning.

[0225] In one embodiment, this application also provides a computer-readable storage medium storing computer-readable instructions that, when executed by one or more processors, cause the one or more processors to perform the steps of the data fusion method as described in any of the above embodiments.

[0226] In one embodiment, this application also provides a computer device, including: one or more processors, and memory.

[0227] The memory stores computer-readable instructions, which, when executed by the one or more processors, perform the steps of the data fusion method as described in any of the above embodiments.

[0228] Indicatively, such as Figure 4 As shown, Figure 4 This is a schematic diagram of the internal structure of a computer device 300 provided in an embodiment of this application. The computer device 300 can be provided as a server. (Refer to...) Figure 4 The computer device 300 includes a processing component 302, which further includes one or more processors, and memory resources represented by memory 301 for storing instructions, such as application programs, that can be executed by the processing component 302. The application programs stored in memory 301 may include one or more modules, each corresponding to a set of instructions. Furthermore, the processing component 302 is configured to execute instructions to perform the data fusion method of any of the above embodiments.

[0229] The computer device 300 may also include a power supply component 303 configured to perform power management of the computer device 300, a wired or wireless network interface 304 configured to connect the computer device 300 to a network, and an input / output (I / O) interface 305. The computer device 300 may operate on an operating system stored in memory 301, such as Windows Server™, Mac OS X™, Unix™, Linux™, Free BSD™, or similar.

[0230] Those skilled in the art will understand that Figure 4 The structure shown is merely a block diagram of a portion of the structure related to the present application and does not constitute a limitation on the computer device to which the present application is applied. Specific computer devices may include more or fewer components than those shown in the figure, or combine certain components, or have different component arrangements.

[0231] Finally, it should be noted that in this document, relational terms such as "first" and "second" are used only to distinguish one entity or operation from another, and do not necessarily require or imply any such actual relationship or order between these entities or operations. Furthermore, the terms "comprising," "including," or any other variations thereof are intended to cover non-exclusive inclusion, such that a process, method, article, or apparatus that comprises a list of elements includes not only those elements but also other elements not expressly listed, or elements inherent to such a process, method, article, or apparatus. Without further limitations, an element defined by the phrase "comprising one..." does not exclude the presence of other identical elements in the process, method, article, or apparatus that includes said element.

[0232] The various embodiments in this specification are described in a progressive manner. Each embodiment focuses on the differences from other embodiments. The various embodiments can be combined as needed, and the same or similar parts can be referred to each other.

[0233] The above description of the disclosed embodiments enables those skilled in the art to make or use this application. Various modifications to these embodiments will be readily apparent to those skilled in the art, and the general principles defined herein may be implemented in other embodiments without departing from the spirit or scope of this application. Therefore, this application is not to be limited to the embodiments shown herein, but is to be accorded the widest scope consistent with the principles and novel features disclosed herein.

Claims

1. A data fusion method characterized by, The method comprises: Synchronously collecting raw point cloud data, target image data and raw attitude data by triggering a laser radar, a visual sensor and an inertial measurement unit through a global clock signal; Determining a timestamp corresponding to each visual key frame in the raw point cloud data or the target image data, and interpolating the raw attitude data according to each timestamp to obtain virtual attitude data at each timestamp; Determining a plurality of subintervals and virtual attitude data of each subinterval based on the time distribution of each timestamp, and obtaining a pre-integral quantity of each subinterval after pre-integrating the virtual attitude data of each subinterval; Correcting the virtual attitude data of each subinterval by using the pre-integral quantity of each subinterval to obtain target attitude data; When the raw attitude data meets normal interpolation conditions and the virtual attitude data meets Lie group-Lie algebra optimization conditions, before the step of correcting the virtual attitude data of each subinterval by using the pre-integral quantity of each subinterval, the method further comprises: Interpolating the virtual attitude data of each subinterval by using a Lie algebra space linear interpolation method, and in the interpolation process, dynamically adjusting interpolation weights according to semantic information of each visual key frame, optimizing each virtual attitude data by using a pre-integration-Kalman joint filtering architecture, adjusting an interpolation path by using an acceleration perception Bézier spline path, and performing frequency band processing on noise in each virtual attitude data by using an asymmetric noise suppression strategy to obtain optimized virtual attitude data.

2. The data fusion method of claim 1, wherein, When the raw attitude data meets normal interpolation conditions, the step of interpolating the raw attitude data according to each timestamp to obtain virtual attitude data at each timestamp comprises: Obtaining quaternions of adjacent time points corresponding to each timestamp in the raw attitude data; Interpolating between the quaternions of adjacent time points corresponding to each timestamp by using a quaternion spherical linear interpolation method to obtain virtual attitude data at each timestamp.

3. The data fusion method of claim 1, wherein, When the raw attitude data meets emergency interpolation conditions, the step of interpolating the raw attitude data according to each timestamp to obtain virtual attitude data at each timestamp comprises: Interpolating the raw attitude data at each timestamp by using a preset emergency interpolation mode to obtain virtual attitude data at each timestamp.

4. The data fusion method of claim 1, wherein, The step of pre-integrating the virtual attitude data of each subinterval to obtain a pre-integral quantity of each subinterval comprises: Converting the virtual attitude data of each subinterval to a key frame coordinate system to obtain converted virtual attitude data; Pre-integrating the converted virtual attitude data in each subinterval to obtain a pre-integral quantity of each subinterval.

5. The data fusion method of claim 1, wherein, The step of correcting the virtual attitude data of each subinterval by using the pre-integral quantity of each subinterval to obtain target attitude data comprises: Constructing a target function with the minimum overall residual error in multi-sensor fusion as an optimization objective based on the pre-integral quantity of each subinterval, and obtaining an optimization result after optimizing and solving the target function; Correcting the virtual attitude data of each subinterval according to the optimization result to obtain target attitude data.

6. A data fusion apparatus characterized by comprising: The method comprises: The data acquisition module is configured to trigger the laser radar, the visual sensor, and the inertial measurement unit to synchronously collect raw point cloud data, target image data, and raw attitude data through a global clock signal. The data interpolation module is configured to determine time stamps corresponding to each visual key frame in the raw point cloud data or the target image data, and interpolate the raw attitude data according to each time stamp to obtain virtual attitude data at each time stamp. The pre-integration module is configured to determine a plurality of subintervals and virtual attitude data of each subinterval based on time distribution of each time stamp, and obtain pre-integration quantities of each subinterval by pre-integrating the virtual attitude data of each subinterval. The data correction module is configured to correct the virtual attitude data of each subinterval by using the pre-integration quantities of each subinterval to obtain target attitude data. When the raw attitude data meets normal interpolation conditions and the virtual attitude data meets Lie group-Lie algebra optimization conditions, the data correction module further includes: The Lie algebra space linear interpolation method is used to interpolate the virtual attitude data of each subinterval, and in the interpolation process, the interpolation weight is dynamically adjusted according to the semantic information of each visual key frame, the pre-integration-Kalman joint filtering architecture is used to optimize each virtual attitude data, the acceleration-aware Bézier spline path is used to adjust the interpolation path, and the asymmetric noise suppression strategy is used to process noise in each virtual attitude data in different frequency bands to obtain optimized virtual attitude data.

7. A computer-readable storage medium, characterized in that: The computer readable storage medium stores computer readable instructions, and the computer readable instructions are executed by one or more processors to perform the steps of the data fusion method according to any one of claims 1 to 5.

8. A computer device, comprising: The computer readable storage medium stores computer readable instructions, and the computer readable instructions are executed by one or more processors to perform the steps of the data fusion method according to any one of claims 1 to 5. The computer readable storage medium stores computer readable instructions, and the computer readable instructions are executed by one or more processors to perform the steps of the data fusion method according to any one of claims 1 to 5. ​

Images