A multi-sensor tightly coupled synchronous positioning and mapping method based on common view constraint

Abstract

The present invention relates to the field of azimuth measurement technology, and in particular to a multi-sensor tightly coupled synchronous positioning and mapping method based on common view constraints, comprising the following steps: realizing a distributed front-end odometer based on visual inertial navigation pose estimation based on feature points and laser radar inertial navigation pose estimation based on geometric constraints, which is used for front-end positioning of visual sensors, laser radar sensors and inertial navigation sensors; synchronizing the output data of visual sensors, laser radar sensors and inertial navigation sensors based on a keyframe data synchronization method; tightly coupling the output data constraints of visual sensors, laser radar sensors and inertial navigation sensors based on a manifold cross-modal constraint standardization method; and realizing back-end mapping based on a sensor joint optimization method based on keyframe common view constraints. The method provided by the present invention improves the real-time performance of the system and improves the positioning and mapping accuracy.

Description

Technical Field

[0001] The present invention relates to the field of azimuth measurement technology, and in particular to a multi-sensor tightly coupled synchronous positioning and mapping method based on common view constraints. Background Art

[0002] Simultaneous Localization and Mapping (SLAM) technology is widely used in virtual reality, augmented reality, autonomous driving, household service robots, special robots and other fields. Mobile robots need to accurately estimate their own position and posture while sensing the external environment in order to perform automated tasks autonomously. The SLAM algorithm usually consists of two parts: front-end odometer and back-end mapping. The front-end odometer first estimates the inter-frame pose increment through the input data of two adjacent frames, and the back-end mapping uses multi-frame observations to perform back-end optimization to achieve more accurate pose estimation.

[0003] The simultaneous positioning and mapping algorithm is closely related to the sensor. The most commonly used sensors are cameras, lidars, and inertial navigation units, each of which has different characteristics. Visual sensors include monocular cameras, binocular cameras, and depth cameras. Their advantages are low hardware cost, dense features, rich texture information, and high accuracy in small-scale scenes. However, under conditions such as sparse textures, large changes in lighting conditions, and violent movements, the SLAM algorithm using visual sensors is difficult to guarantee accuracy, and the monocular visual SLAM algorithm has the problem of unobservable scale. Lidar has the advantages of stable performance, accurate positioning of geometric information, large field of view, and no interference from lighting conditions. In terms of algorithms, its error model is simple and theoretical research is relatively mature. However, due to its over-reliance on geometric information, the SLAM algorithm using lidar will have large drift in scenes with severe structural degradation and unclear geometric features. In addition, lidar has the problem of motion distortion within a single scanning cycle: within a single scanning cycle, as the sensor module moves, the base coordinate system of the lidar capturing distance information changes accordingly, but when packaging the data, it is still treated as an unchanged base coordinate system, resulting in point cloud position offset. Inertial navigation can provide linear acceleration and angular velocity measurements of the body at a high frequency, and can effectively cope with scenes with intense movements. However, inertial navigation is an internal perception sensor that lacks the ability to perceive the external environment, and its measurements are inevitably affected by time-varying deviations. Based on the above sensor characteristics, it is difficult to obtain high accuracy and robustness using a single sensor SLAM method, so the existing methods mostly use multiple sensor fusion methods.

[0004] Existing multi-sensor fusion methods can be divided into loosely coupled methods and tightly coupled methods. Loosely coupled methods usually separate visual inertial navigation methods and lidar inertial navigation methods, lacking tight information fusion. Some loosely coupled methods integrate visual inertial navigation odometry and laser mapping frameworks, using visual-inertial navigation odometry as the motion prior of the laser mapping subsystem. There are also many loosely coupled methods that use lidar point cloud data to provide depth information for visual image features. A more advanced loosely coupled method divides the entire framework into a visual inertial navigation subsystem and a laser inertial navigation subsystem, and uses factor graphs to optimize the pose. The two can run independently. Although coupling operations are performed in multiple links such as initialization, point cloud registration, and loop closure, the disadvantages of loose coupling are still retained, and it is difficult to fully utilize constraint information to achieve higher positioning accuracy. The above loosely coupled methods do not incorporate laser measurement into the optimization of visual inertial navigation constraints, so the accuracy has a certain upper limit. Tightly coupled methods achieve more accurate pose estimation by integrating multiple sensor information in optimization. However, the existing tightly coupled methods only tightly couple inertial navigation measurements, sparse visual features, and radar point cloud features in a filtering framework for state estimation. The front end does not perform multi-sensor data association, and the back-end optimization module is abandoned, and it does not have the ability to associate multi-frame information. Summary of the invention

[0005] The technical problem to be solved by the present invention is to provide a multi-sensor tightly coupled synchronous positioning and mapping method based on common view constraints, improve the real-time performance of the system by designing a distributed front-end odometer, unify the multi-sensor constraints through data synchronization and cross-modal standardization methods, and finally use the key frame common view information to associate multi-frame constraints in the back-end mapping for tight coupling optimization, so as to make full use of multiple sensor information to improve the positioning and mapping accuracy.

[0006] The present invention is achieved through the following technical solutions:

[0007] A multi-sensor tightly coupled synchronous positioning and mapping method based on common view constraint comprises the following steps:

[0008] S1: Distributed front-end odometers are implemented based on feature point-based visual inertial navigation pose estimation and geometric constraint-based lidar inertial navigation pose estimation, which are used for front-end positioning of visual sensors, lidar sensors and inertial navigation sensors;

[0009] S2: Synchronize the output data of the visual sensor, lidar sensor and inertial navigation sensor based on the key frame data synchronization method;

[0010] S3: Manifold-based cross-modal constraint normalization method to tightly couple the output data constraints of visual sensors, lidar sensors and inertial navigation sensors;

[0011] S4: Backend mapping is achieved by joint sensor optimization based on keyframe common view constraints.

[0012] Further, in step S1: realizing the distributed front-end odometer based on the visual inertial navigation pose estimation based on feature points and the laser radar inertial navigation pose estimation based on geometric constraints includes the following steps:

[0013] S11: Use inertial navigation data pre-integration to calculate the initial pose value of each frame of camera image output by the visual sensor, then use the triangulation principle to restore the depth of image feature points, and simultaneously optimize the pose between camera image frames;

[0014] S12: For each frame of point cloud data output by the lidar sensor, the initial value of the pose is first calculated using the inertial navigation data pre-integration, and then the pose between the point cloud data frames is estimated based on the plane feature constraint;

[0015] S13: Distributed front-end odometer is implemented using a multi-threaded interaction method based on the inter-frame pose of camera images and the inter-frame pose of point cloud data.

[0016] Further, the key frame-based data synchronization method in step S2 synchronizes the output data of the visual sensor, the lidar sensor, and the inertial navigation sensor, including the following steps:

[0017] S21: Matching the point cloud data with the most recent camera image data based on the set time period;

[0018] S22: Perform point cloud transformation on the matched point cloud data according to formula (1), so that the point cloud data output by the lidar sensor is converted to the image timestamp that matches it:

[0019] (1);

[0020] in: Represents the output of the LiDAR sensor. Point cloud locations, express The transformed point cloud position, represents the fixed transformation of the extrinsic parameters of the camera and lidar sensor, represents the position of the visual sensor at the moment of image reception, Indicates the position of the lidar sensor at the time of receiving the point cloud data, Indicates the inverse operation;

[0021] S23: Select multiple key frames from the camera image. If the key frame has not been matched with the point cloud data, perform the operation of step S22 using the point cloud data that has been matched with other camera images most recently to the key frame, so that the point cloud data is converted to the timestamp of the key frame. If the key frame has been matched with the point cloud data, there is no need to perform the operation of step S22.

[0022] Furthermore, in step S3, the following method is used to tightly couple the output data constraints of the visual sensor, the lidar sensor and the inertial navigation sensor based on the cross-modal constraint standardization method of the manifold:

[0023] S31: Calculate the normalized Jacobian matrix according to formula (2) ,

[0024] (2);

[0025] in: represents the subtraction of special orthogonal groups and Euclidean space, represents addition of a special Euclidean group, Represents the rotation component of the current inertial navigation attitude, Represents the translation component of the current inertial navigation attitude, represents the right Jacobian matrix of the Lie algebra, represents the increment of the state variable, represents the matrix transpose, express and The combined matrix;

[0026] S32: According to formula (3), the standardized tightly coupled multi-sensor constraint operation is performed to define the Jacobian matrix of the position and posture in the special orthogonal group and Euclidean space of the lidar inertial navigation residual pair Transformed into the Jacobian matrix of the laser radar inertial navigation residual to the pose defined in a special Euclidean group :

[0027] (3).

[0028] Furthermore, step S4 uses the following method to implement back-end mapping: use the synchronized and standardized tightly coupled multi-sensor constraints to construct the maximum a posteriori problem based on the keyframe common view constraint, and use the graph optimization method to solve it, so as to obtain the final multi-sensor pose and visual feature point position, and realize back-end mapping.

[0029] Furthermore, the least squares form of the maximum a posteriori problem in step S4 is formula (4):

[0030] (4);

[0031] in: for A set of keyframe states, for A collection of map points, is the two-norm, is the inertial navigation residual, For the The map points are projected onto The reprojection residual of the frame image, is the lidar common view constraint residual, To observe the A collection of keyframes for map points. is the covariance matrix of the inertial navigation residual, is the covariance matrix of the visual reprojection residual, is the covariance matrix of the lidar common-view constraint residual.

[0032] Optimally, the visual sensor in step S1 is a binocular camera.

[0033] Optimized, the time period is set in step S21 to ,in The camera image receiving frequency.

[0034] Beneficial effects of the invention:

[0035] The present invention provides a multi-sensor tightly coupled synchronous positioning and mapping method based on common view constraint. Before performing back-end optimization, a keyframe-based data synchronization method and a manifold-based cross-modal constraint standardization method are first used to improve the reasonable definition of the optimization problem, and then a multi-sensor joint optimization method with keyframe common view constraint is used to realize back-end mapping, making full use of multi-sensor observation information for pose estimation. Compared with the existing methods, the special features of this method are: using a distributed front-end odometer to ensure the real-time performance of pose estimation; using data synchronization and cross-modal standardization methods to improve the perfection of the optimization problem definition; making full use of keyframe common view information to improve the reliability of multi-sensor constraints, thereby having higher positioning and mapping accuracy. BRIEF DESCRIPTION OF THE DRAWINGS

[0036] Figure 1 It is a schematic diagram of the process of the present invention.

[0037] Figure 2 It is a schematic diagram of matching point cloud data and camera image data of the present invention. DETAILED DESCRIPTION

[0038] A multi-sensor tightly coupled synchronous positioning and mapping method based on common view constraint, the flow chart is as follows Figure 1 As shown, the specific steps include:

[0039] S1: Distributed front-end odometer is realized based on visual inertial navigation pose estimation based on feature points and lidar inertial navigation pose estimation based on geometric constraints, and front-end positioning of visual sensors, lidar sensors and inertial navigation sensors is performed. The visual sensor here can preferably be a binocular camera.

[0040] The specific steps include:

[0041] S11: Pre-integrate the inertial navigation data to calculate the initial pose of each frame of the camera image output by the visual sensor, then use the triangulation principle to restore the depth of the image feature points, and synchronously optimize the pose between camera image frames; Here, pre-integrate the inertial navigation data to calculate the initial pose of each frame of the camera image output by the visual sensor, use the triangulation principle to restore the depth of the image feature points, and synchronously optimize the pose between camera image frames, all of which are conventional technical means in the field.

[0042] S12: For each frame of point cloud data output by the lidar sensor, the initial value of the pose is first calculated using the inertial navigation data pre-integration, and then the pose between the point cloud data frames is estimated based on the plane feature constraint;

[0043] Here, for each frame of point cloud data output by the lidar sensor, the inertial navigation data is first used for pre-integration to calculate the initial value of the pose, and the pose between point cloud data frames is estimated based on plane feature constraints, both of which are conventional technical means in the field.

[0044] S13: Distributed front-end odometer is implemented using a multi-threaded interaction method based on the inter-frame pose of camera images and the inter-frame pose of point cloud data.

[0045] Here, the use of a multi-threaded interaction method based on the inter-frame pose of camera image frames and the inter-frame pose of point cloud data is a conventional technical means in this field to realize a distributed front-end odometer.

[0046] The above method can be used to perform real-time pose estimation and provide a basic framework for subsequent multi-sensor information interaction.

[0047] S2: Synchronize the output data of the visual sensor, lidar sensor and inertial navigation sensor based on the key frame data synchronization method;

[0048] The specific steps include:

[0049] S21: Matching the point cloud data with the most recent camera image data based on the set time period;

[0050] As attached Figure 2 As shown in the figure, the downward arrow on the timeline indicates the time when the point cloud data is received, and the upward arrow on the timeline indicates the time when the image data is received. After the data is received, As the basis for judgment, The camera image receiving frequency is used to match the point cloud data with the most recent image data: if the time interval between the point cloud and the current image is less than , then the point cloud matches the current image, which is case A; if the time interval between the point cloud and the current image is greater than , then the point cloud does not match the current image, and the point cloud is matched with the next frame of the current image. This is case B.

[0051] Since the frequency of receiving point cloud data is usually lower than that of receiving image data, not all images have matching point clouds. Therefore, the following operations are required after matching.

[0052] S22: Perform point cloud transformation on the matched point cloud data according to formula (1), so that the point cloud data output by the lidar sensor is converted to the image timestamp that matches it:

[0053] (1);

[0054] in: Represents the output of the LiDAR sensor. Point cloud locations, express The transformed point cloud position, represents the fixed transformation of the extrinsic parameters of the camera and lidar sensor, represents the position of the visual sensor at the moment of image reception, Indicates the position of the lidar sensor at the time of receiving the point cloud data, Indicates the inverse operation;

[0055] After the point cloud data output by the lidar sensor is converted to the timestamp of the image that matches it through the above operation, it can be considered that the image that matches it is received at the same time.

[0056] S23: Select multiple key frames from the camera image. If the key frame has not been matched with the point cloud data, perform the operation of step S22 using the point cloud data that has been matched with other camera images most recently to the key frame, so that the point cloud data is converted to the timestamp of the key frame. If the key frame has been matched with the point cloud data, there is no need to perform the operation of step S22.

[0057] At the beginning of this step, you first need to select key frames from the image, and the subsequent parts of this step are only performed on key frames. Since the camera image reception frequency is generally greater than the point cloud reception frequency, the key frames selected from the image may or may not match the point cloud data. Therefore, this step needs to be performed so that all key frames can match the point cloud data, which facilitates the interaction of multi-sensor information and provides a basis for subsequent standardization.

[0058] S3: Manifold-based cross-modal constraint normalization method to tightly couple the output data constraints of visual sensors, lidar sensors and inertial navigation sensors;

[0059] Furthermore, in step S3, the following method is used to tightly couple the output data constraints of the visual sensor, the lidar sensor and the inertial navigation sensor based on the cross-modal constraint standardization method of the manifold:

[0060] S31: Calculate the normalized Jacobian matrix according to formula (2) ,

[0061] (2);

[0062] in: represents the subtraction of special orthogonal groups and Euclidean space, represents addition of a special Euclidean group, Represents the rotation component of the current inertial navigation attitude, Represents the translation component of the current inertial navigation attitude, represents the right Jacobian matrix of the Lie algebra, represents the increment of the state variable, represents the matrix transpose, express and The combined matrix;

[0063] S32: According to formula (3), the standardized tightly coupled multi-sensor constraint operation is performed to define the Jacobian matrix of the position and posture in the special orthogonal group and Euclidean space of the lidar inertial navigation residual pair Transformed into the Jacobian matrix of the laser radar inertial navigation residual to the pose defined in a special Euclidean group , which is unified with the Jacobian matrix of the visual inertial navigation residual to the pose defined in a special Euclidean group:

[0064] (3).

[0065] By constructing a standardized Jacobian matrix, the multi-sensor constraint residuals can be standardized and the reasonable definition of the optimization problem can be improved.

[0066] S4: A sensor joint optimization method based on keyframe common view constraints is used to realize back-end mapping. The specific method is as follows: Use the synchronized and standardized tightly coupled multi-sensor constraints to construct the maximum a posteriori problem based on keyframe common view constraints, and use the graph optimization method to solve it to obtain the final multi-sensor pose and visual feature point positions to realize back-end mapping.

[0067] Among them, the least squares form of the maximum a posteriori problem is formula (4):

[0068] (4);

[0069] in: for A set of keyframe states, for A collection of map points, represents the two-norm, is the inertial navigation residual, For the The map points are projected onto The reprojection residual of the frame image, is the lidar common view constraint residual, To observe the A collection of keyframes for map points. is the covariance matrix of the inertial navigation residual, is the covariance matrix of the visual reprojection residual, is the covariance matrix of the lidar common-view constraint residual.

[0070] Specifically, The map points are projected onto Reprojection residual of frame image The definition is as follows:

[0071] ;

[0072] in: Indicates the world coordinate system The inertial navigation pose of the frame image, Indicates The position of a map point in the world coordinate system, represents the camera projection function, It is The map point is The observed pixel coordinates in the frame image, represents the camera inertial navigation extrinsic parameters obtained through calibration, Indicates right Perform posture transformation.

[0073] Inertial navigation residual The pre-integration method is defined as follows:

[0074] ;

[0075] in: Represents the residual corresponding to the rotation component of the inertial navigation attitude, represents the residual corresponding to the inertial navigation velocity, Represents the residual corresponding to the translation component of the inertial navigation attitude;

[0076] in: The definition is as follows:

[0077] ;

[0078] For the The moment of the frame image corresponds to the rotation component of the inertial navigation posture, Indicates Frame image and Pre-integrated increment of the rotational component of the inertial navigation pose between frames.

[0079] is defined as follows:

[0080] ;

[0081] in: Indicates The inertial navigation speed corresponding to the moment of the frame image, Indicates Frame image and The pre-integrated increment of the inertial navigation velocity between frame images, Indicates Frame image and The time interval between frame images, Represents the acceleration due to gravity.

[0082] is defined as follows:

[0083] ;

[0084] in: Indicates The moment of the frame image corresponds to the translation component of the inertial navigation posture, Indicates Frame image and Pre-integrated increment of the translation component of the inertial navigation pose between frame images.

[0085] is defined as follows:

[0086] ;

[0087] in: represents the point cloud position covariance matrix, Indicates that Arranged from largest to smallest feature values.

[0088] The definition is as follows:

[0089] ;

[0090] in: Indicates The corresponding frame point cloud The feature of feature points, Indicates The corresponding frame point cloud The total number of feature points of the point cloud plane feature, Indicates The total number of feature points corresponding to the point cloud plane features, Indicates The center point position of a point cloud plane feature.

[0091] is defined as follows:

[0092] .

[0093] This step integrates multi-sensor common-view constraints to realize back-end mapping, making full use of the inter-frame observation information of multiple sensors for pose estimation, improving the reliability of multi-sensor constraints and achieving higher positioning and mapping accuracy.

[0094] In summary, the present invention provides a multi-sensor tightly coupled synchronous positioning and mapping method based on common view constraints. The method uses a distributed front-end odometer to ensure the real-time performance of pose estimation. The method uses data synchronization and cross-modal normalization methods to improve the completeness of the optimization problem definition. The method makes full use of the common view information of key frames to improve the reliability of multi-sensor constraints, thereby achieving higher positioning and mapping accuracy.

[0095] The above description is only a preferred embodiment of the present invention and is not intended to limit the present invention. For those skilled in the art, the present invention may have various modifications and variations. Any modification, equivalent replacement, improvement, etc. made within the spirit and principle of the present invention shall be included in the protection scope of the present invention.

Claims

1. A multi-sensor tightly coupled synchronous positioning and mapping method based on common view constraints, characterized by: The steps include: S1: Distributed front-end odometers are implemented based on feature point-based visual inertial navigation pose estimation and geometric constraint-based lidar inertial navigation pose estimation, which are used for front-end positioning of visual sensors, lidar sensors and inertial navigation sensors; The distributed front-end odometer is realized by visual inertial navigation pose estimation based on feature points and lidar inertial navigation pose estimation based on geometric constraints, which includes the following steps: S11: Use inertial navigation data pre-integration to calculate the initial pose value of each frame of camera image output by the visual sensor, then use the triangulation principle to restore the depth of image feature points, and simultaneously optimize the pose between camera image frames; S12: For each frame of point cloud data output by the lidar sensor, the initial value of the pose is first calculated using the inertial navigation data pre-integration, and then the pose between the point cloud data frames is estimated based on the plane feature constraint; S13: Implement distributed front-end odometer based on the inter-frame pose of camera image and inter-frame pose of point cloud data using multi-threaded interaction method; S2: Synchronize the output data of the visual sensor, lidar sensor and inertial navigation sensor based on the key frame data synchronization method; The keyframe-based data synchronization method synchronizes the output data of the visual sensor, the lidar sensor, and the inertial navigation sensor, including the following steps: S21: Matching the point cloud data with the most recent camera image data based on the set time period; S22: Perform point cloud transformation on the matched point cloud data according to formula (1), so that the point cloud data output by the lidar sensor is converted to the image timestamp that matches it: （1）； in: Represents the output of the LiDAR sensor. Point cloud locations, express The transformed point cloud position, represents the fixed transformation of the extrinsic parameters of the camera and lidar sensor, represents the position of the visual sensor at the moment of image reception, Indicates the position of the lidar sensor at the time of receiving the point cloud data, Indicates the inverse operation; S23: Select multiple key frames from the camera image. If the key frame has not been matched with the point cloud data, perform the operation of step S22 using the point cloud data that has been matched with other camera images closest to the key frame, so that the point cloud data is converted to the timestamp of the key frame. If the key frame has been matched with the point cloud data, there is no need to perform the operation of step S22. S3: Manifold-based cross-modal constraint normalization method to tightly couple the output data constraints of visual sensors, lidar sensors and inertial navigation sensors; S4: Backend mapping is achieved by using a joint sensor optimization method based on keyframe common view constraints.

2. According to claim 1, a multi-sensor tightly coupled synchronous positioning and mapping method based on common view constraint is characterized by: In step S3, the following method is used to tightly couple the output data constraints of the visual sensor, the lidar sensor and the inertial navigation sensor based on the cross-modal constraint standardization method of the manifold: S31: Calculate the normalized Jacobian matrix according to formula (2) , （2）； in: represents the subtraction of special orthogonal groups and Euclidean space, represents addition of a special Euclidean group, Represents the rotation component of the current inertial navigation attitude, Represents the translation component of the current inertial navigation attitude, represents the right Jacobian matrix of the Lie algebra, represents the increment of the state variable, represents the matrix transpose, express and The combined matrix; S32: According to formula (3), the standardized tightly coupled multi-sensor constraint operation is performed to define the Jacobian matrix of the position and posture in the special orthogonal group and Euclidean space of the lidar inertial navigation residual pair Transformed into the Jacobian matrix of the laser radar inertial navigation residual to the pose defined in a special Euclidean group : （3）。 3. The method for multi-sensor tightly coupled synchronous positioning and mapping based on common view constraint according to claim 2, characterized in that: Step S4 uses the following method to achieve back-end mapping: Use synchronized and standardized tightly coupled multi-sensor constraints to construct a maximum a posteriori problem based on keyframe common view constraints, and use graph optimization methods to solve it, obtain the final multi-sensor pose and visual feature point positions, and achieve back-end mapping.

4. The method for multi-sensor tightly coupled synchronous positioning and mapping based on common view constraint according to claim 3, characterized in that: The least squares form of the maximum a posteriori problem in step S4 is formula (4): （4）； in: for A set of keyframe states, for A collection of map points, is the two-norm, is the inertial navigation residual, For the The map points are projected onto The reprojection residual of the frame image, is the lidar common view constraint residual, To observe the A collection of keyframes for map points. is the covariance matrix of the inertial navigation residual, is the covariance matrix of the visual reprojection residual, is the covariance matrix of the lidar common-view constraint residual.

5. The method for multi-sensor tightly coupled synchronous positioning and mapping based on common view constraint according to claim 1, characterized in that: In step S1, the visual sensor is a binocular camera.

6. The method for multi-sensor tightly coupled synchronous positioning and mapping based on common view constraint according to claim 1, characterized in that: In step S21, the time period is set to ,in The camera image receiving frequency.

Images