Preview lateral control for automated driving

Abstract

A method for providing vehicle lateral steering control. The method includes providing a mathematical model of vehicle dynamics that includes a state variable, a steering control variable and a future road disturbance factor that defines the upcoming road curvature, banks and slopes of the roadway. The method determines an optimal steering control signal that includes a feedback portion and a feed-forward portion, where the feed-forward portion includes the road disturbance factor. The method determines a state variable and a control variable for the current roadway curvature, bank and slope for stationary motion of the vehicle for constant speed, yaw rate and lateral velocity. The method then introduces a new state variable and control variable for dynamic vehicle motion for variable speed, yaw rate and lateral velocity that is a difference between the state and control variables for predicted future times and the steady state variables.

Claims

1. A method for providing lateral steering control for an autonomously driven or semi-autonomously driven vehicle traveling along a roadway, said method comprising: providing a mathematical model of vehicle dynamics that includes a state variable, a steering control variable and a future roadway disturbance factor that defines curvature, bank and/or slope of the roadway, wherein providing a mathematical model includes providing a four-degree of freedom mathematical model or a six-degree of freedom mathematical model where the model uses a one-track bicycle model for the four-degree of freedom mathematical model and uses a combination of a one-track bicycle model and a one-degree of freedom steering model for the six-degree of freedom mathematical model; determining a steering control goal using the steering control variable that reduces a difference between a current vehicle path and a desired vehicle path; determining an optimal steering control signal using the mathematical model that provides the steering control goal and that includes a feedback portion and a feed-forward portion, where the feed-forward portion includes the road disturbance factor; and providing the control signal to a steering controller.

2. The method according to claim 1 wherein providing a mathematical model includes using the one-track bicycle model to provide model dimensions of lateral offset Δy of a vehicle center of gravity from the desired vehicle path, a vehicle heading angle error Δψ, vehicle lateral velocity Δy and vehicle yaw rate r, and the one-degree of freedom steering column model to provide steering wheel angle φ and steering wheel angle rotation rate {dot over (φ)}.

3. The method according to claim 2 wherein a derivative of the state variable is defined as:
{dot over (X)}=AX+BU+h where X is the state variable, U is the steering control variable, h is the road disturbance factor, and A and B are matrices.

4. The method according to claim 3 wherein providing a mathematical model includes defining the state variable X by lane position dynamics and the bicycle model for an angle interface steering control as:
X=[Δy,ΔΨ,V.sub.y,r].sup.T where the state variable X is a road wheel angle, and defining the state variable X by the lane position dynamics, the bicycle model and the steering model for a torque interface steering control as:
X=[Δy,ΔΨ,V.sub.y,r,{dot over (φ)},φ].sup.T where the state variable X a combined torque command and a road wheel angle.

5. The method according to claim 4 wherein the road disturbance factor h is defined as: $h=\left[\begin{array}{c}0\\ -{\kappa V}_x\\ g\cos \beta \sin \gamma \\ 0\end{array}\right]$ for the angle interface steering control, or by: $h=\left[\begin{array}{c}0\\ -{\kappa V}_x\\ g\cos \beta \sin \gamma \\ 0\\ 0\\ 0\end{array}\right]$ for the torque interface steering control, where κ is roadway curvature, γ is roadway bank and β is roadway slope.

6. The method according to claim 1 wherein determining a steering control goal includes defining a relationship between a steering command and a cost function.

7. The method according to claim 6 wherein the cost function is a quadratic function.

8. The method according to claim 7 wherein the quadratic function is defined by the equation: $J={\int }_0^{\Delta T}\left\{{\left[\begin{array}{cc}y& \phi \end{array}\right]}_{\mathrm{err}}.Math.Q\left(t\right).Math.{\left[\begin{array}{c}y\\ \phi \end{array}\right]}_{\mathrm{err}}+U.Math.R\left(t\right).Math.u^T\right\}dt,$ where J is the cost function, y.sub.err is a lateral offset error (y.sub.desired−y.sub.predicted,), φ.sub.err is a heading angle error (φ.sub.desired−φ.sub.predicted,), and Q(t) is a turning weighting matrix for predicted errors given as: $Q=\left(\begin{array}{cc}{Q}_{\mathrm{lateral}}& 0\\ 0& Q_{\mathrm{heading}}\end{array}\right),$ where Q.sub.lateral is the weight associated with lateral error, Q.sub.heading is a weight associated with heading error, and R(t) is a weighting matrix for control actions given as: $R=\left(\begin{array}{ccc}{R}_{\mathrm{front}}& 0& 0\\ 0& R_{\mathrm{rear}}& 0\\ 0& 0& R_{\mathrm{db}}\end{array}\right),$ where R.sub.front is a weight associated with front steer, R.sub.rear is a weight associated with rear steer, and R.sub.db is a weight associated with differential braking.

9. A method for providing lateral steering control for an autonomously driven or semi-autonomously driven vehicle traveling along a roadway, said method comprising: providing a mathematical model of vehicle dynamics that includes a state variable, a steering control variable and a future roadway disturbance factor that defines curvature, bank and/or slope of the roadway; determining a steering control goal using the steering control variable that reduces a difference between a current vehicle path and a desired vehicle path; determining an optimal steering control signal using the mathematical model that provides the steering control goal and that includes a feedback portion and a feed-forward portion, where the feed-forward portion includes the road disturbance factor, wherein determining a steering control signal includes using the mathematical model to define a state variable and a control variable for a vehicle steady state or stationary motion condition, where the stationary motion has a constant speed, yaw rate and lateral velocity, for a current curvature, bank and slope of the roadway; and providing the control signal to a steering controller.

10. The method according to claim 9 wherein determining a steering control signal includes transforming the state variable and the control variable for the vehicle steady state condition by subtracting the state variable and the control variable for the steady state condition from the state variable and the control variable at future sample times to obtain a new state variable and a new control variable for each sample time that provides a deviation of the dynamic motion of the vehicle from the stationary motion.

11. The method according to claim 10 wherein determining a steering control signal includes using model predictive control.

12. The method according to claim 10 wherein determining a steering control signal includes using the equation: $U_k=\frac{\begin{array}{c}{.Math.}_{j=1}^{j=p}{\left(C_d{.Math.}_{i=1}^j{A}_d^{i-1}{B}_d\right)}^T{Q}_{k+j}[r_{k+j}-\\ Q_{k+j}\left[r_{k+j}-C_d{A}_d^j{x}_k-C_d{.Math.}_{i=1}^j{A}_d^{i-1}{h}_k\right]\end{array}}{\left(R_k+{.Math.}_{j=1}^{j=p}{\left(C_d{.Math.}_{i=1}^j{A}_d^{i-1}{B}_d\right)}^T{Q}_{k+j}\left(C_d{.Math.}_{i=1}^j{A}_d^{i-1}{B}_d\right)\right)}$ where:
−[Σ.sub.j=1.sup.p[Σ.sub.i=1.sup.j(CA.sup.i−1B).sup.TR.sub.k+j(CA.sup.i−1B)]+Q.sub.k].sup.−1Σ.sub.j=1.sup.p[Σ.sub.i=1.sup.j(CA.sup.i−1B).sup.TR.sub.k+jCA.sup.j]X.sub.k, is the feedback portion of the steering control variable U.sub.k, and:
[Σ.sub.j=1.sup.p[Σ.sub.i=1.sup.j(CA.sup.i−1B).sup.TR.sub.k+j(CA.sup.i−1B)]+Q.sub.k].sup.1Σ.sub.j=1.sup.p[Σ.sub.i=1.sup.j(CA.sup.i−1B).sup.TR.sub.k+jr.sub.k+j−CΣ.sub.i=1.sup.jA.sup.i−1h.sub.k+j−i]+K.sub.kx.sub.SS+U.sub.SS, is the feed-forward portion of the control variable U.sub.k, and:
CΣ.sub.i=1.sup.jA.sup.i−1h.sub.k+j−i, is future curvature, bank and slope information, and:
K.sub.kx.sub.SS+U.sub.SS, is current curvature, bank and slope information, where A.sub.d, B.sub.d, C.sub.d and h.sub.k are discreet versions of matrices A, B, C and h, respectively, Q.sub.k+j is a weight on path tracking error, R.sub.k is a weight on actuator command, and r.sub.k+j is a vector of set points.

13. The method according to claim 9 wherein determining a steering control goal includes defining a relationship between a steering command and a cost function.

14. The method according to claim 13 wherein the cost function is a quadratic function.

15. A method for providing lateral steering control for an autonomously driven or semi-autonomously driven vehicle traveling along a roadway, said method comprising: providing a four-degree of freedom or a six-degree of freedom mathematical model of vehicle dynamics that includes a state variable, a steering control variable and a future roadway disturbance factor that defines curvature, bank and/or slope of the roadway; determining a steering control goal using the steering control variable that reduces a difference between a current vehicle path and a desired vehicle path; determining an optimal steering control signal using the mathematical model that provides the steering control goal and that includes a feedback portion and a feed-forward portion, where the feed-forward portion includes the road disturbance factor, wherein determining the steering control signal includes defining a state variable and a control variable for a vehicle steady state or stationary motion condition for a current curvature, bank and slope of the roadway, where the stationary motion has a constant speed, yaw rate and lateral velocity, and transforming the state variable and the control variable for the vehicle steady state condition by subtracting the state variable and the control variable for the steady state condition from the state variable and the control variable at future sample times to obtain a new state variable and a new control variable for each sample time that provides a deviation of the dynamic motion of the vehicle from the stationary motion; and providing the control signal to a steering controller.

16. The method according to claim 15 wherein determining a steering control signal includes using the equation: $U_k=\frac{\begin{array}{c}{.Math.}_{j=1}^{j=p}{\left(C_d{.Math.}_{i=1}^j{A}_d^{i-1}{B}_d\right)}^T{Q}_{k+j}[r_{k+j}-\\ C_d{A}_d^j{x}_k-C_d{.Math.}_{i=1}^j{A}_d^{i-1}{h}_k]\end{array}\begin{array}{c}\\ \end{array}}{R_k+{.Math.}_{j=1}^{j=p}{\left(C_d{.Math.}_{i=1}^j{A}_d^{i-1}{B}_d\right)}^T{Q}_{k+j}\left(C_d{.Math.}_{i=1}^j{A}_d^{i-1}{B}_d\right)}$ where:
−[Σ.sub.j=1.sup.p[Σ.sub.i=1.sup.j(CA.sup.i−1B).sup.TR.sub.k+j(CA.sup.i−1B)]+Q.sub.k].sup.−1Σ.sub.j=1.sup.p[Σ.sub.i=1.sup.j(CA.sup.i−1B).sup.TR.sub.k+jCA.sup.j]X.sub.k, is the feedback portion of the steering control variable U.sub.k, and:
[Σ.sub.j=1.sup.p[Σ.sub.i=1.sup.j(CA.sup.i−1B).sup.TR.sub.k+j(CA.sup.i−1B)]+Q.sub.k].sup.1Σ.sub.j=1.sup.p[Σ.sub.i=1.sup.j(CA.sup.i−1B).sup.TR.sub.k+jr.sub.k+j−CΣ.sub.i=1.sup.jA.sup.i−1h.sub.k+j−i]+K.sub.kx.sub.SS+U.sub.SS, is the feed-forward portion of the control variable U.sub.k, and:
CΣ.sub.i=1.sup.jA.sup.i−1h.sub.k+j−i, is future curvature, bank and slope information, and:
K.sub.kx.sub.SS+U.sub.SS, is current curvature, bank and slope information, where A.sub.d, B.sub.d, C.sub.d and h.sub.k are discreet versions of matrices A, B, C and h, respectively, Q.sub.k+j is a weight on path tracking error, R.sub.k is a weight on actuator command, and r.sub.k+j is a vector of set points.

17. The method according to claim 15 wherein providing a mathematical model includes providing a mathematical model that uses a one-track bicycle model for the four-degree of freedom mathematical model and uses a combination of a one-track bicycle model and a one-degree of freedom steering model for the six-degree of freedom mathematical model.

18. The method according to claim 17 wherein providing a mathematical model includes using the one-track bicycle model to provide model dimensions of lateral offset Δy of a vehicle center of gravity from the desired vehicle path, a vehicle heading angle error Δψ, vehicle lateral velocity Δy and vehicle yaw rate r, and the one-degree of freedom steering column model to provide steering wheel angle φ and steering wheel angle rotation rate {dot over (φ)}.

19. The method according to claim 18 wherein a derivative of the state variable is defined as:
{dot over (X)}=AX+BU+h where X is the state variable, U is the steering control variable, h is the road disturbance factor, and A and B are matrices.

20. The method according to claim 19 wherein providing a mathematical model includes defining the state variable X by lane position dynamics and the bicycle model for an angle interface steering control as:
X=[Δy,ΔΨ,V.sub.y,r].sup.T where the state variable X is a road wheel angle, and defining the state variable X by the lane position dynamics, the bicycle model and the steering model for a torque interface steering control as:
X=[Δy,ΔΨ,V.sub.y,r,φ;φ].sup.T where the state variable X is a combined torque command and a road wheel angle.

21. The method according to claim 20 wherein the road disturbance factor h is defined as: $h=\left[\begin{array}{c}0\\ -{\kappa V}_x\\ g\cos \beta \sin \gamma \\ 0\end{array}\right]$ for the angle interface steering control or by: $h=\left[\begin{array}{c}0\\ -{\kappa V}_x\\ g\cos \beta \sin \gamma \\ 0\\ 0\\ 0\end{array}\right]$ for the torque interface steering control, where κ is roadway curvature, γ is roadway bank and β is roadway slope.

Description

BRIEF DESCRIPTION OF THE DRAWINGS

(1) FIG. 1 is an illustration of a vehicle including a path processing system for providing vehicle lateral steering control assist in an autonomously driven or semi-autonomously driven vehicle;

(2) FIG. 2 is a flow chart diagram showing a method for providing the lateral steering control assist of the system shown in FIG. 1;

(3) FIG. 3 is an illustration of a one-track linear bicycle model; and

(4) FIG. 4 is an illustration including different sample points along a curved travel lane showing a transformation between steady state control variables and dynamic motion control variables.

DETAILED DESCRIPTION OF THE EMBODIMENTS

(5) The following discussion of the embodiments of the invention directed to a system and method that provide lateral steering control assist in an autonomously driven or semi-autonomously driven vehicle including providing a preview of upcoming road curvature, slopes and banks is merely exemplary in nature, and is in no way intended to limit the invention or its applications or uses. For example, the system and method of the invention may have application in other industries and for other types of conveyances.

(6) As will be discussed in detail below, the present invention proposes a system and method for providing lateral steering control assist for vehicle steering in an autonomously or semi-autonomously driven vehicle that may be employed for lane centering, lane changing, lane following, collision imminent steering, etc., where the system and method provide preview information of upcoming road curves, banks and/or slopes. The system and method disclosed herein may incorporate one or more of the algorithms or processes discussed in the references above for detecting a vehicle lane, objects, etc., determining an optimal steering path, and providing a steering angle command signal or steering torque command signal in the particular steering system of the vehicle to steer the vehicle along the path.

(7) FIG. 1 is an illustration 10 showing a vehicle 12 traveling along a travel lane 14 of a roadway, where the vehicle 12 includes a path processing system 16 that identifies the position of the vehicle 12 relative to the lane 14 and provides vehicle steering commands to maintain the vehicle 12 in the travel lane 14 in any suitable manner as generally or specifically discussed herein. The system 16 is intended to represent all of the various modules, controllers, memories, central processing units (CPUs), processors, electronic control units (ECUs), etc. that are necessary to perform and operate the various algorithms and processes discussed herein. The vehicle 12 also includes a map database 20, a navigation system 22, a GPS unit 24, sensors/detectors 26, a vehicle dynamics module 28 and a vehicle controller 30. The map database 20 stores map information at any level of detail that is available, including specific information about travel lanes, and operates in association with the navigation system 22 to display the various maps and other information that is available. The sensor/detectors 26 are intended to represent any and all object detection sensors or cameras on the vehicle 12, such as forward, rear and side cameras, back-up cameras, lidar sensors, long range radar detectors, short range radar detectors, etc., located in any suitable position on the vehicle 12. For the purposes described herein, the sensors/detectors 26 detect lane markings and other suitable objects that identify the lane 14 that can be used by the system 16 to provide proper steering. The vehicle dynamics module 28 provides vehicle longitudinal and lateral speed, vehicle yaw rate, vehicle longitudinal and lateral acceleration, steering angle, steering torque, etc. The vehicle controller 30 controls the operation of the vehicle 12 including steering, brake, throttle, etc. For steering purposes, the controller 30 may provide torque commands and/or steering angle commands to make corrections between a desired course and a predicted course to steering control actuators 32 that provide one or more of electric power steering (EPS), active front steering (AFS), active rear steering (ARS), differential braking (DB), torque vectoring, etc., all well known to those skilled in the art.

(8) FIG. 2 is a flow chart diagram 40 showing a process for providing a steering control command to the vehicle controller 30 as determined by the path processing system 16. At box 42, a mathematical model of the vehicle dynamics is generated for model predictive control (MPC), which is a dynamic vehicle model provided through equations of motion and may employ a one-track linear bicycle model coupled with front steering, rear steering, and/or differential braking, and a one-degree of freedom steering column using an EPS. Such a vehicle steering control using MPC may be found in one or more of the references discussed above. As will be discussed in detail below, the one-track linear bicycle model provides four dimensions or parameters, namely, lateral offset Δy of the vehicle center of gravity (CG) from the lane 14, vehicle heading angle error ΔΨ, vehicle lateral velocity V.sub.y and vehicle yaw rate r, and the one-degree of freedom steering column model provides two other dimensions or parameters, namely, steering wheel angle φ (or pinion angle) and steering wheel angle rotation rate {dot over (φ)}.

(9) In one embodiment, the model is a six-dimensional model including a combination of the one-track bicycle model and the one-degree of freedom steering column model. A one-track linear bicycle model is shown by illustration 50 in FIG. 3 including wheels 52 and 54, vehicle center of gravity 58 and roadway 56, and describes vehicle position with respect to the lane markings of the roadway 56 and clarifies sign convention. The state-space model for vehicle motion includes equations defining relationships of the vehicle 12 to the roadway 56 as:
Δ{dot over (y)}=V.sub.xΔΨ+V.sub.y,  (1)
Δ{dot over (Ψ)}=r−V.sub.xχ,  (2)
and a classical bicycle model for a single track is:

(10) $\begin{array}{cc}{\stackrel{.}{V}}_y=-\frac{C_f+C_r}{M{V}_x}{V}_y-\left(V_x+\frac{a{C}_f-b{C}_r}{M{V}_x}\right)r+\frac{C_f}{M}{\delta }_f,& \left(3\right)\\ \stackrel{.}{r}=\frac{a{C}_f-b{C}_r}{I_z{V}_x}{V}_y\frac{a^2{C}_f+b^2{C}_r}{I_z{V}_x}r+\frac{a{C}_f}{I_z}{\delta }_f,& \left(4\right)\end{array}$
where Δy is the lateral offset of the vehicle center of gravity from the lane (lane offset), ΔΨ is the vehicle heading with respect to the lane (lane heading), V.sub.y is the vehicle lateral velocity, r is the vehicle yaw rate, V.sub.x is the vehicle longitudinal velocity, C.sub.f and C.sub.r are the cornering stiffness of the front and rear axles, respectively, a is the distance from the vehicle's center of gravity to the vehicle's front axle, b is the distance from the vehicle's center of gravity to the vehicle's rear axle, M is the mass of the vehicle 12, I.sub.z is the yaw moment of inertial of the vehicle 12, χ is the path curvature, and δ.sub.f is the front road wheel angle.

(11) It is assumed that the front and rear axle lateral forces F.sub.f and F.sub.r are linear functions of lateral slip angles as:
F.sub.f=C.sub.fa.sub.f,  (5)
F.sub.r=C.sub.ra.sub.r,  (6)
where a.sub.f and a.sub.r are front and rear slip angles, respectively, and where:

(12) $\begin{array}{cc}{\alpha }_f={\delta }_f-\frac{V_y}{V_x}-\frac{a}{V_x}r,& \left(7\right)\\ {\alpha }_r=\frac{V_y}{V_x}+\frac{b}{V_x}r.& \left(8\right)\end{array}$

(13) The mathematical model employs a state variable X that is determined depending on whether the EPS uses an EPS angle interface, where the lane position dynamics and the bicycle model are employed, or an EPS torque interface, where the lane position dynamics, the bicycle model and the steering column model are employed. A torque overlay command can be provided to the EPS to add or subtract torque to the steering system in either direction independent of the driver steering input. Also, for the angle interface, the EPS actuator can add or subtract a steering angle in addition to the steering angle provided by the driver, or provide a steering angle independent of the driver. The state variable X for the angle interface embodiment is defined by:
X=[Δy,ΔΨ,V.sub.y,r].sup.T,  (9)
and the state variable X for the torque interface embodiment is defined by:
X=[Δy,ΔΨ,V.sub.y,r,{dot over (φ)},φ].sup.T  (10)

(14) The change or derivative of the state variable X is defined as:
{dot over (X)}=AX+BU+h,  (11)
where U is a steering control variable, such as steering control for total EPS torque, rear steering angle, differential braking, etc., and is an angle for the angle interface and is a torque for the torque interface, and where in the case for combined front steering, differential braking and rear steering, the control variable U for the angle interface is front steer angle, yaw moment and rear steer angle and for the torque interface is front torque, yaw moment and rear steer angle, and h is a road disturbance factor that defines the curvature, bank and slope of the roadway, which for the angle interface embodiment is:

(15) $\begin{array}{cc}h=\left[\begin{array}{c}0\\ -\kappa V_x\\ g\cos \beta \sin \gamma \\ 0\end{array}\right],& \left(12\right)\end{array}$
and for the torque interface embodiment is:

(16) $\begin{array}{cc}h=\left[\begin{array}{c}0\\ -{\kappa V}_x\\ g\cos \beta \sin \gamma \\ 0\\ 0\\ 0\end{array}\right],& \left(13\right)\end{array}$
where κ is the road curvature, γ is the road bank and β is the road slope, and where governing matrices A and B have the following form.

(17) $\begin{array}{cc}A=\left[\begin{array}{cccccc}0& V_x& 1& 0& 0& 0\\ 0& 0& 0& 1& 0& 0\\ 0& 0& a_{11}& a_{12}& 0& \frac{b_1}{n}\\ 0& 0& a_{21}& a_{22}& 0& \frac{b_2}{n}\\ 0& 0& \frac{2D_f}{{\mathrm{IV}}_x}& \frac{2D_{f}a}{{\mathrm{IV}}_x}& -\frac{c}{I}& -\frac{2D_f}{nI}\\ 0& 0& 0& 0& 1& 0\end{array}\right],& \left(14\right)\\ B=\left[\begin{array}{c}0\\ 0\\ 0\\ 0\\ 1/I\\ 0\end{array}\right],& \left(15\right)\\ a_{11}=\frac{C_f+C_r}{{\mathrm{MV}}_x},& \left(16\right)\\ a_{12}=-V_x-\frac{{\mathrm{aC}}_f-{\mathrm{bC}}_r}{{\mathrm{MV}}_x},& \left(17\right)\\ a_{21}=-\frac{{\mathrm{aC}}_f-{\mathrm{bC}}_r}{I_z{V}_x},& \left(18\right)\\ a_{22}=-\frac{a^2{C}_f+b^2{C}_r}{I_z{V}_x},& \left(19\right)\\ b_1=\frac{C_f}{M},& \left(20\right)\\ b_2=\frac{{\mathrm{aC}}_f}{I_z}.& \left(21\right)\end{array}$

(18) It is noted that matrix B in equation (15) will have different forms depending on the steering actuator set available, where each column in the matrix defines a control input to a steering actuator. The example given above where the matrix B has one column, the vehicle 12 has front EPS only. If the vehicle 12 has front and rear steering actuators, then the matrix B will have two columns, if the vehicle 12 has front steer and differential braking, then the matrix B will have two columns, and if the vehicle 12 has three actuators for front and rear steering and differential braking, then matrix B will have three columns. It is also noted that instead of the factor h being a curvature term defined as a known disturbance in the '528 application, the factor h is the road disturbance for bank, slope and curvature as defined herein that is provided by cameras, the map database 20, etc.

(19) Once the model is provided, the algorithm identifies a steering control goal at box 44 that minimizes the deviation of the vehicle center of gravity (CG) from the planned or desired path to provide corrected steering. Many algorithms are known in the art to define a desired steering path for collision avoidance purposes. As will be discussed below, the steering control goal obtains the control variable U that minimizes a quadratic cost function J. The planned path (or reference trajectory) of the vehicle 12 at any time instant k is a set of points r.sub.k+j, j=1,2, . . . ,p. Each point r.sub.k+j is a two-dimensional vector including the lane lateral position of the vehicle center of gravity, and relative to the vehicle heading angle ΔΨ. The number of set points p is driven by a prediction horizon T.sub.p=pT.sub.s, where T.sub.s is a sampling time. Future outputs of the mathematical model should follow the reference trajectory r.sub.k+j, or in other words, the cost function J associated with the errors y.sub.k+j−r.sub.k+j, j=1,2, . . . ,p should be minimized from its path in the lane.

(20) Since the state variable X.sub.k for sample time k is known, the problem of finding the optimal steering control variable U.sub.k reduces to finding a global minimum of the quadratic function J in the control variable U.sub.k. Such a problem can be solved in a standard quadratic programming manner. A detailed discussion of how the cost function J is minimized can be found in the '528 application, where the cost function J is defined as:

(21) $\begin{array}{cc}J={\int }_0^{\Delta T}\left\{{\left[\begin{array}{cc}y& \phi \end{array}\right]}_{\mathrm{err}}.Math.Q\left(t\right).Math.{\left[\begin{array}{c}y\\ \phi \end{array}\right]}_{\mathrm{err}}+U.Math.R\left(t\right).Math.u^T\right\}\mathrm{dt},& \left(22\right)\end{array}$
where y.sub.err is the lateral offset error (y.sub.desired−y.sub.predicted,), φ.sub.err is the heading angle error (φ.sub.desired−φ.sub.predicted,), and Q(t) is a turning weighting matrix for predicted errors given as:

(22) $\begin{array}{cc}Q=\left(\begin{array}{cc}{Q}_{\mathrm{lateral}}& 0\\ 0& Q_{\mathrm{heading}}\end{array}\right),& \left(23\right)\end{array}$
where Q.sub.lateral is the weight associated with lateral error (output), Q.sub.heading is the weight associated with heading error (output), and R(t) is the weighting matrix for control actions given as:

(23) $\begin{array}{cc}R=\left(\begin{array}{ccc}{R}_{\mathrm{front}}& 0& 0\\ 0& R_{\mathrm{rear}}& 0\\ 0& 0& R_{\mathrm{db}}\end{array}\right),& \left(24\right)\end{array}$
where R.sub.front is the weight associated with front steer (input to the controller), R.sub.rear is the weight associated with rear steer (input to the controller) and R.sub.db is the weight associated with differential braking (input to the control).

(24) At box 46, the algorithm uses the determined future curvature, bank and slope information of the roadway to compute a feed-forward part of the steering control variable U by solving the cost function J of equation (22) discussed above. The algorithm uses the MPC or an optimal linear quadratic (LQ) regulator to determine optimal control variables U for one or more of a front angle overlay or a front torque overlay, a rear steering angle, a yaw moment for differential braking and/or torque vectoring. The algorithm first determines the state variable X and the steering control variable U for stationary motion of the vehicle 12 for a current horizontal curvature, bank and slope of the roadway, where the stationary motion is a circular motion that the vehicle 12 would travel with a constant speed V.sub.y, yaw rate r and lateral velocity V.sub.y, i.e., the roadway is approximated to a circular track. The dynamic motion of the vehicle 12 is defined by equation (11), and by setting the derivative of the state value X to 0, the steady state or stationary motion of the vehicle 12 can be defined as:
AX.sub.SS+BU.sub.SS+h.sub.SS=0,  (25)
where the steady state variables X.sub.SS and U.sub.SS can be obtained analytically using mathematical models or can be measured experimentally, and where the stationary values for circular motion can be defined as:

(25) $\begin{array}{cc}\mathrm{\Delta \gamma }=0,& \left(26\right)\\ \mathrm{\Delta \psi }=-V_y{V}_x,& \left(27\right)\\ V_y=\left(b-\frac{{\mathrm{aMV}}_x^2}{{\mathrm{LC}}_r}\right){\kappa V}_x,& \left(28\right)\\ r={\kappa V}_x,& \left(29\right)\\ \stackrel{.}{\phi }=0,& \left(30\right)\\ \phi =\kappa \left(L+K_{\mathrm{us}}{V}_x^2\right)\mathrm{SR},& \left(31\right)\end{array}$
where L is the vehicle wheel base, which is a+b, K.sub.us is a vehicle understeer gradient, and SR is steering ratio, and where the other variables are defined above.

(26) Once the steady state variables X.sub.SS and U.sub.SS have been obtained, new state variables X.sub.1 and U.sub.1 are introduced that describe a deviation of the dynamic motion of the vehicle 12 from the stationary motion, which is linearized around the steady state, where the deviation from the steady state is defined as:
X.sub.1=X−X.sub.SS,  (32)
U.sub.1=U−U.sub.SS,  (33)
and where the dynamic motion having variable velocity V.sub.x, yaw rate r and lateral velocity V.sub.y for the actual road geometry is defined as:
{dot over (X)}.sub.1=AX.sub.1+BU.sub.1+h.sub.1,  (34)
where h.sub.1=−{dot over (X)}.sub.SS.

(27) This transformation of the state and control variables X and U for the deviation of the dynamic motion from the stationary motion of the vehicle 12 can be described with the help of FIG. 4, which is an illustration 60 showing a curve 62 representing the travel lane that the vehicle 12 is following, where sample point 66 is the current location of the vehicle 12 and has a disturbance factor h(t) for the current curvature, bank and slope of the roadway, where t is time. The curvature, bank and slope of the roadway at sample points 64 for future locations of the vehicle 12 along the curve 62 are defined as h(t+τ), h(t+2τ), . . . , h(t+nτ), where τ is the change in time. The state and control variables X and U are transformed to X.sub.1 and U.sub.1 at each of the sample points 64 illustrated by line 68 for the new disturbance factor h.sub.1.

(28) The MPC or LQ control algorithm is then employed using equations with the terms of the new state and control variables X.sub.1 and U.sub.1. The final expression for the steering control variable U.sub.k is provided as follows, where the final control action is the sum of the feedback and feed-forward terms, and where the feed-forward terms are the sum of the steady-state control value and the planned path related terms so as to consider the roadway curvature, bank and/or slope. Thus, the steering control variable U.sub.k is defined as:
U.sub.k=−[Σ.sub.j=1.sup.p[Σ.sub.i=1.sup.j(CA.sup.i−1B).sup.TR.sub.k+j(CA.sup.i−1B)]+Q.sub.k].sup.−1Σ.sub.j=1.sup.p[Σ.sub.i=1.sup.j(CA.sup.i−1B).sup.TR.sub.k+jCA.sup.j]X.sub.k+[Σ.sub.j=1.sup.p[Σ.sub.i=1.sup.j(CA.sup.i−1B).sup.TR.sub.k+j(CA.sup.i−1B)]+Q.sub.k].sup.1Σ.sub.j=1.sup.p[Σ.sub.i=1.sup.j(CA.sup.i−1B).sup.TR.sub.k+jr.sub.k+j−Ci=1jAi−1hk+j−i+KkxSS+USS,  (35)
where:
−[Σ.sub.j=1.sup.p[Σ.sub.i=1.sup.j(CA.sup.i−1B).sup.TR.sub.k+j(CA.sup.i−1B)]+Q.sub.k].sup.−1Σ.sub.j=1.sup.p[Σ.sub.i=1.sup.j(CA.sup.i−1B).sup.TR.sub.k+jCA.sup.j]X.sub.k,  (36)
is the feedback term of the control variable U.sub.k, and:

(29) 0$\begin{array}{cc}{\left[\underset{j=1}{\overset{p}{.Math.}}\left[\underset{i=1}{\overset{j}{.Math.}}{\left({\mathrm{CA}}^{i-1}B\right)}^T{R}_{k+j}\left({\mathrm{CA}}^{i-1}B\right)\right]+Q_k\right]}^1{.Math.}_{j=1}^p\left[{.Math.}_{i=1}^j{\left({\mathrm{CA}}^{i-1}B\right)}^T{R}_{k+j}{r}_{k+j}-C{.Math.}_{i=1}^j{A}^{i-1}{h}_{k+j-i}\right]+K_k{x}_{\mathrm{ss}}+U_{\mathrm{ss}},& \left(37\right)\end{array}$
is the feed-forward term of the control variable U.sub.k, and:
CΣ.sub.i=1.sup.jA.sup.i−1h.sub.k+j−i,  (38)
is the future curvature, bank and slope information, and:
K.sub.kx.sub.SS+U.sub.SS,  (39)
is the current steady state curvature, bank and slope information.

(30) At box 48, the algorithm provides the steering command to the controller 30 for either front or rear steering, which may be a torque overlay command for an EPS.

(31) As will be well understood by those skilled in the art, the several and various steps and processes discussed herein to describe the invention may be referring to operations performed by a computer, a processor or other electronic calculating device that manipulate and/or transform data using electrical phenomenon. Those computers and electronic devices may employ various volatile and/or non-volatile memories including non-transitory computer-readable medium with an executable program stored thereon including various code or executable instructions able to be performed by the computer or processor, where the memory and/or computer-readable medium may include all forms and types of memory and other computer-readable media.

(32) The foregoing discussion discloses and describes merely exemplary embodiments of the present invention. One skilled in the art will readily recognize from such discussion and from the accompanying drawings and claims that various changes, modifications and variations can be made therein without departing from the spirit and scope of the invention as defined in the following claims.