CN104537834A - Intersection identification and intersection trajectory planning method for intelligent vehicle in urban road running process - Google Patents

Abstract

The invention relates to an intersection identification and intersection trajectory planning method for an intelligent vehicle in an urban road running process. The method is actual application of an automatic driving technology and an auxiliary driving technology on the basis of intelligent traffic. The method is characterized in that intersections are divided into four types of typical intersection bodies, intersection identification is conducted through a multisensory infusion mode and a four-layer early warning template matching method, road edge disappearing points are determined as intersection entry points, template center points matched with the intersections are determined as intersection exit points, trajectory planning is conducted through a double-RRT method as so to achieve intersection identification and tracking. The intersection identification and intersection trajectory planning method has the advantages that the intelligent vehicle can cope with the complex environment of urban traffic intersections through intersection identification and planning, the intelligent driving problems that the intelligent vehicle is lost at the intersections and can pass through the intersections only through accurate navigation are solved, the adaptive capacity to the environment of the intelligent vehicle is improved, and important technological support is provided for intelligent driving under the complex urban environment.

Description

The crossing of a kind of intelligent vehicle in urban road travels identifies the method with crossing trajectory planning

Technical field

The present invention relates to the crossing identification of a kind of intelligent vehicle in urban road travels and the method for crossing trajectory planning, belong to intelligent transportation field.

Background technology

Automated driving system occupies an important position at intelligent transportation field, is the product that multiple technology-oriented discipline such as Artificial intelligence, robotics, control theory and electronic technology intersects.It is made according to the information that each sensor obtains and analyzes and judge, people is freed from single lasting driving-activity, reduce driving behavior difference to the impact of traffic flow stability, the vehicle pass-through rate being conducive to improving existing road network alleviates traffic congestion, ride safety of automobile can be improved on the other hand, reduce road accident rate and improve traffic safety, reduce energy resource consumption and environmental pollution, have great strategic importance to China's energy decreasing pollution transition, alleviation traffic congestion and automobile industry autonomous innovation.

The final purpose of intelligent vehicle realizes replacing driver in traveling, complete the starting of driver, stop, cruise, track keeps, the driving behaviors such as barrier, vehicle lane-changing are kept away in track, on the basis of dynamics of vehicle, figure is built in combining environmental perception, location, and the modules such as planning navigation, wagon control, carry out map foundation to surrounding environment, recognition and tracking is carried out to road signs and barrier, undertaken planning by discovery learning and navigate, carrying out transverse and longitudinal control, finally realize autonomous driving.But urban traffic environment is complicated, intersection road structure is complicated, and have again the impact of pedestrian, traffic signals and traffic sign, intelligent vehicle is difficult to carry out decision-making and planning.To location, crossing generally based on the road network file of GPS information, but GPS jitter under the impact of urban skyscraper thing sometimes, be difficult to accurately locate, cannot intelligent driving be completed, so crossing identifies with planning under urban highway traffic environment is a difficult problem urgently to be resolved hurrily.

Summary of the invention

The present invention is intended to overcome above deficiency, a kind of intelligent vehicle is provided effectively to identify the method with crossing trajectory planning in crossing in urban road travels, crossing is divided into four kinds of typical types, application Multi-sensor Fusion and four layers of early warning template matching method, carry out crossing identification, crossing inlet point is determined by the central point of both sides curb end point, the template center's point matched with this crossing is as crossing exit point, crossing global path planning is carried out with two RRT planing method, carry out local path planning with five subparameter equations and follow the tracks of crossing global path, identification and the trajectory planning at crossing can be realized comparatively accurately, effectively can solve intelligent driving crossing in the complex environment of city and identify the difficult problem with planning, the application of following intelligent driving vehicle can alleviate urban traffic blocking to a great extent, ubiquitous traffic problems in the big city such as traffic safety and environmental pollution.

For achieving the above object, the present invention by the following technical solutions:

Intelligent vehicle crossing in urban road travels effectively identifies and comprises the following steps the method with crossing trajectory planning:

(1) set up two-dimensional grid map in conjunction with single line radar, four line radars and camera data, body of a map or chart is 50m*20m, and the size of each grid is 20cm*20cm;

(2) carry out crossing classification, with four typical crossing models for template, be respectively crossroad, T-shaped road junction, the T-shaped crossing in T-shaped crossing, right side and left side;

(3) four layers of early warning sensor merge identification crossing, and ground floor early warning, when vehicle camera recognizes traffic sign or traffic lights, produces the first early warning signal; Then open second layer early warning, in grating map, set single line radar data rectangular area, long with 8m-16m, the wide dynamic rectangular region of 10m is template, with from left to right, order from top to bottom carries out template slip, determines whether have barrier in rectangular area.If Signal aspects exists barrier in rectangular area, illustrate that this region does not meet crossing and to pass through condition, system stops path planning program, and waiting area barrier is removed.If there is no barrier, then carry out the coupling of four standard crossing shapes, determine concrete crossing shape, open third layer early warning afterwards; Third layer early warning is applied four line radar datas and is judged curb information, and when sweeping to curb, four layers of radar data projection overlap as straight line, determine curb point accordingly.Open the 4th layer of early warning after obtaining curb radar points, calculate curb end point to determine exact position, crossing;

(4) central point in curb end point region is defined as crossing inlet point, determine that the central point of crossing template is crossing exit point in conjunction with template matching method, the two RRT method of application carries out global path planning when vehicle travels at crossing place, carry out Track Pick-up sector planning with five subparameter curves simultaneously, make its formation curve continual curvature everywhere, remove the curve that side acceleration is greater than the corresponding curvature of 0.4g, wherein g is acceleration of gravity, choose and barrier zero lap curve, follow the tracks of the path, overall crossing that two RRT generates.

A kind of intelligent vehicle effectively identifies the method with crossing trajectory planning in crossing in urban road travels, be platform based on intelligent vehicle, adopt camera, single line radar and four line radars gather environmental information and build grating map, application camera carries out the identification of traffic sign and traffic lights, the application UDP network transmission protocol carries out network service, and all obstacle informations and early warning information are transferred to planning layer.Obtain traffic lights information and road signs information by camera, open single line radar, dynamic rectangular region is set and carries out crossing judgement, open four line radars and curb and curb end point are determined.The two RRT algorithm of application carries out crossing global path planning, and generates the global path of track following RRT generation with five subparameter equations.

Beneficial effect

Owing to have employed the mode of four layers of early warning layer and Multi-sensor Fusion template matches, crossing typical in urban road is identified and path planning, make intelligent vehicle by during crossing without the need to accurate locating information, determine the ingress and egress point at crossing simultaneously, the method of the two RRT random tree of application carries out crossing path planning, efficiently solves the difficult problem that intelligent vehicle travels at crossing, city.Because vehicle is a nonholonomic motion system, there is kinematic and dynamic constraints, the path of not all generation all meets the requirement of vehicle actual travel, and therefore trajectory planning process must meet related constraint, with vehicle driving trace in the environment of better matching actual crossing.Random tree method embedded in vehicle kinematics constraint generation pass, and planning needs to determine that crosswise joint parameter makes vehicle travel along path planning after obtaining the path of vehicle expectation traveling.Although provide the expectation crosswise joint amount of any position vehicle on expected path, but due to error and the time delay existence of control system, these controlled quentity controlled variables directly can not be used for path trace as controling parameters, but need the real time kinematics state according to intelligent vehicle, by by the FEEDBACK CONTROL minimized between vehicle and expected path for the purpose of position and course deviation, could determine that suitable crosswise joint amount is to realize path trace accurately.Rational crosswise joint amount is determined by path following control device, therefore in order to ensure that in complex environment intelligent vehicle can safety traffic, path following control device except can meet follow the tracks of path planning function except, also must have barrier avoiding function simultaneously.Although traditional can obtain good path trace effect based on the tracking control unit taken aim in advance with very little operand, but does not have barrier avoiding function.For this reason, the present invention devises five subparameter equations and generates track and add dynamics of vehicle constraint, enables generation track follow the tracks of the path planning of random tree generation, possesses barrier avoiding function simultaneously, ensures vehicle smooth ride safety.

Accompanying drawing illustrates:

Fig. 1 is algorithm block diagram of the present invention

Fig. 2 is the present invention's four layers of early warning block diagram

Fig. 3 is crossing of the present invention classification chart

Fig. 4 is crossing of the present invention template shiding matching

Fig. 5 is curb identification of the present invention

Fig. 6 is that the two RRT of the present invention plans generation figure

Fig. 7 is that the present invention five subparameter curve produces starting and terminal point trajectory diagram

Fig. 8 is the trajectory diagram that the present invention does not comprise dynamics of vehicle constraint

Fig. 9 is the trajectory diagram that the present invention comprises dynamics of vehicle constraint

Figure 10 is that crossing of the present invention identifies program results

Embodiment:

Be algorithm flow chart shown in Fig. 1, first intelligent vehicle is by environment sensing module perception environmental information, and builds map.Environment sensing module mainly installs three sensors, camera, single line radar and four line radars.Camera is used for identifying traffic lights and traffic sign, and single line radar carries out the identification of barrier, sets up the grating map based on barrier simultaneously, and four line radars, to scanning, apply the end point of four line radar data determination curbs and curb nearby.Open four layers of early warning sensor to merge and template matching method, carry out identification and the planning at crossing, early warning layer one: the early warning of traffic sign and traffic lights, after camera recognizes traffic sign and traffic lights, open early warning layer two at once: it is 8-16m that application single line radar data builds length, width is the dynamic rectangular region of 10m, carries out shiding matching with four standard crossing shapes.Open early warning layer three, apply four line radar datas and obtain curb information, when sweeping to curb, coincidence is straight line by the projection of four layers of radar data, determines curb point accordingly.Open early warning layer four, determine the central point in curb end point region, and this is defined as crossing inlet point, in conjunction with template matching method with template center's point for crossing exit point.The method of the two RRT random tree of application carries out the trajectory planning by crossing, and application configuration space carries out Track Pick-up in conjunction with vehicle kinematics constraint.RRT method K summit below rapid build from initial point, RRT method is made up of following four parts: sensor selection problem, growing direction, increases Distance geometry and detects and connect, and then generate feasible path.

Be illustrated in figure 2 four layers of planning early warning, when vehicle camera recognizes traffic sign or traffic lights, produce the first early warning signal, second layer early warning is simultaneously opened, and sets single line radar data rectangular area in grating map, long with 8m-16m, the dynamic rectangular region that 10m is wide, with from left to right, order from top to bottom carries out template slip, judges whether have barrier in rectangular area.If Signal aspects exists barrier in rectangular area, illustrate that this region does not meet crossing and to pass through condition, system stops path planning program, and waiting area barrier is removed.If there is no barrier, then carry out the coupling of four standard crossing shapes, determine concrete crossing shape, open third layer early warning afterwards.Four line radar determination curb information are applied in third layer early warning, if curb information is determined, open the 4th early warning layer and calculate curb end point to determine exact position, crossing.

Crossing is divided into four basic crossing shapes by the present invention, as classification model, as shown in Figure 3: crossroad, left T crossing, right T crossing, T-shaped road junction.

Be illustrated in figure 4 template matches mode, application single line radar scanning data acquisition rectangle frame carries out double-template slip, and sliding process is from top to bottom, from left to right, and the dynamic template length that changes mates with standard crossing simultaneously.

Be illustrated in figure 5 curb identification figure, when four line radars carry out curb identification, curb is a longitudinal tangent plane, when the total data of four lines of four line radars projects on a longitudinal surface, four line data be scattered in a little as straight line, as radar data point in Fig. 5 describes the distribution in alignment of curb part four line radar points.

Figure 6 shows that application configuration space carries out Track Pick-up in conjunction with vehicle kinematics constraint, RRT method K summit below rapid build from initial point, be made up of following four parts: sensor selection problem, growing direction, increase Distance geometry and detect connection generation feasible path.Carry out coordinates measurement from path starting point and path termination, finally overlapping forms feasible path simultaneously.Vehicle kinematics constraint is as follows: x y is transverse and longitudinal coordinate, and θ is yaw angle, and l is wheelbase, and δ is front wheel slip angle, be respectively longitudinal velocity and side velocity, for yaw velocity, v is car speed.

$\left\{\begin{array}{c}\stackrel{.}{x}=v\cos \mathrm{\θ}\\ \stackrel{.}{y}=v\sin \mathrm{\θ}\\ \stackrel{.}{\mathrm{\θ}}=(v/l)\tan \mathrm{\δ}\end{array}\right.$

Fig. 7 is that employing five subparameter curve local path generates schematic diagram, and Fig. 8 is the geometric locus not comprising dynamics of vehicle constraint, and Fig. 9 is the geometric locus meeting dynamics of vehicle constraint.

In order to ensure vehicle driving safety in complex environment, path following control device, except can meeting the function of tracking path planning, also must have barrier avoiding function simultaneously.Although traditional can obtain good path trace effect based on the tracking control unit taken aim in advance with very little operand, but does not possess barrier avoiding function.For this reason, the present invention adopts quintic algebra curve parametric equation to carry out Track Pick-up:

x(u)＝x ₀+x ₁u+x ₂u ²+x ₃u ³+x ₄u ⁴+x ₅u ⁵

y(u)＝y ₀+y ₁u+y ₂u ²+y ₃u ³+y ₄u ⁴+y ₅u ⁵

W(u)＝[x(u),y(u)] ^T,u∈[0,1].

The span of u parameter is 0-1; The slope of five subparameter equations is:

$k\left(u\right)=[\stackrel{.}{x}\left(u\right)\stackrel{..}{y}\left(u\right)-\stackrel{..}{x}\left(u\right)\stackrel{.}{y}\left(u\right)]/{[{\left(\stackrel{.}{x}\left(u\right)\right)}^2+{\left(\stackrel{.}{y}\left(u\right)\right)}^2]}^{3/2}$

Single order and second derivative are:

$\stackrel{.}{y}\left(u\right)=y_1+2y_{2}u+3y_3{u}^2+4y_4{u}^3+5y_5{u}^4$

$\stackrel{..}{x}\left(u\right)=2x_2+6x_{3}u+12x_4{u}^2+20x_5{u}^3$

$\stackrel{..}{y}\left(u\right)=2y_2+6y_{3}u+12y_4{u}^2+20y_5{u}^3$

Wherein, x, y are location parameters, and u is parametric variable, as u=0, and (x, y) corresponding start position; And as u=1, (x, y) corresponding final position.

The position of given starting and terminal point, after course and curvature condition, the coefficient of five subparameter curves can be calculated by following formula:

x ₀＝x _A

x ₁＝η ₁cosθ _A

$x_2=\frac{1}{2}({\mathrm{\η}}_3\cos {\mathrm{\θ}}_A-{{\mathrm{\η}}_1}^2{k}_A\sin {\mathrm{\θ}}_A)$

$x_3=10(x_B-x_A)-({6\mathrm{\η}}_1+\frac{3}{2}{\mathrm{\η}}_3)\cos {\mathrm{\θ}}_A-({4\mathrm{\η}}_2-\frac{1}{2}{\mathrm{\η}}_4)\cos {\mathrm{\θ}}_B+\frac{3}{2}{\mathrm{\η}}_1^2{k}_A\sin {\mathrm{\θ}}_A-\frac{1}{2}{{\mathrm{\η}}_2}^2{k}_B\sin {\mathrm{\θ}}_B$

$x_4=-15(x_B-x_A)+({8\mathrm{\η}}_1+\frac{3}{2}{\mathrm{\η}}_3)\cos {\mathrm{\θ}}_A+({7\mathrm{\η}}_2-{\mathrm{\η}}_4)\cos {\mathrm{\θ}}_B-\frac{3}{2}{{\mathrm{\η}}_1}^2{k}_A\sin {\mathrm{\θ}}_A+{{\mathrm{\η}}_2}^2{k}_B\sin {\mathrm{\θ}}_B$

$x_5=6(x_B-x_A)-({3\mathrm{\η}}_1+\frac{1}{2}{\mathrm{\η}}_3)\cos {\mathrm{\θ}}_A-({3\mathrm{\η}}_2-\frac{1}{2}{\mathrm{\η}}_4)\cos {\mathrm{\θ}}_B+\frac{1}{2}{{\mathrm{\η}}_1}^2{k}_A\sin {\mathrm{\θ}}_A-\frac{1}{2}{{\mathrm{\η}}_2}^2{k}_B\sin {\mathrm{\θ}}_B$

y ₀＝y _A

y ₁＝η ₁sinθ _A

$y_2=\frac{1}{2}({\mathrm{\η}}_3\sin {\mathrm{\θ}}_A-{{\mathrm{\η}}_1}^2{k}_A\cos {\mathrm{\θ}}_A)$

$y_3=10(y_B-y_A)-({6\mathrm{\η}}_1+\frac{3}{2}{\mathrm{\η}}_3)\sin {\mathrm{\θ}}_A-({4\mathrm{\η}}_2-\frac{1}{2}{\mathrm{\η}}_4)\sin {\mathrm{\θ}}_B+\frac{3}{2}{\mathrm{\η}}_1^2{k}_A\cos {\mathrm{\θ}}_A-\frac{1}{2}{{\mathrm{\η}}_2}^2{k}_B\cos {\mathrm{\θ}}_B$

$y_4=-15(y_B-y_A)+({8\mathrm{\η}}_1+\frac{3}{2}{\mathrm{\η}}_3)\sin {\mathrm{\θ}}_A+({7\mathrm{\η}}_2-{\mathrm{\η}}_4)\sin {\mathrm{\θ}}_B+\frac{3}{2}{{\mathrm{\η}}_1}^2{k}_A\cos {\mathrm{\θ}}_A-{{\mathrm{\η}}_2}^2{k}_B\cos {\mathrm{\θ}}_B$

$y_5=6(y_B-y_A)-({3\mathrm{\η}}_1+\frac{1}{2}{\mathrm{\η}}_3)\sin {\mathrm{\θ}}_A-({3\mathrm{\η}}_2-\frac{1}{2}{\mathrm{\η}}_4)\sin {\mathrm{\θ}}_B-\frac{1}{2}{{\mathrm{\η}}_1}^2{k}_A\cos {\mathrm{\θ}}_A+\frac{1}{2}{{\mathrm{\η}}_2}^2{k}_B\cos {\mathrm{\θ}}_B$

X ₁~ x ₅y ₁~ y ₅be respectively non trivial solution, η ₁, η ₂, η ₃, η ₄be constrained parameters, obtained by optimization computation, x _afor A point horizontal ordinate, y _afor A point ordinate, θ _afor A point course angle, x _bfor B point horizontal ordinate, y _bfor B point ordinate, θ _bfor B point course angle.

Solve equation of locus and obtain parametric line as shown in Figure 8, remove the geometric locus that side acceleration is greater than 0.4g (g is acceleration of gravity), result as shown in Figure 9.

Figure 10 is that crossing identifies the result with planning, black point spreading point is the obstacle object point that detections of radar arrives, the long curved path of grey is the crossing global path of RRT planning, in figure from the cluster curve of any be the local track path that quintic curve generates, according to the optimal path of the matching degree determination parametric controller of path and expected path.

Key of the present invention determines crossing entry and exit point by four layers of early warning system, carries out path planning and local trajectory planning, be further detailed below in conjunction with example to the communication of modules and flow process:

1. initialization four layers of early warning system, to judge whether camera has traffic sign or traffic lights information to determine whether open system early warning system.

2. apply four line radars and carry out curb identification, four layer data are projected to same plane, and coincidence straight line portion is curb, determines that the central point in curb end point region, both sides is entrance, crossing, and calculate crossing point coordinate, with template matches crossing central point for exit point.

3. the two RRT random tree paths planning method of application carries out global path planning to the path by crossing, to be starting point and crossing exit point be entrance, crossing terminal simultaneously coordinates measurement, can generation feasible path faster

4. apply the feasible trajectory that five subparameter equations generate slope rate continuity, remove side acceleration be greater than the unstability curve of 0.4g (g is acceleration of gravity) and encounter the curve of barrier, choose and depart from minimum curve with global path, carry out in conjunction with vehicle width information the traveling that avoiding barrier completes crossing simultaneously.

Claims (1)

1. intelligent vehicle crossing in urban road travels effectively identifies and comprises the following steps the method with crossing trajectory planning:

(1) set up two-dimensional grid map in conjunction with single line radar, four line radars and camera data, body of a map or chart is the size of each grid of 50m*20m is 20cm*20cm;

(2) carry out crossing classification, with four typical crossing models for template, be respectively crossroad, T-shaped road junction, the T-shaped crossing in T-shaped crossing, right side and left side;

(3) four layers of early warning sensor merge identification crossing, and ground floor early warning, when vehicle camera recognizes traffic sign or traffic lights, produces the first early warning signal; Then open second layer early warning, in grating map, set single line radar data rectangular area, long with 8m-16m, the wide dynamic rectangular region of 10m is template, with from left to right, order from top to bottom carries out template slip, determines whether have barrier in rectangular area; If Signal aspects exists barrier in rectangular area, illustrate that this region does not meet crossing and to pass through condition, system stops path planning program, and waiting area barrier is removed; If there is no barrier, then carry out the coupling of four standard crossing shapes, determine concrete crossing shape, open third layer early warning afterwards; Third layer early warning is applied four line radar datas and is judged curb information, and when sweeping to curb, four layers of radar data projection overlap as straight line, determine curb point accordingly; Open the 4th layer of early warning after obtaining curb radar points, calculate curb end point to determine exact position, crossing;

(4) central point in curb end point region is defined as crossing inlet point, determine that the central point of crossing template is crossing exit point in conjunction with template matching method, the two RRT method of application carries out global path planning when vehicle travels at crossing place, carry out Track Pick-up sector planning with five subparameter curves simultaneously, make its formation curve continual curvature everywhere, remove the curve that side acceleration is greater than the corresponding curvature of 0.4g, wherein g is acceleration of gravity, choose and barrier zero lap curve, follow the tracks of the path, overall crossing that two RRT generates.