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

[0002] This invention pertains to a vehicle, specifically to computing and storing its trajectory data and sensor data for a manually-driven vehicle. This invention falls into the Classification Definitions of 701/002 and 701/025.

[0003] Wheeled unmanned vehicles are widely adopted in manufacturing, services, and other industries. For instance, in manufacturing factories, they are used for unmanned material handling tasks. In semiconductor clean rooms, unmanned vehicles with articulated manipulators are widely used for silicon cassette handling to eliminate human operators who may produce unwelcome dust. Some hospitals use meal-delivering vehicles for cost-saving measures.

[0004] Heretofore, two methods have been known to navigate unmanned vehicles: an electrical/optical method and a programming method. The electrical/optical navigation method uses inductive wires or optical tapes pasted on the floor that are tracked by a vehicle. Another method is navigating a vehicle by computer programming.

[0005] U.S. Pat. No. 4,816,998 described a method for a self-pivoting vehicle of computing a steering angle signal by dead reckoning from a predetermined desired trajectory for the vehicle.

[0006] U.S. Pat. No. 5,109,340 describes a method for an autonomous vehicle of planning smooth paths by using Clothoid curves that has the feature of curvature continuity.

[0007] U.S. Pat. No. 5,170,352 describes a method for an autonomous vehicle of detecting obstacles and detecting the target areas of which the vehicle to be directed. The vehicle is supposed to operate within a predetermined physical area such as on the deck of an aircraft carrier.

[0008] U.S. Pat. No. 5,219,036 describes a method for an unmanned industrial trucks of guiding themselves using reference points embedded in the floor and a path-tracking algorithm.

[0009] U.S. Pat. No. 5,233,526 describes a method for a driverless vehicle of relocking on a theoretical trajectory during the passage of the vehicle by modifying its curvature. This method guarantees that the curve has the curvature-continuity.

[0010] U.S. Pat. No. 5,621,291 describes a method for a self-propelled robotic vacuum cleaner of controlling its motion by ultrasonic sensors.

[0011] U.S. Pat. No. 6,134,486 describes a method for a vehicle of tracking a path consisting of directed lines and circles.

[0012] U.S. Pat. No. 6,138,064 describes a method for a vehicle of branching from a main line and of merging into the main line again.

[0013] These just described navigation methods, however, suffer from a number of disadvantages: (A) As to the electrical/optical method, it is time consuming and costly to implement the wires or optical tapes in the working environment. The cost increases proportionally with its total travelling distance. Furthermore, it is also time consuming and costly to change a once implemented wire/tape because of environmental or mission modifications. In clean room applications, using wires/tapes themselves is not desirable because they may become a source of dust. (B) The programming method has problems also. Measurement of a path in the vehicle's operating environment is needed to program the vehicle. This is a time-consuming task. In some situations, an ideal path for the task cannot be defined easily by programming.

[0014] There is another problem where an unmanned vehicle is used for practical applications. A geometrical map of the vehicle environment is necessary for the vehicle's precise motion. Heretofore, there is no economical and simple method known to do this task.

[0015] The prior art made of record cited in the previous section describe methods of controlling an autonomous vehicle by computer programs. However, this invention states a manual-controlling method of a non-autonomous vehicle.

[0016] One objective of the invention is to provide a method of recording trajectory data for a manually-driven vehicle. More specifically, the trajectory data contains the trajectory length traveled by the vehicle, vehicle frame (position and orientation), and trajectory curvature computed at each motion execution interval. This is a method of manually teaching trajectories to vehicles. Another objective of the invention is, in case a sensor is mounted on the vehicle, to provide a method of storing trajectory sensor data taken along the vehicle's manual motion at each sensor execution interval. The sensor data contains the trajectory length, vehicle frame, sensor frame, and information collected by the sensor.

[0017] An advantage of the invention is that the trajectory data contains the necessary and sufficient geometrical information (the length, position, orientation, curvature of each section of the trajectory) needed to reproduce the manually generated trajectory. The curvature is essential information to obtain better control of vehicles. Approximating each trajectory section by a circular arc or by a straight line is the best known method for vehicle odometry.

[0018] Another advantage is that any trajectory that is hard to define mathematically can be generated by the method since the vehicle trajectory is generated manually. Thus the invention solves one of the problems stated earlier that an ideal path for some task is difficult to define by a computer program.

[0019] Still further advantage of the invention is that it provides a timesaving and economical method of defining and modifying trajectories to a vehicle. For a vehicle equipped with a program implementing the invention, teaching a new trajectory can be done in a single manual driving session. Thus the invention solves another problem stated earlier that implementing and changing inductive wires or optical tapes is time consuming and costly.

[0020] Still further advantage of the invention is that it provides only spatial information of the vehicle trajectory without any timing information. This method allows more general playing back of the recorded motion, because a user can select any speed profile for a play-back motion independent of the speed profile at the teaching mode.

[0021] Still further advantage of the invention is to provide a hardware-independent method of handling the trajectory information. The invention can be applied to different vehicles with an arbitrary size and with an arbitrary wheel architecture such as differential-drive, tricycle, or car-like.

[0022] Still further advantage of this invention is to provide a straightforward method of recording on-board sensor information about the external world while the vehicle is manually-driven. This information helps the vehicle later when it reproduces the same motion for positional identification (or localization) and other purposes.

[0023] Still further advantage of this invention is to provide a straightforward method of creating a map of the world by a vehicle using an appropriate range sensor, such as a sonar. The range sensor data is an envelope of objects in the world that is scanned by the sensor. The envelope can be edited manually or by a computer program to generate a geometrical map. This method is far easier than the method of physically measuring the world by hand to generate a map. If the vehicle is operated by a person who has the knowledge of the physical characteristics of the sensor, he/she can drive the vehicle to maximize the sensor information.

[0024] Still further advantage of this invention is that the world map taken by a sensor during the teaching mode and the sensor data obtained in the play-back mode are consistent, since the sensor used in both modes are the same used at similar positions and orientations. Therefore, a localization task (or vehicle's positional error resetting) becomes easier and meaningful. If the world map had been manually made without the sensor, inherent differences between the hand-made map and the sensor data would degrade the localization task.

[0025] In this invention, an odometry method of computing the trajectory information (the trajectory length, coordinates, orientation, and curvature) for recording is stated. The method apparently is parallel to the one of computing the trajectory information of a computer-controlled vehicle as a part of the position-feedback control algorithm. However, it should be noted that in this invention the trajectory information is given by manually-controlled motions intending to be used later for the reproduction of the same motion.

[0026] FIG. 1 shows a vehicle frame (position and orientation) Q=(p,θ)=((x, y), θ) and the reference frame, ((0, 0), 0), of a manually-driven vehicle in the global frame.

[0027] FIG. 2 shows a sample trajectory of a manually-driven vehicle, which is composed by short curves separated by vehicle frames.

[0028] FIG. 3 is a block diagram of a manually-driven vehicle system that comprises of the main vehicle subsystem and the motion control subsystem.

[0029] FIG. 4 shows an incremental transformation of a manually-driven vehicle characterized by a circular arc generated during a motion-execution interval.

[0030] FIG. 5 is a figure made from FIG. 4 by moving the first frame Q_iinto the reference frame keeping the geometrical relation between the two frames unchanged. This figure also shows that the incremental transformation of a manually-driven vehicle is computed by the incremental trajectory length and vehicle orientation change.

[0031] FIG. 6 shows that, in a manually-driven differential-drive vehicle, the incremental trajectory length and vehicle orientation change can be computed from the incremental moving distances at both wheels.

[0032] FIG. 7 shows that an on-board sensor is mounted on the manually-driven vehicle, where the vehicle frame is Q and the sensor frame local to the vehicle frame is R.

[0033] FIG. 8 shows that an on-board range finder is mounted on the manually-driven vehicle, where the vehicle frame is Q, the range-finder frame local to the vehicle frame is R, and the range to a detected object is d.

[0034] FIG. 9 is an example of a set of object points in a sonar data V. In this case, the sonar is mounted horizontally (φ=0) on the manually-driven vehicle, the object data points are on a single plane.

[0035] Therefore, the invention is related to the two functions of a manually-driven vehicle: The first function is to record a trajectory data. The second function is to record a sensor data. There are no other inventions that have similar objectives. Therefore, the invention is not an improvement of any other invention.

[0036] [1] Two Operating Modes of a Vehicle

[0037] In the invention, the vehicle has two distinct operating modes: a teaching mode and a play-back mode. In the teaching mode, its motion is driven or directed by a human, and its trajectory and sensing data are recorded by a computer program. In the play-back mode, its motion is controlled by a computer program to reproduce the motion that was recorded in the teaching mode.

[0038] [2] Manual Driving Methods

[0039] In the teaching mode, several different methods for manual driving are possible. They include:

[0040] (A) A method where a human operator physically moves the vehicle by hand(s). The motors of the vehicle must be set free so that the operator can move the vehicle with little resistance.

[0041] (B) A method where a sensor system mounted on a vehicle actively detects an object held by a human operator. For instance, a sonar system is mounted on a vehicle and detect an object that is held by a human operator. The vehicle is programmed to move so that the distance between the vehicle's front end and the object is in a predefined range and the object is in the heading orientation of the vehicle.

[0042] (C) A method where the vehicle is remotely-controlled by a human operator.

[0043] (D) The vehicle may be driven by a human operator who is riding on it.

[0044] This invention is applicable to any other methods as long as the vehicle motion is controlled or directed by a human operator in any way, because the invention is related to only the method how the resultant motion is computed and recorded.

[0045] [3] Motion-Data Computation

[0046] To clearly define the position and orientation of a vehicle, we paste a local Cartesian frame on it (FIG. 1). In this specific embodiment, we assume the vehicle has one or two fixed wheel(s) where a fixed wheel is a wheel that cannot be steered. We also assume that if there are two fixed wheels, they are aligned on the same axle. There are at least three wheel architectures—a differential drive, a bicycle, and a car-like—that possess this property. The origin of the local frame is put on the axle of the fixed wheel(s) and its Y-axis is aligned to the axle. Then the local X-axis direction which is equal to the vehicle's heading orientation is perpendicular to the wheel axle.

[0047] A global frame is defined on the plane where the vehicle moves around. The local frame on the vehicle moves along the vehicle's motion. As shown in FIG. 1, the vehicle frame Q in the global frame is defined as Q=(p, θ)=((x, y), θ) through the local frame. Here p=(x, y) is the position of the local frame origin and θ the orientation of the local X-axis both measured in the global frame. Specifically, Q_ref=((0, 0), 0) is called the reference frame.

[0048] The vehicle is manually driven starting from an initial vehicle frame Q₀=(p₀, θ₀) at time t₀. The initial frame is known to the vehicle. The vehicle frame Q=(p, θ) moves continuously with the vehicle, and the origin p of the vehicle frame generates a motion trajectory as shown in FIG. 2. This motion trajectory is discretely represented as a sequence (Q₀, Q₁, . . . , Q_i, Q_i+1, . . . , Q_n) of vehicle frames at motion execution times, t₀, t₁, . . . , t_i, t_i+1, . . . , t_n. Each vehicle frame Q_i=(p_i, θ_i) at time t_iis shown by a tick in FIG. 2 (i=0, 1, . . . , n). The tangential orientation to the trajectory at any point is equal to the X-axis orientation of the vehicle frame at the point. The tick at p_icorresponds to the Y-axis of the vehicle frame Q_i. The vehicle is in the stop state at the initial and final frames, Q₀and Q_n.

[0049] Now we demonstrate a method of computing a vehicle frame Q_i+1from the previous frame Q_i(i=0, 1, . . . , n−1), assuming Q_iis known. Notice that Q₀is known. At the motion sampling time t_i+1, we assume that the vehicle can measure an incremental trajectory length Δs_iand vehicle orientation change Δθ_igenerated during the motion-execution interval [t_i, t_i+1]. The vehicle has a system architecture shown in FIG. 3, comprising of a main vehicle subsystem and a motion control subsystem. The motion control subsystem executes the motor-control and motion-measurement tasks, and reports the incremental trajectory length Δs_iand vehicle orientation change Δθ_ito the main vehicle subsystem that executes the main algorithm described in this section. Detailed operations in the Motion Control Subsystem is described in Section [4]. The trajectory length Δs_iis positive or negative. If Δs_i<0, the vehicle has moved backwards manually. The vehicle orientation change Δθ_iis positive, negative, or equal to 0.

[0050] Noticing that the incremental vehicle trajectory from Q_ito Q_i+1in this period [t_i, t_i+1] is normally small compared with the vehicle size, we assume that the curvature in the period is constant, which is equal to the average curvature

[0051] κ_i=Δθ_i/Δs_i.

[0052] Thus the incremental trajectory is a circular arc or a straight segment with this curvature κ_i(FIG. 4). Namely the path segment is a short straight line if κ_i=0, or a short circular arc otherwise. The circular arc is counterclockwise if κ_i>0, and is clockwise if κ_i<0.

[0053] FIG. 5 is a figure made from FIG. 4 by moving the first vehicle frame Q_iinto the reference frame ((0, 0), 0) keeping the geometrical relation between the two frames, Q_iand Q_i+1, unchanged. FIGS. 4 and 5 demonstrate a specific case where the circular arc is counterclockwise with κ_i>0. First we define an incremental transformation

[0054] ΔQ_i=((Δx_i, Δy_i), Δθ_i),

[0055] which is a transformation that transforms Q_iinto Q_i+1. Let us show how to evaluate this incremental transformation. If Δθ_iis not equal to zero, then the radius r_iof the circular arc is

[0056] r_i=1/κ_i=Δs_i/Δθ_i.

[0057] From FIG. 5, we can evaluate

[0058] Δx_i=r_isin Δθ_i=(sin Δθ_i/Δθ_i)Δs_i

[0059] Δy_i=r_i(1−cos Δθ_i)=((1−cos Δθ_i)/Δθ_i)Δs_i.

[0060] On the other hand, if Δθ_i=0, the incremental trajectory is a straight segment and

[0061] Δx_i=Δs_i

[0062] Δy_i=0.

[0063] As a result, the incremental transformation ΔQ_iis obtained as

[0064] ΔQ_i=((Δs_i, 0), 0)

[0065] ΔQ_i=(((sin Δθ_i/Δθ_i)Δs_i, ((1−cos Δθ_i)/Δθ_i)Δs_i), Δθ_i)

[0066] if

[0067] Δθ_i=0,

[0068] otherwise. This incremental transformation ΔQ_itransforms the vehicle frame Q_i=((x_i, y_i), θ_i) into Q_i+1using the following two-dimensional composition operator (*):

[0069] Q_i+1=Q_i*ΔQ_i=((x_i, y_i), θ_i)*((Δx_i, Δy_i), Δθ_i)=((x_i+Δx_icos θ_i−Δy_isin θ_i, y_i+Δx_isin θ_i+Δy_icos θ_i), θ_i+Δθ_i).

[0070] The trajectory length s_i+1from the start point p₀to p_i+1is computed recursively by S_i+1=s_i+Δs_iwith an initial condition, s₀=0.

[0071] [4] Incremental Motion Detection

[0072] In this section, we describe how the motion control subsystem shown in FIG. 3 computes the incremental trajectory length Δs_iand orientation change Δθ_i. In this section only, we use Δs and Δθ to denote Δs_iand Δθ_irespectively for simplicity. As a sample wheel architecture for the vehicle, we take the differential-drive architecture, where two independently-driven wheels share the same axle as shown in FIG. 6. In this wheel architecture, the incremental rolling distances, Δs_Rand Δs_Lat the right and left wheels can be computed as follows, knowing the right and left motor-encoder pulse counts, Δn_Rand Δn_L, in the same motion-control-execution period, the total number of pulses N in one rotation of the encoder, the reduction-gear ratio G, and the wheel radius D:

[0073] Δs_R=(Δn_R/N)(1/G)πD

[0074] Δs_L=(Δn_L/N)(1/G)πD

[0075] Since we assume that the vehicle made a constant-crvature motion in this period, the three trajectories by the vehicle frame origin, the right wheel, and the left wheel are all concentric circular arcs, or are parallel line segments. First we assume that the curves are circular arcs; i.e., Δθ not equal to 0. Let r and 2W be the distance from the reference point to the circle center C and the distance between the two wheels respectively. From FIG. 6,

[0076] Δs_R+Δs_L=(r+W)Δθ+(r−W)Δθ=2rΔθ=2Δs,

[0077] Δs_R−Δs_L=(r+W)Δθ−(r−W)Δθ=2WΔθ.

[0078] Therefore,

[0079] Δs=(Δs_R+Δs_L)/2,  (1)

[0080] Δθ=(Δs_R−Δs_L)/(2W).  (2)

[0081] Thus, the pair (Δs, Δθ) of the incremental trajectory length and vehicle orientation change are computed from the two values Δs_Rand Δs_Lusing Equations (1) and (2) respectively. These equations are obviously correct even if the curves are straight segments, where Δs_R=Δs_Land the curves are straight segments. This concludes the description of the task of a motion control subsystem.

[0082] Even if the vehicle uses other wheel architectures such as the tricycle or the car-lick architecture, the evaluation method of the two incremental-motion-related values is straightforward for an individual who is knowledgeable in this field.

[0083] [5] Motion-Data Recording

[0084] Now we are ready to record the motion data. The ith trajectory record A_imust include the trajectory length and vehicle frame such as:

[0085] A_i=(s_i, Q_i)

[0086] This motion record A_iis recorded in a memory at time t_i. A trajectory data M is a sequence

[0087] M=(A₀, A₁, . . . , A_i, A_i+1, . . . , A_n)

[0088] of trajectory records.

[0089] [6] Motion Reproduction in Playing-Back Mode

[0090] The vehicle plays back the original motion using the trajectory data, M=(A₀, A₁, . . . , A_i, A_i+1, . . . , A_n) in its play-back mode. A method of reproducing the recorded motion is found, for instance, in U.S. Pat. No. 6,134,486. In this filed invention, when a path description is given, a vehicle is steered using the positional error, directional error, and curvature error. Notice the information on position and orientation is included in the vehicle frame Q_iin each motion record A_i=(s_i, Q_i). Although each curvature K_iis not directly contained in any motion record, it is calculated as

[0091] κ_iΔθ_i/Δs_i=(θ_i−θ_i−1)/(s_i−s_i−1).

[0092] Thus the motion record M contains all the necessary and sufficient geometrical information to reproduce the manually generated trajectory.

[0093] [7] Sensor Data Recording

[0094] Furthermore, assume that a sensor or sensors are mounted on the vehicle that has the motion capacity described in the previous sections. An example of a sensor is an image sensor, a sonar, a laser range finder, or an infrared sensor. The relative position and orientation of a sensor on the vehicle should be generally described by a three-dimensional frame in the vehicle local frame rather than a two-dimensional one. For instance, the effects of sensors mounted at different heights may be different. Likewise, the effects of sensors mounted in a horizontal orientation and in a vertical one are obviously different.

[0095] A three-dimensional frame or transformation T is represented as

[0096] T=((u, v, w), (α, φ, ψ)),

[0097] where u is X-coordinate, v Y-coordinate, w Z-coordinate, α yaw (rotation about Z-axis), φ pitch (rotation about Y-axis), and ψ roll (rotation about X-axis). To deal with three-dimensional frames, a vertical Z-axis is added to the global frame with its origin on the ground. Namely, the set of points with z=0 is the ground surface. The two-dimensional vehicle frame Q=((x, y), θ) is changed into a three-dimensional one Q′:

[0098] Q′=((x, y,0), (θ, 0, 0)).

[0099] A vertical Z-axis is added also to the local vehicle frame and its origin is set at the ground level. Suppose a sensor is mounted on a vehicle (FIG. 7). Its three-dimensional sensor frame R local to the vehicle frame is represented as

[0100] R=((a, b, c), (α, φ, ψ)).

[0101] The sensor's global frame S is obtained by the following three-dimensional composition:

[0102] S=Q′*R=((x, y,0), (θ, 0, 0))*((a, b, c),(α, φ, ψ))=((x+acos θ−bsin θ, y+asin θ+bcos θ, c), (θ+α, φ, ψ))

[0103] Let U be the data collected by the sensor. For instance, if it is a sonar, the data is merely a range value d. If it is an image sensor, the data is a two-dimensional intensity information. If it is a proximity sensor, the data is only a one-bit on-off information. An example of a sensor record B is a quadruple

[0104] B=(s, Q, S, U),

[0105] which comprises of the trajectory length s, the vehicle positional information Q, sensor positional information S, and sensed data U itself. A sensor data is a sequence

[0106] P=(B₀, B₁, . . . , B_i, B_i+1, . . . , B_m)

[0107] of sensor records. Notice that the function execution intervals for motion and sensing are not necessarily the same, and hence, the number of trajectory records in a trajectory data M and the number of sensor records in a sensor data P generated in a single teaching session may not be equal. If more than one sensor is mounted on the vehicle, a multiple number of sensor data are obtained in a single teaching session.

[0108] [8] Range-Finder Data Recording

[0109] In this section, we consider an embodiment where the sensor described in Section [7] is a range finder. A sonar is one of typical range finders. Suppose a range finder is mounted on a vehicle (FIG. 8). Its three-dimensional sensor frame R local to the vehicle frame is

[0110] R=((a, b, c), (α, φ, 0)),

[0111] as defined in the previous Section [7] except that the roll ψ=0, because of the nature of a range finder. The range finder's global frame S is obtained in a similar equation described in Section 7 as

[0112] S=Q′*R=((x, y,0), (θ, 0, 0))*((a, b, c), (α, φ, 0))=((x+acos θ−bsin θ, y+asin θ+bcos θ, c), (θ+α, φ, 0)).

[0113] If the range finder detects an object with a range of d, the object frame in the range-finder frame is ((d, 0, 0), (0, 0, 0)). The best estimate V of the frame of the object in the global frame is obtained by the three-dimensional composition (FIG. 8):

[0114] V=S*((d,0, 0), ( 0, 0, 0))=((x+acos θ−bsin θ, y+asin θ+bcos θ, c), (θ+α, φ, 0))*((d,0, 0), (0, 0, 0))=

[0115] ((x+acos θ−bsin θ+dcos θcos(θ+α), y+asin θ+bcos θ+dcos θsin(θ+α), c+dsin φ), (θ+α, φ, 0)).

[0116] This object frame is further added into the sensor record B

[0117] B=(s, Q, S, U, V).

[0118] to obtain a range-finder record. A range-finder data is a sequence

[0119] P=(B₀, B₁, . . . , B_i, B_i+1, . . . , B_m)

[0120] of range-finder records. If more than one range-finder is mounted on the vehicle, a multiple number of range-finder data are registered in one teaching session.

[0121] FIG. 9 shows an example set of positions of the object frame V in a range-finder data. In this specific experiment, the elevation φ was 0 and z becomes constant. Therefore, all the points are on a single horizontal plane.

[0122] [9] Another Embodiment

[0123] As opposed to the typical embodiment of the invention stated in Section [1], this invention can be embodied by a vehicle that has only one mode, a teaching mode. In this vehicle, a motion data obtained may be used for purposes other than reproducing the motion on the same or a similar vehicle.

[0124] Also, with this vehicle, a sensor record obtained may not be used for playing back with the same vehicle. For instance, the sensor data generated by a human-driven operation may be utilized for the generation of an abstract geometrical map of the world, but not for the vehicle's unmanned navigation.

[0125] Obviously many modifications and variations of the present invention are possible in light of the above teachings. It is, therefore, to be understood that the present invention may be practiced within the scope of the claims other than as specifically described.