Systems and methods for controlling the motion of articulated vehicles

Abstract

A system for controlling the motion of an articulated vehicle having one or more trailers is described. The system is configured to estimate a state of the vehicle, determine a motion path having forward and reverse motion of the vehicle based on the state, and formulate an optimal control problem to optimize the motion path. The optimal control problem includes an integrated tracking error function that indicates an integrated tracking error based on a motion cusp associated with the motion path. The motion cusp indicates a switch between forward and reverse motion. The motion path is optimized over a prediction horizon based on solving the optimal control problem. Control commands for the vehicle are generated based on the optimized motion path and a vehicle model, and the motion of the articulated vehicle is controlled based on the control commands to change its state.

Description

[Technical Field]

[0001] The present disclosure relates to controlling vehicle motion, and more particularly to motion planning and predictive control for autonomous or semi-autonomous driving vehicle-trailer systems having one or more trailers. [Background technology]

[0002] Automated transportation systems have the potential to reduce accidents and improve road safety. Connected and automated vehicles (CAVs) also have the potential to reduce congestion, travel times, vehicle emissions, and fuel consumption through improvements to vehicle infrastructure and traffic flow. Commercial heavy-duty vehicles (HDVs) such as trucks are an important use case for vehicle automation in logistics and delivery. In particular, autonomous control of heavy articulated vehicles has the potential to provide significant benefits to the transportation sector with regard to economic, environmental, and societal factors.

[0003] In recent years, optimization-based planning and control have been increasingly used for automated vehicle operation. For heavy-duty articulated vehicles, current research has focused primarily on platooning and control of articulated vehicles, especially on highways. Therefore, optimization-based motion planning and control techniques have been utilized to automate a variety of tasks. Additionally, various techniques have been proposed for articulated vehicle motion planning, such as sampling-based algorithms, lattice-based algorithms, or hybrid algorithms. Various control algorithms have also been proposed to track the resulting motion plans for articulated vehicles, including, for example, sliding mode control, linear-quadratic regulation, input-state linearization, continuous time-varying nonlinear feedback laws, dynamic programming, and model predictive control.

[0004] However, maneuvers such as precise reversing are more difficult to automate, and most docking or parking of articulated vehicles relies on skilled drivers and requires extensive and costly labor training. During docking operations, drivers must precisely control the position and orientation of an articulated vehicle, such as a tractor-trailer, while executing a series of forward and reverse maneuvers to park the articulated vehicle within a designated bay area with strict spatial and static configuration error specifications. Drivers may also have limited visibility of the area surrounding the designated docking space. As a result, automated docking or parking tasks require extremely high precision and are therefore critical operations in the operation of heavy articulated vehicles.

[0005] Reliably tracking and controlling such articulated vehicles during parking and docking maneuvers is challenging due to the complex parking and docking maneuvers and corresponding disturbances and modeling errors, and small tracking errors during parking maneuvers can lead to infrastructure damage to both the vehicles and the docking bays.

[0006] Furthermore, tracking and control of an articulated vehicle during a parking maneuver becomes even more difficult when multiple trailers are connected in series without active steering. Any disturbances acting on the closed-loop system, such as unmodeled dynamics, will have a significant effect on the tracking performance of the rear trailer of an articulated vehicle when reversing, requiring a control system that changes both in structure and tuning when performing forward and reverse path tracking.

[0007] In some cases, standard n-trailer (SNT) models can be used in control system designs for controlling articulated vehicles. SNT models allow accurate feedback linearization, resulting in powerful nonlinear feedback laws that can be combined with linear control synthesis techniques. However, such control system designs based on SNT are highly vulnerable to modeling errors and cannot easily incorporate state constraints necessary to ensure operational safety.

[0008] An alternative is to use a model predictive controller (MPC) to realize path tracking for articulated vehicles. However, linear MPC is limited to path tracking for straight-line motion (e.g., cruise control). Furthermore, a control system combining linear and nonlinear MPC (NMPC) can be used for path tracking and control of articulated vehicles, considering forward and reverse motion. In some cases, linear MPC is complemented by an extended Kalman filter specifically designed to estimate disturbances, while linear MPC using fuzzy logic is proposed. Alternatively, NMPC can be used for path tracking and control of any dynamically realizable path for both forward and reverse motion for special paths with motion primitives such as linear segments and circular trajectories.

[0009] However, such linear or nonlinear MPC control systems do not consider the effects of modeling errors. Therefore, reverse motion presents significant challenges, especially in path tracking and control for docking or parking maneuvers, because even small modeling errors are amplified through unstable system chains. While docking maneuvers may appear simple, they require relatively high precision, a critical task for the operation of heavy articulated vehicles. Therefore, there is a need to overcome the technical challenges associated with docking maneuvers, specifically automated tractor-trailer docking operations. Summary of the Invention

[0010] An object of some embodiments is to disclose a system for controlling the motion of an articulated vehicle. Another object of some embodiments is to disclose a method for controlling the motion of an articulated vehicle. Another object of some embodiments is to disclose a new type of control system that uses a motion planning algorithm and a real-time reference tracking controller tailored to the task of automatic docking or parking operations. Another object of some embodiments is to provide such a system and method that uses a tracking controller that uses nonlinear model predictive control for motion planning to facilitate robust path tracking for docking operations of one or more serially connected trailers.

[0011] Some embodiments recognize that docking operations (also called docking operations or parking operations) are critical operations in the operation of heavy articulated vehicles because they require extremely high precision to successfully dock the articulated vehicles. Some embodiments recognize that docking operations require extensive and costly labor training, especially for vehicle-trailer docking operations.

[0012] Some embodiments recognize that during a docking operation, a driver must precisely control the position and orientation of an articulated vehicle while executing a series of forward and reverse motions. Additionally, the driver may have limited visibility of the area surrounding a designated docking space for connecting the rear end of the trailer to a docking bay. Therefore, reliably tracking and controlling such an articulated vehicle during a docking operation is challenging due to external disturbances and modeling errors. In particular, any external disturbances or unmodeled dynamics can significantly affect the tracking performance of the articulated vehicle's rear trailer when reversing, raising serious safety concerns during automated docking operations.

[0013] Some embodiments are based on the recognition that the use of linear MPC for path tracking and control of articulated vehicles is limited to simpler motion primitives, such as the forward motion used for cruise control on highways.

[0014] Some embodiments are based on the understanding that path tracking and control for forward and backward motion for special paths with motion primitives such as linear segments and circular orbits is challenging as any modeling error is greatly amplified through the chain of unstable systems in nonlinear MPC.

[0015] Some embodiments are based on the recognition that the controller can perform explicit linearization and provide linear state feedback, but this may prevent backward movement from all initial states due to a jackknife effect.

[0016] Some embodiments are based on the understanding that hybrid control systems can be used that switch between different linear state feedback controllers defined for different motion primitives. However, such control systems can generate a large set of linear state feedbacks for the different linear state feedback controllers and can remain vulnerable to modeling errors and disturbances.

[0017] Some embodiments are based on the recognition that lateral tracking errors may be introduced, for example, due to unknown steering angle bias. While the bias may be tolerable in forward motion, reverse motion may amplify the effect of disturbances due to the bias, causing steering angle bias and modeling errors, resulting in very large tracking errors.

[0018] The purpose of this disclosure is to focus on articulated vehicles, such as tractor-trailers, operating with one or more trailers, and to disclose techniques for performing autonomous docking operations for articulated vehicles, which requires high precision due to the potential for infrastructure damage and is a critical task in fully autonomous heavy vehicle operations.

[0019] Some embodiments recognize that it is common for motion planners to generate fragmented paths that include multiple motion cusps, i.e., segments connected by rest configurations or points in state space. Motion cusps add flexibility to the space search in planning and tend to provide paths with the shortest distance. Conventional motion planners strive for the shortest path and may therefore generate unnecessary motion cusps. Such motion cusps can impair the positioning accuracy of the integrated system. Conventional motion planners tend to improve the generated path in terms of path length but do not consider the number of motion cusps. Some embodiments recognize that conventional motion planners can consider motion cusps by integrating a cost function with soft constraints. This can result in a significant increase in computation time.

[0020] Some embodiments recognize that, in theory, the presence of motion cusps may require gear shifting and thus increase inconvenience, but may not be a problem if large tracking errors can be tolerated. However, in practical applications requiring precise parking, such as docking operations, tracking error reduction and system robustness may be severely affected when reversing. This is especially true when multiple trailers are connected in series without active steering. In addition, frequent motion cusps and extended reversing times may lead to large and dangerous final tracking errors in docking operations.

[0021] Some embodiments are based on the recognition that the presence of motion cusps requires the articulated vehicle to move at zero and near-zero velocities, which is problematic for control accuracy because the inertia of the articulated vehicle dominates its dynamic behavior. Therefore, each motion cusp in the path of the articulated vehicle introduces uncertainty that increases with the total number of motion cusps, thereby causing tracking accuracy problems.

[0022] Some embodiments are based on the recognition that MPC-based control systems may introduce significant offsets in the trailer's lateral tracking error when performing special paths, such as those involving forward and reverse motion along lines and circular paths, in docking or parking maneuvers. Some embodiments are based on the recognition that NMPC-based control systems that utilize nonlinear vehicle models may perform even worse when there is a modeling mismatch between the controller's predictive model and the underlying system dynamics.

[0023] The objective of some embodiments is to generate a path for performing a docking operation. Some embodiments are based on the understanding that a sampling-based motion planning controller creates a graph by adding each new node based on a cost-to-go, which indicates the cost of having a path that passes through the new node. Therefore, adding a cost-to-go penalty for an excessive number of motion cusps and constraint violations is very important. However, using a soft constraint on the total number of motion cusps in the cost function can be computationally expensive for sampling-based motion planning and can lead to failure to find a feasible solution within a certain period of time. As a result, the real-time usefulness of such soft constraints is affected.

[0024] Therefore, to overcome the above problems with docking operations, some embodiments of the present disclosure disclose a multi-stage path and motion planning system that aims to find a feasible path using sampling-based motion planning during a first stage. Furthermore, after finding a feasible path, instead of terminating the planning and outputting the path to a controller, the multi-stage path and motion planning system continues to expand the graph using hard constraints related to the motion of the articulated vehicle during a second stage.

[0025] Additionally, it is an objective of some embodiments to provide sampling-based motion planning that may enable greater precision in articulated vehicle motion to perform articulated vehicle docking operations.

[0026] An objective of some embodiments of the present disclosure is to provide an integrated system comprising a graph-based motion planning algorithm and a real-time reference tracking controller, configured to perform the task of automatic articulated docking. An objective of some embodiments of the present disclosure is to provide a modified variant of the A-search guided tree (AGT) path and motion planning algorithm.

[0027] It is an objective of some embodiments of the present disclosure to disclose techniques for tracking a reference trajectory obtained from an AGT algorithm for performing a docking maneuver.

[0028] An objective of some embodiments is to provide a real-time and feasible implementation of a nonlinear model predictive controller with integral action (iNMPC) to account for the combined lateral and longitudinal dynamics of an articulated vehicle.

[0029] Some embodiments are based on the recognition that an articulated vehicle, such as a truck or tractor-trailer, alternates between forward and reverse motion as it maneuvers toward a docking bay or docking area, where a docking mechanism on the articulated vehicle connects to a docking door.

[0030] Some embodiments are based on the recognition that, unlike conventional automated driving on highways or in urban environments, docking operations are more tolerant of path tracking errors when articulated vehicles are far from the docking configuration but near the docking area within the docking bay, where both lateral position and heading error requirements are very strict.

[0031]

number

[0032] The objective of some embodiments is to calculate the trajectory and control the articulated vehicles to achieve an initial configuration q(0)∈C with zero velocity. free Starting from the final set q(t f )∈Cf⊆C free While performing the docking operation to the target docking configuration at the final time t f >0 and meet certain requirements.

[0033] Some embodiments are based on the understanding that there may be constraints on the total time T to perform a docking maneuver that need to be considered in both the planning and control phases.

[0034]

number

[0035] Some embodiments are based on the recognition that the planning and control algorithms disclosed in this disclosure are configured to meet the real-time requirements of path generation and motion control of articulated vehicles.

[0036] An objective of some embodiments is to provide a coordinated modification of the IAGT algorithm for motion planning of articulated vehicle docking operations.

[0037] An objective of some embodiments is to provide an NMPC that uses integrated tracking error for offset-free fiducial tracking to meet stringent accuracy requirements for docking operations.

[0038] It is an objective of some embodiments to provide a system that considers the effects of modeling errors, such as errors due to constant steering angle bias, errors due to inaccurate hitching offsets, and errors due to actuator dynamics, during tracking and path planning.

[0039] Accordingly, one embodiment discloses a system for controlling the motion of an articulated vehicle having one or more trailers using a motion planner. The system includes a memory configured to store computer-executable instructions and one or more processors configured to execute the instructions, the one or more processors configured to execute the instructions to collect feedback signals indicative of a state of the articulated vehicle and determine a motion path for the articulated vehicle based on the feedback signals, the motion path including one or more forward motions of the articulated vehicle and one or more reverse motions of the articulated vehicle. The one or more processors are configured to generate an optimal control problem for optimizing the motion path, the optimal control problem including an integral tracking error function for the motion path, the integral tracking error function indicating at least an integral tracking error due to one or more motion cusps, the one or more motion cusps indicating a switch between forward motion and reverse motion in the motion path. The one or more processors are configured to optimize the motion path over a prediction horizon based on solving the optimal control problem, the prediction horizon being associated with a vehicle model of the articulated vehicle having a state. The one or more processors are configured to generate control commands for the articulated vehicle based on the optimized motion path and the vehicle model, and to control motion of the articulated vehicle based on the control commands, thereby changing a state of the articulated vehicle.

[0040] The one or more processors are further configured to create a graph having a plurality of nodes defining states of the articulated vehicle over a prediction horizon. The plurality of nodes of the graph include an initial node defining a goal state of the articulated vehicle and a destination node defining the initial state of the articulated vehicle, with each pair of nodes in the graph connected by an edge defined by one or a combination of collision-free motion primitives for moving the articulated vehicle between the respective states of the connected nodes. The plurality of nodes connected through corresponding edges form a first path through the graph connecting the initial node to the destination node by a series of motion primitives for moving the articulated vehicle from the initial state to the destination state. The one or more processors are further configured to generate a motion path based on the first path formed by the graph, and, if the motion path is formed before the termination condition is met, control the motion of the articulated vehicle based on the motion path. In particular, if a second path is formed before the termination condition is met, perform an expansion of the first graph until the termination condition is met and control the movement of the vehicle based on the second path.

[0041] Some embodiments are based on the recognition that the one or more processors are configured to control movement of the vehicle based on the first route if the second route is not formed until an exit condition is met.

[0042] The one or more processors are further configured to connect a goal node of the graph to an initial configuration of the articulated vehicle by solving a linear quadratic regulator (LQR) steering problem.

[0043] The one or more processors are further configured to determine a first number of motion cusps in the series of motion primitives of the first path and a first number of initial tracking errors caused by solving the LQR steering problem. The one or more processors are further configured to, upon determining that the first number of motion cusps is greater than a predetermined motion cusp threshold or that the first number of initial tracking errors is greater than a predetermined initial tracking error threshold, expand the graph to add one or more new nodes until a termination condition is met. The expansion of the graph is subject to a constraint associated with at least one of the total number of motion cusps or the total number of initial tracking errors. The graph is expanded to form a second path by connecting the initial node to a goal node. The second path has at least a second number of motion cusps that is less than the first number of motion cusps or a second number of initial tracking errors that is less than the first number of initial tracking errors.

[0044] Some embodiments are based on the understanding that the second path includes a minimum number of motion cusps to move the articulated vehicle from the initial state to the target state. Other embodiments are based on the recognition that a key factor in successful operation is how effectively the graph connects to the initial configuration, using a predetermined initial tracking error threshold for the initial tracking error to determine the feasibility of the second path.

[0045] Some embodiments are based on the recognition that the one or more processors are further configured to create a graph to form a first path by repeatedly expanding a node of the plurality of nodes to generate a new node or by connecting two existing nodes of the plurality of nodes.

[0046] In some embodiments, to form at least one of the first path or the second path, the one or more processors further calculate a final configuration q(t f )∈C freeThe one or more processors are further configured to construct a tree of nodes starting from a target node in q(0)∈C. The one or more processors further configure the tree to match an initial configuration q(0)∈C of the articulated vehicles. free and the path segment closest to the initial configuration, calculated as the solution of a linear quadratic regulation (LQR) steering problem. In this way, the node in the tree closest to the initial configuration of the articulated vehicles is controlled to be as close as possible to the initial configuration of the articulated vehicles, resulting in a small but acceptable initial tracking error q(0) ≠ q ref A kinematically feasible reference trajectory with (0) is obtained, and this error, called the initial tracking error, along with the number of motion cusps in the motion path, may be used to determine the quality of the computed motion path.

[0047] In some embodiments, to form at least one of the first path or the second path, the one or more processors are further configured to select an expandable node from the graph based on a cost associated with the expandable node. Furthermore, the one or more processors are configured to expand the graph by adding a child node connected to the expandable node by an edge defined by a collision-free motion primitive, such that the cost of the child node is lower than the cost of the expandable node. The cost of the child node is the minimum cost to reach the goal node from the initial node through the child node. In one example, the cost of the child node includes a first cost of an initial path through a first set of nodes, a second cost of a goal path through a second set of nodes, and a third cost of a connection path between the first set of nodes and the second set of nodes.

[0048] Some embodiments are based on the recognition that the one or more processors are further configured to remove the child node from the queue while expanding the graph to form the second path if the edge between the child node and the corresponding expandable node includes a number of motion cusps greater than a predetermined motion cusp threshold.

[0049] Some embodiments are based on the recognition that the one or more processors are further configured to select expandable nodes for forming a first path without a constraint associated with the number of motion cusps. Further, the one or more processors are configured to select expandable nodes for forming a second path, wherein the expansion of the graph is subject to constraints associated with the total number of motion cusps and / or the total number of initial tracking errors and / or the total computation time of the plan when connecting the tree of nodes to the initial configuration.

[0050] Some embodiments are based on the recognition that the one or more processors are further configured to initialize the second tree with a root node that is at a distance from the target node and for cusp-free travel to the target node.

[0051] Some embodiments are based on the recognition that to avoid introducing errors close to the final configuration in the docking bay where safety and small initial tracking errors are paramount, the root node is connected to the target node using an edge that defines linear motion.

[0052] The one or more processors are further configured to generate a reference trajectory for movement of the articulated vehicle along at least one of the first path or the second path as a function of time, and to generate control commands for the articulated vehicle to cause the articulated vehicle to follow the reference trajectory.

[0053] In some embodiments, the one or more processors are further configured to generate a reference trajectory for movement of the articulated vehicle by adding a time dimension to at least one of the first path or the second path, where adding the time dimension includes adding additional time at one or more movement cusps of the reference trajectory.

[0054] The one or more processors are further configured to determine a safety region for movement of the articulated vehicle based on the generated reference trajectory, check for a dynamic obstacle within the safety region, and, if the dynamic obstacle is identified to be within the safety region, initiate an automatic emergency braking (AEB) system to perform a braking maneuver to stop movement of the articulated vehicle along the reference trajectory. The safety region includes one or more safety sets along the reference trajectory.

[0055] The path and motion planning system includes a predictive controller configured with at least a constraint function, a secondary objective function, and a vehicle model, the predictive controller configured to track the motion of the articulated vehicle.

[0056] When the predictive controller comprises one or more nonlinear constraints and a nonlinear vehicle model, the one or more processors are further configured to generate equality and inequality constraint functions for the predictive controller. The one or more processors are further configured to generate an objective function for the predictive controller, the objective function configured to minimize an error between one or more predicted states and a reference trajectory over a prediction time horizon of the predictive controller. The one or more processors are further configured to solve a constrained optimization problem associated with the predictive controller based on the equality and inequality constraint functions and the objective function using one or more iterations of sequential quadratic programming.

[0057] Some embodiments are based on the recognition that the equality constraint functions, inequality constraint functions, and objective functions for a constrained optimization problem associated with a predictive controller are tuned for at least one of the forward motion of the articulated vehicle or the reverse motion of the articulated vehicle.

[0058] Some embodiments are based on the recognition that one or more functional relationships defining the integral action for the constrained optimization problem of the predictive controller are determined differently for one or more forward motions and one or more reverse motions, and the integral tracking error state is reset upon the occurrence of one or more motion cusps.

[0059] Some embodiments are based on the recognition that the articulated vehicle comprises a tractor coupled to one or more trailers, and the integral action is implemented to integrate at least one of a lateral position tracking error of a trailing trailer when the articulated vehicle is performing one or more reverse movements, or a lateral position tracking error of the tractor when the articulated vehicle is performing one or more forward movements.

[0060] Some embodiments are based on the recognition that the predictive controller includes one or more collision avoidance constraints for enforcing one or more collision-free motion primitives in the environment while executing a series of motion primitives for moving the articulated vehicle.

[0061] Some embodiments are based on the recognition that each of the one or more collision avoidance constraints is implemented using a number of non-linear inequality constraints.

[0062] Some embodiments are based on the recognition that the articulated vehicle is a tractor-trailer vehicle comprising a tractor coupled to one or more trailers, and the articulated vehicle is controlled by a control system to perform a docking operation.

[0063] Another embodiment discloses a method for controlling motion of an articulated vehicle. The method includes collecting feedback signals indicative of a state of the articulated vehicle, determining a motion path for the articulated vehicle based on the feedback signals, and generating an optimal control problem to optimize the motion path. The motion path includes one or more forward motions of the articulated vehicle and one or more reverse motions of the articulated vehicle. The optimal control problem includes an integral tracking error function for the motion path. The integral tracking error function indicates at least an integral tracking error based on one or more motion cusps. The one or more motion cusps indicate a transition between forward motion and reverse motion in the motion path. The method further includes optimizing the motion path over a prediction horizon based on solving the optimal control problem, the prediction horizon being associated with a vehicle model of the articulated vehicle having a state. The method further includes generating control commands for the articulated vehicle based on the optimized motion path and the vehicle model, and controlling the motion of the articulated vehicle based on the control commands, thereby changing the state of the articulated vehicle.

[0064] Another embodiment discloses a non-transitory computer-readable storage medium having embedded thereon a program executable by a processor to execute a method for controlling motion of an articulated vehicle. The method includes collecting feedback signals indicative of a state of the articulated vehicle, determining a motion path for the articulated vehicle based on the feedback signals, and generating an optimal control problem to optimize the motion path. The motion path includes one or more forward motions of the articulated vehicle and one or more reverse motions of the articulated vehicle. The optimal control problem includes an integral tracking error function for the motion path. The integral tracking error function indicates at least an integral tracking error based on one or more motion cusps. The one or more motion cusps indicate a transition between forward motion and reverse motion in the motion path. The method further includes optimizing the motion path over a prediction horizon based on solving the optimal control problem, the prediction horizon being associated with a vehicle model of the articulated vehicle having a state. The method further includes generating control commands for the articulated vehicle based on the optimized motion path and the vehicle model, and controlling the motion of the articulated vehicle based on the control commands, thereby changing the state of the articulated vehicle.

[0065] Some embodiments are based on the understanding that performing a docking operation for an articulated vehicle requires complex, time-consuming, and costly labor training. Furthermore, due to limited visibility around the area associated with the docking bay, the driver of the articulated vehicle may have to perform extra movements, such as several forward and reverse movements, to accurately approach the docking bay and the final vehicle attitude. Due to the limited visibility, the docking operation may be prone to accidents. Furthermore, conventional path and motion planning systems do not automatically perform such docking operations for articulated vehicles. To overcome these limitations, the present disclosure discloses a system for controlling the movement of an articulated vehicle to perform a docking operation. Therefore, an object of the present disclosure is to enable automatic docking of an articulated vehicle having one or more trailers. An automatic docking operation may reduce the time required for docking and the probability of an accident. In this manner, complex docking operations can be performed automatically.

[0066] Embodiments of the present disclosure will be further described with reference to the accompanying drawings, in which the drawings are not necessarily to scale, emphasis instead generally being placed upon illustrating the principles of embodiments of the present disclosure. [Brief explanation of the drawings]

[0067] [Figure 1A] FIG. 1 illustrates an example block diagram of a path and motion planning system for controlling the motion of an articulated vehicle, according to some embodiments of the present disclosure. [Figure 1B] FIG. 1 illustrates an articulated vehicle having multiple trailers, according to some embodiments of the present disclosure. [Figure 1C] 1A-1C illustrate examples of automatic docking operations according to some embodiments of the present disclosure. [Figure 1D] FIG. 1 illustrates a block diagram of a system that may be used to control the movement of an articulated vehicle, according to some embodiments of the present disclosure. [Figure 1E]FIG. 1 illustrates a schematic diagram of a vehicle connected to a trailer to form an articulated vehicle, according to some embodiments of the present disclosure. [Figure 1F] FIG. 1 illustrates the difference between hard and soft constraints employed in optimization-based motion planning and control, according to some embodiments of the present disclosure. [Figure 2] FIG. 1 illustrates a block diagram of a system for controlling the motion of an articulated vehicle, according to some embodiments of the present disclosure. [Figure 3A] FIG. 1 illustrates an example method for generating a first path for controlling the movement of an articulated vehicle, according to some embodiments of the present disclosure. [Figure 3B] FIG. 10 illustrates an example of a graph structure for forming a first path, according to some embodiments of the present disclosure. [Figure 4A] FIG. 10 illustrates an example method for controlling movement of an articulated vehicle based on a second path, according to some embodiments of the present disclosure. [Figure 4B] FIG. 10 illustrates an example method for performing graph expansion during the second stage, according to some embodiments of the present disclosure. [Figure 4C] FIG. 1 illustrates an example of a node expansion method according to some embodiments of the present disclosure. [Figure 5A] FIG. 1 illustrates a graphical representation of a movement primitive according to some embodiments of the present disclosure. [Figure 5B] FIG. 10 is a diagram illustrating movement primitives as a function of time, according to some embodiments of the present disclosure. [Figure 6A] FIG. 2 illustrates an example block diagram of a linear predictive controller according to some embodiments of the present disclosure. [Figure 6B] FIG. 2 illustrates an example block diagram of a nonlinear predictive controller according to some embodiments of the present disclosure. [Figure 6C] 1A-1C illustrate examples of docking operations according to some embodiments of the present disclosure. [Figure 7A] 1A-1C illustrate examples of reference trajectories according to some embodiments of the present disclosure. [Figure 7B]FIG. 1 illustrates an example of a method for switching nonlinear program objective and constraint functions of a nonlinear predictive controller, according to some embodiments of the present disclosure. [Figure 8A] FIG. 1 illustrates an example of performing an automatic docking maneuver with automatic emergency braking, according to some embodiments of the present disclosure. [Figure 8B] FIG. 1 illustrates an example method for reinitializing a predictive controller, according to some embodiments of the present disclosure. [Figure 9] FIG. 1 illustrates an example method for controlling the motion of an articulated vehicle, according to some embodiments of the present disclosure. [Figure 10] FIG. 1 shows a schematic diagram of a system according to some embodiments of the present disclosure. DETAILED DESCRIPTION OF THE INVENTION

[0068] In the following description, for purposes of explanation, numerous specific details are set forth in order to provide a thorough understanding of the present disclosure. However, it will be apparent to those skilled in the art that the present disclosure may be practiced without these specific details. In other instances, devices and methods are shown in block diagram form solely to avoid obscuring the disclosure. It is intended that various changes may be made in the function and arrangement of elements without departing from the spirit and scope of the disclosed subject matter, as set forth in the appended claims.

[0069] As used in this specification and claims, the terms "for example," "for instance," and "such as," as well as the verbs "comprising," "having," "including," and other forms of these verbs, when used in conjunction with a list of one or more components or other items, should each be construed as open-ended, meaning that the list should not be considered to exclude further components or items. The term "based on" means based at least in part on. Furthermore, it should be understood that the style and terminology used herein are for purposes of description and should not be considered limiting. Any headings used herein are for convenience only and are not to be considered legal or limiting.

[0070] Specific details are provided in the following description for a thorough understanding of the embodiments. However, those skilled in the art will understand that the embodiments may be practiced without these specific details. For example, systems, processes, and other elements in the disclosed subject matter may be shown as components in block diagram form in order to avoid obscuring the embodiments with unnecessary detail. In other instances, well-known processes, structures, and techniques may be shown without unnecessary detail in order to avoid obscuring the embodiments. Furthermore, like reference numbers and names in the various drawings indicate like elements.

[0071] An object of some embodiments is to disclose a system for controlling the motion of an articulated vehicle in real time. In one example, the system is based on a Model Predictive Controller (MPC) for determining control inputs based on a dynamic model and constraints of the system. The system also includes a path and motion planning algorithm, for example, an A-Search Guide Tree (AGT). Another object of some embodiments is to disclose a technique for controlling the motion or operation of an articulated vehicle during an automated, i.e., non-human, docking operation. Another object of some embodiments is to disclose a technique for controlling the motion or operation of an articulated vehicle during an automated, i.e., non-human, docking operation, while avoiding collisions between the articulated vehicle and any stationary and / or dynamic obstacles in the environment, such as an initial pose q(0)∈C. free Docking posture q(t f )∈C f The present invention discloses a technique for online calculation of dynamically or kinematically feasible reference trajectories up to the target point.

[0072] FIG. 1A illustrates a block diagram of a path and motion planning system 100 for controlling the motion of an articulated vehicle, according to some embodiments of the present disclosure. In one example, the articulated vehicle is a tractor-trailer vehicle comprising a tractor coupled to one or more trailers. The articulated vehicle is controlled by the path and motion planning system 100 to perform a docking or parking maneuver with respect to the trailer. In this regard, the path and motion planning system 100 is configured to maintain a target tracking error of the trailer 124 relative to a reference trajectory 107 below a threshold during the final time period of the maneuver to successfully execute the docking or parking maneuver. Such a maneuver may be executed, for example, in a shipping yard management system, a parking lot, or the like. The path and motion planning system 100 (hereinafter referred to as system 100) is configured to execute the maneuver while avoiding collisions between the yard dog or trailer and any obstacles in the shipping yard environment.

[0073] The system 100 is configured to control the motion and movement of an articulated vehicle. In one example, the system 100 may be configured to control the motion of the articulated vehicle using a motion planner 102, a predictive controller 104, and a control system 106 while performing a docking maneuver. During a docking maneuver, the articulated vehicle may have to continuously perform forward and reverse maneuvers while maneuvering toward a docking bay. At the docking bay, the last trailer of the articulated vehicle may connect to a docking door. Once docked, goods can be loaded onto and / or unloaded from the trailer. The configuration of the articulated vehicle is described in more detail below with reference to FIG. 1B.

[0074] 1B , an articulated vehicle 120 having multiple trailers is shown in accordance with some embodiments of the present disclosure. Note that the articulated vehicle 120 comprises a tractor 122 and multiple trailers (shown as trailers 124a, 124b, 124c, 124d, and 124e, collectively referred to as trailers 124). In this regard, a first trailer 124a of the multiple trailers 124 can be connected to the tractor 122. In some examples, the trailer 124 may not have any active steering wheels. Thus, the movement and orientation of the trailer 124 may be controlled by the tractor 122. The articulated vehicle 120, i.e., the tractor 122 and the trailers 124, may need to perform a docking maneuver to dock the articulated vehicle 120 within a docking bay.

[0075] According to one example, the configuration of the articulated vehicle 120 can be expressed as q=(x0, y0, θ0, θ1, ..., θ n )∈R 2 ×(S 1 ) n+1=C. Note that other parameters can be used to define the configuration of the articulated vehicle 120 in other implementations. For example, the configuration of the articulated vehicle 120 can be defined using the relative heading angles of the trailers 124. In one example, the relative angle between the first trailer 124a and the tractor 122 can be defined as the hitching offset a, 126. For example, the hitching offset 126 can be non-zero (α≠0°), indicating a general trailer configuration of the first trailer 124a relative to the tractor 122. In some cases, the hitching offset 126 can be zero (a=0°), indicating a standard trailer configuration of the first trailer 124a relative to the tractor 122. The standard trailer configuration can be a subset of the general trailer configuration.

[0076] It should be noted that such a configuration of trailer 124 and tractor 122 is illustrative only and should not be construed as limiting. In other embodiments of the present disclosure, articulated vehicle 120 may include multiple trailers connected in series, a single trailer, or the like.

[0077] 1A , the system 100 includes a motion planner 102 connected to a predictive controller 104 and a control system 106, for example, via a state estimator 108. In one example, the predictive controller 104 is a nonlinear model predictive controller (NMPC) configured with a vehicle model 110. The vehicle model 110 may include a set of difference or differential equations that represent how the state of the articulated vehicle 120 and the measurement outputs 103 of the control system 106 change over time. In one example, the set of difference or differential equations may represent the change in the state and outputs 103 of the articulated vehicle 120 as a function of a current input 105, a previous input, and a previous output. The vehicle model 110 may further include constraints 112 that represent the physical and operational limitations of the articulated vehicle 120.

[0078] During operation, motion planner 102 generates a reference trajectory 107 for controlling the motion of articulated vehicle 120. In this regard, predictive controller 104 receives reference trajectory 107, which indicates a desired behavior of control system 106. Reference trajectory 107 may be, for example, a desired sequence of one or more motion commands. In one example, reference trajectory 107 may indicate a time function of a path that articulated vehicle 120 should follow to perform a maneuver, such as a docking maneuver. For example, motion planner 102 may generate a path based on, for example, a graph structure. Additionally, a time dimension may be added to the path to generate reference trajectory 107.

[0079] In response to receiving the reference trajectory 107, the predictive controller 104 generates a control signal (also referred to as a control input) 105 that serves as an input to the control system 106. In response to receiving the control input 105, the control system 106 updates the output 103 of the control system 106. Based on measurements of the output 103 of the control system 106 and knowledge of the control input 105, the state estimator 108 can update an estimated state 109 of the control system 106. The estimated state 109 of the control system 106 provides state feedback to the predictive controller 104. For example, the predictive controller 104 uses the estimated state 109 of the control system 106 to track the motion of the articulated vehicle 120 to ensure that the articulated vehicle 120 follows the reference trajectory 107.

[0080] Control system 106 may be any machine or device that is controlled by a particular operational control input 105. In some examples, control input 105 may be associated with a physical quantity such as voltage, pressure, force, torque, etc. For example, control input 105 may represent a control value such as steering angle, acceleration, driving speed, etc., for controlling articulated vehicle 120, as examples.

[0081] The control system 106 may be configured to generate controlled output signals 103 (also referred to as outputs 103). For example, the outputs 103 may also be associated with physical quantities such as current, flow rate, speed, or position indicating a transition of the state of the articulated vehicle 120 from a previous state to a current state. In one example, the outputs 103 may be associated in part with previous output values ​​of the control system 106 and in part with previous and current input values. Dependencies on previous inputs and previous outputs may be encoded within the state of the control system 106. During operation of the control system 106, for example, the control system 106 may generate outputs 103 indicating changes in the state of the articulated vehicle 120, e.g., changes in the precise position, orientation, or settings of the articulated vehicle 120 being controlled, based on the movement of the articulated vehicle 120 from one state to another while following the reference trajectory 107. In some examples, control inputs 105 may also be provided to actuators of articulated vehicle 120 to move articulated vehicle 120 in an automated manner based on reference trajectory 107. Output 103 may include a sequence of output values ​​generated by control system 106 following application of particular input values.

[0082] The vehicle model 110 may be used by the control system 106. The vehicle model 110 may include a set of mathematical equations that describe how the output 103 of the control system 106 may change over time as a function of current inputs, previous inputs, and previous outputs. The state of the control system 106 may correspond to any set of information, such as information that varies over time. For example, the state of the control system 106 may be a subset of current and previous inputs and outputs that, together with the vehicle model 110 and future inputs of the control system 106, can uniquely define the motion of the articulated vehicle 120 or the output 103 of the control system 106.

[0083] The control system 106 may be subject to physical limitations and specific constraints 112. The constraints 112 may be applied to limit the range of the outputs 103, inputs 105, and possibly some or all states of the control system 106, in which the control system 106 is allowed to operate. In some examples, the constraints 112 may correspond to driving speed limits, physical area restrictions associated with the environment, etc.

[0084] The predictive controller 104 may be implemented in hardware or as a software program executing on a processor, such as a microprocessor. The predictive controller 104 may receive an estimated state 109 of the control system 106 at fixed or variable control sampling time intervals. In one example, the estimated state 109 may indicate the future motion of the articulated vehicle 120 based on current and previous inputs and current and previous outputs of the control system 106. Based on the control inputs 105 provided by the predictive controller 104, actuators of the articulated vehicle 120 may be controlled to move the articulated vehicle 120. The actuators may be controlled to cause the articulated vehicle 120 to travel on a reference trajectory 107. Controlling the articulated vehicle 120 based on such control inputs 105 may cause a change in the state of the articulated vehicle 120. The predictive controller 104 can then monitor and track the motion or movement of the articulated vehicle 120 by determining the estimated state 109 based on the outputs 103 of the control system 106.

[0085] The predictive controller 104 may receive the estimated states 109 and the desired reference trajectory 107. Further, the predictive controller 104 may be configured to use the received information to determine an input, e.g., a control signal or control input 105, for operating the control system 106 to control the movement of the articulated vehicle 120. The predictive controller 104 may also track the movement of the articulated vehicle 120 based on the estimated states 109 and the reference trajectory 107.

[0086] The motion planner 102 may be implemented in hardware or as a software program executing on a processor. Such processor may be either the same or a different processor than the predictive controller 104. The motion planner 102 is configured to receive the estimated state 109 and the desired target state 101 of the control system 106 at a fixed or variable control time sampling interval. The motion planner 102 is configured to use the received information to determine a reference trajectory 107 for the predictive controller 104. In some examples, the reference trajectory 107 may be updated based on changes in the estimated state 109.

[0087] The state estimator 108 may be implemented in hardware or as a software program executing on a processor. Such a processor may be either the same as or a different processor from the predictive controller 104 or the motion planner 102. The state estimator 108 may be configured to receive the output 103 of the control system 106 at a fixed or variable control-time sampling interval. Furthermore, the state estimator 108 may use new and previous output measurements to determine an estimated state 109 of the control system 106. In this manner, the reference trajectory 107 generated by the motion planner 102 may be updated based on the estimated state 109 of the control system 106 and the control signals or control inputs 105 generated by the predictive controller 104. The updated reference trajectory may optimize the performance of a task, such as a maneuver by the articulated vehicle 120.

[0088] In some examples, the state estimator 108 may be deterministic or may propagate a probabilistic estimated state 109 of the control system 106 as particles representing moments or probabilistic distributions that depend on the received outputs 103 according to a probabilistic measurement model. For example, the state estimator 108 may be based on a linear Kalman filtering framework, a nonlinear Kalman filtering framework, and a particle filtering framework.

[0089] According to embodiments of the present disclosure, system 100 may be configured to optimize path and reference trajectory 107 generated by motion planner 102 to minimize tracking errors, including integral tracking errors. Some embodiments are based on the recognition that path tracking and control involving forward and backward motion, i.e., due to the presence of motion cusps, is challenging because any modeling errors are greatly amplified through the chain of unstable systems in nonlinear MPC (NMPC). In particular, path tracking and control is challenging for special paths corresponding to docking maneuvers, for example, having motion primitives such as linear segments and circular trajectories.

[0090] 1C illustrates an example of an automatic docking operation 130 according to some embodiments of the present disclosure. For example, an articulated vehicle 120 may need to perform a docking operation 130 to dock the rear of the articulated vehicle 120 within a docking bay 132. For example, this rear may correspond to the rear of a trailer 134, and the rear of the trailer 134 may be docked in the docking bay 132. To this end, the docking operation 130 may include a series of movements including one or more forward movements 136 and one or more reverse movements 138 to reach a desired goal configuration 140 (hereinafter referred to as goal state 140 or target state 140) in front of the docking bay. To reach target state 140, the articulated vehicle 120 may start from an initial state or configuration 142 (hereinafter referred to as initial state 142). The docking operation 130 must be performed without causing any collision between the articulated vehicle 120 and any obstacles 144 within an environment 146 of the articulated vehicle 120. Following this example of docking operation 130, since the rotation of articulated vehicle 120 may change based on collision with obstacle 144, obstacle-free environment 146 is a free subset C of articulated vehicle 120 configurations. free is different.

[0091] As previously mentioned, unlike typical automated driving on highways or in urban environments, docking maneuver 130 is more tolerant of path tracking errors when articulated vehicle 120 is far from target state 140 near docking bay 132. However, docking maneuver 130 has very strict requirements for both lateral position and heading error near the docking point within docking bay 132. This prevents damage to the docking mechanism and allows trailer 134 to be safely maneuvered into docking bay 132.

[0092] According to some embodiments of the present disclosure, the automatic docking maneuver 130 is performed based on a reference trajectory 107. The reference trajectory 107 may be calculated by the motion planner 102. For example, the reference trajectory 107 may include a forward motion 136 followed by a reverse motion 138. Some embodiments of the present disclosure are based on the recognition that, in order to enable the trailer 134 to successfully dock with the docking bay 132, the position and heading errors relative to the reference trajectory 107 need to be sufficiently small in the final stages, i.e., towards the end of the reference trajectory 107, when the docking bay 132 is approaching.

[0093]

number

[0094] Additionally, the control system 106 for controlling the movement of the articulated vehicle 120 for the automatic docking operation 130 must run on an automotive-grade embedded system platform and meet real-time requirements or constraints. In particular, the motion planner 102 may need to perform motion planning at standstill and calculate the reference trajectory 107 within a maximum computation time of 2-5 seconds. Furthermore, the predictive controller 104 may need to predict when the articulated vehicle 120 will be in motion. s It may be necessary to perform trajectory tracking continuously while moving with a sampling time of = 50 milliseconds (ms).

[0095] In one example, the system 100 may control the movement of the articulated vehicle 120 to perform a docking maneuver 130 under the assumption of normal operating conditions, i.e., non-limiting operation. The vehicle model 110 may then be based on a single-track model in which the two wheels on each axle of the articulated vehicle 120 are grouped together. Furthermore, if the articulated vehicle 120 has multiple rear axles, these multiple axles are grouped together to obtain a model with only two wheels, one at the front and one at the rear of the tractor 122. The vehicle model 110, i.e., a dynamic model, based on a balance of forces and torques, may be more accurate than a kinematic model, but the discrepancy is small when the system is moving slowly. This is typically the case for the docking maneuver 130, and the resulting small modeling error can be handled by the feedback nature of the predictive controller 106.

[0096]

number

[0097] In some embodiments of the present disclosure, the state of the articulated vehicle 120 may be used in the vehicle model 110 along with the vehicle configuration or attitude. The state of the articulated vehicle 120 may indicate, for example, the velocity, integrated tracking error, and steering angle rate of the articulated vehicle 120. The state of the articulated vehicle 120 may be denoted by X, and the time derivative of the state of the articulated vehicle 120 tracked by the predictive controller 104 may be denoted by {X}. The state of the articulated vehicle 120 may be defined based on its corresponding configuration, for example, the initial state 142 may correspond to an initial configuration q(0) of the articulated vehicle 120, and the target state 140 may correspond to a target docking configuration q(t f ) may also be used.

[0098] In one example, the motion planner 102 is based on an A-Search Guide Tree (AGT) graph. For example, the AGT graph includes multiple nodes that define the state and / or configuration of the articulated vehicle 120 over a prediction horizon. The multiple nodes of the AGT graph include an initial node that defines a goal state 140 of the articulated vehicle 120 and a target node that defines an initial state 142 of the articulated vehicle 120. According to an example embodiment of the present disclosure, the motion planner 102 can generate or create an AGT graph having multiple nodes that indicate corresponding state vectors, which may indicate, for example, but are not limited to, velocity, configuration or attitude, disturbances, and integral tracking error. In one example, transitions between nodes in the AGT graph are made based on zero-velocity boundary conditions. Each pair of nodes in the AGT graph is then connected to an edge defined by one or a combination of collision-free motion primitives as boundary conditions for moving the articulated vehicle 120 between the respective states of the connected nodes. A plurality of nodes connected through corresponding edges form a path or reference trajectory 107 through the AGT graph that connects the initial node to the destination node by a series of motion primitives for moving the articulated vehicle 120 from the initial state 142 to the destination state 140.

[0099] Continuing further, one or more road-side units (RSUs) or infrastructure sensing devices (denoted as RSUs 148a and 148b) may be used for accurate execution of the automatic docking operation 130 based on accurate sensing of the current state, the initial state 142, the target state 140, and the current environment 146 of the articulated vehicle 120 and the system 100. For example, the RSUs 148a and 148b may include one or more sensors, such as a distance rangefinder, radar, LIDAR, and / or camera, and sensor fusion techniques, to accurately detect the state of the articulated vehicle 120 and the geometry of the environment 146 surrounding the articulated vehicle 120.

[0100] In some embodiments of the present disclosure, computations for sensor fusion techniques may be performed in the cloud or on one or more mobile edge computers (MECs), which may be embedded as part of RSUs 148a and 148b or may be separate devices connected to RSUs 148a and 148b. In some embodiments of the present disclosure, communication network 149 may be used for real-time communication between articulated vehicles 120, RSUs 148a and 148b, and system 100.

[0101] Figure 1D shows a block diagram of the system 100 illustrated in Figure 1A that can be used to control the movement of an articulated vehicle 120, according to an example embodiment. Figure 1D will be described in conjunction with Figure 1A.

[0102] System 100 may include at least one processor 150, memory 152, and I / O interface 154. Memory 152 may include modules shown as motion planner 102, predictive controller 104, control system 106, and vehicle model 110. According to an embodiment, system 100 may store or retrieve data generated by processor 150 and / or other modules, such as motion planner 102, predictive controller 104, control system 106, and vehicle model 110, while performing corresponding operations from a database associated with system 100. In one example, this data may include articulated vehicle 120 information, reference trajectory 107, control inputs 105, control outputs 103, estimated states 109, navigation commands, etc.

[0103] The processor 150 may retrieve computer-executable instructions, which may be stored in the memory 152, for execution of the computer-executable instructions. The memory 152 may store computer-executable instructions that, when implemented, perform operations associated with the motion planner 102, the predictive controller 104, the control system 106, and the vehicle model 110. The memory 152 may also store the current state, current configuration, target state, target configuration, and predicted reference trajectory 107 of the articulated vehicle 100.

[0104] Processor 150 can be embodied in several different ways. For example, processor 150 may be embodied as one or more of various hardware processing means, such as a coprocessor, a microprocessor, a controller, a digital signal processor (DSP), a processing element with or without a DSP, or various other processing circuits including integrated circuits, such as, for example, an application-specific integrated circuit (ASIC), a field-programmable gate array (FPGA), a microcontroller unit (MCU), a hardware accelerator, a dedicated computer chip, etc. As such, in some embodiments, processor 150 may include one or more processing cores configured to execute independently. A multi-core processor may enable multi-processing within a single physical package. Additionally or alternatively, processor 150 may include one or more processors configured in tandem via a bus to enable independent execution of instructions, pipeline processing, and / or multithreading. Additionally or alternatively, processor 150 may include one or more processors capable of handling large workloads and operations to provide support for big data analytics. In the example embodiment, the processor 150 may communicate with the memory 152 via a bus to pass information between components of the system 100 .

[0105] Memory 152 may be non-transitory and may include, for example, one or more volatile and / or non-volatile memories. In other words, for example, memory 152 may be an electronic storage device (e.g., a computer-readable storage medium) with gates configured to store data (e.g., bits) that may be retrievable by a machine (e.g., a computing device such as processor 150). Memory 152 may be configured to store information, data, content, applications, instructions, etc. to enable system 100 to perform various functions according to example embodiments of the present disclosure. For example, memory 152 may be configured to buffer input data for processing by processor 150. As illustrated in FIG. 1D , memory 152 may be configured to store instructions for execution by processor 150. Thus, memory 152 may store motion planner 102, predictive controller 104, control system 106, and vehicle model 110. Note that memory 152 may also store other instructions that, when executed, cause other operations associated with components of system 100 to be performed, such as a state estimator, constraints, etc.

[0106] It should be noted that processor 150, whether configured by hardware methods, software methods, or a combination thereof, may represent an entity (e.g., physically embodied in circuitry) capable of performing operations according to embodiments of the present disclosure while so configured. Thus, for example, if processor 150 is embodied as an ASIC, FPGA, etc., processor 150 may be hardware specifically configured to perform the operations described herein.

[0107] Alternatively, as another example, if processor 150 is embodied as executing software instructions, the instructions, when executed, may specifically configure processor 150 to perform the algorithms and / or operations described herein. However, in some cases, processor 150 may be a processor-specific device (e.g., a mobile terminal or a fixed computing device) configured to employ embodiments of the present disclosure by further configuring processor 150 with instructions to perform the algorithms and / or operations described herein. Processor 150 may include, among other things, a clock, an arithmetic logic unit (ALU), and logic gates configured to support the operation of processor 150. A network environment, such as 130, can be accessed using I / O interface 154 of system 100. I / O interface 154 can provide an interface for accessing various features and data stored in system 100.

[0108] The processor 150 of the system 100 may be configured to determine a motion path for the articulated vehicle 120 based on the feedback signal. Additionally, the processor 150 may optimize the motion path over a prediction horizon based on solving an optimal control problem (OCP). After solving the OCP, the processor 150 may generate an optimized motion path and an optimized reference trajectory with a reduced integrated tracking error and a reduced number of motion cusps. The processor 150 may generate control commands for the articulated vehicle 120 based on the optimized reference trajectory. The articulated vehicle 120 may be controlled to perform a docking maneuver 130 based on the control commands.

[0109] The memory 152 of the system 100 may be configured to store datasets related to the articulated vehicle 120, such as, but not limited to, state and configuration data related to the articulated vehicle 120, target states, initial and / or current states, sensor data, reference trajectories, optimized reference trajectories, predetermined motion cusp thresholds, predetermined initial tracking error thresholds, optimal control problems, and map data. According to an embodiment, the memory 152 may include processing instructions for processing the stored data. The datasets may include real-time and historical data related to the articulated vehicle 120 and / or the docking operation 130.

[0110] In some example embodiments, I / O interface 154 may communicate with system 100 and display inputs and / or outputs of system 100. To that end, I / O interface 154 may include a display, and in some embodiments, may also include a keyboard, a mouse, a joystick, a touchscreen, a touch area, soft keys, one or more microphones, multiple speakers, or other input / output mechanisms. In one embodiment, system 100 may include user interface circuitry configured to control at least some functionality of one or more I / O interface elements, such as a display and, in some embodiments, multiple speakers, a ringtone, one or more microphones, and / or the like. Processor 150 and / or I / O interface 154 circuitry comprising processor 150 may be configured to control one or more functions of one or more I / O interface 154 elements through computer program instructions (e.g., software and / or firmware) stored in memory 152 accessible to processor 150. The processor 150 may further render notifications associated with the articulated vehicle 120 and / or the docking operation 130, such as gear changes, ETAs, warnings, etc., via the I / O interface 154 to an audio or display onboard the user equipment or the articulated vehicle 120.

[0111] In some embodiments, processor 150 may be configured to provide Internet of Things (IoT) related functionality to users of system 100 disclosed herein. IoT related functionality can then be used to provide smart city solutions, such as by using cloud-based mapping systems to provide real-time alerts, big data analytics, and sensor-based data collection to provide accurate navigation instructions and ensure driver safety. I / O interface 154 can provide an interface for accessing data and various features stored in system 100.

[0112] During operation, the processor 150 may collect feedback signals. In one example, the motion planner 102 may collect feedback signals indicative of the state of the articulated vehicle 120. For example, the feedback signals may indicate the current state of the articulated vehicle 120. Following the docking operation 130, the current state of the articulated vehicle 120 may correspond to an initial state 142 during the initiation of the docking operation 130. In one example, such feedback signals may be obtained from the control system 106 and / or the predictive controller 104, which track the motion of the articulated vehicle 120.

[0113] Further, the processor 150 may determine a motion path for the articulated vehicle 120. In one example, the motion planner 102 may determine a motion path for the articulated vehicle 120 based on the feedback signal. The motion path includes one or more forward motions of the articulated vehicle 120 and one or more reverse motions of the articulated vehicle 120. For example, the motion path having a time dimension may be a reference trajectory 107 for performing the docking maneuver 130. In one example, the motion path may be a dynamically feasible collision-free path. However, some motion cusps caused by switching between successive forward-reverse-forward motions may affect the control of the articulated vehicle 120 while performing the docking maneuver 130. In particular, reverse motions at motion cusps may significantly amplify the effects of modeling errors, making it impossible to accurately control and track the articulated vehicle 120 during such docking maneuver 130.

[0114] To overcome the above problems, the predictive controller 104 may be configured to track the motion path or reference trajectory generated by the motion planner 102 so that the reference trajectory takes into account modeling errors in the backward motion and enables the complex docking maneuver 130 to be reliably performed. In this regard, the processor 150 may generate an optimal control problem (OCP) for optimizing the motion path. For example, the predictive controller 104 may generate an OCP for optimizing the motion path such that the optimized motion path is tracked. The optimal control problem may include, for example, an integral tracking error function for the motion path. For example, the integral tracking error function may be a quadratic objective function that indicates at least the integral tracking error based on one or more motion cusps present in the motion path or reference trajectory over a prediction horizon of length N. It will be appreciated that one or more motion cusps indicate a switch between forward and backward motion in the motion path. In one example, the predictive controller 104 may be implemented by a nonlinear model predictive controller with integral action (iNMPC), where the integral tracking error function is introduced into the iNMPC predictive model.

[0115] Further, the processor 150 can optimize a motion path over a prediction horizon based on solving the OCP. For example, the motion planner 102 can optimize the motion path by solving the OCP. The prediction horizon may be associated with the vehicle model 110 of the articulated vehicle 120 having states. In one example, the OCP may be solved by a suitable implementation of sequential quadratic programming (SQP), known as a real-time iteration (RTI) scheme. For example, an RTI algorithm may perform one SQP iteration per control time step and use a continuation-based warm start of the state and control trajectory from one time step k to the next k+1 to solve the OCP.

[0116] After solving the OCP, the motion planner 102 can generate an optimized motion path and an optimized reference trajectory with a reduced number of integrated tracking errors. Furthermore, the processor 150 can generate control commands for the articulated vehicle 120. In one example, the control system 106 may generate control commands for the articulated vehicle 120 based on the optimized motion path and the vehicle model 110. The processor 150 can control the motion of the articulated vehicle 120 based on the control commands. In one example, the control system 106 can control the motion of the articulated vehicle 120 based on the control commands, thereby changing the state of the articulated vehicle. Furthermore, the predictive controller 104 can track the motion of the articulated vehicle 120 while following the optimized motion path. In this manner, the articulated vehicle 120 can be moved from the initial state 142 to the target state 140 to perform a docking maneuver.

[0117] Therefore, including an integral tracking error function in the optimal control problem (as in iNMPC) reduces the impact of modeling errors during path and motion tracking of the articulated vehicle 120. Additionally, steering angle bias and tracking error are significantly smaller when using iNMPC in the forward direction. As a result, reliable and efficient path and motion tracking can be achieved during reverse motion with iNMPC. In particular, the integral tracking error function used in the optimal control problem employed in iNMPC reduces tracking errors, such as integral tracking error, in both forward and reverse motion, and even more so in reverse motion. In particular, iNMPC is effective in addressing steering angle bias, hitching offset, and other considered modeling errors. Furthermore, including the integral tracking error function significantly reduces trailer final error, thereby enabling accurate and reliable final lateral tracking during docking maneuvers.

[0118] FIG. 1E illustrates a schematic diagram 155 of a vehicle connected to a trailer forming an articulated vehicle 120, according to some embodiments of the present disclosure. According to this example, the vehicle may be a tractor 122, and the trailer may be a trailer 124 or a trailer 134. The articulated vehicle 120 may include or be connected to the system 100. While the articulated vehicle 120 is shown as a tractor-trailer, this is by way of example only and should not be construed as limiting. In other embodiments of the present disclosure, examples of the articulated vehicle 120 may include, but are not limited to, a car-trailer, a heavy vehicle, a tractor-trailer, a bus, and a rover. Furthermore, the techniques disclosed in the present disclosure for performing an automatic docking operation 130 are not limited to articulated vehicles, but may be applied to any autonomous or semi-autonomous vehicle, such as a car, a bus, a truck, etc.

[0119] According to some embodiments of the present disclosure, techniques are disclosed for controlling movement of the tractor 122. Examples of such movement may include, but are not limited to, lateral movement of the tractor 122 controlled by the steering system 162. In one embodiment, the steering system 162 is controlled by the system 100.

[0120] The tractor 122 may include an engine 164 that can be controlled by the system 100 or by other components of the tractor 122. The tractor 122 may also include one or more sensors 156 for sensing the surrounding environment 146. Examples of the sensors 156 may include, but are not limited to, a distance rangefinder, radar, LIDAR, and a camera. The tractor 122 may also include one or more sensors 158 for sensing the current momentum and internal status of the tractor 122. Examples of the sensors 158 may include, but are not limited to, a global positioning system (GPS), an accelerometer, an inertial measurement unit, a gyroscope, a shaft rotation sensor, a torque sensor, a deflection sensor, a pressure sensor, and a flow sensor. The sensors 156 and 158 can provide information to the system 100. The tractor 122 may also include a transceiver 160 that enables communication functions of the system 100 through wired or wireless communication channels.

[0121] For example, the system 100 may control the operation or movement of the tractor 122. In this regard, the system 100 may generate a reference trajectory 107 for movement of the tractor 122 to accomplish a task. Additionally, the system 100 may generate motion commands for the tractor 122 based on the reference trajectory 107 to follow. Additionally, the system 100 may control components, such as a steering system 162 of the tractor 122, to configure the tractor 122 to perform the task. In one example, the task may be a docking maneuver 130, and the motion commands may include a series of forward and reverse movements.

[0122] FIG. 1F shows an example diagram 166 illustrating no constraints, hard constraints, and soft constraints associated with motion primitives for motion planning, according to some embodiments. Note that vehicle path planning plays an important role in vehicle navigation systems. Therefore, path and motion planning are essential tasks for the navigation of autonomous vehicles. In particular, map data and sensor-based data form the basis for generating trajectories, such as reference trajectory 107, which serves as a target value tracked by predictive controller 104. When generating reference trajectory 107, kinematic feasibility and possible collisions are considered, in addition to comfort aspects.

[0123] During path planning and trajectory generation, constrained optimization may need to be performed. Therefore, vehicle motion must be planned and adjusted to accomplish the driving task while respecting the constraints introduced by the selected vehicle model. In one example, a motion planning layer is responsible for calculating a safe, comfortable, and dynamically feasible trajectory based on the vehicle's current configuration and state and its target configuration and state. The target configuration may vary depending on the situation. For example, for a docking operation 130, the target location may be a docking bay 132, where the rear end of the last trailer 124e or 134 of the articulated vehicle 120 is close enough to the docking bay 132 to facilitate loading or unloading.

[0124] The path planning problem is to find a path σ(α):[0,1]→X in state space for the articulated vehicle 120 (or more generally, the robot) that starts from an initial state 142 or configuration of the articulated vehicle 120 and reaches a goal state 140 or configuration while satisfying the constraints 112. Solving the path planning problem generates a dynamically feasible path or trajectory. The generated trajectory must be smooth and free of extreme turns because the robot has several motion constraints, such as nonholonomic states in the under-actuated system, which become more restrictive as the number of trailers 124 increases. Feasible path planning is the problem of determining a path that satisfies several given problem constraints (i.e., hard constraints) related to obstacle avoidance, without focusing on the quality of the solution.

[0125] However, while generating a feasible path ensures obstacle-free navigation, the quality of the generated path may be affected due to imposed hard constraints. For example, the path or trajectory may have sharp turns, longer routes with poorer quality, a greater number of motion cusps, etc. As shown in FIG. 1E, a path planning problem with hard constraints 168 on the motion primitives may hinder the efficiency of the generated path.

[0126] For this reason, it is important not to apply any constraints 112 in dynamic optimization, as they reflect safety and quality limits. However, no constraints 170 can result in infeasible paths without efficiently overcoming obstacles.

[0127] Therefore, optimal path planning is needed. Optimal path planning is the problem of finding a path that optimizes some quality criterion given some constraints. The problem of finding an optimal path is difficult, and there may not be an efficient algorithm that can solve all cases of the problem while satisfying the constraints 112. Therefore, it may be necessary to impose soft constraints 172 to generate an optimal path.

[0128] Soft constraints are conditions imposed on the trajectory generated by a solution that may have to be met, rather than not being met, but that are prepared to be tolerated because of the cost of meeting them or because of conflicts with other constraints or goals. When there are multiple soft constraints, it is necessary to determine which of the various constraints should take precedence if there are conflicts between the soft constraints themselves, or if meeting them proves to be costly.

[0129] According to the present disclosure, to ensure path accuracy, soft constraints 172 are imposed on one or more motion cusps in the generated path. An embodiment of the present disclosure describes imposing soft constraints 172 based on an integrated tracking error associated with one or more motion cusps in the generated path.

[0130] The manner in which the path and motion planning system 100 operates to generate paths with soft constraints associated with motion cusps is described in detail with the following figures.

[0131] 2 illustrates a block diagram of a system 100 for controlling the movement of an articulated vehicle 120, according to some embodiments of the present disclosure. It will be appreciated that autonomous and semi-autonomous vehicles are complex systems that require the integration of a high degree of interconnected sensing and control elements. Autonomous control of a vehicle is complex due to the complexities associated with developing the software and hardware elements of an autonomous vehicle, as well as dynamic driving conditions.

[0132] According to an embodiment, the system 100 for controlling the motion of an articulated vehicle 120 may include a path and motion planning layer 202 and a vehicle controller layer 204. For example, the path and motion planning layer 202 may be implemented by the motion planner 102, and the vehicle controller layer 204 may be implemented by the predictive controller 104.

[0133] In one example, the path and motion planning layer 202 calculates a reference trajectory 107 and provides the reference trajectory 107 to the vehicle controller layer 204. Additionally, the vehicle controller layer 204 calculates control inputs 105 for the control system 106 to track the reference trajectory 107. In some cases, the control system 106 may update the reference trajectory 107 to optimize vehicle motion during the docking maneuver 130. The vehicle controller layer 204 can then execute a desired sequence of one or more motion commands.

[0134] According to some embodiments of the present disclosure, the layers of the system 100 may also include a decision layer 206 and / or an actuator controller layer 208.

[0135] In operation, a series of destinations in the form of a route may be calculated through a road network by a route planner (not shown). For example, the route planner may use map-based techniques to generate a route between a source location and a destination location. Such a route is then navigated automatically or semi-automatically by articulated vehicle 120. In accordance with the present disclosure, the route may be traveled automatically by articulated vehicle 120. The route may also include docking maneuvers, parking maneuvers, etc., before reaching the destination location.

[0136] Given a route, the decision layer 206 may be responsible for determining one or more local driving goals (or target goals) corresponding to a target location and for making corresponding discrete decisions 210 for the articulated vehicle 120. Each of the discrete decisions 210 may be a desired sequence of one or more motion commands. Examples of motion commands may include, but are not limited to, a right turn, a no lane change, a left turn, a lane change, or a complete stop at a specific location. In this regard, several sensing and mapping modules may use information from one or more sensors 156 and 158, such as radar, LIDAR, inertial measurement unit (IMU), camera, and / or global positioning system (GPS) information, along with previous map information, to estimate an estimated state from the current state of the articulated vehicle 120 and surrounding portions of the environment 146 associated with the system 100 and the articulated vehicle 120 for a particular driving scenario. One, multiple, or all layers of the system 100 may utilize sensing and mapping modules.

[0137] Based on one or more driving goals and corresponding discrete decisions 210, the path and motion planning layer 202 is responsible for determining a reference trajectory 107, which is provided to the vehicle controller layer 204. According to some embodiments of the present disclosure, the reference trajectory 107 is a safe, desirable, and dynamically feasible trajectory for the articulated vehicle 120 to follow. The reference trajectory 107 may be determined based on the output of the sensing and mapping module, i.e., one or more sensors, and the output 103 of the control system 106. Some embodiments are based on the recognition that an important requirement of the reference trajectory 107 calculated by the path and motion planning layer 202 is that the reference trajectory 107 be collision-free, dynamically feasible, and capable of being tracked by the predictive controller 104 in the vehicle controller layer 204. In other words, the reference trajectory 107 avoids any collision with any stationary or dynamic objects in the environment 146 and achieves or reaches the one or more driving goals while taking into account the vehicle model 110 of the control system 106, which may be represented by a set of mathematical equations.

[0138] Some embodiments of the present disclosure are based on the recognition that a typical limiting factor in path and motion planning tasks is the non-convexity of the constraints of the dynamic optimization problem. As a result, only local optimal solutions are achieved, which may differ significantly from the global optimal solution. In some cases, solving the optimization problem involves utilizing significant computational load and time to find only feasible solutions. According to some embodiments, path and motion planning tasks may be performed using sampling-based methods such as rapidly exploring random trees (RRTs), or graph search methods such as A-star, D-star, and other variants of these algorithms.

[0139] Continuing further, the vehicle controller layer 204 aims to achieve the nominal trajectory 107 by calculating control signals or control inputs 105 for operating the control system 106, taking into account the vehicle model 110 and constraints 112. The control inputs 105 may include one or more actuation commands, such as steering angle, wheel torque, and brake force values. In some embodiments of the present disclosure, the vehicle controller layer 204 provides the control inputs 105 to an additional layer of one or more controllers in an actuator controller layer 208. The actuator controller layer 208 directly adjusts the actuators to achieve the desired movement of the articulated vehicle 120.

[0140] Different embodiments of the present disclosure may use different techniques in the vehicle controller layer 204 to track the reference trajectory 107 calculated by the path and motion planning layer 202. In some embodiments of the present disclosure, a model predictive controller (MPC) is used in the vehicle controller layer 204 so that future information in the long-term reference trajectory 107 calculated by the path and motion planning layer 202 can be effectively used to achieve a desired movement or operation of the articulated vehicle 120.

[0141] In some embodiments of the present disclosure, a linear model predictive controller (LMPC) may be used in the vehicle controller layer 204. The LMPC may be the result of a linear vehicle model 110 used in combination with linear constraints 112 and a quadratic objective function to track the reference trajectory 107 calculated by the path and motion planning layer 202. In other embodiments of the present disclosure, one or more of the constraints 112 and / or the objective function may be nonlinear and / or the equations of the vehicle model 110 describing the vehicle state behavior may be nonlinear, resulting in a nonlinear model predictive controller (NMPC) that tracks the reference trajectory 107 calculated by the path and motion planning layer 202.

[0142] Some embodiments of the present disclosure are based on the recognition that the path and motion planning layer 202 can compute long-term, highly predictive motion plans, but must typically run at a slow sampling frequency to do so. In some embodiments of the present disclosure, the path and motion planning layer 202 is run once for each desired sequence of one or more motion commands. For example, the path and motion planning layer 202 may be run for an automatic docking operation 130, as shown in FIG. 1C . In some embodiments of the present disclosure, the path and motion planning layer 202 may be run multiple times for each desired sequence of one or more motion commands to perform the automatic docking operation 130, for example, to allow for re-planning when the surrounding environment 146 of the articulated vehicle 120 changes substantially.

[0143] Some embodiments of the present disclosure are based on the recognition that the predictive vehicle controller layer 204 can track the reference trajectory 107 by calculating the control inputs 105 over a short prediction horizon while running at a high sampling frequency. For example, the vehicle controller layer 204 may use a prediction horizon of 1-10 seconds, and the vehicle controller layer 204 may run at a sampling frequency of 10-100 times per second. The vehicle controller layer 204 may be highly sensitive to local deviations in the vehicle state estimation due to, for example, obstacle-related uncertainties in the articulated vehicle's 120 environment 146, as well as other uncertainties in the sensing and mapping modules.

[0144] In some embodiments of the present disclosure, different vehicle models may be used in different components within the multi-tier system 100 to control the motion of the autonomous or semi-autonomous articulated vehicle 120. For example, a simple but computationally inexpensive kinematic model may be used in the path and motion planning layer 202, while a relatively accurate but computationally more expensive dynamic single-track or double-track vehicle model may be used in the vehicle controller layer 204.

[0145] It should be noted that information can be shared between different components or layers within the multi-tiered system 100 for autonomous or semi-autonomous control of the articulated vehicle 120. For example, information 212 regarding one or more sensor outputs, map data, route, and vehicle surroundings may be shared between the decision layer 206 and the path and motion planning layer 202. Further, information 214 regarding one or more sensor outputs, map data, route, and vehicle surroundings may be shared between the path and motion planning layer 202 and the vehicle controller layer 204. Further, information 216 regarding one or more sensor outputs, map data, route, and vehicle surroundings may be shared between the vehicle controller layer 204 and the actuator controller layer 208. Additionally, some embodiments of the present disclosure are based on the recognition that reliability and safety in the control of the articulated vehicle 120 can be improved by using diagnostic information, such as performance metrics of the success and / or failure of an algorithm in one component, which can be shared with components of the multi-tiered system 100.

[0146] In one embodiment, a two-phase approach may be used in the path and motion planning layer 202 to create a graph for generating a path for performing the docking maneuver 130. The graph may include a set of nodes representing states and a set of edges. In one example, the graph may include a root node representing a goal state 140 or configuration of the articulated vehicle 120. Note that the goal state 140 or configuration may correspond to a docking bay 132. The graph may include multiple nodes that define the state of the articulated vehicle 120 over a prediction horizon of, for example, 1 to 10 seconds. The multiple nodes of the graph may include an initial node that defines the goal state 140 of the articulated vehicle 120 and a goal node that defines the initial state 142 of the articulated vehicle 120. Furthermore, each pair of nodes in the graph is connected to an edge that is defined by one or a combination of collision-free motion primitives that move the articulated vehicle 120 between the respective states of the connected nodes. As used herein, any node or edge of the graph may include position and heading information for the articulated vehicle 120. A set of nodes and a set of edges may be collision-free and may represent kinematically or dynamically feasible transitions between states, and may represent all states in which articulated vehicle 120 does not collide with any obstacles in environment 146.

[0147] In one example, an AGT algorithm may be used to create the graph. Further, a goal node of the initial state 142 of the graph may be connected to an initial configuration of the articulated vehicle 120 by solving a linear quadratic regulator (LQR) steering problem. In particular, based on solving the LQR steering problem, edges that are potential solutions to the steering problem are calculated. For example, because the articulated vehicle 120 is believed not to allow a closed-form analytical solution to the steering problem in a typical setting, the steering problem may be solved numerically rather than analytically. Further, based on the edges, a goal node may be connected to the initial configuration when it is sufficiently close to the initial state 142.

[0148] The motion planner 102 is configured to generate a motion path based on which a reference trajectory 107 is generated that is based on a graph or a first path formed in the graph. In particular, a plurality of nodes connected through corresponding edges form a first path through the graph that connects an initial node to a destination node by a series of motion primitives for moving the articulated vehicle 120 from an initial state 142 to a destination or goal state 140.

[0149] Additionally, control system 106 may control the movement of articulated vehicle 120 based on the motion path if the motion path is formed before an end condition, such as within a predetermined time limit, is met. How motion planner 102 calculates a path for controlling the movement of articulated vehicle 120 to perform docking maneuver 130 and / or how control system 106 optimizes the generated first path to reduce tracking error is described in detail below.

[0150] 3A illustrates an example method 300 for generating a first path for controlling the movement of an articulated vehicle 120, according to some embodiments of the present disclosure. In one example, the movement of the articulated vehicle 120 may correspond to a docking operation from an initial state 142 or configuration to a target state 140 or configuration. For example, for the environment 146 disclosed in FIG. 1C , the initial state 142 may correspond to a starting configuration of the articulated vehicle 120, and the target state 140 corresponds to a position of the articulated vehicle 120 corresponding to the docking bay 132.

[0151] It should be noted that system 100 is capable of interacting with environment 146 to receive sensor data corresponding to the state of articulated vehicle 120 and surrounding environment 146. In certain examples, articulated vehicle 120 is configured with system 100 for controlling its movement in an autonomous manner. For example, system 100 may include one or more components for determining a first path of articulated vehicle 120 and further controlling its movement to perform a particular operation, such as a docking or parking operation. According to an embodiment of the present disclosure, the operation of articulated vehicle 120 may correspond to a docking maneuver 130. However, such an operation should not be construed as limiting, and system 100 may be used to perform other operations, such as cruise control, hitching, parking, etc. For example, system 100 may provide a set of movement commands for controlling the movement of articulated vehicle 120 based on the first path. In one example, the system 100, and in particular the motion planner 102, may utilize an A-Search Guide Tree (AGT) algorithm to generate a first path for the articulated vehicle 120 to cause the articulated vehicle 120 to perform a docking maneuver. Method 300 describes using the AGT algorithm to generate a path and perform a docking maneuver in an effective and efficient manner.

[0152] In one example, the motion planner 102 of the system 100 may generate the first motion path and reference trajectory 107 by performing the steps of the method 300 .

[0153] It will be understood that the AGT path and motion planning algorithm is a variation of the A-star based algorithm. The AGT algorithm may have the capability to efficiently calculate kinematically feasible solutions for automated vehicle parking tasks. In accordance with an embodiment of the present disclosure, the described docking maneuver 130 can be considered as a specialized parking task with very stringent requirements on the final stage of the maneuver in terms of lateral position error and heading angle error.

[0154] According to an embodiment of the present disclosure, the AGT algorithm is a hybrid A-star algorithm. Given a configuration or pose of the articulated vehicle 120, including its position and heading angle, the AGT algorithm searches for a solution that prioritizes control actions at each node to balance optimality of the calculated motion path and computational efficiency. The AGT algorithm also defines a subgraph, i.e., a final docking configuration q, with a set of nodes with associated state vectors Xi. ref (t f ) to the goal node for the initial configuration q(0), also called the goal tree.

[0155] In one example, a tree of a graph may be defined as the union of a set of nodes and a set of edges. In this regard, a tree can be defined as T = (V,E), where V is a set of nodes and E is a set of edges. To this end, E(X i ,X f ) is the state X i and X f represents the possible collision-free trajectory between free is implicitly obtained by nucleating collisions with stationary obstacles, such as obstacle 144, in the environment 146. M represents a finite set of pre-computed motion primitives from the available control actions, and V max It can be assumed that represents the maximum number of nodes allowed in the graph.

[0156] According to some embodiments of the present disclosure, tailored modifications are made to generate the first path using the AGT algorithm to improve the performance of the AGT algorithm for path and motion planning in the automatic docking operation 130 and to improve the performance of the predictive controller 104 for tracking the resulting reference trajectory 107.

[0157] At 302, a tree of nodes is created from the initial node. The initial node of the tree is the target or goal state 140, X f, and the goal node of the tree corresponds to a state close to the initial state 142 of the articulated vehicle 120. In one example, the tree (also called a goal tree) corresponds to the goal state 140, X f Therefore, kinematic infeasibility is mainly caused by the initial tracking error, and can be suppressed before the target state 140 is reached if the initial tracking error is small enough.

[0158] At 304, a graph is created to form a first path. According to some embodiments, the graph for the first path may aim to minimize path cost without constraining the motion primitives for moving from the initial state 142 to the goal state 140. In this regard, the system 100 may utilize the motion planner 102 corresponding to the path and motion planning layer 202 to generate the first path.

[0159] 3B illustrates an example graph structure 310 for forming a first path, according to some embodiments. According to some embodiments, the graph structure 310 may have multiple nodes, shown as nodes 312a-312i. The nodes 312a-312i are used to determine the state X of the articulated vehicle 120. i and X f The plurality of nodes 312a to 312i of the graph 310 define the target state X f and the initial state X of the articulated vehicle 120. i and a target node 312i corresponding to the node 312i.

[0160] Target state X fmay correspond to a docking configuration. It will be appreciated that each pair of nodes 312a-312i in graph 310 is connected to an edge (shown as edges 314a-314h) defined by one or a combination of collision-free motion primitives for moving articulated vehicle 120 between the corresponding respective states of the connected nodes. Additionally, each node 312a-312i may include a number of motion cusps representing the number of motion cusps in the path from either 312a or 312i to the particular node. For example, multiple nodes 312a-312i connected via corresponding edges 314a-314h may be used to generate a first path through graph 310 connecting initial node 312a to destination node 312i by a series of motion primitives for moving articulated vehicle 120 from initial state Xj to destination state Xf.

[0161] In one example, the graph 310 may be created by iteratively expanding a node of the plurality of nodes 312 a-312 i to generate a new node, or by connecting two existing nodes of the plurality of nodes 312 a-312 i. The new node and the expanded node, or the two existing nodes, may be connected with corresponding edges that represent collision-free motion primitives for moving the articulated vehicle 120.

[0162] In particular, an AGT algorithm may be employed by the motion planner 102 to create the graph 310. In one example, the AGT may create a tree 316 of nodes from a target configuration to an initial configuration of the articulated vehicle 120. In another example, a tree 318 may be created from the initial configuration to the target configuration. In another example, two simultaneous trees 316 and 318 may be generated. As a bidirectional AGT (BIAGT), tree 318 may start from the initial configuration and tree 316 may start from the target configuration. In yet another example, tree 316 may be created from the target configuration until it is sufficiently close to the initial configuration at a distance represented on a configuration manifold. In this manner, a first path 320 may be formed based on tree 316 and / or tree 318. Once close enough, for example, when tree 316 reaches node 312g, tree 316 is connected to the initial configuration by solving a linear-quadratic regulation (LQR) steering problem. Thus, edge 314h is not a motion primitive, but rather a solution to the LQR steering problem computed online. Because articulated vehicle 120 does not allow for a closed-form analytical solution to the steering problem in the general setting, the solution to the LQR steering problem may be found numerically rather than analytically.

[0163] In one example, the expansion of the tree 320 may be based on a cost function F(·), which sums the heuristic value h(·) and the arrival cost g(·). In one example, the heuristic value of a node in the AGT algorithm is calculated based on a first cost-to-go estimated by a neural network module and a second cost-to-go that represents the Reeds-Shepp (RS) path length from the current node toward the initial configuration of the articulated vehicles 120 while ignoring obstacles 144.

[0164] In one embodiment, the heuristic value h(·) is the weighted sum of the first cost-to-go and the second cost-to-go. The arrival cost g(·) of a node is the sum of the costs of all edges 314a-314h that connect the target configuration to node 312g that is close to the initial configuration. In one embodiment, the RS path of the articulated vehicle 120 is calculated as the RS path of the tractor 122.

[0165] In one embodiment, the AGT algorithm is modified to generate a second path based on the first path 320. In this regard, the second path is generated based on hard constraints associated with the number of motion cusps and the total number of initial tracking errors allowed in a connection path between two states. In such a case, the connection path of motion primitives between two states for generating the second path may have a minimum number of motion cusps and a minimum number of initial tracking errors. According to some embodiments, the second path may be generated based on a hard constraint associated with the number of motion cusps and the total number of initial tracking errors allowed in a connection path between two states, e.g., X i and X j The motion primitives between may represent the construction of a shortest distance path. Such an embodiment relating to the generation of a second path is described in detail below with reference to Figures 4A, 4B, and 4C. In some embodiments, the initial state X i The motion primitives to be applied in may not introduce additional cusps or tracking errors, i.e., the target node 312i or the initial state X i The velocity of the motion primitive to be applied is calculated from its parent node 312g to the initial state X i It is possible that the velocity of the motion primitive reaching

[0166] If the first path 320 is successfully generated, the system 100, and in particular the motion planner 102, may generate the first path 320 without constraining the motion primitives corresponding to the movement of the articulated vehicle 120. The cost associated with the first path 320 may be minimized. However, if the total number of nodes 312a-312i in the tree 316 and / or 318 is greater than V, the total number of nodes 312a-312i in the tree 316 and / or 318 may be greater than V. maxis reached and a solution trajectory, i.e., first path 320, has not been found, path planning fails. For example, if path planning fails, generation of the first path may be restarted, or control of articulated vehicle 120 may be transferred to manual steering.

[0167] Based on the generated first path 320, a reference trajectory 107 may be generated as a function of time to facilitate movement of the articulated vehicle 120 along the first path 320. Control commands for the articulated vehicle 120 may then be generated based on the reference trajectory 107 such that the articulated vehicle 120 follows the reference trajectory 107. Thus, the modified AGT algorithm employed by the motion planner 102 may improve the performance of the predictive controller 104 to track the reference trajectory 107 of the articulated vehicle 120 and to enable a successful docking operation 130. Example embodiments of the modified AGT algorithm used by the system 100 or motion planner 102 for controlling movement of the articulated vehicle 120 to perform the docking operation 130 are defined in detail below.

[0168] 4A illustrates an example method 400 for controlling movement of an articulated vehicle 120 based on a second path, according to some embodiments of the present disclosure. For example, the movement of the articulated vehicle 120 may correspond to a docking operation 130 from an initial configuration or state 142 to a target configuration or state 140. In one example, the steps of method 400 may be performed after generation of the first path 320 described in FIGS. 3A and 3B.

[0169] According to some embodiments, path and motion planning for an automatic docking maneuver 130 is described herein. In this regard, the AGT algorithm is modified to improve the performance of the predictive controller 104 for tracking the resulting reference trajectory 107. The modified AGT algorithm is configured to continue expanding the graph 310 to generate a second path even after generating a first path 320 if the first path does not meet a criterion for path quality, for example, if the number of motion cusps in the first path exceeds a threshold or the number of initial tracking errors exceeds a threshold. The second path also continues to grow a tree 316, grown backward in time from an initial node 312a, to an initial state 142 (X i ) to the goal node 312i. A second path is created in this method 400.

[0170] At 402, a first number of motion cusps can be determined in the series of motion primitives of the first path 320. In particular, a motion cusp indicates a switch between forward and reverse motion in the series of motion primitives. As used herein, a motion cusp refers to a switch from forward motion to reverse motion or a switch from reverse motion to forward motion in the series of motion primitives defining the first path 320.

[0171] Conventional motion planners, which typically seek to obtain the shortest path, may provide solutions with unnecessary motion cusps. This is primarily because the cost-to-go guide tree construction does not consider the number of motion cusps. Such motion cusps can impair the integrated path and positioning accuracy of the system 100. Motion cusps require the articulated vehicle 120 to move at zero and near-zero velocities, and such motion is problematic for the accuracy of the path and motion planning system 100 because the inertia of the articulated vehicle 120 dominates its dynamic behavior.

[0172] Additionally, for heavy-duty vehicles (HDVs), gear shifting is required at each motion cusp, which also incurs significant time costs. Therefore, each motion cusp in the first path 320 of the articulated vehicle 120 amplifies modeling errors and disturbances that accumulate with the number of motion cusps, creating accuracy issues and increasing the overall time of the docking operation 130.

[0173] In some cases, conventional planners for nonholonomic systems may introduce small residuals in the nonholonomic constraints of multiple articulated vehicles. In other words, the motion planner 102 may have relaxation or numerical errors that can cause error transients. Because the articulated vehicles 120 are inherently robust when moving in a forward direction, these errors are suppressed if they occur early in the maneuver. However, error suppression is much slower when the articulated vehicles 120 are operating in a reverse or reversing direction. This is particularly challenging for docking maneuvers 130, which typically end with a long reverse maneuver. Furthermore, due to the reverse or reversing maneuver at the maneuver cusp, modeling errors due to, for example, steering angle bias, inaccurate hitching offsets, and actuator dynamics may affect the tracking of the first path 320. As a result, accurately following the first path 320 may be difficult.

[0174] It is therefore understood that in one embodiment of the present disclosure, small planning errors can be tolerated if they occur early in the docking operation 130, for example, near the initial state 142 of the articulated vehicle 120 or near the target node 312i. Thus, the tree 316 grows backward in time from the initial node 312a corresponding to the target configuration and connects to the target node 312i corresponding to the initial configuration by a short steering segment calculated online. However, since a large initial tracking error can lead to a substantial error in the target docking configuration, the initial tracking error in the steering segment, in addition to the motion cusp of the first path 320, can determine the performance of the docking operation 130.

[0175] Therefore, to overcome issues related to the number of motion cusps and connecting the tree 316 to an initial configuration, a first number of motion cusps in the first path 320 is determined. Also, a first number of initial tracking errors in the first path 320 is determined. In one example, the first number of initial tracking errors in the first path 320 is determined by solving an LQR steering problem, which is used to connect node 312g to the initial node 312a as the tree 316 is created backward. For example, the first number of initial tracking errors may indicate the amount of difference between the current state of the articulated vehicle 120 and the state at the start of the first path 320 or reference trajectory 107.

[0176] Further, at 404, a determination is made to determine whether the first number of motion cusps in the first path 320 is greater than a predetermined motion cusp threshold, and whether the first number of initial tracking errors is greater than a predetermined initial tracking error threshold.

[0177] Continuing further, at 406, if the first number of motion cusps in first path 320 is less than a predetermined motion cusp threshold and the first number of initial tracking errors is less than a predetermined initial tracking error threshold, motion planner 102 may output first path 320 for controlling motion of articulated vehicle 120 according to first path 320. In this regard, reference trajectory 107 may be generated based on first path 320.

[0178] At 408, if it is determined that the first number of motion cusps in the first path 320 is greater than a predetermined motion cusp threshold or if it is determined that the first number of initial tracking errors is greater than a predetermined initial tracking error threshold, the motion planner 102 is configured to continue expanding the graph 310 of the first path 320. The motion planner 102 may expand the graph 310 to generate a second path. In particular, the graph 310 is expanded to add new nodes to the graph 310 until a termination condition is met. Such expansion of the graph 310 is subject to hard constraints associated with the total number of motion cusps and the total number of initial tracking errors in the graph 310 or the first path 320. The graph 310 is further expanded to form a second path connecting the initial node 312 a to the goal node 312 i. The second path may have a second number of motion cusps that is less than the first number of motion cusps in the first path 320, and / or the second path may have a second number of initial tracking errors that is less than the first number of initial tracking errors in the first path 320.

[0179] For example, the graph 310 may be expanded based on a first number of motion cusps and a first number of initial tracking errors. In particular, upon comparing the first number of motion cusps and the first number of initial tracking errors with corresponding thresholds and determining that the constraints are violated, the motion planner 102 may continue to expand the graph 316 to add more new nodes. The predetermined motion cusp threshold indicates a maximum number of motion cusps allowed in the path, and the predetermined initial tracking error threshold indicates a small number related to the initial tracking error allowed in the path. For example, if the first number of motion cusps in the first path 320 is less than the predetermined motion cusp threshold and the first number of initial tracking errors is small or less than the predetermined initial tracking error threshold, the articulated vehicle 120 may be able to follow the reference trajectory 107 based on the first path 320 with greater accuracy. However, if the first number of motion cusps in first path 320 is greater than a predetermined motion cusp threshold, or if the first number of initial tracking errors is large or greater than a predetermined initial tracking error threshold, a large amount of uncertainty may be added to each of the motion cusps, which, in addition to the initial tracking errors, may impair the accuracy of the path planning and may prevent articulated motion vehicle 120 from moving along reference trajectory 107 based on first path 320 to docking bay 132.

[0180] For example, the predetermined motion cusp threshold and the predetermined initial tracking error threshold may be user selectable or dynamically determined based on the first path 320, e.g., based on the length of the first path 320.

[0181] In one example, the predetermined initial tracking error threshold may be related to a cost function of the predictive controller 104 used to track the motion of the articulated vehicle 120. Furthermore, the motion planner 102 may have a final condition for generating the second path. In particular, the final condition may correspond to computation time, i.e., the computation time for generating the second path may be limited. Therefore, to reduce the initial tracking error within the limited computation time, two thresholds for the initial tracking error may be defined. This may enable the motion planner 102 to output and evaluate the best candidate motion plan found before the final condition is met, i.e., within the limited computation time, if the motion planner 102 does not generate a solution that meets strict thresholds (or hard constraints) on the number of motion cusps and the initial tracking error.

[0182] According to one example, motion planner 102 may perform the expansion of graph 310 until a termination condition is met. System 100 may then control movement of articulated vehicle 120 based on the second path if the second path is formed before the termination condition is met. However, if the second path is not generated before the termination condition is met or before the limited computation time, method 400 may return to 406 and output first path 320 for controlling movement of articulated vehicle 120.

[0183] At 410, the system 100 is configured to control the movement of the articulated vehicle 120 based on the second path. In particular, if the second path is generated before the termination condition is met and the second path has a reduced number of motion cusps or a lower number of initial tracking errors compared to the first path 320, the motion planner 102 may generate a reference trajectory 107 based on the second path. The system 100 may then use the second path to control the movement of the articulated vehicle 120, for example, during the docking operation 130. In one example, the termination condition may be time-based. For example, the termination condition may indicate the expiration of a predetermined period or time limit for the generation of the second path. In one example, the period or time limit of the termination condition may range from 10 seconds to 30 seconds. For example, the termination condition is met upon the expiration of a period of 10 seconds. In another example, the period of the termination condition may range from 1 second to 60 seconds.

[0184] According to some embodiments, the generation of the second route may be initiated based on a search for a new node, and the selection of the node is based on penalizing changes in speed direction when calculating the arrival cost associated with the node. However, to balance completeness, computational efficiency, and route quality, such penalizing methods may substantially increase the time to generate the second route. Therefore, according to this embodiment, the second route is generated based on the first route 320. The second route is created using a modified AGT algorithm.

[0185] 4B illustrates an example method 412 for performing expansion of the graph 310 during the second stage, according to some embodiments. During the second stage, a second path can be generated based on the first path 320.

[0186] At 414, the priority queue is sorted based on the hard constraint and node cost. In some embodiments, the hard constraint indicates the maximum number of motion cusps in a path from a root node, such as the goal node 312i, to a first node, such as the initial node 312a of the graph. For example, in a first stage during generation of the first path 320, several motion cusps for each node are calculated and recorded. Based on the number of motion cusps for the nodes 312a-312i of the graph 310 corresponding to the first path 320, the priority queue for the nodes 312a-312i is sorted. Such sorting of the nodes 312a-312i is based, for example, on the number of motion cusps and node cost. In this regard, one or more nodes 312a-312i with the fewest number of motion cusps have the highest priority for selection compared to nodes with a similar number of motion cusps. For example, a node with the smallest cost and the smallest number of motion cusps is prioritized. In this manner, the priority queue is sorted by increasing the priority of nodes with the smallest number of motion cusps and cost.

[0187] Once the priority queue is sorted, a best node is removed or selected from the priority queue at 416. At 418, if the number of motion cusps contained in the best node is greater than a predetermined motion cusp threshold, which is the maximum number of motion cusps allowed, it may be determined that there is no way to improve the quality of the first path 320. In such a case, the motion planner 102 returns the first path 320 as the best path. Otherwise, at 420, node expansion is performed starting from the best node. Node expansion may be performed by applying any feasible motion primitives to steer vehicle dynamics from the starting state corresponding to the best node to new child nodes to form a tree.

[0188] At 422, if child nodes and associated edges between the best node and the corresponding child nodes exist, they are added to the graph 310. Note that this example uses the best node to illustrate the expansion of the graph 310. However, this should not be construed as limiting. In other embodiments, other nodes may be used to expand the graph 310. Furthermore, note that the node for generating the second path may be selected from the nodes 312a-312i or may be a new node added to the graph 310. In other words, the graph 310 associated with the first path 320 may be expanded and modified to form the second path.

[0189] At 424, a second path may be generated based on the new node added to the graph. For example, the second path may be generated using the new node if a cost function associated with the node forming the second path is less than a threshold. In particular, the second path with the new node and / or best node may have a motion cusp less than a predetermined motion cusp threshold and an initial tracking error less than a predetermined initial tracking error threshold.

[0190] 4C shows an example of a method 426 for node expansion, according to some implementations. At 428, the best node of the graph 310 is retrieved. Given the best node, at 430, a determination is made whether the best node of the graph 310 is close to the initial configuration.

[0191] If the best node is determined to be close to the initial configuration, then at 432 an LQR steering problem may be solved to generate a solution for connecting the best node with the initial configuration of the articulated vehicle 120 .

[0192] However, if the best node is not close to the initial configuration, then at 434, motion primitives are applied to expand the best node and add the corresponding child nodes.

[0193] At 436, a determination is made to see if the node expansion of the best node will produce an edge connecting the best node to the initial configuration or initial state 142. This determination is made to see if the node expansion in steps 432 or 434 resulted in an edge connecting the best node of the graph 310 to the initial configuration. If neither the expansion of the best node with a motion primitive nor the LQR steering problem results in expanding the graph 310, then the node expansion of the best node is terminated. Another node, such as a child node of the previous best node or second best node, may then be selected for expansion according to the method steps described in Figures 4A, 4B, and 4C.

[0194] However, if the node expansion of the best node generates a new edge connecting the best node to the initial configuration, the new edge is recorded 438 and the first path 320 is updated using the new edge as a new path. In this regard, if the new path generated using the new edge includes fewer motion cusps than the first number of motion cusps and / or fewer initial tracking errors than the first number of initial tracking errors of the first path 320, the new path is updated as a new first path, and the new first path may be further optimized until a termination condition is met. Alternatively, if the new path generated using the new edge includes a second number of motion cusps less than a predetermined motion cusp threshold and a second number of initial tracking errors less than a predetermined initial tracking error threshold, the new path may be recorded as a second path.

[0195] Thus, continuous expansion of the graph 310 is performed to reduce path costs. In particular, continuous expansion of the graph 310 is performed to satisfy hard constraints associated with the selection of nodes in the graph by applying restrictive motion primitives. In one example, the motion primitives are restricted based on the presence of motion cusps and the presence of initial tracking errors in edges associated with pairs of nodes.

[0196] 5A illustrates examples of types of motion primitives, according to some embodiments. For example, motion primitives may be applied to nodes to provide collision-free child nodes. It will be understood that motion primitives are pre-calculated motions that articulated vehicle 120 may perform. Motion primitives represent motions that articulated vehicle 120 may transition smoothly into. For example, motion primitives may be used to form graph edges, such as edges 314a-314g in graph 310. For example, a motion primitive may be superimposed on graph 310 at any of nodes 312a-312i to identify its child nodes.

[0197] In one example, motion primitives may be applied to initial node 312a to identify the initial node 312a, and the motion primitives forming edges 314a may be used to connect the initial node to its child nodes. Each node from trees 316 and 318 may then be connected to its child nodes using motion primitives indicated by edges 314a-i. The motion primitives may be uniquely defined by the trajectory of the control input over a finite time interval.

[0198] Therefore, the initial node 312a or X f Applying the motion primitive corresponding to edge 314a in (312b) results in a connected path that ends at the state represented by node 312b. For example, edges 314a-314h in graph 310 may be represented by trajectories.

[0199] Following this example, node 504 can be connected to 502a using a motion primitive corresponding to edge 506a. If the state corresponding to node 502a and the corresponding connection path 506a are collision-free, node 502a can become a child node of its parent node, such as node 504. In some examples, node 504 can have multiple child nodes. In one example, node 502a can become a child node of node 504 when the path between node 504 and node 502a is collision-free, feasible, and has a cost below a threshold.

[0200] Following this example, node 504 may be the best node in graph 310 that forms first path 320. Furthermore, motion planner 102 may determine whether {a1(t),t∈[0,t f1 ]}~{a1(t),t∈[0,t fj The graph 310 can be expanded using motion primitives defined by a range ([i], [ii], [iii], [iv], [v], [v], [v], [v]). The motion primitives add collision-free nodes, such as node 502a and other nodes 502b-502j. Edge 506a and other edges 506b-506j can connect node 504 to corresponding child nodes 502a-502j. According to some embodiments, child nodes 502a-502j can be added to a path if the number of motion cusps from the corresponding tree root to the child nodes 502a-502j is less than a threshold for the maximum number of allowed motion cusps or a predetermined motion cusp threshold, if the cost of the child nodes 502a-502j is likely to be less than the cost of the parent node 504, and if the initial tracking error at the child nodes is reduced. Such child nodes may be added to the graph 310 to generate a second path. The particular child node may then be selected as the next best node for expansion.

[0201] In one example, the cost of a child node 502a-502j corresponding to a state may be calculated as the sum of an arrival cost and an estimated cost-to-go, where the arrival cost represents the cost of driving the articulated vehicle 120 from the initial state 142 or root node of the tree to the child node 502a-502j, and the estimated cost-to-go represents the estimated cost of driving the articulated vehicle 120 from the initial configuration or initial state 142 to the child node 502a-502j.

[0202] 5B illustrates a graphical representation of types of motion primitives 510, according to some embodiments. For example, motion primitives 510 represented by edges, such as edges 314a-314h, may be i (t),t∈[0,t fj]}512, which includes a high-dimensional trajectory represented by X i and X j The subset of signals representing the high-dimensional trajectories 512 corresponds to the state trajectories 514. In one example, the movement primitive 510 is connected to a node X in the tree. i is applied to the tree to create a new node X j Extend the node to a new node with

[0203] In one embodiment, the state of the articulated vehicle 120 is defined by the velocity and steering angle along with the configuration or attitude of the articulated vehicle 120. i and X j The motion primitive 510 associated with the edge connecting i (t),t∈[0,t fi ]}, where each state and control signal of the vehicle model 110 evolves over time. In the case of a single-trailer articulated vehicle, the motion primitives 510 may include, for example, a position 516 of the vehicle or tractor 122, a position 518 of the trailer 124a or 314, a heading angle 520 of the tractor 122, a heading angle 522 of the trailer 124a or 314, a velocity 524 of the tractor 122, a steering 526 of the tractor 122, an acceleration 528 of the articulated vehicle 120, and a steering angle rate 530 of the tractor 122. It will be appreciated that the elements of the motion primitives 510 are represented as signals. Thus, signals corresponding to the position 516, the position 518, the heading angle 520, the heading angle 522, the velocity 524, and the steering angle 526 allow for the formation of new nodes corresponding to the state vector and state trajectory 514.

[0204] During a first stage for creating the first path 320, the first set of nodes 312 a-312 g in the priority queue may be ordered according to the corresponding cost of the node 312 a-312 g to be selected for expansion, where the cost may not characterize the number of motion cusps. The node with the lowest cost then has the highest priority for selection. During a second stage for creating the second path, constraints may be imposed on node selection related to the number of motion cusps and the number of initial tracking errors in the set of motion primitives of the nodes 312 a-312 g.

[0205] Once a trajectory is generated, for example based on the second path, with sufficiently few motion cusps and small initial tracking errors, the predictive controller 104 is used to accurately track the planned trajectory.

[0206] 6A shows an example block diagram of a linear predictive controller 104 according to some embodiments. The predictive controller 104 is configured to calculate the control signal 105 given the current and estimated states 109 of the control system 106 and the reference trajectory 107. Specifically, the reference trajectory 107 can be determined based on the first path 320 or the second path as a function of time. The reference trajectory 107 for the articulated vehicle 120 to travel can be generated by adding a time dimension to the second path generated by path 320 or 408. In some cases, the motion planner 102 in the system 100 can generate a second path with a minimal number of motion cusps and a small number of initial tracking errors and generate the reference trajectory 107 based on the second path. The predictive controller 104 of the system 100 then tracks the motion of the articulated vehicle 120 in response to the reference trajectory 107.

[0207] In one example, the predictive controller 104 may be configured to generate equality and inequality constraints 602 to control the movement of the articulated vehicle 120. For example, the equality and inequality constraints may represent constraint functions on the physical movement of the articulated vehicle 120 relative to its surroundings. The equality and inequality constraints 602 may be generated when the predictive controller 104 includes one or more nonlinear constraints and a nonlinear vehicle model 110, i.e., when the predictive controller 104 is nonlinear. Furthermore, the predictive controller 104 may be configured to generate an objective function 604 for the predictive controller 104 using the reference trajectory 107 generated by the motion planner 102. In one example, the objective function 604 may represent a starting or initial state 142 of the articulated vehicle 120, a goal or target state 140 of the articulated vehicle 120, and the reference trajectory 107 to follow. For example, during docking operation 130, objective function 604 may indicate an initial state 142 as the current position of articulated vehicle 120 away from docking bay 132 and a target state 140 or target configuration near docking bay 132. Objective function 604 may be configured to minimize the error between one or more predicted states of articulated vehicle 120 and reference trajectory 107 over a prediction time horizon of predictive controller 104.

[0208] For example, the optimal control data of the objective function 604 and the equality and inequality constraints 602 may be used to solve a constrained optimization problem based on the equality constraint functions, the inequality constraint functions, and the objective function. For example, the control inputs 105 may be generated based on the solution of the optimization problem. In one example, the type of optimization problem may depend on the dynamics of the vehicle model 110 of the system 100, the system constraints 112, the estimated state 109 of the system 100, and the reference trajectory 107.

[0209] According to some embodiments, the predictive controller 104 may be configured to calculate a control solution, e.g., a solution vector 606, including a sequence of future optimal control inputs 105 over a predictive time horizon of the motion planner 102, and the predictive controller 104 may be configured to calculate the control solution 606 by solving an optimization problem at each control time step. According to this example, the optimization problem may have inequality constraints 602 and may be in the form of an optimal control structured quadratic program 608.

[0210] In some embodiments, the solution of the inequality-constrained optimization problem, i.e., an optimal control-structured quadratic program (QP) 608, uses state and control values ​​610 over a prediction time horizon from a previous control time step, which can be retrieved from memory. In this manner, a technique for warm-starting or hot-starting the optimization problem may be realized, which, in some embodiments, may reduce the amount of required computational effort of the predictive controller 104. Similarly, the corresponding solution vector 606 generated by solving the optimal control-structured QP 608 may be used to generate a sequence of optimal or suboptimal updated state and control values ​​612 for the next control time step.

[0211] According to some embodiments, the predictive controller 104 can calculate the control inputs 105 for controlling and tracking the movement of the articulated vehicle 120 by solving an optimal control structured QP 608, given the estimated state 109 of the system 100 and the reference trajectory 107.

[0212]

number

[0213] When k=0,...,N, the optimization variables in the optimal control structure QP608 are the state variables x k and the control input variable u kAccording to some embodiments of the present disclosure, for k=0,...,N, the dimensions of the state and control variables 610 are k , may not be equal to each other. At each sampling time for the predictive controller 104, an optimal control-structured QP 608 is formulated using a QP matrix 614 and a QP vector 616. The optimal control-structured QP 608 is then solved to calculate a solution vector 606. The solution vector 606 may be used to generate updated state and control values ​​612 that can be used in the next iteration. Furthermore, new control inputs 105 are generated based on the solution of the optimal control-structured QP 608.

[0214] The objective function in the constrained optimal control structured QP solved by the predictive controller 104 includes one or more least-squares reference tracking terms 618 that penalize the difference between the sequence 620 of predicted state and / or output values ​​and the sequence of reference state and / or output values ​​for the reference trajectory 107 calculated by the motion planner 102.

[0215]

number

[0216] For example, output functions may include, but are not limited to, longitudinal or lateral velocity and / or acceleration, slip ratio or slip angle, azimuth angle or angular velocity, wheel speed, force, and torque of the articulated vehicle 120.

[0217] In various embodiments, the penalty between the reference value corresponding to the reference trajectory 107 determined by the motion planner 102 and the value determined by the predictive controller 104 is weighted by a weighting matrix that assigns different weights to different state variables of the target state. Additionally or alternatively, some embodiments add additional reference tracking terms that are considered by the predictive controller 104. Such additional reference tracking terms may relate, for example, to driving comfort, speed limits, energy consumption, pollution, etc. These embodiments balance the cost of reference tracking with the additional reference tracking terms.

[0218] According to an example, additional criterion tracking terms may be determined based on cost functions in the form of linear-quadratic stage costs 622 and / or linear-quadratic final cost terms or matrices 624. These additional linear-quadratic criterion tracking terms, including stage costs 622 and final cost matrix 624, may include linear and / or quadratic penalties of one or more combinations of one or more state and / or control input variables. For example, the objective function in constrained QP 608 may include linear or quadratic penalties of vehicle longitudinal or lateral velocity and / or acceleration, slip ratio or slip angle, azimuth angle or angular velocity, wheel speed, force, torque, or any combination of such quantities. The linear-quadratic objective terms in stage costs 622 and final cost matrix 624 are determined based on the matrix Q in QP matrix 614. k , S k and R k and the gradient value q in the QP vector 616 k , r k It is determined by:

[0219]

number

[0220]

number

[0221]

number

[0222] Inequality constraints may include, for example, constraints on the longitudinal or lateral velocity, acceleration, position and / or orientation of articulated vehicle 120 relative to its surroundings, slip ratio or slip angle, azimuth angle, angular velocity, wheel speed, force and / or torque. For example, obstacle avoidance constraints may be implemented in predictive controller 104 by defining a set of one or more inequality constraints on a linear function of the predicted position, velocity, and orientation of articulated vehicle 120 relative to the predicted position, velocity, and orientation of one or more obstacles 144 in articulated vehicle's 120 surrounding environment 146.

[0223] In this manner, the equality and inequality constraints 602, the objective function 604, the previous state and control values ​​610, the QP matrix 614, and the QP vector 616 may be used in formulating and solving the QP 608. By solving the QP 608, the predictive controller 104 may generate values ​​for the control inputs 105. For example, the values ​​for the control inputs 105 may include values ​​for, for example, the longitudinal or lateral velocity of the articulated vehicle 120, the longitudinal or lateral acceleration of the articulated vehicle 120, the slip ratio or slip angle of the articulated vehicle 120, the azimuth angle, the angular velocity, the wheel speeds, the forces, and the torques. Based on the values ​​for the control inputs 105 corresponding to the different functions, the articulated vehicle 120 may be controlled to perform the docking maneuver 130. For example, the control inputs 105 may be provided to actuators of the articulated vehicle 120 to control the movement of the articulated vehicle 120 based on a reference trajectory 107 corresponding to the first path 320 or the second path.

[0224] The predictive controller 104 may provide control inputs 105 to the control system 106 to generate and estimate vehicle states 121 for control purposes. The estimated states 109 of the control system 106 provide state feedback to the predictive controller 104. For example, the predictive controller 104 tracks the movement of the articulated vehicle 120 to ensure it is following a reference trajectory 107.

[0225] Some embodiments of the present disclosure may use the Hessian matrix H at 622. k , the final cost matrix Q N 624 and the weighting matrix W k This is based on the recognition that the optimal control structured QP 608 is convex if 618 is positive definite or positive semi-definite. Embodiments of the present disclosure can use an iterative optimization algorithm to solve the optimal control structured QP 608 to find a solution vector 606 that is either feasible with respect to the constraints, globally optimal, feasible but suboptimal, or the algorithm can find a low-precision approximate control solution that is neither feasible nor optimal. As part of the predictive controller 104, the optimization algorithm may be implemented in hardware or as a software program running on a processor.

[0226] Examples of iterative optimization algorithms for solving the QP 608 may include, but are not limited to, primal or dual gradient-based methods, projected or proximal gradient methods, forward-backward splitting methods, alternating direction multiplier methods, primal, dual, or primal-dual active constraint methods, primal or primal-dual interior point methods, or variations of such optimization algorithms. In some embodiments of the present disclosure, a block-sparse optimal control structure in the QP matrix 614 may be utilized in one or more of the linear algebra operations of the optimization algorithm to reduce computational complexity and thereby reduce the runtime and memory footprint of the QP optimization algorithm.

[0227] Other embodiments of the present disclosure may use optimization algorithms for nonlinear programming, such as sequential quadratic programming (SQP) or interior point method (IPM), to solve the non-convex optimal control structured QP 608. A suboptimal, locally optimal, or globally optimal control solution may then be found for the inequality constrained optimization problem at each sampling time of the predictive controller 104.

[0228] 6B shows an example block diagram of a nonlinear predictive controller 104 according to some embodiments. In one example, the predictive controller 104 may be implemented by solving an optimal control structured nonlinear program (NLP) 640 to calculate the control signal 105 given the estimated state 109 of the control system 106 and the reference trajectory 107. In some embodiments of the present disclosure, the predictive controller 104 is a motion predictive controller configured using a linear-quadratic or nonlinear-quadratic objective function used in combination with a linear or nonlinear vehicle model 110 and a constraint function 644 and a combination of linear and nonlinear inequality constraints to predict the behavior of the articulated vehicle 120. The predictive controller 104 can track the articulated vehicle 120 based on the predicted behavior.

[0229]

number

[0230] When k=0,...,N, the optimization variables in the optimal control structure NLP640 are the state variables x k and the control input variable u k In some embodiments, for k=0,...,N, the dimensions of the state and control variables 610 are k, may not be equal to each other. At each sampling time of the predictive controller 104, an optimal control structured NLP 640 is formulated using a criterion and weighting matrix 642 at the reference tracking cost, and an NLP objective and constraint function 644. The optimal control structured NLP 640 is solved to calculate a solution vector 606. The solution vector 606 may be used to generate updated state and control values ​​612 that can be used in the next iteration. Furthermore, a new control input 105 is generated based on solving the optimal control structured NLP 640.

[0231]

number

[0232] The predictive model includes first-order velocity and steering angle actuators with time constants τ and τ as velocity and steering angle actuators, respectively. d and steering angle ψ d , it takes some time, characterized by a time constant, to achieve the desired response. Additionally, some embodiments of the controller include a hitching offset α in the predictive model. Furthermore, the steering angle bias ψ b is considered, which may be estimated online or calculated in a calibration step. Note that other actuator and bias models may be considered in a complementary manner to obtain the vehicle model in (2). Therefore, high-order actuation models, nonlinear actuation models, and actuation models including pure delays may also be included in the predictive model or vehicle model.

[0233]

number

[0234] In one example, the objective function in the constrained optimal control structured NLP 640 that can be solved by the predictive controller 104 includes one or more linear and / or nonlinear least-squares reference tracking terms 646. The reference tracking terms 646 may penalize the difference between the sequence of predicted state and / or output values ​​and the sequence of reference state and / or output values ​​of the reference trajectory 107 calculated by the motion planner 102 in the path and motion planning layer 202.

[0235] In some embodiments of the present disclosure, for k=0,...,N, the weighting matrix W k is used for the least squares reference tracking term 646. Therefore, each weighting matrix W k may be fitted in the control cost function 642 based on the reference trajectory 107. For k=0,...,N, the output value y for k (x k ,u k ) may be defined as any linear or non-linear function of state and / or control input variables.

[0236]

number

[0237] Embodiments of the present disclosure may define additional tracking terms in the cost function 642 in the form of stage cost and / or final cost terms 648. Any of these cost terms may include any combination of linear, linear-quadratic, or nonlinear functions. These additional objective terms may include penalties that are functions of state and / or control input variables. For example, the objective function 644 in the constrained optimal control structured NLP 640 may include linear, quadratic, or nonlinear penalties of the vehicle's longitudinal or lateral velocity and / or acceleration, slip ratio or slip angle, azimuth angle or angular velocity, wheel speed, force, torque, or any combination of such quantities.

[0238]

number

[0239] Some embodiments of the present disclosure are based on the recognition that a discrete-time dynamic model 650 for predicting the behavior of an articulated vehicle 120 can be obtained by performing a time discretization of a set of continuous-time differential or differential-algebraic equations. Such time discretization may be performed analytically, but requires the use of numerical simulation routines to compute a numerical approximation of the discrete-time evolution of the state trajectory. Examples of numerical routines for simulating a set of continuous-time differential or differential-algebraic equations include, but are not limited to, explicit or implicit Runge-Kutta methods, explicit or implicit Euler methods, backward differential equations, and other single-step or multi-step methods.

[0240]

number

[0241] Inequality constraints may include, for example, constraints on the longitudinal or lateral speed, acceleration, position and / or heading angle of articulated vehicle 120 relative to its surroundings, slip ratio or slip angle, azimuth angle or angular velocity, wheel speed, force, and / or torque. For example, obstacle avoidance constraints may be implemented in nonlinear predictive controller 104 by defining a set of one or more inequality constraints on linear or nonlinear functions of the predicted position, speed, and orientation of articulated vehicle 120 relative to the predicted position, speed, and orientation of one or more obstacles 144 in articulated vehicle's 120 environment 146.

[0242] Some embodiments of the present disclosure are based on a tailored optimization algorithm for efficiently solving a constrained optimal control structured NLP 640 at each sampling instant of the nonlinear predictive controller 104. Such an optimization algorithm can find a low-precision approximate control solution by finding a solution vector 606 that is either feasible and globally optimal, feasible but locally optimal, feasible but suboptimal, or neither feasible nor locally optimal with respect to the constraints, as computed by an iterative optimization algorithm. Examples of NLP optimization algorithms include, but are not limited to, variations of interior point methods and variations of sequential quadratic programming (SQP).

[0243] In particular, some embodiments of the present disclosure require the use of a real-time iteration (RTI) algorithm, an online variant of sequential quadratic programming (SQP), in combination with a quasi-Newton or generalized Gauss-Newton type semidefinite Hessian approximation to solve at least one convex block sparse QP approximation at each sampling instant of the nonlinear predictive controller 104.

[0244] In one example, each iteration of RTI consists of two steps: in the first step, corresponding to the preparation phase, the system or vehicle dynamics are discretized and linearized, the remaining constraint functions are linearized, and a quadratic objective approximation is evaluated to construct an optimal control structured QP subproblem; in the second step, corresponding to the feedback phase, the QP subproblem is solved to update the current state and control values ​​of all optimization variables, obtain the next control input 105, and provide feedback to the control system 106.

[0245] In some embodiments of the present disclosure, block-sparse optimal control structures in the Hessian and constraint Jacobian matrices may be used in one or more of the linear algebra operations of the optimization algorithm to reduce the computational complexity and thereby reduce the runtime and memory footprint of the NLP optimization algorithm.

[0246] It should be noted that the amount of precision required in the docking maneuver 130 may require the predictive controller 104 to be robust to certain disturbances. This can be achieved by introducing integral action in the predictive controller 104. Additionally, when traveling forward, it becomes more important to track the state (x0, y0, θ0) of the tractor 122 of the articulated vehicle 120, while during reverse or backing maneuvers, it becomes more important to track the state (x0, y0, θ0) associated with the last trailer 124e or 134. n ,y n ,θ n ) becomes more important, different sets of integral tracking errors may be introduced corresponding to traveling in forward and reverse motion.

[0247]

number

[0248]

number

[0249]

number

[0250]

number

[0251] It should be noted that the integral tracking error function and the integral action are defined differently for forward and reverse motion, i.e., forward and reverse directions. Specifically, one or more functional relationships defining the integral action or the integral tracking error function for the constrained optimization problem 604 of the predictive controller 104 are determined differently for forward and reverse motion in the reference trajectory 107. Some embodiments are based on the recognition that it is important to reset the integral tracking error state at every instant, i.e., at each time step, when the direction of motion changes corresponding to a motion cusp. In other words, at time t j If the articulated vehicle 120 shifts gears during the planned cusp of motion, then at that time, the predictive model of the predictive controller 104 will j )≠0, η(t j ) = 0 may be reset. In one example, the integral state at the next most recent switch is stored, and the integral state is instead set to the value corresponding to the most recent time that control system 106 similarly switched between forward and reverse motion.

[0252]

number

[0253]

number

[0254]

number

[0255]

number

[0256]

number

[0257] For example, we can use the Nonlinear Optimal Control Structure NLP640, along with a Gauss-Newton type Hessian approximation, to implement sequential quadratic programming (SQP) known as a real-time iterative (RTI) scheme. S = 50 ms sampling time. The RTI algorithm performs a single SQP iteration per control time step, and at some time step t i to the next time step t i+1 The state and control trajectory (X i ,U i ;S i ) using a continuation-based warm start. Nonlinear functions and their first derivatives can be efficiently evaluated using C code generation in CasADi. The solver PRESAS may be used to solve quadratic programs (QPs), which applies block-structured factorization techniques with low-rank updates to preconditioning of an iterative solver within a primal active set algorithm with dedicated warm start. When combined with the C code generated by CasADi, the PREAS solver results in an efficient and reliable predictive controller 104 solver for solving optimal control structured NLP 640 problems suitable for embedded system platforms.

[0258] Based on the control inputs 105 generated by the predictive controller 104, the articulated vehicle 120 can be controlled to perform the docking maneuver 130 by solving an optimal control structured NLP 640 problem.

[0259] 6C illustrates in detail an example docking operation 130, according to an embodiment of the present disclosure. During the docking operation 130, the rear trailer 124e of the articulated vehicle 120 approaches the door of the docking bay 132 and moves into the target configuration or state X f 140. During the docking operation 130, there may be other articulated vehicles or obstacles 144 close to the target docking configuration, shown as obstacles 602a, 602b.

[0260] In one embodiment, both obstacles 602a and 602b and trailer 124e of articulated vehicle 120 are represented as inflated using p-th order superellipsoids as 604a, 604b, and 606, respectively. Superellipsoids 604a, 604b, 606 contain within them the corresponding obstacles 602a and 602b, or superellipsoid 606 contains within it the trailer 124e of articulated vehicle 120. In some embodiments, superellipsoids 604a, 604b, and 606 may be calculated such that the areas of superellipsoids 604a, 604b, and 606 are minimized. In other words, the superellipsoids 604a, 604b, and 606 may be calculated to minimize the maximum margin between the superellipsoids 604a, 604b and the corresponding obstacles 602a, 602b, and between the superellipsoid 606 and the trailer 124e in a direction perpendicular to the obstacles 602a, 602b, or trailer 124e boundary. In another embodiment, the superellipsoids 604a, 604b, and 606 may be calculated using an analytical formulation of feasible superellipsoids.

[0261]

number

[0262] In some embodiments of the present disclosure, the predictive controller 104 executes a series of motion primitives for moving the articulated vehicle 120 while including one or more collision avoidance constraints for enforcing one or more collision-free motion primitives within the environment. To this end, each of the one or more collision avoidance constraints is implemented using multiple nonlinear inequality constraints. In this regard, the set of nonlinear inequality constraints may be defined as h(x k ,u k)≦0, whereby points 608a and 608b on rear trailer 124e are constrained to be outside of super ellipsoids 604a and 604b. A set of nonlinear inequality constraints ensures that one or more points of tractor 122 and / or trailers 124a-124e are constrained to be outside of the super ellipsoids surrounding each of obstacles 602a and 602b in environment 146 of articulated vehicle 120. A set of nonlinear inequality constraints can also ensure that one or more points of each obstacle 602a and 602b are constrained to be outside of the super ellipsoid surrounding tractor 122 and outside of the super ellipsoid 606 surrounding trailer 124e at each time step of the prediction time horizon.

[0263] Additionally, points 610a and 610b on obstacles 602a and 602b are constrained to lie outside a superellipsoid 606 associated with trailer 124e of articulated vehicle 120. Note that the constraint may be sufficient to ensure that the final trailer 124e does not intersect with either obstacle 602a or 602b. It will also be appreciated that the resulting constraint is differentiable (ifp>1) and therefore useful in formulating a predictive controller 104. This is in contrast to the common approach of using collision checking, in which the exterior of the perimeter of trailer 124e is discretized into a set of points 612a-612j, and each point in this set is checked in turn to ensure it lies outside each of obstacles 602a and 602b. Such constraints based on a discretized set of points 612a-612j may not be differentiable and may not be very useful in a predictive controller 104. In addition, such a collision checking method can introduce numerous constraints that dramatically increase the computation time of the predictive controller 104 .

[0264] 7A illustrates an example of a reference trajectory 107, according to some embodiments of the present disclosure. The reference trajectory 107 may be calculated by the motion planner 102 in the path and motion planning layer 202. As such, the reference trajectory 107 may include one or more motion cusps, i.e., a transition from forward motion 702 to reverse motion 704 or a transition from reverse motion 704 to forward motion 702. Some embodiments of the present disclosure are based on the recognition that the reference trajectory 107 must include a sufficiently large pause period 710 at each of the one or more motion cusps so that the predictive controller 104 can command a gear change with a sufficiently large preview period 712 due to a time delay period 714 of a gear shift mechanism in the controlled system.

[0265] 7A shows a graph of the reference trajectory 107 against time 708, including forward movement 702 at a positive reference speed value, followed by a pause at rest at a zero reference speed value 706, followed by reverse movement 704 at a negative reference speed value. The pause period 710 is greater than a preview period 712 for the predictive controller 104 to command a gear change. The preview period 712 is greater than a time delay period 714 of the gearshift mechanism in the controlled articulated vehicle 120.

[0266] In particular, the GT motion planning algorithm is modified to introduce a pause at each motion cusp, i.e., at each forward motion 702 and reverse motion 704 of the articulated vehicle 120 and at each switch, to reach the target docking configuration or target state 140 at the end of the docking maneuver 130. A pause period 710, Δt, during which the articulated vehicle 120 is planned to come to rest at a zero reference speed value 706, is then entered. pause At , a preview period 712, Δt prevtew The gear shift is initiated at . The preview period 712 is selected to be greater than the time delay period 714, which indicates the average time it takes for the articulated vehicle 120 to change gears: Δt preview >Δt gearFor example, the longitudinal speed limit may vary according to the current gear, with v≧0 for forward movement 702 of the vehicle 120 and v≦0 for reverse movement 704 of the vehicle 120.

[0267] 7B illustrates an example method 720 for switching the NLP objective and constraint functions 644 of the nonlinear predictive controller 104, according to some embodiments of the present disclosure. In particular, the objective and constraint functions 644 of the predictive controller 104 may be switched depending on whether the current reference motion is a forward motion 702 or a reverse motion 704.

[0268] At 722, a current reference trajectory 107 is received. The current reference trajectory 107 may be determined by the motion planner 102 based on the first path or the second path. Note that the second path may have a second number of motion cusps and a second number of initial tracking errors that are less than or smaller than the first number of motion cusps and the first number of initial tracking errors of the first path.

[0269] At 724, a determination is made to ascertain which portion of the reference trajectory 107 over the prediction horizon length of the predictive controller 104 has forward motion 702, has backward motion 704, or is stationary if it has a zero reference velocity value 706. In other words, for example, a determination is made to identify which portion of the reference trajectory 107 over the prediction horizon length of the predictive controller 104 is positive, negative, or close to zero.

[0270] Based on the determination at 724, the predictive controller 104 may be adjusted for forward motion at 726. In particular, the objective and constraint function 644, which indicates the equality and inequality constraint functions, and the objective function 604 for the constrained optimization problem associated with the predictive controller 104, is adjusted for one or more sampling time steps t i , can be adjusted to the forward motion 702 of the articulated vehicle 120. This can ensure that the speed of the articulated vehicle 120 is always greater than or equal to zero.

[0271] Further, at 728, the predictive controller 104 can be tuned for the backward motion. In this regard, the objective and constraint function 644, which indicates the equality and inequality constraint functions, and the objective function 604 for the constrained optimization problem associated with the predictive controller 104, are calculated for the backward motion portion of the reference trajectory 107 at one or more sampling time steps t i , can be adjusted for reverse vehicle motion 704. This can ensure that the speed of the articulated vehicle 120 is always below zero.

[0272] At 730, the predictive controller 104 is configured to calculate the current sampling time step t i This can be achieved by solving a constrained optimization problem for predictive reference tracking in . The constrained optimization problem can be solved based on the objective and constraint functions 644 and the estimated state 109 for the current reference trajectory 107.

[0273] At 732, the control input 105 is generated by solving a constrained optimization problem. For example, the objective and constraint function 644 may relate to optimization problems such as the optimal control structured QP 608 and optimal control structured NLP 640 of the predictive controller 104. Thus, forward vehicle motion 702 followed by reverse vehicle motion 704 may correspond to a motion cusp.

[0274] 8A illustrates an example of performing an automatic docking maneuver 130 with automatic emergency braking, according to some embodiments of the present disclosure. The docking maneuver 130 may be performed by the articulated vehicle 120. To perform the docking maneuver 130, a modified AGT motion planning algorithm implemented by the motion planner 102 and the nonlinear predictive controller 104 may be incorporated.

[0275] It should be noted that the AGT motion planning algorithm utilized by the motion planner 102 calculates a kinematically feasible trajectory, such as the reference trajectory 107. The reference trajectory 107 may be generated based on the first path 320 or the second path. The reference trajectory 107 avoids collisions between the articulated vehicle and any static obstacles 144 in the environment 146.

[0276] However, the motion planner 102 does not consider any dynamic obstacles that may enter the environment 146. With respect to dynamic obstacles, certain constraints are imposed on the system 100 to control the movement of the articulated vehicle 120.

[0277] In this regard, some embodiments recognize that docking operations 130 are often performed in closed environments, i.e., environments where the area of ​​interest is free of pedestrians, cyclists, or vehicles and dynamic obstacles appear only sporadically. For example, the area of ​​interest may correspond to a safety area. The safety area may be determined for the movement of articulated vehicle 120 based on reference trajectory 107. The safety area includes safety sets, shown as safety sets 802, 804, and 806, along reference trajectory 107. In particular, safety sets 802, 804, and 806 may be centered around a current state configuration 808 of articulated vehicle 120 within environment 146 and / or predicted state configurations 810, 812 of articulated vehicle 120. It should be noted that during docking operations 130, articulated vehicle 120 may move from the current state configuration 808 to the predicted state configurations 810, 812.

[0278] Furthermore, some embodiments of the present disclosure are based on the recognition that if a dynamic obstacle appears within the safety set 802, 804, 806 while performing the docking maneuver 130, the system 100 or the predictive controller 104 stops the movement of the articulated vehicle 120 by performing automatic emergency braking (AEB). The predictive controller 104 then resumes performing the docking maneuver 130 after the dynamic obstacle moves outside the predetermined safety set 802, 804, 806.

[0279] Some embodiments of the present disclosure are based on the recognition that a time-varying volumetric safety tube may be constructed around the reference trajectory 107 that the articulated vehicle 120 follows to perform a docking maneuver. Additionally, an AEB system may be initiated whenever a dynamic obstacle enters this time-varying safety tube.

[0280] In some embodiments of the present disclosure, one or more road side units (RSUs) or infrastructure sensing devices 148a and 148b may be used for accurate execution of the automatic docking operation 130 based on accurate detection of current conditions and dynamic obstacles in the environment 146 of the controlled articulated vehicle 120 or the safety sets 802, 804, 806. For example, the one or more RSUs 148a and 148b may include one or more sensors, such as a distance rangefinder, radar, LIDAR, and / or camera, and sensor fusion techniques, to accurately detect the state of the articulated vehicle 120 and the state of dynamic obstacles in the environment 146 of the articulated vehicle 120 and the system 100. In some embodiments of the present disclosure, a communication network 149 may be used for real-time communication between the articulated vehicle 120 and the one or more RSUs or infrastructure sensing devices 148a and 148b.

[0281] In one example, the AEB system is initiated when a dynamic obstacle is detected using sensors onboard the articulated vehicle 120 and / or possibly by communication from one or more RSUs 148a and 148b. Thus, if a dynamic obstacle is present within the combination of the surrounding safety sets 802, 804, 806 of the current state configuration 808 or predicted state configurations 810, 812 of the articulated vehicle 120, the predictive controller 104 is suspended and the AEB system participates in executing a braking maneuver.

[0282] If the dynamic obstacle moves outside the combination of the safe sets 802, 804, 806, the predictive controller 104 is reinitialized and the automatic docking operation 130 continues. In some cases, the speed at which the articulated vehicle 120 performs the docking operation 130 is so slow that the same motion plan, i.e., reference trajectory 107, may be reused to complete the docking operation 130, and the AGT algorithm need not be run after such an interruption. However, in some cases, a motion plan or a re-plan of a new reference trajectory must be generated quickly, and the obstacle avoidance constraints in the optimization problem formulation may be changed.

[0283] 8B illustrates a method 820 for reinitializing the predictive controller 104, according to some embodiments. For example, a dynamic obstacle may disrupt the motion of the articulated vehicle 120 and the tracking of the predictive controller 104. In some cases, returning to the old reference trajectory may not be feasible, for example, if the dynamic obstacle has not moved for a long time or due to changes in the environment 146 of the articulated vehicle 120 caused by the dynamic obstacle.

[0284] At 822, information regarding the current state and current configuration 808 of the articulated vehicle 120 and the surroundings of the articulated vehicle 120 is received. For example, the surroundings of the articulated vehicle 120 may correspond to the safety sets 802, 804, 806. In one example, the predictive controller 104 uses a detection mechanism to determine the current state and current configuration 808 of the articulated vehicle 120 at each sampling time step t i The information may be sought to determine whether it is safe to perform an operation, such as docking operation 130, at the vehicle. For example, the detection mechanism may include using RSUs 148a and 148b or one or more other on-board sensors to sense the current configuration 808 of articulated vehicle 120 and the surroundings of articulated vehicle 120.

[0285] At 824, a determination is made to ascertain whether each of one or more safety checks associated with the maneuver is satisfied based on the current state 808 and environment 146 of the vehicle 153 and the current reference trajectory 107.

[0286] Once the safety checks are met, the predictive controller 104 may be implemented at 826 by solving a constrained optimization problem for the predictive reference tracking of the articulated vehicle 120. The constrained optimization problem is i The predictive controller 104 may generate control inputs 105 for controlling the articulated vehicle 120 to perform the docking maneuver 130.

[0287] However, if the safety checks are not met at 828, an automatic emergency braking (AEB) system is activated. The AEB system is activated to safely bring the articulated vehicle 120 to a stop. In this regard, an AEB control signal is calculated at 830. The generated AEB control signal is applied to actuators of the articulated vehicle 120 to bring the articulated vehicle 120 to a stop.

[0288] At 832, if one or more of the safety checks are not met, for example, if one or more dynamic obstacles are detected within the safety sets 802, 804, 806 around the current state configuration 808 and / or predicted state configurations 810 and 812 of the articulated vehicle 120, the AEB system may be activated.

[0289] Some embodiments of the present disclosure are based on the recognition that the AEB system will continue to run unless one or more of the safety checks are met.

[0290] Further, at 834, a reinitialization step of the state and control trajectory of the predictive controller 104 is performed when it is again safe to perform the docking maneuver 130. For example, to ensure a relatively smooth transition after AEB is switched back on to the docking maneuver 130, the predictive controller 104 may calculate the closest point in the reference trajectory 107 to the current state configuration 808 of the articulated vehicle 120. The predictive controller 104 can then start tracking the articulated vehicle 120 from the current state configuration 808 of the articulated vehicle 120 on the reference trajectory 107 to obtain an updated reference trajectory for the predictive controller 104. For example, the reference trajectory 107 is set to the current state configuration 808 of the articulated vehicle 120 with a reference speed value of zero. The new reference state is then time-shifted within the horizon to account for the vehicle reaching the reference speed from rest after braking.

[0291] 9 shows an example flowchart 900 for implementing a method for controlling the motion of an articulated vehicle 120, according to an example embodiment. More, fewer, or different steps may be provided. In one example, the steps of method 900 may be performed by path and motion planning system 100.

[0292] At 902, feedback signals indicative of the state of articulated vehicle 120 may be collected. For example, processor 150 may collect feedback signals from one or more sensors, such as sensors 156 and 158 and / or RSUs 148a and 148b, mounted on articulated vehicle 120, and the feedback signals may be indicative of the current state and current configuration of articulated vehicle 120.

[0293] At 904, a motion path for the articulated vehicle 120 is determined. In some examples, the motion planner 102 may determine the motion path based on the feedback signal. For example, the motion path includes one or more forward motions of the articulated vehicle 120 and one or more reverse motions of the articulated vehicle 120. In some examples, the motion path may be generated based on a first path generated by the motion planner 102. In some examples, the motion path based on the first path may have a first number of motion cusps and a first number of initial tracking errors. For example, if the first number of motion cusps and the first number of initial tracking errors are greater than a predetermined motion cusp threshold and a predetermined initial tracking error threshold, respectively, the motion path may need to be optimized. In this regard, it may be necessary to ensure reliable tracking of the trajectory followed by the articulated vehicle 120 by reducing the number of motion cusps and the number of initial tracking errors.

[0294] At 906, an optimal control problem for optimizing the motion path is generated. The OCP includes an integral tracking error function for the motion path. The integral tracking error function indicates an integral tracking error based on one or more motion cusps. The one or more motion cusps indicate a switch between forward and backward motion in the motion path. For example, the integral tracking error function may be a quadratic objective function that indicates at least an integral tracking error based on one or more motion cusps present in the motion path or the reference trajectory over a prediction horizon. In one example, the OCP may correspond to a constrained optimal control structured NLP.

[0295] In particular, the integral tracking error function is defined differently for forward and reverse motion, i.e., forward and backward directions. Specifically, one or more functional relationships defining the integral tracking error function for the constrained optimization problem of the predictive controller 104 are determined differently for forward and backward motion in the motion path or reference trajectory 107.

[0296] At 908, the motion path is optimized over a prediction horizon based on solving an optimal control problem. The prediction horizon may be associated with a vehicle model of the articulated vehicle 120 having states of the articulated vehicle 120. In one example, the OCP may be solved by a suitable implementation of sequential quadratic programming (SQP), known as a real-time iterative (RTI) scheme. For example, an RTI algorithm may perform one SQP iteration per control time step and use a continuation-based warm start of the state and control trajectory from one time step k to the next k+1 to solve the OCP.

[0297] In one example, the predictive controller 104 may solve the OCP to generate a solution or output 105. Furthermore, the output 105 may be provided as feedback to the motion planner 102. The motion planner may then generate an optimized motion path and an optimized reference trajectory.

[0298] At 910, control commands are generated for the articulated vehicle 120 based on the optimized motion path and vehicle model. In one example, control commands for the articulated vehicle 120 may be generated based on the optimized reference trajectory 107 such that the articulated vehicle 120 follows the optimized reference trajectory 107.

[0299] Thereafter, at 912, the motion of the articulated vehicle 120 is controlled based on the control commands, thereby changing its state. To this end, the modified AGT algorithm employed by the motion planner 102 can improve the performance of the predictive controller 104 for tracking the reference trajectory 107 of the articulated vehicle 120 and for enabling a successful docking maneuver 130. To this end, the motion of the articulated vehicle 120 is controlled based on the control commands. Furthermore, tracking the motion of the articulated vehicle 120 by the predictive controller 104 during maneuvering enables the docking maneuver to be performed automatically. In this regard, an optimized reference trajectory 107 with a reduced number of motion cusps and a small initial tracking error can ensure accurate tracking of the motion of the articulated vehicle 100 and accurate following of the reference trajectory 107.

[0300] Thus, the blocks of flowcharts 300, 400, 412, 426, 720, 820, and 900 support a combination of means for performing the specified functions and a combination of acts for performing the specified functions. It will also be understood that one or more blocks of flowcharts 300, 400, 412, 426, 720, 820, and 900, and combinations of blocks of flowcharts 300, 400, 412, 426, 720, 820, and 900, can be implemented by a dedicated hardware-based computer system that performs the specified functions, or a combination of dedicated hardware and computer instructions.

[0301] Alternatively, system 100 may include means for performing each of the above operations. In this regard, according to an example embodiment, examples of means for performing operations may include processor 150 and / or devices or circuitry for executing instructions or executing algorithms for processing information, for example, as described above.

[0302] Upon implementing the methods 300 , 400 , 412 , 426 , 720 , 820 and 900 disclosed herein, the end result produced by the system 100 is tangible control of the articulated vehicle 120 to perform the docking maneuver 130 .

[0303] FIG. 10 shows a schematic diagram of a system 1000 according to one embodiment. The system 1000 includes an articulated vehicle 120 including a processor 1002 configured to perform path and motion planning 1004. The articulated vehicle 120 also includes at least one sensor, such as a pressure sensor, a force sensor, a distance range finder, radar, LIDAR, and / or a camera. The sensor may be operatively connected to the processor 1002 and configured to sense information indicative of an initial state and a target state of the articulated vehicle 120. The articulated vehicle 120 may be a tractor-trailer type vehicle as described in the embodiments above. Using this information, the processor 1002 is configured to perform motion and path planning for the articulated vehicle 120 using one or more of the techniques described in the embodiments above.

[0304] The system 1000 may include one or a combination of sensors 1006, an inertial measurement unit (IMU) 1010, a processor 1012, a memory 1014, a transceiver 1016, and a display / screen 1018, which may be operatively coupled to other components via connections 1008. The connections 1008 may include buses, lines, fibers, links, or combinations thereof.

[0305] The transceiver 1016 may include, for example, a transmitter adapted to transmit one or more signals over one or more types of wireless communication networks and a receiver for receiving one or more signals transmitted over one or more types of wireless communication networks. The transceiver 1016 may enable communication with wireless networks based on various technologies, such as, but not limited to, femtocells, Wi-Fi® networks or wireless local area networks (WLANs) that may be based on the IEEE 802.11 family of standards, wireless personal area networks (WPANs) such as Bluetooth®, near field communication (NFC), networks based on the IEEE 802.15x family of standards, and / or wireless wide area networks (WWANs) such as LTE, WiMAX®, etc. The system 1000 may also include one or more ports for communicating over a wired network.

[0306] In some embodiments, the sensor 1006 may include an image sensor, such as a CCD or CMOS sensor, a laser, and / or a camera, hereinafter referred to as "sensor 1006." For example, the sensor 1006 may convert an optical image into an electronic or digital image and transmit the captured image to the processor 1012. Additionally or alternatively, the image sensor may detect light reflected from a target object in the scene and provide the captured light intensity to the processor 1012.

[0307] For example, sensor 1006 may include a color or grayscale camera that provides "color information." As used herein, the term "color information" refers to color and / or grayscale information. Generally, as used herein, a color image or color information can be viewed as including 1 to N channels, where N is some integer depending on the color space used to store the image. For example, an RGB image includes three channels: one channel each for red, blue, and green information.

[0308] In some embodiments, the processor 1012 may also receive input from the IMU 1010. In other embodiments, the IMU 1010 may include a three-axis accelerometer, a three-axis gyroscope, and / or a magnetometer. The IMU 1010 may provide velocity, orientation, and / or other position-related information to the processor 1012. In some embodiments, the IMU 1010 may output measured information synchronized with the capture of each image frame by the sensor 1006. In some embodiments, a portion of the IMU 1010 output is used by the processor 1012 to fuse sensor measurements and / or to further process the fused measurements.

[0309] System 1000 may also include a screen or display 1018 that renders images, such as color and / or depth images. In some embodiments, display 1018 can be used to display live images captured by sensor 1006, fused images, augmented reality (AR) images, graphical user interfaces (GUIs), motor control instructions, and other program output. In some embodiments, display 1018 can include and / or be housed with a touchscreen that allows a user to input data via some combination of a virtual keyboard, icons, menus, or other GUIs, user gestures, and / or input devices such as styluses and other writing implements. In some embodiments, display 1018 can be implemented using a liquid crystal display (LCD) display or a light-emitting diode (LED) display, such as an organic LED (OLED) display. In other embodiments, display 1018 may be a wearable display. In some embodiments, the results of the fusion may be rendered on display 1018 or provided to different applications, which may be internal or external to system 1000.

[0310] The example system 1000 may be modified in numerous ways, such as by adding, combining, or omitting one or more of the illustrated functional blocks, consistent with this disclosure. For example, in some configurations, the system 1000 does not include the IMU 1010 or the transceiver 1016. Furthermore, in some example implementations, the system 1000 includes various other sensors (not shown), such as an ambient light sensor, a microphone, an acoustic sensor, an ultrasonic sensor, a laser range finder, etc. In some embodiments, portions of the system 1000 take the form of one or more chipsets or the like.

[0311] The processor 1012 may be implemented using a combination of hardware, firmware, and software. The processor 1012 may represent one or more circuits configurable to execute at least a portion of a computational procedure or process related to sensor fusion and / or a method for further processing the fused measurements. The processor 1012 retrieves instructions and / or data from the memory 1014. The processor 1012 may be implemented using one or more application-specific integrated circuits (ASICs), central and / or graphics processing units (CPUs and / or GPUs), digital signal processors (DSPs), digital signal processors (DSPDs), programmable logic devices (PLDs), field-programmable gate arrays (FPGAs), controllers, microcontrollers, microprocessors, embedded processor cores, electronic devices, other electronic units designed to perform the functions described herein, or combinations thereof.

[0312] Memory 1014 may be implemented within processor 1012 and / or external to processor 1012. As used herein, the term "memory" refers to any type of long-term, short-term, volatile, non-volatile, or other memory and is not limited to any particular type or number of memories or the type of physical medium on which the memory is stored. In some embodiments, memory 1014 holds program code that facilitates automated parking or motion planning for tractor-trailer based articulated vehicle 120.

[0313] For example, memory 1014 may store sensor measurements, such as still images, depth information, video frames, program results, and the like, as well as data provided by the IMU 1010 and other sensors. Memory 1014 may include memory that stores the geometry of articulated vehicle 120, a map of the environment in which the vehicle operates, a kinematic model of articulated vehicle 120, and a dynamic system model of articulated vehicle 120. In general, memory 1014 may represent any data storage mechanism. Memory 1014 may include, for example, primary memory and / or secondary memory. Primary memory may include, for example, random access memory, read-only memory, etc. Although shown in FIG. 10 as separate from processor 1012, it should be understood that all or a portion of the primary memory may be provided within processor 1012, otherwise co-located with processor 1012, and / or coupled to processor 1012.

[0314] The secondary memory may include, for example, the same or similar type of memory and / or one or more data storage devices or systems as the primary memory, e.g., flash / USB memory drives, memory card drives, disk drives, optical disk drives, tape drives, solid-state drives, hybrid drives, etc. In some implementations, the secondary memory may be capable of operatively receiving or otherwise configurable to non-transitory computer-readable media in a removable media drive (not shown). In some embodiments, the non-transitory computer-readable media forms part of the memory 1014 and / or the processor 1012.

[0315] The above-described embodiments of the present invention may be implemented in any of numerous ways. For example, embodiments may be implemented using hardware, software, or a combination thereof. If implemented in software, the software code may be executed on any suitable processor or collection of processors, whether located on a single computer or distributed across multiple computers. Such a processor may be implemented as an integrated circuit, with one or more processors being components of the integrated circuit. However, a processor may be implemented using circuitry in any suitable format.

[0316] Also, embodiments of the present invention may be implemented as a method, examples of which are provided. The order of operations performed as part of the method may be arranged in any suitable manner. Thus, embodiments may be configured to perform operations in an order different from that illustrated, which may include performing some operations simultaneously, even though in the illustrated embodiment they are shown as a sequence of operations.

[0317] In the claims, ordinal terms such as "first" and "second" modifying a claim element do not in themselves imply any priority, precedence, or order of a claim element relative to another element, or any chronological order in which the actions of a method should be performed, but are merely used as labels to distinguish claim elements (when no ordinal term is used) from other elements of the same name.

[0318] Although the present disclosure has been described with examples of preferred embodiments, it is to be understood that various other adaptations and modifications can be made within the spirit and scope of the invention.

[0319] Therefore, it is the object of the appended claims to cover all such variations and modifications as come within the true spirit and scope of the invention.

Claims

1. 1. A path and motion planning system for controlling the movement of an articulated vehicle having one or more trailers, said path and motion planning system comprising: a memory configured to store computer-executable instructions; and one or more processors configured to execute the instructions, wherein the one or more processors execute the instructions to: collecting feedback signals indicative of a state of the articulated vehicles; determining a motion path for the articulated vehicle based on the feedback signal, the motion path including one or more forward movements of the articulated vehicle and one or more reverse movements of the articulated vehicle; generating an optimal control problem for optimizing the motion path, the optimal control problem including an integral tracking error function for the motion path, the integral tracking error function indicating at least an integral tracking error due to one or more motion cusps, the one or more motion cusps indicating a transition between forward and reverse motion in the motion path; optimizing the motion path over a prediction horizon based on solving the optimal control problem, the prediction horizon being associated with a vehicle model of the articulated vehicle having the states; generating control commands for the articulated vehicle based on the optimized motion path and the vehicle model; A path and motion planning system configured to control the movement of the articulated vehicle based on the control commands, thereby changing a state of the articulated vehicle.

2. The one or more processors further comprise: creating a graph having a plurality of nodes defining states of the articulated vehicle over the prediction horizon, the plurality of nodes of the graph including an initial node defining a goal state of the articulated vehicle and a goal node defining an initial state of the articulated vehicle, each pair of nodes in the graph being connected by an edge defined by one or a combination of collision-free motion primitives for moving the articulated vehicle between respective states of the connected nodes, the plurality of nodes connected through corresponding edges forming a first path through the graph connecting the initial node to the goal node by a series of motion primitives for moving the articulated vehicle from the initial state to the goal state; generating the motion path based on the first path formed by the graph; The path and motion planning system of claim 1 , configured to control motion of the articulated vehicle based on the motion path if the motion path is formed before an end condition is met.

3. 3. The path and motion planning system of claim 2, wherein the one or more processors are further configured to connect the goal node of the graph to an initial configuration of the articulated vehicle by solving a linear quadratic regulator (LQR) steering problem.

4. The one or more processors further comprise: determining a first number of motion cusps in the set of motion primitives of the first path; determining a first number of initial tracking errors caused by solving the LQR steering problem; configured, upon at least one of a determination that the first number of motion cusps is greater than a predetermined motion cusp threshold or a determination that the first number of initial tracking errors is greater than a predetermined initial tracking error threshold, to expand the graph to add one or more new nodes until the termination condition is met; the extension of the graph is subject to a constraint related to at least one of a total number of motion cusps or a total number of initial tracking errors; the graph is expanded to form a second path by connecting the initial node to the goal node; 4. The path and motion planning system of claim 3, wherein the second path has at least a second number of motion cusps that is less than the first number of motion cusps or a second number of initial tracking errors that is less than the first number of initial tracking errors.

5. The one or more processors further comprise: generating a reference trajectory for movement of the articulated vehicle along at least one of the first path or the second path as a function of time; The path and motion planning system of claim 4 configured to generate control commands for the articulated vehicle to cause the articulated vehicle to follow the reference trajectory.

6. The one or more processors further comprise: determining a safety region for movement of the articulated vehicle based on the generated reference trajectory, the safety region including one or more safety sets along the reference trajectory; Identifying a moving obstacle within the safety zone; 6. The path and motion planning system of claim 5, configured to initiate an automatic emergency braking (AEB) system to perform a braking maneuver to stop movement of the articulated vehicle along the reference trajectory when the dynamic obstacle is identified within the safety zone.

7. 10. The path and motion planning system of claim 1, wherein the system comprises a predictive controller configured with at least a constraint function, a quadratic objective function, and the vehicle model, the predictive controller configured to track motion of the articulated vehicle.

8. The one or more processors further comprise: generating equality and inequality constraint functions for the predictive controller, the predictive controller comprising one or more nonlinear constraints and a nonlinear vehicle model; generating an objective function for the predictive controller, the objective function minimizing an error between one or more predicted states and the reference trajectory over a prediction time horizon of the predictive controller; 8. The path and motion planning system of claim 7, configured to solve a constrained optimization problem associated with the predictive controller based on the equality constraint functions, the inequality constraint functions, and the objective function using one or more iterations of sequential quadratic programming.

9. 9. The path and motion planning system of claim 8, wherein the equality constraint function, the inequality constraint function, and the objective function for the constrained optimization problem associated with the predictive controller are tuned for at least one of forward motion of the articulated vehicle or reverse motion of the articulated vehicle.

10. 10. The path and motion planning system of claim 9, wherein one or more functional relationships defining an integral action for the constrained optimization problem of the predictive controller are determined to be different for the one or more forward motions and the one or more reverse motions, and an integral tracking error state is reset upon the occurrence of the one or more motion cusps.

11. 11. The path and motion planning system of claim 10, wherein the articulated vehicle comprises a tractor coupled to one or more trailers, and the integral action is implemented to integrate at least one of a lateral position tracking error of a trailing trailer when the articulated vehicle is performing the one or more reverse maneuvers or a lateral position tracking error of the tractor when the articulated vehicle is performing the one or more forward maneuvers.

12. 8. The path and motion planning system of claim 7, wherein the predictive controller includes one or more collision avoidance constraints for enforcing one or more collision-free motion primitives in an environment while executing a series of motion primitives for moving the articulated vehicle.

13. The path and motion planning system of claim 12 , wherein each of the one or more collision avoidance constraints is implemented using a plurality of non-linear inequality constraints.

14. 10. The path and motion planning system of claim 1, wherein the articulated vehicle is a tractor-trailer vehicle comprising a tractor coupled to one or more trailers, and the articulated vehicle is controlled by the path and motion planning system to perform a docking operation.

15. 1. A method for controlling the movement of an articulated vehicle having one or more trailers, the method comprising: collecting feedback signals indicative of a state of the articulated vehicles; determining a motion path for the articulated vehicle based on the feedback signal, the motion path including one or more forward movements of the articulated vehicle and one or more reverse movements of the articulated vehicle; generating an optimal control problem for optimizing the motion path, the optimal control problem including an integral tracking error function for the motion path, the integral tracking error function indicating at least an integral tracking error based on one or more motion cusps, the one or more motion cusps indicating a transition between forward motion and reverse motion in the motion path; optimizing the motion path over a prediction horizon based on solving the optimal control problem, the prediction horizon being associated with a vehicle model of the articulated vehicle having the states; generating control commands for the articulated vehicle based on the optimized motion path and the vehicle model; controlling movement of the articulated vehicle based on the control commands, thereby changing a state of the articulated vehicle.

16. The method further comprises: creating a graph having a plurality of nodes defining states of the articulated vehicle over the prediction horizon, the plurality of nodes of the graph including an initial node defining an initial state of the articulated vehicle and a goal node defining a goal state of the articulated vehicle, each pair of nodes in the graph being connected by an edge defined by one or a combination of collision-free motion primitives for moving the articulated vehicle between respective states of the connected nodes, the plurality of nodes connected through corresponding edges forming a first path through the graph connecting the initial node to the goal node by a series of motion primitives for moving the articulated vehicle from the initial state to the goal state; generating the motion path based on the first path formed by the graph; and if the motion path is formed before an end condition is met, controlling motion of the articulated vehicle based on the motion path.

17. The method further comprises: determining a first number of motion cusps in the set of motion primitives of the first path; determining a first number of initial tracking errors caused by solving a linear quadratic regulator (LQR) steering problem; upon at least one of determining that the first number of motion cusps is greater than a predetermined motion cusp threshold or determining that the first number of initial tracking errors is greater than a predetermined initial tracking error threshold, expanding the graph to add one or more new nodes until the termination condition is met; the extension of the graph is subject to a constraint related to at least one of a total number of motion cusps or a total number of initial tracking errors; the graph is expanded to form a second path connecting the initial node to the goal node; 17. The method of claim 16, wherein the second path has at least a second number of motion cusps that is less than the first number of motion cusps or a second number of initial tracking errors that is less than the first number of initial tracking errors.

18. The method further comprises: generating a reference trajectory for movement of the articulated vehicle along at least one of the first path or the second path as a function of time; and generating control commands for the articulated vehicle to cause the articulated vehicle to follow the reference trajectory.

19. The method further comprises: generating equality constraint functions and inequality constraints for the predictive controller, where the predictive controller comprises one or more nonlinear constraints and a nonlinear vehicle model; generating an objective function for the predictive controller with respect to a constrained optimization problem, the objective function configured to minimize an error between one or more predicted states and the reference trajectory over a prediction time horizon of the predictive controller; the equality constraint function, the inequality constraint function, and the objective function associated with the predictive controller are adjusted for at least one of forward motion of the articulated vehicle or reverse motion of the articulated vehicle; one or more functional relationships defining an integral action of the predictive controller with respect to the constrained optimization problem are determined to be different for the one or more forward motions and the one or more backward motions; An integrated tracking error state is reset at each time step of the one or more motion cusps, the method further comprising:

16. The method of claim 15, comprising solving the constrained optimization problem associated with the predictive controller based on the equality constraint functions, the inequality constraint functions, and the objective function using one or more iterations of sequential quadratic programming.

20. A non-transitory computer-readable storage medium having embedded therein a program executable by a processor to perform a method, the method comprising: collecting feedback signals indicative of a state of the articulated vehicles; determining a motion path for the articulated vehicle based on the feedback signal, the motion path including one or more forward movements of the articulated vehicle and one or more reverse movements of the articulated vehicle; generating an optimal control problem for optimizing the motion path, the optimal control problem including an integral tracking error function for the motion path, the integral tracking error function indicating at least an integral tracking error based on one or more motion cusps, the one or more motion cusps indicating a transition between forward motion and reverse motion in the motion path; optimizing the motion path over a prediction horizon based on solving the optimal control problem, the prediction horizon being associated with a vehicle model of the articulated vehicle having the states; generating control commands for the articulated vehicle based on the optimized motion path and the vehicle model; controlling movement of the articulated vehicles based on the control commands, thereby changing a state of the articulated vehicles.

Images