CN104732518B

CN104732518B - A kind of PTAM improved methods based on intelligent robot terrain surface specifications

Info

Publication number: CN104732518B
Application number: CN201510025810.7A
Authority: CN
Inventors: 贾松敏; 王可; 宣璇; 张鹏; 董政胤
Original assignee: Beijing University of Technology
Current assignee: Beijing University of Technology
Priority date: 2015-01-19
Filing date: 2015-01-19
Publication date: 2017-09-01
Anticipated expiration: 2035-01-19
Also published as: CN104732518A

Abstract

A kind of PTAM improved methods based on intelligent robot terrain surface specifications, first, complete parameter correction, this includes parameter definition and camera calibration；Then current environment texture information is obtained using camera, and builds four layers of Gaussian image pyramid, extract the characteristic information in present image with FAST Corner Detection Algorithms, the data correlation set up between Corner Feature obtains pose estimation model；In the initial drawing stage of map, obtain two key frames and camera is erected in mobile robot；In initialization procedure, mobile robot starts movement, while angle point information and setting up association in camera capture current scene；After the initialization for realizing sparse 3 D map, update key frame and set up characteristic point sub-pixel precision mapping relations with block matching method using polar curve search, accurately relocated with reference to pose estimation model realization camera.Match point is finally projected into space, current global context three-dimensional map is completed and creates.

Description

PTAM improvement method based on ground characteristics of intelligent robot

Technical Field

The invention belongs to the field of robot vision, and relates to improvement of a PTAM algorithm based on ground features.

Background

With the closer and closer relationship between the robot and the human being, the related technology of the intelligent robot is greatly emphasized. Synchronous positioning and Mapping (SLAM) of mobile robots is one of the most popular positioning technologies of current intelligent mobile robots. The method is actually a motion estimation problem, and utilizes internal and external data acquired by a sensor to calculate the position of the mobile robot at a certain moment and establish a dependent map model. The vision-based SLAM belongs to the research category of vision measurement, and a vision sensor has unique advantages: the volume is small, the weight is light, the price is low, the installation is convenient, and the extracted external information is very rich. These advantages further facilitate the current study and application of vision-based SLAM. Application number 201310678018.2 provides a feature detection operator using SURF as monocular vision SLAM, which is innovated and improved from three aspects of interest point detection, SURF descriptor generation, SURF point matching and the like, and provides a SURF feature matching method based on region growing to complete simultaneous positioning and map creation of a robot, so that in the SURF descriptor matching process, a descriptor is only compared with the descriptor most likely to be matched with the descriptor, the comparison times in the monocular vision SLAM problem are obviously reduced, and the matching speed is improved. However, the method still has the problems that a metric map cannot be established and three-dimensional characteristic information cannot be provided, and the robustness and the real-time performance of the algorithm are to be further improved.

The technology of creating environment model from motion is a method for simultaneously solving the motion track of a camera and a scene structure model. As a representative method of SFM, the concept of ptam (parallel Tracking and mapping) was first proposed by g.klein et al in the field of Augmented Reality (AR) in 2007 to solve the environmental recognition problem in real time. However, the unmodified PTAM algorithm also has the problem that a metric map cannot be established, and the movement of the camera is strictly limited.

Disclosure of Invention

Aiming at the existing problems, the invention provides a ground feature-based PTAM improved algorithm, on the basis of adding ground features, the PTAM synchronously estimates the current pose of a monocular camera and a three-dimensional space point, and updates and detects the feature point by using a FAST corner point method, and simultaneously continuously updates the poses of the camera and the three-dimensional feature point by using an optimal local and global beam adjustment method, thereby completing high-precision positioning and map creation.

The invention adopts the following technical scheme:

firstly, completing parameter correction, which comprises parameter definition and camera correction; tracking features in the key frame, initializing the pose, and estimating plane parameters on the basis; then, acquiring current environment texture information by using a camera, constructing a four-layer Gaussian image pyramid, extracting feature information in a current image by using a FAST corner detection algorithm, and establishing data association among corner features to obtain a pose estimation model; in the initial drawing stage of the map, two key frames are acquired, a camera is arranged on a mobile robot, and the initialization process is started after the camera is calibrated with the ground; in the initialization process, the mobile robot starts to move, meanwhile, a camera captures angular point information in a current scene and establishes correlation, a pose estimation model acquires initial estimation of the current pose, and then a pose estimation method based on ground features is used for acquiring accurate estimation of the current pose; after the initialization of the three-dimensional sparse map is realized, the key frame is updated, the sub-pixel precision mapping relation of the feature points is established by using an epipolar line search and block matching method, and the accurate relocation of the camera is realized by combining a pose estimation model. And finally, projecting the matching points to a space to complete the creation of the current global environment three-dimensional map. Meanwhile, global and local bundling adjustment is utilized to complete consistency optimization of the algorithm.

The method specifically comprises the following steps:

step 1, parameter correction

Step 1.1, parameter definition

(1) Robot pose

Establishing a world coordinate system at the beginning of the mobile robot, and during the movement of the robot, representing the pose as follows:

from the above formula, the pose of the mobile robot can be described by x ═ x, y, and θ, where x and y represent the translation of the mobile robot in the x-axis and y-axis directions, respectively, and θ is the rotation angle of the mobile robot around the z-axis.

In the world coordinate system, the pose of the camera can be expressed as:

wherein, T_c∈ SE (3), SE (3) being a special rigid transformation group, R_cIs a 3 × 3 rotation matrix, t_cIs a translation matrix of 3 × 1. the position matrix establishes a world coordinate system and a point p under a camera coordinate system_c，p_wIs a corresponding relation of, i.e. p_c＝T_cp_w. In the present study, as shown in fig. 2, the camera is fixed on the mobile robot, and there is a fixed rotational-translational relationship T between the camera and the robot_cr. In the moving process of the robot, if the pose increment of the robot is T at a certain time interval_rsCorresponding to this, the pose increment of the camera is T_csAnd then:

T_rs＝T_rcT_csT_cr(3)

wherein,

(2) ground plane calibration parameters

And fixing the camera on the mobile robot, and flatly placing the calibration target on the ground. The camera establishes a pose relationship between the camera and the robot by acquiring image information with a calibration target. T is_cp∈ SE (3) is a transformation matrix between camera and calibration target, R_cp，t_cpRespectively represents T_cpOf the rotational and translational components of, inverse toAssume that the robot's origin of coordinates is coincident with the camera and that the robot's x-axis is oriented the same as the camera's z-axis. In the target coordinate system, the x direction of the robotThe vector may be represented by T_cpThe first two components of the row 3 vector of (a). At this time, the transformation relationship between the robot coordinate system and the target coordinate system can be expressed as:

wherein gamma is the rotation angle of the robot coordinate system and the target coordinate system, (x)_r,y_r) The coordinates of the robot origin in the target coordinate system can be determined by the translation vector of the camera and the target. Therefore, the transformation relationship between the robot and the camera can be expressed as T_cr＝T_cpT_pr，According to the pose relation between the camera and the target plane, a ground plane equation under a camera coordinate system can be obtained:

(0,0,1,0)T_pcp_c＝0 (5)

wherein p is_cFor a three-dimensional point in the camera coordinate system, the ground plane parameter can be represented as (n)^TD), and n ═ 0,0,1) R_pc，d＝t_p(3)。

Step 1.2, Camera calibration

There is distortion due to the lens manufacturing, and the distortion is mainly divided into a mirror distortion due to the lens shape and a tangential distortion due to the camera assembly process. The correction of the monocular camera is realized by adopting an FOV model, and the correction model is shown in figure 3. The model is a method for wide-angle camera distortion removal proposed by Devernay and Faugeras, and the mathematical description is as follows:

in the formula, x_uIs the pixel normalized coordinate, u_dIs the distorted pixel coordinate, ω is the distortion coefficient, r_d,r_uRespectively the distances from the normalized coordinates to the center of the coordinate system before and after correction.

By utilizing the camera model, mapping the pixel coordinates of the image to a normalized coordinate plane, and simultaneously combining a parameter matrix K in the camera to realize image distortion correction, namely:

u＝Kx_u(7)

step 2, initialization based on ground features

Step 2.1, feature tracking

FAST corner detection is a simple and FAST corner detection algorithm proposed by Edward Rosten and Tom Drummond. The algorithm judges whether a pixel point is an angular point or not by using the difference of gray values of adjacent pixel points in a certain neighborhood (usually a circular region) of the pixel point, and the following formula is as follows:

wherein u is_fIs a candidate corner, s is any point in the neighborhood of the candidate corner, c (u)_f) Represents u_fAnd (x) is the image gray value at x, and if N exceeds the threshold condition, the candidate corner point at x is the corner point candidate point.

Before a camera tracking thread, a system acquires current environment texture information by using a camera, constructs a four-layer Gaussian image pyramid, adopts a corner detection algorithm of FAST, establishes data correlation among corner features in a block search mode, predicts the corresponding relation between a three-dimensional point and a feature point in a current frame by using a projection relation, and searches a fixed area near the position to acquire an accurate matching relation.

Step 2.2, pose initialization

In the moving process of the robot, ground information is acquired through a camera, and tracking of corner features is achieved through a FAST algorithm. According to the data association information, a homography relation of the plane points can be established, namely:

π(Hu_r)＝u_p(9)

where pi (p) is the projection function, p ═ x, y, z, pi (p) ═ x/z, y/z,1), u_r，u_pRespectively a reference frame and a current frame. According to the translation amount T of camera_cAnd ground plane equation parameters (n)^TD), homography of the plane points can be established:

H＝KT_c(I|n_d)^TK^-1(10)

wherein n is_dAnd K is an intra-camera parameter matrix. Formula (3) is substituted into formula (10), and the homography relationship between the pose of the robot and the ground features can be described as follows:

H＝KT_crT_rsT_rc(I|n_d)TK^-1(11)

by utilizing the homography relation of the image feature points, a pose estimation model can be established:

wherein r (x) pi (h (x) u_ref)-u_pIs a homography error function, i.e., a projection error function, and H (x) is a homography matrix. According to the small displacement assumption, the first order Taylor expansion of the error function r is expressed as:

in this case, the extreme condition of the energy function is:

namely:

wherein,the pose estimation of the mobile robot can be obtained by iteratively solving the above formula. In order to further improve the positioning accuracy, a robustness weight is introduced to construct a pose estimation model based on M-animation, and at the moment, an energy function is expressed as:

wherein w (x) is a robust Tukey weighting function, and the extreme condition of the energy function is:

step 2.3, plane parameter estimation

The robot realizes robot positioning by utilizing the homography relation based on the ground features, but the positioning precision is difficult to ensure due to the influence of the non-ground features, so the invention adopts a rapid non-ground feature removing method. And constructing a normal estimation model according to a parallel vector cross multiplication mode. The experiment shows that the angle isAnd a certain error exists in point detection, so that ideal cross multiplication conditions cannot be met, and the normal estimation model adaptability is poor. And determining the non-ground characteristic information by using the projection error r as a judgment basis. But the method discriminates significantly less as the distance between the non-terrestrial features and the observation point decreases. Compared with the method, the method has the advantages that on the basis of initial estimation of the attitude, the homography matrix is used for directly deducing the normal estimation model, and whether the angular point is the ground characteristic information or not is judged by acquiring the difference between the normal estimation model and the parameters of the ground level equation. According to the plane homography relation, a plane parameter n can be established_dThe estimation model of (2):

wherein r (n)_d)＝π(H(n_d)u_ref)-u_pAs a function of the corresponding projection error. Similar to the pose estimation solving process, on the basis of the small displacement assumption, a vertical (18) solving model can be built according to a first-order Taylor formula of an error function.

At this time, the plane parameters may be obtained by iterative equation (18):

wherein,

step 3, pose estimation

The pose estimation module has the main function of completing the real-time update of the pose of the camera. The specific implementation process is as follows. After the initialization work is finished, a pose estimation model is established according to the projection error, and the mathematical description is as follows:

wherein e is_jIs a projection error, andis Tukey's double-weight objective function, sigma_TAnd (3) an unbiased estimation value of the feature point matching standard deviation, and mu is a camera pose six-tuple description vector.

For the failure condition existing in the tracking, the characteristic observation ratio is proposed to evaluate the tracking quality of each frame of image. If the ratio is lower than the set threshold, the tracking effect can be poor, the tracking is continued, but the system cannot update the frame into the map; if the situation that the characteristic observation ratio is lower than the set threshold value occurs in the tracking of the multi-frame images, the situation of 'tracking loss' can be considered, and then the tracking process is initialized. Through the steps, the pose is accurately estimated, and the tracking process is normally carried out.

Step 4, map creation

Step 4.1, map initialization

The system utilizes the ground features and a standard stereo camera algorithm model to establish a current environment initialization map. In the process of initializing the map, the FAST corner matching relation in the image is utilized, the RANSAC algorithm is combined, an initial pose estimation method based on ground features is adopted, a triangulation method is used for calculating the three-dimensional coordinates of the current feature point, the initial point is determined to be the origin of a global world coordinate system, and the initialization of the three-dimensional sparse map is completed.

In the initial drawing stage of the map, the specific process of acquiring two key frames is as follows, the camera is arranged on the mobile robot, and the initialization process is started after the camera is calibrated with the ground. In the initialization process, the mobile robot starts to move, meanwhile, the camera captures corner point information in the current scene and establishes association, and initial estimation of the current pose is obtained through a solving formula (12). When the initial estimate exceeds a set threshold d₀In practice, 2And 2, acquiring accurate estimation of the current pose by the pose estimation method based on the ground features.

Step 4.2, keyframe update and epipolar line search

When the map is initialized, only two key frames are contained, and only a small adjacent volume space can be described, and when the camera leaves the initial position, new key frames and map features need to be added. If the number of image frames between the camera and the current key frame exceeds the threshold value condition, the tracking effect of the camera is optimal, and the distance between the camera and the nearest map key point is as small as possible, the key frame adding process is automatically executed. Due to real-time constraints, the tracking system may only measure a subset of the visible features in the frame, and thus the mapping thread needs to re-plan and measure the remaining map features.

In the updating of the key frame, the system firstly performs Shi-Tomas evaluation on all corner points in the newly added key frame to acquire the current corner point information with the salient features, the salient points near the successfully observed feature points are discarded, and the retained salient points are used as candidate map nodes. Because the new map node needs known depth information and cannot be obtained only by a single-frame image, a key frame closest to the new map node needs to be selected as a second view, and a characteristic point sub-pixel precision mapping relation is established by using an epipolar line search and block matching method. In the second view, a zero mean SSD method is adopted, and accurate relocation of the camera is achieved by combining a pose estimation model. And finally, projecting the matching points to the space to generate the current global environment three-dimensional map.

Step 4.3, optimization

And (3) realizing the global consistency optimization of the current map by utilizing an optimal local and global LM (Levenberg-Marquardt) bundling adjustment algorithm to improve the robustness. The mathematical description of the bundle adjustment algorithm is as follows:

wherein σ_jiFor unbiased estimation of the match standard deviation of FAST feature points in the ith keyframe, μ_iPose six-tuple description vector, p, representing the ith keyframe_iAre points in the global map.

The stable cost function in (21) is made extremely small by the constant bundling adjustment. Global clustering adjusts the pose of all keyframes and the position of map points while exploiting the inherent sparsity in the structure-motion problem so that the complexity of the overall matrix decomposition presents a cubic-scale reduction, represented by O ((N + M)³) Down to O (N)³) However, in most estimation cases, the computation is limited by the generated outer dot product, and the computation amount increases significantly as the map grows.

Aiming at the problems, local clustering adjustment is introduced, only the poses of the subset of the key frames are adjusted, and the target function at the moment becomes:

x is a set to be adjusted of the series key frames, Y is a fixed frame set, and Z is a map point subset. Set X consists of the latest key frame and the 4 closest key frames in the map, and all the viewpoints in the map of key frames constitute set Z, and finally Y contains all the key frames obtained from the measurement data in Z. Therefore, the local clustering adjustment optimizes the pose of the latest and latest key frame, updates all points in the map, and meanwhile, the complexity is remarkably reduced and does not exceed O (NM).

Compared with the original PTAM algorithm, the invention adds the ground characteristic to improve the algorithm, and realizes the following advantages:

(1) the initialization process is improved, and the strict limitation of the motion of the camera by the PTAM algorithm is effectively removed.

(2) The relatively accurate metric map can be effectively established, and reliable guarantee is provided for further environmental measurement.

Drawings

FIG. 1 is a flow chart of a method according to the present invention;

FIG. 2 camera calibration model;

FIG. 3 is a schematic view of a planar parameter calibration.

Detailed Description

The invention is further described in detail below with reference to the accompanying drawings.

The flow chart of the PTAM improved algorithm based on the ground features is shown in the attached drawing (1), and specifically comprises the following steps:

step 1, parameter correction

Step 1.1, parameter definition

And establishing a robot pose representation according to the relation between the robot coordinate system and the world coordinate system, and determining ground plane calibration parameters according to the pose relation between the camera and the target plane.

Step 1.2, Camera calibration

And correcting the monocular camera by adopting an FOV model, mapping the image pixel coordinates to a normalized coordinate plane, and simultaneously combining a parameter matrix K in the camera to realize image distortion correction.

Step 2, initialization based on ground features

Step 2.1, feature tracking

And acquiring environment texture information, extracting features and establishing corner feature association.

Step 2.2, pose initialization

And establishing a homography relation and a pose initialization estimation model to realize pose initialization.

Step 2.3, plane parameter estimation

Step 3, pose estimation

And on the basis of finishing initialization, realizing the initialization pose estimation of the camera according to a pose estimation model based on the ground features. Further, the PTAM establishes a sub-pixel precision matching relation of corner feature in an image pyramid according to the camera initialization pose by adopting an epipolar line search mode, and brings a pose estimation model again to realize accurate relocation of the camera.

Step 4, map creation

Step 4.1, map initialization

System for establishing current environment initialization map by using standard stereo camera algorithm model

Step 4.2, keyframe update and epipolar line search

And adding a new key frame, establishing a sub-pixel precision mapping relation of the feature points by using an epipolar search and block matching method, and realizing accurate relocation of the camera by combining a pose estimation model. And finally, projecting the matching points to a space to complete the creation of the current global environment three-dimensional map.

Step 4.3, optimization

And the global and local clustering adjustment is utilized to realize the consistency optimization of the map.

Claims

1. A PTAM improvement method based on intelligent robot ground features is characterized in that:

firstly, completing parameter correction, which comprises parameter definition and camera correction; tracking features in the key frame, initializing the pose, and estimating plane parameters on the basis; then, acquiring current environment texture information by using a camera, constructing a four-layer Gaussian image pyramid, extracting feature information in a current image by using a FAST corner detection algorithm, and establishing data association among corner features to obtain a pose estimation model; in the initial drawing stage of the map, two key frames are acquired, a camera is arranged on a mobile robot, and the initialization process is started after the camera is calibrated with the ground; in the initialization process, the mobile robot starts to move, meanwhile, a camera captures angular point information in a current scene and establishes correlation, a pose estimation model acquires initial estimation of the current pose, and then a pose estimation method based on ground features is used for acquiring accurate estimation of the current pose; after the initialization of the three-dimensional sparse map is realized, updating the key frame, establishing a sub-pixel precision mapping relation of the feature points by using an epipolar line search and block matching method, and realizing the accurate relocation of the camera by combining a pose estimation model; finally, the matching points are projected to the space, and the creation of the current global environment three-dimensional map is completed; meanwhile, the consistency optimization of the algorithm is completed by utilizing global and local bundling adjustment;

the method specifically comprises the following steps:

step 1, parameter correction

Step 1.1, parameter definition

(1) Robot pose

Establishing a world coordinate system at the beginning of the mobile robot, and expressing the pose of the mobile robot in the moving process of the robot as follows:

<mrow> <mtable> <mtr> <mtd> <mrow> <msub> <mi>T</mi> <mi>r</mi> </msub> <mo>=</mo> <mfenced open = "(" close = ")"> <mtable> <mtr> <mtd> <msub> <mi>R</mi> <mi>r</mi> </msub> </mtd> <mtd> <msub> <mi>t</mi> <mi>r</mi> </msub> </mtd> </mtr> <mtr> <mtd> <mn>0</mn> </mtd> <mtd> <mn>1</mn> </mtd> </mtr> </mtable> </mfenced> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mi>R</mi> <mi>r</mi> </msub> <mo>=</mo> <mfenced open = "(" close = ")"> <mtable> <mtr> <mtd> <mrow> <mi>c</mi> <mi>o</mi> <mi>s</mi> <mi>&theta;</mi> </mrow> </mtd> <mtd> <mrow> <mi>s</mi> <mi>i</mi> <mi>n</mi> <mi>&theta;</mi> </mrow> </mtd> <mtd> <mn>0</mn> </mtd> </mtr> <mtr> <mtd> <mrow> <mo>-</mo> <mi>s</mi> <mi>i</mi> <mi>n</mi> <mi>&theta;</mi> </mrow> </mtd> <mtd> <mrow> <mi>cos</mi> <mi>&theta;</mi> </mrow> </mtd> <mtd> <mn>0</mn> </mtd> </mtr> <mtr> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mn>1</mn> </mtd> </mtr> </mtable> </mfenced> <mo>,</mo> <msub> <mi>t</mi> <mi>r</mi> </msub> <mo>=</mo> <mfenced open = "(" close = ")"> <mtable> <mtr> <mtd> <mi>x</mi> </mtd> </mtr> <mtr> <mtd> <mi>y</mi> </mtd> </mtr> <mtr> <mtd> <mn>0</mn> </mtd> </mtr> </mtable> </mfenced> </mrow> </mtd> </mtr> </mtable> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>1</mn> <mo>)</mo> </mrow> </mrow>

as can be seen from the above formula, the pose of the mobile robot is described by x ═ x, y, and θ, where x and y represent the translation of the mobile robot in the x-axis and y-axis directions, respectively, and θ is the rotation angle of the mobile robot around the z-axis;

in the world coordinate system, the pose of the camera is expressed as:

wherein, T_c∈ SE (3), SE (3) being a special rigid transformation group, R_cIs a 3 × 3 rotation matrix, t_cA translation matrix of 3 × 1 is used, and the position matrix establishes a world coordinate system and a point p under a camera coordinate system_c，p_wIs a corresponding relation of, i.e. p_c＝T_cp_w(ii) a In the method, a camera is fixed on a mobile robot, and a fixed rotation translation relation T exists between the camera and the robot_cr(ii) a In the moving process of the robot, if the pose increment of the robot is T at a certain time interval_rsCorresponding to this, the pose increment of the camera is T_csAnd then:

T_rs＝T_rcT_csT_cr(3)

wherein,

(2) ground plane calibration parameters

Fixing a camera on a mobile robot, and horizontally placing a calibration target on the ground; the camera establishes a pose relationship between the camera and the robot by acquiring image information with a calibration target; t is_cp∈ SE (3) is a transformation matrix between camera and calibration target, R_cp，t_cpRespectively represents T_cpOf the rotational and translational components of, inverse toAssuming that the coordinate origin of the robot is consistent with the camera, and the x-axis of the robot is oriented in the same direction as the z-axis of the camera; under the target coordinate system, the x-direction vector of the robot is represented by T_cpThe first two component representations of the row 3 vector of (a); at this time, the transformation relationship between the robot coordinate system and the target coordinate system is expressed as:

<mrow> <mtable> <mtr> <mtd> <mrow> <msub> <mi>p</mi> <mi>r</mi> </msub> <mo>=</mo> <msub> <mi>T</mi> <mrow> <mi>r</mi> <mi>p</mi> </mrow> </msub> <msub> <mi>p</mi> <mi>p</mi> </msub> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mi>T</mi> <mrow> <mi>r</mi> <mi>p</mi> </mrow> </msub> <mo>=</mo> <mfenced open = "(" close = ")"> <mtable> <mtr> <mtd> <mrow> <mi>c</mi> <mi>o</mi> <mi>s</mi> <mi>&gamma;</mi> </mrow> </mtd> <mtd> <mrow> <mi>sin</mi> <mi>&gamma;</mi> </mrow> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <msub> <mi>x</mi> <mi>r</mi> </msub> </mtd> </mtr> <mtr> <mtd> <mrow> <mo>-</mo> <mi>sin</mi> <mi>&gamma;</mi> </mrow> </mtd> <mtd> <mrow> <mi>cos</mi> <mi>&gamma;</mi> </mrow> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <msub> <mi>y</mi> <mi>r</mi> </msub> </mtd> </mtr> <mtr> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mn>1</mn> </mtd> <mtd> <mn>0</mn> </mtd> </mtr> <mtr> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mn>1</mn> </mtd> </mtr> </mtable> </mfenced> </mrow> </mtd> </mtr> </mtable> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>4</mn> <mo>)</mo> </mrow> </mrow>

wherein gamma is the rotation angle of the robot coordinate system and the target coordinate system, (x)_r,y_r) The coordinates of the robot origin under a target coordinate system are determined by translation vectors of the camera and the target; the transformation relationship between the robot and the camera is therefore denoted T_cr＝T_cpT_pr，Acquiring a ground plane equation under a camera coordinate system according to the pose relation between the camera and the target plane:

(0,0,1,0)T_pcp_c＝0 (5)

wherein p is_cThe ground plane parameter is expressed as (n) for a three-dimensional point under the camera coordinate system^TD), and n ═ 0,0,1) R_pc，d＝t_p(3)；

Step 1.2, Camera calibration

There is distortion due to the lens manufacturing, and the distortion is mainly divided into a mirror distortion caused by the lens shape and a tangential distortion caused by the camera assembly process; the method adopts an FOV model to realize the correction of the monocular camera; the model is a method for wide-angle camera distortion removal proposed by Devernay and Faugeras, and the mathematical description is as follows:

<mrow> <mtable> <mtr> <mtd> <mrow> <msub> <mi>u</mi> <mi>d</mi> </msub> <mo>=</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <msub> <mi>u</mi> <mn>0</mn> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mi>v</mi> <mn>0</mn> </msub> </mtd> </mtr> </mtable> </mfenced> <mo>+</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <msub> <mi>f</mi> <mi>u</mi> </msub> </mtd> <mtd> <mn>0</mn> </mtd> </mtr> <mtr> <mtd> <mn>0</mn> </mtd> <mtd> <msub> <mi>f</mi> <mi>v</mi> </msub> </mtd> </mtr> </mtable> </mfenced> <mfrac> <msub> <mi>r</mi> <mi>d</mi> </msub> <msub> <mi>r</mi> <mi>u</mi> </msub> </mfrac> <msub> <mi>x</mi> <mi>u</mi> </msub> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mi>r</mi> <mi>d</mi> </msub> <mo>=</mo> <mfrac> <mn>1</mn> <mi>&omega;</mi> </mfrac> <mi>arctan</mi> <mrow> <mo>(</mo> <mn>2</mn> <msub> <mi>r</mi> <mi>u</mi> </msub> <mi>tan</mi> <mfrac> <mi>&omega;</mi> <mn>2</mn> </mfrac> <mo>)</mo> </mrow> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mi>r</mi> <mi>u</mi> </msub> <mo>=</mo> <mfrac> <mrow> <mi>tan</mi> <mrow> <mo>(</mo> <msub> <mi>r</mi> <mi>d</mi> </msub> <mi>&omega;</mi> <mo>)</mo> </mrow> </mrow> <mrow> <mn>2</mn> <mi>tan</mi> <mfrac> <mi>&omega;</mi> <mn>2</mn> </mfrac> </mrow> </mfrac> </mrow> </mtd> </mtr> </mtable> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>6</mn> <mo>)</mo> </mrow> </mrow>

in the formula, x_uIs the pixel normalized coordinate, u_dIs the distorted pixel coordinate, ω is the distortion coefficient, r_d,r_uRespectively the distances from the normalized coordinates to the center of the coordinate system before and after correction;

mapping the pixel coordinates of the image to a normalized coordinate plane by using a camera model, and simultaneously combining a parameter matrix K in the camera to realize image distortion correction, namely:

u＝Kx_u(7)

step 2, initialization based on ground features

Step 2.1, feature tracking

FAST corner detection is a simple and FAST corner detection algorithm proposed by Edward Rosten and Tom Drummond; the algorithm judges whether the pixel point is an angular point or not by utilizing the difference of the gray values of adjacent pixel points in a certain neighborhood of the pixel point, and the following formula is as follows:

<mrow> <mi>N</mi> <mo>=</mo> <munder> <mi>&Sigma;</mi> <mrow> <mi>s</mi> <mo>&Element;</mo> <mi>c</mi> <mrow> <mo>(</mo> <msub> <mi>u</mi> <mi>f</mi> </msub> <mo>)</mo> </mrow> </mrow> </munder> <mo>|</mo> <mi>I</mi> <mrow> <mo>(</mo> <mi>s</mi> <mo>)</mo> </mrow> <mo>-</mo> <mi>I</mi> <mrow> <mo>(</mo> <msub> <mi>u</mi> <mi>f</mi> </msub> <mo>)</mo> </mrow> <mo>|</mo> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>8</mn> <mo>)</mo> </mrow> </mrow>

wherein u is_fIs a candidate corner, s is any point in the neighborhood of the candidate corner, c (u)_f) Represents u_fThe neighborhood of (a), I(s), is the image gray value at s, if N exceeds the threshold condition, the candidate corner at s is the corner;

before a camera tracking thread, a system acquires current environment texture information by using a camera, constructs a four-layer Gaussian image pyramid, adopts a corner detection algorithm of FAST, establishes data correlation among corner features in a block search mode, predicts the corresponding relation between a three-dimensional point and a feature point in a current frame by using a projection relation, and searches a fixed area near the position to acquire an accurate matching relation;

step 2.2, pose initialization

In the moving process of the robot, acquiring ground information through a camera, and tracking corner features by using a FAST algorithm; according to the data association information, establishing a homography relation of the plane points, namely:

π(Hu_r)＝u_p(9)

where pi (p) is the projection function, p ═ x, y, z, pi (p) ═ x/z, y/z,1), u_r，u_pRespectively corresponding feature points under a reference frame and a current frame; according to the translation amount T of camera_cAnd ground plane equation parameters (n)^TD), establishing a homography relation of the plane points:

H＝KT_c(I|n_d)^TK^-1(10)

wherein n is_dN/d, and K is an in-camera parameter matrix; formula (3) is substituted into formula (10), and the homography relationship between the pose of the robot and the ground features is described as follows:

H＝KT_crT_rsT_rc(I|n_d)TK^-1(11)

establishing a pose estimation model by using the homography relation of the image feature points:

<mrow> <mtable> <mtr> <mtd> <mrow> <mi>x</mi> <mo>=</mo> <mi>arg</mi> <munder> <mi>min</mi> <mi>x</mi> </munder> <mi>f</mi> <mrow> <mo>(</mo> <mi>x</mi> <mo>)</mo> </mrow> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <mi>f</mi> <mrow> <mo>(</mo> <mi>x</mi> <mo>)</mo> </mrow> <mo>=</mo> <munder> <mi>&Sigma;</mi> <mi>p</mi> </munder> <mo>|</mo> <mo>|</mo> <mi>r</mi> <mrow> <mo>(</mo> <mi>x</mi> <mo>)</mo> </mrow> <mo>|</mo> <msup> <mo>|</mo> <mn>2</mn> </msup> </mrow> </mtd> </mtr> </mtable> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>12</mn> <mo>)</mo> </mrow> </mrow>

wherein r (x) pi (h (x) u_ref)-u_pIs a homography error function, i.e. a projection error function, H (x) is a homography matrix; according to the small displacement assumption, the first order Taylor expansion of the error function r is expressed as:

in this case, the extreme condition of the energy function is:

<mrow> <mfrac> <mrow> <mo>&part;</mo> <mi>f</mi> </mrow> <mrow> <mo>&part;</mo> <mi>&xi;</mi> </mrow> </mfrac> <mo>=</mo> <msup> <mi>&Sigma;J</mi> <mi>T</mi> </msup> <mi>r</mi> <mo>=</mo> <mn>0</mn> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>14</mn> <mo>)</mo> </mrow> </mrow>

namely:

<mrow> <mtable> <mtr> <mtd> <mrow> <munder> <mi>&Sigma;</mi> <mi>p</mi> </munder> <msup> <mi>J</mi> <mi>T</mi> </msup> <mrow> <mo>(</mo> <mi>r</mi> <mo>(</mo> <mn>0</mn> <mo>)</mo> <mo>+</mo> <mi>J</mi> <mi>&xi;</mi> <mo>)</mo> </mrow> <mo>=</mo> <mn>0</mn> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <mi>&xi;</mi> <mo>=</mo> <mo>-</mo> <msup> <mrow> <mo>(</mo> <munder> <mi>&Sigma;</mi> <mi>p</mi> </munder> <msup> <mi>J</mi> <mi>T</mi> </msup> <mi>J</mi> <mo>)</mo> </mrow> <mrow> <mo>-</mo> <mn>1</mn> </mrow> </msup> <munder> <mi>&Sigma;</mi> <mi>p</mi> </munder> <msup> <mi>J</mi> <mi>T</mi> </msup> <mi>r</mi> <mrow> <mo>(</mo> <mn>0</mn> <mo>)</mo> </mrow> </mrow> </mtd> </mtr> </mtable> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>15</mn> <mo>)</mo> </mrow> </mrow>

wherein,obtaining pose estimation of the mobile robot by iterative solution of the above formula; in order to further improve the positioning accuracy, a robustness weight is introduced to construct a pose estimation model based on M-animation, and at the moment, an energy function is expressed as:

<mrow> <mi>f</mi> <mrow> <mo>(</mo> <mi>x</mi> <mo>)</mo> </mrow> <mo>=</mo> <munder> <mo>&Sigma;</mo> <mi>p</mi> </munder> <mi>w</mi> <mrow> <mo>(</mo> <mi>r</mi> <mo>)</mo> </mrow> <mo>|</mo> <mo>|</mo> <mi>r</mi> <mo>|</mo> <msup> <mo>|</mo> <mn>2</mn> </msup> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>16</mn> <mo>)</mo> </mrow> </mrow>

w (r) is a robust Tukey weighting function, and the extreme condition of the energy function is:

<mrow> <mi>&xi;</mi> <mo>=</mo> <mo>-</mo> <msup> <mrow> <mo>(</mo> <munder> <mi>&Sigma;</mi> <mi>p</mi> </munder> <msup> <mi>J</mi> <mi>T</mi> </msup> <mi>w</mi> <mo>(</mo> <mi>r</mi> <mo>)</mo> <mi>J</mi> <mo>)</mo> </mrow> <mrow> <mo>-</mo> <mn>1</mn> </mrow> </msup> <munder> <mi>&Sigma;</mi> <mi>p</mi> </munder> <msup> <mi>J</mi> <mi>T</mi> </msup> <mi>w</mi> <mrow> <mo>(</mo> <mi>r</mi> <mo>)</mo> </mrow> <mi>r</mi> <mrow> <mo>(</mo> <mn>0</mn> <mo>)</mo> </mrow> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>17</mn> <mo>)</mo> </mrow> </mrow>

step 2.3, plane parameter estimation

The robot realizes robot positioning by utilizing a homography relation based on ground features, but the positioning precision is difficult to ensure due to the influence of non-ground features, so a rapid non-ground feature removing method is adopted; constructing a normal estimation model according to a parallel vector cross multiplication mode; because the angular point detection has errors, ideal cross multiplication conditions cannot be met, and the normal estimation model has poor fitness; determining non-ground characteristic information by using the projection error r as a judgment basis; but as the distance between the non-ground features and the observation points is reduced, the discrimination of the method is obviously reduced; compared with the method, on the basis of initial estimation of the attitude, a homography matrix is used for directly deducing a normal estimation model, and whether the angular point is ground characteristic information is judged by obtaining the difference between the normal estimation model and parameters of a ground plane equation; establishing a plane parameter n according to a plane homography relation_dThe estimation model of (2):

<mrow> <mi>f</mi> <mrow> <mo>(</mo> <msub> <mi>n</mi> <mi>d</mi> </msub> <mo>)</mo> </mrow> <mo>=</mo> <munder> <mo>&Sigma;</mo> <mi>p</mi> </munder> <mo>|</mo> <mo>|</mo> <mi>r</mi> <mrow> <mo>(</mo> <msub> <mi>n</mi> <mi>d</mi> </msub> <mo>)</mo> </mrow> <mo>|</mo> <msup> <mo>|</mo> <mn>2</mn> </msup> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>18</mn> <mo>)</mo> </mrow> </mrow>

wherein r (n)_d)＝π(H(n_d)u_ref)-u_pAs a function of the corresponding projection error; similar to the pose estimation solving process, on the basis of the small displacement assumption, a vertical (18) solving model is established according to a first-order Taylor formula of an error function;

at this time, the plane parameters are obtained by iterative equation (18):

<mrow> <mtable> <mtr> <mtd> <mrow> <msub> <mi>n</mi> <mi>d</mi> </msub> <mo>=</mo> <msub> <mi>n</mi> <mi>d</mi> </msub> <mo>+</mo> <mover> <mi>n</mi> <mo>^</mo> </mover> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <mover> <mi>n</mi> <mo>^</mo> </mover> <mo>=</mo> <mo>-</mo> <msup> <mrow> <mo>(</mo> <munder> <mo>&Sigma;</mo> <mi>p</mi> </munder> <msubsup> <mi>J</mi> <mi>n</mi> <mi>T</mi> </msubsup> <msub> <mi>J</mi> <mi>n</mi> </msub> <mo>)</mo> </mrow> <mrow> <mo>-</mo> <mn>1</mn> </mrow> </msup> <munder> <mo>&Sigma;</mo> <mi>p</mi> </munder> <msubsup> <mi>J</mi> <mi>n</mi> <mi>T</mi> </msubsup> <mi>r</mi> <mrow> <mo>(</mo> <msub> <mi>n</mi> <mi>d</mi> </msub> <mo>)</mo> </mrow> </mrow> </mtd> </mtr> </mtable> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>19</mn> <mo>)</mo> </mrow> </mrow>

wherein,

step 3, pose estimation

The pose estimation module has the main function of completing the real-time update of the pose of the camera; the specific implementation process is as follows; after the initialization work is finished, a pose estimation model is established according to the projection error, and the mathematical description is as follows:

<mrow> <mi>&xi;</mi> <mo>=</mo> <munder> <mi>argmin</mi> <mi>&xi;</mi> </munder> <mi>&Sigma;</mi> <mi>O</mi> <mi>b</mi> <mi>j</mi> <mrow> <mo>(</mo> <mfrac> <mrow> <mo>|</mo> <msub> <mi>e</mi> <mi>j</mi> </msub> <mo>|</mo> </mrow> <msub> <mi>&sigma;</mi> <mi>j</mi> </msub> </mfrac> <mo>,</mo> <msub> <mi>&sigma;</mi> <mi>T</mi> </msub> <mo>)</mo> </mrow> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>20</mn> <mo>)</mo> </mrow> </mrow>

wherein e is_jIs a projection error, andis Tukey's double-weight objective function, sigma_TThe unbiased estimation value of the feature point matching standard deviation is obtained, and mu is a camera pose six-tuple description vector;

for the failure condition existing in the tracking, a characteristic observation ratio is provided to evaluate the tracking quality of each frame of image; if the ratio is lower than the set threshold value, if the tracking effect is poor, the tracking is continued, but the system cannot update the frame into the map; if the situation that the characteristic observation ratio is lower than the set threshold value occurs in the tracking of the multi-frame images, namely the situation of 'tracking loss' is considered, then the tracking process is initialized; accurate estimation of the pose is achieved through the steps, and the tracking process can be normally carried out;

step 4, map creation

Step 4.1, map initialization

The system utilizes the ground characteristics and a standard stereo camera algorithm model to establish a current environment initialization map; in the process of initializing the map, utilizing a FAST corner matching relation in an image, combining a RANSAC algorithm, adopting an initial pose estimation method based on ground features, calculating three-dimensional coordinates of a current feature point by using a triangulation method, and determining an initial point as an origin of a global world coordinate system to complete initialization of the three-dimensional sparse map;

in the initial drawing stage of the map, the specific process of acquiring two key frames is as follows, a camera is arranged on a mobile robot, and the initialization process is started after the camera is calibrated with the ground; in the initialization process, the mobile robot starts to move, meanwhile, a camera captures corner point information in a current scene and establishes association, and initial estimation of a current pose is obtained through a solving formula (12); when the initial estimate exceeds a set threshold d₀Then, a pose estimation method based on the ground features in the step 2.2 is used for obtaining accurate estimation of the current pose;

step 4.2, keyframe update and epipolar line search

When the map is initialized, only two key frames are contained, and only a small adjacent volume space can be described, and when the camera leaves the initial position, new key frames and map features need to be added; if the number of image frames between the camera and the current key frame exceeds a threshold value condition, the tracking effect of the camera is optimal, and the distance between the camera and the nearest map key point is as small as possible, the key frame adding process is automatically executed; due to real-time constraints, the tracking system may measure only a subset of the visible features in the frame, and therefore the mapping thread needs to re-plan and measure the remaining map features;

in the updating of the key frame, the system firstly carries out Shi-Tomas evaluation on all corner points in the newly added key frame to acquire the current corner point information with the salient features, the salient points near the feature points which are observed successfully are discarded, and the retained salient points are taken as candidate map nodes; because the new map node needs known depth information and can not be obtained only by a single-frame image, a key frame closest to the new map node needs to be selected as a second view, and a characteristic point sub-pixel precision mapping relation is established by using an epipolar line search and block matching method; in the second view, a zero mean SSD method is adopted, and accurate relocation of the camera is realized by combining a pose estimation model; finally, the matching points are projected to the space to generate a current global environment three-dimensional map;

step 4.3, optimization

The optimal local and global LM cluster adjustment algorithm is utilized to realize the global consistency optimization of the current map so as to improve the robustness; the mathematical description of the bundle adjustment algorithm is as follows:

<mrow> <mo>{</mo> <mo>{</mo> <msub> <mi>&mu;</mi> <mn>2</mn> </msub> <mo>...</mo> <msub> <mi>&mu;</mi> <mi>N</mi> </msub> <mo>}</mo> <mo>,</mo> <mo>{</mo> <msub> <msup> <mi>p</mi> <mo>&prime;</mo> </msup> <mn>1</mn> </msub> <mo>...</mo> <msub> <msup> <mi>p</mi> <mo>&prime;</mo> </msup> <mi>M</mi> </msub> <mo>}</mo> <mo>}</mo> <mo>=</mo> <munder> <mrow> <mi>arg</mi> <mi>min</mi> </mrow> <mrow> <mo>{</mo> <mo>{</mo> <mi>&mu;</mi> <mo>}</mo> <mo>,</mo> <mo>{</mo> <mi>p</mi> <mo>}</mo> <mo>}</mo> </mrow> </munder> <munder> <mo>&Sigma;</mo> <mrow> <mi>i</mi> <mo>&Element;</mo> <mi>X</mi> <mo>&Element;</mo> <mi>Y</mi> </mrow> </munder> <munder> <mo>&Sigma;</mo> <mrow> <mi>j</mi> <mo>&Element;</mo> <msub> <mi>S</mi> <mi>i</mi> </msub> </mrow> </munder> <mi>O</mi> <mi>b</mi> <mi>j</mi> <mrow> <mo>(</mo> <mfrac> <mrow> <mo>|</mo> <msub> <mi>e</mi> <mrow> <mi>j</mi> <mi>i</mi> </mrow> </msub> <mo>|</mo> </mrow> <msub> <mi>&sigma;</mi> <mrow> <mi>j</mi> <mi>i</mi> </mrow> </msub> </mfrac> <mo>,</mo> <msub> <mi>&sigma;</mi> <mi>T</mi> </msub> <mo>)</mo> </mrow> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>21</mn> <mo>)</mo> </mrow> </mrow>

wherein σ_jiFor unbiased estimation of the match standard deviation of FAST feature points in the ith keyframe, μ_iPose six-tuple description vector, p, representing the ith keyframe_iPoints in the global map;

the stable cost function in the formula (21) is extremely small through continuous bundling adjustment; global clustering adjusts the pose of all keyframes and the position of map points while exploiting the inherent sparsity in the structure-motion problem so that the complexity of the overall matrix decomposition presents a cubic-scale reduction, represented by O ((N + M)³) Down to O (N)³) However, in most estimation cases, the method is limited by the generated outer dot vector product, and the calculation amount is increased remarkably along with the growth of the map;

<mrow> <mo>{</mo> <mo>{</mo> <msub> <mi>&mu;</mi> <mi>&chi;</mi> </msub> <mo>&Element;</mo> <mi>X</mi> <mo>}</mo> <mo>,</mo> <mo>{</mo> <msub> <msup> <mi>p</mi> <mo>&prime;</mo> </msup> <mi>z</mi> </msub> <mo>&Element;</mo> <mi>Z</mi> <mo>}</mo> <mo>}</mo> <mo>=</mo> <munder> <mi>argmin</mi> <mrow> <mo>{</mo> <mo>{</mo> <mi>&mu;</mi> <mo>}</mo> <mo>,</mo> <mo>{</mo> <mi>p</mi> <mo>}</mo> <mo>}</mo> </mrow> </munder> <munder> <mo>&Sigma;</mo> <mrow> <mi>i</mi> <mo>&Element;</mo> <mi>X</mi> <mo>&Element;</mo> <mi>Y</mi> </mrow> </munder> <munder> <mo>&Sigma;</mo> <mrow> <mi>j</mi> <mo>&Element;</mo> <mi>Z</mi> <mo>&cap;</mo> <msub> <mi>S</mi> <mi>i</mi> </msub> </mrow> </munder> <mi>O</mi> <mi>b</mi> <mi>j</mi> <mrow> <mo>(</mo> <mi>i</mi> <mo>,</mo> <mi>j</mi> <mo>)</mo> </mrow> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>22</mn> <mo>)</mo> </mrow> </mrow>

x is a set to be adjusted of a series of key frames, Y is a fixed frame set, and Z is a map point subset; the set X consists of a latest key frame and 4 nearest key frames in the map, all the visual points in the map of the key frames form a set Z, and finally Y comprises all the key frames obtained by the measurement data in Z; therefore, the local clustering adjustment optimizes the pose of the latest and latest key frame, updates all points in the map, and meanwhile, the complexity is remarkably reduced and does not exceed O (NM).