GB2158965A - Driverless vehicle - Google Patents

Abstract

A vehicle control and guidance system in which a desired route for the vehicle is stored in the vehicle in the form of co-ordinates in a ground reference frame. The "vectors" between these junction points are divided by successive reference points into incremental vectors, the reference points being generated ahead of the vehicle at regular intervals. A dead reckoning system predicts the position of the vehicle at the end of each interval and this estimate is corrected, using a Kalman filter, by an independant fixed-target detection system using a scanning laser. The error between the estimated vehicle position and the local incremental vector provides a steering angle correction for the vehicle and the vehicle speed is dependent upon the lag of the vehicle behind the generation of reference points.

Description

SPECIFICATION Vehicle control and guidance system This invention relates to a vehicle control and guidance system in which one or more vehicles each having its own motive power and steering capability can be accurately moved within a predetermined area. Although the position of a vehicle at particular locations can be precisely determined using on-board sensors and external position markers, difficulties arise in trying to control accurately the movement of a vehicle between these particular locations in a smooth and economical fashion. Generally, an unmanned vehicle is constrained to move along predetermined paths, using either fixed rails which engage the wheels, or cables (or like metallic lines) buried under the paths to be followed. Such track or cable installations are expensive and unduly permanent since routes are thence-forth determined by the installation.

An object of the present invention is therefore to provide a free ranging un-manned vehicle control system such as to simulate a driver-controlled vehicle with a route to follow.

According to the present invention a vehicle control and guidance system comprises a vehicle, having motive power means for driving the vehicle, steering means for controlling the path of the vehicle, dead reckoning means for determining the position and heading of the vehicle on an incremental basis, electromagnetic direction finding means for providing sequential determinations of the bearing relative to the vehicle of one or more fixed reference targets, means for correcting dead reckoning determinations of vehicle position and heading in dependence upon the reference target bearing determinations, means for defining a desired route for the vehicle, and means for controlling the motive power means in dependence upon errors between the corrected determination of vehicle position and the desired route.

The means for defining a desired route preferably comprises storage means for storing the route in terms of a series of straight line segments and means for converting the junction of the straight line segments into smooth curved transitional segments.

There is preferably provided means for storing for each straight and curved segment, a permissible maximum vehicle speed and a permissible path width, according to the local route conditions.

The dead reckoning means may comprise means for estimating the position and heading of the vehicle after each of a series of time or distance increments, from the positon and heading of the vehicle at the beginning of the increment and in dependence upon the forward and rotational movement of the vehicle during the increment.

The system preferably comprises means for predicting the bearing of a reference target from the vehicle, on the basis of the estimated position of the vehicle and the last determined position of the reference target.

There may be include means providing a bearing error difference between the predicted bearing of the target and the bearing of the target as determined by the electromagnetic direction finding means, Kalman filter means producing correction products, being products of the bearing error and Kalman gain factors in respect of each of the position coordinate and the vehicle heading, and means for combining the correction products with the respective estimates of position and heading provided by the dead reckoning means, the results of the combination being best estimates of the position and heading of the vehicle.

There is preferably provided means for applying the best estimates to the dead reckoning means as a basis from which to estimate the vehicle position and heading one increment later.

Means may be provided for generating sequentially the coordinates of incremental reference points on said segments defining incremental vectors, the distance between such points being equal to the product of the maximum permissible speed for that segment and a basic time increment. There may also be provided error detecting means for comparing the best estimates of the vehicle position and heading with the position and heading of the nearest incremental vector and producing distance and heading error signals, and means for producing a steering angle demand signal in dependence upon the distance and heading error signals.This error detecting means preferably comprises means for transforming the coordinates of the best estimates of vehicle position and heading into a reference frame having one coordinate axis in alignment with a local incremental vector and having an origin coincident with the reference point to which the local incemental vector is directed.

A speed demand signal for the vehicle may be made dependent upon the number of reference points generated ahead of the vehicle position.

A vehicle control and guidance system in accordance with the invention will now be described, by way of example, with reference to the accompanying drawings, of which: Figure 1 is a diagrammatic layout of a factory floor area showing a vehicle route; Figure 2 is a diagrammatic view of a vehicle to be guided and controlled; Figures 3 and 4 are diagrams illustrating the position and orientation of the vehicle on the factory floor area in relation to factory coordinate axes; Figure 5 and 6 show diagrammatically the route of a vehicle, the free path width of the vehicle and bends in the route; Figure 7 is a block diagram of the system; Figure 8 is a block diagram of a Kalman filter process of the system; Figure 9 is a diagram showing displacement and heading errors of the vehicle; and Figure 10 is a diagram showing a transofrmation between 'factory frame' coordinates and 'vehicle frame' coordinates for the determination of positional errors.

It will be apparent that, in this specification the term "dead reckoning means" implies means for navigating based on detection of relative movement between vehicle and ground.

Referring to the drawings, Fig. 1 shows a factory floor area 1 having coordinate axes X and Y marked on it to provide a coordinate system. The coordinates of a location of this area will be referred to as 'factory coordinates' to distinguish from 'vehicle coordinates' to be explained subsequently.

A vehicle T, which may be a flat, load bearing truck, is required to move, un-manned, around the area 1 between stations A, B, C 8 D along a route defined by the factory coordinates of these stations. The route shown is, of course, purely for purposes of illustration and may in fact be more or less extensive.

Referring also to Fig. 2 the vehicle includes driving wheels 3 with any necessary differential gearing and a castor wheel 5 at one or both ends which is controllable in steering angle and which incorporates a steering angle transducer (not shown) and a 'distance-moved' transducer (not shown) for feedback purposes.

The driving wheels 3 are speed-controlled and supplied by way of a gearbox and DC convertor from a battery 7 in known manner.

The essential features of the vehicle are that it has a controllable steering angle and speed controlled driving equipment. The actual mechanical arrangement can obviously be optimised for the purposes of this invention but is not critical. Thus steering may alternatively be achieved by differential control of two driving wheels instead of by driven control of a castor wheel. The mechanical construction of the vehicle equipment need not therefore be detailed further.

Drive control and guideance electronics 9 receive distance and steering angle signals from the castor wheel 5 and provide steering angle control signals and speed control signals as will be explained.

The equipment so far mentioned is necessary for a dead-reckoning navigating system in which the vehicle location is determined on an incremental basis from a knowledge of distance moved and direction taken. Such systems, while satisfactory in some situations where distances are small, can suffer from cumulative faults due to wheel slippage, uneven surfaces, wear etc.

Accordingly therefore, it is a feature of the present invention that a supervisory referencing and correcting system is provided based upon the detection of fixed reference points and the location of the vehicle relative to them. The vehicle is provided with a"laser source 11 which is mounted to rotate continuously about a vertical axis. The laser beam is narrow in width and extensive in height so as to form a thin vertical line of radiation incident upon any obstructing target. A number of targets 1 3 are fixed at various positions around the area so that as far as possible one or more can be 'seen' by the vehicle from any position in the area. Of course, in a factory situation equipment and stores are moved around and are likely to obscure one or more of the targets from certain positions.In such circumstances the dead-reckoning system may be, temporarily at least, 'on its own'.

The targets 1 3 are formed from retro-reflectors which return incident light in the direction whence it came. They may be formed from vertical strips of retro-reflective material, the strips being coded by width or by presence and absence to provide an indication of their identity.

They may conveniently be mounted on boards positioned at or above head height to avoid the laser beam being interrupted by objects on the floor. The scanning laser beam may be directed upwards accordingly and may have a vertical angular extent sufficient to encompass the targets at ranges in question.

The laser scanning system is the subject of UK Patent Application No. 8313339 and will not be described in greater detail here.

The vehicle incorporates a receiver which receives the reflected beam from a target 1 3 and provides an indication of the direction of the reflected beam and thus of the target direction relative to the vehicle heading, This latter parameter is the angle that the vehicle longitudinal axis makes with the X axis of the factory coordinate system. The 'heading' is not necessarily the direction of travel since the steering wheel may not be straight ahead at the time.

Also included in the vehicle equipment is a microprocessor and data storage facility 1 9.

Amongst the data stored prior to a travel operation is the route 'man' in the form of the coordinates of the point A, B, C 8 D. The route may include sections which require reduced vehicle speed and the limiting speed for each straight section (segment or vector) A to B etc, is specified and stored.

In addition to limitations imposed on the speed along each route vector it may be that the tolerance on transverse vehicle displacement from the route line varies from vector to vector.

There is thus in effect a free path width for each vector beyond which the vehicle must not stray and this pathwidth may change at vector junctions. The path width for each vector is also stored prior to each travel operation.

The above information, route identification (coordinates of junction points), permissible speed in each vector and path width for each vector, may be stored in the vehicle data store by manual insertion of a program or may be communicated from a base station 1 5 to a communication beacon 1 7 on the vehicle, the beacon accepting and passing on the data to the vehicle data store and microprocessor 1 9. The communication unit may regularly inform the base station of the current vehicle position or may be interrogated for the same purpose.

The operation and processing involved will be explained with reference to the subsequent Figures additionally. Fig. 3 illustrates the vehicle navigation coordinates, the position being given by the x and y 'factory' coordinates, the vehicle heading being the angle Ip, i.e. the angle between the vehicle axis and the X axis, the forward velocity being V, and the rotation rate, i.e.

angular velocity of the vehicle being U.

Fig. 4 illustrates the vehicle-target coordinates additionally. The target reflector R, at factory coordinates xi and y; is detected at an angle Aj to the vehicle heading, the vehicle itself having position coordinates x and y, and heading +, at a particular time 't'.

Fig. 5 shows the junction of the two vectors where the free path widths as determined by the stored data, are the same. It is essential for smooth operation that the route at vector junctions is continuous rather than angular and a curved path or segment is calculated to fit the intersection. A requirement is that the curvature shall be as small as possible (i.e. maximum radius of curvature) to reduce forces on the vehicle traversing the bend, while at the same time the path width shall nowhere be less than the smaller of the two path widths. In Fig. 5 the radius of the curve is therefore half of the common path width. In Fig. 6 however, showing junctions between vectors of different path width, the fitted curves have radii pew,/2 and pew,/2 at the points E and F respectively.

Referring now to Fig. 7 this illustrates in block schematic form the navigation processing performed on the vehicle.

Data indicating the coordinate points on the route and the path widths of the various route vectors are received and stored (21). The processor then calculates (23) the smooth curves necessary at the vector junctions, as mentioned previously, on the basis of the path widths. The constant radius curves suggested in Figs. 5 and 7 are in fact not possible to achieve since a transition between a straight path and a circular curve would involve an instantaneous change from zero to a finite angular velocity, and thus infinite acceleration and force. An ideal curve would involve a linear increase and then decrease of curvature through the bend.

An approximation to this curve is provided by the following equation: x(a) = (1-a)2x1 + 2a(1-a)x2 + a2x3 (1) where a is that fraction into which the bend is divided, i.e, tenths, fifteenths, or whatever, #2 is a vector representing the coordinates of a junction point; X1 andx3 are vectors representing the coordinates of points at the beginning and end of the curve; and is the general vector representing the coordinates of points on the curve after each fraction a.

Thus by inserting one-tenth, two-tenths, three-tenths for a, the coordinates of successive points on the curve are obtained. This process is effected at 23 in Fig. 7 using a value of a determined as will be explained.

This principle of incremental construction of the curves is in fact common to the straight portions as well, the successive coordinates of points on the straight portion being given by x(a)=(1-a)xO+ax1 where x0 and xr are the coordinates of points at the beginning and end of the straight portion.

While the length of the straight portion might commonly be 20 or 30 metres the length of each section, each incremental vector that is, would commonly be perhaps 5 cms. The fraction a would then be about 1/500.

This generation of the incremental vectors is performed in block 25 and employs initial data (27) defining the speed limits applicable to the various route vectors.

Each incremental vector is defined by the coordinates of the point (the 'reference point') at its leading end. The generation of each such reference point occurs once in a basic time interval (at conveniently 20 Hz) which is derived from a clock pule generator of the system. Since the speed (maximum) for each vector is predetermined and the time interval is fixed, the maximum length of each incremental vector is determined. At a speed of 1 miser therefore, the incremental vector length is 5 cms. Having thus determined the maximum length this can be reduced slightly to make the number of incremental vectors in the route vector an integral number, of which a (above) is the inverse.

It will be apparent that if the incremental vector length is reduced from its maximum permissible, the speed of the vehicle for that portion of the .route will be reduced accordingly.

Dynamic control of the speed is thus provided.

In constructing the curve as explained above, by calculation of successive reference points, the incremented reference heading +, is determined as the angle of the line between successive reference points (relative to the factory X axis).

The reference points so calculated, which do of course define the required route as opposed to the actual route followed, are generated once per basic interval and stored for comparison with the 'actual' positions taken by the vehicle. These 'actual' positions are, in fact, estimates derived from the dead reckoning system as checked and corrected by the laser/target reference system.

If the generated reference points advance ahead of the vehicle, due to inertia etc., the difference in position between the latest reference point and the vehicle may be considered as an error distance which the vehicle tries to reduce. The greater this error distance the greater should be the speed of the vehicle (within the limit set for the particular route vector or bend) to try to reduce it. Conversely, the speed should be reduced if the generated reference points are only just ahead of the vehicle position. This last situation will obtain when the vehicle is approaching a stopping station at the end of a route vector. The number of reference points 'lying in wait' is thus an indication of speed requirement and the vehicle drive motor is controlled accordingly.

The generated reference points and the associated incremental reference heading Ip are passed, one pair at a time, for position and heading error determination (29).

The estimated vehicle position and heading is provided by a Kalman filter predictor process 37 in Fig. 7. This employs target detection inputs 0 (Fig. 4) derived from the laser/ reflector system 33. Distance moved by the vehicle and steering angle signals are obtained from transducers referred to previously and indicated in Fig. 7 at 35. The Kalman predictor process itself (37) is illustrated in greater detail in Fig. 8.

Referring to Fig. 8, the first process to be performed is the estimation of position and heading at the end of one basic time interval (At) given the forward and rotational speeds of the vehicle during that interval and the position, either actual or estimated, at the beginning of the interval.

The transducer inputs to the position predictor process are the steering angle f picked off the steering castor, and the distance travelled by that castor wheel produced as distance-counting pulses. The distance increment and the increment At give the speed in the direction of the castor wheel from which the forward speed V of the vehicle, along its heading, is obtained as a product with the cosine of the steering (castor) angle cp. The angular velocity of the vehicle, U, is derived from the products of the castor wheel velocity and the sine of the steering angle f in accordance with the geometry of the vehicle wheels. These velocities V and U are calculated in each time interval on the assumption that the velocities and steering angle are constant for the (short) time interval At.

The equations used in the process of the position predictor 39 of Fig. 8 are derived as follows. The rates of change of the position coordinates x and y, and of the heading angle Ip can be be seen from inspection of Fig. 3 to be given by: A = U x =VcosA y =Vsin4' By the integration of these equations over the period At the following equations are derived: #(t + #t) = #(t) + U.#t x(t X #t) = x(t) + V(sin(#(t) + U.#t)- sin(#(t)))#t/U.#t y(t + #t) = y(t) - V(cos(#(t) + U.#t)- cos(#)t))).#t/U.#t The first of these is expressed as: the value of + at time (t + At) is equal to the value of + at time t plus the angle through which the vehicle has rotated in the time interval At. In the second and third equations t and (t + At) have the same significance as in the first.

These equations may be re-written for estimated values as: #(t + #t/t) = #(t/t) + U.#t x(t + #t/t) = x(t/t) + V(sin(#(t/t) + U#t)- sin#(t/t))/U y(t + At/t) = 9(t/t) - V(cos(+(t/t) + U At) - cost/t))/U The 'hat' over a parameter indicating an estimated value, and the symbol "t/t" meaning "evaluated at time t".

It may be seen that these equations are sufficient to estimate the position and heading of the vehicle at time (t + At) knowing the estimated position and heading time t. The output of block 39 is thus the estimated coordinates x and y and the estimated heading # of the vehicle after a further time interval At, and, but for the input 41, based only on the dead reckoning system of distance travelled and the angle moved through.

From Fig. 4 the estimated angle of a target reflector R may be obtained in terms of the vehicle heading + and the coordinates of the vehicle and the target as:

where #1 is the estimated target angle at time (t + At) evaluated at time t. The coordinates xi and yi of the target are predetermined from the layout of the targets on the factory floor. The estimated values x and y of the vehicle are derived from process 39 and also the estimated value + of the heading.The equation is thus processed in a target bearing predictor 43 which produces the output The output of the laser target detection system 33 provides an accurate observation of the target angle 0 which is differenced with the estimated value #i in a process 45 to give a target estimation error #i - #i.

This error signal is processed by the Kalman filter 47 which effectively produces products of the error with respective Kalman gain factors k+, kx, and ky. These correction products are then added in process 49 to the dead-reckoning predictions of process 39 in accordance with the following equations to give corrected estimates of the vehicle heading and position: #(t + #t/t + #t) = #(t + #t/t) + k#(#i - #i) x(t + #t/t + #t) = x(t + #t/t) + kx(#i - #i) y(t + #t/t + #t) = y(t + #t/t) + ky(#i - #i) The derivation of Kalman correction products and the operation of Kalman filters is given in the book "Optimisation of Stochastic Systems" by M. Aoki, published by Academic Press 1967.

Thus corrected estimates of heading and position at time (t + At) evaluated at that time are obtained from estimates of heading and position obtained at time t and corrected by the Kalman filter process. These best estimates are output to the error determining process 29 of Fig. 7 but are also applied as 'present' inputs (41) to the position predictor 39 of Fig. 8 from which to predict the next reference point. Thus in the absence of one or more target reference corrections, due, for example, to obscuring of the targets, the next reference point is predicted by dead reckoning from the last reference point that had the benefit of target-detection correction.

Referring back to Fig. 7, the determination of distance and heading errors will now be described. The two inputs to process 29 are (a) the generated series of reference points defining the ideal route, and (b) the best estimate of vehicle heading and position derived from the Kalman process of Fig. 8.

Referring to Fig. 9, this shows succesive incremental vectors IV1 and IV2 and their associated reference points RP1 and RP2 as generated in accordance with process 25 (Fig. 7). Errors in the navigation of the vehicle T are determined as the perpendicular distance de between the centre of the vehicle and the local incremental vector, and the angular error Be between the heading of the vehicle and the direction of the local incremental vector.

Measurement of these errors de and 0e are achieved by a transformation of the actual vehicle position in factory coordinates to a position in a vehicle reference frame in which the origin is at the reference point of the local incremental vector and the new X axis coincides with the local incremental vector. This transformation is illustrated in Fig. 10 in which X and Y are the factory coordinate axes, X* and Y* are the vehicle frame axes, x, and y, are the coordinates in the factory frame of the local incremental vector reference point, x, y are the coordinates of the vehicle in the factory frame and x* y* are the vehicle coordinates in the vehicle frame.

The angle 4'r is the heading of the incremental vector relative to the factory frame. From Fig. 9 the following transformations are derived by simple geometry: x* = (xx,)cos+, + (yy,)sin+, y* = (y = (yy,)cosAr(xx,)sinAr In this vehicle reference frame it may be seen that the distance error de is the y* coordinate of the vehicle centre and the angle error Oe is the transformation angle +, directly.

When the vehicle passes the local reference point x, y, the polarity of x* will change from negative to positive. This change will initiate discarding of the current vehicle reference frame and re-establishing it with origin on the succeeding reference point and x* axis aligned with the next incremental vector. It will be seen therefore that the vehicle frame steps along synchronously with the vehicle.

The error values d0 and He are then used to derive a steering-angle-demand signal at as a direct function of these error values. The demanded angular velocity Ud is first derived as: Ud = Kide + K2Oe where K1 and K2 are gain functions defined by the dynamics of the vehicle itself. The demand steering angle is then calculated from the vehicle geometry and the demanded forward speed of the vehicle.

As mentioned above, speed of the vehicle is controlled by measuring the number of incremental vectors between the vehicle and the last one generated. This last generated one will always be ahead of the vehicle and will appear to 'pull' the vehicle along as if an elastic band connects the vehicle to the last position reference generated.

From a stationary position the reference points will be generated at a linear rate away from the 'rest' station and will in effect stretch the above mentioned elastic. The vehicle will accelerate in accordance with the error distance to the latest reference point and dependent upon its inertia and power, and gradually the error will reduce until a steady state is reached where the vehicle speed is equal to the rate of advance of the reference points.

Reference point speed and therefore vehicle speed can be altered dynamically during travel along a defined trajectory by altering the incrementing distance (i.e. by varying a in algorithm (1) above) between successive reference points.

When the generation of incrementing vectors and reference points is complete, the final incrementing vector will terminate on the final stopping position demanded by the original route specified in coordinate position defining route vectors. The vehicle speed will slow down as the distance error decreases. By shaping the distance-error/demanded-speed relationship the speed is controlled so that the final point is reached without overshoot. The vehicle frame is again directly applicable here as the x coordinate is the 'distance-to-go' in that frame and this can be monitored without further calculation.

The electromagnetic direction-finding means is in the above embodiment a laser system providing direction sensing by means of a narrow laser beam scanning in azimuth. It is however, envisaged that radar beams could be employed providing accurate direction finding by phase comparison techniques. In addition, the reflectors above could be replaced by transponders with coded emissions.

Claims (11)

1. A vehicle control and guidance system comprising a vehicle, said vehicle housing motive power means for driving the vehicle, steering means for controlling the path of the vehicle, dead reckoning means for determining the position and heading of the vehicle on an incremental basis, electromagnetic direction finding means for providing sequential determinations of the bearing relative to the vehicle of one or more fixed reference targets, means for correcting dead reckoning determinations of vehicle position and heading in dependence upon the reference target bearing determinations, means for defining a desired route for the vehicle, and means for controlling said motive power means in dependence upon errors between the corrected determination of vehicle position and said desired route.

2. A vehicle control and guidance system according to Claim 1, wherein said means for defining a desired route comprises storage means for storing said route in terms of a series of straight line segments and means for converting the junction of said straight line segments into smooth, curved transitional segments.

3. A vehicle control and guidance system according to Claim 2, including means for storing, for each straight and curved segment, a permissible maximum vehicle speed and a permissible path width, according to the local route conditions.

4. A system according to Claim 3, wherein said means for converting said junction into curved transitional segments comprises means for calculating the radius of curvature of a curved segment as not less than half the value of the path width and, where there are different path widths at the junction, half the value of the greater of the two path widths.

5. A system accordng to any of Claims 2, 3 and 4, wherein said dead reckoning means comprises means for estimating the position and heading of the vehicle after each of a series of time or distance increments, from the position and heading of the vehicle at the beginning of the increment and in dependence upon the forward and rotational movement of the vehicle during the increment.

6. A system according to Claim 5, comprising means for predicting the bearing of a said reference target from the vehicle on the basis of the said estimated position of the vehicle and the last determined position of said reference target.

7. A system according to Claim 6, including means providing a bearing error difference between the predicted bearing of said target and the bearing of said target as determined by said electromagnetic direction finding means, Kalman filter means producing correction products, said products being products of the said bearing error and Kalman gain factors in respect of each of the position co-ordinates and the vehicle heading, and means for combining said correction products with the respective estimates of position and heading provided by said dead reckoning means, the results of the combination being best estimates of the position and heading of the vehicle.

8. A system according to Claim 7, comprising means for applying said best estimates to said dead reckoning means as a basis from which to estimate the vehicle position and heading one increment later.

9. A system according to any of Claims 2, 3 and 4 comprising means for generating sequentially the coordinates of incremental reference points on said segments defining incremental vectors, the distance between such points being equal to the product of the maximum permissible speed for that segment and a basic time increment.

10. A system according to Claim 7, further comprising means for storing, for each of said straight and curved line segments, a permissible maximum vehicle speed and a permissible path width, according to local route conditions, means for generating sequentially the coordinates of incremental reference points on said segments defining incremental vectors, the distance between such points being equal to the product of the maximum permissible speed for that segment and a basic time increment, error detecting means for comparing said best estimates of the vehicle position and heading with the position and heading of the nearest incremental vector and producing distance and heading error signals, and means for producing a steering angle demand signal in dependence upon said distance and heading error signals.

11. A system according to Claim 10, wherein said detecting means comprises means for transforming the coordinates of said best estimates of vehicle position and heading into a reference frame having one coordinate axis in alignment with a local incremental vector and having an origin coincident with the reference point to which the local incremental vector is directed.

1 2. A system according to Claim 10, or Claim 11, comprising means for providing a speed demand signal for the vehicle dependent upon the number of reference points generated ahead of the vehicle position.

1 3. A system according to any preceding claim, wherein said direction finding means comprises a laser beam source scanning in azimuth and means for detecting the bearing of a target reflector relative to the heading of the vehicle, from the direction of the reflected beam.

1 4. A vehicle control and guidance system substantially as hereinbefore described with reference to the accompanying drawings.