Embedded platform-oriented lightweight multi-sensor data real-time fusion method and system

Abstract

The invention discloses an embedded platform-oriented lightweight multi-sensor data accurate and real-time fusion method, which comprises the following steps of: establishing a double-ring buffer area based on DMA (Direct Memory Access) to realize real-time acquisition of multi-sensor data; aligning multi-sensor data based on hardware timestamps and dynamic drift compensation; carrying out filtering processing on multi-sensor data by adopting dynamic cascade filtering, sliding window rapid sorting, segmented self-adaptive loss filtering and robust iterative estimation based on noise confidence; performing data fusion on the filtered multi-sensor data by compressing state dimensions and simplifying covariance matrix operation; the priority of multi-sensor data acquisition, multi-sensor data filtering and multi-sensor data fusion is distributed through a real-time operating system, and task scheduling is carried out. According to the method provided by the invention, the dynamic cascade filtering for lightweight data processing is combined with the lightweight dynamic weight anti-interference fusion algorithm, so that the real-time responsiveness and the data accuracy of the system can be improved.

Description

Technical Field

[0001] The present invention belongs to the technical field of embedded systems and signal processing, and in particular relates to a method and system for real-time fusion of lightweight multi-sensor data for embedded platforms. Background Art

[0002] The flight control system is the core of the drone, which can control the drone to complete various flight missions in real time. Once an abnormality occurs in the flight control system, the drone's movements will not be well controlled and the expected state and trajectory cannot be guaranteed, which will seriously affect the safety of the drone. The sensor is the input device of the flight control system, and its accuracy directly affects the drone's flight control effect.

[0003] Multi-sensor data fusion is an emerging research field, focusing on data processing for the specific problem of using multiple sensors in a single system. Multi-sensor data fusion technology, a highly practical and applied technology developed in recent years, is a multidisciplinary new technology that involves theories such as signal processing, probability and statistics, information theory, pattern recognition, artificial intelligence, and fuzzy mathematics. In recent years, multi-sensor data fusion has seen widespread application in both military and civilian fields. Multi-sensor fusion technology has become a key area of interest in military, industrial, and high-tech development. It is widely used in C3I systems, complex industrial process control, robotics, automatic target recognition, traffic control, inertial navigation, ocean surveillance and management, agriculture, remote sensing, medical diagnosis, image processing, and pattern recognition. Practice has proven that, compared to single-sensor systems, the use of multi-sensor data fusion technology can enhance system survivability, improve overall system reliability and robustness, enhance data credibility, improve accuracy, expand the system's temporal and spatial coverage, and increase real-time performance and information utilization.

[0004] The current mainstream approach to drone data processing and fusion involves first processing data using traditional robustness filtering, followed by data fusion with a traditional Kalman filter. This approach results in insufficient real-time performance, severely restricting its application in resource-constrained scenarios. When running the Kalman filter on a general-purpose controller, multi-sensor data synchronization and fusion algorithms struggle to meet microsecond-level real-time requirements due to issues such as task scheduling delays and insufficient interrupt response. However, recent developments in embedded lightweight algorithms and lightweight dynamic data fusion algorithms hold promise for addressing the current challenges of drones, which suffer from insufficient computing resources, leading to poor real-time performance and an inability to quickly respond to current status. Summary of the Invention

[0005] The technical problem to be solved by the present invention is to provide a method and system for real-time fusion of lightweight multi-sensor data for embedded platforms in response to the shortcomings of the existing technology. The method combines dynamic cascade filtering of lightweight data processing with a lightweight dynamic weight anti-interference fusion algorithm to improve the real-time responsiveness of the system and the accuracy of data.

[0006] To achieve the above objectives, according to one aspect of the present invention, a method for lightweight real-time fusion of multi-sensor data for an embedded platform is provided, comprising:

[0007] Establishing a dual ring buffer based on DMA (Direct Memory Access) to achieve real-time data acquisition from multiple sensors, including a laser radar (LiDAR) and an inertial measurement unit.

[0008] Based on hardware timestamps and dynamic drift compensation, multi-sensor data with different sampling frequencies are aligned. Dynamic cascade filtering is used, along with sliding window fast sorting based on embedded robust filtering, piecewise adaptive loss filtering, and robust iterative estimation based on noise confidence to filter the aligned multi-sensor data.

[0009] By compressing the state dimension and simplifying the covariance matrix operation, an optimized lightweight dynamic weight anti-interference fusion algorithm is obtained to achieve data fusion of multi-sensor data after filtering.

[0010] The real-time operating system is used to assign priorities to multi-sensor data acquisition, multi-sensor data filtering, and multi-sensor data fusion for task scheduling.

[0011] In the above solution, the method for real-time fusion of lightweight multi-sensor data for embedded platforms further includes:

[0012] The noise is evaluated, the kurtosis of the noise is calculated, and the enabling of the sliding window quick sorting, the piecewise adaptive loss filtering, and the noise confidence-based robust iterative estimation is selected in combination with the noise threshold.

[0013] In the above scheme, the method for real-time fusion of lightweight multi-sensor data for embedded platforms also includes: when sensor data is lost, starting the prediction mode, and generating pseudo observation values based on the time series prediction model fitted with historical data.

[0014] In the above scheme, the method for filtering the aligned multi-sensor data based on the sliding window quick sorting of the embedded anti-aliasing filter is as follows:

[0015] The ARM CMSIS-DSP sorting function arm_sort_f32 is used to reduce the complexity of data points by optimizing the fast sorting and merge sorting algorithms, and quickly remove impulse noise and obvious outliers.

[0016] In the above scheme, the method for filtering the aligned multi-sensor data based on the segmented adaptive loss filtering of the embedded robust filtering is as follows:

[0017] Calculate the residuals of the data after quick sorting of the sliding window and dynamically adjust the weights; combine the quadratic function and the linear function, use square loss for small residuals and linear loss for large residuals; wherein the small residual is the absolute value of the residual less than the dynamic threshold, corresponding to the prediction error within the normal range, and the large residual is the absolute value of the residual greater than or equal to the dynamic threshold;

[0018] A micro neural network is used to dynamically adjust the segmented adaptive loss filter threshold.

[0019] In the above scheme, the method for filtering the aligned multi-sensor data based on the robust iterative estimation based on noise confidence of the embedded anti-error filtering is as follows:

[0020] According to the generalized robust estimation framework, a loss function is adopted to suppress outliers.

[0021] In the above scheme, the optimized lightweight dynamic weight anti-interference fusion algorithm is obtained by compressing the state dimension and simplifying the covariance matrix operation. The data fusion of the multi-sensor data after filtering also includes:

[0022] The square loss is replaced by piecewise adaptive loss filtering loss to suppress the influence of outliers and improve robustness.

[0023] In the above solution, the method of establishing a DMA-based dual ring buffer to achieve real-time acquisition of multi-sensor data includes:

[0024] Hardware initialization configuration: enable clock source, turn on DMA controller clock, and enable sensor interface peripheral clock;

[0025] Configure the GPIO pin clock and multiplexing function. According to the sensor interface type, map the SPI's SCK, MISO, and MOSI pins to SPI1 with push-pull output, no pull-up or pull-down, high-speed mode, and multiplexing functions. Set the IIC's SDA and SCL pins and the UART's RX and TX pins to GPIO multiplexing mode and bind them to the corresponding peripherals.

[0026] The SPI protocol transmits data through full-duplex communication between master and slave devices, configures the operating mode, clock polarity, phase, data bit width, and chip select control;

[0027] DMA transfer configuration and startup: specify the DMA transfer source and destination addresses, and define a buffer to store the data to be sent / received; configure DMA transfer parameters, start DMA transfers, and enable transfer completion interrupts; perform DMA channel binding. The DMA controller manages data transfers through channels, allocating independent DMA channels for SPI and IIC transmission and reception; associate the DMA handle with the SPI and IIC peripherals, configure the DMA transfer direction, priority, and data width, and then initialize the DMA channel; finally, the SPI, TXE, and RXNE flags in the peripheral events can automatically trigger DMA transfers without CPU intervention;

[0028] Interrupt configuration and processing: Use NVIC to manage interrupt priorities, assign priorities to DMA transfer completion interrupt (TC) and transfer error interrupt (TE), configure SPI and IIC error interrupts; after completing the interrupt service routine, DMA transfer is completed or an error occurs, triggering an interrupt and executing the callback function.

[0029] In the above solution, the method for aligning the multi-sensor data based on hardware timestamp and dynamic drift compensation is:

[0030] Perform hardware timestamp marking, allocate independent hardware timers for each sensor LiDAR and IMU, and configure them in free-running mode;

[0031] When the sensor collects data, the double buffer mechanism is triggered through DMA transmission, and the current timer value is recorded in the data frame header;

[0032] Perform global clock reference alignment to establish a unified time reference;

[0033] Performing dynamic drift compensation, periodically detecting the drift rate of each timer relative to the unified time reference, and calculating a timer error model using a least squares method;

[0034] Correct sensor timestamps in real time;

[0035] Perform data frame interpolation and alignment.

[0036] In addition, to achieve the above objectives, the present invention also proposes a lightweight multi-sensor data real-time fusion system for embedded platforms, comprising:

[0037] A data acquisition module, configured to establish a dual-ring buffer based on DMA to enable real-time data acquisition from multiple sensors, including a laser radar (LiDAR) and an inertial measurement unit;

[0038] The data filtering module is used to align multi-sensor data with different sampling frequencies based on hardware timestamps and dynamic drift compensation. Dynamic cascade filtering is used to filter the aligned multi-sensor data using sliding window fast sorting based on embedded robust filtering, piecewise adaptive loss filtering, and robust iterative estimation based on noise confidence.

[0039] The data fusion module is used to achieve data fusion of multi-sensor data after filtering by compressing the state dimension and simplifying the covariance matrix operation to obtain an optimized lightweight dynamic weight anti-interference fusion algorithm;

[0040] The task scheduling module is used to assign priorities for multi-sensor data acquisition, multi-sensor data filtering, and multi-sensor data fusion through a real-time operating system to perform task scheduling.

[0041] In the present invention, the sliding window quick sorting is used to meet the advantage of fast response. The sorting function arm_sort_f32 of ARM CMSIS-DSP is used to reduce the complexity to O(nlogn) by optimizing the algorithm for quick sorting and merge sorting, thereby improving the real-time response. The contribution of outliers is suppressed by introducing a robust loss function. The present invention combines three types of filtering: sliding window quick sorting, piecewise adaptive loss filtering, and robust iterative estimation based on noise confidence. The filtering effect is greatly enhanced, and the data accuracy is greatly improved compared with ordinary filtering. The present invention adopts an improved strategy of a lightweight dynamic weight anti-interference fusion algorithm to compress the state dimension, track only the key states, simplify the covariance matrix, avoid real-time calculation of covariance updates, reduce O(n3) operations, and replace the square loss with the piecewise adaptive loss filtering loss. The influence of outliers is suppressed and robustness is improved. The present invention uses micro-neural network embedding, utilizes lightweight models for online learning, and continuously updates threshold parameters to achieve better filtering effects. The present invention makes full use of DMA and floating-point units for data processing and transmission, greatly accelerating the data transmission processing speed and reducing the CPU burden. The present invention combines adaptive Kalman filtering with hardware timer calibration to estimate and compensate for the frequency-temperature drift of the quartz oscillator in real time, realizing multi-source data acquisition, and designs a DMA-based dual-ring buffer. Data acquisition and processing are performed in parallel, supporting real-time synchronization of LiDAR data at 100,000 points / second. When the sensor fails, a pseudo data stream is generated based on the ARIMA model for a short time to ensure synchronization continuity and prevent data gaps.

[0042] In general, the above technical solutions conceived by the present invention can achieve the following beneficial effects compared with the prior art:

[0043] The present invention provides a lightweight multi-sensor data real-time fusion method for embedded platforms. The method can improve the system's real-time responsiveness and data accuracy by combining dynamic cascade filtering for lightweight data processing with a lightweight dynamic weight anti-interference fusion algorithm. BRIEF DESCRIPTION OF THE DRAWINGS

[0044] Various other advantages and benefits will become apparent to those skilled in the art upon reading the detailed description of the preferred embodiment below. The accompanying drawings are for illustration purposes only and are not to be considered as limiting the present invention. Throughout the drawings, the same reference figures denote the same components. In the drawings:

[0045] Figure 1 The figure is a flow chart of a method for real-time fusion of lightweight multi-sensor data for embedded platforms in the first embodiment of the present invention.

[0046] Figure 2 This is a flow chart of implementing sensor DMA transmission and interrupt control based on the HAL library in the first embodiment of the present invention.

[0047] Figure 3 This is a FreeRTOS scheduling flow chart in Example 1 of the present invention. DETAILED DESCRIPTION

[0048] In order to make the objectives, technical solutions and advantages of the present invention more clearly understood, the present invention is further described in detail below with reference to the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are merely for the purpose of explaining the present invention and are not intended to limit the present invention. In addition, the technical features involved in the various embodiments of the present invention described below may be combined with each other as long as they do not conflict with each other.

[0049] It should be understood that the size of the serial numbers of the steps in the embodiment does not mean the order of execution. The execution order of each process should be determined by its function and internal logic, and should not constitute any limitation on the implementation process of the embodiment of the present application.

[0050] Example 1

[0051] The present invention provides a lightweight multi-sensor data real-time fusion method for embedded platforms. Figure 1 , Figure 1 The flowchart of a method for real-time fusion of lightweight multi-sensor data for an embedded platform in an embodiment of the present application is as follows:

[0052] S1, establishes a dual ring buffer based on DMA to realize real-time acquisition of multi-sensor data; the multi-sensors include lidar and inertial measurement unit.

[0053] In the embodiments of this application, it should be noted that this embodiment is for the high-real-time fusion of drone laser radar (LiDAR) point cloud data and IMU (Inertial Measurement Unit) posture data. Specifically, the drone's flight control system in this embodiment is divided into two parts: hardware and software. The hardware requires a development board with an STM32 main controller that supports hardware double-precision FPU (floating point unit) and DMA (Direct Memory Access); for extended storage, external SRAM (Static Random-Access Memory) is used for dynamic data caching.

[0054] The multi-sensor system includes a laser radar (LiDAR) and an inertial measurement unit (IMU). The laser LiDAR uses the SPI protocol to transmit VLP-16 point cloud data. The LiDAR can transmit and receive VLP-16 point cloud data at a rate of up to 300,000 points per second. The IMU uses the MPU6050 and uses the I2C protocol for communication, with a sampling frequency of up to 100 Hz.

[0055] like Figure 2 As shown, Figure 2 The following is a flow chart of implementing sensor DMA transmission and interrupt control based on the HAL library. In this embodiment, a method for establishing a dual ring buffer based on DMA to implement real-time multi-sensor data acquisition specifically includes:

[0056] Complete the hardware initialization configuration. The STM32 peripherals rely on clock signals to work. Enable the clock source through the Reset and ClockControl module, then turn on the DMA controller clock and enable the sensor interface peripheral clocks including the SPI, I2C, and UART communication modules.

[0057] Next, configure the GPIO pin clock and multiplexing functions. Based on the sensor interface type, map the SPI's SCK, MISO, and MOSI pins to SPI1, using push-pull outputs without pull-up or pull-down switches, in high-speed mode, and multiplexing functions. Set the GPIOs for the IIC's SDA and SCL pins and the UART's RX and TX pins to multiplexing mode and bind them to the corresponding peripherals.

[0058] After completing the sensor interface configuration, the SPI protocol transmits data through full-duplex communication between the master and slave devices, configuring the operating mode, clock polarity, phase, data bit width, and chip select control.

[0059] Then configure and start the DMA transfer, specify the DMA transfer source address and destination address, and define the buffer to store the data to be sent / received. Configure the DMA transfer parameters, start the DMA transfer and enable the transfer completion interrupt. In this embodiment, the transfer length is one byte. Then perform DMA channel binding. The DMA controller manages data transmission through the channel. Independent DMA channels must be allocated for SPI and IIC transmission and reception. Associate the DMA handle with the SPI and IIC peripherals, configure the DMA transfer direction, priority, and data width, and then initialize the DMA channel. Finally, the SPI TXE / RXNE and other flags in the peripheral events can automatically trigger DMA transfers without CPU intervention.

[0060] Specifically, DMA data transfer time calculation:

[0061]

[0062] Among them, N bytes is the number of bytes transmitted, and f1 is the SPI clock frequency.

[0063] Then, interrupt configuration and processing are performed. The NVIC (Nested Vectored Interrupt Controller) is used to manage interrupt priorities, assign priorities to the DMA transfer completion interrupt (TC) and transfer error interrupt (TE), and configure SPI and IIC error interrupts. After the interrupt service routine is completed, the DMA transfer is completed or an error occurs, triggering an interrupt and executing the callback function.

[0064] Specifically, interrupt latency analysis:

[0065] T Latency =T1+T2+T3

[0066] Where T1 is the interrupt response time, T2 is the register save time, approximately 20 cycles, and T3 is the callback function execution time. In this example, the total time is less than 1ms, meeting the real-time requirements.

[0067] Finally, call the HAL library function HAL_SPI_Transmit_DMA(&hspi1,tx_buffer,sizeof(tx_buffer)) to start DMA transmission.

[0068] S2, based on hardware timestamp and dynamic drift compensation, aligns multi-sensor data with different sampling frequencies; dynamic cascade filtering is used to filter the aligned multi-sensor data based on sliding window fast sorting with embedded robust filtering, piecewise adaptive loss filtering, and robust iterative estimation based on noise confidence.

[0069] In the embodiment of the present application, it is understandable that since the data transmission frequencies of various sensors are different, data may not be updated synchronously. Therefore, in this example, a multi-source data real-time synchronization method based on hardware timestamp alignment is used.

[0070] Specifically, in this embodiment, the method for aligning multi-sensor data based on hardware timestamp and dynamic drift compensation is as follows:

[0071] Perform hardware timestamp marking, allocate independent hardware timers for each sensor LiDAR and IMU, and configure them in free-running mode; the timer value is recorded as τ sensor , unit is microsecond;

[0072] When the sensor collects data, the double buffer mechanism is triggered by DMA transmission, and the current timer value is recorded in the data frame header; the data format D = {τ raw ,Data};

[0073] Perform global clock reference alignment. The main control chip STM32 calibrates all TIM timers through the hardware synchronization signal PPS pulse to establish a unified time reference τ base , the initial deviation compensation formula is:

[0074] τ′ sensor =τ sensor +Δ offset

[0075] where Δ offset is the initial deviation of each timer from the unified time base.

[0076] Then, dynamic drift compensation is performed, and the timers are periodically detected relative to the unified time reference τ. base The drift rate ρ is calculated using the least squares method to fit the timer error model:

[0077] ρ=α·τ base +β

[0078] Among them, α is the frequency drift coefficient, and β is the initial phase difference.

[0079] Then start correcting the sensor timestamps in real time:

[0080] τ corrected =τ raw (1-α)-β

[0081] Then, the data frame interpolation and alignment are performed to generate interpolated time series according to the target frequency f for the data streams LiDAR and IMU with different sampling rates. The interpolation formula is:

[0082]

[0083] in

[0084] In particular, in this embodiment, if the sensor data is lost, the prediction mode is started, and a time series prediction model (in this embodiment, an ARIMA model) is fitted based on the historical data to generate pseudo observations:

[0085]

[0086] A short period of pseudo data stream generated based on the ARIMA model ensures synchronization continuity until the sensor returns to normal.

[0087] In the embodiment of the present application, Stm32 performs data processing after collecting data, and adopts lightweight data processing for drone sensors and lightweight dynamic weight anti-interference fusion algorithm to improve the real-time response of the system and data accuracy.

[0088] It is understandable that least squares filtering is the principle and mathematical basis of anti-error filtering. Traditional least squares filtering uses the least squares method to estimate by minimizing the residual sum of squares:

[0089]

[0090] It is sensitive to outliers, as the squared term amplifies the impact of the residual, leading to estimation bias. In this example, a robust loss function or adaptive weights are introduced to suppress the contribution of outliers.

[0091]

[0092] Here, ρ(·) is a robust loss function that needs to satisfy the characteristics of being approximately squared for small residuals and insensitive to large residuals.

[0093] Specifically, in an embodiment of the present application, a total of three stages of three embedded anti-error filtering are used to improve data stability, namely sliding window quick sorting, segmented adaptive loss filtering, and robust iterative estimation based on noise confidence, and a unique embedded method is used to improve real-time performance.

[0094] (1) The traditional filtering method requires sorting 2k+1 data points in each window, with a time complexity of O(n2). The first stage sliding window fast sorting used in this example is to meet the fast response advantage. The ARM CMSIS-DSP sorting function arm_sort_f32 is used to optimize the algorithm to reduce the complexity to O(nlogn) by fast sorting and merge sorting. The data is first divided into blocks: the LiDAR point cloud is divided into blocks according to azimuth (θ) and distance (r). Then, sliding window sorting is used to sort the data in each block by distance, and the median is taken as the output. The CMSIS-DSP library is then used to accelerate the sorting. Finally, outliers are removed. If the deviation of a point from the median exceeds the threshold of 3σ, it is marked as invalid to ensure data accuracy. The first stage achieves the rapid removal of impulse noise and obvious outliers, such as flying points in the LiDAR point cloud.

[0095] Specifically, for a sequence with a window size of 2k+1 (x i-k ,x i-k+1 ,…,x i+k ), the filtered output is:

[0096]

[0097] (2) In the embodiment of the present application, the second stage of dynamic cascade filtering uses piecewise adaptive loss filtering. The principle of piecewise adaptive loss filtering is to combine quadratic functions with linear functions, using square loss for small residuals and linear loss for large residuals. Then, the residual is calculated for the data after the sliding window is quickly sorted, and the weight is dynamically adjusted.

[0098] Residual calculation:

[0099]

[0100] in, is the estimated value for the kth iteration.

[0101] Then update the filter weights:

[0102]

[0103] Solve the weighted least squares after the update:

[0104]

[0105] Finally, update the estimate:

[0106]

[0107] After the second level of filtering, the effect of suppressing Gaussian noise and balancing smoothness and detail retention can be achieved.

[0108] Specifically, in this example, in order to ensure that the segmented adaptive loss filter threshold δ is always a reasonable value, a micro neural network is used to dynamically adjust the adaptive loss filter threshold δ.

[0109] First, data preprocessing is performed to standardize the raw sensor data, eliminate dimensional differences, and improve the model convergence speed. Read 10 consecutive sensor data points to form an input window:

[0110]

[0111] Then normalize:

[0112]

[0113] where μ hist is the sliding mean of historical data, σ hist is the historical standard deviation, ∈=10 -6 , to prevent division by zero errors.

[0114] Specifically, μ hist The update formula is:

[0115]

[0116] Among them, γ = 0.9 is the forgetting factor.

[0117] Specifically, σ hist The calculation formula is:

[0118]

[0119] Then, through the lightweight 1D CNN network, the abnormal probability p of the input data is calculated:

[0120] Enter X norm , where the number of convolution kernels K = 4, the convolution kernel size τ = 3, and the convolution step size Weight Bias

[0121] Then output the feature map:

[0122]

[0123] i∈{0,1,…,7}i∈{0,1,…,7} is the feature map position, k∈{0,1,2,3}k∈{0,1,2,3} is the convolution kernel index;

[0124] The activation function ReLU is Yconv←max(0,Yconv);

[0125] Then perform global average pooling and input Yconv;

[0126] Output pooling effect:

[0127]

[0128] Then enter the fully connected layer, input The weight Bias b fc ∈R;

[0129] Finally, the abnormal probability p is output:

[0130] z=W fc y pool +b fc

[0131]

[0132] To increase the computing speed of the micro-neural network, this example uses the CMSIS-NN library to accelerate convolution calculations, significantly reducing the single-frame inference time, improving real-time performance, and reducing resource consumption.

[0133] This example also performs exception handling, and decides whether the data enters the subsequent filtering process based on the exception probability p. The decision logic is as follows:

[0134] If p>0.5, mark the current data window as abnormal and discard it directly;

[0135] If p≤0.5, X is sent to the filtering queue and a confidence weight ω=1-p is added.

[0136] Online learning is then performed to dynamically optimize network parameters using new data to adapt to sensor characteristic drift.

[0137] Store the latest 100 frames of data, {X norm , y}, the label y is automatically generated by the filter residual:

[0138]

[0139] Randomly extract 50 frames of data and calculate the cross entropy loss + L2 regularization:

[0140]

[0141] Among them, λ = 0.001 is used to control the regularization strength.

[0142] Then use stochastic gradient descent with momentum:

[0143]

[0144] Here, η = 0.001 is the learning rate and Θ includes all weights and biases.

[0145] Finally, the segmented adaptive loss filter threshold δ is dynamically adjusted according to the abnormal probability p to enhance the filter robustness; the adjustment formula is:

[0146] δ new =δ base ·(1+κ·p)

[0147] Among them, δ base is the base threshold, and κ is the adjustment coefficient, which controls the influence of the abnormal probability on the threshold. In this example, we found that the best effect was achieved when κ = 0.2.

[0148] (3) In this embodiment, the third stage of dynamic cascade filtering for lightweight data processing adopts robust iterative estimation based on noise confidence. The principle of robust iterative estimation based on noise confidence is a generalized robust estimation framework, which suppresses outliers by selecting a suitable ρ(e) function.

[0149] Specifically, the loss function used in this example is Cauchy loss:

[0150]

[0151] The robust iterative estimation parameter c based on noise confidence is dynamically adjusted according to the residual standard deviation σ after the sliding window is quickly sorted:

[0152] c=2.385σ

[0153] Selecting the loss function Cauchy loss can balance robustness and computational complexity. Calculate the influence function:

[0154]

[0155] Then update the residuals and weights:

[0156]

[0157] Finally, solve the weighted normal equation to get accurate data:

[0158]

[0159] Finally, the number of iterations is limited. In this example, the maximum number of iterations is limited to 3, balancing accuracy and real-time performance. Using a table lookup method, a lookup table for ψ(e) is pre-calculated to avoid real-time calculation of transcendental functions. This can improve real-time performance. In this example, the mean square error of the filtered data is calculated:

[0160]

[0161] Specifically, during the implementation of this embodiment, it was found from the two data that the mean square error was 11.5cm2 Down to 1.2cm 2 , single frame processing time ≤ 1.2ms.

[0162] S3, an optimized lightweight dynamic weight anti-interference fusion algorithm is obtained by compressing the state dimension and simplifying the covariance matrix operation to realize data fusion of multi-sensor data after filtering.

[0163] Specifically, in this embodiment, the obtained LiDAR point cloud data and the pose provided by the IMU are fused and complemented to obtain a more accurate UAV pose. However, traditional Kalman filtering has certain limitations. It relies too much on the Gaussian assumption. Traditional Kalman filtering assumes that process noise and observation noise are Gaussian distributed, is sensitive to non-Gaussian noise, amplifies outliers, and the minimum mean square error criterion's squared penalty on outliers can lead to estimation bias, all of which reduce robustness. The computational complexity is high, and the updating and inversion of the covariance matrix has a high time complexity of O(n³), making it difficult to complete in real time on embedded platforms.

[0164] This embodiment adopts an improved strategy for a lightweight dynamic weighted anti-interference fusion algorithm. To overcome the above problems, the lightweight dynamic weighted anti-interference fusion algorithm is optimized in the following aspects:

[0165] Compress the state dimension and only track the key states, positions and velocities, ignoring the acceleration in the higher-order states.

[0166] It is understandable that the traditional Kalman filter basic formula is:

[0167]

[0168] Update phase:

[0169]

[0170] In this embodiment, the improved lightweight dynamic weight anti-interference fusion algorithm:

[0171] Let the system simplify the traditional 9-dimensional state (including acceleration) to only position and velocity (6-dimensional state):

[0172] x=[p x ,p y ,p z ,v x ,v y ,v z ] T

[0173] Furthermore, we simplify the covariance matrix and fix the process noise covariance Q and the observation noise covariance R to diagonal matrices to avoid real-time calculation. We preset Q and R to diagonal matrices:

[0174] Q=diag(0.1 2 ,0.1 2 ,0.1 2 ,0.05 2 ,0.05 2 ,0.05 2 )

[0175] R=diag(0.5 2 ,0.5 2 ,0.5 2 )

[0176] In this embodiment, the above operation avoids the real-time calculation of covariance update, reducing O(n 3 ) operation.

[0177] In this embodiment, the robustness is enhanced by a lightweight dynamic weight anti-interference fusion algorithm:

[0178] Piecewise adaptive loss filter loss function

[0179] In the update stage, the square loss is replaced by piecewise adaptive loss filtering loss to suppress the influence of outliers.

[0180] The corresponding weight function:

[0181]

[0182] In this example, a table lookup method is used to pre-calculate the lookup table of the piecewise adaptive loss filter weight w(e) to avoid real-time calculation of branch judgment.

[0183] The steps for data fusion using the lightweight dynamic weight anti-interference fusion algorithm are as follows:

[0184] First initialize the state vector:

[0185]

[0186] Then initialize the error covariance matrix:

[0187]

[0188] Then make state prediction:

[0189]

[0190] Where A is the state transfer matrix and B is the control input matrix.

[0191] Then make the covariance prediction:

[0192]

[0193] By utilizing the diagonal characteristics of the covariance matrix, the matrix multiplication is converted into a scalar operation, which greatly reduces the complexity of the operation.

[0194]

[0195] Finally, update the data:

[0196] First calculate the residual:

[0197]

[0198] Then calculate the weight matrix:

[0199] W k =diag(w(e k1 ),w(e k2 ),…)

[0200] Specifically, in this example, the segmented adaptive loss filter threshold δ is set to 1.345 times the standard deviation of the observation noise.

[0201] Then update the status:

[0202]

[0203] Then do the covariance update:

[0204]

[0205] Finally, outlier processing is performed to detect residuals. If a residual Exceeding the threshold (3σ), its weight Set it to 0 to completely remove the observation. Perform threshold adaptation and dynamically adjust δ based on historical residuals:

[0206] δ k =γδ k-1 +(1-γ)·median(|e k-1 |)

[0207] Here, γ is the forgetting factor, which is 0.95 in this example.

[0208] The final fused data has a higher accuracy of pose.

[0209] S4, assigns priorities for multi-sensor data acquisition, multi-sensor data filtering, and multi-sensor data fusion through the real-time operating system to perform task scheduling.

[0210] Specifically, the real-time operating system in this embodiment is freertos. Figure 3 As shown, Figure 3This is a FreeRTOS scheduling flow chart in the embodiment of this application. Specifically, in this embodiment, the real-time kernel layer FreeRTOS task scheduling is implemented to assign data collection, filtering, and fusion task priorities as follows:

[0211] First, define the task division and priority. The data acquisition task has a priority of 5 because it is responsible for reading sensor data from LiDAR and IMU in real time and requires the highest priority to ensure data is not lost. The data filtering task, which has a priority of 4, preprocesses the raw data and performs dynamic cascade filtering. This task must be given the second-highest priority to reduce processing latency. The data fusion task, which has a priority of 3, uses a lightweight dynamic weighted anti-interference fusion algorithm to fuse multi-source data, point clouds, and IMU poses to generate high-precision output. Finally, the system monitoring task, which has a priority of 2, monitors task status, resource utilization, and exception handling and has a low priority.

[0212] In this example, the data acquisition task's logic initializes the sensor, configures the SPI / I2C interface, and starts DMA. It executes in a loop, waiting for sensor data to be ready and activating the DMA transfer completion interrupt. It reads data from the DMA buffer and encapsulates it into a data packet, including a timestamp and sensor ID. It then sends the data packet to the filtering queue. It releases the data-ready semaphore and blocks, suspending the task if no data is available to free up CPU resources. The data filtering task's logic first waits for the data-ready semaphore. It then receives raw data packets from the filtering queue, executes the filtering algorithm, and sends the filtered data to the fusion queue. The data fusion task's logic receives data from multiple sources from the fusion queue and then executes the data fusion algorithm. The system monitoring task's logic periodically reads task status and stack usage, checks queue availability, and dynamically adjusts data flow. Exception handling is implemented, restarting peripherals or resetting the task in the event of queue overflow or task stalls.

[0213] In summary, the embodiments of the present application provide a method for real-time fusion of lightweight multi-sensor data for embedded platforms. This method can improve the real-time responsiveness and data accuracy of the system by combining dynamic cascade filtering of lightweight data processing with a lightweight dynamic weight anti-interference fusion algorithm.

[0214] Example 2

[0215] On one hand, an embodiment of the present application provides a lightweight multi-sensor data real-time fusion method for an embedded platform, wherein the method of this embodiment is substantially the same as that of the first embodiment, except that the method further includes:

[0216] The noise is evaluated, the kurtosis of the noise is calculated, and the sliding window fast sorting, piecewise adaptive loss filtering, and robust iterative estimation based on noise confidence are selected in combination with the noise threshold.

[0217] Specifically, in this embodiment, the noise is evaluated, the kurtosis of the noise is calculated, and the enabling of quicksort of the sliding window, piecewise adaptive loss filtering, and robust iterative estimation based on noise confidence is selected in combination with the noise threshold, providing a switching basis for dynamic cascade filtering and avoiding unnecessary calculations, thereby increasing the real-time performance.

[0218] First, each time the most recent N = 100 data points are processed, the window slides and updates as new data arrives, and then the mean value is calculated:

[0219]

[0220] It can be understood that the variance calculation of the current traditional method:

[0221]

[0222] In this instance, embedded optimization is performed, and a recurrence formula is used to avoid repeated calculations:

[0223]

[0224] Then the kurtosis is calculated:

[0225]

[0226] Where: K ∼ 0: Gaussian noise; K > 0: Impulse noise (spike); K < 0: Flat noise (uniform distribution).

[0227] In this embodiment, through multiple experiments and calibration, it is obtained that:

[0228] Low noise threshold: σ1 = 0.1, K1 = 1;

[0229] High noise threshold: σ2 = 0.5, K2 = 2.

[0230] Finally, the dynamic cascade filtering selection mode:

[0231] 1. When σ 2 ≤ σ1 and |K| < K1, enable quicksort of the sliding window.

[0232] 2. When σ 2 ≤ σ2 and |K| > K2, enable quicksort of the sliding window, piecewise adaptive loss filtering, and robust iterative estimation based on noise confidence.

[0233] 3. In other cases, enable quicksort of the sliding window and piecewise adaptive loss filtering.

[0234] On the other hand, an embodiment of the present application provides a lightweight multi-sensor data real-time fusion system for an embedded platform, including:

[0235] The data acquisition module is used to establish a dual-ring buffer based on DMA to realize real-time data acquisition of multiple sensors; the multiple sensors include lidar and inertial measurement unit;

[0236] The data filtering module is used to align multi-sensor data with different sampling frequencies based on hardware timestamps and dynamic drift compensation. Dynamic cascade filtering is used to filter the aligned multi-sensor data using sliding window fast sorting based on embedded robust filtering, piecewise adaptive loss filtering, and robust iterative estimation based on noise confidence.

[0237] The data fusion module is used to achieve data fusion of multi-sensor data after filtering by compressing the state dimension and simplifying the covariance matrix operation to obtain an optimized lightweight dynamic weight anti-interference fusion algorithm;

[0238] The task scheduling module is used to assign priorities for multi-sensor data acquisition, multi-sensor data filtering, and multi-sensor data fusion through a real-time operating system to perform task scheduling.

[0239] It should be pointed out that, according to the needs of implementation, the various steps described in this application can be split into more steps, or two or more steps or partial operations of the steps can be combined into new steps to achieve the purpose of the present invention.

[0240] It will be easily understood by those skilled in the art that the above description is merely a preferred embodiment of the present invention and is not intended to limit the present invention. Any modifications, equivalent substitutions, and improvements made within the spirit and principles of the present invention should be included in the scope of protection of the present invention.

Claims

1. A lightweight multi-sensor data real-time fusion method for embedded platforms, characterized by: include: Establish a dual ring buffer based on DMA to achieve real-time acquisition of multi-sensor data; The multi-sensor comprises a lidar and an inertial measurement unit; Based on hardware timestamps and dynamic drift compensation, multi-sensor data with different sampling frequencies are aligned. Dynamic cascade filtering is used, along with sliding window fast sorting based on embedded robust filtering, piecewise adaptive loss filtering, and robust iterative estimation based on noise confidence to filter the aligned multi-sensor data. By compressing the state dimension and simplifying the covariance matrix operation, an optimized lightweight dynamic weight anti-interference fusion algorithm is obtained to achieve data fusion of multi-sensor data after filtering. The real-time operating system is used to assign priorities to multi-sensor data acquisition, multi-sensor data filtering, and multi-sensor data fusion for task scheduling.

2. The method for real-time fusion of lightweight multi-sensor data for embedded platforms according to claim 1, characterized in that: The method further comprises: The noise is evaluated, the kurtosis of the noise is calculated, and the enabling of the sliding window quick sorting, the piecewise adaptive loss filtering, and the noise confidence-based robust iterative estimation is selected in combination with the noise threshold.

3. The method for real-time fusion of lightweight multi-sensor data for embedded platforms according to claim 1, characterized in that: The method further comprises: When sensor data is lost, the prediction mode is started and pseudo observations are generated based on the fitting time series prediction model based on historical data.

4. The method for real-time fusion of lightweight multi-sensor data for embedded platforms according to claim 1, characterized in that: The method for filtering the aligned multi-sensor data based on the sliding window quick sorting of the embedded anti-error filtering is as follows: The ARM CMSIS-DSP sorting function arm_sort_f32 is used to reduce the complexity of data points by optimizing the fast sorting and merge sorting algorithms, and quickly remove impulse noise and obvious outliers.

5. The method for real-time fusion of lightweight multi-sensor data for embedded platforms according to claim 1, characterized in that: The method for filtering the aligned multi-sensor data using segmented adaptive loss filtering based on embedded robust filtering is as follows: Calculate the residuals of the data after quick sorting of the sliding window and dynamically adjust the weights; combine the quadratic function and the linear function, use square loss for small residuals and linear loss for large residuals; wherein the small residual is the absolute value of the residual less than the dynamic threshold, and the large residual is the absolute value of the residual greater than or equal to the dynamic threshold; A micro neural network is used to dynamically adjust the segmented adaptive loss filter threshold.

6. The method for real-time fusion of lightweight multi-sensor data for embedded platforms according to claim 1, characterized in that: The method for filtering the aligned multi-sensor data based on the robust iterative estimation of noise confidence based on embedded anti-error filtering is as follows: According to the generalized robust estimation framework, a loss function is adopted to suppress outliers.

7. The method for real-time fusion of lightweight multi-sensor data for embedded platforms according to claim 1, characterized in that: By compressing the state dimension and simplifying the covariance matrix operation, an optimized lightweight dynamic weight anti-interference fusion algorithm is obtained. The data fusion of multi-sensor data after filtering is also realized. The square loss is replaced by piecewise adaptive loss filtering loss to suppress the influence of outliers and improve robustness.

8. The method for lightweight multi-sensor data real-time fusion for embedded platforms according to claim 1, characterized in that: The method for establishing a dual ring buffer based on DMA to realize real-time acquisition of multi-sensor data includes: Hardware initialization configuration: enable clock source, turn on DMA controller clock, and enable sensor interface peripheral clock; Configure the GPIO pin clock and multiplexing function. According to the sensor interface type, map the SPI's SCK, MISO, and MOSI pins to SPI1 with push-pull output, no pull-up or pull-down, high-speed mode, and multiplexing functions. Set the IIC's SDA and SCL pins and the UART's RX and TX pins to GPIO multiplexing mode and bind them to the corresponding peripherals. The SPI protocol transmits data through full-duplex communication between master and slave devices, configures the operating mode, clock polarity, phase, data bit width, and chip select control; DMA transfer configuration and startup: specify the DMA transfer source and destination addresses, and define a buffer to store the data to be sent / received; configure DMA transfer parameters, start DMA transfers, and enable transfer completion interrupts; perform DMA channel binding. The DMA controller manages data transfers through channels, allocating independent DMA channels for SPI and IIC transmission and reception; associate the DMA handle with the SPI and IIC peripherals, configure the DMA transfer direction, priority, and data width, and then initialize the DMA channel; finally, the SPI, TXE, and RXNE flags in the peripheral events can automatically trigger DMA transfers without CPU intervention; Interrupt configuration and processing: Use NVIC to manage interrupt priorities, assign priorities to DMA transfer completion interrupts and transfer error interrupts, and configure SPI and IIC error interrupts; after completing the interrupt service routine, DMA transfer completion or an error occurs, triggering an interrupt and executing the callback function.

9. The method for lightweight multi-sensor data real-time fusion for embedded platforms according to claim 1, characterized in that: The method for aligning the multi-sensor data based on hardware timestamp and dynamic drift compensation is: Perform hardware timestamp marking, allocate independent hardware timer for each sensor, and configure it in free-running mode; When the sensor collects data, the double buffer mechanism is triggered through DMA transmission, and the current timer value is recorded in the data frame header; Perform global clock reference alignment to establish a unified time reference; Performing dynamic drift compensation, periodically detecting the drift rate of each timer relative to the unified time reference, and calculating a timer error model using a least squares method; Correct sensor timestamps in real time; Perform data frame interpolation and alignment.

10. A lightweight multi-sensor data real-time fusion system for embedded platforms, characterized by: include: Data acquisition module, used to establish a dual ring buffer based on DMA to realize real-time acquisition of multi-sensor data; The multi-sensor comprises a lidar and an inertial measurement unit; The data filtering module is used to align multi-sensor data with different sampling frequencies based on hardware timestamps and dynamic drift compensation. Dynamic cascade filtering is used to filter the aligned multi-sensor data using sliding window fast sorting based on embedded robust filtering, piecewise adaptive loss filtering, and robust iterative estimation based on noise confidence. The data fusion module is used to achieve data fusion of multi-sensor data after filtering by compressing the state dimension and simplifying the covariance matrix operation to obtain an optimized lightweight dynamic weight anti-interference fusion algorithm; The task scheduling module is used to assign priorities for multi-sensor data acquisition, multi-sensor data filtering, and multi-sensor data fusion through a real-time operating system to perform task scheduling.

Images