CN116758153A

CN116758153A - Multi-factor graph-based back-end optimization method for accurate pose acquisition of robot

Info

Publication number: CN116758153A
Application number: CN202310632057.2A
Authority: CN
Inventors: 沈志熙; 胡琴; 韦佳汛; 向军; 李涛; 付鹏
Original assignee: Chongqing University
Current assignee: Chongqing University
Priority date: 2023-05-31
Filing date: 2023-05-31
Publication date: 2023-09-15

Abstract

The invention relates to a back-end optimization method based on a multi-factor graph for acquiring the accurate pose of a robot, which is used for constructing an improved multi-factor graph model, firstly, carrying out pre-integration on IMU data to obtain an observation model error of an IMU sensor and constructing an IMU factor, simultaneously, calculating the observation model error of a laser radar sensor by utilizing the pose change relation output by a front-end laser odometer, constructing a laser odometer factor, constructing a closed-loop factor when loop success is detected, then connecting the three constraint factors with an associated state variable by edges, and adding the three constraint factors into a factor graph basic model. The method improves the accuracy and the efficiency of loop detection, calculates the pose transformation between two frame point clouds forming the loop after the loop is successfully detected, builds a closed loop factor, adds the closed loop factor into a multi-factor graph model for optimization, and further improves the precision of pose estimation in back-end optimization.

Description

Multi-factor graph-based back-end optimization method for accurate pose acquisition of robot

Technical Field

The invention relates to the technical field of laser SLAM, in particular to an optimization method based on a multi-factor graph for acquiring the precise pose of a robot.

Background

After the pose of the laser odometer of the robot is obtained by the traditional SLAM method, the obtained positioning and map result still has larger error. Particularly, under the condition of long movement mileage of the robot, even if the accuracy of the laser mileage calculation method is higher, the laser mileage calculation method is affected by accumulated errors, so that the accuracy of positioning and mapping is reduced. Therefore, the SLAM method also needs to further optimize the result at the rear end to obtain a more accurate pose positioning result of the robot.

In SLAM, the back-end optimization not only can filter and correct the sensor data, but also can reduce noise and errors and improve the stability and the robustness of the system; the map information at different times and locations can also be fused and corrected to optimize the consistency and consistency of the map. The back-end optimization algorithm mainly comprises a filter-based method and a nonlinear (graph optimization) -based method. The filter algorithm, although fast in calculation speed, has relatively low estimation accuracy and is only suitable for small-scale, linear or approximately linear systems. The most representative of the nonlinear optimization algorithms is a factor graph-based optimization algorithm.

However, the conventional factor-based graph back-end optimization method still has the following problems:

(1) In order to solve the problem that a single sensor is used for positioning and mapping, the defect that scene change causes characteristic degradation can be overcome, and more accurate positioning and mapping can be realized by fusing a plurality of sensors, for example, using IMU, GPS and other data to perform back-end optimization. And the loosely coupled scheme of the laser radar and the IMU has high calculation efficiency, but the accumulated error of pose estimation is large.

(2) Loop detection is an important link of back-end optimization, and the existing work about loop detection generally utilizes the local or global geometric features of point cloud to encode, generate a local descriptor or global descriptor of the point cloud, and perform matching identification on the point cloud descriptor. Wherein local descriptors are susceptible to noise and re-recognition requires a large amount of geometric computation. The global descriptor can effectively capture the overall form information of the object, so that the global descriptor has better robustness, but an improper feature coding mode can seriously influence the calculation efficiency of position identification, and further influence the performance of back-end optimization.

Disclosure of Invention

Aiming at the problems existing in the prior art, the invention aims to solve the technical problems that: how to obtain the accurate pose of the robot.

In order to solve the technical problems, the invention adopts the following technical scheme: a back-end optimization method based on a multi-factor graph for acquiring the precise pose of a robot comprises the following steps:

s100: construction of an improved multi-factor graph model F (X):

in the formula f_(i,j) (X _i ,X _j ) Representing the relation factor between the pose node of the ith frame and the pose node of the jth frame, solid line box C _i Dashed box F _i The black node is divided into IMU factors f _IMU Laser odometer factor f _Lidar And a closed loop factor f _Loop ，f _IMU ＝f _IMU (X _i ,X _i+1 )、f _Lidar ＝f _Lidar (X _i ,X _i+1 )，f _Loop ＝f _Loop (X _c ,X _i+1 ) E represents all edges associated with a node in F (X).

The back-end optimized objective function is converted into a solution least squares problem, expressed as:

in the formula And->Error of IMU factor, laser odometer factor and closed loop factor observation model are respectively represented, and +.> And->Respectively the corresponding measurement noise covariance. Wherein, the error term can be uniformly expressed as:

in the formula T_i And T is _j Pose of ith and jth keyframes of the robot in world coordinate system, delta T _i,j Representing the relative pose transformation relationship between two variable nodes calculated by three factors, deltaT _i,j IncludedAnd

s200: f outputting the content _IMU and f_Lidar Incrementally adding the motion detection information to F (X), and adding F into F (X) when the mobile robot accesses the same place in the motion process, namely after loop detection is successful _Loop Then, the related variable nodes are subjected to pose optimization according to the updating of the factor nodes, and based on the optimized state set X ^* Projection image M established by front-end mileage timing _k And (3) re-projecting the corresponding point cloud to a world coordinate system to update the precise pose of the robot.

Preferably, f is obtained in S100 _IMU The process of (2) is as follows:

first, assume that the original angular velocity and acceleration output by the IMU at time t are usedAnd->The representation is known from IMU measurement models:

in the formula A rotation matrix representing the map coordinate system W to the sensor coordinate system B, g is the gravitational acceleration in W, ω _t and a_t Respectively representing the actual angular velocity and the actual acceleration of the laser radar at the moment t;

and />IMU zero bias, IMU zero bias of acceleration, IMU zero bias of original angular velocity, IMU zero bias of acceleration, IMU zero bias> and />Collectively referred to as IMU zero offset b _t ，/> and />White noise representing the original angular velocity and the acceleration, respectively, +.> and />Collectively referred to as n _t ；

At time t+ [ delta ] t, the speed v of the robot _t+Δt Position p of robot _t+Δt And rotation transformation R _t+Δt Calculated from the following formulas:

wherein ,R_t ，p _t ，v _t The rotation conversion, position and speed of the lidar at time t are shown, respectively, and Δt represents a short time.

A second step of calculating the relative movement change between the moments i and j of the robot, the speed change Deltav in the period of time _ij Position conversion Δp _ij And rotation transformation DeltaR _ij Calculated from the following formulas:

wherein ,v_i ，p _i ，R _i Indicating the speed, position and rotation of the lidar at time i, v _j ，p _j ，R _j Respectively representing the speed, position and rotation transformation of the laser radar at the moment j, delta t _ij The time difference between the instant i and the instant j is shown.

Then, using open source GTSAM optimizing library to optimize IMU speed change Deltav _ij Position conversion Δp _ij And rotation transformation DeltaR _ij Three state variables. The GTSAM optimizer outputs updated position, velocity and rotation transforms and zero offset matrix b before the next pre-integration result is input _i ′。

With updated zero offset b _i ' recalculating the pre-integral by the following equation to obtain the speed of the optimized IMU at the beta momentPosition->And rotation transformation->

wherein ,b_i ′ ^a and b_i ′ ^ω IMU zero offset representing the updated original angular velocity and IMU zero offset representing the updated acceleration respectively, and />White noise representing the original angular velocity at the i-th time and white noise representing the acceleration at the i-th time.

The relative pose change between the two determined moments is obtained through three-stage pre-integral calculation, and then the relative pose change is obtainedThe calculation formula is as follows:

the updated i and j time points are respectively represented by a speed change, a position change and a rotation change.

f _IMU Representing observed model errors of IMU sensor between i frame and i+1 frameObtained by pre-integrationSubstituting formula (41) to obtain the observation model error of the IMU sensor between the i frame and the j frame>The same method can be used to obtain +.>

Preferably, f is obtained in S100 _Lidar The process of (2) is as follows:

the robot laser odometer outputs pose transformation relation delta T between two adjacent key frames i and i+1 _i,i+1 Will DeltaT _i,i+1 The error term used to calculate the laser odometer factor is expressed as:

f _Lidar representing observed model errors of a lidar sensor between i frames and i+1 framesWill be DeltaT _i,i+1 Substitution into formula (41) gives +.>

Preferably, f is obtained in S100 _Loop The process of (2) is as follows:

obtaining accurate pose transformation delta T between two point sets by using a traditional iterative nearest point feature matching method, and setting the pose of the current point cloud as T _i+1 The pose of the loop frame is T _c The relative pose change between the two is obtained, namely the closed loop factor is expressed as:

wherein ,

f _Loop indicating loop detection of observed model errors between the i+1st frame of the current frame and the two frames of the c frame of the loop frameWill->Substitution into formula (41) gives +.>

Preferably, the loop detection in S200 is as follows:

s201: the point cloud obtained by the laser radar obtaining the environment information comprises distance information and intensity information. Normalizing and calibrating the intensity information by an equation (59) to obtain eta _k ' suppose that each point cloud in the kth frame is denoted as [ x ] _k ,y _k ,z _k ,η _k ′]Projecting the 3D point cloud under the Cartesian coordinate system to a polar coordinate system formed by a X Y plane, wherein the transformed point cloud has polar coordinatesExpressed as:

s202: then equally dividing the point cloud into N parts by taking the sensor as the center through the annular angle and the radial direction _s ×N _r Each segment is represented as:

wherein a is E [1, N _s ]，b∈[1,N _r ]，L _max Indicating that the lidar is scanning the largest sensing range in turn. Because the intensity value returned by the same object is the same for each small region S _ab The point cloud in the space is replaced by the maximum intensity information in the subarea _ab ：

S203: extracts each sub-region S _ab After the intensity information of (a), the point set in the region is further encoded, and each sub-region S is then encoded _ab The point cloud in the system is divided into 8 units in the vertical direction and is marked as z from bottom to top _k Where k e (1, 2,., 8), points with the same z-coordinate are in the same cell. If there is no laser spot in a cell, then z will be _k Set to zero, otherwise, if there is at least one laser spot in a cell, then z _k Set to 1. Thus, each sub-region S _ab An 8-bit binary code is obtained and then based on z _k The value converts binary into decimal value, the geometric information d of each region _ab Can be expressed as:

wherein z (·) represents S _ab Is converted into a function of decimal values. Geometric information in the formulaCombining the intensity information in equation (64) yields a point cloud global description Fu (a, b):

s204: a two-step layered rapid detection method is adopted, geometrical similarity and intensity similarity are calculated respectively to accelerate matching, and then the matching effectiveness is verified based on consistency.

S204-1: geometric similarity calculation

Representing global descriptors of a current point cloud as Ω ^q The global descriptor of the point cloud to be matched is omega ^c The geometric coding values at the same index are respectivelyAnd->Calculate->And->And normalizing the cosine distance of (2), then obtaining a geometric similarity function:

wherein ,N_S Represents the aforementioned N _S Sectors d ^q ，d ^c Representing the current point cloud global description Fu respectively ^q And a point cloud global description Fu to be matched ^c Corresponding geometric information in the model (a).

Assume that the global descriptor of a point cloud isWhere n represents the global descriptor of the point cloudN units of translation are made in the horizontal direction. Column displacement number n ^* And a moved geometric similarity score D (D ^q ,d ^c ) Respectively defined as:

s204-2: intensity similarity calculation

Is provided with and />Respectively eta ^q and η^c Column with the same index value, get +> and />Calculate intensity similarity score +.>The following are provided:

wherein ,η^q and η^c Respectively representing the intensity characteristics between two point cloud descriptors,respectively representing intensity similarity scores.

By comparing eta ^c and η^q Translation n ^* Intensity features after columnThe final calculation result of the calculation intensity similarity is as follows:

n ^* indicating a change in viewing angle.

S204-3: consistency verification

Using the average value of the similarity of the past N frame point cloud descriptors of the current frame and the past N frame point cloud descriptors of the K candidate frames as the final similarity, and taking the final similarity as a verification standard of consistency, wherein the calculation formula is as follows:

wherein Θ(p_q ,p _c ) Average value of similarity of past N frame point cloud descriptors representing current frame and past N frame point cloud descriptors of one frame in K candidate frames, p _q Representing the current frame, p _c Representing a frame of K candidate frames, d _q-i ，d _c-i ，η _q-i ，η _c-i And respectively representing geometric information and intensity information in the point cloud global descriptors of the current frame and the candidate frame.

In the case of a reverse access of the mobile robot, i.e. a change in the angle of view observed by the sensor, Ω _q-i Correspondingly change to omega _q+i . And constructing a K neighbor search tree by using the obtained K objects to be matched, comparing the descriptors of the target point cloud with the search tree, and measuring the similarity between the descriptors by adopting a formula (72). Finally, the most similar candidate point is selected by setting a threshold, and is used as a revisit position, which is expressed as:

Where C represents the index values of K objects to be matched in the search tree,is a set threshold. c ^* Representing the finally obtained closed loop position index value, and obtaining the pose T of the loop frame from the historical key frame _c And is used for subsequent construction of the closed loop factor.

Compared with the prior art, the invention has at least the following advantages:

according to the invention, IMU factors are added into the existing factor graph optimization model, besides pose constraint constructed by the SLAM frame front-end laser odometer, the IMU factors are constructed by utilizing the observation data of the inertial sensor, so that tight coupling between the IMU and the laser radar sensor is realized, and meanwhile, the real-time property of SLAM is met by introducing a point cloud key frame and a sliding window strategy.

According to the method, a novel loop detection method based on the point cloud global descriptor is fused, the accuracy and the efficiency of loop detection are improved, the pose transformation between two frames of point clouds forming a loop is calculated after the loop is successfully detected, a closed loop factor is built and added into a multi-factor graph model for optimization, and then the precision of pose estimation in SLAM method back-end optimization is improved.

Drawings

FIG. 1 is a flow chart of a laser odometer estimation method.

FIG. 2 is a framework diagram of a FCNN-based split network model.

Fig. 3 is a frame diagram of a multi-target tracking method.

Fig. 4 is a frame diagram of the method of the present invention.

FIG. 5 is a modified multi-factor graph optimization model.

Fig. 6 is a frame diagram of a loop detection method.

Fig. 7 is a schematic view of point cloud segmentation.

Fig. 8 is a schematic diagram of geometric information coding.

FIG. 9 is a Bayesian network model

Fig. 10 is a factor graph model.

Fig. 11 shows a global track comparison before and after the loop checking function is started, wherein (a) is a 02-sequence open closed loop, (b) is a 02-sequence closed loop, (c) is a 05-sequence open closed loop, and (d) is a 05-sequence closed loop.

Detailed Description

The present invention will be described in further detail below.

The estimation method of the laser odometer comprises the following steps:

a laser odometer estimation method based on dynamic target tracking comprises the following steps:

s1: preprocessing all laser points in each frame of laser radar point cloud acquired by the laser radar, and detecting potential dynamic targets by the preprocessed point cloud by adopting a point cloud segmentation network based on FCNN;

s2: performing state estimation on the detected potential dynamic target by using a Kalman filtering method to obtain an optimal estimated value of the centroid state of the potential dynamic target at the moment t wherein ,/>Plane coordinates representing potential dynamic targets at time t, < - >Representing the speed of a potential dynamic target at the time t;

s3: calculating geometric information and intensity information of potential dynamic targets;

s4: fusing geometric information and intensity information of potential dynamic targets, detecting all the potential dynamic targets by using a correlation method, judging the potential dynamic targets as static targets according to the speed of the potential dynamic targets calculated in S2 if the speed at the moment t of the speed of the potential dynamic targets is 0, otherwise, judging the potential dynamic targets as dynamic targets

S5: extracting feature points of the static target through curvature;

s6: different weights are given to the feature points through curvature, then a pose constraint function is constructed based on the weighting function, and pose estimation of a static target is obtained through solving;

s7: performing distortion compensation update on the laser points corresponding to the static targets based on the sliding window, the key frames and the pose estimation obtained in the step S6 to obtain the latest laser odometer, namely obtaining the pose transformation relation delta T of the adjacent key frames _k,k+1 And the updated latest coordinates of the laser points corresponding to the static targets. The pose transformation of the adjacent two-frame laser radar can be obtained through the pose transformation of the adjacent two-frame static target point cloud, and the pose estimation of the current frame of the laser radar can be calculated through the pose transformation relation of the current frame relative to the previous frame and the pose of the previous frame of the laser radar. The initial pose of the laser radar is obtained by IMU pre-integration, then pose transformation of two adjacent frames of laser radar can be obtained by pose transformation of two adjacent frames of static target point clouds, and pose estimation of the current frame of the laser radar can be calculated through the pose transformation relation of the current frame relative to the previous frame and the pose of the previous frame of laser radar.

Specifically, the step of preprocessing all the laser points in the point cloud acquired by the laser radar in S1 includes the following steps:

when the line mechanical laser radar rotates to acquire environment three-dimensional information, the frequency is 10HZ, namely only 0.1s is needed for scanning one circle, which results in that the robot is in the moving process, and because of the rotation or translation of the robot, all points in the point cloud of the laser radar in one frame may not be acquired at the same position. In the forward movement process of the carrier, the coordinate systems of the point clouds acquired by the same frame are different. For the generated motion distortion, the distortion of the laser point cloud must be eliminated by data compensation, i.e. the point cloud of a certain frame is re-projected under the same time coordinate system.

Let the laser point cloud P collected by the kth scan _k Representing that the point cloud distortion is compensated by P _k Is re-projected to t _k And (5) under a radar coordinate system at the end of the moment. By utilizing the transformation relation between the laser radar coordinate system and the IMU coordinate systemCan map the laser point cloud to an IMU coordinate systemThe following steps:

in the formula p_I And p is as follows _L Respectively representing the laser point in the IMU coordinate system and the laser point in the radar coordinate system.

Because the point cloud acquired by the laser radar only contains geometric information and intensity information and does not contain an acquisition sequence, serialization operation is needed to be carried out on the point cloud before interpolation processing is carried out on the point cloud data. The laser radar rotates at constant angular velocity at constant speed, and can number point clouds according to angle difference value based on the time sequence of scanning a circle of point clouds, and theta _e Represents the angle of the ending scan line with respect to the starting line, θ _n For the angle of the current scan line relative to the start line, the moment at which the current point is located is expressed as:

wherein ,t_s Indicating the laser radar initial scanning time, and f indicating the laser radar rotation frequency.

After serializing the unordered point cloud, finding the t based on time sequence _k Two frames of IMU data with the nearest moment are subjected to pre-integration calculation, and then the calculation result is projected to the world coordinate system W, so that t is obtained _k IMU posture at moment of time WIn the same way, t can be obtained _k+1 Moment IMU gesture->Based on this, t can be obtained _k+1 Time of day relative to t _k Pose at moment:

finally throughPerforming transformation calculation to obtain a transformation value at t _n Motion compensation transformation matrix for each laser point at time instant +.>

Realizing the compensation of motion distortion and obtaining a compensated laser point

Specifically, the process of detecting the potential dynamic target by the FCNN-based point cloud segmentation network in S1 is as follows:

s1.1: performing dimension reduction on the preprocessed point cloud, dividing a 3D space of the preprocessed point cloud into areas according to bird's eye view projection, establishing a 2D index of the areas, and extracting characteristic information of point columns in grids in a projection diagram, wherein the characteristic information is used for generating input of an FCNN network, namely a two-dimensional characteristic diagram;

Specifically, 8 pieces of characteristic information of the dot columns in each grid in the projection graph are extracted, wherein the 8 pieces of characteristic information comprise the number of laser dots in each cell, average intensity, average height, intensity of the highest point, maximum height, angle deviation and distance between the center of each cell and the origin, and binary representation of whether each cell is occupied or not, and the characteristic information is used for generating input of the FCNN network, namely, a two-dimensional characteristic graph. The aerial projection point cloud data is cut by adopting a two-dimensional grid, each laser point falls into one grid, so that the two-dimensional grid becomes an ordered two-dimensional matrix, and a plurality of laser points or 0 laser points can exist in one grid.

The FCNN network comprises a feature encoder, a feature decoder and an attribute predictor;

s1.2: the feature encoder performs semantic feature extraction on the two-dimensional feature map in a convolution mode of a convolution layer, continuously downsamples the spatial resolution of the two-dimensional feature map, and finally outputs a feature vector, wherein the convolution layer comprises a trained VGG network and a trained ResNet network;

s1.3: the feature vector is input into a feature decoder, and up-sampling is carried out by adopting a deconvolution layer to obtain a prediction graph with the same size as the two-dimensional feature graph;

s1.4: the attribute predictor calculates four attribute values of each grid in the prediction graph, wherein the four attribute values refer to a center offset value, a center offset direction value, an attribute probability value and an object height value;

S1.5: and constructing a deflection pointer of each grid according to the center offset value and the center offset direction of each grid, wherein cells pointed by each plurality of pointers serve as candidate classification objects.

For candidate classified objects, the classified objects are considered to be reliable clustered objects according to the fact that the attribute probability value of the classified objects is not smaller than the probability threshold value

The reliable cluster objects screened out are potential dynamic targets.

Specifically, the calculating process of the geometric information and the intensity information of the potential dynamic target in the step S3 is as follows:

s3.1: calculating geometric information of the potential dynamic target, namely calculating the centroid position of the potential dynamic target, and calculating the centroid position of the potential dynamic target by utilizing the laser point coordinates corresponding to the potential dynamic targetAs shown in formula (6):

wherein n represents the number of laser points corresponding to the potential dynamic target, and x _i ,y _i ,z _i Representing the coordinates of the ith laser spot corresponding to the dynamic object,coordinates representing a centroid of the dynamic object;

s3.2: and converting the original laser spot data output by the laser radar into decimal representation, and performing normalization processing to obtain the intensity information of each laser spot.

S3.3: the intensity channel of lidar is noisy due to effects of target surface characteristics such as roughness, surface reflectivity, and acquisition geometry. Thus, to reduce the interference of these factors, the intensity information of the laser spot is calibrated by a mapping function:

Where d is distance information of the original laser spot, η _k Representing intensity information, η, of the kth original laser spot _k ' represents intensity information after the kth laser spot calibration.

Specifically, the process of detecting all potential dynamic targets by using the association method in S4 is as follows:

s4.1: calculating the surface reflection coefficient rho' of the laser point corresponding to the potential dynamic target:

wherein M is a constant, the constant is related to laser radar transmitting power, an optical transmission system, an atmospheric transmission coefficient and the like, and alpha is a laser incident angle;

s4.2: judging whether potential dynamic targets in different frames are the same target, if the formula (9) is satisfied, considering that the potential dynamic targets in the different frames are the same target, otherwise, not:

in the formula d_thr and ρ_thr Respectively representing a set distance threshold and an intensity threshold,representing the centroid position of the potential dynamic target a at time t and time t-1, < ->And (3) representing the average value of the intensities at the time t and the time t-1, and obtaining the reflectivity rho of the surface of the potential dynamic target by taking the average value of the reflectivity rho' of each laser point corresponding to the potential dynamic target.

And (3) correlating the potential dynamic targets in the t-moment picture with the potential dynamic targets in the t-1 moment picture through a formula (9), and correlating the potential dynamic targets shielded by the obstacle, so as to detect all the potential dynamic targets.

After the association method is finished, the target of the current frame and the target of the previous frame can be associated, as shown in fig. 3, the multi-target tracking method combines the association result and the state prediction of the Kalman filter on the target to obtain a tracking result, namely the tracking result comprises states such as a motion track, a position and a speed of the target, and then the target is divided into a static target and a dynamic target according to whether the speed of the target is 0 or not.

Specifically, the process of extracting the feature points of the static target through the curvature in S5 is as follows:

s5.1: the laser radar scans a vertical plane with M parallel readings, each parallel line has N points, and all laser points corresponding to all static targets of the kth laser radar scan are denoted as P _k ' each laser spot is denoted asWherein m is E [1, M],n∈[1,N]，x _k ,y _k ,z _k Three-dimensional coordinates of a kth laser spot;

will beThe continuous point set of the wire harness in the horizontal direction is +.>Representation, then for->Planes of adjacent points->Curvature of->The calculation formula of (2) is as follows:

in the formula N_s Representation ofThe number of laser spots in the medium,/->Representation->The j laser spot in the m-th row.

Then according to the magnitude of the c valueAll the laser points in the plane are classified, the c value is larger than the set threshold value and is used as a corner point or an edge point, and the c value is smaller than or equal to the set threshold value and is regarded as a plane characteristic point.

S5.2: point by set threshold sigmaThe edge points are marked as edge points to be selected and plane feature points to be selected.In order to obtain uniformly distributed feature points, each scan line is acquired +.>The method is characterized in that the method comprises the steps of dividing the method into four sub-areas, extracting at most two edge characteristic points and four plane characteristic points in each sub-area, and marking the set of all the edge characteristic points in the four sub-areas as epsilon _k The set of all planar feature points in the four sub-regions is denoted as s _k 。

Specifically, the process of solving and obtaining the static target pose estimation in the step S6 is as follows:

s6.1: representing all laser points corresponding to all static targets scanned by the kth laser radar as P _k ′，P _k ' one laser spot is denoted as p _k ＝{x _k ,y _k ,z _k ,1} ^T Let the pose of the laser radar in the kth frame represent T _k The 6-degree-of-freedom pose transformation between two consecutive frames k-1 and k is:

wherein ,is->Is a lie algebraic representation of (c). The function log (·) is a mapping from homogeneous transformation matrix to lie algebra, from kth frame to short time delta _t The pose transformation in the model is expressed as:

in the formula Indicating the position of all laser points corresponding to the static target of the current k frame to be re-projected to the starting moment of the current frame.

Assume ε _k Mid-edge feature point p _ε Is c _ε ，s _k Mid-plane feature point p _s Is c _s Their weight definitions are shown in formula (14):

/>

wherein ,W(p_ε )、W(p _s ) Respectively represent p _ε and p_s Is used for the weight of the (c),representing epsilon _k The ith row and the jth edge feature point of the middle rowIs provided for the curvature of the lens.

S6.2: calculating each edge feature point p _ε Covariance matrix of adjacent points in global edge feature set, namely all epsilon contained in one frame of image _k If the covariance matrix contains the largest eigenvalue, specify p _ε The adjacent points in the global edge feature are distributed on a straight line, and the feature vector corresponding to the maximum feature valueSeen as a straight line, the geometric center of the neighboring point +.>Viewed as a straight line, at this time the edge feature point p _ε The distance to the global edge feature is:

in which symbol is dot product, p _n Is a unit vector;

s6.3: calculate each plane feature point p _S Covariance matrix of adjacent points in global plane feature set, namely all s contained in one frame of image _k If the covariance matrix contains a minimum eigenvalue, which indicates that the neighboring points are distributed on a plane, the eigenvector corresponding to the minimum eigenvalueSeen as the direction of the normal of the curved surface, the geometrical centre of the point of proximity +. >Can be regarded as the position of the plane, at which point the plane feature point p _S The distance to the global planar feature set is expressed as:

s6.4: the optimization function is defined as:

s6.5: solving a nonlinear equation of the formula (12) by using a Gauss Newton method to obtain an optimal attitude estimation, and estimating a jacobian matrix J by using a model with left disturbance _p ：

wherein [Tp]_× I.e. [ T ] _k p _k ] _× Represents converting the 4D point expression { x, y, z,1} into the 3D point expression { x, y, z } and calculating its oblique symmetry matrix, delta, zeta represent short time, delta respectively _t And 6-degree-of-freedom pose transformation between two consecutive framesT _k p _k The vector representing one 4*4 matrix and 4*1 yields one 4*1 vector, jacobian J of edge residuals _ε Calculated by the following formula:

jacobian matrix J of plane residuals _s ：

Representing an iteration delta DeltaT by means of a Jacobian matrix _k Using DeltaT _k Updating the estimation result of the laser odometer, and repeatedly iterating and optimizing the gesture until delta T is reached _k Convergence, the pose estimation T of the current laser radar can be calculated _k 。

Specifically, the process of obtaining the latest laser odometer output in S7 is as follows:

s7.1: the key frame is selected by setting a pose transformation threshold, the translation and rotation thresholds are respectively set to 1m and 10 degrees, namely, when the pose transformation of the robot between the kth moment and the k+1 moment exceeds the pose transformation threshold, the current frame is selected as the key frame F _k+1 And add a sliding window, F _k+1 And p is as follows _k Is matched with the pose of the model (C); to store a fixed number of projections of the most recent frame laser point cloud, a stack is first maintained as a sliding window. Then, i nearest key frames are extracted in time sequence and expressed as a sub key frame set { F } _k-i ,...,F _k By a transformation matrix { T 'with W }' _k-i ,...,T' _k Projection of the sub-key frame set under W to obtain projection map M _k 。

M _k Projection from edge feature point cloudPlane feature point cloud projection view +.>Composition is prepared. Each key frame and corresponding projection map M _k The association relationship is defined as follows:

wherein ,represents the edge feature points of the kth key frame and k-i key frames in the sub-key frame set respectively,/-, and>representing the planar feature points of the kth key frame and k-i key frames in the sub-key frame set respectively.

S7.2: the latest obtained k+1 key frames are F _k+1 Conversion to W and to the projection map M _k Correlating, obtaining delta pose transformation delta T of the (k+1) th frame _k,k+1 ：

wherein ,representing T _k Inverse matrix of T _k+1 Representing pose estimation of the k+1 frame lidar.

And (3) recalculating distortion by using 6-degree-of-freedom pose transformation to obtain:

in the formula Is DeltaT _k,k+1 The optimal positioning estimation after the laser radar update can be calculated through the formula (19), and delta T _k,k+1 Representing the pose transformation relationship of two adjacent key frames, < ->Representing the latest coordinates of the laser points corresponding to the updated static targets;

all laser points corresponding to the static target of the (k+1) th laser radar scan are denoted as P' _k+1 ，P′ _k+1 One laser spot of (a) is denoted as p _k+1 ，For 6-degree-of-freedom pose transformation between two consecutive frames k and k+1, according to DeltaT of the k+1 frame _k,k+1 Calculated out->Representing the latest coordinates of the laser point corresponding to the updated static target.

Output ofAnd the pose transformation relation delta T of two adjacent key frames _k,k+1 。

The Factor Graph back-end optimization method is a common Graph optimization method, and uses variables and factors in the Factor Graph to represent the relationship between the robot pose and the environmental characteristics. Which is essentially a Bayesian networkAs shown in fig. 9, the bayesian network is a directed acyclic graph, assuming t _i The state quantity of the robot at the moment is denoted as X _K ＝{x _i I e (0..k) represents the position of the observed object detected by the sensor, i.e. the road sign point is L _N ＝{l _j J e (1..n), where the received laser sensor observes Z _K ＝{z _i U for control input value of robot _K ＝{u _i And } represents where i.epsilon. (0..K). The thin and thick lines in the figure represent the predictive equation g and the measurement equation h, respectively. Road sign l in solid line frame ₁ And pose x ₁ Determination of the observed data z under the influence of the measurement equation h ₁ . Pose x ₁ According to the predictive equation g and the control input u ₂ The next pose x is determined together ₂ And gradually progresses through this basic process.

The process of estimating the state quantity from the observed quantity can be converted into a posterior probability problem expressed as:

P(X _K |Z _K ) (26)

according to bayesian law, there are:

in the formula P(Z_K |X _K) and P(X_K ) Commonly referred to as likelihood and prior probability, according to the markov assumption, the observables for each state are independent of each other, and the current state is only related to the last time, then there are:

at each time, considering the control input value of the robot as the parameter value of the state quantity, the right-side motion model of the equation (27) can be further rewritten as:

wherein P(x₀ ) Is a priori of the initial state.

Then solving the optimal state estimation problem becomes solving the problem of the joint probability density function of each variable and the measured value:

assuming that the predictive model is a gaussian measurement model, the predictive model can be expressed as:

x _i ＝g(x _i-1 ,u _i )+ω _i (31)

wherein ω_i Representing t in the course of movement _i Prediction noise of moment input, and omega _i ～N(0,Λ _i )，Λ _i Representing the noise covariance matrix.

Similarly, assuming that the observation model is also a gaussian measurement model, the observation model can be expressed as:

z _i ＝h(x _i ,l _j )+v _i (33)

wherein v_i To measure noise, and v _i ～N(0,Γ _i )，Γ _i Representing its corresponding covariance matrix.

Then combining equations (32) and (34), and combining probability density function P (X) _K ,L _K ,U _K ,Z _K ) Taking the negative logarithm, the maximum posterior probability problem can be converted into a least squares problem, expressed as:

the factor graph model { X, F, E } is simplified on a Bayesian network model, as shown in FIG. 10, with factor node F for probability linkage between variable nodes _i E F, i.e. state variable node x _i Factor f between E and X _i (x _i-1 ,x _i ) Correspondence probability P (x) _i |x _i-1 ,u _i ) Factor correspondence probability P (z _i |x _i ,l _j )。e _ij E is the edge connecting the factor node and the variable node, and represents the constraint relation between the nodes. Then equation (30) combines the probability density functions P (X) _K ,L _K ,U _K ,Z _K ) Can be converted into a product of multiple factors:

in the formula X_i Is the sum factor node f _i Adjacent x _i The set of factor graph model optimization equations of fig. 10 are:

F(X,L)＝f ₀ (x ₀ )f ₁ (x ₀ ,x ₁ )f ₂ (x ₁ ,x ₂ )f ₃ (x ₂ ,x ₃ )f ₄ (l ₁ ,x ₁ )f ₅ (l ₂ ,x ₂ ) (37)

in the formula f₀ (x ₀ ) Corresponding a priori factor p (x ₀ ) And then, the nonlinear least square problem is obtained in a negative logarithm mode, and the variable to be solved can be optimized according to the nonlinear least square problem solving method LM, so that the global optimal pose is obtained.

A back-end optimization method based on a multi-factor graph for acquiring the precise pose of a robot comprises the following steps:

S100: construction of an improved multi-factor graph model F (X):

The method comprises the steps of constructing a multi-factor graph model based on an IMU factor, a laser odometer factor and a closed loop factor for back-end optimization, firstly pre-integrating IMU data to obtain an observation model error of an IMU sensor, and constructing an IMU factor f _IMU Meanwhile, the observation model error of the laser radar sensor is calculated by utilizing the pose change relation output by the front-end laser odometer, and a laser odometer factor f is constructed _Lidar And constructing a closed loop factor f when loop success is detected _Loop These three constraint factors are then combined with the associated state variable X _i The factor graph optimization model of fig. 10 may be further represented as a multi-factor graph optimization model, as shown in fig. 5, with edge connections added to the factor graph basic model.

in the formula And->Error of IMU factor, laser odometer factor and closed loop factor observation model are respectively represented, and +. > And->Respectively the corresponding measurement noise covariance. Wherein, the error term can be uniformly expressed as:

Specifically, f is obtained in S100 _IMU The process of (2) is as follows:

to more accurately estimate the position and orientation of the robot, IMU factor calculation is done in three steps. The first step is to calculate the primary pre-integral, and the second step is to optimize the primary pre-integral result by using an open source factor graph optimization framework GTSAM and correct the error in IMU data; and thirdly, calculating integration by using the updated IMU zero offset, and outputting IMU factor related items. The specific calculation process is as follows:

in the formula The rotation matrix representing the map coordinate system W to the sensor coordinate system B, g is the gravitational acceleration in W, from the equation +.> and />Subject to zero offset b of IMU _t And white noise n _t Influence of omega _t and a_t Respectively representing the actual angular velocity and the actual acceleration of the laser radar at the moment t;

Then, a first-stage calculation is performed, and the motion of the robot is estimated by integrating the multi-frame angular speed and the acceleration of the IMU. At time t+ [ delta ] t, the speed v of the robot _t+Δt Position p of robot _t+Δt And rotation transformation R _t+Δt Calculated from the following formulas:

wherein ,R_t ，p _t ，v _t The rotation change 4, the position and the speed of the lidar at time t are respectively indicated, and Δt represents a short time.

Then, using open source GTSAM optimizing library to optimize IMU speed change Deltav _ij Position conversion Δp _ij And rotation transformation DeltaR _ij Three state variables. In the GTSAM optimization library, the prior factors are the speed and the position of the IMU at the moment i, the zero bias matrix of the IMU is used as a factor node, and the optimized variable nodes are the speed, the position and the transformation matrix at the moment j. The GTSAM optimizer outputs updated position, velocity and rotation transforms and zero offset matrix b before the next pre-integration result is input _i ′。

The relative pose change between the two determined moments is obtained through three-stage pre-integral calculation, and then the relative pose change is obtained The calculation formula is as follows:

Specifically, f is obtained in S100 _Lidar The process of (2) is as follows:

Specifically, f is obtained in S100 _Loop The process of (2) is as follows:

and after the closed loop detection is successful, constructing a closed loop factor by forming a relative pose change relation between two closed key frames. Obtaining accurate pose transformation delta T between two point sets by using a traditional iterative nearest point feature matching method, and setting the pose of the current point cloud as T _i+1 The pose of the loop frame is T _c The relative pose change between the two is obtained, namely the closed loop factor is expressed as:

wherein ,f_Loop Indicating loop detection of observed model errors between the i+1st frame of the current frame and the two frames of the c frame of the loop frameWill->Substitution into formula (41) gives +.>

Specifically, the loop detection process in S200 is as follows:

the loop detection mainly carries out position identification on the robot through different judging strategies, and detects whether the robot forms a closed loop with the historical pose or route in the running process; if a closed loop is formed, a closed loop factor is built and added into a factor graph model, and the pose of the robot is optimized, so that the track with larger drift is effectively corrected.

s202: then equally dividing the point cloud into N parts by taking the sensor as the center through the annular angle and the radial direction _s ×N _r In the sub-region, as shown in FIG. 7, a frame of point cloud is segmented into N _s Sectors and N _r And a ring. Each segment is represented as:

wherein a is E [1, N _s ]，b∈[1,N _r ]，L _max Indicating the maximum induction range of the laser radar in turn. Because the intensity value returned by the same object is the same for each small region S _ab The point cloud in the space is replaced by the maximum intensity information in the subarea _ab ：

S203: extracts each sub-region S _ab After the intensity information of the region, the point set in the region needs to be encoded continuously. The coding scheme is shown in FIG. 8, in which each sub-region S is divided into _ab The point cloud in the system is divided into 8 units in the vertical direction and is marked as z from bottom to top _k Where k e (1, 2,., 8), points with the same z-coordinate are in the same cell. If there is no laser spot in a cell, then z will be _k Set to zero, otherwise, if there is at least one laser spot in a cell, then z _k Set to 1. Thus, each sub-region S _ab An 8-bit binary code is obtained and then based on z _k The value converts binary into decimal value, the geometric information d of each region _ab Can be expressed as:

wherein z (·) represents S _ab Is converted into a function of decimal values. Combining the geometric information in this equation with the intensity information in equation (64) yields a point cloud global description Fu (a, b):

Position identification is the process of finding the historic position closest to the current position by calculating the euclidean distance between the current position and the historic position. I.e. the matching of the descriptors of the current location and the descriptors of the historical locations. But as the historical locations increase, the computational cost of searching for matches increases.

S204-1: geometric similarity calculation

For a pair of global descriptors to be matched, a geometric similarity score is first calculated for both global descriptors. Representing global descriptors of a current point cloud as Ω ^q The global descriptor of the point cloud to be matched is omega ^c The geometric coding values at the same index are respectivelyAnd->Calculate->And->And normalizing the cosine distance of (2), then obtaining a geometric similarity function:

However, the representation of the calculated point cloud global scanner is affected by the sensor position, i.e. the scanning position of the lidar is different, and the calculated descriptors at the same index are also different. The method is mainly characterized in that the method is coded according to a circumferential sequence, and when the point cloud rotates, the constructed point cloud descriptor can be horizontally displaced on an index.

In order to solve the rotation invariance problem of the point cloud descriptors, all possible displacement distances between the global descriptors of each pair of point clouds in the horizontal direction can be calculated first, and a set of distance values can be obtained. Finally, selecting the minimum value from the group of distance values as the distance between the point cloud descriptor pair, and then resolving the geometric similarity, wherein the calculation process is as follows:

assume that the global descriptor of a point cloud isHere n represents that the global descriptor of the point cloud is translated n units in the horizontal direction. Column displacement number n and moved geometric similarity score D (D ^q ,d ^c ) Respectively defined as:

s204-2: intensity similarity calculation

The purpose of the intensity similarity matching is to reduce errors and noise in the geometric similarity matching, thereby obtaining a more accurate matching result. The core is that the intensity characteristic eta between two point cloud descriptors is calculated by columns ^q and η^c Is achieved by the similarity of (c). Is provided with and />Respectively eta ^q and η^c Column with the same index value, get +> and />Similar computational intensityDegree scoreThe following are provided:

By comparing eta ^c and η^q Translation n ^* Intensity features after columnThe final calculation result of the calculation intensity similarity is as follows: />

n ^* Indicating a change in viewing angle.

And selecting K history descriptors which are most similar to the target point cloud, and inputting the history descriptors of the corresponding K candidate frames into a consistency verification part for verification.

S204-3: consistency verification

In the SLAM system, the similarity of adjacent frame point clouds of the loop frame is high, and the sensor feedback is continuous in time, in order to reduce the situation of error matching, an average value of the similarity of past N frame point cloud descriptors of the current frame and past N frame point cloud descriptors of K candidate frames is used as a final similarity, the final similarity is used as a verification standard of consistency, and a calculation formula is as follows:

And (3) experimental verification:

the classical laser radar SLAM frame LOAM algorithm is adopted as a baseline model frame, the back-end optimization algorithm based on the multi-factor graph provided by the invention is added to the LOAM frame, and the following groups of comparison experiments are respectively carried out aiming at the proposed back-end optimization algorithm of the multi-factor graph: the first group is a comparison experiment of the back-end optimization algorithm before and after the loop detection method is started, the second group is a comparison experiment of the loop detection method adopted by the invention and other loop detection algorithms based on point cloud descriptors, and the third group is a comparison experiment of the back-end optimization algorithm in different laser SLAM frames.

1. Experimental data set

The experiment used a RawData dataset in the KITTI dataset, which total 22 sets of data, the first 11 of which

The sequences (0-10) have ground truth tracks, and the remaining 11 sequences (11-21) have no ground truth tracks, and specific information of each sequence can be seen in Table 1.

TABLE 1KITTI data set sequence information tables

/>

2. Evaluation criteria

The effectiveness of the loop detection method is verified, the classification effect of a classification problem is essentially inspected, the inspected indexes are accuracy (Precision) and Recall Rate (Recall Rate), and the two indexes are obtained through calculation through four basic indexes, namely True Positive (TP), true Negative (TN), false Positive (FP) and False Negative (FN).

Accuracy rate K of closed loop detection _p The duty cycle of the closed loop, expressed as correct prediction, at all predicted as having loops is calculated as:

recall K for closed loop detection experiments _r The duty ratio of the closed loop to the total closed loop environment, which is expressed as correct prediction, is calculated as:

the related indexes for evaluating the performance of the back-end optimization algorithm comprise absolute pose errors of algorithm output tracks(ATE) and Relative Pose Error (RPE), the absolute pose error (ATE) is obtained by calculating the error between the track output by the SLAM algorithm and the real track, and the accuracy of the algorithm and the consistency of the track can be evaluated. Let i moment algorithm output pose be p _i The ground truth value of the pose is q _i And the transformation matrix from the output pose to the real pose is T, the absolute pose difference E at the moment i _i The method comprises the following steps:

usually E is used _i As an ATE index, to evaluate global positioning consistency, and to calculate RMSE of the global track from the absolute pose errors at each moment:

Where m represents the number of poses in the trajectory, trans (E _i ) Representing the translational part of the absolute error.

The Relative Pose Error (RPE) describes the error of the measuring odometer by calculating the pose difference value of two frames with equal time intervals, which can help us to know the pose change condition between adjacent frames and reflect the accuracy and the robustness of the local pose estimation of the positioning algorithm in the global track. Similarly, the relative pose error at each moment is calculated first using the Root Mean Square Error (RMSE) as the RPE index, and the pose difference F at moment i is calculated assuming the equal time interval is Δ _i The method comprises the following steps:

the relative pose difference value at each moment is RMSE:

/>

where n represents the number of poses in the trajectory, trans (F _i ) A translation portion representing the relative error.

3. Experimental setup

As can be seen from Table 1, only the 00, 02, 05-08 sequences in the first 11 sequences in the dataset have loops, so the closed loop detection experiment was performed on several sequences in the KITTI dataset. On one hand, the performance of the closed-loop detection method is evaluated by comparing the RPE index of the global track under the condition of opening the closed-loop detection function and closing the closed-loop detection function of the back-end optimization algorithm provided by the invention. On the other hand, the loop detection method is continuously compared with the closed loop detection method of the front edge in other SLAM algorithms, including the DBoW3 method in visual SLAM, the loop detection method based on the point cloud global descriptor SC and the Lidar Iris loop detection method based on the descriptor. Meanwhile, the rear-end optimization algorithm provided by the invention is compared with the ground truth value of the track output by the LOAM and the LeGO-LOAM under the 07 sequence and the 08 sequence of the KITTI data set, and the ATE and RPE related values of the track output by each SLAM system are calculated, so that the pose estimation precision of the whole algorithm is compared.

4. Analysis of experimental results

FIG. 11 shows a global trace comparison of the method of the present invention in both the on and off loop detection functions, with the RPE index for each sequence shown in Table 2.

TABLE 2 RPE results before and after open closed loop detection

Table 2RPE results before and after opening closed-loop detection

The circled place in fig. 11 shows a closed loop scene of the sequence, the left side is a global positioning result under the sequence of 02 and 05 when the back-end optimization algorithm provided by the invention starts loop detection, the right side is a global positioning result when the loop detection is closed, and it can be intuitively seen that when the loop detection function is closed, errors are accumulated along with the time, obvious deviation occurs in pose estimation, and the motion track output by the algorithm is introduced at the loop position to not completely realize closing; on the contrary, after the closed loop detection is started, the method can better realize closed loop matching in a complex environment with reverse access to a closed loop scene under the 02 sequence, and the accumulated drift is eliminated due to the fact that the closed loop factors are successfully built and added into the multi-factor graph optimization model, so that the overall pose estimation precision is improved.

As can be seen from the Relative Pose Error (RPE) results in table 2, in the two modes of opening and closing the closed loop, the RPE results on the 05 sequence of the loop detection method adopted by the invention have obvious difference, and the global positioning error after the closed loop function is closed is obviously larger; in a 02 sequence with a more complex loop scene, the pose estimation RPE difference of the two modes is more obvious, and compared with the pose error after the closed loop function is opened, the pose error is reduced by 71% when the closed loop function is closed, so that the importance of the loop detection method adopted by the invention in an SLAM back-end optimization algorithm is proved.

The loop detection method is continuously compared with the leading edge closed loop detection method in other SLAM algorithms, including the DBoW3 method in visual SLAM, the loop detection method based on the point cloud global descriptor SC and the Lidar Iris loop detection method based on the descriptor, and the results are shown in Table 3.

Table 3 comparison of closed loop detection method results under KITTI data set

As can be seen from table 3, for the 02 and 08 sequences with reverse access closed loop scenarios, the visual based method recall is significantly reduced because the visual based method cannot achieve reverse closed loop matching, such that the detected loops are reduced, while the present method and Lidar Iris method take into account the impact of multi-angle access history locations when building global descriptors, thus exhibiting better recall. However, in the scene with more non-structural features of the 02 sequence, the road is narrow, the non-building road sections of trees are arranged on both sides of the road, the precision of loop detection is affected by the geometric shape, and the difference of global descriptors constructed through geometric information and intensity information is small, so that the performances of the SC and the loop detection method are lost, and the precision is reduced. Under the 00 sequence and the 05 sequence, the method has higher precision and recall rate than other methods, and the stability of the scene recognition capability of the method is fully shown.

Table 4 results of experiments for each algorithm under the 07 sequence

Table 4 shows the global positioning results and the errors in the horizontal direction and the rotation direction of the sequence of the KITTI dataset 07 by LOAM, leGO-LOAM and the back-end optimization algorithm proposed by the invention, respectively. As can be seen from the data of the table, the performance difference of the three algorithms under the 07 sequence is not too large, but it can be seen that the error of the algorithm in the horizontal direction is obviously lower than that of the other two algorithms, and for the odometer with the distance of 694 meters, the root mean square error and standard deviation of the absolute track error and the relative track error of the method are better, wherein the maximum value of the absolute track error is controlled within 0.451 meter, and the relative pose error is controlled within 0.081 meter, which indicates that the accuracy and the robustness of the pose estimation are better than those of the other two methods.

Table 5 results of the algorithm experiments under the 08 sequence

Table 5 shows the global positioning results and the errors in the horizontal direction and the rotation direction of the sequence of the KITTI data set 08 by LOAM, leGO-LOAM and the back-end optimization algorithm provided by the invention respectively.

The data of the table show that after the rear end is optimized, the three algorithms effectively correct the drift accumulated in the front end, and for the range of up to 3222 meters, the performance difference of the three algorithms under the 08 sequence is obvious, and the accuracy of the algorithm is better than that of the other two algorithms according to the positioning result after the rear end is optimized. In combination with pose error data in table 5, it can be seen that compared with the low back end optimization without loop detection, the LeGO-low back end optimization directly combines the frame-to-map (scan to map) matching mode, and the effect of correcting long-term accumulated drift by fusion of IMU factors is most obvious in the back end optimization, and the LeGO-low method is next.

In summary, through the KITTI data sets with 2 different sequences, the performance and the performance of the LOAM, the LeGO-LOAM and the multi-factor graph-based back-end optimization algorithm provided by the invention under different scenes are respectively verified. It can be obtained that the advantage of the algorithm is not obvious in a short-distance road scene; the algorithm has better improvement on precision and stability under long-distance dynamic complex environment.

Finally, it is noted that the above embodiments are only for illustrating the technical solution of the present invention and not for limiting the same, and although the present invention has been described in detail with reference to the preferred embodiments, it should be understood by those skilled in the art that modifications and equivalents may be made thereto without departing from the spirit and scope of the technical solution of the present invention, which is intended to be covered by the scope of the claims of the present invention.

Claims

1. A back-end optimization method based on a multi-factor graph for acquiring the precise pose of a robot is characterized by comprising the following steps of: the method comprises the following steps:

s100: construction of an improved multi-factor graph model F (X):

in the formula f_(i,j) (X _i ,X _j ) Representing the relation factor between the pose node of the ith frame and the pose node of the jth frame, solid line box C _i Dashed box F _i The black node is divided into IMU factors f _IMU Laser odometer factor f _Lidar And a closed loop factor f _Loop ，f _IMU ＝f _IMU (X _i ,X _i+1 )、f _Lidar ＝f _Lidar (X _i ,X _i+1 )，f _Loop ＝f _Loop (X _c ,X _i+1 ) E represents all edges of F (X) associated with the node;

in the formula And->Error of IMU factor, laser odometer factor and closed loop factor observation model are respectively represented, and +.> And->The corresponding measurement noise covariances respectively, wherein the error terms can be uniformly expressed as:

in the formula T_i And T is _j Pose of ith and jth keyframes of the robot in world coordinate system, delta T _i,j Representing the relative pose transformation relationship between two variable nodes calculated by three factors, deltaT _i,j Included and />

2. The back-end optimization method based on multi-factor graph for accurate pose acquisition of robot according to claim 1, wherein the back-end optimization method is characterized in that: obtaining f in the S100 _IMU The process of (2) is as follows:

in the formula A rotation matrix representing the map coordinate system W to the sensor coordinate system B, g is the gravitational acceleration in W, ω _t and a_t Respectively representActual angular velocity and acceleration of the laser radar at the moment t;

and />IMU zero bias, IMU zero bias of acceleration, IMU zero bias of original angular velocity, IMU zero bias of acceleration, IMU zero bias> and />Collectively referred to as IMU zero offset b _t ， and />White noise representing the original angular velocity and the acceleration, respectively, +.> and />Collectively referred to as n _t ；

wherein ,R_t ，p _t ，v _t Respectively representing the rotation transformation, the position and the speed of the laser radar at the moment t, wherein Deltat represents a short time;

wherein ,v_i ，p _i ，R _i Indicating the speed, position and rotation of the lidar at time i, v _j ，p _j ，R _j Respectively representing the speed, position and rotation transformation of the laser radar at the moment j, delta t _ij The time difference between the moment i and the moment j is represented;

Then, using open source GTSAM optimizing library to optimize IMU speed change Deltav _ij Position conversion Δp _ij And rotation transformation DeltaR _ij The GTSAM optimizer outputs updated position, velocity and rotation transforms and zero bias matrix b 'before the next pre-integration result is input' _i ；

With updated zero offset b' _i By means ofThe precomputation is carried out to obtain the speed of the optimized IMU at the beta momentPosition->And rotation transformation->

wherein , and />IMU zero offset representing the updated original angular velocity and IMU zero offset representing the updated acceleration respectively, and (2)>Andwhite noise representing the original angular velocity at the i-th time and white noise representing the acceleration at the i-th time;

two time points determined by three-stage pre-integration calculationThe relative pose between the marks changes, and then obtainThe calculation formula is as follows:

respectively representing the updated speed conversion, position conversion and rotation conversion between the i time instant and the j time instant;

f _IMU representing observed model errors of IMU sensor between i frame and i+1 framePre-integration of +.>Substituting formula (41) to obtain the observation model error of the IMU sensor between the i frame and the j frame>Can be obtained by the same method

3. The back-end optimization method based on multi-factor graph for accurate pose acquisition of robot according to claim 2, wherein the back-end optimization method is characterized in that: obtaining f in the S100 _Lidar The process of (2) is as follows:

4. A multi-factor graph-based back-end optimization method for precise pose acquisition of a robot as claimed in claim 3, wherein: obtaining f in the S100 _Loop The process of (2) is as follows:

wherein ,

5. The back-end optimization method based on multi-factor graph for accurate pose acquisition of robot according to claim 4, wherein the back-end optimization method based on multi-factor graph for accurate pose acquisition of robot is characterized in that: the loop detection process in S200 is as follows:

s201: the point cloud obtained by acquiring the environmental information through the laser radar comprises distance information and intensity information, and eta 'is obtained by normalizing and calibrating the intensity information through an equation (59)' _k Assume that each point cloud in the kth frame is denoted as [ x ] _k ,y _k ,z _k ,η′ _k ]Projecting the 3D point cloud under the Cartesian coordinate system to a polar coordinate system formed by a X Y plane, wherein the transformed point cloud has polar coordinatesExpressed as:

wherein a is E [1, N _s ]，b∈[1,N _r ]，L _max Indicating the largest induction range of the laser radar in sequence; because the intensity value returned by the same object is the same for each small region S _ab The point cloud in the space is replaced by the maximum intensity information in the subarea _ab ：

S203: extracts each sub-region S _ab After the intensity information of (a), the point set in the region is further encoded, and each sub-region S is then encoded _ab The point cloud in the system is divided into 8 units in the vertical direction and is marked as z from bottom to top _k Where k e (1, 2,., 8), points with the same z-coordinate are in the same cell; if there is no laser spot in a cell, then z will be _k Set to zero, otherwise, if there is at least one laser spot in a cell, then z _k Set to 1, each sub-region S _ab An 8-bit binary code is obtained and then based on z _k The value converts binary into decimal value, the geometric information d of each region _ab Can be expressed as:

wherein z (·) represents S _ab The binary code of (a) is converted into a function of decimal values, and the geometric information in the formula is combined with the intensity information in the formula (64) to obtain the point cloud global description Fu (a, b):

s204: a two-step layered rapid detection method is adopted, geometrical similarity and intensity similarity are calculated respectively to accelerate matching, and then the matching effectiveness is verified based on consistency;

s204-1: geometric similarity calculation

wherein ,N_S Represents the aforementioned N _S Sectors d ^q ，d ^c Representing the current point cloud global description Fu respectively ^q And a point cloud global description Fu to be matched ^c Corresponding geometric information in the database;

assume that the global descriptor of a point cloud isWhere n represents the global descriptor of the point cloud translated n units in the horizontal direction, the column displacement number n and the moved geometric similarity score D (D ^q ,d ^c ) Respectively defined as:

s204-2: intensity similarity calculation

wherein ,η^q and η^c Respectively representing the intensity characteristics between two point cloud descriptors,respectively representing intensity similarity scores;

n ^* indicating a change in viewing angle;

s204-3: consistency verification

wherein Θ(p_q ,p _c ) Average value of similarity of past N frame point cloud descriptors representing current frame and past N frame point cloud descriptors of one frame in K candidate frames, p _q Representing the current frame, p _c Representing a frame of K candidate frames, d _q-i ，d _c-i ，η _q-i ，η _c-i Respectively representing geometric information and intensity information in the point cloud global descriptors of the current frame and the candidate frame;

in the case of a reverse access of the mobile robot, i.e. a change in the angle of view observed by the sensor, Ω _q-i Correspondingly change to omega _q+i Constructing a K neighbor search tree by using the obtained K objects to be matched, and setting targetsComparing the descriptors of the point cloud with those in the search tree, and measuring the similarity between the descriptors by adopting a formula (72); finally, the most similar candidate point is selected by setting a threshold, and is used as a revisit position, which is expressed as:

Where C represents the index values of K objects to be matched in the search tree,is a set threshold; c ^* Representing the finally obtained closed loop position index value, and obtaining the pose T of the loop frame from the historical key frame _c And is used for subsequent construction of the closed loop factor.