CN104732518A - PTAM improvement method based on ground characteristics of intelligent robot - Google Patents

Abstract

The invention discloses a PTAM improvement method based on ground characteristics of an intelligent robot. The PTAM improvement method based on ground characteristics of the intelligent robot comprises the steps that firstly, parameter correction is completed, wherein parameter correction includes parameter definition and camera correction; secondly, current environment texture information is obtained by means of a camera, a four-layer Gausses image pyramid is constructed, the characteristic information in a current image is extracted by means of the FAST corner detection algorithm, data relevance between corner characteristics is established, and then a pose estimation model is obtained; two key frames are obtained so as to erect the camera on the mobile robot at the initial map drawing stage; the mobile robot begins to move in the initializing process, corner information in the current scene is captured through the camera and association is established at the same time; after a three-dimensional sparse map is initialized, the key frames are updated, the sub-pixel precision mapping relation between characteristic points is established by means of an extreme line searching and block matching method, and accurate re-positioning of the camera is achieved based on the pose estimation model; finally, matched points are projected in the space, so that a three-dimensional map for the current overall environment is established.

Description

A kind of PTAM based on intelligent robot terrain surface specifications improves one's methods

Technical field

The invention belongs to robot vision field, relate to a kind of PTAM algorithm improvement based on terrain surface specifications.

Background technology

Along with the relation of robot and the mankind is more and more tightr, the correlation technique of intelligent robot receives great attention.The synchronous location of mobile robot is one of location technology of the most main flow of current intelligent mobile robot with building figure (Simultaneous Localization and Mapping, SLAM).In fact it is exactly an estimation problem, the inside and outside data utilizing sensor to obtain, and calculates mobile robot position at a time, sets up the cartographic model relied on simultaneously.The SLAM of view-based access control model is the research category belonging to vision measurement, and because vision sensor has the advantage of self uniqueness: volume is little, lightweight, low price is easy for installation, and the external information of extraction is very abundant.These advantages further promote the investigation and application of the SLAM of current view-based access control model.Application number is 201310678018.2 provide a kind of SURF that adopts as the feature detection operator of monocular vision SLAM, respectively from the detection of point-of-interest, three aspects such as the generation of SURF descriptor and SURF Point matching are innovated and improve, propose the simultaneous localization and mapping that the SURF feature matching method increased based on region completes robot, make in SURF descriptor matching process, the descriptor that a certain descriptor only matches with most probable is compared, thus the comparison number of times decreased significantly in monocular vision SLAM problem, improve matching speed.But the method still exists the problem can not set up Metric map and cannot provide three-dimensional feature information, Algorithm robustness and real-time need to be improved further.

From motion, creation environment modelling technique is a kind of method simultaneously solving camera motion track and scene structure model.As the exemplary process of SFM, the people such as G.Klein first proposed the concept of PTAM (Parallel Tracking and Mapping) in augmented reality (AR) field in 2007, be used for solving Context awareness problem in real time.But also there is the problem cannot setting up Metric map in the PTAM algorithm do not improved, has strict restriction to the movement of camera simultaneously.

Summary of the invention

For above Problems existing, the present invention proposes a kind of PTAM innovatory algorithm based on terrain surface specifications, on the basis adding terrain surface specifications, PTAM synchronously estimates the current pose of monocular-camera and three dimensions point, upgrade while detecting unique point utilizing FAST angle point method, with local and the overall light-stream adjustment of optimum, constantly upgrade the pose of video camera and three-dimensional feature point, complete hi-Fix and map building, this invention efficiently solves the problem that cannot create Metric map, extract in real-time and three-dimensional feature simultaneously and have great raising, for solution robot navigation location, relevant issues important in inhibiting in three-dimensional reconstruction.

The present invention adopts following technical scheme:

First, complete parameter correction, this comprises parameter definition and camera calibration; Then the feature in key frame is followed the tracks of, then realize the initialization of pose, then this basis is estimated plane parameter; Then utilize camera to obtain current environment texture information, and build four layers of Gaussian image pyramid, use the characteristic information in FAST Corner Detection Algorithm extraction present image, set up the data correlation between Corner Feature, obtain pose estimation model; In the initial drawing stage of map, obtain two key frames and camera frame is located on mobile robot, start to carry out initialization procedure after being demarcated by camera and ground; In initialization procedure, mobile robot starts mobile, camera is caught angle point information in current scene and is associated simultaneously, obtains current pose initial estimation, then use the position and orientation estimation method based on terrain surface specifications to obtain the accurate estimation of current pose by pose estimation model; After realizing the initialization of sparse 3 D map, upgrade key frame and utilize polar curve to search for and set up unique point sub-pixel precision mapping relations with block matching method, realize the accurate reorientation of camera in conjunction with pose estimation model.Finally match point is projected to space, complete current global context three-dimensional map and create.Utilize overall situation and partial situation's boundling to adjust the consistance optimization of algorithm simultaneously.

Specifically comprise the following steps:

Step 1, parameter correction

Step 1.1, parameter defines

(1) robot pose

Set up world coordinate system at mobile robot's section start, in robot moving process, its pose can be expressed as:

$\begin{array}{c}{T}_r=\left(\begin{array}{cc}{R}_r& t_r\\ 0& 1\end{array}\right)\\ R_r=\left(\begin{array}{ccc}\cos \mathrm{\θ}& \sin \mathrm{\θ}& 0\\ -\sin \mathrm{\θ}& \cos \mathrm{\θ}& 0\\ 0& 0& 1\end{array}\right),t_r=\left(\begin{array}{c}x\\ y\\ 0\end{array}\right)\end{array}---\left(1\right)$

Can be described by x=(x, y, θ) by the known mobile robot's pose of above formula, wherein, x, y represent that mobile robot is in x-axis respectively, the translational movement in y-axis direction, and θ is the anglec of rotation of mobile robot around z-axis.

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

$T_c=\left(\begin{array}{cc}{R}_c& t_c\\ 0& 1\end{array}\right)---\left(2\right)$

Wherein, T _c∈ SE (3), SE (3) are special rigid body translation group, R _cbe 3 × 3 rotation matrixs, t _cbe 3 × 1 translation matrix.This position auto―control puts p under establishing world coordinate system and camera coordinates system _c, p _wcorresponding relation, i.e. p _c=T _cp _w.Shown in accompanying drawing 2, in this research, camera is fixed on mobile robot, there is fixing rotation translation relation T between camera and robot _cr.In robot moving process, if robot pose increment in interval is T sometime _rs, corresponding with it, the pose increment of camera is T _cs, then:

T _rs＝T _rcT _csT _cr(3)

Wherein, $T_{\mathrm{cr}}=T_{\mathrm{rc}}^{-1}.$

(2) ground level calibrating parameters

Camera is fixed on mobile robot, and demarcation target is lain in ground.Camera sets up position orientation relation between camera and robot by obtaining the image information with demarcation target.T _cp∈ SE (3) is the transformation matrix between camera and demarcation target, R _cp, t _cprepresent T respectively _cprotation and translation component, its inverse transformation T _cp=T _p ^- _c ¹.Assuming that the true origin of robot and camera are consistent, and the z-axis of the x-axis of robot and camera is towards identical.Under target co-ordinates system, the x direction vector of robot can by T _cpthe first two representation in components of the 3rd row vector.Now, the transformation relation of robot coordinate system and target co-ordinates system can be expressed as:

p _r＝T _rpp _p

$T_{\mathrm{rp}}=\left(\begin{array}{cccc}\cos \mathrm{\γ}& \sin \mathrm{\γ}& 0& x_r\\ -\sin \mathrm{\γ}& \cos \mathrm{\γ}& 0& y_r\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right)---\left(4\right)$

Wherein, γ is the anglec of rotation of robot coordinate system and target co-ordinates system, (x _r, y _r) be the coordinate of robot initial point under target co-ordinates system, this coordinate can be determined by the translation vector of camera and target.Therefore the transformation relation between robot and camera can be expressed as T _cr=T _cpt _pr, according to camera and the interplanar position orientation relation of target, the ground level equation under camera coordinates system can be obtained:

(0,0,1,0)T _pcp _c＝0 (5)

Wherein, p _cfor three-dimensional point under camera coordinates system, ground level parameter can be expressed as (n ^t, d), and n=(0,0,1) R _pc, d=t _p(3).

Step 1.2, camera calibration

Because the reason of lens on manufacturing can exist distortion, distortion mainly divides for the mirror caused by lens shape to the tangential distortion distorted and caused by video camera assembling process.Adopt FOV model realization to the rectification of monocular camera herein, calibration model is as Fig. 3.This model is that the one proposed by Devernay and Faugeras goes distortion method for wide angle camera, and its mathematical description is as follows:

$u_d=\left[\begin{array}{c}{u}_0\\ v_0\end{array}\right]+\left[\begin{array}{cc}{f}_u& 0\\ 0& f_v\end{array}\right]\frac{r_d}{r_u}{x}_u$

$r_d=\frac{1}{\mathrm{\ω}}\arctan \left(2r_u\tan \frac{\mathrm{\ω}}{2}\right)---\left(6\right)$

$r_u=\frac{\tan \left(r_d\mathrm{\ω}\right)}{2\tan \frac{\mathrm{\ω}}{2}}$

In formula, x _upixel normalization coordinate, u _dbe pixel coordinate after distortion, ω is distortion factor, r _d, r _ube respectively and correct the distance of normalization coordinate in front and back to coordinate system center.

Utilize above-mentioned camera model, image pixel coordinates is mapped to normalization coordinate plane, simultaneously combining camera Intrinsic Matrix K, realize pattern distortion and correct, that is:

u＝Kx _u(7)

Step 2, based on the initialization of terrain surface specifications

Step 2.1, signature tracking

FAST Corner Detection is a kind of simple and quick Corner Detection Algorithm proposed by Edward Rosten and Tom Drummond.This algorithm utilizes the difference of the interior neighbor pixel gray-scale value of certain neighborhood of pixel (being generally border circular areas), judges whether this pixel is angle point, as shown in the formula:

$N=\underset{S\∈C\left(u_f\right)}{\mathrm{\Σ}}|I\left(s\right)-I\left(u_f\right)|---\left(8\right)$

Wherein, u _ffor candidate angular, s is any point in candidate angular neighborhood, c (u _f) represent u _fneighborhood, I (x) is at x place image intensity value, if N exceeds threshold condition, this candidate point is angle point.

Before camera track thread, system utilizes camera to obtain current environment texture information, and build four layers of Gaussian image pyramid, adopt the Corner Detection Algorithm of FAST, the mode of combined block search sets up the data correlation between Corner Feature, utilize projection relation to predict the corresponding relation of unique point in three-dimensional point and present frame, and fixed area is searched near position, to obtain matching relationship accurately.

Step 2.2, pose initialization

In robot moving process, obtain terrestrial information by camera, and utilize the tracking of FAST algorithm realization Corner Feature.According to above-mentioned data association message, the homography relation of planar point can be set up, that is:

π(Hu _r)＝u _p(9)

Wherein, π (p) is projection function, p=(x, y, z), π (p)=(x/z, y/z, 1), u _r, u _pbe respectively character pair point under reference frame and present frame.According to camera translational movement T _cand ground level equation parameter (n ^t, d), the homography relation of planar point can be set up:

H＝KT _c(I|n _d) ^TK ^-1(10)

Wherein, n _d=n/d, K are camera Intrinsic Matrix.Formula (3) is substituted into formula (10), and now the pose of robot and terrain surface specifications homography relation can be described as:

H＝KT _rcT _rsT _rc(I|n _d) ^TK ^-1(11)

Utilize the homography relation of above-mentioned image characteristic point, pose estimation model can be set up:

$\begin{array}{c}x=\arg \underset{x}{\min }f\left(x\right)\\ f\left(x\right)=\underset{p}{\mathrm{\Σ}}{\left|\right|r\left(x\right)\left|\right|}^2\end{array}---\left(12\right)$

Wherein, r (x)=π (H (x) u _ref)-u _pfor homography relationship error function, i.e. projection error function, H (x) is homography matrix.According to thin tail sheep hypothesis, the first order Taylor of error function r launches to be expressed as:

$r\left(\mathrm{\ξ}\right)\left(0\right)+\frac{\∂r}{\∂\mathrm{\ξ}}{|}_{\mathrm{\ξ}=0}\mathrm{\ξ}---\left(13\right)$

Now, the extremum conditions of energy function is:

$\frac{\∂f}{\∂\mathrm{\ξ}}=\mathrm{\Σ}{J}^{T}r=0---\left(14\right)$

That is:

$\begin{array}{c}\underset{p}{\mathrm{\Σ}}{J}^T(r\left(0\right)+\mathrm{J\ξ})=0\\ \mathrm{\ξ}=-{\left(\underset{p}{\mathrm{\Σ}}{J}^{T}J\right)}^{-1}\underset{p}{\mathrm{\Σ}}{J}^{T}r\left(0\right)\end{array}---\left(15\right)$

Wherein, the pose obtaining mobile robot by iterative above formula is estimated.In order to improve positioning precision further, introduce the pose estimation model of robustness weights structure based on M-estimation herein, now energy function is expressed as:

$f\left(x\right)=\underset{p}{\mathrm{\Σ}}w\left(r\right){\left|\right|r\left|\right|}^2---\left(16\right)$

Wherein, w (x) is robustness Tukey weighting function, and now, the extremum conditions of energy function is:

$\mathrm{\ξ}=-{\left(\underset{p}{\mathrm{\Σ}}{J}^{T}w\left(r\right)J\right)}^{-1}\underset{p}{\mathrm{\Σ}}{J}^{T}w\left(r\right)r\left(0\right)---\left(17\right)$

Step 2.3, plane parameter is estimated

Robot utilizes based on the homography relational implementation robot localization of terrain surface specifications, but due to the impact of non-terrain surface specifications, be difficult to ensure positioning precision, therefore the present invention adopts one non-terrain surface specifications minimizing technology fast.Normal estimation model is built according to parallel vector cross multiplication.Experiment finds, because Corner Detection exists certain error, desirable multiplication cross condition cannot be met, and normal estimation model fitness is poor.Utilize projection error r as judgment basis, thus determine non-terrain surface specifications information.But along with non-terrain surface specifications and observation point spacing reduce, the method discrimination obviously reduces.Compare said method, the present invention, on the basis of pose initial estimation, utilizes homography matrix direct derivation normal estimation model, by obtaining the difference of itself and ground level equation parameter, differentiates whether this angle point is terrain surface specifications information.According to plane shock wave sexual intercourse formula, plane parameter n can be set up _destimation model:

$f\left(n_d\right)=\underset{p}{\mathrm{\Σ}}{\left|\right|r\left(n_d\right)\left|\right|}^2---\left(18\right)$

Wherein, r (n _d)=π (H (n _d) u _ref)-u _pfor homolographic projection error function.Estimate that solution procedure is similar with pose, on the basis of thin tail sheep hypothesis, the solving model of formula (18) can be set up according to the first order Taylor formula of error function.

Now, plane parameter obtains by iterative (18):

$n_d=n_d+\hat{n}$

$\hat{n}=-{\left(\underset{p}{\mathrm{\Σ}}{J}_n^T{J}_n\right)}^{-1}\underset{p}{\mathrm{\Σ}}{J}_n^{T}r\left(n_d\right)---\left(19\right)$

Wherein, $J_n=\frac{\∂r\left(n_d\right)}{\∂n_d}=\frac{\∂\mathrm{\π}}{\∂p}\frac{\∂H}{\∂n_d}.$

Step 3, pose is estimated

The major function of pose estimation module has been the real-time update of camera pose.Its specific implementation process is as follows.After completing initial work, according to projection error, set up pose estimation model, its mathematical description is as follows:

$\mathrm{\ξ}=\underset{\mathrm{\ξ}}{\arg \min }\mathrm{\ΣObj}(\frac{\left|e_j\right|}{{\mathrm{\σ}}_j},{\mathrm{\σ}}_T)---\left(20\right)$

Wherein, e _jprojection error, and for Tukey two power objective function, σ _tfor the unbiased estimator of Feature Points Matching standard deviation, μ is camera pose hexa-atomic group of description vectors.

For the failure scenarios existed in tracking, observation of characteristics ratio is proposed in order to assess the tracking quality of every two field picture.If ratio is lower than setting threshold value, can be that tracking effect is poor, then continue to follow the tracks of, but this frame can not be updated in map by system; If when all there is the situation of observation of characteristics ratio lower than setting threshold value in the tracking of multiple image, the situation belonging to " with losing " can have been thought, so with regard to initialization tracing process.Achieved the accurate estimation of pose by above step, tracing process is normally carried out.

Step 4, map building

Step 4.1, map initialization

System utilizes terrain surface specifications and standard stereo camera algorithm model to set up current environment initialization map.In the initialization procedure of map, utilize FAST corners Matching relation in image, in conjunction with RANSAC algorithm, adopt the initial bit orientation estimation method based on terrain surface specifications, triangulation is used to calculate the three-dimensional coordinate at current signature point place, and determine that starting point is overall world coordinate system initial point, complete the initialization of sparse 3 D map.

In the initial drawing stage of map, the detailed process obtaining two key frames is as follows, is located at by camera frame on mobile robot, starts to carry out initialization procedure after being demarcated by camera and ground.In initialization procedure, mobile robot starts mobile, and camera is caught angle point information in current scene and is associated simultaneously, obtains current pose initial estimation by solving formula (12).When this initial estimation exceedes setting threshold value d ₀time, use the accurate estimation obtaining current pose in 2.2 based on the position and orientation estimation method of terrain surface specifications.

Step 4.2, key frame upgrades to be searched for polar curve

During map initialization, only comprise two key frames, and can only be described contiguous less volumetric spaces, when camera leaves initial position, need to add new key frame and map feature.If camera and current key interframe number of image frames exceed threshold condition, camera tracking effect is best, and camera and nearest map key point distance are as far as possible hour, will automatically perform interpolation key frame process.Due to the constraint of real-time, tracker just may estimate the subset of visual properties in frame, and therefore mapping thread needs to replan and measure remaining map feature.

In the renewal of key frame, first system will carry out Shi-Tomas assessment to angle points all in the key frame newly increased, obtain the current angle point information with notable feature, significant point near the unique point that success is observed will be dropped, and the conspicuousness point remained is by map nodes alternatively.Because new map nodes needs known depth information, only cannot obtain by single-frame images, therefore need to choose key frame nearest with it as the second view, utilize polar curve to search for and set up unique point sub-pixel precision mapping relations with block matching method.In the second view, adopt zero-mean SSD method, realize the accurate reorientation of camera in conjunction with pose estimation model.Finally match point is projected to space, to generate current global context three-dimensional map.

Step 4.3, optimizes

Utilize optimum local to realize current map global coherency with overall LM (Levenberg-Marquardt) boundling adjustment algorithm and optimize to improve robustness.The mathematical description of this boundling adjustment algorithm is:

$\left\{\right\{{\mathrm{\μ}}_2...{\mathrm{\μ}}_N\},\{{p^{\text{'}}}_1...{p^{\text{'}}}_M\left\}\right\}=\underset{\left\{\right\{\mathrm{\μ}\},\{p\left\}\right\}}{\arg \min }\underset{i\∈X\∈Y}{\mathrm{\Σ}}\underset{j\∈S_i}{\mathrm{\Σ}}\mathrm{Obj}(\frac{\left|e_{\mathrm{ji}}\right|}{{\mathrm{\σ}}_{\mathrm{ji}}},{\mathrm{\σ}}_T)---\left(21\right)$

Wherein, σ _jifor in i-th key frame, the unbiased esti-mator of FAST Feature Points Matching standard deviation, μ _irepresent pose hexa-atomic group of description vectors of i-th key frame, p _ifor the point in global map.

Adjusted by continuous boundling, make the stable cost function in (21) minimum.Overall situation boundling adjusts the pose of all key frames and the position of point map, utilizes the openness complexity that global matrix is decomposed in the inherence in structure-motion problems to present the decline of cube rank, by O ((N+M) simultaneously ³) be down to O (N ³), but the restriction of the exterior point vector product generated again in the estimation situation of majority, along with the growth of map, this operand can significantly increase.

For above problem, introduce local boundling adjustment, only adjust the subset pose of key frame here, objective function now becomes:

$\left\{\right\{{\mathrm{\μ}}_{\mathrm{\χ}}\∈X\},\{{p^{\text{'}}}_z\∈Z\left\}\right\}=\underset{\left\{\right\{\mathrm{\μ}\},\{p\left\}\right\}}{\arg \min }\underset{i\∈X\∈Y}{\mathrm{\Σ}}\underset{j\∈Z\∩S_i}{\mathrm{\Σ}}\mathrm{Obj}(i,j)---\left(22\right)$

X is the set to be adjusted of serial key frame, and Y is anchor-frame collection, and Z is point map subset.Set X is made up of a up-to-date key frame and 4 immediate key frames in map, and the visible dots in all maps of key frame constitutes set Z, last Y and contains all key frames obtained by measurement data in Z.Thus, the pose of the key frame that local boundling adjusting and optimizing is up-to-date, nearest, have updated institute in map a little, complexity also significantly reduces simultaneously, is no more than O (NM).

Compared with former PTAM algorithm, present invention adds terrain surface specifications to make improvements, achieve following several advantages:

(1) improve initialization procedure, efficient solution is except PTAM algorithm is to the strict restriction of camera movement.

(2) can effectively setting up relatively accurate Metric map, providing Reliable guarantee for carrying out environment measurement further.

Accompanying drawing explanation

Fig. 1 is the process flow diagram of method involved in the present invention;

Fig. 2 camera calibration model;

Fig. 3 plane parameter demarcates schematic diagram.

Embodiment

Below in conjunction with accompanying drawing, patent of the present invention is further elaborated.

Based on terrain surface specifications PTAM innovatory algorithm process flow diagram as shown in Figure 1, specifically comprise following step:

Step 1, parameter correction

Step 1.1, parameter defines

By the relation of robot coordinate system and world coordinate system, build robot pose and represent, and by camera and the interplanar position orientation relation of target plane reference parameter definitely.

Step 1.2, camera calibration

Adopt the correction of FOV model realization monocular camera, image pixel coordinates is mapped to normalization coordinate plane, simultaneously combining camera Intrinsic Matrix K, realize pattern distortion and correct.

Step 2, based on the initialization of terrain surface specifications

Step 2.1, signature tracking

Obtain environment texture information, extract feature, set up Corner Feature association.

Step 2.2, pose initialization

Set up homography relation and pose initialization estimation model, realize pose initialization.

Step 2.3, plane parameter is estimated

Step 3, pose is estimated

Complete on initialized basis, according to the appearance estimation model based on terrain surface specifications, the initialization pose realizing camera is estimated.Further, PTAM, according to camera initialization pose, adopts polar curve way of search, sets up Corner Feature sub-pixel precision matching relationship in image pyramid, and again bring pose estimation model into, realize the accurate reorientation of camera.

Step 4, map building

Step 4.1, map initialization

System utilizes standard stereo camera algorithm model to set up current environment initialization map

Step 4.2, key frame upgrades to be searched for polar curve

Add new key frame and utilize polar curve to search for and set up unique point sub-pixel precision mapping relations with block matching method, realize the accurate reorientation of camera in conjunction with pose estimation model.Finally match point is projected to space, complete current global context three-dimensional map and create.

Step 4.3, optimizes

Overall situation and partial situation's boundling adjustment is utilized to realize the consistance optimization of map.

Claims (2)

1. improve one's methods based on the PTAM of intelligent robot terrain surface specifications for one kind, it is characterized in that:

First, complete parameter correction, this comprises parameter definition and camera calibration; Then the feature in key frame is followed the tracks of, then realize the initialization of pose, then this basis is estimated plane parameter; Then utilize camera to obtain current environment texture information, and build four layers of Gaussian image pyramid, use the characteristic information in FAST Corner Detection Algorithm extraction present image, set up the data correlation between Corner Feature, obtain pose estimation model; In the initial drawing stage of map, obtain two key frames and camera frame is located on mobile robot, start to carry out initialization procedure after being demarcated by camera and ground; In initialization procedure, mobile robot starts mobile, camera is caught angle point information in current scene and is associated simultaneously, obtains current pose initial estimation, then use the position and orientation estimation method based on terrain surface specifications to obtain the accurate estimation of current pose by pose estimation model; After realizing the initialization of sparse 3 D map, upgrade key frame and utilize polar curve to search for and set up unique point sub-pixel precision mapping relations with block matching method, realize the accurate reorientation of camera in conjunction with pose estimation model; Finally match point is projected to space, complete current global context three-dimensional map and create; Utilize overall situation and partial situation's boundling to adjust the consistance optimization of algorithm simultaneously;

Specifically comprise the following steps:

Step 1, parameter correction

Step 1.1, parameter defines

(1) robot pose

Set up world coordinate system at mobile robot's section start, in robot moving process, its pose can be expressed as:

Can be described by x=(x, y, θ) by the known mobile robot's pose of above formula, wherein, x, y represent that mobile robot is in x-axis respectively, the translational movement in y-axis direction, and θ is the anglec of rotation of mobile robot around z-axis;

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

Wherein, T _c∈ SE (3), SE (3) are special rigid body translation group, R _cbe 3 × 3 rotation matrixs, t _cbe 3 × 1 translation matrix; This position auto―control puts p under establishing world coordinate system and camera coordinates system _c, p _wcorresponding relation, i.e. p _c=T _cp _w; In this method, camera is fixed on mobile robot, there is fixing rotation translation relation T between camera and robot _cr; In robot moving process, if robot pose increment in interval is T sometime _rs, corresponding with it, the pose increment of camera is T _cs, then:

T _rs＝T _rcT _csT _cr(3)

Wherein,

(2) ground level calibrating parameters

Camera is fixed on mobile robot, and demarcation target is lain in ground; Camera sets up position orientation relation between camera and robot by obtaining the image information with demarcation target; T _cp∈ SE (3) is the transformation matrix between camera and demarcation target, R _cp, t _cprepresent T respectively _cprotation and translation component, its inverse transformation assuming that the true origin of robot and camera are consistent, and the z-axis of the x-axis of robot and camera is towards identical; Under target co-ordinates system, the x direction vector of robot can by T _cpthe first two representation in components of the 3rd row vector; Now, the transformation relation of robot coordinate system and target co-ordinates system can be expressed as:

p _r＝T _rpp _p

Wherein, γ is the anglec of rotation of robot coordinate system and target co-ordinates system, (x _r, y _r) be the coordinate of robot initial point under target co-ordinates system, this coordinate can be determined by the translation vector of camera and target; Therefore the transformation relation between robot and camera can be expressed as T _cr=T _cpt _pr, according to camera and the interplanar position orientation relation of target, the ground level equation under camera coordinates system can be obtained:

(0,0,1,0)T _pcp _c＝0 (5)

Wherein, p _cfor three-dimensional point under camera coordinates system, ground level parameter can be expressed as (n ^t, d), and n=(0,0,1) R _pc, d=t _p(3);

Step 1.2, camera calibration

Because the reason of lens on manufacturing can exist distortion, distortion mainly divides for the mirror caused by lens shape to the tangential distortion distorted and caused by video camera assembling process; This method adopts FOV model realization to the rectification of monocular camera; This model is that the one proposed by Devernay and Faugeras goes distortion method for wide angle camera, and its mathematical description is as follows:

In formula, x _upixel normalization coordinate, u _dbe pixel coordinate after distortion, ω is distortion factor, r _d, r _ube respectively

Before and after correcting, normalization coordinate is to the distance at coordinate system center;

Utilize above-mentioned camera model, image pixel coordinates is mapped to normalization coordinate plane, simultaneously combining camera Intrinsic Matrix K, realize pattern distortion and correct, that is:

u＝Kx _u(7)

Step 2, based on the initialization of terrain surface specifications

Step 2.1, signature tracking

FAST Corner Detection is a kind of simple and quick Corner Detection Algorithm proposed by Edward Rosten and Tom Drummond; This algorithm utilizes the difference of the interior neighbor pixel gray-scale value of certain neighborhood of pixel (being generally border circular areas), judges whether this pixel is angle point, as shown in the formula:

Wherein, u _ffor candidate angular, s is any point in candidate angular neighborhood, c (u _f) represent u _fneighborhood, I (x) is at x place image intensity value, if N exceeds threshold condition, this candidate point is angle point;

Before camera track thread, system utilizes camera to obtain current environment texture information, and build four layers of Gaussian image pyramid, adopt the Corner Detection Algorithm of FAST, the mode of combined block search sets up the data correlation between Corner Feature, utilize projection relation to predict the corresponding relation of unique point in three-dimensional point and present frame, and fixed area is searched near position, to obtain matching relationship accurately;

Step 2.2, pose initialization

In robot moving process, obtain terrestrial information by camera, and utilize the tracking of FAST algorithm realization Corner Feature; According to above-mentioned data association message, the homography relation of planar point can be set up, that is:

π(Hu _r)＝u _p(9)

Wherein, π (p) is projection function, p=(x, y, z), π (p)=(x/z, y/z, 1), u _r, u _pbe respectively character pair point under reference frame and present frame; According to camera translational movement T _cand ground level equation parameter (n ^t, d), the homography relation of planar point can be set up:

H＝KT _c(I|n _d) ^TK ^-1(10)

Wherein, n _d=n/d, K are camera Intrinsic Matrix; Formula (3) is substituted into formula (10), and now the pose of robot and terrain surface specifications homography relation can be described as:

H＝KT _rcT _rsT _rc(I|n _d) ^TK ^-1(11)

Utilize the homography relation of above-mentioned image characteristic point, pose estimation model can be set up:

(12)

Wherein, r (x)=π (H (x) u _ref)-u _pfor homography relationship error function, i.e. projection error function, H (x) is homography matrix; According to thin tail sheep hypothesis, the first order Taylor of error function r launches to be expressed as:

Now, the extremum conditions of energy function is:

That is:

(15)

Wherein, the pose obtaining mobile robot by iterative above formula is estimated; In order to improve positioning precision further, introduce the pose estimation model of robustness weights structure based on M-estimation herein, now energy function is expressed as:

Wherein, w (x) is robustness Tukey weighting function, and now, the extremum conditions of energy function is:

Step 2.3, plane parameter is estimated

Robot utilizes based on the homography relational implementation robot localization of terrain surface specifications, but due to the impact of non-terrain surface specifications, be difficult to ensure positioning precision, therefore the present invention adopts one non-terrain surface specifications minimizing technology fast; Normal estimation model is built according to parallel vector cross multiplication; Experiment finds, because Corner Detection exists certain error, desirable multiplication cross condition cannot be met, and normal estimation model fitness is poor; Utilize projection error r as judgment basis, thus determine non-terrain surface specifications information; But along with non-terrain surface specifications and observation point spacing reduce, the method discrimination obviously reduces; Compare said method, the present invention, on the basis of pose initial estimation, utilizes homography matrix direct derivation normal estimation model, by obtaining the difference of itself and ground level equation parameter, differentiates whether this angle point is terrain surface specifications information; According to plane shock wave sexual intercourse formula, plane parameter n can be set up _destimation model:

Wherein, r (n _d)=π (H (n _d) u _ref)-u _pfor homolographic projection error function; Estimate that solution procedure is similar with pose, on the basis of thin tail sheep hypothesis, the solving model of formula (18) can be set up according to the first order Taylor formula of error function;

Now, plane parameter obtains by iterative (18):

Wherein,

Step 3, pose is estimated

The major function of pose estimation module has been the real-time update of camera pose; Its specific implementation process is as follows; After completing initial work, according to projection error, set up pose estimation model, its mathematical description is as follows:

Wherein, e _jprojection error, and for Tukey two power objective function, σ _tfor the unbiased estimator of Feature Points Matching standard deviation, μ is camera pose hexa-atomic group of description vectors;

For the failure scenarios existed in tracking, observation of characteristics ratio is proposed in order to assess the tracking quality of every two field picture; If ratio is lower than setting threshold value, can be that tracking effect is poor, then continue to follow the tracks of, but this frame can not be updated in map by system; If when all there is the situation of observation of characteristics ratio lower than setting threshold value in the tracking of multiple image, the situation belonging to " with losing " can have been thought, so with regard to initialization tracing process; Achieved the accurate estimation of pose by above step, tracing process is normally carried out;

Step 4, map building

Step 4.1, map initialization

System utilizes terrain surface specifications and standard stereo camera algorithm model to set up current environment initialization map; In the initialization procedure of map, utilize FAST corners Matching relation in image, in conjunction with RANSAC algorithm, adopt the initial bit orientation estimation method based on terrain surface specifications, triangulation is used to calculate the three-dimensional coordinate at current signature point place, and determine that starting point is overall world coordinate system initial point, complete the initialization of sparse 3 D map;

In the initial drawing stage of map, the detailed process obtaining two key frames is as follows, is located at by camera frame on mobile robot, starts to carry out initialization procedure after being demarcated by camera and ground; In initialization procedure, mobile robot starts mobile, and camera is caught angle point information in current scene and is associated simultaneously, obtains current pose initial estimation by solving formula (12); When this initial estimation exceedes setting threshold value d ₀time, use the accurate estimation obtaining current pose in 2.2 based on the position and orientation estimation method of terrain surface specifications;

Step 4.2, key frame upgrades to be searched for polar curve

During map initialization, only comprise two key frames, and can only be described contiguous less volumetric spaces, when camera leaves initial position, need to add new key frame and map feature; If camera and current key interframe number of image frames exceed threshold condition, camera tracking effect is best, and camera and nearest map key point distance are as far as possible hour, will automatically perform interpolation key frame process; Due to the constraint of real-time, tracker just may estimate the subset of visual properties in frame, and therefore mapping thread needs to replan and measure remaining map feature;

In the renewal of key frame, first system will carry out Shi-Tomas assessment to angle points all in the key frame newly increased, obtain the current angle point information with notable feature, significant point near the unique point that success is observed will be dropped, and the conspicuousness point remained is by map nodes alternatively; Because new map nodes needs known depth information, only cannot obtain by single-frame images, therefore need to choose key frame nearest with it as the second view, utilize polar curve to search for and set up unique point sub-pixel precision mapping relations with block matching method; In the second view, adopt zero-mean SSD method, realize the accurate reorientation of camera in conjunction with pose estimation model; Finally match point is projected to space, to generate current global context three-dimensional map;

Step 4.3, optimizes

Utilize optimum local to realize current map global coherency with overall LM (Levenberg-Marquardt) boundling adjustment algorithm and optimize to improve robustness; The mathematical description of this boundling adjustment algorithm is:

Wherein, σ _jifor in i-th key frame, the unbiased esti-mator of FAST Feature Points Matching standard deviation, μ _irepresent pose hexa-atomic group of description vectors of i-th key frame, p _ifor the point in global map;

Adjusted by continuous boundling, make the stable cost function in (21) minimum; Overall situation boundling adjusts the pose of all key frames and the position of point map, utilizes the openness complexity that global matrix is decomposed in the inherence in structure-motion problems to present the decline of cube rank, by O ((N+M) simultaneously ³) be down to O (N ³), but the restriction of the exterior point vector product generated again in the estimation situation of majority, along with the growth of map, this operand can significantly increase;

For above problem, introduce local boundling adjustment, only adjust the subset pose of key frame here, objective function now becomes:

X is the set to be adjusted of serial key frame, and Y is anchor-frame collection, and Z is point map subset; Set X is made up of a up-to-date key frame and 4 immediate key frames in map, and the visible dots in all maps of key frame constitutes set Z, last Y and contains all key frames obtained by measurement data in Z; Thus, the pose of the key frame that local boundling adjusting and optimizing is up-to-date, nearest, have updated institute in map a little, complexity also significantly reduces simultaneously, is no more than O (NM).

2. a kind of PTAM based on intelligent robot terrain surface specifications according to claim 1 improves one's methods, and it is characterized in that:

The method specifically comprises following step:

Step 1, parameter correction

Step 1.1, parameter defines

By the relation of robot coordinate system and world coordinate system, build robot pose and represent, and by camera and the interplanar position orientation relation of target plane reference parameter definitely;

Step 1.2, camera calibration

Adopt the correction of FOV model realization monocular camera, image pixel coordinates is mapped to normalization coordinate plane, simultaneously combining camera Intrinsic Matrix K, realize pattern distortion and correct;

Step 2, based on the initialization of terrain surface specifications

Step 2.1, signature tracking

Obtain environment texture information, extract feature, set up Corner Feature association;

Step 2.2, pose initialization

Set up homography relation and pose initialization estimation model, realize pose initialization;

Step 2.3, plane parameter is estimated

Step 3, pose is estimated

Complete on initialized basis, according to the appearance estimation model based on terrain surface specifications, the initialization pose realizing camera is estimated; Further, PTAM, according to camera initialization pose, adopts polar curve way of search, sets up Corner Feature sub-pixel precision matching relationship in image pyramid, and again bring pose estimation model into, realize the accurate reorientation of camera;

Step 4, map building

Step 4.1, map initialization

System utilizes standard stereo camera algorithm model to set up current environment initialization map

Step 4.2, key frame upgrades to be searched for polar curve

Add new key frame and utilize polar curve to search for and set up unique point sub-pixel precision mapping relations with block matching method, realize the accurate reorientation of camera in conjunction with pose estimation model; Finally match point is projected to space, complete current global context three-dimensional map and create;

Step 4.3, optimizes

Overall situation and partial situation's boundling adjustment is utilized to realize the consistance optimization of map.