Cameras to determine vehicle heading

• US 7,774,113 B2
• Filed: 10/07/2004
• Issued: 08/10/2010
• Est. Priority Date: 04/10/2002
• Status: Active Grant

• Current Assignee: TRW Limited (Zeppelin - Stiftung der Stadt Friedrichshafen)
• Original Assignee: TRW Limited (Zeppelin - Stiftung der Stadt Friedrichshafen)
• Inventors: Oyaide, Andrew Oghenovo
• Primary Examiner(s): Tran; Khoi
• Assistant Examiner(s): Broadhead; Brian J
• Application Number:
  US10/960,886
• Publication Number:
  US 20050113995A1
• Time in Patent Office: 2,133 Days
• Field of Search: 701/36, 701/28, 701/41, 701/300, 348/118, 348/119, 382/104, 382/106, 382/107, 356/27, 356/150, 356/139.04
• US Class Current: 701/36
• CPC Class Codes: G01C 11/00 Photogrammetry or videogrammetry, e.g. stereogrammetry; Photographic surveying
• CPC Class Codes: G01C 21/26 specially adapted for navigation in a road network

Abstract

A method of calculating the heading angle of a vehicle using a camera associated with the vehicle, the method comprising capturing at least two images with the camera, calculating the lateral speed of the vehicle from the images, calculating the ground speed of the vehicle and calculating, from the ground speed and the lateral speed, a first heading angle of the vehicle. The method may also include calculating a second heading value from the images and from that a misalignment value indicative of the alignment of the camera from some datum. The misalignment value may be used to correct the first heading angle and the process repeated iteratively. The method may also include fitting features that follow features that follow the direction of travel of the vehicle. A camera system and data carrier embodying the method are also disclosed.

Claims

1. A method of calculating a heading angle of a vehicle using a camera associated with said vehicle, said method comprising capturing at least two images with said camera, calculating a lateral speed of said vehicle from said two images, calculating a ground speed of said vehicle independently of said two images and calculating, from said ground speed and said lateral speed, a first heading angle of said vehicle and, further comprising a step of calculating a second heading angle from said images.

2. The method of claim 1 wherein said second heading angle is compared to said first heading angle to produce a misalignment value indicative of an angular offset of said camera from a datum.

3. The method of claim 1 wherein said second heading angle is calculated without reference to said ground speed.

4. The method of claim 3 wherein said second heading angle is calculated from features of said two images only.

5. The method of claim 1 wherein said first heading angle is calculated by calculating an angle between a direction of said lateral velocity and a direction in which said vehicle is moving, and taking the angle calculated between the direction of said lateral velocity and the direction in which said vehicle is moving away from 90°

to produce said first heading angle.

6. The method of claim 5 wherein said first heading angle is calculated by decomposing said ground velocity into two perpendicular components to form a right-angled triangle, one of said components being a lateral velocity, said first heading angle being the angle in said triangle opposite said lateral velocity.

7. A camera system adapted to correct for errors in alignment based upon a calculated heading, said camera system being adapted to be fitted to a vehicle, said camera system comprising at least one camera fitted to the vehicle, said camera system using the method of claim 1 to calculate the heading.

8. A camera system according to claim 7 wherein said system is configured to calculate a misalignment value, wherein said system includes a fault indicator, which is adapted to alert a user should said misalignment value of said camera exceed certain bounds or change at a rate that exceeds certain bounds.

9. A camera system according to claim 7, further including a controller means adapted to capture multiple images from the camera, calculate from said images a position of features on a road, and calculate heading angles from positions of said features and the speed at which said vehicle is travelling

10. A camera system according to claim 7 wherein a speed of said vehicle is sensed by means of a speed sensor associated with a controller.

11. A camera system according to claim 7 wherein said system is configured to calculate misalignment values, wherein said control means is also provided with a memory adapted to store said misalignment values.

12. The method of claim 3 further comprising a step of performing an inverse perspective transform on said image viewed by said camera before features are fitted to curves, said transform being given by:

$x=\frac{\mathrm{hX}}{H-Y}<br><br><br><br>\mathrm{and}<br><br><br><br>z=\frac{\mathrm{fh}}{H-Y}$

where X and Y are image co-ordinates referenced from a centre of a line of said captured image, H is indicative of a position of a horizon, f is a focal length of said camera, h is a height of said camera above ground, z is a distance from said camera in a direction in which said camera is pointing and x is a distance from said camera in a perpendicular direction to the direction in which said camera is pointing.

18. A method of improving an accuracy of a camera system associated with a vehicle, said method comprising calculating a misalignment value according to claim 2 and using this value to correct said accuracy of said system.

19. The method of claim 18 further comprising repeatedly recalculating said misalignment value, with said first heading angle being first corrected by the amount indicated by said misalignment value with each successive recalculation.

20. The method of claim 19 wherein said misalignment value calculated by each successive iteration is filtered by a filter such that a rate of change of said misalignment value is constrained to be below a maximum value.

21. The method of claim 18 further comprising a step of alerting a user should said misalignment value exceed certain bounds.

22. The method of claim 18 further comprising a step of alerting a user should said misalignment value change at a rate greater than a predefined limit.

35. A data storage device carrying software which, when running on a processor, causes the processor to perform the method of claim 1.

13. A method of calculating a heading angle of a vehicle using a camera associated with said vehicle, said method comprising capturing at least two images with said camera, calculating a lateral speed of said vehicle from said two images, calculating a ground speed of said vehicle independently of said two images and calculating, from said ground speed and said lateral speed, a first heading angle of said vehicle and, further including the step of analyzing said two images in order to calculate heading angles by finding features that follow a direction of travel of said vehicle.

14. The method of claim 13 wherein said lateral velocity is calculated by measuring a rate at which said features move perpendicular to a direction of view of said camera.

15. The method of claim 13 wherein said features are fitted to a curve having a form:

x=c₁+c₂z+c₃z²

where z is the distance from said camera in a plane substantially parallel to a road in a direction in which said camera is pointing, x is the distance from said camera in the plane substantially parallel to a road in a perpendicular direction to the direction in which said camera is pointing, and c₁, c₂ and c₃ are coefficients to be determined.

16. The method of claim 15 wherein said lateral velocity is calculated as a rate of change of c₁.

17. The method of claim 15 wherein said second heading angle is calculated as c₂.

23. A method of estimating an error in a measured heading angle of a vehicle travelling along a road, said road having features aligned along a length of said road, comprising the steps of:

a) Periodically capturing images of said road with a camera;

b) Analysing each image to determine a position of said features;

c) Fitting said features to a curve;

d) Calculating a lateral velocity of said curve relative to said vehicle;

e) Dividing said lateral velocity by a ground speed of said vehicle to produce a ratio;

f) Taking an inverse cosine of said ratio to produce an angle;

g) Taking said angle away from 90°

to give a first heading angle;

h) Calculating a second heading angle from a curve to which said features have been fitted; and

i) Calculating a difference between said first heading angle and said second heading angle to give a misalignment value indicative of a misalignment between said camera and a datum.

24. The method of claim 23 wherein said step of analysing said features includes performing an inverse perspective transform.

25. The method of claim 24 wherein said inverse perspective transform is of a form:

$x=\frac{\mathrm{hX}}{H-Y}<br><br><br><br>\mathrm{and}<br><br><br><br>z=\frac{\mathrm{fh}}{H-Y}$

where X and Y are the image co-ordinates referenced from a centre of a line of said captured image, H is indicative of a position of a horizon, f is a focal length of said camera, h is a height of said camera above ground, z is a distance from said camera in a direction in which said camera is pointing and x is a distance from said camera in a perpendicular direction to the direction in which said camera is pointing.

26. The method of claim 23 wherein said curve to which said features are fitted in step (c) is a quadratic equation of a form x=c₁+c₂z+c₃z² where z is a distance from said camera in a plane substantially parallel to a road in a direction in which said camera is pointing, x is a distance from said camera in a plane substantially parallel to a road in a perpendicular direction to the direction in which said camera is pointing, and c₁, c₂ and c₃ are coefficients to be determined.

27. The method of claim 26 wherein said goodness of fit to this curve is used to give a level of confidence of said fit.

28. The method of claim 26 wherein said lateral velocity is calculated as a rate of change of c₁.

29. The method of claim 26 wherein said second heading angle is calculated as c₂ in radians.

30. The method of claim 23, further comprising:

j) Repeating said method and correcting said image viewed by said camera by an amount indicated by said misalignment value.

31. The method of claim 30 wherein step (j) is repeated iteratively.

32. The method of claim 30 wherein step (j) is only carried out when a ground speed of said vehicle is above a certain limit.

33. The method of claim 30 wherein step (j) is only carried out when a level of confidence of fit of said features to said curve is above a certain level.

34. The method of claim 33 wherein said misalignment value is used as an angle through which said camera should be physically moved in order to improve its alignment.