REACTIVE PATH PLANNING FOR AUTONOMOUS DRIVING

0Associated
Cases 
0Associated
Defendants 
0Accused
Products 
8Forward
Citations 
0
Petitions 
1
Assignment
First Claim
1. A method of adaptively regenerating a planned path for an autonomous driving maneuver comprising the steps of:
 (a) obtaining, by vehiclebased devices, object data associated with sensed objects in a road of travel;
(b) constructing, by a processor, an object map based on the sensed objects in a road of travel;
(c) resetting and actuating a timer;
(d) generating, by the processor, a planned path for autonomously maneuvering the vehicle around the sensed objects, the planned path being generated based on a costdistance function;
(e) autonomously maneuvering the vehicle along the planned path;
(f) updating the object map based on updated sensed data from the vehiclebased devices;
(g) determining whether the planned path is feasible based on the updated object map;
(h) returning to step (a) in response to a determination that the planned path is infeasible;
otherwise continuing to step (i);
(i) determining whether the timer has expired; and
(j) returning to step (a) in response to the timer expiring;
otherwise, returning to step (f).
1 Assignment
0 Petitions
Accused Products
Abstract
A method of adaptively regenerating a planned path for an autonomous driving maneuver. An object map is generated based on the sensed objects in a road of travel. A timer reset and actuated. A planned path is generated for autonomously maneuvering the vehicle around the sensed objects. The vehicle is autonomously maneuvered along the planned path. The object map is updated based on sensed data from the vehiclebased devices. A safety check is performed for determining whether the planned path is feasible based on the updated object map. The planned path is regenerated in response to a determination that the existing path is infeasible, otherwise a determination is made as to whether the timer has expired. If the timer has not expired, then a safety check is reperformed; otherwise, a return is made to replan the path.
10 Citations
View as Search Results
System and method for trajectory planning for unexpected pedestrians  
Patent #
US 9,857,795 B2
Filed 03/24/2016

Current Assignee
Honda Motor Company

Sponsoring Entity
Honda Motor Company

Target vehicle deselection  
Patent #
US 10,093,315 B2
Filed 09/19/2016

Current Assignee
Ford Global Technologies LLC

Sponsoring Entity
Ford Global Technologies LLC

Method and System for Determining Drivable Navigation Path for an Autonomous Vehicle  
Patent #
US 20190056739A1
Filed 09/29/2017

Current Assignee
Wipro Limited

Sponsoring Entity
Wipro Limited

Method for providing obstacle maps for vehicles  
Patent #
US 10,460,603 B2
Filed 09/22/2017

Current Assignee
BMW AG

Sponsoring Entity
BMW AG

Method and system for determining drivable navigation path for an autonomous vehicle  
Patent #
US 10,503,171 B2
Filed 09/29/2017

Current Assignee
Wipro Limited

Sponsoring Entity
Wipro Limited

Traveling assistance apparatus  
Patent #
US 10,569,781 B2
Filed 06/15/2017

Current Assignee
DENSO Corporation

Sponsoring Entity
DENSO Corporation

Autonomous vehicle driving systems and methods for critical conditions  
Patent #
US 10,725,470 B2
Filed 06/13/2017

Current Assignee
GM Global Technology Operations LLC

Sponsoring Entity
GM Global Technology Operations LLC

Method and apparatus for shelf feature and object placement detection from shelf images  
Patent #
US 10,726,273 B2
Filed 05/01/2017

Current Assignee
Symbol Technologies LLC

Sponsoring Entity
Symbol Technologies LLC

METHOD OF GENERATING SPATIAL MAP USING FREELY TRAVELLING ROBOT, METHOD OF CALCULATING OPTIMAL TRAVELLING PATH USING SPATIAL MAP, AND ROBOT CONTROL DEVICE PERFORMING THE SAME  
Patent #
US 20110153137A1
Filed 12/08/2010

Current Assignee
Electronics and Telecommunications Research Institute

Sponsoring Entity
Electronics and Telecommunications Research Institute

Dynamically Maintaining A Map Of A Fleet Of Robotic Devices In An Environment To Facilitate Robotic Action  
Patent #
US 20160129592A1
Filed 09/24/2015

Current Assignee
X Development LLC, James Joseph Kuffner, Julian Mason, Rohit Ramesh Saboo

Sponsoring Entity
Google LLC

20 Claims
 1. A method of adaptively regenerating a planned path for an autonomous driving maneuver comprising the steps of:
(a) obtaining, by vehiclebased devices, object data associated with sensed objects in a road of travel; (b) constructing, by a processor, an object map based on the sensed objects in a road of travel; (c) resetting and actuating a timer; (d) generating, by the processor, a planned path for autonomously maneuvering the vehicle around the sensed objects, the planned path being generated based on a costdistance function; (e) autonomously maneuvering the vehicle along the planned path; (f) updating the object map based on updated sensed data from the vehiclebased devices; (g) determining whether the planned path is feasible based on the updated object map; (h) returning to step (a) in response to a determination that the planned path is infeasible;
otherwise continuing to step (i);(i) determining whether the timer has expired; and (j) returning to step (a) in response to the timer expiring;
otherwise, returning to step (f). View Dependent Claims (2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19, 20)
1 Specification
An embodiment relates to autonomous path planning.
Path planning is required for autonomous and semiautonomous highway driving and for advanced driver assistance systems such as collision avoidance. Path planning must be reactive to changes in a host vehicle dynamics and other static and dynamic objects on the road. The planned path must result in a safe collision free path within the road boundaries which must also be feasible for the host vehicle control in light of the vehicle dynamic constraints such as maximum lateral acceleration/jerk. Known path planning techniques do not consider either the dynamics of the host vehicle and other moving target vehicles or are too computationally intensive for realtime applications to be reactive in appropriate time.
An advantage of an embodiment is a fast path planning technique for autonomous driving maneuver that is reactive to dynamics of a host vehicle and other moving vehicles as well as stationary objects surrounding the host vehicle. The technique utilizes a Delaunay Triangulation process to identified segments for generating the planned path. The identified segments are selected based on a costdistance function which takes into account various factors including a shortest length, offset from a previous planned path, offset from a center of a lane, slope of each selected segment relative to adjacent segments, and distance from other vehicles and targets. In addition, the routine refines the planned path by identifying a corridor offset from the planned path and identifying a smoothed path within the corridor. The technique further determines the feasibility of the replanned path by identifying lateral accelerations of the vehicle and distance from other dynamic vehicle. Moreover, the technique will redetermine a planned path after a predetermined period of time; however, the technique will continuously check a safety of the existing path during the predetermined period of time. As a result, the technique described herein reduces the amount of time that is needed to regenerate a planned path by only regenerating a planned path at timed intervals or when the existing path is no longer feasible; however, checks are constantly performed in between the timed intervals to verify whether the planned path remains feasible.
An embodiment contemplates a method of adaptively regenerating a planned path for an autonomous driving maneuver comprising the steps of (a) obtaining, by vehiclebased devices, object data associated with sensed objects in a road of travel; (b) constructing, by a processor, an object map based on the sensed objects in a road of travel; (c) resetting and actuating a timer; (d) generating, by the processor, a planned path for autonomously maneuvering the vehicle around the sensed objects, the planned path being generated based on a costdistance function; (e) autonomously maneuvering the vehicle along the planned path; (f) updating the object map based on updated sensed data from the vehiclebased devices; (g) determining whether the planned path is feasible based on the updated object map; (h) returning to step (a) in response to a determination that the planned path is infeasible; otherwise continuing to step (i); (i) determining whether the timer has expired; and (j) returning to step (a) in response to the timer expiring; otherwise, returning to step (f).
The image capture device 12 captures images exterior of the vehicle. The images captured by the image capture device 12 are analyzed for detecting lanes of travel of the road represented by lane markings.
The sensingbased device 14 may include, but is not limited to, radarbased devices, lidarbased devices, and ultrasonicbased devices for sensing objects both stationary and moving objects surrounding the vehicle.
A processor 16 processes the image data captured by the image capture device 12 and the sensed data sense by the sensing device 14. The processor 16 analyzes the respective data and identifies objects in the road of travel for determining a planned path for generating a planned path maneuver.
The processor 16 may be coupled to one or more controllers 18 for initiating or actuating a control action for generating the planned path maneuver. One or more vehicle subsystems may be actuated and controlled for performing the planned path maneuver. The respective vehicle subsystems that may be controlled for performing the planned path maneuver include, but are not limited to, a steering control subsystem 20, a speed control subsystem 22, and a braking control subsystem 24. A communication system 26 may also be utilized for communicating a planned path to detected target vehicles using vehiclevehicle communications for making the detected target vehicles aware of the plan path maneuver.
The steering subsystem 20 may be controlled for actuating a steering maneuver for steering the vehicle around a detected target in the host vehicles lane of travel.
The braking subsystem 24 may enable an electrical, electrohydraulic or hydraulic braking system where a braking strategy is readied in the event that an autonomous light braking force is required by the vehicle when performing the lane change maneuver.
Speed control subsystem 22 may control the speed of the vehicle for either accelerating or decelerating the vehicle during a lane change maneuver.
The RPP technique described herein is referred to as reactive since the host vehicle path is regenerated after a short period of time (e.g., 0.5 sec) based on the new sensor data even before an end point of a previously determined path is reached. Therefore, the RPP technique is reactive to any changes in the lane data or object map data.
In step 30, the routine is enabled and the routine proceeds to step 31. In step 31, criteria is analyzed for identifying whether to generate a new planned path or continue analyzing the current planned path. It should be understood that the RPP technique is a repetitive in that the planned path is constantly analyzed and revised based on the surrounding environment. Therefore, decisions in step 31 are the results of conditions that are constantly analyzed throughout the RPP routine. The following conditions are used for determining whether a new planned path should be generated or whether the routine should continue monitoring the existing path. The conditions include, but are not limited to (1) identifying whether an existing planning time (T_{plan}) is expired; (2) determining whether the previous host vehicle path is not safe; (3) determining whether the previous host vehicle path is not feasible; (4) determining whether an offset from the host vehicle path is greater than a predetermined offset threshold. If any of the conditions are present, then the routine proceeds to step 32 for generating a next planned path; otherwise, the routine proceeds to step 42 to continue analyzing the existing planned path.
In step 32, object map data and lane data are obtained.
(ID_{i}^{t},X_{i}^{t},Y_{i}^{t},θ_{i}^{t},V_{i}^{t}):[(x_{i1}^{s},y_{i1}^{s}),(x_{i2}^{s},y_{i2}^{s}), . . . ,(x_{im}_{i}^{s},y_{im}_{i}^{s})], (1)
for i=1, 2, . . . , n
where n is the number of detected targets, ID_{i}^{t }is a unique index number of an ith target, (X_{i}^{t},Y_{i}^{t}) is the median point of the ith target transformed to the global frame, θ_{i}^{t }and V_{i}^{t }represent a heading angle and velocity of the ith target, respectively, (x_{ij}^{s},y_{ij}^{s}) represents a jth scan point of the ith target in a global frame, and m_{i }is a number of scan points corresponding to the ith target.
The lane data is in the form of the coefficients of two cubic polynomials representing left and right lane markings. The representations are as follows:
y
_{L}
=a
_{0L}
+a
_{1L}
x+a
_{2L}
x
^{2}
+a
_{3L}
x
^{3 }
y_{R}=a_{0R}+a_{1R}x+a_{2R}x^{2}+a_{3R}x^{3} (2)
where y_{L }and y_{R }represent the left and the right lane markings in a host vehicle frame, respectively.
The lane data also includes parameters determining the quality of vision sensor readings and the type of lane markings such as solid or dashed lines.
The RPP technique further utilizes other host vehicle data such as a vehicle'"'"'s position (X_{h},Y_{h}) in the global frame, a heading angle θ_{h}, a velocity V_{h}, a yaw rate ω_{h}, and longitudinal and lateral accelerations, a_{h }and a_{hL}.
The host vehicle frame is referred to a frame attached to a center of gravity of the host vehicle with an xaxis heading toward a front of the vehicle. A global frame is identical to the host vehicle frame at every planning instance of time. This indicates that the global frame is transformed to the current host vehicle frame whenever a new path is generated. This also indicates that the current host vehicle frame is fixed on the ground between planning times.
Targets that are detected far from the current host vehicle position which have no effects on the path planning should be ignored. As a result, a virtual window is formed around the host vehicle with all the targets identified in eq. (1) as having an impact on the planned path. The window is represented as follows:
where L_{win }and W_{win }represent the window around the vehicle, 1.5 L_{win }is the length and 2 W_{win }is the width of such window, where a respective planned path is calculated therein. The scan points for each target are also simplified using DouglasPeucker method to reduce the computational cost.
If the quality of the camera readings have insufficient resolution, then the following default values of a straight lane for a left lane markings and right lane markings are represented as follows:
a_{0L}=1.75, a_{1L}=a_{2L}=a_{3L}=0
a_{0R}=−1.75, a_{1R}=a_{2R}=a_{3R}=0. (4)
where the units of the lane coefficients are such that Eq. (2) results in meters.
In step 33, a safe space for the host vehicle maneuver is determined by adding virtual nodes created utilizing the Delaunay Triangulation technique. The Delaunay Triangulation technique uses a combination of triangles in the space between scan points, adjacent lanes, and the host vehicle. Delaunay triangulation technique takes a portion of the virtual nodes as inputs and creates triangles to represent the convex space defined by those nodes. For the RPP technique described herein, the input nodes utilized in the Delaunay triangulation are defined as lane virtual nodes, host vehicle virtual nodes, ending virtual nodes, and shifted scan nodes.
Lane virtual nodes are generated on the lane boundaries of the road to ensure that the host vehicle path is on the road. The lane virtual nodes are located at equal distances from each other along the lane boundaries and are calculated as follows:
where (x_{k}^{L},y_{k}^{L}) and (x_{k}^{R},y_{k}^{R}) represent the virtual nodes in the host vehicle frame along the left and right lane boundaries, respectively, L_{h }is a length of the host vehicle taken as the longitudinal distance between virtual nodes, W_{lane }is a width of a current lane and Boolean variables T_{L }and T_{R }are true if the left and right lane markings are solid thereby implying that there is no adjacent lane on that side.
It should be understood that the nodes in Eq. (5) require transformation to a global frame in order to be used by the Delaunay Triangulation technique.
Host vehicle virtual nodes include two virtual nodes with equilateral distances from a current position of the host vehicle. This defines a start of the search space and is represented by the following parameters:
X
_{1,2}
^{h}
=X
_{h }
Y_{1,2}^{h}=Y_{h}±1 (6)
End virtual nodes define the end of the search space and are represented as follows:
x_{1,2}^{e}=max_{k}x_{k}^{L}+L_{h}, x_{3,4}^{e}=max_{k}x_{k}^{R}+L_{h }
y
_{1}
^{e}
=y
_{max(k)}
^{L}
, y
_{2}
^{e}
=y
_{max(k)}
^{L}
−W
_{lane }
y_{3}^{e}=y_{max(k)}^{R}, y_{4}^{e}=y_{max(k)}^{R}+W_{lane}. (7)
The nodes in eq. (7) are in the host vehicle frame and require transformation into the global frame. If the left and/or right lane markings are solid, then the virtual nodes (x_{2}^{e},y_{2}^{e}) and/or (x_{4}^{e},y_{4}^{e}) are ignored, respectively, or are ignored if no target is detected inside the planning window.
Shifted scan node points are nodes which are shifted based on relative velocities of each of the detected target vehicles. The scan points in eq. (1) are shifted along the road as determined by the following equations:
where T_{i}^{r }is a variable representing the amount of time the host vehicle needs to reach the ith target. T_{i}^{r }is calculated as follows:
where D_{min }and V_{min }are the constant parameters denoting distance and velocity thresholds for shifting the scan points. Those shifted scan points will be considered in the triangulation calculation if the following three conditions are satisfied:
The Delaunay Triangulation representation of the exemplary scenario in
In step 34, a search graph is generated. A search graph is a graph defined in triangulation space and is composed of a number of vertices and segments connecting associated vertices. Vertices are generated and located on specific triangle edges that satisfy the following conditions: (1) each edge is not an edge on the boundary; (2) each edge is not connecting two scan points from the same target; (3) each edge is not connecting two lane virtual nodes; (4) each edge length has a length is greater than a threshold value (L_{min}); (5) each edge connects two ending virtual nodes; and (6) each edge is inside the convex polygon defined by the lane virtual nodes.
If a triangle edge satisfies each of the above conditions, vertex points 108 are defined along that edge as shown in
where (x_{ij}^{v},y_{ij}^{v}) denotes the jth vertex on the ith edge, (x_{i}^{sr},y_{i}^{sr}) and (x_{i}^{tg},y_{i}^{tg}) represents the source node and the target node for the ith edge, x respectively, d_{v }is the constant distance between the vertices along the edge, n_{e }is the number of edges satisfying conditions above, and L_{i}^{e }is the length of the ith edge.
If the ith edge is vertical in the global frame (i.e., x_{i}^{tg}≈x_{i}^{sr}), then the following formula is used to find the vertices:
In addition to the vertices obtained in eq. (13) & (14), a source vertex is added at the current host vehicle position. In addition, a target vertex is added with its Xcoordinate value greater than the maximum X value of the other vertices and Ycoordinate value as the average value of the Ycoordinates of ending virtual nodes in eq. (7). The source and target vertices are used in Dijkstra'"'"'s algorithm to find the shortest path which will be discussed in detail later.
With respect to the parameter d_{v}, which is the constant distance between the vertices along an edge, has a significant role in generated the search graph. If d_{v }is chosen small, the number of vertices in the graph will increase which results in a finer graph resolution for the space within a respective triangular regions and increases the smoothness of the final path, which in addition, is also closer to a driverchosen path; however, having a finer resolution will require more computational power for the algorithm. Therefore, a balance is preferable when selecting the d_{v }since this is an instrumental tuning parameter in the technique. To reduce the computational power while maintaining a reasonable level of smoothness for the selected path, two candidate values are set for d_{v }and for each triangle edge. A respective value is selected based on a proximity of the selected edge to the host vehicle and shifted target vehicles on the road. If a triangle edge is close to the host or shifted point of the target vehicle, then a smaller value is chosen for d_{v}, otherwise larger value relative to the smaller value is selected.
Once the vertex points 108 are determined, they are connected by graph segments 110. Graph segments connect two vertices which (1) belong to the same triangle, and (2) do not belong to the same edge. The vertex points 108 and the graph segments 110 are shown plotted in
In step 35, using the Dijkstra'"'"'s algorithm, a search is performed to find the shortest path in the search graph which connects the source vertex to the target vertex. The shortest path is not found in terms of a conventional distance definition, but in terms of a costdistance function defined as follows:
where D_{i }is a distance for the ith path from the source vertex to the target vertex, D_{ij}^{L}, D_{ij}^{s}, D_{ij}^{d}, D_{ij}^{c}, and D_{ij}^{p }are distance function components of the jth segment for the ith path, α_{L}, α_{s}, α_{d }and α_{p }are constant weight coefficients, and n_{path }and n_{seg }are the number of paths from the source vertex to target vertex and the number of segments in each path, respectively.
The term D_{ij}^{L }in eq. (15) corresponds to the length of the jth segment of the ith path. This term considers the shortest distance from a convention measurement. The equation for determining the term D_{ij}^{L }is represented as follows:
D_{ij}^{L}=√{square root over ((x_{ij}^{vs}−x_{ij}^{vt})^{2}+(y_{ij}^{vs}−y_{ij}^{vt})^{2})} (16)
where (x_{ij}^{vs},y_{ij}^{vs}) and (x_{ij}^{vt},y_{ij}^{vt}) denote the source and target vertices of the corresponding segment, respectively.
The term D_{ij}^{L}, in eq (15) is included in the distance function so that the actual length of the path plays a role in calculating the shortest path. D_{max}^{L}, in the denominator, represents a maximum possible length of a segment and is used to normalize the length cost to [0 1] range. The weight coefficient α_{L }is a positive constant that requires tuning along with other weight coefficients to generate a practical host vehicle path for different scenarios.
The term, D_{ij}^{s}, in eq. (15) corresponds to the relative slope of the jth segment of the ith path to the heading of the host vehicle or the lane. The parameter considers the smooth transition of the steering maneuver such as minimizing abrupt changes/jerks in the steering maneuver. The equation for determining the term D_{ij}^{s }is represented as follows:
where θ_{ij }is the angle of the segment in the global frame, D_{HV }is a positive constant representing a close distance ahead of the host vehicle, γ_{s}ε[0 1] is a tuning parameter, and θ_{lane }is the lane heading at the segment'"'"'s position. The lane heading θ_{lane }is represented as follows:
θ_{lane}=θ_{h}+a tan(½[a_{1L}+a_{1R}+2 max(x_{ij}^{vs},x_{ij}^{vt})(a_{2L}+a_{2R})+3[max(x_{ij}^{vs},x_{ij}^{vt})]^{2}(a_{3L}+a_{3R})]) (18)
The term D_{ij}^{s}, in eq (15) is included in the distance function to ensure that the resulted shortest path is sufficiently aligned with a current host vehicle heading in a close region ahead of the vehicle and is aligned with the lane heading thereafter. The positive angle D_{max}^{s }is employed to normalize the cost to [0 1] range, and the weight coefficient α_{s }is a tuning parameter which will affect the alignment of the shortest path to the host vehicle heading compared to other distance function components.
The term, D_{ij}^{d}, in eq. (15) is related to the distance of a segment in the search graph to the host vehicle path found at a previous planning time. This prevents any significant deviation from the previous planned path which could otherwise result in significant steering changes to the vehicle. The equation for determining the term D_{ij}^{d }is represented as follows:
D_{ij}^{d}=D_{ij}^{ds}+D_{ij}^{dt} (19)
where d_{ij}^{ds }and D_{ij}^{dt }are the weighted offsets from the previous host vehicle path for the source and target vertices of the segments with more weights directed on closer vertices to the host vehicle, specifically,
for k=1, 2, . . . , n_{HV}−1,
where ({circumflex over (X)}_{k}^{HV},Ŷ_{k}^{HV}) denotes the kth waypoint in the previous feasible host vehicle path and n_{HV }is the number of host vehicle path waypoints.
The term D_{ij}^{d }is included in the distance function so that a current shortest path, particularly the segment closer to the current host vehicle position is forced to be sufficiently close to the previous planned path. This assists the processor in tracking the generated host vehicle path more efficiently. As described earlier, D_{max}^{d }is used to normalize this cost to [0 1] range and α_{d }is a tuning parameter.
The term D_{ij}^{c }in eq. (15) is related to the distance of a segment in the search graph to the centerline of the current lane. This relates to an offset from a center of each lane. The equation for determining the term D_{ij}^{c }is represented as follows:
D_{ij}^{c}=min(d_{ij}^{c1},d_{ij}^{c2},d_{ij}^{c3}) (21)
where d_{ij}^{c1}, d_{ij}^{c2 }and d_{ij}^{c3 }are the offset values of the segment from the centers of the current lane, adjacent left lane, and adjacent right lane, respectively. The offsets are represented as follows:
The term D_{ij}^{c }in eq. (15) is included in the distance function so that the shortest path is forced to be sufficiently close to the center of the lane that the corresponding segment is closest to. This will result in a lane centering behavior when there is no target vehicle around or no threat posed to the host vehicle. As describe earlier, D_{max}^{c }is used to normalize this cost to [0 1] range and α_{c }is a tuning parameter.
The term D_{ij}^{p }in eq. (15) is included to ensure that the shortest path is at least at a safe distance from the surrounding obstacles such as other moving target vehicles or stationary objects. The component takes into consideration the dynamics of the moving targets when planning a collisionfree path. Referring to eq. (15) scan points are shifted based on position of the corresponding segment vertices as follows:
where (
The shifted scan points are then transformed to the segment'"'"'s local frame as utilizing the following equations:
{circumflex over (x)}_{kl}^{ij}=[
ŷ_{kl}^{ij}=−[
The virtual potential field value D_{ij}^{p }of the jth segment of the ith path is then obtained from the following representation:
where d_{safe}^{lon }and d_{safe}^{lat }are the longitudinal and lateral safe distances from the obstacles, and D_{max}^{p }is used to normalize the potential field to [0 1] range and α_{p }is the tuning parameter.
To ensure that the resulting path is at a safe distance from the obstacles, α_{p }is preferably chosen to have the highest value among all the weight coefficients in eq. (15). This provides the greatest emphasis on safety in determining the planned path for the maneuver. It should be understood that the weight coefficients may be proportioned other than what is described herein to accommodate a driving condition set forth by a manufacturer.
If the length of the segment is shorter than a threshold value, L_{min}^{seg}, or its relative slope to the current lane is larger than a threshold value, S_{max}^{seg}, then the segment heading, θ_{ij}, in eq. (26) is replaced by the heading angle of the current lane. This is performed to ensure that any segments with short lengths and/or relatively large slopes do not cause nonreal potential fields for the segment.
A resulted shortest path 112 with the distance function in eq. (15) is shown in
In step 36, a determination is made whether the source vertex connects the target vertex utilizing the resulting path. If a determination is made that the planned path does not connect the respective vertices, then the routine proceeds to step 42 where the routine sets the path as not feasible. The routine returns to step 31 to replan a path. If the determination was made in step 36 that the resulting path connects the source vertex to the target vertex, then the routine proceeds to step 37.
In step 37, in response to determining the shortest path, a safe corridor is generated around the shortest path. The safe corridor is identified such that host vehicle path satisfies the following conditions while it is inside the corridor. The conditions are as follows: (1) the host vehicle path is fairly close to the shortest path; (2) the host vehicle path is at a safe distance from all surrounding objects; and (3) the host vehicle path stays on the road.
To find the safe corridor, the vertices (x_{i*j}^{v},y_{i*j}^{v}), j=1, 2, . . . , n_{sp }along the shortest path are utilized. In utilizing these parameters, left and right corridor points are calculated for each vertex. In addition, dynamics of the moving targets must be taken into consideration in the calculation. The scan points are first shifted as follows:
where (
The shifted scan points are then transformed to the shortest path segment'"'"'s local frame using the following formulas:
{circumflex over (x)}_{kl}^{i*j}=[
ŷ_{kl}^{i*j}=−[
where θ_{i*j }is the slope of the segment connecting vertices (x_{i*(j1)}^{v},y_{i*(j1)}^{v}) to (x_{i*j}^{v},y_{i*j}^{v}).
The following minimum and maximum values of the transferred scan points are then determined utilizing the following conditions:
y_{min}^{i*j}=min_{k,l}(ŷ_{kl}^{i*j}) for all {circumflex over (x)}_{kl}^{i*j}≦L_{h}&ŷ_{kl}^{i*j}>0
y_{max}^{i*j}=max_{k,l}(ŷ_{kl}^{i*j}) for all {circumflex over (x)}_{kl}^{i*j}≦L_{h}&ŷ_{kl}^{i*j}<0 (31)
The left and right corridor points for the corresponding shortest path vertex are calculated using the following conditions:
where (x_{j}^{lt},y_{j}^{lt}) and (x_{j}^{rt},y_{j}^{rt}) denote the jth left and right corridor points in the segment'"'"'s local frame.
The points in eq. (32) are then transformed back to the global frame to obtain (x_{j}^{lt},yY_{j}^{lt}) and (x_{j}^{rt},y_{j}^{rt}), j=1, 2, . . . , n_{sp}. If the length of the segment is shorter than a threshold value, L_{min}^{seg}, or its relative slope to the current lane is larger than a threshold value, S_{max}^{seg}, then the segment heading, θ_{i*j}, in eq. (30) is replaced by the heading angle of the current lane.
In step 38, in response to identifying a safe corridor, the routine determines a path inside the corridor for the host vehicle which is smooth enough to be tracked. The dynamic constraints imposed by the host vehicle are the maximum lateral acceleration and lateral jerk on the path. This implies limits for a maximum curvature and curvature rate for a feasible host vehicle path. As a result, the technique refines the path points inside the corridor such that the final path is as close as possible to a path which can be tracked by the host vehicle and minimizes excessive curvatures in the planned path.
This step is described herein as a subroutine. In step 381, equaldistance vertical lines in the global frame are determined in the range of the safe corridor and are represented as follows:
X=
where d_{L }is a constant parameter representing the distance between the vertical lines.
In step 382, intersection points of the vertical lines in eq. (33) are determined with the left and right corridor lines. The resulting points are referred to as vertical corridor points. The left corridor points are represented by p_{i}^{lt}:(
In step 383, path points (P_{i}) are defined as follows:
X_{i}^{p}=
where λ_{i}ε[0,1] moves the path point P_{i }along the vertical line inside the corridor.
In step 384, each of the path points are initially positioned at the middle point of the vertical line in between the vertical corridor points by setting λ_{i}=0.5, for i=1, 2, . . . , n_{HV}.
In step 385, for the path point P_{i}, a cost function is defined as follows:
F_{i}=w_{1}C_{i}^{2}+w_{2}(ΔC_{i})^{2} (35)
where C_{i }and ΔC_{i }denote the estimated curvature and curvature rate at P_{i}, respectively,
The sin ( ) function in eq. (32) is calculated using the cross product formula as:
In step 386, an assumption is made that P_{i}−P_{i+1} and P_{i}−P_{i−1} are constant values, and the curvature and curvature rate are linear function of λ_{i}. Therefore, the cost function F_{i }will be a quadratic function of λ_{i}. A minimum point of the cost function
In step 387, path point P_{i }is updated based on the following criteria:
In step 388, steps 385 to 387 are repeated until all path P_{i}, i=1, 2, . . . , n_{HV }are updated once.
In step 389, the return repeats steps 385 to 388 for a maximum number of iterations N_{itr }until the curvature and curvature rate at all path points are less than predetermined threshold rate values. If the maximum curvature and curvature rate exceed their respective predetermined threshold rates after N_{itr }iterations, then the routine is stopped thereby implying that a feasible host vehicle path cannot be found.
The path resulting from applying this technique to the safe corridor in
In step 39, in response to obtaining the host vehicle path determined in step 38, a verification is made utilizing two conditions to ensure that the resulted path is feasible. First, the maximum lateral acceleration at the host vehicle path waypoints must be less than predetermined threshold acceleration values. A lateral acceleration can be estimated utilizing the following equation:
a_{i}^{lat}=v_{h}^{2}C_{i}, i=1,2, . . . ,n_{HV} (39)
where C_{i }is the estimated curvature value calculated in eq. (36).
A second condition is that the host vehicle path must be at a safe distance from all surrounding obstacles. To verify this condition, scan points are first shifted and transformed to the host vehicle path segment'"'"'s local frame using eq. (28)(30) where the shortest path vertices are replaced by host vehicle waypoints. The following condition is then checked for all host vehicle path waypoints to ensure that the host vehicle path is sufficiently far from any target vehicles or other obstacles. The condition is represented as follows:
{circumflex over (x)}_{kl}^{pi}L_{h}&ŷ_{kl}^{pi}>W_{h}/2 (40)
for k=1, 2, . . . , n, l=1, 2, . . . , m_{k}, i=1, 2, . . . , n_{HV }
In step 40, a decision is made as to whether the path regenerated planned path is feasible. If the determination is made that the path is not feasible based on the feasibility analysis performed in step 40, then routine proceeds to step 31. If the determination is made that the path is feasible, then the routine proceeds to step 41.
In step 41, the host vehicle planned path that is determined as being feasible is sent to the controller where the controller executes the planned path autonomously. In response to implementing the planned path, a return is made to step 31.
In step 31, a check is made whether a time since the last path was planned has expired. As described earlier, a planned path is generated at the end of each cycle time T_{plan}. T_{plan }represents a first predetermined rate of time that the routine waits until a next path is planned. As a result, the system will await a duration of time equal to T_{plan }(unless a return is made indicating existing path is unsafe or not feasible), and then generate a next planned path after the duration of time equal to T_{plan }as expired. For example, T_{plan }may be duration of time set to 0.5 sec. As a result, the processor will plan a new path every 0.5 sec. It should be understood that the duration of time 0.5 sec is exemplary and that times other than 0.5 sec may be utilized. Therefore, when the routine loops from step 42 to 32, a determination is made as to whether T_{plan }(e.g., 0.5 sec) has elapsed. If the determination is made that T_{plan }has not elapsed and that there are no safety or feasibility concerns with the existing planned path, then the routine proceeds to step 42.
In step 42, object map data is obtained from the sensing devices and imaging device to perform a safety check. The safety check is performed at a second predetermined rate of time T_{s }(e.g., 10 msec). It should be understood that the 10 msec is exemplary and that other rates of times may be used. It should further be understood that the safety check does not determine or generate a next planned path; rather, the safety check repetitiously checks the safety of the current path in between the planned path times to verify no new threads are introduced to the last planned path. As a result, a plurality of safety checks is repetitiously performed based on new sensor and imaging data obtained in between path planning stages.
In step 43, the safety of the current path is analyzed by monitor incoming sensor data and determining whether the current planned path is safe. During the safety check, the most recent host vehicle planned path found to be feasible will remain unchanged over a period of time T_{plan }before the path is replanned based on the new sensor data. This is done to reduce the computational cost to achieve a fast RPP planning process in practical realtime implementations on the vehicle. Although T_{plan }is set to be short enough to assume that the road scenario does not change significantly during that period, the additional safety check is performed for every T_{s }to ensure that the existing planned path vehicle path is safe between the planning time periods.
To check the safety of the current planned path, all the host vehicle path waypoints are transformed to each moving target local frame as represented by the following expressions:
x_{i}^{pj}=[X_{i}^{p}−X_{j}^{t}] cos(θ_{j}^{t})+[Y_{i}^{P}−Y_{j}^{t}] sin(θ_{j}^{t}) for i=1,2, . . . ,n_{HV }
y_{i}^{pj}=−[X_{i}^{p}−X_{j}^{t}] sin(θ_{j}^{t})+[Y_{i}^{P}−Y_{j}^{t}] cos(θ_{j}^{t}) j=1,2, . . . ,n. (41)
A region ahead of each moving target is defined and the minimumX waypoint inside that region (if existed) is identified as shown in
Time intervals for the moving target T_{j}^{rt}, and the host vehicle T_{h}^{r}, to reach this minimum point is then determined by the following representations:
The existing host vehicle path is considered safe if the following condition is satisfied for all the moving targets:
In step 44, a determination is made whether the existing planned path is still safe. If the determination is made that the existing planned path is still safe, then the routine proceeds to step 45.
In step 45, a host vehicle offset is determined and this information is provided to the controller in step 41. The routine returns to step 31 where the routine performs another safety check if T_{plan }has not expired. If the determination is made that T_{plan }is expired, then the routine proceeds to step 32 to plan a next path.
Referring again to step 44, if the determination is made that the existing path is not safe, then a return is made to step 31. In step 31, a flag is raised relating to the safety of the existing path, and the routine immediately proceeds to step 32 to recalculate a next planned path based on newly obtained object map data regardless of the whether the T_{plan }is expired. In response a determination of new planned path, T_{plan }is reset and the time period for next planned path determination is set at the expiration of T_{plan}.
While certain embodiments of the present invention have been described in detail, those familiar with the art to which this invention relates will recognize various alternative designs and embodiments for practicing the invention as defined by the following claims.