CN105469405A - Visual ranging-based simultaneous localization and map construction method - Google Patents

Abstract

The invention provides a visual ranging-based simultaneous localization and map construction method. The method includes the following steps that: a binocular image is acquired and corrected, so that a distortion-free binocular image can be obtained; feature extraction is performed on the distortion-free binocular image, so that feature point descriptors can be generated; feature point matching relations of the binocular image are established; the horizontal parallax of matching feature points is obtained according to the matching relations, and based on the parameters of a binocular image capture system, real space depth is calculated; the matching results of the feature points of a current frame and feature points in a world map are calculated; feature points which are wrongly matched with each other are removed, so that feature points which are successfully matched with each other can be obtained; a transform matrix of the coordinates of the feature points which are successfully matched with each other under a world coordinate system and the three-dimension coordinates of the feature points which are successfully matched with each other under a current reference coordinate system is calculated, and a pose change estimated value of the binocular image capture system relative to an initial position is obtained according to the transform matrix; and the world map is established and updated. The visual ranging-based simultaneous localization and map construction method of the invention has low computational complexity, centimeter-level positioning accuracy and unbiased characteristics of position estimation.

Description

The simultaneous localization and mapping method of view-based access control model ranging

Technical field

The present invention relates to computing machine and electronic information technical field, the simultaneous localization and mapping method of particularly a kind of view-based access control model ranging.

Background technology

In recent years, mobile working platform as unmanned plane, one of focus becoming research such as mobile robot.These equipment have very high dirigibility, are widely used in the scene such as disaster relief, geological exploration.The major technique of robot autonomous navigational system comprises: build space environment three-dimensional map, self poisoning, route planning and obstacle avoidance etc.Wherein, the three-dimensional map in constructing environment space and self poisoning are the very corns of a subject, and route planning, obstacle avoidance rely on the cognition of unmanned plane to environment and the result of location.

Current outdoor unmanned plane has had ripe navigational system, such as GPS and inertial navigation system.In indoor navigation, complicated closed space environment has shielding and interference to wireless signal, therefore, depends on the positioning system of satellite in indoor inefficacy; Meanwhile, inertial navigation system cumulative errors and make it not be suitable for high-precision indoor positioning to initial value sensitivity.Active sensor is as laser scanner, and sonar etc. can be measured and obtain one-dimensional distance and drift angle for location estimation, but the usual heavy of active sensor, expensive and power consumption is comparatively large, is not suitable for small-sized movable platform; Meanwhile, single data type and low data rate limit the lifting of indoor position accuracy.

In recent years along with vision sensor price reduces gradually, it is applied in vision guided navigation task widely.Visual information is more intuitive locator meams for biological, and vision sensor can provide abundanter data message, not only can realize the estimation of robot self-position and attitude, also can realize the cognition to space.Correct and dense spatial cognition information can be effectively applied in the contingency question such as route planning and obstacle avoidance.Wherein binocular camera system can obtain real depth and realize location when not providing environment prior imformation, and this expands the range of application of mobile platform in circumstances not known.

Vision ranging is the basic algorithm in robot navigation field.Its main thought is, obtain inter motion information by the coordinate conversion relation calculating front and back frame same characteristic features point, the pose being obtained robot relative initial position by the Integral Thought accumulated frame by frame is estimated.But actual experiment shows, although the location algorithm of view-based access control model ranging provides the lower position and orientation estimation method of complexity, its mode of accumulating frame by frame makes location estimation drift about in time, also namely produces cumulative errors.

Simultaneous localization and mapping algorithm (SimultaneousLocalizationandMapping, SLAM) is the study hotspot in robot navigation field in recent years.It refers to that the robot of movement in circumstances not known recovers the movement locus of self by sensor measurement data, constructs an increment type map about space simultaneously.In this process, robot self-position and space terrestrial reference have dual uncertainty, solve this problem by setting up Probability model.Simultaneous localization and mapping algorithm based on EKF is proved to be as the best SLAM algorithm of current convergence, but its open defect to be computational complexity very high, square being directly proportional of the feature point number comprised to world map.The computation complexity of quadratic term limits the scale of world map on the one hand, also namely limits the number of features in residing environment and environment area; On the other hand, the computing velocity of location navigation algorithm is constrained.Consider the characteristic of the low load of indoor mobile robot, low-power consumption, realize traditional SLAM system and reach real-time processing speed being restricted mobile platform business.

Summary of the invention

The present invention is intended to solve one of technical matters in above-mentioned correlation technique at least to a certain extent.

For this reason, the object of the invention is to a kind of simultaneous localization and mapping method proposing view-based access control model ranging, the method only relies on binocular vision to navigate, without the need to merging other sensing datas, without the need to having priori to environment, there is lower computation complexity, the positioning precision of centimetre-sized and to location estimation without inclined characteristic.

To achieve these goals, embodiments of the invention propose a kind of simultaneous localization and mapping method of view-based access control model ranging, comprise the following steps: S1: by binocular image capture systems synchronous acquisition binocular image, and described binocular image is corrected, to obtain undistorted binocular image; S2: feature point extraction is carried out to described undistorted binocular image, and according to the unique point generating feature point descriptor extracted; S3: the Feature Points Matching relation setting up described binocular image according to described unique point descriptor; S4: the horizontal parallax obtaining matching characteristic point according to the Feature Points Matching relation of described binocular image, and the space real depth calculating described matching characteristic point according to the described horizontal parallax of matching characteristic point and the parameter of binocular image capture systems; S5: the unique point obtaining present frame binocular image, and the described unique point of present frame binocular image is mated with the unique point in world map; S6: the matching result of the unique point in the unique point of described present frame binocular image and world map is analyzed, to remove the unique point of wherein erroneous matching, the unique point of coupling of succeeding; S7: the transformation matrix calculating the coordinate of unique point under world coordinate system of described successful match and the three-dimensional coordinate under current reference coordinates system, obtains the pose change estimated value of described binocular image capture systems relative initial position according to described transformation matrix; S8: when first frame calculates, world map described in initialization, and when non-first frame calculates, according to described transformation matrix upgrade in world map with the three-dimensional coordinate of the unique point of present frame successful match and Feature Descriptor, and in world map, add the newly observed unique point of present frame and Feature Descriptor thereof.

In addition, the simultaneous localization and mapping method of view-based access control model ranging according to the above embodiment of the present invention can also have following additional technical characteristic:

In some instances, described S1 comprises further: the imaging parameters extracting described binocular image capture systems, and corrects described binocular image according to described imaging parameters, to obtain undistorted binocular image.

In some instances, in described S2, by scale invariant feature conversion method, feature point extraction is carried out to described undistorted binocular image.

In some instances, in described S3, whether be less than the matching relationship between unique point that specific threshold sets up described binocular image by the vector angle between the unique point descriptor that judges described binocular image.

In some instances, described image capture system comprises described left camera and right camera, and described S4 comprises further: obtain the pixel coordinate of described matching characteristic point at the magazine imaging point of left camera and the right side respectively; The horizontal parallax of the magazine imaging point of the magazine imaging point in a described left side and the right side is calculated according to the coordinate of the central point of described camera; Obtain the space real depth of described matching characteristic point according to the horizontal parallax of the magazine imaging point of the magazine imaging point in a described left side and the right side and space geometry relation, wherein, the computing formula of described space real depth is as follows:

$Z_p=\frac{B_L\×f_C}{Disparity}$

Wherein, Z _pfor the space real depth of described matching characteristic point, B _lfor baseline, f _cfor focal length, Disparity is horizontal parallax.

In some instances, described S5 comprises further: be stored in by row in the first matrix by the descriptor of present frame unique point, and is stored in the second matrix by row by the descriptor of the unique point in described world map; Travel through the whole unique points in described world map, from described world map, extract the unique point the highest with each unique point similarity in described present frame respectively; The unique point the highest according to described similarity sets up the unique point mapping relations table from described present frame unique point to described world map.

In some instances, the similarity of described unique point is determined according to the angle between described unique point descriptor vector, and wherein, when the angle when between described unique point descriptor vector is minimum, the similarity of described unique point is the highest.

In some instances, in described S6, analyzed by the matching result of random sampling consistent method to the unique point in the unique point of described present frame and world map, to remove the unique point of wherein erroneous matching, the unique point of coupling of succeeding, specifically comprise: S61: from the unique point of N group successful match, choose m group matching characteristic point at random, and set up global registration equation, the coordinate calculating described m group matching characteristic point according to described global registration equation transforms to the transformation matrix of Current camera reference frame from world coordinate system; S62: by the coordinate under the Current camera reference frame of N-m group matching characteristic point do not chosen according to described transformation matrix inverse mapping to world coordinate system, obtain coordinate inverse mapping result CoorTemp, and calculate the Euclidean distance of described CoorTemp and the actual observation coordinate CoorGlobal of unique point under world coordinate system; S63: judge whether described Euclidean distance is greater than predetermined threshold value, if described Euclidean distance is greater than predetermined threshold value, then judge that characteristic of correspondence point is as point not in the know, if described Euclidean distance is less than described predetermined threshold value, then judge that characteristic of correspondence point is as intra-office point, and the number put in statistics bureau; S64: iteration performs described S61 to S63, and the number and the transformation matrix of coordinates that record intra-office point in each iterative process; S65: choose from successive ignition process and comprise a maximum iteration of intra-office point number, and using the unique point of the intra-office point in this iteration as successful match, using the unique point of point not in the know as erroneous matching, and remove the unique point of described erroneous matching.

In some instances, described S7 comprises further: obtain the coordinate CoorGlobal of unique point under world coordinate system of described successful match and the coordinate CoorCamera under present image capture systems reference frame; The rotation matrix R of the unique point calculating described successful match between three-dimensional coordinate and translation matrix T, the pose change estimated value of described binocular image capture systems relative initial position is obtained according to described rotation matrix R and translation matrix T, wherein, described rotation matrix R and translation matrix T meet following relational expression:

$\underset{R,T}{\min }\underset{inliers}{\Σ}\left|\right|CoorCamera-T-R\×CoorGlobsl|{|}^2.$

In some instances, described S8 comprises further: when first frame calculates, world map described in initialization, is defined as the world coordinate system of this location, the three-dimensional coordinate of whole for first frame unique point and Feature Descriptor is added world map by the image capture system reference frame of first frame; When non-first frame calculates, according to described transformation matrix by the three-dimensional coordinate CoorCamera inverse mapping of the unique point of present frame successful match to world coordinate system CoorGlobal ', wherein, conversion formula is:

CoorGlobal′＝inv(R)×(CoorCamera-T)；

Be the average of repeatedly observing by the coordinate renew of described successful match unique point under world coordinate system, wherein, when (n+1)th time is observed this unique point, the coordinate renew formula in its world map is:

$CoorGlbal=\frac{CoorGlobal\×n+{\mathrm{CoorGlobal}}^{\′}}{n+1};$

The unique point descriptor of described successful match unique point is updated to the last unique point descriptor observed, and unique point three-dimensional coordinate newly observed for present frame and unique point descriptor are added described world map.

According to the simultaneous localization and mapping method of the view-based access control model ranging of the embodiment of the present invention, by binocular camera positioning system, the real depth of space characteristics point can be recovered in without the indoor environment of priori; Vision ranging method is adopted to carry out pose estimation, effectively reduce the computation complexity of simultaneous localization and mapping algorithm, computing velocity and world map scale are got a promotion, is applicable to locate mobile platform in real time, and can be operated in the wider interior space; Meanwhile, effectively eliminated the unique point of erroneous matching by random sampling consistent method, avoid as pose is estimated to introduce control information; In addition, effectively eliminate the accumulative of measuring error by setting up and upgrading world map, ensure that indoor position accuracy is not drifted about in time in centimetre rank; In addition, constructed world map comprises dense spatial signature information, can be used for follow-up obstacle avoidance, path planning work.

Additional aspect of the present invention and advantage will part provide in the following description, and part will become obvious from the following description, or be recognized by practice of the present invention.

Accompanying drawing explanation

Above-mentioned and/or additional aspect of the present invention and advantage will become obvious and easy understand from accompanying drawing below combining to the description of embodiment, wherein:

Fig. 1 is the process flow diagram of the simultaneous localization and mapping method of view-based access control model ranging according to an embodiment of the invention;

Fig. 2 is three-dimensional reconstruction space geometry principle schematic according to an embodiment of the invention;

Fig. 3 is the process flow diagram of random sampling consistent method according to an embodiment of the invention;

Fig. 4 is world coordinate system and camera coordinates system relation schematic diagram according to an embodiment of the invention;

Fig. 5 is indoor positioning movement locus and world map schematic diagram according to an embodiment of the invention;

Fig. 6 is indoor according to an embodiment of the invention unbiased test result schematic diagram.

Embodiment

Be described below in detail embodiments of the invention, the example of described embodiment is shown in the drawings, and wherein same or similar label represents same or similar element or has element that is identical or similar functions from start to finish.Being exemplary below by the embodiment be described with reference to the drawings, only for explaining the present invention, and can not limitation of the present invention being interpreted as.

Below in conjunction with accompanying drawing, the simultaneous localization and mapping method according to the view-based access control model ranging of the embodiment of the present invention is described.

Fig. 1 is the process flow diagram of the simultaneous localization and mapping method of view-based access control model ranging according to an embodiment of the invention.As shown in Figure 1, the method comprises the following steps:

Step S1: gather binocular image by binocular image capture systems, and binocular image is corrected, to obtain undistorted binocular image.

In concrete example, binocular image capture systems is binocular camera, and it is made up of the camera of two parallel placements, and, this distance is baseline between two cameras in a distance.Particularly, binocular camera triggers by identical signal, ensures that the moment of captured picture is identical.In concrete example, binocular image capture systems mobile platform motion in a series of photo stream of the capture of 30 frames per second, using the input as positioning system.

Further, charge-coupled image sensor (ChargeCoupledDevice, CCD) phase chance produces the distortion caused by lens imaging in imaging process, as radial distortion, and oblique distortions etc.For binocular image capture systems, there is optical distortion in each one camera, and simultaneously two camera angles are not quite identical also can bring difficulty to subsequent image data process.More specifically, there is the relation of rotation and translation between two camera reference frames, this relation makes the unique point in the same space different separately at two magazine pixel coordinates, thus adds the computation complexity that three-dimensional reconstruction recovers real depth.Therefore, in one embodiment of the invention, the binocular camera aligning tool case such as provided by matlab extracts the imaging parameters of binocular camera, and according to this imaging parameters, taken binocular image is corrected in advance, the picture after correcting is made to there is not optical distortion, and the pixel coordinate of same characteristic features point in binocular image has identical ordinate, only there are differences in horizontal ordinate direction, namely obtain undistorted binocular image.

Step S2: undistorted binocular image is rotated, stretches, the feature point extraction of illumination invariant, and according to the unique point generating feature point descriptor extracted.

Particularly, this step carries out feature point detection respectively from the binocular image after correction, obtains the pixel coordinate of unique point in two dimensional image, and the descriptor of extract minutiae.Feature extraction may comprise scale invariant feature (scale-invariantfeaturetransform, SIFT) and accelerate robust feature (SpeededUpRobustFeatures, SURF) etc.

In one embodiment of the invention, scale invariant feature conversion method is such as adopted to carry out feature point extraction to undistorted binocular image.Scale invariant feature conversion method is the algorithm carrying out feature point extraction and set up matching relationship between unique point.This Feature Correspondence Algorithm rotates at image, scaling and brightness keep relative stability when changing.Consider that indoor belong near field environment, along with the scaling effect of the mobile space object of shooting platform is comparatively obvious; The illumination condition of indoor existence complexity simultaneously.Therefore, this algorithm be applicable to indoor binocular image, inter frame image feature point extraction with mate.Identical unique point can extract and set up characteristic matching relation from different shooting conditions and shooting visual angle.In scale invariant feature conversion, the vector description that each unique point descriptor is tieed up with 128, the matching relationship of two unique points is whether the vector angle by judging its unique point descriptor is less than specific threshold and determines.It should be noted that, embodiments of the invention are not limited only to adopt scale invariant feature conversion method to carry out feature point extraction to undistorted binocular image, and also can be further feature point extracting method, be only exemplary displaying herein.

Step S3: the Feature Points Matching relation setting up binocular image according to unique point descriptor.This step and binocular ranging step, wherein, in binocular ranging, the unique point of successful match can recover its spatial depth according to three-dimensional reconstruction step, and the unsuccessful unique point setting up coupling is eliminated in this step.

Further, whether binocular ranging step is less than by the vector angle between judging characteristic point descriptor the matching relationship that specific threshold determines between unique point, and its concrete operation step is similar with step S5 afterwards.

Particularly, suppose that the descriptor of the unique point that left figure extracts is stored in matrix L ([DscpL by row ₁, DscpL ₂..., DscpL _m] ^t) in, the descriptor of the unique point in right figure is stored in matrix R ([DscpR ₁, DscpR ₂..., DscpR _n] ^t) in.For each unique point in left figure, travel through whole unique points of right figure, find the unique point the most similar to it and set up unique point mapping relations from left figure to right figure.Further, the tolerance of similarity is provided by the angle between unique point descriptor vector: the angle between vector is less, and two vector similarities are higher; The similarity of two unique point Feature Descriptors is higher, and the coupling degree of confidence of unique point is larger.

Step S4: the horizontal parallax obtaining matching characteristic point according to the Feature Points Matching relation of binocular image, and the space real depth calculating matching characteristic point according to the horizontal parallax of matching characteristic point and the parameter of binocular image capture systems (binocular camera).Wherein, the parameter of binocular camera such as comprises: the focal length of binocular camera, baseline and central point etc.

In one embodiment of the invention, image capture system comprises left camera and right camera, and step S4 comprises further: obtain the pixel coordinate of matching characteristic point at the magazine imaging point of left camera and the right side respectively; The horizontal parallax of left magazine imaging point and the magazine imaging point in the right side is calculated according to the coordinate of image center point; Obtain the space real depth of matching characteristic point according to the horizontal parallax of the magazine imaging point of the magazine imaging point in a left side and the right side and space geometry relation, wherein, the computing formula of space real depth is as follows:

$Z_p=\frac{B_L\×f_C}{Disparity}.$

Wherein, Z _pfor the space real depth of matching characteristic point, B _lfor baseline, f _cfor focal length, Disparity is horizontal parallax.

Specifically, in work in the past, the real depth calculating unique point often needs there is certain priori to environment, such as: the object of known depth in scene, signature identification of inserting environment etc.Which has limited the range of application of algorithm in circumstances not known.In one embodiment of the invention, without the need to knowing the degree of depth of object in space in advance or interfering environment in advance, by the imaging characteristics of binocular camera, three-dimensional reconstruction work is carried out to the unique point captured in space, its 3 d space coordinate of camera reference frame relative to the shooting moment can be obtained.

Further, with reference to shown in Fig. 2, three-dimensional reconstruction space geometry principle schematic (overlooking) is given.The camera reference frame of the binocular camera after correction is by (x ₁, y ₁, z ₁) and (x ₂, y ₂, z ₂) represent, it has identical imaging plane.On a left side, magazine imaging point is P to space mid point P _l, on the right side, magazine imaging point is P _r.Binocular camera parameter can be obtained in step s 2 as focal length (f _c), baseline (B _l), central point etc.Especially, for the unique point of mating in binocular image, because binocular image is through correcting in advance, its imaging point P _lwith P _ronly there is pixel value difference in the horizontal direction, in the vertical direction pixel coordinate is identical.Particularly, the distance in the real space calculating corresponding to the magazine imaging point of left magazine imaging point and the right side by the coordinate of camera imaging resolution and central point, the i.e. horizontal parallax (Disparity) of two imaging points.Further, the space real depth Z of matching characteristic point (as a P) is calculated according to space geometry relation _p, wherein, the computing formula of space real depth is as follows:

$Z_p=\frac{B_L\×f_L}{Disparity}.$

Further, according to camera pinhole imaging system principle, can by space real depth Z _p, pixel coordinate (x _p, y _p) and camera focus f _ccalculate the three-dimensional coordinate (X of unique point in left camera reference frame (subsequent calculations with left camera for benchmark camera) _p, Y _p, Z _p), circular is:

$\{\begin{array}{c}{X}_p=x_p\frac{Z_p}{f_C}\\ Y_p=y_p\frac{Z_p}{f_C}\end{array}.$

Step S5: the unique point obtaining present frame binocular image, and the unique point of present frame binocular image is mated with the unique point in world map.In other words, this step and world map coupling step, refer to set up matching relationship between the unique point that comprises in the unique point extracted in the current frame and world map.

In one embodiment of the invention, this step S5 comprises further: be stored in by row in the first matrix by the descriptor of present frame unique point, and is stored in the second matrix by row by the descriptor of the unique point in world map; Whole unique points in traversal world map, extract the unique point the highest with each unique point similarity in present frame respectively from world map; The unique point the highest according to similarity sets up the unique point mapping relations table from present frame unique point to world map.

Specifically, suppose that the descriptor of the unique point that present frame extracts is stored in the first matrix A ([DscpC by row ₁, DscpC ₂..., DscpC _m] ^t) in, the descriptor of the whole unique points comprised in world map is stored in the second matrix B ([DscpG ₁, DscpG ₂..., DscpG _n] ^t) in.For each unique point of present frame, the whole unique points in traversal world map, the searching unique point the most similar to it sets up the unique point mapping relations from present frame to world map.Particularly, the similarity of unique point is determined according to the angle between unique point descriptor vector, and the angle between vector is less, and two vector similarities are higher, and when the angle when between unique point descriptor vector is minimum, the similarity of unique point is the highest.

Further, world map coupling comprises following operation: the descriptor column vector DscpC of the second matrix B and unique point i _ibe multiplied, result is n × 1 column vector, and n element in vector represents DscpC respectively _iand DscpG _j(j=1...n) vectorial angle cosine value.Further, can vector angle be obtained by arc cosine formulae discovery, be stored in n × 1 column vector Res _iin, wherein element Res _il () (l=1...n) represents the vector angle of the unique point descriptor of l unique point in i-th unique point and world map that present frame extracts.

Further, Res is traveled through _iin each element, find wherein minimum element be denoted as Res _i(l ₁), and the second little element is denoted as Res _i(l ₂), wherein, l ₁and l ₂represent minimum and the second little element at Res _iin position (namely at world map comprise position in unique point).Further, l in i-th unique point of and if only if when meeting the following conditions present frame and world map ₁individual unique point successful match:

Res _i(l ₁)＜0.8Res _i(l ₂)。

Further, successively by world map unique point descriptor storage matrix B and present frame each unique point descriptor DscpC _ibe multiplied, the process be multiplied can be accelerated by matrix operation.Particularly, by matrix multiplication B × A ^t, the vectorial angle cosine matrix of each unique point descriptor in the whole unique point descriptor of present frame and world map can be gone out by a command calculations.Further, the calculated example of arc cosine is as being approximately a series of matrix multiplication, addition of matrices computing by Maclaurin expansion.By relevant matrix operation storehouse as MKL (IntelMathKernelLibrary, Intel's mathematics core function storehouse), cuBLAS (NVIDIACudaBasicLinearAlgebraSubroutines, tall and handsomely reach the basic linear algebra subroutine library) can realize based on CPU (CentralProcessingUnit, central processing unit) and the high performance matrix computing of GPU (GraphicProcessingUnit, graphic process unit).Consider that world map dimension is comparatively large, therefore, the method effectively improves world map coupling and binocular ranging step in the algorithm operational efficiency of software end, improves the frame per second of algorithm process image, makes the real-time location of mobile platform become possibility.

Step S6: the matching result of the unique point in the unique point of present frame binocular image and world map is analyzed, to remove the unique point of wherein erroneous matching, the unique point of coupling of succeeding.Particularly, can form the unique point of some erroneous matching in world map coupling, the unique point of these erroneous matching has similar unique point descriptor, but does not belong to same picture point.The unique point of erroneous matching can be estimated to introduce error to pose, makes positioning result inaccurate.Therefore, in one embodiment of the invention, the matching result of random sampling consistent method to the unique point in the unique point of present frame binocular image and world map is adopted to analyze, to remove the unique point of wherein erroneous matching.Specifically, with reference to shown in Fig. 3, the process flow diagram of random sampling consistent method is given.Coordinate conversion relation between the coordinate of unique point (intra-office point) under system of the world of the correct coupling of random sampling consistent method hypothesis and the coordinate in Current camera reference frame can be described by unified model, and the unique point of erroneous matching (point not in the know) does not meet this coordinate transformation model.Meanwhile, the unique point of erroneous matching belongs to the exception in measurement, in whole feature point set, belong to small probability event.

Particularly, in one embodiment of the invention, remove erroneous matching unique point by random sampling consistent method, comprise the following steps:

S61: choose m group matching characteristic point at random from the unique point of N group successful match, and set up global registration equation, the coordinate calculating described m group matching characteristic point according to global registration equation transforms to the transformation matrix of Current camera reference frame from world coordinate system.

S62: by coordinate under Current camera reference frame of the N-m group matching characteristic point do not chosen in step S61 according to the transformation matrix inverse mapping calculated in step S61 to world coordinate system, obtain coordinate inverse mapping result CoorTemp, and calculate the Euclidean distance of CoorTemp and the actual observation coordinate CoorGlobal of unique point under world coordinate system, be called Matching power flow.

S63: in determining step S62, whether Euclidean distance (Matching power flow) is greater than predetermined threshold value, and if this Euclidean distance be greater than predetermined threshold value, then judge that characteristic of correspondence point is as point not in the know, does not namely meet the transformation matrix of coordinates in step S61; If this Euclidean distance is less than predetermined threshold value, then judge that characteristic of correspondence point is as intra-office point, and add up the number of intra-office point in this calculating.Wherein, this predetermined threshold value both by presetting experiment measuring in the environment, also can be provided by experience, and wanting the process noise of Considering experimental environment and measurement when arranging predetermined threshold value.

S64: iteration performs S61 to S63 (i.e. repeated execution of steps S61 to S63), and records number and the transformation matrix of coordinates of intra-office point in each iterative process;

S65: choose from successive ignition process and comprise a maximum iteration of intra-office point number, and using the unique point of the intra-office point in this iteration as successful match, using the unique point of point not in the know as erroneous matching, in subsequent calculations, remove the unique point (point not in the know) of erroneous matching.

Step S7: the transformation matrix calculating the coordinate of unique point under world coordinate system of successful match and the three-dimensional coordinate under current reference coordinates system, obtains the pose change estimated value of binocular image capture systems (binocular camera) relative initial position according to transformation matrix.

Particularly, with reference to shown in Fig. 4, world coordinate system and camera coordinates system relation schematic diagram is given.World coordinate system is defined as the camera coordinates system in image taking first frame moment, no longer changes in subsequent calculations.Camera reference frame is along with the motion of mobile platform being loaded with binocular image capture systems, relative world coordinate system rotates and translation, the target that pose is estimated is the transformation relation between acquisition two coordinate systems, is the motion that mobile platform relative initial position occurs.Further, by step S1 to step S6, succeed the coordinate of unique point under world coordinate system of coupling and the coordinate in camera reference frame thereof.According to geometrical principle, the coordinate system transformation of Two coordinate system and the coordinate transform of same point under two coordinate systems inverse process each other, this is the basis of carrying out pose estimation.

Further, the coordinate CoorGlobal of unique point under world coordinate system of successful match and the coordinate CoorCamera under present image capture systems reference frame thereof is obtained.Transformation relation between three-dimensional coordinate can describe by rotation matrix R and translation matrix T, therefore, the rotation matrix R of the unique point that can calculate successful match between three-dimensional coordinate and translation matrix T, namely obtain the pose change estimated value of binocular image capture systems relative initial position.Particularly, the process that pose is estimated is for finding one group of coordinate conversion relation R and T, after making, by this group coordinate conversion relation, the feature point set of coupling is transformed into camera reference frame from world coordinate system, minimum with the square error sum of its actual observation coordinate under present frame camera reference frame, also namely rotation matrix R and translation matrix T meet following relational expression:

$\underset{R,T}{\min }\underset{inliers}{\Σ}\left|\right|CoorCamera-T-R\×CoorGlobsl|{|}^2.$

Step S8: when first frame calculates, initialization world map, and when non-first frame calculates, according to the transformation matrix that calculates in step S7 upgrade in world map with the three-dimensional coordinate of the unique point of present frame successful match and Feature Descriptor, and in world map, add the newly observed unique point of present frame and Feature Descriptor thereof.Wherein, world map comprises the coordinate of unique point under world coordinate system and the Feature Descriptor of unique point that shooting area extracts.When calculating present frame, the world coordinates of the unique point of successful match will be updated, and newly observed unique point will be added into world map.The O&M of world map, the historical information combining each frame on the one hand obtains the accurate estimation of unique point three-dimensional coordinate under world coordinate system, successfully set up the data correlation of present frame and historical frames on the other hand by world map coupling step, compensate for effect by error and effectively reduce measuring error and cumulative errors.In addition, constructed world map comprises dense space characteristics, can be used for follow-up obstacle avoidance, path planning work.

Particularly, when first frame calculates, world map described in initialization, is defined as the world coordinate system of this location by the image capture system reference frame of first frame, three-dimensional coordinate and the Feature Descriptor of the whole unique points observed by first frame add in world map, and the initialization as world map builds.World coordinate system is once determining no longer to change in subsequent calculations.

Further, in the unique point that present frame extracts, a part and world map successfully establish matching relationship, estimate to obtain coordinate conversion relation R and T according to the pose in step S7, by the inverse world coordinate system that projects back of the three-dimensional coordinate of unique point under Current camera reference frame, as the once new observations CoorGlobal ' of unique point, conversion formula is:

CoorGlobal′＝inv(R)×(CoorCamera-T)。

Further, the three-dimensional coordinate of the unique point of successful match under world coordinate system is updated to repeatedly the average of observations.Particularly, in world map, the coordinate of certain unique point is the average after observing this unique point n-th time, observes this unique point at present frame and calculates observations CoorGlobal ' for (n+1)th time, and the formula upgrading three-dimensional coordinate in this world map is:

$CoorGlbal=\frac{CoorGlobal\×n+{\mathrm{CoorGlobal}}^{\′}}{n+1}.$

Particularly, according to central limit theorem, measuring error is similar to that to meet with true value be the normal distribution of average.By getting arithmetic mean to each observations of unique point, measuring error is compensated for effect by error and is effectively reduced, and in world map, the three-dimensional coordinate of unique point is closer to true value.

Further, the unique point descriptor of the unique point of successful match in world map is upgraded by the Feature Descriptor extracted in a nearest two field picture.Consider the continuity of the time and space, a nearest two field picture and follow-up shooting image have more close image-forming condition, use the last unique point descriptor and the unique point descriptor extracted in successive image to be successfully established the probability mated larger.

Further, the unique point newly added is not considered in present frame with the unique point of world map successful match.The viewing area that its most probable is new after appearing at view transformation also may be the supplementary extraction to overlapping viewing area unique point.Its three-dimensional coordinate under camera reference frame is projected to world coordinate system by inverse equally, and is added in world map, and its unique point descriptor is also added into world map.

Further, embodiments of the invention have and compare the simpler computation complexity of EKF simultaneous localization and mapping algorithm (EKF-SLAM).

Particularly, the method of the embodiment of the present invention with all comprise the world map described in step S5 based on EKF simultaneous localization and mapping method and mate, its computation complexity is: establish world map to comprise n unique point, the Feature Descriptor of each unique point is p dimensional vector, having m unique point to participate in the matching primitives with world map in present frame, is O (m × n × p) at the computation complexity of this step matrix multiplication.In one particular embodiment of the present invention, if having m in present frame _cindividual unique point and world map establish matching relationship, and the computation complexity that the random sampling consistent method in step S6 and the pose in step S7 are estimated is about O (3 × m _c× k), wherein k represents the iterations in random sampling consistent method.For based on EKF simultaneous localization and mapping method, its pose estimating step needs the covariance matrix safeguarding world map, and this step computation complexity is about O (n × n).Consider the feature point number m that iterations k and present frame mate with world map _cbe far smaller than the feature point number n in world map, therefore, embodiment of the present invention method is compared has simpler computation complexity based on EKF simultaneous localization and mapping method, consider the low computing power of mobile platform, low power consumption characteristic, the inventive method is more suitable for the real-time location tasks of small-sized movable platform.

In order to verify the performance of the simultaneous localization and mapping method of the view-based access control model ranging that the embodiment of the present invention proposes, embodiments of the invention take indoor real scene by binocular image capture systems, and implement the software verification of the method at MATLABR2014b.Wherein, binocular image capture systems is arranged on horizontal slide rail to ensure the rectilinear motion of mobile platform.As shown in table 1, give binocular image capture systems to move the pose estimated value that different distances, method when rotating different angles in the embodiment of the present invention provide, wherein X/Y/Z represents the position coordinates of binocular image capture systems in world coordinate system, and Pitch/Yaw/Roll represents the angle of pitch/crab angle/roll angle of binocular image capture systems in world coordinate system.Measuring true value is rule registration by horizontal rail and protractor gained, and when binocular image capture systems remains static, positive and negative values represents the standard deviation to estimation; When binocular image capture systems moves, the estimated result provided in table 1 is the mean value of ten estimation.As can be seen from Table 1, in this embodiment, motion estimation error is less than two centimetres, and this is close with the positioning precision based on the simultaneous localization and mapping algorithm of EKF.Further, the error of estimation under calculating stationary state, this indicates the drift situation of embodiments of the invention to motion estimation result:

${\mathrm{Error}}_T=\sqrt{mean(X^2+Y^2+Z^2)}=1.23cm,$

Table 1

Further, as shown in Figure 5, indoor positioning movement locus and the world map schematic diagram of one embodiment of the invention is given.In this embodiment, binocular image capture systems is placed on the dolly of assembling pulley.Promote dolly and move forward about 80cm with the speed of 5cm/s, be rotated counterclockwise about 65 °, then move forward in the direction.Can find out, the position and orientation estimation method that the present invention proposes gives pose estimated result accurately.Meanwhile, in the world map of structure, comprise the three-dimensional information of a large amount of space characteristics point, can be used for follow-up obstacle avoidance, the work such as path planning.

Further, as shown in Figure 6, the indoor unbiased test result schematic diagram of one embodiment of the invention is given.In this embodiment, binocular image capture systems is along same segment distance to-and-fro movement 20 times on horizontal slide rail, and the distance estimations of depth direction, in repeatedly calculating, unidirectional drift does not occur.This illustrates that the location estimation method that the present invention proposes is not passed in time and cumulative errors occurs and then floats gradually from true value.By the data correlation that world map is set up, effectively reduce the cumulative errors that closed loop moving in space is estimated.

To sum up, according to the simultaneous localization and mapping method of the view-based access control model ranging of the embodiment of the present invention, by binocular camera positioning system, the real depth of space characteristics point can be recovered in without the indoor environment of priori; Vision ranging method is adopted to carry out pose estimation, effectively reduce the computation complexity of simultaneous localization and mapping algorithm, computing velocity and world map scale are got a promotion, is applicable to locate mobile platform in real time, and can be operated in the wider interior space; Meanwhile, effectively eliminated the unique point of erroneous matching by random sampling consistent method, avoid as pose is estimated to introduce control information; In addition, effectively eliminate the accumulative of measuring error by setting up and upgrading world map, ensure that indoor position accuracy is not drifted about in time in centimetre rank; In addition, constructed world map comprises dense spatial signature information, can be used for follow-up obstacle avoidance, path planning work.

In describing the invention, it will be appreciated that, term " " center ", " longitudinal direction ", " transverse direction ", " length ", " width ", " thickness ", " on ", D score, " front ", " afterwards ", " left side ", " right side ", " vertically ", " level ", " top ", " end " " interior ", " outward ", " clockwise ", " counterclockwise ", " axis ", " radial direction ", orientation or the position relationship of the instruction such as " circumference " are based on orientation shown in the drawings or position relationship, only the present invention for convenience of description and simplified characterization, instead of indicate or imply that the device of indication or element must have specific orientation, with specific azimuth configuration and operation, therefore limitation of the present invention can not be interpreted as.

In addition, term " first ", " second " only for describing object, and can not be interpreted as instruction or hint relative importance or imply the quantity indicating indicated technical characteristic.Thus, be limited with " first ", the feature of " second " can express or impliedly comprise at least one this feature.In describing the invention, the implication of " multiple " is at least two, such as two, three etc., unless otherwise expressly limited specifically.

In the present invention, unless otherwise clearly defined and limited, the term such as term " installation ", " being connected ", " connection ", " fixing " should be interpreted broadly, and such as, can be fixedly connected with, also can be removably connect, or integral; Can be mechanical connection, also can be electrical connection; Can be directly be connected, also indirectly can be connected by intermediary, can be the connection of two element internals or the interaction relationship of two elements, unless otherwise clear and definite restriction.For the ordinary skill in the art, above-mentioned term concrete meaning in the present invention can be understood as the case may be.

In the present invention, unless otherwise clearly defined and limited, fisrt feature second feature " on " or D score can be that the first and second features directly contact, or the first and second features are by intermediary indirect contact.And, fisrt feature second feature " on ", " top " and " above " but fisrt feature directly over second feature or oblique upper, or only represent that fisrt feature level height is higher than second feature.Fisrt feature second feature " under ", " below " and " below " can be fisrt feature immediately below second feature or tiltedly below, or only represent that fisrt feature level height is less than second feature.

In the description of this instructions, specific features, structure, material or feature that the description of reference term " embodiment ", " some embodiments ", " example ", " concrete example " or " some examples " etc. means to describe in conjunction with this embodiment or example are contained at least one embodiment of the present invention or example.In this manual, to the schematic representation of above-mentioned term not must for be identical embodiment or example.And the specific features of description, structure, material or feature can combine in one or more embodiment in office or example in an appropriate manner.In addition, when not conflicting, the feature of the different embodiment described in this instructions or example and different embodiment or example can carry out combining and combining by those skilled in the art.

Although illustrate and describe embodiments of the invention above, be understandable that, above-described embodiment is exemplary, can not be interpreted as limitation of the present invention, and those of ordinary skill in the art can change above-described embodiment within the scope of the invention, revises, replace and modification.

Claims (10)

1. a simultaneous localization and mapping method for view-based access control model ranging, is characterized in that, comprise the following steps:

S1: by binocular image capture systems synchronous acquisition binocular image, and described binocular image is corrected, to obtain undistorted binocular image;

S2: feature point extraction is carried out to described undistorted binocular image, and according to the unique point generating feature point descriptor extracted;

S3: the Feature Points Matching relation setting up described binocular image according to described unique point descriptor;

S4: the horizontal parallax obtaining matching characteristic point according to the Feature Points Matching relation of described binocular image, and the space real depth calculating described matching characteristic point according to the described horizontal parallax of matching characteristic point and the parameter of binocular image capture systems;

S5: the unique point obtaining present frame binocular image, and the described unique point of present frame binocular image is mated with the unique point in world map;

S6: the matching result of the unique point in the unique point of described present frame binocular image and world map is analyzed, to remove the unique point of wherein erroneous matching, the unique point of coupling of succeeding;

S7: the transformation matrix calculating the coordinate of unique point under world coordinate system of described successful match and the three-dimensional coordinate under current reference coordinates system, obtains the pose change estimated value of described binocular image capture systems relative initial position according to described transformation matrix;

S8: when first frame calculates, world map described in initialization, and when non-first frame calculates, according to described transformation matrix upgrade in world map with the three-dimensional coordinate of the unique point of present frame successful match and Feature Descriptor, and in world map, add the newly observed unique point of present frame and Feature Descriptor thereof.

2. the simultaneous localization and mapping method of view-based access control model ranging according to claim 1, is characterized in that, described S1 comprises further:

Extract the imaging parameters of described binocular image capture systems, and according to described imaging parameters, described binocular image is corrected, to obtain undistorted binocular image.

3. the simultaneous localization and mapping method of view-based access control model ranging according to claim 1, is characterized in that, in described S2, carries out feature point extraction by scale invariant feature conversion method to described undistorted binocular image.

4. the simultaneous localization and mapping method of view-based access control model ranging according to claim 1, it is characterized in that, in described S3, whether be less than the matching relationship between unique point that specific threshold sets up described binocular image by the vector angle between the unique point descriptor that judges described binocular image.

5. the simultaneous localization and mapping method of view-based access control model ranging according to claim 1, is characterized in that, wherein, described binocular image capture systems comprises left camera and right camera, and described S4 comprises further:

Obtain the pixel coordinate of described matching characteristic point at the magazine imaging point of left camera and the right side respectively;

The horizontal parallax of the magazine imaging point of the magazine imaging point in a described left side and the right side is calculated according to the coordinate of camera imaging resolution and described central point;

Obtain the space real depth of described matching characteristic point according to the horizontal parallax of the magazine imaging point of the magazine imaging point in a described left side and the right side and space geometry relation, wherein, the computing formula of described space real depth is as follows:

$Z_p=\frac{B_L\×f_C}{Disparity}$

Wherein, Z _pfor the space real depth of described matching characteristic point, B _lfor baseline, f _cfor focal length, Disparity is horizontal parallax.

6. the simultaneous localization and mapping method of view-based access control model ranging according to claim 1, is characterized in that, described S5 comprises further:

The descriptor of present frame unique point is stored in the first matrix by row, and the descriptor of the unique point in described world map is stored in the second matrix by row;

Travel through the whole unique points in described world map, from described world map, extract the unique point the highest with each unique point similarity in described present frame respectively;

The unique point the highest according to described similarity sets up the unique point mapping relations table from described present frame unique point to described world map.

7. the simultaneous localization and mapping method of view-based access control model ranging according to claim 6, it is characterized in that, the similarity of described unique point is determined according to the angle between described unique point descriptor vector, wherein, when angle when between described unique point descriptor vector is minimum, the similarity of described unique point is the highest.

8. the simultaneous localization and mapping method of view-based access control model ranging according to claim 1, it is characterized in that, in described S6, analyzed by the matching result of random sampling consistent method to the unique point in the unique point of described present frame and world map, to remove the unique point of wherein erroneous matching, the unique point of coupling of succeeding, specifically comprises:

S61: choose m group matching characteristic point at random from the unique point of N group successful match, and set up global registration equation, the coordinate calculating described m group matching characteristic point according to described global registration equation transforms to the transformation matrix of Current camera reference frame from world coordinate system;

S62: by the coordinate under the present image capture systems reference frame of N-m group matching characteristic point do not chosen according to described transformation matrix inverse mapping to world coordinate system, obtain coordinate inverse mapping result CoorTemp, and calculate the Euclidean distance of described CoorTemp and the actual observation coordinate CoorGlobal of unique point under world coordinate system;

S63: judge whether described Euclidean distance is greater than predetermined threshold value, if described Euclidean distance is greater than predetermined threshold value, then judge that characteristic of correspondence point is as point not in the know, if described Euclidean distance is less than described predetermined threshold value, then judge that characteristic of correspondence point is as intra-office point, and the number put in statistics bureau;

S64: iteration performs described S61 to S63, and the number and the transformation matrix of coordinates that record intra-office point in each iterative process;

S65: choose from successive ignition process and comprise a maximum iteration of intra-office point number, and using the unique point of the intra-office point in this iteration as successful match, using the unique point of point not in the know as erroneous matching, and remove the unique point of described erroneous matching.

9. the simultaneous localization and mapping method of view-based access control model ranging according to claim 1, is characterized in that, described S7 comprises further:

Obtain the coordinate CoorGlobal of unique point under world coordinate system of described successful match and the coordinate CoorCamera under present image capture systems reference frame;

The rotation matrix R of the unique point calculating described successful match between three-dimensional coordinate and translation matrix T, the pose change estimated value of described binocular image capture systems relative initial position is obtained according to described rotation matrix R and translation matrix T, wherein, described rotation matrix R and translation matrix T meet following relational expression:

$\underset{R,T}{\min }\underset{inliers}{\Σ}\left|\right|CoorCamera-T-R\×CoorGlobal|{|}^2.$

10. the simultaneous localization and mapping method of view-based access control model ranging according to claim 9, is characterized in that, described S8 comprises further:

When first frame calculates, world map described in initialization, is defined as the world coordinate system of this location, the three-dimensional coordinate of whole for first frame unique point and Feature Descriptor is added world map by the image capture system reference frame of first frame;

When non-first frame calculates, according to described transformation matrix by the three-dimensional coordinate CoorCamera inverse mapping of the unique point of present frame successful match to world coordinate system CoorGlobal ', wherein, conversion formula is:

CoorGlobal′＝inv(R)×(CoorCamera-T)；

Be the average of repeatedly observing by the coordinate renew of the unique point of described successful match under world coordinate system, wherein, when (n+1)th time is observed this unique point, the coordinate renew formula in its world map is:

$CoorGlobal=\frac{CoorGlobal\×n+{\mathrm{CoorGlobal}}^{\′}}{n+1};$

The unique point descriptor of described successful match unique point is updated to the last unique point descriptor observed, and unique point three-dimensional coordinate newly observed for present frame and unique point descriptor are added described world map.