Factor graph optimization method based on long-term trend factors

Abstract

The invention provides a factor graph optimization method based on a long-term trend factor and a program product. The method comprises the following steps: acquiring sensor data of a radar, a camera, an IMU (Inertial Measurement Unit) and the like; carrying out data preprocessing on the collected data to obtain information such as an initial feature matrix; performing preliminary positioning by using the radar sub-module to obtain an initial pose of the radar module; performing preliminary positioning by using a visual submodule to obtain an initial pose of a visual module; calculating visual module confidence for the radar sub-module and the visual sub-module respectively; introducing a long-term trend factor, an auxiliary radar factor, a visual factor and a pre-integration factor to optimize the pose of the robot; and using the pose information of the latest frame to update the long-term trend factor, and outputting a final robot pose. According to the invention, real-time high-robustness and high-precision positioning and mapping can be realized in an environment with weak texture and insufficient illumination, and the positioning precision and robustness of the SLAM in a severe environment are improved.

Description

Technical Field

[0001] The present disclosure relates to the fields of computer vision and multi-source fusion SLAM, and particularly to a factor graph optimization method based on long-term trend factors. Background Art

[0002] This section aims to provide background or context for the embodiments of the present disclosure recited in the claims. The description herein is not admitted to be prior art merely by virtue of its inclusion in this section.

[0003] Factor graph optimization methods in SLAM are mainly used to solve data association and error accumulation problems encountered in the process of localization and map construction. By adding observation data, motion models, etc. as factors into the factor graph and optimizing the factor graph. Factor graph optimization methods can effectively improve the accuracy of robot pose estimation and the quality of map construction. It can flexibly process different types of sensor data, support efficient optimization in large-scale environments, and can naturally incorporate prior information. However, it lacks robustness when there is a lack of reference structural texture in the environment.

[0004] To overcome the problem of insufficient robustness in the face of weak texture environments in SLAM, researchers have tried to introduce long-term trend factors into the factor graph. Long-term trend factors can use the recent motion state of the robot to assist in estimating the pose change of the current frame. It provides a solution for multi-source fusion SLAM in weak texture environments. Summary of the Invention

[0005] In view of this, the purpose of the present disclosure is to propose a factor graph optimization method based on long-term trend factors, which can at least solve some technical problems in the prior art to a certain extent.

[0006] Based on the above purpose, the first aspect of the exemplary embodiment of the present disclosure provides a factor graph optimization method based on long-term trend factors, the method comprising:

[0007] Obtain IMU, lidar, and visual image data collected by sensors;

[0008] Perform SLAM based on the obtained data to obtain a preliminary localization result;

[0009] Calculate the confidence of each module according to the characteristics of each module, and obtain the long-term trend factor of the current frame;

[0010] Construct a factor graph, add the long-term trend factor as a node to the factor graph, and perform factor graph optimization to obtain an optimized localization result;

[0011] Use the optimized localization result to update the long-term trend factor of each module;

[0012] According to the optimized positioning result, an accurate robot pose is obtained;

[0013] As can be seen from the above, the factor graph optimization method proposed in the embodiments of the present disclosure solves the limitations of traditional SLAM positioning to a certain extent and improves the positioning accuracy and robustness in weak texture environments through steps such as multi-sensor fusion and introduction of long-term trend factors. BRIEF DESCRIPTION OF THE DRAWINGS

[0014] In order to more clearly illustrate the technical solutions in the present disclosure or related technologies, the following will briefly introduce the drawings required for use in the embodiments or related technology descriptions. Obviously, the drawings in the following description are only the embodiments of the present disclosure. For those of ordinary skill in the art, without creative efforts, other drawings can also be obtained based on these drawings.

[0015] Figure 1 It is a schematic diagram of the overall process of the factor graph optimization method based on long-term trend factors provided by the embodiments of the present disclosure;

[0016] Figure 2 It is a schematic diagram of the LiDAR module process of the factor graph optimization method based on long-term trend factors provided by the embodiments of the present disclosure;

[0017] Figure 3 It is a schematic diagram of the visual camera module process of the factor graph optimization method based on long-term trend factors provided by the embodiments of the present disclosure;

[0018] Figure 4 It is a schematic diagram of the factor graph optimization of the factor graph optimization method based on long-term trend factors provided by the embodiments of the present disclosure; DETAILED DESCRIPTION OF THE EMBODIMENTS

[0019] It can be understood that before using the technical solutions disclosed in the embodiments of the present application, the types, usage scopes, usage scenarios, etc. of the personal information involved in the present application should be informed to the user and the user's authorization should be obtained in an appropriate manner in accordance with relevant laws and regulations.

[0020] For example, when responding to a user's active request, a prompt message is sent to the user to clearly prompt the user that the operation requested by the user will require obtaining and using the user's personal information. Thus, the user can autonomously choose whether to provide personal information to software or hardware such as an electronic device, application program, server, or storage medium that executes the operation of the technical solution of the present application according to the prompt message.

[0021] As an optional but non-limiting implementation manner, in response to receiving an active request from a user, the manner of sending a prompt message to the user may be, for example, in the form of a pop-up window, and the prompt message may be presented in text in the pop-up window. In addition, the pop-up window may also carry selection controls for the user to select "agree" or "disagree" to provide personal information to the electronic device.

[0022] It can be understood that the above notification and user authorization acquisition process is only illustrative and does not limit the implementation manner of the present application. Other manners that comply with relevant laws and regulations can also be applied to the implementation manner of the present application.

[0023] It can be understood that the data involved in the present technical solution (including but not limited to the data itself, the acquisition or use of the data) should comply with the requirements of corresponding laws, regulations and related regulations.

[0024] To make the purpose, technical solution and advantages of the present disclosure clearer and more understandable, the principles and spirit of the present disclosure will be described below with reference to several exemplary embodiments. It should be understood that these embodiments are only provided to enable those skilled in the art to better understand and then implement the present disclosure, rather than limiting the scope of the present disclosure in any way. On the contrary, these embodiments are provided to make the present disclosure more thorough and complete, and to be able to convey the scope of the present disclosure completely to those skilled in the art.

[0025] In this article, it should be understood that any number of elements in the drawings is for illustration rather than limitation, and any naming is only for distinction and does not have any limiting meaning.

[0026] It should be noted that unless otherwise defined, the technical terms or scientific terms used in the embodiments of the present disclosure should be the ordinary meanings understood by those of ordinary skill in the art to which the present disclosure belongs. The "first", "second" and similar terms used in the embodiments of the present disclosure do not indicate any order, quantity or importance, but are only used to distinguish different components. The terms such as "including" or "comprising" mean that the elements or objects appearing before the term cover the elements or objects listed after the term and their equivalents, without excluding other elements or objects. The terms such as "connected" or "coupled" are not limited to physical or mechanical connections, but may include electrical connections, whether direct or indirect. The terms such as "upper", "lower", "left" and "right" are only used to represent relative positional relationships, and when the absolute position of the object being described changes, the relative positional relationship may also change accordingly. The article "a" or "an" before an element does not exclude the existence of multiple such elements.

[0027] Next, the principles and spirit of the present disclosure will be elaborated in detail with reference to several representative embodiments of the present disclosure.

[0028] Existing SLAM algorithms often face many challenges in the face of weak texture environments. For example, in weak texture scenes such as tunnels and squares, due to the lack of obvious feature points, the shapes of lidar point clouds in different frames are highly similar, which easily leads to errors in point cloud matching and affects the accuracy and efficiency of map construction. For V-SLAM that mainly uses visual sensors, there are also problems in such environments because low light conditions or repetitive structures may cause feature detection and direct comparison failures.

[0029] The inventors of the present disclosure found that existing factor graph optimization algorithms can localize normally by fusing sensors such as vision, lidar, and IMU in some harsh environments, but their robustness is still poor in extreme scenarios such as tunnels.

[0030] To overcome the limitations in the prior art, the present disclosure provides a new positioning optimization method by introducing a factor graph optimization system based on long-term trend factors. By using long-term trend factors, when the sensors cannot operate normally, the recent motion trend of the robot is used to assist in positioning. For example, in a tunnel, the long-term trend factor can determine the possible pose changes and assist the system to avoid large offsets and other phenomena. This method further optimizes the positioning result and has strong practical application value, especially in fields such as autonomous driving and robot navigation, with broad application prospects.

[0031] After introducing the basic principle of the present disclosure, the various non-limiting embodiments of the present disclosure will be specifically introduced below.

[0032] Refer to Figure 1 It is a schematic diagram of the overall process of the factor graph optimization method based on long-term trend factors provided by an embodiment of the present disclosure, including a sensor device 101, a data preprocessing module 102, a lidar sub-module 103, a vision sub-module 104, a lidar confidence calculation module 105, a vision confidence calculation module 106, a factor graph optimization module 107, and an accurate map output module 108.

[0033] The sensor device 101 includes the most commonly used sensors in SLAM, including lidar, cameras, and IMUs. Each sensor has excellent scenarios and inapplicable scenarios, and collecting data from multiple sensors at the same time facilitates subsequent sensor fusion positioning.

[0034] The data preprocessing module 102 mainly operates to initialize the data for the subsequent lidar sub-module and vision sub-module. This includes initializing the feature matrix, aligning the timestamps between sensors, etc.

[0035] The main purpose of the lidar sub-module 103 is to use lidar and IMU data to complete the preliminary estimation of the robot's pose. The lidar has high measurement accuracy and is not affected by the lighting in the environment. Although the accuracy of the IMU is lower than that of the lidar, it has the characteristic of being unaffected by the environment. This module is introduced in detail as follows:

[0036] Reference Figure 2 The flowchart of the lidar module of the factor graph optimization method based on the long-term trend factor provided by the embodiment of the present disclosure. It includes an initialization module 201, an IMU pre-integration module 202, a point cloud registration module 203, a local optimization module 204, and an output of the local map 205.

[0037] The initialization module 201 mainly completes its operations in the data preprocessing module 102 and is responsible for passing the initialized data into the lidar sub-module.

[0038] The IMU pre-integration module 202 mainly pre-integrates the IMU data before each frame of lidar data arrives to obtain the relative motion estimation relative to the previous LiDAR moment. And it provides prior information for the motion prediction between subsequent frames.

[0039] The point cloud registration module 203. The main purpose of this module is to estimate the pose change of the relative motion of the lidar by precisely aligning the current frame of point cloud data with the previous key frame (usually the previous frame). The iterative closest point (ICP) algorithm or other advanced point cloud matching techniques are mainly used in the point cloud registration process. With these methods, the system can effectively calculate the pose transformation relationship of the current frame relative to the key frame and preliminarily determine the pose of the lidar.

[0040] The local optimization module 204 mainly integrates the information between multiple frames to improve the accuracy and consistency of the trajectory estimation. To further optimize the calculation efficiency and reduce information redundancy, the present disclosure introduces a key frame selection mechanism to construct a factor graph. The selection criterion for the key frame is based on the information gain of the current frame relative to the nearest key frame and its spatial distance. When the newly acquired frame contains enough new information and maintains a certain spatial or temporal interval from the nearest key frame, this frame is recognized as a new key frame and incorporated into the factor graph. This strategy not only helps to maintain the quality of the factor graph but also provides a solid data foundation for subsequent global optimization, ensuring the robustness and accuracy of the system.

[0041] The output of the local map 205 mainly outputs the generated trajectory estimation results and the constructed map for subsequent applications.

[0042] The main purpose of the vision sub-module 103 is to use visual information to complete the preliminary estimation of the robot's pose. The visual information is relatively dense, and each frame contains a large amount of environmental information, which can well estimate the robot's pose. This module is introduced in detail as follows:

[0043] Reference Figure 3 Figure 3 This is a schematic diagram of the visual camera module process for the factor graph optimization method based on long-term trend factors provided by the embodiments of the present disclosure. It includes an initialization module 301, a tracking module 302, a local mapping module 303, a local BA optimization module 304, and an output local map 305.

[0044]

[0044] The initialization module 301 mainly completes its operations in the data preprocessing module 102 and is responsible for passing the initialized data into the visual sub-module.

[0045]

[0045] The tracking module 302 mainly extracts ORB feature points for each frame of image. It then performs feature matching between the current frame and the previous frame to predict camera movement. If the matching fails, it tries to match with the points in the local map to resume tracking. Subsequently, the current camera pose estimation is updated.

[0046]

[0046] The local mapping module 303 mainly inserts new key frames and generates a local map based on the key frames provided by the tracking thread.

[0047]

[0047] The local BA optimization module 304 mainly performs local BA for each new key frame to optimize the key frame and its visible map points.

[0048]

[0048] The output local map 305 mainly outputs the generated trajectory estimation results and the constructed map for subsequent applications.

[0049]

[0049] After the visual subsystem is initialized, the system starts to receive continuous visual image frames and uses a pre-configured feature detector to identify significant feature points in each frame. In this project, the ORB feature detector, which has good performance in the SLAM field, is mainly used. It combines the high efficiency of the FAST corner detector and the compactness of the BRIEF descriptor, and can stably extract representative feature points under different lighting conditions. After screening and descriptor calculation, these feature points form a high-quality set of feature points, providing a reliable basis for subsequent matching.

[0050]

[0050] The system then matches the feature points of the current frame with those of the previous frame. During the matching process, the Hamming distance is used to measure the similarity of the feature point descriptors, and the RANSAC algorithm is used to eliminate false matches to ensure the accuracy of the matching results. Based on the correct matching pairs, the system can estimate the relative pose of the current frame with respect to the previous key frame to achieve the preliminary determination of the camera pose.

[0051] When a new frame enters the system, the system checks whether a new key frame needs to be added. If the current frame provides sufficient new information and is far enough away from the existing key frames, it is added as a new key frame to the local map. Subsequently, the ORB feature matching algorithm is used to match the feature points in the current frame with the existing feature points. If the overlap ρ between the current frame and the nearest key frame K ref is less than the threshold θ, it is selected as a new key frame, as shown in Equation 3-10. Where K t is the key frame to be output, K ref is the currently nearest key frame, and κ is the set of existing key frames.

[0052]

[0053] Subsequently, the key frame factor and the IMU factor of the IMU pre-integration result are added to the factor graph for optimization. Finally, the local mapping is completed, and it is ready to enter the sensor detection stage for use by the map optimization module.

[0054] The radar confidence calculation module 105 mainly calculates the confidence of the lidar sub-module. The details of this module are introduced as follows:

[0055] This topic adopts the degradation factor algorithm to effectively detect and evaluate the degradation situation. For the linear equation system Ax = b, the degradation factor r D is defined as a metric that only depends on A T The eigenvalues λ i of the A matrix.

[0056] r D = λ min + 1

[0057] In the formula, λ min represents the minimum eigenvalue of A T The confidence W L of the lidar subsystem can be calculated from the degradation factor r D

[0058] The visual confidence calculation module 106 mainly calculates the confidence of the visual sub-module. The details of this module are introduced as follows:

[0059] The confidence of the visual subsystem ensures the reliability of the camera image. To calculate this confidence T v , this topic uses the result of feature point matching for calculation. This process involves processes such as feature point detection and descriptor matching.

[0060] ​First, a series of potential feature points are extracted from the current frame image using a detector such as FAST, and descriptors are generated for these feature points. For each pair of consecutive frames, as many consistent matching pairs as possible are tried to be found.

[0061] Next, using the BFMatcher method, the best matching pairs are searched for between the front and back frames. During the matching process, a ratio test is adopted, that is, the matching pairs with the ratio of the nearest neighbor distance to the second nearest neighbor distance less than a certain threshold are retained. This can exclude some obvious incorrect matches.

[0062] Based on the matching results, the precision and the feature quantity ratio can be calculated as preliminary quality indicators. The precision P measures the proportion of correct matches; the feature quantity ratio W measures whether the number of feature points extracted in the current frame is sufficient. These two indicators can be calculated by the following formulas:

[0063]

[0064] where n matches refers to all the matching point pairs obtained by the matching algorithm; n correct is the number of truly successfully matched point pairs; n i is the number of feature points extracted in the current frame; n m is the maximum value of the number of feature points extracted in all previous frames.

[0065] Finally, based on these two indicators, the confidence of the vision subsystem W v can be calculated. In the formula, w1 and w2 represent the weight values of the two indicators respectively.

[0066] W v = w1 * P + w2 * W

[0067] The main operation of the factor graph optimization module 107 is to assist in factor graph optimization by introducing a long-term trend factor into the factor graph. The detailed introduction of this module is as follows:

[0068] For the lidar and the vision camera, a long-term trend factor h L and h V are respectively maintained. After each frame operation, the confidence w * of the current frame measurement is compared with the corresponding long-term trend factor, and the weight w of the current subsystem is calculated through weighted calculation:

[0069]

[0070] where h * and w * are the long-term trend factor and weight of the current frame, k is a settable smoothing parameter. When the weight and the long-term trend factor are not very different, the final weighted weight is closer to h *When the weight and the long-term trend factor differ significantly, the final result is closer to w * .

[0071] After calculating the weights for each sub-module within the factor graph, the final factor graph fusion stage will be carried out. This part of the factor graph refers to Figure 4 . It includes the prior factor 401, the VIO factor 402, the LIO factor 403, the pre-integration factor 404, and the long-term trend factor 405.

[0072] The prior factor 401 includes the poses of the frames that have been optimized previously. The VIO factor 402 represents the pose output by the visual sub-module, the LIO factor 403 represents the pose output by the radar sub-module, and the pre-integration factor 404 represents the initial pose value obtained by IMU pre-integration.

[0073] The long-term trend factor 405 stores the weighted average of all pose change amounts within the previous frames. It can reflect the pose change trend of the robot over a previous period of time. This factor x trend is defined as:

[0074]

[0075] where Δx, Δy, and Δz respectively represent the specific numerical change amounts of the current position in the three directions of the coordinate system, and Δθ, Δψ respectively represent the change amounts of the pitch angle, yaw angle, and roll angle of the orientation.

[0076] Thus, combining the pose p i-1 of the previous frame, i the pose estimate p

[0077] of the current frame can be calculated. trend In this way, the loss function f

[0078]

[0079] of the long-term trend factor i can be expressed as:

[0080] where x

[0081] is the pose to be optimized. trend Subsequently, the LIO factor, the VIO factor, the IMU pre-integration factor, and the long-term trend factor are transformed into Huber factors using the Huber algorithm. Finally, the pose solution is completed using the ceres non-linear optimization library.

[0082]

[0083] Where c is a threshold set in advance, and k is the smoothing factor introduced in the previous section. The Tukey algorithm enables it to rapidly reduce the impact when encountering large errors, enabling the long-term trend factor to better ignore these outliers while maintaining sensitivity to small errors and promptly correcting the long-term trend factor.

[0084] Those skilled in the art know that the embodiments of the present disclosure can be implemented as a system, method, or computer program product. Therefore, the present disclosure can be specifically implemented in the following forms, namely: completely hardware, completely software (including firmware, resident software, microcode, etc.), or a combination of hardware and software, generally referred to herein as "circuit", "module", or "system". In addition, in some embodiments, the present disclosure can also be implemented in the form of a computer program product in one or more computer-readable media, which contains computer-readable program code.

[0085] Any combination of one or more computer-readable media can be adopted. The computer-readable medium can be a computer-readable signal medium or a computer-readable storage medium. The computer-readable storage medium can be, for example, but not limited to, an electrical, magnetic, optical, electromagnetic, infrared, or semiconductor system, apparatus, or device, or any combination of the above. More specific examples (non-exhaustive examples) of the computer-readable storage medium can include, for example: an electrical connection having one or more wires, a portable computer disk, a hard disk, a random access memory (RAM), a read-only memory (ROM), an erasable programmable read-only memory (EPROM or flash memory), an optical fiber, a portable compact disk read-only memory (CD-ROM), an optical storage device, a magnetic storage device, or any suitable combination of the above. In this document, the computer-readable storage medium can be any tangible medium that contains or stores a program, and this program can be used by or in combination with an instruction execution system, apparatus, or device.

[0086] The computer-readable signal medium can include a data signal propagated in a baseband or as part of a carrier wave, which carries the computer-readable program code. Such a propagated data signal can take various forms, including but not limited to electromagnetic signals, optical signals, or any suitable combination of the above. The computer-readable signal medium can also be any computer-readable medium other than the computer-readable storage medium, and this computer-readable medium can send, propagate, or transmit a program for use by or in combination with an instruction execution system, apparatus, or device.

[0087] The program code contained on the computer-readable medium can be transmitted by any suitable medium, including but not limited to wireless, wire, optical cable, RF, etc., or any suitable combination of the above.

[0088] Computer program code for performing the operations of this disclosure may be written in one or more programming languages or combinations thereof, including object-oriented programming languages such as Python, Smalltalk, C++, and also including conventional procedural programming languages such as the "C" language or similar programming languages. The program code may execute entirely on the user's computer, partly on the user's computer, as a stand-alone software package, partly on the user's computer and partly on a remote computer, or entirely on the remote computer or server. In the case of a remote computer, the remote computer may be connected to the user's computer through any type of network, including a local area network (LAN) or a wide area network (WAN), or alternatively, may be connected to an external computer (e.g., through the Internet using an Internet service provider).

[0089] It should be understood that each block of the flowchart and / or block diagram, and combinations of blocks in the flowchart and / or block diagram, can be implemented by computer program instructions. These computer program instructions may be provided to a processor of a general purpose computer, special purpose computer, or other programmable data processing apparatus, thereby producing a machine, which, when executed by the computer or other programmable data processing apparatus, results in an apparatus for implementing the functions / operations specified in the block of the flowchart and / or block diagram.

[0090] These computer program instructions may also be stored in a computer-readable medium that can direct a computer or other programmable data processing apparatus to function in a particular manner, such that the instructions stored in the computer-readable medium produce an article of manufacture including an instruction apparatus for implementing the functions / operations specified in the block of the flowchart and / or block diagram.

[0091] The computer program instructions may also be loaded onto a computer, other programmable data processing apparatus, or other device, such that a series of operational steps are performed on the computer, other programmable data processing apparatus, or other device to produce a computer-implemented process, thereby enabling the instructions executed on the computer or other programmable apparatus to provide a process for implementing the functions / operations specified in the block of the flowchart and / or block diagram.

[0092] In addition, although the operations of the method of this disclosure are depicted in the figures in a particular order, this is not required or implied to perform these operations in that particular order, or to perform all of the illustrated operations to achieve the desired result. Instead, the steps depicted in the flowchart may be changed in the order of execution. Additionally or alternatively, certain steps may be omitted, multiple steps may be combined into one step and executed, and / or one step may be decomposed into multiple steps and executed.

[0093] The flowcharts and block diagrams in the accompanying drawings illustrate the possible architectures, functions, and operations of systems, methods, and computer program products according to various embodiments of the present application. Among them, each block in the flowchart or block diagram may represent a module, a program segment, or a part of code, and the above-mentioned module, program segment, or part of code contains one or more executable instructions for implementing the specified logical function. It should also be noted that in some alternative implementations, the functions marked in the blocks may occur in a different order from that marked in the accompanying drawings. For example, two consecutive blocks shown may actually be executed substantially in parallel, and they may sometimes be executed in the reverse order, depending on the functions involved. It should also be noted that each block in the block diagram or flowchart, as well as the combination of blocks in the block diagram or flowchart, can be implemented by a dedicated hardware-based system for performing the specified functions or operations, or can be implemented by a combination of dedicated hardware and computer instructions.

[0094] It should be noted that although several modules or units of devices for action execution are mentioned in the above detailed description, such a division is not mandatory. In fact, according to the embodiments of the present application, the features and functions of the two or more modules or units described above can be embodied in one module or unit. Conversely, the features and functions of one module or unit described above can be further divided and embodied by multiple modules or units.

[0095] Those of ordinary skill in the art should understand that: the discussion of any of the above embodiments is only exemplary and is not intended to imply that the scope of the present application (including the claims) is limited to these examples; under the concept of the present application, the technical features between the above embodiments or different embodiments can also be combined, the steps can be implemented in any order, and there are many other variations in different aspects of the embodiments of the present application as described above, and they are not provided in detail for the sake of brevity.

[0096] In addition, for the sake of simplicity of description and discussion, and in order not to make the embodiments of the present application difficult to understand, the well-known power / ground connections to integrated circuit (IC) chips and other components may or may not be shown in the provided drawings. In addition, the device may be shown in the form of a block diagram in order to avoid making the embodiments of the present application difficult to understand, and this also takes into account the fact that the details of the implementation of these block diagram devices are highly dependent on the platform on which the embodiments of the present application are to be implemented (i.e., these details should be completely within the understanding of those skilled in the art). In the case where specific details (such as circuits) are set forth to describe the exemplary embodiments of the present application, it will be apparent to those skilled in the art that the embodiments of the present application can be implemented without these specific details or with variations of these specific details. Therefore, these descriptions should be considered illustrative rather than restrictive.

[0097] Although the present application has been described in connection with specific embodiments thereof, many alternatives, modifications, and variations of these embodiments will be apparent to those of ordinary skill in the art in light of the foregoing description. For example, other memory architectures (e.g., dynamic RAM (DRAM)) may be used with the embodiments discussed.

[0098] Embodiments of the present application are intended to cover all such alternatives, modifications, and variations that fall within the broad scope of the appended claims. Accordingly, any omissions, modifications, equivalent substitutions, improvements, etc. made within the spirit and principle of the embodiments of the present application shall be included within the protection scope of the present application.

[0099] Although the spirit and principles of the present disclosure have been described with reference to several specific embodiments, it should be understood that the present disclosure is not limited to the specific embodiments disclosed, and the division of each aspect does not mean that the features in these aspects cannot be combined for benefit. Such division is only for the convenience of expression. The present disclosure is intended to cover various modifications and equivalent arrangements included within the spirit and scope of the appended claims. The scope of the appended claims is to be accorded the broadest interpretation so as to encompass all such modifications and equivalent structures and functions.

Claims

1. A method for optimizing a factor graph based on a long-term trend factor, characterized in that, Including: Obtain lidar and visual image data; Perform SLAM based on the obtained data to get a preliminary positioning result; Calculate the confidence of each module according to the characteristics of each module, and obtain the long-term trend factor of the current frame; Construct a factor graph, add the long-term trend factor as a node to the factor graph, and perform factor graph optimization to obtain an optimized positioning result; Use the optimized positioning result to update the long-term trend factor of each module; According to the optimized positioning result, obtain the accurate robot pose.

2. The method according to claim 1, wherein Calculate the confidence of each module according to the characteristics of each module, and obtain the long-term trend factor of the current frame, including: In the lidar module, detect information such as the number of feature points and the number of outliers in the lidar point cloud, and calculate the lidar point cloud confidence; In the lidar module, detect the minimum eigenvalue of the covariance matrix corresponding to the point cloud features, and jointly calculate the lidar module confidence with the lidar point cloud confidence; In the visual camera module, detect the reprojection error and the number of feature points of the visual image, and calculate the visual camera module confidence; Obtain the long-term trend factor of the current frame, which represents obtaining the change trend of the motion state within a certain period of time.

3. The method according to claim 2, wherein The long-term trend factor includes: The long-term trend factor represents the motion change trend of the robot within a period of time; The information of the long-term trend factor includes: The recent confidence change trend of the lidar module and the camera module; The direction and speed magnitude of the robot's recent motion; The change trend of the acceleration on the x, y, and z axes respectively; The change trend of the yaw angle, pitch angle, and roll angle of the robot.

4. The method according to claim 1, wherein Construct a factor graph, add the long-term trend factor as a node to the factor graph, and perform factor graph optimization to obtain an optimized positioning result, including: The lidar factor, which contains the positioning result and confidence of the lidar module; The visual camera factor, which contains the positioning result and confidence of the visual camera module: The IMU factor, which contains the result of IMU pre-integration; The long-term trend factor, which reflects the motion trend of the robot in the recent time period; Use the least squares method to change the pose until the positioning data and orientation with the minimum residual are obtained as the optimized pose.

5. The method according to claim 1, wherein Update the long-term trend factor of each module, including: Calculate the change amount of the pose between the current frame and the previous frame, and update the speed in the long-term trend factor according to the lidar confidence and camera confidence; Calculate the change amounts of the acceleration on the x, y, and z axes and the pitch angle, yaw angle, and roll angle, and update the corresponding elements in the long-term trend factor according to the residual size of the IMU factor during optimization; According to the lidar confidence and camera confidence of the current frame, update the corresponding confidence factors in the long-term trend factor at a certain ratio.

6. An apparatus for an optimization method of a factor graph based on a long-term trend factor, characterized in that, Including: The data acquisition module, which acquires the data collected by the radar, camera, and IMU sensors; The data preprocessing module, which performs preliminary processing on various data, including removing outliers and calculating some parameters; The radar positioning module, which completes the preliminary positioning of the robot pose by using lidar data; The camera positioning module, which completes the preliminary positioning of the robot pose by using visual camera data; Confidence Estimation Module, which calculates the confidence of the corresponding module by analyzing each type of sensor data; Factor Graph Optimization Module, which constructs a factor graph based on the long-term trend factor and the previously calculated confidence, and optimizes the pose of the factor graph; Localization Result Output Module, which updates the long-term trend factor and outputs the optimized pose of the robot.

Images