FIELD OF THE INVENTIONThe present invention relates to a method for calculating a collision-avoiding trajectory for a driving maneuver of a vehicle. The invention likewise relates to a method for collision avoidance for a vehicle. The invention further relates to a computer program, a computer program product, and an apparatus for executing or carrying out such methods.
BACKGROUND INFORMATIONGermanpatent document DE 10 2004 056 120 A1 relates to a method for collision avoidance or collision consequence mitigation during a driving operation in which a motor vehicle approaches an obstacle, in particular a preceding vehicle; in one method, at least the relative speed between the motor vehicle and the obstacle is ascertained; a remaining time span until the latest onset of a collision-avoiding evasive maneuver, constituting an evasion time span, is ascertained; and a collision-avoiding or collision-consequence-mitigating action is taken as a function of the evasion time span that has been ascertained.
A variety of methods for trajectory planning are known in particular from robotics, on the basis of which methods a robot can be moved in collision-free fashion through a set of obstacles. These can refer to both a manipulator and a vehicle. The motion can occur in a two- or three-dimensional space.
Methods that take into account only stationary obstacles are known from practical use. Further methods are capable of also incorporating movable obstacles. The so-called “road maps” method connects all the vertices of the sensed obstacles to one another and thereby constructs a graph over all possible paths. Based on this graph, a route through an environment having obstacles can then be calculated. A procedure of this kind is discussed, for example, in US 2005/0192749 A1. There are also methods that subdivide the overall environment into collision-free and colliding cells, and connect a selection of collision-free cells into a collision-free route through existing obstacles. Such methods are, however, suitable only for stationary obstacles.
Methods based on virtual forces allocated to the obstacles also exist. If all the obstacles possess a repelling effect on the robot, and only the destination point has an attractive effect, it is possible to construct a potential field (similar to an electric field) through which a path through the obstacles can be calculated on the basis of the cumulative total force proportional to the gradient of the field. This principle is independent of whether the obstacles are or are not moving, since the path is determined only from the resulting total force of the field at the particular current point in time. Such methods can thus be used to the same extent for moving and non-moving obstacles. The robot guidance apparatus discussed in DE 42 07 001 A1 uses a resistance lattice having nodes, each of which represents an individual and discrete position within a travelable environment, and having connections between the nodes. Connections between nodes with an open circuit result in errors when the robot attempts to move along a travelable path, and can result in collisions with obstacles within the travelable environment.
SUMMARY OF THE INVENTIONThe method according to the present invention for calculating a collision-avoiding or collision-consequence-mitigating trajectory for a driving maneuver of a vehicle, in particular a motor vehicle, in order to evade at least one obstacle approaching the motor vehicle during driving operation, the lateral speed of the motor vehicle being taken into account in the calculation independently of the longitudinal speed of the motor vehicle, enables a very effective, simple, and fast trajectory calculation for vehicles, in particular motor vehicles or vehicle-like mobile robots, for scenarios typical of traffic. The method according to the present invention is suitable in particular for traffic scenarios in which much higher speeds typically prevail in a longitudinal than in a lateral direction. It is thereby advantageously possible to reduce trajectory planning to a calculation of the lateral position for a known longitudinal position that results from the longitudinal speed (which can be assumed to be known). The originally two-dimensional route planning can thus be reduced by one dimension, resulting in considerable simplification and thus in a faster calculation.
In accordance therewith, a method is proposed which can calculate a trajectory for the own vehicle in two-dimensional space for stationary and moving obstacles, and which is suitable for scenarios and obstacle constellations of typical road traffic. The dimensional limitation allows the trajectory to be calculated easily and very effectively, although in principle an expansion to a further dimension is conceivable. The assumption of a lateral speed that is relatively small in relation to the longitudinal speed allows the longitudinal and transverse kinematics to be decoupled to a good approximation. It is thus relatively simple to find a trajectory that leads past potential obstacles in collision-free fashion. This can be achieved by the fact that the passing locations and times are determined on the basis of the known longitudinal speed of the vehicle, and the possible attainable lateral positions at those locations are calculated.
Described herein is a method for collision avoidance for a vehicle, in particular a motor vehicle.
A computer program having a program code arrangement, and a computer program product having program code arrangement that is stored on a computer-readable data medium, in order to execute the methods according to the present invention, are also described herein.
Also described herein is an apparatus, in particular to a driver assistance system of a vehicle, in particular a motor vehicle, for carrying out the method according to the present invention.
The method according to the present invention for calculating a collision-avoiding trajectory for a driving maneuver of a vehicle, or the method according to the present invention for collision avoidance for a vehicle, in particular a motor vehicle, may be implemented as a computer program on a control device of a driver assistance system of a vehicle, in particular a motor vehicle, although others solutions are of course possible. For this purpose, the computer program is stored in a memory element of the control device. The method is carried out by execution on a microprocessor of the control device. The computer program can be stored as a computer program product on a computer-readable data medium (diskette, CD, DVD, hard drive, USB memory stick, memory card, or the like) or on an Internet server, and can be transferred from there into the memory element of the control device.
Advantageous embodiments and refinements of the invention are evident from the dependent claims. An exemplifying embodiment of the invention is explained in principle below with reference to the drawings.
BRIEF DESCRIPTION OF THE DRAWINGSFIG. 1 shows a highly simplified schematic depiction of a motor vehicle in which a method according to the present invention is implemented.
FIG. 2 shows a simplified schematic depiction of a dependence of the lateral speed vyon the lateral position y of a motor vehicle.
FIG. 3 shows a simplified diagram of a propagation of a lateral speed vyas a function of the lateral position y of a motor vehicle.
FIG. 4 shows a simplified schematic depiction of a development of the attainable lateral positions y for a motor vehicle after repeated bounding by obstacles.
FIG. 5 shows a simplified depiction of blocking regions and passing gates within the method according to the present invention.
FIG. 6 shows a driving track with interpolation points, for description of a collision-avoiding trajectory.
DETAILED DESCRIPTIONA method according to the present invention for calculating a collision-avoiding trajectory13 (seeFIG. 6) for a driving maneuver of a vehicle is described below with reference to a motor vehicle1 (seeFIG. 1). In further exemplifying embodiments (not depicted), other vehicles such as, for example, vehicle-like mobile robots or the like could of course also be equipped—with corresponding modifications—with an implementation of the method according to the present invention.
FIG. 1 depictsmotor vehicle1 on which the method according to the present invention, for calculating a collision-avoidingtrajectory13 for a driving maneuver ofmotor vehicle1 in order to evade at least one obstacle11 (seeFIG. 5) approachingmotor vehicle1 during driving operation, is implemented. Based thereon, an apparatus, embodied as adriver assistance system2, for carrying out the method according to the present invention can assist the driver in evadingobstacles11, and in the event of an imminent collision can guide him or her, autonomously or semiautonomously, onto asafe trajectory13 that does not collide with anyobstacle11 surrounding theown motor vehicle1. This can be accomplished on the one hand by displaying a corresponding warning, or on the other hand by an active intervention bydriver assistance system2 by way of a correcting brake intervention via a controlled increase in braking force, or by way of a corresponding steering intervention via a steering system (not depicted) ofmotor vehicle1.Motor vehicle1 has asensing device3afor instrumental sensing of the traffic situation in front ofmotor vehicle1, and asensing device3bfor sensing the traffic situation behindmotor vehicle1. Sensingdevices3a,3bcan be embodied as an environment-sensing sensor apparatus, for example as a radar, camera, laser scanner, or the like.
Sensingdevices3a,3bare connected to anevaluation device4 with which, on the basis of the sensor signals ofsensing devices3a,3b, variables such as the distance, speed, or acceleration of preceding or following objects orobstacles11 can be ascertained and tracked.Motor vehicle1 furthermore has anarrangement5 for ascertaining vehicle data ofmotor vehicle1, for example the own-vehicle speed, own-vehicle acceleration, assumed coefficient of friction of the road, brake actuation, steering wheel actuation, and steering angle. Anarrangement5 can also be provided in other vehicle systems that are not depicted, for example in an electronic stability program (ESP) or a steering system. Acontrol device6 is electronically connected viaevaluation device4 to sensingdevices3a,3b, and through the connection receives data concerning preceding and followingobstacles11.Control device6 is furthermore electronically connected to anarrangement5 for ascertaining vehicle data,i.e. control device6 receives the corresponding vehicle data from anarrangement5 or, via a CAN bus ofmotor vehicle1, from other vehicle systems, in particular vehicle dynamics systems (not depicted). In the context ofdriver assistance system2, the method according to the present invention for collision avoidance formotor vehicle1 then executes oncontrol device6, in which method, in the context of anobstacle11 approaching during driving operation, a driving maneuver ofmotor vehicle1 for evasion is carried out autonomously or semiautonomously, or is proposed via a warning device to the driver or to further vehicle systems. The driving maneuver is in turn based ontrajectory13 ascertained by the calculation method according to the present invention, the lateral speed vyofmotor vehicle1 being taken into account in the calculation independently of the longitudinal speed ofmotor vehicle1. As a result of the decoupling of the lengthwise and transverse speeds, or of longitudinal and lateral speeds vxand vy, a lateral offset Δy can be described by the following equation (a) for a time interval Δt:
Δy=vyΔt+½ayΔt2 (a)
In order to calculate the maximum attainable lateral position Δy at thenext obstacle11, on the one hand the maximum acceleration aymust be assumed, but on the other hand the speed vyat theprevious obstacle11 must also be incorporated. The one-dimensional kinematic equations for the lateral position y and lateral speed vyfor a time interval Δt (y0and v0referring to the lateral position and lateral speed at the beginning of the calculation, i.e. to the respective starting points) are:
y=y0+v0Δt+½ayΔt2 (b)
vy=v0+ayΔt (c)
Placing equations (b) and (c) inside one another, and eliminating the time dependence, yields the following dependence (d) of speed vyon position y:
vy=±√{square root over (v02+2ay(y−y0))} (d)
It is thereby possible to calculate, for each lateral position yiofmotor vehicle1, the maximum and minimum lateral speed vysup, vyinf. This is illustrated inFIG. 2 formotor vehicle1. This yieldsareas10,10′, as depicted by way of example inFIG. 3, for a propagation in the context of a starting speed vy=5 m/s and a maximum acceleration ay=0.2 G (G=9.81 m/s2) for the period t=0 to 2.5 s. InFIG. 3, lateral position y is plotted on the vertical axis, and lateral speed vyon the horizontal axis. Based on these diagrams orareas10,10′ it is now possible to ascertain, for each passing point i or each longitudinal position xiatobstacle11, the maximum and minimum attainable positions yisup, yiinfas a position range i (yiinf, yisup), i.e. a respective associated attainable lateral position range (yiinf, yisup) ofmotor vehicle1 is calculated for the longitudinal positions xiofmotor vehicle1.
If theown motor vehicle1 is confined by obstacles11 (seeFIG. 5) or by a road edge,areas10,10′ are bounded or cut off by the fact that the upper or lower tip is detached at the relevant y position, as indicated inFIG. 3 by the partly crosshatchedarea10′ for the last propagation depicted. In thismotor vehicle1 must not be offset laterally more than +10 m out from the zero line at longitudinal position xi, since otherwise it would collide with anobstacle11. Propagation must then be continued only with this cut-off area10′, which in this case extends from yinf=6 m to ysup=10 m. Upon further propagation, the cut-off y-vydiagrams once again develop substantially in accordance with equation (d), i.e. further calculation of the attainable maximum and minimum lateral positions ysup, yinfcan be accomplished analogously. The subsequent attainable positions can therefore be determined very effectively despite repeated bounding of the travelable regions byvarious obstacles11. It is particularly advantageous that the propagation increment Δt can be varied, so that the maximum and minimum lateral speed vysup, vyinfupon arrival at thenext obstacle11 can be calculated immediately. At the level ofobstacle11, based on the maximum and minimum lateral position ysup, yinfit is possible to define a passing gate for passing, which gate on the one hand is in fact attainable bymotor vehicle1 and on the other hand is definitely collision-free.FIG. 4 illustrates this method with an exemplifying propagation sequence. Indicated for each of the four points in time t1to t4are the respective attainable lateral positions ysup, yinfthat were subsequently laterally bounded or cut off by the blocked areas or blockingregions12, depicted inFIG. 5, ofobstacles11. The attainable lateral positions ysup, yinfcan be read off, the respective passing gate that must be traveled through being indicated with crosshatching. The distances in the longitudinal direction between the passing gates are not depicted inFIG. 4 because of the orthogonal breakdown, and are required only in the form of times t1to t4.
FIG. 5 shows a sample scenario with amotor vehicle1 and fourobstacles11. Blockingregions12, which would result in a collision because of a simultaneous positional superposition ofmotor vehicle1 andobstacle11 and which therefore must not be traveled in bymotor vehicle1, are defined In a space surroundingmotor vehicle1 on the basis of the longitudinal motion ofmotor vehicle1 and the lateral position of thecorresponding obstacle11. Blockingregions12 are equipped with additional safety regions (not depicted) that take into account the geometry ofmotor vehicle1 and its possible rotation in the context of a yaw angle conditioned by travel along collision-avoidingtrajectory13. What results accordingly, in consideration of the geometry ofmotor vehicle1, is a blocked area or blockingregion12 around eachobstacle11, which region inevitably leads to a collision when entered bymotor vehicle1. The lateral position y of motor vehicle1 (the own vehicle) is limited on the basis of said blockingregions12, thus resulting in passing gates Gate1 to Gate9 that can be attained and traveled through bymotor vehicle1 and that therefore conveymotor vehicle1 in collision-free fashionpast obstacles11. The attainable lateral position ranges yiinf, yisupare thus cut off and/or bounded by therespective blocking regions12 ofobstacles11, thus yielding passing gates Gate1 to Gate9.
In the context of the method according to the present invention, an accelerated motion ofmotor vehicle1 and/or ofobstacles11 can also be taken into account in the calculation of collision-avoidingtrajectory13.
The dimensions and position of each blockingregion12 are obtained from the first contact point xiin the longitudinal direction—for example, in the case of a preceding vehicle asobstacle11, the point at which the front edge ofmotor vehicle1 touches the back of the precedingobstacle11—and from the last contact point, which is then determined by contact between the front bumper ofobstacle11 and the rear bumper ofmotor vehicle1 ifmotor vehicle1 could virtually “drive through”obstacle11. For simplicity, rotation of the geometries in blockingregion12 can be taken into account, for different yaw angles, by incorporating the first and the last contact point, and the maximum and the minimum lateral extension, independently of one another, so that blockingregion12 is defined by the surrounding bounding contour or “bounding box.” Assuming, in the simplest case, an unaccelerated motion ofmotor vehicle1 and ofobstacles11, the first and last contact time and contact point can then be calculated from the following kinematic equations:
xmotor vehicle=v0t (e)
xobstacle=x0+vobstaclet (f)
tcontact=x0/(v0−vobstacle) (g)
xcontact=(v0x0)/(v0−vobstacle) (h)
Equations (e) to (h) become correspondingly more complex and more detailed for the case of an acceleratedmotor vehicle1 and/orobstacle vehicle11; also possible is an acceleration range in which the equations must be respectively set up for the worst-case constellations, so that the extension of blockingregions12 becomes maximal. There is, however, no resulting change in the principle of calculatingblocking regions12 by way of the first and the last contact point.
As is evident fromFIG. 5, in the simplest case allobstacles11 are non-moving, so that blockingregions12 result immediately from the position ofobstacle11 at time t=0. If the lateral position ofobstacle11 at the first and the last contact time is then considered, and if the geometry ofmotor vehicle1 is taken into account using an additional safety region, blockingregion12 for eachobstacle11 can be indicated directly from these coordinates. A corresponding avoidance path must be laid out around all the blockingregions12.
In the case of a curved road, by projecting the curved road surface into a straight line it is moreover possible to apply the same method for calculatingblocking regions12, and thus to calculate a path for the curved track as well.
Based on blockingregions12, it is now possible to calculate the maximum and minimum attainable lateral offset Δy fromobstacles11 based on the speed propagation shown inFIG. 3, since the diagram supplies the attainable passing gates Gate1 to Gate9 by way of the maximum and minimum laterally attainable positions ysupand yinf, so that a possible avoidance path passes through a specific passing-gate sequence.
Anobstacle11 can be passed on both the right and the left; ifn obstacles11 are present, this yields a maximum of 2npossible evasion paths. As is evident fromFIG. 5, however, in this particular application the blockingregions12 limit the possible collision-free paths to only two rather than the maximum of 24=16 evasion paths, those being marked by the passing-gate sequences Gate1-Gate2-Gate3-Gate4-Gate5-Gate6-Gate7 and Gate1-Gate2-Gate3-Gate8-Gate9.
A collision-free evasion trajectory can now be calculated by way of an optimization method for a sequence of passing gates Gate1 to Gate9, i.e. a collision-avoidingtrajectory13 that passes through lateral position ranges yinf, ysupor passing gates Gate1 to Gate9, the upper bound yisupand lower bound yiinfof the lateral position range i (yisup, yiinf) being accounted for in the optimization method as a boundary condition. For example, under the boundary condition that for passing gate Gate(i) at position xi,trajectory13 must be less than or equal to upper gate bound yisupand greater than or equal to lower gate bound yiinf, it is possible to calculate, for example, a constant-curvature spline that passes through interpolation points (xi, yi) for i=1 to n passing gates Gate1 to Gate9. Collision-avoidingtrajectory13 is consequently described by the constant-curvature spline. In further exemplifying embodiments, collision-avoidingtrajectory13 could also be described by a polyline having equidistant interpolation points.
An optimization criterion that can be utilized to calculate the as-yet unknown interpolation points (xi, yi) for the constant-curvature spline is, for example, minimization of the integral over the square of the curvature κ(s) over the entire length l of trajectory13:
- Boundary condition: ∀yi(xi):yiinf(xi)≦yi(xi)≦yisup(xi)
The description oftrajectory13 using a spline arrangement that the integral can be solved analytically, and the optimization criterion can be indicated directly as a function of the parameters of the splines. The profile oftrajectory13 within interval i is described by a third-order polynomial:
yi(x)=p0i+p1ix+p2ix2+p3ix3 (j)
If curvature κ within interval i is set—as a very good approximation, given the typically elongated profile ofevasion trajectory13—to equal the second-order derivative yi″(x), the optimization criterion that then follows from the parameters of spline polynomial p0ito p3iis:
Any other desired optimization method and optimization criteria derived therefrom can, however, also be used. Also conceivable, for example, is a minimization of the maximum lateral acceleration that occurs, or weighted combinations of the two criteria.
As depicted inFIG. 6, collision-avoidingtrajectory13 can also be described by interpolation points at equidistant spacings. A drivingtrack14, whoseedges15a,15bboundevasion trajectory13 at the top and bottom, is thereby defined through passing gates Gate1 to Gate6. The polyline resulting therefrom must minimize the above optimization criterion, or if applicable an alternative one. What is described in accordance therewith is the possible collision-avoidingtrajectory13 through passing gates Gate1 to Gate6 that is attainable bymotor vehicle1 in accordance with its predetermined vehicle-dynamics properties, and that leads in collision-free fashionpast obstacles11.
Advantageously, the method for collision avoidance formotor vehicle1 can then be operated onmotor vehicle1, in which method, in the context of anobstacle11 approaching during driving operation, a driving maneuver ofmotor vehicle1 for evasion is autonomously or semiautonomously carried out or proposed, the driving maneuver being based on collision-avoidingtrajectory13.