Multi-sensor fusion positioning method and system based on factor graph optimization

Abstract

The invention provides a multi-sensor fusion positioning method and system based on factor graph optimization. The method comprises the following steps: acquiring measurement data of a current area acquired by multiple sensors; constructing pose factors according to the measurement data, wherein the pose factors comprise a pre-integration factor, an odometer factor, a loopback factor and a GPS factor; determining an optimized pose of the robot by adopting a factor graph optimization method according to the pose factors; and positioning the robot in the current area according to the optimized pose. According to the multi-sensor fusion positioning method and system based on factor graph optimization provided by the invention, the measurement data of the preorder state is utilized, and the pre-integration factor, the odometer factor, the loopback factor and the GPS factor are introduced, so that the accumulative error of the odometer is greatly reduced, and an effective means is provided for improving the precision and robustness of the system. According to the method, the real-time performance, robustness and accuracy of positioning are improved, and the method has great application research significance for autonomous navigation positioning of the robot.

Description

Technical Field

[0001] The present invention relates to the field of mobile robot positioning technology, and in particular to a multi-sensor fusion positioning method and system based on factor graph optimization. Background Art

[0002] In the field of autonomous robotics, being able to freely manipulate a robot in a complex and unknown environment is an extremely important challenge. Accurately estimating the robot's own posture and position and sensing the environment are necessary prerequisites for completing this challenge. On this basis, the robot can perform a variety of automated tasks including navigation, planning, control, and autonomous decision-making.

[0003] With the development of satellite positioning technology, the Global Navigation Satellite System (GNSS) can solve the problem of autonomous positioning in most unknown scenarios. However, the positioning accuracy of GNSS is heavily dependent on the quality of satellite signals. When the robot is located in an environment such as an urban canyon, where the satellite signal is blocked or interfered with, the positioning accuracy of the system will be severely reduced or even fail. In addition, the positioning data obtained by GNSS is of low frequency and cannot meet the positioning needs of the robot.

[0004] In recent years, with the development of sensor technology, the simultaneous localization and mapping (SLAM) technology based on monocular cameras or lidar has become the mainstream solution to the problem of autonomous positioning in unmanned systems. The SLAM algorithm analyzes the measurement data of the sensors equipped by the unmanned system to achieve the robot's self-positioning and accurate construction of the surrounding environment map in a complex and unplanned environment. This process greatly enhances the robot's ability to recognize its own state and the external environment, and provides the necessary information basis for the robot's navigation and task execution. The principle is to use the data obtained by the sensor at the current moment to associate with the previous data in the global map, so as to solve the position and posture changes during this period of movement, and then fuse the latest sensor data into the global map through posture.

[0005] By fusing data from multiple sensors, the multi-sensor fusion SLAM system can make full use of the unique measurement advantages of each sensor and significantly enhance the robustness of the system in the face of changing environments. Due to its high precision and strong robustness, LiDAR has been widely used in many fields such as autonomous driving, smart logistics, and smart agriculture. However, there are still some problems with the single LiDAR SLAM technology. Therefore, multi-sensor fusion SLAM with LiDAR, Global Positioning System (GPS), and Inertial Measurement Unit (IMU) as the main sensors has become the main choice of experts and scholars.

[0006] As a sensor that can sense its own dynamics, the Inertial Measurement Unit (IMU) is known for its high-frequency measurement capability and high resistance to environmental changes. Therefore, it is often used in combination with cameras and lidar to enhance the overall accuracy and stability of the system. The fusion methods of IMU and other sensors are divided into two types: loose coupling and tight coupling. The loose coupling strategy adopts a modular design, allowing different sensor systems to work independently, simplifying the system design and debugging process, and improving the flexibility and scalability of the system. Another advantage of modularity is that the sensor data processing is relatively independent, and the failure of one sensor will not directly affect the data processing of other sensors, thereby improving the fault tolerance of the system. The tightly coupled strategy considers the relevance and complementarity of all sensor data in the processing stage, which can more deeply explore the potential relationship between data and improve data utilization efficiency. At the same time, the relevance and complementarity of all sensor data are comprehensively considered, which can provide more accurate state estimation results. At the same time, the implementation of tight coupling requires complex algorithms and a large amount of computing resources. The various parts of the system are highly coupled. The error of one module will directly affect the performance of the entire system, and its scalability and flexibility are limited.

[0007] In a multi-sensor fusion SLAM system based on a loose coupling strategy, the data from the IMU and other sensors are processed separately, and then the pose information obtained by different sensors is fused; tight coupling is to fuse the data from the IMU and other sensors, and put them into the optimization problem as constraints, which generally has better results. A major problem faced by monocular camera SLAM systems is the inability to accurately obtain scale information. Relatively speaking, IMU can provide angular velocity and linear acceleration data with scale information, which provides a supplementary scale reference for the system. Therefore, many researchers fuse IMU with the camera to form a visual-inertial odometer (Visual Interial Odometry, VIO) or a visual inertial navigation system (Visual Interial Navigation System, VINS).

[0008] Although the accuracy of SLAM algorithms integrated with IMUs has significantly improved the robustness and accuracy compared to single-sensor SLAM algorithms, they are unable to overcome the natural defects of cameras and lidars. For example, even if the lidar can successfully obtain environmental structure information when it is in a scene with structural degradation (corridors, tunnels, etc.), its point cloud data is difficult to align with the point cloud; cameras are prone to degradation in scenes with frequent lighting changes or lack of lighting or weak textures. In addition, the front-end laser-visual-inertial odometer has the problem of causing cumulative errors to the system, and the system cannot build a high-precision map under GNSS denial conditions. Summary of the invention

[0009] The purpose of the present invention is to provide a multi-sensor fusion positioning method and system based on factor graph optimization to solve the technical problems raised in the background technology.

[0010] In order to achieve the above object, in a first aspect, the present invention provides a multi-sensor fusion positioning method based on factor graph optimization, comprising:

[0011] Get the measurement data of the current area collected by multiple sensors;

[0012] Constructing a pose factor according to the measurement data, wherein the pose factor includes a pre-integration factor, an odometer factor, a loopback factor, and a GPS factor;

[0013] According to the posture factors, determining the optimal posture of the robot using a factor graph optimization method;

[0014] The robot is positioned in the current area according to the optimized posture.

[0015] Furthermore, the multi-sensor includes a laser radar, a monocular camera, an IMU and a GPS.

[0016] Further, the method for constructing the pre-integration factor includes:

[0017] Get the IMU's pose estimation in the world coordinate system at the i-th and j-th key frame moments, as well as the relative pose transformation of the i-th and j-th key frame state nodes;

[0018] The pre-integration factor is obtained according to the pose estimation and the relative pose transformation.

[0019] Further, the pre-integration factor r is calculated by the following formula: p :

[0020] in, is the pose estimation of the IMU in the world coordinate system at the i-th and j-th key frames, ΔT ij for

[0021]

[0022] The relative pose transformation between the i-th and j-th keyframe state nodes.

[0023] Furthermore, the method for constructing the odometer factor includes:

[0024] Get the IMU's pose estimation in the world coordinate system at the i-th and j-th key frame moments, as well as the relative pose transformation of the i-th and j-th key frame state nodes;

[0025] The odometer factor is obtained according to the pose estimation and the relative pose transformation.

[0026] Further, the odometer factor r is calculated by the following formula: o :

[0027] r o =ΔT ij (T i ) -1 T j

[0028] Where T is the estimated position of the lidar in the world coordinate system at the i-th and j-th key frames, ΔT ij is the relative pose transformation between the i-th and j-th key frame state nodes.

[0029] Further, the method for constructing the GPS factor includes:

[0030] filtering the GPS data;

[0031] Convert the format of the filtered GPS data to obtain GPS data in a local Cartesian coordinate system;

[0032] Extended Kalman filtering is performed on the GPS data in the local Cartesian coordinate system to obtain the GPS factor.

[0033] Further, the method for constructing the loopback factor includes:

[0034] Store the single-frame point cloud in the world coordinate system into a container, and create a new keyframe when a sufficient number of points are accumulated;

[0035] Pick up all descriptors in the key frame and calculate hash keys, use the hash keys to locate the descriptors in the corresponding containers in the hash table, and vote once for the key frames with the descriptors in the container, and use the top ten key frames with the most votes as loop candidate key frames with matching descriptors;

[0036] Use the random sampling consistency algorithm to find the pose transformation that maximizes the number of correctly matched descriptors, calculate the plane overlap area between the current frame and the loop candidate keyframe, and perform set verification;

[0037] If the verification is passed, the pose transformation calculated by the descriptor is used as the initial value of the point cloud registration to calculate the relative pose transformation between the two frames, and the loop closure factor is calculated based on the relative pose transformation.

[0038] Further, according to the posture factor, a factor graph optimization method is used to determine the optimal posture of the robot, including:

[0039] Constructing a factor graph model according to the pre-integration factor, the odometer factor, the loop factor and the GPS factor;

[0040] The factor graph model is optimized to obtain an optimized posture of the robot.

[0041] In a second aspect, the present invention further provides a multi-sensor fusion positioning system based on factor graph optimization, comprising:

[0042] A data acquisition module, used to acquire measurement data of the current area collected by multiple sensors;

[0043] A posture factor construction module, used to construct a posture factor according to the measurement data, wherein the posture factor includes a pre-integration factor, an odometer factor, a loopback factor and a GPS factor;

[0044] An optimized posture determination module is used to determine the optimized posture of the robot using a factor graph optimization method according to the posture factor;

[0045] A positioning module is used to position the robot in the current area according to the optimized posture.

[0046] The multi-sensor fusion positioning method and system based on factor graph optimization provided by the present invention utilizes the measurement data of the previous state and introduces the pre-integration factor, odometer factor, loop factor and GPS factor, which greatly reduces the cumulative error of the odometer and provides an effective means for improving the accuracy and robustness of the system. The present invention improves the real-time, robustness and accuracy of positioning, and has great application research significance for the autonomous navigation and positioning of robots. BRIEF DESCRIPTION OF THE DRAWINGS

[0047] Figure 1 A flow chart of a multi-sensor fusion positioning method based on factor graph optimization provided by an embodiment of the present invention;

[0048] Figure 2 A schematic diagram of factor graph optimization provided by an embodiment of the present invention;

[0049] Figure 3 A schematic diagram of a plane search provided by an embodiment of the present invention;

[0050] Figure 4 A schematic diagram of processing boundary voxels provided by an embodiment of the present invention;

[0051] Figure 5 A schematic diagram of the structure of a multi-sensor fusion positioning system based on factor graph optimization provided by an embodiment of the present invention. DETAILED DESCRIPTION

[0052] The specific implementation of the present invention will be described in more detail below in conjunction with the schematic diagram. The advantages and features of the present invention will become clearer based on the following description. It should be noted that the drawings are all in a very simplified form and are not in exact proportions, and are only used to facilitate and clearly assist in explaining the purpose of the embodiments of the present invention.

[0053] First, as Figure 1 As shown, a flow chart of a multi-sensor fusion positioning method based on factor graph optimization provided by an embodiment of the present invention. The method may include the following steps:

[0054] Step S110: Acquire measurement data of the current area collected by multiple sensors.

[0055] Step S120: constructing a pose factor according to the measurement data, wherein the pose factor includes a pre-integration factor, an odometer factor, a loopback factor and a GPS factor.

[0056] Step S130: Determine the optimal posture of the robot using a factor graph optimization method according to the posture factor.

[0057] Step S140: Positioning the robot in the current area according to the optimized posture.

[0058] The multi-sensor fusion positioning method based on factor graph optimization provided in this embodiment transforms the state estimation problem in the multi-sensor system into a maximum a posteriori probability (MAP) problem, and uses factor graphs to model the system. The principle is as follows: Figure 2 As shown, the pose factor includes a pre-integration factor, an odometer factor, a loopback factor, and a GPS factor. A keyframe strategy is used to add state nodes to reduce the system computational load. The odometer factor and the pre-integration factor provide inter-frame constraints for two adjacent keyframes, the loopback factor provides constraints for the keyframes between two loopback points, and the GPS factor provides global constraints for all state nodes in the optimizer. Specifically, in step S110, the multi-sensor includes a lidar, a monocular camera, an IMU, and a GPS.

[0059] The full state maintained in the factor graph can be expressed as:

[0060]

[0061] Since each sensor is based on the Gaussian noise model assumption and the observations are conditionally independent, solving MAP is equivalent to solving a nonlinear least squares problem, and its objective function is:

[0062]

[0063] Among them, r p 、r o 、r L 、r G They represent the pre-integration factor, odometer factor, loop factor and GPS factor respectively.

[0064] In this embodiment, the method for constructing the pre-integration factor includes:

[0065] Get the IMU's pose estimation in the world coordinate system at the i-th and j-th key frame moments, as well as the relative pose transformation of the i-th and j-th key frame state nodes;

[0066] The pre-integration factor is obtained according to the pose estimation and the relative pose transformation.

[0067] Specifically, the pre-integration factor r is calculated by the following formula: p :

[0068] in, is the pose estimation of the IMU in the world coordinate system at the i-th and j-th key frames, ΔT ij for

[0069]

[0070] The relative pose transformation between the i-th and j-th keyframe state nodes.

[0071] In this embodiment, the method for constructing the odometer factor includes:

[0072] Get the IMU's pose estimation in the world coordinate system at the i-th and j-th key frame moments, as well as the relative pose transformation of the i-th and j-th key frame state nodes;

[0073] The odometer factor is obtained according to the pose estimation and the relative pose transformation.

[0074] Specifically, the odometer factor r is calculated by the following formula: o :

[0075] r o =ΔT ij (T i ) -1 T j

[0076] Where T is the estimated position of the lidar in the world coordinate system at the i-th and j-th key frames, ΔT ij is the relative pose transformation between the i-th and j-th key frame state nodes.

[0077] In this embodiment, the method for constructing the loopback factor includes:

[0078] Store the single-frame point cloud in the world coordinate system into a container, and create a new keyframe when a sufficient number of points are accumulated;

[0079] Pick up all descriptors in the key frame and calculate hash keys, use the hash keys to locate the descriptors in the corresponding containers in the hash table, and vote once for the key frames with the descriptors in the container, and use the top ten key frames with the most votes as loop candidate key frames with matching descriptors;

[0080] Use the random sampling consistency algorithm to find the pose transformation that maximizes the number of correctly matched descriptors, calculate the plane overlap area between the current frame and the loop candidate keyframe, and perform set verification;

[0081] If the verification is passed, the pose transformation calculated by the descriptor is used as the initial value of the point cloud registration to calculate the relative pose transformation between the two frames, and the loop closure factor is calculated based on the relative pose transformation.

[0082] The first step of loop detection is to select the keyframe with the highest similarity to the current keyframe from the historical keyframes to form a closed loop, use the point cloud registration method to calculate the relative pose transformation between the two frames, and construct a loop factor to add to the optimizer to eliminate the cumulative error between the two frames. When the loop range is relatively small, the odometer cumulative error is usually relatively small, and the loop point can be searched from the historical keyframes within a certain Euclidean distance range. However, when the system runs for a long time, more historical keyframes are retained in the system, and the use of the Euclidean distance method will increase the computational load. In addition, when the cumulative error between the two frames is too large, the point cloud registration method cannot obtain a good initial value and is very likely to fall into a local optimal solution. For this reason, the loop detection module in SLAM usually uses laser point clouds to construct descriptors, and the registration descriptors provide good initial values ​​for point cloud registration.

[0083] However, the descriptors constructed by the existing loop detection methods do not have rotation invariance. When the point cloud is projected into the two-dimensional image under rotation, the overall translation of pixels is presented, which is not conducive to loop detection. Therefore, this embodiment adopts a loop detection method based on a stable triangle descriptor. The triangle descriptor constructed by the embodiment is composed of the three side lengths of the triangle and the angle between the normal vectors, which has rotation and translation invariance and maintains a high degree of distinguishability.

[0084] Specifically, the single-frame point cloud in the world coordinate system output by the laser visual inertial odometer is first stored in a container, and a new key frame is created when a sufficient number of points are accumulated. When the key frame point cloud is obtained, the plane detection is first performed using region growing, and the point cloud is allocated to voxels of fixed size. Each voxel contains a set of points P i (i=1,…,N), and then calculate the covariance matrix ∑ of each voxel midpoint:

[0085]

[0086] in, is the center of mass of the voxel. Let λ k is the kth largest eigenvalue of the covariance matrix ∑, and the plane determination criterion is: Among them, σ1 and σ2 are preset hyperparameters. This standard can be used to check whether the points in the voxel can form a plane. If so, the voxel is called a planar voxel. Then a plane is initialized from any planar voxel, and the plane is expanded by searching its adjacent voxels. If the adjacent voxel is a planar voxel and is in the same plane as the current voxel (has the same plane normal vector direction and the distance is lower than the threshold), the searched voxel is added to the growing plane set. Otherwise, it is added to the boundary voxel set of the plane. The above process is repeated until all added adjacent voxels are expanded or reach the boundary voxel, such as Figure 3 shown.

[0087] For the boundary voxels, project the points they contain into the corresponding plane, such as Figure 4 As shown in (a)-(b) in , for each plane, an image that coincides with the plane is constructed, and the pixel value corresponding to each pixel represents the maximum distance from the midpoint of the boundary voxel of the plane to the plane, as shown in Figure 4 Then a point is selected, and the maximum pixel value in its 5×5 neighborhood is selected as the key point, which corresponds to a 3D point in the input point cloud and the normal of the plane to which it belongs is attached.

[0088] Construct a kD tree and use the nearest neighbor search to search 20 neighboring points for each key point in the key to form a triangular descriptor. Duplicate descriptors with the same side length will be removed. The triangular descriptor extracted from the kth key can be expressed as:

[0089] Where q is the centroid of the triangle descriptor; p1, p2 and p3 are the three vertices of the triangle descriptor, arranged in increasing order of side length; n1, n2 and n3 are the projection normal vectors of the planes to which the three vertices belong; l 12 , l 23 and l 31 are the three edges in the triangle descriptor, and l 12 <l 23 <l 31 In addition, all planes extracted in the current key Π = (π1, π1, ..., π n ), which is used in the geometric verification step in the subsequent loop detection. Since the boundary points are projected onto a plane extracted from the 3D point cloud instead of being extracted from the depth image, the extracted key points are invariant to viewpoint changes. In addition, the properties of the six descriptors are also invariant to arbitrary rigid transformations. Therefore, the entire method is rotationally and translationally invariant.

[0090] Hundreds of descriptors can usually be extracted from a keyframe. To quickly query and match descriptors, a hash table is used to store all descriptors in the keyframe. The hash key is calculated using six attributes that are rotationally and translationally invariant: the side length l, 12 , l 23 , l 31And the inner product of the projection normal vector n1·n2, n2·n3, n1·n3. Six descriptors with similar attributes will be expressed by the same hash key and stored in the same container. For a key frame to be detected, all its descriptors are also extracted and the hash key is calculated. At the same time, the hash key is used to locate it to the corresponding container in the hash table, and vote once for the key frame with the descriptor in this container. When all the descriptors in the key frame to be detected are processed, the matching process ends. The key frames with the top ten votes are regarded as loop candidate key frames with matching descriptors and saved for the loop detection step.

[0091] When a candidate frame is given, geometric verification is used to eliminate false positives caused by mismatching of descriptors. Since the shape of a triangle is unique when the side length is determined, once △a and △b are matched, the vertices (p a1 ,p a2 ,p a3 ) and (p b1 ,p b2 ,p b3 ) is naturally matched, and then the relative pose transformation ΔT between the two key frames can be easily calculated by singular value decomposition (SVD) using the correspondence between the points as constraints. Loop =(R, t):

[0092]

[0093] [U, S, V] = SVD(H)

[0094] R=VU T , t=-R*q a +q b

[0095] In order to increase the robustness of the algorithm, the random sampling consensus algorithm (RANSAC) is used to find the pose transformation that maximizes the number of correctly matched descriptors. Based on this pose transformation, the plane overlap area between the current frame and the candidate frame is calculated for set verification. A plane π in a voxel is represented by a center point g and a normal vector u. The plane set of the current frame is expressed as:

[0096]

[0097] The plane set of candidate frames is represented as:

[0098]

[0099] The rigid body transformation is expressed as:

[0100] C T B =( C RB , C t B )∈SE(3)

[0101] Where n and m are the number of planes in the current frame and the candidate frame respectively. C The center point of Π( c g1, c g2, ..., c g m ) construct a kD tree (k = 3), through the pose transformation C T B Focus B g i (i=1,2,...,n)∈ B Π conversion, then search for the nearest neighbor in the kD tree C g j , and determine whether the two planes coincide by the difference in normal vectors and the distance from the point to the plane:

[0102] || c R B B u i - C u j ||2<σ n

[0103] C u j ( C T B B g i - C g j )<σ d

[0104] Among them, σ n and σ d is a preset hyperparameter. If a pair of planes satisfy the constraints of the normal vector and the distance from the point to the plane in the above formula, the pair of planes are considered to be overlapping. After checking all the planes in the current frame, the percentage of plane overlap is calculated. c :

[0105]

[0106] Among them, N coincide and N sum are the number of overlapping planes and the number of all planes in the current frame respectively. When the overlap percentage exceeds the threshold σ pc , and finally it is regarded as a valid closed loop detection. Then the pose transformation calculated by the descriptor is used C T BCalculate the relative pose transformation ΔT between the two frames as the initial value of point cloud registration loop , assuming the current frame pose is T i , the loop frame pose is T j , the loop factor is recorded as: r L =ΔT Loop (T i ) -1 T j .

[0107] Specifically, the method for constructing the GPS factor in this embodiment includes: filtering the GPS data; converting the format of the filtered GPS data to obtain GPS data in a local Cartesian coordinate system; and performing extended Kalman filtering on the GPS data in the local Cartesian coordinate system to obtain the GPS factor.

[0108] Although loop factors are introduced into the graph optimization model to eliminate the accumulated error of the odometer, when there are too many state nodes between two loop points, the loop factors cannot provide sufficient constraints and may even cause incorrect registration, leading to system failure. To solve this problem, this technical solution introduces the Global Positioning System (GPS) to provide absolute measurement values ​​for the multi-sensor fusion system.

[0109] Since GPS signals are affected by many factors, in order to ensure data accuracy and timely feedback of GPS status, DOP value is usually used to detect GPS abnormal status. GPS abnormal status usually has the following three situations:

[0110] 1) GPS is in a state of poor performance for a long time, such as insufficient number of satellites or persistently high PDOP. This is usually caused by GPS being in an obstructed environment such as an urban canyon.

[0111] 2) Signal interference causes GPS to be unstable, with the number of satellites and PDOP values ​​jumping frequently. For example, the laser beam emitted by the lidar cuts the satellite signal.

[0112] 3) Loss of lock occurs when there is strong electronic interference or when the receiver is located in a tunnel, under a bridge or other place where GPS signals cannot cover. When the lock is lost, the positioning information cannot be output, and the positioning system will be divergent.

[0113] In the first case, the positioning precision factor value will become larger or the number of satellites searched will always be small. You can determine whether it is abnormal by detecting the GPS status within a certain period of time. Assuming that sampling starts at time k, calculate the PDOP and the weighted average of the number of satellites of the n data before time k:

[0114]

[0115] in and are the weight coefficients of the ith (i=1, 2, 3, ..., n) PDOP and the number of satellites. When a higher weight is assigned to the latest measurement data, these data have a more significant impact on the positioning results of the system. In order to specifically evaluate the impact of the positioning precision factor and the number of satellites on the performance of the positioning system, a function for GPS anomaly detection is introduced:

[0116]

[0117] Among them, ρ status Indicates the current GPS system status. The larger the value, the worse the GPS status. PDOP and λ SAT are the weight factors of PDOP and number of stars respectively. Assume that η status is the system status threshold, and the GPS system status is determined by the following formula:

[0118]

[0119] where η status It is the state detection threshold. When K=1, it means that the current GPS is in a signal blocked state.

[0120] For the second case, in an environment where GPS is facing interference, due to the continuous changes in the PDOP value and the number of visible satellites, the current working status of GPS can be evaluated by estimating the probability of these jumps. and They are the PDOP and star number jump values ​​measured for the i-th time:

[0121]

[0122] in and are the measured values ​​of PDOP and number of stars for the i-th measurement respectively. Given the jump threshold ζ ΔPDOP and ΔSAT , within a given m-second time window, record the number of events where the position precision factor PDOP and the number of satellites change beyond the preset threshold, and count the number of jumps per unit time:

[0123]

[0124] Where N(Δρ PDOP >ζ ΔPDOP ) represents the number of times the position precision factor PDOP jumps beyond the threshold within the statistical time window, P(Δρ PDOP >ζ ΔPDOP) indicates the number of jumps exceeding the threshold in a unit time. Similarly, the distribution of satellite jumps is counted. Whether the current GPS status is abnormal is determined based on whether the number of jumps exceeds the preset threshold:

[0125]

[0126] where ζ ΔPDOP and ΔSAT They are PDOP and satellite number abnormal jump thresholds respectively. When K=1, it means that the current GPS is interfered and is in a frequent jump state.

[0127] In the last case, GPS will lose lock when it is severely interfered or completely blocked, and GPS data is completely unavailable. To prevent false detection due to satellite noise, the time when the lock is lost is taken as the starting time, and the number of times the lock is lost within a period of time is counted. If a certain number is reached, it means that GPS has completely lost lock.

[0128] When the system receives GPS measurements, it first detects the GPS data using the aforementioned method, removes poor quality observations, and then converts them from the WGS84 coordinate system to the local Cartesian coordinate system. It then adds a new state node to the factor graph and builds a GPS factor associated with the node. Since there is no hardware synchronization between the GPS and the lidar, the GPS measurements are spherically linearly interpolated according to the timestamp of the lidar data. Since the cumulative error of the laser-visual-inertial odometer grows very slowly, a new GPS factor is added only when the GPS position covariance is less than the odometer covariance. In addition, due to the limitations of the equipment used, this article only considers single point positioning (SPP) results as input to construct the GPS factor:

[0129]

[0130] in, The position observations of the system in the world coordinate system provided by GPS and odometer respectively.

[0131] In this embodiment, a factor graph optimization method is used to determine the optimized posture of the robot according to the posture factors, including: constructing a factor graph model according to the pre-integration factor, odometer factor, loop factor and GPS factor; optimizing the factor graph model to obtain the optimized posture of the robot.

[0132] The prior values ​​obtained through state prediction at time k and the observed values ​​at the current time are:

[0133]

[0134] Where x is the nominal state quantity; is the prior state, y is the observation, and both are considered to be independent random vectors that obey the multivariate normal distribution; R and P are the covariance matrices of the observation noise and the state, respectively. Except for the nominal state quantity x, all other quantities are known. The state update problem can be described as calculating the optimal estimate through the above known quantities and the associated covariance matrix P + .

[0135] First, the observation equation is linearized to the first order to simplify its expression without affecting the rigor of the result:

[0136] y=h(x)+n,n~N(0,R)

[0137] Then define the latest estimate in the update process as x op，k , and its corresponding error state is δx op，k =x k -x op，k , perform a first-order Taylor expansion of the observation equation on the latest estimate, G k The Jacobian matrix of the observation matrix relative to the estimated state:

[0138]

[0139]

[0140] The cost function in the update process is constructed by prior terms and observation terms:

[0141]

[0142] The above process can define a unified prior and observed measurement value and the observation equation Then the residual term r k (x k )=z k -f(x k ), where z k ~N(f(x k ), Q k ), Calculate the observation equation f(x k ) for x k In x op,k The derivative J k :

[0143]

[0144] This gives the optimal solution of the Gauss-Newton method

[0145]

[0146] Then calculate the updated posterior covariance matrix First, the observation equation z l In x op,k Linearize at:

[0147] z k =f(x k )+w k ≈z op，k +J k δx op，k +w k

[0148] z op，k =f(x op，k )

[0149] The cost function can be written as:

[0150]

[0151] This gives the posterior covariance after the Gauss-Newton method update:

[0152]

[0153] The right side of the equal sign is another form of the posterior covariance solved by the iterative Kalman filter. Obviously, the posterior covariance matrix obtained by the Gauss-Newton method and the iterative Kalman filter is mathematically completely equivalent.

[0154] The above cost function is constructed from the perspective of Mahalanobis norm, and then the above Mahalanobis norm form is converted into the binary norm form. Define Q -1 The trivial root of K ,Right now Then C(x op,k ) is in the form of the two norm:

[0155]

[0156] where r′ k (x op，k )=S k r k (x op，k ). Then define the unified observation equation f′(x k )=S k f(x k ), then the observation equation f′(x k ) Treat the estimated state x k In x op,k The derivative at is:

[0157]

[0158] Then the final estimate in the two-norm form is

[0159]

[0160] J K and Q K Expanding and using the matrix inversion theorem, we get the following identity:

[0161]

[0162] The right side of the equation is the general form of the Kalman gain. Combining the above two equations:

[0163]

[0164] Above, the update formula of the iterative extended Kalman filter is obtained from the update equation of the optimal value solved by the Gauss-Newton method, that is, the Gauss-Newton method and the iterative extended Kalman filter are completely equivalent in mathematics. At the same time, in the first iteration It can be found that the first iteration of the Gauss-Newton method is mathematically equivalent to the extended Kalman update equation:

[0165]

[0166] On the second aspect, based on the same inventive concept, Figure 5 As shown, the present invention also provides a multi-sensor fusion positioning system based on factor graph optimization. The system may include:

[0167] The data acquisition module 201 is used to acquire the measurement data of the current area collected by multiple sensors;

[0168] A pose factor construction module 202, configured to construct a pose factor according to the measurement data, wherein the pose factor includes a pre-integration factor, an odometer factor, a loopback factor and a GPS factor;

[0169] An optimized posture determination module 203 is used to determine the optimized posture of the robot using a factor graph optimization method according to the posture factor; and

[0170] The positioning module 204 is used to position the robot in the current area according to the optimized posture.

[0171] In summary, the multi-sensor fusion positioning method and system based on factor graph optimization provided by the present invention utilizes the measurement data of the previous state and introduces the pre-integration factor, odometer factor, loop factor and GPS factor, which greatly reduces the cumulative error of the odometer and provides an effective means for improving the accuracy and robustness of the system. The present invention improves the real-time, robustness and accuracy of positioning, and has great application research significance for the autonomous navigation and positioning of robots.

[0172] The above is only a preferred embodiment of the present invention and does not limit the present invention in any way. Any technician in the relevant technical field, without departing from the scope of the technical solution of the present invention, makes any form of equivalent replacement or modification to the technical solution and technical content disclosed in the present invention, which does not depart from the content of the technical solution of the present invention and still falls within the protection scope of the present invention.

Claims

1. A multi-sensor fusion positioning method based on factor graph optimization, characterized in that: include: Get the measurement data of the current area collected by multiple sensors; Constructing a pose factor according to the measurement data, wherein the pose factor includes a pre-integration factor, an odometer factor, a loopback factor, and a GPS factor; According to the posture factors, determining the optimal posture of the robot using a factor graph optimization method; The robot is positioned in the current area according to the optimized posture.

2. A multi-sensor fusion positioning method based on factor graph optimization as claimed in claim 1, characterized in that: The multi-sensor includes a laser radar, a monocular camera, an IMU and a GPS.

3. The multi-sensor fusion positioning method based on factor graph optimization as claimed in claim 2, characterized in that: The method of constructing the pre-integration factor comprises: Get the IMU's pose estimation in the world coordinate system at the i-th and j-th key frame moments, as well as the relative pose transformation of the i-th and j-th key frame state nodes; The pre-integration factor is obtained according to the pose estimation and the relative pose transformation.

4. The multi-sensor fusion positioning method based on factor graph optimization as claimed in claim 3, characterized in that: The pre-integration factor r is calculated by the following formula p : in, is the pose estimation of the IMU in the world coordinate system at the i-th and j-th key frames, ΔT ij is the relative pose transformation between the i-th and j-th key frame state nodes.

5. The multi-sensor fusion positioning method based on factor graph optimization as claimed in claim 2, characterized in that: The method of constructing the odometer factor includes: Get the IMU's pose estimation in the world coordinate system at the i-th and j-th key frame moments, as well as the relative pose transformation of the i-th and j-th key frame state nodes; The odometer factor is obtained according to the pose estimation and the relative pose transformation.

6. A multi-sensor fusion positioning method based on factor graph optimization as claimed in claim 5, characterized in that: The odometer factor r is calculated by the following formula o : r o =ΔT ij (T i ) -1 T j Where T is the estimated position of the lidar in the world coordinate system at the i-th and j-th key frames, ΔT ij is the relative pose transformation between the i-th and j-th key frame state nodes.

7. The multi-sensor fusion positioning method based on factor graph optimization as claimed in claim 2, characterized in that: The method of constructing the GPS factor includes: filtering the GPS data; Convert the format of the filtered GPS data to obtain GPS data in a local Cartesian coordinate system; Extended Kalman filtering is performed on the GPS data in the local Cartesian coordinate system to obtain the GPS factor.

8. The multi-sensor fusion positioning method based on factor graph optimization as claimed in claim 2, characterized in that: The method for constructing the loopback factor includes: Store the single-frame point cloud in the world coordinate system into a container, and create a new keyframe when a sufficient number of points are accumulated; Pick up all descriptors in the key frame and calculate hash keys, use the hash keys to locate the descriptors in the corresponding containers in the hash table, and vote once for the key frames with the descriptors in the container, and use the top ten key frames with the most votes as loop candidate key frames with matching descriptors; Use the random sampling consistency algorithm to find the pose transformation that maximizes the number of correctly matched descriptors, calculate the plane overlap area between the current frame and the loop candidate keyframe, and perform set verification; If the verification is passed, the pose transformation calculated by the descriptor is used as the initial value of the point cloud registration to calculate the relative pose transformation between the two frames, and the loop closure factor is calculated based on the relative pose transformation.

9. The multi-sensor fusion positioning method based on factor graph optimization according to claim 1, characterized in that: According to the posture factors, a factor graph optimization method is used to determine the optimal posture of the robot, including: Constructing a factor graph model according to the pre-integration factor, the odometer factor, the loop factor and the GPS factor; The factor graph model is optimized to obtain an optimized posture of the robot.

10. A multi-sensor fusion positioning system based on factor graph optimization, characterized in that: include: A data acquisition module, used to acquire measurement data of the current area collected by multiple sensors; A posture factor construction module, used to construct a posture factor according to the measurement data, wherein the posture factor includes a pre-integration factor, an odometer factor, a loopback factor and a GPS factor; An optimized posture determination module is used to determine the optimized posture of the robot using a factor graph optimization method according to the posture factor; A positioning module is used to position the robot in the current area according to the optimized posture.

Images