Steering controller for an autonomous vehicle

A mobile machine having a chassis, a plurality of ground-engaging elements, a plurality of actuators for driving movement of the ground-engaging elements, and a controller for controlling each of the actuators to cause the mobile machine to follow a guidance path along a ground surface. The controller is configured to generate a first set of control values for driving the machine according to a first heading based on a reference heading error of the machine and generate a second set of control values for driving the machine according to a second heading based on a distance heading error of the machine, determine a weight scheme for the first set of control values and the second set of control values dependent on a distance of the machine from the guidance path, combine the control signals using the weight scheme and drive the machine using the combined control signals.

FIELD

Embodiments of the present invention relate to control systems for autonomous machines. More particularly, embodiments of the present invention relate to navigational control systems for enabling autonomous machines to follow guidance paths.

BACKGROUND

Machinery and processes used in the agriculture industry continue to evolve with advancements in technology. Computer technology and Global Navigation Satellite Systems (GNSS), for example, have enabled the use of automated guidance on some agricultural machines. Automated guidance enables a machine to automatically follow a designated path without operator control or input and has improved the efficiency of agricultural operations not only by enabling more precise operation (e.g., less overlap and fewer missed areas), but also by allowing the operator to focus on other areas of the machine's operation, such as user-defined settings that can affect performance and efficiency.

Another technology that is increasingly of interest within the agriculture industry is full machine automation. A machine that is fully automated may be operated without any user input or control and, therefore, may be designed without an operator seat or cabin. Eliminating operator space renders machine design more flexible. A machine with no operator space, for example, may be smaller and simpler than a manned machine. However, the automated control of such machines must be even more robust than automated guidance on manned machines as there is no operator oversight to handle anomalies in operation or to make corrections.

The above section provides background information related to the present disclosure which is not necessarily prior art.

SUMMARY

A mobile machine according to a first embodiment of the invention comprises a chassis, a plurality of ground-engaging elements for supporting the chassis on a ground surface and propelling the chassis relative to the ground surface, each of the plurality of ground-engaging elements being at a fixed angle relative to the chassis, a plurality of actuators for driving movement of the ground-engaging elements, each of the actuators being configured to drive movement of one of the ground-engaging elements independently of the other ground-engaging elements, and a controller for controlling each of the actuators to cause the mobile machine to follow a guidance path along a ground surface.

The controller is configured to generate a first set of control values for driving the machine according to a first heading based on a reference heading error of the machine, generate a second set of control values for driving the machine according to a second heading based on a distance heading error of the machine, and determine a weight scheme for the first set of control values and the second set of control values, the weight scheme depending on a distance error and giving greater weight to the first set of control values with smaller distance errors and giving less weight to the first set of control values with larger distance errors, the distance error being a distance between the machine's location and the guidance path. The controller is further configured to generate control signals for driving the machine by combining the first set of control values and the second set of control values according to the weight scheme, and use the control signals to control the actuators to thereby cause the machine to follow the guidance path.

A mobile machine according to another embodiment of the invention comprises a chassis, a plurality of ground-engaging elements for supporting the chassis on a ground surface and propelling the chassis relative to the ground surface, each of the plurality of ground-engaging elements being at a fixed angle relative to the chassis, a plurality of actuators for driving movement of the ground-engaging elements, each of the actuators being configured to drive movement of one of the ground-engaging elements independently of the other ground-engaging elements, and a controller for controlling each of the actuators to cause the autonomous vehicle to follow a guidance path. The controller is configured to determine a heading adjustment value dependent on a distance of the machine from the guidance path, determine a steering error by subtracting the heading adjustment value and an orientation of the machine from a reference heading, the reference heading being a heading required to follow the guidance path if the vehicle were located on the guidance path, use the steering error to generate steering control signals, communicate the steering control signals to the plurality of actuators to thereby drive the machine to follow the guidance path.

These and other important aspects of the present invention are described more fully in the detailed description below. The invention is not limited to the particular methods and systems described herein. Other embodiments may be used and/or changes to the described embodiments may be made without departing from the scope of the claims that follow the detailed description.

DESCRIPTION

The following detailed description of embodiments of the invention references the accompanying drawings. The embodiments are intended to describe aspects of the invention in sufficient detail to enable those skilled in the art to practice the invention. Other embodiments can be utilized and changes can be made without departing from the spirit and scope of the invention as defined by the claims. The following description is, therefore, not to be taken in a limiting sense. Further, it will be appreciated that the claims are not necessarily limited to the particular embodiments set out in this description.

When elements or components are referred to herein as being “connected” or “coupled,” the elements or components may be directly connected or coupled together or one or more intervening elements or components may also be present. In contrast, when elements or components are referred to as being “directly connected” or “directly coupled,” there are no intervening elements or components present.

Turning now to the drawing figures, and initiallyFIG. 1, an exemplary mobile machine10constructed in accordance with embodiments of the invention is illustrated. The machine10is an autonomous vehicle configured to perform an agricultural operation such as planting seeds. The machine10includes a chassis12, four wheels14, a product holding and distribution system16, and a communications and control system18. The four wheels14are configured to support the chassis12on a ground surface and propel the machine10along the ground surface. Two or more of the wheels14are driven to propel the machine10and each of the wheels14is at a fixed angle relative to the chassis12. In other words, none of the wheels14is configured to pivot from side to side to steer the machine10. Rather, the communications and control system18steers the machine10by rotating driven wheels on different sides of the machine10at different rates—a so-called “skid steer” system. The product holding and distribution system16includes a tank (not shown) for holding seeds to be planted in a field. An applicator (not shown) for applying or distributing the product includes a mechanism for placing seed in the ground.

A block diagram depicting various components of the communications and control system18is depicted inFIG. 2and includes a controller20, two actuators22,24for driving two or more of the wheels14, an actuator26for operating the product holding and distribution system16, a positioning component28, a communications component30, a power source32, and two or more sensors34,36. The actuators22,24for driving the wheels14include electric motors with each electric motor driving one of the rear wheels14. Each of the actuators22,24is driven independently of the other such that the wheels14may be operated at different rotational speeds to thereby steer the machine10. The positioning component28uses a global navigation satellite system (GNSS) or similar technology to determine a geographic position of the machine10. The geographic position of the machine10may be used, for example, to navigate the machine10according to a guidance path and to generate a work record. The communications component30enables the machine10to communicate wirelessly with external devices or entities such as other autonomous machines and/or a central control unit controlling operation of the machine10. The power source32stores energy and energizes the other components of the machine10and may include a rechargeable battery. The sensors34,36collect machine state data, environmental data, or both. Machine state data may include, for example, operating status, fill level of the product holding and distribution system16or inertial movement of the machine10. Environmental data may include, for example, ground surface features or ambient weather information.

The controller20communicates with the other components of the communications and control system18and generally directs operation of the system18and the machine10. The controller20preferably includes a digital integrated circuit and may be or include a general use, commercial off-the-shelf computer processor. Alternatively, the controller20may be a programmable logic device configured for operation with the system18or may be an application specific integrated circuit (ASIC) especially manufactured for use in the system18. While illustrated as a single component in the block diagram ofFIG. 2, the controller20may include two or more separate devices working in cooperation to control operation of the system18. Furthermore, if the controller20includes multiple elements, such as multiple integrated circuits, the elements may be placed at various places on the machine10and in wired or wireless communication with each other.

The machine10may be between one-half meter and one meter in length, between one-fourth and one-half meter in width, and between one-fourth meter and one meter in height. It will be appreciated, however, that principles of the present invention may be used with machines of varying sizes and configurations and that the particular size and configuration of the machine10is not critical. In some applications it is desirable to use a relatively small machine. Advantages of using relatively small, autonomous machines for agricultural operations include reduced cost, improved scalability, less environmental impact, greater reliability and increased productivity.

The use of relatively small, autonomous machines presents advantages for some agricultural operations. By way of example, small autonomous machines cost less to build because they do not require an operator cabin or operator environment and therefore require fewer materials to build and are simpler to design and manufacture. Operations involving small autonomous machines may be scaled from one or two machines to hundreds of machines depending on the size and needs of the operation, and their operation may be more environmentally friendly because they may be electric, consume less energy and avoid soil damage such as soil compaction. These machines are typically more reliable because each machine is smaller and simpler in design than traditional agricultural equipment, resulting in fewer machine malfunctions or failures and less time to repair when a malfunction or failure occurs. Furthermore, if a group of autonomous machines is used for an operation, the failure or malfunction of one or two machines would typically have limited impact on the overall operation as the remaining machines could continue operating. The use of autonomous machines may result in increased productivity as autonomous machines are not limited by operator availability and may work continuous all day and all night.

The controller20is configured to control the machine10to follow a guidance path and to perform an agricultural operation, such as planting seeds, along the guidance path. A guidance path (also referred to herein as a “reference path”), such as the path38illustrated inFIG. 3, may be stored on the machine10or may be communicated to the machine10via the communications component30. The path38is illustrated and described herein on a two-dimensional plane defined by an X axis and a Y axis, as variations in ground surface altitude are negligible and, thus, it may be assumed that the machine10is operating on a flat surface and the third dimension (Z axis) may be neglected. Therefore, the machine's position may be defined by a location (PX, PY) of the machine's center40and an orientation of the machine's velocity vector42.

The guidance path38illustrated inFIG. 3is defined by a series of geographic locations L1-L15, also referred to herein as landmarks, connected by direction vectors44defining a direction of travel from one landmark to the next landmark. For curved paths a larger number of landmarks L results in a more accurate path definition. As the machine10travels along the guidance path38, it uses each direction vector as a navigational reference until it arrives at the next direction vector, then uses that vector as a navigational reference. To determine which of the direction vectors the machine10is to use for its present reference vector and when to switch to the next vector the machine10uses a method that is illustrated inFIG. 4and explained below.

FIG. 4depicts a scenario in which the machine10uses as a navigational reference a direction vector44n, also referred to herein as the reference vector, defined by a previous landmark Llastand a next landmark Lnext. The distance48from the machine10to the next landmark Lnextis determined as the shortest distance between the machine's center40and a vector46passing through the next landmark Lnextthat is orthogonal to the reference vector44n. The distance48from the machine's center40to the next landmark will also be referred to herein as dnlm. The orthogonal vector46may be determined using the reference vector44naccording to equation (1):

dir→ortho=[0-110]·dir→(1)
where {right arrow over (dιr)}orthois the orthogonal vector46and {right arrow over (dιr)} is the reference vector44n. The distance48to the next landmark may be determined using the orthogonal vector46according to equation (2):

dnlm=dir→ortho×(r→p-r→nlm)dir→ortho(2)
Neglecting the Z coordinate, equation (2) simplifies to equation (3):

If the absolute value of the determinant in equation (3) is ignored the value of dnlmis positive or negative depending on the position of the machine10with respect to the orthogonal vector46. Thus, when the sign of dnlmchanges the machine has crossed the orthogonal vector46and the reference vector becomes the next direction vector44n+1.

The distance50from the machine's center40to the reference vector44nmay be determined using a method similar to the method set forth above for finding the distance48to the orthogonal vector46. Replacing the orthogonal vector ({right arrow over (dιr)}ortho) with the reference vector ({right arrow over (dιr)}) and replacing {right arrow over (r)}nlmwith {right arrow over (r)}llmin equation (2) results in equation (4):

d=dirXrpX-rllmXdirYrpY-rllmY(dirXdirY)(4)
where d is the distance50between the machine center40and the reference vector44n.

With reference now toFIG. 5, the machine's reference heading error is illustrated. While the machine10inFIG. 5is positioned on the reference vector44(that is, d=0), the machine's velocity vector42is not aligned with the reference vector44. The machine's reference heading error is the difference between the machine's heading, represented by the heading vector42, and the reference vector44. If an angle of the machine's heading vector42is defined by an angle52between the vector42and the X axis, and an angle of the reference vector44is defined by an angle54between the vector44and the X axis, the reference heading error may be defined as the angle56corresponding to the difference between the angle52and the angle54. Thus, in the scenario ofFIG. 5there is a need to adjust the machine's heading to align with the reference vector44. If the machine10were perfect in adjusting for reference heading errors, simply controlling the machine's heading relative to the reference vector44would be sufficient to keep the machine10on the reference vector. In practice, however, even an efficient controller would not be able to perfectly adjust the machine's heading and the machine10would deviate from the reference vector44such that d≠0.

In the scenario depicted inFIG. 6the machine10is not located on the reference vector44but the machine's heading, represented by the machine's velocity vector42, is parallel with the reference vector44. In other words, the machine10has a distance error (d>0) but does not have a reference heading error. If the controller20were configured to adjust only the machine's heading relative to the reference heading44—and not to correct the machine's distance d from the reference vector44—the machine10would continue to travel along a path parallel to the reference vector44(rather than move toward the reference vector44) in the scenario depicted inFIG. 6because the machine's reference heading error is zero. Thus, the controller20must be configured to reduce the distance d to zero if the machine is going to return to the reference vector44.

A controller configured to correct the distance error d separately from the reference heading error may attempt to follow the shortest line to the reference vector by turning the machine ninety degrees clockwise, driving along the line58, and then when the machine10is on the reference vector44turn ninety degrees counterclockwise so that the machine's heading is once again aligned with the reference vector44. Given that in practice the machine10will almost always be off the reference vector44by at least a small distance, this approach would not be effective. Embodiments of the present invention involve controlling a machine to follow a reference path by considering both reference heading errors and distance errors.

As the machine10follows the guidance path38, its positioning error can be described by two heading errors—a distance heading error and a reference heading error. The distance heading error is the difference between the machine's actual heading, represented by the velocity vector42, and the optimal heading to return the machine to the reference vector44. The optimal heading to return the machine to the reference vector44will also be referred to herein as the optimal distance heading. The distance heading error is ninety degrees in the scenario illustrated inFIG. 6because the optimal heading to return the machine10to the reference vector44would be aligned with the line58. The reference heading error is the difference between the machine's actual heading represented by the velocity vector42and the heading defined by the reference vector44, as explained above. A control method addressing either one of these heading errors—without addressing the other—would be insufficient to navigate the machine10along the path38for the reasons set forth above. Embodiments of the present invention employ a control method that uses both heading errors to generate control signals for driving the machine to follow the guidance path38.

For convenience, reference is made herein to two controllers—a distance heading controller and a reference heading controller. This dichotomy has reference to two control functions, a first that attempts to guide the machine to be aligned with the optimal distance heading and a second that attempts to guide the machine to be aligned with the reference heading. It will be appreciated, however, that the discussion of the present invention in terms of two controllers does not require two distinct controllers to be implemented. In other words it does not require two distinct systems, two distinct devices or even two distinct pieces of software. Principles of operation of the two controllers may be combined into a single controller presenting the functionality of two, combined controllers.

FIG. 7illustrates exemplary interaction between the distance heading controller and the reference heading controller. The distance heading controller seeks to follow path60to reduce the distance d to zero and quickly return the machine10to the reference vector44, while the reference heading controller seeks to follow path62that is parallel with the reference vector44notwithstanding the distance error d. Neither of these controller outputs is, by itself, a practical solution as explained above. Embodiments of the present invention use a combination of the distance heading controller and the reference heading controller to determine a target heading64that is then used to navigate the machine10toward and along the reference vector44that is part of the guidance path38. As illustrated inFIG. 7, the target heading64is a combination of a heading generated by the reference heading controller and a heading generated by the distance heading controller. More particularly, the controller combines the two headings using a weighting algorithm that assigns a weight to each of the headings60and62according to the distance50of the machine10from the reference vector44.

The controller arbitrates the reference heading and the distance heading using a hyperbolic tangent function according to the following equations:
ratiodc=tanh(errslope×|d|)  (5)
ratiohc=1−ratiodc(6)
where ratiodcdenotes the weight of the distance controller and ratiohcdenotes the weight of the heading controller. The additional gain errslopein the argument is a tunable parameter that influences the slope of the hyperbolic tangent curve. Both ratiodcand ratiohcadd up to one so that the total control output is never amplified.

FIG. 8illustrates desired machine headings relative to the reference vector44dependent on the distance d between the machine and the reference vector44and for different values of errslope. The graphs ofFIG. 8illustrate that as the distance d between the machine10and the reference vector increases, the target heading angle increases toward ninety degrees. In other words, as the distance d between the machine10and the reference vector44increases the target heading increasingly points towards the reference vector44.FIG. 9depicts approach shapes for different values of errslope. When the machine10is more than one or two meters from the reference vector44(depending on the value of errslope) it first drives straight toward the reference vector44to reduce the distance error d, then follows a path that gradually turns to follow the reference vector44.

An exemplary competitive-cooperative control structure100is illustrated inFIG. 10and may be implemented using the controller20described above. The control structure100includes various functions or sections referred to herein as modules with the understanding that each of the various modules may be implemented in hardware, software or a combination thereof. Furthermore, two or more of the modules may be combined into a single piece of hardware or software.

During operation the machine's velocity vxand yaw rate ω are detected, and the machine's orientation θ is determined by integrating the yaw rate ω over time. A position determination module102uses the machine's velocity vxand orientation θ to determine the machine's geographic position using equations (7) and (8):
Px(t)=Px(t=0)+∫vxcos θdt(7)
Py(t)=Py(t=0)+∫vxsin θdt(8)

An error computation module104receives data defining the guidance path38, the machine's orientation θ and the machine's position (Px,Py) and determines the distance error d using equation (4) described above. The error computation module104also determines the reference heading error (headerr,rh) and the distance heading error (headerr,d) by comparing the machine's orientation θ with the reference heading and the distance heading. A heading controller module106receives the reference heading error and a distance controller module108receives the distance heading error. The heading controller module106implements a first proportional-integral-derivative (PID) control function to generate a first set of control values (ΔωW,rh) for operating the driven wheels of the machine10to adjust the machine's heading to align with the reference heading. The first set of control values represents a change in the rotational speed of the driven wheel or wheels on the left side and on the right side of the machine10necessary to steer the machine10to align with the reference heading. Similarly, the distance controller module108implements a second proportional-integral-derivative (PID) control function to generate a second set of control values (ΔωW,d) for operating the wheels of the machine10to align the machine's heading with the distance heading. The second set of control values represents a change in the rotational speed of the driven wheel or wheels on the left side and on the right side of the machine10necessary to drive the machine to align the machine with the distance heading.

An arbitration module111receives the distance error d from the error computation module104and generates the weight values ratiodcand ratiohcaccording to equations (5) and (6) as described above. The weight values are used as multipliers to adjust the first and second sets of control values to generate a first set of weighted control values and a second set of weighted control values. The first and second sets of weighted control values are added to generate a combined set of control values, and the combined set of control values is communicated to a drive system110and used to drive the wheels14of the machine10according to the desired machine velocity vx,des.

In the control structure illustrated inFIG. 10a separate PID controller is employed by each of the heading controller module106and the distance controller module108. This has some advantages in that each of the PID controllers may be separately adjusted, however it does render the analytical description of the system using classic signal theory more complex. An alternative implementation is illustrated inFIG. 11that reduces the complexity by applying a single PID controller after the arbitration function rather than applying two separate PID controllers before the arbitration function. If both of the PID controllers have identical parameters, the value ΔωWofFIG. 10can be defined by the following equation:

ΔωW=(kp+kis+kd⁢s)×headerr,rh×ratiohc+(kp+kis+kd⁢s)×headerr,d×(1-ratiohc)(9)
which can be rewritten as

ΔωW=(kp+kis+kd⁢s)×(headerr,rh×ratiohc+headerr,d×(1-ratiohc))(10)
Equation (10) illustrates that the heading errors may first be weighted using the error arbitration module112, and then a single PID controller applied after the heading errors are weighted. For simplicity the PID controller is labeled steering controller114inFIG. 11.

The control structure116illustrated inFIG. 11is simpler and easier to work with than the control structure100illustrated inFIG. 10. The control structure116may be further reduced. The heading controller106and the distance controller108act on headings that are always perpendicular to each other, therefore the target heading of the machine10may be determined as a function of the distance error and the reference heading as illustrated in equation (11):

ΔωW=(kp+kis+kd⁢s)×((headref-π2⁢tanh⁡(-errslope×d))-θ)(11)
Applying equation (11), the control structure depicted inFIG. 11can be reduced to the control structure120depicted inFIG. 12. The control structure120uses the reference heading and the distance d, determines a heading adjustment value dependent on the distance d, determines a steering error by subtracting the heading adjustment value and the orientation θ of the machine from the reference heading, uses the steering error to generate steering control signals, and communicates the steering control signals to the drive system110.

More particularly, using the control structure120, the error computation module122calculates the distance error d and, if necessary, the reference heading headref. It communicates the distance error d and the reference heading headrefto a set steering computation module124. The set steering computation module124multiplies the distance error d by the parameter errslopeand determines the hyperbolic tangent of the product of the values d and errslope. The set steering computation module124then multiplies the result of the hyperbolic tangent function by π/2, and subtracts that result from the reference heading to generate a set heading (headset) value. The machine's orientation θ is subtracted from the headsetvalue to generate a steering error steererr, which is communicated to the steering controller module114to generate the ΔωWvalue that is communicated to the drive system110.

It should be noted that the control structure120includes two feedback loops. The inner loop controls the robot's heading while the outer loop reduces the distance error. Stated differently, the outer loop determines what the machine's heading should be (referred to herein as the “set heading” or headset) while the inner loop controls the robot to follow the set heading. The outer loop is not a classic linear controller as its behavior follows a defined nonlinear function. The error slope (errslope) is associated with the outer loop and influences the shape of the approach path followed by the outer control loop, as illustrated inFIGS. 8 and 9.FIG. 8illustrates how the set heading changes according to the distance of the machine from the guidance path with different error slope values. Larger error slope values result in greater accuracy in that they enable the machine to reach the guidance path more quickly, but they also result in turns with smaller turning radii (that is, sharper turns) closer to the guidance path. This is also illustrated inFIG. 9, which charts machine paths corresponding to the different error slope values ofFIG. 8. Smaller turning radii can result in less stability, as explained below.

It should be noted that it is not necessary for the machine to have a forward velocity to correct heading errors because the machine, being a skid-steer machine, can turn (that is, change its heading) when forward velocity is zero. Thus, the machine's forward velocity may be reduced to avoid or mitigate the accumulation of distance errors without impeding the machine's ability to correct for heading errors. The forward velocity can even be reduced to zero to execute a turn with a turn radius of zero (in other words, a turn corresponding to a sharp corner).

The controller may use one or more internal values as indicators to determine when the machine's speed should be adjusted. By way of example, the value of the steering error steererr(see, e.g.,FIGS. 11 and 12) exceeding a particular threshold may indicate that the controller is unable to adjust the machine's heading error quickly enough to keep up with the changing set heading. In that situation the distance error (the distance of the machine's location from the reference path) may continue to increase if the machine's heading cannot be corrected quickly enough. Therefore, in that situation it is desirable to temporarily reduce the machine's forward velocity to allow the controller to correct the machine's heading error before the distance error grows larger. As the machine10slows down the controller continues correcting the machine's heading error. As the heading error approaches the reference heading the controller may increase the machine's forward velocity to the desired velocity. If a large heading error is detected the controller may reduce the forward velocity of the machine to zero while the heading error is corrected. This situation may occur, for example, when the machine10reaches a sharp bend (corner) in the reference path.

A control structure130according to an embodiment of the invention is illustrated inFIG. 13and includes a velocity regulation module132. In the control structures100,116and120the drive system receives the desired machine velocity vx,desand uses vx,desas the target velocity without regulating or adjusting the target velocity. The velocity regulation module132of the control structure130receives the target velocity vx,desand regulates it using the steering error (steererr) value according to the following equation:
vx,c=vx,des×(0.5×(1.0−tanh(velslope×(|steererr|−velswitch,err))))  (12)
Where vx,cis the current velocity communicated to the drive system110, velswitch,erris an error threshold and velslopeis a design parameter. It may not be desirable to regulate the velocity at all when the steering error is relatively small. Hence, the error threshold value velswitch,erris used to determine when the velocity regulation is engaged. By way of example, the value of velswitch,errmay be between five degrees and forty degrees. In some embodiments of the invention the value of velswitch,errmay be about fifteen degrees. The hyperbolic tangent function (tanh) provides for smooth changes in the machine's velocity and avoids abrupt changes that may occur when the steering error crosses the threshold value without the use of the hyperbolic tangent function. It was found in testing a particular prototype that a value for velslopeof 6.85 was effective, but different values of velslopewill be optimal for different machines and in different applications.

Because the controller has guidance path information defining the guidance path, it can anticipate a need to adjust the machine's forward velocity. This is beneficial because in practice machines are typically unable to stop immediately such as, for example, where there is a sharp bend or corner in the guidance path. In that situation by the time the controller reacts to the change in heading error and stops the machine10, the machine10would have passed the bend and left the guidance path. Anticipating the bend in the guidance path and reducing the machine's velocity in advance of reaching the bend helps to avoid the problem of overshooting the bend. This approach also takes advantage of the machine's ability to execute a zero radius turn. On very sharp bends or corners, for example, the machine's velocity may be reduced to zero while the machine's heading is adjusted.

Equation 13 is an algorithm that may be used by the machine to implement the feed forward velocity adjustment:

In this equation there are two hyperbolic tangent functions. The first hyperbolic tangent function ensures that the feed forward velocity management is used only if the angle of the upcoming turn in the guidance path exceeds the threshold ffth,ang. In other words, if the angle of the upcoming turn in the guidance path is small (less than the threshold ffth,ang) there is less need to use the feed forward velocity management and the velocity regulation module132primarily uses the steering error (steererr) to regulate the velocity. The first hyperbolic tangent function softens the engagement of the feedforward steering regulation.

The second hyperbolic tangent function in equation (13) regulates the machine's forward velocity as a function of the distance remaining to the turn in the guidance path. Because the specified velocity vx,desis in the denominator of the slopedist,rem/vx,desterm, the slope of the hyperbolic tangent function changes according to the designated velocity. More particularly, the higher the designated velocity, the smaller the slope.FIG. 15illustrates velocity curves resulting from equation (13) at various designated velocities. By comparison,FIG. 16illustrates the velocity curves in the same situations as inFIG. 15except that the controller does not use a slope term with the designated velocity in the denominator. As can be seen fromFIG. 16, if the slope is not adjusted for the designated velocity, the velocity decreases rapidly within the last one to two meters, perhaps requiring active braking which wastes energy.

In order for the machine to have the same velocity within a close proximity to the turn the slope of the hyperbolic tangent curve is changed for different initial velocities of the machine. The slope of the hyperbolic tangent curve, then, is a design parameter. Exemplary skeleton computer code for implementing velocity regulation as described herein is set forth in Table 1, below.

The control structure120can become unstable in situations where the error slope (errslope), the machine's velocity, or both are relatively high. This can happen where the set heading defined by the outer loop of the cascading control structure changes at a rate faster than the machine's maximum yaw rate. In other words, the outer loop in the control structure120defines a path that the inner loop cannot keep up with. When that happens, the machine overshoots a bend in the defined path. When the controller attempts to correct the overshoot it may define another path with a heading change that exceeds the machine's maximum yaw rate, causing the machine to again overshoot the defined path. This may lead to the machine oscillating around the guidance path, even if the guidance path is a straight line.

As illustrated inFIGS. 8 and 9, larger values of the error slope result in larger machine heading changes closer to the guidance path. Additionally, the greater the machine's velocity the more quickly it must change its heading to keep up with changes in the set heading during the turn. For these reasons the machine10is most susceptible to this instability at higher values of the error slope and higher machine velocities. Adjusting the machine's velocity can mitigate this problem, as explained above, but solving the problem by reducing the machine's velocity may not always be desirable. Therefore, the present invention includes a solution to this problem that involves dynamically adjusting the value of the error slope independently of the velocity. With reference again toFIGS. 8 and 9, the controller may use the machine's maximum yaw rate to adjust the error slope so that the machine's turn is more gradual and the heading change never exceeds the machine's maximum yaw rate. Embodiments of the present invention may employ both velocity adjustment and error slope adjustment. Embodiments of the invention automatically adjust the error slope to avoid instability while maintaining optimal tracking performance by maintaining the error slope as high as possible without sacrificing stability.

The set heading is defined as

∂Θset∂t,
which (applying the chain rule) is equivalent to:

∂Θset∂t=∂Θset∂d⁢∂d∂t(14)
As the mobile machine is located a distance d from the reference path, according to the control algorithm, the desired set heading for the robot may be computed using:

θset=(π2-π2⁢tanh⁡(d×errslope))(15)
Assuming that the mobile machine can respond immediately and perfectly to changes in the set heading (that is, θ=θset), the distance d changes with time according to:

∂d∂t=-vx⁢⁢cos⁢⁢θset(16)
Using equations (14), (15) and (16), the change rate of the set heading over time can be defined as:

∂Θset∂t=π2⁢errslope⁡(1-tanh2⁡(d×errslope)×vx⁢⁢cos⁢⁢(π2-π2⁢tanh⁡(d×errslope)))(17)
This function is plotted inFIG. 17for a constant forward velocity of one meter per second and different values of the error slope (errslope), and inFIG. 18for an error slope of 1.0 and different speeds.FIGS. 17 and 18illustrate that for each value of the error slope there is a peak rate of change of the machine's heading. The larger the value of the error slope the higher the peak and the closer it is to the reference path.

If the error slope remains constant the controller must use the error slope value with a peak that does not exceed the mobile machine's maximum yaw rate to preserve stability. As can be seen from the graphs depicted inFIG. 17andFIG. 18, if the peak rate of change is limited to the mobile machine's maximum yaw rate the mobile machine will be operating below its maximum yaw rate for most of the path toward the reference path. This approach would not take full advantage of the mobile machine's turning ability and may not be the most effective approach for balancing stability and accuracy.

In some embodiments of the invention the controller makes use of an adaptation algorithm to adjust the error slope to a maximum allowed value after the machine has passed the peak yaw rate and according to the machine's current speed and distance from the reference path.FIG. 19illustrates the ideal or theoretical error slope value where an initial error slope value of 0.63 is used to determine the set heading until the mobile machine has passed the peak yaw rate for that error slope value (0.84 meters from the reference path), at which time the error slope value is adjusted to maintain the yaw rate at the maximum yaw rate corresponding to the error slope value of 0.63 until the maximum value of the slope is reached.FIG. 20illustrates actual error slope values calculated by the controller dynamically using the approximations explained herein. This adaptation algorithm allows the mobile machine to reach the reference path as quickly as possible while remaining stable.

The change of set heading may be approximated by equation (18) as follows:

∂Θset∂t≈(-103⁢vx⁢errslope3)×(d-12⁢errslope)2+(56⁢vx⁢errslope)(18)
It should be noted that this approximation may not be accurate for portions of the path not between the point on the path corresponding to the peak yaw rate and the reference path, but that is not a problem here as the approximation is only used for portions of the path corresponding to the point on the path between the peak yaw rate and the reference path. Assuming the maximum yaw rate of the mobile machine (or inner loop) is π/6 radians per second, the maximum error slope may be determined by resolving equation (18) into the error slope according to equation (19) as follows:

103⁢vx⁢d2⁢errslope3-103⁢vx⁢derrslope2+θ.max=0(19)
This cubic equation without a linear term can be solved using Cardano's method. A cubic equation of the form ax3+bx2+cx+d=0 (where a≠0) always incorporates three real or one real and two complex roots. The general cubic equation is transformed into the reduced cubic equation y3+3py+q=0, with p=3ac−b2and q=2b3−9abc+27a2d. The discriminant D is computed as D=q2+4p3. If D is less than zero there exist three different solutions, and the intermediate sizes are given as follows:

x1=y1-b3⁢a(23)x2=y2-b3⁢a(24)x3=y3-b3⁢a(25)
As we are concerned only with the path between the peak yaw rate and the reference path, the discriminant D is always less than zero and the desired solution is always x3. Exemplary skeleton computer code for implementing the adaptation algorithm is set forth in Table 2, below. The only input into the adaptation system is the actual distance error relative to the reference path and the forward velocity of the mobile machine10. The maximum error slope and the maximum yaw rate are predefined.

Although the invention has been described with reference to the preferred embodiment illustrated in the attached drawing figures, it is noted that equivalents may be employed and substitutions made herein without departing from the scope of the invention as recited in the claims. By way of example, while the machine10has four wheels including two driven wheels, other configurations are within the ambit of the present invention. The machine10may include four driven wheels, or a total of three wheels including two driven wheels and a single caster wheel. In yet another embodiment, the machine10may include track assemblies instead of wheels, such as one track assembly on each side for a total of two track assemblies. The machine10may be configured to perform any of various different agricultural operations including, without limitation, planting seeds, applying fertilizer and applying pesticide or herbicide.

Having thus described the preferred embodiment of the invention, what is claimed as new and desired to be protected by Letters Patent includes the following: