CN104002809B - A kind of section, vehicle fork detection device and detection method - Google Patents

Abstract

The present invention relates to vehicle safety auxiliary driving technology, disclose a kind of section, vehicle fork detection device and detection method.This device includes vehicle-mounted industrial computer, vehicle speed sensor, height for the twice limit, left and right of detection vehicle self road ahead in real time and the multi-line laser radar sensor of the distance on vehicle and twice limit, left and right, steering wheel angle sensor for detection Vehicular turn angle in real time, vehicle-mounted industrial computer is according to Vehicular turn angle, car speed, the distance calculating and plotting on the height on twice limit, left and right and vehicle and twice limit, left and right obtains prediction locus curve and twice limit, the left and right estimation curve in section, fork, front that vehicle travels, display for the prediction locus curve that display limit, Liang Tiao road, left and right estimation curve travels with vehicle in real time.This device can obtain two side line limit estimation curve and the prediction locus curve of vehicle traveling in section, fork, and prompting driver takes suitably to operate, and alleviates driver and judges burden, reduces wrongheaded possibility.

Description

A kind of section, vehicle fork detection device and detection method

Technical field

The present invention relates to vehicle safety auxiliary driving technology, particularly relate to a kind of section, vehicle fork detection device and detection Method.

Background technology

Showing according to Ministry of Public Security's statistics of traffic accidents annual report data in recent years, the vehicle accident in section, fork is sent out often Raw.Owing to this section vehicle flowrate is big, speed fast, and driver's seat blind area is big, and driver makes subjective improperly most probably Judge and take the excessive or too small steering operation of steering angle, make the vehicle of traveling be in potential collision or rollover danger In.Being easily caused vehicle when steering angle is excessive and go out turnout, when steering angle is too small, vehicle cannot pass through turnout.If Predict the linear and curvature at road ahead fork, the expected trajectory curve travelled in combination with current vehicle in advance, drive Member just can take the steering operation of proper angle previously according to prompting, it is to avoid accident occurs.In theory, utilize the GPS technology can Sensed in advance road ahead fork is linear and curvature, but needs to measure all of road of storage in advance in actual promoting the use of Fork information, this process workload is excessive, therefore cannot promote the use of.

Summary of the invention

It is an object of the invention to provide a kind of section, vehicle fork detection device and detection method, this device can obtain The prediction locus curve that the two side line limit estimation curve in section, fork and vehicle travel, and point out driver to take suitably to turn to Operation, alleviates judgement burden for driver, reduces wrongheaded probability.

For realizing above-mentioned technical purpose, the present invention adopts the following technical scheme that and is achieved.

Scheme one:

A kind of section, vehicle fork detection device, it is characterised in that including vehicle-mounted industrial computer, vehicle speed sensor, for real Time the height on twice limit, left and right of detection vehicle self road ahead and many line lasers of vehicle and the distance on twice limit, left and right Radar sensor, for the steering wheel angle sensor at detection Vehicular turn angle in real time, described vehicle-mounted industrial computer turns according to vehicle To angle, the distance calculating and plotting on car speed, the height on twice limit, left and right and vehicle and twice limit, left and right obtains vehicle and travels Prediction locus curve and twice limit, the left and right estimation curve in section, fork, front, estimate in real time display limit, Liang Tiao road, left and right The display of the prediction locus curve that index contour travels with vehicle；

The outfan of described vehicle speed sensor, the outfan of multi-line laser radar sensor, steering wheel angle sensor Outfan is electrically connected the corresponding I/O input of described vehicle-mounted industrial computer, and the video output terminals of described vehicle-mounted industrial computer is electrically connected Connect the video inputs of described display；

Described multi-line laser radar sensor is two, is separately mounted to symmetrical position, vehicle front bumper both sides； Described steering wheel angle sensor is arranged on vehicle steering shaft；Described vehicle speed sensor is fixedly mounted on turning of vehicle front vehicle wheel On joint；Described display is fixedly mounted on meter panel of motor vehicle.

The feature of the technical program and further improvement is that:

Described multi-line laser radar sensor uses IBEO LUX4 scanning laser radar, and rate of scanning is 12.5Hz, scanning Distance range is 0.3m 200m.

Described vehicle speed sensor uses W221 wheel speed sensors.

Described steering wheel angle sensor uses KMT32B angular transducer

Described display uses 2.2 cun of LCD display.

Scheme two:

A kind of vehicle fork section detecting method, detects device based on section, above-mentioned vehicle fork, it is characterised in that include Following steps:

(1) data acquisition: the real-time speed of vehicle speed sensor collection vehicle, and the real-time speed of vehicle is sent to vehicle-mounted Industrial computer；The left and right twice of two, the left and right multi-line laser radar sensor road that corresponding Real-time Collection vehicle is travelling respectively The discrete four layer laser scanning point data on limit, every layer of laser scanning point data is multiple, and Real-time Collection is arrived discrete Four layers of laser scanning point data send to vehicle-mounted industrial computer；The corner of steering wheel angle sensor Real-time Collection vehicle, and will be real Time the vehicle corner that gathers send to vehicle-mounted industrial computer；

(2) data process: vehicle-mounted industrial computer obtains, according to above-mentioned four layers of laser scanning point data, the road that vehicle is travelling The height on the twice limit, left and right on road, and the distance on vehicle and twice limit, left and right, calculating and plotting obtains the left side in section, fork, front Right twice limit estimation curve；

The vehicle-mounted industrial computer real-time speed according to vehicle, the real-time corner of vehicle, it is pre-that calculating and plotting obtains that vehicle travels Survey geometric locus；

(3) display is judged: if the prediction locus curve approximation that twice limit, left and right estimation curve travels for vehicle is parallel, The shown in green bar in color viewing area of display；If the prediction locus curve that vehicle travels and limit, Liang Tiao road estimation curve exist Certain is intersected, the shown in red bar in color viewing area of display.

The feature of the technical program and further improvement is that:

During the calculating and plotting of twice limit, the left and right estimation curve in section, fork, front in step 2, choose fixing Rectangular coordinate system (X₀,O,Y₀) it is the fixed reference frame of calculating and plotting process；For a multi-line laser radar sensor one layer Laser scanning point, this layer of multiple laser scanning point are started to choose according to regular length by vehicle-mounted industrial computer from the first laser scanning point For this layer the first Scanning Section, then with first laser scanning that the tail laser scanning point of this layer the first Scanning Section is this layer the second Scanning Section Point, is chosen for this layer the second Scanning Section according to regular length, is remained by this layer according to the choosing method of above-mentioned this layer the second Scanning Section Remaining laser scanning point chooses this layer of corresponding Scanning Section of formation, by the most right for the head and the tail laser scanning point in this layer of all Scanning Section Should connect this layer of formation and estimate line, this layer of laser scanning point is judged as when estimating the vertical dimension of line less than setting threshold value to this layer This layer of road edge point, this layer of road edge point of matching is the limit matched curve of this layer of road；Four layers of laser of one multi-line laser radar sensor Scanning element matching respectively obtains limit, the road matched curve of four homonymies；Then limit, the road matched curve to four homonymies is melted Closing, the abscissa value after fusion is with this fixing rectangular coordinate system (X₀,O,Y₀) the scale value value of axis of abscissas, each horizontal seat The ordinate value of corresponding four roads limit matched curve of scale value, the ordinate value after fusion is the vertical coordinate of four roads limit matched curve The meansigma methods of value, merges limit, the road matched curve of four homonymies and obtains complete limit, a road estimation curve；Left and right is more than two Two four layers of laser scanning points of line laser radar sensor obtain limit, the Liang Tiao road estimation curve of correspondence, i.e. obtain fork, front Twice limit, the left and right estimation curve in section.

During the calculating and plotting of the prediction locus curve that vehicle in step 2 travels, choose fixing rectangular coordinate system (X₀,O,Y₀) it is the fixed reference frame of calculating and plotting process；Set up with Vehicle Speed direction as axis of abscissas, with abscissa Axle is in same level and the dynamic coordinate system (X that axle be axis of ordinates vertical with axis of abscissas_j,j,Y_j), when setting prediction Between section T_PThe lateral velocity of interior vehicle, longitudinal velocity and transverse acceleration, longitudinal acceleration keep constant, by T_PIt is subdivided into J etc. Point, often it is divided into Δ t_pIf each decile moment point is j, j=1,2 ... J,For moment point j Vehicle Speed direction With X₀Angle, utilize simplify formula

$\left\{\begin{array}{c}{\stackrel{\·}{x}}_j={\stackrel{\·}{x}}_{j-1}+{\stackrel{\·\·}{x}}_{j-1}\·{\mathrm{\Δt}}_p\\ {\stackrel{\·}{y}}_j={\stackrel{\·}{y}}_{j-1}+{\stackrel{\·\·}{y}}_{j-1}\·{\mathrm{\Δt}}_p\end{array}\right.\left\{\begin{array}{c}{x}_j=x_{j-1}+{\stackrel{\·}{x}}_{j-1}\·{\mathrm{\Δt}}_p+\frac{1}{2}\·{\stackrel{\·\·}{x}}_{j-1}\·{{\mathrm{\Δt}}^2}_p\\ y_j=y_{j-1}+{\stackrel{\·}{y}}_{j-1}\·{\mathrm{\Δt}}_p+\frac{1}{2}\·{\stackrel{\·\·}{y}}_{j-1}\·{{\mathrm{\Δt}}^2}_p\end{array}\right.$

Calculate according to above-mentioned formula and can try to achieve dynamic coordinate system (X_j,j,Y_jUnder), vehicle is at the position coordinates of moment point j；

By dynamic coordinate system (X_j,j,Y_jUnder), vehicle is transformed into same fixing rectangular coordinate at the position coordinates of moment point j System (X₀,O,Y₀Under), if dynamic coordinate system (X_j,j,Y_j) and fixing rectangular coordinate system (X₀,O,Y₀) relative rotation angle be ψ_j, Then from dynamic coordinate system (X_j,j,Y_j) to fixing rectangular coordinate system (X₀,O,Y₀) transition matrix be

$A_j=\left[\begin{array}{cc}{\cos \mathrm{\ψ}}_j& -{\sin \mathrm{\ψ}}_j\\ {\sin \mathrm{\ψ}}_j& {\cos \mathrm{\ψ}}_j\end{array}\right]$

Thus provide formula:

$\left[\begin{array}{c}{\stackrel{\·}{x}}_j\\ {\stackrel{\·}{y}}_j\end{array}\right]=\left[\begin{array}{c}{\stackrel{\·}{x}}_{j-1}\\ {\stackrel{\·}{y}}_{j-1}\end{array}\right]+A_{j-1}(\left[\begin{array}{c}{\stackrel{\·\·}{x}}_{j-1}\\ {\stackrel{\·\·}{y}}_{j-1}\end{array}\right]\·{\mathrm{\Δt}}_p)$

$\left[\begin{array}{c}{x}_j\\ y_j\end{array}\right]=\left[\begin{array}{c}{x}_{j-1}\\ y_{j-1}\end{array}\right]+\left[\begin{array}{c}{\stackrel{\·}{x}}_{j-1}\\ {\stackrel{\·}{y}}_{j-1}\end{array}\right]\·{\mathrm{\Δt}}_p+A_{j-1}\·(\frac{1}{2}\left[\begin{array}{c}{\stackrel{\·\·}{x}}_{j-1}\\ {\stackrel{\·\·}{y}}_{j-1}\end{array}\right]\·{{\mathrm{\Δt}}^2}_p)$

Vehicle-mounted industrial computer obtains, according to above-mentioned formula and then calculating and plotting, the prediction locus curve that vehicle travels.

Preferably, in step 2, the height of left and right Dao Bian of the road that vehicle is travelling and vehicle are being obtained with left After the distance on right twice limit, utilize the distance on the twice limit, left and right of the road that vehicle travelling by Kalman's discrete type wave filter Data are filtered.

Section, the vehicle fork detection device of the present invention and detection method, this detection device can obtain twice limit, left and right and estimate The prediction locus curve that index contour and vehicle travel, if the prediction locus that twice limit, left and right estimation curve travels for vehicle is bent Line less parallel, then explanation driver can be with section, safety fork, the shown in green bar in color viewing area of display；As The prediction locus curve that really vehicle travels intersects in certain with limit, Liang Tiao road, left and right estimation curve, then explanation driver needs to continue Continuous adjustment direction dish steering angle to pass through section, fork, the shown in red bar in color viewing area of display.

The critical piece employing multi-line laser radar sensor of this device and vehicle-mounted industrial computer, simple to operate, easy to detect, Certainty of measurement is high, and is susceptible to ambient weather and affects the problem that the most there is not potential safety hazard；This section, vehicle fork detection dress Putting investment cost few, be suitable for large-scale promotion and use, its detection method has the advantages that intellectuality, automatization and reliability are high, Steering operation information accurately can be provided for driver in time, reduce the generation of vehicle accident.

Accompanying drawing explanation

With detailed description of the invention, the present invention is described in further detail below in conjunction with the accompanying drawings.

Fig. 1 is the mounting structure schematic diagram of section, a kind of vehicle fork detection device of the present invention；

Fig. 2 is the electrical connection schematic diagram of section, a kind of vehicle fork detection device of the present invention；

Fig. 3 is the Coordinate Conversion schematic diagram of the present invention；

In figure: 1, vehicle；2, left side multi-line laser radar sensor；3, right side multi-line laser radar sensor；4, vehicle-mounted Industrial computer；5, display；6, limit, road, fork.

Detailed description of the invention

It is an object of the invention to provide a kind of section, vehicle fork detection device and detection method, this device can obtain The prediction locus curve that the left and right twice limit estimation curve in section, fork and vehicle travel, and point out driver to take suitable turning To operation, alleviate judgement burden for driver, reduce wrongheaded probability.

With reference to Fig. 1, for the mounting structure schematic diagram of section, a kind of vehicle fork detection device of the present invention.

In order to achieve the above object, it is necessary first to carry out the installation of device, the process of installation is as follows: by many for left side line lasers Radar sensor 2 and right side multi-line laser radar sensor 3 are respectively adopted bolt and are fixedly mounted on a left side, vehicle 1 front bumper both sides Right symmetric position, left side multi-line laser radar 2 and right side multi-line laser radar 3 are respectively adopted IBEO LUX4 scanning laser radar, Rate of scanning is 12.5Hz, and scanning distance scope is 0.3m 200m；Steering wheel angle sensor is arranged on vehicle 1 steering spindle On, steering wheel angle sensor uses KMT32B angular transducer；Vehicle speed sensor is fixedly mounted on turning to of vehicle 1 front vehicle wheel On joint, vehicle speed sensor uses W221 wheel speed sensors, and vehicle speed sensor can utilize the vehicle speed sensor that vehicle is self-contained, Advantageously reduce cost；Display 5 is fixedly mounted on the region that meter panel of motor vehicle left handle drive person is easily noted that, display 5 Use 2.2 cun of LCD display；Vehicle-mounted industrial computer 4 is arranged on under-dash in car.

Left side multi-line laser radar sensor 2 and right side multi-line laser radar sensor 3 are for detection vehicle self in real time The distance on the height on the twice limit, left and right of road ahead and vehicle and twice limit, left and right, steering wheel angle sensor is in real time Detection Vehicular turn angle, vehicle-mounted industrial computer 4 according to Vehicular turn angle, car speed, the height on twice limit, left and right and vehicle with The distance calculating and plotting on twice limit, left and right obtains prediction locus curve and the left and right twice in section, fork, front that vehicle travels Limit estimation curve, display 5 is for the prediction locus curve of display limit, Liang Tiao road, left and right estimation curve in real time with vehicle traveling.

With reference to Fig. 2, for the electrical connection schematic diagram of section, a kind of vehicle fork detection device of the present invention；Vehicle speed sensor Outfan, left side multi-line laser radar sensor 2 and the outfan of right side multi-line laser radar sensor 3, steering wheel angle The outfan of sensor is electrically connected the corresponding I/O input of described vehicle-mounted industrial computer 4, the video frequency output of vehicle-mounted industrial computer 4 The video inputs of end electrical connection display 5.

Detect to realize the section, vehicle fork of the present invention, based on section, above-mentioned vehicle fork detection device, tool Body step is as follows:

(1) data acquisition: the real-time speed of vehicle speed sensor collection vehicle, and the real-time speed of vehicle is sent to vehicle-mounted Industrial computer 4；The left and right two of two, the left and right multi-line laser radar sensor road that corresponding Real-time Collection vehicle is travelling respectively The discrete four layer laser scanning point data on limit, road, every layer of laser scanning point data is multiple, and Real-time Collection is arrived discrete Four layers of laser scanning point data send to vehicle-mounted industrial computers 4；The side of the corner of steering wheel angle sensor Real-time Collection vehicle To value and angle value, wherein direction value characterizes vehicle is to the left or to bend to right, and the degree that angle value characterizes turn inside diameter is big Little, and the vehicle corner of Real-time Collection is sent to vehicle-mounted industrial computer.

(2) data process: vehicle-mounted industrial computer 4 obtains, according to above-mentioned four layers of laser scanning point data, the road that vehicle is travelling The height on the twice limit, left and right on road, and the distance on vehicle and twice limit, left and right, calculating and plotting obtains the left side in section, fork, front Right twice limit estimation curve.

During the calculating and plotting of twice limit, the left and right estimation curve in section, fork, front, choose fixing rectangular coordinate system (X₀,O,Y₀) it is the fixed reference frame of calculating and plotting process；One layer of laser scanning for a multi-line laser radar sensor Point, this layer of multiple laser scanning point are started to be chosen for this layer according to regular length by vehicle-mounted industrial computer from the first laser scanning point One scan section, then with the first laser scanning point that the tail laser scanning point of this layer the first Scanning Section is this layer the second Scanning Section, according to Regular length is chosen for this layer the second Scanning Section, according to the choosing method of above-mentioned this layer the second Scanning Section by this layer of remaining laser Scanning element chooses this layer of corresponding Scanning Section of formation, connects shape by each corresponding for the head and the tail laser scanning point in this layer of all Scanning Section Becoming this layer to estimate line, this layer of laser scanning point is judged as limit, this layer of road when estimating the vertical dimension of line less than setting threshold value to this layer Point, this layer of road edge point of matching is the limit matched curve of this layer of road；Four layers of laser scanning point of one multi-line laser radar sensor divide Other matching obtains limit, the road matched curve of four homonymies；Then limit, the road matched curve to four homonymies is merged, after fusion Abscissa value with this fixing rectangular coordinate system (X₀,O,Y₀) the scale value value of axis of abscissas, each abscissa value correspondence four The ordinate value of individual road limit matched curve, the ordinate value after fusion is the average of the ordinate value of four roads limit matched curve Value, merges limit, the road matched curve of four homonymies and obtains complete limit, a road estimation curve；Left and right line laser thunder more than two Two the four layers of laser scanning points reaching sensor obtain limit, the Liang Tiao road estimation curve of correspondence, i.e. obtain the left side in section, fork, front Right twice limit estimation curve.

For avoiding the impacts such as barrier, laser scanning point being used RANSAC algorithm, iteration randomly draws three Individual some fit Plane, the plane that last most of laser scanning points meet is limit height, and the purpose obtaining limit, road height exists Obtain in clearly scanning is the boss of road edge, rather than other barriers.

Typical road region includes region, section, crossing transitional region and region, crossing three class.The vehicle of the present invention travels In region, section, the limit, road at fork, laser radar sensor Real-time Collection front before being also introduced into crossing transitional region, is utilized to believe Breath.Common fork type is divided into Y shape, T-shaped and cross.If two partial distances are not less than road width, i.e. fork about fork It is vertically oriented to pass through, is cross fork.Otherwise it is Y shape fork or T-shaped fork.Due to what the present embodiment used LUX4 tetra-line laser radar is four layers, and the line laser that above-below direction is spaced apart 0.8 degree by four angles forms, nethermost one The laser scanning line angle of depression is maximum, thus its lower extreme point is affected minimum by car body angle of pitch change.

The vehicle-mounted industrial computer 4 real-time speed according to vehicle, the real-time corner of vehicle, it is pre-that calculating and plotting obtains that vehicle travels Survey geometric locus.

During the calculating and plotting of the prediction locus curve of vehicle traveling, choose fixing rectangular coordinate system (X₀,O,Y₀) it is The fixed reference frame of calculating and plotting process；Set up with Vehicle Speed direction as axis of abscissas, with axis of abscissas at same water Dynamic coordinate system (the X that axle is axis of ordinates that plane is interior and vertical with axis of abscissas_j,j,Y_j), set predicted time section T_PInterior car Lateral velocity, longitudinal velocity and transverse acceleration, longitudinal acceleration keep constant, according to infinitely small principle, by T_PSegmentation For J decile, often it is divided into Δ t_pIf each decile moment point is j, j=1,2 ... J, owing to the persistent period is the shortest, can neglect Slightly longitudinal direction of car and laterally between influence each other,For moment point j Vehicle Speed direction and X₀Angle, utilize letter Change formula

$\left\{\begin{array}{c}{\stackrel{\·}{x}}_j={\stackrel{\·}{x}}_{j-1}+{\stackrel{\·\·}{x}}_{j-1}\·{\mathrm{\Δt}}_p\\ {\stackrel{\·}{y}}_j={\stackrel{\·}{y}}_{j-1}+{\stackrel{\·\·}{y}}_{j-1}\·{\mathrm{\Δt}}_p\end{array}\right.\left\{\begin{array}{c}{x}_j=x_{j-1}+{\stackrel{\·}{x}}_{j-1}\·{\mathrm{\Δt}}_p+\frac{1}{2}\·{\stackrel{\·\·}{x}}_{j-1}\·{{\mathrm{\Δt}}^2}_p\\ y_j=y_{j-1}+{\stackrel{\·}{y}}_{j-1}\·{\mathrm{\Δt}}_p+\frac{1}{2}\·{\stackrel{\·\·}{y}}_{j-1}\·{{\mathrm{\Δt}}^2}_p\end{array}\right.$

Calculate according to above-mentioned formula and can try to achieve dynamic coordinate system (X_j,j,Y_jUnder), vehicle is at the position coordinates of moment point j；

With reference to Fig. 3, for the Coordinate Conversion schematic diagram of the present invention.Calculating due to the position coordinates of above-mentioned each moment point j The coordinate system of institute's reference is the most different, therefore can not the result of calculation of each step be added simply, it is necessary to convert it to same Could be added under one reference frame, need to be by dynamic coordinate system (X_j,j,Y_jUnder), vehicle is transformed into same at the position coordinates of moment point j One fixing rectangular coordinate system (X₀,O,Y₀Under), if dynamic coordinate system (X_j,j,Y_j) and fixing rectangular coordinate system (X₀,O,Y₀) Relative rotation angle is ψ_j, then from dynamic coordinate system (X_j,j,Y_j) to fixing rectangular coordinate system (X₀,O,Y₀) transition matrix be

$A_j=\left[\begin{array}{cc}{\cos \mathrm{\ψ}}_j& -{\sin \mathrm{\ψ}}_j\\ {\sin \mathrm{\ψ}}_j& {\cos \mathrm{\ψ}}_j\end{array}\right]$

Thus provide formula:

$\left[\begin{array}{c}{\stackrel{\·}{x}}_j\\ {\stackrel{\·}{y}}_j\end{array}\right]=\left[\begin{array}{c}{\stackrel{\·}{x}}_{j-1}\\ {\stackrel{\·}{y}}_{j-1}\end{array}\right]+A_{j-1}(\left[\begin{array}{c}{\stackrel{\·\·}{x}}_{j-1}\\ {\stackrel{\·\·}{y}}_{j-1}\end{array}\right]\·{\mathrm{\Δt}}_p)$

$\left[\begin{array}{c}{x}_j\\ y_j\end{array}\right]=\left[\begin{array}{c}{x}_{j-1}\\ y_{j-1}\end{array}\right]+\left[\begin{array}{c}{\stackrel{\·}{x}}_{j-1}\\ {\stackrel{\·}{y}}_{j-1}\end{array}\right]\·{\mathrm{\Δt}}_p+A_{j-1}\·(\frac{1}{2}\left[\begin{array}{c}{\stackrel{\·\·}{x}}_{j-1}\\ {\stackrel{\·\·}{y}}_{j-1}\end{array}\right]\·{{\mathrm{\Δt}}^2}_p)$

Vehicle-mounted industrial computer 4 obtains, according to above-mentioned formula and then calculating and plotting, the prediction locus curve that vehicle travels.

(3) display is judged: if the prediction locus curve approximation that twice limit, left and right estimation curve travels for vehicle is parallel, Less parallel is that the prediction locus curve that twice limit, left and right estimation curve travels for vehicle will not intersect, then explanation driver can With section, safety fork, the shown in green bar in color viewing area of display 4；If the prediction locus curve that vehicle travels Intersect in certain with limit, Liang Tiao road estimation curve, then explanation driver needs to continue adjustment direction dish steering angle to pass through Section, fork, the shown in red bar in color viewing area of display 4.

During data process, obtain the height of left and right Dao Bian of the road that vehicle is travelling and vehicle with After the distance on twice limit, left and right, utilize the road that vehicle travelling by Kalman's discrete type wave filter twice limit, left and right away from It is filtered from data.Vehicle-mounted industrial computer 4 utilizes Kalman filtering to realize the detecting and tracking to section, fork, extracts road profile Information.Making filter vector is X (k)=[A_lB_lA_rB_rA_uB_u]^T, state equation is X (k)=f [X (k-1), U (k)]+V (k), whereinObservational equation is Z (k)=X (k)+W (k).Predicting covariance matrix is

$P\left(k\right)=\left(\frac{\∂f}{\∂X}\right)P(k-1){\left(\frac{\∂f}{\∂X}\right)}^T+\left(\frac{\∂f}{\∂U}\right)G_u{\left(\frac{\∂f}{\∂U}\right)}^T+R$

Kalman observes renewal equation group

$\left\{\begin{array}{c}\mathrm{kg}=P\left(k\right)/(P\left(k\right)+Q)\\ X\left(k\right)=X\left(k\right)+\mathrm{kg}(Z\left(k\right)-X\left(k\right))\\ P\left(k\right)=P\left(k\right)-\mathrm{kg}\·\×P\left(k\right)\end{array}\right.$

Wherein, W (k), V (k) are white Gaussian noise, R, Q, G_uIt is respectively the covariance of W (k), V (k), U (k).

Section, a kind of vehicle fork detection device of the present invention, critical piece uses multi-line laser radar and vehicle-mounted industry control Machine, simple to operate, easy to detect, certainty of measurement is high, and be susceptible to ambient weather impact and the most there is not many asking of potential safety hazard Topic.A kind of vehicle fork section detecting method of the present invention detects device based on section, above-mentioned fork, and it is to various types of troubles Mouth detection is completed by vehicle-mounted industrial computer, and intelligent high, simple to operate, driver can be according to testing result directly perceived, reliable Carry out course changing control effectively.

Although embodiment of the present invention being described above in association with accompanying drawing, but the invention is not limited in above-mentioned Specific embodiments and applications field, above-mentioned specific embodiments the most schematic, guiding rather than limit Property.Those of ordinary skill in the art is under the enlightenment of description, in the scope protected without departing from the claims in the present invention In the case of, it is also possible to making a variety of forms, these belong to the row of present invention protection.

Claims (9)

1. section, vehicle fork detection device, it is characterised in that including vehicle-mounted industrial computer, vehicle speed sensor, in real time The height on the twice limit, left and right of detection vehicle self road ahead and many line lasers thunder of the distance on vehicle and twice limit, left and right Reaching sensor, for the steering wheel angle sensor at detection Vehicular turn angle in real time, described vehicle-mounted industrial computer is according to Vehicular turn Angle, car speed, the height on twice limit, left and right and vehicle obtain what vehicle travelled with the distance calculating and plotting on twice limit, left and right Twice limit, the left and right estimation curve in prediction locus curve and section, fork, front, estimates for display limit, Liang Tiao road, left and right in real time The display of the prediction locus curve that curve travels with vehicle；

The outfan of described vehicle speed sensor, the outfan of multi-line laser radar sensor, the output of steering wheel angle sensor End is electrically connected the corresponding I/O input of described vehicle-mounted industrial computer, and the video output terminals of described vehicle-mounted industrial computer electrically connects institute State the video inputs of display；

Described multi-line laser radar sensor is two, is separately mounted to symmetrical position, vehicle front bumper both sides；Described Steering wheel angle sensor is arranged on vehicle steering shaft；Described vehicle speed sensor is fixedly mounted on the knuckle of vehicle front vehicle wheel On；Described display is fixedly mounted on meter panel of motor vehicle.

A kind of section, vehicle fork detection device the most according to claim 1, it is characterised in that described multi-line laser radar Sensor uses IBEO LUX4 scanning laser radar, and rate of scanning is 12.5Hz, and scanning distance scope is 0.3m 200m.

A kind of section, vehicle fork detection device the most according to claim 1, it is characterised in that described vehicle speed sensor is adopted Use W221 wheel speed sensors.

A kind of section, vehicle fork detection device the most according to claim 1, it is characterised in that described steering wheel angle passes Sensor uses KMT32B angular transducer.

A kind of section, vehicle fork detection device the most according to claim 1, it is characterised in that described display uses 2.2 cun of LCD display.

6. a vehicle fork section detecting method, detects device based on the section, a kind of vehicle fork described in claim 1, its It is characterised by, comprises the following steps:

(1) data acquisition: the real-time speed of vehicle speed sensor collection vehicle, and the real-time speed of vehicle is sent to vehicle-mounted industry control Machine；The twice limit, left and right of two, the left and right multi-line laser radar sensor road that corresponding Real-time Collection vehicle is travelling respectively Four layers of discrete laser scanning point data, every layer of laser scanning point data is multiple, and discrete four layer arrived by Real-time Collection Laser scanning point data sends to vehicle-mounted industrial computer；The corner of steering wheel angle sensor Real-time Collection vehicle, and will adopt in real time The vehicle corner of collection sends to vehicle-mounted industrial computer；

(2) data process: vehicle-mounted industrial computer obtains, according to above-mentioned four layers of laser scanning point data, road that vehicle travelling The height on twice limit, left and right, and the distance on vehicle and twice limit, left and right, calculating and plotting obtains the left and right two in section, fork, front Limit, road estimation curve；

The vehicle-mounted industrial computer real-time speed according to vehicle, the real-time corner of vehicle, calculating and plotting obtains the prediction rail that vehicle travels Trace curve；

(3) display is judged: if the prediction locus curve approximation that twice limit, left and right estimation curve travels for vehicle is parallel, display The shown in green bar in color viewing area of device；If vehicle travel prediction locus curve with limit, Liang Tiao road estimation curve a certain Place intersects, the shown in red bar in color viewing area of display.

A kind of vehicle fork the most according to claim 6 section detecting method, it is characterised in that front in step 2 During the calculating and plotting of twice limit, the left and right estimation curve in section, fork, choose fixing rectangular coordinate system (X₀,O,Y₀) for calculating The fixed reference frame of drawing process；For one layer of laser scanning point of a multi-line laser radar sensor, vehicle-mounted industrial computer will This layer of multiple laser scanning point start to be chosen for this layer the first Scanning Section according to regular length from the first laser scanning point, then with this The first laser scanning point that tail laser scanning point is this layer the second Scanning Section of layer the first Scanning Section, is chosen for this according to regular length Layer the second Scanning Section, according to the choosing method of above-mentioned this layer the second Scanning Section, this layer of remaining laser scanning point being chosen formation should The corresponding Scanning Section of layer, estimates line by each corresponding this layer of formation that connects of the head and the tail laser scanning point in this layer of all Scanning Section, should The vertical dimension that layer laser scanning point estimates line to this layer is judged as this layer of road edge point, limit, this layer of road of matching less than when setting threshold value The limit matched curve of Dian Weigaiceng road；Four layers of laser scanning point matching respectively of one multi-line laser radar sensor obtain four together Limit, the road matched curve of side；Then limit, the road matched curve to four homonymies is merged, and the abscissa value after fusion is solid with this Determine rectangular coordinate system (X₀,O,Y₀) the scale value value of axis of abscissas, corresponding four roads limit matched curve of each abscissa value Ordinate value, the ordinate value after fusion is the meansigma methods of the ordinate value of four roads limit matched curve, by the road of four homonymies Limit matched curve is merged and is obtained complete limit, a road estimation curve；Two four layers of two the multi-line laser radar sensors in left and right Laser scanning point obtains limit, the Liang Tiao road estimation curve of correspondence, i.e. obtains twice limit, the left and right estimation curve in section, fork, front.

A kind of vehicle fork the most according to claim 6 section detecting method, it is characterised in that vehicle in step 2 During the calculating and plotting of the prediction locus curve travelled, choose fixing rectangular coordinate system (X₀,O,Y₀) it is calculating and plotting process Fixed reference frame；Set up with Vehicle Speed direction as axis of abscissas, with axis of abscissas in same level and with horizontal seat Dynamic coordinate system (the X that axle is axis of ordinates that parameter is vertical_j,j,Y_j), set predicted time section T_PThe lateral velocity of interior vehicle, Longitudinal velocity and transverse acceleration, longitudinal acceleration keep constant, by T_PIt is subdivided into J decile, is often divided into Δ t_pIf, often first-class Timesharing punctum is j, j=1,2 ... J,For moment point j Vehicle Speed direction and X₀Angle, utilize simplify formula

$\left\{\begin{array}{c}{\stackrel{\·}{x}}_j={\stackrel{\·}{x}}_{j-1}+{\stackrel{\·\·}{x}}_{j-1}\·{\mathrm{\Δt}}_p\\ {\stackrel{\·}{y}}_j={\stackrel{\·}{y}}_{j-1}+{\stackrel{\·\·}{y}}_{j-1}\·{\mathrm{\Δt}}_p\end{array}\right.\left\{\begin{array}{c}{x}_j=x_{j-1}+{\stackrel{\·}{x}}_{j-1}\·{\mathrm{\Δt}}_p+\frac{1}{2}\·{\stackrel{\·\·}{x}}_{j-1}\·{{\mathrm{\Δt}}^2}_p\\ y_j=y_{j-1}+{\stackrel{\·}{y}}_{j-1}\·{\mathrm{\Δt}}_p+\frac{1}{2}\·{\stackrel{\·\·}{y}}_{j-1}\·{{\mathrm{\Δt}}^2}_p\end{array}\right.$

Calculate according to above-mentioned formula and can try to achieve dynamic coordinate system (X_j,j,Y_jUnder), vehicle is at the position coordinates of moment point j；

By dynamic coordinate system (X_j,j,Y_jUnder), vehicle is transformed into same fixing rectangular coordinate system at the position coordinates of moment point j (X₀,O,Y₀Under), if dynamic coordinate system (X_j,j,Y_j) and fixing rectangular coordinate system (X₀,O,Y₀) relative rotation angle be ψ_j, then From dynamic coordinate system (X_j,j,Y_j) to fixing rectangular coordinate system (X₀,O,Y₀) transition matrix be

$A_j=\left[\begin{array}{cc}{\cos \mathrm{\ψ}}_j& -{\sin \mathrm{\ψ}}_j\\ {\sin \mathrm{\ψ}}_j& {\cos \mathrm{\ψ}}_j\end{array}\right]$

Thus provide formula:

$\left[\begin{array}{c}{\stackrel{\·}{x}}_j\\ {\stackrel{\·}{y}}_j\end{array}\right]=\left[\begin{array}{c}{\stackrel{\·}{x}}_{j-1}\\ {\stackrel{\·}{y}}_{j-1}\end{array}\right]+A_{j-1}(\left[\begin{array}{c}{\stackrel{\·\·}{x}}_{j-1}\\ {\stackrel{\·\·}{y}}_{j-1}\end{array}\right]\·{\mathrm{\Δt}}_p)$

$\left[\begin{array}{c}{x}_j\\ y_j\end{array}\right]=\left[\begin{array}{c}{x}_{j-1}\\ y_{j-1}\end{array}\right]+\left[\begin{array}{c}{\stackrel{\·}{x}}_{j-1}\\ {\stackrel{\·}{y}}_{j-1}\end{array}\right]\·{\mathrm{\Δt}}_p+A_{j-1}\·(\frac{1}{2}\left[\begin{array}{c}{\stackrel{\·\·}{x}}_{j-1}\\ {\stackrel{\·\·}{y}}_{j-1}\end{array}\right]\·{{\mathrm{\Δt}}^2}_p)$

Vehicle-mounted industrial computer obtains, according to above-mentioned formula and then calculating and plotting, the prediction locus curve that vehicle travels.

A kind of vehicle fork the most according to claim 6 section detecting method, it is characterised in that in step 2, obtaining After the distance on the height of the left and right Dao Bian of the road that vehicle is travelling and vehicle and twice limit, left and right, utilize Kalman discrete The range data on the twice limit, left and right of the road that vehicle is being travelled by mode filter is filtered.