Method and apparatus for on-vehicle calibration and orientation of object-tracking systems

• US 7,991,550 B2
• Filed: 05/19/2008
• Issued: 08/02/2011
• Est. Priority Date: 02/03/2006
• Status: Active Grant

• Current Assignee: GM Global Technology Operations LLC (General Motors Company)
• Original Assignee: GM Global Technology Operations LLC (General Motors Company)
• Inventors: Zeng, Shuqing
• Primary Examiner(s): Nguyen; Cuong H
• Application Number:
  US12/123,332
• Publication Number:
  US 20080300787A1
• Time in Patent Office: 1,170 Days
• Field of Search: 701/301, 701/117, 701/201, 342/59, 342/70, 342/95, 342/174, 342/195, 342450-451, 340435-436, 340/703
• US Class Current: 701/301
• CPC Class Codes: G01S 13/726 Multiple target tracking
• CPC Class Codes: G01S 13/867 Combination of radar systems with cameras
• CPC Class Codes: G01S 13/931 of land vehicles
• CPC Class Codes: G01S 2013/932 using own vehicle data, e.g. ground speed, steering wheel direction
• CPC Class Codes: G01S 2013/9321 Velocity regulation, e.g. cruise control
• CPC Class Codes: G01S 2013/93271 in the front of the vehicles
• CPC Class Codes: G01S 7/4026 Antenna boresight
• CPC Class Codes: G01S 7/403 in azimuth, i.e. in the horizontal plane
• CPC Class Codes: G01S 7/4091 during normal radar operation
• CPC Class Codes: G01S 7/4972 Alignment of sensor
• CPC Class Codes: G01S 7/52004 Means for monitoring or calibrating short-range imaging G01S7/5205

Abstract

A method for simultaneously tracking a plurality of objects and registering a plurality of object-locating sensors mounted on a vehicle relative to the vehicle is based upon collected sensor data, historical sensor registration data, historical object trajectories, and a weighted algorithm based upon geometric proximity to the vehicle and sensor data variance.

Claims

1. Method for simultaneously tracking a plurality of objects and registering a plurality of object-locating sensors mounted on a vehicle relative to the vehicle, comprising:

monitoring sensor data from the plurality of object-locating sensors;

monitoring a plurality of historical target tracks for the plurality of objects;

monitoring historical sensor registration data;

simultaneously tracking the plurality of objects and registering the plurality of object-locating sensors based upon the sensor data, the historical sensor registration data, the plurality of historical target tracks, location and orientation of the sensors to the vehicle and a sensor data covariance matrix;

updating the plurality of historical target tracks for each of the plurality of objects based upon the tracking of the plurality of objects; and

updating the historical sensor registration data based upon the registering of the plurality of object-locating sensors.

2. The method of claim 1, further comprising:

determining the plurality of historical target tracks for the plurality of objects though data associations and a time propagations.

3. The method of claim 2, wherein the data associations comprise:

matching sensor data from the plurality of object-locating sensors with the plurality of historical target tracks.

4. The method of claim 2, wherein the time propagations comprise:

predicting the tracking of the plurality of objects in a subsequent time cycle based upon the plurality of historical target tracks, a vehicle speed, a vehicle acceleration, and a vehicle yaw rate.

5. The method of claim 2, wherein the time propagations comprise:

predicting the registering of the plurality of object-locating sensors in a subsequent time cycle based upon the plurality of historical target tracks, a vehicle speed, a vehicle acceleration, and a vehicle yaw rate.

6. The method of claim 1, wherein the historical sensor registration data comprises:

a location error, an azimuth registration error, and a range offset.

7. The method of claim 1, further comprising:

aligning the plurality of object-locating sensors relative to a vehicle coordinate system.

8. The method of claim 1, further comprising:

correcting the registering of the plurality of object-locating sensors based upon uncertainty measures and the historical sensor registration data; and

updating the historical sensor registration data and updating the plurality of historical target tracks based upon the corrected registration.

9. The method of claim 1, wherein simultaneously tracking the plurality of objects and registering the plurality of object-locating sensors comprises:

10. The method of claim 1, further comprising:

monitoring lane geometry;

determining if the sensor registration data has changed over a threshold based upon lane geometry; and

resetting the historical sensor registration data when the sensor registration data has changed over said threshold.

11. The method of claim 1, further comprising:

generating a recursive error function; and

recursively minimizing the recursive error function.

12. The method of claim 11, wherein the recursive error function comprises:

error accumulated from an initial start time (t0) to and including the previous time frame (t−

1);

wherein the recursive error function is based upon;

an uncertainty measure of a previous estimation of target tracks and the sensor registration data, and a priori statistical distribution of target tracks and the sensor registration data;

a magnitude of the discrepancy between monitored sensor data measurements and a measuring model, wherein the measuring model is a function of an unknown target track and an unknown sensor registration data; and

an uncertainty measure of noise parameters in the system dynamics model, wherein the system dynamics model maps the previous estimation of target tracks and the sensor registration data to a current target track and a current sensor registration data.

13. The method of claim 11, wherein the recursive error function is given by:

J_t=e(t−

1)+∥

{tilde over (R)}_ts(t)−

{tilde over (z)}_t∥

²+∥

h(s(t))−

o(t)∥

²+∥

R_w(t)−

z_w(t)∥

²

where

e(t−

1) denotes a sum of an estimation error up to and including a time t−

1,

[{tilde over (R)}_t,{tilde over (z)}_t] is an array and denotes an a priori statistical distribution of a joint vector s including target track and the sensor registration data,

h denotes a measurement function, and

[R_w(t),z_w(t)] is an array and denotes a statistical distribution of the noise parameters in a system dynamics model.

14. The method of claim 13, wherein the measurement function h comprises:

mapping from an unknown target track to a target position in a sensor coordinate system based on sensor registration.

15. The method of claim 14, wherein the recursive error function comprises:

16. The method of claim 1, further comprising:

monitoring a sensor measurement matrix;

fusing sensor data with a dependency map;

identifying sparseness in covariance matrices including previous estimations of target tracks and sensor registration data and the sensor measurement matrix based on the dependency map;

using Givens rotations to eliminate non-zero triangular elements of the measurement matrix; and

simultaneously tracking the plurality of objects and registering the plurality of object-locating sensors based upon the sensor measurement matrix.

17. The method of claim 16, further comprising:

merging the sensor data and an a priori statistical distribution of a previous estimation of target tracks and sensor registration data via a measurement factorization.

18. The method of claim 17, wherein the measurement factorization comprises:

generating a sparse matrix; and

applying a factorization operator {circumflex over (T)} to the matrix.

19. The method of claim 18, wherein the sparse matrix comprises:

an array based upon the a priori statistical distribution; and

wherein the sparse matrix is based upon a measuring model, sensor data and lane geometry including curvature and orientation of the vehicle with respect to the lane geometry.

20. The method of claim 17, further comprising:

using Givens transformations for time propagation factorization; and

generating a sparse matrix.

21. The method of claim 20, wherein the sparse matrix comprises:

a posterior distribution based upon a measurement update;

a coefficient of a system dynamics model after applying a linearization operation; and

the noise parameters of the system dynamics model.

22. The method of claim 21, wherein the sparse matrix is factorized by an orthogonal transformation {tilde over (T)}:

23. The method of claim 22, wherein the linearized system dynamics equation is given by:

x(t+1)=Φ

_xx(t)+G_xw(t)+u

where

u=f(x*,w*)−

Φ

_xx*−

G_xw*;

x* and w* denote a neighborhood to apply linearization operation,

function f is a system dynamics equation, and

w denotes a noise term in the system dynamics model.

24. The method of claim 23, further comprising:

applying Givens rotations for each row from the left to the right;

applying Givens rotations for each column from the bottom to the top; and

combining each non-zero low-triangular element in an i-th row of a matrix block C_x with an element in a same column in the i-th row of the matrix block {tilde over (R)}_x.

25. The method of claim 24, further comprising:

applying Givens rotations only when both elements involved in the Givens rotations are nonzero.

26. The method of claim 20, wherein the sparse matrix comprises:

an array based upon the a priori distribution wherein the sparse matrix is based upon a measuring model, the sensor data, and lane geometry including curvature and orientation of the vehicle with respect to the lane geometry.

27. The method of claim 26, further comprising:

applying a factorization operator {circumflex over (T)} to yield an upper triangular matrix.

28. Method for simultaneously tracking a plurality of objects and registering a plurality of object-locating sensors mounted on a vehicle relative to the vehicle, comprising:

monitoring sensor data from the plurality of object-locating sensors;

monitoring a plurality of historical target tracks for the plurality of objects;

monitoring historical sensor registration data;

monitoring a sensor measurement matrix;

monitoring lane geometry including curvature and orientation of the vehicle with respect to the lane geometry;

simultaneously tracking the plurality of objects and registering the plurality of object-locating sensors based upon the sensor data, the historical sensor registration data, the plurality of historical target tracks, the sensor measurement matrix, the lane geometry, location and orientation of the sensors to the vehicle and sensor data variance;

updating the plurality of historical target tracks for each of the plurality of objects based upon the tracking of the plurality of objects; and

updating the historical sensor registration data based upon the registering of the plurality of object-locating sensors.

29. The method of claim 28, further comprising:

generating a sparse matrix.

30. The method of claim 29, wherein the sparse matrix comprises:

an a priori statistical distribution in an information array form; and

wherein the sparse matrix is based upon a measuring model and the sensor data; and

lane geometry information including curvature and orientation of the vehicle with respect to the lane geometry.

31. The method of claim 28, further comprising:

determining if the sensor registration data changes based upon lane geometry; and

resetting the historical sensor registration data when the sensor registration data has changed in excess of a predetermined amount.

32. The method of claim 31, further comprising:

monitoring a target in a common lane with the vehicle;

monitoring a lateral offset of the target with respect to the common lane; and

resetting the historical sensor registration data when the lateral offset exceeds a predetermined threshold.

33. The method of claim 31, further comprising:

resetting a priori information regarding the historical sensor registration data from a last epoch to a non-information a priori.

34. The method of claim 33, wherein the a priori information from the last epoch comprises [z_a(t−

1) R_a(t−

1)], wherein resetting a priori information comprises setting z_a(t−

1)=0 and R_a(t−

1)=ε

I wherein ε

denotes a sufficient small floating number, when the sensor registration data changes a predetermined amount.