CN104121902A - Implementation method of indoor robot visual odometer based on Xtion camera - Google Patents

Abstract

The invention relates to an implementation method of an indoor robot visual odometer based on an Xtion camera. The method comprises the following steps: firstly, performing information acquisition on the field of view in front of a robot by use of the Xtion camera installed on the robot so as to obtain RGB (Red, Green and Blue) information and three-dimensional coordination information of spatial points; secondly, performing coarse feature point matching on acquired sequential images based on an SIFT (Scale Invariant Feature Transform) feature matching algorithm in combination with the PFH (Point Feature Histogram) three-dimensional features of the spatial points, and rejecting wrong matched points in coarse matching by use of RANSAC (Radom Sample Consensus) to obtain accurate matched points; and finally, establishing an equation set and solving motion parameters of the robot by use of a least square method. The method has the characteristics that the Xtion camera is used for acquiring information so that the three-dimensional information of the spatial points can be directly obtained, and the feature extraction and matching are realized by use of the texture features and the three-dimensional features of the spatial points, and therefore, the efficiency and accuracy of robot positioning are obviously improved.

Description

Indoor Robot visual odometry implementation method based on Xtion video camera

Technical field

The present invention relates to indoor mobile robot autonomous navigation technology field, be especially a kind ofly applied to the vision mileage implementation method in wheel driving type mobile robot autonomous navigation technology in indoor environment.

Background technology

In the research of indoor mobile robot, it is extremely important obtaining in real time the high-precision kinematic parameter of robot, is related to robot navigation, keeps away the realization of barrier and the tasks such as path planning.Owing to encountering barrier wheel in robot traveling process, there is long term wear skidding, make the displacement accurate information that the equipment such as photoelectric code disk speed measuring motor can not Accurate Determining robot; Adopt GPS location, exist resolution low, and a little less than indoor signal, be not suitable for indoor mobile robot.

Visual odometry is by collection analysis associated picture sequence, determine robot location and towards, can make up above-mentioned problem, strengthened navigation accuracy when robot moves by any way on any surface.Traditional visual odometry obtains image by monocular camera, binocular camera or omnidirectional's camera, by the conversion of coordinate system, obtain the three-dimensional information of spatial point, thereby and by image RGB information, two two field pictures are carried out to the kinematic parameter that feature extraction and matching obtains robot.When obtaining the three-dimensional information of spatial point, due to the conversion of camera lenses error and coordinate system, make that computation process is complicated, efficiency is low and precision is not high.When to image characteristics extraction, coupling, owing to only having utilized the RGB information of image, lose three-dimensional information, usually occurred the situation of erroneous matching.

Summary of the invention

The object of this invention is to provide a kind of indoor mobile robot visual odometry implementation method based on Xtion video camera, the method is utilized Xtion camera, can directly obtain three-dimensional information and the RGB information of spatial point, and utilize the textural characteristics and the three-dimensional feature that extract spatial point to mate, thereby significantly improved efficiency and the precision of robot location.

For achieving the above object, the present invention adopts following scheme to realize: the method comprises the following steps:

Step S01: by being arranged on Xtion video camera the looking robot the place ahead in robot

Information acquisition is carried out in field, obtains RGB information and the three-dimensional coordinate information of spatial point;

Step S02: utilize SIFT Feature Correspondence Algorithm and in conjunction with the PFH three-dimensional feature of spatial point, the unique point that Xtion is collected between adjacent two width images is mated;

Step S03: use RANSAC to reject the Mismatching point in coupling;

Step S04: the match point of adjacent two two field pictures before and after obtaining wherein footmark p represents former frame image, a two field picture after c representative, and i is match point number; Set up system of equations $\left[\begin{array}{c}{x}_i^c\\ y_i^c\\ z_i^c\\ 1\end{array}\right]=\left[\begin{array}{cc}R& T\\ 0& 1\end{array}\right]\left[\begin{array}{c}{x}_i^p\\ y_i^p\\ z_i^p\\ 1\end{array}\right],$ Wherein R is rotation matrix, and T is translation vector; Utilize least square method can solve the kinematic parameter of robot.

Further, described Feature Points Matching specifically comprises the steps:

1) feature point detection: detect local extremum simultaneously using as unique point in image two dimensional surface space and DoG metric space, DoG operator definitions is the difference of the gaussian kernel of two different scales, DoG operator is suc as formula shown in (1):

D(x,y,σ)＝(G(x,y,kσ)-G(x,y,σ))*I(x,y)＝L(x,y,kσ)-L(x,y,σ) (1)

Wherein, G (x, y, σ) is changeable scale Gaussian function, (x, y) is volume coordinate, and σ is yardstick coordinate, and I (x, y) is former figure, and L (x, y, σ) is metric space.

When detecting yardstick spatial extrema, certain pixel need to 8 pixels in surrounding field of same yardstick and 9 * 2, the field pixel of adjacent yardstick correspondence position altogether 26 pixels compare, to guarantee local extremum can be detected at metric space and 2 dimension image spaces;

2) unique point is described, and sets up proper vector, comprising SIFT proper vector and PFH three-dimensional feature vector;

SIFT proper vector: each key point by 2 * 2 totally 4 Seed Points form, each Seed Points comprises 8 direction vector information; To each key point adopt 4 * 4 totally 16 Seed Points describe, to produce 128 data, last each key point is set up the SIFT proper vector of 128 dimensions;

PFH three-dimensional feature vector: utilize KD number to find near K the point of each key point, and obtain these normal line vector n in space coordinates; For key point and near K point it, get its all mutual two somes combinations: P _i, P _j, according to judging <n _i, P _j-P _i> and <n _j, P _i-P _jthe size of > arranges point of origin P _swith impact point P _t, wherein n is normal line vector, if the former is P greatly _s=P _i, P _t=P _jotherwise, P _s=P _j, P _t=P _i;

According to the premises, set up local coordinate system: $\begin{array}{c}u=n_s\\ v=u\&times;\frac{(P_t-P_s)}{{\left|\right|P_t-P_s\left|\right|}_2}\\ w=u\&times;v\end{array},$ Under this coordinate system, calculate three parameters: $\begin{array}{c}\mathrm{\&alpha;}=v\&CenterDot;n_t\\ \mathrm{\&phi;}=u\&CenterDot;\frac{P_t-P_s}{{\left|\right|P_t-P_s\left|\right|}_2}\\ \mathrm{\&theta;}=\arctan (\mathrm{\&omega;}\&CenterDot;n_t,u\&CenterDot;n_t)\end{array},$ Each parametrization is divided into 5 sub-ranges and sets up 3 ⁵in the histogram in individual interval, three parameters (α, φ, θ) of every pair of point add one by the right histogram of answering, and the histogram that statistics institute produces after is a little normalized, and is 125 dimension PFH three-dimensional features vectorial;

3) carry out characteristic matching to obtain candidate matches point: after the SIFT of two two field pictures proper vector and the generation of FPH three-dimensional feature vector, the Euclidean distance between next step employing key point proper vector is used as the similarity determination tolerance of key point in two two field pictures; Get the nearest front two pairs of key points of two two field picture Euclidean distances, in these two pairs of key points, if nearest distance is removed near distance in proper order, be less than certain proportion threshold value, accept this pair of match point;

4) eliminate mistake coupling: adopt RANSAC algorithm to remove exterior point, RANSAC algorithm is by repeatedly randomly drawing certain sample, parameter to be estimated, first draw initial parameter model, then according to estimated parameter, all data are classified, part data are within the scope of specification error, be called interior point, otherwise be called exterior point, through iterative computation repeatedly, go out optimization model parameter.

Further, to solve the kinematic parameter method of robot as follows for described least square method:

Solving least square solution makes || P _ci-(RP _pi+ T) || ²minimum, wherein P _piand P _pifor the former frame image in adjacent two sequence images and a rear two field picture, i is that Corresponding matching is counted; Model covariance matrix ${\mathrm{\&Sigma;}}_{\mathrm{cp}}=\frac{1}{n}\underset{i=1}{\overset{n}{\mathrm{\&Sigma;}}}(P_{\mathrm{ci}}-{\mathrm{\&mu;}}_c){(P_{\mathrm{pi}}-{\mathrm{\&mu;}}_p)}^T,$ Wherein ${\mathrm{\&mu;}}_n=\frac{1}{n}\underset{i=1}{\overset{n}{\mathrm{\&Sigma;}}}{P}_{\mathrm{pi}},$ then to matrix Σ _cpcarry out Eigenvalues Decomposition Σ _cp=UDV, U wherein, V is unitary matrix, D=diag (d _i), d ₁>=d ₂>=d _m>=0 is singular value matrix, d in formula _ifor non-zero singular value, note $S=\left\{\begin{array}{cc}I,& \det \left({\mathrm{\&Sigma;}}_{\mathrm{cp}}\right)\&GreaterEqual;0,\\ \mathrm{diag}(\mathrm{1,1},...,-1),& \det \left({\mathrm{\&Sigma;}}_{\mathrm{cp}}\right)<0,\end{array}\right.;$ Finally solve rotation matrix R=USV, bring former variance into and can solve translation vector T.

Relative prior art, the invention has the beneficial effects as follows:

1, adopt Xtion video camera directly to obtain RGB information and the three-dimensional information of spatial point, avoided traditional visual odometry to need repeatedly transformed coordinate system, carry out a large amount of computings and just can solve the three-dimensional coordinate of spatial point, significantly very high computing velocity and precision.

2, the textural characteristics and the three-dimensional feature that take full advantage of spatial point mate sequence image, have improved the defect that traditional visual odometry only relies on the RGB information of spatial point, make matching result more accurate.

Accompanying drawing explanation

Fig. 1 is overview flow chart of the present invention.

embodiment

Below in conjunction with drawings and Examples, the present invention will be further described.

As shown in Figure 1, first the method for the present embodiment carries out information acquisition by the Xtion video camera being arranged in robot to the visual field in robot the place ahead, obtains RGB information and the three-dimensional coordinate information of spatial point.Then based on SIFT Feature Correspondence Algorithm and in conjunction with PFH (some feature histogram) three-dimensional feature of spatial point, the sequence image gathering is carried out to thick Feature Points Matching, and use RANSAC (stochastic sampling consistance) to reject the Mismatching point in thick coupling, obtain exact matching point.Finally set up system of equations, utilize least square method to solve the kinematic parameter of robot.

Concrete, Xtion video camera is placed in to robot the place ahead, when robot moves, Xtion video camera will be recorded the information of 640 * 480 points in robot the place ahead, institute presents two dimensional image as shown in Figure 2, three-dimensional point cloud information as shown in Figure 3, the information that obtains be D coordinates value and the RGB value of color of each spatial point.

Utilize SIFT Feature Correspondence Algorithm and in conjunction with the three-dimensional feature of spatial point,, obtain the thick Feature Points Matching in two images, and use RANSAC (stochastic sampling consistance) to reject the Mismatching point in thick coupling, obtain exact matching point.Concrete steps are as follows:

1) feature point detection: detect local extremum simultaneously using as unique point in image two dimensional surface space and DoG (Difference-of-Gaussian) metric space, DoG operator definitions is the difference of the gaussian kernel of two different scales, and DoG operator is suc as formula shown in (1):

D(x,y,σ)＝(G(x,y,kσ)-G(x,y,σ))*I(x,y)＝L(x,y,kσ)-L(x,y,σ) (1)

Wherein, G (x, y, σ) is changeable scale Gaussian function, (x, y) is volume coordinate, and σ is yardstick coordinate, and I (x, y) is former figure, and L (x, y, σ) is metric space.

When detecting yardstick spatial extrema, certain pixel need to 8 pixels in surrounding field of same yardstick and 9 * 2, the field pixel of adjacent yardstick correspondence position altogether 26 pixels compare, to guarantee local extremum can be detected at metric space and 2 dimension image spaces.

2) unique point is described, and sets up proper vector, comprising SIFT proper vector and PFH three-dimensional feature vector.

SIFT proper vector: each key point by 2 * 2 totally 4 Seed Points form, each Seed Points comprises 8 direction vector information.In order to strengthen coupling stability, to each key point adopt 4 * 4 totally 16 Seed Points describe, for each key point, just can produce 128 data like this, finally each key point is set up the SIFT proper vector of 128 dimensions.

PFH three-dimensional feature vector: utilize KD number to find near K the point of each key point, and obtain these normal line vector n in space coordinates.For key point and near K point it, get its all mutual two somes combinations: P _i, P _j, according to judging <n _i, P _j-P _i> and <n _j, P _i-P _jthe size of > arranges point of origin P _swith impact point P _t, wherein n is normal line vector, if the former is P greatly _s=P _i, P _t=P _jotherwise, P _s=P _j, P _t=P _i.

According to the premises, set up local coordinate system: $\begin{array}{c}u=n_s\\ v=u\&times;\frac{(P_t-P_s)}{{\left|\right|P_t-P_s\left|\right|}_2}\\ w=u\&times;v\end{array},$ Under this coordinate system, calculate three parameters: $\begin{array}{c}\mathrm{\&alpha;}=v\&CenterDot;n_t\\ \mathrm{\&phi;}=u\&CenterDot;\frac{P_t-P_s}{{\left|\right|P_t-P_s\left|\right|}_2}\\ \mathrm{\&theta;}=\arctan (\mathrm{\&omega;}\&CenterDot;n_t,u\&CenterDot;n_t)\end{array},$ Each parametrization is divided into 5 sub-ranges and sets up 3 ⁵in the histogram in individual interval, three parameters (α, φ, θ) of every pair of point add one by the right histogram of answering, and the histogram that statistics institute produces after is a little normalized, and is 125 dimension PFH three-dimensional features vectorial.

3) carry out characteristic matching to obtain candidate matches point: after the SIFT of two two field pictures proper vector and the generation of FPH three-dimensional feature vector, the Euclidean distance between next step employing key point proper vector is used as the similarity determination tolerance of key point in two two field pictures.Get the nearest front two pairs of key points of two two field picture Euclidean distances, in these two pairs of key points, if nearest distance is removed near distance in proper order, be less than certain proportion threshold value, accept this pair of match point.

4) eliminate mistake coupling: adopt RANSAC algorithm to remove exterior point herein, RANSAC algorithm is by repeatedly randomly drawing certain sample, parameter to be estimated, first draw initial parameter model, then according to estimated parameter, all data are classified, part data are within the scope of specification error, be called interior point, otherwise be called exterior point, through iterative computation repeatedly, go out optimization model parameter.

Obtain the match point of adjacent two two field pictures wherein i is match point number.Set up system of equations $\left[\begin{array}{c}{x}_i^c\\ y_i^c\\ z_i^c\\ 1\end{array}\right]=\left[\begin{array}{cc}R& T\\ 0& 1\end{array}\right]\left[\begin{array}{c}{x}_i^p\\ y_i^p\\ z_i^p\\ 1\end{array}\right],$ Wherein R is rotation matrix, and T is translation vector.Utilize least square method can solve the kinematic parameter of robot.Figure tetra-and figure five are adjacent front and back two two field pictures rotation translation transformation schematic diagram, the transformation results solving for (unit: m): $\left[\begin{array}{cc}R& T\\ 0& 1\end{array}\right]=\left[\begin{array}{cccc}0.992999& 0.0260354& 0.115213& -0.0287673\\ -0.0232128& 0.999398& -0.0257746& 0.0418021\\ -0.115815& 0.0229197& 0.993006& 0.284341\\ 0& 0& 0& 1\end{array}\right].$

In example according to odometer record, 5 ° of the actual rotations of video camera, mobile x=0.03m, y=0.04m, z=0.30m.According to measurement result, maximum absolute error is 0.016m, relative error 5.7%.

Claims (3)

1. the Indoor Robot visual odometry implementation method based on Xtion video camera, is characterized in that comprising the following steps:

Step S01: by the Xtion video camera being arranged in robot, information acquisition is carried out in the visual field in robot the place ahead, obtain RGB information and the three-dimensional coordinate information of spatial point;

Step S02: utilize SIFT Feature Correspondence Algorithm and in conjunction with the PFH three-dimensional feature of spatial point, the unique point that Xtion is collected between adjacent two width images is mated;

Step S03: use RANSAC to reject the Mismatching point in coupling;

Step S04: the match point of adjacent two two field pictures before and after obtaining wherein superscript p represents former frame image, a two field picture after c representative, and i is match point number; Set up system of equations $\left[\begin{array}{c}{x}_i^c\\ y_i^c\\ z_i^c\\ 1\end{array}\right]=\left[\begin{array}{cc}R& T\\ 0& 1\end{array}\right]\left[\begin{array}{c}{x}_i^p\\ y_i^p\\ z_i^p\\ 1\end{array}\right],$ Wherein R is rotation matrix, and T is translation vector; Utilize least square method can solve the kinematic parameter of robot.

2. the Indoor Robot visual odometry implementation method based on Xtion video camera according to claim 1, is characterized in that: described Feature Points Matching specifically comprises the steps:

1) feature point detection: detect local extremum simultaneously using as unique point in image two dimensional surface space and DoG metric space, DoG operator definitions is the difference of the gaussian kernel of two different scales, DoG operator is suc as formula shown in (1):

D(x,y,σ)＝(G(x,y,kσ)-G(x,y,σ))*I(x,y)＝L(x,y,kσ)-L(x,y,σ) (1)

Wherein, G (x, y, σ) is changeable scale Gaussian function, (x, y) is volume coordinate, and σ is yardstick coordinate, and I (x, y) is original image, and L (x, y, σ) is metric space.

When detecting yardstick spatial extrema, certain pixel need to 8 pixels in surrounding field of same yardstick and 9 * 2, the field pixel of adjacent yardstick correspondence position altogether 26 pixels compare, to guarantee local extremum can be detected at metric space and 2 dimension image spaces;

2) unique point is described, and sets up proper vector, comprising SIFT proper vector and PFH three-dimensional feature vector;

SIFT proper vector: each key point by 2 * 2 totally 4 Seed Points form, each Seed Points comprises 8 direction vector information; To each key point adopt 4 * 4 totally 16 Seed Points describe, to produce 128 data, last each key point is set up the SIFT proper vector of 128 dimensions;

PFH three-dimensional feature vector: utilize KD number to find near K the point of each key point, and obtain these normal line vector n in space coordinates; For key point and near K point it, get its all mutual two somes combinations: P _i, P _j, according to judging <n _i, P _j-P _i> and <n _j, P _i-P _jthe size of > arranges point of origin P _swith impact point P _t, n wherein _iand n _jbe respectively corresponding P _iand P _jnormal vector, if the former greatly remembers P _s=P _i, P _t=P _jotherwise, note P _s=P _j, P _t=P _i;

According to the premises, set up local coordinate system: $\begin{array}{c}u=n_s\\ v=u\&times;\frac{(P_t-P_s)}{{\left|\right|P_t-P_s\left|\right|}_2}\\ w=u\&times;v\end{array},$ Under this coordinate system, calculate three parameters: $\begin{array}{c}\mathrm{\&alpha;}=v\&CenterDot;n_t\\ \mathrm{\&phi;}=u\&CenterDot;\frac{P_t-P_s}{{\left|\right|P_t-P_s\left|\right|}_2}\\ \mathrm{\&theta;}=\arctan (\mathrm{\&omega;}\&CenterDot;n_t,u\&CenterDot;n_t)\end{array},$ Each parametrization is divided into 5 sub-ranges and sets up 3 ⁵in the histogram in individual interval, three parameters (α, φ, θ) of every pair of point add one by the right histogram of answering, and the histogram that statistics institute produces after is a little normalized, and is 125 dimension PFH three-dimensional features vectorial;

3) carry out characteristic matching to obtain candidate matches point: after the SIFT of two two field pictures proper vector and the generation of FPH three-dimensional feature vector, the Euclidean distance between next step employing key point proper vector is used as the similarity determination tolerance of key point in two two field pictures; Get the nearest front two pairs of key points of two two field picture Euclidean distances, in these two pairs of key points, if nearest distance is removed near distance in proper order, be less than certain proportion threshold value, accept this pair of match point;

4) eliminate mistake coupling: adopt RANSAC algorithm to remove exterior point, RANSAC algorithm is by repeatedly randomly drawing certain sample, parameter to be estimated, first draw initial parameter model, then according to estimated parameter, all data are classified, part data are within the scope of specification error, be called interior point, otherwise be called exterior point, through iterative computation repeatedly, go out optimization model parameter.

3. the Indoor Robot visual odometry implementation method based on Xtion video camera according to claim 1, is characterized in that: the kinematic parameter method that described least square method solves robot is as follows:

Solving least square solution makes || P _ci-(RP _pi+ T) || ²minimum, wherein P _piand P _cifor the former frame image in adjacent two sequence images and a rear two field picture, i is that Corresponding matching is counted; Model covariance matrix ${\mathrm{\&Sigma;}}_{\mathrm{cp}}=\frac{1}{n}\underset{i=1}{\overset{n}{\mathrm{\&Sigma;}}}(P_{\mathrm{ci}}-{\mathrm{\&mu;}}_c){(P_{\mathrm{pi}}-{\mathrm{\&mu;}}_p)}^T,$ Wherein ${\mathrm{\&mu;}}_n=\frac{1}{n}\underset{i=1}{\overset{n}{\mathrm{\&Sigma;}}}{P}_{\mathrm{pi}},{\mathrm{\&mu;}}_c=\frac{1}{n}\underset{i=1}{\overset{n}{\mathrm{\&Sigma;}}}{P}_{\mathrm{ci}}$ Corresponding matching is counted; Then to matrix Σ _cpcarry out Eigenvalues Decomposition Σ _cp=UDV, U wherein, V is unitary matrix, D=diag (d _i), d ₁>=d ₂>=d _m>=0 is singular value matrix, d in formula _ifor non-zero singular value, note $S=\left\{\begin{array}{cc}I,& \det \left({\mathrm{\&Sigma;}}_{\mathrm{cp}}\right)\&GreaterEqual;0,\\ \mathrm{diag}(\mathrm{1,1},...,-1),& \det \left({\mathrm{\&Sigma;}}_{\mathrm{cp}}\right)<0,\end{array}\right.;$ Finally solve rotation matrix R=USV, bring former variance into and can solve translation vector T.