Unmanned ship positioning and mapping method based on triple factor graph optimization

Abstract

The invention discloses an unmanned ship positioning and mapping method based on triple factor graph optimization. The method comprises the following steps: inputting laser radar data, inertial measurement unit data and global satellite navigation system measurement data into a first factor graph for optimization to obtain the pose of an unmanned ship; taking a key frame marginalized by a speedometer as the input of a second factor graph, and obtaining a local map, wherein the key frame comprises the pose of the unmanned ship; and taking the plurality of local maps as input of a third factor graph to obtain a global map. The invention provides a registration factor fused with light intensity information. The weighted sum of three weighted Gaussian distributions corresponding to the mean value of the light intensity is used for description, and a light intensity kernel function is established. According to the invention, by adding extra light intensity information, the problem of insufficient environment information is solved. In addition, a triple factor graph optimization framework is arranged, so that the accuracy and robustness of positioning and mapping of the unmanned ship can be effectively improved.

Description

Technical Field

[0001] The present invention relates to the technical field of mapping and positioning, and in particular to an unmanned boat positioning and mapping method based on triple factor graph optimization. Background Art

[0002] Synchronous positioning and mapping technology has always been a hot research direction for unmanned boat navigation, using cameras to obtain information about the surrounding environment and locate its own position. However, the existing unmanned boat positioning and mapping systems used in water-bank alternating environments basically adopt a system architecture optimized by a single factor graph, which can neither fully utilize the computing power of the onboard computer nor guarantee the accuracy of positioning and mapping in complex water-bank alternating environments.

[0003] However, the following problems still exist when using odometer to estimate the local motion state of the unmanned boat: ① A single geometric feature is often used for optimization, and the odometer will degenerate when the feature points on the shore are sparse; ② The degradation of the laser point cloud when the unmanned boat is far away is not considered, or the measurement information of the global satellite positioning navigation system is added based on loose coupling, resulting in insufficient overall positioning accuracy.

[0004] Finally, the following problems still exist when performing closed-loop detection: ① The closed-loop detection method based on Euclidean distance is difficult to close successfully in large-scale scenes; ② The closed-loop detection method based on geometric descriptors will have reduced performance in dynamic scenes; ③ The closed-loop detection method based on the global navigation satellite system is easily affected by satellite signals. Therefore, a method and system that can improve the accuracy and robustness of composition and positioning is urgently needed. Summary of the invention

[0005] In view of the above-mentioned defects, the purpose of the present invention is to propose an unmanned boat positioning and mapping method based on triple factor graph optimization to solve the problem that feature matching cannot be performed when the shore feature points are sparse.

[0006] To achieve this purpose, the present invention adopts the following technical solution: a method for positioning and mapping an unmanned boat based on triple factor graph optimization, comprising the following steps:

[0007] Step S1: constructing a first factor graph, wherein the first factor graph includes a registration factor, a marginalization prior factor, a guard factor, and a pre-integration factor;

[0008] Input the laser radar data, inertial measurement unit data, and global satellite navigation system measurement data into the first factor graph for optimization to obtain the position and posture of the unmanned boat;

[0009] Step S2: constructing a second factor graph, wherein the second factor graph includes a registration factor, a guard factor, and a pre-integration factor;

[0010] The odometer marginalized key frame is used as the input of the second factor graph, where the key frame includes the position and posture of the unmanned boat, and a local map is obtained;

[0011] Step S3: constructing a third factor graph, wherein the third factor graph includes a registration factor, a closed-loop factor, a navigation priori factor, a satellite guidance factor, and a pre-integration factor;

[0012] Use multiple local maps as input of the third factor graph to obtain a global map;

[0013] The registration factor integrates the light intensity information, and the residual acquisition formula of the specific registration factor is as follows:

[0014] where r i Li represents the residual of the registration factor, η ij is the light intensity weight, p i is the i-th point in the current scan of the laser radar in the carrier coordinate system, T i Represents point p i Translate to the world system, n is a non-negative integer, μ j is the mean value of light intensity, exp() represents the exponential function with natural base e as base;

[0015] in

[0016] I(p i ) is the light intensity kernel function, where where β j is the weight factor. The stronger the light intensity, the greater the weight factor β j The bigger, are the mean and covariance of light intensity respectively, and N() represents normal distribution.

[0017] Preferably, the steps of constructing the first factor graph are as follows:

[0018] Step S11: construct a sliding window, wherein the sliding window only obtains the latest five states of the unmanned boat;

[0019] Step S21: Establish a factor graph for the state of the unmanned boat, wherein the establishment is regularized as follows:

[0020] Establishment of marginalization prior factors:

[0021] Establish marginalization prior factors for states at the edge of the sliding window;

[0022] Establishment of registration factors:

[0023] Get the latest three states, and establish a registration factor between the latest state and the previous state, and establish a registration factor between the latest state and the previous state;

[0024] Determine whether there is a key frame in the five states in the sliding window. If there is a key frame, establish a registration factor for the key frame;

[0025] Establishing a registration factor between the latest state and the marginalized prior factor;

[0026] Pre-integration factor establishment:

[0027] Establishing pre-integration factors between adjacent states;

[0028] Wei Dao points factor establishment:

[0029] It is determined whether a global satellite navigation system measurement signal is detected when the state is acquired. If detected, a satellite navigation factor is established for the corresponding state.

[0030] Preferably, the use process of the sliding window is as follows:

[0031] Remove the guard factor and use only the registration factor, pre-integration factor, and marginalization prior factor for sliding window optimization to obtain the residual of the first factor graph;

[0032] Perform eigenvalue decomposition of the Hessian matrix on the residual of the first factor graph to obtain the minimum eigenvalue of the Hessian matrix, and determine whether the minimum eigenvalue has an abnormality. If not, output the position and posture of the unmanned boat according to the residual of the first factor graph. If there is an abnormality, re-add the satellite guidance factor, add the robust kernel function to the satellite guidance factor, and re-execute the sliding window optimization.

[0033] Get the chi-square value of the guard factor and remove outliers based on the chi-square value result;

[0034] Remove the robust kernel function of the satellite guidance factor, re-execute the sliding window optimization, obtain the residual of the first factor graph, and output the position and posture of the unmanned boat according to the residual of the first factor graph;

[0035] The residual formula of the first factor graph is as follows:

[0036]

[0037] Where X is a set of state variables to be optimized, and its expression is X = (x1, ..., x5), and x t =[T t ,v t ,b t ], where T t is the position of the unmanned boat at time t, including three degrees of freedom translation and three degrees of freedom rotation, v tis the velocity value of the inertial measurement unit at time t, b t is the zero bias of the inertial measurement unit at time t, indicating The residuals are modulo and weighted using the covariance matrix, and their values ​​are the chi-square value of the factor, r i Li represents the residual of the i-th registration factor, Li is the identifier of the registration factor, r i imu represents the residual of the i-th pre-integration factor, r i imu , imu is the identifier of the pre-integration factor, r i gnss represents the residual of the ith guidance factor, gnss is the identifier of the guidance factor, r i marg represents the residual of the marginalized prior factor, marg is the identifier of the marginalized prior factor, ∑Li, ∑imu, ∑gnss, and ∑marg are the covariance matrices of the registration factor, pre-integration factor, guard factor, and marginalized prior factor, respectively.

[0038] Preferably, the specific steps in step S2 are as follows:

[0039] Determine whether there is a key frame in the odometer that is marginalized. If so, obtain the relative position and posture of the marginalized key frame, obtain the translation of the unmanned boat according to the relative position and posture, and determine whether the translation exceeds the threshold. If so, discard the key frame. If not, input the key frame into the local map module optimizer and insert the key frame into the second factor graph.

[0040] Determine whether the number of frames in the local map optimizer exceeds a threshold number, and if so, perform factor graph optimization to obtain an optimized state quantity, and obtain a local map according to the state quantity;

[0041] The formula for obtaining the relative pose is as follows:

[0042]

[0043] in is the pose corresponding to the marginalized keyframe, T i W The latest position of the unmanned boat;

[0044] The formula for obtaining the translation amount is as follows: in is the translation amount, is the rotation matrix;

[0045] The factor graph in the second factor graph is established as follows:

[0046] Adding a registration factor and a pre-integration factor between two adjacent states;

[0047] Determine whether the state detects a global satellite navigation system measurement signal when acquiring the state, and if so, establish a satellite navigation factor for the corresponding state;

[0048] The formula for obtaining the state quantity is as follows:

[0049]

[0050] Preferably,

[0051] The specific steps of step S3 are as follows:

[0052] The local map x lk As input, it is fed into the global map optimizer;

[0053] Construct a third factor graph for the input of the global map optimizer;

[0054] Periodically optimize the factor graph to obtain the residual of the third factor graph, and construct a global map based on the residual of the third factor graph;

[0055] The factor graph in the third factor graph is established as follows:

[0056] Establishment of registration factors:

[0057] In two adjacent local maps x lk With the local map x lk+1 Add a registration factor between

[0058] Establishment of navigation prior factors:

[0059] Get local map x lk The two endpoint states are recorded as and Two endpoint states With endpoint status Add navigation prior factors;

[0060] Establishment of pre-integration factor:

[0061] In a local map x lk Endpoint status With the next local map x lk+1 Endpoint status Add a pre-integration factor between

[0062] Establishment of the protective factor

[0063] Determine the local map x lkWhether the global satellite navigation system measurement signal is detected during acquisition, if it is detected, the corresponding local map x lk Establishment of protective factors;

[0064] Establishment of closed-loop factors:

[0065] In two adjacent local maps x lk With the local map x lk+1 Add a closed-loop factor between them;

[0066] The residual formula for the third factor graph is as follows:

[0067]

[0068] where r i loop is the residual of the closed-loop factor, and ∑loop is the covariance matrix of the closed-loop factor.

[0069] Preferably, before synthesizing the global map in step S3, loop closure detection is also required;

[0070] The steps of the closed-loop detection are as follows:

[0071] A three-dimensional coordinate system is constructed, with the unmanned boat as the origin of the coordinate system, the horizontal forward direction as the X-axis direction of the coordinate system, the vertical upward direction as the Z-axis direction, and the Y-axis direction meeting the requirements of the Cartesian coordinate system;

[0072] In the top view direction of the point cloud of the local map, the point cloud is divided into several areas according to the division rules, where the division rules are as follows: with the origin as the center, the point cloud is divided into Nr rings at equal distances. At the same time, with the Z axis as the rotation center, the point cloud is divided into Nc sectors at equal angles. The area where the ring and the sector intersect is a segmentation unit, marked as Bin(i,j);

[0073] A point cloud segmentation model (SPVNAS) using sparse point-voxel convolution (SPVConv) and 3D neural architecture search (3D-NAS) to extract labels and confidences of point clouds in local maps;

[0074] Map the point clouds of all local maps into a point cloud image with semantic information weights, where the mapping formula is as follows:

[0075] Where sem(s i,j ) is the semantic information weight of the segmentation unit Bin(i,j), p i,j is the coordinate information of the segmentation unit Bin(i,j), f z (p i,j) is the height information, ∪· represents the union, and the information of each segmentation unit Bin(i,j) is spliced ​​into the point cloud semantic image B;

[0076] Convert all point cloud images into one dimension, generate factor vectors, and continue to store the factor vectors of each frame of point cloud;

[0077] Obtain a point cloud image B of any local map and another local map in the storage device p With B q , calculate the point cloud image B based on the cosine value p With point cloud image B q The similarity score between them is used, and the two point cloud images with the highest similarity score are used as the preliminary closed loop;

[0078] The formula for obtaining the similarity score is as follows:

[0079]

[0080] in is the point cloud image B q The image after segmentation and reorganization of the nth column, and They are point cloud images B p With point cloud image B q The factor vector of the k-th column in ;

[0081] Construct the least squares problem under the semantic ICP of point cloud, where the least squares problem is as follows:

[0082]

[0083] in is the relative position between the two frames of point cloud images, which is the variable to be optimized. and They are the original point cloud and the target point cloud. The target point cloud is the point cloud in the prepared closed loop. and are the covariance of the i-th point of the original point cloud and the i-th point of the target point cloud, w i is the semantic label matching degree between two points;

[0084] The least squares problem is optimized by the Levenberg-Marquardt optimizer, and the Hessian matrix of the optimization problem is subjected to eigenvalue decomposition;

[0085] It is determined by the eigenvalues ​​whether they are all positive. If the eigenvalues ​​are all positive, the Hessian matrix of the optimization problem of the least squares problem shown is a positive definite matrix. The preliminary closed loop is assumed to be a true closed loop. If there are negative characteristics, the preliminary closed loop is obtained again.

[0086] Preferably, the factor vector is stored in a KD-Tree structure.

[0087] One of the above technical solutions has the following advantages or beneficial effects: 1. The present invention proposes a registration factor that integrates light intensity information. First, according to the intensity of the light intensity, the mean of the light intensity is divided into three levels: strong, medium, and weak. For a certain voxel, the overall light intensity distribution of the points in the voxel is described by the weighted sum of three weighted Gaussian distributions corresponding to the mean of the light intensity, and a light intensity kernel function is established. The present invention solves the problem of insufficient environmental information by adding additional light intensity information. After adding the light intensity information, due to the existence of the light intensity weight, the registration factor will be biased towards places with strong light when processing, thereby solving the problem of no match or inability to match due to sparse point cloud. This solves the problem of odometer degradation when the feature points on the shore are sparse, and improves the overall robustness of the algorithm.

[0088] 2. When generating maps, the laser radar data, inertial measurement unit data, and global satellite navigation system measurement data are optimized through the set triple factor graph optimization framework, which can effectively improve the accuracy and robustness of unmanned boat positioning and mapping. BRIEF DESCRIPTION OF THE DRAWINGS

[0089] Figure 1 is a flow chart of an embodiment of the method of the present invention;

[0090] Figure 2 is a schematic diagram of the structure of a first factor graph in an embodiment of the method of the present invention;

[0091] Figure 3 is a flow chart of using the first factor graph in one embodiment of the method of the present invention;

[0092] Figure 4 is a schematic diagram of the structure of the second factor graph in an embodiment of the method of the present invention;

[0093] Figure 5 is a schematic diagram of the structure of the third factor graph in an embodiment of the method of the present invention;

[0094] Figure 6 It is a structural diagram of constructing a coordinate system in one embodiment of the method of the present invention. DETAILED DESCRIPTION

[0095] The embodiments of the present invention are described in detail below, and examples of the embodiments are shown in the accompanying drawings, wherein the same or similar reference numerals represent the same or similar elements or elements having the same or similar functions from beginning to end. The embodiments described below with reference to the accompanying drawings are exemplary and are only used to explain the present invention, and cannot be understood as limiting the present invention.

[0096] In the description of the embodiments of the present invention, the terms "first" and "second" are used for descriptive purposes only and are not to be understood as indicating or implying relative importance or implicitly indicating the number of the indicated technical features. Thus, the features defined as "first" and "second" may explicitly or implicitly include one or more of the features. In the description of the embodiments of the present invention, the meaning of "plurality" is two or more, unless otherwise clearly and specifically defined.

[0097] In addition, the terms "first" and "second" are used for descriptive purposes only and should not be understood as indicating or implying relative importance or implicitly indicating the number of the indicated technical features. Therefore, the features defined as "first" and "second" may explicitly or implicitly include one or more of the features. In the description of the present invention, unless otherwise specified, the meaning of "plurality" is two or more. For those of ordinary skill in the art, the specific meanings of the above terms in the present invention can be understood according to specific circumstances.

[0098] like Figures 1 to 6 As shown, a method for positioning and mapping an unmanned boat using a triple factor graph optimization method includes the following steps:

[0099] Step S1: constructing a first factor graph, wherein the first factor graph includes a registration factor, a marginalization prior factor, a guard factor, and a pre-integration factor;

[0100] Input the laser radar data, inertial measurement unit data, and global satellite navigation system measurement data into the first factor graph for optimization to obtain the position and posture of the unmanned boat;

[0101] Step S2: constructing a second factor graph, wherein the second factor graph includes a registration factor, a guard factor, and a pre-integration factor;

[0102] The odometer marginalized key frame is used as the input of the second factor graph, where the key frame includes the position and posture of the unmanned boat, and a local map is obtained;

[0103] Step S3: constructing a third factor graph, wherein the third factor graph includes a registration factor, a closed-loop factor, a navigation priori factor, a satellite guidance factor, and a pre-integration factor;

[0104] Use multiple local maps as input of the third factor graph to obtain a global map;

[0105] The registration factor integrates the light intensity information, and the residual acquisition formula of the specific registration factor is as follows:

[0106] where r i Li represents the residual of the registration factor, η ij is the light intensity weight, pi is the i-th point in the current scan of the laser radar in the carrier coordinate system, T i Represents point p i Translate to the world system, n is a non-negative integer, μ j is the mean value of light intensity, exp() represents the exponential function with natural base e as base;

[0107] in

[0108] I(p i ) is the light intensity kernel function, where where β j is the weight factor. The stronger the light intensity, the greater the weight factor β j The bigger, are the mean and covariance of light intensity respectively, and N() represents normal distribution.

[0109] For unmanned boats working in complex environments with alternating water and shore, the features collected by the laser radar are derived from the shore and have a certain sparsity. In order to improve the accuracy and robustness of the registration, the present invention proposes a registration factor that integrates light intensity information. First, according to the intensity of the light intensity, the mean of the light intensity is divided into three levels: strong, medium, and weak. For a certain voxel, the overall light intensity distribution of the points in the voxel is described by the weighted sum of three weighted Gaussian distributions corresponding to the mean of the light intensity, and a light intensity kernel function is established. The present invention solves the problem of insufficient environmental information by adding additional light intensity information. After adding the light intensity information, due to the existence of the light intensity weight, the registration factor will be biased towards places with strong light when processing, thereby solving the problem of no match or inability to match due to sparse point clouds. This solves the problem of odometer degradation when the feature points on the shore are sparse, and improves the overall robustness of the algorithm.

[0110] It is worth mentioning that, in the present invention, the marginalization prior factor, the guard factor and the pre-integration factor all adopt the factors provided by the conventional SLAM system, so no excessive description and explanation will be given here.

[0111] like Figure 2 As shown, preferably, the steps of constructing the first factor graph are as follows:

[0112] Step S11: construct a sliding window, wherein the sliding window only obtains the latest five states of the unmanned boat;

[0113] Step S21: Establish a factor graph for the state of the unmanned boat, wherein the establishment is regularized as follows:

[0114] Establishment of marginalization prior factors:

[0115] Establish marginalization prior factors for states at the edge of the sliding window;

[0116] Establishment of registration factors:

[0117] Get the latest three states, and establish a registration factor between the latest state and the previous state, and establish a registration factor between the latest state and the previous state;

[0118] Determine whether there is a key frame in the five states in the sliding window. If there is a key frame, establish a registration factor for the key frame; the state of the unmanned boat is the result of the laser radar scan, and the key frame is a scan obtained by the key frame selection algorithm in the laser radar scan result. The essence of the key frame is still a laser radar scan result.

[0119] Establishing a registration factor between the latest state and the marginalized prior factor;

[0120] Pre-integration factor establishment:

[0121] Establishing pre-integration factors between adjacent states;

[0122] Wei Dao points factor establishment:

[0123] It is determined whether a global satellite navigation system measurement signal is detected when the state is acquired. If detected, a satellite navigation factor is established for the corresponding state.

[0124] The registration factors of the first factor graph use a special connection method, instead of the dense registration factor connection between the two states like the following two factor graphs. This is mainly because the control system of the unmanned boat has high requirements for the real-time and smoothness of the odometer. The connection method of the registration factors of the first factor graph, firstly, because the first three states are registered, and the key frames in the sliding window are registered, and the marginalized frames are also registered, the accuracy and robustness of the odometer are guaranteed, which is more accurate than the ordinary odometer using the filtering algorithm. Secondly, no dense connection is used to ensure the real-time odometer. This structure is the best product of the trade-off between real-time and accuracy, that is, it does not discard key information and ensures real-time.

[0125] Preferably, the use process of the sliding window is as follows:

[0126] Remove the guard factor and use only the registration factor, pre-integration factor, and marginalization prior factor for sliding window optimization to obtain the residual of the first factor graph;

[0127] Perform eigenvalue decomposition of the Hessian matrix on the residual of the first factor graph to obtain the minimum eigenvalue of the Hessian matrix, and determine whether the minimum eigenvalue has an abnormality. If not, output the position and posture of the unmanned boat according to the residual of the first factor graph. If there is an abnormality, re-add the satellite guidance factor, add the robust kernel function to the satellite guidance factor, and re-execute the sliding window optimization.

[0128] Get the chi-square value of the guard factor and remove outliers based on the chi-square value result;

[0129] Remove the robust kernel function of the satellite guidance factor, re-execute the sliding window optimization, obtain the residual of the first factor graph, and output the position and posture of the unmanned boat according to the residual of the first factor graph;

[0130] The residual formula of the first factor graph is as follows:

[0131]

[0132] Where X is a set of state variables to be optimized, and its expression is X = (x1, ..., x5), and x t =[T t ,v t ,b t ], where T t is the position of the unmanned boat at time t, including three degrees of freedom translation and three degrees of freedom rotation, v t is the velocity value of the inertial measurement unit at time t, b t is the zero bias of the inertial measurement unit at time t, indicating The residuals are modulo and weighted using the covariance matrix, and their values ​​are the chi-square value of the factor, r i Li represents the residual of the i-th registration factor, Li is the identifier of the registration factor, r i imu represents the residual of the i-th pre-integration factor, r i imu , imu is the identifier of the pre-integration factor, r i gnss represents the residual of the ith guidance factor, gnss is the identifier of the guidance factor, r i marg represents the residual of the marginalized prior factor, marg is the identifier of the marginalized prior factor, ∑Li, ∑imu, ∑gnss, and ∑marg are the covariance matrices of the registration factor, pre-integration factor, guard factor, and marginalized prior factor, respectively.

[0133] like Figure 3As shown, in the present invention, there are three stages for the optimization of the first factor graph, wherein the first stage is to use the registration factor, the pre-integration factor, and the marginalization prior factor to perform sliding window optimization, obtain the residual of the first factor graph, and then perform the eigenvalue decomposition of the Hessian matrix on the residual of the first factor graph to obtain the minimum eigenvalue of the Hessian matrix. If the minimum eigenvalue is abnormal, it means that the Hessian matrix is ​​degraded, because the system at this time is not enough to provide a very accurate estimate, and the satellite guidance information is needed to correct the system. At this time, the second stage of sliding window optimization is performed, wherein in the sliding window optimization of the second stage, the satellite guidance factor is added to the sliding window optimization for assistance, but on the other hand, the signal of the satellite guidance factor has always been an unstable signal, which will be affected by many factors, such as buildings, trees, etc. At this time, a mechanism is needed to ensure the quality of the satellite guidance signal, and for this reason, in the second stage of the present invention, a robust kernel function is added to the satellite guidance factor, wherein the robust kernel function can be a Cauchy robust kernel function, which is used to reduce the influence of other things on the satellite guidance factor. The specific method is that after adding the robust kernel function, the outliers of the satellite guidance factor can be obtained through the chi-square value. These outliers are the drift or jump of the signal received by the receiver after the satellite signal is affected by other objects. After removing the outliers, the stability of the satellite guidance factor can be improved. After the satellite guidance factor is proposed, the residual of the first factor graph needs to be obtained again. At this time, since the satellite guidance factor tends to be stable, there is no need to consider the problem of outliers. Therefore, the robust kernel function in the satellite guidance factor is removed to reduce the meaningless waste of computing power.

[0134] Since the residual calculation uses the initial posture of the unmanned boat (a conventional technical means in the SLAM system), calculating the residual of the first factor graph is the optimization of the initial posture of the unmanned boat. Finally, the residual formula of the first factor graph can be used to obtain the corresponding posture of the unmanned boat.

[0135] Preferably, the specific steps in step S2 are as follows:

[0136] Determine whether there is a key frame in the odometer that is marginalized. If so, obtain the relative position and posture of the marginalized key frame, obtain the translation of the unmanned boat according to the relative position and posture, and determine whether the translation exceeds the threshold. If so, discard the key frame. If not, input the key frame into the local map module optimizer and insert the key frame into the second factor graph.

[0137] Determine whether the number of frames in the local map optimizer exceeds a threshold number, and if so, perform factor graph optimization to obtain an optimized state quantity, and obtain a local map according to the state quantity;

[0138] The formula for obtaining the relative pose is as follows:

[0139]

[0140] in is the pose corresponding to the marginalized keyframe, T i W The latest position of the unmanned boat;

[0141] The formula for obtaining the translation amount is as follows: in is the translation amount, is the rotation matrix;

[0142] The factor graph in the second factor graph is established as follows:

[0143] Adding a registration factor and a pre-integration factor between two adjacent states;

[0144] Determine whether the state detects a global satellite navigation system measurement signal when acquiring the state, and if so, establish a satellite navigation factor for the corresponding state;

[0145] The formula for obtaining the state quantity is as follows:

[0146]

[0147] like Figure 4 As shown in the second factor graph, factor graph optimization is to optimize the residual formula through the LM optimizer to obtain the optimized state quantity, where the state quantity involved in the optimization is the point cloud involved in the optimization. By mapping the optimized point cloud to the global coordinate system, the corresponding local map can be obtained. Since this method is a common technical means of SLAM, it will not be described in detail here.

[0148] Preferably, the specific steps of step S3 are as follows:

[0149] The local map x lk As input, it is fed into the global map optimizer;

[0150] Construct a third factor graph for the input of the global map optimizer;

[0151] Periodically optimize the factor graph to obtain the residual of the third factor graph, and construct a global map based on the residual of the third factor graph;

[0152] The factor graph in the third factor graph is established as follows:

[0153] Establishment of registration factors:

[0154] In two adjacent local maps x lk With the local map x lk+1 Add a registration factor between

[0155] Establishment of navigation prior factors:

[0156] Get local map x lk The two endpoint states are recorded as and Two endpoint states With endpoint status Add navigation prior factors;

[0157] Establishment of pre-integration factor:

[0158] In a local map x lk Endpoint status With the next local map x lk+1 Endpoint status Add a pre-integration factor between

[0159] Establishment of the protective factor

[0160] Determine the local map x lk Whether the global satellite navigation system measurement signal is detected during acquisition, if it is detected, the corresponding local map x lk Establishment of protective factors;

[0161] Establishment of closed-loop factors:

[0162] In two adjacent local maps x lk With the local map x lk+1 Add a closed-loop factor between them;

[0163] The residual formula for the third factor graph is as follows:

[0164]

[0165] where r i loop is the residual of the closed-loop factor, and ∑loop is the covariance matrix of the closed-loop factor.

[0166] In the third factor graph, Figure 5 As shown in the figure, the workflow of the third factor graph is the same as that of the second factor graph. The factor graph optimization optimizes the residual formula through the LM optimizer, inputs the local map, and then maps the local map to the global coordinate system to obtain the corresponding global map.

[0167] It is worth mentioning that, in the present invention, obtaining the local map x lk The two endpoint states are separated by a pre-integration factor, and a pre-integration factor is added to the adjacent endpoint states to avoid the loss of velocity and zero bias prior information of the adjacent endpoint state quantities, thereby improving the stability of the algorithm.

[0168] Preferably, before synthesizing the global map in step S3, loop closure detection is also required;

[0169] The steps of the closed-loop detection are as follows:

[0170] A three-dimensional coordinate system is constructed, with the unmanned boat as the origin of the coordinate system, the horizontal forward direction as the X-axis direction of the coordinate system, the vertical upward direction as the Z-axis direction, and the Y-axis direction meeting the requirements of the Cartesian coordinate system;

[0171] In the top view direction of the point cloud of the local map, the point cloud is divided into several areas according to the division rules, where the division rules are as follows: with the origin as the center, the point cloud is divided into Nr rings at equal distances. At the same time, with the Z axis as the rotation center, the point cloud is divided into Nc sectors at equal angles. The area where the ring and the sector intersect is a segmentation unit, marked as Bin(i,j);

[0172] A point cloud segmentation model (SPVNAS) using sparse point-voxel convolution (SPVConv) and 3D neural architecture search (3D-NAS) to extract labels and confidences of point clouds in local maps;

[0173] Map the point clouds of all local maps into a point cloud image with semantic information weights, where the mapping formula is as follows:

[0174] Where sem(s i,j ) is the semantic information weight of the segmentation unit Bin(i,j), p i,j is the coordinate information of the segmentation unit Bin(i,j), f z (p i,j ) is the height information, ∪· represents the union, and the information of each segmentation unit Bin(i,j) is spliced ​​into the point cloud semantic image B;

[0175] Convert all point cloud images into one dimension, generate factor vectors, and continue to store the factor vectors of each frame of point cloud;

[0176] Obtain a point cloud image B of any local map and another local map in the storage device p With B q , calculate the point cloud image B based on the cosine value p With point cloud image B q The similarity score between them is used, and the two point cloud images with the highest similarity score are used as the preliminary closed loop;

[0177] The formula for obtaining the similarity score is as follows:

[0178]

[0179] in is the point cloud image B q The image after segmentation and reorganization of the nth column, and They are point cloud images B p With point cloud image B q The factor vector of the k-th column in ;

[0180] Construct the least squares problem under the semantic ICP of point cloud, where the least squares problem is as follows:

[0181]

[0182] in is the relative position between the two frames of point cloud images, which is the variable to be optimized. and They are the original point cloud and the target point cloud. The target point cloud is the point cloud in the prepared closed loop. and are the covariance of the i-th point of the original point cloud and the i-th point of the target point cloud, w i is the semantic label matching degree between two points;

[0183] The least squares problem is optimized by the Levenberg-Marquardt optimizer, and the Hessian matrix of the optimization problem is subjected to eigenvalue decomposition;

[0184] It is determined by the eigenvalues ​​whether they are all positive. If the eigenvalues ​​are all positive, the Hessian matrix of the optimization problem of the least squares problem shown is a positive definite matrix. The preliminary closed loop is assumed to be a true closed loop. If there are negative characteristics, the preliminary closed loop is obtained again.

[0185] In order to reduce the global cumulative error and improve the consistency of the global map, loop closure detection needs to be considered. The traditional loop closure detection method based on Euclidean distance is difficult to close the loop successfully in large-scale scenes; the performance of the loop closure detection method based on geometric descriptors will decrease in dynamic scenes; the loop closure based on the global satellite navigation system is easily affected by satellite signals.

[0186] In order to solve the above problems, this patent proposes a closed loop detection based on semantic descriptors. The closed loop detection includes a geometric descriptor that maps the point cloud from three dimensions to 2.5 dimensions (XY plus height) and then to one dimension, such as Figure 6As shown in the figure, the search goes from one dimension to 2.5 dimensions, and finally the three dimensions are judged based on the closed-loop information of the 2.5-dimensional image. This structure can greatly reduce the computing power consumption of closed-loop detection, and the closed-loop can be opened throughout the process without affecting the real-time performance of the system. At the same time, based on this geometric descriptor, semantic information is added, which can greatly increase the robustness of closed-loop detection. Finally, semantic ICP is used to detect candidate closed loops. The significance of this step is to ensure that the obtained closed loop is not a pseudo closed loop, because the pseudo closed loop system has a greater impact on accuracy. The significance of using semantic ICP is that compared with traditional ICP, semantic ICP has higher accuracy.

[0187] Preferably, the factor vector is stored in a KD-Tree structure.

[0188] The KD-Tree structure is a commonly used data index structure, which can be used to quickly index and find the corresponding factor vector.

[0189] In the description of this specification, the description with reference to the terms "one embodiment", "some embodiments", "illustrative embodiments", "examples", "specific examples", or "some examples" means that the specific features, structures, materials or characteristics described in conjunction with the embodiment or example are included in at least one embodiment or example of the present invention. In this specification, the schematic representation of the above terms does not necessarily refer to the same embodiment or example. Moreover, the specific features, structures, materials or characteristics described may be combined in any one or more embodiments or examples in a suitable manner.

[0190] Although the embodiments of the present invention have been shown and described above, it is to be understood that the above embodiments are exemplary and are not to be construed as limitations of the present invention. A person skilled in the art may make changes, modifications, substitutions and variations to the above embodiments within the scope of the present invention.

Claims

1. A triple factor graph optimized unmanned boat positioning and mapping method, It is characterized in that The following steps are involved: Step S1: constructing a first factor graph, wherein the first factor graph includes a registration factor, a marginalization prior factor, a guard factor, and a pre-integration factor; Input the laser radar data, inertial measurement unit data, and global satellite navigation system measurement data into the first factor graph for optimization to obtain the position and posture of the unmanned boat; Step S2: constructing a second factor graph, wherein the second factor graph includes a registration factor, a guard factor, and a pre-integration factor; The odometer marginalized key frame is used as the input of the second factor graph, where the key frame includes the position and posture of the unmanned boat, and a local map is obtained; Step S3: constructing a third factor graph, wherein the third factor graph includes a registration factor, a closed-loop factor, a navigation priori factor, a satellite guidance factor, and a pre-integration factor; Use multiple local maps as input of the third factor graph to obtain a global map; The registration factor integrates the light intensity information, and the residual acquisition formula of the specific registration factor is as follows: where r i Li represents the residual of the registration factor, η ij is the light intensity weight, p i is the i-th point in the current scan of the laser radar in the carrier coordinate system, T i Represents point p i Translate to the world system, n is a non-negative integer, μ j is the mean value of light intensity, exp() represents the exponential function with natural base e as base; in I(p i ) is the light intensity kernel function, where where β j is the weight factor. The stronger the light intensity, the greater the weight factor β j The bigger, are the mean and covariance of light intensity respectively, and N() represents normal distribution.

2. The unmanned boat positioning and mapping method based on triple factor graph optimization according to claim 1, It is characterized in that The steps for constructing the first factor graph are as follows: Step S11: construct a sliding window, wherein the sliding window only obtains the latest five states of the unmanned boat; Step S21: Establish a factor graph for the state of the unmanned boat, wherein the establishment is regularized as follows: Establishment of marginalization prior factors: Establish marginalization prior factors for states at the edge of the sliding window; Establishment of registration factors: Get the latest three states, and establish a registration factor between the latest state and the previous state, and establish a registration factor between the latest state and the previous state; Determine whether there is a key frame in the five states in the sliding window. If there is a key frame, establish a registration factor for the key frame; Establishing a registration factor between the latest state and the marginalized prior factor; Pre-integration factor establishment: Establishing pre-integration factors between adjacent states; Wei Dao points factor establishment: It is determined whether a global satellite navigation system measurement signal is detected when the state is acquired. If detected, a satellite navigation factor is established for the corresponding state.

3. The unmanned boat positioning and mapping method based on triple factor graph optimization according to claim 2, It is characterized in that The use process of the sliding window is as follows: Remove the guard factor and use only the registration factor, pre-integration factor, and marginalization prior factor for sliding window optimization to obtain the residual of the first factor graph; Perform eigenvalue decomposition of the Hessian matrix on the residual of the first factor graph to obtain the minimum eigenvalue of the Hessian matrix, and determine whether the minimum eigenvalue has an abnormality. If not, output the position and posture of the unmanned boat according to the residual of the first factor graph. If there is an abnormality, re-add the satellite guidance factor, add the robust kernel function to the satellite guidance factor, and re-execute the sliding window optimization. Get the chi-square value of the guard factor and remove outliers based on the chi-square value result; Remove the robust kernel function of the satellite guidance factor, re-execute the sliding window optimization, obtain the residual of the first factor graph, and output the position and posture of the unmanned boat according to the residual of the first factor graph; The residual formula of the first factor graph is as follows: Where X is a set of state variables to be optimized, and its expression is X = (x 1 ,…,x 5 ), and x t =[T t ,v t ,b t ], where T t is the position of the unmanned boat at time t, including three degrees of freedom translation and three degrees of freedom rotation, v t is the velocity value of the inertial measurement unit at time t, b t is the zero bias of the inertial measurement unit at time t, indicating The residuals are modulo and weighted using the covariance matrix, and their values ​​are the chi-square value of the factor, r i Li represents the residual of the i-th registration factor, Li is the identifier of the registration factor, r i imu represents the residual of the i-th pre-integration factor, r i imu , imu is the identifier of the pre-integration factor, r i gnss represents the residual of the ith guidance factor, gnss is the identifier of the guidance factor, r i marg represents the residual of the marginalized prior factor, marg is the identifier of the marginalized prior factor, ∑Li, ∑imu, gnss, and ∑marg are the covariance matrices of the registration factor, pre-integration factor, guard factor, and marginalized prior factor, respectively.

4. The method for positioning and mapping an unmanned boat using a triple factor graph optimization method according to claim 3, It is characterized in that The specific steps in step S2 are as follows: Determine whether there is a key frame in the odometer that is marginalized. If so, obtain the relative position and posture of the marginalized key frame, obtain the translation of the unmanned boat according to the relative position and posture, and determine whether the translation exceeds the threshold. If so, discard the key frame. If not, input the key frame into the local map module optimizer and insert the key frame into the second factor graph. Determine whether the number of frames in the local map optimizer exceeds a threshold number, and if so, perform factor graph optimization to obtain an optimized state quantity, and obtain a local map according to the state quantity; The formula for obtaining the relative pose is as follows: in is the pose corresponding to the marginalized keyframe, T i W The latest position of the unmanned boat; The formula for obtaining the translation amount is as follows: in is the translation amount, is the rotation matrix; The factor graph in the second factor graph is established as follows: Adding a registration factor and a pre-integration factor between two adjacent states; Determine whether the state detects a global satellite navigation system measurement signal when acquiring the state, and if so, establish a satellite navigation factor for the corresponding state; The formula for obtaining the state quantity is as follows:

5. The unmanned boat positioning and mapping method based on triple factor graph optimization according to claim 1, It is characterized in that The specific steps of step S3 are as follows: The local map x lk As input, it is fed into the global map optimizer; Construct a third factor graph for the input of the global map optimizer; Periodically optimize the factor graph to obtain the residual of the third factor graph, and construct a global map based on the residual of the third factor graph; The factor graph in the third factor graph is established as follows: Establishment of registration factors: In two adjacent local maps x lk With the local map x lk+1 Add a registration factor between Establishment of navigation prior factors: Get local map x lk The two endpoint states are recorded as and Two endpoint states With endpoint status Add navigation prior factors; Establishment of pre-integration factor: In a local map x lk Endpoint status With the next local map x lk+1 Endpoint status Add a pre-integration factor between Establishment of the protective factor Determine the local map x lk Whether the global satellite navigation system measurement signal is detected during acquisition, if it is detected, the corresponding local map x lk Establishment of protective factors; Establishment of closed-loop factors: In two adjacent local maps x lk With the local map x lk+1 Add a closed-loop factor between them; The residual formula for the third factor graph is as follows: where r i loop is the residual of the closed-loop factor, and ∑loop is the covariance matrix of the closed-loop factor.

6. The method for positioning and mapping an unmanned boat using triple factor graph optimization according to claim 5, It is characterized in that Before synthesizing the global map in step S3, loop closure detection is also required; The steps of the closed-loop detection are as follows: A three-dimensional coordinate system is constructed, with the unmanned boat as the origin of the coordinate system, the horizontal forward direction as the X-axis direction of the coordinate system, the vertical upward direction as the Z-axis direction, and the Y-axis direction meeting the requirements of the Cartesian coordinate system; In the top view direction of the point cloud of the local map, the point cloud is divided into several areas according to the division rules, where the division rules are as follows: with the origin as the center, the point cloud is divided into Nr rings at equal distances. At the same time, with the Z axis as the rotation center, the point cloud is divided into Nc sectors at equal angles. The area where the ring and the sector intersect is a segmentation unit, marked as Bin(i,j); A point cloud segmentation model (SPVNAS) using sparse point-voxel convolution (SPVConv) and 3D neural architecture search (3D-NAS) to extract labels and confidences of point clouds in local maps; Map the point clouds of all local maps into a point cloud image with semantic information weights, where the mapping formula is as follows: Where sem(s i,j ) is the semantic information weight of the segmentation unit Bin(i,j), p i,j is the coordinate information of the segmentation unit Bin(i,j), f z (p i,j ) is the height information, ∪• represents the union, and the information of each segmentation unit Bin(i,j) is spliced ​​into the point cloud semantic image B; Convert all point cloud images into one dimension, generate factor vectors, and continue to store the factor vectors of each frame of point cloud; Obtain a point cloud image B of any local map and another local map in the storage device p With B q , calculate the point cloud image B based on the cosine value p With point cloud image B q The similarity score between them is used, and the two point cloud images with the highest similarity score are used as the preliminary closed loop; The formula for obtaining the similarity score is as follows: in is the point cloud image B q The image after segmentation and reorganization of the nth column, and They are point cloud images B p With point cloud image B q The factor vector of the k-th column in ; Construct the least squares problem under the semantic ICP of point cloud, where the least squares problem is as follows: in is the relative position between the two frames of point cloud images, which is the variable to be optimized. and They are the original point cloud and the target point cloud. The target point cloud is the point cloud in the prepared closed loop. and are the covariance of the i-th point of the original point cloud and the i-th point of the target point cloud, w i is the semantic label matching degree between two points; The least squares problem is optimized by the Levenberg-Marquardt optimizer, and the Hessian matrix of the optimization problem is subjected to eigenvalue decomposition; It is determined by the eigenvalues ​​whether they are all positive. If the eigenvalues ​​are all positive, the Hessian matrix of the optimization problem of the least squares problem shown is a positive definite matrix. The preliminary closed loop is assumed to be a true closed loop. If there are negative characteristics, the preliminary closed loop is obtained again.

7. The method for positioning and mapping unmanned boats using triple factor graph optimization according to claim 6, It is characterized in that The factor vectors are stored in the KD-Tree structure.

Images