System and method for collision avoidance maneuver path determination with jerk limit

[0001] The present invention is related to vehicle collision avoidance determination based on, for example, sensor measured data, vehicle dynamics constraints, and other information.

[0002] Many vehicles are equipped with systems and devices to semi-autonomously or autonomously control a vehicle to avoid a collision or mitigate the severity of a collision. Typical collision avoidance control systems may, for example, provide pre-collision preparation, output warning(s) to the driver when a collision threat exists, apply automated braking, fully or partially control vehicle steering of vehicle, and/or perform other functions.

[0003] Collision avoidance control systems may control the steering of a vehicle to guide the vehicle around an object that may pose a collision threat to the vehicle. Collision avoidance control systems may, for example, determine a vehicle path around an object based on sensor measured data and provide the determined vehicle path to an automated vehicle control system. Typical collision avoidance vehicle paths may subject vehicle occupants to large lateral acceleration, jerk (e.g., the rate of change of lateral acceleration), and other types of loads.

[0004] In a vehicle, an optimal path curvature limited by one or more constraints may be determined. The constraints may be related to lateral jerk and one or more vehicle dynamics constraints. Based on the optimal path curvature, an optimal vehicle path around an object may be determined. The optimal vehicle path may be output to a collision avoidance control system. The collision avoidance control system may cause the vehicle to take a certain path.

[0013] It will be appreciated that for simplicity and clarity of illustration, elements shown in the figures have not necessarily been drawn to scale. For example, the dimensions of some of the elements may be exaggerated relative to other elements for clarity. Further, where considered appropriate, reference numerals may be repeated among the figures to indicate corresponding or analogous elements.

[0014] In the following detailed description, numerous specific details are set forth in order to provide a thorough understanding of the invention. It will however be understood by those skilled in the art that the present invention may be practiced without these specific details. In other instances, well-known methods, procedures, and components have not been described in detail so as not to obscure the present invention.

[0015] Unless specifically stated otherwise, as apparent from the following discussions, it is appreciated that throughout the specification discussions utilizing terms such as “processing,” “computing,” “storing,” “determining,” “evaluating,” “calculating,” “measuring,” “providing,” “transferring,” or the like, refer to the action and/or processes of a computer or computing system, or similar electronic computing device, that manipulates and/or transforms data represented as physical, such as electronic, quantities within the computing system's registers and/or memories into other data similarly represented as physical quantities within the computing system's memories, registers or other such information storage, transmission or display devices.

[0016] A collision avoidance control system may be a semi-autonomous driving system that controls the input to for example automated steering, differential braking, automated braking, and/or other systems. The collision avoidance control system may, for example, measure the relative location of objects (e.g., a vehicle, obstacle in the road) with respect to a vehicle (e.g., a host vehicle or other vehicle). The relative location of objects with respect to a vehicle may, for example, mean relative to the vehicle (where relative to the vehicle may in some embodiments mean relative to a specific point within a vehicle, relative to a global positioning system (GPS) system within the vehicle, relative to the center of the vehicle, etc.). If the relative distance between the host vehicle and an object is within a predefined distance and the relative velocity of the host vehicle with respect to the object or other reference is within a predefined value and/or range, a collision avoidance control system may determine that the object poses a collision threat to the vehicle. In response, the collision avoidance control system may output a signal to the driver (e.g., an audible warning), output a command to a braking system to reduce the vehicle speed, output a steering angle command to an automated steering system, and/or perform other actions.

[0017] According to some embodiments, if an object which poses a collision threat to the host vehicle is detected, the collision avoidance control system may first output a signal to the driver. If the driver does not mitigate the collision threat, the collision avoidance control system may control the vehicle to avoid the collision or mitigate the severity of the collision. The collision avoidance control system may output control commands to an automated braking system, automated steering control system, and/or other system. For example, if the relative distance between the vehicle and the object is less than a predefined threshold, automated braking may be applied to avoid collision. In some embodiments for some situations, the collision avoidance control system may determine that the vehicle could avoid collision by driving around the object.

[0018] According to some embodiments, the collision avoidance control system may calculate a desired path or optimal path around the object. Once calculated, the vehicle may be guided on the desired path by steering angle control, steering torque control, or other commands output to an automated steering control system.

[0019] The desired or optimal path may, for example, be determined or calculated based on an optimal path curvature. The optimal path curvature may, for example, be represented by, defined by, or may be a piece-wise linear function of distance and/or time (e.g., piece-wise linear optimal path curvature). For example, the piece-wise linear function may define or represent the relation between path or trajectory curvature (e.g., in units of l/m or other units) and time, between path or trajectory curvature and horizontal distance, and/or between path or trajectory curvature and another parameter or set of parameters. The piece-wise linear function may, in some embodiments, satisfy or be defined by one or more pre-defined curvature limits or constraints, one or more curvature rate limits or constraints, and/or other limits or constraints. Curvature limits or constraints may, for example, represent or be related to vehicle dynamics constraints or limits such as a vehicle lateral acceleration limit or constraint. Limits on curvature rate may, for example, represent or be related to vehicle dynamics constraints or limits such as a lateral acceleration rate limit or constraint. Lateral acceleration rate may, for example, be the rate of change of lateral acceleration or jerk of the vehicle. Other thresholds may be used.

[0020] According to some embodiments, an optimal or desired vehicle path may be determined or calculated based on the determined optimal path curvature. Optimal or desired vehicle path may, in some embodiments, be calculated based on optimal path curvature by integrating optimal path curvature twice with respect to longitudinal position (e.g., determining or calculating a second integral of optimal path curvature) or using other mathematical approaches or methods. For example, optimal vehicle path may be, may include, may lie on, or may be defined by multiple quadratic, cubic and/or other order path segments or function(s) separated by waypoints or breakpoints. Waypoints or breakpoints may, for example, be points where the character of the optimal vehicle path curve or other curve changes and may be points in which the functions defining the optimal path, the first derivative of the optimal path, and the second derivative of the optimal path are continuous. Waypoints may, for example, be determined by matching conditions for optimal vehicle path, the first derivative of optimal vehicle path, and the second derivative of optimal vehicle path at the waypoints (e.g., points or transitions between curve segments).

[0021] According to some embodiments, the optimal vehicle path may be output to a vehicle collision avoidance control system, or another vehicle control system. The collision avoidance control system may determine one or more vehicle steering angle control commands, steering torque commands, or other commands to guide the vehicle on the optimal or desired path. The collision avoidance control system may, for example, output steering angle control commands, steering torque commands, or other commands to an electrical power steering (EPS), active front steering (AFS), active rear steering, differential braking system, or other system to guide the vehicle on the optimal vehicle path.

[0022] FIG. 1 is a schematic diagram of a vehicle with a collision avoidance control system according to an embodiment of the present invention. A vehicle or host vehicle 10 (e.g., a car, truck, or another vehicle) may include a collision avoidance control system or device 100. Collision avoidance control system 100 may operate in conjunction with or separate from one or more automatic vehicle control systems, autonomous driving applications or vehicle automated steering systems 90. Vehicle automated steering system 90 may, for example, be or include an adaptive lane centering, low speed lane centering, lane keeping assist, and/or other application(s). One or more vehicle automated steering system(s) 90 may be component(s) of system 100, or vehicle automated steering system(s) 90 may be separate from system 100. Vehicle automated steering system 90 may, when engaged, fully or partially control the steering of the vehicle and alter or reduce driver (e.g., operator of the vehicle) steering control input via the steering wheel 82 and/or steering system, which may include an electrical power steering (EPS) system and/or other components.

[0023] One or more sensor(s) may be attached to or associated with the vehicle 10. A computer vision sensor (e.g., a camera) 24, light detection and ranging (LIDAR) sensor 20 (e.g., laser radar (LADAR) sensor), a radio detection and ranging (RADAR) sensor 22, or other remote sensing device may obtain data allowing system 100 to determine or measure the relative location of vehicle 10 with respect to objects (e.g., other vehicles, target vehicle(s)), lane markers(s), road shoulder(s), median barrier(s), edge of the road, and/or other features. Camera 24 may, for example, measure distance to and/or relative orientation of objects (e.g., objects which may pose a collision threat) and other vehicles (e.g., vehicles in front of vehicle 10 that may pose a collision threat). Camera 24 may, for example, measure lane offset, heading angle, lane curvature and provide the information to system 90.

[0024] In one embodiment of the present invention, vehicle 10 may include one or more devices or sensors to measure vehicle steering input, vehicle steering conditions, vehicle steering parameters, vehicle dynamics, driver input, or other vehicle related conditions or measurements. The vehicle dynamics measurement device(s) may include one or more steering angle sensor(s) 70 (e.g., connected to steering wheel 82 and/or another component of the steering system). The vehicle dynamics measurement device(s) may also include one or more accelerometer(s) 72, speedometer(s) 74, wheel speed sensor(s) 76, inertial measurement unit(s) (IMU) 78, gear shift position sensor(s) 84, gas pedal position sensor 86, brake pedal position sensor 88, or other devices. The vehicle dynamics measurement device(s) may measure driver input or vehicle dynamics parameters including lateral (i.e., angular or centripetal) acceleration, longitudinal acceleration, lateral jerk (e.g., rate of change of lateral acceleration, jolt, surge, lurch), longitudinal jerk, steering angle, steering torque, steering direction, yaw-rate, lateral and longitudinal velocity, wheel rotation velocity and acceleration, and other vehicle dynamics characteristics of vehicle 10. The measured vehicle dynamics, vehicle conditions, steering measurements, steering conditions, or driver input information may be sent or transferred to system 100 via, for example, a wire link 40 (e.g., a controller area network (CAN) bus, Flexray bus, Ethernet cable) or a wireless link. The measured vehicle dynamics, vehicle conditions, steering measurements, steering conditions, or driver input information data may be used by system 100 or another system to calculate optimal or desired path curvature, optimal or desired vehicle path, and/or other parameters.

[0025] in one embodiment of the present invention, collision avoidance control system 100 may include a computing device mounted on the dashboard of the vehicle, in passenger compartment 50, or in trunk 60. In alternate embodiments, collision avoidance control system 100 may be located in another part of the vehicle, may be located in multiple parts of the vehicle, or may have all or part of its functionality remotely located (e.g., in a remote server or in a portable computing device such as a cellular telephone).

[0026] While various sensors and inputs are discussed, in certain embodiments only a subset (e.g. one) type of sensor or input may be used.

[0027] FIG. 2 is a schematic diagram of a collision avoidance control system according to an embodiment of the present invention. Collision avoidance control system 100 may include one or more processor(s) or controller(s) 110, memory 120, long term storage 130, input device(s) or area(s) 140, and output device(s) or area(s) 150. Input device(s) or area(s) 140 may be, for example, a touchscreen, a keyboard, microphone, pointer device, or other device. Output device(s) or area(s) 150 may be, for example, a display, screen, audio device such as speaker or headphones, or other device. Input device(s) or area(s) 140 and output device(s) or area(s) 150 may be combined into, for example, a touch screen display and input which may be part of system 100.

[0028] System 100 may include one or more databases 170, which may include, for example, lateral acceleration limit value(s), curvature limit value(s), lateral acceleration rate limit value(s) (e.g., lateral jerk limit values), curvature rate limit value(s), object width(s) (e.g., target vehicle width), vehicle longitudinal velocity, and other information. Databases 170 may be stored all or partly in one or both of memory 120, long term storage 130, or another device.

[0029] Processor or controller 110 may be, for example, a central processing unit (CPU), a chip or any suitable computing or computational device. Processor or controller 110 may include multiple processors, and may include general-purpose processors and/or dedicated processors such as graphics processing chips. Processor 110 may execute code or instructions, for example, stored in memory 120 or long-term storage 130, to carry out embodiments of the present invention.

[0030] Memory 120 may be or may include, for example, a Random Access Memory (RAM), a read only memory (ROM), a Dynamic RAM (DRAM), a Synchronous DRAM (SD-RAM), a double data rate (DDR) memory chip, a Flash memory, a volatile memory, a non-volatile memory, a cache memory, a buffer, a short term memory unit, a long term memory unit, or other suitable memory units or storage units. Memory 120 may be or may include multiple memory units.

[0031] Long term storage 130 may be or may include, for example, a hard disk drive, a floppy disk drive, a Compact Disk (CD) drive, a CD-Recordable (CD-R) drive, a universal serial bus (USB) device or other suitable removable and/or fixed storage unit, and may include multiple or a combination of such units.

[0032] FIG. 3 is a schematic diagram of a collision avoidance control system controlling a vehicle along an optimal vehicle path according to embodiments of the present invention. Collision avoidance control system 100 may be passive or operate in the background during normal vehicle operation. System 100 may become active when, for example, vehicle sensor data indicates likelihood of imminent collision, a collision threat, or at other times. For example, a vehicle 10 may approach or encounter an object 202 (e.g., a vehicle, stationary object, or other obstacle of object or vehicle width 204) in the road. If vehicle 10 is within a predefined distance to the object 202 that poses a collision threat, within a predefined velocity range, and/or within a predefined acceleration range, system 100 or other systems associated with vehicle 10 may, for example, provide pre-collision preparation and/or warnings to the driver of vehicle 10. The warnings to driver of vehicle may be a signal, for example, an audible warning, a warning light or other form of warning. If the driver does not mitigate the collision threat, collision avoidance control system 100 may control vehicle 10, for example, through automated steering control, automated braking, and/or other controls or maneuvers in order to avoid obstacle 202 or mitigate the impact between vehicle 10 and object 202. Automated braking may, for example, include common or typical braking (e.g., applying all brakes simultaneously), differential braking (e.g., applying brakes on each wheel independent of other wheel brakes), and/or another braking system or method. Common braking may, for example, be used to control vehicle 10 in a longitudinal direction. Differential braking may, for example, be used to control vehicle 10 in both longitudinal and lateral directions.

[0033] Which evasive maneuver or actuation system (e.g., automated steering, differential braking, automated braking, or other system) collision avoidance control system 100 employs may be based on, for example, the velocity of vehicle 10, relative distance between vehicle 10 and object 202, other objects in the road, dimensions of the road, curvature of the road, and other factors. For example, if vehicle 10 is within a predefined steering maneuver threshold distance 210, it may not be possible for vehicle 10 to drive

[0034] According to some embodiments, collision avoidance control system 100 may determine that vehicle 10 may avoid collision with object 202 by maneuvering around the object. Collision avoidance control system 100 may, for example, determine or calculate an optimal or desired vehicle path 220 around object 202. Optimal vehicle path 220 may, for example, be a path that guides vehicle 10 around object 202 with the least distance between vehicle 10 and object 202. For example, optimal path 220 may minimize the longitudinal distance 210 between object 202 and vehicle 10 at the start or beginning of the collision avoidance maneuver. The minimum longitudinal distance may be a distance which minimizes the relative distance between vehicle 10 traveling on optimal vehicle path 220 and the object 202. In some embodiments, a margin (e.g., 0.5 meters or another value) may be included in or added to the least distance between vehicle 10 and object 202 to account and/or compensate for uncertainties (e.g., sensor measurement error, road conditions, vehicle conditions, and/or other uncertainties). Optimal path 220 may guide or steer vehicle 10 away from object 202 and onto a target trajectory (e.g., into a target lane 230).

[0035] Optimal vehicle path 220 may, for example, be a path that guides vehicle 10 around object 202 with the least distance between vehicle 10 and object 202 such that vehicle collision avoidance is minimally intrusive and activated only when needed. A collision avoidance maneuver following optimal path 220 may, in some embodiments, be an uncomfortable maneuver for vehicle occupants.

[0036] Optimal path 220 may, for example, be determined, calculated, and/or computed by minimizing longitudinal distance 210 between vehicle 10 and object 202 subject to vehicle dynamics (e.g., lateral acceleration, lateral acceleration rate) and/or other limits or constraints. Optimal path 220 around the object may, for example, be determined based on an optimal vehicle trajectory curvature or optimal path curvature. Optimal path 220 may follow or lie on a smooth curve (e.g., a second order, parabolic curve, and/or continuous curve including second order, parabolic and/or curves or segments of another order).

[0037] FIG. 4 is a graph of path curvature versus time or position representing an optimal path curvature around an object according to embodiments of the present invention. Optimal path 220 may, for example, be determined based an optimal path curvature 310 (e.g., desired trajectory curvature, desired path curvature). Optimal or desired path curvature 310 may, for example, define the curvature or trajectory curvature of the optimal vehicle path 220 away from and/or around object 202. Optimal path curvature 310 may, for example, be a function of or be related to path curvature or trajectory curvature 312, κ, (e.g., in l/meters (1/m) or other units) and time 314 (e.g., in seconds (s) or another unit of time). Optimal path curvature 310 may, in some embodiments, be a function of or be related to path curvature or trajectory curvature 312, κ, and longitudinal distance 316 (e.g., in meters (m) or other units of distance).

[0038] Optimal path curvature 310 may, for example, be determined by minimizing longitudinal distance 210 between vehicle 10 and object 202 at the start of the vehicle maneuver around object 202 subject to, limited by, and/or constrained by one or more constraints. Constraints (e.g., path curvature constraint 322, path curvature rate constraint 320) may, for example, be related to vehicle dynamics constraints which may, for example, include lateral acceleration limit(s) or constraint(s), a_ymax, lateral acceleration rate or jerk limit(s) or constraint(s),

[0039] ${\left(\frac{d{a}_y}{dt}\right)}_{\max }={\eta }_{\max },$
and/or other parameters. Other path calculation or determination methods may be used.

[0040] Other limits or constraints may be used.

[0041] Longitudinal distance 210 between vehicle 10 and object 202 may, for example, be determined based on a sensor (e.g., computer vision sensor 24, LIDAR sensor 20 or other sensor(s)) measured relative location of object 202 (e.g., target vehicle) with respect to vehicle 10 (or absolute location). A minimum longitudinal distance 212 between vehicle 10 and object 202 at the start of a maneuver around an object may be calculated based on a measured relative location of object 202 with respect to vehicle 10 and/or longitudinal distance 210. For example, minimum longitudinal distance 212 may be a distance which minimizes the relative distance between vehicle 10 traveling on optimal vehicle path 220 and object 202. Minimum longitudinal distance 212 may, for example, be calculated or determined by system 100 and may be based on vehicle testing, vehicle dynamics characteristics, and/or other factors.

[0042] Lateral acceleration constraint, a_ymax, may, for example, be a vehicle dynamics constraint and may be determined during vehicle dynamics testing, manufacturing, human factor studies, and/or using other methods. Lateral acceleration constraint may, for example, be determined based on vehicle stability and/or vehicle dynamics parameters. Lateral acceleration constraint, a_ymax, may, in some embodiments, be 6 meters per second squared (m/s²), another value, or other range of values. Other limits and/or constraints may be used.

[0043] Lateral acceleration rate constraint (e.g., jerk, jolt, surge, or lurch constraint), η_max, may, for example, be determined during vehicle dynamic testing, manufacturing, human factor studies, and/or using other methods. Lateral acceleration rate constraint, η_max, may, in some embodiments, be determined based on human safety studies and may, for example, be a vehicle lateral acceleration rate which would not substantially harm and/or influence vehicle occupants. Lateral acceleration rate constraint, η_max, may, for example, be 1-2 m/s³ (e.g., 1-2 g/s), another value, or range of values. Other limits and/or constraints may be used.

[0044] According to some embodiments, path curvature or trajectory curvature 312, κ, may be related to vehicle lateral acceleration, a_y, and vehicle longitudinal velocity, V, by an equation such as:

[0045] $\kappa =\frac{a_y}{V^2}$

[0046] In some embodiments, it may be assumed that the magnitude of vehicle longitudinal velocity, V, may remain constant or approximately constant throughout or during the collision avoidance maneuver. According to some embodiments, assuming the magnitude of vehicle longitudinal velocity, V, may be constant or roughly constant, the derivative of an equation defining trajectory curvature 312,

[0047] $\kappa =\frac{a_y}{V^2},$
may be determined. In some embodiments, the rate of change of path curvature or time derivative of path curvature,

[0048] $\frac{d\kappa }{dt},$
may be related to lateral acceleration rate,

[0049] $\frac{d{a}_y}{dt}=\eta ,$
by an equation such as:

[0050] $\frac{d\kappa }{dt}=\frac{\eta }{V^3}$

[0051] According to some embodiments, lateral acceleration rate, η, may be constrained by the lateral acceleration rate constraint, η_max. The magnitude of rate of change of path curvature,

[0052] $\left|\frac{d\kappa }{dt}\right|,$
may, for example, be constrained by, and/or related to lateral acceleration rate constraint, η_max, and vehicle longitudinal velocity, V, by an equation such as:

[0053] $\left|\frac{d\kappa }{dt}\right|\le \frac{{\eta }_{\max }}{V^3}$
Trajectory curvature rate limit or constraint 322,

[0054] $\frac{{\eta }_{\max }}{V^3},$
for example, be a maximum rate of change of trajectory curvature. The magnitude of the slope of optimal path curvature 310 may, for example, be limited or constrained by, trajectory curvature rate constraint 322,

[0055] $\frac{{\eta }_{\max }}{V^3}.$

[0056] According to some embodiments, lateral acceleration, a_y, may be limited by lateral acceleration constraint, a_ymax. The trajectory curvature 312, κ, may, for example, be limited by, constrained by, and/or related to lateral acceleration constraint, a_ymax and vehicle longitudinal velocity, V, by an equation such as:

[0057] $\kappa \le \frac{a_{y\max }}{V^2}$
Trajectory curvature limit or constraint 320,

[0058] $\left|\frac{a_{y\max }}{V^2}\right|,$
may, for example, be a maximum magnitude of trajectory curvature based on the maximum vehicle lateral acceleration, a_ymax, vehicle longitudinal velocity, V, and/or other variables. The magnitude of optimal path curvature 310 may, for example, be limited or constrained by trajectory curvature limit 320,

[0059] $\left|\frac{a_{y\max }}{V^2}\right|.$

[0060] According to some embodiments, optimal path curvature 310 may be defined by a piece-wise linear function of trajectory curvature 312 and time 314, trajectory curvature 312 and longitudinal distance 316, or trajectory curvature 312 and other variables. Optimal path curvature 310 may, for example, include multiple path curvature segments, trajectory curvature parts, or curvature segments (e.g., path curvature segments 330, 332, 334, 336, 338, and/or 340 or other curve segments) connected by waypoints or breakpoints (e.g., waypoints 350, 352, 354, 356, and/or 358 or other waypoints). Waypoints may, for example, be the points or transitions between curve segments, points at which the slope or characteristics of optimal path curvature 310 change, and/or other points.

[0061] According to some embodiments, during first path curvature segment 330 and second path curvature segment 332, host vehicle 10 may increase to maximum curvature or another curvature to steer away from the object 202. During third path curvature segment 334, host vehicle 10 may, for example, have avoided object 202 and may be reducing curvature to maximum negative curvature or other curvature to correct and/or compensate path curvature after avoiding object 202. During fourth path curvature segment 336, path curvature may, for example, remain constant and vehicle 10 may counter steer to correct or align vehicle position in target lane 230. During fifth path curvature segment 338, path curvature may, for example, be adjusted to match target lane. Other path curvature, vehicle paths, and vehicle maneuvers may of course be used.

[0062] According to some embodiments, a first path curvature segment 330 may define or represent the beginning of the collision avoidance maneuver. In first path curvature segment 330, trajectory curvature 312 may, for example, increase at a rate equal to or less than trajectory curvature rate constraint 322,

[0063] $\frac{{\eta }_{\max }}{V^3},$
or another rate to a maximum trajectory curvature value (e.g., equal to trajectory curvature constraint 320,

[0064] $\left|\frac{a_{y\max }}{V^2}\right|)$
or trajectory curvature value at a first waypoint 350, x₁. Second path curvature segment 332 may be bounded by, lie between, or be between a first waypoint 350, x₁, and a second waypoint 352, x₂. Trajectory curvature 312 may, for example, be constant in second path curvature segment 332 and may, for example, be equivalent to trajectory curvature constraint 320,

[0065] $\left|\frac{a_{y\max }}{V^2}\right|$
or another trajectory curvature.

[0066] In a third curve segment 334, which may begin at second waypoint 352, x₂, trajectory curvature 312 may decrease at a rate equal to or less than trajectory curvature rate constraint 322,

[0067] $\frac{{\eta }_{\max }}{V^3},$
or another rate to a minimum trajectory curvature value (e.g., trajectory curvature constraint 320,

[0068] $\left|\frac{a_{y\max }}{V^2}\right|)$
or another trajectory curvature value at a third waypoint 354, x₃. In a fourth curve segment 336, which may begin at third waypoint 354, x₃, trajectory curvature 312 may remain constant at a minimum trajectory curvature value (e.g., trajectory curvature constraint 320,

[0069] $\left|\frac{a_{y\max }}{V^2}\right|)$
or another trajectory curvature value to a fourth waypoint 356, x₄. In a fifth curve segment 338, which may begin at fourth waypoint 356, x₄, trajectory curvature 312 may increase at a rate equal to or less than trajectory curvature rate constraint 322,

[0070] $\frac{{\eta }_{\max }}{V^3},$
or another rate to a fifth waypoint 358, x₅. Sixth curve segment 340, which may begin at fifth waypoint 358, x₅, may lie on, follow, or be defined by the target lane curvature. Target lane curvature may, for example, be defined by the lane curvature of target lane 230 (e.g., a lane adjacent to or in near object 202 or another lane) and/or path along the road. Thus, path curvature segments (e.g., segments 330, 332, 334, 336, 338, and/or 340) may be limited by a trajectory curvature rate constraint 322 and the magnitude of the linear optimal path curvature segments may be limited by a trajectory curvature constraint 320. Other numbers of waypoints and curve segments may be used.

[0071] FIG. 5 is a graph of vehicle lateral position versus vehicle longitudinal position representing an optimal vehicle path around an object according to embodiments of the present invention. Optimal or desired vehicle path 220 around object 202 may be defined in vehicle 10 reference frame (e.g., a vehicle based coordinate system) or another reference frame. For example, the origin of the reference frame may be the center of gravity (CG) of vehicle 10 or another location in vehicle. Optimal vehicle path 220 may, in some embodiments, be a function of or related to lateral offset, lateral position, or vehicle lateral position 402 and longitudinal position or vehicle longitudinal position 404.

[0072] According to some embodiments, optimal vehicle path 220 may be determined or calculated based on optimal path curvature 310. In some embodiments, optimal vehicle path 220, y(x), may be related to trajectory curvature 312, κ, by an equation such as:

[0073] $\kappa =\frac{y^{″}\left(x\right)}{{\left(1+y^{\prime }\left(x\right)\right)}^{3\text{/}2}}$
Trajectory curvature 312, κ, may, for example, be related to the second derivative of optimal path, y″(x), the first derivative of optimal path, y′(r), and possibly other parameters or variables. According to some embodiments, optimal path 220, y(x), may be calculated based on optimal path curvature 310, which may be a function of or related to trajectory curvature 312, κ, and longitudinal vehicle position 316, x. Optimal path 220, y(x), may, for example, be calculated based on optimal curvature 310 by integrating optimal path curvature 310 twice with respect to longitudinal position 316, x, (e.g., determining or calculating a second integral of optimal path curvature 310). Optimal vehicle path 220, y(x), resulting from integrating optimal curvature 310 twice with respect to longitudinal position 316, x, may, for example, be multiple quadratic, cubic and/or other order function(s) or path segments (e.g., path segments or parts 410, 412, 414, 416, 418, and 420 and/or other segment(s) or part(s)) separated by waypoints (e.g., waypoints 430, 432, 434, 436, and/or 438 or other waypoints). Waypoints may, for example, be determined by matching conditions for optimal vehicle path 220, y(x), the first derivative of optimal vehicle path, y′(x), and the second derivative of optimal vehicle path, y″(x), at the waypoints (e.g., points or transitions between curve segments).

[0074] According to some embodiments, optimal path 220 may include one or more path segment(s) or part(s). First path segment or part 410, y₁(x), may, for example, be a cubic function (e.g., third order function) or other type function, which defines optimal vehicle path 220 from the starting point of the collision avoidance maneuver (e.g., the longitudinal point x=0 or another point) to a first waypoint 430, x₁. First path segment 410, y₁(x), may, for example, be related to a parameter, a₁, and longitudinal distance, x, by an equation such as:

[0075] $y_1\left(x\right)=\frac{a_1{x}^{\mathrm{3`}}}{6},x\in \left[0,x_1\right]$
Parameter, a₁, may, for example, be related to lateral acceleration rate constraint (e.g., jerk), η_max, and vehicle longitudinal velocity, V, by an equation such as:

[0076] $a_1=\frac{{\eta }_{\max }}{V^3}$

[0077] According to some embodiments, first waypoint 430, x₁, may be related to lateral acceleration constraint, a_ymax, and lateral acceleration rate constraint (e.g., jerk), η_max, and vehicle longitudinal velocity, V, by an equation such as:

[0078] $x_1=\frac{{\mathrm{Va}}_{y\max }}{{\eta }_{\max }}$

[0079] According to some embodiments, second path segment or part 412, y₂(x), may, for example, be a quadratic function (e.g., second order function) or other type of function, which defines optimal vehicle path 220 from first waypoint 430, x₁, to second waypoint 432, x₂. Second path segment 412, y₂(x), may, for example, be related to longitudinal distance, x, parameter, b₂, parameter, c₂, parameter, d₂, and first waypoint 430, x₁, by an equation such as:

[0080] $y_2\left(x\right)=\frac{{b_2\left(x-x_1\right)}^2}{2}+c_2\left(x-x_1\right)+d_2,x\in \left[x_1,x_2\right]$

[0081] According to some embodiments, parameter, b₂, parameter, c₂, and parameter, d₂, may be determined or calculated by matching conditions (e.g., continuity of optimal path 220, y(x), the first derivative of optimal path, y′(x), and the second derivative of optimal path, y″(x)) at first waypoint 430, x₁. Parameter, b₂, parameter, c₂, and parameter, d₂, may, for example, be related to parameter, a₁, and first waypoint 430, x₁, by equations such as:
d₂=⅙a₁x₁³; c₂=½a₁x₁²; b₂=a₁x₁

[0082] According to some embodiments, third path segment or part 414, y₃(x), may, for example, be a cubic function (e.g., third order function) or other type of function, which defines optimal vehicle path 220 from second waypoint 432, x₂, to third waypoint 434, x₃. Third path segment 414, y₃(x), may, for example, be related to longitudinal distance, x, parameter, a₃, parameter, b₃, parameter, c₃, parameter, d₃, and second waypoint 432, x₂, by an equation such as:

[0083] $y_3\left(x\right)=\frac{{a_3\left(x-x_2\right)}^3}{6}+\frac{{b_3\left(x-x_2\right)}^2}{2}+c_3\left(x-x_2\right)+d_3,x\in \left[x_2,x_3\right]$

[0084] According to some embodiments, third waypoint 434, x₃, may, for example, be related to second waypoint 432, x₂, and first waypoint 430, x₁, by an equation such as:
x₃=x₂+2x₁
The relationship between third waypoint 434, x₃, second waypoint 432, x₂, and first waypoint 430, x₁, may, for example, be determined to maintain or approximate symmetry in optimal vehicle path 220.

[0085] According to some embodiments, parameter, a₃, parameter, b₃, parameter, c₃, and parameter, d₃, may be determined or calculated by matching conditions (e.g., continuity of optimal vehicle path 220, y(x), the first derivative of optimal vehicle path, y′(x), and the second derivative of optimal vehicle path, y″(x)) at second waypoint 432, x₂. Parameter, a₃, parameter, b₃, parameter, c₃, and parameter, d₃, may, for example, be related to parameter, a₁, parameter, b₂, parameter, c₂, parameter, d₂, first waypoint 430, x₁, and second waypoint 432, x₂, by equations such as:
d₃=½b₂(x₂−x₁)²+c₂(x₂−x₁)+d₂; c₃=b₂(x₂−x₁)+c₂; b₃=b₂; a₃=−a₁

[0086] According to some embodiments, fourth path segment or part 416, y₄(x), may, for example, be a quadratic function (e.g., second order function) or other type of function, which defines optimal vehicle path 220 from third waypoint 434, x₃, to fourth waypoint 436, x₄. Fourth path segment 416, y₄(x), may, for example, be related to longitudinal distance, x, parameter, b₄, parameter, c₄, parameter, d₄, and third waypoint 434, x₃, by an equation such as:

[0087] $y_4\left(x\right)=\frac{{b_4\left(x-x_3\right)}^2}{2}+c_4\left(x-x_3\right)+d_4,x\in \left[x_3,x_4\right]$

[0088] According to some embodiments, parameter, b₄, parameter, c₄, and parameter, d₄, may be determined or calculated by matching conditions (e.g., continuity of optimal vehicle path 220, y(x), the first derivative of optimal vehicle path, y′(x), and the second derivative of optimal vehicle path, y″(x)) at third waypoint 434, x₃. Parameter, b₄, parameter, c₄, and parameter, d₄, may, for example, be related to parameter, a₃, parameter, b₃, parameter, c₃, parameter, d₃, second waypoint 432, x₂, and third waypoint 434, x₃, by equations such as:
d₄=⅙a₃(x₃−x₂)³+½b₃(x₃−x₂)²+c₃(x₃−x₂)+d₃
c₄=½a₃(x₃−x₂)²+b₃(x₃−x₂)+c₃; b₄=a₃(x₃−x₂)+b₃

[0089] According to some embodiments, fifth path segment or part 418, y₅(x), may, for example, be a cubic function (e.g., third order function) or other type function, which defines optimal vehicle path 220 from fourth waypoint 436, x₄, to fifth waypoint 438, x₅. Fifth path segment 418, y₅(x), may, for example, be related to longitudinal distance, x, parameter, a₅, parameter, b₅, parameter, c₅, parameter, d₅, and fourth waypoint 436, x₄, by an equation such as:

[0090] $y_5\left(x\right)=\frac{{a_5\left(x-x_4\right)}^3}{6}+\frac{{b_5\left(x-x_4\right)}^2}{2}+c_5\left(x-x_4\right)+d_5,x\in \left[x_4,x_5\right]$

[0091] According to some embodiments, parameter, a₅, parameter, b₅, parameter, c₅, and parameter, d₅, may be determined or calculated by matching conditions (e.g., continuity of optimal vehicle path 220, y(x), the first derivative of optimal vehicle path, y′(x), and the second derivative of optimal vehicle path, y″(x)) at fourth waypoint 436, x₄. Parameter, a₅, parameter, b₅, parameter, c₅, and parameter, d₅, may, for example, be related to parameter, a₁, parameter, b₄, parameter, c₄, parameter, d₄, third waypoint 434, x₃, and fourth waypoint 436, x₄, by equations such as:
d₅=½b₄(x₄−x₃)²+c₄(x₄−x₃)+d₄; c₅=b₄(x₄−x₃)+c₄; b₅=b₄; a₅=a₁

[0092] According to some embodiments, sixth path segment or part 420, y₆(x), may, for example, match, lie on, and/or follow the path of the target lane path, y_lane(x), (e.g., target lane 230 or target lane centerline). Target lane path, y_lane(x), may, for example, be or may define the path of the lane adjacent to and/or next to object 202. Sixth path segment 420, y₆(x), may, for example, be related to target lane path, y_lane(x), by an equation such as:
y₆(x)=y_lane(x), x>x₅

[0093] According to some embodiments, target lane path, y_lane(x), (e.g., target lane 230 or target lane centerline) may, for example, be described or represented by a quadratic function (e.g., second order function) or other type of function. In some embodiments, target lane path, y_lane(x), may be related to lane curvature, β, lane heading, γ, lane offset, δ, and fifth waypoint 438, x₅, by an equation such as:
y_lane=½βx₅²+γx+δ

[0094] According to some embodiments, third waypoint 434, x₃, fourth waypoint 436, x₄, and fifth waypoint 438, x₅, may be determined or calculated by matching conditions (e.g., continuity of optimal path 220, y(x), the first derivative of optimal path, y′(x), and the second derivative of optimal path, y″(x)) at fifth waypoint 438, x₅. Fourth waypoint 436, x₄, and fifth waypoint 438, x₅, may, for example, be related to parameter, a₅, parameter, b₅, parameter, c₅, parameter, d₅, lane curvature, β, lane heading, γ, and lane offset, δ, by equations such as:
⅙a₅(x₅−x₄)³+½b₅(x₅−x₄)²+c₅(x₅−x₄)+d₅=½βx₅²+γx₅+δ
½a₅(x₅−x₄)²+b₅(x₅−x₄)+c₅=βx₅+γ
a₅(x₅−x₄)+b₅=β

[0095] Using the above equations, fifth waypoint 438, x₅, may, for example, be expressed in terms of fourth waypoint 436, x₄, lane curvature, β, parameter, b₂, and parameter, a₁, or, alternatively, fourth waypoint 436, x₄, lane curvature, β, vehicle longitudinal velocity, V, lateral acceleration constraint, a_ymax, and lateral acceleration rate constraint (e.g., jerk), η_max, using equations such as:

[0096] $x_5=x_4+\frac{\beta +b_2}{a_1}=x_4+\frac{\beta V^3+V{a}_{y\max }}{{\eta }_{\max }}$

[0097] According to some embodiments, second waypoint 432, x₂, fourth waypoint 436, x₄, and fifth waypoint 438, x₅, may be determined or calculated using simultaneous equation solution techniques, simultaneously non-linear equation solution techniques, numerical methods, algebraic methods, or another mathematical approach. For example, second waypoint 432, x₂, may be expressed in terms of fourth waypoint 436, x₄, and/or other variables or parameters. The relationship between second waypoint 432, x₂, and fourth waypoint 436, x₄, may, in some embodiments, be a linear (e.g., first order) or other order relationship. In order to determine second waypoint 432, x₂, and/or other waypoints, simultaneous non-linear equation solution techniques or another mathematical approach may be used to calculate or determine a quadratic equation with respect to second waypoint 432, x₂. The coefficients of the quadratic equation may, for example, be determined using numerical solution techniques or another mathematical approach. The quadratic equation may, for example, be solved explicitly to determine second waypoint 432, x₂. Third waypoint 434, x₃, may, for example, be determined or calculated based on second waypoint 432, x₂, and first waypoint 430, x₁.

[0098] According to some embodiments, optimal path 220 may be a continuously differentiable function with respect to time and/or longitudinal position and may include first path segment 410, y₁(x), second path segment 412, y₂(x), third path segment 414, y₃(x), fourth path segment 416, y₄(x), fifth path segment 418, y₅(x), and sixth path segment 420, y₆(x). Each of the path segment(s) may, for example, be connected to or intersect with adjacent path segments at waypoints (e.g., waypoints 430, 432, 434, 436, and/or 438 or other waypoints). Optimal or desired vehicle path 220 may, for example, be output or transferred to a vehicle collision avoidance control system or device 100.

[0099] FIG. 6 is a graph of vehicle lateral position with respect to vehicle longitudinal position, and shows that a jerk constraint may have an effect on vehicle collision avoidance maneuvers. Graph 500 represents an example vehicle path during multiple simulated collision avoidance maneuvers. Graph segment 510 and graph segment 520 may, for example, represent vehicles traveling at a longitudinal velocity, V, or speed of 100 kilometers per hour (kph) or another speed or velocity. Graph segment 510 may represent a vehicle path during a collision avoidance maneuver with a constraint or limit on lateral acceleration rate or jerk (e.g., 4 g/sec or another value). Graph segment 530 may represent a vehicle path during a collision avoidance maneuver with a constraint or limit on lateral acceleration rate or jerk (e.g., 2 g/sec or another value). Graph segment 520 may represent a vehicle path during a collision avoidance maneuver without a constraint or limit on lateral acceleration rate (e.g., jerk). The vehicle path represented by graph segment 530 (e.g., vehicle path with a jerk limit or constraint of 2 g/sec) may be a smoother path than the vehicle path represented by graph segment 510 (e.g., vehicle path with a jerk limit or constraint of 4 g/sec). The vehicle path represented by graph segment 510 (e.g., vehicle path with a jerk constraint) may be a smoother path than the vehicle path represented by graph segment 520 (e.g., vehicle path without a jerk constraint). Reducing the jerk constraint or limit or constraint may, for example, increase the smoothness of optimal vehicle path 220. Reducing the jerk constraint may, for example, increase the amount of time required for vehicle 10 to maneuver around object 202. For example, the vehicle path represented by graph segment 530 may require a greater amount of time and longitudinal distance to maneuver around object 202 (e.g., to avoid collision with object) than vehicle path represented by graph segment 510. The vehicle path represented by graph segment 510 may, for example, require a greater amount of time (e.g., 0.4-0.6 seconds or another time) and longitudinal distance to maneuver around object 202 (e.g., to avoid collision with object) than vehicle path represented by graph segment 520. Decreasing the jerk constraint value may, therefore, result in a smoother vehicle path 220 but may require more time to maneuver around object 202. Of course, other vehicles, and other embodiments, may correspond to graphs with different data.

[0100] FIG. 7 is a flowchart of a method according to embodiments of the present invention. In operation 600, an optimal path curvature (e.g., optimal path curvature 310 of FIG. 4) limited by one or more constraints (e.g., trajectory curvature constraint 320 and trajectory curvature rate constraint 322 of FIG. 4) may be determined. The constraints may relate to lateral jerk and one or more vehicle dynamics constraints (e.g., lateral acceleration constraint). Optimal path curvature 310 may include, for example, one or more path segments (e.g., path curvature segments 330, 332, 334, 336, 338, and/or 340 of FIG. 4) and one or more waypoints (e.g., waypoints 350, 352, 354, 356, and/or 358 of FIG. 4).

[0101] In operation 610, based on the optimal path curvature, an optimal vehicle path (e.g., optimal vehicle path 220 of FIG. 3) around an object (e.g., object 202 of FIG. 3) may be determined.

[0102] In operation 620, the optimal vehicle path may be output to a collision avoidance control system (e.g., collision avoidance control system or device 100 of FIG. 1).

[0103] In operation 630, a collision avoidance control system may cause (e.g., via an EPS or AFS) the vehicle to take a certain path.

[0104] Other or different series of operations may be used.

[0105] Embodiments of the present invention may include apparatuses for performing the operations described herein. Such apparatuses may be specially constructed for the desired purposes, or may comprise computers or processors selectively activated or reconfigured by a computer program stored in the computers. Such computer programs may be stored in a computer-readable or processor-readable non-transitory storage medium, any type of disk including floppy disks, optical disks, CD-ROMs, magnetic-optical disks, read-only memories (ROMs), random access memories (RAMs) electrically programmable read-only memories (EPROMs), electrically erasable and programmable read only memories (EEPROMs), magnetic or optical cards, or any other type of media suitable for storing electronic instructions. It will be appreciated that a variety of programming languages may be used to implement the teachings of the invention as described herein. Embodiments of the invention may include an article such as a non-transitory computer or processor readable non-transitory storage medium, such as for example a memory, a disk drive, or a USB flash memory encoding, including or storing instructions, e.g., computer-executable instructions, which when executed by a processor or controller, cause the processor or controller to carry out methods disclosed herein. The instructions may cause the processor or controller to execute processes that carry out methods disclosed herein.

[0106] Different embodiments are disclosed herein. Features of certain embodiments may be combined with features of other embodiments; thus, certain embodiments may be combinations of features of multiple embodiments. The foregoing description of the embodiments of the invention has been presented for the purposes of illustration and description. It is not intended to be exhaustive or to limit the invention to the precise form disclosed. It should be appreciated by persons skilled in the art that many modifications, variations, substitutions, changes, and equivalents are possible in light of the above teaching. It is, therefore, to be understood that the appended claims are intended to cover all such modifications and changes as fall within the true spirit of the invention.