CN101975951A - Field environment barrier detection method fusing distance and image information - Google Patents

Abstract

The invention belongs to the technical field of barrier detection, particularly relates to a field environment barrier detection method fusing distance and image information and aims to provide a method for detecting the common barriers under the field driving condition of an unmanned vehicle so as to plan a driving route for the vehicle and enhance the independent field driving capability of the unmanned vehicle. The method comprises the following steps of: establishing a mathematical model; detecting a laser radar distance data barrier; and fusing the image processing of a video camera and a result. By the method, the common barriers such as grasslands, roads, trees, bushes and the like under the field driving condition of the unmanned vehicle can be detected and recognized and color modeling is performed on a driving region, so that an abnormal part of the driving region is further detected. The environment around the vehicle can be partitioned into a non-driving region, the driving region, an unknown region and the like, so that the planning of the driving route of the vehicle is facilitated and the independent field driving capability of the unmanned vehicle is enhanced.

Description

The field environment disorder detection method of a kind of fusion distance and image information

Technical field

The invention belongs to the obstacle detection technical field, be specifically related to the field environment disorder detection method of a kind of fusion distance and image information.

Background technology

The detection of obstacles of automatic driving vehicle on travel is the important component part in the environment sensing technical research field.In the application of obstacle detection, sensor commonly used has laser radar, video camera, millimetre-wave radar, ultrasonic sensor etc.Laser radar is by measurement emission light with from the mistiming measuring distance between the body surface reflected light.It can directly obtain range data, for vehicle provides the convenient information of environment description intuitively.The concrete application of laser radar has a lot of forms, the for example location of object and tracking, environmental modeling and keep away barrier, location and map structuring (SLAM), the classification of landform and morphologic characteristics etc. can also utilize the echo strength information of laser radar to carry out obstacle detection and tracking.The physical message of the surrounding environment that video camera obtains is abundant, and has higher disguise, has therefore obtained using widely.No matter be the single camera or the application of multiple-camera, a lot of researchs are all arranged in the prior art.But camera review is subjected to the influence of illumination, smog, environment easily, though the stereoscopic vision that multiple-camera forms can access actual range, the coupling complexity between image, consuming time has influenced practical application effect.The millimetre-wave radar volume is little, and is in light weight, can survey distance, and the ability of penetrating fog, cigarette, dust is strong, but resolution and precision are lower, is mainly used in automobile collision preventing and controls brake system etc. based on the vehicle tracing system of spacing measuring ability with knocking into the back to alleviate.Ultrasonic sensor is transmitted into the time that runs into obstacle reflected back receiver from transmitter and finds range by detecting ultrasound wave.The ultrasonic sensor volume is little cheap again, uses comparatively general, but owing to precision is low, can only be used in the occasion not high to the environment sensing accuracy requirement.

And laser radar scan-data and optical imagery have very strong complementarity to the description of environment, can obtain the intensive three-dimensional coordinate of body surface rapidly and accurately as laser scanning data, and optical imagery have comprised abundant colors.Therefore, merge laser scanning data and can obtain the comprehensive more information of automatic driving vehicle running environment, improved the rapidity of obstacle detection and the adaptive faculty of complex environment with optical imagery.About the information fusion problem of distance and image, there are many scholars to study.Peter merges the data of monocular-camera image and sonar ranging under indoor environment, has set up the indoor environment model, but has been not suitable for using under outdoor complicated field environment.The Tsai-hong Hong of University of Maryland tests the requirement that unmanned car lowered in field environment is travelled in the works at U.S. Demo III, proposed to merge the obstacle detection algorithm of three-dimensional laser radar data and camera review, be that road sign, pond and road are discerned only, lack for example research discerned of meadow, trees of other common obstacle in the environment at three kinds of typical road conditions.Monteiro uses the information after laser radar and video camera merge, and is used to detect the pedestrian and the vehicle that move on the road, surrounding environment is not discerned understanding.Xiang Zhiyu has proposed obstacle detection method in a kind of thick grass of merging laser radar and video camera information.This method is divided into groups laser radar data earlier, determine barrier point, be mapped in the camera review then, with this be partitioned in the camera review can not running region and must running region, reduce the erroneous judgement that occurs when pure laser radar is differentiated, and improved the integrality that final barrier profile detects.But when the color of barrier and thick grass very near the time, the effect of fusion detection can descend to some extent, and can not running region according to what judge, but marks off running region, lacks environment is understood more specifically.To sum up, prior art does not have and can plan driving path thereby be convenient to vehicle for automatic driving vehicle detects common obstacle under the driving conditions in the open air, improves the disorder detection method of the capacity of will that travels in the automatic driving vehicle field.

Summary of the invention

The purpose of this invention is to provide a kind of can the detection common obstacle under the driving conditions in the open air for automatic driving vehicle, thereby be convenient to vehicle planning driving path, improve the fusion distance of the capacity of will that travels in the automatic driving vehicle field and the field environment disorder detection method of image information.

The present invention is achieved in that

The field environment disorder detection method of a kind of fusion distance and image information comprises the steps:

The first step: set up mathematical model;

Second step: laser radar range data obstacle detection;

The 3rd step: camera review is handled;

The 4th step: the result merges.

Aforesaid foundation in the mathematical model step set up car body and laser radar, video camera and inertial navigation coordinate system, and described coordinate system is interrelated, specifically comprises the steps:

(1) sets up bodywork reference frame O _bX _bY _bZ _b

Initial point O _bBe positioned at certain point of fixity of car body, initial point be the laser radar light source with plane that ground overlaps on projection, X _bAxle points to car body front-right, Y _bAxle points to car body dead ahead, Z _bAxle points to the car body top; The bodywork reference frame initial point is h to the height on ground ₀

(2) set up laser radar coordinate system O _lX _lY _lZ _l

Define this coordinate origin O _lBe positioned at the laser radar light source center, X _lThe axle straight up, Y _lAxle horizontal to the right, Z _lAxle points to car body the place ahead;

(3) set up camera coordinate system O _cX _cY _cZ _c

Coordinate origin O _cBe positioned at the focus of camera lens, X _cAxle and plane of delineation horizontal direction parallel, direction is to the right; Y _cAxle is parallel with the vertical direction of the plane of delineation, is directed downwards; Z _cThe axle perpendicular to the plane of delineation, directed straight ahead;

(4) set up inertial navigation coordinate system O _IX _IY _IZ _I

The inertial navigation coordinate system is consistent with the bodywork reference frame direction;

(5) determine the coordinate system transformational relation, specifically comprise the steps:

(a) from laser radar coordinate system O _lX _lY _lZ _lBe transformed into bodywork reference frame O _bX _bY _bZ _b

Earlier around Y _lThe axle half-twist is again around Z _lThe axle half-twist is then along Z _lThe downward translation h of direction of principal axis ₀Distance;

$\left[\begin{array}{c}{x}_b\\ y_b\\ z_b\end{array}\right]=\left[\begin{array}{ccc}0& 1& 0\\ -1& 0& 0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{ccc}0& 0& -1\\ 0& 1& 0\\ 1& 0& 0\end{array}\right]\left[\begin{array}{c}{x}_l\\ y_l\\ z_l\end{array}\right]+\left[\begin{array}{c}0\\ 0\\ h_0\end{array}\right]=\left[\begin{array}{c}{y}_l\\ z_l\\ x_l+h_0\end{array}\right]---\left(2\right)$

Each meaning of parameters is same as described above in the formula;

(b) camera coordinate system is transformed into image coordinate system;

According to existing video camera pin-hole model, convert the three-dimensional coordinate of following of image coordinate system on the image location of pixels;

$s\left[\begin{array}{c}u\\ v\\ 1\end{array}\right]=\left[\begin{array}{ccc}{\mathrm{\α}}_x& \mathrm{\γ}& u_0\\ 0& {\mathrm{\α}}_y& v_0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}{x}_c\\ y_c\\ z_c\end{array}\right]=A\left[\begin{array}{c}{x}_c\\ y_c\\ z_c\end{array}\right]---\left(3\right)$

Wherein A is a camera intrinsic parameter,

α _x, α _yBe respectively the scale factor of u axle and v axle in the image coordinate system, u ₀, v ₀Be optical centre, γ is the u axle and the v axle out of plumb factor, under a lot of situations, makes γ=0; Other meaning of parameters is same as described above in the formula;

(c) from laser radar coordinate system O _lX _lY _lZ _lBe transformed into camera coordinate system O _cX _cY _cZ _c

$\left[\begin{array}{c}{x}_c\\ y_c\\ z_c\end{array}\right]=R\·\left[\begin{array}{c}{x}_l\\ y_l\\ z_l\end{array}\right]+t---\left(17\right)$

Wherein R and t are respectively rotation matrix and the translation vector that is transformed into camera coordinate system from the laser radar coordinate system; Other meaning of parameters is same as described above in the formula

(d) range data is proofreaied and correct;

The car body angle of pitch of supposing inertial navigation equipment output is that α, roll angle are γ, and the range data after the correction obtains by following formula:

$\left[\begin{array}{c}{x}_g\\ y_g\\ z_g\end{array}\right]=\left[\begin{array}{ccc}1& 0& 0\\ 0& \cos (-\mathrm{\α})& \sin (-\mathrm{\α})\\ 0& -\sin (-\mathrm{\α})& \cos (-\mathrm{\α})\end{array}\right]\left[\begin{array}{ccc}\cos (-\mathrm{\γ})& 0& -\sin (-\mathrm{\γ})\\ 0& 1& 0\\ \sin (-\mathrm{\γ})& 0& \cos (-\mathrm{\γ})\end{array}\right]\left[\begin{array}{c}{x}_b\\ y_b\\ z_b\end{array}\right]---\left(30\right)$

Meaning of parameters is same as described above in the formula.

Aforesaid laser radar range data obstacle detection step comprises the steps:

(1) range data pre-service;

The laser radar range data is carried out filtering;

(2) Region Segmentation;

The three-dimensional point cloud that the laser radar scanning circumstance obtains is cut apart, obtained a plurality of zones;

(3) zone identification;

Meadow, road, trees, bushes are carried out analyzing identification.

Aforesaid Region Segmentation step comprises the steps:

(a) retrieve certain three dimensions point p _iNearest k point in the field calculates by this k+1 and puts the point set Q that forms on every side _iNormal, as the normal vector of this analyzing spot;

(b) three dimensions is put p _iCoordinate

And normal

The composition characteristic vector

The cartesian space distance of two three-dimensional point is ρ _e(p _i, p _j)=‖ p _i-p _j‖, wherein, p _jCoordinate

p _jWith its normal The composition characteristic vector

Angular distance is ρ _a(n _i, n _j)=0.5-0.5 *＜n _i, n _j〉/‖ n _i‖ ‖ n _j‖, wherein,

For space length and angular distance are provided with different threshold value ρ _{E, max}, ρ _{A, max}When the space length between two points or angular distance during, think that these two points are not at the same area greater than respective threshold;

(c) to all consecutive point calculating space length ρ each other _eWith angle ρ _aDistance; If ρ _e≤ ρ _{E, Max}And ρ _a≤ ρ _{A, max}, these two some clusters are merged;

(d) if certain a bit can't with other cluster, a newly-built zone;

(e) with a cluster is arranged after, check every class to have a little quantity, if quantity is less than a certain threshold value n _Cmin, and the average height of putting in the class is far longer than the height h of vehicle leaping over obstacles _Max, such is a noise, with its deletion.

In the aforesaid Region Segmentation step, the span of k is 10～30.

Aforesaid regional identification step specifically comprises the steps:

(a) zoning feature;

Calculate average height, singular value, regional Surface Method vector; Computing method are as follows:

Average height:

N is the quantity of point in this zone, and i is the sequence number of point;

Singular value: [σ ₁σ ₂σ ₃]; The coordinate of being had a few in this zone is formed matrix, carry out svd U ∑ V ^T, the element in the ∑ is singular value, and with σ ₁, σ ₂, σ ₃Descending ordering;

Zone Surface Method vector: γ; Minimum singular value σ ₃Corresponding V ^TIn vector of unit length;

(b) identification comprises the steps:

1. meadow identification;

When satisfying following condition, be identified as the meadow;

1) the some average height is lower than vehicle obstacle clearing capability h in the zone _Max

2) the penetrance height of point in the zone, singular value is distributed as σ ₁≈ σ ₂≈ σ ₃

3) field method vector γ upwards;

After identifying the meadow, these points are merged to the same area, but and mark this zone for running region;

2. road Identification;

When satisfying following condition, be identified as road:

1) point highly is lower than the vehicle obstacle clearing capability in the zone

2) penetrance of point is low in the zone, σ ₁＞σ ₂＞＞σ ₃

3) field method vector γ upwards;

3. trunk identification;

When the regularity of distribution of singular value is σ ₁＞＞σ ₂≈ σ ₃, when field method vector γ is positioned at horizontal direction, be identified as trunk;

4. bushes identification;

When singular value is σ ₁＞σ ₂＞σ ₃,, when field method vector γ also is positioned at horizontal direction, be identified as bushes.

In the aforesaid identification step, as singular value σ ₁With σ ₂Ratio σ ₁/ σ ₂In the time of in [1 4], σ ₁≈ σ ₂As singular value σ ₁With σ ₂Ratio σ ₁/ σ ₂Greater than 50 o'clock, singular value σ ₁＞＞σ ₂

Aforesaid camera review treatment step specifically comprises the steps:

(1) laser radar data in the zone that division is come out is transformed under the image coordinate system of video camera;

(2) but the running region of preliminary judgement is further discerned;

Specifically comprise the steps:

(a) set up gauss hybrid models;

The sequence of supposing pixel in the image is { x ₁, x ₂X _i, the value of i pixel is x _i=[R _i, G _i, B _i], the probability of this pixel is:

$P\left(x_i\right)=\underset{k=1}{\overset{K}{\mathrm{\Σ}}}{\mathrm{\ω}}_{k,j}\mathrm{\η}(x_i,{\mathrm{\μ}}_{k,j},{\mathrm{\Σ}}_{k,j})---\left(31\right)$

Wherein K is the number of Gaussian distribution, selects 3～5, ω _{K, i}Be the estimated value of weight, μ _{K, i}Be the average of k Gaussian distribution, ∑ _{K, i}Be the covariance matrix of k Gaussian distribution, suppose that covariance matrix meets

Rule; The probability density of pixel i in k gauss of distribution function calculated by following formula:

$\mathrm{\η}(x_i,\mathrm{\μ},\mathrm{\Σ})=\frac{1}{{\left(2\mathrm{\π}\right)}^{n/2}{\left|\mathrm{\Σ}\right|}^{1/2}}{e}^{-\frac{1}{2}{(x_i-{\mathrm{\μ}}_i)}^T{\mathrm{\Σ}}^{-1}(x_i-{\mathrm{\μ}}_i)}---\left(32\right)$

μ _iBe the average of i Gaussian distribution, ∑ is the covariance matrix of Gaussian distribution;

(b) zone identification;

The image in the Gauss model of the travelled field color set up and current zone to be determined is carried out the absolute value comparison, if | x _i-f| 〉=T, T are threshold value, and then this point is the non-region point of travelling.

Aforesaid fusion steps as a result specifically comprises the steps:

The differentiation result of laser radar and video camera is merged, but judge running region and can not running region, the constructing environment map; The differentiation result of comprehensive laser radar and video camera, but grating map is divided into running region and can not running region, and the principle of division is:

(1) if it is can not running region that laser radar is differentiated the result, then grating map being divided into can not running region;

(2) but if it be running region that laser radar is differentiated the result, then need to analyze according to video camera differentiation result;

(a) but if it is running region that video camera is differentiated the result, but then be running region;

(b), then be can not running region if it is can not running region that video camera is differentiated the result;

(3) if the zone that exists laser radar and video camera all not to detect should the zone be a zone of ignorance then.

The invention has the beneficial effects as follows:

The present invention can for automatic driving vehicle in the open air under the driving conditions common to obstacle for example meadow, road, trees, bushes etc. detect identification, but and to the color modeling of running region, but further detect the unusual part of running region.Vehicle-periphery can be marked off " can not running region ", " but running region " and " zone of ignorance " etc., help vehicle planning driving path, improve the capacity of will that travels in the automatic driving vehicle field.

Description of drawings

Fig. 1 is the process flow diagram of the field environment disorder detection method of a kind of fusion distance of the present invention and image information;

Fig. 2 is that the mutual alignment of bodywork reference frame of the present invention, laser radar coordinate system and camera coordinate system concerns synoptic diagram;

Fig. 3 of the present inventionly is transformed into camera coordinate system coordinate conversion synoptic diagram from the laser radar coordinate system.

Embodiment

Be introduced below in conjunction with the field environment disorder detection method of drawings and Examples a kind of fusion distance of the present invention and image information:

As shown in Figure 1, the field environment disorder detection method of a kind of fusion distance and image information comprises the steps:

The first step: set up mathematical model;

When setting up the vehicle sensor model, need set up car body and laser radar, video camera and inertial navigation coordinate system, and these coordinate systems are interrelated, specifically comprise the steps:

(1) sets up bodywork reference frame O _bX _bY _bZ _b

The bodywork reference frame that foundation and vehicle body are fixed together, it moves with vehicle.Bodywork reference frame is defined as: initial point O _bBe positioned at certain point of fixity of car body, to specify initial point be the laser radar light source with plane that ground overlaps on projection, X _bAxle points to car body front-right, Y _bAxle points to car body dead ahead, Z _bAxle points to the car body top.The bodywork reference frame initial point is h to the height on ground ₀

(2) set up laser radar coordinate system O _lX _lY _lZ _l

Laser radar is installed on the The Cloud Terrace, and the laser that laser radar is launched can only scan 2-D data, could realize 3-D scanning by the The Cloud Terrace swing.Laser radar coordinate system and the laser radar of getting along well are connected this moment, but and The Cloud Terrace consider as a whole together, promptly define this coordinate origin O _lBe positioned at the laser radar light source center, X _lThe axle straight up, Y _lAxle horizontal to the right, Z _lAxle points to car body the place ahead.

(3) set up camera coordinate system O _cX _cY _cZ _c

The camera coordinate system that foundation and video camera are fixed together.Coordinate origin O _cBe positioned at the focus of camera lens, X _cAxle and plane of delineation horizontal direction parallel, direction is to the right; Y _cAxle is parallel with the vertical direction of the plane of delineation, is directed downwards; Z _cThe axle perpendicular to the plane of delineation, directed straight ahead.

(4) set up inertial navigation coordinate system O _IX _IY _IZ _I

Inertial navigation equipment is fixedly mounted on the car body.Set up the inertial navigation coordinate system consistent with the bodywork reference frame direction.The alignment error of inertial navigation equipment can be carried out sport car experiment when mounted and resolved out, and after alignment error was proofreaied and correct, the attitude angle of inertial navigation equipment output was exactly the attitude angle of car body.

When adopting laser radar data to carry out obstacle identification, the angle of pitch and the roll angle of the car body that needs use inertial navigation sensitivity arrives; By the structure rotation matrix, range data is corrected in the coordinate system of true origin on surface level.

The mutual alignment relation of bodywork reference frame, laser radar coordinate system and camera coordinate system as shown in Figure 2.

(5) determine the coordinate system transformational relation, specifically comprise the steps:

(a) from laser radar coordinate system O _lX _lY _lZ _lBe transformed into bodywork reference frame O _bX _bY _bZ _b

Earlier around Y _lThe axle half-twist is again around Z _lThe axle half-twist is then along Z _lThe downward translation h of direction of principal axis ₀Distance.

Can get:

$\left[\begin{array}{c}{x}_b\\ y_b\\ z_b\end{array}\right]=\left[\begin{array}{ccc}0& 1& 0\\ -1& 0& 0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{ccc}0& 0& -1\\ 0& 1& 0\\ 1& 0& 0\end{array}\right]\left[\begin{array}{c}{x}_l\\ y_l\\ z_l\end{array}\right]+\left[\begin{array}{c}0\\ 0\\ h_0\end{array}\right]=\left[\begin{array}{c}{y}_l\\ z_l\\ x_l+h_0\end{array}\right]---\left(2\right)$

(b) camera coordinate system is transformed into image coordinate system;

According to existing video camera pin-hole model, convert the three-dimensional coordinate of following of image coordinate system on the image location of pixels.

$s\left[\begin{array}{c}u\\ v\\ 1\end{array}\right]=\left[\begin{array}{ccc}{\mathrm{\α}}_x& \mathrm{\γ}& u_0\\ 0& {\mathrm{\α}}_y& v_0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}{x}_c\\ y_c\\ z_c\end{array}\right]=A\left[\begin{array}{c}{x}_c\\ y_c\\ z_c\end{array}\right]---\left(3\right)$

Wherein A is a camera intrinsic parameter, α _x, α _yBe respectively the scale factor of u axle and v axle in the image coordinate system, u ₀, v ₀Be optical centre, γ is the u axle and the v axle out of plumb factor, under a lot of situations, makes γ=0.A can be obtained by the plane target drone method that people such as Zhang Zhengyou propose.

The camera marking method based on plane target drone according to people such as Zhang Zhengyou propose requires video camera to take same plane target drone in orientation different more than two, and video camera and plane target drone can move freely, and need not to know kinematic parameter.In calibration process, the inner parameter of supposing video camera is constant, when different angles are taken target, have only external parameter to change, take the image of diverse location gridiron pattern target by video camera, the angle point that extracts grid on the gridiron pattern target is set up the relation between target point and the corresponding diagram picture point as unique point, thereby calculates the intrinsic parameter of video camera and the target external parameter with respect to camera coordinate system.

Concrete, by with the plane gridiron pattern as target, video camera is taken the image of target at diverse location, extract the unique point of the angle point of grid on the gridiron pattern target as the target plane, set up the unique point on target plane and the relation between the point on the correspondence image plane, calculate the intrinsic parameter matrix A of video camera and target external parameter R with respect to camera coordinate system _c, t _cCalibration process is as described below:

At first, find the solution homography matrix H:

If the unique point on target plane

Point on the corresponding plane of delineation

The mapping relations of the two are as shown in Equation (4):

$s\tilde{m}=A\left[\begin{array}{ccc}{r}_{c1}& r_{c2}& t_c\end{array}\right]\tilde{M}---\left(4\right)$

In the formula (4), s is a scale factor, r _C1, r _C2The unique point that is respectively the target plane transforms to the rotation matrix r of image coordinate system from the target plane coordinate system _cPreceding two row, t _cIt is corresponding translation vector; A is a camera intrinsic parameter,

Wherein, α _x, α _yBe respectively the scale factor of u axle and v axle in the image coordinate system, u ₀, v ₀Be optical centre, γ is the u axle and the v axle out of plumb factor, under a lot of situations, makes γ=0.

Make H=A[r _C1r _C2t _c], then formula (4) can be write a Chinese character in simplified form into formula (5):

$s\tilde{m}=H\tilde{M}---\left(5\right)$

Order

Wherein,

The transposition of three column vectors of expression H matrix, the mapping relations of the point on the plane of delineation and the unique point on target plane can formulate (6) form:

$\left[\begin{array}{ccc}{\tilde{M}}^T& 0^T& -u{\tilde{M}}^T\\ 0^T& {\tilde{M}}^T& -v{\tilde{M}}^T\end{array}\right]f=0---\left(6\right)$

According to n point on every width of cloth image of taking, adopt least square method can solve the pairing matrix H of every width of cloth image _i, i=1,2 ...

Secondly, according to the H matrix of all images that obtains, find the solution intermediate vector b:

Make B=A ^-TA ^-1, then:

$B=A^{-T}{A}^{-1}=\left[\begin{array}{ccc}{B}_{11}& B_{12}& B_{13}\\ B_{21}& B_{22}& B_{23}\\ B_{31}& B_{32}& B_{33}\end{array}\right]---\left(7\right)$

Write above-mentioned matrix as vector form, as shown in Equation (8):

b＝[B ₁₁?B ₁₂?B ₂₂?B ₁₃?B ₂₃?B ₃₃] ^T (8)

So, just have relational expression shown in the formula (9) to exist:

$h_i^T{\mathrm{Bh}}_j=v_{\mathrm{ij}}^{T}b---\left(9\right)$

Wherein, h ₁=[h _I1h _I2h _I3] ^T, h _j=[h _J1h _J2h _J3] ^T, which width of cloth image i and j all represent, i=1, and 2, J=1,2,

v _ij＝[h _i1h _j1?h _i1h _j2+h _i2h _j1?h _i2h _j2?h _i3h _j1+h _i1h _j2?h _i3h _j2+h _i2h _j3?h _i3h _j3] ^T

………………………………………………………(10)

With all v _IjForm matrix V, V is the matrix of 2n * 6, wherein, n=1,2, There is relational expression shown in the formula (11):

Vb＝0 (11)

Can solve b by formula (11).

At last, can decomposite the intrinsic parameter matrix A of video camera according to b, and according to A ^-1Find the solution R _c, t _c:

The intrinsic parameter that decomposites video camera according to b is specially: utilize Cholesky matrix decomposition algorithm to solve A b ^-1, inverting obtains A again;

Demarcate the external parameter t of every width of cloth image _cFor:

r _c1＝λA ^-1h ₁ (12)

r _c2＝λA ^-1h ₂ (13)

r _c3＝r _c1×r _c2 (14)

t _c＝λA ^-1h ₃ (15)

λ=1/ ‖ A wherein ^-1h ₁‖=1/ ‖ A ^-1h ₂‖, so the rotation matrix in the external parameter of demarcating is:

R _c＝[r _c1?r _c2?r _c3] (16)

(c) from laser radar coordinate system O _lX _lY _lZ _lBe transformed into camera coordinate system O _cX _cY _cZ _c

$\left[\begin{array}{c}{x}_c\\ y_c\\ z_c\end{array}\right]=R\·\left[\begin{array}{c}{x}_l\\ y_l\\ z_l\end{array}\right]+t---\left(17\right)$

Wherein R and t are respectively rotation matrix and the translation vector that is transformed into camera coordinate system from the laser radar coordinate system, and R and t computation process are as follows:

With black and white square plane gridiron pattern target at interval, target is positioned over the video camera position different with the laser radar front, start video camera and laser radar simultaneously, gather visible images and range data and record.It is right to obtain a series of " images-distance " like this.This target both had been used for the demarcation of camera intrinsic parameter A, can be used for the external parameter R of video camera and laser radar and the demarcation of t again.

As shown in Figure 3, be example with i target plane, the initial point O of camera coordinate system _cDistance to i target plane is λ _{C, i}, the vertical line direction is γ _{C, i}Three-dimensional laser radar coordinate origin O _lDistance to i target plane is λ _{L, i}, the vertical line direction is γ _{L, i}, computing method are as follows.

${\mathrm{\γ}}_{c,i}=R_{c,i}\left[\begin{array}{c}0\\ 0\\ -1\end{array}\right]=-R_{3,c,i}---\left(18\right)$

${\mathrm{\λ}}_{c,i}={\mathrm{\γ}}_{c,i}^T\·t_{c,i}---\left(19\right)$

R wherein _{3, c, i}Expression R _{C, i}The 3rd row.The camera coordinate system initial point can be write as following form to the normal direction and the distance on all target planes:

Γ _c＝[γ _c，1?γ _c，2…γ _c，n] (20)

Λ _c＝[λ _c，1?λ _c，2…λ _c，n] ^T (21)

Γ wherein _cBe 3 * n matrix, Λ _cBe n * 1 column vector, n is " distance-image " logarithm.

Use least square method to simulate the target plane from the target point data of laser radar scanning, can obtain the laser radar coordinate origin is γ to the normal direction on i target plane _{L, i}And distance lambda _{L, i}If x _{L, i}Be i target plane m _i* 3 some set, m _iBe the quantity of putting on the target plane,

Be the average of dot matrix set, then

$x_{l,i}^{\′}=x_{l,i}-{\stackrel{\‾}{x}}_{l,i}---\left(22\right)$

Will

Carry out svd, can get

$U_{l,i}{S}_{l,i}{V}_{l,i}^T=x_{l,i}^{\′T}{x}_{l,i}^{\′}---\left(23\right)$

So γ _{L, i}And λ _{L, i}For

γ _l，i＝U _3，l，i (24)

${\mathrm{\λ}}_{l,i}={\mathrm{\γ}}_{l,i}^T{\stackrel{\‾}{x}}_{l,i}^T---\left(25\right)$

Wherein, U _{3, l, i}Expression U _{L, i}The 3rd row.The laser radar coordinate origin can be write as following form to the normal direction and the distance on all target planes:

Γ _l＝[γ _l，1?γ _l，2…γ _l，n] (26)

Λ _l＝[λ _l，1?λ _l，2…λ _l，n] ^T (27)

Γ wherein _lBe 3 * n matrix, Λ _lBe n * 1 column vector, n is " distance-image " logarithm.

R＝VU ^T (28)

Wherein V and U are

Carry out the result of svd, promptly

$t={\left({\mathrm{\Γ}}_c{\mathrm{\Γ}}_c^T\right)}^{-1}{\mathrm{\Γ}}_c({\mathrm{\Λ}}_c-{\mathrm{\Λ}}_l)---\left(29\right)$

(d) range data is proofreaied and correct;

The car body angle of pitch of supposing inertial navigation equipment output is that α, roll angle are γ, and the range data after proofreading and correct so can obtain by following formula:

$\left[\begin{array}{c}{x}_g\\ y_g\\ z_g\end{array}\right]=\left[\begin{array}{ccc}1& 0& 0\\ 0& \cos (-\mathrm{\α})& \sin (-\mathrm{\α})\\ 0& -\sin (-\mathrm{\α})& \cos (-\mathrm{\α})\end{array}\right]\left[\begin{array}{ccc}\cos (-\mathrm{\γ})& 0& -\sin (-\mathrm{\γ})\\ 0& 1& 0\\ \sin (-\mathrm{\γ})& 0& \cos (-\mathrm{\γ})\end{array}\right]\left[\begin{array}{c}{x}_b\\ y_b\\ z_b\end{array}\right]---\left(30\right)$

Second step: laser radar range data obstacle detection comprises the steps:

(1) range data pre-service;

Because there is certain noise in the influence of factors such as laser radar drifted about, body surface characteristic and vehicle movement in the ranging data, therefore at first tackling the raw range data carries out pre-service.Comprise the noise spot and the mistake measuring point that bring by disturbance in the measurement data, these points are carried out filtering, can improve the degree of accuracy of arithmetic speed and feature detection.In addition, because the mixed pixel phenomenon of laser has some data acnodes with front and back data point spacing is very big in the raw data, these are nonsensical for environment understanding.Therefore, for these scattered data pointses and isolated point, can adopt 3 * 3 template to carry out medium filtering and be handled.

(2) Region Segmentation;

The three-dimensional point cloud that the laser radar scanning circumstance obtains is cut apart, obtained a plurality of zones, these zones are discerned respectively.

For handling cloud data, multiple dividing method is arranged in the prior art.A kind of is will point three-dimensional coordinate and color unification be placed in the six-vector, the calculation criterion of three-dimensional coordinate and color distance is merged in definition, adopts the region growing method, and the plane of color similarity in the scene is split.Another kind is to adopt the plane that body surface is carried out match, and the plane seed is constantly carried out region growing, has realized cutting apart a cloud.The third be the radial boundary arest neighbors (A Radially Bounded Nearest Neighbor, RBNN) clustering method can be cut apart putting cloud preferably, and step is as follows:

(a) retrieve certain three dimensions point p _iNearest k point in the field calculates by this k+1 and puts the point set Q that forms on every side _iNormal, as the normal vector of this analyzing spot.In the present embodiment, the span of k is 10～30, as 10,20 or 30.

Concrete computing method are as follows: with Q _iIn point carry out svd U ∑ V ^T, with V ^TIn the vector of unit length of minimum value in the corresponding ∑, as p _iNormal

Above-mentioned svd is a prior art.

(b) three dimensions is put p _iCoordinate

And normal

The composition characteristic vector

The cartesian space distance of two three-dimensional point of definition is ρ _e(p _i, p _j)=‖ p _i-p _j‖, wherein, p _jCoordinate

p _jWith its normal

The composition characteristic vector

Angular distance is ρ _a(n _i, n _j)=0.5-0.5 *＜n _i, n _j〉/‖ n _i‖ ‖ n _j‖, wherein,

For space length and angular distance are provided with different threshold value ρ _{E, max}, ρ _{A, max},, think that promptly these two points are not at the same area when the space length between two points or angular distance during greater than respective threshold.

(c) to all consecutive point calculating space length ρ each other _eWith angle ρ _aDistance.If ρ _e≤ ρ _{E, max}And ρ _a≤ ρ _{A, max}, these two some clusters can be merged so.All consecutive point are merged to a zone as far as possible.

(d) if certain a bit can't with other cluster, so with regard to a newly-built zone.

(e) with a cluster is arranged after, check every class to have a little quantity, if quantity is less than a certain threshold value n _Cmin, and the average height of putting in the class is far longer than the height h of vehicle leaping over obstacles _Max, think that such is a noise, can delete.Threshold value n _CminHeight h with the vehicle leaping over obstacles _MaxChoose according to actual conditions.

(3) zone identification;

Automatic driving vehicle travels in the lowered in field environment, has the various road conditions of meadow, road, trees, bushes etc., need analyze identification to these landform, specifically comprises the steps:

(a) zoning feature;

Before the identification, regional several characteristic be need calculate, average height, singular value, regional Surface Method vector etc. comprised.Computing method are as follows:

Average height:

N is the quantity of point in this zone, and i is the sequence number of point.

Singular value: [σ ₁σ ₂σ ₃].The coordinate of being had a few in this zone is formed matrix, carry out svd U ∑ V ^T, the element in the ∑ is singular value, and with σ ₁, σ ₂, σ ₃Descending ordering.

Zone Surface Method vector: γ.Minimum singular value σ ₃Corresponding V ^TIn vector of unit length.

(b) identification comprises the steps:

1. meadow identification;

When laser was got on the meadow, because blade of grass is at random, the data point that receives was very mixed and disorderly, and general edge detection method can't accurately detect.The maximum difference of meadow and ordinary road is that the density of analyzing spot is different, just the penetrance difference.The thick grass that branches and leaves are sparse, the surface pore of laser scanning is more, and penetrance is bigger; And dense bushes shoot and leaf growth is in great numbers, and then penetrance is less; If the laser scanning surface is hard surface, the distance value of each analyzing spot is evenly distributed, and its penetrance is just approximate to equal zero.When automatic driving vehicle travels lowered in field environment, the thick grass that covers the face of land can travel by.Therefore when detecting the bigger thick grass of penetrance, but should be designated running region.

When satisfying following condition, be identified as the meadow.

1) the some average height is lower than vehicle obstacle clearing capability h in the zone _Max

2) the penetrance height of point in the zone, singular value is distributed as σ ₁≈ σ ₂≈ σ ₃

3) field method vector γ upwards.

Because in Region Segmentation, may be divided into different zones to the point on the meadow, therefore after identifying the meadow, these points need be merged to the same area, but and mark this zone and be running region.Merging herein is meant the point in several zones is placed in the zone.

2. road Identification;

The maximum difference in road area and zone, meadow is laser beam penetration capacity difference.The surface ratio of road is more smooth, so light beam irradiates is on the road surface, and analyzing spot distributes more even.But road is a running region.

When satisfying following condition, be identified as road.

1) point highly is lower than the vehicle obstacle clearing capability in the zone

2) penetrance of point is low in the zone, σ ₁＞σ ₂＞＞σ ₃

3) field method vector γ upwards.

3. the identification of trunk;

Analyzing spot on the trunk distributes and generally is the part on the face of cylinder.When the regularity of distribution of singular value is σ ₁＞＞σ ₂≈ σ ₃, when field method vector γ is positioned at horizontal direction, be identified as trunk.

4. bushes identification;

Analyzing spot all distributes in one plane on the bushes, when singular value is σ ₁＞σ ₂＞σ ₃, when field method vector γ also is positioned at horizontal direction, be identified as bushes.

In the present invention, as singular value σ ₁With σ ₂Ratio σ ₁/ σ ₂In the time of in [14], think σ ₁≈ σ ₂As singular value σ ₁With σ ₂Ratio σ ₁/ σ ₂Greater than 50 o'clock, think singular value σ ₁＞＞σ ₂

The 3rd step: camera review is handled, and specifically comprises the steps:

(1) laser radar data in the zone that division is come out is transformed under the image coordinate system of video camera;

Change according to the coordinate transformation relation in the first step.

(2) but the running region of preliminary judgement is further discerned;

Because in the lowered in field environment, except the road that can travel, thick grass, may have water beach, stone etc.Laser radar scanning is on the water beach, part laser can not reflect, cause the laser radar scan-data to have the cavity, also might reflected back part laser, the height of this part point is all on surface level, therefore only adopt laser radar data to carry out obstacle identification, but be easy to the water beach is treated as running region.And in fact, present technology also can't be assessed the degree of depth on water beach, soft degree, so in a single day automatic driving vehicle sail into, just may cause danger.May have the stone of some in addition in the running environment,, automatic driving vehicle is jolted, influence the speed of vehicle though volume is little.Only adopt the range data of laser radar,, can't accurately depict the stone shape,, can judge these less barriers so adopt the image of video camera this moment because the resolution of laser radar is relatively low.

But for running region and can not running region separately, (GaussianMixture Models GMM) classifies to color to use gauss hybrid models.But the color of running region is obeyed certain statistical law, and each color of pixel value can be represented with the mixture model of several Gaussian distribution.But behind the image that obtains new running region, just upgrade the model that mixed Gaussian distributes, if the pixel of present image and model are complementary, but judge that so this point is the point in the running region, otherwise just judge that this point is the point of other character.

Specifically comprise the steps:

(a) set up gauss hybrid models;

The sequence of supposing pixel in the image is { x ₁, x ₂X _i, the value of i pixel is x _i=[R _i, G _i, B _i], the probability of this pixel is:

$P\left(x_i\right)=\underset{k=1}{\overset{K}{\mathrm{\Σ}}}{\mathrm{\ω}}_{k,j}\mathrm{\η}(x_i,{\mathrm{\μ}}_{k,j},{\mathrm{\Σ}}_{k,j})---\left(31\right)$

Wherein K is the number of Gaussian distribution, generally selects 3～5, ω _{K, i}Be the estimated value of weight, μ _{K, i}Be the average of k Gaussian distribution, ∑ _{K, i}Be the covariance matrix of k Gaussian distribution, suppose that generally covariance matrix meets

Rule.The probability density of pixel i in k gauss of distribution function can be calculated by following formula:

$\mathrm{\η}(x_i,\mathrm{\μ},\mathrm{\Σ})=\frac{1}{{\left(2\mathrm{\π}\right)}^{n/2}{\left|\mathrm{\Σ}\right|}^{1/2}}{e}^{-\frac{1}{2}{(x_i-{\mathrm{\μ}}_i)}^T{\mathrm{\Σ}}^{-1}(x_i-{\mathrm{\μ}}_i)}---\left(32\right)$

μ _iBe the average of i Gaussian distribution, ∑ is the covariance matrix of Gaussian distribution;

Can the travel process of gauss hybrid models of field color feature of foundation is made up of following step:

1. the processed images zone is carried out the conversion of color space.

2. each parameter of initialization comprises the overall situation region threshold T that can travel, learning rate α, Gauss model quantity K, and initializes weights ω _{K, i}

3. the color value of the road that travels of the initial appointment of reading images and meadow image, as the average of gauss hybrid models, variance is set up gauss hybrid models for predetermined empirical value.

But 4. read the view data of the running region of initial appointment, each color of pixel value and existing Gauss model compared, judge whether following formula is set up:

$|x_i-{\mathrm{\&mu;}}_k|<2.5{\mathrm{\&sigma;}}_k^2---\left(33\right)$

In the formula, x _iBe the color of pixel value, It is the variance of k Gaussian distribution model.

Coupling: upgrade the parameter and the weight of k gauss hybrid models, parameter comprises expectation, variance, the study factor.

μ _i＝(1-ρ)μ _i-1+ρx _i (34)

${\mathrm{\&sigma;}}_i^2=(1-\mathrm{\&rho;}){\mathrm{\&sigma;}}_{i-1}^2+\mathrm{\&rho;}{(x_i-{\mathrm{\&mu;}}_i)}^T(x_i-{\mathrm{\&mu;}}_i)---\left(35\right)$

ρ＝αη(x _i|μ _k，σ _k) (36)

In the formula, μ _kAnd σ _kBe respectively the mean value vector and the variance of k Gaussian distribution model this moment, α is a learning rate, and ρ is the study factor that model adapts to, and acts on similar to α.

Do not match: if k＜K increases a Gauss model, the color value that new pixel is got in new Gauss model distribution is an average, and variance and weight are got empirical value.If k=K replaces the minimum model of weight with new Gaussian distribution model, average and variance are the same.

Weights omega _kMore new formula be

ω _k，i＝(1-α)ω _k，i-1+αM _k，i (37)

In the formula, ω _{K, i}Be current weight, α is a learning rate, ω _{K, i-1}Be last respective weights, M _{K, i}Be the coupling quantized value, if coupling M _{K, i}=1, then M does not match _{K, i}=0.

(b) zone identification;

The image in the Gauss model of the travelled field color set up and current zone to be determined is carried out the absolute value comparison, if | x _i-f| 〉=T, T are threshold value, think that so this point is the non-region point of travelling.The value of T is chosen according to actual needs.Point in this zone is set up corresponding gauss hybrid models, but it is identical to set up the model of process and running region, and adds in the knowledge base, be convenient to analyze.

The 4th step: the result merges, and specifically comprises the steps:

The differentiation result of laser radar and video camera is merged, but judge running region and can not running region, the constructing environment map.

Vivid, easy-to-use grating map usually is used for simulating ground surface environment.Grating map is proposed by Elfes and Moravec the earliest, afterwards constantly development.Existing grating map generally can be divided into two classes, two-dimensional grid map and 3 d grid map.Each grid cell in the two-dimensional grid map, can only represent has and does not have two states, but uses conveniently; And the 3 d grid map can also be represented other attributes of this unit, for example highly, thus meticulousr expression terrain environment.During the structure grating map, the size of environment terrain resolution and lattice dimensions is closely related.If the high resolving power of requirement will reduce the size of grid so, grid quantity increases, and causes complexity of calculation.And reduce grid quantity, and just must increase lattice dimensions, resolution reduces.Therefore, the foundation of grating map need be taken all factors into consideration the various factorss such as arithmetic capability of the driveability of automatic driving vehicle itself, sensor resolution, environment topographic features, system.

The differentiation result of comprehensive laser radar and video camera, but grating map is divided into running region and can not running region, and the automatic driving vehicle of being more convenient for is planned the path, has therefore set up two-dimentional grating map.The principle of above-mentioned division is:

(1) if it is can not running region that laser radar is differentiated the result, then grating map being divided into can not running region;

(2) but if it be running region that laser radar is differentiated the result, then need to analyze according to video camera differentiation result;

(a) but if it is running region that video camera is differentiated the result, but then be running region;

(b) if it is can not running region that video camera is differentiated the result, then being can not running region.

(3) if the zone that exists laser radar and video camera all not to detect should the zone be a zone of ignorance then.

The detailed process of having set up the two-dimensional grid map is as follows:

(1) in bodywork reference frame, divides grid size.

(2) but with laser radar data and video camera merge back identification running region, can not running region, zone of ignorance carries out mark.

Be described below in conjunction with the field environment disorder detection method of a specific embodiment a kind of fusion distance of the present invention and image information:

Start three-dimensional laser radar, scene around the scanning obtains one group of range data of describing surrounding environment, gathers the attitude information of inertial navigation equipment this moment simultaneously.

The first step: set up mathematical model;

Set up coordinate system according to above-mentioned steps, the angle of pitch and attitude angle according to inertial navigation equipment output are corrected to this group range data in the coordinate system of true origin on surface level.

, and then be transformed under the bodywork reference frame.

Second step: laser radar range data obstacle detection;

(1) data pre-service;

Each point in the scanning distance data successively adopts 3 * 3 template that data are carried out medium filtering.

(2) Region Segmentation;

Adopt region segmentation method, these range data are cut apart, parameter is set to k=20, n _Cmin=150, h _Max=0.1 meter, ρ _{E, max}=0.04 meter, ρ _{A, max}=0.011.Can obtain several regions after cutting apart.

(3) zone identification;

The zoning feature is comprising average height

, singular value [σ ₁σ ₂σ ₃], regional Surface Method vector γ.

According to the characteristic quantity in zone, which kind of road conditions discerns each zone is, as meadow, trees, road, bushes etc.But meadow and road can be thought running region, and road and bushes are obstacle.Parameter is set to h _Max=0.1.

Start the video camera photographic images.But with the range data in the running region, be transformed under the camera coordinate system,, obtain the corresponding color information in this zone according to camera review.

The 3rd step: camera review is handled;

Analyze camera review, but set up the gauss hybrid models of running region.Parameter is set to K=5, and initializes weights is ω=0.05, the overall situation region threshold T=0.7 that can travel, learning rate α=0.2.

The 4th step: the result merges;

The recognition result of the data of adjusting the distance and camera review merges.According to gauss hybrid models, further analysis image, but the obstacle in the identification running region adds new obstacle in the knowledge base to.

But in conjunction with laser radar data and video camera merge back identification running region, can not running region, zone of ignorance, give corresponding attribute for corresponding grid, set up grating map.

Claims (9)

1. the field environment disorder detection method of fusion distance and image information comprises the steps:

The first step: set up mathematical model;

Second step: laser radar range data obstacle detection;

The 3rd step: camera review is handled;

The 4th step: the result merges.

2. the field environment disorder detection method of a kind of fusion distance according to claim 1 and image information, it is characterized in that: described foundation in the mathematical model step, set up car body and laser radar, video camera and inertial navigation coordinate system, and described coordinate system is interrelated, specifically comprise the steps:

(1) sets up bodywork reference frame O _bX _bY _bZ _b

Initial point O _bBe positioned at certain point of fixity of car body, initial point be the laser radar light source with plane that ground overlaps on projection, X _bAxle points to car body front-right, Y _bAxle points to car body dead ahead, Z _bAxle points to the car body top; The bodywork reference frame initial point is h to the height on ground ₀

(2) set up laser radar coordinate system O _lX _lY _lZ _l

Define this coordinate origin O _lBe positioned at the laser radar light source center, X _lThe axle straight up, Y _lAxle horizontal to the right, Z _lAxle points to car body the place ahead;

(3) set up camera coordinate system O _cX _cY _cZ _c

Coordinate origin O _cBe positioned at the focus of camera lens, X _cAxle and plane of delineation horizontal direction parallel, direction is to the right; Y _cAxle is parallel with the vertical direction of the plane of delineation, is directed downwards; Z _cThe axle perpendicular to the plane of delineation, directed straight ahead;

(4) set up inertial navigation coordinate system O _IX _IY _IZ _I

The inertial navigation coordinate system is consistent with the bodywork reference frame direction;

(5) determine the coordinate system transformational relation, specifically comprise the steps:

(a) from laser radar coordinate system O _lX _lY _lZ _lBe transformed into bodywork reference frame O _bX _bY _bZ _b

Earlier around Y _lThe axle half-twist is again around Z _lThe axle half-twist is then along Z _lThe downward translation h of direction of principal axis ₀Distance;

Each meaning of parameters is same as described above in the formula;

(b) camera coordinate system is transformed into image coordinate system;

According to existing video camera pin-hole model, convert the three-dimensional coordinate of following of image coordinate system on the image location of pixels;

Wherein A is a camera intrinsic parameter,

α _x, α _yBe respectively the scale factor of u axle and v axle in the image coordinate system, u ₀, v ₀Be optical centre, γ is the u axle and the v axle out of plumb factor, under a lot of situations, makes γ=0; Other meaning of parameters is same as described above in the formula;

(c) from laser radar coordinate system O _lX _lY _lZ _lBe transformed into camera coordinate system O _cX _cY _cZ _c

Wherein R and t are respectively rotation matrix and the translation vector that is transformed into camera coordinate system from the laser radar coordinate system; Other meaning of parameters is same as described above in the formula

(d) range data is proofreaied and correct;

The car body angle of pitch of supposing inertial navigation equipment output is that α, roll angle are γ, and the range data after the correction obtains by following formula:

Meaning of parameters is same as described above in the formula.

3. the field environment disorder detection method of a kind of fusion distance according to claim 1 and image information is characterized in that: described laser radar range data obstacle detection step comprises the steps:

(1) range data pre-service;

The laser radar range data is carried out filtering;

(2) Region Segmentation;

The three-dimensional point cloud that the laser radar scanning circumstance obtains is cut apart, obtained a plurality of zones;

(3) zone identification;

Meadow, road, trees, bushes are carried out analyzing identification.

4. the field environment disorder detection method of a kind of fusion distance according to claim 3 and image information is characterized in that: described Region Segmentation step comprises the steps:

(a) retrieve certain three dimensions point p _iNearest k point in the field calculates by this k+1 and puts the point set Q that forms on every side _iNormal, as the normal vector of this analyzing spot;

(b) three dimensions is put p _iCoordinate

And normal

The composition characteristic vector

The cartesian space distance of two three-dimensional point is ρ _e(p _i, p _j)=|| p _i-p _j||, wherein, p _jCoordinate

p _jWith its normal

The composition characteristic vector

Angular distance is ρ _a(n _i, n _j)=0.5-0.5 *＜n _i, n _j〉/|| n _i|| || n _j‖, wherein,

For space length and angular distance are provided with different threshold value ρ _{E, max}, ρ _{A, max}When the space length between two points or angular distance during, think that these two points are not at the same area greater than respective threshold;

(c) to all consecutive point calculating space length ρ each other _eWith angle ρ _aDistance; If ρ _e≤ ρ _{E, max}And ρ _a≤ ρ _{A, max}, these two some clusters are merged;

(d) if certain a bit can't with other cluster, a newly-built zone;

(e) with a cluster is arranged after, check every class to have a little quantity, if quantity is less than a certain threshold value n _{C min}, and the average height of putting in the class is far longer than the height h of vehicle leaping over obstacles _Max, such is a noise, with its deletion.

5. the field environment disorder detection method of a kind of fusion distance according to claim 3 and image information is characterized in that: in the described Region Segmentation step, the span of k is 10～30.

6. the field environment disorder detection method of a kind of fusion distance according to claim 3 and image information is characterized in that: described regional identification step specifically comprises the steps:

(a) zoning feature;

Calculate average height, singular value, regional Surface Method vector; Computing method are as follows:

Average height: N is the quantity of point in this zone, and i is the sequence number of point;

Singular value: [σ ₁σ ₂σ ₃]; The coordinate of being had a few in this zone is formed matrix, carry out svd U ∑ V ^T, the element in the ∑ is singular value, and with σ ₁, σ ₂, σ ₃Descending ordering;

Zone Surface Method vector: γ; Minimum singular value σ ₃Corresponding V ^TIn vector of unit length;

(b) identification comprises the steps:

1. meadow identification;

When satisfying following condition, be identified as the meadow;

1) the some average height is lower than vehicle obstacle clearing capability h in the zone _Max

2) the penetrance height of point in the zone, singular value is distributed as σ ₁≈ σ ₂≈ σ ₃

3) field method vector γ upwards;

After identifying the meadow, these points are merged to the same area, but and mark this zone for running region;

2. road Identification;

When satisfying following condition, be identified as road:

1) point highly is lower than the vehicle obstacle clearing capability in the zone

2) penetrance of point is low in the zone, σ ₁＞σ ₂＞＞σ ₃

3) field method vector γ upwards;

3. trunk identification;

When the regularity of distribution of singular value is σ ₁＞＞σ ₂≈ σ ₃, when field method vector γ is positioned at horizontal direction, be identified as trunk;

4. bushes identification;

When singular value is σ ₁＞σ ₂＞σ ₃, when field method vector γ also is positioned at horizontal direction, be identified as bushes.

7. the field environment disorder detection method of a kind of fusion distance according to claim 6 and image information is characterized in that: in the described identification step, as singular value σ ₁With σ ₂Ratio σ ₁/ σ ₂In the time of in [1 4], σ ₁≈ σ ₂As singular value σ ₁With σ ₂Ratio σ ₁/ σ ₂Greater than 50 o'clock, singular value σ ₁＞＞σ ₂

8. the field environment disorder detection method of a kind of fusion distance according to claim 1 and image information is characterized in that: described camera review treatment step specifically comprises the steps:

(1) laser radar data in the zone that division is come out is transformed under the image coordinate system of video camera;

(2) but the running region of preliminary judgement is further discerned;

Specifically comprise the steps:

(a) set up gauss hybrid models;

The sequence of supposing pixel in the image is { x ₁, x ₂X _i, the value of i pixel is x _i=[R _i, G _i, B _i], the probability of this pixel is:

Wherein K is the number of Gaussian distribution, selects 3～5, ω _{K, i}Be the estimated value of weight, μ _{K, i}Be the average of k Gaussian distribution, ∑ _{K, i}Be the covariance matrix of k Gaussian distribution, suppose that covariance matrix meets

Rule; The probability density of pixel i in k gauss of distribution function calculated by following formula:

μ _iBe the average of i Gaussian distribution, ∑ is the covariance matrix of Gaussian distribution;

(b) zone identification;

The image in the Gauss model of the travelled field color set up and current zone to be determined is carried out the absolute value comparison, if | x _i-f| 〉=T, T are threshold value, and then this point is the non-region point of travelling.

9. the field environment disorder detection method of a kind of fusion distance according to claim 1 and image information is characterized in that: described fusion steps as a result specifically comprises the steps:

The differentiation result of laser radar and video camera is merged, but judge running region and can not running region, the constructing environment map; The differentiation result of comprehensive laser radar and video camera, but grating map is divided into running region and can not running region, and the principle of division is:

(1) if it is can not running region that laser radar is differentiated the result, then grating map being divided into can not running region;

(2) but if it be running region that laser radar is differentiated the result, then need to analyze according to video camera differentiation result;

(a) but if it is running region that video camera is differentiated the result, but then be running region;

(b) if it is can not running region that video camera is differentiated the result, then being can not running region.

Images