System and method for controlling the movement of a vehicle

Abstract

A control system for controlling the movement of a vehicle is disclosed. The control system is configured to create a graph having a plurality of nodes that define the state of the vehicle. The plurality of nodes includes an initial node that defines an initial state and a target node that defines a target state. Each pair of nodes is connected by an edge defined by a collision-free motion primitive, and the nodes include motion cusps. The plurality of nodes connected through the edges form a first path. When a first number of motion cusps in the first path is determined and it is determined that the first number of motion cusps exceeds a threshold value, the graph is expanded to add new nodes until an end condition is satisfied. The expansion of the graph is subject to a constraint associated with the total number of motion cusps. Further, a second path having fewer motion cusps than the first path is determined.

Description

[Technical Field]

[0001] This disclosure relates in general to vehicle control, and more specifically to motion planning and predictive control of autonomous or semi-autonomous vehicle-trailer systems. [Background technology]

[0002] Automated transportation systems, even if only partially automated, have the potential to reduce accidents and lead to more efficient use of vehicles and infrastructure. Connected and automated vehicles (CAVs) show great potential to improve safety and traffic flow, thereby reducing congestion, travel time, emissions, and energy consumption. Heavy-duty vehicles (HDVs), such as trucks, which are commercially available, are a key use case for vehicle automation. Automated trucks, in particular, have the potential to offer significant economic, environmental, and social benefits, given the increasing complexity of supply chain management. [Overview of the Initiative] [Problems that the invention aims to solve]

[0003] In recent years, significant progress has been made in optimization-based planning and control for autonomous vehicle operation. For large vehicles, ongoing research primarily focuses on platooning and control of articulated vehicles, particularly on highways. Therefore, optimization-based motion planning and control techniques are being studied for the automation of various HDV tasks, such as platooning or motion planning, while driving on highways. Regarding tractor-trailer motion planning, various techniques have been proposed, including sampling-based, grid-based, or composite algorithms. For example, various control algorithms, including sliding mode control, linear quadratic regulation, input-state linearization, dynamic planning, and model predictive control, have been proposed to track the resulting motion plans of tractor-trailer systems.

[0004] However, another highly complex operation requiring extensive and costly vocational training is tractor-trailer hitching. During hitch operation, the driver must precisely control the vehicle's position and orientation while performing a series of forward and reverse movements to connect a truck or other vehicle to a stationary trailer. During this process, the driver's field of view around the target hitch point may be limited. For this reason, the task of automated tractor-trailer hitch requires extremely high precision and is therefore a crucial operation in heavy vehicle operation.

[0005] Most conventional research has focused on articulated tractor-trailer operation, while neglecting hitch operation, which seems simpler but requires relatively high precision and is a critical task in HDV operation. Therefore, it is necessary to overcome the technical problems associated with hitch operation, specifically automated tractor-trailer hitch operation. [Means for solving the problem]

[0006] The objective of some embodiments is to disclose a control system for controlling the movement of a vehicle using a motion planner. Another objective of some embodiments is to disclose a method for controlling the movement of a vehicle. Yet another objective of some embodiments is to disclose a novel type of control system using a motion planning algorithm and a real-time reference tracking controller tailored to the task of automatic hitch operation. Yet another objective of some embodiments is to provide a system and method using a modified variation of the bi-directional A-search guided tree (BIAGT) algorithm and a tracking controller using nonlinear model predictive control for motion planning.

[0007] Some embodiments are based on the recognition that hitch operation (also called hitch operation) is an extremely important operation in heavy vehicle operations because it requires very high precision to successfully hitch a trailer. Some embodiments are based on the recognition that hitch operation in vehicle-trailer hitches requires extensive and costly professional training.

[0008] Some embodiments are based on the understanding that during hitch operation, the driver needs to precisely control the vehicle's position and orientation while performing a series of forward and reverse movements to connect the vehicle to the trailer. During this process, the driver's field of view of the area around the target hitch point is limited, which can increase the complexity of the hitch operation.

[0009] Some embodiments are based on the understanding that current research work focuses on articulated tractor-trailer operations, while neglecting hitch operations, which seem simpler but require relatively high precision and are a critical task in HDV operations.

[0010] Some embodiments are based on the understanding that the decrease in accuracy in certain types of operations along paths generated by sampling-based motion planning is not due to the motion planning itself, but rather to the control of the vehicle's motion along the paths generated by sampling-based motion planning.

[0011] Some embodiments are based on the recognition that sampling-based motion planning commonly generates fragmented paths containing multiple motion cusps. The presence of motion cusps is common because it not only adds flexibility to spatial search but also tends to provide the shortest distance path. Conventional motion planners, typically seeking the shortest path, end up providing paths plagued by unnecessary motion cusps that impair the positioning accuracy of the integrated control system. Conventional motion planners tend to improve path optimality in terms of path distance and therefore cannot address the problem of unnecessary motion cusps, but rather, as theory suggests, they sometimes add more motion cusps. Some inventions are based on the recognition that taking motion cusps into account by integrating the cost function with soft constraints typically results in longer computation time but reduces the number of unnecessarily large cusps.

[0012] Some embodiments are based on the understanding that, theoretically, the presence of a cusp of motion may lead to inconvenience and the need for gear shifting, but should not result in accuracy problems.

[0013] Some embodiments are based on the understanding that the presence of motion cusps necessitates vehicle motion at zero and near-zero velocities, but such motion is problematic for control accuracy because the vehicle's inertia governs its dynamic behavior. Therefore, the uncertainty arising from each motion cusp in the vehicle's path increases with the number of motion cusps, leading to accuracy problems.

[0014] The objective of some embodiments is to generate paths for performing operations while reducing the number of motion cusps in the paths. Some embodiments are based on the understanding that sampling-based motion planning constructs a tree by adding new nodes based on a so-called cost-to-go, which indicates the cost of creating a path that passes through the new node. Therefore, it is reasonable to penalize the cost of a node for creating motion cusps. However, the approach of using motion cusps as a soft constraint in the cost function is computationally expensive for sampling-based motion planning and may fail to find a feasible solution.

[0015] Therefore, to overcome the above problems, some embodiments disclose a multi-stage motion planner that aims to find feasible paths using a sampling-based motion planner without considering the number of motion cusps in the first stage. However, instead of terminating the planner and outputting the paths to the controller after finding feasible paths, the multi-stage motion planner continues to expand the tree in the second stage using hard constraints on the number of motion cusps.

[0016] In addition, the objective of some embodiments is to provide sampling-based motion planning that can enable more accurate vehicle movement. Examples of maneuvers that benefit from improved accuracy include hitching and docking of vehicles such as trucks. However, even in ordinary parking in confined spaces, the precise execution of parking maneuvers can be beneficial.

[0017] The objective of some embodiments is to provide an integrated system that uses a tree-based motion planning algorithm and a real-time reference tracking controller tailored to the task of automated tractor-trailer hitching. The objective of some embodiments is to provide a bidirectional A-search guide tree (BIAGT) path and modified variations of the motion planning algorithm. When tracking a reference trajectory obtained from the BIAGT algorithm to successfully complete the hitch operation, the objective of some embodiments is to provide a real-time feasible implementation of a nonlinear model predictive controller (NMPC) for the combined lateral and longitudinal dynamics of the tractor.

[0018] Some embodiments are based on the understanding that a vehicle, such as a truck or tractor, is continuously steered forward and in the reverse direction while steering toward the hitch point where the vehicle's hitch mechanism connects to a trailer.

[0019] Some embodiments are based on the understanding that, unlike typical autonomous driving on highways or in urban environments, hitch operation is more lenient towards path-tracking errors when the vehicle is far from the hitch point but both the lateral position error requirements and the direction of travel error requirements in the vicinity of the hitch point are very strict. Therefore, there are specific requirements for automated hitch operation to connect to the trailer safely without damaging the hitch mechanism.

[0020] In some embodiments, a set of vehicle attitudes is P⊂R 3 Defined as, P free This is based on the understanding that ⊂P represents a set of collision-free postures for a vehicle.

[0021] The objective of some embodiments is to satisfy specific requirements while maintaining an initial zero-velocity attitude p0∈P free From hitch position p f ∈P free This involves calculating the trajectory and controlling the vehicle in order to perform the hitch operation.

[0022] Some embodiments are based on the understanding that, in a set where P f ∈P f is around, and P f ⊂P free as a requirement of the automatic hitch system, the lateral position error |e Y | with respect to the preferred hitch orientation is < 0.1 meter (m) and the travel direction error |e Ψ | is < 10 degrees (deg).

[0023] Some embodiments are based on the recognition that the planning and control algorithms disclosed in this disclosure need to be executed on an automotive-grade embedded system platform and meet real-time requirements.

[0024] The purpose of some embodiments is to provide an adjusted modification of the BIAGT algorithm for the motion planning of vehicle-trailer hitch operations.

[0025] The purpose of some embodiments is to provide an NMPC design for offset-free reference tracking to meet the strict accuracy requirements of hitch operations.

[0026] The purpose of some embodiments is to provide hardware-in-the-loop (HIL) verification results for planners and controllers for automotive vehicle-trailer hitches.

[0027] Accordingly, one embodiment discloses a control system for controlling the motion of a vehicle using a motion planner. The control system comprises at least one processor and a memory storing instructions. The at least one processor is configured to create a graph having a plurality of nodes that define the states of the vehicle. The plurality of nodes in the graph include an initial node that defines the initial state of the vehicle and a target node that defines the target state of the vehicle, and each pair of nodes in the graph is connected to an edge defined by one or a combination of collision-free motion primitives that move the vehicle between the respective states of the connected nodes. Each node includes several motion cusps, and the plurality of nodes connected through the corresponding edges form a first path through the graph that connects the initial node to the target node by a set of motion primitives that move the vehicle from the initial state to the target state. The at least one processor is configured to determine a first number of motion cusps in the set of motion primitives in the first path, where each of the first number of motion cusps indicates a switch between forward motion and reverse motion in the set of motion primitives. At least one processor is configured to expand the graph to add new nodes until a termination condition is met, once it determines that the first number of motion cusps exceeds a threshold. The graph expansion is constrained by the total number of motion cusps, and the graph expands to form a second path connecting the initial nodes to the target nodes. The second path has a second number of motion cusps that is less than the first number of motion cusps. At least one processor is configured to control the movement of the vehicle based on the second path.

[0028] At least one processor is further configured to expand the graph until a termination condition is met, and, if a second path is formed before the termination condition is met, to control the movement of the vehicle based on the second path.

[0029] At least one processor is further configured to control the movement of the vehicle based on the first path if a second path is not formed until a termination condition is met.

[0030] At least one processor is further configured to compare the first number of moving cusps to a threshold, and if the first number of moving cusps in the first path exceeds the threshold, expand the graph to add new nodes.

[0031] Some embodiments are based on the understanding that the second path includes the minimum number of motion cusps necessary to move the vehicle from an initial state to a target state.

[0032] At least one processor is further configured to generate a trajectory for the vehicle's movement along at least one of a first or second path, as a function of time. Furthermore, at least one processor is configured to generate control commands for the vehicle to cause it to follow this trajectory.

[0033] At least one processor is further configured to generate a trajectory for the movement of a vehicle by adding a time dimension to at least one of the first or second trajectories, the addition of the time dimension including adding an additional period at one or more cusps of the trajectory.

[0034] At least one processor is further configured to create a graph that forms a first path by repeatedly extending one of several nodes to create a new node, or by connecting two existing nodes of several nodes.

[0035] In some embodiments, in order to form at least one of a first path or a second path, at least one processor is further configured to create a first tree of a first set of nodes starting from an initial node and a second tree of a second set of nodes starting from a target node. Furthermore, at least one processor is configured to connect the first tree to the second tree by connecting a node from the first set of nodes to another node from the second set of nodes.

[0036] At least one processor is further configured to form a first path by connecting a first tree to a second tree using a collision-free connection path having one of 48 Reeds-Shepp (RS) patterns. Furthermore, at least one processor is configured to form a second path by connecting a first tree to a second tree using a collision-free connection path having one of 8 RS patterns from the 48 RS patterns, the 8 RS patterns being cuspless.

[0037] In some embodiments, in order to form at least one of a first or second path, at least one processor is further configured to select an expandable node from at least one of a first node in a first tree or a second node in a second tree, based on the cost associated with the expandable node. Furthermore, at least one processor is configured to expand the graph by adding child nodes connected to the expandable node by edges defined by collisionless motion primitives, such that the cost of the child nodes is less than the cost of the expandable node. The cost of a child node is the minimum cost to reach the target node from the initial node through the child nodes, and the cost of a child node includes a first cost for the initial path through the first set of nodes, a second cost for the target path through the second set of nodes, and a third cost for the connecting path between the nodes in the first set and the nodes in the second set.

[0038] At least one processor is further configured to remove a child node from the queue while expanding the graph to form a second path if the edge between the child node and the corresponding expandable node contains a number of moving cusps greater than a predetermined threshold.

[0039] In some embodiments, at least one processor is further configured to select expandable nodes to form a first path without constraints associated with the number of moving cusps. Furthermore, at least one processor is configured to select expandable nodes to form a second path, where the expansion of the graph is constrained by the total number of moving cusps.

[0040] At least one processor is further configured to initialize a second tree with a root node located at a distance from the target node, with no cuspatic movement to the target node.

[0041] Some embodiments are based on the understanding that the root node is connected to the target node along with an edge that defines the linear motion.

[0042] Another embodiment discloses a method for controlling the movement of a vehicle. This method includes creating a graph having a plurality of nodes that define the states of the vehicle. The plurality of nodes in the graph include an initial node that defines the initial state of the vehicle and a target node that defines the target state of the vehicle, and each pair of nodes in the graph is connected to an edge defined by one or a combination of collision-free motion primitives that move the vehicle between the respective states of the connected nodes. Each node includes several motion cusps, and the plurality of nodes connected through the corresponding edges form a first path through the graph that connects the initial node to the target node by a set of motion primitives that move the vehicle from the initial state to the target state. This method includes determining a first number of motion cusps in the set of motion primitives in the first path, where each of the first number of motion cusps indicates a switch between forward and reverse motion in the set of motion primitives. If this method determines that the first number of motion cusps exceeds a threshold, it includes extending the graph to add new nodes until a termination condition is met. The graph is extended under constraints related to the total number of moving cusps, so that the graph forms a second path connecting the initial nodes to the target nodes. The second path has a second number of moving cusps, which is fewer than the first number of moving cusps. This method involves controlling the movement of a vehicle based on the second path.

[0043] Further embodiments disclose a computer-readable storage medium incorporating a processor-executable program for performing a method for controlling the movement of a vehicle using a motion planner. The method includes creating a graph having a plurality of nodes that define the states of the vehicle. The plurality of nodes in the graph include an initial node that defines the initial state of the vehicle and a target node that defines the target state of the vehicle, and each pair of nodes in the graph is connected to an edge defined by one or a combination of collision-free motion primitives that move the vehicle between the respective states of the connected nodes. Each node includes several motion cusps, and the plurality of nodes connected through the corresponding edges form a first path through the graph, connecting the initial node to the target node by a set of motion primitives that move the vehicle from the initial state to the target state. The method includes determining a first number of motion cusps in the set of motion primitives in the first path, where each of the first number of motion cusps indicates a switch between forward and reverse motion in the set of motion primitives. If the method determines that the first number of motion cusps exceeds a threshold, it includes extending the graph to add new nodes until a termination condition is met. The graph is extended under constraints related to the total number of moving cusps, so that the graph forms a second path connecting the initial nodes to the target nodes. The second path has a second number of moving cusps, which is fewer than the first number of moving cusps. This method involves controlling the movement of a vehicle based on the second path.

[0044] Some embodiments are based on the understanding that performing a vehicle hitch operation to hitch a vehicle onto a trailer is a complex, time-consuming, and costly professional training process. In addition, because visibility of the area associated with the hitch point is limited, the vehicle driver may have to make several extra movements (such as forward and reverse movements) to accurately hitch the vehicle onto the trailer. Due to the limited visibility, the hitch operation may also be prone to accidents. However, current route and motion planning systems do not automate such vehicle hitch operations. Therefore, the objective of this disclosure is to enable automated vehicle hitch to trailer. Automation can reduce the time required for hitch and the probability of accidents. In this way, such complex hitch operations can be performed automatically.

[0045] Embodiments of this disclosure will be further described with reference to the accompanying drawings. The drawings shown are not necessarily to scale and are rather primarily intended to illustrate the principles of the embodiments of this disclosure. [Brief explanation of the drawing]

[0046] [Figure 1A] This figure shows examples of graphs illustrating unconstrained, hard-constrained, and soft-constrained motion primitives for motion planning, according to some embodiments of the present disclosure. [Figure 1B] This figure shows an example of an environment in which a control system is implemented according to some embodiments of this disclosure. [Figure 1C] This figure shows an example of an automatic hitch operation according to some embodiments of the present disclosure. [Figure 1D] A schematic diagram of a vehicle including a system employing the principles of several embodiments, according to some embodiments of this disclosure, is shown. [Figure 1E] This figure shows an example of plotting a path for controlling the motion of a vehicle, according to some embodiments of the present disclosure. [Figure 2]A block diagram of a system for controlling the movement of a vehicle, according to some embodiments of the present disclosure, is shown. [Figure 3A] This figure shows an example of a method for generating a first path to control the motion of a vehicle, according to some embodiments of the present disclosure. [Figure 3B] This figure shows an example of a graph structure for forming a first path according to some embodiments of the present disclosure. [Figure 4A] This figure shows an example of a method for controlling the motion of a vehicle based on a second path, according to some embodiments of the present disclosure. [Figure 4B] This figure shows a graph structure 420 as an example for forming a second path according to some embodiments of the present disclosure. [Figure 4C] This figure shows an example of a method for performing graph creation during the second stage according to some embodiments of the present disclosure. [Figure 4D] This figure shows an example of a node extension method according to some embodiments of the present disclosure. [Figure 5A] This figure shows an example of a type of motion primitive according to one embodiment of the present disclosure. [Figure 5B] This figure shows an example of a type of motion primitive according to another embodiment of the present disclosure. [Figure 6A] An example of a block diagram of a linear prediction controller according to one embodiment of this disclosure is shown. [Figure 6B] An example of a block diagram of a nonlinear prediction controller according to another embodiment of this disclosure is shown. [Figure 7A] This figure shows an example of a reference orbit according to some embodiments of the present disclosure. [Figure 7B] This figure shows an example of a method for switching the NLP objective function and constraint function of a nonlinear prediction controller according to some embodiments of the present disclosure. [Figure 8A] This figure shows an example of an automatic hitch operation according to some embodiments of the present disclosure. [Figure 8B]This figure shows an example of a method for reinitializing a prediction controller according to some embodiments of the present disclosure. [Figure 9] Schematic diagrams of systems according to some embodiments of this disclosure are shown. [Modes for carrying out the invention]

[0047] In the following description, numerous specific details are included for illustrative purposes to ensure a full understanding of this disclosure. However, it will be apparent to those skilled in the art that this disclosure can be implemented without these specific details. In other examples, apparatus and methods are shown in block diagram form solely for the purpose of avoiding obscuration of this disclosure. The intended modifications are those that may be made to the function and configuration of the elements, as described in the appended claims, without departing from the spirit and scope of the disclosed subject matter.

[0048] The terms “for example,” “for instance,” and “such as,” as used herein and in the claims, as well as the verbs “comprising,” “having,” and “including,” and each of these verbs in other forms, should be interpreted as open-ended, meaning that when used with an enumeration of one or more components or other items, the enumeration should not be considered to exclude any further components or items. The term “based on” means based at least partially. Furthermore, it should be understood that the style and terminology used herein are for illustrative purposes only and should not be considered restrictive. Any headings used herein are for convenience only and have no legal or restrictive effect.

[0049] Specific details are provided in the following description for a full understanding of the embodiments. However, those skilled in the art will understand that embodiments may be carried out without these specific details. For example, to avoid obscuring the embodiments with unnecessary details, systems, processes, and other elements in the disclosed subject matter may be shown as components in the form of block diagrams. In other examples, well-known processes, structures, and techniques may be shown without unnecessary details to avoid obscuring the embodiments. Furthermore, similar reference numbers and names in different drawings refer to similar elements.

[0050] The objective of some embodiments is to disclose systems and methods for controlling the motion of a vehicle using a motion planner. Another objective of some embodiments is to disclose path and motion planning algorithms and predictive controllers for controlling the movement of a vehicle in real time. An example of a predictive controller is model predictive control (MPC) for determining control inputs based on a dynamic model and constraints of the controlled system. An example of a path and motion planning algorithm is the bidirectional A-search guide tree (BIAGT). Yet another objective of some embodiments is to disclose techniques for controlling the movement or motion of a vehicle during hitch operation in an automated manner, i.e., without human intervention. Yet another objective of some embodiments is to control the vehicle from an initial posture p0 to an ending or goal posture p while avoiding collisions between the vehicle and any stationary obstacles in the environment. f The objective is to disclose a technique for online calculation of kinematically feasible reference orbits up to that point.

[0051] Typically, the BIAGT algorithm can compute kinematically feasible solutions to automated vehicle parking tasks by forming a graph and searching for a trajectory. In operation, the BIAGT algorithm can find the shortest path from the initial node to the goal node by performing a bidirectional search and initiating this search simultaneously from both directions. In particular, BIAGT can initiate the generation of subgraphs from both directions, for example, one starting from the initial node and the other from the goal node. In this way, the BIAGT algorithm can perform a forward search from the initial node to the goal node and a reverse search from the goal node to the initial node. Therefore, the search can be terminated when the two subgraphs intersect. The trajectory can then be determined based on this graph.

[0052] Therefore, vehicle hitch operation can be considered a special parking task with very strict requirements regarding errors in lateral position and direction of travel angle at the final stage of operation. The BIAGT is modified to perform vehicle hitch operation. Embodiments of this disclosure will be described in detail with reference to the accompanying drawings.

[0053] Figure 1A shows an example of a graph illustrating unconstrained, hard-constrained, and soft-constrained motion primitives for motion planning. According to some embodiments, vehicle path planning plays a crucial role in vehicle navigation systems. Therefore, trajectory planning is an essential task for autonomous vehicles. Map and sensor-based data form the basis for generating trajectories that serve as target values ​​tracked by the controller. When generating target trajectories, feasibility and potential collisions must be considered, in addition to comfort.

[0054] During path planning and trajectory generation, constrained optimization may be necessary. The vehicle's motion must be planned and adjusted to accomplish the driving task, taking into account the constraints imposed by the selected model. The motion planning layer is responsible for calculating a safe, comfortable, and mechanically feasible trajectory from the vehicle's current configuration or state to the goal configuration or state. The goal configuration may vary depending on the situation. For example, the goal location might be a hitch point where a vehicle, such as a tractor, can hitch onto a trailer to form a tractor-trailer configuration.

[0055] The path planning problem is the problem of finding a path σ(α): [0,1]→X for a vehicle (or more broadly, a robot) in its constituent space X, starting from an initial state and reaching a target position, while satisfying constraints. By solving the path planning problem, it is possible to generate a mechanically feasible path. Since the robot may have several motion constraints, such as non-holonomic states in an under-actuated system, the generated trajectory must be smooth without extreme changes of direction. A feasible path planning problem is one in which the quality of the solution is not considered, but rather the determination of a path that satisfies several given problem constraints (i.e., hard constraints).

[0056] However, while generating feasible paths guarantees obstacle-free navigation, the quality of the generated paths may be affected by the imposition of hard constraints. For example, paths or trajectories may have abrupt changes of direction, be of low quality, or be longer. As shown in Figure 1, motion planning with hard constraints 102 on motion primitives can hinder path efficiency.

[0057] Therefore, since constraints reflect the limits of safety and quality, it is important not to apply any constraints in dynamic optimization. In addition, in the case of no constraints, 104, it may result in an unfeasible path that does not efficiently overcome obstacles.

[0058] Therefore, an optimal path planning solution is needed. Optimal path planning is the problem of finding a path that optimizes certain quality criteria under given constraints. Finding the optimal path is difficult. Furthermore, there may not be a single (efficient) algorithm that can solve all cases of the problem while satisfying the constraints. Therefore, it may be necessary to impose soft constraints to generate the optimal path.

[0059] Soft constraints are conditions on the trajectory generated by the solution. These may not be unsatisfactory, but rather must be satisfied. However, we are prepared to accept that they may not be satisfied due to the cost of satisfying them or conflicts with other constraints or goals. If there are multiple soft constraints and there are conflicts among them, or if the cost of satisfying them turns out to be high, we need to decide which of the various constraints should take priority.

[0060] According to this disclosure, a soft constraint 106 is imposed on the number of moving cusps to ensure the accuracy of the path. Embodiments of this disclosure describe imposing a soft constraint on the path plan based on the number of moving cusps in the generated path.

[0061] Figure 1B shows an environment in which a control system 100 is implemented according to some embodiments of the present disclosure. The control system 100 comprises a motion planner 108, a predictive controller 110, and a system 120. The motion planner 108 may be configured to control the movement of a vehicle. In one example, the motion planner 108 may be configured to control the movement of a vehicle while performing a hitch operation. During a hitch operation, the vehicle may require continuous forward and reverse movement while steering toward the hitch point. At the hitch point, a fifth wheel of the vehicle hitch mechanism can be connected to the trailer.

[0062] Furthermore, the motion planner 108 is connected to the predictive controller 110 and the system 120, for example, via the state estimator 130. In some implementations, the predictive controller 110 is an MPC controller configured with a dynamic system model 107. The system model 107 may include a set of equations that describe the changes over time of the state and output 103 of the system 120. In one example, this set of equations can express the changes in the state and output 103 as a function of the current input, previous input, and previous output. The system model 107 may further include constraints 109 that describe the physical and operational limitations of the system 120.

[0063] During operation, the predictive controller 110 receives a reference trajectory 105 calculated by the motion planner 108, which represents the desired behavior of the system 120. The reference trajectory 105 may be, for example, a desired sequence of one or more motion commands. In one example, the reference trajectory 105 may represent a time function of the path the vehicle should travel to perform an operation such as a hitch operation. For example, the motion planner 108 may generate the path based on, for example, a graph structure. Furthermore, a time dimension may be added to the path to generate the reference trajectory 105.

[0064] The prediction controller 110 generates a control signal (also called a control input) 111, which functions as an input to the system 120, in response to receiving the reference track 105. The system 120 updates its output 103 in response to receiving the control input 111. Based on the measurement of the output 103 of the system 120, the state estimator 130 updates the estimated state 121 of the system 120. The estimated state 121 of the system 120 provides state feedback to the prediction controller 110. For example, the prediction controller 110 uses the state of the system 120 to track the movement of the vehicle to determine whether the vehicle is traveling along the reference track 105.

[0065] System 120 may be any machine or device controlled by a certain operation control input 111. In one example, the control input 111 may be associated with a physical quantity such as voltage, pressure, force, or torque. For example, the control input 111 may represent a control value such as steering angle, acceleration, or driving speed for controlling a vehicle.

[0066] System 120 may be configured to generate several controlled output signals 103 (also called outputs 103). For example, outputs 103 may be associated with physical quantities such as current, flow rate, speed, or position indicating a state transition of System 120 from a previous state to the current state. In one example, outputs 103 may be partially associated with previous output values ​​of System 120, and partially associated with previous and current input values. Dependencies on previous inputs and previous outputs may be encoded within the state of System 120. During the operation of System 120, for example, System 120 may generate outputs 103 indicating a change in the state of a vehicle, such as a change in the precise position, orientation, or setting of the controlled vehicle, based on the vehicle's movement from one state to another while following a reference track 105. In one example, a control input 111 may also be provided to the vehicle's actuators to move the vehicle in an automated manner based on the reference track. Outputs 103 may include a set of output values ​​generated by System 120 following the application of a certain input value.

[0067] System model 107 may be associated with system 120. System model 107 may include a set of mathematical equations that describe how the output 103 of system 120 may change over time as a function of the current input, previous input, and previous output. The state of system 120 may correspond to any set of information, such as information that changes over time. For example, the state of system 120 may be a subset of current and previous inputs and outputs that, together with system model 107 of system 120 and future inputs, can uniquely determine the future motion of the vehicle or the output of system 120.

[0068] System 120 may be subject to physical limitations and specific constraints 109. These constraints 109 may be applied to limit the range of outputs 103 and inputs 111, and, in some cases, to limit the states in which System 120 is permitted to operate. In some examples, constraints may correspond to operating speed limits, limitations on the physical area associated with the environment, etc.

[0069] The predictive controller 110 may be implemented in hardware or as a software program executed on a processor, such as a microprocessor. The predictive controller 110 may receive an estimated state 121 of the system 120 at a fixed or variable control period sampling interval. In one example, the estimated state 121 may indicate the future motion of the vehicle based on the current and previous inputs and outputs of the system 120. Based on the control inputs 111 provided by the predictive controller 110, the vehicle's actuators may be controlled to move the vehicle. The actuators may be controlled to drive the vehicle on a reference track 105. Control of the vehicle based on such control inputs 111 may result in a change of the vehicle's state. Subsequently, by determining the estimated state 121 based on the outputs of the system 120, the predictive controller 110 can monitor and track the vehicle's movement.

[0070] The predictive controller 110 can receive an estimated state 121 and a desired reference trajectory 105. Furthermore, the predictive controller 110 may be configured to use the received information to determine an input, such as a control signal or control input 111, to operate the system 120 to control the movement of the vehicle. The predictive controller 110 may also track the movement of the vehicle based on the estimated state 121 and the reference trajectory 105.

[0071] The motion planner 108 may be implemented in hardware or as a software program executed on a processor. Such a processor may be the same as or different from the predictive controller 110. The motion planner 108 is configured to receive an estimated state 121 of the system 120 and a desired target state 101 at a fixed or variable control period sampling interval. The motion planner 108 is configured to use the received information to determine a reference trajectory 105 for the predictive controller 110. In one example, the reference trajectory may be updated based on changes in the estimated state 121.

[0072] The state estimator 130 may be implemented in hardware or as a software program executed on a processor. Such a processor may be the same as or different from the predictive controller 110 or the motion planner 108. The state estimator 130 may be configured to receive the output 103 of the system 120 at a fixed or variable control period sampling interval. Furthermore, the state estimator 130 may use new output measurements and previous output measurements to determine the estimated state 121 of the system 120. In this way, the reference trajectory 105 generated by the motion planner 108 may be updated based on the estimated state 121 of the system 120 and the control signals or control inputs 111 generated by the predictive controller 110. The updated reference trajectory can optimize the operation of tasks such as vehicle steering.

[0073] Figure 1C shows an example of an automatic hitch operation according to some embodiments of the present disclosure. For example, a vehicle 153 may need to perform a hitch operation to hitch its fifth wheel to a trailer 154. The hitch between the vehicle 153 and the trailer may occur at a hitch point. For this purpose, the hitch operation may include a sequence of one or more forward movements 151 and one or more reverse movements 152 to reach an initial state configuration 157 (also referred to as initial state 157 or source state 157) from a target goal state configuration 156 (also referred to as goal state 156 or target state 156). The hitch operation must be performed without causing any collision between the vehicle 153 and any obstacles in the vehicle 153's environment (indicated as obstacles 158a and 158b).

[0074] As mentioned earlier, unlike typical autonomous driving on main roads or in urban environments, hitch operation is prone to path tracking errors when the vehicle 153 is far from the hitch point. However, hitch operation is very stringent in terms of both lateral position error requirements and forward-direction error requirements near the hitch point. Therefore, it is possible to prevent damage to the hitch mechanism and successfully connect to the trailer 154 for hitch operation.

[0075] In some embodiments of the present disclosure, the automatic hitch operation must be performed based on a reference track 105. The reference track 105 may be calculated by a motion planner 108. For example, the reference track 105 may include a forward motion 151, followed by a reverse motion 152, and then a hitch motion between the vehicle 153 and the trailer 154. Some embodiments of the present disclosure are based on the understanding that, in order to enable a successful hitch between the fifth wheel of the tractor 153 and the hitch system of the trailer 154, the positional and directional errors with respect to the reference track 105 must be sufficiently small towards the end of the reference track 105 in the final stage of the hitch operation, i.e., when the hitch point is very close.

[0076] According to some embodiments, a set of vehicle attitudes of vehicle 153 is P⊂R3 Defined as, P free ⊂P represents a set of collision-free postures for vehicle 153. Therefore, several requirements or constraints exist for performing automatic hitch operation. In one example, the requirements for automatic hitch operation may include automating the vehicle's hitch operation to eliminate human intervention. Furthermore, the requirements for automatic hitch operation may include changing the hitch posture p of vehicle 153 from its initial posture p0 to its current posture p f This may include calculating the reference trajectory 105 online in real time or near real time so that the reference trajectory 105 is kinematically feasible while avoiding collisions between the vehicle 153 and any stationary obstacles in the environment, such as obstacles 158a and 158b. In one example, the requirements for automatic hitch operation may also include a lateral position error |e| relative to the preferred hitch orientation for the success of the hitch and the safety of the hitch mechanism. Y | must be less than 0.1 meters (m), and the direction error |e Ψ This may also include the requirement that | must be less than 10 degrees (deg). In addition, lateral position error and direction error are included in hitch attitude p f ∈P f A group P centered around f ⊂P free It is necessary that... Furthermore, the requirements for automatic hitch operation may include that if a dynamic obstacle enters a safety set around the current or predicted position of the vehicle 153, real-time trajectory operation and tracking will be interrupted by an emergency braking system, and real-time trajectory operation and tracking will be resumed when the dynamic obstacle moves away from or leaves the environment.

[0077] In addition, the system for controlling the movement of the vehicle 153 for automatic hitch operation must run on an automotive-grade embedded system platform and meet real-time requirements. In particular, the motion planner 108 may have to execute motion planning when stopped and calculate the reference trajectory 105 within a maximum calculation time of 2 to 5 seconds. Furthermore, the predictive controller 110 is T SIn some cases, it may be necessary to continuously perform trajectory tracking while vehicle 153 is moving, with a sampling time of 50 milliseconds (ms).

[0078] In some cases, the path and motion planning system may need to control the movement of vehicle 153 to perform a hitch operation under the assumption of normal driving conditions, i.e., during non-limit operations. Subsequently, the vehicle model may be based on a single-track model in which two wheels on each axle of vehicle 153 are grouped together. Furthermore, if vehicle 153 has multiple rear axles, these multiple axles may be grouped together into a model with only two wheels: one front wheel and one rear wheel. Dynamic state models based on force and torque balance are generally more accurate than kinematic models, but the differences at relatively low speeds, which are typical in vehicle-trailer hitch operations, are small and compensated for by the feedback properties of the predictive controller 110.

[0079]

number

[0080]

number

[0081] In some embodiments of the present disclosure, one or more road-side units (RSUs) or infrastructure sensing devices 165 may be used for the precise execution of automated vehicle-trailer hitch operations based on accurate sensing of the current state of the vehicle 153 and the route and motion planning system and the current environment. For example, one or more RSUs 165 may include one or more sensors, such as distance rangefinders, radar, LiDAR, and / or cameras, as well as sensor fusion technology, to accurately sense the state of the vehicle 153 and the dynamic environment around the vehicle 153.

[0082] In some embodiments of the present disclosure, the calculations for sensor fusion technology may be performed in the cloud or on one or more mobile edge computers (MECs), which may be embedded as part of one or more RSUs 165 or as separate devices connected to one or more RSUs 165. In some embodiments of the present disclosure, the communication network 160 may be used for real-time communication between the vehicle 153 and one or more RSUs 165 or infrastructure sensing devices 165 and the route and motion planning system.

[0083] Figure 1D shows a schematic diagram of a vehicle 153 that includes a route and motion planning system 172 employing the principles of several embodiments. Examples of vehicle 153 may include, but are not limited to, passenger cars, heavy vehicles, tractor-trailers, buses, or rovers. In addition, vehicle 153 may be an autonomous or semi-autonomous vehicle.

[0084] In accordance with several embodiments of this disclosure, techniques for controlling the movement of a vehicle 153 are disclosed. Examples of such movement may include, but are not limited to, the lateral movement of the vehicle 153 controlled by the vehicle 153's steering system 173. In one embodiment, the steering system 173 is controlled by a path and motion planning system 172. In addition to or instead of that, the steering system 173 may be controlled by the driver of the vehicle 153.

[0085] Vehicle 153 may also include an engine 176 that can be controlled by a route and motion planning system 172 or by other components of vehicle 153. Vehicle 153 may also include one or more sensors 174 for sensing the surrounding environment. Examples of sensors 174 may include, but are not limited to, a distance rangefinder, radar, LiDAR, and a camera. Vehicle 153 may also include one or more sensors 175 for sensing the current momentum and internal state of vehicle 153. Examples of sensors 175 may include, but are not limited to, a Global Positioning System (GPS), an accelerometer, an inertial measuring device, a gyroscope, a shaft rotation sensor, a torque sensor, a deflection sensor, a pressure sensor, and a flow sensor. Sensors 174 and 175 may provide information to the route and motion planning system 172. Vehicle 153 may include a transceiver 177 that enables communication functions of the route and motion planning system 172 via a wired or wireless communication channel.

[0086] For example, the route and motion planning system 172 may control the operation or movement of the vehicle 153. In this regard, the route and motion planning system 172 may generate a reference trajectory 105 for the vehicle 153 to move in order to complete a task. Furthermore, the route and motion planning system 172 may generate motion commands for the vehicle 153 based on the reference trajectory 105 to be followed. Furthermore, the route and motion planning system 172 may control components of the vehicle 153, such as the steering system 173, to configure the vehicle 153 to perform a task. In one example, the task may be a hitch task, in which case the motion commands may include a sequence of forward and reverse motion.

[0087] Figure 1E shows an example of plotting paths for controlling the motion of a vehicle according to several embodiments. In this example, path 182 includes several nodes (shown as nodes 186a, 186b, 186c, 186d, 188a, 188b, and 188c), and path 184 includes nodes 190a, 190b, 190c, 190d, 190e, 192a, and 192b. Paths 182 and 184 also include several edges connecting each corresponding pair of nodes. Nodes 186a, 186b, 186c, 186d, 188a, 188b, and 188c are selected to generate path 182 from several other nodes so that nodes 186a, 186b, 186c, 186d, 188a, 188b, and 188c are mechanically feasible, i.e., to ensure a collision-free path 182. Similarly, nodes 190a, 190b, 190c, 190d, 190e, 192a, and 192b are dynamically feasible nodes.

[0088] For accurate hitch operation, it may be necessary to generate routes with constraints on the total number of motion cusps in the route. Specifically, when the vehicle is close to the hitch point, some motion cusps should not be present or there should be fewer motion cusps. For this reason, route 182 shows that it imposes a hard constraint on the total number of motion cusps, particularly towards the end of the route when the vehicle may be close to the hitch point. As shown, in route 182, nodes 186a, 186b, 186c, and 186d may not completely eliminate motion cusps in their corresponding motion primitives, while nodes 188a, 188b, and 188c may satisfy the hard constraint on the number of motion primitives and may contain no motion cusps at all.

[0089] Generating such a path 182 may be difficult, particularly in terms of satisfying hard constraints toward the ends of path 182. Therefore, embodiments of the present disclosure provide a technique that ensures that some of the nodes of path 184, such as nodes 192a and 192b, satisfy the constraints, while ensuring that the entire path 184 satisfies the constraints to some extent without compromising path quality. In this way, soft constraints are imposed on a dynamically feasible path 182 in order to generate a path 184 that satisfies the constraints associated with the maximum number of moving cusps in the path.

[0090] A mode in which the path and motion planning system 172 operates to generate a path having constraints associated with the maximum number of motion cusps will be described in detail with reference to the following drawings.

[0091] Figure 2 shows schematic diagrams of different layers of a path and motion planning system 172 for controlling the movement of a vehicle 153, according to some embodiments of the present disclosure. It will be understood that autonomous and semi-autonomous vehicles are complex systems that require the integration of highly interconnected sensing and control components. The complexity associated with the development of software and hardware components of autonomous vehicles and dynamic driving conditions makes autonomous control of the vehicle complex.

[0092] According to one embodiment, a route and motion planning system 172 for controlling the movement of a vehicle 153 may include a route and motion planning layer 210 and a vehicle controller layer 220. For example, the route and motion planning layer 210 may be implemented by a motion planner 108, and the vehicle controller layer 220 may be implemented by a predictive controller 110.

[0093] In one example, the route and motion planning layer 210 calculates a reference track 105 and provides it to the vehicle controller layer 220. Furthermore, the vehicle controller layer 220 calculates control inputs 111 for the system 120 to track the reference track 105. The system 120 can update the reference track 105 to optimize vehicle motion. Then, the vehicle controller layer 220 can execute a desired sequence of one or more motion commands.

[0094] According to some embodiments of the present disclosure, the layers of the path and motion planning system 172 may also include a decision layer 200 and / or an actuator controller layer 230.

[0095] During operation, the sequence of destinations in the form of a route may be calculated by a route planner through a road network. For example, the route planner may utilize map-based techniques to generate a route between a source and a destination. Such a route is then navigated by the vehicle 153 in an automatic or semi-automatic manner. According to this disclosure, the vehicle 153 may travel this route in an automatic manner. In addition, this route may accommodate hitch operations, parking operations, etc.

[0096] Given a route, the decision layer 200 can play a role in determining one or more local driving goals (or target goals corresponding to target locations) and corresponding discrete decisions 201 for the vehicle 153. Each of the discrete decisions 201 may be a desired sequence of one or more motion commands. Examples of motion commands may include, but are not limited to, turning right, staying in a lane, turning left, changing lanes, or coming to a complete stop at a particular location. In this regard, several sensing and mapping modules may use information from one or more sensors 165, such as radar, LiDAR, inertial measurement unit (IMU), camera, and / or Global Positioning System (GPS) information, along with prior map information, to estimate a state for a particular driving scenario from the surrounding portion of the environment related to system 120 and vehicle 153 and the current state of system 120. The sensing and mapping modules may make one, more, or all of the layers of the route and motion planning system 172 available.

[0097] Based on one or more driving goals and corresponding discrete decisions 201, the path and motion planning layer 210 plays a role in determining a reference trajectory 105 provided to the vehicle controller layer 220. In some embodiments of this disclosure, the reference trajectory 105 is a safe, desirable, and mechanically feasible trajectory that the vehicle 153 should follow. The reference trajectory 105 may be determined based on the output from the sensing and mapping module and the system 120. In some embodiments, a key requirement of the reference trajectory 105 calculated by the path and motion planning layer 210 is the recognition that the reference trajectory 105 is collision-free, mechanically feasible, and trackable by the predictive controller 110 in the vehicle controller layer 220. This means that the reference trajectory 105 achieves or reaches one or more target goals while avoiding any collisions with objects in the environment and taking into account a dynamic system model 107 of the controlled system 120 that can be expressed in a set of mathematical equations.

[0098] Some embodiments of this disclosure are based on the recognition that a typical limiting factor in path and motion planning tasks is the non-convexity of constrained dynamic optimization problems. This can result in achieving only local optima, which may be extremely far from the global optimum. In some cases, solving an optimization problem can incur significant computational load and time just to find feasible solutions. According to some embodiments, path and motion planning may be performed using sampling-based methods such as rapidly exploring random trees (RRTs), or graph search methods such as A*, D*, and other variations.

[0099] As shown in Figure 2, the vehicle controller layer 220 aims to realize the reference trajectory 105 by calculating control signals or control inputs 111 for operating the system 120, taking into account the dynamic system model 107 and constraints 109. The control inputs 111 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 220 provides the control inputs 111 to an additional layer consisting of one or more controllers in the actuator controller layer 230. The actuator controller layer 230 directly adjusts the actuators to achieve the desired movement of the vehicle 153.

[0100] Different embodiments of the present disclosure may use different techniques in the vehicle controller layer 220 to track the reference track 105 calculated by the route and motion planning layer 210. In some embodiments of the present disclosure, a model predictive controller (MPC) is used in the vehicle controller layer 220 so as to be able to effectively use future information in the long-term reference track 105 calculated by the route and motion planning layer 210 to achieve a desired movement or action of the vehicle 153.

[0101] In some embodiments of the present disclosure, a linear model predictive controller (LMPC) may be used in the vehicle controller layer 220. The LMPC may be the result of a linear dynamic system model 107 used in combination with linear constraints 109 and a quadratic objective function to track a reference trajectory 105 calculated by the path and motion planning layer 210. In other embodiments of the present disclosure, one or more of the constraints 109 and / or objective functions may be nonlinear, and / or the equation of the dynamic system model 107 describing the vehicle state behavior may be nonlinear, resulting in a nonlinear model predictive controller (NMPC) that tracks the reference trajectory 105 calculated by the path and motion planning layer 210.

[0102] Some embodiments of the present disclosure are based on the recognition that the path and motion planning layer 210 can compute relatively long-term predictable motion plans, but typically need to be run at a relatively slow sampling frequency. In some embodiments of the present disclosure, the path and motion planning layer 210 is run once for each desired sequence of one or more motion commands. For example, the path and motion planning layer 210 may be run for an automatic hitch operation as shown in Figure 1B. In some embodiments of the present invention, the path and motion planning layer 210 may be run multiple times for each desired sequence of one or more motion commands to perform an automatic hitch operation, for example, to allow for replanning when the surrounding environment of the vehicle 153 changes substantially.

[0103] Some embodiments of this disclosure are based on the understanding that the predictive vehicle controller layer 220 can track the reference track 105 by calculating the control input 111 over a relatively short prediction range while running at a relatively high sampling frequency. For example, the vehicle controller layer 220 can use a prediction horizon of 1 to 10 seconds while running at 10 to 100 times per second. The vehicle controller layer 220 may be highly responsive to local deviations resulting from uncertainties related to obstacles in the vehicle's surrounding environment in vehicle state estimation, and other uncertainties in the sensing and mapping modules.

[0104] In some embodiments of this disclosure, different dynamic vehicle models may be used in different components within the multilayer route and motion planning system 172 to control the movement of an autonomous or semi-autonomous vehicle. For example, a relatively simple but computationally inexpensive kinematic model may be used in the route and motion planning layer 210, while a relatively accurate but computationally expensive dynamic single-track or double-track vehicle model may be used in the vehicle controller layer 220.

[0105] As shown in Figure 2, information may be shared between different components within the multilayer route and motion planning system 172 for autonomous or semi-autonomous control of the vehicle 153. For example, map and vehicle surroundings information 205 may be shared between the decision layer 200 and the route and motion planning layer 210. Also, map and vehicle surroundings information 215 may be shared between the route and motion planning layer 210 and the vehicle controller layer 220. Information 225 may be shared between the vehicle controller layer 220 and the actuator controller layer 230. In addition, some embodiments of this disclosure are based on the understanding that reliability and safety in the control of the vehicle 153 can be improved by using diagnostic information, such as success and / or failure performance metrics of an algorithm in one component that can be shared with the components of the multilayer route and motion planning system 172.

[0106] In one embodiment, a two-stage approach may be used in the path and motion planning layer 210 to construct two trees, each starting from an initial position (or source position) and a target position. The trees may consist of sets of nodes and sets of edges that represent states. Hereinafter, any node or edge of the tree may contain position and direction information of the vehicle 153. The sets of nodes and edges may represent kinematically or mechanically feasible transitions between states without collisions. In addition, the sets of nodes and edges may represent all possible states in which the vehicle 153 does not overlap with any obstacles in the environment.

[0107] The motion planner 108 is configured to generate a path, and a reference track 105 is generated based on this path. The method by which the motion planner 108 calculates the path to control the movement of the vehicle 153 in order to perform a task is described in detail below.

[0108] Figure 3A shows an example 300 of a method for generating a first path to control the motion of a vehicle 153, according to some embodiments of the present disclosure. In one example, the motion of the vehicle 153 may correspond to a hitch operation from an initial state to a target state. For example, with respect to the environment disclosed in Figure 1B, the initial state 157 may correspond to the starting attitude of the vehicle 153, and the target state 158 corresponds to the position of the vehicle 153 corresponding to the hitch operation.

[0109] It should be noted that the vehicle 153 is equipped with the ability to interact with the environment and receive sensor data corresponding to the state of the vehicle and the surrounding environment. The vehicle 153 is configured with a route and motion planning system 172 for controlling the movement of the vehicle 153 in an automated manner. For example, the route and motion planning system 172 may include one or more components for determining a first route for the vehicle 153 to perform a particular action and further controlling the movement of the vehicle 153. According to embodiments of this disclosure, the action of the vehicle 153 may correspond to a hitch operation. For example, the route and motion planning system 172 may provide a set of motion commands for controlling the motion of the vehicle 153 based on the first route. In one example, the route and motion planning system 172 may utilize the BIAGT algorithm to generate a route for the vehicle 153 to perform a hitch operation. Method 300 describes the use of the BIAGT algorithm to generate a route for performing a hitch operation in an effective and efficient manner.

[0110] In one example, the motion planner 108 of the route and motion planning system 172 may perform the steps of method 300 to generate the route and reference trajectory 105.

[0111] The Bidirectional A-Search Guide Tree (BIAGT) path and motion planning algorithm will be understood to be a variation of the A*-based algorithm. The BIAGT algorithm may be capable of efficiently computing a kinematically feasible solution for an automated vehicle parking task. According to embodiments of this disclosure, the described hitch operation can be considered a special parking task in which the requirements for the final stage of the operation are very strict in terms of lateral position errors and directional angle errors.

[0112] The BIAGT algorithm improves upon the hybrid A* algorithm in two ways. Given a set of vehicle attitude configuration spaces P, including position and direction of travel, the BIAGT algorithm prioritizes control actions at each node to balance the optimality of the calculated path with computational efficiency. In addition, the BIAGT algorithm simultaneously extends two trees or subgraphs: a first tree (also called the initial tree) leading from the initial node to the goal node, and a second tree (also called the goal tree) leading from the hitch point on trailer 154 corresponding to the goal node to the initial node. In particular, each of the first and second trees estimates the cost-to-go of its own nodes by leveraging the arrival costs of the other tree or other subgraphs. Once a feasible path is found, BIAGT may perform a motion planning step to determine the velocity profile along the feasible path and output a motion plan.

[0113] In some examples, a tree of a graph may be defined as the union of a set of nodes and a set of edges. In this case, the tree is T=(υ,ε) and υ⊂P free , E(X i ,X j )∈ε. For this reason, E(X i ,X j ) is the state value X i and X j This represents a feasible and collision-free trajectory between P. free This is implicitly obtained by examining collisions with stationary obstacles such as obstacles 158a and 158b in the environment. M represents a finite set of motion primitives pre-calculated from available control actions, and V max We can assume that this represents the maximum number of nodes allowed in the graph tree.

[0114] According to some embodiments of the present disclosure, modifications are made to use the BIAGT algorithm to generate a first path in order to improve the performance of the BIAGT algorithm for path and motion planning in automatic hitch operation, and to improve the performance of the predictive vehicle controller 110 for tracking the resulting reference trajectory.

[0115] Continuing this example, at 302, a first tree is constructed starting from the initial node corresponding to the initial state 157. The first tree may have a first set of nodes starting from the initial state 157. In one example, the BIAGT algorithm constructs a first tree (also called the start tree) T starting from X0. S To construct.

[0116] In step 304, a second tree is constructed, starting from the target node corresponding to goal state 156. The second tree may have a second set of nodes starting from goal state 156. In one example, the second tree (also called the goal tree) T g is X f It starts with this.

[0117] In 306, a graph forming a first path is constructed. According to some embodiments, the graph of the first path may aim to minimize the path cost without restricting the motion primitives for traveling from the initial state 157 to the target state 156. In this regard, the path and motion planning system 172 may utilize the motion planner 108 corresponding to the path and motion planning layer 210 to generate the first path.

[0118] Figure 3B shows a graph structure 310 as an example for forming a first path according to some embodiments. According to some embodiments, the graph structure 310 represents state X of vehicle 153 i and X jThe graph may have multiple nodes (shown as nodes 312a to 312i) that define states such as the vehicle 153. The multiple nodes 312a to 312i of graph 310 include an initial node 312a that defines the initial state X0157 of the vehicle 153, and a target state X0157 of the vehicle 153. f This may include a target node 312i that defines 156.

[0119] Target state X f 156 may correspond to a hitch point. 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 the vehicle 153 between the corresponding states of the connected nodes. In addition, each node 312a-312i may contain several motion cusps, representing the number of motion cusps in the path from either 312a or 312i to a particular node. For example, a first path through graph 310 may be generated using multiple nodes 312a-312i connected through corresponding edges 314a-314h, connecting the initial node 312a to the target node 312i by a set of motion primitives for moving the vehicle 153 from an initial state 157 to a target state or goal or target state 156.

[0120] In one example, graph 310 may be created by repeatedly extending one of the multiple nodes 312a to 312i to create a new node, or by connecting two existing nodes from the multiple nodes 312a to 312i. The new node and the extended node or the two existing nodes may be connected to corresponding edges that represent collision-free motion primitives for moving the vehicle 153.

[0121] In particular, to create graph 310, the motion planner 108 may employ the BIAGT algorithm. The BIAGT algorithm can connect two nodes corresponding to the states of the vehicle 153. For example, two states of the vehicle can be X i and X jIt can be expressed as follows. Next, the two states X i and X j These can be connected by edges that represent collisionless motion primitives. For example, two states X i and X j These can be connected using a collision-free connection path having one of 48 Reeds-Shepp (RS) patterns. In one example, the connection path may be selected from the 48 RS patterns without any constraint on the number of moving cusps in the connection path. The 48 Reeds-Shepp (RS) patterns are described in detail in relation to Figure 5B.

[0122] The BIAGT algorithm may simultaneously extend each of the first tree 320 and the second tree 330, i.e., the state in each of the first tree 320 and the second tree 330. In one example, the extension of the first tree 320 and the second tree 330 may be based on a cost function F(·) which is the sum of the heuristic value h(·) and the arrival cost g(·). In one example, the heuristics in the BIAGT algorithm are calculated based on the Reeds-Shepp (RS) path lengths to the corresponding goals of the first tree 320 and the second tree 330, while ignoring obstacles 158a and 158b. If either the first tree 320 or the second tree 330 approaches the corresponding goal, or if the distance between the two trees 320 and 330 is less than or equal to a threshold ε, BIAGT may connect the two trees 320 and 330 with a dynamically feasible edge 316. If the connection is successful, all parents of the connected nodes in one tree are added to the other tree to obtain a feasible first path 318. In one example, the first path 318 may be formed by connecting the first tree 320 to the second tree 330 using a collision-free and dynamically feasible edge 316 having one of 48 RS patterns. For example, the first path 318 may be a composite path formed by nodes 312a, 312c, 312d, 312e, 312f, 312g, and 312i and their corresponding edges 314a, 314c, 314e, 316, 314f, and 314h.

[0123] Therefore, since there are no constraints associated with the number of motion cusps while generating the first path 318, graph 310 may be formed by connecting different states or nodes of trees 320 and 330 using any one of the 48 RS patterns.

[0124] However, the BIAGT algorithm is modified to generate a second path based on the first path 318. In this regard, the second path is generated based on a hard constraint associated with the number of motion cusps allowed in the connecting path between the two states. In such a case, the connecting path of motion primitives between the two states for generating the second path may have the minimum number of motion cusps. According to some embodiments, two states X i and X j The motion primitive between and may represent the shortest distance path in one of the eight cuspless RS patterns out of 48 RS patterns. Such embodiments associated with the generation of a second path will be described in detail below in conjunction with Figures 4A, 4B, 4C, and 4D. In some embodiments, state X i The motion primitive to be applied in X must not introduce additional cusps, i.e., X i The velocity of the kinetic primitive to be applied in this case is X from its parent. i It must be the same as the velocity of the motion primitive that reaches it.

[0125] If the generation of the first path 318 is successful, the path and motion planning system 172, specifically the motion planner 108, may generate the first path 318 without restricting the motion primitives corresponding to the movement of the vehicle 153. The cost associated with the first path 318 can be minimized. However, if the total number of nodes 312a to 312i on both trees 320 and 330, i.e., the forward direction tree 320 and the reverse direction tree 330, is V max If the condition is not met and a solution trajectory, i.e., the first path, has not been found, the path planning fails. For example, if the path planning fails, the generation of the first path may be restarted, or control of the vehicle 153 may be transferred for manual operation.

[0126] Based on the generated first path 318, the modified BIAGT algorithm employed by the motion planner 108 may improve the performance of the predictive controller 110 in order to track the vehicle 153's reference trajectory 105 and enable the successful hitch operation. The manner in which the motion planner 108 uses the modified BIAGT algorithm to control the movement of the vehicle 153 and efficiently perform the hitch operation is described in detail below.

[0127] Figure 4A shows a method 400 as an example for controlling the motion of a vehicle 153 based on a second path, according to some embodiments of the present disclosure. For example, the motion of the vehicle 153 may correspond to a hitch operation from an initial state 157 to a target state 156. In one example, the steps of method 400 may be performed after the generation of the first path 318, as described in Figure 3B.

[0128] According to several embodiments, paths and motion plans for automatic hitch operations are described herein. In this regard, the BIAGT algorithm is modified to improve the performance of the predictive controller 110 for tracking the resulting reference trajectory 105. The modified BIAGT algorithm is configured to continue expanding the graph 310 to generate a second path even after the generation of the first path 318 if the first path does not meet the criteria for path quality, for example, if the number of motion cusps of the first path exceeds a threshold. Note that the second path is also constructed by simultaneously expanding two trees, namely the first tree 320 from the initial node 312a and the second tree 330 from the goal or target node 312i. The method for constructing the second path is described in this method 400.

[0129] In 402, a first number of motion cusps in the series of motion primitives of the first path 318 can be determined. In particular, each of the first number of motion cusps indicates a switch between forward motion and backward motion in the series of motion primitives. As used herein, a motion cusp switches forward motion to backward motion or backward motion to forward motion in the series of motion primitives that define the first path 318.

[0130] Conventional motion planners typically attempt to find the shortest path, resulting in paths like the first path 318 that have unnecessary motion cusps. This is primarily due to the fact that the cost-to-go used to guide the tree structure does not take into account the number of motion cusps. Such motion cusps can impair the positioning accuracy of the integrated path and motion planning system 172. Motion cusps require the vehicle 153's velocity to be zero or near zero, and such motion is problematic for the accuracy of the path and motion planning system 172 because the vehicle 153's inertia governs its dynamic behavior. Thus, each motion cusp in the vehicle 153's path amplifies the effect of uncertainty, which accumulates with the number of motion cusps, leading to accuracy problems.

[0131] To overcome the problems associated with the number of motion cusps, if 404 determines that the first number of motion cusps in the first path 318 exceeds a threshold, the motion planner 108 is configured to continue expanding the graph 310 of the first path 318 while taking the number of motion cusps into account. In particular, if the first number of motion cusps is compared with a threshold and it is determined that the first number of motion cusps exceeds the threshold, the motion planner 108 may continue expanding the graph 310 to add more new nodes. The threshold is defined as the maximum number of motion cusps allowed in a path. For example, if the first number of motion cusps in the first path is less than the threshold, the vehicle 153 may be able to follow the reference track 105 based on the first path 318 with greater accuracy. However, if the first number of motion cusps in the first path is greater than the threshold, a large amount of uncertainty is added to each motion cusp, which may impair the accuracy of the path planning and prevent the vehicle 153 from traveling along the reference track 105 based on the first path 318.

[0132] For example, the threshold associated with the number of cusps may be user-selectable or dynamically determined based on the first path 318, for example, based on the length of the first path 318.

[0133] To continue, if the first number of motion cusps in the first path 318 is less than a threshold, the motion planner 108 may output a first path in 406 to control the motion of the vehicle 153 according to the first path 318. In this regard, the reference track 105 may be generated based on the first path 318.

[0134] Further, at 408, the motion planner 108 further extends graph 310 to generate a second path. In particular, it extends graph 310 to add new nodes to graph 310 until the termination condition is met. Such extensions of graph 310 are subject to hard constraints associated with the total number of motion cusps in graph 310 or the path. Graph 310 is further extended to form a second path connecting the initial node 157 or 312a to the target node 156 or 312i. The second path may have a second number of motion cusps, which is fewer than the first number of motion cusps in the first path 318.

[0135] According to one example, the motion planner 108 may extend graph 310 until the termination condition is met. The route and motion planning system 172 may then control the movement of vehicle 153 based on the second route if a second route is formed before the termination condition is met. However, if a second route is not generated before the termination condition is met, this method may return to 406 and output the first route 318 for controlling the movement of vehicle 153.

[0136] In 410, the route and motion planning system 172 is configured to control the movement of the vehicle 153 based on a second route. In particular, if the second route is generated before the termination condition is met and the number of motion cusps in the second route is less than that of the first route 318, the motion planner 105 may generate a reference trajectory 105 based on the second route. The route and motion planning system 172 may then use the second route to control the movement of the vehicle 153, for example, during a hitch operation. In one example, the termination condition may be time-based. For example, the termination condition may indicate the end of a predetermined period or time limit for generating the second route. In one example, the period or time limit of this termination condition may be in the range of 10 to 30 seconds. For example, the termination condition is met when a period of 30 seconds has expired. In another example, the period of the termination condition may be in the range of 30 to 60 seconds.

[0137] According to some embodiments, the generation of a second path may be initialized based on the search for new nodes, and node selection is based on penalizing changes in the velocity direction when calculating the arrival cost associated with the nodes. However, in order to balance completeness, computational efficiency, and path quality, such a method of imposing penalties may substantially increase the time required to generate the second path. Therefore, according to this embodiment, the second path is generated based on the first path 318. The second path is constructed using a modified BIAGT algorithm.

[0138] Figure 4B shows a graph structure 420 as an example for forming a second path according to several embodiments. The second path may be generated for precise control of the movement of the vehicle 153. In one example, the movement of the vehicle 153 may correspond to a hitch operation from an initial state 157 to a target state 156. However, while embodiments of this disclosure describe controlling the movement of the vehicle 153 during a hitch operation, this should not be construed as limiting. In other embodiments of this disclosure, the path and motion planning system 172 may use a second path having a reduced number of motion cusps during other types of maneuvers such as docking and parking.

[0139] To construct a second path, the motor planner 108 may utilize the modified BIAGT algorithm. The motor planner 108 may construct graph 420 using the modified BIAGT algorithm.

[0140] The graph consists of two trees, shown as the first tree or start tree 422 and the second tree or goal tree 424. The start tree 422 begins from the initial state X0 or 157. The first tree 422 is constructed to start from the initial node 426a corresponding to the initial state 157. In one example, the motion planner 108 uses the modified BIAGT algorithm to construct the start tree 422T S To construct.

[0141] For example, the construction of the start tree 422 begins with adding the initial state 157 as the root node or initial node 426a. The root node or initial node 426a of the start tree 422 may be pushed into a priority queue, and the target state 156 may be added as the goal node of the start tree 422. The construction of the start tree 422 involves selecting the best node, such as node 426b, which has the lowest cost to move toward the goal node, i.e., the target or goal state 156. The construction of the start tree 422 further involves extending the best node by applying motion primitives, which grows the start tree 422 by adding collisionless child nodes and collisionless edges, each of which collisionless edges connects the best node 426b to one or more corresponding child nodes. Once all possible motion primitives of a particular type that give collisionless child nodes in the start tree have been applied to the node, this node is removed from the priority queue.

[0142] Subsequently, the root node 426c of the second tree 330, located at a distance from the target state 156, is initialized. In other words, strictly speaking, the target state X f Alternatively, instead of starting the second tree 330 or goal tree from the hitch point corresponding to 156, the root node 426c of the second tree 330 is used for the target state X in the trailer. f It is positioned in the preferred hitch direction, at a user-defined distance from the hitch point. In one example, the position where the root node 426c of the second tree 330 is placed, and the target state 156 or X fThe distance between the corresponding node may be 1 meter, 5 meters, 10 meters, 20 meters, etc. In particular, the hitch point or target state 156 may be connected in a straight line to the root node 426c of the second tree 330. In other words, the root node 426c is connected to the node corresponding to the target state 156 by an edge that defines linear motion. This ensures that the BIAGT algorithm calculates a motion plan or second path that ends in a straight segment before the vehicle 153 reaches the vicinity of the hitch point. This prevents any impediment to the accuracy of the hitch mechanism and ensures that the strict requirements of the hitch operation are met. In this way, a motion without cleavage points is guaranteed from at least the root node 426c of the second tree 330 to the target state 156, i.e., from the region near the hitch point to the hitch point.

[0143] Once the start tree or first tree 422 and the target tree or second tree 424 are initialized, the search for the second path begins. Some embodiments are based on the recognition that path quality, such as the number of moving cusps, can be an important factor for the accuracy of the hitch operation. Therefore, the BIAGT algorithm is modified to search for and generate new nodes in the second path, specifically the second path, so that the second path is formed with the minimum number of moving cusps.

[0144] As illustrated in Figures 3A and 3B, the first path 318 generated during the first stage is made feasible by running the BIAGT algorithm to discover the first path 318 as quickly as possible. Furthermore, during the second stage, the path quality of the first path 318 is improved by imposing a desired maximum number of moving cusps as a hard constraint during node selection.

[0145] In some embodiments, the lowest cost node is selected from a priority queue. For example, node 426c may be selected as expandable node 426c from at least one of the first set of nodes in the first tree 422 or the second set of nodes in the second tree 424, based on the cost associated with expandable node 426c. During the second stage for generating the second path, the selection of expandable node 426c for the expansion of graph 420 is subject to constraints associated with the total number of moving cusps.

[0146] In one example, expandable nodes in the first tree 422 and / or the second tree 424 are identified based on their corresponding costs. Furthermore, expandable nodes are expanded by applying motion primitives and adding child nodes to them. Such additions of child nodes are based on the determination of whether the child node introduces more motion cusps than a specified maximum number into the corresponding tree 422 or 424 and / or associated path. In particular, for any node in a priority queue, when expanding graph 420, if the segment between this node and the root node of the corresponding tree 422 or 424 contains more motion cusps than the maximum allowed number, such node is removed from the priority queue without further expansion. For example, such additions of new nodes for path quality improvement continue until a termination condition is met or until a second path 428 with the minimum number of motion cusps is found. For example, the termination condition may be set based on a time constraint. When the first tree 422 and / or the second tree 424 are extended based on new nodes and / or previous nodes, the two trees 422 and 424 may be connected using collision-free connection paths having one of eight apex-less RS patterns out of 48 RS patterns to form a second path 428. In particular, the eight RS patterns are apex-less path patterns.

[0147] The expansion of the start tree 422 and / or the target tree 424 may be performed by selecting the node with the lowest associated cost. Furthermore, it may be necessary to determine the child nodes of the selected node. In one example, the child nodes may be determined such that each child node has a lower cost compared to the cost of the selected node. Furthermore, this node and its corresponding child nodes may be connected using a certain type of collision-free motion primitive, such as motion primitive 425. Motion primitive 425 may be applied at 426b to give collision-free child nodes such as node 426d.

[0148] In one example, for a start tree 422, the cost of node 426b may be calculated as the sum of the arrival cost and the estimated cost-to-go, where the arrival cost represents the cost of driving vehicle 153 from the initial state 157 or initial node 426a to node 426b, and the estimated cost-to-go represents the estimated cost of driving vehicle 153 from the target state 156 or root node 426c. The arrival cost is known, but the cost-to-go is unknown and must be estimated.

[0149] In one example, graph 420 may be expanded by adding child nodes 426d connected to an expandable node 426b using edges defined by collisionless motion primitives 425. The cost of child node 426d is the minimum cost to reach the corresponding goal node of tree 422 from the initial node 426a (or the corresponding root node) to child node 426d. For example, the cost of child node 426d may include a first cost for the initial path formed by the start tree 422 through a first set of nodes, a second cost for the goal path formed by the goal tree 424 through a second set of nodes, and a third cost for the connecting path between the nodes of the first set and the nodes of the second set.

[0150] The target tree 424 may be extended in a similar manner. When the start tree 422 and the goal tree 424 are close together, that is, when the distance between these trees falls below a predetermined threshold, the goal tree 424 is connected to the start tree 422 to generate a path. The extension of the start tree 422 and the target tree 424 may continue until the distance between the start tree 422 and the goal or target tree 424 falls below a threshold.

[0151] If the distance between tree 422 and goal or target tree 424 is less than a threshold, the two trees 422 and 424 may be connected using different types of collisionless motion primitives 430. The motion primitive 430 is applied at 426b to connect both trees 422 and 424. A second path 428 is then identified. For example, the second path 428 may include nodes such as 426a, 426b, and 426c, and edges such as the edge between nodes 426a and 426b, the edge based on the motion primitive 430, and other edges in trees 422 and 424 that can be collisionless and have fewer motion cusps.

[0152] For example, there may be several connection paths between the start tree 422 and the target tree 424 (shown by dotted lines from nodes 426a, 426b, and 426d). However, the connection paths between the start tree 422 and the target tree 424 may be selected based on the number of moving cusps, feasibility, etc.

[0153] Subsequently, a reference track 105 for controlling the movement of the vehicle 153 is generated based on the second path 428. In particular, if the second path 428 is identified or generated before the termination condition is met, the reference track 105 is generated based on the second path 428. Typically, the second path 428 may also include a specific number of motion cusps, such as a second number of motion cusps, fewer than the first number of motion cusps associated with the first path 318. For this reason, the reference track 105 for hitch operation based on the second path 428 may include a minimum number of motion cusps, i.e., switching between the forward and reverse driving directions.

[0154] In some embodiments, the motion planner 108 may generate a reference trajectory 105 for controlling the movement of the vehicle 153 along either a first path 318 or a second path 428. The reference trajectory 105 may be generated as a function of time. For example, the reference trajectory 105 may indicate a type or pattern of motion and the corresponding amount of time over which the pattern of motion is performed. The reference trajectory 105 may then be provided to a predictive controller 110 that generates control inputs 111. The control inputs may include motion control commands, such as values ​​for steering angle and acceleration, to control the actuators of the vehicle 153 so that the vehicle moves along and follows the reference trajectory 105.

[0155] According to some embodiments, the generated reference trajectory 105 may include pauses at each apex. In one example, the pauses may be 1 second, 2 seconds, 5 seconds, etc. In one example, the pauses at the apex can correspond to the physical time required for gear shifting. In addition, the pauses at the apex can improve tracking performance when a predictive controller 110 is used.

[0156] Figure 4C shows an example of a method for creating graph 420 during the second stage according to several embodiments. A second path 428 may be generated during the second stage based on the first path 318.

[0157] In 432, the priority queue is sorted to integrate hard constraints. In some embodiments, the hard constraint specifies the maximum number of motion cusps in a path from the root node of tree 422 or 424 to a first node. The number of motion cusps for each node is calculated and recorded in the first stage of generating the first path 318, so that the nodes 312a-312i in graph 310 corresponding to the first path 318 in the priority queue are sorted. Such sorting of nodes 312a-312i is based on two keys: the number of motion cusps as the first key and the node cost as the second key. In this regard, one or more nodes 312a-312i with the fewest number of cusps have the highest priority to be selected (as nodes 426 to form the second path 428) compared to nodes with the same number of cusps. For example, a node with the lowest cost and the fewest number of motion cusps is preferred. In this way, the priority queue is sorted by increasing the priority of nodes with the fewest number of cusps and the fewest cost.

[0158] When the priority queue is sorted, at 434, the best node, such as node 426b, is retrieved or selected. At 436, it may be identified that there is no way to improve the quality of the first path 318 if the number of motion cusps in the best node 426b exceeds the threshold for the maximum number of allowable motion cusps. In such a case, the motion planner 108 returns the first path 318 as the best path. Otherwise, at 438, node expansion is performed on the best node 426b. Node expansion may be performed by applying motion primitives to the best node 426b.

[0159] In 440, if there are child nodes and associated edges between the best node 426b and its corresponding child nodes, these are added to the tree 422. Note that in this example, the best node 426b is used to illustrate the extension of the first tree or start tree 422. However, this should not be construed as limiting. In other embodiments of this disclosure, the best node for the extension of the target tree 424 may be identified in the second tree or target tree 424. Furthermore, note that the nodes for generating the second path 428 (nodes 426a, 426b, 426c, and 426d, etc.) may be selected from nodes 312a to 312i, or they may be new nodes added to graph 310 to form graph 420 for the second path 428. In other words, graph 310 associated with the first path 318 may be extended and modified to form graph 420 for identifying and generating the second path 428.

[0160] In 442, if the node expansion of the best node 426b leads to a new path and this new path satisfies the cost objective, this new path is used as the output to generate the reference track 105. According to one example, the cost objective is associated with the total number of cusps in the new path. For example, the number of motion cusps in the new path (called the second number of motion cusps) is calculated. If the second number of motion cusps of the new path is less than the first number of motion cusps of the first path 318, the new path is updated as the first path 318. The updated first path is then used to calculate the reference track 105. In such cases, another best node may be taken out for further processing. However, if the second number of motion cusps of the new path is less than the threshold for the maximum number of motion cusps allowed, the new path is recorded as the second path 428. The second path 428 is then output to generate the reference track 105 and control the motion of the vehicle 153.

[0161] Figure 4D shows an example of a node extension method according to several embodiments. In 450, the best node 426b of the start tree 422 or the target tree 424 is taken. Given the best node 426b, in 452 it is determined whether the best node 426b of the given tree is close to the other tree. If it is close, in 454 an attempt may be made to connect the best node 426b to a specific node on the other tree that is within a certain distance using a specific type of motion primitive, such as motion primitive 430. If it is not close, in 456 the best node 426b may be extended by applying another type of motion primitive, such as motion primitive 425, to add a corresponding child node, indicated as node 426d.

[0162] In step 458, a determination is made to determine whether the node expansion by the motion primitive 430 or 425 in step 454 or 456 resulted in an edge connecting the best node 426b of a given tree to the other tree. If neither motion primitive 430 nor 425 results in a new edge connecting both trees 422 and 424, the node expansion of the best node 426b is terminated. Another node, such as a child node 426d, may then be selected for expansion according to the steps of the method described in Figures 4A, 4B, 4C, and 4D. Otherwise, in step 460, the new edge is recorded and the path is updated using the new edge as a new path. In this regard, if the new path generated using the new edge contains fewer motion cusps than the first number of motion cusps of the first path 318, the new path is updated as the first path. Alternatively, if a new path generated using the new edge contains fewer moving cusps than the threshold for the maximum number of allowed moving cusps, the new path may be recorded as a second path 428.

[0163] Therefore, continuous expansion of graph 310 is performed to reduce path costs. In particular, continuous expansion of graph 310 is performed to satisfy hard constraints associated with node selection in graph 420 by applying restrictive motion primitives. In one example, the motion primitives are restricted based on the presence of motion cusps in the edges associated with the nodes.

[0164] Figure 5A shows examples of types of motion primitives 425 according to several embodiments. For example, motion primitive 425 is applied to a node to give a collision-free node. It will be understood that the motion primitive is a pre-calculated motion that the vehicle 153 can perform. The motion primitive represents a motion that the vehicle 153 can transition smoothly into. For example, the motion primitive may be superimposed on a node in a graph, such as node 426b, to identify its child nodes.

[0165] In one example, motion primitive 425 may be applied to the initial node 426a to identify node 426a, and motion primitive 425a may be used to connect to its child nodes. Subsequently, each node of tree 422 and 424 may be connected to its child nodes using motion primitive 425i. Motion primitive 425i may be uniquely determined by the trajectory of the control input over a finite time interval.

[0166] Therefore, X i Applying motion primitive 425i in this case yields a connection path ending in state 502i. For example, state 502i may correspond to a child node connected from a corresponding parent node. If state 502i and the corresponding connection path 506i are collision-free, state 502i may be a child node of its parent node, such as node 504. In one example, node 504 may have multiple child nodes. In one example, state 502a may be a child node of node 504 if the path between node 504 and state 502a is collision-free, feasible, and has low cost and a small number of motion cusps.

[0167] It will be understood that the construction of a tree such as the start tree 422 or the target tree 424 involves extending a best node such as node 426b by applying two types of motion primitives 425 and 430. The motion primitive 425 grows the tree by adding collision-free child nodes and collision-free edges. Each collision-free edge connects a best node to a child node.

[0168] Following this example, node 504 may also be the best node in the tree, and the motion planner 108 will generate this tree as motion primitive 425a({a1(t),t∈[0,t f1 ]} is determined by)~425i(a i (t), t∈[0,t fi The extension may be performed using (defined by ]}. The motion primitives 425a-425i add collision-free edges 506a-506i. Edges 506a-506i can connect node 504 to the corresponding child nodes 502a-502i. According to some embodiments, child nodes 502a-502i may be added to the path if the number of motion cusps from the root of the corresponding tree to the child node is less than a threshold for the maximum number of motion cusps allowed. In addition, the cost of the child node may be lower than the cost of the parent node 504. Such child nodes may be added to the graph to generate a second path 428. Then, a particular child node may be selected as the next best node for extension.

[0169] In one example, the cost of a child node corresponding to states 502a to 502i may be calculated as the sum of the arrival cost and the estimated cost-to-go, where the arrival cost represents the cost of driving vehicle 153 from the initial state or root node of the tree to the child node, and the estimated cost-to-go represents the estimated cost of driving vehicle 153 from the target state (e.g., the state corresponding to the hitch point) to child node 520.

[0170] Figure 5B shows examples of types of motion primitives 430 according to several embodiments. For example, motion primitive 430 does not take obstacles into consideration, X i and X j The shortest distance path between any two of the following states is determined. In one example, the motion primitive 430 is moved to state X of a tree such as the start tree 422. i Apply this to connect the start tree to another tree such as the target tree 424. In particular, state X i and X j If they are close to each other, apply the motion primitive 430 to the state X of the start tree 422. i X of the target tree j Connect to it.

[0171] Reeds-Shepp revealed that there are up to 48 solutions connecting any two states, and it will be understood that these 48 solutions allow for the patterns shown in Table 510 of Figure 5B. In this regard, "L" corresponds to a left turn, "S" to a straight line, and "R" to a right turn. The superscripts "+" and "-" for "L", "S", and "R" indicate forward and reverse movement, respectively. The subscripts indicate that vehicle 153 must traverse a circle to change its direction of travel angle. Lines represent cusps of motion. Note that cusps of motion can indicate a change in direction of movement, i.e., a change from forward motion to reverse motion, or from reverse motion to forward motion. Two states X i and X j The motion primitive 430 between represents the construction of the shortest distance path by one of the 48 patterns 430a.

[0172] The motion primitive 430 does not have to be predetermined. Instead, only the path pattern or structure is predetermined. Patterns and structures are used interchangeably in this disclosure. During the first stage, the nodes in the priority queue are sorted according to the cost of the node to be selected for expansion. The node with the lowest cost has the highest priority to be selected.

[0173] In another embodiment, the interstate motion primitive 430 represents the construction of the shortest distance path in one of the first eight cuspless patterns 430b.

[0174] Unlike motion primitive 425, motion primitive 430 is not predetermined. Instead, only the pattern or structure of the pathway (first pathway or second pathway) is predetermined. The pattern and structure are used interchangeably in this invention.

[0175] During the first stage of constructing the first path 318, nodes in a priority queue are sorted according to the cost of the nodes to be selected for expansion, where cost may not characterize the number of motion cusps. The lowest cost node has the highest priority to be selected. During the second stage of constructing the second path 428, constraints associated with the number of motion cusps in the node's motion primitive are imposed for node selection.

[0176] Once a trajectory is generated based on the second path 428 having the minimum number of cusps, or the first path 318, the model prediction controller (MPC) 110 is used to accurately track the planned trajectory.

[0177] Figure 6A shows an example block diagram of a model predictive controller (MPC) 110 according to several embodiments. The predictive controller 110 is configured to calculate a control signal 111 given the current state and estimated state 121 of the system 120 and a reference trajectory 105. Specifically, the reference trajectory 105 may be determined as a function of time based on a first path 318 or a second path 428. The reference trajectory 105 for the vehicle 153 to travel may be generated by adding a time dimension to the path 318 or 428. The motion planner 108 may be configured to implement embodiments described in conjunction with Figures 3A, 3B, 4A, 4B, 4C, 4D, 5A, and 5B above in order to generate the first path 318 or a second path 428 and the reference trajectory 105. In certain cases, the motion planner 108 within the control system 100 may generate a second path 428 having the minimum number of motion cusps, and generate a reference trajectory based on the second path 428. The predictive controller 110 of the control system 100 then tracks the motion of the vehicle 153 based on this reference trajectory.

[0178] In one example, the prediction controller 110 may be configured to generate equality and inequality constraints 602 to control the movement of the vehicle 153. For example, the equality and inequality constraints may represent constraints on the corresponding physical movement of the vehicle around it. Furthermore, the prediction controller 110 may be configured to generate an objective function 604 for the MPC prediction controller 110 using a reference trajectory 105 generated by the motion planner 108. In one example, the objective function 604 may represent the start or initial state of the vehicle 153, the goal or target state of the vehicle 153, and the reference trajectory to be followed. For example, during a hitch operation, the objective function 604 may represent the initial state as the current position of the vehicle away from the hitch point, and the target state as the hitch point. For example, optimal control data of the objective function 604 and the equality and inequality constraints 602 may be used to solve an optimization problem. For example, a control signal 111 may be generated based on the solution to the optimization problem. In one example, the type of optimization problem may depend on the dynamic system model 107 of system 120, the system constraints 109, the estimated state 121 of system 120, and the reference trajectory 105.

[0179] According to some embodiments, the prediction controller 110 may be configured to compute a control solution, for example, a solution vector 606 containing a sequence of future optimal control inputs 111 over the prediction time range of the motion planner 108. The prediction controller 110 may be configured to compute 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 and may take the form of an optimal control structured quadratic plan 608.

[0180] In some embodiments, the solution to an inequality-constrained optimization problem, i.e., an optimal control-structured quadratic program (QP) 608, uses states and control values ​​610 across a predicted time horizon from previous control time steps that can be read from memory. In this way, techniques for warm-start or hot-start the optimization problem may be realized. This can significantly reduce the amount of computational effort required of the predictive controller 110 in some embodiments. 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 states and control values ​​612 for the next control time step.

[0181] According to some embodiments, given the estimated state 121 of the system 120 and the reference track 105, the predictive controller 110 may calculate control inputs 111 for controlling and tracking the movement of the vehicle 153 by solving the optimal control structured quadratic plan QP608.

[0182]

number

[0183] If k=0,...,N, then the optimization variables in the optimal control structure QP608 are the state variables x k and control input variable u k It consists of. According to some embodiments of this disclosure, if k=0,...,N, the dimensions of the state and control variables 610 are, at each discrete time point t k They do not need to be equal to each other. At each sampling time for the prediction controller 110, the optimal control structure QP 608 is formulated using the QP matrix 614 and the QP vector 616. Then, the optimal control structure QP 608 is solved to compute the solution vector 606. The solution vector 606 may be used to generate updated states and control values ​​612 that can be used in the next iteration. Furthermore, a new control input 111 is generated based on the solution of the optimal control structure QP 608.

[0184] The objective function in the constrained optimal control structured QP solved by the prediction controller 110 includes one or more least-squares criterion tracking terms 618 that penalize the difference between the sequence of predicted states and / or output values ​​620 and the sequence of reference states and / or output values ​​for the reference trajectory 105 calculated by the motion planner 108.

[0185]

number

[0186] For example, the output function may include, but is not limited to, the longitudinal or lateral velocity and / or acceleration of the vehicle 153, slip ratio or slip angle, azimuth angle or angular velocity, wheel speed, force, and torque.

[0187] In various embodiments, the penalty between a reference value corresponding to a reference trajectory 105 determined by the motion planner 108 and a value determined by the prediction controller 110 is weighted by a weighting matrix that assigns different weights to different state variables of the target state. In addition to or instead of this, some embodiments add additional reference tracking terms that are considered by the prediction controller 110. Such additional reference tracking terms may relate to, for example, driving comfort, speed limits, energy consumption, pollution, etc. These embodiments balance the cost of reference tracking with the additional reference tracking terms.

[0188] According to one example, the additional criterion tracking terms in the MPC may be determined based on a cost function in the form of a linear quadratic stage cost 622 and / or a linear quadratic terminal cost term or matrix 624. These additional linear quadratic criterion tracking terms, including the stage cost 622 and terminal cost matrix 624, may include linear and / or quadratic penalties on one or more states and / or one or more combinations of control input variables. For example, the objective function in constrained QP 608 may include linear or quadratic penalties on 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. The linear quadratic objective terms in the stage cost 622 and terminal cost matrix 624 are matrix Q in the QP matrix 614. k S k and R k And the gradient value q in QP vector 616 k , r k It is determined by [the relevant party].

[0189]

number

[0190]

number

[0191]

number

[0192] Inequality constraints may include, for example, constraints on the longitudinal or lateral velocity, acceleration, position and / or orientation of the vehicle 153 relative to its surroundings, slip ratio or slip angle, azimuth angle, angular velocity, wheel speed, force, and / or torque of the vehicle 153. For example, obstacle avoidance constraints may be implemented in the predictive controller 110 by defining a set of one or more inequality constraints on linear functions of the predicted position, velocity, and orientation of the vehicle 153 with respect to the predicted position, velocity, and orientation of one or more obstacles 158a, 158b in the environment surrounding the vehicle 153.

[0193] In this way, 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 when formulating and solving QP 608. By solving QP 608, the predictive controller 110 can generate values ​​for the control input 111. For example, the values ​​for the control input may include, for example, the longitudinal or lateral velocity of the vehicle 153, the longitudinal or lateral acceleration of the vehicle 153, the slip ratio or slip angle of the vehicle 153, the azimuth angle, angular velocity, wheel speed, force, and torque values. Based on the values ​​of the control input 111 corresponding to different functions, the vehicle 153 may be controlled to perform a hitch operation. For example, the control input 111 may be provided to the actuators of the vehicle 153, and the movement of the vehicle may be controlled based on a reference track 105 corresponding to a first path 318 or a second path 428.

[0194] The predictive controller 110 may provide a control input 111 to the system 120 for generating an estimated state of the vehicle 153 in order to track the vehicle 153. The estimated state 121 of the system 120 provides state feedback to the predictive controller 110. For example, the predictive controller 110 tracks the movement of the vehicle 153 to determine whether the vehicle 153 is traveling along the reference track 105 and / or whether the actual state of the vehicle 153 is deviating from the reference track 105.

[0195] Some embodiments of this disclosure relate the Hessian matrix H in 622. k Terminal cost matrix Q N 624 and weighted matrix W k This is based on the understanding that the optimal control structure QP608 is convex if 618 is positive definite or semi-positive definite. Embodiments of this disclosure can use an iterative optimization algorithm to solve the optimal control structure QP608 and discover a solution vector 606 that is feasible with respect to constraints, globally optimal, feasible but suboptimal, or the algorithm can discover a low-precision approximate control solution that is neither feasible nor optimal. The optimization algorithm may be implemented as part of the predictive controller 110, either in hardware or as a software program executed on a processor.

[0196] Examples of iterative optimization algorithms for solving QP608 may include, but are not limited to, primal or dual gradient-based methods, projection or proximal gradient methods, forward-backward separation methods, alternating direction multiplier methods, primal, dual, or primal-dual effective constraint methods, primal or primal-dual interior point methods, or variations of such optimization algorithms. In some embodiments of this disclosure, a block-sparse optimal control structure in the QP matrix 614 may be used in one or more of the linear algebraic operations of the optimization algorithm to reduce computational complexity and thereby reduce the execution time and memory footprint of the QP optimization algorithm.

[0197] Other embodiments of this disclosure can solve the non-convex optimal control structure QP608 using optimization algorithms for nonlinear programming, such as sequential quadratic programming (SQP) or the interior point method (IPM). Subsequently, a suboptimal, locally optimal, or globally optimal control solution can be found for the inequality-constrained optimization problem of the MPC controller 110 at each sampling time.

[0198]

number

[0199] If k=0,...,N, then the optimization variables in the optimal control structured NLP640 are the state variables x k and control input variable u k It consists of the following. In some embodiments, if k=0,...,N, the dimensions of the state and control variables 610 are as follows: k The values ​​do not have to be equal to each other. At each sampling time of the prediction controller 110, the optimal control structured NLP 640 is formulated using the criterion and weighting matrix in the criterion tracking cost 642, as well as the NLP objective and constraint functions 644. The optimal control structured NLP 640 is solved to compute the solution vector 606. The solution vector 606 may be used to generate updated state and control values ​​612 that can be used in subsequent iterations. Furthermore, a new control input 111 is generated based on solving the optimal control structured NLP 640.

[0200]

number

[0201] The objective function in the constrained optimal control structured NLP 640, which can be solved by the prediction controller 110, includes one or more linear and / or nonlinear least-squares criterion tracking terms 646. The criterion tracking terms 646 may impose a penalty on the difference between the sequence of predicted states and / or output values ​​and the sequence of reference states and / or output values ​​of the reference trajectory 105 calculated by the motion planner 108 in the path and motion planning layer 210.

[0202] In some embodiments of this disclosure, if k=0,...,N, then the weight matrix W k The sequence of is used for the least squares criterion tracking term 646. Therefore, each weight matrix W kThis may be adapted in the control cost function 642 based on the reference trajectory 105. If k=0,...,N, the output value y used in the reference tracking term 646 is k (x k ,u k ) may be defined as any linear or nonlinear function of the state and / or control input variables.

[0203]

number

[0204] Embodiments of the present disclosure may define additional tracking terms for the MPC cost function 642 in the form of stage cost and / or terminal 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 for 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 for longitudinal or lateral vehicle velocity and / or acceleration, slip ratio or slip angle, azimuth angle or angular velocity, wheel speed, force, torque, or any combination of such quantities.

[0205]

number

[0206] Some embodiments of this disclosure are based on the understanding that a discrete-time dynamic model 650 for predicting the behavior of a vehicle 153 can be obtained by performing a time discretization of a set of continuous-time differential equations 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 approximately simulating a set of continuous-time differential equations 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.

[0207]

number

[0208] Inequality constraints may include, for example, constraints on the longitudinal or lateral velocity, acceleration, position and / or orientation of the vehicle 153 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 the nonlinear predictive controller 110 by defining a set of one or more inequality constraints on linear or nonlinear functions of the predicted position, velocity, and orientation of the vehicle 153 relative to the predicted position, velocity, and orientation of one or more obstacles 158a and 158b in the environment surrounding the vehicle 153.

[0209]

number

[0210] Some embodiments of this disclosure are based on a tuned optimization algorithm for efficiently solving a constrained optimal control structured NLP 640 at each sampling time point of a nonlinear predictive controller 110. Such an optimization algorithm may find a solution vector 606 that is feasible and globally optimal with respect to the constraints, feasible but locally optimal, or feasible but suboptimal, or an iterative optimization algorithm may find a low-precision approximate control solution that is neither feasible nor locally optimal. Examples of NLP optimization algorithms include, but are not limited to, variations of the interior-point method and variations of the successive quadratic programming (SQP) method.

[0211] In particular, some embodiments of this disclosure require the use of a real-time iteration (RTI) algorithm, which is an online variation of successive quadratic programming (SQP), in combination with a quasi-Newtonian or generalized Gauss-Newtonian type semi-definite Hessian approximation, and that at least one convex block sparse QP approximation be solved at each sampling time of the nonlinear prediction controller 110.

[0212] Each iteration of RTI consists of two steps. In the first step, corresponding to the preparation phase, the system dynamics are discretized and linearized, the remaining constraint functions are linearized, and the quadratic objective approximation is evaluated to construct the optimally controlled 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, and the next control input 111 is obtained to provide feedback to the system 120.

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

[0214]

number

[0215] The vehicle speed v and the front-wheel steering angle δ are typically control commands transmitted to a low-level wire control system such as an actuator of a component of the vehicle 153. However, according to Equation (2), for the formulation of the optimal control structured NLP640 problem, the acceleration α and the steering angle change rate δ are introduced into the objective function 604 and the constraint 602. rate becomes possible.

[0216]

Number

[0217]

Number

[0218]

Number

[0219]

Number

[0220]

Number

[0221]

Number

[0222] For example, the behavior of the vehicle 153 may be predicted by applying the explicit fourth-order Runge-Kutta method to perform time discretization. It should be noted that the use of the Runge-Kutta method is only illustrative, and in other embodiments, other integration methods may be used to achieve the desired accuracy in predicting the behavior.

[0223] [Number]

[0224] [Number]

[0225] [Number]

[0226] [Number]

[0227] For example, the non - linear optimal control structure NLP640 can be solved within a sampling time of T S = 50 ms using an adjusted implementation of sequential quadratic programming (SQP), known as the real - time iteration (RTI) scheme, together with a Gauss - Newton type Hessian approximation. The RTI algorithm performs a single SQP iteration per control time step and uses a continuation - based warm - start of the state and control trajectories (X i from a certain time step t i+1 to the next time step t i , U i ; S i ). Non - linear functions and their first - order derivatives can be efficiently evaluated using C code generation in CasADi. The solver PRESAS may be used to solve the QP, which applies block - structured factorization techniques with low - rank updates to the pre - conditioning of an iterative solver within a primal active - set algorithm with a dedicated warm - start. When combined with the C code generated by CasADi, the PREAS solver yields an efficient and reliable NMPC solver for solving the optimal control structure NLP640 problem suitable for embedded system platforms.

[0228] By solving the optimal control structured NLP640 problem based on the control input 111 generated by the predictive controller 110, the vehicle 153 can be controlled to perform a hitch operation.

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

[0230] For example, Figure 7A shows a graph of the reference track 105 against time 708, which includes forward motion 702 with a positive reference velocity value, followed by a pause at a stationary position with a reference velocity value of zero 706, and then reverse motion 704 with a negative reference velocity value. The pause period 710 is longer than the preview period 712 for the predictive controller 110 to command a gear change. The preview period 712 is longer than the time delay period 714 of the gear shift mechanism in the controlled vehicle 153.

[0231] In particular, the BIAGT motion planning algorithm is modified to introduce a pause at each motion clemency, i.e., at each transition between the forward motion 702 and reverse motion 704 of the vehicle 153, so that the vehicle reaches the hitch point at the end of the maneuver. Pause period 710, Δt pause In this case, before vehicle 153 begins to move in the opposite direction, preview period 712, Δt previewGear shifting is then initiated. The preview period 712 is selected to be greater than the time delay period 714 indicating the average time it takes for the vehicle 153 to switch gears: Δt preview >Δt gear . For example, the forward and backward speed limits may vary according to the current gear, with v≧0 for the forward movement 702 of the vehicle 153 and v≦0 for the reverse movement 704 of the vehicle 153.

[0232] FIG. 7B shows an example of a method for switching the NLP objective function and the constraint function 644 of the non - linear prediction controller 110 according to some embodiments of the present disclosure. In particular, the objective and constraint functions 644 of the prediction controller 110 may be switched according to whether the current reference motion is forward motion 702 or reverse motion 704.

[0233] At 720, using the current reference trajectory 105, it is possible to detect whether most of the reference trajectory 105 over the prediction horizon length of the prediction controller 110 is forward motion 702, reverse motion 704, or stationary at the zero reference speed value 706. In other words, for example, a determination is made to identify whether most of the reference trajectory 105 over the prediction horizon length of the prediction controller 110 is positive, negative, or close to zero.

[0234] According to some embodiments of the present disclosure, at 722, the objective and constraint functions 644 are adjusted according to the forward motion 702 of the vehicle at one or more sampling time steps t i . This can ensure that the speed of the vehicle 153 is always non - negative.

[0235] In some embodiments of the present disclosure, at 724, the objective and constraint functions 644 are adjusted according to the reverse motion 704 of the vehicle at one or more sampling time steps t i . This can ensure that the speed of the vehicle 153 is always non - positive.

[0236] Given the objective and constraint function 644 and the estimated state 121 of the current reference trajectory 105, the prediction controller 110 may be implemented at 726 by solving an optimization problem with constraints for predictive reference tracking to generate the control input 111. For example, the objective and constraint function 644 may be related to an optimization problem of the prediction controller 110, such as the optimal control structured QP608 and the optimal control structured NLP640. For this reason, the presence of the vehicle reverse movement 704 following the vehicle forward movement 702 may correspond to a kinematic singularity. i For example, the objective and constraint function 644 may be related to an optimization problem of the prediction controller 110, such as the optimal control structured QP608 and the optimal control structured NLP640. For this reason, the presence of the vehicle reverse movement 704 following the vehicle forward movement 702 may correspond to a kinematic singularity.

[0237] FIG. 8A shows an example of an automatic hitch operation according to some embodiments of the present disclosure. The hitch operation may include one or more vehicle systems 153. To perform the hitch operation, a modified BIAGT motion planning algorithm implemented by the motion planner 108 and the non-linear prediction controller 110 may be incorporated.

[0238] Note that the BIAGT motion planning algorithm utilized by the motion planner 108 calculates a kinematically feasible trajectory such as the reference trajectory 105. The reference trajectory 105 may be generated based on the first path 318 or the second path 428. The reference trajectory 105 also avoids any collision between the vehicle 153 and any stationary obstacles 158a, 158b in the environment.

[0239] However, the motion planner 108 does not consider any dynamic obstacles that may enter the environment. Regarding dynamic obstacles, specific restrictions are imposed on the path and motion planning system 172 to control the movement of the vehicle 153.

[0240] In this regard, several embodiments are based on the understanding that hitch operations are typically performed in enclosed environments, i.e., environments where there are no pedestrians, cyclists, or passenger vehicles in the target area and dynamic obstacles appear only sporadically. For example, the target area may correspond to the safety areas (indicated as safety sets 802, 804, and 806) surrounding the vehicle's current state configuration 808 and / or the predicted state configurations 810, 812 within the vehicle 153's environment. Note that during the operation, the vehicle 153 may move from the current state configuration 808 to the predicted state configurations 810, 812.

[0241] Furthermore, some embodiments of this disclosure are based on the recognition that if a dynamic obstacle appears within safety sets 802, 804, and 806 while a hitch operation is being performed, the path and motion planning system 172 or predictive controller 110 will stop the movement of the vehicle 153 by applying automatic emergency braking (AEB). The predictive controller 110 then resumes performing the operation after the dynamic obstacle has moved outside the designated safety sets 802, 804, and 806.

[0242] Some embodiments of this disclosure are based on the understanding that a safety tube whose volume changes over time can be constructed around a reference track 105 that the vehicle 153 follows for operation. In addition, the AEB system may be activated whenever a dynamic obstacle enters this time-varying safety tube.

[0243] In some embodiments of the present disclosure, one or more roadside units (RSUs) or infrastructure sensing devices 165 can be used for the precise execution of automatic hitch operations based on accurate detection of the current state and accurate detection of dynamic obstacles in the environment or safety sets 802, 804, 806 of the controlled vehicle 153. For example, one or more RSUs 165 may include one or more sensors, such as distance rangefinders, radar, LiDAR, and / or cameras, as well as sensor fusion technology, to accurately detect the state of the vehicle and the state of dynamic obstacles in the environment of the controlled system. In some embodiments of the present invention, a communication network 160 may be used for real-time communication between the vehicle 153 and one or more RSUs or infrastructure sensing devices 165.

[0244] In one example, the AEB system is activated when a dynamic obstacle is detected using sensors mounted on the vehicle 153 and / or possibly by communication from one or more RSUs 165. Therefore, if a dynamic obstacle is present in any of the combinations of safety sets 802, 804, 806 surrounding the vehicle 153's current state configuration 808 or predictive state configurations 810, 812, the predictive controller 110 is suspended and the AEB system begins to perform braking.

[0245] If a dynamic obstacle moves outside the combination of safety sets 802, 804, and 806, the predictive controller is reinitialized and the automatic hitch operation continues. In some cases, the speed at which vehicle 153 performs the hitch operation is slow enough that the same motion plan, i.e., the reference trajectory, may be reused to complete the operation, and the BIAGT algorithm does not need to be run after such an interruption. However, in some cases, a motion plan or a replanning of a new reference trajectory must be generated at high speed, and the obstacle avoidance constraints in the optimization problem formulation may be changed.

[0246] Figure 8B shows an example of a method for reinitializing the predictive controller 110 according to several embodiments. For example, the motion of the vehicle 153 and the tracking of the predictive controller 110 may be interrupted by a dynamic obstacle. In some cases, it may not be feasible to return to the old reference track, for example, if the dynamic obstacle has not moved for a long time or if the environment of the vehicle 153 has changed due to the dynamic obstacle.

[0247] In 820, information about the current state configuration 808 and the surroundings of the vehicle 153 is tracked. For example, the surroundings of the vehicle may correspond to safety sets 802, 804, and 806. In one example, the predictive controller 110, based on the tracking of the vehicle 153, uses a detection mechanism to track each sampling time step t i Information may be sought to determine whether it is safe to perform operations such as hitch operation. For example, the detection mechanism may include using the RSU 165 or one or more on-board sensors to detect the current state 808 of the vehicle 153 and the surroundings of the vehicle 153.

[0248] In 822, a determination is made to determine whether each of the one or more safety inspections associated with the operation is met, based on the current condition of the vehicle 153 808, the environment, and the current reference track 105.

[0249] Once the safety check is met, at 720, the sampling time step t i The predictive controller 110 may be realized by solving a constrained optimization problem for predictive reference tracking of the vehicle 153 based on the reference track 105 and estimated state 121 in the system. The predictive controller 110 can generate control inputs 111 for controlling the vehicle 153 to perform a hitch operation.

[0250] However, if the safety checks in 822 are not met, the automatic emergency braking (AEB) system is activated. In 824, the AEB system is activated to safely bring the vehicle 153 to a stop. In this regard, the AEB control signal is calculated in 826. The generated AEB control signal is applied to the actuators of the vehicle 153 to bring the vehicle 153 to a stop. If one or more of the safety checks are not met, the AEB system may be activated in 822, for example, if one or more dynamic obstacles are detected inside the safety sets 802, 804, 806 around the current state configuration 808 and / or predicted state configurations 810 and 812 of the vehicle 153.

[0251] Some embodiments of this disclosure are based on the understanding that the AEB system will continue its operation unless one or more of the safety checks in 828 are met.

[0252] Furthermore, in 830, the reinitialization step of the predictive controller 110's state and control trajectory is performed when it becomes safe again to perform the operation. For example, to ensure a relatively smooth transition after the AEB is switched off and the hitch operation is returned, the predictive controller 110 may calculate the closest point in the reference trajectory 105 to the current state configuration 808 of the vehicle 153. The predictive controller 110 can then begin tracking the vehicle 153 in the reference trajectory 105 from the current state configuration 808 of the vehicle 153, and obtain an updated reference trajectory for the predictive controller 110. For example, the reference trajectory 105 is set to the current state configuration 808 of the vehicle 153 with a reference speed value of zero. A new reference state is then time-shifted within the horizon, taking into account that the vehicle goes from a standstill to a reference speed after braking.

[0253] Figure 9 shows a schematic diagram of a system 900 according to one embodiment. The system 900 includes a vehicle 902 including a processor 904 configured to perform route and motion planning 906. The vehicle 902 also includes at least one sensor, such as a pressure sensor, force sensor, distance rangefinder, radar, LIDAR and / or camera. The sensor may be operationally connected to the processor 904 and is configured to detect information indicating the initial and target states of the vehicle 902. The vehicle 902 may be a tractor-trailer type vehicle as described in the above embodiments. Using this information, the processor 904 is configured to perform motion and route planning for the vehicle 902 using one or more techniques as described in the above embodiments.

[0254] The system 900 may include one or a combination of the following: a sensor 910, an inertial measurement unit (IMU) 930, a processor 950, a memory 960, a transceiver 970, and a display / screen 980, which may be operationally coupled to other components via connections 920. The connections 920 may include buses, lines, fibers, links, or a combination thereof.

[0255] The transceiver 970 may include, for example, a transmitter capable of transmitting 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 970 can enable communication with wireless networks based on a variety of technologies, including, but not limited to, femtocells, Wi-Fi® networks or wireless local area networks (WLANs) which 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 and WiMAX®. The system 900 may also include one or more ports for communication over a wired network.

[0256] In some embodiments, the sensor 910 may include an image sensor, such as a CCD or CMOS sensor, a laser, and / or a camera, hereinafter referred to as "sensor 910". For example, sensor 910 can convert an optical image into an electronic or digital image and transmit the acquired image to processor 950. In addition to or instead of this, the image sensor can detect light reflected from a target object in the scene and provide the intensity of the captured light to processor 950.

[0257] For example, sensor 910 may include a color or grayscale camera that provides “color information.” As used herein, “color information” refers to color and / or grayscale information. Generally, as used herein, a color image or color information can be viewed as containing 1 to N channels, where N is some integer depending on the color space used to store the image. For example, an RGB image contains three channels, with one channel each for red, blue, and green information.

[0258] In some embodiments, the processor 950 may also receive input from the IMU 930. In other embodiments, the IMU 930 may include a three-axis accelerometer, a three-axis gyroscope, and / or a magnetometer. The IMU 930 can provide the processor 950 with velocity, orientation, and / or other position-related information. In some embodiments, the IMU 930 may output measured information in synchronization with the acquisition of each image frame by the sensor 910. In some embodiments, a portion of the output of the IMU 930 is used by the processor 950 to fuse sensor measurements and / or to further process the fused measurements.

[0259] System 900 may also include a screen or display 980 that renders images such as color and / or depth images. In some embodiments, the display 980 can be used to display live images, fused images, augmented reality (AR) images, graphical user interfaces (GUIs), motion control commands, and other program outputs captured by the sensor 910. In some embodiments, the display 980 may include and / or house a touchscreen that allows the user to input data via any combination of a virtual keyboard, icons, menus, or other GUIs, user gestures, and / or input devices such as a stylus and other writing instruments. In some embodiments, the display 980 can be implemented using a liquid crystal display (LCD) or a light-emitting diode (LED) display such as an organic LED (OLED) display. In other embodiments, the display 980 may be a wearable display. In some embodiments, the fusion results may be rendered on the display 980 or provided to different applications which may be inside or outside System 900.

[0260] System 900 as an example may be modified in various ways to be consistent with the present disclosure, for example, by adding, combining, or omitting one or more of the functional blocks shown. For example, in some configurations, System 900 does not include the IMU 930 or the transceiver 970. Furthermore, in some exemplary implementations, System 900 includes various other sensors (not shown), such as an ambient light sensor, a microphone, an acoustic sensor, an ultrasonic sensor, a laser rangefinder, and so on. In some embodiments, a part of System 900 takes the form of one or more chipsets or the like.

[0261] The processor 950 can be implemented using a combination of hardware, firmware, and software. The processor 950 may represent one or more circuits configurable to perform at least a portion of computational procedures or processes related to sensor fusion and / or methods for further processing fused measurements. The processor 950 retrieves instructions and / or data from memory 1060. The processor 950 can 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 processing units (DSPDs), programmable logic units (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 a combination thereof.

[0262] Memory 960 may be implemented within and / or outside of the processor 950. As used herein, the term “memory” refers to any kind of long-term, short-term, volatile, non-volatile, or other memory, and is not limited to any particular type of memory or number of memories, or the type of physical medium in which the memory is stored. In some embodiments, memory 960 holds program code that facilitates the automatic parking or motion planning of a tractor-trailer based vehicle 902.

[0263] For example, memory 960 can store sensor measurements such as still images, depth information, video frames, and program results, as well as data provided by the IMU 930 and other sensors. Memory 960 can also store memory for storing the geometric shape of the vehicle 902, a map of the environment in which the vehicle's operations are performed, a kinematic model of the vehicle 902, and a dynamic system model of the vehicle 902. In general, memory 960 can represent any data storage mechanism. Memory 960 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 separately from the processor 950 in Figure 9, it should be understood that all or part of the primary memory may be located inside the processor 950, or in the same location as the processor 950, and / or coupled to the processor 950.

[0264] Secondary memory may include, for example, memory of the same or similar type as primary memory and / or one or more data storage devices or systems, such as flash / USB memory drives, memory card drives, disk drives, optical disc drives, tape drives, solid-state drives, hybrid drives, etc. In some implementations, secondary memory may be operationally capable of accepting non-temporary computer-readable media in a removable media drive (not shown), or otherwise configurable for non-temporary computer-readable media. In some embodiments, the non-temporary computer-readable media forms part of memory 960 and / or processor 950.

[0265] The above embodiments of the present invention can be implemented in any of a number of ways. For example, the embodiments may be implemented using hardware, software, or a combination thereof. If implemented in software, the software code can be executed on any suitable processor or array of processors, whether it resides on a single computer or is distributed across multiple computers. Such a processor may be implemented as an integrated circuit, with one or more processors as components of the integrated circuit. However, the processor may be implemented using any suitable format of circuitry.

[0266] Furthermore, embodiments of the present invention may also be implemented as methods, and examples of such methods are provided. The order of operations performed as part of this method may be determined in any suitable manner. Thus, embodiments may be configured such that operations are performed in an order different from that exemplified, which may include performing some operations simultaneously, although they are shown as a series of operations in the exemplified embodiments.

[0267] In a claim, terms indicating order, such as "first" or "second," that modify an element of a claim do not inherently imply superiority, precedence, or order of one element of the claim over another, or the temporal order in which the actions of the method are performed. Rather, they are simply used as labels to distinguish the elements of a claim, (when no terms indicating order are used) to differentiate one element of a claim having a particular name from another element having the same name.

[0268] While this disclosure is illustrated using examples of preferred embodiments, it should be understood that various other adaptations and modifications are possible within the spirit and scope of the invention.

[0269] Therefore, the purpose of the attached claims is to cover all such variations and modifications that fall within the true spirit and scope of the invention.

Claims

1. A control system for controlling the movement of a vehicle using a motion planner, wherein the control system is At least one processor, The control system comprises a memory in which instructions are stored, and the instructions are transmitted to at least one processor of the control system. A graph is created having a plurality of nodes that define the state of the vehicle, wherein the plurality of nodes in the graph include an initial node that defines the initial state of the vehicle and a target node that defines the target state of the vehicle, and each pair of nodes in the graph is connected to an edge defined by one or a combination of collision-free motion primitives that move the vehicle between the respective states of the connected nodes, each node includes several motion cusps, and the plurality of nodes connected through the corresponding edges form a first path through the graph that connects the initial node to the target node by a set of motion primitives that move the vehicle from the initial state to the target state. The system is made to determine a first number of motion cusps in the series of motion primitives of the first path, where each of the first number of motion cusps indicates a switch between forward motion and backward motion in the series of motion primitives. When it is determined that the first number of the moving cusps exceeds a threshold, the graph is expanded to add new nodes until the termination condition is met, the expansion of the graph is subject to constraints associated with the total number of moving cusps, the graph is expanded to form a second path connecting the initial node to the target node, the second path having a second number of moving cusps that is less than the first number of moving cusps, A control system that controls the movement of the vehicle based on the second path.

2. The instruction further provides to at least one processor of the control system: The extension of the graph is performed until the termination condition is met, The control system according to claim 1, which, if the second path is formed before the termination condition is met, controls the movement of the vehicle based on the second path.

3. The control system according to claim 2, wherein the instruction further causes the at least one processor of the control system to control the movement of the vehicle based on the first path if the second path is not formed until the termination condition is met.

4. The instruction further provides to at least one processor of the control system: The first number of the aforementioned cusps of motion is compared with the threshold value, The control system according to claim 1, which causes the graph to be expanded to add new nodes if the first number of motion cusps in the first path exceeds the threshold.

5. The control system according to claim 1, wherein the second path includes a minimum number of motion cusps for moving the vehicle from the initial state to the target state.

6. The instruction further provides to at least one processor of the control system: As a function of time, generate a trajectory for the movement of the vehicle along at least one of the first or second paths, The control system according to claim 1, which generates control commands for the vehicle to cause the vehicle to follow the track.

7. The instruction further causes the at least one processor of the control system to generate the trajectory for the movement of the vehicle by adding a time dimension to at least one of the first or second trajectories. The control system according to claim 6, wherein the addition of the time dimension includes adding an additional period at one or more apex points of motion of the trajectory.

8. The instruction further provides the at least one processor, Based on the generated trajectory, the system is made to determine the safe area of ​​the vehicle's movement, the safe area comprising one or more sets of safety along the trajectory. To confirm the presence of dynamic obstacles within the aforementioned safety area, The control system according to claim 6, which, upon identifying the dynamic obstacle within the safety area, activates the automatic emergency braking system and performs a braking operation to stop the movement of the vehicle along the trajectory.

9. The control system according to claim 1, wherein the instruction further causes the at least one processor of the control system to create the graph that forms the first path by repeatedly extending one of the plurality of nodes to create a new node, or by connecting two existing nodes of the plurality of nodes.

10. The control system according to claim 1, wherein the control system comprises a model prediction controller, the model prediction controller is associated with at least a constraint function, a quadratic objective function, and a dynamic system model.

11. The instruction further provides the at least one processor, The system generates equality and inequality constraint functions for the aforementioned model prediction controller, and the aforementioned model prediction controller has nonlinear constraints and a nonlinear system model. The system is made to generate an objective function for the aforementioned model prediction controller, the objective function representing the initial state and the target state of the vehicle, The control system according to claim 10, wherein one or more iterations of a sequential quadratic program are used to perform the solution of an optimization problem associated with the model predictive controller based on the equation and inequality constraint functions and the objective function.

12. The control system according to claim 11, wherein the constraint function and the objective function relating to the optimization problem associated with the model prediction controller are adjusted to at least one of forward vehicle motion or reverse vehicle motion.

13. In order to form at least one of the first path or the second path, the instruction further provides to the at least one processor of the control system: Creating a first tree of a first set of nodes starting from the initial node, Creating a second tree of a second set of nodes starting from the aforementioned target node, The control system according to claim 1, which causes the first tree to be connected to the second tree by connecting a node from the first set of nodes to another node from the second set of nodes.

14. The instruction further provides to at least one processor of the control system: The first path is formed by connecting the first tree to the second tree using a collision-free connection path having one of the 48 Reeds-Shepp patterns, The control system according to claim 13, wherein the system causes the first tree to be connected to the second tree by a collision-free connection path having one of eight Reeds-Shepp patterns from the 48 Reeds-Shepp patterns, the eight Reeds-Shepp patterns being cuspless.

15. In order to form at least one of the first path or the second path, the instruction further provides to the at least one processor of the control system: Selecting an expandable node from at least one of the first node of the first tree or the second node of the second tree, based on the cost associated with the expandable node. The control system according to claim 13, which extends the graph by adding child nodes connected to the expandable node by edges defined by collisionless motion primitives, such that the cost of the child nodes is less than the cost of the expandable node, wherein the cost of the child nodes is the minimum cost to reach the target node from the initial node through the child nodes, and the cost of the child nodes includes a first cost for the initial path through the first set of nodes, a second cost for the target path through the second set of nodes, and a third cost for the connecting path between the first set of nodes and the second set of nodes.

16. The control system according to claim 15, wherein the instruction further causes the at least one processor of the control system to remove an expandable node from the queue while expanding the graph to form the second path if a set of motion primitives between a root node and a corresponding expandable node includes a number of motion cusps greater than a predetermined threshold.

17. The instruction further provides to at least one processor of the control system: Selecting the expandable nodes to form the first path without the constraint associated with the number of cusps of motion, The control system according to claim 15, wherein the system performs the task of selecting the expandable nodes to form the second path, and the expansion of the graph is constrained to the total number of motion cusps.

18. The control system according to claim 13, wherein the instruction further causes the at least one processor of the control system to initialize the second tree with a root node located at a distance from the target node, having a cuspless movement to the target node.

19. The control system according to claim 18, wherein the root node is connected to the target node together with an edge that defines linear motion.

20. A method for controlling an entity, wherein the method is The invention includes creating a graph having a plurality of nodes that define the state of a vehicle, wherein the plurality of nodes in the graph include an initial node that defines the initial state of the vehicle and a target node that defines the target state of the vehicle, and each pair of nodes in the graph is connected to an edge defined by one or a combination of collision-free motion primitives that move the vehicle between the respective states of the connected nodes, each node includes several motion cusps, and the plurality of nodes connected through the corresponding edges form a first path through the graph that connects the initial node to the target node by a set of motion primitives that move the vehicle from the initial state to the target state. This includes determining a first number of motion cusps in the series of motion primitives of the first path, where each of the first number of motion cusps indicates a switch between forward motion and backward motion in the series of motion primitives. When it is determined that the first number of motion cusps exceeds a threshold, the process includes extending the graph to add new nodes until a termination condition is met, wherein the extension of the graph is subject to constraints associated with the total number of motion cusps, and the graph is extended to form a second path connecting the initial node to the target node, the second path having a second number of motion cusps that is less than the first number of motion cusps. A method comprising controlling the movement of the vehicle based on the second path.

21. The above method further, The extension of the graph is performed until the termination condition is met, If the second path is formed before the termination condition is met, the movement of the vehicle is controlled based on the second path, or The method according to claim 20, further comprising controlling the movement of the vehicle based on the first path if the second path is not formed until the termination condition is met.

22. To form at least one of the first or second paths, the method further: Creating a first tree of a first set of nodes starting from the initial node, Creating a second tree of a second set of nodes starting from the aforementioned target node, Connecting the first tree to the second tree by connecting a node from the first set of nodes to another node from the second set of nodes, The method according to claim 20, further comprising causing the vehicle to follow a track by a control command to the vehicle.

23. To form at least one of the first or second paths, the method further: Selecting an expandable node from at least one of the first node of the first tree or the second node of the second tree, based on the cost associated with the expandable node. The method according to claim 22, comprising extending the graph by adding child nodes connected to the expandable node by edges defined by collisionless motion primitives, wherein the cost of the child nodes is less than the cost of the expandable node, the cost of the child nodes being the minimum cost to reach the target node from the initial node through the child nodes, and the cost of the child nodes being a first cost of the initial path through the first set of nodes, a second cost of the target path through the second set of nodes, and a third cost of the connecting path between the first set of nodes and the second set of nodes.

24. A non-temporary computer-readable storage medium having a processor-executable program incorporated in order to perform a method, wherein the method is The invention includes creating a graph having a plurality of nodes that define the state of a vehicle, wherein the plurality of nodes in the graph include an initial node that defines the initial state of the vehicle and a target node that defines the target state of the vehicle, and each pair of nodes in the graph is connected to an edge defined by one or a combination of collision-free motion primitives that move the vehicle between the respective states of the connected nodes, each node includes several motion cusps, and the plurality of nodes connected through the corresponding edges form a first path through the graph that connects the initial node to the target node by a set of motion primitives that move the vehicle from the initial state to the target state. This includes determining a first number of motion cusps in the series of motion primitives of the first path, where each of the first number of motion cusps indicates a switch between forward motion and backward motion in the series of motion primitives. When it is determined that the first number of motion cusps exceeds a threshold, the process includes extending the graph to add new nodes until a termination condition is met, wherein the extension of the graph is subject to constraints associated with the total number of motion cusps, and the graph is extended to form a second path connecting the initial node to the target node, the second path having a second number of motion cusps that is less than the first number of motion cusps. A non-temporary computer-readable storage medium that includes controlling the movement of the vehicle based on the second path.

Images