CN103424112A - Vision navigating method for movement carrier based on laser plane assistance - Google Patents

Abstract

The invention discloses a vision navigating method for a movement carrier based on the laser plane assistance. The method comprises the following steps: building a distance measurement and environment system, determining the position of a rectangular bright spot to be detected in an image, determining the center coordinate of the rectangular bright spot, obtaining a geometric distance measurement model, correcting the distance measurement model, segmenting the visual image, determining a local vectorization environmental space with distance information, positioning the position of the carrier per se, and building a global environmental space. Through the adoption of the method different from the traditional movement carrier autonomous navigation middle distance obtaining method, a laser plane is introduced as an auxiliary means, a single or a plurality of vision sensors are combined, the threadlike bright spot formed by the reflection of the laser plane on an object to be irradiated is fully utilized and taken as an cooperated distance measurement target, and the geometric relationship between the vision sensor and a laser light source is effectively utilized, so that the requirements for the precision, yield ratio and real-time performance of the distance calculating can be met. Therefore, the method is a reliable navigation method, and can be applied to a movement carrier autonomous navigation system.

Description

A kind of motion carrier vision navigation method auxiliary based on laser plane

Technical field

The present invention relates to a kind of vision navigation method for motion carrier, relate in particular to a kind of auxiliary method of carrying out the vision range-finding navigation of laser plane that adopts, belong to motion carrier independent navigation field.

Background technology

In the situation that external environmental information input while lacking high-precision real, all kinds of motion carriers are difficult to realize independent navigation by inertia/GNSS system.Utilize vision technique realize motion carrier to the high precision perception of unknown barrier and hide, can effectively improve the application power of carrier.Monocular or used for multi-vision visual rely on it simple in structure, and the advantages such as low-power consumption, small size, having broad application prospects aspect the indoor and outdoor navigation field.Research is the ranging technology to the place ahead barrier based on laser-assisted motion carrier, and the estimation collision time, not only can evade service for barrier effectively, positions oneself simultaneously and provide information with the reconstruct of exterior three dimensional environment.Therefore, vision navigation method is significant to the independent navigation of motion carrier fast and accurately.

The vision guided navigation algorithm has been carried out to many research both at home and abroad, proposed to realize by the method for pure vision the independent navigation of carrier in part paper and document, but pure vision guided navigation is affected by environment large and obtaining information is limited; Propose to have used vision/inertia/multiple measurement means such as ultrasound wave in some papers and document, such method is in the situation that shortage cooperative target effect is limited; Some researchists are on the basis of having used vision sensor, using laser radar as main range finding means, realized navigation feature, be aided with other sensors such as inertia, odometer, ultrasound wave, sonar after effect very outstanding, but cost and energy consumption cost are higher.And in general laser radar can only be realized the two dimension range finding, the environment construction effect is limited.

Summary of the invention

Technical matters

The technical problem to be solved in the present invention be in the autonomous vision guided navigation algorithm of conventional motion carrier by the two dimensional image coordinate derive the space ambiguity problem draw the three-dimensional world coordinate and range information amount not fully, algorithm process real-time and distance accuracy problem.In order to simplify LASER Light Source and video camera mounting condition (without the specific (special) requirements such as parallel, vertical), the air navigation aid of a kind of auxiliary vision range finding based on laser plane, autonomous location, environment construction is provided, the required hardware configuration of the method is simple, volume is little, energy consumption is low, has overcome the restriction that is subject to self can't carrying large volume, large power consumption device due to motion carrier; The range information obtained is carried out to the matrixing storage, and combining image processing acquisition Vectorgraph, merge the Inertial Measurement Unit information realization to hiding of carrier self poisoning, peripheral obstacle and obtaining of environmental information.

Technical scheme

In order to solve above-mentioned technical matters, the motion carrier vision navigation method auxiliary based on laser plane of the present invention, comprise the steps:

Step 1: by by being arranged on LASER Light Source on motion carrier and vision sensor as range finding and environment construction system.Utilize LASER Light Source to send laser plane, produce and reflect to form the wire speck on the testee of carrier the place ahead, then take and obtain the visual pattern that comprises this wire speck by vision sensor;

Step 2: determine one group of speck to be measured and coordinate range thereof in step 1 gained visual pattern, utilize the stencil matching method, in image, the position of rectangle speck to be measured in image determined in search, wherein:

$R(i,j)=\frac{\underset{m=1}{\overset{M}{\mathrm{\Σ}}}\underset{n=1}{\overset{N}{\mathrm{\Σ}}}(S^{i,j}(m,n)\×T(m,n))}{\underset{m=1}{\overset{M}{\mathrm{\Σ}}}\underset{n=1}{\overset{N}{\mathrm{\Σ}}}{\left(S^{i,j}(m,n)\right)}^2}$

Template and subimage size are all the rectangle of M * N pixel; (i, j) is the reference point coordinate, represents the subimage position;

For masterplate, it is normal value;

For variable, with reference point, change; Represent masterplate and the subimage degree of correlation, when subimage masterplate and masterplate similarity maximum, can obtain the subimage position that reference point is (I, J), i.e. rectangle speck to be measured position;

Step 3: choose the brightness of rectangle speck to be measured as weights, by gray scale weights method, to each pixel weighted sum of rectangle speck, draw the centre coordinate of rectangle speck after average:

$x_c=\frac{\underset{x=I}{\overset{I+m}{\mathrm{\Σ}}}\underset{y=J}{\overset{J+n}{\mathrm{\Σ}}}S(x,y)x}{\underset{x=I}{\overset{I+m}{\mathrm{\Σ}}}\underset{y=J}{\overset{J+n}{\mathrm{\Σ}}}S(x,y)},y_c=\frac{\underset{x=I}{\overset{I+m}{\mathrm{\Σ}}}\underset{y=J}{\overset{J+n}{\mathrm{\Σ}}}S(x,y)y}{\underset{x=I}{\overset{I+m}{\mathrm{\Σ}}}\underset{y=J}{\overset{J+n}{\mathrm{\Σ}}}S(x,y)},(S(x,y)\≥T)$

Wherein, (I, J) is the reference point coordinate of the rectangle speck of step 3 acquisition, S ^i,j(m, n) is weight, and T is weight threshold, if S ^i,j(m, n) is less than T and is set to 0, obtains rectangle speck centre coordinate after weighted mean;

Step 4: according to the relative position of the vision sensor on carrier in step 1 and lasing light emitter, according to the pinhole imaging system principle, obtain the range finding model how much, how much range finding models are as follows:

$S=\frac{d}{m-\cos \mathrm{\β}(\tan \mathrm{\θ}+\tan a\tan \mathrm{\β})},m=\frac{(v_{B0}-v_{B20})\cos \mathrm{\β}}{\mathrm{ay}}$

Wherein, d is LASER Light Source and camera light distance in the heart; α, β, θ installs angle for each, and ay is focal length and pixel physical size ratio, v _B0, v _B20For each auxiliary point image coordinate;

Step 5: according to parameter and the known conditions in the model of finding range for how much in step 4, in conjunction with the error composition principle, the reason that affects distance accuracy is analyzed, in conjunction with experimental data, produce the parameter of error in compensation range finding model, the range finding model after being proofreaied and correct;

Step 6: while carrying out laser ranging, the range finding model after proofreading and correct by the speck centre coordinate substitution to be measured of obtaining in step 3, obtain the result of finding range at every turn; Or, before navigation, directly bring image coordinate into the range finding model by a fixed step size, and the result obtained is carried out to the matrixing storage, obtain in navigation procedure after facula position and directly extract corresponding distance value, to obtain fast the actual range of laser beam loca;

Step 7: the laser line segment according to known definite distance, utilize region growing method, visual pattern is cut apart, thereby obtained the image segmentation result with range information;

Step 8: the image segmentation result that will have range information carries out vector quantization, and realizes environment reconstruct with the vector quantization figure.All can obtain the Local Vector environment space with range information after each laser scanning;

Step 9: in conjunction with the Inertial Measurement Unit on motion carrier, and the local environment space that obtains of step 8, carry out many information fusion by means of filtering, orient the carrier self-position and build the global context space, realize real-time independent navigation function.

In method of the present invention, the first step adopts laser-assisted method, introduced laser plane as supplementary means, and in conjunction with vision sensor, take full advantage of wire speck that laser plane reflects to form on irradiated object as the cooperation measuring distance of target, can solve in some other vision location algorithms, draw three-dimensional world coordinate existing space ambiguity problem or the less problem that is difficult to range finding of quantity of information by the two dimensional image coordinate, this distance-finding method to the installation site of LASER Light Source and video camera without specific (special) requirements such as parallel grade.In the 6th step, the range information obtained is carried out to matrixing and store the efficiency that can improve the actual range that obtains the laser beam loca.The 7th step and the 8th step obtain the vector quantization environment space by region growing and vector quantization, can effectively reduce the local environmental information amount, can realize the independent navigation of motion carrier after the 9th step information fusion.

Beneficial effect

Method of the present invention has following beneficial effect:

(1) be different from traditional motion carrier independent navigation middle distance acquisition methods, introduced laser plane as supplementary means, and in conjunction with single or multiple vision sensors, take full advantage of wire speck that laser plane reflects to form on irradiated object as the cooperation measuring distance of target, and the method can be carried out the matrixing storage by the range information obtained, need not calculate when speck being detected, directly utilize matrix to extract the distance measurements corresponding with image coordinate.Operand is little, and the resolving time is short, is simple and easy to realize, the range information obtained is stable, not the problem of Existential Space ambiguity.

(2) during installing device, the installation site of video camera and light source is not had to specific (special) requirements, do not require that the laser plane sent must be parallel with camera optical axis, or vertical with the carrier surface level, reduced to install that requirement being set.

(3) effectively utilize the geometric relationship of vision sensor and LASER Light Source, derive the range finding model and improve speck detection efficiency to be measured, and effectively improve distance accuracy by error analysis, suppress the situation that the range finding result is dispersed, effectively utilize the information of visual pattern by region growing, and built the Local Vector environment space with accurate distance information, can build global space after having merged each local space, and realize independent navigation in this space.

(4) by the detection laser speck, the calmodulin binding domain CaM growth, can effectively detect all kinds of barriers in carrier the place ahead, contributes to realize keeping away barrier and environment reconstruct, more simpler effectively than the method for pure vision.

Generally speaking, this method reaches the requirement apart from calculation accuracy, success ratio, real-time, is a kind of air navigation aid that possesses reliability, can be applicable in the motion carrier autonomous navigation system.

The accompanying drawing explanation

The overall flow figure that Fig. 1 is the inventive method;

Fig. 2 is laser plane range finding horizontal field of view scheme of installation;

Fig. 3 is laser plane range finding model scenograph;

Fig. 4 is laser plane range images perspective view.

Embodiment

The auxiliary motion carrier vision navigation method based on laser plane of the present embodiment comprises the steps:

Step 1: by by being arranged on LASER Light Source on motion carrier and vision sensor as range finding and environment construction system.Utilize LASER Light Source to send laser plane, produce and reflect to form the wire speck on the testee of carrier the place ahead, then take and obtain the visual pattern that comprises this wire speck by vision sensor; Step 2: determine one group of speck to be measured and coordinate range thereof in step 1 gained image, utilize the stencil matching method, in image, the position of rectangle speck to be measured in image determined in search, wherein:

$R(i,j)=\frac{\underset{m=1}{\overset{M}{\mathrm{\Σ}}}\underset{n=1}{\overset{N}{\mathrm{\Σ}}}(S^{i,j}(m,n)\×T(m,n))}{\underset{m=1}{\overset{M}{\mathrm{\Σ}}}\underset{n=1}{\overset{N}{\mathrm{\Σ}}}{\left(S^{i,j}(m,n)\right)}^2}$

Template and subimage size are all the rectangle of M * N pixel; (i, j) is the reference point coordinate, represents the subimage position;

For masterplate, it is normal value;

For variable, with reference point, change;

Represent masterplate and the subimage degree of correlation, when subimage masterplate and masterplate similarity maximum, can obtain the subimage position that reference point is (I, J), i.e. rectangle speck to be measured position;

Step 3: choose the brightness of rectangle speck to be measured as weights, by gray scale weights method, to each pixel weighted sum of rectangle speck, draw the centre coordinate of rectangle speck after average:

$x_c=\frac{\underset{x=I}{\overset{I+m}{\mathrm{\Σ}}}\underset{y=J}{\overset{J+n}{\mathrm{\Σ}}}S(x,y)x}{\underset{x=I}{\overset{I+m}{\mathrm{\Σ}}}\underset{y=J}{\overset{J+n}{\mathrm{\Σ}}}S(x,y)},y_c=\frac{\underset{x=I}{\overset{I+m}{\mathrm{\Σ}}}\underset{y=J}{\overset{J+n}{\mathrm{\Σ}}}S(x,y)y}{\underset{x=I}{\overset{I+m}{\mathrm{\Σ}}}\underset{y=J}{\overset{J+n}{\mathrm{\Σ}}}S(x,y)},(S(x,y)\≥T)$

Wherein, (I, J) is the reference point coordinate of the rectangle speck of step 3 acquisition, S ^i,j(m, n) is weight, and T is weight threshold, if S ^i,j(m, n) is less than T and is set to 0, obtains rectangle speck centre coordinate after weighted mean;

Step 4: according to the relative position of the vision sensor on carrier in step 1 and lasing light emitter, according to the pinhole imaging system principle, obtain the range finding model how much, how much range finding models are as follows:

$S=\frac{d}{m-\cos \mathrm{\β}(\tan \mathrm{\θ}+\tan a\tan \mathrm{\β})},m=\frac{(v_{B0}-v_{B20})\cos \mathrm{\β}}{\mathrm{ay}}$

D is LASER Light Source and camera light distance in the heart; α, β, θ installs angle for each, and ay is focal length and pixel physical size ratio, v _B0, v _B20For each auxiliary point image coordinate;

Step 5: according to parameter and the known conditions in the model of finding range for how much in step 4, in conjunction with the error composition principle, the reason that affects distance accuracy is analyzed, in conjunction with experimental data, produce the parameter of error in compensation range finding model, the range finding model after being proofreaied and correct;

Step 6: while carrying out laser ranging, the range finding model after proofreading and correct by the speck centre coordinate substitution to be measured of obtaining in step 3, obtain the result of finding range at every turn; Or, before navigation, directly bring image coordinate into the range finding model by a fixed step size, and the result obtained is carried out to the matrixing storage, obtain in navigation procedure after facula position and directly extract corresponding distance value, to obtain fast the actual range of laser beam loca;

Step 7: the laser line segment according to known definite distance, utilize region growing method, visual pattern is cut apart, thereby obtained the image segmentation result with range information.

Step 8: the image segmentation result that will have range information carries out vector quantization, and realizes environment reconstruct with the vector quantization figure.All can obtain the Local Vector environment space with range information after each laser scanning.

Step 9: in conjunction with the Inertial Measurement Unit on motion carrier, and the local environment space that obtains of step 8, carry out many information fusion by means of filtering, orient the carrier self-position and build the global context space, realize real-time independent navigation function.

The main performing step that below will relate to the present embodiment method is described further:

(1) laser speck position determines

After lasing light emitter sends optical plane and is mapped to objects in front, therefore due to the speck that forms reflection and can produce respectively line-like.And, because brightness is higher, if image is converted into to 8 gray-scale maps, it is 255 that the pixel value of spot center part can reach maximum; If from the aberration angle analysis, what suppose to send is red laser, and the red color component value in the laser speck will be higher than green and blue component.Under this condition that possesses certain priori, stencil matching is a kind of feasible method.Preliminary work before stencil matching comprises carries out filtering, the operation such as level and smooth, remove portion noise to image.

It should be noted that, when laser plane, forwardly on object after reflection, can form a wire speck.But the position difference due to each irradiated object, in most cases the wire speck can not be a complete straight line, there will be interrupted situation, after projecting in the plane of delineation, can not a continuous straight line equally, now can determine according to the actual requirements horizontal (indulging) coordinate range of several rectangles impact point to be measured and image thereof on each interrupted straight line, extract again afterwards the accurate location of rectangle tested point.

Suppose that masterplate is T, size is M * N, and searched figure is that the S(length and width all are more than or equal to masterplate), the search graph covered by masterplate is subgraph S ^i,j, the subgraph size is identical with masterplate.Wherein (i, j) is subgraph upper left corner coordinate, is reference point.

Can be by masterplate and reference diagram respective pixel are done differ from and sue for peace, the check similarity:

$\mathrm{sum}(i,j)=\underset{m=1}{\overset{M}{\mathrm{\Σ}}}\underset{n=1}{\overset{N}{\mathrm{\Σ}}}{(S^{i,j}(m,n)-T(m,n))}^2---\left(1\right)$

After expansion:

$\mathrm{sum}(i,j)=\underset{m=1}{\overset{M}{\mathrm{\Σ}}}\underset{n=1}{\overset{N}{\mathrm{\Σ}}}{\left(S^{i,j}(m,n)\right)}^2-\underset{m=1}{\overset{M}{\mathrm{\Σ}}}\underset{n=1}{\overset{N}{\mathrm{\Σ}}}(S^{i,j}(m,n)\×T(m,n))+\underset{m=1}{\overset{M}{\mathrm{\Σ}}}\underset{n=1}{\overset{N}{\mathrm{\Σ}}}{\left(T(m,n)\right)}^2---\left(2\right)$

In formula (2) For masterplate, it is normal value;

For variable, represent the energy of subimage, with reference point, change;

Represent masterplate and the subimage degree of correlation, if when both are complementary, the value maximum, so can obtain thus related function:

$R(i,j)=\frac{\underset{m=1}{\overset{M}{\mathrm{\Σ}}}\underset{n=1}{\overset{N}{\mathrm{\Σ}}}(S^{i,j}(m,n)\×T(m,n))}{\underset{m=1}{\overset{M}{\mathrm{\Σ}}}\underset{n=1}{\overset{N}{\mathrm{\Σ}}}{\left(S^{i,j}(m,n)\right)}^2}---\left(3\right)$

Masterplate and subimage similarity are larger, and R (i, j) more approaches 1.When subimage masterplate and masterplate similarity maximum, can obtain the subimage position that reference point is (I, J), i.e. rectangle speck to be measured position.

(2) Facula Center Location

After determining the rectangle speck to be measured position that laser forms on testee, can extract on this basis the centre coordinate of rectangle speck on image coordinate.When processing image problem, determine that in image, coordinate can only be accurate to pixel scale, generally can meet accuracy requirement.If measured target from carrier positions away from the time, pixel of every variation, will form larger range deviation.Therefore need to consider the sub-pix problem in the Facula Center Location process, to improve distance accuracy.

Based on considering of the aspects such as complexity and real-time, adopt the grey scale centre of gravity method, imaging characteristic according to the speck zone, the gray-scale value at speck center is maximum, the frontier area gray-scale value is lower, so can obtain centre coordinate after weighted mean using the pixel value of each coordinate as weight, realize the solving precision of sub-pix.

$x_c=\frac{\underset{x=I}{\overset{I+m}{\mathrm{\Σ}}}\underset{y=J}{\overset{J+n}{\mathrm{\Σ}}}S(x,y)x}{\underset{x=I}{\overset{I+m}{\mathrm{\Σ}}}\underset{y=J}{\overset{J+n}{\mathrm{\Σ}}}S(x,y)},y_c=\frac{\underset{x=I}{\overset{I+m}{\mathrm{\Σ}}}\underset{y=J}{\overset{J+n}{\mathrm{\Σ}}}S(x,y)y}{\underset{x=I}{\overset{I+m}{\mathrm{\Σ}}}\underset{y=J}{\overset{J+n}{\mathrm{\Σ}}}S(x,y)},(S(x,y)\≥T)---\left(4\right)$

In formula by summit (I, J) and length and width be respectively m, the rectangle that n forms need be slightly larger than (needing to guarantee that the pixel count that calculates the speck center is greater than certain numerical value to obtain result preferably) Matching sub-image picture, and add threshold determination while calculating, if certain some pixel value is less than threshold value T, weight is made as 0.

(3) how much range finding models based on pinhole imaging system

The straight line hot spot that the laser plane that the laser assisted distance-finding method can send by light source forms on testee is found range.Below will describe in detail this model of finding range.

As shown in Figure 3,4: establish world coordinate system XwYwZw, CC ₁For camera optical axis, uov is imaging plane, and optical axis and imaging plane meet at photocentre C ₀, image coordinate is (u ₀, v ₀), make the relative level distance of video camera and tested point as to be measured.

L is LASER Light Source, supposes optical axis CC ₁Be parallel to the Yw axle, imaging plane uov is perpendicular to surface level XwOwYw, ou, and ov is parallel to respectively axle Xw and Zw; As shown in Figure 2, lasing light emitter L is positioned at the camera top, and distance is d, laser plane and optical axis CC that order is launched ₁Angle be θ ₀, and the angle of the intersection of laser plane and surface level XwOwYw and axle Xw can not be 0.

If laser plane is radiated on the different vertical plane in two positions, emission forms two wire specks, easily gets an A on one therein, makes the projection A on an A plane of delineation ₀(u _A0, v _A0) horizontal ordinate and photocentre C ₀Identical, and cross some A and be L ' perpendicular to the Yw axle in laser plane; Get a B on another speck, the projection on the plane of delineation is B ₀(u _B0, v _B0), LB and L ' meet at B ', and cross B and make L ' ' and be parallel to L '; Can establish L ' is all α with the angle of the coaxial Xw of L ' ' simultaneously.

Cross L, C does respectively two planes that are parallel to surface level XwOwYw, and some A, B, B ' are respectively A in the projection on these two planes ₁And C ₁, B ₁And B ₂, B ₁' and B ₂', B ₂Be projected as B at the plane of delineation ₂₀(u _B20, v _B20).Make ∠ ALA ₁For θ, ∠ BLB ₁(∠ B ' LB ₁') be θ ', ∠ B ₁LA ₁(∠ B ₂CC ₁) be β.

LB ₁As to be measured be the relative level distance of photocentre C and tested point B, be below concrete derivation:

At first tested point A is asked to distance, can be by LC (A at this ₁C ₁), α, the θ known quantity during as erecting equipment, at Δ ACC ₁In:

$\frac{A_0{C}_0}{{\mathrm{AC}}_1}=\frac{{\mathrm{CC}}_0}{{\mathrm{CC}}_1}---\left(5\right)$

Wherein can directly try to achieve AC ₁, A ₀C ₀And CC ₀Value:

$\left\{\begin{array}{c}{\mathrm{AC}}_1={\mathrm{AA}}_1+A_1{C}_1={\mathrm{CC}}_1\tan \mathrm{\θ}+d\\ A_0{C}_0=(v_{A0}-v_{C0})\mathrm{dy}\\ {\mathrm{CC}}_0=f\end{array}\right.---\left(6\right)$

To in three equation substitutions (5) of (6), can obtain:

${\mathrm{CC}}_1=\frac{d}{m-\tan \mathrm{\θ}},m=\frac{(v_{A0}-v_{C0})}{\mathrm{ay}}---\left(7\right)$

By above step, about the information of tested point A, all can obtain, tested point B that just can be more complicated to solution procedure as condition is calculated.

At Δ B ₂' CC ₁In:

$\tan \mathrm{\β}\frac{B_2{C}_0}{{\mathrm{CC}}_0}\stackrel{0}{=}\frac{(u_{B0}-u_{C0})d}{f}=\frac{({\mathrm{xu}}_{B0}-u_{C0})}{\mathrm{ax}}---\left(8\right)$

At Δ BCB ₂In:

$\frac{B_0{B}_{20}}{{\mathrm{BB}}_2}=\frac{{\mathrm{CB}}_{20}}{{\mathrm{CB}}_2},$

(9) in, in 4 variablees, only need to obtain except CB ₂The value of 3 amounts or only comprise CB in addition ₂Expression formula, just can obtain CB ₂Result.

B wherein ₀B ₂₀Be the physical distance of 2 pixel coordinates:

$B_0{B}_{20}=(v_{B0}-v_{B20})\mathrm{dy}---\left(10\right)$

(9) BB in ₂Can be expressed as:

${\mathrm{BB}}_2={\mathrm{BB}}_1+B_1{B}_2=d+{\mathrm{BB}}_1---\left(11\right)$

(11) BB in ₁Can be obtained by the following step,

At Δ LBB ₁In:

$\frac{B^{\′}{B}_1^{\′}}{{\mathrm{BB}}_1}=\frac{{\mathrm{LB}}_1^{\′}}{{\mathrm{LB}}_1}---\left(12\right)$

Wherein:

$\left\{\begin{array}{c}{B}^{\′}{B}_1^{\′}={\mathrm{AA}}_1+A_1{B}_1^{\′}\tan \mathrm{\α}={\mathrm{LA}}_1\tan \mathrm{\θ}+A_1{B}_1^{\′}\tan \mathrm{\α}\\ A_1{B}_1^{\′}=C_1{B}_2^{\′}={\mathrm{CC}}_1\tan \mathrm{\β}={\mathrm{LA}}_1\tan \mathrm{\β}\end{array}\right.---\left(13\right)$

${\mathrm{LB}}_1^{\′}=\frac{{\mathrm{LA}}_1}{\cos \mathrm{\β}}=\frac{{\mathrm{CC}}_1}{\cos \mathrm{\β}}---\left(14\right)$

By (13), in (14) substitution (12), obtain:

${\mathrm{BB}}_1={\mathrm{LB}}_1\cos \mathrm{\β}(\tan \mathrm{\θ}+\tan a\tan \mathrm{\β})---\left(15\right)$

(9) CB in ₂₀:

${\mathrm{CB}}_{20}\frac{{\mathrm{CC}}_0}{\cos \mathrm{\β}}=\frac{f}{\cos \mathrm{\β}}---\left(16\right)$

Finally, by (10) (11) (15) (16) substitutions (9), can finally obtain:

${\mathrm{CB}}_2=\frac{d}{m-\cos \mathrm{\β}(\tan \mathrm{\θ}+\tan a\tan \mathrm{\β})},m=\frac{(v_{B0}-v_{B20})\cos \mathrm{\β}}{\mathrm{ay}}---\left(17\right)$

From formula, (21) can the results are as follows, only need to know the focal length of video camera and the physical size of image pixel, just can be in the hope of speck to be measured and video camera relative distance.

According to formula (17), when β trends towards 0, formula becomes:

${\mathrm{CB}}_2=\frac{d}{m-\tan \mathrm{\θ}},m=\frac{(v_{B0}-v_{B20})}{\mathrm{ay}}---\left(18\right)$

Find that formula (18) and formula (7) are very similar, just the ordinate difference of tested point image coordinate.According to these characteristics, can obtain such description, as β, close to 0 the time, the laser plane of launching draws in becomes laser beam, and the planar Ranging model is degenerated becomes the range finding model of the single imaging point that laser beam is formed.

The video camera used is demarcated, need be obtained the following internal reference of shooting:

$K=\left[\begin{array}{ccc}\mathrm{ax}& 0& u_{C0}\\ 0& \mathrm{ay}& v_{C0}\\ 0& 0& 1\end{array}\right]$

While adopting the laser plane distance-finding method, camera, LASER Light Source are fixed on carrier, as shown in Figure 2, wherein photocentre and light source distance are d in installation site.

(4) error analysis and correction

Owing to there being systematic error, need to the error of system be compensated.Below with alignment error, be compensated for as example, the spacing d of camera and lasing light emitter and angle theta and α compensated.

4.1 the laser plane range error is analyzed

Formula (18) makes CB ₂Respectively d, θ and α are asked respectively to local derviation:

$\left\{\begin{array}{c}\frac{\∂{\mathrm{CB}}_2}{\∂d}=\frac{1}{m-\cos \mathrm{\β}(\tan \mathrm{\θ}+\tan \mathrm{\α}\tan \mathrm{\β})}\\ \frac{\∂{\mathrm{CB}}_2}{\∂\mathrm{\θ}}=\cos \mathrm{\β}\frac{d}{{[m-\cos \mathrm{\β}(\tan \mathrm{\θ}+\tan \mathrm{\α}\tan \mathrm{\β})]}^2}\frac{1}{{\cos }^2\mathrm{\θ}}\\ \frac{\∂{\mathrm{CB}}_2}{\∂\mathrm{\α}}=\cos \mathrm{\β}\tan \mathrm{\β}\frac{d}{{[m-\cos \mathrm{\β}(\tan \mathrm{\θ}+\tan \mathrm{\α}\tan \mathrm{\β})]}^2}\frac{1}{{\cos }^2\mathrm{\α}}\end{array}\right.---\left(19\right)$

Due to d, θ and α separate, therefore according to the systematic error composition principle:

$\mathrm{\Δ}{\mathrm{CB}}_2=\mathrm{\Δd}\frac{\∂{\mathrm{CB}}_2}{\∂d}+\mathrm{\Δ\θ}\frac{\∂{\mathrm{CB}}_2}{\∂\mathrm{\θ}}+\mathrm{\Δ\α}\frac{\∂{\mathrm{CB}}_2}{\∂a}---\left(20\right)$

4.2 error correction

Using Δ d, Δ θ and Δ α unknown parameter in measurement model, carry out nonlinear fitting with True Data, solve corrected d, θ and α value, by the modified value substitution range finding model obtained, the range finding model after being proofreaied and correct.

(5) matrixing storage

From above-mentioned steps, can learn, once each parameter of range finding model is definite, the result that each coordinate of image is brought into after the range finding model also no longer changes, corresponding one by one with image coordinate itself.For can the quick obtaining range information, can be after the model that obtains accurately the finding range disposable distance value that calculates all image coordinate (image coordinate need be brought into according to a fixed step size range finding model in), and by its matrixing storage.Follow-up needs to detect speck coordinate to be measured and just can directly obtain distance value.

(6) environmental map vector quantization

According to the laser line segment of known definite distance, by region growing method, the Image Segmentation Using that vision sensor is obtained, thus obtain the image segmentation result with range information.The boundary sections that will have in the image of range information is carried out vector quantization, and realizes the reconstruct of indoor environment with the vector quantization figure, thereby obtains the Local Vector environment space with range information.

(7) information fusion

After having obtained the Local Vector environment space, movable information in conjunction with the output of the Inertial Measurement Unit on motion carrier, carry out information fusion by means of filtering, orient the carrier self-position, and obtain accurate carrier pose change information, and just can realize building the global context space after merging accordingly local space, realize real-time independent navigation function.

So far, by the motion carrier vision navigation method idiographic flow auxiliary based on laser plane, finish.

Claims (2)

1. a motion carrier vision navigation method auxiliary based on laser plane, is characterized in that, comprises the steps:

Step 1: by by being arranged on LASER Light Source on motion carrier and vision sensor as range finding and environment construction system, utilize LASER Light Source to send laser plane, produce and reflect to form the wire speck on the testee of carrier the place ahead, then take and obtain the visual pattern that comprises this wire speck by vision sensor;

Step 2: determine one group of speck to be measured and coordinate range thereof in step 1 gained visual pattern, utilize the stencil matching method, in image, the position of rectangle speck to be measured in image determined in search, wherein:

$R(i,j)=\frac{\underset{m=1}{\overset{M}{\mathrm{\Σ}}}\underset{n=1}{\overset{N}{\mathrm{\Σ}}}(S^{i,j}(m,n)\×T(m,n))}{\underset{m=1}{\overset{M}{\mathrm{\Σ}}}\underset{n=1}{\overset{N}{\mathrm{\Σ}}}{\left(S^{i,j}(m,n)\right)}^2}$

Described template and subimage size are all the rectangle of M * N pixel; (i, j) is the reference point coordinate, represents the subimage position; For masterplate, it is normal value; For variable, with reference point, change;

Represent masterplate and the subimage degree of correlation, when subimage masterplate and masterplate similarity maximum, can obtain the subimage position that reference point is (I, J), i.e. rectangle speck to be measured position;

Step 3: choose the brightness of rectangle speck to be measured as weights, by gray scale weights method, to each pixel weighted sum of rectangle speck, draw the centre coordinate of rectangle speck after average;

Step 4: according to the relative position of the vision sensor on carrier in step 1 and lasing light emitter, according to the pinhole imaging system principle, obtain the range finding model how much, how much range finding models are as follows:

$S=\frac{d}{m-\cos \mathrm{\β}(\tan \mathrm{\θ}+\tan \mathrm{\α}\tan \mathrm{\β})},m=\frac{(v_{B0}-v_{B20})\cos \mathrm{\β}}{\mathrm{ay}}$

Wherein, d is LASER Light Source and camera light distance in the heart; α, β, θ installs angle for each, and ay is focal length and pixel physical size ratio, v _B0, v _B20For each auxiliary point image coordinate;

Step 5: according to parameter and the known conditions in the model of finding range for how much in step 4, in conjunction with the error composition principle, the reason that affects distance accuracy is analyzed, in conjunction with experimental data, produce the parameter of error in compensation range finding model, the range finding model after being proofreaied and correct;

Step 6: while carrying out laser ranging, the range finding model after proofreading and correct by the speck centre coordinate substitution to be measured of obtaining in step 3, obtain the result of finding range at every turn;

Step 7: the laser line segment according to known definite distance, utilize region growing method, visual pattern is cut apart, thereby obtained the image segmentation result with range information;

Step 8: the image segmentation result that will have range information carries out vector quantization, and realizes environment reconstruct with the vector quantization figure, after each laser scanning, all can obtain the Local Vector environment space with range information;

Step 9: in conjunction with the Inertial Measurement Unit on motion carrier, and the Local Vector environment space that obtains of step 8, carry out many information fusion by means of filtering, orient the carrier self-position and build the global context space, realize real-time independent navigation function.

2. the motion carrier vision navigation method auxiliary based on laser plane as claimed in claim 1, it is characterized in that, in described step 6, before navigation, directly bring image coordinate into the range finding model by a fixed step size, the result obtained is carried out to the matrixing storage, obtain in navigation procedure after facula position and directly extract corresponding distance value, to obtain fast the actual range of laser beam loca, obtain the result of finding range.

Images