Single-mode implement steering
A guidance system identifies a path on a field and then calculates a position and heading of a trailer relative to the path. The guidance system steers a vehicle connected to the trailer based on the calculated trailer position and heading to minimize the trailer positional error and more quickly and accurately align the trailer with the path. The guidance system may align the trailer with the path while steering the vehicle in a reverse direction and may steer the vehicle based on a predicted trailer position and heading.
1. A guidance system comprising:
a processor; and
memory coupled to the processor and storing instructions that, when executed by the processor, cause the guidance system to:
calculate a vehicle heading error based on a heading of the vehicle relative to a path;
calculate a trailer heading error for a trailer attached to the vehicle based on a heading of the trailer relative to the path;
calculate a trailer cross-track error based on a distance of the trailer from the path;
calculate a vehicle curvature error based on the vehicle heading error, trailer heading error, and trailer cross-track error; and
use the vehicle curvature error to steer the vehicle and move the trailer toward the path, wherein:
e
κ
v
=
[
K
1
K
2
K
3
]
[
e
ψ
v
e
ψ
t
e
ct
t
]
+
K
int
∫
e
ct
t
dt
,
e K v is the vehicle curvature error,
e ψ v is the vehicle heading error,
e ψ t is the trailer heading error,
e ct t is the trailer cross-track error,
K 1 , K 2 , and K 3 are gain values for a steering controller, and
K int is an integral term that acts to minimize the trailer cross-track error.
2. A non-transitory computer-readable medium storing instructions that, when executed by a processor, are operable to cause a guidance system to:
calculate a vehicle heading error based on a heading of the vehicle relative to a path;
calculate a trailer heading error for a trailer attached to the vehicle based on a heading of the trailer relative to the path;
calculate a trailer cross-track error based on a distance of the trailer from the path;
calculate a vehicle curvature error based on the vehicle heading error, trailer heading error, and trailer cross-track error; and
use the vehicle curvature error to steer the vehicle and move the trailer toward the path, wherein:
e
κ
v
=
[
K
1
K
2
K
3
]
[
e
ψ
v
e
ψ
t
e
ct
t
]
+
K
int
∫
e
ct
t
dt
,
e K v is the vehicle curvature error,
e ψ v is the vehicle heading error,
e ψ t is the trailer heading error,
e ct t is the trailer cross-track error,
K 1 , K 2 , and K 3 are gain values for a steering controller, and
K int is an integral term that acts to minimize the trailer cross-track error.
3. The non-transitory computer-readable medium of claim 2 , wherein:
K
1
=
2
ζω
h
+
ω
l
-
V
L
2
+
c
V
K
2
L
2
V
,
K
2
=
ω
h
2
+
2
ζω
h
ω
l
+
c
ω
h
2
ω
l
V
-
V
L
2
(
2
ζ
ω
h
+
ω
l
-
V
L
2
)
V
2
L
2
(
1
+
c
L
2
)
,
K
3
=
L
2
ω
h
2
ω
l
V
3
,
ωh and ωl define desired high and low frequencies pole locations,
ζ defines a damping factor,
L2 is a length of the trailer,
c is a hitch length of the vehicle behind a control point, and
V is a speed of the vehicle.
4. The non-transitory computer-readable medium of claim 2 , wherein the instructions are further operable to:
identify an articulation angle between the vehicle and the trailer;
identify an articulation range limit; and
adjust the vehicle curvature error to maintain the articulation angle within the articulation range limit.
5. The non-transitory computer-readable medium of claim 2 , wherein the instructions are further operable to:
predict the trailer heading error and the trailer cross-track error based on a speed of the vehicle, a steering angle of the vehicle, and a heading of the vehicle; and
calculate the vehicle curvature error based on the predicted trailer heading error and the predicted trailer cross-track error.
6. The non-transitory computer-readable medium of claim 2 , wherein the instructions are further operable to:
measure the trailer heading error and cross-track error based on readings from a global positioning system (GPS) receiver and inertial sensor located on the trailer;
compare the predicted trailer heading error and cross-track error with the measured trailer heading error and cross-track error; and
generate a notification when a divergence is detected between the predicted trailer heading error and cross-track error and the measured trailer heading error and cross-track error.
7. The non-transitory computer-readable medium of claim 2 , wherein the instructions are further operable to calculate the vehicle curvature error for the vehicle moving in a reverse direction that pushes the trailer into alignment with the path.
8. The non-transitory computer-readable medium of claim 2 , wherein the instructions are further operable to:
cause the vehicle to overshoot the path and first align the trailer with the path;
cause the vehicle to steer back onto the path to align the vehicle with the path; and
cause the vehicle to continue moving over the path while both the vehicle and trailer remain aligned over the path.
9. A method comprising:
calculating, by a guidance system, a vehicle heading error based on a heading of the vehicle relative to a path;
calculating, by the guidance system, a trailer heading error for a trailer attached to the vehicle based on a heading of the trailer relative to the path;
calculating, by the guidance system, a trailer cross-track error based on a distance of the trailer from the path;
calculating, by the guidance system, a vehicle curvature error based on the vehicle heading error, trailer heading error, and trailer cross-track error; and
using the vehicle curvature error, by the guidance system, to steer the vehicle and move the trailer toward the path, wherein:
e
κ
v
=
[
K
1
K
2
K
3
]
[
e
ψ
v
e
ψ
t
e
ct
t
]
+
K
int
∫
e
ct
t
dt
,
e K v is the vehicle curvature error,
e ψ v is the vehicle heading error,
e ψ t is the trailer heading error,
e ct t is the trailer cross-track error,
K 1 , K 2 , and K 3 are gain values for a steering controller, and
K int is an integral term that acts to minimize the trailer cross-track error.