CN101975951B - 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 perception technology 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 the measurement utilizing emitted light with from the mistiming measuring distance between the body surface reflected light.It can directly obtain range data, for vehicle provides convenient intuitively environment descriptor.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, therefore is widely used.No matter be single camera or the application of multiple-camera, a lot of researchs are arranged in the prior art.But camera review is subject to the impact of illumination, smog, environment easily, although the stereoscopic vision that multiple-camera forms can access actual range, the coupling between image is complicated, consuming time, has affected practical application effect.The millimetre-wave radar volume is little, and is lightweight, 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 from transmitter and runs into the time that obstacle is reflected back receiver and find 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 Scanning data and optical imagery have very strong complementarity to the description of environment, can obtain rapidly and accurately the intensive three-dimensional coordinate of body surface such as laser scanning data, and optical imagery has comprised abundant color.Therefore, merge laser scanning data and can obtain the more comprehensive information of automatic driving vehicle running environment with optical imagery, improved the rapidity of obstacle detection and to the adaptive faculty of complex environment.About the information fusion problem of distance and image, there are many scholars to be studied.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 the unmanned vehicle lowered in field environment is travelled in the works for 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 identified for three kinds of typical road conditions only, lack for example research identified of meadow, trees of other common obstacle in the environment.Monteiro uses the information after laser radar and video camera merge, and pedestrian and vehicle for detection of mobile on the road do not carry out Understanding to surrounding environment.Xiang Zhiyu has proposed obstacle detection method in a kind of thick grass of merging laser radar and video camera information.The method is divided into groups laser radar data first, determine barrier point, then be mapped in the camera review, with this be partitioned in the camera review can not running region and 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, marks off the wheeled zone, 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 for automatic driving vehicle in the open air, 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 processed;

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 _lAxle 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 _cThe vertical direction of axle and the plane of delineation is parallel, and direction is downward; Z _cAxle is perpendicular to the plane of delineation, and is 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

First 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 putting under the 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 camera intrinsic parameter,

α _x, α _yRespectively the scale factor of u axle and v axle in the image coordinate system, u ₀, v ₀Be optical centre, γ is u axle and the v axle out of plumb factor, in 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 environment 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 arrange different threshold value ρ _{E, max}, ρ _{A, max}When the space length between two points or angular distance during greater than respective threshold, think that these two points are not at the same area;

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

(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 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 having 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) penetrance of point is high in the zone, and singular value is distributed as σ ₁≈ σ ₂≈ σ ₃

3) field method vector γ upwards;

After identifying the meadow, these points are merged to the same area, and to mark this zone be wheeled zone;

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 will divide out is transformed under the image coordinate system of video camera;

(2) the wheeled zone of preliminary judgement is further identified;

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,i}\mathrm{\η}(x_i,{\mathrm{\μ}}_{k,i},{\mathrm{\Σ}}_{k,i})---\left(31\right)$

Wherein K is the number of Gaussian distribution, selects 3～5, ω _{K, i}The estimated value of weight, μ _{K, i}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 wheeled 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 is non-wheeled region point.

Aforesaid as a result fusion steps specifically comprises the steps:

The differentiation result of laser radar and video camera is merged, judge wheeled zone and can not running region, constructing environment map; The differentiation result of comprehensive laser radar and video camera is divided into wheeled zone and can not running region with grating map, 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) if it is the wheeled zone that laser radar is differentiated the result, then needs to differentiate the result according to video camera and analyze;

(a) if it is the wheeled zone that video camera is differentiated the result, then be the wheeled zone;

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

(3) if the zone that exists laser radar and video camera all not to detect should the zone be 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 such as meadow, road, trees, bushes etc. detect identification, and to the color modeling in wheeled zone, further detect the unusual part in wheeled zone.Vehicle-periphery can be marked off " can not running region ", " wheeled zone " and " zone of ignorance " etc., be conducive to vehicle planning driving path, the capacity of will that travels in raising 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 to 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 to 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, swings by The Cloud Terrace and could realize 3-D scanning.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, namely define this coordinate origin O _lBe positioned at the laser radar light source center, X _lAxle 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 _cThe vertical direction of axle and the plane of delineation is parallel, and direction is downward; Z _cAxle is perpendicular to the plane of delineation, and is 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 when mounted sport car experiment 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

First 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 putting under the 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 camera intrinsic parameter,

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

The camera marking method based on plane target drone according to the 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, only have external parameter to change, take the image of diverse location gridiron pattern target by video camera, extract the angle point of grid on the gridiron pattern target as unique point, set up the relation between target point and the corresponding diagram picture point, thereby calculate the intrinsic parameter of video camera and target with respect to the external parameter of camera coordinate system.

Concrete, by with the plane gridiron pattern as target, video camera is taken target at the image of diverse location, extract the angle point of grid on the gridiron pattern target as the unique point on target plane, set up the unique point on target plane and the relation between the point on the correspondence image plane, calculate the Intrinsic Matrix A of video camera and target with respect to the external parameter R of 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 shown in formula (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 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 _cFirst two columns, t _cIt is corresponding translation vector; A is camera intrinsic parameter,

Wherein, α _x, α _yRespectively the scale factor of u axle and v axle in the image coordinate system, u ₀, v ₀Be optical centre, γ is u axle and the v axle out of plumb factor, in 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 corresponding matrix H of every width of cloth image _i, i=1,2 ...

Secondly, according to the H matrix of all images that obtain, 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, shown in formula (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 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 the 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 the plane gridiron pattern target at a black and white square interval, target is positioned over the video camera position different with the laser radar front, start simultaneously video camera and laser radar, gather visible images and range data and record.It is right to obtain so a series of " images-distance ".This target both had been used for the demarcation of camera intrinsic parameter A, can be used for video camera and the external parameter R of laser radar and the demarcation of t again.

As shown in Figure 3, take i target plane as example, 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 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 _iThe quantity of target Plane-point,

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 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, namely

$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 impact of the 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 are brought 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 large 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 processed.

(2) Region Segmentation;

The three-dimensional point cloud that the Laser Radar Scanning environment obtains is cut apart, obtained a plurality of zones, these zones are identified respectively.

For processing 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 is radial boundary arest neighbors (A Radially Bounded Nearest Neighbor, RBNN) clustering method, can 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, such as 10,20 or 30.

Circular is as follows: with Q _iIn point carry out svd U ∑ V ^T, with V ^TThe vector of unit length of minimum value in the middle corresponding ∑ is as p _iNormal

Above-mentioned svd is 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 arrange different threshold value ρ _{E, max}, ρ _{A, max}, when the space length between two points or angular distance during greater than respective threshold, think that namely these two points are not at the same area.

(c) to all consecutive point calculating space length ρ each other _eWith angle ρ _aDistance.If ρ _e≤ ρ _{E, max}And ρ _a≤ ρ _{A, max}, so can be with these two some Cluster mergings.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 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 to analyze identification to these landform, specifically comprises the steps:

(a) zoning feature;

Before the identification, regional several feature be need to 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 having 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, and namely penetrance is different.The thick grass that branches and leaves are sparse, the surface pore of laser scanning is more, and penetrance is larger; 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 approximate equalling zero just.When automatic driving vehicle travels lowered in field environment, the thick grass that covers the earth's surface can travel by.Therefore when detecting the larger thick grass of penetrance, should be designated the wheeled zone.

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) penetrance of point is high in the zone, and 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 to be merged to the same area, and to mark this zone be the wheeled zone.Merging herein refers to the point in several zones is placed in the zone.

2. road Identification;

The maximum difference in road area and zone, meadow is that the laser beam penetration capacity is different.The surface ratio of road is more smooth, so light beam irradiates is on the road surface, and analyzing spot distributes more even.Road is the wheeled zone.

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 processed, and specifically comprises the steps:

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

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

(2) the wheeled zone of preliminary judgement is further identified;

Because in the lowered in field environment, except the road of wheeled, 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 Scanning data to have the cavity, also might be reflected back a part of laser, the height of this part point is all on surface level, therefore only adopt laser radar data to carry out obstacle identification, be easy to the water beach is treated as the wheeled zone.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 in addition the stone of some in the running environment, although volume is little, automatic driving vehicle is jolted, affect the speed of vehicle.Only adopt the range data of laser radar, because the resolution of laser radar is relatively low, can't accurately depict the stone shape, so adopt the image of video camera this moment, can judge these less barriers.

For with wheeled zone and can not running region separately, use gauss hybrid models (GaussianMixture Models, GMM) that color is classified.The color in wheeled zone is obeyed certain statistical law, and the color value of each pixel can represent with the mixture model of several Gaussian distribution.Behind the image that obtains new wheeled zone, just upgrade the model that mixed Gaussian distributes, if the pixel of present image and model are complementary, judge that so this point is the point in the wheeled zone, 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,i}\mathrm{\η}(x_i,{\mathrm{\μ}}_{k,i},{\mathrm{\Σ}}_{k,i})---\left(31\right)$

Wherein K is the number of Gaussian distribution, generally selects 3～5, ω _{K, i}The estimated value of weight, μ _{K, i}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;

The process of setting up the gauss hybrid models of wheeled region color feature is comprised of following step:

1. the image-region of processing is carried out the conversion of color space.

2. the initialization parameters comprises overall wheeled region threshold T, learning rate α, Gauss model quantity K, and initializes weights ω _{K, i}

3. the color value of the road of the wheeled 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.

4. read the view data in the wheeled zone of initial appointment, with color value and the comparison of existing Gauss model of each pixel, 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 value of pixel,

It is the variance of k Gaussian distribution model.

Coupling: upgrade 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 mean value vector and the variance of k Gaussian distribution model this moment, α is learning rate, and ρ is the study factor that model adapts to, and acts on similar to α.

Do not mate: if k＜K increases a Gauss model, the color value that new pixel is got in new Gauss model distribution is 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 learning rate, ω _{K, i-1}Be last respective weights, M _{K, i}Be the coupling quantized value, if coupling M _{K, i}=1, do not mate then M _{K, i}=0.

(b) zone identification;

The image in the Gauss model of the wheeled 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 non-wheeled region point.The value of T is chosen according to actual needs.Point in this zone is set up corresponding gauss hybrid models, and the process of setting up is identical with the model in wheeled zone, and adds in the knowledge base, is 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, judge wheeled zone and can not running region, 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 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 to have and without two states, it is convenient still to use; And the 3 d grid map can also represent other attributes of this unit, for example highly, thus meticulousr expression terrain environment.During the structure grating map, the resolution of environment landform and the size of lattice dimensions are closely related.If the high resolving power of requirement will reduce the size of grid so, grid quantity increases, and causes the complicacy of calculating.And reduce grid quantity, just must increase lattice dimensions, decrease resolution.Therefore, the foundation of grating map need to consider 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 is divided into wheeled zone and can not running region with grating map, 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) if it is the wheeled zone that laser radar is differentiated the result, then needs to differentiate the result according to video camera and analyze;

(a) if it is the wheeled zone that video camera is differentiated the result, then be the wheeled zone;

(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 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) laser radar data and video camera are merged after identification the wheeled zone, 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 to 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 simultaneously this moment attitude information of inertial navigation equipment.

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 some zones 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 the identification regional is, such as meadow, trees, road, bushes etc.Meadow and road can be thought the wheeled zone, and road and bushes are obstacle.Parameter is set to h _Max=0.1.

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

The 3rd step: camera review is processed;

Analyze camera review, set up the gauss hybrid models in wheeled zone.Parameter is set to K=5, and initializes weights is ω=0.05, overall wheeled region threshold T=0.7, 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 is identified the obstacle in the wheeled zone, and new obstacle is added in the knowledge base.

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

Claims (8)

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

The first step: set up mathematical model;

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 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 _lAxle 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 _cThe vertical direction of axle and the plane of delineation is parallel, and direction is downward; Z _cAxle is perpendicular to the plane of delineation, and is 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

First 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;

(2)

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 putting under the image coordinate system on the image location of pixels;

(3)

Wherein A is camera intrinsic parameter,

α _x, α _yRespectively the scale factor of u axle and v axle in the image coordinate system, u ₀, v ₀Be optical centre, γ is u axle and the v axle out of plumb factor, in 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

(17)

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:

(30)

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

Second step: laser radar range data obstacle detection;

The 3rd step: camera review is processed;

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 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 environment obtains is cut apart, obtained a plurality of zones;

(3) zone identification;

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

3. the field environment disorder detection method of a kind of fusion distance according to claim 2 and image information, it 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 arrange different threshold value ρ _{E, max}, ρ _{A, max}When the space length between two points or angular distance during greater than respective threshold, think that these two points are not at the same area;

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

(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 noise, with its deletion.

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

5. the field environment disorder detection method of a kind of fusion distance according to claim 2 and image information, it 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 having 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) penetrance of point is high in the zone, and singular value is distributed as σ ₁≈ σ ₂≈ σ ₃

3) field method vector γ upwards;

After identifying the meadow, these points are merged to the same area, and to mark this zone be wheeled zone;

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.

6. the field environment disorder detection method of a kind of fusion distance according to claim 5 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 σ ₁＞＞σ ₂

7. 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 camera review treatment step specifically comprises the steps:

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

(2) the wheeled zone of preliminary judgement is further identified;

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}The estimated value of weight, μ _{K, i}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:

(32)

μ _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 wheeled 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 is non-wheeled region point.

8. 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 as a result fusion steps specifically comprises the steps:

The differentiation result of laser radar and video camera is merged, judge wheeled zone and can not running region, constructing environment map; The differentiation result of comprehensive laser radar and video camera is divided into wheeled zone and can not running region with grating map, 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) if it is the wheeled zone that laser radar is differentiated the result, then needs to differentiate the result according to video camera and analyze;

(a) if it is the wheeled zone that video camera is differentiated the result, then be the wheeled zone;

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

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

Images