Model predictive lane centering control with reference switching and disturbance rejection

1 . A system for model predictive lane centering (MPLC) control with reference switching and disturbance rejection, the system comprising:

a vehicle;

one or more sensors disposed on the vehicle, the one or more sensors measuring real-time static and dynamic data about the vehicle;

one or more actuators disposed on the vehicle, the one or more actuators altering static and dynamic characteristics of the vehicle;

a controller having a processor, a memory, and input/output (I/O) ports, the I/O ports in communication with the one or more sensors and the one or more actuators, the processor executing program code portions stored in the memory, the program code portions including an MPLC application comprising:

a first control logic for utilizing model predictive control (MPC) for selectively tracking static and dynamic references;

a second control logic for adapting MPC weights and constraints for trajectory tracking when the system switches between static and dynamic references;

a third control logic that utilizes a Kalman filter to estimate a lateral force disturbance and a yaw moment disturbance acting upon the vehicle, and a measurement bias corrupting a measured yaw rate;

a fourth control logic for detecting anomalies in MPC reference trajectory; and

a fifth control logic for adjusting actuator constraints in response to a detected anomaly, wherein the system for MPLC control actively and continuously adjusts actuator output commands to selectively cause the vehicle to track and follow a current lane center, wherein upon receiving a lane change command from one or more of a vehicle occupant and an advanced driver assistance system (ADAS), the MPLC application switches to tracking the dynamic reference, wherein the dynamic reference is a smooth path that extends between a current static reference, and a future static reference; and upon receipt of the lane change command, smoothly executes a lane change by instructing the one or more actuators to alter position and smoothly alter a vehicle trajectory to enter an adjacent lane before returning to tracking and following a center of the adjacent lane.

2 . The system of claim 1 , wherein the first control logic further comprises:

control logic for obtaining data from the one or more sensors and the one or more actuators;

control logic for performing static reference tracking, wherein the static reference tracking causes the vehicle to track and follow the current lane center; and

control logic for offsetting the vehicle from the current lane center while following and tracking the current lane center.

3 . The system of claim 1 , wherein the second control logic further comprises:

control logic for adaptively adjusting MPC weights based on control objectives, wherein when a reference changes, the weights change; and

control logic that, upon receipt of the lane change command, adaptively adjusts weights and constraints and subsequently ramps the weights and constraints back to nominal values over a predetermined period of time, thereby smoothing a transition between tracking static and dynamic references.

4 . The system of claim 3 , further comprising:

control logic that penalizes rapid changes in actuators positions;

control logic that, depending on whether a reference is a static reference or a dynamic reference, alters three distinct inputs to the MPC, namely: initial lateral error (e y ), initial heading error (e ψ ), and reference curvature waypoints ({dot over (ψ)} r ) according to:

d

dt

[

e

y

V

y

e

ψ

ψ

.

]

=

[

0

1

V

x

r

0

0

-

C

f

+

C

r

mV

x

r

0

-

V

x

r

-

C

f

⁢

1

f

-

C

r

⁢

1

r

mV

x

r

0

0

0

1

0

-

C

f

⁢

1

f

-

C

r

⁢

1

r

I

zz

⁢

V

x

r

0

-

C

f

⁢

1

f

2

-

C

r

⁢

1

r

2

I

zz

⁢

V

x

r

]

[

e

y

V

y

e

ψ

ψ

.

]

+

[

0

c

f

m

0

C

f

⁢

1

f

I

zz

]

[

⁠

δ

f

]

+

[

0

a

y

d

-

ψ

.

r

R

z

d

]

where: V x or V xr defines a vehicle longitudinal velocity; V y is vehicle lateral velocity; ω z or ψ is a yaw rate of the vehicle; e y is a lateral error (distance) from a target path; e ψ is a heading error from target path; ψ r is a yaw rate reference of target path (related to target path curvature); m is a mass of the vehicle; Izz is a rotational inertia of the vehicle; l f , l r are distances from a center of gravity of the vehicle to front and rear axles of the vehicle; C f , C r are front and rear tire cornering stiffnesses; δ f is a front steering angle, and δ r is a rear steering angle; and a yd and R zd are lateral acceleration (force) and yaw acceleration (moment) disturbances, respectively.

5 . The system of claim 1 , wherein the third control logic further comprises:

control logic that compensates for estimated lateral force disturbance and estimated yaw moment disturbance by applying each of the estimated lateral force disturbance and estimated yaw moment disturbance along a prediction horizon in the MPC; and

control logic that utilizes a bias-adjusted yaw rate in an initial state of the MPC, thereby compensating for an estimated measurement bias.

6 . The system of claim 5 , wherein Kalman filter estimations of lateral force disturbance and estimated yaw moment disturbance use measured vehicle speeds and vehicle motion relative to a measured static reference, wherein the measured static reference defines a static reference path.

7 . The system of claim 1 , wherein the fourth control logic further comprises:

control logic that detects anomalies in the MPC reference trajectory by comparing a trajectory offset, a heading, and a curvature with respect to moving averages of the trajectory offset, heading, and curvature; and

control logic that detects an anomaly when a difference between one or more of the trajectory offset, heading and curvature and moving averages of the trajectory offset, heading and curvature exceed a predetermined threshold.

8 . The system of claim 7 , further comprising:

control logic that inhibits anomaly detection using semantics from a trajectory planner in predefined situations including when a lane change has been requested.

9 . The system of claim 1 , wherein the fifth control logic further comprises:

control logic that adjusts actuator constraints by decreasing actuator magnitude and rate limits from a first level to a second level less than the first level for a predetermined period of time upon detecting an anomaly, wherein the actuator magnitude limit is adjusted to limit vehicle lateral acceleration to within a band of predicted lateral acceleration based on a last anomaly-free reference trajectory curvature.

10 . The system of claim 9 , further comprising:

control logic that steps the actuator rate limit down to a calibratable fraction of a nominal actuator rate limit during the predetermined period of time; and

wherein both magnitude and rate limits are temporarily held at reduced value before increasing linearly back to nominal values.

11 . A method for model predictive lane centering (MPLC) control with reference switching and disturbance rejection, the method comprising:

measuring real-time static and dynamic data about a vehicle with one or more sensors disposed on the vehicle;

altering static and dynamic characteristics of the vehicle with one or more actuators disposed on the vehicle;

executing, by a processor of a controller of the vehicle, program code portions stored in memory of the controller, the controller further including input/output (I/O) ports, communicating with the one or more sensors and the one or more actuators, the program code portions including an MPLC application comprising control logic for:

utilizing model predictive control (MPC) for selectively tracking static and dynamic references;

adapting MPC weights and constraints for trajectory tracking when switching between static and dynamic references, including:

adaptively adjusting MPC weights based on control objectives, wherein when a reference changes, the weights change; and

upon receipt of a lane change command, adaptively adjusting weights and constraints and subsequently ramps the weights and constraints back to nominal values over a predetermined period of time, thereby smoothing a transition between tracking static and dynamic references;

utilizing a Kalman filter to estimate a lateral force disturbance and a yaw moment disturbance acting upon the vehicle, and a measurement bias corrupting a measured yaw rate;

detecting anomalies in MPC reference trajectory; and

adjusting actuator constraints in response to a detected anomaly, wherein the MPLC application actively and continuously adjusts actuator output commands to selectively cause the vehicle to track and follow a current lane center, and upon receipt of a lane change command, smoothly executes a lane change by instructing the one or more actuators to alter position and smoothly alter a vehicle trajectory to enter an adjacent lane before returning to tracking and following a center of the adjacent lane.

12 . The method of claim 11 , further comprising:

obtaining data from the one or more sensors and the one or more actuators;

performing static reference tracking, wherein the static reference tracking causes the vehicle to track and follow the current lane center; and

offsetting the vehicle from the current lane center while following and tracking the current lane center.

13 . The method of claim 11 , wherein upon receiving the lane change command from one or more of a vehicle occupant and an advanced driver assistance system (ADAS), executing control logic of the MPLC application that switches to tracking the dynamic reference, wherein the dynamic reference is a smooth path that extends between a current static reference, and a future static reference.

14 . The method of claim 11 , further comprising:

penalizing rapid changes in actuators positions;

depending on whether a reference is a static reference or a dynamic reference, alters three distinct inputs to the MPC, namely: initial lateral error (e y ), initial heading error (e ψ ), and reference curvature waypoints ({dot over (ψ)} r ) according to:

d

dt

[

e

y

V

y

e

ψ

ψ

.

]

=

[

0

1

V

x

r

0

0

-

C

f

+

C

r

mV

x

r

0

-

V

x

r

-

C

f

⁢

1

f

-

C

r

⁢

1

r

mV

x

r

0

0

0

1

0

-

C

f

⁢

1

f

-

C

r

⁢

1

r

I

zz

⁢

V

x

r

0

-

C

f

⁢

1

f

2

-

C

r

⁢

1

r

2

I

zz

⁢

V

x

r

]

[

e

y

V

y

e

ψ

ψ

.

]

+

[

0

c

f

m

0

C

f

⁢

1

f

I

zz

]

[

⁠

δ

f

]

+

[

0

a

y

d

-

ψ

.

r

R

z

d

]

where: V x or V xr defines a vehicle longitudinal velocity; V y is vehicle lateral velocity; ω z or ψ is a yaw rate of the vehicle; e y is a lateral error (distance) from a target path; e ψ is a heading error from target path; ψ r is a yaw rate reference of target path (related to target path curvature); m is a mass of the vehicle; Izz is a rotational inertia of the vehicle; l f , l r are distances from a center of gravity of the vehicle to front and rear axles of the vehicle; C f , C r are front and rear tire cornering stiffnesses; δ f is a front steering angle, and δ r is a rear steering angle; and a yd and R zd are lateral acceleration (force) and yaw acceleration (moment) disturbances, respectively.

15 . The method of claim 11 , further comprising:

compensating for estimated lateral force disturbance and estimated yaw moment disturbance by applying each of the estimated lateral force disturbance and estimated yaw moment disturbance along a prediction horizon in the MPC; and

utilizing a bias-adjusted yaw rate in an initial state of the MPC, thereby compensating for an estimated measurement bias.

16 . The method of claim 15 , further comprising:

using measured vehicle speeds and vehicle motion relative to a measured static reference, wherein the measured static reference defines a static reference path to generate Kalman filter estimations of lateral force disturbance and estimated yaw moment disturbance.

17 . The method of claim 11 , further comprising:

detecting anomalies in the MPC reference trajectory by comparing a trajectory offset, a heading, and a curvature with respect to moving averages of the trajectory offset, heading, and curvature; and

detecting an anomaly when a difference between one or more of the trajectory offset, heading and curvature and moving averages of the trajectory offset, heading and curvature exceed a predetermined threshold.

18 . The method of claim 11 , further comprising:

adjusting actuator constraints by decreasing actuator magnitude and rate limits from a first level to a second level less than the first level for a predetermined period of time upon detection of an anomaly, wherein the actuator magnitude limit is adjusted to limit vehicle lateral acceleration to within a band of predicted lateral acceleration based on a last anomaly-free reference trajectory curvature; and

stepping the actuator rate limit down to a calibratable fraction of a nominal actuator rate limit during the predetermined period of time; and

wherein both magnitude and rate limits are temporarily held at reduced value before increasing linearly back to nominal values.

19 . The method of claim 17 further comprising:

inhibiting anomaly detection using semantics from a trajectory planner in predefined situations including when a lane change has been requested.

20 . A method for model predictive lane centering (MPLC) control with reference switching and disturbance rejection, the method comprising:

measuring real-time static and dynamic data about a vehicle with one or more sensors disposed on the vehicle;

altering static and dynamic characteristics of the vehicle with one or more actuators disposed on the vehicle;

executing, by a processor of a controller of the vehicle, program code portions stored in memory of the controller, the controller further including input/output (I/O) ports, communicating with the one or more sensors and the one or more actuators, the program code portions including an MPLC application comprising control logic for:

utilizing model predictive control (MPC) for selectively tracking static and dynamic references, including:

obtaining data from the one or more sensors and the one or more actuators;

performing static reference tracking, wherein the static reference tracking causes the vehicle to track and follow a current lane center; and

offsetting the vehicle from the current lane center while following and tracking the current lane center, wherein upon receiving a lane change command from one or more of a vehicle occupant and an advanced driver assistance system (ADAS), executing control logic of the MPLC application that switches to tracking the dynamic reference, wherein the dynamic reference is a smooth path that extends between a current static reference, and a future static reference;

adapting MPC weights and constraints for trajectory tracking when switching between static and dynamic references, including:

adaptively adjusting MPC weights based on control objectives, wherein when a reference changes, the weights change; and

upon receipt of the lane change command, adaptively adjusting weights and constraints and subsequently ramps the weights and constraints back to nominal values over a predetermined period of time, thereby smoothing a transition between tracking static and dynamic references;

penalizing rapid changes in actuators positions;

depending on whether a reference is a static reference or a dynamic reference, alters three distinct inputs to the MPC, namely: initial lateral error (e y ), initial heading error (e ψ ), and reference curvature waypoints ({dot over (ψ)} r ) according to:

d

dt

[

e

y

V

y

e

ψ

ψ

.

]

=

[

0

1

V

x

r

0

0

-

C

f

+

C

r

mV

x

r

0

-

V

x

r

-

C

f

⁢

1

f

-

C

r

⁢

1

r

mV

x

r

0

0

0

1

0

-

C

f

⁢

1

f

-

C

r

⁢

1

r

I

zz

⁢

V

x

r

0

-

C

f

⁢

1

f

2

-

C

r

⁢

1

r

2

I

zz

⁢

V

x

r

]

[

e

y

V

y

e

ψ

ψ

.

]

+

[

0

c

f

m

0

C

f

⁢

1

f

I

zz

]

[

⁠

δ

f

]

+

[

0

a

y

d

-

ψ

.

r

R

z

d

]

where: V x or V xr defines a vehicle longitudinal velocity; V y is vehicle lateral velocity; ω z or ψ is a yaw rate of the vehicle; e y is a lateral error (distance) from a target path; e ψ is a heading error from target path; ψ r is a yaw rate reference of target path (related to target path curvature); m is a mass of the vehicle; Izz is a rotational inertia of the vehicle; l f , l r are distances from a center of gravity of the vehicle to front and rear axles of the vehicle; C f , C r are front and rear tire cornering stiffnesses; δ f is a front steering angle, and δ r is a rear steering angle; and a yd and R zd are lateral acceleration (force) and yaw acceleration (moment) disturbances, respectively;

utilizing a Kalman filter to estimate a lateral force disturbance and a yaw moment disturbance acting upon the vehicle, and a measurement bias corrupting a measured yaw rate, including:

compensating for estimated lateral force disturbance and estimated yaw moment disturbance by applying each of the estimated lateral force disturbance and estimated yaw moment disturbance along a prediction horizon in the MPC;

utilizing a bias-adjusted yaw rate in an initial state of the MPC, thereby compensating for an estimated measurement bias; and

using measured vehicle speeds and vehicle motion relative to a measured static reference, wherein the measured static reference defines a static reference path to generate Kalman filter estimations of lateral force disturbance and estimated yaw moment disturbance;

detecting anomalies in MPC reference trajectory, including:

detecting anomalies in the MPC reference trajectory by comparing a trajectory offset, a heading, and a curvature with respect to moving averages of the trajectory offset, heading, and curvature;

detecting an anomaly when a difference between one or more of the trajectory offset, heading and curvature and moving averages of the trajectory offset, heading and curvature exceed a predetermined threshold; and

inhibiting anomaly detection using semantics from a trajectory planner in predefined situations including when a lane change has been requested; and

adjusting actuator constraints in response to a detected anomaly, including:

adjusting actuator constraints by decreasing actuator magnitude and rate limits from a first level to a second level less than the first level for a predetermined period of time upon detection of an anomaly, wherein the actuator magnitude limit is adjusted to limit vehicle lateral acceleration to within a band of predicted lateral acceleration based on a last anomaly-free reference trajectory curvature; and

stepping the actuator rate limit down to a calibratable fraction of a nominal actuator rate limit during the predetermined period of time; and

wherein both magnitude and rate limits are temporarily held at reduced value before increasing linearly back to nominal values, wherein the MPLC application actively and continuously adjusts actuator output commands to selectively cause the vehicle to track and follow a current lane center, and upon receipt of a lane change command, smoothly executes a lane change by instructing the one or more actuators to alter position and smoothly alter a vehicle trajectory to enter an adjacent lane before returning to tracking and following a center of the adjacent lane.