Optical tracking vehicle control system and method

Abstract

A vehicle control system having a controller and a spatial database adapted to provide spatial data to the controller at control speed. The spatial data provided from the spatial database to the controller includes images collected from an optical sensor subsystem in addition to other data collected by a variety of sensor types, including a GNSS or inertial measurement system. The spatial data received by the controller from the database forms at least part of the control inputs that the controller operates on to control the vehicle. The advantage provided by the present invention allows control system to “think” directly in terms of spatial location. A vehicle control system in accordance with one particular embodiment of the invention comprises a task path generator, a spatial database, at least one external spatial data receiver, a vehicle attitude compensation module, a position error generator, a controller, and actuators to control the vehicle.

Description

CROSS-REFERENCE TO RELATED APPLICATION

This application LED(GNSS)138219 319
where n is the number of states.

In general, the mathematical model used to model the vehicle's motion and aspects of its operation will comprise a series of differential equations. The number of equations will be the same as the number of states. In some cases, the differential equations will be linear in terms of the states, whereas in other situations the equations may be nonlinear in which case they must generally be “linearized” about a point in the “state space”. Linearization techniques that may be used to do this will be well known to those skilled in this area.

Next, by noting that any j^th order linear differential equations can be re-written equivalently as a set
where

• • C is a j×n matrix linking the outputs to the states,
  • D is a j×m matrix linking the outputs to the inputs,
  • j is the number of outputs, and
  • Mv(t) is a quantity (represented by an n×1 vector) called the “measurement noise”. The measurement noise represents errors and noise that invariably exist in measurements taken from the actual vehicle. Like Ew(t) above, Mv(t) is assumed to be Gaussian, white, have zero mean, to act directly on the state derivatives and to be uncorrelated with the process noise or itself.

Next, it will be noted that both the state equation and the measurement equation defined above are continuous functions of time. However, continuous time functions do not often lend themselves to easy digital implementation (such as will generally be required in implementing the present invention) because digital control systems generally operate as recursively repeating algorithms. Therefore, for the purpose of implementing the equations digitally, the continuous time equations may be converted into the following recursive discrete time equations by making the substitutions set out below and noting that (according to the principle of superposition) the overall response of a linear system is the sum of the free (unforced) response of that system and the responses of that system due to forcing/driving inputs. The recursive discrete time equations are:
X_k+1=FX_k+GU_k+1+Lw_k+1
Y_k+1=ZY_k+JU_k+1+Nw_k+1
where

• • k+1 is the time step occurring immediately after time step k,
  • Z=C, J=D and Nv is the discrete time analog of the continuous time measurement noise Mv(t).
  • F is a transition matrix which governs the free response of the system. F is given by:
    F=e^AΔt
  • GU_k+1 is the forced response of the system, i.e. the system's response due to the driving inputs. It is defined by the convolution integral as follows:

$G{\underset{\_}{U}}_{k+1}={\int }_0^{\Delta t}{e}^{A\left(\Delta t-\tau \right)}B\underset{\_}{U}\left(t_{k+1}+\tau \right)d\tau$

• • 
    Those skilled in the art will recognize that both h and H are inherently directional quantities.

    Finally, the “curvature error” is the difference between the actual instantaneous radius of curvature r of the vehicle's motion and the desired instantaneous radius of curvature R. The curvature error is given by:
    Curvature Error=1/R−1/r

    It will also be clearly appreciated that there may be many other vehicle variables or parameters which also need to be controlled if, for example, acceleration/deceleration or the vehicle's mode of equipment operation are also to be controlled.

    Referring next to FIG. 16, it can be seen that a vehicle control system in accordance with one particular embodiment of the invention comprises: a task path generator; a spatial database; at least one external spatial data source; a vehicle attitude compensation module; a position error generator; a controller; and actuators to control (steer) the vehicle.

    In the overall operation of the control system, the desired path trajectory for the vehicle is first entered into the control system by the user via the user terminal 608. The task path generator then interprets this user-defined path definition and converts it into a series of points of sufficient spatial density to adequately represent the desired path to the requisite level of precision. The task path generator typically also defines the vehicle's desired trajectory along the user-defined path, for example, by generating a desired vehicle position, a desired heading H and a desired instantaneous radius of curvature R for each point on the path. This information is then loaded into the spatial database. The way in which this and other spatial information is stored within the database in representative embodiments, and in particular the way in which pieces of data are given memory allocations according to their spatial location, is described further below.

    As the vehicle moves along the user-defined path, it will invariably experience various perturbations in its position and orientation due to, for example, bumps, potholes, subsidence beneath the vehicle's wheels, vehicle wheel-spin, over/under-steer, etc. Those skilled in this area will recognize that a huge range of other similar factors can also influence the instantaneous position and orientation of the vehicle as it moves. One of the purposes of the present control system is to automatically correct for these perturbations in position and orientation to maintain the vehicle on the desired path (or as close to it as possible).

    As the vehicle moves, the control system progressively receives updated information regarding spatial location from the external spatial data sources. The external spatial data sources will typically include GPS. However, a range of other spatial data sources may also be used in addition to, or in substitute for GPS. For example, the inertial navigation systems (INS), visual navigation systems, etc. described above may also be used as external data sources in the present control system.

    Those skilled in the art will recognize that the spatial data collected by the external spatial data sources actually pertains to the specific location of the external spatial data receivers, not necessarily the vehicle/implement reference location itself (which is what is controlled by the control system). In FIG. 14, the reference location is on the vehicle 601 and is indicated by the intersection (i.e. the origin) of the roll, pitch and yaw axes. In other embodiments, the reference location may be located elsewhere on the vehicle, or on the implement 602, etc. In any event, to illustrate this point, it will be seen that the GPS antenna 604 in FIG. 14 is located on the roof of the vehicle some distance from the vehicle's reference point. Therefore, the spatial data collected by the GPS antenna actually relates to the instantaneous location of the vehicle's roof, not the location of the vehicle's reference point. Likewise, the spatial data collected by the optical sensor 606 actually pertains to the particular location of the optical sensor (slightly out in front of the vehicle in FIG. 14).

    In addition to this, changes in the vehicle's attitude will also influence the spatial position readings received by the different receivers. For example, if one of the vehicle's wheels passes over, or is pushed sideways by a bump, this may cause the vehicle to rotate about at least one (and possibly two or three) of the axes shown in FIG. 14. This will in turn change the relative position of the spatial data receiver(s) such as GPS antenna 604 with respect to the reference location on the vehicle or implement. This can be used (typically in combination with other sources of external spatial data or “feedback” data) to determine the orientation of the vehicle. The orientation of the vehicle may be considered to be the relative orientation of the vehicle's axes in space.

    In order to compensate for the difference in position between the vehicle's reference point and the location of the spatial data receiver(s), and also to account for changes in the vehicle's orientation, a vehicle attitude compensation module is provided. This is shown in FIG. 16. The vehicle attitude compensation module converts all readings taken by the various spatial data receivers (which relate to the different specific locations of the receivers) into readings pertaining to the spatial location and orientation of the vehicle's reference point. This data pertaining to the spatial location and orientation of the vehicle's reference point is then fed into the spatial database.

    Those skilled in the art will recognize that the one or more external spatial data sources will progressively receive updated data readings in rapid succession (e.g., in “real time” or as close as possible to it). These readings are then converted by the vehicle attitude compensation module and fed into the spatial database. The readings may also be filtered as described above. Therefore, whilst each reading from each spatial data source is received, converted (ideally filtered) and entered into the spatial database individually, nevertheless the rapid successive way in which these readings (possibly from multiple “parallel” data sources) are received, converted and entered effectively creates a “stream” of incoming spatial data pertaining to the vehicle's continuously changing instantaneous location and orientation. In order to provide sufficient bandwidth, successive readings from each external spatial data source should be received and converted with a frequency of the same order as the clock speed (or at least one of the clock speeds) of the controller, typically 3 Hz-12 Hz or higher.

    Referring again to FIG. 16, the position error generator next receives information from the spatial database. The information it receives from the database includes: the vehicle's desired position, heading H and instantaneous radius of curvature R. (It will be recalled that this information is originally generated by the task path generator and then entered into the spatial database, based on the user-defined path trajectory); and the vehicle's actual position, heading h and instantaneous radius of curvature r. (This information is based on spatial data progressively received from the external spatial data sources as described above, and typically also on data received through feedback.)

    The position error generator then uses this information to calculate an instantaneous “error term” for the vehicle. The “error term” incorporates the vehicle's instantaneous cross-track error, heading error and curvature error (as described above). The error term is then fed into the controller. The controller is shown in greater detail in FIG. 17.

    From FIG. 17 it can be seen that the controller incorporates a cross-track error
    where n is the number of states.

    • • 
        
        where k+1 is the time step occurring immediately after time step k, Z=C, J=D and Nv is the discrete time analog of the continuous time measurement noise Mv(t). F is a transition matrix which governs the free response of the system. F is given by:
        GU.sub.k+1 is the forced response of the system, i.e. the system's response due to the driving inputs. It is defined by the convolution integral as follows:

        $\mathrm{GU}k+1={\int }_0^{\Delta t}{e}^{A\left(\Delta t-\tau \right)\mathrm{dt}}$
        

        Similarly, the quantity Lw.sub.k+1 is the (forced) response of the system due to the random “error” inputs that make up the process noise. Hence, conceptually this quantity may be defined as:

        $\mathrm{Lw}k+1={\int }_0^{\Delta t}{e}^{A\left(\Delta t-\tau \right)\mathrm{dt}}\mathrm{Ew}\left(tk+1+\tau \right)d\tau$
        

        However, as noted above, the quantity Ew(t) is not deterministic and so the integral defining Lw.sub.k+1 cannot be performed (even numerically). It is for this reason that it is preferable to use statistical filtering techniques such as a “Kalman Filter” to statistically optimize the states estimated by the mathematical model.

        In general, a “Kalman Filter” operates as a “predictor-corrector” algorithm. Hence, the algorithm operates by first using the mathematical model to “predict” the value of each of the states at time step k+1 based on the known inputs at time step k+1 and the known value of the states from the previous time step k. It then “corrects” the predicted value using actual measurements taken from the vehicle at time step k+1 and the optimized statistical properties of the model. In summary, the Kalman Filter comprises the following equations each of which is computed in the following order for each time step:

        $\begin{array}{c}{X}_{k+1❘k}=F{\underset{\_}{X}}_{k❘k}+G{\underset{\_}{U}}_{k+1}\\ P_{k+1❘k}={\mathrm{FP}}_{k❘k}{F}^T+Q\\ K_{k+1}=P_{k+1❘k}{Z^T\left({\mathrm{ZP}}_{k+1❘k}{Z}^T+R\right)}^{-1}\\ {\underset{\_}{Y}}_{k+1}=Z{\underset{\_}{X}}_{k+1❘k}+J{\underset{\_}{U}}_{k+1}\\ {\underset{\_}{\upsilon }}_{k+1}={\underset{\_}{\hat{Y}}}_{k+1}-{\underset{\_}{Y}}_{k+1}\end{array}\}\mathrm{predictor}\begin{array}{c}{\underset{\_}{X}}_{k+1❘k+1}={\underset{\_}{X}}_{k+1❘k}+K_{k+1}{\upsilon }_{k+1}\\ P_{k+1❘k+1}=\left(I-K_{k+1}Z\right)P_{k+1❘k}\end{array}\}\mathrm{corrector}$
        
        where the notation k+1|k means the value of the quantity in question at time step k+1 given information from time step k. Similarly, k+1|k+1 means the value of the quantity at time step k+1 given updated information from time step k+1. [0135]P is the co-variance in the difference between the estimated and actual value of X. [0136]Q is the co-variance in the process noise. [0137]K is the “Kalman gain” which is a matrix of computed coefficients used to optimally “correct” the initial state estimate. [0138]R is the co-variance in the measurement noise. [0139] is a vector containing measurement values taken from the actual vehicle.

        The operation of the discrete time state space equations outlined above, including the Kalman gain and the overall feedback closed loop control structure, are represented graphically in FIG. 19.

        In relation to the spatial database, it is mentioned above that a wide range of methods are known for arranging data within databases. One commonly used technique is to provide a “hash table”. The hash table typically operates as a form of index allowing the computer (in this case the control system CPU) to “look up” a particular piece of data in the database (i.e. to look up the location of that piece of data in memory). In the context of the present invention, pieces of data pertaining to particular locations along the vehicle's path are assigned different hash keys based on the spatial location to which they relate. The hash table then lists a corresponding memory location for each hash key. Thus, the CPU is able to “look up” data pertaining to a particular location by looking up the hash key for that location in the hash table which then gives the corresponding location for the particular piece of data in memory. In order to increase the speed with which these queries can be carried out, the hash keys for different pieces of spatial data can be assigned in such a way that “locality” is maintained. In other words, points which are close to each other in the real world should be given closely related indices in the hash table (i.e. closely related hash keys).

        The spatial hash algorithm used to generate hash keys for different spatial locations in representative embodiments of the present invention may be most easily explained by way of a series of examples. To begin, it is useful to consider the hypothetical vehicle path trajectory shown in FIG. 20. In FIG. 20, the successive points which define the path are described by a simplified integer based (X,Y) coordinate system. Hence, in FIG. 20, the vehicle moves in the X direction along the entire length of the first swath from (0,0) to (4,0), before moving up in the Y direction to then move back along the second swath in the opposite direction from (4,1) to (0,1), etc.

        As outlined above, in the present invention all data is stored within the spatial database with reference to spatial location. Therefore, it is necessary to assign indices or “hash keys” to each piece of data based on the spatial location to which each said piece of data relates. However, it will be recalled that the hash table must operate by listing the hash key for each particular spatial location together with the corresponding memory location for data pertaining to that spatial location. Therefore, the hash table is inherently one-dimensional, and yet it must be used to link hash keys to corresponding memory allocations for data that inherently pertains to two-dimensional space.

        One simple way of overcoming this problem would be to simply assign hash keys to each spatial location based only on, say, the Y coordinate at each location. The hash keys generated in this way for each point on the vehicle path in FIG. 20 are given in Table 1 below.

        TABLE 1
        Spatial Hash Key Generated Using only the Y Coordinate
        	(X,Y)	Hash key	Hash key
        	coordinates	(hexadecimal)	(decimal)
        	(0, 0)	0x0	0
        	(1, 0)	0x0	0
        	(2, 0)	0x0	0
        	(3, 0)	0x0	0
        	(4, 0)	0x0	0
        	(0, 1)	0x1	1
        	(1, 1)	0x1	1
        	(2, 1)	0x1	1
        	(3, 1)	0x1	1
        	(4, 1)	0x1	1
        	(0, 2)	0x2	2
        	(1, 2)	0x2	2
        	(2, 2)	0x2	2
        	(3, 2)	0x2	2
        	(4, 2)	0x2	2
        	(0, 3)	0x3	3
        	(1, 3)	0x3	3
        	(2, 3)	0x3	3
        	(3, 3)	0x3	3
        	(4, 3)	0x3	3
        	(0, 4)	0x4	4
        	(1, 4)	0x4	4
        	(2, 4)	0x4	4
        	(3, 4)	0x4	4
        	(4, 4)	0x4	4

        The prefix “0x” indicates that the numbers in question are expressed in hexadecimal format. This is a conventional notation.

        Those skilled in the art will recognize that the above method for generating hash keys is far from optimal because there are five distinct spatial locations assigned to each different hash key. Furthermore, in many instances, this method assigns the same hash key to spatial locations which are physically remote from each other. For instance, the point (0,1) is distant from the point (4,1), and yet both locations are assigned the same hash key. An identically ineffective result would be obtained by generating a hash key based on only the X coordinate.

        An alternative method would be to generate hash keys by concatenating the X and Y coordinates for each location. The hash keys generated using this method for each point on the vehicle path in FIG. 20 are given in Table 2 below.

        TABLE 2
        Hash Keys Generated by Concatenating the X and Y Coordinates
        (X, Y)	Hash key	Hash key
        coordinates	(hexadecimal)	(decimal)
        (0, 0)	0x0	0
        (1, 0)	0x100	256
        (2, 0)	0x200	512
        (3, 2)	0x302	770
        (4, 2)	0x402	1026
        (0, 3)	0x3	3

        
        

        TABLE 2
        Hash Keys Generated by Concatenating the X and Y Coordinates
        (X, Y)	Hash key	Hash key
        coordinates	(hexadecimal)	(decimal)
        (0, 0)	0x0	0
        (1, 0)	0x100	256
        (2, 0)	0x200	512
        (3, 0)	0x300	768
        (4, 0)	0x400	1024
        (0, 1)	0x1	1
        (1, 1)	0x101	257
        (2, 1)	0x201	513
        (3, 1)	0x301	769
        (4, 1)	0x401	1025
        (0, 2)	0x2	1
        (1, 2)	0x102	258
        (2, 2)	0x202	514
        (3, 2)	0x302	770
        (4, 2)	0x402	1026
        (0, 3)	0x3	3
        (1, 3)	0x103	759
        (2, 3)	0x203	515
        (3, 3)	0x303	771
        (4, 3)	0x403	1027
        (0, 4)	0x4	4
        (1, 4)	0x104	260
        (2, 4)	0x204	516
        (3, 4)	0x304	772
        (4, 4)	0x404	1028

        In order to understand how the numbers listed in Table 2 above were arrived at, it is necessary to recognize that in the digital implementation of the present control system, all coordinates will be represented in binary. For the purposes of the present example which relates to the simplified integer based coordinate system in FIG. 20, a simplified 8-bit binary representation has been used.

        Hence, to illustrate the operation of the spatial hash key algorithm used to generate the numbers in Table 2, consider the point (3,3). Those skilled in the art will understand that the decimal number 3 may be written as 11 in binary notation. Therefore, the location (3,3) may be rewritten in 8-bit binary array notation as (00000011,00000011). Concatenating these binary coordinates then gives the single 16-bit binary hash key 0000001100000011 which can equivalently be written as the hexadecimal number 0x303 or the decimal number 771. The process of converting between decimal, binary and hexadecimal representations should be well known to those skilled in the art and need not be explained.

        It will be noted from Table 2 above that concatenating the X and Y coordinates leads to unique hash keys (in this example) for each spatial location. However, the hash keys generated in this way are still somewhat sub-optimal because points which are located close to each other are often assigned vastly differing hash keys. For example, consider the points (0,0) and (1,0). These are adjacent point in the “real world”. However, the hash keys assigned to these points using this method (written in decimal notation) are 0 and 256 respectively. In contrast, the point (0,4) is much further away from (0,0) and yet it is assigned the much closer hash key 4. Therefore, this algorithm does not maintain “locality”, and an alternative algorithm would be preferable.

        Yet a further method for generating hash keys is to use a technique which shall hereinafter be referred to as “bitwise interleaving”. As for the previous example, the first step in this technique is to represent the (X,Y) coordinates in binary form. Hence, using the 8-bit binary array representation discussed above, the point (X,Y) may be re-written in 8-bit binary array notation as (X1X2X3X4X5X6X7×8, Y1Y2Y3Y4Y5Y6Y7Y8). Next, rather than concatenating the X and Y coordinates to arrive at a single 16-bit binary hash key, the successive bits from the X and Y binary coordinates are alternatingly “interleaved” to give the following 16-bit binary hash key X1Y1X2Y2X3Y3X4Y4×5Y5X6Y6X7YX8Y8. The hash keys generated using this method for each point on the vehicle path in FIG. 20 are given in Table 3 below.

        TABLE 3
        Hash Keys Generated by “Bitwise Interleaving” the
        X and Y Coordinates (X, Y)
        (X, Y)	Hash key	Hash key
        coordinates	(hexadecimal)	(decimal)
        (0, 0)	0x0	0
        (1, 0)	0x2	2
        (2, 0)	0x8	8
        (3, 0)	0xa	10
        (4, 0)	0x20	32
        (0, 0)	0x1	1
        (1, 0)	0x3	3
        (2, 0)	0x9	9
        (3, 0)	0xb	11
        (4, 0)	0x21	33
        (0, 2)	0x4	4
        (1, 2)	0x6	6
        (2, 2)	0xc	12
        (3, 2)	0xc	14
        (4, 2)	0x24	36
        (0, 3)	0x5	5
        (1, 3)	0x6	7
        (2, 3)	0xd	13
        (3, 3)	0xf	15
        (4, 3)	0x25	37
        (0 ,4)	0x10	16
        (1, 4)	0x12	18
        (2, 4)	0x18	24
        (3 ,4)	0x1a	26
        (4, 4)	0x30	48

        To further illustrate the operation of the spatial hash algorithm used to generate the numbers in Table 3, consider the point (3,4). As noted above, the decimal number 3 may be written as 11 in binary notation. Similarly, decimal number 4 is written as 100 in binary. Therefore, the location (3,4) may be rewritten in 8-bit binary array notation as (00000011,00000100). Bitwise interleaving these binary coordinates then gives the single 16-bit binary hash key 0000000000011010, which can equivalently be written as the hexadecimal number 0x1a or the decimal number 26.

        From Table 3 it will be seen that generating hash keys by “bitwise interleaving” the X and Y coordinates leads to unique hash keys (in this example) for each spatial location. Also, the hash keys generated in this way satisfy the requirement that points which are close together in the real world are assigned closely related hash keys. For example, consider again the points (0,0) and (1,0). The hash keys now assigned to these points by “bitwise interleaving” (when written in decimal notation) are 0 and 2 respectively. Furthermore, the point (0,1) which is also nearby is also assigned the closely related hash key 1. Conversely, points which are separated by a considerable distance in the real world are given considerably differing hash keys, for example, the hash key for (4,3) is 37.

        From the example described with reference to Table 3, it can be seen that generating hash keys by “bitwise interleaving” the binary X and Y coordinates preserves “locality”. This example therefore conceptually illustrates the operation of the bitwise interleaving spatial hash algorithm that may be used with representative embodiments of the present invention. However, the above example is based on the simplified integer based coordinate system shown in FIG. 20. In order to understand the actual algorithm that may be used in the implementation of the present control system, it is necessary to take into account certain other complexities. These complexities include:

        The fact that GPS and other similar systems which describe spatial location typically do so using IEEE double-precision floating-point numbers (not simple integers). For instance, GPS supplies coordinates in the form of (X,Y) coordinates where X corresponds to longitude, and Y corresponds to latitude. Both X and Y are given in units of decimal degrees.

        the fact that certain spatial locations have negative coordinate values when described using GPS and other similar coordinate systems. For example, using the WGS84 datum used by current GPS, the coordinates (153.00341,−27.47988) correspond to a location in Queensland, Australia (the negative latitude value indicates southern hemisphere).

        Complexities inherent in representing numbers in accordance with the IEEE double-precision floating-point numbers standard.

        FIG. 21 shows an example vehicle path similar to that shown in FIG. 20, except that the coordinates used to describe the points along the path in FIG. 21 correspond to a “realistic” coordinate system such as that used by current GPS. In order to understand the implementation of the bitwise interleaving spatial hash algorithm when applied to these realistic coordinates, it is necessary to first appreciate certain aspects regarding the way numbers are represented using the standard IEEE double-precision floating-point number format.

        A double-precision floating-point number represented in accordance with the IEEE 754 standard comprises a string of 64 binary characters (64 bits) as shown in FIG. 22. The number is represented in three parts, namely the sign, the exponent and the mantissa. The sign comprises one bit. If the sign bit is 1 then the number is negative, and conversely if the sign bit is 0 then the number is positive. The exponent comprises eleven binary characters, and hence can range from 00000000000 to 11111111111. However, because of the need to represent numbers that are both greater and smaller than one, it is necessary to be able to represent both large positive and large negative values for the exponent. However, it is not desirable to use one of the exponent bits to represent the sign of the exponent because this would leave fewer bits available to represent the exponent's actual value and would therefore greatly limit the size of the numbers that could be represented. Therefore, in the IEEE standard 64 bit format, the true value of the exponent is given by the binary number actually written by the eleven exponent bits minus an implied exponent bias.

        Hence, actual exponent value=written exponent value-exponent bias.

        The exponent bias is 0x3ff=1023. Consequently, the maximum true exponent value that can be represented (written in decimal notation) is 1023, and the minimum true exponent value that can be represented is −1022.

        Finally, the remaining 52 bits form the mantissa. However, as all non-zero numbers must necessarily have a leading “1” when written in binary notation, an implicit “1” followed by a binary point is assumed to exist at the front of the mantissa. In other words, the leading “1” and the binary point which must necessarily exist for all non-zero binary numbers is simply omitted from the actual written mantissa in the IEEE 64-bit standard format. This is so that an additional bit may be used to represent the number with greater precision. However, when interpreting numbers which are represented in accordance with the IEEE standard, it is important to remember that this leading “1” and the binary point implicitly exist even though they are not written.

        Bearing in mind these issues, it is possible to understand the actual spatial hash algorithm used in representative implementations of the present control system. A “worked” example illustrating the operation of the spatial hash algorithm to generate a hash key based on the coordinate (153.0000°, −27.0000° is given in the form of a flow diagram in FIG. 23. The points are initially expressed in terms of decimal degrees as this is the format in which they are delivered from, for example, GPS.

        From FIG. 23 it can be seen that in order to implement the algorithm the X and Y coordinates are separated. The next step is to “normalise” the signs of the respective coordinates (in this case only the Y coordinate needs to be normalized). The reason for normalising the signs of the coordinate is because, when calculating a spatial hash key, it is more convenient to eliminate negative sign bits from the coordinates. In the case of the latitude coordinate, those skilled in this area will recognize that latitude is conventionally written as a number in the range (−90°≤latitude≤90°. Therefore, by simply adding 90° to the value of the latitude coordinate, the spatial hash algorithm can operate with values in the equivalent “un-signed” or “normalised” latitude range (0°≤latitude≤180°). Those skilled in the art will appreciate that the longitude coordinates can also be normalised to fall within the range (0°≤longitude≤360.degree.), although that is not necessary in this example.

        After normalising the coordinates, the next step is to convert the respective coordinates from their representations in decimal degrees into binary IEEE double-precision floating-point number format. This is shown as step 3) in FIG. 23. However, it will be noted that the binary coordinate representations (and all other numbers which are generated or used by the algorithm in binary form) have been written in the alternative hexadecimal notation for ease of reference and to save space in FIG. 23.

        Next, the binary representations of the two coordinates are split into their respective exponent (11 bits) and mantissa (52 bits) portions. This is step 4) in FIG. 23. Then, in order to determine the correct (“true”) value of the exponent, the exponent for each of the coordinate is “de-biased” by subtracting the implicit exponent bias (0x3ff=1023) as described above. This is step 5).

        After de-biasing the exponents, the resulting exponents are then adjusted by a selected offset. The size of the offset is selected depending on the desired “granularity” of the resulting fix-point number. In the particular example shown in step 6) of FIG. 23, the offset is 37, however those skilled in the art will appreciate this number can be varied to suit.

        After adjusting the exponent, the next step is to “resurrect” the leading “1” and the binary point which implicitly exist in the mantissa but which are left off when the mantissa is actually written (see above). Hence, the leading “1” and the binary point are simply prepended to the mantissa of each of the coordinates. This is step 7) in FIG. 23.

        The mantissa for each coordinate is then right-shifted by the number of bits in the corresponding exponent. The exponents for each coordinate are then prepended to their corresponding mantissas forming a single character string for each coordinate. There is then an optional step of discarding the high-order byte for each of the two bit fields. This may be done simply to save memory if required, but is not necessary. Finally, the resultant bit fields for each coordinate are bitwise interleaved to obtain a single hash key corresponding to the original coordinates. In the example shown in FIG. 23, the resultant hash key is 32-bits in length. However, the length of the resultant hash key may vary depending on, for example whether the high-order byte is discarded, etc.

        Those skilled in the art will recognize that various other alterations and modifications may be made to the particular embodiments, aspects and features of the invention described without departing from the spirit and scope of the invention may be made to the particular embodiments, aspects and features of the invention described without departing from the spirit and scope of the invention.

Claims

1. A system for controlling a vehicle, the vehicle including an automatic steering system and roll, pitch and yaw axes, and the control system comprising:

a spatial database containing spatial data corresponding to GPS-defined positions in the region;

a controller mounted on said vehicle and adapted for computing guidance signals, to receive spatial data from the spatial database at control speed, and to control the steering of the vehicle;

a guidance subsystem mounted on said vehicle and connected to said controller, said guidance subsystem being adapted for receiving said guidance signals from said controller and utilizing said guidance signals for guiding said vehicle;

external spatial data sources mounted on said vehicle, comprising at least an optical movement sensor subsystem adapted for optically sensing movement of said vehicle relative to a surface over which said vehicle is traveling;

said optical movement sensor subsystem including an optical movement sensor connected to said controller and adapted for providing optically-sensed vehicle movement signals thereto corresponding to optically-sensed relative vehicle movement;

said optical movement sensor subsystem including an optical movement sensor and an optimal estimator providing a statistically optimal estimate of the position and attitude information received from the optical movement sensor;

said optimal estimator including algorithms that receive the position and attitude information from the optical movement sensor and converts said information into a calculated or determined position and attitude of said vehicle producing a statistically optimal estimate of the calculated or determined position and attitude of said vehicle;

said controller being adapted for computing said guidance signals utilizing said vehicle movement signals;

the controller correlating images from said optical movement sensor subsystem to obtain data relating to the vehicle's motion;

a vehicle reference point located at an intersection of the vehicle roll, pitch and yaw axes; and

the spatial database being adapted to receive updated spatial data from the controller and the external spatial data sources as the vehicle traverses the region.

2. The system for controlling a vehicle according to claim 1, further comprising:

a global navigation satellite system (GNSS) positioning subsystem mounted on said vehicle and adapted for providing GNSS-derived position signals to said controller;

said controller using said GNSS-derived position signals for computing said guidance signals;

said GNSS positioning subsystem including a pair of antennas mounted on said vehicle; and

said antennas receiving GNSS ranging signals corresponding to their respective geo-reference locations.

3. The system for controlling a vehicle according to claim 2, further comprising:

said processor being adapted for computing an attitude of said vehicle using ranging differences between the GNSS signals received by said antennas; and

said GNSS antennas being mounted on said vehicle in transversely-spaced relation.

4. The system for controlling a vehicle according to claim 3, further comprising:

said vehicle including a motive component and an implement connected to said motive component;

a GNSS antenna mounted on said implement and connected to said GNSS receiver; and

said guidance subsystem being adapted for automatically steering said vehicle utilizing said positioning signals to accommodate an offset between said tractor and implement and correct relative positioning of said tractor and implement to maintain said implement on a guide path.

5. The system for controlling a vehicle according to claim 4, further comprising:

said guidance subsystem including an hydraulic steering valve block connected to said controller and to a steering mechanism of said vehicle; and

said guidance subsystem including a graphic user interface (GUI) adapted for displaying a guide path of said vehicle.

6. The system for controlling a vehicle according to claim 5, further comprising:

a GNSS base station including a radio transmitter and a radio receiver;

said vehicle including an RF receiver adapted to receive RF transmissions from said base station; and

a real-time kinematic (RTK) correction subsystem using carrier phase satellite transmissions with said vehicle in motion.

7. The system for controlling a vehicle according to claim 1 wherein said optical movement sensor subsystem includes:

a pair of said optical movement sensors fixedly mounted in spaced relation on said vehicle.

8. The system for controlling a vehicle according to claim 1, wherein said external spatial data sources mounted on the vehicle further comprise:

a GNSS system including an antenna and a receiver;

an inertial navigation system (INS) including a gyroscope and an accelerometer; and

a tilt sensor.

9. A control system as claimed in claim 8, wherein the controller uses the GPS system, the inertial navigation system, the gyroscope, the accelerometer and the tilt sensor to generate a control signal for controlling the vehicle.

10. A system for controlling an agricultural vehicle, the vehicle including an automatic steering system and roll, pitch and yaw axes, and the control system comprising:

a spatial database containing spatial data corresponding to GPS-defined positions in the region;

a controller mounted on said vehicle and adapted for computing guidance signals, to receive spatial data from the spatial database at control speed, and to control the steering of the vehicle;

a guidance subsystem mounted on said vehicle and connected to said controller, said guidance subsystem being adapted for receiving said guidance signals from said controller and utilizing said guidance signals for guiding said vehicle;

external spatial data sources mounted on said vehicle, comprising at least an optical movement sensor subsystem adapted for optically sensing movement of said vehicle relative to a surface over which said vehicle is traveling;

said optical movement sensor subsystem including an optical movement sensor connected to said controller and adapted for providing optically-sensed vehicle movement signals thereto corresponding to optically-sensed relative vehicle movement;

said optical movement sensor subsystem including an optical movement sensor and an optimal estimator providing a statistically optimal estimate of the position and attitude information received from the optical movement sensor;

said optimal estimator including algorithms that receive the position and attitude information from the optical movement sensor and converts said information into a calculated or determined position and attitude of said vehicle producing a statistically optimal estimate of the calculated or determined position and attitude of said vehicle;

said controller being adapted for computing said guidance signals utilizing said vehicle movement signals;

the controller correlating images from said optical movement sensor subsystem to obtain data relating to the vehicle's motion;

a vehicle reference point located at an intersection of the vehicle roll, pitch and yaw axes;

the spatial database being adapted to receive updated spatial data from the controller and the external spatial data sources as the vehicle traverses the region;

a global navigation satellite system (GNSS) positioning subsystem mounted on said vehicle and adapted for providing GNSS-derived position signals to said controller;

said controller using said GNSS-derived position signals for computing said guidance signals;

said GNSS positioning subsystem including a pair of antennas mounted on said vehicle;

said antennas receiving GNSS ranging signals corresponding to their respective geo-reference locations;

said processor being adapted for computing an attitude of said vehicle using ranging differences between the GNSS signals received by said antennas;

said GNSS antennas being mounted on said vehicle in transversely-spaced relation;

said vehicle including a motive component and an implement connected to said motive component;

a GNSS antenna mounted on said implement and connected to said GNSS receiver;

said guidance subsystem being adapted for automatically steering said vehicle utilizing said positioning signals to accommodate an offset between said tractor and implement and correct relative positioning of said tractor and implement to maintain said implement on a guide path;

said guidance subsystem including an hydraulic steering valve block connected to said controller and to a steering mechanism of said vehicle;

said guidance subsystem including a graphic user interface (GUI) adapted for displaying a guide path of said vehicle;

a GNSS base station including a radio transmitter and a radio receiver;

said vehicle including an RF receiver adapted to receive RF transmissions from said base station; and

a real-time kinematic (RTK) correction subsystem using carrier phase satellite transmissions with said vehicle in motion.

11. A method for controlling a vehicle within a region to be traversed, the vehicle including an automatic steering system and roll, pitch and yaw axes, the method comprising the steps:

providing a spatial database;

populating said database with spatial data corresponding to GPS-defined positions in the region;

providing a position error generator;

providing a controller;

mounting said controller to said vehicle;

traversing the region with said vehicle;

receiving spatial data with said controller from the spatial database at control speed;

controlling the steering of the vehicle with the controller as the vehicle traverses the region;

providing the controller with a task path generator;

receiving data from the spatial database with the controller and controller task path generator;

providing the controller with a vehicle attitude compensation module;

mounting external spatial data sources, including at least an optical movement sensor subsystem, on said vehicle and optically sensing movement of said vehicle relative to a surface over which said vehicle is traveling;

said optical movement sensor subsystem including an optimal estimator providing a statistically optimal estimate of the position and attitude information received from the optical movement sensor;

providing said optimal estimator with algorithms that receive the position and attitude information from the optical movement sensor and convert said information into a calculated or determined position and attitude of said vehicle producing a statistically optimal estimate of the calculated or determined position and attitude of said vehicle;

populating said spatial database with ground images from said optical movement sensor subsystem;

inputting said ground images to the controller;

correlating the images with said controller to obtain data relating to the vehicle's motion;

designating and locating a vehicle reference point at an intersection of the vehicle roll, pitch, and yaw axes; and

updating said spatial database with spatial data from the controller and said external spatial data sources as the vehicle traverses the region.

12. The method for controlling a vehicle according to claim 11, further comprising the steps:

providing a global navigation satellite system (GNSS) positioning subsystem mounted on said vehicle and providing GNSS-derived position signals to said controller;

providing said GNSS positioning subsystem with a pair of antennas mounted on said vehicle;

receiving with said antennas GNSS ranging signals corresponding to their respective geo-reference locations; and

computing with said processor an attitude of said vehicle using ranging differences between the GNSS signals received by said antennas.

13. The method for controlling a vehicle according to claim 12, further comprising the steps:

mounting said GNSS antennas on said vehicle in transversely-spaced relation.

14. The method for controlling a vehicle according to claim 12, further comprising the steps:

providing said vehicle with a motive component and an implement connected to said motive component;

mounting a GNSS antenna on said implement and connecting said implement-mounted GNSS antennas to said GNSS receiver; and

said guidance subsystem automatically steering said vehicle utilizing said positioning signals to accommodate an offset between said tractor and said implement and to maintain said implement on a guide path.

15. The method according to claim 11, which includes the additional steps of:

providing said optical movement sensor subsystem with a pair of optical movement sensors; and

fixedly mounting said optical movement sensors in spaced relation on said vehicle.

16. The method for controlling a vehicle according to claim 11, wherein said external spatial data sources mounted on the vehicle further comprise:

a GNSS system including an antenna and a receiver;

an inertial navigation system (INS) including a gyroscope and an accelerometer; and

a tilt sensor.

17. The method for controlling a vehicle according to claim 16, wherein the controller uses the GPS system, the inertial navigation system, the gyroscope, the accelerometer and the tilt sensor to generate a control signal for controlling the vehicle.

18. An apparatus for controlling a vehicle, the apparatus comprising:

a spatial database containing spatial data corresponding to absolute positions in a region; and

a controller, in communication with a single gimbal-mounted optical movement sensor of the vehicle or plural optical movement sensors mounted on the vehicle in transversely-spaced relation, the controller configured to:

convert position and attitude information from the single optical movement sensor or the plural optical movement sensors into a calculated position and attitude of the vehicle, wherein the calculated attitude defines a roll, yaw, and pitch of the vehicle,

steer the vehicle using the calculated position and attitude of the vehicle and the spatial data from the spatial database, and

update the spatial database with updated spatial data as the vehicle traverses the region.

19. The apparatus of claim 18, further comprising an optimal estimator to calculate the calculated position and attitude of the vehicle by calculating a statistically optimal estimate of the position and attitude information received from the single optical movement sensor or the plural optical movement sensors.

20. The apparatus of claim 19, wherein the optimal estimator includes algorithms that receive the position and attitude information from the single optical movement sensor or the plural optical movement sensors and convert the information into the calculated position and attitude of the vehicle by calculating the statistically optimal estimate.

21. A method of controlling a vehicle having a single gimbal-mounted optical movement sensor mounted thereon or plural optical movement sensors mounted thereon in transversely-spaced relation, the method comprising:

converting position and attitude information from the single optical movement sensor or the plural optical movement sensors into a calculated position and attitude of the vehicle, wherein the calculated attitude defines a roll, yaw, and pitch of the vehicle;

steering the vehicle using the calculated position and attitude of the vehicle and spatial data corresponding to absolute positions in a region;

wherein the spatial data is from a database, and the method further comprises updating the database with updated spatial data as the vehicle traverses the region.

22. The method of claim 21, further comprising calculating a statistically optimal estimate of the position and attitude information received from the single optical movement sensor or the plural optical movement sensors.

23. The method of claim 22, further comprising:

receiving the position and attitude information from the single optical movement sensor or the plural optical movement sensors; and

converting the received information into the calculated position and attitude of the vehicle by calculating the statistically optimal estimate.