Model based predictive control for automated lane centering/changing control systems

A system and method for providing steering control for lane changing and lane centering purposes in an autonomous or semi-autonomous vehicle system. A vehicle vision system calculates roadway lane marking information, such as lateral offset, yaw angle and roadway curvature with respect to the vehicle's centered coordinate system. The roadway is then modeled as a second order polynomial equation. The method then predicts roadway lateral position and yaw angle over a pre-defined lane change completion time using a vehicle dynamic model. The method then compares a predicted vehicle path with a desired vehicle path to generate an error value, and calculates a steering angle command to minimize the error value, where the steering angle command is calculated as a function of vehicle lateral position, vehicle lateral speed, vehicle yaw rate and vehicle yaw angle. The steering angle command is then sent to the vehicle steering system.

BACKGROUND OF THE INVENTION

1. Field of the Invention

This invention relates generally to a system and method for providing steering angle control for lane centering and lane changes in an autonomously driven vehicle and, more particularly, to a system and method for providing steering angle control for lane centering and lane changing in an autonomously driven vehicle where a steering angle command is calculated as a simplified function of vehicle lateral position, vehicle lateral speed, vehicle yaw rate and vehicle yaw angle.

2. Discussion of the Related Art

The operation of modern vehicles is becoming more autonomous, i.e., the vehicles are able to provide driving control with less driver intervention. Cruise control systems have been on vehicles for a number of years where the vehicle operator can set a particular speed of the vehicle, and the vehicle will maintain that speed without the driver operating the throttle. Adaptive cruise control systems have been recently developed in the art where not only does the system maintain the set speed, but also will automatically slow the vehicle down in the event that a slower moving preceding vehicle is detected using various sensors, such as radar and cameras. Certain modern vehicles also provide autonomous parking where the vehicle will automatically provide the steering control for parking the vehicle. Some vehicle systems intervene if the driver makes harsh steering changes that may affect vehicle stability. Some vehicle systems attempt to maintain the vehicle near the center of a lane on the road. Further, fully autonomous vehicles have been demonstrated that can drive in simulated urban traffic up to 30 mph, observing all of the rules of the road.

As vehicle systems improve, they will become more autonomous with the goal being a completely autonomously driven vehicle. For example, future vehicles probably will employ autonomous systems for lane changing, passing, turns away from traffic, turns into traffic, etc. As these systems become more prevalent in vehicle technology, it will also be necessary to determine what the driver's role will be in combination with these systems for controlling vehicle speed and steering, and overriding the autonomous system.

U.S. patent application Ser. No. 12/143,439, titled “Path Generation Algorithm for Automated Lane Centering and Lane Changing Control System”, assigned to the assignee of this application and herein incorporated by reference, discloses a system and method for providing path generation for lane centering and lane changing purposes in an autonomous or semi-autonomous vehicle. However, in order to be fully functional, the system needs to generate a steering angle in response to the path generation so that the vehicle follows the desired path. Systems known in the art that provide steering angle commands in autonomous vehicle systems typically are complex.

SUMMARY OF THE INVENTION

In accordance with the teachings of the present invention, a system and method are disclosed for providing steering control for lane changing and lane centering purposes in an autonomous or semi-autonomous vehicle system. A vehicle vision system calculates roadway lane marking information, such as lateral offset, yaw angle and road curvature with respect to the vehicle's centered coordinate system. The roadway is then modeled as a second order polynomial equation. The method then predicts roadway lateral position and yaw angle over a pre-defined lane change completion time using a vehicle dynamic model. The method then compares a predicted vehicle path with a desired vehicle path to generate an error value, and calculates a steering angle command to minimize the error value, where the steering angle command is calculated as a function of vehicle lateral position, vehicle lateral speed, vehicle yaw rate and vehicle yaw angle. The steering angle command is then sent to the vehicle steering system.

DETAILED DESCRIPTION OF THE EMBODIMENTS

The following discussion of the embodiments of the invention directed to a system and method for providing steering angle control for lane centering or lane changing purposes in an autonomous or semi-autonomous vehicle is merely exemplary in nature, and is in no way intended to limit the invention or its applications or uses.

FIG. 1is an illustration of a vehicle system10including a lane change adaptive cruise control (LXACC) core algorithm12that includes lane centering and lane changing control, as will be discussed in detail below. Known LXACC systems provide a unified framework for automated lateral motion control, such as automated lane changing and lane centering functions. Because the lateral dynamics of a vehicle are related to the vehicle's longitudinal motion, the longitudinal motion control is usually considered in the same framework.

The system10includes a forward vision camera14and long range and short range radars16that detect road lane markings and neighboring objects around the vehicle and provide the object information in the format of range, range rate and azimuth angle. The lane marking information and the object information are provided to a sensor fusion sub-system18along with other sensor information from vehicle sensors20, such as vehicle speed, gyroscope, steering angle, etc. The sensor signals from the sensors20, can be combined with the roadway representations to enhance the roadway profile prediction and the vehicle motion state estimation of lateral speed, yaw rate, lateral offset and heading angle. The sensor fusion sub-system18provides filtering and post processing of the raw sensor data. The sensor fusion sub-system18also groups the detected objects, tracks them and reports the object location and relative speed in Cartesian coordinates. After the post-processing, the sensor fusion sub-system18provides signals that represent the roadway with vehicle lateral offset and heading angle with respect to the current lane the vehicle is traveling along.

The LXACC core algorithm12includes an object path prediction processor22and a roadway estimation processor24that receive the sensor information from the sensor fusion sub-system18, as will be discussed in further detail below. Additionally, the core algorithm12includes a vehicle state estimation processor26that receives the vehicle speed, yaw rate and steering angle information from the sensors20and provides predicted vehicle path and vehicle state information, as will also be described in further detail below. The core algorithm12also includes a threat assessment processor28that receives path prediction signals and roadway estimation signals from the processors22and24, and determines if neighboring vehicles may cause potential collision problems during lane changing maneuvers. The threat assessment processor28analyzes the lane and traffic information, and if a collision is predicted during the lane changing maneuver, the maneuver is aborted and yields the control to the lane centering.

The LXACC core algorithm12also includes a desired path generation processor30, also discussed below, that generates the desired path of the vehicle for lane centering and lane changing purposes, such as disclosed in the '439 application referenced above. The described path signal from the processor30is sent to a lane change lane center control processor32for providing a steering angle command based on the desired path generation from the processor30. The control processor32generates a sequence of future steering angle commands that minimize the orientation and offset errors between the vehicle's desired path and the predicted vehicle path.

The steering angle command signal is sent to a vehicle steering controller34that controls a vehicle steering system. The controller34controls brake and throttle at box38of the vehicle and an electric power steering (EPS) system40of the vehicle. The system10assumes that the vehicle steering sub-system34has an angle overlay or a torque overlay function so that the system10can control the steering angle. The system10decouples the longitudinal motion control from the lateral control so that the longitudinal control is not considered in the same framework as the lateral control. Instead, the system10sends a desired longitudinal speed to a smart adaptive cruise control sub-system36if the current vehicle speed needs to be adjusted to maintain a comfortable lane centering or lane changing maneuver. If high lateral acceleration is expected during the lane change maneuver, for example, on a sharp curve road, the system10first tries a lane change with longer lane change execution times. If the lateral acceleration still exceeds a certain limit with the lane change execution time, the system10reduces the vehicle speed.

The system10also includes a human machine interface (HMI) controller42that takes driver inputs of engagement and disengagement of the system10and delivers them to the LXACC core algorithm12. The LXACC core algorithm12sends the operating status, diagnosis messages and instruction messages to the HMI controller42and the HMI controller42processes the signals through various devices44, such as an LCD display, haptic seat, audio warnings, steering wheel vibrations, etc.

FIG. 2is a schematic block diagram of an LXACC system60that provides path control for a vehicle when changing lanes, either on a straight road or a curved road, and lane centering in an autonomous or semi-autonomous vehicle system, such as in the system10shown inFIG. 1. The system60includes a desired path generation processor62that receives a drivers request for a lane change. The desired path generation processor62generates a smooth path for the vehicle when turning that does not have abrupt changes that would otherwise provide passenger discomfort as discussed in detail in the '439 application. The system60produces a steering angle command δ to guide the vehicle along the generated path.

The desired path is represented as a series of lateral offsets, heading angles and longitudinal distances over a time period that the lane change will take place. This path information is provided to a comparator64that receives a signal from a path prediction processor66and provides an error signal between the desired path and the predicted path. The lateral speed vy, the yaw angle φ and the lateral position yrof the vehicle are predicted over the pre-defined lane change completion time.

FIG. 3is an illustration of a vehicle80traveling on a roadway82showing a desired path84for a lane change maneuver as generated by the path generation processor62and a predicted path86that is generated by the path prediction processor66.

The forward vision system calculates the roadway lane markings information, such as vehicle lateral offset yr, roadway curvature ρ and vehicle yaw angle φrwith respect to the vehicle's centered coordinate system at the box62. The roadway is modeled as a second order polynomial equation as:
yr(x)=Ax2+Bx+C,0≦x≦xrange(1)
Where xrangerepresents the range of the forward vision camera.

From the geometric relationship between the roadway and the roadway representation of equation (1), the coefficients of equation (1) with the measured roadway parameters yr, ρ and φrcan be related as:

After the roadway model of equation (1) is obtained, the roadway lateral position yrand the yaw angle φrcan be predicted at the box66over the pre-defined lane change completion time using a vehicle dynamic model:

Given the roadway lateral offset yr, the heading angle φrand roadway curvature ρ, the path-generation function at the box62generates a smooth desired path appropriate for a single lane changing maneuver. The path should be smooth and satisfy the vehicles dynamic capability. The desired path is obtained without an abrupt change or discontinuity in the sense of the second order geometric continuity. With these considerations, a fifth order polynomial equation for the desired path generation problem is provided as:
yd(t)=a5xd5(t)+a4xd4(t)+a3xd3(t)+a2xd2(t)+a1xd1(t)+a0(4)

The fifth order polynomial path generation captures the roadway parameters yr, ρ and φrat the beginning and the end of a lane change maneuvering and guarantees the smoothness of the path up to the second order path derivatives. In addition, the path generation can be obtained by a few simple algebraic computations using the road geometry measurement, thus it does not require heavy computing power.

The error signal from the comparator64is applied to a lane change controller68that provides a steering angle command signal δcmdfor the lane change that minimizes the error signal. The lane change controller68generates a sequence of future steering angle commands that minimize the orientation and offset errors between the vehicles desired path and the predicted vehicle path, as will be described in further detail below.

The lateral motion control algorithm compares the predicted vehicle path with the vehicles desired path (xd, yd), and calculates the steering angle command signal δcmdby minimizing the path difference, where the steering angle command signal δcmdis obtained by:

δcmd⁡(k)=∑i=0N-1⁢(zd⁡(k+i+1)-CAi+1⁢x⁡(k))T⁢Q⁡(k+i+1)⁢(CAi⁢B)∑i=0N-1⁢(CAi⁢B)⁢Q⁡(k+i+1)⁢(CAi⁢B)+R⁡(k)(5)
Where x=[yφvyr]T, zd(k)=[ydφd]T, Q and R are weighting matrices used in the minimization with the system matrices definitions

The steering angle command signal δcmdis then sent to a steering system70that provides the steering control of a vehicle72. The steering system70receives the steering angle command signal δcmdand controls the steering angle to achieve the desired steering angle as commanded.

As the vehicle turns, various sensors on the vehicle72, such as a steering angle sensor, speedometer and yaw rate sensor, provide measured signals of the motion of the vehicle72. These measured vehicle motion signals are sent back to the desired path generation processor62. The vehicle72also includes vision devices, such as cameras. Several inertial sensors, such as a speedometer, a rate gyro and a steering angle sensor, are used to measure vehicle states, such as longitudinal speed vx, vehicle yaw rate r and steering angle δ. No sensor is installed in the vehicle72for direct measurement of the lateral speed vy. The lateral speed estimation can be described as:

[v^y·r^·]=[-Cf+CrmvxbCr-aCfmvx-vxbCr-aCfIvx-a2⁢Cf+b2⁢CrIvx]·[v^yr^]+[CfmaCfI]·δ+K·[0r-r^](6)
Where r is a measured vehicle yaw rate, {circumflex over (v)}yand {circumflex over (f)} are the estimated lateral speed and the vehicle yaw rate, respectively, and K is a yaw rate observer gain.

The vehicle motion information is provided to a vehicle state estimation processor74that provides estimated vehicle state signals, namely, vehicle longitudinal speed vx, vehicle lateral speed vyand vehicle yaw rate r. The vehicle state estimation processor74uses a vehicle model to filter the estimated vehicle state signals. The state signals are sent to the path prediction processor66that predicts the vehicle path for the next few instances in time based on that information. The path prediction processor66estimates the vehicle future path based on the current vehicle speed vx, yaw rate r and steering angle δ.

The camera signals from the vision devices and the filtered sensor signals from the vehicle72are also provided to a lane mark detection processor76that corrects the parameters of the lane markings based on the motion of the vehicle72. The lane mark detection processor76recognizes the lane markings and represents them with the parameters of lane curvature, tangential angle and lateral offset, where the output of the lane mark detection processor76is the heading angle φrand the curvature ρ of the road. The position of the lane markings relative to the vehicle72is then sent to the desired path generation processor62through a roadway estimation processor78to provide the desired path generation updating. The path generation processor62generates a smooth desired path for a lane change according to the vehicle dynamics and the detected lane marks.