CN103424112B - A kind of motion carrier vision navigation method auxiliary based on laser plane - Google Patents

Abstract

The present invention discloses a kind of motion carrier vision navigation method auxiliary based on laser plane, comprise build range finding and environmental system, determine the bright spot of rectangle to be measured position in the picture, bright spot of determining rectangle centre coordinate, ask for geometry range finding model, correction range finding model, segmentation visual pattern, the Local Vector environment space determining to have range information, orient carrier self-position and build the steps such as global context space. The method of the present invention is different from traditional motion carrier independent navigation middle distance acquisition methods, introduce laser plane as supplementary means, and in conjunction with single or multiple vision sensor, make full use of the bright spot of wire that laser plane reflects to form on irradiated object as cooperation measuring distance of target, effectively make use of the geometric relationship of vision sensor and LASER Light Source, reach the requirement of distance calculation accuracy, success ratio, real-time, it is a kind of navigation method possessing reliability, can be applicable in motion 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, particularly relate to and a kind of adopt laser plane to assist the method carrying out visual token navigation, belong to motion carrier independent navigation field.

Background technology

When lacking high-precision real, external environmental information inputs, all kinds of motion carrier is difficult to realize independent navigation by inertia/GNSS system. Vision technique is utilized to realize motion carrier to the high precision perception of unknown obstacle with hiding, it is possible to effectively to improve the application ability of carrier. Single order or multi-vision visual rely on its structure simple, and the advantage such as low-power consumption, small volume, has broad application prospects in indoor and outdoor navigation field. Studying the motion carrier based on laser assisted to the ranging technology of preceding object thing, estimation collision time, not only effectively for obstacle evades service, can position oneself and exterior three dimensional environment reconstruct offer information simultaneously. Therefore, vision navigation method is significant to the independent navigation of motion carrier fast and accurately.

Vision guided navigation algorithm is carried out many research both at home and abroad, part paper and document have been proposed to be realized by the method for pure vision the independent navigation of carrier, but pure vision guided navigation is affected by environment relatively big and obtaining information is limited; Proposing to employ the multiple measurement means such as vision/inertia/ultrasonic wave in some papers and document, such method is lacking limited efficiency in cooperative target target situation; Some researchists are on the basis employing vision sensor, using lidar as main range finding means, achieving navigation feature, after being aided with other sensors such as inertia, odometer, ultrasonic wave, sonar, effect is very outstanding, but cost and energy penalty are higher. And in general lidar can only realize two dimension range finding, environment construction limited efficiency.

Summary of the invention

Technical problem

The technical problem to be solved in the present invention be the autonomous vision guided navigation algorithm of conventional motion carrier is derived by two dimensional image coordinate draw the space ambiguity problem of 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 pick up camera mounting condition (without the need to the particular requirement such as parallel, vertical), provide a kind of navigation method of visual token auxiliary based on laser plane, autonomous positioning, environment construction, the method necessary hardware structure is simple, volume is little, energy consumption is low, overcomes the restriction being subject to self carrying large volume, big power consumption device due to motion carrier; The range information of acquisition is carried out matrixing storage, and combining image process obtains Vectorgraph, merge Inertial Measurement Unit information realization to carrier self location, the hiding and the acquisition of environmental information of peripheral obstacle.

Technical scheme

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

Step one: by by the LASER Light Source being arranged on motion carrier and vision sensor as range finding and environment construction system. Utilize LASER Light Source to send laser plane, carrier front testee produces reflect to form the bright spot of wire, then obtain, by vision sensor shooting, the visual pattern comprising the bright spot of this wire;

Step 2: determine one group of bright spot to be measured and coordinate range thereof in step one gained visual pattern, utilize stencil matching method, in image, the bright spot of rectangle to be measured position in the picture is 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 subgraph size are all the rectangle of M �� N pixel; (i, j) is reference point coordinate, represents subgraph position;For masterplate, it it is constant value;For variable, change with reference point;Represent masterplate and the subgraph degree of correlation, when subgraph masterplate and masterplate similarity are maximum, the subgraph position that reference point is (I, J) can be obtained, i.e. the bright spot position of rectangle to be measured;

Step 3: the brightness choosing the bright spot of rectangle to be measured, as weights, by gray scale weights method, to each pixel weighted sum of the bright spot of rectangle, draws the centre coordinate of the bright spot of rectangle 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, the reference point coordinate of the bright spot of rectangle that (I, J) obtains for step 3, S^i,j(m, n) is weight, and T is weight threshold, if S^i,j(m, n) is less than T and is then set to 0, obtains the bright spot centre coordinate of rectangle after weighted mean;

Step 4: according to the relative position of the vision sensor on carrier in step one and laser source, obtains geometry range finding model according to pinhole imaging system principle, and geometry range finding model is 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; ��, ��, �� are each installation angle, and ay is focal length and pixel physics dimension ratio, v_B0,v_B20For each auxiliary point image coordinate;

Step 5: the parameter in model of finding range according to geometry in step 4 and known condition, in conjunction with error estimate formula principle, the reason affecting distance accuracy is analyzed, Binding experiment data, compensate the parameter producing error in range finding model, the range finding model after being corrected;

Step 6: when every time carrying out laser ranging, substitutes into the range finding model after correction by the to be measured bright spot centre coordinate by obtaining in step 3, obtains range finding result; Or before navigation, directly bring image coordinate into range finding model by a fixed step size, the result obtained is carried out matrixing storage, navigation procedure obtains the distance value that extracting directly after facula position is corresponding, to obtain the actual range of laser beam place point fast;

Step 7: according to the known laser line segment determining distance, utilize region growing method, visual pattern is split, thus obtain the image segmentation result with range information;

Step 8: the image segmentation result with range information is carried out vector, and realize environment reconstruct with vectorizations algorithms. The Local Vector environment space with range information all can be obtained after each laser scanning;

Step 9: in conjunction with the Inertial Measurement Unit on motion carrier, and the local environment space that step 8 obtains, carry out Multi-information acquisition by means of filtering, orients carrier self-position and builds global context space, it is achieved real-time independent navigation function.

In the method for the present invention, the first step adopts the method for laser assisted, introduce laser plane as supplementary means, and in conjunction with vision sensor, make full use of the bright spot of wire that laser plane reflects to form on irradiated object as cooperation measuring distance of target, can solve in some other visual token algorithms, drawn the space ambiguity problem existing for three-dimensional world coordinate or the less problem being difficult to range finding of quantity of information by two dimensional image coordinate, LASER Light Source is waited particular requirement with the installation position of pick up camera without the need to parallel by this distance-finding method. The range information obtained is carried out the efficiency that matrixing storage can improve the actual range obtaining laser beam place point by the 6th step. 7th step and the 8th step obtain vector environment space by region growing and vector, can effectively reduce local environmental information amount, can realize the independent navigation of motion carrier after the 9th step information fusion.

Useful effect

The method of the present invention has following useful effect:

(1) it is different from traditional motion carrier independent navigation middle distance acquisition methods, introduce laser plane as supplementary means, and in conjunction with single or multiple vision sensor, make full use of the bright spot of wire that laser plane reflects to form on irradiated object as cooperation measuring distance of target, and the range information obtained can be carried out matrixing storage by this method, need not calculate when bright spot being detected, directly utilize matrix to extract the distance measurements corresponding with image coordinate. Computing amount is little, separates evaluation time short, is simple and easy to realize, and the range information of acquisition is stablized, the not problem of Existential Space ambiguity.

(2), when device is installed, the installation position of pick up camera and light source is not had particular requirement, does not require that the laser plane sent must be parallel with camera optical axis, or vertical with carrier levels face, reduce installation and requirement is set.

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

(4) by the bright spot of detection laser, in conjunction with region growing, can effectively detect out all kinds of obstacle in carrier front, contribute to realizing avoidance and environment reconstruct, more simpler than the method for pure vision effective.

Generally speaking, present method reaches the requirement of distance calculation accuracy, success ratio, real-time, is a kind of navigation method possessing reliability, can be applicable in motion carrier autonomous navigation system.

Accompanying drawing explanation

Fig. 1 is the overall flow figure of 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 sciagraph.

Embodiment

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

Step one: by by the LASER Light Source being arranged on motion carrier and vision sensor as range finding and environment construction system. Utilize LASER Light Source to send laser plane, carrier front testee produces reflect to form the bright spot of wire, then obtain, by vision sensor shooting, the visual pattern comprising the bright spot of this wire; Step 2: determine one group of bright spot to be measured and coordinate range thereof in step one gained image, utilize stencil matching method, in image, the bright spot of rectangle to be measured position in the picture is 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 subgraph size are all the rectangle of M �� N pixel; (i, j) is reference point coordinate, represents subgraph position;For masterplate, it it is constant value;For variable, change with reference point;Represent masterplate and the subgraph degree of correlation, when subgraph masterplate and masterplate similarity are maximum, the subgraph position that reference point is (I, J) can be obtained, i.e. the bright spot position of rectangle to be measured;

Step 3: the brightness choosing the bright spot of rectangle to be measured, as weights, by gray scale weights method, to each pixel weighted sum of the bright spot of rectangle, draws the centre coordinate of the bright spot of rectangle 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, the reference point coordinate of the bright spot of rectangle that (I, J) obtains for step 3, S^i,j(m, n) is weight, and T is weight threshold, if S^i,j(m, n) is less than T and is then set to 0, obtains the bright spot centre coordinate of rectangle after weighted mean;

Step 4: according to the relative position of the vision sensor on carrier in step one and laser source, obtains geometry range finding model according to pinhole imaging system principle, and geometry range finding model is 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; ��, ��, �� are each installation angle, and ay is focal length and pixel physics dimension ratio, v_B0,v_B20For each auxiliary point image coordinate;

Step 5: the parameter in model of finding range according to geometry in step 4 and known condition, in conjunction with error estimate formula principle, the reason affecting distance accuracy is analyzed, Binding experiment data, compensate the parameter producing error in range finding model, the range finding model after being corrected;

Step 6: when every time carrying out laser ranging, substitutes into the range finding model after correction by the to be measured bright spot centre coordinate by obtaining in step 3, obtains range finding result; Or before navigation, directly bring image coordinate into range finding model by a fixed step size, the result obtained is carried out matrixing storage, navigation procedure obtains the distance value that extracting directly after facula position is corresponding, to obtain the actual range of laser beam place point fast;

Step 7: according to the known laser line segment determining distance, utilize region growing method, visual pattern is split, thus obtain the image segmentation result with range information.

Step 8: the image segmentation result with range information is carried out vector, and realize environment reconstruct with vectorizations algorithms. The Local Vector environment space with range information all can be obtained after each laser scanning.

Step 9: in conjunction with the Inertial Measurement Unit on motion carrier, and the local environment space that step 8 obtains, carry out Multi-information acquisition by means of filtering, orients carrier self-position and builds global context space, it is achieved real-time independent navigation function.

Hereinafter main performing step the present embodiment method related to is described further:

(1) determination of the bright spot position of laser

Laser source sends after optical plane is mapped to objects in front, owing to forming reflection therefore can produce the bright spot of line-like respectively. And due to brightness higher, namely if image is converted into 8 gray-scale maps, the pixel value of spot center part can reach maximum 255; If from aberration angle analysis, it is assumed that what send is red laser, then the red color component value in the bright spot of laser is higher than green and blue component. When this kind possesses certain priori, stencil matching is a kind of feasible method. Preparation work before stencil matching comprises and image carries out filtering, the operation such as level and smooth, remove portion noise jamming.

It should be noted that, after laser plane reflects on objects in front, a bright spot of wire can be formed. But owing to the position of each irradiated object is different, in most cases the bright spot of wire can not be a complete straight line, there will be interrupted situation, project after in image plane, can not be a continuous print straight line equally, now can determine some rectangle object points to be measured and image horizontal stroke (indulging) coordinate range thereof according to actual demand on the interrupted straight line of each bar, extract the accurate position of rectangle tested point afterwards again.

Assuming that masterplate is T, size is M �� N, and searched figure is that S(length and width are all more than or equal to masterplate), the search graph covered by masterplate is subgraph S^i,j, subgraph size is identical with masterplate. Wherein (i, j) is subgraph top left co-ordinate, is reference point.

Can by doing differ from by masterplate and reference drawing respective pixel and sue for peace, 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 it is constant value;For variable, represent the energy of subgraph, change with reference point;Representing masterplate and the subgraph degree of correlation, if when both mate mutually, value is maximum, it is possible to thus obtain 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 subgraph similarity are more big, then R (i, j) is more close to 1. When subgraph masterplate and masterplate similarity are maximum, the subgraph position that reference point is (I, J) can be obtained, i.e. the bright spot position of rectangle to be measured.

(2) Facula Center Location

After determining the bright spot position of rectangle to be measured that laser is formed on testee, the centre coordinate of the bright spot of rectangle in image coordinate can be extracted on this basis. When processing image problem, it is determined that in image, coordinate can only be accurate to pixel scale, generally can meet accuracy requirement. If when measured target is away from carrier positions, often changes a pixel, bigger deviation in range will be formed. Therefore need to consider sub-pix problem in Facula Center Location process, to improve distance accuracy.

Based on considering of the aspect such as complexity and real-time, adopt grey scale centre of gravity method, imaging characteristic according to bright spot region, the gray-scale value at Liang Ban center is maximum, frontier area gray-scale value is lower, so using the pixel value of each coordinate as weight, centre coordinate can be obtained, it is achieved the solving precision of sub-pix after weighted mean.

$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)$

By summit (I in formula, J) and length and width be respectively m, the rectangle that n is formed need to slightly larger than (needing the pixel count ensureing to calculate Liang Ban center to be greater than certain numerical value to obtain good result) Matching sub-image picture, and add threshold determination when calculating, if certain some pixel value is less than threshold value T, then weight is set to 0.

(3) based on the geometry range finding model of pinhole imaging system

The straight line hot spot that the laser plane that laser assisted distance-finding method sends by light source is formed on testee is found range. Model of below this being found range illustrates in detail.

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

L is LASER Light Source, it is assumed that optical axis CC₁Being parallel to Yw axle, imaging plane uov is perpendicular to horizontal plane XwOwYw, and ou, ov are parallel to axle Xw and Zw respectively; As shown in Figure 2, laser source L is positioned at above camera, and distance is d, the laser plane that order is launched and optical axis CC₁Angle be ��₀, and the angle of the friendship line of laser plane and horizontal plane XwOwYw and axle Xw can not be 0.

If laser plane is radiated on the different vertical surface in two positions, launches and form two bright spots of wire, easily one is got an A wherein, make the projection A in an A image plane₀(u_A0,v_A0) X-coordinate and photocentre C₀Identical, and in laser plane, cross a some A be L ' and be perpendicular to Yw axle; Getting a B on another bright spot, projection on the image plane is B₀(u_B0,v_B0), LB and L ' meets at B ', and crosses B and make L ' ' and be parallel to L '; The angle of the coaxial Xw of L ' and L ' ' can be set all as �� simultaneously.

Crossing L, C makes the plane that two are parallel to horizontal plane XwOwYw respectively, and some A, B, B ' it is respectively A in the projection of these two planes₁And C₁��B₁And B₂��B₁' and B₂', B₂It is B in image plane projection₂₀(u_B20,v_B20). Make �� ALA₁For ��, �� BLB₁(�� B ' LB₁') it is �� ', �� B₁LA₁(�� B₂CC₁) it is ��.

LB₁As the i.e. relative horizontal throw of photocentre C with tested point B to be measured, it is concrete derivation below:

First tested point A is sought distance, can by LC (A at this₁C₁), ��, �� as known quantity during 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)$

Three equatioies of (6) are substituted in (5) and can obtain:

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

By above step, the information about tested point A all can obtain, and just can be calculated by the tested point B that the process that solve is more complicated as condition.

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, 4 variablees only need to obtain except CB₂In addition 3 amount values or only comprise CB₂Expression formula, so that it may obtain CB₂Result.

Wherein B₀B₂₀It is 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 obtain 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), (14) substitute in (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 (10) (11) (15) (16) are substituted in (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, namely only needs to know the focal length of pick up camera and the physics size of image pixel, so that it may in the hope of bright spot to be measured and pick up camera relative distance.

According to formula (17), when �� trends towards 0, formula turns into:

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

Finding that formula (18) and formula (7) are very similar, just the ordinate zou of tested point image coordinate is different. According to this feature, it is possible to obtain such description, when �� is close to 0, the laser plane launched draws in becomes laser beam, and planar Ranging model degradation becomes the range finding model to the single imaging point that laser beam is formed.

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

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

When adopting laser plane distance-finding method, being fixed on carrier by camera, LASER Light Source, install position as shown in Figure 2, wherein photocentre and light source distance are d.

(4) errot analysis and correction

Owing to there is systematic error, it is necessary to the error of system is compensated. It is compensated as example with installation error below, namely the spacing d of camera and laser source and angle theta and �� is compensated.

4.1 laser plane range finding errot analyses

Formula (18) makes CB₂Respectively d, �� and �� are asked respectively and partially lead:

$\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)$

Owing to d, �� and �� are separate, therefore according to 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 d, �� and �� value revised, the modified value obtained is substituted in range finding model, the range finding model after being corrected.

(5) matrixing stores

Can learn from above-mentioned steps, once range finding each parameter of model determine, each coordinate of image bring into range finding model after result also no longer change, one_to_one corresponding own with image coordinate. In order to range information can be obtained fast, it is possible to obtaining accurately finding range the disposable distance value (image coordinate need to be brought into according to a fixed step size in range finding model) calculating all image coordinate after model, and its matrixing is stored. Follow-up need to be detected bright spot coordinate to be measured and just can directly obtain distance value.

(6) environment map vector

According to the known laser line segment determining distance, by region growing method, the image that vision sensor obtains is split, thus obtain the image segmentation result with range information. Boundary sections in the image with range information is carried out vector, and realizes the reconstruct of indoor environment with vectorizations algorithms, thus obtain the Local Vector environment space with range information.

(7) information fusion

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

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

Claims (2)

1. the motion carrier vision navigation method assisted based on laser plane, it is characterised in that, comprise the steps:

Step one: by by the LASER Light Source being arranged on motion carrier and vision sensor as range finding and environment construction system, LASER Light Source is utilized to send laser plane, motion carrier front testee produces reflect to form the bright spot of wire, then obtain, by vision sensor shooting, the image comprising the bright spot of this wire;

Step 2: determine one section of bright spot of wire and coordinate range thereof in step one gained image, utilize stencil matching method, in image, this section of bright spot of wire position in the picture is determined in search, wherein:

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

Described template and subgraph size are all the rectangle of M �� N pixel, and subgraph is the image covered by template, and this rectangle only comprises one section of bright spot of above-mentioned wire; (i, j) is reference point coordinate, represents subgraph position;For masterplate, it it is constant value;For variable, change with reference point;Represent masterplate and the subgraph degree of correlation, when subgraph masterplate and masterplate similarity are maximum, the subgraph position that reference point is (I, J) can be obtained, i.e. the bright spot position of wire to be measured;

Step 3: the brightness choosing the bright spot of wire to be measured, as weights, by gray scale weights method, to each pixel weighted sum of the bright spot of wire, draws the centre coordinate of the bright spot of wire after average;

Step 4: according to the relative position of the vision sensor on motion carrier in step one and laser source, obtains geometry range finding model according to pinhole imaging system principle, and geometry range finding model is as follows:

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

Wherein, d is LASER Light Source and camera light distance in the heart; ��, ��, �� are each installation angle, and ay is focal length and pixel physics dimension ratio, v_B0,v_B20For each auxiliary point image coordinate;

Step 5: the parameter in model of finding range according to geometry in step 4 and known condition, in conjunction with error estimate formula principle, the reason affecting distance accuracy is analyzed, Binding experiment data, compensate the parameter producing error in range finding model, the range finding model after being corrected;

Step 6: when every time carrying out laser ranging, substitutes into bright for the wire by obtaining in step 3 spot centre coordinate the range finding model after correction, obtains range finding result;

Step 7: according to the known laser line segment determining distance, utilize region growing method, visual pattern is split, thus obtain the image segmentation result with range information;

Step 8: the image segmentation result with range information is carried out vector, and realize environment reconstruct with vectorizations algorithms, 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 Vector environment space that step 8 obtains, carry out Multi-information acquisition by means of filtering, orient motion carrier self-position and build global context space, it is achieved real-time independent navigation function.

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