Note : Les descriptions sont présentées dans la langue officielle dans laquelle elles ont été soumises.
CA 03113336 2021-03-18
WO 2020/076427
PCT/US2019/048136
AUTOMATED REVERSE IMPLEMENT PARKING
The present application claims priority to U.S. Provisional Patent Application
Ser. No.
62/742,671 filed on October 8, 2018, entitled: AUTOMATED REVERSE IMPLEMENT
PARKING, which is incorporated by reference in its entirety. The present
application is also a
continuation-in-part of U.S. Patent Application No. 16/277,569, filed February
15, 2019; which
is a continuation of U.S. Patent Application Ser. No. 15/345,792 filed
November 8, 2016,
entitled: SINGLE-MODEL IMPLEMENT STEERING, now U.S. Patent No. 10,239,555;
which claims priority to U.S. Provisional Patent Application Ser. No.
62/257,396 filed on
November 19, 2015, entitled: PASSIVE, CLOSED-LOOP, SINGLE-MODE IMPLEMENT
STEERING which are all incorporated by reference in their entireties.
TECHNICAL FIELD
[0001] One or more implementations relate generally to automated reverse
implement parking.
BACKGROUND
[0002] For many agricultural applications, the task performed by the implement
is of
paramount importance. Whether it be for planting, fertilizing, grading or
harvesting, performing
these tasks accurately is essential for high yields and minimal wastage. These
tasks are performed
by specific agricultural implements attached to a vehicle. Typically, these
implements can be
classified as hitched, trailing implements that are towed by a vehicle and are
free to rotate about
a hitch point.
[0003] Following field operations, these trailed implements are moved to a
storage area, where
they either remain attached to the vehicle, or detached. These vehicle/trailer
systems are typically
parked by reversing into a parking area, where the implement is precisely
positioned at a desired
location. Unfortunately, it is difficult to back up an agricultural vehicle
with an attached
implement into a precise location and position.
[0004] When driven manually, the forward motion of a vehicle/trailer system is
stable, with the
implement naturally and predictably converging behind the vehicle over time.
When driven in
reverse however, the vehicle/trailer system is inherently unstable, where
small disturbances in
position can potentially lead to unpredictable and dangerous trailer
movements, increasing the risk
of collision and jackknifing. Steering a vehicle/trailer system in reverse for
an inexperienced driver
1
CA 03113336 2021-03-18
WO 2020/076427
PCT/US2019/048136
can also be counter intuitive, with opposite control needed in the vehicle to
steer the trailer in the
correct direction.
BRIEF DESCRIPTION OF THE DRAWINGS
[0005] The included drawings are for illustrative purposes and serve to
provide examples of
possible structures and operations for the disclosed inventive systems,
apparatus, methods and
computer-readable storage media. These drawings in no way limit any changes in
form and detail
that may be made by one skilled in the art without departing from the spirit
and scope of the
disclosed implementations.
[0006] Figure 1 shows an example of dual-mode implement steering.
[0007] Figure 2 shows an example of a single-mode implement steering.
[0008] Figure 3 shows an example of a single-mode implement steering on a
curved path.
[0009] Figure 4 shows an example guidance system that calculates steering
commands for
executing single-mode implement steering.
[0010] Figures 5 and 6 shows vehicle/trailer models.
[0011] Figure 7 shows an example system for measuring implement position.
[0012] Figure 8 shows an example system for predicting implement position.
[0013] Figure 9 shows an example process for passive closed-loop single-mode
implement
steering.
[0014] Figure 10 shows how single-mode implement steering may steer a vehicle
in a reverse
direction.
[0015] Figure 11 shows an example single-mode implement steering in reverse
onto a circular
path of non-zero curvature.
[0016] Figure 12A-12D show how the guidance system performs a reverse parking
operation.
[0017] Figures 13A-13E show how the guidance system performs a reverse parking
operation
when the trailer is too close to a parking area to start in a reverse
direction.
2
CA 03113336 2021-03-18
WO 2020/076427
PCT/US2019/048136
[0018] Figure 14 describes in more detail steering operations performed in
Figures 13A-13E.
[0019] Figure 15 shows an example guidance system for performing single-mode
implement
steering.
[0020] Figure 16 shows in more detail the guidance system of Figure 15.
DETAILED DESCRIPTION
[0021] A vehicle guidance system steers uses a reverse parking algorithm to
combine closed-
loop implement steering with vehicle throttle/speed control to maneuver and
stop a vehicle and
trailed implement into a desired position.
[0022] Instead of first aligning the vehicle over the path, the vehicle
guidance system first
aligns the implement over the path by minimizing implement positional error
relative to the path.
In one example, the guidance system uses a passive closed-loop single-mode
implement steering
scheme rather than first switching between a vehicle steering control mode and
a second
implement steering control mode.
[0023] The single-mode steering scheme may use both the implement and vehicle
positions
and orientations relative to the path. The guidance system may obtain the
implement position
and orientation using two different methods. A first method places sensors on
the implement
itself (GPS and inertial sensors) and compares their relative positions and
orientations to the
vehicle. The vehicle is also fitted with sensors to monitor its own position
and orientation. The
second method predicts implement position and orientation based on known
vehicle states and
implement geometries.
[0024] Single-mode steering may manage implement errors from engagement,
regardless of
the current position of the implement relative to a way line. This is
different from alternative
implement steering strategies such as dual-mode steering where a steering
controller initially
places the vehicle online. The dual-mode steering controller then waits for
the implement to
naturally converge onto the way line, generally a time consuming process,
depending on the
length and configuration of the implement. Once the implement has neared the
desired path
within a suitable threshold (typically an implement cross-track error
threshold), the controller
switches to a separate implement steering controller to make the final
correction and manage
small variations in implement position. Acquisition performance of dual-mode
steering is slow,
3
CA 03113336 2021-03-18
WO 2020/076427
PCT/US2019/048136
and may consume a significant amount of time and distance along the desired
path before starting
implement control.
[0025] Figure 1 illustrates the principles behind dual-mode implement
steering. Initially in
stage 1, vehicle 100 and implement 104 are offset from a desired path 110. A
dual-mode
controller 102 engages and steers vehicle 100 onto line 110 until acquisition
is completed at stage
2. At stage 3, vehicle 100 is in-line with path 110; however, implement 104
has not yet reduced
a position error relative to path 110 enough for initiating a second implement
steering mode.
[0026] Once implement 104 finally reaches a desired position error threshold,
the dual-mode
controller 102 switches into a second implement steering mode, forcing vehicle
100 to perform a
final correction maneuver in stage 4 to eventually place implement 104 in-line
with path 110 in
stage 5. For example, during the second steering mode, controller 102 may
start reading position
signals from a GPS receiver on implement 104 to determine a distance of
implement 104 from
path 110 and steer vehicle 100 to reduce the position error of implement 104
with path 110.
[0027] Figure 2 shows how a single-mode implement steering scheme places
implement 104
onto desired path 110 quicker and more accurately than the dual-mode
acquisition scheme in
Figure 1. Initially at stage 1, vehicle 100 and implement 104 are offset from
desired path 110. A
single-mode guidance system 120 is engaged at stage 1 and immediately starts
steering vehicle
100 so that implement 104, instead of vehicle 100, first aligns with path 110.
In other words,
instead of initially reducing the position offset of vehicle 100, guidance
system 120 immediately
starts steering vehicle 100 through stages 2 and 3 to reduce a positional
offset of implement 104
relative to path 110.
[0028] During stages 2 and 3, guidance system 120 may intentionally cause
vehicle 100 to
overshoot path 110 aggressively bringing implement 104 in-line with path 110.
This is contrary
to the dual-mode steering in Figure 1, which waits for vehicle 100 to first
converge with path 110
before then aligning implement 104 with path 110. The aggressive attack angle
taken by vehicle
100 while traveling toward and over path 110 more quickly aligns implement 104
and vehicle
100 with path 110 at stage 4 and uses fewer steering stages than the dual-mode
steering shown in
Figure 1.
[0029] Figure 3 shows how the single-mode steering scheme more quickly and
accurately
tracks an implement along a circular or curved path 130. At stage 1 vehicle
100 and implement
104 are again offset from curved path 130. In state 2, guidance system 120
engages and steers
4
CA 03113336 2021-03-18
WO 2020/076427
PCT/US2019/048136
vehicle 100 to reduce a positional error of implement 104 relative to path
130. Similar to the
straight path in Figure 2, guidance system 120 may cause vehicle 100 to
overshoot path 130 to
more quickly reduce the positional error of implement 104 relative to path
130.
[0030] Guidance system 120 detects or predicates implement 104 aligned over
curved path 130
in stage 3. For example, the guidance system 120 may receive GPS signals from
a GPS receiver
(not shown) mounted on implement 104 or may calculate a predicted position of
implement 104
based on vehicle and implement parameters as described in more detail below.
After aligning
implement 104 with curved path 130, guidance system 120 holds vehicle 100 in a
steady turn
radius so implement 104 remains in a same aligned position with curved path
130.
[0031] Guidance system 120 may maintain a position and heading offset 132
between vehicle
100 and curved path 130 to keep implement 104 in-line with curved path 130.
Offset 132 could
be problematic for dual-mode controllers that first place vehicle 100 in-line
with path 130 in a
first mode before waiting for implement 104 to converge with path 130 in the
second mode. If a
steady-state position of implement 100 is outside a switching threshold, the
dual-mode controller
may remain fixed in the first vehicle alignment mode and never switch to the
second implement
alignment mode.
[0032] Figure 4 shows in more detail guidance system 120 that controls
automated passive,
closed-loop, single-mode implement steering. Referring to Figures 1-4 and more
specifically to
Figure 4, vehicle sensors 150 are located on vehicle 100 and may generate
vehicle state data 154.
Implement sensors 152 are located on implement 104 and may generate implement
state data 156.
[0033] Vehicle sensors 150 and implement sensors 152 may include any
combination of global
positioning system (GPS) receivers and inertial sensors, such as gyroscopes
and accelerometers.
Vehicle sensors 150 may generate any combination of navigation signals that
identify a state of
vehicle 100, such as latitudinal and longitudinal positions, heading, speed,
steering angle, pitch,
roll, yaw, etc. Implement sensors 152 generate similar state information for
implement 104.
[0034] A navigation processor 158 may aggregate vehicle state data 154 and
implement state
data 156 to derive position and heading data and other navigation information
for vehicle 100 and
implement 104. Navigation processor 158 also may include a computer with a
computer screen
that a user accesses to perform path planning such as inputting a desired set
of way lines defining
a path over a field.
CA 03113336 2021-03-18
WO 2020/076427
PCT/US2019/048136
[0035] A single-mode implement steering controller 162 receives the reference
path, positional
data for vehicle 100 and positional data for implement 104 from navigation
processor 158.
Controller 162 calculates error/distances of vehicle 100 and implement 104
relative to path 110.
For example, controller 162 may calculate a vehicle heading error, an
implement heading error,
and an implement cross-track error relative to the stored path entered by the
user.
[0036] Controller 162 generates steering commands 164 based on the derived
vehicle and
implement error values. Steering commands 164 are sent to a vehicle steering
and speed control
system 166 that steers and controls the speed of vehicle 100 according to the
single-mode tracking
scheme to more quickly and accurately align implement 104 onto the target path
as described
above in Figures 2 and 3.
[0037] In one example, controller 162 may perform single-mode implement
steering using only
vehicle state data 154 from vehicle sensors 150. In this example, controller
162 may use predicted
error values for implement 104.
[0038] In one example, navigation processor 158 and single-mode implement
steering
controller 162 are functional delineations within guidance system 120. For
example, the same or
different processing devices in guidance system 120 may perform any
combination of operations
in navigation processor 158 and steering controller 162. For example, a first
set of software
executed in one or more processing devices located on vehicle 100 may
implement navigation
processor 158 and a second set of software executed by the same or different
combination of
processing devices may use single-mode controller 162.
Kinematic Vehicle/Trailer Model
[0039] Figure 5 shows a vehicle/trailer model (geometry) and Figure 6 shows a
vehicle/trailer
model (states). Implement 104 in Figures 1-3 is alternatively referred to
below as a trailer.
Kinematic models may only use spatial and geometric properties to describe the
motion of a
system, and may not consider causal forces such as friction and weight to
explain those behaviors.
Kinematic models may provide an idealized view of the motion and interactions
between
components within the system, and generally provide good representations of
system dynamics.
[0040] Figures 5 and 6 illustrate the geometric representation of a vehicle-
implement system.
In this system, Li denotes vehicle wheelbase, L2 the trailer length, and c
represents a hitch length
of the vehicle behind a control point. In this model, it is assumed that the
vehicle control point is
at the center of the rear axle. The equations of motion that govern this
system are as follows:
6
CA 03113336 2021-03-18
WO 2020/076427
PCT/US2019/048136
cas 11.60 (2.1)
= Vst
(2.3)
= 7¨V, tan
=
¨ (2.4)
(I)
(2.5)
r
(2.6)
(2.7)
yt = sin ( ¨ .L2 sin 1.00
where
0 = taTI-1 (t'tmgM) (2.8)
L
= =
= mx (0) (2.9)
[0041] For this system, the terms x and y represent the position of the
vehicle control point in
the local frame, titv represents the heading of the vehicle, t represents the
trailer heading, F
represents the articulation angle of the vehicle (the heading difference
between the vehicle and
trailer), and xt and yt represent the position of the trailer in the local
frame. The terms V and 65,
represent the speed and steering angles of the vehicle respectively, and are
the system control
inputs.
[0042] One difference between a vehicle and vehicle/trailer system is the
additional states for
trailer heading vt , articulation angle F, and trailer position xt and yt. The
behavior of the
trailer when the system is in motion is characterized by these states, and is
influenced by the
trailer geometry.
System Linearization
[0043] From a control perspective, the states to be managed when controlling
the vehicle/trailer
system are vehicle heading, trailer heading, and trailer cross-tracker error.
The parameters are
therefore linearized about these states so a suitable plant can be formulated
to form the basis of
the controller design. To do so, non-linear definitions are determined for
vehicle and trailer
heading rates:
(2.10)
= Kt
7
CA 03113336 2021-03-18
WO 2020/076427
PCT/US2019/048136
=,:v, ....
(2.11)
where
tiai(4.0 (2.12)
- tan.-11.seicv) (2.14)
V sec (0)
[0044] Applying small angle approximations to Equations 2.10 to 2.14 results
in the following
linearized system:
vt, I644 (2.15)
11,t eKto (2.16)
[0045] In the design of the single-mode controller, it is more applicable to
represent the
relevant vehicle and trailer states as functions of error states as the
controller may act as a
regulator (reference of zero). Consequently, the system can be expressed as:
(2.17)
(2.18)
La La
Veio. (2.19)
where ewv denotes vehicle heading error, evt is trailer heading error, ecit is
trailer cross-track,
and ekv is vehicle curvature error.
[0046] Heading errors refer to the difference in heading between the vehicle
and trailer relative
to the desired path. If the vehicle or trailer is travelling parallel with the
desired path, their
respective heading errors will be zero. Cross-track error refers to the
lateral position offset of the
trailer to the desired path. If the trailer is either left or right of the
path, the cross-track will be
non-zero. The trailer is travelling on-line when both the heading errors and
cross-track errors are
zeros. Vehicle curvature error is the amount of curvature demand applied by
the vehicle to steer
the vehicle onto the desired path.
8
CA 03113336 2021-03-18
WO 2020/076427
PCT/US2019/048136
[0047] In state-space form, the system can be expressed as:
0 01 ''f.'N& V
Jv V. V
et:t. ri' z = 3:'.A '+' "" 1:27 ex.t, (2.20)
::=st = 0 V 0 edi I
which is in the linear state-space form:
Ths, (2.21)
Controller Gains
[0048] When designing controllers using the pole-placement technique, a
desired characteristic
equation is defined. The following closed-loop characteristic equation was one
example selected
for the system described in Equation 2.20.
I' (s) razz( +) w + (2.22)
here coh and co/ define the desired high and low frequencies pole locations,
with defining the
damping factor. The desired characteristic equation is third order to
accommodate the three states
in the system to be controlled. Expanding the expression and grouping the
polynomial into
coefficients of s yields:
fp e$3=3 + (.26:ok = ........... 82 + + + 4.44 (2.23)
[0049] In state-space, the desired controller can be expressed as:
(2.24)
where
K;d1 (2.25)
represents a vector of controller gains acting on the system states expressed
in Equation 2.20.
Substituting Equation 2.24, the closed-loop system can be expressed as:
u
A.+ BK.. x (2.26)
3.
9
CA 03113336 2021-03-18
WO 2020/076427
PCT/US2019/048136
[0050] The closed-loop characteristic equation for this system can be found by
calculating the
determinant of the following transfer function realization:
e
41,µi.2(s) ¨ P ¨ (.4 4-, BR)] ¨1 (2.27)
As a result, the closed-loop characteristic equation expressed in coefficients
of s is:
. al $2 +12.2, + Q.3 (2.28)
where
(2.29)
ai) = I
v
itKi. ¨ ti9c2 + 7,7 (2.30)
(2.31)
(2.32)
[0051] By equating the coefficients ao to a3 with the coefficients of s in
Equation 2.23, it is
possible to evaluate the controller gains in K as a function of desired closed-
loop pole locations
and system geometries. Expressed in matrix form, the coefficients can be
equated to: [
co 1 z=:, .,.:
k (2.33)
1..; 17 ¨ 7::;" i -11 2 --= wf,, zAokstp.4.4
44;4
Solving Equation 2.33 yields:
(2.34)
.,...;.,= ' .
- ',.: ' '3 '' . - = ' 4''''.4 '''$ I:L.0k,, K
3`,... ) (2.35)
K2 ¨
.3e...1h.a....
(2.36)
which are the gains acting on the system states to formulate the control input
u (Equation 2.24),
which in this case is the vehicle curvature error eicv =
[0052] The single-mode implement steering controller is driven by three error
states - vehicle
heading error ewv, , trailer heading error evt , and trailer cross-track error
ecit . Management of
these error states allows for the formulation of the demanded vehicle
curvature for the system,
expressed generally as:
CA 03113336 2021-03-18
WO 2020/076427
PCT/US2019/048136
[ ep,
K2. K.31 g::4 + Kim j ed,dt (2.37)
ett,
where eKv is the demanded vehicle curvature error used for steering the
vehicle.
[0053] The terms Ki , K2, K3, expressed in Equations 2.34, 2.35 and 2.36,
define the
evaluated controller terms. They are calculated to manage each respective
error state, and are
automatically adjusted based on vehicle speed to maintain consistent implement
acquisition and
online performance across the operational speed range. The term Kint is an
integral term that
acts to minimize steady-state implement cross-track error.
Constraint Management
[0054] A consideration when controlling a vehicle/trailer system is managing
constraints,
namely, the articulation angle between the vehicle and trailer. As the vehicle
maneuvers, the
trailer pivots about the hitch point, altering the angle it makes with the
vehicle. If the vehicle
happens to steer too aggressively, the potential exists for the trailer angle
to increase to a point
that the system jackknifes, causing the trailer to collide with the vehicle.
[0055] To prevent this scenario, the single-mode controller monitors the
articulation angle F,
where the rate calculation is described in Equation 2.5. To ensure that the
system does not
jackknife, upper and lower limits for the articulation angle are obtained
either through physical
measurements or calibration such that:
Irmin < :r < rma, (2.38)
where Frn in is the lower articulation angle limit and Fmax is the upper
articulation angle limit.
Any demanded vehicle curvature error generated by the controller, calculated
in Equation 2.37,
may use these limits for implement steering.
CONTROL APPLICATIONS
[0056] As explained above, passive, closed-loop, single-mode implement
steering may use
implement navigation states (position, speed, heading and yaw rate) for more
efficiently locating
an implement onto a path. The implement navigation states may be obtained
using either a
measured implement scheme or a virtual implement scheme.
11
CA 03113336 2021-03-18
WO 2020/076427
PCT/US2019/048136
[0057] Figure 7 shows on example system that uses the measured implement
scheme. Vehicle
100 may include the guidance system 120 described above for processing vehicle
and implement
navigation data and generating steering commands for steering vehicle 100.
Guidance system
120 may include a central processor and vehicle inertial sensors 150B. A GPS
receiver 150A
also may be installed on vehicle 100. Inertial sensor 150B and GPS sensor 150A
may generate
the navigation states for vehicle 100 such as, position, speed, heading and
yaw rate, etc.
[0058] An implement GPS receiver 152A and implement inertial sensors 152B are
installed on
implement 104. GPS receiver 152A and inertial sensors 152B may generate and
send navigation
states for implement 104 to guidance system 120 via wired or wireless
connections.
[0059] Guidance system 120 uses the vehicle navigation data from GPS 150A and
inertial
sensor 150B and the implement navigation data from GPS 152A and inertial
sensor 152B to
generate a steering control solution using Equation 2.37. The formulated
control solution is sent
to vehicle steering system 166 in Figure 4 to steer implement 104 onto the
desired path as shown
above in Figures 2 and 3.
[0060] One advantage of the measured implement scheme in Figure 7 is that
navigation states
of implement 104 are measured directly, allowing accurate steering control
while also managing
disturbances. For example, the direct measurements from implement GPS sensor
152 and inertial
sensor 152B allow guidance system 120 to compensate for ruts, rocks, or any
other obstruction
that may move implement 104 off of the target path. For example, guidance
system 120 may
adjust the steering commands via Equation 2.37 to more quickly move implement
140 back onto
the target path. The measured implement scheme uses the implement navigation
data to perform
terrain compensation and disturbance management for high accuracy control on
level and sloped
terrain.
[0061] Figure 8 shows a virtual implement system for executing the virtual
implement scheme.
Inertial sensors 150 and GPS receiver 150B are still mounted on vehicle 100.
However,
implement GPS receiver 152A and implement inertial sensor 152B may no longer
be mounted
on implement 104. Implement 104 is considered virtual since a true implement
position is not
directly measured. Instead of directly measuring implement navigation states,
guidance system
120 predicts the implement states. Guidance system 120 then uses the predicted
implement states
in 2.37 to calculate steering solutions for steering vehicle 100 so a virtual
calculated position of
implement 104 is located over the desired path.
12
CA 03113336 2021-03-18
WO 2020/076427
PCT/US2019/048136
[0062] In formulating an analytic solution for vehicle heading, begin with
Equation 2.4:
V, = = (3.1)
.0t =r=-, sin Opt, -- et -- 0)
Integrating tift with respect to time t results in the following analytic
form:
3 K-atl _______________________________________
(t) # 2 tan (3.2)
?'2
where titto is the initial trailer heading. Equation 3.2 provides an analytic
representation of trailer
heading over time. A derivation of Equation 3.2 is described below. The
analytic solution
assumes that basic vehicle information is available (V, 6s and 'v), which can
be used to
determine the subsequent trailer heading after a given period of time.
[0063] From equation 3.2, the position and speed of implement 104 relative to
vehicle 100 is
predicted through Equations 2.6 and 2.7. Over time, the predicted implement
heading converges
onto the true implement heading, even from an initial unknown position. One
advantage of the
virtual implement scheme is no additional sensor hardware is needed on
implement 104 and is
suitable for operating on flat terrain with few disturbances.
[0064] The virtual implement scheme of Figure 8 also provides a level of
redundancy for the
measured implement scheme of Figure 7. For example, implement navigation data
from
implement sensors 152A and 152B in Figure 7 may become unavailable. Guidance
system 120
may still perform single-mode implement steering by switching over to the
virtual implement
scheme described in Figure 8.
[0065] The virtual implement scheme also may validate the accuracy of the
measured
implement data obtained from implement sensors 152A and 152B. For example, if
the measured
implement data from sensors 152A and 152B starts diverging from predicted
implement
measurements, guidance system 120 may generate a warning signal or execute a
test operation to
detect possible corruption of the measured implement data.
[0066] Figure 9 shows one example single-mode implement steering process. In
operation
200A, the guidance system identifies the position of the desired path. For
example, a user may
13
CA 03113336 2021-03-18
WO 2020/076427
PCT/US2019/048136
enter way lines into an electronic map displayed on a user interface attached
to the guidance
system.
[0067] In operation 200B, the guidance system may receive vehicle sensor data
and possibly
implement sensor data. For example, the guidance system may receive location,
speed, heading,
pitch, roll, yaw, or any other vehicle navigation data described above from
GPS and/or inertial
sensors located on the vehicle. The guidance system also may receive similar
location, speed,
heading, pitch, roll, yaw, etc. from GPS and/or inertial sensors located on
the implement. In an
alternative example described above, the implement may not include sensors,
and the guidance
system may predict the position and heading of the implement.
[0068] In operation 200C, the guidance system calculates a vehicle heading
error based on a
vehicle heading relative to the target path. For example, the guidance system
uses the vehicle
navigation data to calculate a heading of the vehicle and derives the vehicle
heading error by
calculating the difference between the vehicle heading and the path direction.
[0069] In operation 200D, the guidance system calculates an implement heading
error based
on a heading of the implement relative to the path. For example, the guidance
system uses the
implement navigation data, if any, to calculate a heading of the implement and
then derives the
implement heading error by calculating the difference between the implement
heading and the
path direction. As explained above, if the implement does not include
navigation sensors, the
guidance system may calculate the implement heading error based on a predicted
implement
heading.
[0070] In operation 200E, the guidance system calculates the implement cross-
track error
based on a distance of the implement from the path. For example, the guidance
system uses the
implement navigation data, if any, to calculate a location of the implement
and then derives the
implement cross-track error by calculating a distance of the implement
location from the path
location. As explained above, if the implement does not include navigation
sensors, the guidance
system may calculate the implement cross-track error based on a predicted
implement location.
[0071] In operation 200F, the guidance system calculates a vehicle curvature
error based on
the vehicle heading error, trailer heading error, and trailer cross-track
error. For example, the
guidance system may calculate the demanded vehicle curvature error using
equation 2.37. As
mentioned above, instead of initially reducing the position offset of the
vehicle, the guidance
14
CA 03113336 2021-03-18
WO 2020/076427
PCT/US2019/048136
system immediately starts steering the vehicle to reduce a positional offset
of the implement
cross-track error relative to path 110.
[0072] In operation 200G, the guidance system generates steering commands
based on the
calculated vehicle curvature error and sends the steering commands to a
steering controller. The
steering commands provide single-mode vehicle steering so the implement first
aligns over the
desired path before the vehicle. In one example, the steering commands may
cause the vehicle
to overshoot the path while aligning the implement with the path. The steering
commands then
may cause the vehicle to turn back and align over the path. In another
example, the path may be
curved or the field may be contoured and the guidance system may keep the
vehicle at an offset
from the target path while the implement remains aligned over the path.
Reverse Single-Mode Implement Steering
[0073] When driven manually, the forward motion of a vehicle/trailer system is
stable, with
the implement naturally and predictably converging behind the vehicle over
time. When driven
in reverse however, the vehicle/trailer system is inherently unstable, where
small disturbances in
position may potentially lead to unpredictable trailer movements and run the
risk of collision and
jackknifing. Steering a vehicle/trailer system in reverse can also be counter-
intuitive for an
inexperienced driver, with opposite control needed to steer the trailer in the
correct direction.
[0074] Figure 10 shows how the guidance system performs reverse single-mode
implement
steering. By using the implement states as part of the vehicle/trailer model
as described above in
Equation 2.37, guidance system 120 may perform single-mode implement steering
while vehicle
100 moves in reverse.
[0075] Initially vehicle 100 is offset from desired path 110 in stage 1. When
guidance system
120 is engaged, vehicle 100 first steers away from path 110 in order to force
implement 104
towards path 110 in stage 2. This maneuver highlights the counter-
intuitiveness of steering a
vehicle/trailer system in reverse, as opposite steering control is required to
achieve the desired
implement course change. As vehicle 100 nears path 110, vehicle 100
straightens to place
implement 104 on-line in stage 3, with both vehicle 100 and implement 104 on-
line at stage 4.
[0076] Conventional automated steering usually only steers based on vehicle
location and the
initial vehicle maneuver at stage 2 would normally be directly towards path
110. This rearward
movement toward path 110 would cause implement 104 to head away from path 110
and
CA 03113336 2021-03-18
WO 2020/076427 PCT/US2019/048136
ultimately jackknife. Guidance system 120 avoids these undesired situations by
incorporating
implement heading and position states into the steering control model describe
above.
100771 Figure 11 shows an example single-mode implement steering in reverse
onto a circular
path of non-zero curvature. Guidance system 120 again first steers vehicle 100
away from desired
circular path 130 directing trailer 104 towards desired circular path 130.
Guidance system 120
then starts steering vehicle 100 more towards desired path 130 until trailer
104 moves onto desired
circular path 130 and vehicle 100 is spaced and substantially parallel with
desired circular path
130. This reverse operation is more challenging when attempted to be steered
manually, as the
driver would need to constantly correct the position of vehicle 100 to
maintain a steady trajectory
of implement 104 onto and along circular path 130.
[0078] Guidance system 120 uses equation 2.37, and any of the other algorithms
described
above, to first steer vehicle 100 so trailer 104 first moves onto circular
path 130. Guidance system
120 then continues to steer vehicle 100 at an angular spaced distance from
circular path 130 based
on equation 2.37 to maintain the alignment of trailer 104 over circular path
130.
Virtual Implement Integral Evaluation
[0079] The following equations explain how the position and heading of the
implement may
be predicted. Defining implement heading rate is as follows:
vst. itsm kvs ¨ 0)
(Al)
_____________________________ õõ g= (A.2)
st
Using a standard integral solution for an integral in the form:
f ........................ = (raA (4li")) (A.3)
gives,
(0.4: ____________________________ L C (A.4)
.:, =
t = = Ot,=0
When , therefore:
16
CA 03113336 2021-03-18
WO 2020/076427 PCT/US2019/048136
Ot. __________________________________ = .
in (at ) in (ea _____ 9)) (A.5)
(AA __ ¨6) .lik (oot r.µ", 1 A' = (A.6)
-3
1-Ã.4
(A.7)
0,A ___________________________________
[0080] Taking the natural logarithm of the left hand side (LHS) and right hand
side (RHS) and
rearranging the trigonometric component gives:
taar-9.:'24.")
. . __ . . e
(A.8)
taxt Izz4%-9
And solving for gives:
# ¨ 2 tan -1 __________________________________________________________
(A.9)
Reverse Parking
[0081] Figures 12 and 13 show how guidance system 120 performs reverse parking
operations.
Guidance system 120 uses similar reverse steering operations described above,
combined with
controlling a throttle/speed control system in vehicle 100. In one example, a
desired parking path
204 is defined to inform guidance system 120 which path to steer vehicle 100
and trailer 104 to
reach target point 212 in parking area 210.
[0082] In one example, an electronic map is preloaded with parking path 204,
parking area
210, and target point 212. In other examples, guidance system 120 may
automatically generate
parking path 204 to target point 212 in real-time based on known obstructions
between vehicle
100 and target point 212. For example, a known chart plotting system can be
used in combination
with an electronic map that includes the area between vehicle 100 and target
point 212. The
electronic map may identify known obstructions, such as trees, fences, rocks,
etc. The chart
plotting system plots parking path 204 from vehicle 100 to target point 212
that avoids the known
obstructions. Parking path 204 may include any combination of straight lines
and curved lines
17
CA 03113336 2021-03-18
WO 2020/076427
PCT/US2019/048136
as described above in Figures 10 and 11. Guidance system 120 is then activated
to automatically
steer vehicle 100 onto plotted parking path 204 and then to target point 212.
[0083] Guidance system 120 may use a range/distance 206 between vehicle 100
and target
point 212 to determine how fast to move vehicle 100 along parking path 204.
Guidance system
120 may issue reduced speed commands to the speed controller system in vehicle
100 as
range/distance 206 to target point 212 gets smaller. As range 206 starts
approaching zero,
guidance system 120 gradually slows and then stops vehicle 100 when trailer
104 is located on
target point 212.
[0084] Figures 12A-12D show a reverse parking example where vehicle 100 and
trailer 104
are positioned well forward of target point 212. Figure 12A shows a first
state of vehicle 100
initially offset from parking path 204 and travelling in a reverse direction
prior to engaging
passive implement steering guidance system 120. Vehicle 100 is currently at
range/distance
206A from target point 212.
[0085] Figure 12B shows a second state of vehicle 100 where guidance system
120 is engaged
and starts steering vehicle 100 and trailer 104 onto parking path 204.
Guidance system 120 sends
commands to the vehicle steering and speed control system 166 in Figure 4 to
manage the speed
of vehicle 100 based on current range 206B from target point 212. For example,
guidance system
120 starts slowing down vehicle 100 as it gets closer to target point 212. As
described above,
guidance system 120 initially turns vehicle 100 away from parking path 204 to
more quickly
move trailer 104 onto parking path 204.
[0086] Figure 12C shows a third state of vehicle 100 where guidance system 120
begins the
final stages of placing trailer 104 onto parking path 204. As also described
above, guidance
system 102 may steer vehicle 100 back toward parking path 204 to align trailer
104 and then
vehicle 100 with parking path 204. In this third state, guidance system 120
further slows the
speed of vehicle 100 due to the smaller range 206C between target point 212
and trailer 104.
[0087] Figure 12D shows a fourth state of vehicle 100 where guidance system
120 has
successfully steered vehicle 100 and trailer 104 onto parking path 204.
Guidance system 120
continues to steer vehicle 100 in reverse along parking path 204 until the end
of trailer 104 is
positioned over target point 212 within parking area 210.
18
CA 03113336 2021-03-18
WO 2020/076427
PCT/US2019/048136
[0088] As described in one example above, guidance system 120 may steer
trailer 104 onto
parking path 204 using single-mode implement steering controller 162 shown in
Figure 4. At the
same time, guidance system 120 also sends speed commands to vehicle steering
and speed control
system 166 to gradually reduce the speed of vehicle 100 as it comes closer to
target point 212.
[0089] Guidance system 120 may use pre-stored speeds for different ranges 206.
For example,
guidance system 120 may send vehicle steering and speed control system 166 a
command for a
first speed when vehicle 100 is further than first range 206A from target
point 212. Guidance
system 120 may send vehicle steering and speed controller 166 a second speed
command for a
second slower speed when vehicle 100 is between first range 206A and second
range 206B from
target point 212.
[0090] Guidance system 120 may send the vehicle steering and speed controller
166 a third
speed command for a third even slower speed when vehicle 100 is between second
range 206B
and third range 206C from target point 212. Guidance system 120 then may start
sending
continuously slower speed commands to controller 166 as vehicle 100 moves
within third range
206C towards target point 212. For example, guidance system 120 may gradually
slow vehicle
100 in range 206 until eventually stopping vehicle 100 when trailer 104
reaches target point 212.
[0091] Figures 13A-13E illustrate a parking scenario where the initial
position of vehicle 100
does not allow immediate reverse engagement to acquire parking path 204 and
reach target point
212. Figure 13A shows a first state where vehicle 100 and trailer 104 are too
close to parking
area 210 to complete a successful reverse parking operation without a high
risk of j ackknifing.
[0092] Parking area 210 may define a space where vehicle 100 and trailer 104
need to be
aligned with parking path 204. For example, parking area 210 may define a
garage or a space
where other vehicles also may park. In other examples, a parking area 210 is
not defined in the
electronic map and guidance system 120 only may need to align vehicle 100 and
trailer with
parking path 204 by the time trailer 104 reaches target point 212.
[0093] Guidance system 120 may store parking area 210 and/or target point 212
in an
electronic map and store a reverse threshold distance 214 either from parking
area 210 or target
point 212. In the first state of Figure 13A, guidance system 120 determines
vehicle 100 or trailer
104 is less than reverse threshold distance 214 from parking area 210. For
explanation purposes,
the remaining description assumes guidance system 120 uses a reverse threshold
distance 214
from parking area 210.
19
CA 03113336 2021-03-18
WO 2020/076427
PCT/US2019/048136
[0094] Figure 13B shows a second state where guidance system 120 sends
commands to
steering and speed control system 166 that steer vehicle 100 and trailer 104
in a forward direction.
Guidance system 120 again use the closed loop single-mode controller 162
described above to
steer trailer 104 over parking path 204. For example, single-mode controller
162 may steer
vehicle 100 over and past parking path 204 to move trailer 104 more quickly
over parking path
204.
[0095] Figure 13C shows a third state where vehicle 100 and trailer 104 have,
based on a path
offset and heading convergence condition, acquired parking path 204 and can
now change
direction and travel in reverse to complete the parking maneuver. At this
point, guidance system
120 sends commands to steering and speed control system 166 to switch
direction from forward
to reverse.
[0096] Figure 13D shows a fourth state where guidance system 120 sends
commands to control
system 166 that steer vehicle 100 and trailer 104 along parking path 204 in
reverse. As described
above, guidance system 120 may slow down vehicle 100 as it gets closer to
target point 212.
Figure 13E shows a fifth state where guidance system 120 stops vehicle 100
when trailer 104
reaches target point 212.
[0097] Guidance system 120 also may determine vehicle 100 and trailer 104 are
too close to
parking area 210 based on a distance of trailer 104 from parking area 210 and
parking path 204.
For example, the closer the vehicle 100 and trailer 104 are to parking path
204, the closer vehicle
100 and trailer 104 can be to parking area 210 and still reverse into parking
area 210 without
jackknifing. Alternatively, the further vehicle 100 and trailer 104 are from
parking path 204, the
further vehicle 100 and trailer 104 need to be from parking area 210 before
reversing into parking
area 210 without jackknifing.
[0098] Guidance system 120 may store a table of threshold trailer-to-parking
area distances for
different trailer-to-parking path distances. Alternatively, guidance system
120 may calculate the
threshold trailer-to-parking area distance on the fly based on a current
trailer-to-parking path
distance and the turning characteristics of vehicle 100 and trailer 104. For
example, guidance
system 120 may calculate a vehicle curvature as explained above while
maintaining vehicle 100
and trailer 104 within a given articulation range. If the vehicle curvature
extends into parking
area 210, guidance system 120 operates in the first state shown in Figure 13A.
Guidance system
120 starts steering vehicle 100 in a forward direction to move trailer onto
parking path 204.
CA 03113336 2021-03-18
WO 2020/076427
PCT/US2019/048136
Otherwise, guidance system 120 operates in the first state shown in Figure 12A
and immediately
starts reversing vehicle 100 and trailer 104 onto parking path 204.
[0099] Figure 14 describes steering operations performed in Figures 13A-13E.
In operation
220A, the guidance system identifies a position of the parking path, parking
area, and target point
in the parking area. As explained above, the parking path, parking area, and
target point may be
preloaded into an electronic map. The guidance system in operation 220B
calculates a current
distance of the trailer from the parking path and in operation 220C calculates
a current distance
of the trailer from the parking area.
[00100] The guidance system in operation 220D calculates a threshold distance
of the trailer
from the parking area based on the current distance of the trailer from the
parking path. As
explained above, the further the trailer is away from the parking path the
further away the trailer
may need to be away from the parking area in order to reverse into the parking
area without jack-
knifing.
[00101] The guidance system in operation 220E determines if the current
distance of the trailer
from the parking area is less than the calculated threshold distance. If the
current trailer distance
is less than the threshold distance, the guidance system in operation 220F
calculates steering
commands to steer the vehicle and trailer in a forward direction onto the
parking path. If the
current distance of the trailer from the parking area is greater than the
threshold distance, the
guidance system in operation 220G calculates steering commands to steer the
vehicle in a reverse
direction onto the parking path.
[00102] The vehicle and trailer are aligned on the parking path at the
completion of operation
220F or 220G. The guidance system in operation 220H calculates additional
steering commands
to further steer the vehicle and trailer in a reverse direction along the
remainder of the parking
path until the trailer reaches the target point.
[00103] Figure 15 generally shows guidance system 120 used in conjunction with
electrical-
mechanical steering and speed control system 166. Without limitation on the
generality of useful
applications of guidance system 120, a GNSS receiver 4 and navigation
processor 158 are
connected to a GNSS antenna 150 and installed into vehicle 100, such as an
agricultural vehicle
or tractor. Single-mode implement steering controller 162 is electrically
connected to navigation
processor 158 and is electro-mechanically interfaced with vehicle 100 via
steering and speed
control system 166.
21
CA 03113336 2021-03-18
WO 2020/076427
PCT/US2019/048136
[00104] Figure 16 shows additional details of guidance system 120. The GNSS
receiver 4 is
further comprised of an RF convertor (i.e., downconvertor) 16, a tracking
device 18, and a rover
RTK receiver element 20. Receiver 4 electrically communicates with, and
provides GNSS
positioning data to, navigation processor 158 and steering controller 162.
Processor 158 or
controller 162 also may include a graphical user interface (GUI) 26, a
microprocessor 24, and a
media element 22, such as a memory storage drive. Steering controller 162
electrically
communicates with, and provides control data to, steering and speed control
system 166. Steering
and speed control system 166 may include a wheel movement detection switch 28
and an encoder
30 for interpreting steering and speed control commands from processor 158
and/or controller
162.
[00105] Steering and speed control system 166 may interface mechanically with
the vehicle's
steering column 34, which is mechanically attached to steering wheel 32. A
controller area
network (CAN) bus may transmit steering and speed commands from processor 158
and
controller 162 to steering and speed control system 166. An electrical
subsystem 44, which
powers the electrical needs of vehicle 100, may interface directly with
control system 166 through
a power cable 46. Steering and speed control system 166 can be mounted to
steering column 34
near the floor of the vehicle, and in proximity to the vehicle's control
pedals 36. Alternatively,
steering and speed control system 166 can be mounted at other locations along
steering column
34.
[00106] Steering and speed control system 166 may physically drive and steer
vehicle 100 by
actively turning steering wheel 32 via steering column 34. Control system 166
controls a motor
45 powered by vehicle electrical subsystem 44 that operates a worm drive 50
that includes a worm
gear affixed to steering column 34. These components are preferably located in
an enclosure. In
other embodiments, auto-steering system 166 is integrated directly with
processor 158 and
controller 162 independently of steering column 34. Steering and speed control
system 166 also
may electronically or mechanically connect to an accelerator controller for
controlling the speed
of vehicle 100.
[00107] Another example integrated guidance system 120 that attaches to
steering wheel 32 is
described in pending U.S. Patent Application Ser. No. 15/878,849 entitled
INTEGRATED
AUTO-STEER SYSTEM FOR VEHICLE; filed Feb. 13, 2018 and is incorporated by
reference
in its entirety.
22
CA 03113336 2021-03-18
WO 2020/076427
PCT/US2019/048136
[00108] Examples of systems, apparatus, computer-readable storage media, and
methods are
provided solely to add context and aid in the understanding of the disclosed
implementations. It
will thus be apparent to one skilled in the art that the disclosed
implementations may be practiced
without some or all of the specific details provided. In other instances,
certain process or methods
also referred to herein as "blocks," have not been described in detail in
order to avoid
unnecessarily obscuring the disclosed implementations. Other implementations
and applications
also are possible, and as such, the following examples should not be taken as
definitive or limiting
either in scope or setting.
[00109] References have been made to accompanying drawings, which form a part
of the
description and in which are shown, by way of illustration, specific
implementations. Although
these disclosed implementations are described in sufficient detail to enable
one skilled in the art
to practice the implementations, it is to be understood that these examples
are not limiting, such
that other implementations may be used and changes may be made to the
disclosed
implementations without departing from their spirit and scope. For example,
the blocks of the
methods shown and described are not necessarily performed in the order
indicated in some other
implementations. Additionally, in other implementations, the disclosed methods
may include
more or fewer blocks than are described. As another example, some blocks
described herein as
separate blocks may be combined in some other implementations. Conversely,
what may be
described herein as a single block may be implemented in multiple blocks in
some other
implementations. Additionally, the conjunction "or" is intended herein in the
inclusive sense
where appropriate unless otherwise indicated; that is, the phrase "A, B or C"
is intended to include
the possibilities of "A," "B," "C," "A and B," "B and C," "A and C" and "A, B
and C."
[00110] A Global navigation satellite system (GNSS) is broadly defined to
include GPS (U.S.)
Galileo (European Union, proposed) GLONASS (Russia), Beidou (China) Compass
(China,
proposed) IRNSS (India, proposed), QZSS (Japan, proposed) and other current
and future
positioning technology using signal from satellites, with or with augmentation
from terrestrial
sources.
[00111] Inertial navigation systems (INS) may include gyroscopic (gyro)
sensors,
accelerometers and similar technologies for providing outputs corresponding to
the inertial of
moving components in all axes, i.e., through six degrees of freedom (positive
and negative
directions along transverse X, longitudinal Y and vertical Z axes). Yaw, pitch
and roll refer to
23
CA 03113336 2021-03-18
WO 2020/076427
PCT/US2019/048136
moving component rotation about the Z, X, and Y axes respectively. Said
terminology will
include the words specifically mentioned, derivative thereof and words of
similar meaning.
[00112] Some of the operations described above may be implemented in software
and other
operations may be implemented in hardware. One or more of the operations,
processes, or
methods described herein may be performed by an apparatus, device, or system
similar to those
as described herein and with reference to the illustrated figures.
[00113] "Computer-readable storage medium" (or alternatively, "machine-
readable storage
medium") used in guidance system 120 may include any type of memory, as well
as new
technologies that may arise in the future, as long as they may be capable of
storing digital
information in the nature of a computer program or other data, at least
temporarily, in such a
manner that the stored information may be "read" by an appropriate processing
device. The term
"computer-readable" may not be limited to the historical usage of "computer"
to imply a complete
mainframe, mini-computer, desktop, wireless device, or even a laptop computer.
Rather,
"computer-readable" may comprise storage medium that may be readable by a
processor,
processing device, or any computing system. Such media may be any available
media that may
be locally and/or remotely accessible by a computer or processor, and may
include volatile and
non-volatile media, and removable and non-removable media.
[00114] Having described and illustrated the principles of a preferred
embodiment, it should be
apparent that the embodiments may be modified in arrangement and detail
without departing from
such principles. Claim is made to all modifications and variation coming
within the spirit and
scope of the following claims.
24
