Model predictive lane centering control with reference switching and disturbance rejection

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;
• 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.

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 (ey), 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:
• Vx or Vxr defines a vehicle longitudinal velocity;
• Vy is vehicle lateral velocity;
• ωz or ψ is a yaw rate of the vehicle;
• ey 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;
• lf, lr are distances from a center of gravity of the vehicle to front and rear axles of the vehicle;
• Cf, Cr are front and rear tire cornering stiffnesses;
• δf is a front steering angle, and δr is a rear steering angle;
• Ayd and Rzd are lateral acceleration (force) and yaw acceleration (moment) disturbances, respectively;
• x, xd are state vector, and vector of disturbance/uncertainty states, respectively;
• and a hat {circumflex over ( )} on top of any variable indicates that a hat-bearing variable is an estimate;
• 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.