Single control scheme for two autonomous driving modes using model predictive control envelope
Systems and methods of using a common control scheme to autonomously control a vehicle during semi-autonomous and fully autonomous driving modes are provided. In particular, embodiments of the presently disclosed technology incorporate reference tracking for driving input and vehicle state into this common control scheme. In some embodiments, this common control scheme may be implemented using Model Predictive Control (“MPC”).
1 . A vehicle comprising:
a memory storing instructions; and
one or more processors communicably coupled to the memory and configured to execute the instructions to:
receive a path of travel for a defined time horizon, the path of travel comprising positions as a function of the defined time horizon;
in response to receiving a driving input and a vehicle state for the defined time horizon and based on the received path of travel, determine a reference driving input and a reference vehicle state for the defined time horizon, wherein the driving input is a human driver input when the vehicle is operating in a semi-autonomous mode and a pseudo-driver input when the vehicle is operating in a fully autonomous mode;
compare the driving input and the reference driving input to an autonomous driving command and the reference vehicle state to the vehicle state within one or more terms of an objective cost function by:
comparing the reference vehicle state to the vehicle state within a first term of the objective cost function;
comparing the reference driving input to the autonomous driving command within a second term of the objective cost function; and
comparing the driving input to the autonomous driving command within a third term of the objective cost function;
generate a control signal which effectuates a refined autonomous driving command according to the comparison within the first, second, and third terms.
2 . The vehicle of claim 1 , wherein the objective cost function further comprises a fourth term containing a slack variable.
3 . The vehicle of claim 2 , wherein the refined autonomous driving command is generated by computing a second autonomous driving command which reduces the objective cost function by driving the slack variable to zero.
4 . The vehicle of claim 2 , wherein the slack variable comprises one or more vectors corresponding to one or more state envelopes for the vehicle, where each of the one or more state envelopes define a plurality of states for stable driving and a plurality of states for non-stable driving.
5 . The vehicle of claim 1 , wherein the vehicle state comprises a data associated with a current operational state of the vehicle.
6 . The vehicle of claim 5 , wherein the vehicle state further comprises one or more data associated with predicted future operational states of the vehicle.
7 . The vehicle of claim 1 , wherein:
the human driver input comprises a current human driver command and one or more predicted human driver commands for the defined time horizon; and
the pseudo-driver input comprises one or more predicted pseudo-driver commands for the defined time horizon.
8 . The vehicle of claim 7 , wherein the current human driver command comprises one or more commands a human driver in the vehicle places on one or more motive systems of the vehicle.
9 . The vehicle of claim 7 , wherein the current human driver command comprises a lateral force command placed on a steering system of the vehicle.
10 . The vehicle of claim 1 , wherein the pseudo-driver input is a stabilizing prediction steering command that is based in part on the received path of travel.
11 . The vehicle of claim 10 , wherein the stabilizing prediction steering command is a stable lane keeping feedforward command.
12 . A method for controlling a vehicle having a semi-autonomous mode and a fully autonomous mode, the method comprising:
receiving a path of travel for a defined time horizon, the path of travel comprising positions as a function of the defined time horizon;
in response to receiving a driving input and a vehicle state for the defined time horizon and based on the received path of travel, determining a reference driving input and a reference vehicle state for the defined time horizon, wherein the driving input is a human driver input when the vehicle is operating in a semi-autonomous mode and a pseudo-driver input when the vehicle is operating in a fully autonomous mode;
comparing the driving input and the reference driving input to an autonomous driving command and the reference vehicle state to the vehicle state within one or more terms of an objective cost function by:
comparing the reference vehicle state to the vehicle state within a first term of the objective cost function;
comparing the reference driving input to the autonomous driving command within a second term of the objective cost function; and
comparing the driving input to the autonomous driving command within a third term of the objective cost function;
generating a control signal which effectuates a refined autonomous driving command according to the comparison within the first, second, and third terms.
13 . The method of claim 12 , wherein the objective cost function further comprises a fourth term containing a slack variable comprising one or more vectors corresponding to one or more state envelopes for the vehicle, where each of the one or more state envelopes define a plurality of states for stable driving and a plurality of states for non-stable driving.
14 . The method of claim 13 , wherein the refined autonomous driving command is generated by computing a second autonomous driving command which reduces the objective cost function by driving each of the one or more vectors of the slack variable to zero.
15 . The method of claim 12 , wherein the vehicle state comprises a data associated with a current operational state of the vehicle.
16 . The method of claim 15 , wherein the vehicle state further comprises one or more data associated with predicted future operational states of the vehicle.
17 . The method of claim 12 , wherein:
the human driver input comprises a current human driver command and one or more predicted human driver commands for the defined time horizon; and
the pseudo-driver input comprises one or more predicted pseudo-driver commands for the defined time horizon.
18 . The method of claim 17 , wherein the current human driver command comprises one or more commands a human driver in the vehicle places on one or more motive systems of the vehicle.
19 . The method of claim 17 , wherein the current human driver command comprises a lateral force command placed on a steering system of the vehicle.
20 . The method of claim 12 , wherein the pseudo-driver input is a stabilizing prediction steering command that is based in part on the received path of travel.