Method of recording trajectory data and sensor data for a manually-driven vehicle

• US 6,314,341 B1
• Filed: 11/26/1999
• Issued: 11/06/2001
• Est. Priority Date: 11/26/1999
• Status: Expired due to Term

• Current Assignee: Yutaka John Kanayama
• Original Assignee: Yutaka John Kanayama
• Inventors: Kanayama, Yutaka John
• Primary Examiner(s): Cuchlinski, Jr., William A.
• Assistant Examiner(s): MANCHO, RONNIE M
• Application Number:
  US09/449,797
• Time in Patent Office: 711 Days
• Field of Search: 700/245, 700/251-254, 700/258, 700/262, 700/275, 701/1, 701/2, 701/5, 701/4, 318/587, 318/139, 318/489, 180/168, 180/169, 180/167
• US Class Current: 701/1
• CPC Class Codes: G05D 1/0221 involving a learning process
• CPC Class Codes: G05D 1/0255 using acoustic signals, e.g. ultra-sonic singals sonar systems designed for anti-collision purposes G01S15/93
• CPC Class Codes: G05D 1/0274 using mapping information stored in a memory device navigation using map-matching G01C21/30

Abstract

A method of recording trajectory data and sensor data for a vehicle while it is manually driven. The trajectory data comprises of the trajectory length and vehicle frame (position and orientation) that are calculated at each motion execution interval. The sensor data comprises of the trajectory length, vehicle frame, sensor frame, and sensor data that are obtained at each sensor execution interval. In particular, if the sensor is a range-finder, an estimated frame of the object that the range finder detects will be added to said sensor data. One objective of this invention is to play back the same motion the vehicle was taught by a human operator. Another objective is to generate a world map for the vehicle. Still further objective is, in the play-back mode, to correct vehicle'"'"'s positional errors using the sensor data.

Claims

1. A method of recording trajectory data for a manually-driven vehicle that moves on a plane, comprising:

providing means for periodical motion function execution with a motion-execution interval Δ

T;

defining a global frame on said plane to describe a vehicle position p=(x, y) and a vehicle orientation θ

of said manually-driven vehicle;

denoting a pair (p, θ

)=((x, y), θ

) of said vehicle position and said vehicle orientation as a vehicle frame Q;

denoting s as a trajectory length measured along a trajectory from a start position of said manually-driven vehicle;

providing means for detecting an incremental trajectory length Δ

s during each motion-execution-interval Δ

T;

providing means for detecting a vehicle orientation change Δ

θ

during each motion-execution interval Δ

T;

initializing said trajectory length s and said vehicle frame Q at an initial state; and

executing a sequence of motion functions at every motion-execution interval Δ

T, comprising;

detecting said incremental trajectory length Δ

s;

detecting said vehicle orientation change Δ

θ

,

computing an incremental transformation Δ

Q using said incremental trajectory length Δ

s and said vehicle orientation change Δ

θ

,

updating said vehicle frame Q by composing said incremental transformation Δ

Q to said vehicle frame Q;

updating said trajectory length s by adding Δ

s;

assembling a trajectory record A which comprises of said trajectory length s and said vehicle frame Q;

providing a memory which is able to store said trajectory record A; and

storing said trajectory record A to said memory.

2. The method of claim 1, further comprising

mounting a sensor on said manually-driven vehicle at a three-dimensional local sensor frame R in said vehicle frame;

providing means for periodical sensor-function execution with a sensor-execution interval Δ

T′

; and

executing a sequence of sensor functions at every sensor-execution interval Δ

T′

, comprising;

converting said vehicle frame Q=((x, y), θ

) into a three-dimensional vehicle frame Q′

=((x, y,

     0), (θ

, 0 ,

     0));

computing a global sensor frame S in said global frame by composing said three-dimensional vehicle frame Q′ and

said local sensor frame R;

collecting a sensor data U from said sensor;

assembling a sensor record B which comprises of said trajectory length s, said vehicle frame Q, said global sensor frame S, and said sensor data U;

providing a memory which is able to store said sensor record B; and

storing said sensor record B to said memory.

3. The method of claim 1, further comprising

mounting a range-finder on said manually-driven vehicle at a three-dimensional local range-finder frame R in said vehicle frame;

providing means for periodical range-finder function execution with a range-finder-execution interval of Δ

T ″

; and

executing a sequence of range-finder functions at every range-finder-execution interval Δ

T″

;

comprising;

converting said vehicle frame Q=((x, y), θ

) into a three-dimensional vehicle frame Q′

=((x, y,

     0), (θ

, 0,

     0));

computing a global range-finder frame S in said global frame by composing said three-dimensional vehicle frame Q′ and

said local range-finder frame R;

collecting a range d to an object from said range finder;

computing an object frame V by composing said global range-finder frame S and a transformation ((d, 0,

     0), (0, 0,

     0));

assembling a range finder record B which comprises of said trajectory length s, said vehicle frame Q, said global range-finder frame S, said range d, and said object frame V;

providing a memory which is able to store said range-finder record B; and

storing said range-finder record B to said memory.