SLAM hardware acceleration architecture suitable for resource-constrained environment

By designing a SLAM hardware acceleration architecture based on FPGA in an embedded system with resource constrained, parallel computing and real-time data processing of SLAM algorithms are realized, solving the problems of SLAM performance bottlenecks and high power consumption in resource constrained environments, and improving the Frame rate and real-time.

CN118982453AActive Publication Date: 2024-11-19HARBIN INST OF TECH

Patent Information

Application Number
CN202411003587.1
Authority / Receiving Office
CN · China
Patent Type
Applications(China)
Current Assignee / Owner
Filing Date
2024-07-25
Publication Date
2024-11-19
Estimated Expiration
2044-07-25

AI Technical Summary

Technical Problem

The prior art is difficult to effectively accelerate SLAM algorithms on resource-constrained embedded system platforms, resulting in performance bottlenecks and high power consumption, and cannot meet the real-time positioning and map construction requirements in complex environments.

Method used

A SLAM hardware acceleration architecture based on FPGA is designed to realize parallel computing and real-time data processing by synchronous binocular image acquisition, image feature extraction and matching, high-speed data communication between FPGA and CPU, and extended Kalman filter accelerator, thereby reducing power consumption and Delay.

Benefits of technology

Without increasing positioning accuracy, the frame rate of the SLAM algorithm is greatly improved, the performance of resource-constrained systems is improved, the needs of real-time and efficientness are met, and data transmission efficiency is optimized through customized hardware logic and DMA interfaces.

✦ Generated by Eureka AI based on patent content.

Smart Images

  • Figure CN118982453A_ABST
    Figure CN118982453A_ABST
Patent Text Reader

Abstract

The invention discloses an SLAM (Simultaneous Localization and Mapping) hardware acceleration architecture suitable for a resource-constrained environment, and belongs to the field of autonomous navigation. The method is provided for greatly improving the performance of an SLAM system in the aspects of power consumption, time delay and the like and realizing SLAM acceleration under resource limitation. According to the invention, the calculation-intensive part in the SLAM algorithm is transplanted to the FPGA for parallel calculation, so that the performance of the SLAM system is greatly improved in the aspects of power consumption, time delay and the like. According to the method, customized design can be carried out according to a specific application scene, the execution efficiency of the SLAM algorithm is improved by adopting a front-end and rear-end dual acceleration idea, customized optimization can be carried out according to specific application requirements, and method guidance can be provided for design of a binocular SLAM SOC chip. The method is applied to the SLAM hardware acceleration method.
Need to check novelty before this filing date? Find Prior Art

Description

Technical Field

[0001] The present invention belongs to the technical field of autonomous navigation, simultaneous positioning and mapping, and in particular relates to a SLAM hardware accelerator architecture for a resource-constrained embedded system platform. Background Art

[0002] SLAM (Simultaneous Localization and Mapping) is one of the most critical technologies in the field of autonomous navigation. It enables unmanned systems to incrementally build maps of the surrounding environment in an unknown environment based on data captured by sensors, and provide real-time and accurate information about their own location, supporting the system to achieve autonomous operation and intelligent decision-making in complex and dynamic environments.

[0003] SLAM has a wide range of applications in autonomous driving, navigation robots, 3D reconstruction and other fields. Its core requirements include efficient data processing capabilities, real-time map building and positioning capabilities, and robustness in complex environments. SLAM algorithms usually need to process a large amount of sensor data, including camera images, lidar data, and inertial measurement unit (IMU) data. The high throughput and real-time processing requirements of these data make SLAM implementation on embedded platforms face severe challenges. Traditional CPUs often cannot meet the computationally intensive requirements of SLAM algorithms, resulting in performance bottlenecks and high power consumption.

[0004] GPU (Graphics Processing Unit) has thousands of computing cores, which can greatly improve the performance of SLAM system by parallelizing the processing of image data, feature extraction and matching and other computationally intensive tasks. However, GPU has several significant disadvantages in SLAM acceleration: first, GPU has high power consumption and is not suitable for embedded systems that are sensitive to power consumption; second, due to the complex architecture and long data transmission path of GPU, it is not suitable for use in environments with extremely high real-time requirements; in addition, although the parallel computing capability of GPU is powerful, its versatility is high, which leads to the inability to achieve performance optimization through flexible architecture design in specific applications.

[0005] FPGA (Field Programmable Gate Array) is a flexible programmable logic device with a large number of programmable logic units (such as logic gates, registers and DSP blocks), as well as programmable connectivity resources (such as on-chip interconnect networks). FPGA can realize various digital circuit functions by configuring and connecting these resources, thereby realizing user-defined hardware functions. Compared with GPU, FPGA has the following significant advantages: First, FPGA has a high degree of parallel processing capability and can process multiple data streams at the same time, which is particularly important for the large number of matrix operations and image processing tasks involved in SLAM algorithms; secondly, the programmability of FPGA allows customization and optimization according to specific application requirements, thereby reducing power consumption while maintaining high performance; in addition, FPGA excels in low-latency data processing and can meet the strict real-time requirements of SLAM systems.

[0006] The prior art with document number CN114253511A discloses a SLAM hardware acceleration based on laser radar, which includes a posture data storage device, a mileage data storage device, a laser data storage device and a map data storage device, a motion model module connected to the posture data storage device and the mileage data storage device respectively, obtains the preliminary posture of the particles at the current moment and superimposes Gaussian noise, a scan matching module connected to the motion model module, the laser data storage device and the map data storage device respectively, and calculates the particle scores of the laser data and the map data in combination with the preliminary calculated values ​​output by the motion model module, a particle data storage device connected to the scan matching module, a resampling module connected to the particle data storage device, accumulates the scores of the laser data and obtains the weights of the particles, and resamples according to the weights, and a posture data storage device for the particles after calculation connected to the resampling module.

[0007] The prior art with document number CN109919825A discloses an ORB-SLAM hardware accelerator, which includes an FPGA hardware acceleration module for accelerating feature extraction and feature matching; a sensor module for capturing images; and a processor system, which acts as a host to control the FPGA hardware acceleration module and the sensor module, and is responsible for running pose estimation, pose optimization, and map updates. It uses the FPGA hardware acceleration module to accelerate the most computationally intensive and time-consuming processes in the ORB-SLAM process, which can effectively increase the running speed of ORB-SLAM and reduce power consumption, greatly improve the energy efficiency ratio, and reduce the difficulty of deploying ORB-SLAM on power-constrained platforms.

[0008] In summary, no one in the existing technology has focused on proposing solutions for embedded system platforms with strictly limited resources such as computing power, storage, and energy. Summary of the invention

[0009] The technical problems to be solved by the present invention are:

[0010] The purpose of the present invention is to propose a SLAM hardware acceleration architecture suitable for resource-constrained environments, so as to significantly improve the performance of the SLAM system in terms of power consumption, latency, etc., and to achieve SLAM acceleration under resource constraints.

[0011] The technical solution adopted by the present invention to solve the above technical problems is:

[0012] A SLAM hardware acceleration architecture suitable for resource-constrained environments, the design process of the SLAM hardware acceleration architecture is:

[0013] Step 1: Synchronous binocular image acquisition based on FPGA

[0014] By utilizing the parallelism of the FPGA system, the left and right image data collected by the binocular camera are synchronously stored in the FIFO buffer. For each image, two BANK storage spaces are opened, and data management is performed using ping-pong operation. After the system is initialized, the first data of the first frame image is written to the first address of BANK1, and the output data is counted through the DRAM control module, and the data is allocated to the corresponding address space. When the counter reaches the maximum write address of the current BANK, it indicates that the storage of a frame of image has been completed. When each frame reset signal arrives, the system will switch to BANK2 for data storage. At this time, the pointer points to the first address of BANK2 and continues the storage process of the next frame of image data.

[0015] At the same time, the collected RGB images are preprocessed. First, the RGB images are converted into grayscale images. Then, the Gaussian filtering method is used to remove isolated noise points and enhance the smoothness and stability of the image. Next, the image distortion caused by the camera lens is corrected to ensure the accuracy and reliability of subsequent feature extraction and matching.

[0016] Step 2: Image feature extraction and matching

[0017] The specific implementation process of this step is as follows: first, the image received in step 1 is divided into blocks and processed in parallel using a multi-level buffer pipeline; the floating-point numbers in the input data and intermediate calculation process are converted into fixed-point format, and data overflow is avoided during the calculation process; the feature extraction step uses FPGA parallel calculation to simultaneously detect multiple pixel blocks with significant changes in the image and output the coordinate pairs of these key points; the feature description step generates a unique descriptor for each feature point; the feature matching step uses methods such as Hamming distance to match image features and identify corresponding feature points between frames;

[0018] Step 3: High-speed data communication between FPGA and CPU

[0019] After the SLAM feature matching is completed, the CPU sends the memory address information to the FPGA through the high-speed bus and starts data transmission; after the FPGA writes the matching result to the specified memory address through DMA, it updates the status to processing completed; after the CPU detects the status update signal, it reads the matching result from the memory and executes the following algorithm operations:

[0020] First, error calculation and optimization are performed, and RANSAC and other algorithms are used to eliminate mismatched feature points to ensure the reliability of the matching results. Then, the CPU performs pose estimation, calculates the pose transformation of the current frame relative to the map, and further optimizes the pose and the position of the map points through the BA optimization algorithm. Subsequently, the key frame management is updated to ensure that the system effectively selects and maintains key frames. After completing the above steps, the CPU again transmits the processed pose data, map data, and motion model information to the FPGA through the DMA high-speed bus, and the FPGA performs hardware acceleration processing of the extended Kalman filter (EKF) process.

[0021] Step 4: Extended Kalman filter accelerator

[0022] In the previous step, the initial state estimation, covariance matrix, system model parameters and sensor observation data of the filter have been loaded into the internal memory of the FPGA; a state prediction unit, a covariance prediction unit, a Kalman gain calculation unit, a state update unit and a covariance update unit are provided inside the FPGA, and the units can realize hardware acceleration of the core steps of the state prediction, covariance prediction, Kalman gain calculation and state update EKF algorithm through parallel computing;

[0023] The hardware architecture also includes multiple parallel computing modules for efficient matrix operations; the control unit coordinates the workflow of all computing units, manages data transmission, allocation of computing tasks and control of iterative processes, ensuring that the SLAM system runs stably and efficiently in real-time applications;

[0024] Step 5: Map update, storage and display

[0025] After EKF processing, FPGA transmits the updated pose and map information back to CPU; CPU updates the pose estimate of the current frame based on the information output by EKF; at the same time, SLAM system optimizes map accuracy by identifying repeated frames through loop detection algorithm; displays map and positioning information in real time through display device, and outputs updated map data in real time; CPU stores updated map information and key frame data in memory or disk for subsequent map optimization and path planning.

[0026] The present invention has the following beneficial technical effects:

[0027] The present invention is a hardware accelerator based on FPGA, which greatly improves the performance of the SLAM system in terms of power consumption, delay, etc. by transplanting the computationally intensive parts (such as feature extraction and matching, matrix operations, etc.) in the SLAM algorithm to the FPGA for parallel computing. The method of the present invention can be customized according to the specific application scenario, and provides an effective solution for SLAM acceleration under resource constraints. The present invention is aimed at an embedded system platform with strictly limited resources such as computing power, storage, and energy, and proposes a SLAM hardware accelerator architecture and design method based on FPGA, which provides an effective way for the deployment of SLAM algorithms on various types of micro-sized motion unmanned systems, and also provides practical guidance for the application of SLAM hardware acceleration methods in actual systems.

[0028] The present invention is aimed at the problem that autonomous exploration of robots in computing resource-constrained environments is difficult to meet real-time requirements. The present invention is based on a synchronous positioning and mapping architecture based on FPGA. The architecture uses the front-end and back-end dual acceleration ideas to improve the execution efficiency of the SLAM algorithm, and can be customized and optimized according to specific application requirements. It can provide method guidance for the design of binocular SLAM SOC chips. Compared with the existing technical solutions, the present invention has the following advantages:

[0029] (1) Aiming at the embedded system platform with strictly limited hardware and software resources, a method is proposed to significantly improve the frame rate of the SLAM algorithm without sacrificing positioning accuracy while consuming less power than a pure CPU platform.

[0030] (2) In order to further leverage the parallelization characteristics of the hardware accelerator and improve the performance of hardware acceleration, a front-end and back-end dual acceleration solution was designed. By introducing the SLAM back-end accelerator architecture, the core steps of EKF such as state prediction, covariance calculation, and state update are effectively parallelized, thereby further improving the performance of the SLAM algorithm on resource-constrained systems;

[0031] In addition, the present invention also uses customized hardware logic and a high-bandwidth DMA interface to exchange data between the FPGA and the CPU, which greatly improves the throughput of data transmission and storage and reduces data transmission delay, and has strong practical value. BRIEF DESCRIPTION OF THE DRAWINGS

[0032] The accompanying drawings are used to provide a further understanding of the present invention and constitute a part of the specification. Together with the embodiments of the present invention, they are used to explain the present invention and do not constitute a limitation of the present invention. In the accompanying drawings:

[0033] Figure 1 Design diagram for FPGA-based SLAM hardware architecture;

[0034] Figure 2 The hardware architecture design diagram for the Gaussian filter block;

[0035] Figure 3 Design diagram for the hardware architecture of the filtering and feature extraction modules;

[0036] Figure 4 Design diagram for describing the submodule hardware architecture;

[0037] Figure 5 Design diagram of the hardware architecture of the feature matching module;

[0038] Figure 6 This is the flow chart of high-speed communication between FPGA and CPU;

[0039] Figure 7 Design diagram for the extended Kalman filter state machine;

[0040] Figure 8 It is the system hardware logic configuration diagram;

[0041] Fig. 9 Build a diagram for the experimental platform;

[0042] Fig.10 Test the effect diagram for the dataset. DETAILED DESCRIPTION

[0043] Aiming at the problem that the existing SLAM algorithm is difficult to run in real time on a low-power platform, the present invention proposes a SLAM hardware accelerator to improve the overall performance-to-power ratio of the system and make low-power real-time operation possible. Figure 1 As shown, the hardware configuration involves modules such as parallel processing architecture, memory management, and driver development, and the algorithm optimization involves modules such as Gaussian filtering, feature point extraction, descriptor calculation, and feature point matching.

[0044] Next, by combining specific examples with the attached Figure 1-10 , the design method of the SLAM hardware acceleration architecture suitable for resource-constrained environments described in the present invention is further described in detail:

[0045] Step 1: Synchronous binocular image acquisition based on FPGA

[0046] Taking advantage of the parallelism of the FPGA system, the left and right image data collected by the binocular camera are synchronously stored in the FIFO (First In First Out) buffer. For each image, two BANK storage spaces are opened, and ping-pong operation is used for data management. After the system is initialized, the first data of the first frame image is written to the first address of BANK1, and the output data is counted through the DRAM control module, and the data is allocated to the corresponding address space. When the counter reaches the maximum write address of the current BANK, it indicates that the storage of a frame of image has been completed. Every time the frame reset signal arrives, the system will switch to BANK2 for data storage. At this time, the pointer points to the first address of BANK2 and continues the storage process of the next frame of image data.

[0047] At the same time, the system preprocesses the collected RGB images. First, the RGB images are converted into grayscale images; then, the Gaussian filtering method is used to remove isolated noise points and enhance the smoothness and stability of the image; next, the image distortion caused by the camera lens is corrected to ensure the accuracy and reliability of subsequent feature extraction and matching.

[0048] Step 2: Image feature extraction and matching

[0049] The specific implementation process of this step is as follows: First, the image received in step 1 is divided into blocks and processed in parallel using a multi-level buffer pipeline. In order to improve computing efficiency and take into account accuracy, it is necessary to convert the input data and floating-point numbers in the intermediate operation process into fixed-point format, and avoid data overflow during the calculation process. The feature extraction step takes advantage of FPGA parallel computing to simultaneously detect multiple pixel blocks with significant changes in the image and output the coordinate pairs of these key points; the feature description step generates a unique descriptor for each feature point; the feature matching step uses methods such as Hamming distance to match image features and identify corresponding feature points between frames.

[0050] Step 3: High-speed data communication between FPGA and CPU

[0051] After the SLAM feature matching is completed, the CPU sends the memory address information to the FPGA through the high-speed bus and starts the data transmission. After the FPGA writes the matching results to the specified memory address through DMA, the status is updated to complete the processing. After the CPU detects the status update signal, it reads the matching results from the memory and performs the following algorithm operations: First, the error calculation and optimization are performed, and the RANSAC and other algorithms are used to eliminate the mismatched feature points to ensure the reliability of the matching results. Next, the CPU performs pose estimation, calculates the pose transformation of the current frame relative to the map, and further optimizes the pose and the position of the map points through the BA optimization algorithm. Subsequently, the key frame management is updated to ensure that the system effectively selects and maintains the key frames. After these steps are completed, the CPU again transmits the processed pose data, map data, motion model and other information to the FPGA through the DMA high-speed bus, and the FPGA performs hardware acceleration processing of the extended Kalman filter (EKF) process.

[0052] Step 4: Extended Kalman filter accelerator

[0053] In the previous step, the initial state estimation of the filter, the covariance matrix, the system model parameters and the sensor observation data have been loaded into the internal memory of the FPGA. The FPGA is equipped with a state prediction unit, a covariance prediction unit, a Kalman gain calculation unit, a state update unit and a covariance update unit, which can achieve hardware acceleration of the core steps of the EKF algorithm such as state prediction, covariance prediction, Kalman gain calculation and state update through parallel computing. In addition, the hardware architecture also includes multiple parallel computing modules for efficient matrix operations; the control unit coordinates the workflow of all computing units, manages data transmission, the allocation of computing tasks and the control of iterative processes, and ensures that the SLAM system runs stably and efficiently in real-time applications.

[0054] Step 5: Map update, storage and display

[0055] After EKF processing, the FPGA transmits the updated pose and map information back to the CPU. The CPU updates the pose estimate of the current frame based on the information output by the EKF. At the same time, the SLAM system optimizes map accuracy by executing a loop detection algorithm to identify repeated frames. The system displays map and positioning information in real time through a display device and outputs updated map data in real time. The CPU stores the updated map information and key frame data in memory or disk for subsequent map optimization and path planning.

[0056] The implementation of the above steps is further explained as follows:

[0057] Step 1: FPGA-based image acquisition and preprocessing system

[0058] In the FPGA, a row-sized first-in-first-out (FIFO) storage area is implemented for image data processing. When the area is read and empty, new valid data is allowed to be written. Once all the data in the row is written, the read state machine is triggered to write the data to the SDRAM until the area is empty again. Then the burst stop command and precharge command are sent, and finally the refresh command is sent to maintain the SDRAM data integrity.

[0059] During the image acquisition process, only the odd-numbered lines and fields are acquired to avoid falling into the field blanking period. Ensure that the acquisition starts after a certain time lag of the field synchronization signal and stops after a certain number of lines are acquired. Start acquiring line data after a certain time lag of the line synchronization signal, and convert the input clock to a clock frequency suitable for acquisition through a frequency division counter. This ensures that when the input image is 640×480 pixels, complete and valid data is acquired to avoid data falling into the blanking period.

[0060] The system stores the converted 30-bit RGB data into SDRAM. To speed up the storage, SDRAM is virtualized into four data ports (two read ports and two write ports) to work with four banks simultaneously. The first bank stores part of the R and G data, and the second bank stores the rest of the G and B data, ensuring that data is accessed on demand and improving efficiency. Then the RGB image is grayed out, and the formula is:

[0061] Y=0.299R+0.587G+0.144B (1)

[0062] Y=((77R+150G+29B)>>8) (2)

[0063] To reduce noise interference, the image is Gaussian filtered. The hardware architecture of the Gaussian filter module is as follows: Figure 2 As shown, the formula is:

[0064]

[0065] In the image filter design, the BRAM module is used to cache the pixel data of the filter core rows, and the MIG_IP core and 128-bit wide FIFO are used to realize continuous data reading and processing. When writing, the data is first stored in the FIFO, and repeated multiple times to cache the entire frame of the image; when reading, the data is first obtained from the FIFO, and then passed to the subsequent image processing module to ensure efficient data processing and smooth operation.

[0066] To prevent image data from interleaving, the system uses two BANKs of DDR3 to cache the data read by the acquisition module. Through ping-pong operation, it switches between the two storage spaces to ensure that each frame of image data is stored and read in sequence. When the count reaches the maximum write address of the DDR3 BANK, it switches to another BANK to continue storage, ensuring the continuity and stability of the data flow.

[0067] Using pre-calibrated camera parameters, the system applies a lens distortion correction algorithm to correct image distortion caused by the camera lens. This correction step is critical to maintaining the accuracy and reliability of feature extraction and matching tasks.

[0068] Step 2: Image feature extraction and matching based on FPGA

[0069] The architecture design of the feature extraction module is as follows Figure 3 As shown. Since multiple lines of image operations are involved, a 5-line line buffer is required to temporarily store image data. The line buffer is implemented using BRAM, which acts as a sequential circuit to obtain data through addressing. In order to achieve parallel operation, a 5×5 image block composed of triggers is designed to ensure that the system can process multiple pixel values ​​at the same time. The grayscale comparison module is responsible for calculating the difference between the pixels in the image block and comparing it with the preset threshold. The result outputs two sets of logic value data, which respectively indicate whether a specific condition is met. The consistency detection module performs consistency detection on the pixels that meet the conditions in parallel. The two detection modules output the results at the same time, and the final output is obtained through an OR operation.

[0070] In FPGA, feature point extraction needs to be stored in a row buffer. Since the RAM on the FPGA board is shared by multiple modules, it cannot be guaranteed that the feature point data can be written to the RAM at any time, so it must be temporarily stored in a row buffer. By adopting a double buffer mechanism, one row buffer writes to the RAM while the other buffer receives the feature point data to ensure that there is sufficient margin for the AXI HP bus transmission speed and the RAM write speed. When the row buffer data is written to the RAM, the multi-row buffer space waits for the CPU on the PS side to transmit it to the CPU processing board via Ethernet. The CPU uses its high performance to parse the feature point data into the specified format and integrate it into the visual inertial odometer.

[0071] In ORB-SLAM, the image pyramid is constructed using an 8-layer pyramid with a scaling factor of 1.2. The implementation method uses the resize function provided by the OpenCV library to complete non-integer image scaling through bilinear interpolation. However, since the logic resources of embedded SoCs are generally limited, complex non-integer scaling cannot be supported. In order to save logic resources, this system adopts an integer image scaling strategy, specifically a 3-layer image pyramid with a scaling factor of 2 for each layer, and resolutions of 752×480, 376×240, and 188×120, respectively.

[0072] The image pyramid layering module is mainly composed of row and column counters, pixel merging modules and pixel buffer rows. The row and column counters are used to guide the pixel merging process. The pixel merging module includes two parts: adjacent pixel merging and inter-row same-column pixel merging. The adjacent pixel merging module is responsible for adding adjacent pixels and outputting the result; the inter-row same-column pixel merging module extracts the same-column pixels from the pixel buffer row according to the number of columns of the current row of pixels, performs addition operations, and outputs the merged pixel values. The pixel buffer row module is responsible for storing the pixel values ​​of the previous row. When a new pixel is transmitted from the camera, the pixel pulse triggers the 10-bit column counter to increment, and the lowest bit signal is sent to the adjacent pixel merging module to control the storage or merging operation of the pixel. The merged pixel data is stored in the first-layer buffer row and input into the inter-row pixel merging module. The inter-row pixel merging module extracts the previous row of pixels of the same column from the buffer row according to the lowest bit signal of the row counter, merges them, and outputs 10-bit width pixel data to complete the 2x scaling process. Next, based on the same principle, the second-layer image pixels are used as input to continue to complete the pixel scaling of the third-layer image. In the process from the second layer to the third layer, the row and column counter uses the previous bit of the count value as the control signal of the pixel merging module.

[0073] In order to improve the quality of feature points, the present invention introduces a non-maximum suppression module in the feature point extraction process. This module is used to eliminate multiple FAST corner points with smaller responses in local image blocks, retaining only points with the most significant features. The non-maximum suppression module shares the same pixel buffer with the FAST corner point detection module, effectively saving FPGA resources. First, the response value calculation module calculates the response values ​​of 16 representative pixel points in the 7×7 image block, stores these 12-bit response values ​​in the row buffer, and stores the temporary calculated values ​​in the 3×3 response value block. Finally, the response value comparison module compares the response value of the center point of the 3×3 response value block with the surrounding 8 response values, and outputs the final result.

[0074] The hardware architecture of the feature description module is as follows Figure 4As shown. The pixel buffer is configured with 31 rows of pixel row buffers, and the row buffer data is synchronized to the 31×31 image block and the 7×7 image block. The point pair relationship calculation module calculates the grayscale numerical relationship of the point pairs in the 31×31 image block and outputs 256 bits of description information. The centroid angle calculation module calculates the centroid position in the 7×7 image block and resolves it into 8 bits of centroid angle information. The descriptor shift and rotation module shifts the 256 bits of description information according to the centroid angle information to achieve the de-orientation operation of the descriptor and finally outputs it. The two main operations of the ORB feature point extraction process are the BRIEF (Binary Robust Independent Elementary Features) description process and the BRIEF rotation process. The description process records the features in the pixel blocks around the feature point through a data sequence of a certain length.

[0075] The hardware block diagram of the feature matching submodule is as follows: Figure 5 As shown. For two images, after using the ORB feature point detection and descriptor generation algorithm, the detected feature point descriptors are generated. Assuming that b1 and b2 are the descriptors of a feature point of the two images, they are matched by calculating their Hamming distance D:

[0076]

[0077] The smaller the value of D, the higher the similarity of the two feature points. If D meets the set Hamming distance, the two feature points are matched successfully, and the coordinates of the corresponding feature points of the two images are obtained. XOR calculation is very easy to implement in FPGA, which further speeds up the feature matching algorithm.

[0078] Step 3: High-speed communication between FPGA and CPU

[0079] The process of step three is as follows Figure 6 As shown in the figure, the CPU prepares the raw image data to be processed and stores it in the system's DRAM. In DRAM, specific memory address spaces are allocated for the raw data and processing results, and access rights to the memory areas are configured to ensure that the FPGA and CPU can correctly access these areas.

[0080] The CPU configures the DMA controller, specifies the original data address and target data address of the data, and sets the data block size, transmission length, and method. The CPU writes control commands to the instruction register of the FPGA through the DMA controller, notifying the FPGA to start the hardware acceleration module. These control commands include the original data address, target data address, and related configuration information to ensure that the FPGA can process data correctly. After receiving these instructions, the registers inside the FPGA will start to execute the corresponding acceleration tasks.

[0081] After starting the FPGA accelerator, the CPU will enter a loop to periodically query the FPGA status register to check the progress of data processing. This query is usually implemented by reading specific registers of the FPGA. At the same time, the FPGA hardware acceleration module initiates a memory access request to the CPU through the DMA controller. This request contains the original data address to be read and the target data address to be written.

[0082] When the CPU receives a memory access request from the FPGA, it passes the request to the memory controller. The memory controller is responsible for performing actual memory read and write operations based on the request, reading data from the DRAM and transmitting it to the FPGA. After obtaining the raw data, the FPGA uses its hardware acceleration module to perform high-speed computing. After the processing is completed, the result data is written back to the predetermined DRAM address through the DMA controller.

[0083] After the FPGA completes data processing, it updates its status register to indicate that the processing is complete. When the CPU is looping to query the status, it detects the processing completion flag in the FPGA status register and jumps out of the query loop. After jumping out of the query loop, the CPU reads the result data in the DRAM for subsequent processing or application. After the entire process is completed, the CPU can continue to perform other tasks, and the FPGA can also enter the standby state while waiting for new instructions.

[0084] Step 4: Expand the Kalman filter module

[0085] In this design, FPGA implements the hardware architecture of the extended Kalman filter algorithm. The architecture includes a state prediction unit, a covariance prediction unit, a Kalman gain calculation unit, a state update unit, and a covariance update unit. The initial state estimate, the initial covariance matrix, the system model parameters, and the sensor observation data are loaded into the FPGA internal memory for efficient processing.

[0086] The state prediction unit predicts the state value at the current moment by using the state estimation value at the previous moment and the state transfer matrix of the system model. The specific formula is as follows:

[0087] X(k|k-1)=A×X(k-1|k-1) (5)

[0088] FPGA processes the prediction calculations of multiple state variables in parallel and optimizes the hardware structure using pipeline design to increase the prediction speed. The final output state prediction value will be passed as input to the covariance prediction unit and the state update unit.

[0089] The covariance prediction unit predicts the error covariance matrix at the current moment based on the error covariance matrix at the previous moment and the state transition matrix of the system model. Its operation process includes using BRAM or FIFO to cache input data for fast access, calculating the product of the state transition matrix, multiplying the above product with the error covariance matrix at the previous moment to obtain an intermediate result, and adding the covariance matrix of the system process noise to the intermediate result to obtain the error covariance prediction value. The specific formula is as follows:

[0090] P(k|k-1)=A×P(k-1|k-1)×A T +Q (6)

[0091] The Kalman gain calculation unit calculates the Kalman gain based on the observation data, the observation matrix and the prediction error covariance matrix. The calculation steps include performing matrix multiplication, adding the result to the observation noise covariance matrix, and finding the inverse matrix through an iterative algorithm or a lookup table. The specific formula is as follows:

[0092] K(k)=P(k|k-1)×H T ×(H×P(k|k-1)×H T +R) -1 (7)

[0093] The state update unit combines the predicted state value, observation data and Kalman gain to update the state estimate. Its operation process includes calculating the residual:

[0094] e(k)=Z(k)-H×X(k|k-1) (8)

[0095] Calculate the state update using the Kalman gain and the residual:

[0096] ΔX(k)=K(k)ge(k) (9)

[0097] Update the state estimate:

[0098] X(k)=X(k|k-1)+ΔX(k) (10)

[0099] The covariance update unit updates the error covariance matrix based on the Kalman gain and the prediction error covariance matrix. Its operation process includes using matrix multiplication and addition operations to update the error covariance matrix for prediction and update at the next moment. The specific formula is as follows:

[0100] P(k|k)=(IK(k)×H)×P(k|k-1) (11)

[0101] Finally, the updated state estimate is output to an external device or system through an appropriate data interface for subsequent control and decision-making tasks. Through the above method, efficient and stable matrix operations and filtering processing are achieved, ensuring the reliability and real-time performance of the system.

[0102] In addition, this design uses a finite state machine (FSM) to represent the finite number of states in the system and constrain the transitions and behaviors between these states. The state transitions and behaviors of the FSM depend on the current state and external input. Since the logical solution process of the EKF algorithm needs to be divided into multiple steps, and each step will be further split during implementation, this increases the complexity of the logic. Therefore, it is necessary to design a reasonable FSM to control the transfer between each operation step to prevent logical confusion. According to the logical relationship between each operation step, the FSM can be divided into eight states. Starting from the initial state, the specific operation implementation of each state is as follows:

[0103] S0: Assign initial values ​​to each related variable and enter S1 state after the assignment is completed.

[0104] S1: Calculate the product of the state Jacobian matrix and the error covariance matrix of the previous moment, store the result in the temporary register and enter the S2 state.

[0105] S2: Calculate the product of the output value of the S1 state and the transpose of the state Jacobian matrix, and sum the operation result with the process noise covariance matrix to obtain the prior error covariance matrix. After the calculation is completed, enter the S3 state.

[0106] S3: Calculate the product of the prior error covariance matrix and the transpose of the measurement Jacobian matrix, store the result in a temporary register and enter S4 state.

[0107] S4: Calculate the product of the measurement Jacobian matrix and the output value of the S3 state, sum the operation result with the measurement noise covariance matrix, send it to the matrix inversion module, and enter the S5 state at the same time.

[0108] S5: Calculate the product of the output value of the S3 state and the output value of the matrix inversion module to obtain the Kalman gain. After the calculation is completed, enter the S6 state.

[0109] S6: Update the optimal state estimate and the error covariance matrix, send the result to the state update module, and enter the S7 state.

[0110] S7: After the waiting state update module completes the calculation, it enters the S0 state and repeats the above process using the calculation result as the initial value.

[0111] Through the above FSM, the operation logic of the EKF algorithm can be effectively constrained, the operation process can be shortened, and unnecessary repeated operations can be avoided, thereby improving the real-time performance of the operation and the stability of the system.

[0112] Step 5: Map update and display

[0113] After processing by the extended Kalman filter (EKF), the FPGA transmits the updated pose and map information back to the central processing unit (CPU). The CPU updates the pose estimate of the current frame based on the information output by the EKF. The pose of each camera frame is used as a node, and the relative transformation between frames is used as an edge to form a pose graph. The feature points detected in the new keyframe are matched with the existing map points, and the unmatched feature points are inserted into the map as new map points. Each time a new frame is added, the local subgraph related to the current frame is optimized.

[0114] When a new keyframe is added, the bag-of-words model is used to find keyframes similar to the current frame as loop candidates. The loop candidate frames are geometrically verified by matching 3D points and features and calculating the relative pose transformation to verify the validity of the loop candidate. Once the loop is closed, new constraint edges are added to the pose graph. All constraint edges (inter-frame constraints and loop closure constraints) are used to perform bundle adjustment (Bundle Adjustment) of the global pose graph to minimize the overall reprojection error, significantly reduce the cumulative error, and improve the accuracy of the map and pose estimation.

[0115] The computing power allocation module configures the FPGA logic circuit in real time according to the solution, adjusts the computing pipeline structure, and increases or decreases the number of pipeline and matrix acceleration modules. When the platform is in a static state, the FPGA is turned off to save power. Considering that the configuration process is short-lived, the module will incrementally update the calculation process and dynamically load the computing pipeline / matrix acceleration circuit configured in the current clock cycle into the calculation process immediately to avoid waiting time and achieve zero-delay dynamic reconfiguration.

[0116] Adaptive dynamic adjustment of computing power: Optimal performance is achieved by designing an energy efficiency ratio function. By designing a power consumption evaluation index function, the FPGA can adaptively adjust the logic circuit structure and scale according to the computing power consumption estimation index, so that the computing power of the logic circuit is dynamically balanced with the navigation computing power requirements in the current environment, achieving the optimal energy consumption ratio.

[0117] The hardware description language code (such as Verilog HDL or VHDL code) is synthesized into a logic circuit and mapped to the physical resources of the FPGA. After the embedded system is powered on, the bootloader runs first. During the boot process of the bootloader, the bitstream file is loaded from a predefined storage location (such as an SD card, eMMC, or flash memory) and written to the FPGA. After the FPGA configuration is completed, the bootloader continues to load the Linux operating system, read the device tree file, and identify the FPGA configuration and hardware layout. After the Linux operating system is started, the driver module is loaded through the insmod command. At this point, the FPGA can work with the ARM processor to perform tasks such as SLAM acceleration.

[0118] Through the above steps and methods, efficient real-time map updating and display, reliable pose estimation and map optimization are achieved, ensuring the reliability and real-time performance of the system while optimizing the energy consumption and performance of the system.

[0119] In order to verify the effectiveness of the proposed embedded SLAM hardware acceleration method and test the function of the constructed FPGA-CPU hardware framework, this solution uses the following method for testing and verification:

[0120] The experimental platform uses the ZYNQ-7020 as the core chip of the ZedBoard development board. The ZYNQ chip is mainly composed of CPU, FPGA and AXI bus. The CPU is a dual Cortex-A9 core ARM and the memory model is DDR3 SDRAM. The experimental platform is built as follows Figure 8 Based on the Vivado development environment, the ORB-SLAM2 algorithm is used to run the KITTI data set. Functions for measuring system time are added at the beginning and end of the main function. The running results are shown in the figure below. Fig. 9 As shown, when running 271 frames of binocular images, the running time is 36.224s in a pure CPU environment and 18.2857s in an FPGA acceleration environment. The SLAM frame rate is increased by 98.1%, and an average of 14.8 frames of images are run per second, thereby verifying the effectiveness of the present invention.

Claims

1. A SLAM hardware acceleration architecture suitable for resource-constrained environments, characterized in that: The design process of the SLAM hardware acceleration architecture is as follows: Step 1: Synchronous binocular image acquisition based on FPGA By utilizing the parallelism of the FPGA system, the left and right image data collected by the binocular camera are synchronously stored in the FIFO buffer. For each channel of images, two BANK storage spaces are opened, and data management is performed using ping-pong operation. After the system is initialized, the first data of the first frame of the image is written to the first address of BANK1, and the output data is counted through the DRAM control module, and the data is allocated to the corresponding address space. When the counter reaches the maximum write address of the current BANK, it indicates that the storage of a frame of the image has been completed. Each time the frame reset signal arrives, the system will switch to BANK2 for data storage. At this time, the pointer points to the first address of BANK2 and continues the storage process of the next frame of image data. At the same time, the collected RGB images are preprocessed. First, the RGB images are converted into grayscale images. Then, the Gaussian filtering method is used to remove isolated noise points and enhance the smoothness and stability of the image. Next, the image distortion caused by the camera lens is corrected to ensure the accuracy and reliability of subsequent feature extraction and matching. Step 2: Image feature extraction and matching The specific implementation process of this step is as follows: first, the image received in step 1 is divided into blocks and processed in parallel using a multi-level buffer pipeline; the floating-point numbers in the input data and intermediate calculation process are converted into fixed-point format, and data overflow is avoided during the calculation process; the feature extraction step uses FPGA parallel calculation to simultaneously detect multiple pixel blocks with significant changes in the image and output the coordinate pairs of these key points; the feature description step generates a unique descriptor for each feature point; The feature matching step uses methods such as Hamming distance to match image features and identify corresponding feature points between frames; Step 3: High-speed data communication between FPGA and CPU After the SLAM feature matching is completed, the CPU sends the memory address information to the FPGA through the high-speed bus and starts data transmission; after the FPGA writes the matching result to the specified memory address through DMA, it updates the status to processing completed; after the CPU detects the status update signal, it reads the matching result from the memory and executes the following algorithm operations: First, error calculation and optimization are performed, and RANSAC and other algorithms are used to eliminate mismatched feature points to ensure the reliability of the matching results. Then, the CPU performs pose estimation, calculates the pose transformation of the current frame relative to the map, and further optimizes the pose and the position of the map points through the BA optimization algorithm. Subsequently, the key frame management is updated to ensure that the system effectively selects and maintains key frames. After completing the above steps, the CPU again transmits the processed pose data, map data, and motion model information to the FPGA through the DMA high-speed bus, and the FPGA performs hardware acceleration processing of the extended Kalman filter (EKF) process. Step 4: Extended Kalman filter accelerator In the previous step, the initial state estimation, covariance matrix, system model parameters and sensor observation data of the filter have been loaded into the internal memory of the FPGA; a state prediction unit, a covariance prediction unit, a Kalman gain calculation unit, a state update unit and a covariance update unit are provided inside the FPGA, and the units can realize hardware acceleration of the core steps of the state prediction, covariance prediction, Kalman gain calculation and state update EKF algorithm through parallel computing; The hardware architecture also includes multiple parallel computing modules for efficient matrix operations. The control unit coordinates the workflow of all computing units, manages data transmission, allocation of computing tasks, and control of iterative processes, ensuring that the SLAM system runs stably and efficiently in real-time applications. Step 5: Map update, storage and display After EKF processing, the FPGA transmits the updated pose and map information back to the CPU; the CPU updates the pose estimate of the current frame based on the information output by the EKF; at the same time, the SLAM system identifies repeated frames by executing the loop detection algorithm to optimize the map accuracy; the map and positioning information are displayed in real time through the display device, and the updated map data is output in real time; The CPU stores the updated map information and key frame data in memory or disk for subsequent map optimization and path planning.

2. A SLAM hardware acceleration architecture suitable for resource-constrained environments according to claim 1, characterized in that: The grayscale processing of RGB image is as follows: Y=0.299R+0.587G+0.144B (1) Y=((77R+150G+29B)>>8) (2) In order to reduce noise interference, the image is subjected to Gaussian filtering, and the formula is: In the image filter design, the BRAM module is used to cache the pixel data of the filter core rows, and the MIG_IP core and 128-bit wide FIFO are used to realize continuous data reading and processing. When writing, the data is first stored in the FIFO, and repeated multiple times to cache the entire frame of the image; when reading, the data is first obtained from the FIFO and then passed to the subsequent image processing module to ensure efficient data processing and smooth operation. The data read by the acquisition module is cached using two BANKs of DDR3 to prevent image data from being interleaved. Ping-pong operation is used to switch between the two storage spaces to ensure that each frame of image data is stored and read in sequence. When the count reaches the maximum write address of the DDR3 BANK, it switches to another BANK to continue storage, ensuring the continuity and stability of the data flow.

3. A SLAM hardware acceleration architecture suitable for resource-constrained environments according to claim 2, characterized in that: The specific implementation process of step 2 is: A 5×5 image block composed of triggers is designed to ensure that the system can process multiple pixel values ​​at the same time. The grayscale comparison module is responsible for calculating the difference between the pixels in the image block and comparing it with the preset threshold. The result outputs two sets of logic value data, which respectively indicate whether a specific condition is met. The coherence detection module performs coherence detection on the pixels that meet the conditions in parallel. The two detection modules output the results at the same time and obtain the final output through an OR operation. A double buffer mechanism is adopted. While one line buffer is writing to RAM, another buffer receives feature point data to ensure sufficient margin for the AXI HP bus transmission speed and RAM writing speed. After the line buffer data is written to RAM, multiple line buffer spaces wait for the CPU on the PS side to transmit to the CPU processing board via Ethernet. The CPU uses its high performance to parse the feature point data into the specified format and integrate it into the visual inertial odometer. In ORB-SLAM, the image pyramid is constructed using an 8-layer pyramid with a scaling factor of 1.

2. With the help of the resize function provided by the OpenCV library, non-integer multiple image scaling is completed through bilinear interpolation. An integer multiple image scaling strategy is adopted, specifically a 3-layer image pyramid, each with a scaling factor of 2, and resolutions of 752×480, 376×240, and 188×120 respectively. The construction of the image pyramid is mainly composed of row and column counters, pixel merging modules and pixel buffer rows. The row and column counters are used to guide the process of pixel merging. The pixel merging module includes two parts: adjacent pixel merging and inter-row and same-column pixel merging. The adjacent pixel merging module is responsible for adding adjacent pixels and outputting the result. The row-to-row pixel merging module extracts pixels in the same column from the pixel buffer row according to the number of columns of the current row of pixels, performs an addition operation, and outputs the merged pixel value; The pixel buffer row module is responsible for storing the pixel values ​​of the previous row. When a new pixel is transmitted from the camera, the pixel pulse triggers the 10-bit column counter to increment. The lowest bit signal is sent to the adjacent pixel merging module to control the storage or merging operation of the pixel. The merged pixel data is stored in the first layer buffer row and input into the inter-row pixel merging module. The inter-row pixel merging module extracts the previous row of pixels in the same column from the buffer row according to the lowest bit signal of the row counter, merges them, and outputs 10-bit wide pixel data to complete the 2x scaling process; next, based on the same principle, the pixels of the second layer image are used as input to continue to complete the pixel scaling of the third layer image; in the process from the second layer to the third layer, the row and column counter uses the previous bit of the count value as the control signal of the pixel merging module.

4. A SLAM hardware acceleration architecture suitable for resource-constrained environments according to claim 3, characterized in that: The specific implementation process of step 2 is: A non-maximum suppression module is introduced in the feature point extraction process. This module is used to eliminate multiple FAST corner points with small responses in the local image block and retain only the points with the most significant features. The non-maximum suppression module and the FAST corner point detection module share the same pixel buffer to effectively save FPGA resources. First, the response value calculation module calculates the response values ​​of 16 representative pixel points in the 7×7 image block, stores these 12-bit response values ​​in the row buffer, and stores the temporary calculated values ​​in the 3×3 response value block. Finally, the response value comparison module compares the response value of the center point of the 3×3 response value block with the surrounding 8 response values ​​and outputs the final result. The pixel buffer is configured with 31 rows of pixel row buffers, and the row buffer data is synchronized to the 31×31 image block and the 7×7 image block; the point pair relationship calculation module calculates the grayscale value relationship of the point pairs in the 31×31 image block and outputs 256 bits of description information; The centroid angle calculation module calculates the centroid position in the 7×7 image block and resolves it into 8-bit centroid angle information; The descriptor shift and rotation module performs a shift operation on the 256-bit description information according to the centroid angle information to achieve the descriptor orientation operation and finally output; After using the ORB feature point detection and descriptor generation algorithm, the detected feature point descriptors are generated. Assuming that b1 and b2 are the descriptors of a feature point in the two images, they are matched by calculating their Hamming Distance D: The smaller the value of D is, the higher the similarity between the two feature points is; if D satisfies the set Hamming distance, the two feature points are matched successfully, thereby obtaining the coordinates of the corresponding feature points of the two images.

5. A SLAM hardware acceleration architecture suitable for resource-constrained environments according to claim 4, characterized in that: The specific implementation process of step three is: The CPU prepares the raw image data to be processed and stores it in the system's DRAM. It allocates specific memory address space in the DRAM for the raw data and processing results, and configures access permissions for the memory area to ensure that the FPGA and CPU can correctly access the memory area. The CPU configures the DMA controller, specifies the original data address and target data address of the data, sets the data block size, the length and mode of transmission; the CPU writes the control command to the instruction register of the FPGA through the DMA controller, notifying the FPGA to start the hardware acceleration module; these control commands include the original data address, the target data address and related configuration information to ensure that the FPGA can process the data correctly; after receiving these instructions, the register inside the FPGA will start to execute the corresponding acceleration task; After starting the FPGA accelerator, the CPU will enter a loop to periodically query the FPGA status register to check the progress of data processing; this query is usually implemented by reading specific registers of the FPGA; at the same time, the FPGA hardware acceleration module initiates a memory access request to the CPU through the DMA controller; the request contains the original data address to be read and the target data address to be written; When the CPU receives a memory access request from the FPGA, it passes the request to the memory controller. The memory controller is responsible for performing actual memory read and write operations based on the request, reading data from the DRAM and transmitting it to the FPGA. After obtaining the original data, the FPGA uses its hardware acceleration module to perform high-speed computing processing. After the processing is completed, the result data is written back to the predetermined DRAM address through the DMA controller. When the FPGA has finished processing the data, it updates its status register to indicate that the processing has been completed; When the CPU is in a loop querying the status, it detects the processing completion flag of the FPGA status register and jumps out of the query loop; after jumping out of the query loop, the CPU will read the result data in the DRAM for subsequent processing or application; after the entire process is completed, the CPU can continue to execute other tasks, and the FPGA can also enter the standby state while waiting for new instructions.

6. A SLAM hardware acceleration architecture suitable for resource-constrained environments according to claim 5, characterized in that: The specific implementation process of step 4 is as follows: Step 4: Expand the Kalman filter module The hardware architecture of the extended Kalman filter algorithm implemented in FPGA includes a state prediction unit, a covariance prediction unit, a Kalman gain calculation unit, a state update unit, and a covariance update unit; the initial state estimate, the initial covariance matrix, the system model parameters, and the sensor observation data are loaded into the FPGA internal memory for efficient processing; The state prediction unit predicts the state value at the current moment by using the state estimation value at the previous moment and the state transfer matrix of the system model. The specific formula is as follows: X(k|k-1)=A×X(k-1|k-1) (5) FPGA processes the prediction calculations of multiple state variables in parallel and uses pipeline design to optimize the hardware structure to increase the prediction speed. The final output state prediction value will be passed as input to the covariance prediction unit and the state update unit; The covariance prediction unit predicts the error covariance matrix at the current moment based on the error covariance matrix at the previous moment and the state transfer matrix of the system model. Its operation process includes using BRAM or FIFO to cache input data for fast access, calculating the product of the state transfer matrix, multiplying the above product with the error covariance matrix at the previous moment to obtain an intermediate result, and adding the covariance matrix of the system process noise to the intermediate result to obtain the error covariance prediction value. The specific formula is as follows: P(k|k-1)=A×P(k-1|k-1)×A T +Q (6) The Kalman gain calculation unit calculates the Kalman gain based on the observation data, the observation matrix and the prediction error covariance matrix. The calculation steps include performing matrix multiplication, adding the result to the observation noise covariance matrix, and finding the inverse matrix through an iterative algorithm or a lookup table. The specific formula is as follows: K(k)=P(k|k-1)×H T ×(H×P(k|k-1)×H T +R) -1 (7) The state update unit combines the predicted state value, the observed data, and the Kalman gain to update the state estimate. Its operation process includes calculating the residual: e(k)=Z(k)-H×X(k|k-1) (8) Calculate the state update using the Kalman gain and the residual: ΔX(k)=K(k)ge(k) (9) Update the state estimate: X(k)=X(k|k-1)+ΔX(k) (10) The covariance update unit updates the error covariance matrix based on the Kalman gain and the prediction error covariance matrix. Its operation process includes using matrix multiplication and addition operations to update the error covariance matrix for prediction and update at the next moment. The specific formula is as follows: P(k|k)=(IK(k)×H)×P(k|k-1) (11) Finally, the updated state estimate is output to an external device or system through an appropriate data interface for use in subsequent control and decision-making tasks.

7. A SLAM hardware acceleration architecture suitable for resource-constrained environments according to claim 6, characterized in that: Finite state machine (FSM) is used to represent the finite states in the system, and the transitions and behaviors between these states are constrained; the state transitions and behaviors of FSM depend on the current state and external input; a reasonable FSM is designed to control the transitions between the operation steps to prevent logical confusion; according to the logical relationship between the operation steps, FSM can be divided into eight states. Starting from the initial state, the specific operation implementation of each state is as follows: S0: Assign initial values ​​to each related variable and enter S1 state after the assignment is completed; S1: Calculate the product of the state Jacobian matrix and the error covariance matrix of the previous moment, store the result in the temporary register and enter the S2 state; S2: Calculate the product of the output value of the S1 state and the transpose of the state Jacobian matrix, and sum the result with the process noise covariance matrix to obtain the prior error covariance matrix. After the calculation is completed, enter the S3 state; S3: Calculate the product of the prior error covariance matrix and the transpose of the measurement Jacobian matrix, store the result in the temporary register and enter the S4 state; S4: Calculate the product of the measurement Jacobian matrix and the output value of the S3 state, sum the operation result with the measurement noise covariance matrix, send it to the matrix inversion module, and enter the S5 state at the same time; S5: Calculate the product of the output value of the S3 state and the output value of the matrix inversion module to obtain the Kalman gain. After the calculation is completed, enter the S6 state; S6: Update the optimal state estimate and error covariance matrix, send the result to the state update module, and enter the S7 state; S7: After the waiting state update module completes the calculation, it enters the S0 state and repeats the above process using the calculation result as the initial value.

8. A SLAM hardware acceleration architecture suitable for resource-constrained environments according to claim 7, characterized in that: The specific implementation process of step five is: After the extended Kalman filter processing, the FPGA transmits the updated pose and map information back to the central processing unit CPU; the CPU updates the pose estimate of the current frame based on the information output by the EKF; the pose of each frame of the camera is used as a node, and the relative transformation between frames is used as an edge to form a pose graph; the feature points detected in the new key frame are matched with the existing map points, and the unmatched feature points are inserted into the map as new map points; Each time a new frame is added, the local subgraph related to the current frame is optimized; When a new keyframe is added, the bag-of-words model is used to find keyframes similar to the current frame as loop candidates; the loop candidate frames are geometrically verified by matching 3D points and features, calculating the relative pose transformation, and verifying the validity of the loop candidate; once the loop is closed, new constraint edges are added to the pose graph; all constraint edges are used to perform BA optimization of the global pose graph to minimize the overall reprojection error, so as to significantly reduce the cumulative error and improve the accuracy of the map and pose estimation.

9. A SLAM hardware acceleration architecture suitable for resource-constrained environments according to claim 8, characterized in that: The computing power allocation module configures the FPGA logic circuit in real time according to the plan, adjusts the computing pipeline structure, and increases or decreases the number of pipeline and matrix acceleration modules. When the platform is in a static state, the FPGA is turned off to save power consumption. As the configuration process is short-lived, the module will incrementally update the computing process and dynamically load the computing pipeline / matrix acceleration circuit configured in the current clock cycle into the computing process immediately to avoid waiting time and achieve zero-delay dynamic reconfiguration. Adaptive dynamic adjustment of computing power: Optimal performance is achieved by designing an energy efficiency ratio function. By designing a power consumption evaluation index function, the FPGA can adaptively adjust the logic circuit structure and scale according to the computing power consumption estimation index, so that the computing power of the logic circuit is dynamically balanced with the navigation computing power requirements in the current environment, achieving the optimal energy consumption ratio.

10. A SLAM hardware acceleration architecture suitable for resource-constrained environments according to claim 9, characterized in that: The hardware description language code is synthesized into a logic circuit and mapped to the physical resources of the FPGA. After the embedded system is powered on, the bootloader runs first. During the bootloader startup process, the bitstream file is loaded from a predefined storage location and written to the FPGA. After the FPGA configuration is completed, the bootloader continues to load the Linux operating system, read the device tree file, and identify the FPGA configuration and hardware layout. After the Linux operating system is started, the driver module is loaded through the insmod command. The FPGA can work with the ARM processor to perform tasks such as SLAM acceleration.

Citation Information

Patent Citations

  • A visual SLAM method based on ORB features

    CN109766758A

  • An ORB-SLAM hardware accelerator

    CN109919825A

  • Unmanned aerial vehicle visual SLAM method based on GPU acceleration

    CN111024078A

  • FPGA-based binocular image feature extraction, matching and depth calculation hardware acceleration method and system

    CN117953229A

  • Visual-inertial odometry implementation method and system

    WO2019157925A1

Cited By

  • Dynamic reconfigurable hardware acceleration method and accelerator for brain-like SLAM algorithm

    CN119919275A

  • Structured light 3D camera system based on FPGA heterogeneous calculation and measurement method

    CN121010493A