SLAM back-end optimization method based on sparse matrix decomposition and hardware accelerator architecture

Abstract

The invention relates to an SLAM back-end optimization method based on sparse matrix factorization and a hardware accelerator architecture, and the method comprises the steps: constructing a Hessian matrix through employing a binary sparse coding mechanism according to the actual observation coordinates of feature points on an image plane and the predicted coordinates of road sign points projected to a camera coordinate system; performing Schel elimination processing on the Hessian matrix based on binary sparse coding to obtain a linear equation set only containing a camera pose variable; and carrying out extended Kalman filtering on a linear equation set only containing a camera pose variable to realize incremental updating of a state pose and a road sign point coordinate. According to the method, the overall delay of SLAM rear-end optimization can be remarkably reduced, and the real-time requirement in a high-speed moving scene is met.

Description

Technical Field

[0001] This invention relates to the field of Simultaneous Localization and Mapping (SLAM) technology, and in particular to a SLAM backend optimization method and hardware accelerator architecture based on sparse matrix factorization. Background Technology

[0002] SLAM (Simultaneous Localization and Analysis) technology is a core foundational technology for achieving autonomous navigation and environmental perception in fields such as autonomous mobile robots, drones, augmented reality (AR), and autonomous driving. In SLAM systems, the backend optimization stage is primarily responsible for eliminating accumulated errors and ensuring global consistency. Bundle Adjustment (BA), as a mainstream backend optimization method, simultaneously optimizes camera pose and 3D landmark coordinates using nonlinear least squares methods, and is a crucial step determining the accuracy and robustness of the SLAM system.

[0003] However, the BA algorithm involves the calculation of large-scale Jacobian matrices and the solution of high-dimensional linear equations, resulting in extremely high computational complexity and memory consumption. On resource-constrained embedded platforms (such as mobile robots and micro-drones), traditional general-purpose processors (CPUs) struggle to meet real-time processing requirements, often leading to system response delays or even positioning loss. Therefore, hardware acceleration solutions based on FPGAs (Field-Programmable Gate Arrays) have become a current research hotspot due to their high parallelism and low power consumption.

[0004] Despite the progress made by existing FPGA accelerators, four key technical bottlenecks still exist in practical high-precision, large-scale SLAM applications:

[0005] Data precision and cumulative error issues: To save hardware resources, existing solutions generally use 32-bit fixed-point data format. Compared with double-precision floating-point operations, fixed-point numbers are prone to truncation and rounding errors when processing BA data with a large dynamic range, and these errors accumulate with iteration, failing to meet the requirements of high-precision scenarios.

[0006] Limited landmark processing capacity: Due to limitations in on-chip storage resources and computing unit architecture of FPGAs, existing accelerators can typically only process a limited number of landmarks (e.g., 200). In complex scenes with rich textures, the system is forced to perform aggressive feature point selection, resulting in the loss of environmental information and reducing the localization accuracy and robustness of the SLAM system.

[0007] Storage architecture and memory access bandwidth bottlenecks: The construction of the Hessian matrix in the BA algorithm involves frequent access to landmark points and pose data, and the access patterns are highly irregular. Existing solutions lack in-depth optimization for sparsity, resulting in huge off-chip memory (DDR) bandwidth consumption in large-scale scenarios, and computing units often stall while waiting for data.

[0008] Lack of system-level collaborative optimization: Existing designs focus on accelerating the BA algorithm itself, neglecting the collaboration with the SLAM front-end and back-end state management, and lacking an efficient incremental update mechanism, resulting in a low overall system energy efficiency ratio.

[0009] Therefore, to address the aforementioned issues of computational efficiency, storage bottlenecks, and accuracy, an end-to-end solution that integrates algorithm optimization and hardware acceleration, specifically designed for the SLAM backend, is needed. Summary of the Invention

[0010] The technical problem to be solved by the present invention is to provide a SLAM backend optimization method and hardware accelerator architecture based on sparse matrix factorization, which can significantly reduce the overall latency of SLAM backend optimization and meet the real-time requirements of high-speed mobile scenarios.

[0011] The technical solution adopted by this invention to solve its technical problem is: to provide a SLAM backend optimization method based on sparse matrix factorization, including the following steps:

[0012] Based on the actual observed coordinates of feature points in the image plane and the predicted coordinates of landmark points projected onto the camera coordinate system, a Hessian matrix is ​​constructed using a binary sparse coding mechanism. The binary sparse coding describes the observation topology of each landmark point within the sliding window, using two-bit binary encoding to represent the state of the two cameras at a given time within the sliding window. The Hessian matrix is ​​expressed as follows: ,in, For pose association blocks, For road sign point association blocks, This is a pose-to-landmark association block used to reflect the camera's observation relationship with the landmarks;

[0013] Based on the binary sparse coding, the Hessian matrix is ​​subjected to Schuler elimination to obtain a system of linear equations containing only camera pose variables.

[0014] Extended Kalman filtering is applied to the linear equations containing only camera pose variables to achieve incremental updates of state pose and landmark coordinates.

[0015] The Hessian matrix is ​​constructed using a binary sparse coding mechanism based on the actual observed coordinates of feature points on the image plane and the predicted coordinates of landmark points projected onto the camera coordinate system. Specifically, this includes:

[0016] The difference between the actual observed coordinates of the feature points in the image plane and the predicted coordinates of the landmark points projected onto the camera coordinate system is calculated to obtain the reprojection error.

[0017] A linear residual model is constructed using a first-order Taylor expansion for the reprojection error;

[0018] A binary sparse coding mechanism is used, and a two-level invalid operation elimination strategy is employed to project the linear residual model onto the Jacobian transpose space to construct a Hessian matrix.

[0019] The specific two-level invalid operation elimination strategy is as follows:

[0020] If the binary codes of both cameras corresponding to a certain state in the binary sparse coding are both 0, then when constructing the Hessian matrix, all related coordinate transformations and Jacobian matrix calculation processes for that state are skipped directly.

[0021] If the binary code of a certain camera corresponding to a certain state in the binary sparse coding is 0, the residual term and Jacobian term corresponding to that camera are directly set to zero when constructing the Hessian matrix.

[0022] When performing Schul elimination on the Hessian matrix based on the binary sparse coding to obtain a system of linear equations containing only camera pose variables, if the coding bit of the binary sparse coding is 0, the multiplication operation between the pose and the landmark block corresponding to that coding bit is skipped.

[0023] The process of applying an extended Kalman filter to the linear equations containing only camera pose variables to achieve incremental updates of state pose and landmark coordinates specifically includes:

[0024] Substitute the linear equations containing only camera pose variables into the extended Kalman filter update equation, and obtain the new state pose by recursion.

[0025] By utilizing the independence of the landmark equations after Schur elimination, the landmark equations are decomposed into multiple independent residual equations. The new state pose is then substituted into the landmark extended Kalman filter solver to obtain the new landmark coordinates.

[0026] The technical solution adopted by this invention to solve its technical problem is: to provide a hardware accelerator architecture applicable to the above-mentioned SLAM backend optimization method based on sparse matrix factorization, including:

[0027] The storage cache module is used to store Hessian matrix data;

[0028] The fully pipelined Hessian building block adopts a five-level deep pipeline architecture design for constructing Hessian matrices;

[0029] The Shur elimination module is used to synchronously perform marginalization updates of the Hessian matrix and residual vector;

[0030] A parallel extended Kalman filter solver array is used to receive the output results of the Schul elimination operation module and perform incremental updates through a state machine.

[0031] The storage cache module includes a pose-associated block symmetric compression storage unit, a landmark-associated block compression storage unit, and a pose-landmark-associated block sparse mask storage unit. The pose-associated block symmetric compression storage unit only latches the data in the main diagonal and upper triangular regions of the pose-associated block, and realizes full-area logical access through address mapping. The landmark-associated block compression storage unit only latches the data in the upper triangular region of the landmark-associated block. The pose-landmark-associated block sparse mask storage unit allocates storage according to the binary sparse encoding.

[0032] The fully pipelined Hessian construction module includes:

[0033] IMU coordinate system transformation and pre-computation stage, used to execute two logic paths in parallel. One path transforms landmarks in the world coordinate system to coordinates in the IMU coordinate system through coordinate difference and quaternion rotation circuits. The other path synchronously calculates the state rotation matrix. IMU points and their antisymmetric matrix And camera-world rotation matrix ;

[0034] Camera coordinate system projection and intermediate matrix calculation stage, used to calculate coordinates in the IMU coordinate system using extrinsic parameters. Transformed into camera coordinates And calculate the state rotation matrix according to the synchronization. and antisymmetric matrix Calculate the intermediate rotation terms;

[0035] The intermediate derivative calculation stage of the chain rule is used to incorporate the coordinate data of the observed feature points, combined with the coordinates under the camera coordinates. Calculate the partial derivatives of the residuals with respect to the coordinate points of the camera system. The partial derivative of the camera coordinates with respect to the state and pose ;

[0036] Jacobian matrix generating stage, used to utilize the partial derivatives of the residuals with respect to the camera coordinate points. The partial derivative of the camera coordinates with respect to the state and pose and the camera-world rotation matrix Perform combination operations to output the Jacobian matrix of reprojection error with respect to state pose. Jacobian matrix of road signs ;

[0037] The Hessian matrix accumulation stage is used to calculate the Jacobian matrix based on the state pose. Jacobian matrix of road signs Perform the outer product operation of the Jacobian matrix and accumulate the calculation result to the storage cache module.

[0038] The pipelined input of the fully pipelined Hessian construction module is coupled with sparse encoding / decoding and decision logic. Based on the bit characteristics of the binary sparse code, the sparse encoding / decoding and decision logic performs two-level dynamic gating at the hardware level, consistent with the software logic:

[0039] The first level of gating is as follows: The decision logic first checks the two-bit code combination representing the current state; if both bits are 0, the control logic sends a bypass signal to the IMU coordinate system transformation and pre-computation stage, directly skipping the transformation from the world coordinate system to the IMU coordinate system, and the pipeline does not experience data flipping.

[0040] The second level of gating is as follows: The decision logic checks the independent encoding bit of the currently processed camera. If the bit is 0, the control logic sends a transmission clock gating signal to the camera coordinate system projection and intermediate matrix calculation stage, the chain rule intermediate derivative calculation stage, the Jacobian matrix generation stage, and the Hessian matrix accumulation stage to block the clock of the coordinate transformation from the IMU coordinate system to the camera coordinate system, block the clock of the partial derivative calculation of the Jacobian matrix, and block the clock of the outer product operation of the Jacobian matrix.

[0041] The Shure elimination module includes:

[0042] A matrix inversion circuit is used to directly read the landmark association block and calculate the inverse matrix of the landmark association block in parallel.

[0043] The cascaded matrix multiplication array includes a first-level multiplication unit and a second-level parallel processing unit. The first-level multiplication unit multiplies the pose-landmark associative block with the inverse matrix of the associative block to obtain an intermediate variable. The second-level parallel processing unit includes a first-path processing unit and a second-path processing unit. The first-path processing unit multiplies the intermediate variable with the transpose of the pose-landmark associative block to obtain a first result. The second-path processing unit multiplies the intermediate variable with the landmark residual vector to obtain a second result.

[0044] The parallel update unit is used to send the first result and the second result back to the storage cache module through the subtractor array, respectively, and to complete the subtraction update of the pose association block and the pose residual vector in parallel.

[0045] The Shure elimination operation module is controlled by a binary sparse coding scheduler, which triggers the clock signal only when the coding bit indication of the pose and landmark block is valid.

[0046] The parallel extended Kalman filter solver array includes:

[0047] The state pose update circuit is used to receive the equivalent residual and Hessian block after Schur elimination, control it with the master state machine, perform incremental updates of state variables, and store the updated pose parameters back to the shared parameter register.

[0048] The landmark parallel update array consists of N independent landmark update processing units. Each processing unit is equipped with a local register file that can load the residual equation data of different landmarks in parallel, and simultaneously update the covariance matrix and correct the coordinates of multiple landmarks.

[0049] The technical solution adopted by the present invention to solve its technical problem is: to provide an electronic device, including a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor executes the computer program to implement the steps of the above-mentioned SLAM back-end optimization method based on sparse matrix decomposition.

[0050] The technical solution adopted by the present invention to solve its technical problem is: to provide a computer-readable storage medium on which a computer program is stored, wherein the computer program, when executed by a processor, implements the steps of the above-mentioned SLAM back-end optimization method based on sparse matrix decomposition.

[0051] Beneficial effects

[0052] By adopting the above-mentioned technical solutions, this invention has the following advantages and positive effects compared with the prior art: This invention accurately characterizes the observation topology through binary sparse coding, eliminates invalid operations in Hessian matrix construction and Shur elimination, significantly reduces redundant calculations, improves real-time performance, and is suitable for high-speed mobile scenarios; it compactly describes the observation state through binary sparse coding, and combines the sparse characteristics of the matrix with elimination dimensionality reduction to reduce storage occupation and adapt to resource-constrained scenarios such as embedded platforms, while also avoiding invalid interference and accumulated errors, improving the accuracy and stability of pose and landmark point estimation, and meeting the needs of high-precision and wide-range applications. Attached Figure Description

[0053] Figure 1 This is a flowchart of the SLAM backend optimization method based on sparse matrix factorization according to the first embodiment of the present invention;

[0054] Figure 2 This is a schematic diagram of the sparse structure of the Hessian matrix in this invention;

[0055] Figure 3 This is a schematic diagram of the Hessian matrix structure after Schuler elimination in this invention;

[0056] Figure 4 This is a schematic diagram of the observation topology of binary sparse coding within a sliding window;

[0057] Figure 5 This is a schematic diagram of binary sparse coding generation and mapping;

[0058] Figure 6 This is a schematic diagram of symmetric compressed storage of Hessian matrix sub-blocks;

[0059] Figure 7 This is a schematic diagram of the five-stage pipeline architecture of the Hessian building blocks;

[0060] Figure 8 This is the overall architecture diagram of the BA hardware accelerator proposed in the second embodiment of the present invention. Detailed Implementation

[0061] The present invention will be further illustrated below with reference to specific embodiments. It should be understood that these embodiments are for illustrative purposes only and are not intended to limit the scope of the invention. Furthermore, it should be understood that after reading the teachings of this invention, those skilled in the art can make various alterations or modifications to the invention, and these equivalent forms also fall within the scope defined by the appended claims.

[0062] The first embodiment of the present invention relates to a SLAM backend optimization method based on sparse matrix factorization, such as... Figure 1 As shown, the upper layer of this method borrows the idea of ​​optimization methods to marginalize the matrix through residual modeling and Schur elimination; the lower layer uses the EKF filtering method to achieve efficient incremental updates of state pose and landmark points. Specifically, it includes the following steps:

[0063] Step 1: Construct a Hessian matrix using a binary sparse coding mechanism based on the actual observed coordinates of feature points on the image plane and the predicted coordinates of landmark points projected onto the camera coordinate system.

[0064] This step first calculates the reprojection error, that is, calculates the actual observed coordinates of the feature points on the image plane. Predicted coordinates of landmark points projected onto the camera coordinate system The difference between them, that is: ;

[0065] To facilitate the solution, this implementation method performs a first-order Taylor expansion on the residual terms to construct a linear residual model:

[0066] ;

[0067] in, and These are the partial derivatives of the residuals with respect to pose and landmarks, respectively. Indicates the pose state increment. Indicates the increment of landmark coordinates. This represents the random noise introduced during the linearization process.

[0068] Finally, the linear residual model is projected onto the Jacobian transpose space to construct the Hessian matrix. ,Right now:

[0069] ;

[0070] Hessian matrix like Figure 2 As shown, where, For pose association blocks, For landmark associative blocks, since the pose states and landmarks are independent of each other, pose associative blocks... Linked blocks with road signs It exhibits strict diagonal sparsity. This is a pose-landmark association block, used to reflect the camera's observation relationship with landmarks. Its sparsity is irregular; that is, the corresponding sub-block is non-zero only if the camera observes a specific landmark. (The last sentence appears to be a separate, unrelated statement: "For pose-landmark association blocks...") To address the irregular sparsity (i.e., the corresponding sub-block is zero when the camera does not observe certain landmarks), this implementation introduces a binary sparse coding mechanism (such as...). Figure 4 As shown in the figure, it is used to accurately describe the observation topology of each 3D landmark point within a sliding window. The format of this encoding is defined as follows: Taking a stereo camera system with 4 state times as an example, the encoding is configured as an 8-bit wide mask, where every 2 bits correspond to the observation state of the left and right cameras at one state time. The encoding mapping follows a temporal arrangement from the most significant bit to the least significant bit.

[0071] Based on this binary sparse coding mechanism, a two-level invalid operation elimination strategy can be executed when constructing the Hessian matrix. The specific details of this two-level invalid operation elimination strategy are as follows:

[0072] State-level coarse-grained pruning: If the binary codes of both cameras corresponding to a certain state in the binary sparse coding are both 0, it means that neither camera has observed the point at that moment. In this case, all related coordinate transformations and Jacobian matrix calculations for that state are skipped.

[0073] Camera-level fine-grained pruning: If the binary code of a camera corresponding to a certain state in binary sparse coding is 0, it indicates that the camera has no valid observation data. In this case, the residual term corresponding to that camera is directly pruned. And Jacobi Set it to zero, thereby blocking invalid multiply-accumulate operations in the subsequent Jacobian outer product process.

[0074] Experiments have verified that, in a typical binocular system scenario (four sliding window times, 177 landmarks, and 508 valid observations), the theoretically required number of Jacobi blocks to be calculated is: The number of Jacobi blocks actually used for computation is reduced to 508 after the binary sparse coding mechanism of this implementation method is eliminated to remove invalid operations, and the amount of computation is significantly reduced.

[0075] Step 2: Perform Schuler elimination on the Hessian matrix based on the binary sparse coding to obtain a system of linear equations containing only camera pose variables.

[0076] To reduce computational dimensionality, this step utilizes Shure elimination to eliminate landmark variables, transforming the problem into a pose-only solution. Since the observations of each landmark are independent given the camera pose, landmark association blocks are used. It is The diagonal matrix of the image is readily inversely computed. Utilizing this diagonal property, the Shure elimination technique marginalizes landmark variables, constructing a system of linear equations containing only camera pose variables, namely:

[0077] ;

[0078] in, For pose increment, Let be the pose residual vector. Let be the landmark residual vector. The Hessian matrix obtained after Schuler elimination by solving the above dimension-reduced equation is as follows: Figure 3 As shown, pose increment Then, by substituting it back, the signpost variables can be efficiently solved. This method significantly reduces computational complexity and storage requirements.

[0079] For pose and landmark associative blocks Irregular sparsity (i.e., the corresponding sub-block is zero when the camera does not observe certain landmarks) is addressed in this implementation by utilizing the designed binary sparse coding (such as...). Figure 5 As shown), the pose and landmark association block Irregular sparsity is represented by a mapping.

[0080] In the logical mapping, when a certain bit of the sparse code is 0, it indicates that the corresponding camera did not observe the landmark at that moment, and the pose in the Hessian matrix is ​​directly associated with the landmark block. Mapped to a zero sub-block; when the encoding bit is 1, it indicates the existence of a valid observation, and the pose in the Hessian matrix is ​​associated with the landmark block. The mapping is to a non-zero valid sub-block that needs to be computed.

[0081] In calculation At this time, the binary code is used as a control signal: when the binary code is 0, the corresponding matrix multiplication is skipped directly, and only the pose and landmark point association block corresponding to the binary code is processed. Perform the calculation. This strategy significantly reduces the number of floating-point multiplications.

[0082] For pose and landmark associative blocks To address the irregular sparsity (i.e., the corresponding sub-block is zero when the camera does not observe certain landmarks), this implementation utilizes an introduced binary sparse coding mechanism. This mechanism skips the pose-landmark association blocks. The invalid operations of zero blocks significantly reduce the computational overhead of the Shur elimination process for Hessian matrices. Experimental data show that, in a typical scenario with 177 landmarks and 508 valid observations, this strategy reduces the number of floating-point multiplications from 7,072,926 to 82,296 compared to the full matrix Shur elimination process, improving computational efficiency by approximately two orders of magnitude.

[0083] Step 3: Apply extended Kalman filtering to the linear equation system containing only camera pose variables to achieve incremental updates of state pose and landmark coordinates.

[0084] Unlike traditional optimization methods that involve multiple iterations, this implementation introduces the EKF concept. First, the equivalent residual equation after Schur elimination is substituted into the EKF update equation to recursively solve for the new state pose. ,Right now:

[0085] ;

[0086] in, This is expressed as Kalman gain.

[0087] Secondly, regarding the update of road sign points, this step utilizes the independence of the road sign point equations after elimination to decompose them into... A set of independent residual equations, namely:

[0088] ;

[0089] in, For the first Set of residual equations Indicates the first The Hessian matrix sub-blocks corresponding to each landmark point Indicates the first The coordinate increment of each road sign point Indicates the first Measurement noise covariance matrix of each road sign point.

[0090] Based on this, by using a multi-parallel EKF solver for landmarks, the latest coordinates of all landmarks are calculated concurrently after the pose update, which greatly improves the update efficiency in large-scale scenarios.

[0091] The second embodiment of the present invention relates to a hardware accelerator architecture that is applicable to the SLAM backend optimization method based on sparse matrix factorization of the first embodiment, such as... Figure 8 As shown, it includes:

[0092] The storage cache module, configured with block-based symmetric compressed storage logic, is used for efficient storage of Hessian matrix data in the FPGA's on-chip RAM (BRAM). The hardware circuit uses an address mapping unit to convert the two-dimensional index of the sparse matrix into a one-dimensional linear address in the physical memory, as specifically implemented below:

[0093] Pose-related blocks Symmetric compressed storage, for the diagonal of the Hessian matrix Pose-related blocks Given its mathematically symmetric positive definite property, the storage control logic in this implementation only latches the data in its main diagonal and upper triangular regions. The hardware addressing logic is configured to latch the two-dimensional coordinates. Mapped to a contiguous physical address space. For The matrix block is allocated only with a storage depth of 21 floating-point numbers. When the arithmetic unit requests to read data in the lower triangular region (i.e., the row index is greater than the column index), the address mapping logic automatically swaps the row and column indices and redirects to the corresponding upper triangular data address, thus ensuring the integrity and correctness of logical access despite the reduction in physical storage resources.

[0094] Road signpoint association block Similarly, for compressed storage with dimension , Road signpoint association block The hardware storage logic allocates only 6 data bits of storage space and also adopts the upper triangular compression mapping mechanism to further reduce the on-chip RAM utilization. Figure 6 This demonstrates the hardware memory mapping mechanism for self-associating sub-blocks in a Hessian matrix. The left side shows the logical matrix structure, with gray blocks representing valid data and white blocks representing redundant data; the right side shows the linear arrangement of data in physical memory. The internal hardware address generator uses this mapping rule to convert from a logical two-dimensional space to a physical one-dimensional space.

[0095] Pose and landmark association block Sparse mask storage for pose and landmark association blocks with high structural sparseness. The memory controller performs write enable control by reading a preset binary sparse code. Only when the corresponding coded bit is at a valid level (such as logic "1") will memory be allocated in RAM. Storage blocks; for invalid encoded bits, no physical storage space is allocated, thereby achieving compact storage of large-scale sparse data.

[0096] The fully pipelined Hessian build module adopts a five-stage deep pipeline architecture in its hardware implementation. It decomposes the complex reprojection error and partial derivative calculations into timing-aligned subtasks using hardware parallel circuits. The specific functions of each stage are as follows:

[0097] Stage 1: IMU coordinate system transformation and pre-computation stage. This stage executes two logic circuits in parallel: one circuit uses coordinate difference and quaternion rotation circuits to transform the landmarks in the world coordinate system. Transform to IMU coordinate system (Right now Figure 7 middle ,in, This represents the rotation quaternion from the world coordinate system to the current state (IMU coordinate system). (This represents the translation vector of the current state (i.e., the IMU coordinate system) in the world coordinate system); another path synchronously calculates the state rotation matrix. IMU points and their antisymmetric matrix And camera-world rotation matrix This provides intermediate variables for subsequent chain rule calculations.

[0098] Stage 2: Camera coordinate system projection and intermediate matrix calculation stage. This stage logically inherits the data from Stage 1, utilizing extrinsic parameters ( The coordinates in the IMU coordinate system will be... Further transformation to coordinates in the camera coordinate system On the other hand, it is equipped with a dedicated matrix multiplication unit, which utilizes the state rotation matrix generated in the previous stage. and antisymmetric matrix Parallel computation of intermediate rotation terms (i.e.) Figure 7 middle , This represents the rotational transformation from the IMU coordinate system to the camera coordinate system, and this term is a key factor in the subsequent calculation of the pose derivative.

[0099] Stage 3: The intermediate derivative calculation stage using the chain rule. This stage focuses on the decomposition and calculation of partial derivatives. The circuit incorporates the coordinate data of observed feature points (i.e., Figure 7 In (Input), combined with the coordinates output from Stage 2 Calculate the partial derivatives of the residuals with respect to the coordinate points of the camera system. And the partial derivatives of the camera coordinates with respect to the state and pose. This hierarchical design reduces the depth of single-level logic, ensuring high-frequency operation of the pipeline.

[0100] Stage 4: Jacobian matrix generation stage. This stage serves as the convergence point for derivative calculations, utilizing the intermediate derivatives output from Stage 3. and And the camera-world rotation matrix transmitted from Stage 1 Perform combinational operations. The circuit outputs the final Jacobian matrix of the reprojection error with respect to the state pose in parallel. and the Jacobian matrix of the landmarks .

[0101] Stage 5: Hessian matrix accumulation stage. This stage, as the end of the pipeline, is equipped with a matrix multiply-accumulate array, based on the Jacobian matrix of the state pose. Jacobian matrix of road signs Perform the outer product operation of the Jacobian matrix and accumulate the result into the corresponding Hessian sub-block of the on-chip cache.

[0102] To translate sparsity into practical power consumption and speed advantages, the pipelined input of the fully pipelined Hessian building block is coupled with sparse encoding / decoding and decision logic. This logic, based on the bit characteristics of binary sparse encoding, performs two levels of dynamic gating at the hardware level, consistent with the software logic:

[0103] Level 1 Gating (State-Level Bypass): The decision logic first checks the 2-bit code combination representing the current state (corresponding to the left and right cameras). If both bits are logic "0" (i.e., 0b00 in the code logic), it indicates that the landmark is completely invisible at that moment. At this time, the control logic sends a bypass signal to Stage 1 (IMU coordinate system transformation stage), directly skipping the high-overhead transformation from the world coordinate system to the IMU coordinate system (including quaternion rotation and translation subtraction circuits), and the pipeline does not experience data flipping.

[0104] Second-level gating (camera-level clock blocking): When data enters subsequent stages, the decision logic further checks the independent coded bit of the currently processed camera. If the coded bit is logic "0", it indicates that there is no observation data for that specific camera. At this time, the control logic sends clock gating signals to Stages 2 to 5, as follows:

[0105] Skip Stage 2: Block the clock for coordinate transformation from IMU coordinates to camera coordinates.

[0106] Skip Stage 3-4: Block the clock for calculating the partial derivatives of the Jacobian matrix.

[0107] Skip Stage 5: Block the clock for the outer product operation of the Jacobian matrix.

[0108] Through this coarse-grained gating mechanism, the hardware accelerator not only eliminates the storage access of invalid data, but also completely cuts off the dynamic power consumption of invalid computation paths at the physical level, achieving a linear correlation between computational load and observation sparsity.

[0109] The Schul elimination module in this embodiment is designed with a cascaded matrix operation data path for synchronously performing marginalization updates of the Hessian matrix and residual vector. The specific circuit structure is as follows: Figure 8 The lower half is shown below:

[0110] Matrix inversion circuit: Directly reads landmark-related blocks Hardware parallel computation of its inverse matrix This provides input for subsequent links.

[0111] The cascaded matrix multiplication array consists of a first-stage multiplication unit and a second-stage parallel processing unit. The first-stage multiplication unit utilizes pose and landmark association blocks. and inverse matrix Calculate intermediate variables The second-level parallel processing unit divides the data stream into two parallel processing paths, one of which is processed in parallel with the transpose matrix. Multiply to get The other path and the residual vector of the landmark Multiply to get .

[0112] The parallel update unit sends the calculation results back to the storage cache module through the subtractor array, thus completing the work of the pose association block in parallel. and pose residual vector Subtraction update.

[0113] The entire data path of the Shure elimination module is controlled by a binary sparse code scheduler, only in the pose and landmark association block. The coded bit indicates that a valid observation triggers a clock signal, automatically skipping invalid operations and memory accesses to zero-value sub-blocks.

[0114] A parallel Extended Kalman Filter (EKF) solver array leverages the independence of landmark equations after Schur elimination, employing a multi-path parallel processing architecture comprising a state pose update circuit and a landmark parallel update array. The state pose update circuit receives the equivalent residuals and Hessian blocks after Schur elimination, uses a master state machine for control, performs incremental updates of state variables, and stores the updated pose parameters back to the shared parameter register. The landmark parallel update array consists of N independent landmark update processing units (PEs). Each PE is configured with a local register file, enabling parallel loading of residual equation data from different landmarks. Because the update calculations for these landmarks are independent of each other, the hardware array can simultaneously update the covariance matrix and correct the coordinates of multiple landmarks, significantly improving the real-time performance of large-scale map construction.

[0115] The master state machine (FSM) controls the entire acceleration process. The state transitions include: IDLE (idle) → LOAD_PARAM (parameter reading) → RES_HESS_CALC (residual and Hessian construction) → SCHUR_ELIM (Schur elimination) → POSE_EKF (pose update) → LANDMARK_EKF (landmark broadcasting and parallel update) → OUTPUT (result output).

[0116] Therefore, this implementation, through its multi-stage pipeline architecture and binary sparse coding gating mechanism, can automatically skip invalid zero-block operations in the sparse matrix, eliminating redundant computational overhead. Simultaneously, the parallel EKF solving array overcomes the serial bottleneck of traditional processors when handling large-scale landmark updates, significantly reducing the overall latency of SLAM backend optimization and meeting the real-time requirements of high-speed mobile scenarios. This implementation leverages the symmetry and sparsity of the Hessian matrix, using block-based symmetric compression and mask indexing mechanisms to significantly reduce the occupancy of on-chip FPGA storage resources, enabling the processing of larger-scale map data with limited hardware resources. Furthermore, the compact storage format significantly reduces data interaction with off-chip memory, alleviating bus bandwidth pressure. In addition, unlike traditional hardware acceleration schemes that use fixed-point approximation, this implementation employs floating-point logic throughout the entire computation path. Combined with the mathematical equivalence of Schur complement and EKF, this ensures that the hardware accelerator can output high-precision state estimation results consistent with the software algorithm, avoiding truncation errors and cumulative drift caused by fixed-point approximation.

[0117] The third embodiment of the present invention relates to an electronic device, including a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor executes the computer program to implement the steps of the SLAM back-end optimization method based on sparse matrix factorization of the first embodiment.

[0118] The fourth embodiment of the present invention relates to a computer-readable storage medium having a computer program stored thereon, which, when executed by a processor, implements the steps of the SLAM back-end optimization method based on sparse matrix factorization of the first embodiment.

[0119] Those skilled in the art will understand that embodiments of this application can be provided as methods, systems, or computer program products. Therefore, this application can take the form of a completely hardware embodiment, a completely software embodiment, or an embodiment combining software and hardware aspects. Furthermore, this application can take the form of a computer program product implemented on one or more computer-usable storage media (including, but not limited to, disk storage and optical storage) containing computer-usable program code.

[0120] This application is described with reference to flowchart illustrations and / or block diagrams of methods, apparatus (systems), and computer program products according to embodiments of this application. It will be understood that each block of the flowchart illustrations and / or block diagrams, and combinations of blocks in the flowchart illustrations and / or block diagrams, can be implemented by computer program instructions. These computer program instructions can be provided to a processor of a general-purpose computer, special-purpose computer, embedded processor, or other programmable data processing apparatus to produce a machine, such that the instructions, which execute via the processor of the computer or other programmable data processing apparatus, generate instructions for implementing the flowchart... Figure 1 One or more processes and / or boxes Figure 1 A device that provides the functions specified in one or more boxes.

[0121] These computer program instructions may also be stored in a computer-readable storage medium that can direct a computer or other programmable data processing device to operate in a particular manner, such that the instructions stored in the computer-readable storage medium produce an article of manufacture including instruction methods implemented in a process. Figure 1 One or more processes and / or boxes Figure 1 The function specified in one or more boxes.

[0122] These computer program instructions may also be loaded onto a computer or other programmable data processing equipment to cause a series of operational steps to be performed on the computer or other programmable equipment to produce a computer-implemented process, thereby providing instructions that execute on the computer or other programmable equipment for implementing the process. Figure 1 One or more processes and / or boxes Figure 1 The steps of the function specified in one or more boxes.

[0123] The above description is merely a specific embodiment of the present invention, but the scope of protection of the present invention is not limited thereto. Any variations or substitutions that can be easily conceived by those skilled in the art within the technical scope disclosed in the present invention should be included within the scope of protection of the present invention. Therefore, the scope of protection of the present invention should be determined by the scope of the claims.

Claims

1. A sparse matrix decomposition based SLAM backend optimization method, characterized in that, Includes the following steps: According to the actual observation coordinates of the feature points in the image plane and the predicted coordinates of the road mark points projected into the camera coordinate system, a Hessian matrix is constructed by using a binary sparse coding mechanism; wherein the binary sparse coding is used to describe the observation topology of each road mark point in the sliding window, and a two-bit binary code is used to represent the state of the two cameras at one time in the sliding window; the Hessian matrix is represented as: wherein, is a pose correlation block, is a road mark point correlation block, is a pose and road mark point correlation block, and is used to reflect the observation relationship of the cameras to the road mark points; Based on the binary sparse coding, the Hessian matrix is ​​subjected to Schuler elimination to obtain a system of linear equations containing only camera pose variables. Extended Kalman filtering is applied to the linear equations containing only camera pose variables to achieve incremental updates of state pose and landmark coordinates.

2. The sparse matrix decomposition based SLAM backend optimization method of claim 1, wherein, The Hessian matrix is ​​constructed using a binary sparse coding mechanism based on the actual observed coordinates of feature points on the image plane and the predicted coordinates of landmark points projected onto the camera coordinate system. Specifically, this includes: The difference between the actual observed coordinates of the feature points in the image plane and the predicted coordinates of the landmark points projected onto the camera coordinate system is calculated to obtain the reprojection error. A linear residual model is constructed using a first-order Taylor expansion for the reprojection error; A binary sparse coding mechanism is used, and a two-level invalid operation elimination strategy is employed to project the linear residual model onto the Jacobian transpose space to construct a Hessian matrix.

3. The sparse matrix decomposition based SLAM backend optimization method of claim 2, wherein, The specific two-level invalid operation elimination strategy is as follows: If the binary codes of both cameras corresponding to a certain state in the binary sparse coding are both 0, then when constructing the Hessian matrix, all related coordinate transformations and Jacobian matrix calculation processes for that state are skipped directly. If the binary code of a certain camera corresponding to a certain state in the binary sparse coding is 0, the residual term and Jacobian term corresponding to that camera are directly set to zero when constructing the Hessian matrix.

4. The sparse matrix decomposition based SLAM backend optimization method of claim 1, wherein, When performing Schul elimination on the Hessian matrix based on the binary sparse coding to obtain a system of linear equations containing only camera pose variables, if the coding bit of the binary sparse coding is 0, the multiplication operation between the pose and the landmark block corresponding to that coding bit is skipped.

5. The sparse matrix decomposition based SLAM backend optimization method of claim 1, wherein, The process of applying an extended Kalman filter to the linear equations containing only camera pose variables to achieve incremental updates of state pose and landmark coordinates specifically includes: Substitute the linear equations containing only camera pose variables into the extended Kalman filter update equation, and obtain the new state pose by recursion. By utilizing the independence of the landmark equations after Schur elimination, the landmark equations are decomposed into multiple independent residual equations. The new state pose is then substituted into the landmark extended Kalman filter solver to obtain the new landmark coordinates.

6. A hardware accelerator architecture, characterized by The SLAM back-end optimization method based on sparse matrix factorization as described in any one of claims 1-5 includes: The storage cache module is used to store Hessian matrix data; The fully pipelined Hessian building block adopts a five-level deep pipeline architecture design for constructing Hessian matrices; The Shur elimination module is used to synchronously perform marginalization updates of the Hessian matrix and residual vector; A parallel extended Kalman filter solver array is used to receive the output results of the Schur elimination operation module and perform incremental updates through a state machine.

7. The hardware accelerator architecture of claim 6, wherein, The storage cache module includes a pose-associated block symmetric compression storage unit, a landmark-associated block compression storage unit, and a pose-landmark-associated block sparse mask storage unit. The pose-associated block symmetric compression storage unit only latches the data in the main diagonal and upper triangular regions of the pose-associated block, and realizes full-area logical access through address mapping. The landmark-associated block compression storage unit only latches the data in the upper triangular region of the landmark-associated block. The pose-landmark-associated block sparse mask storage unit allocates storage according to the binary sparse encoding.

8. The hardware accelerator architecture of claim 6, wherein, The fully pipelined Hessian construction module includes: IMU coordinate system transformation and pre-computation stage for performing in parallel two paths of logic, one path to transform a landmark point in world coordinate system to a coordinate in IMU coordinate system by coordinate differencing and quaternion rotation circuitry ; another path to synchronously compute a state rotation matrix , an IMU point and its skew-symmetric matrix , and a camera-world rotation matrix ; Camera coordinate system projection and intermediate matrix computation stage for transforming coordinates in the IMU coordinate system to coordinates in the camera coordinate system using extrinsic parameters Compute intermediate rotation terms;​​​ chain rule intermediate derivative computation stage for introducing observed feature point coordinate data in combination with the camera coordinate under which the coordinates are computed, respectively, partial derivatives of the residual with respect to the camera system coordinate points and partial derivatives of the camera system coordinate points with respect to the state pose ; Jacobian matrix generating stage, used to utilize the partial derivatives of the residuals with respect to the camera coordinate points. The partial derivative of the camera coordinates with respect to the state and pose and the camera-world rotation matrix Perform combination operations to output the Jacobian matrix of reprojection error with respect to state pose. Jacobian matrix of road signs ; The Hessian matrix accumulation stage is used to calculate the Jacobian matrix based on the state pose. Jacobian matrix of road signs Perform the outer product operation of the Jacobian matrix and accumulate the calculation result to the storage cache module.

9. The hardware accelerator architecture according to claim 8, characterized in that, The pipelined input of the fully pipelined Hessian construction module is coupled with sparse encoding / decoding and decision logic. Based on the bit characteristics of the binary sparse code, the sparse encoding / decoding and decision logic performs two-level dynamic gating at the hardware level, consistent with the software logic: The first level of gating is as follows: The decision logic first checks the two-bit code combination representing the current state; if both bits are 0, the control logic sends a bypass signal to the IMU coordinate system transformation and pre-computation stage, directly skipping the transformation from the world coordinate system to the IMU coordinate system, and the pipeline does not experience data flipping. The second level of gating is as follows: The decision logic checks the independent encoding bit of the currently processed camera. If the bit is 0, the control logic sends a transmission clock gating signal to the camera coordinate system projection and intermediate matrix calculation stage, the chain rule intermediate derivative calculation stage, the Jacobian matrix generation stage, and the Hessian matrix accumulation stage to block the clock of the coordinate transformation from the IMU coordinate system to the camera coordinate system, block the clock of the partial derivative calculation of the Jacobian matrix, and block the clock of the outer product operation of the Jacobian matrix.

10. The hardware accelerator architecture according to claim 6, characterized in that, The Shure elimination module includes: A matrix inversion circuit is used to directly read the landmark association block and calculate the inverse matrix of the landmark association block in parallel. The cascaded matrix multiplication array includes a first-level multiplication unit and a second-level parallel processing unit. The first-level multiplication unit multiplies the pose-landmark associative block with the inverse matrix of the associative block to obtain an intermediate variable. The second-level parallel processing unit includes a first-path processing unit and a second-path processing unit. The first-path processing unit multiplies the intermediate variable with the transpose of the pose-landmark associative block to obtain a first result. The second-path processing unit multiplies the intermediate variable with the landmark residual vector to obtain a second result. The parallel update unit is used to send the first result and the second result back to the storage cache module through the subtractor array, respectively, and to complete the subtraction update of the pose association block and the pose residual vector in parallel.

11. The hardware accelerator architecture according to claim 10, characterized in that, The Shure elimination operation module is controlled by a binary sparse coding scheduler, which triggers the clock signal only when the coding bit indication of the pose and landmark block is valid.

12. The hardware accelerator architecture according to claim 6, characterized in that, The parallel extended Kalman filter solver array includes: The state pose update circuit is used to receive the equivalent residual and Hessian block after Schur elimination, control it with the master state machine, perform incremental updates of state variables, and store the updated pose parameters back to the shared parameter register. The landmark parallel update array consists of N independent landmark update processing units. Each processing unit is equipped with a local register file that can load the residual equation data of different landmarks in parallel, and simultaneously update the covariance matrix and correct the coordinates of multiple landmarks.

13. An electronic device comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, characterized in that, When the processor executes the computer program, it implements the steps of the SLAM back-end optimization method based on sparse matrix factorization as described in any one of claims 1-5.

14. A computer-readable storage medium having a computer program stored thereon, characterized in that, When the computer program is executed by the processor, it implements the steps of the SLAM back-end optimization method based on sparse matrix factorization as described in any one of claims 1-5.

Images