Systems and methods for detecting and compensating camera yaw angle offset for autonomous vehicles

A method for controlling a vehicle using a forward-facing camera, comprising:

• determining a look ahead point of the forward-facing camera and a distance to the look ahead point during operation of the vehicle;
• determining a location of a lane center line of a lane of a road occupied by the vehicle during operation of the vehicle;
• determining a location of a vehicle center line;
• determining a first lateral offset between the determined location of the lane center line and the look ahead point at a first longitudinal distance from the vehicle;
• determining a second lateral offset between the vehicle and the determined location of the center lane at a second longitudinal distance from the vehicle, wherein the first and second longitudinal distances are different, the first longitudinal distance is a distance in front of the vehicle based on the look ahead point, and the second longitudinal distance corresponds to a location of the vehicle;
• receiving N samples of the first lateral offset, N samples of the second lateral offset, and the distance to the look ahead point, where N is an integer greater than one;
• calculating a yaw angle offset of the forward-facing camera using both the N samples of the first lateral offset and the N samples of the second lateral offset and the distance to the look ahead point, wherein the yaw offset angle of the forward-facing camera corresponds to alignment of the forward-facing camera relative to the vehicle,
• the calculation of the yaw angle offset being performed using the equation:
• Θyaw=arctan [(Σ(YLH+YC0)/XLH)/N],
• where Θyaw is the yaw angle offset, YLH are the samples of the first lateral offset, Yc0 are the samples of the second lateral offset, XLH is the look ahead distance, and N is the integer greater than one;
• adjusting the yaw angle of the forward-facing camera in response to the calculation of the yaw angle offset of the forward-facing camera;
• and
• based on the yaw angle offset, controlling at least one of acceleration of the vehicle, application of brakes of the vehicle of the vehicle, and steering of the vehicle.

A control system for a vehicle using a forward-facing camera, comprising:

• a look ahead circuit configured to determine a look ahead point of the forward-facing camera and a distance to the look ahead point during operation of the vehicle;
• a lane center circuit configured to determine a location of a lane center line of a lane of a road occupied by the vehicle during operation of the vehicle;
• a vehicle center line circuit configured to determine a location of a vehicle center line;
• a first lateral offset circuit configured to determine a first lateral offset corresponding to a lateral distance between the determined location of the lane center line and the look ahead point at a first longitudinal distance from the vehicle equal to the look ahead point;
• a second lateral offset circuit configured to determine a second lateral offset corresponding to a lateral distance between the determined location of the lane center line and the vehicle center line at a second longitudinal distance from the vehicle of zero, wherein the first and second longitudinal distances are different and the second longitudinal distance corresponds to a location of the vehicle;
• a yaw angle offset calculating circuit configured to:
• receive N samples of the first lateral offset, N samples of the second lateral offset, and the distance to the look ahead point, where N is an integer greater than one;
• calculate a yaw angle offset of the forward-facing camera using both the N samples of the first lateral offset and the N samples of the second lateral offset and the distance to the look ahead point, wherein the yaw offset angle of the forward-facing camera corresponds to alignment of the forward-facing camera relative to the vehicle,
• the calculation of the yaw angle offset being performed using the equation:
• Θyaw=arctan [(Σ(YLH+YC0)/XLH)/N],
• where Θyaw is the yaw angle offset, YLH are the samples of the first lateral offset, Yc0 are the samples of the second lateral offset, XLH is the look ahead distance, and N is the integer greater than one;
• and
• compensate a yaw angle error of the forward-facing camera in response to the calculation of the yaw angle offset of the forward-facing camera;
• an enabling circuit configured to selectively enable the calculation of the yaw angle offset in response vehicle speed, road curvature ahead of the vehicle and steering wheel angle changes;
• and
• an autonomous driving circuit configured to, based on the yaw angle offset, control at least one of acceleration of the vehicle, application of brakes of the vehicle of the vehicle, and steering of the vehicle.