Auto-tunable path controller with dynamic avoidance capability
A path control systems and methods are disclosed that can aide in keeping an autonomous vehicle on a path. A current estimate of the autonomous vehicle's first position (x, y), heading, and velocity as well as a path of interest (e.g., breadcrumbs, line-arcs, or clothoid segments each containing velocity information) can be used to output a command velocity, command curvature, and/or a vector of waypoints. The command velocity, command curvature, and/or a vector of waypoints can be followed to move the autonomous vehicle from the first position onto the path of interest. An obstacle map may also be used to provide a path to the path of interest that avoids the obstacles on the obstacle map.
1 . A method for planning the path of an autonomous vehicle, the method comprising:
receiving a first position, initial velocity, and/or yaw of the autonomous vehicle;
receiving a target path along which the autonomous vehicle is expected to drive, wherein the first position is not on the target path;
trimming the target path based on the first position of the autonomous vehicle;
calculating a target path length of the target path;
planning a modified target path based on the target path and the target path length;
creating one or more graft paths from the first position to the modified target path;
optimizing the one or more graft paths to produce a single graft path;
determining a length rate of curvature from the single graft path;
determining a command steer angle from the graft path;
determining command velocity from the graft path; and
outputting the command steer angle and the command velocity.
2 . The method according to claim 1 , wherein the command steer angle comprises a steer angle determined from a tangent of the graft path initial curvature times a wheelbase of the autonomous vehicle.
3 . The method according to claim 1 , wherein the command steer angle comprises a last calculated command steer angle.
4 . The method according to claim 1 , wherein the command velocity is less than a max steering rate of the autonomous vehicle.
5 . The method according to claim 1 , wherein the command velocity is less than a predetermined avoidance speed.
6 . The method according to claim 1 , wherein the length rate of curvature is determined from the maximum of the current speed times.
7 . An autonomous vehicle comprising:
a sensor array;
an engine output control system;
a braking control system;
a steering control system,
a communication system; and
a controller communicatively coupled with the sensor array, the engine output control system, the transmission control system, the steering control system, and the braking control system, the controller configured to:
retrieve a first position, initial velocity, and/or yaw of the autonomous vehicle from the sensor array;
receive a target path for the autonomous vehicle wherein the first position is not on the target path;
trim the target path based on the first position of the autonomous vehicle;
calculate a target path length from the target path along which the autonomous vehicle is expected to drive;
plan a modified target path based on the target path and the target path length;
create one or more graft paths from the first position to the modified target path;
optimize the one or more graft paths to produce a single graft path;
determine a length rate of curvature from the single graft path;
determine a command steer angle from the graft path;
determine command velocity from the graft path;
output the command steer angle to the steering control system; and
output the command velocity to the engine control system and/or the braking control system.
8 . The autonomous vehicle according to claim 7 , wherein the command steer angle comprises a steer angle determined from a tangent of the graft path initial curvature times a wheelbase of the autonomous vehicle.
9 . The autonomous vehicle according to claim 7 , wherein the command steer angle comprises a last calculated command steer angle.
10 . The autonomous vehicle according to claim 7 , wherein the command velocity is less than a max steering rate of the autonomous vehicle.
11 . The autonomous vehicle according to claim 7 , wherein the command velocity is less than a predetermined avoidance speed.
12 . The autonomous vehicle according to claim 7 , wherein the length rate of curvature is determined from the maximum of the current speed times.
13 . A non-transitory computer-readable recording medium storing instructions for executing a method for planning the path of an autonomous vehicle, the method comprising:
receiving a first position, initial velocity, and/or yaw of the autonomous vehicle;
receiving a target path along which the autonomous vehicle is expected to drive wherein the first position is not on the target path;
trimming the target path based on the first position of the autonomous vehicle;
calculating a target path length of the target path;
planning a modified target path based on the target path and the target path length;
creating one or more graft paths from the first position to the modified target path;
optimizing the one or more graft paths to produce a single graft path;
determining a length rate of curvature from the single graft path;
determining a command steer angle from the graft path;
determining command velocity from the graft path; and
outputting the command steer angle and the command velocity.
14 . The non-transitory computer-readable recording medium according to claim 13 , wherein the command steer angle comprises a steer angle determined from a tangent of the graft path initial curvature times a wheelbase of the autonomous vehicle.
15 . The non-transitory computer-readable recording medium according to claim 13 , wherein the command steer angle comprises a last calculated command steer angle.
16 . The non-transitory computer-readable recording medium according to claim 13 , wherein the command velocity is less than a max steering rate of the autonomous vehicle.
17 . The non-transitory computer-readable recording medium according to claim 13 , wherein the command velocity is less than a predetermined avoidance speed.
18 . The non-transitory computer-readable recording medium according to claim 13 , wherein the length rate of curvature is determined from the maximum of the current speed times.