CN101067557A - Environment sensing one-eye visual navigating method adapted to self-aid moving vehicle - Google Patents

Abstract

The invention is suitable to the independent motion vehicles environment sensation monocular visual navigation method, first surveys the camera distortion parameter, puts the camera on the independent motion vehicles, determines transformation relation from image plane coordinate system to world coordinate system; then records current car body posture, gathers the picture in the current car body visible scope, feedback the picture to the picture operation contact surface in order to definite the walk mode of the car body and do distortion correction on the gathering picture, select the path and the path spot on the path, according to the coordinates transformation relations under the world coordinate system put the current position and choice path result to the car body and demonstrate on the graph operation contact surface by the car body width; the car body walks according to the independently evades bonds or the way track way till the current visible scope last way spot, and then circling start from the gather picture to the final object point. The invention applies the monocular measuring technique in the unknown environment the visual navigation.

Description

Be applicable to the monocular vision air navigation aid of the environment sensing of autonomous moving vehicle

Technical field

The present invention relates to a kind of monocular vision air navigation aid that is applicable to the environment sensing of autonomous moving vehicle.

Background technology

A gordian technique of autonomous moving vehicle is the perception to surrounding environment.The three-dimensional coordinate that mainly comprises circumstances not known on every side recovers and the identification of obstacle, thereby guarantees the navigation control system operate as normal, and makes vehicle can walk and arrive the intended target point safely to finish the scientific exploration task.In order to strengthen the environment sensing ability of autonomous vehicle, improve it from the master program ability, thereby realization independent navigation, domestic how tame unit has all carried out broad research to the multi-eye stereo vision technique, but that is that all right is ripe because theory on computer vision is in the research of autonomous moving vehicle application facet, the precision of algorithm and the difficult problem of coupling are solved at all, produce bottleneck on engineering is used.

Than binocular stereo vision, the method for utilizing single camera to navigate according to image presentation feature is also studied a lot of, has obtained the application under some actual scenes.But because the monocular three-dimensional measurement is incomplete theoretically, lack an initial conditions, so being used for the algorithm of car body navigation, seen monocular is mostly to yet there are no application in semi-structured or known scene about monocular and measure the relevant report of on the autonomous roaming car of circumstances not known, navigating.

Chinese patent publication number CN1569558, open day on January 26th, 2005, invention and created name is the mobile robot visual air navigation aid based on the image appearance feature, the scene image that this application case discloses a kind of robot present position that obtains by the single width camera is finished the method for navigation, and this method must obtain the known scene map that builds in advance.Chinese patent publication number CN1873656, open day on Dec 6th, 2006, invention and created name is the natural target detection method in the robot visual guidance, this application case discloses a kind ofly finishes the method navigate of extracting to the natural target in the image, this method need be before off-line state be got off walking, and the model of setting up natural target just can carry out.

The content of invention

Technology of the present invention is dealt with problems and is: overcome the deficiencies in the prior art, a kind of monocular vision air navigation aid that is applicable to the environment sensing of autonomous moving vehicle is provided, this method is applied to monocular measuring method in the circumstances not known, by real-time correction track path, assurance reaches the precision of impact point, and, guarantee the safety traffic of car body by the automatic obstacle avoiding walking manner.

Technical solution of the present invention is: be applicable to the monocular vision air navigation aid of the environment sensing of autonomous moving vehicle, it is characterized in that comprising the following steps:

(1) measures the intrinsic parameter that camera distorts, after the measurement camera is installed on the autonomous moving vehicle, determine the transformational relation of photo coordinate system to world coordinate system;

(2) record is when the front vehicle body attitude, in images acquired in the front vehicle body visual range, and the image of gathering is back to pattern manipulation interface, select the car body walking manner according to target location and image situation, then the image that collects is carried out distortion correction, make the image after the correction satisfy the pinhole imaging system principle, according to the path point on selection walking path of the image behind the distortion correction and the path, the path point of car body current location and selection is converted under the world coordinate system according to the transformational relation of the described photo coordinate system of step (1) to world coordinate system, the result issues car body with conversion, and the path the selected form with width of the carbody is presented on the pattern manipulation interface;

(3) car body when car body in the step (2) selects path point to follow the tracks of walking manner, is revised the coordinate figure of path point under world coordinate system, last path point in arriving current visual range along drafting the path walking in real time;

When car body is selected the automatic obstacle avoiding walking manner in the step (2), the camera pitching is taken pictures to current scene to specified angle, barrier in the scene is carried out Region Segmentation, the cognitive disorders zone, estimation obstacle height, planning gets around the path of obstacle, and car body is according to the path walking of planning, last path point in arriving current visual range;

(4) in car body arrives current visual range, during last path point, begin circulation from step (2) and carry out, arrive the final goal position until car body.

Also regular images acquired in the car body walking process in the described step (3), after image is back to pattern manipulation interface image is carried out distortion correction, and will be according to the path point of car body correction or planning being presented on the pattern manipulation interface when front vehicle body position, attitude, when car body does not have avoiding obstacles, car body is sent the instruction of stopping in emergency.

The present invention compared with prior art beneficial effect is:

(1) the present invention adopts the monocular vision air navigation aid to compare with binocular stereo vision, simplifies and calculates, requirement of real time, good stability.

(2) the present invention with monocular measuring method be applied in the circumstances not known with existing known scene map under the monocular air navigation aid relatively, be applicable to non-structured unknown scene, effectively improve the precision of following the tracks of.

(3) the present invention is owing to adopt based on the distant method of operating that quantizes figure, and the scene grid information that shows the track of planning and standard at distant end is on image, and handled easily has better guaranteed security.

Description of drawings

Fig. 1 is the inventive method process flow diagram;

Fig. 2 is the synoptic diagram of image distortion correction of the present invention;

Fig. 3 is the synoptic diagram of monocular of the present invention projection coordinate conversion;

Fig. 4 is the real-time synoptic diagram of revising of monocular projection coordinate of the present invention;

Fig. 5 independently carries out the schematic diagram of obstacle level identification for the present invention;

Fig. 6 is the design sketch of automatic obstacle avoiding of the present invention;

Fig. 7 quantizes the design sketch of graphic presentation for the present invention.

Embodiment

As shown in Figure 1, be the inventive method process flow diagram, concrete steps are as follows:

(1) measures the intrinsic parameter that camera distorts, after the measurement camera is installed on the autonomous moving vehicle, determine the transformational relation of photo coordinate system to world coordinate system;

(2) record is when the front vehicle body attitude, in images acquired in the front vehicle body visual range, and the image of gathering is back to pattern manipulation interface, select the car body walking manner according to target location and image situation, then the image that collects is carried out distortion correction, make the image after the correction satisfy the pinhole imaging system principle, according to the path point on selection walking path of the image behind the distortion correction and the path, the path point of car body current location and selection is converted under the world coordinate system according to the transformational relation of the described photo coordinate system of step (1) to world coordinate system, the result issues car body with conversion, and the path the selected form with width of the carbody is presented on the pattern manipulation interface;

As shown in Figure 2, o-X _Lc-Y _Lc-Z _LcRepresent camera coordinates system, u-v-f represents photo coordinate system, and P is an actual point, and P ' is the corresponding point of P in the picture plane, and captured image satisfies the principle of pinhole imaging system, can derive the following formula that satisfies pixel coordinate and camera photocentre coordinate.

$\left\{\begin{array}{c}\frac{\mathrm{udx}}{f}=\frac{y_{\mathrm{lc}}}{x_{\mathrm{lc}}}\\ \frac{\mathrm{vdy}}{f}=\frac{z_{\mathrm{lc}}}{x_{\mathrm{lc}}}\end{array}\right.$

If x _Lc=t obtains the parametric equation of imaging formula:

$\left[\begin{array}{c}{x}_{\mathrm{lc}}\\ y_{\mathrm{lc}}\\ z_{\mathrm{lc}}\end{array}\right]=\left[\begin{array}{c}1\\ \frac{u\·\mathrm{dx}}{f}\\ \frac{v\·\mathrm{dy}}{f}\end{array}\right]*t$ (formula 1)

Wherein u, v are respectively the horizontal ordinate and the ordinate of pixel, and dx is the physical length of a pixel on the CCD target surface u direction, and dy is the physical length of a pixel on the CCD target surface v direction.

This is the ultimate principle of monocular projection, under little pore model, image coordinate system is projected on the calculative plane, obtains the three-dimensional coordinate of pixel under this plane.But in the application of reality, because the factors such as optical distortion of camera lens, imaging is difficult to satisfy pinhole imaging system.So before the conversion of carrying out above-mentioned formula, must carry out some main distortion parameters to image and proofread and correct, make it satisfy the pinhole imaging system principle as far as possible, apply mechanically above-mentioned formula again and calculate.The present invention mainly considers three kinds of distortion of camera lens: radial distortion, decentering distortion and the distortion of picture plane.Formula is as follows:

x＝(x-x ₀) y＝(y-y ₀)

r ²＝ x ²+ y ²

$\left\{\begin{array}{c}\mathrm{\Δ}{x}_r=K_1\stackrel{\‾}{x}{r}^2+K_2\stackrel{\‾}{x}{r}^4+K_3\stackrel{\‾}{x}{r}^6+\·\·\·\\ \mathrm{\Δ}{y}_r=K_1\stackrel{\‾}{y}{r}^2+K_2\stackrel{\‾}{y}{r}^4+K_3\stackrel{\‾}{y}{r}^6+\·\·\·\end{array}\right.$

$\left\{\begin{array}{c}{\mathrm{\Δx}}_d=p_1(r^2+2{\stackrel{\‾}{x}}^2)+2p_2\stackrel{\‾}{x}\×\stackrel{\‾}{y}\\ \mathrm{\Δ}{y}_d=p_2(r^2+2{\stackrel{\‾}{y}}^2)+2p_1\stackrel{\‾}{x}\×\stackrel{\‾}{y}\end{array}\right.$

$\left\{\begin{array}{c}{\mathrm{\Δx}}_m=b_1\stackrel{\‾}{x}+b_2\stackrel{\‾}{y}\\ {\mathrm{\Δy}}_m=0\end{array}\right.$

$\left\{\begin{array}{c}{x}^{\′}=x+{\mathrm{\Δx}}_r+{\mathrm{\Δx}}_d+{\mathrm{\Δx}}_m\\ y^{\′}=y+{\mathrm{\Δy}}_r+{\mathrm{\Δy}}_d+{\mathrm{\Δy}}_m\end{array}\right.$

Δ x wherein _rΔ y _rBe radial distortion, Δ x _dΔ y _dBe decentering distortion, Δ x _mΔ y _mBe the distortion of picture plane.

Under breadboard accurate known environment, can calibrate above deformation coefficient exactly through optical means, promptly adopt special-purpose photogrammetric optics calibration system, the image that when practical application shooting obtained is through above distortion treatment for correcting, obtains new image and then thinks the principle that satisfies pinhole imaging system.

Through type 1 can be set up the relation between photo coordinate system and the camera coordinates system, practical application for Navigation Control, need be under bodywork reference frame or under certain navigation coordinate system on ground with the coordinate conversion under the camera coordinates system, be called world coordinate system at this, so should clearly describe each coordinate system and the mutual transformational relation thereof of probe vehicles pose.

As shown in Figure 3, be defined as follows coordinate system for pose and the coordinate transformation relation of describing detector:

Wherein:

O _LcX _LcY _LcZ _Lc: be camera coordinates system, initial point is positioned at camera photocentre, X _LcAlong camera light axially before, Z _LcPerpendicular to X _LcUpwards, Y _LcWith X _Lc, Z _LcBecome right-handed system;

O _cX _cY _cZ _c: camera support coordinate system, initial point are positioned at camera support center, Y _cAlong support left, X _cVertical Y _cAnd be positioned at X _LcY _LcIn the plane, place, Z _cWith X _c, Y _cBecome right-handed system;

O _lX _lY _lZ _l: be the mast coordinate system, initial point is positioned at the junction of mast and car body, X _lFor the car body apical axis forward, Y _lVertical X _lLeft, Z _lWith X _l, Y _lBecome right-handed system;

O _bX _bY _bZ _b: be bodywork reference frame, initial point is positioned at car body geometric center, X _bBe car body forward direction, Y _bPoint to the car body left side;

O _rX _rY _rZ _r: be world coordinate system, initial point be the car body barycenter vertically downward with the intersection point of local horizontal coordinates, the same bodywork reference frame of coordinate system direction.

Parameter-definition is as follows simultaneously:

α: the camera angle of pitch;

β: camera crab angle;

S _ABC: for A is that initial point is the translational movement of initial point in the C direction to B;

The camera angle of pitch and crab angle all are that camera coordinates is tied to the rotation matrix of bodywork reference frame under zero the situation.

Step 1 is finished the transformational relation of camera coordinates system to the camera support coordinate system

$\left[\begin{array}{c}{x}_c\\ y_c\\ z_c\end{array}\right]=(\left[\begin{array}{ccc}{r1}_1& {r1}_2& {r1}_3\\ {r1}_4& {r1}_5& {r1}_6\\ {r1}_7& {r1}_8& {r1}_9\end{array}\right]*\left[\begin{array}{c}{x}_{\mathrm{lc}}\\ y_{\mathrm{lc}}\\ z_{\mathrm{lc}}\end{array}\right]+\left[\begin{array}{c}{S}_{\mathrm{lx}}\\ S_{\mathrm{ly}}\\ S_{\mathrm{lz}}\end{array}\right])$

[S _LxS _LyS _Lz]: the camera coordinates initial point is to the translational movement of camera support coordinate system.All be that camera support coordinate system and bodywork reference frame are completely parallel under zero the situation in the camera angle of pitch and crab angle.

Step 2 is finished the conversion that the camera support coordinate is tied to the mast coordinate system

$\left[\begin{array}{c}{x}_l\\ y_l\\ z_l\end{array}\right]=\left[\begin{array}{ccc}\cos \mathrm{\α}\cos \mathrm{\β}& -\sin \mathrm{\β}& \sin \mathrm{\α}\cos \mathrm{\β}\\ \cos \mathrm{\α}\sin \mathrm{\β}& \cos \mathrm{\β}& \sin \mathrm{\α}\sin \mathrm{\β}\\ -\sin \mathrm{\α}& 0& \cos \mathrm{\α}\end{array}\right]*\left[\begin{array}{c}{x}_c\\ y_c\\ z_c\end{array}\right]+\left[\begin{array}{c}{S}_{\mathrm{clx}}\\ S_{\mathrm{cly}}\\ S_{\mathrm{clz}}\end{array}\right]$

Step 3 is finished the conversion that the mast coordinate is tied to bodywork reference frame

$\left[\begin{array}{c}{x}_b\\ y_b\\ z_b\end{array}\right]=\left[\begin{array}{c}{x}_l\\ y_l\\ z_l\end{array}\right]+\left[\begin{array}{c}{S}_{\mathrm{lbx}}\\ S_{\mathrm{lby}}\\ S_{\mathrm{lbz}}\end{array}\right]$

Step 4 is finished the conversion of bodywork reference frame to world coordinate system

According to the definition of world coordinate system this moment, bodywork reference frame is exactly a simple translation to the conversion of world coordinate system, and is as follows

$\left[\begin{array}{c}{x}_r\\ y_r\\ z_r\end{array}\right]=\left[\begin{array}{c}{x}_b\\ y_b\\ z_b\end{array}\right]+\left[\begin{array}{c}0\\ 0\\ H\end{array}\right]$

Step 5, in comprehensive above four steps, progressively iteration converts and obtains the corresponding relation of world coordinate system and camera coordinates system, the overall formula when obtaining issuing path point:

$\left[\begin{array}{c}{x}_r\\ y_r\\ z_r\end{array}\right]=\left(\begin{array}{c}\left[\begin{array}{ccc}\cos \mathrm{\α}\cos \mathrm{\β}& -\sin \mathrm{\β}& \sin \mathrm{\α}\cos \mathrm{\β}\\ \cos \mathrm{\α}\sin \mathrm{\β}& \cos \mathrm{\β}& \sin \mathrm{\α}\sin \mathrm{\β}\\ -\sin \mathrm{\α}& 0& \cos \mathrm{\α}\end{array}\right]*\\ (\left[\begin{array}{ccc}{r1}_1& {r1}_2& {r1}_3\\ {r1}_4& {r1}_5& {r1}_6\\ {r1}_7& {r1}_8& {r1}_9\end{array}\right]*\left[\begin{array}{c}{x}_{\mathrm{lc}}\\ y_{\mathrm{lc}}\\ z_{\mathrm{lc}}\end{array}\right]+\left[\begin{array}{c}{S}_{\mathrm{lx}}\\ S_{\mathrm{ly}}\\ S_{\mathrm{lz}}\end{array}\right])+\left[\begin{array}{c}{S}_{\mathrm{lbx}}+S_{\mathrm{clx}}\\ S_{\mathrm{lby}}+S_{\mathrm{cly}}\\ H+S_{\mathrm{lbz}}+S_{\mathrm{clz}}\end{array}\right]\end{array}\right)$ (formula 2)

Step 6, simultaneous formula (1) and formula (2) just can be resolved and obtain the conversion relational expression of photo coordinate system to world coordinate system.Because the plane of delineation coordinate of two dimension resolves three-dimensional coordinate and lacks an amount, z is proposed _r=0 hypothesis just can be resolved out by the coordinate figure that three-dimensional is definite.But hypothesis can produce error with the situation of reality is inconsistent like this, and error is further compensating in the motion.

(3) as shown in Figure 4, select path point tracking mode walking path point for car body and revise synoptic diagram in real time, because real landform does not conform to the plane of hypothesis, in initial piece image, specify a series of tracing points of detector motion in motion thereafter, all can change, in order to guarantee the precision of track following, revise the coordinate of these tracing points under world coordinate system in real time according to the attitude variation that car body is current, the distance that while, acquisition was walked according to locating information, appear at the rear of car body when the tracing point of revising, show car body this point of having passed by, change into and follow the tracks of the next impact point of revising.The frequency of revising is high more, and the precision that tracing point is followed the tracks of is high more.

Concrete implementation step: provide five signal points among Fig. 4, the path points of five signal points for selecting in the step (2) are according to z this moment _r=0 world coordinates plane issues the initial path point coordinate.In the process that car body is advanced, because the variation of car body attitude and position uses the world coordinates plane of initial position can cause error increasing unalterablely.As shown in the figure, five ray equations of five initial points under camera coordinates system determine that its intersection point with the hypothesis plane is the corresponding three-dimensional coordinate point of this pixel.When the hypothesis plane for when the landform tangent plane of front vehicle body position the time, if tangent line is enough lacked, the coordinate of three-dimensional point can approach actual value gradually.So, revise the z that is associated with the real-time position and attitude of car body according to the change of car body with respect to initial attitude and position _r=0 plane equation utilizes the car body position and the attitude information that record in real time to revise, and finally makes the more approaching real face of land of the coordinate figure of point under world coordinate system, path situation.When intersecting point coordinate is positioned at when the rear of front vehicle body position, think that then this path point passes by, change into and follow the tracks of next path point, last the path point in current visual range.

The coordinate correction formula of path point is as follows:

$\left[\begin{array}{c}{x}_r^{\′}\\ y_r^{\′}\\ z_r^{\′}\end{array}\right]=\left[\begin{array}{ccc}\cos \mathrm{\Δp}& \sin \mathrm{\Δr}\sin \mathrm{\Δp}& \cos \mathrm{\Δ}r\sin \mathrm{\Δp}\\ 0& \cos \mathrm{\Δr}& -\sin \mathrm{\Δr}\\ -\sin \mathrm{\Δp}& \sin \mathrm{\Δr}\cos \mathrm{\Δp}& \cos \mathrm{\Δr}\cos \mathrm{\Δp}\end{array}\right]*(\left(\begin{array}{c}\left[\begin{array}{ccc}\cos \mathrm{\α}\cos \mathrm{\β}& -\sin \mathrm{\β}& \sin \mathrm{\α}\cos \mathrm{\β}\\ \cos \mathrm{\α}\sin \mathrm{\β}& \cos \mathrm{\β}& \sin \mathrm{\α}\sin \mathrm{\β}\\ -\sin \mathrm{\α}& 0& \cos \mathrm{\α}\end{array}\right]*\\ (\left[\begin{array}{ccc}{r1}_1& {r1}_2& {r1}_3\\ {r1}_4& {r1}_5& {r1}_6\\ {r1}_7& {r1}_8& {r1}_9\end{array}\right]*\left[\begin{array}{c}{x}_{\mathrm{lc}}\\ y_{\mathrm{lc}}\\ z_{\mathrm{lc}}\end{array}\right]+\left[\begin{array}{c}{S}_{\mathrm{lx}}\\ S_{\mathrm{ly}}\\ S_{\mathrm{lz}}\end{array}\right])\\ +\left[\begin{array}{c}{S}_{\mathrm{lbx}}+S_{\mathrm{clx}}\\ S_{\mathrm{lby}}+S_{\mathrm{cly}}\\ H+S_{\mathrm{lbz}}+S_{\mathrm{clz}}\end{array}\right]\end{array}\right)-\left[\begin{array}{c}{S}_x^{\′}\\ S_y^{\′}\\ S_z^{\′}\end{array}\right])$ (formula 3)

Wherein, Δ r: when the roll angle of the relative initial position of front vehicle body;

Δ p: when the angle of pitch of the relative initial position of front vehicle body;

S _x', S _y', S _z' for the current location of car body issues coordinate figure under the initial position world coordinate system at the path point, provide by navigator fix information.

Through the real-time correction of car body position in the traveling process and attitude, the tracking accuracy of next path point has been improved more near actual value in initial impact point position.

When current road conditions were judged that car body selects automatic obstacle avoiding to advance mode, concrete steps were as follows:

Step 1 is taken pictures camera pitching specified angle to current scene.

Step 2 according to the characteristic of the barrier gray scale in the scene, adopts the least error dividing method to carry out the Region Segmentation of barrier.

The principle of least error dividing method is based on the half-tone information of image, and the gray scale function of establishing image is f (i), and i is a gray-scale value, is 0～255 in the gray level image of 8bits.Calculate total sample number, distribution average and the variance of two class intensity profile:

$p_0\left(t\right)=\underset{i=\min }{\overset{t}{\mathrm{\Σ}}}f\left(i\right)$ $p_1\left(t\right)=\underset{i=t}{\overset{\max }{\mathrm{\Σ}}}f\left(i\right)$

${\mathrm{\μ}}_0\left(t\right)=\frac{\underset{i=\min }{\overset{t}{\mathrm{\Σ}}}f\left(i\right)*i}{p_0\left(t\right)}$ ${\mathrm{\μ}}_1\left(t\right)=\frac{\underset{i=t}{\overset{\max }{\mathrm{\Σ}}}f\left(i\right)*i}{p_1\left(t\right)}$

${\mathrm{\σ}}_0\left(t\right)=\frac{\underset{i=\min }{\overset{t}{\mathrm{\Σ}}}{[i-{\mathrm{\μ}}_0\left(t\right)]}^{2}f\left(i\right)}{p_0\left(t\right)}$ ${\mathrm{\σ}}_1\left(t\right)=\frac{\underset{i=t}{\overset{\max }{\mathrm{\Σ}}}{[i-{\mathrm{\μ}}_1\left(t\right)]}^{2}f\left(i\right)}{p_1\left(t\right)}$

Min, max are minimum gray value and the gray scale maximal value that occurs in the piece image, and t is the gray threshold of cutting apart.Defining smallest error function then is:

H(t)＝1+2*[p ₀(t)lnσ ₀(t)+p ₁(t)lnσ ₁(t)-2*[p ₀(t)ln?p ₀(t)+p ₁(t)ln?p ₁(t)]]

The H of all t of cycle calculations (t) value is therefrom found out the minimum pairing t value of H (t) value, utilizes the t value that image is carried out binary conversion treatment then.Image after the binaryzation is at first removed the isolated point of extraction, and promptly region area is eliminated the cavity through basic morphology operations corrosion and expansion algorithm again less than the point set of a certain threshold level, and employed structural element is in the program $B=\left[\begin{array}{ccc}1& 1& 1\\ 1& 1& 1\\ 1& 1& 1\end{array}\right].$

Step 3, to the zone that links up that obtains, promptly removing zone behind isolated point and the cavity identifies with area and judges, the effective barrier of zone conduct greater than threshold area, the zone of threshold area can be set according to size of the stone in the internal field and picture characteristics, is taken as 2000 pixels.Roughly estimate the elevation information of barrier, the stone that is higher than maximum height of surmountable obstacle need be kept away barrier as the hazardous location.

As shown in Figure 5, obstacle height principle of calculating figure, the front end face of supposing barrier have maximum height and with the level ground approximate vertical, Δ r in the modification formula 3 and Δ p, the rolling and the angle of pitch of surface level is (though uneven according to landform relative to the earth to change the current attitude of car body into, but the characteristic of general planar), suppose the equation of the earth surface level, obtain after world coordinate system compensated the pitching of relative surface level and roll angle, calculating its length that intercepts according to the lower edge of barrier and upper edge on surface level is Obstacle, subtended angle between the pixel coordinate of lower edge and upper edge correspondence is δ, and the attitude during according to the camera current shooting can obtain lower edge and the sight line of upper edge correspondence and the angle α of surface level _LowAnd α _High, then the high computational formula of barrier is:

Height=the Obstacle of obstacle * tan α _High

This hypothesis has been carried out verification experimental verification to the vertical height thing on the level ground, proves this method principle establishment and can reach degree of precision.In actual applications, because the fluctuating of landform and the irregularly shaped meeting of barrier bring the inaccuracy of measurement, but, can realize that the safety of big obstacle is evaded by consideration to the height allowance.

Step 4, the big obstacle that needs were kept away extracts its four coordinate extreme values under photo coordinate system, generate the envelope rectangle frame by these four coordinate extreme values, Δ r in the modification formula 3 and Δ p, change the current attitude of the car body rolling and the angle of pitch of surface level relative to the earth into, other conversions are identical, these four points have formed a hazardous location scope on surface level, carry out path planning according to hazardous location scope and the visual scope of camera, cook up one and both can keep away the hazardous location, can reach the secure path of impact point again, paths planning method can be for using more Artificial Potential Field method at present, fuzzy logic algorithm, the neural network method, genetic algorithm and grid method etc., the path planning aftercarriage is walked along path planning, last path point in current visual range.

(3) also regular images acquired in the car body walking process, after image is back to pattern manipulation interface image is carried out distortion correction, and will be according to the path point of car body correction or planning being presented on the pattern manipulation interface when front vehicle body position, attitude, when car body does not have avoiding obstacles, car body is sent the instruction of stopping in emergency.

As shown in Figure 6, be the design sketch of obstacle identification of the present invention, wherein white box is the obstacle envelope rectangle of discerning after the Flame Image Process, the first dangerous obstacle in the image that be designated of barrier represented in 1 of the lower left corner.All the other white mesh lines are the horizontal 0.5m of car body current location surface level and the equal space line of vertical 1m.Need to prove, be that image is back to distant end here, and promptly pattern manipulation interface shows, the processing of actual vehicle-mounted end can not show, but the data of gained are with identical here.

In the operating process of distant end, in order to guarantee the security of car body more intuitively, proposed based on the distant operation conception that quantizes figure, and done the technology realization, on the pattern manipulation interface of distant end, show the scene grid information of calculating according to car body current location attitude information that issues track and standard, and next step movable scope that reaches of car body is shown.

Method realizes it mainly being the transformational relation of coordinate to be negated separate, and obtains the some world coordinate system under position and programme by graphical interfaces under photo coordinate system and is presented on the image, and its theoretical formula is as follows:

$\left[\begin{array}{c}{x}_{\mathrm{lc}}\\ y_{\mathrm{lc}}\\ z_{\mathrm{lc}}\end{array}\right]={\left[\begin{array}{ccc}{r1}_1& {r1}_2& {r1}_3\\ {r1}_4& {r1}_5& {r1}_6\\ {r1}_7& {r1}_8& {r1}_9\end{array}\right]}^T*$

$(\left[\begin{array}{ccc}\cos \mathrm{\α}\cos \mathrm{\β}& \cos \mathrm{\α}\sin \mathrm{\β}& -\sin \mathrm{\α}\\ -\sin \mathrm{\β}& \cos \mathrm{\β}& 0\\ \sin \mathrm{\α}\cos \mathrm{\β}& \sin \mathrm{\α}\sin \mathrm{\β}& \cos \mathrm{\α}\end{array}\right]*((\left[\begin{array}{c}{x}_r\\ y_r\\ z_r\end{array}\right]-\left[\begin{array}{c}0\\ 0\\ H\end{array}\right])-\left[\begin{array}{c}{S}_{\mathrm{lbx}}\\ S_{\mathrm{lby}}\\ S_{\mathrm{lbz}}\end{array}\right]-\left[\begin{array}{c}{S}_{\mathrm{clx}}\\ S_{\mathrm{cly}}\\ S_{\mathrm{clz}}\end{array}\right])-\left[\begin{array}{c}{S}_{\mathrm{lx}}\\ S_{\mathrm{ly}}\\ S_{\mathrm{lz}}\end{array}\right])$

After the image passback, the grid of monospacing under the world coordinate system, the reachable path point range of the path that distant operation issues and next step car body continuous motion can calculate it and be presented on the image as planimetric coordinates according to the anti-solution formula of projection relation, the track result who makes car be about to walking comes into plain view, handled easily person's intuitive judgment has important practical value.

As shown in Figure 6, effect synoptic diagram for the internal field experiment, the path of the expression planning in centre of three curves among the figure, the width range of two expression car bodies on both sides, from figure, can judge the correctness that issues the path and the security of walking very intuitively, can guarantee also that simultaneously but next path point is positioned at the scope that the car body continuous motion arrives, and then carry out the path and issue.

(4) in car body arrives current visual range, during last path point, begin circulation from step (2) and carry out, arrive the final goal position until car body.

Claims (7)

1, is applicable to the monocular vision air navigation aid of the environment sensing of autonomous moving vehicle, it is characterized in that comprising the following steps:

(1) measures the intrinsic parameter that camera distorts, after the measurement camera is installed on the autonomous moving vehicle, determine the transformational relation of photo coordinate system to world coordinate system;

(2) record is when the front vehicle body attitude, in images acquired in the front vehicle body visual range, and the image of gathering is back to pattern manipulation interface, select the car body walking manner according to target location and image situation, then the image that collects is carried out distortion correction, make the image after the correction satisfy the pinhole imaging system principle, according to the path point on selection walking path of the image behind the distortion correction and the path, the path point of car body current location and selection is converted under the world coordinate system according to the transformational relation of the described photo coordinate system of step (1) to world coordinate system, the result issues car body with conversion, and the path the selected form with width of the carbody is presented on the pattern manipulation interface;

(3) car body when car body in the step (2) selects path point to follow the tracks of walking manner, is revised the coordinate figure of path point under world coordinate system, last path point in arriving current visual range along selecting the path walking in real time;

When car body is selected the automatic obstacle avoiding walking manner in the step (2), the camera pitching is taken pictures to current scene to specified angle, barrier in the scene is carried out Region Segmentation, the cognitive disorders zone, estimation obstacle height, planning gets around the path of obstacle, and car body is according to the path walking of planning, last path point in arriving current visual range;

(4) in car body arrives current visual range, during last path point, begin circulation from step (2) and carry out, arrive the final goal position until car body.

2, according to the claim 1 described monocular vision air navigation aid that is applicable to the environment sensing of autonomous moving vehicle, it is characterized in that: also regular images acquired in the car body walking process in the described step (3), after image is back to pattern manipulation interface image is carried out distortion correction, and will be according to the path point of car body correction or planning being presented on the pattern manipulation interface when front vehicle body position, attitude, when car body does not have avoiding obstacles, car body is sent the instruction of stopping in emergency.

3, according to claim 1 or the 2 described monocular vision air navigation aids that are applicable to the environment sensing of autonomous moving vehicle, it is characterized in that: the photo coordinate system in the described step (1) is tied to by camera coordinates to the transformational relation of world coordinate system that the conversion of camera support coordinate system, camera support coordinate are tied to the conversion of mast coordinate system, the mast coordinate is tied to bodywork reference frame conversion, bodywork reference frame after four steps of world coordinate system conversion, goes on foot comprehensively to be combined into as the parameters of formula equation with four to obtain the transformational relation of photo coordinate system to world coordinate system.

4, according to claim 1 or the 2 described monocular vision air navigation aids that are applicable to the environment sensing of autonomous moving vehicle, it is characterized in that: the path with walking in the described step (2) is presented at the form of width of the carbody adopts on the image based on the distant method of operating that quantizes figure, photo coordinate system negated to the transformational relation of world coordinate system separate, obtain the position of point under photo coordinate system under the world coordinate system.

5, according to claim 1 or the 2 described monocular vision air navigation aids that are applicable to the environment sensing of autonomous moving vehicle, it is characterized in that: revise the coordinate figure of path point under world coordinate system in the described step (3) in real time, change correction in real time according to the car body position that records in real time, attitude, correction formula is:

$\left[\begin{array}{c}{x}_r^{\′}\\ y_r^{\′}\\ z_r^{\′}\end{array}\right]=\left[\begin{array}{ccc}\cos \mathrm{\Δp}& \sin \mathrm{\Δ}r\sin \mathrm{\Δp}& \cos \mathrm{\Δ}r\sin \mathrm{\Δp}\\ 0& \cos \mathrm{\Δr}& -\sin \mathrm{\Δr}\\ -\sin \mathrm{\Δp}& \sin \mathrm{\Δr}\cos \mathrm{\Δp}& \cos \mathrm{\Δ}r\cos \mathrm{\Δp}\end{array}\right]*(\left(\begin{array}{c}\left[\begin{array}{ccc}\cos \mathrm{\α}\cos \mathrm{\β}& -\sin \mathrm{\β}& \sin \mathrm{\α}\cos \mathrm{\β}\\ \cos \mathrm{\α}\sin \mathrm{\β}& \cos \mathrm{\β}& \sin \mathrm{\α}\sin \mathrm{\β}\\ -\sin \mathrm{\α}& 0& \cos \mathrm{\α}\end{array}\right]\\ \left.([\begin{array}{ccc}{\mathrm{rl}}_1& {\mathrm{rl}}_2& {\mathrm{rl}}_3\\ {\mathrm{rl}}_4& {\mathrm{rl}}_5& {\mathrm{rl}}_6\\ {\mathrm{rl}}_7& {\mathrm{rl}}_8& {\mathrm{rl}}_9\end{array}\right]*\left[\begin{array}{c}{x}_{\mathrm{lc}}\\ y_{\mathrm{lc}}\\ z_{\mathrm{lc}}\end{array}\right]+\left[\begin{array}{c}{S}_{\mathrm{lx}}\\ S_{\mathrm{ly}}\\ S_{\mathrm{lz}}\end{array}])\right.\\ +\left[\begin{array}{c}{S}_{\mathrm{lbx}}+S_{\mathrm{clx}}\\ S_{\mathrm{lby}}+S_{\mathrm{cly}}\\ H+S_{\mathrm{lbz}}+S_{\mathrm{clz}}\end{array}\right]\end{array}\right)-\left[\begin{array}{c}{S}_x^{\′}\\ S_y^{\′}\\ S_z^{\′}\end{array}\right])$

Wherein, α: the camera angle of pitch;

β: camera crab angle;

[S _TxS _TyS _Tz]: the camera coordinates initial point is to the translational movement of camera support coordinate system;

Subscript lc, l, b, c represent camera coordinates system, mast coordinate system, bodywork reference frame, camera support coordinate system respectively;

S _ABC: for A is that initial point is the translational movement in the C direction of initial point to B;

$\left[\begin{array}{ccc}{\mathrm{rl}}_1& {\mathrm{rl}}_2& {\mathrm{rl}}_3\\ {\mathrm{rl}}_4& {\mathrm{rl}}_5& {\mathrm{rl}}_6\\ {\mathrm{rl}}_7& {\mathrm{rl}}_8& {\mathrm{rl}}_9\end{array}\right]:$ The camera angle of pitch and crab angle all are that camera coordinates is tied to the rotation matrix of bodywork reference frame under zero the situation;

Δ r: when the roll angle of the relative initial position of front vehicle body;

Δ p: when the angle of pitch of the relative initial position of front vehicle body;

S _x', S _y', S _z' issue coordinate figure under the initial position world coordinate system at the path point for the current location of car body;

H: bodywork reference frame is to the translational movement of world coordinate system.

6, according to claim 1 or the 2 described monocular vision air navigation aids that are applicable to the environment sensing of autonomous moving vehicle, it is characterized in that: barrier is carried out Region Segmentation adopt the least error dividing method in the described step (3), at first calculate the total sample number of intensity profile, the distribution average, variance, then according to the total sample number of calculating, the distribution average, variance is calculated the smallest error function of gray threshold, find out minimum corresponding threshold in the smallest error function of all gray thresholds, utilize the threshold value that obtains that image is carried out binary conversion treatment at last, and adopt the corrosion dilation operation of image to remove the isolated point that extracts, eliminate the cavity.

7, according to claim 1 or the 2 described monocular vision air navigation aids that are applicable to the environment sensing of autonomous moving vehicle, it is characterized in that: the estimation obstacle height in the described step (3), the front end face of supposing barrier have maximum height and with the ground approximate vertical, then according to the triangle relation dyscalculia height that obtains after the hypothesis.

Images