EP3863909A1

EP3863909A1 - Automated reverse implement parking

Info

Publication number: EP3863909A1
Application number: EP19765874.3A
Authority: EP
Inventors: Eran D.B. MEDAGODA; Mohammad Assef; Joseph Chai; Tri M. Dang
Original assignee: Agjunction LLC
Current assignee: Agjunction LLC
Priority date: 2018-10-08
Filing date: 2019-08-26
Publication date: 2021-08-18
Also published as: WO2020076427A1; CA3113336A1; BR112021006427A2

Abstract

A guidance system (120) identifies a parking path (204) and a target point (212) in a parking area (210). The guidance system (120) calculates steering commands (164) to steer the vehicle (100) and trailer (104) onto the parking path (204). The guidance system (120) calculates a distance of the trailer (104) from the target point (212) and calculates speed commands for the vehicle (100) based on the distance of the trailer (104) from the target point (212). The guidance system (120) sends the steering and speed commands to a steering and speed control system (166) to steer the vehicle (100) and move the trailer (104) along the parking path (204) until the trailer (104) reaches the target point (212) in the parking area (210).

Description

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 systemin reverse for an inexperienced driver 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. [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, 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 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. [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, L \ denotes vehicle wheelbase, 1.2 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:

where

[0041] For this system, the terms x and v represent the position of the vehicle control point in the local frame, yn represents the heading of the vehicle, i//_/ represents the trailer heading, G 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 s 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 y t , articulation angle G, 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: where

(2.14)

[0044] Applying small angle approximations to Equations 2.10 to 2.14 results in the following linearized system:

[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:

where by_n denotes vehicle heading error, L /_// is trailer heading error, ectt 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. [0047] In state-space form, the system can be expressed as:

which is in the linear state-space form:

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.

here mh and ml define the desired high and low frequencies pole locations, with z 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:

[0049] In state-space, the desired controller can be expressed as: (2.24) where

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: [0050] The closed-loop characteristic equation for this system can be found by calculating the determinant of the following transfer function realization:

As a result, the closed-loop characteristic equation expressed in coefficients of s is: where

2 29

[0051] By equating the coefficients ao to as with the coefficients of s in Equation 2.23, it is possible to evaluate the controller gains in as a function of desired closed-loop pole locations and system geometries. Expressed in matrix form, the coefficients can be equated to:

Solving Equation 2.33 yields:

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 c^v

[0052] The single-mode implement steering controller is driven by three error states - vehicle heading error cy_/v , trailer heading error cy_// . and trailer cross-track error cc_// . Management of these error states allows for the formulation of the demanded vehicle curvature for the system, expressed generally as:

where bkn is the demanded vehicle curvature error used for steering the vehicle.

[0053] The terms K\ , 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 K _l 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 G, 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: where Tmin is the lower articulation angle limit and G max 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. [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. [0062] In formulating an analytic solution for vehicle heading, begin with Equation 2.4:

Integrating y/t with respect to time t results in the following analytic form:

where i//¾o 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, Ss and yn ), 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 200 A, the guidance system identifies the position of the desired path. For example, a user may 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 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 ultimately jackknife. Guidance system 120 avoids these undesired situations by incorporating implement heading and position states into the steering control model describe above.

[0077] 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:

J .SlTi ? J ^:::: J (A.2)

Using a standard integral solution for an integral in the form:

gives,

When

[0080] Taking the natural logarithm of the left hand side (LHS) and right hand side (RHS) and rearranging the trigonometric component gives:

And solving gives:

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 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. [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 jackknifing.

[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. [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. 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. [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. [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 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.

Claims

CLAIMS What is claimed is:

1. A guidance system for steering a vehicle and trailer, comprising:

a processor; and

memory coupled to the processor and storing instructions that, when executed by the processor, cause the guidance system to:

identify a parking path and a target point in a parking area;

calculate steering commands to steer the trailer onto the parking path;

calculate a distance of the trailer from the target point;

calculate speed commands for the vehicle based on the distance of the trailer from the target point; and

send the steering commands and speed commands to a steering and speed control system to steer the vehicle and move the trailer onto and along the parking path until the trailer reaches the target point in the parking area.

2. The guidance system of claim 1, wherein the memory further stores instructions for causing the guidance system to:

identify distances of the trailer from the target point; and

adjust the speed commands to reduce the speed of the vehicle as the trailer moves closer to the target point.

3. The guidance system of claim 1, wherein the memory further stores instructions for causing the guidance system to:

store a threshold distance of the trailer from the parking area;

calculate a current distance of the trailer from the parking area; and

calculate forward steering commands to steer the vehicle and move the trailer in a forward direction onto the parking path when the current distance of the trailer from the parking area is less than the threshold distance.

4. The guidance system of claim 3, wherein the memory further stores instructions for causing the guidance system to:

detect the trailer aligned over the parking path; and calculate reverse steering commands to steer the vehicle and trailer in a reverse direction along the parking path toward the target point after the vehicle and trailer are steered onto the parking path.

5. The guidance system of claim 1, wherein the memory further stores instructions for causing the guidance system to:

calculate a current distance of the trailer from the parking path;

calculate a current distance of the trailer from the parking area;

calculate a threshold distance of the trailer from the parking area based on the current distance of the trailer from the parking path;

calculate forward steering commands for steering the vehicle and the trailer in a forward direction onto the parking path when the current distance of the trailer from the parking area is less than the threshold distance; and

calculate reverse steering commands for steering the vehicle and trailer in a reverse direction over the parking path when the current distance of the trailer from the parking area is more than the threshold distance.

6. The guidance system of claim 1, wherein the memory further stores instructions for causing the guidance system to:

calculate reverse steering commands to steer the vehicle and trailer in a reverse direction along the parking path until the trailer reaches the target point.

7. The guidance system of claim 1, wherein the parking path and target position are stored in an electronic map displayed on a user interface coupled to the guidance system.

8. The guidance system of claim 1, wherein the memory further stores instructions for causing the guidance system to:

calculate a vehicle heading error for the vehicle based on a heading of the vehicle relative to the parking path;

calculate a trailer heading error for the trailer based on a heading of the trailer relative to the parking path;

calculate a trailer cross-track error based on a distance of the trailer from the parking path; calculate a vehicle curvature error based on the vehicle heading error, trailer heading error, and trailer cross-track error; and

calculate the steering commands to steer the vehicle and move the trailer onto the parking path based on the vehicle curvature error.

9. The guidance system of claim 8, wherein the memory further stores instructions for causing the guidance system to:

adjust the vehicle curvature error to maintain an articulation angle between a lower limit for the articulation angle and an upper limit for the articulation angle.

10. The guidance system of claim 8, wherein calculating the vehicle heading error is based on navigation data received from a sensor coupled to the vehicle.

11. The guidance system of claim 10, wherein calculating the trailer heading error is based on navigation data received from a sensor coupled to the trailer.

12. The guidance system of claim 8, wherein the trailer heading error and the trailer cross track error are predicted based on one or more of: a speed of the vehicle, a steering angle of the vehicle, and a heading of the vehicle.

13. The guidance system of claim 12, wherein the memory further stores instructions for causing the guidance system to:

calculate an actual trailer heading error and an actual trailer cross-track error based on readings from a sensor coupled to the trailer;

compare the predicted trailer heading error with the actual trailer heading error;

compare the predicted trailer cross-track error with the actual trailer cross-track error; and

generate a notification when a divergence is detected between the predicted trailer heading error and the actual trailer heading error, or between the predicted trailer cross-track error and the actual trailer cross-track error.

14. A non-transitory computer-readable medium storing instructions that, when executed by a processor, cause a guidance system to:

identify a parking path and a target point in a parking area; calculate steering commands to steer the vehicle and trailer onto the parking path; calculate a distance of the trailer from the target point;

calculate speed commands for controlling speed of the vehicle based on the distance of the trailer from the target point; and

15. The non-transitory computer-readable medium of claim 14, wherein the instructions when executed by the processor, cause the guidance system to:

identify a distance of the trailer from the target point; and

16. The non-transitory computer-readable medium of claim 14, wherein the instructions when executed by the processor, cause the guidance system to:

store a threshold distance of the trailer from the parking area;

calculate a current distance of the trailer from the parking area; and

17. The non-transitory computer-readable medium of claim 16, wherein the instructions when executed by the processor, cause the guidance system to:

detect the trailer aligned over the parking path; and

calculate reverse steering commands to steer the vehicle and move the trailer in a reverse direction along the parking path toward the target point after the trailer moves in a forward direction onto the parking path.

18. The non-transitory computer-readable medium of claim 14, wherein the instructions when executed by the processor, cause a guidance system to:

calculate a current distance of the trailer from the parking path;

calculate a current distance of the trailer from the parking area; and calculate a threshold distance of the trailer from the parking area based on the current distance of the trailer from the parking path;

calculate forward steering commands for steering the vehicle and moving the trailer in a forward direction over the parking path when the current distance of the trailer from the parking area is less than the threshold distance; and

calculate reverse steering commands for steering the vehicle and moving trailer in a reverse direction over the parking path when the current distance of the trailer from the parking area is more than the threshold distance.

19. The non-transitory computer-readable medium of claim 14, wherein the instructions when executed by the processor, cause a guidance system to:

calculate reverse steering commands to steer the vehicle and move the trailer in a reverse direction along the parking path until the trailer reaches the target point.

20. The non-transitory computer-readable medium of claim 14, wherein the instructions when executed by the processor, cause a guidance system to:

calculate a vehicle heading error based on a heading of the vehicle relative to the parking path;

calculate a trailer cross-track error based on a distance of the trailer from the parking path;

calculate a vehicle curvature error based on the vehicle heading error, trailer heading error, and trailer cross-track error; and

21. A method comprising:

calculating, by a guidance system, a vehicle heading error based on a heading of the vehicle relative to a parking 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 parking path;

calculating, by the guidance system, a trailer cross-track error based on a distance of the trailer from the parking 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

steering, by the guidance system, the vehicle and moving the trailer onto and along the parking path to a target point in a parking area based on the vehicle curvature error.