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] The present disclosure generally relates to vehicle control, and more specifically to motion planning and predictive control of autonomous or semi-autonomous vehicle-trailer systems.

Background Art

[0002] Automated transport systems, even if the automation is partial, may lead to a reduction in accidents and more efficient use of vehicles and infrastructure. Connected and automated vehicles (CAVs) have shown 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 an important use case for vehicle automation. In particular, considering the increasing complexity of supply chain management, autonomous trucks may offer significant advantages in terms of economic, environmental, and social factors.

Summary of the Invention

Problems to be Solved by the Invention

[0003] In recent years, significant progress has been made in optimization-based planning and control for autonomous vehicle operation. In the case of heavy-duty vehicles, the focus of ongoing research has mainly been on the platooning and control of articulated vehicles, especially during freeway driving. Therefore, optimization-based motion planning and control techniques have been studied for the automation of various tasks of HDVs such as platooning or motion planning while driving on freeways. Regarding the motion planning of tractor-trailers, various techniques such as sampling-based, grid-based, or composite algorithms have been proposed. For example, various control algorithms including sliding mode control, linear quadratic regulation, input-state linearization, dynamic programming, and model predictive control have been proposed to track the obtained motion plan of the tractor-trailer system.

[0004] However, as another highly difficult operation that requires large-scale and costly vocational training, there is the hitching of a tractor-trailer. During the hitching operation, the driver needs to accurately control the position and orientation of the vehicle while performing a series of forward and reverse movements to connect a vehicle such as a truck to a stationary trailer. During this process, the driver's view of the area around the target hitch point may be restricted. Therefore, the task of automatic tractor-trailer hitching is a very important operation in large vehicle operation as it requires very high precision.

[0005] Most conventional research has focused on articulated tractor-trailer operations, while ignoring the hitching operation, which seems easier but is an important task in HDV operations that requires relatively high precision. Therefore, it is necessary to overcome the technical problems associated with the hitching operation, specifically the automatic tractor-trailer hitching 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. Another objective of some embodiments is to disclose a new type of control system that uses a motion planning algorithm and a real-time reference tracking controller adapted to the task of automatic hitching operation. Another objective of some embodiments is to provide a system and method that uses a modified variant of the bi-directional A-search guided tree (BIAGT) algorithm and a tracking controller that uses non-linear model predictive control for motion planning.

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

[0008] Some embodiments are based on the recognition that during the hitch operation, the driver needs to accurately control the position and orientation of the vehicle while making a series of forward and backward movements to connect the vehicle to the trailer. During this process, the complexity of the hitch operation may increase because the driver's view of the area around the target hitch point is restricted.

[0009] Some embodiments are based on the understanding that current research efforts focus on articulated tractor-trailer maneuvers while ignoring the hitch operation, which, although seemingly simpler, is an important task in HDV operations that requires relatively high precision.

[0010] Some embodiments are based on the recognition that the cause of the loss of precision in some types of operations along a path generated by a sampling-based motion plan lies not in the motion plan itself, but also in the control of the vehicle's motion following the path generated by the sampling-based motion plan.

[0011] Some embodiments are based on the recognition that it is common in sampling-based motion planning to generate a fragmented path that includes multiple motion cusps. The presence of motion cusps is common because it not only adds flexibility to spatial exploration but also tends to provide a shortest-distance path. Conventional motion planners typically seek to obtain the shortest path and thus provide a path that is troubled by unnecessary motion cusps that degrade the positioning accuracy of the integrated control system. Conventional motion planners tend to improve path optimality in terms of the distance of the path, and thus, conventional motion planners cannot address the problem of unnecessary motion cusps and, rather, in some cases add more motion cusps as the theory suggests. Some inventions are based on the recognition that when motion cusps are taken into account by integrating a cost function as a soft constraint, it typically results in longer computation time but an unnecessarily small number of cusps.

[0012] Some embodiments are based on the recognition that, in theory, the presence of motion cusps may introduce inconvenience and the need for gear shifting but should not cause accuracy problems.

[0013] Some embodiments are based on the recognition that when motion cusps are present, vehicle motion at zero and near-zero speeds is required, but such motion is a problem for control accuracy because the inertia of the vehicle dominates its dynamic behavior. Therefore, the uncertainty arising from each motion cusp in the vehicle's path increases with the number of motion cusps, causing accuracy problems.

[0014] The objective of some embodiments is to generate a path for performing an operation while reducing the number of kinematic singularities within the path. Some embodiments are based on the understanding that a sampling-based motion planner creates a tree by adding new nodes based on a so-called cost-to-go that indicates the cost of providing a path passing through the new node. Therefore, it is reasonable to penalize the cost of a node for having a kinematic singularity. However, the approach of using kinematic singularities as a soft constraint in the cost function may fail to find a feasible solution because the computational cost for sampling-based motion planning is high.

[0015] Accordingly, to overcome the above problems, some embodiments disclose a multi-stage motion planner, which aims to find a feasible path using a sampling-based motion planner without considering the number of kinematic singularities during the first stage. However, instead of ending the plan after finding a feasible path and outputting the path to the controller, the multi-stage motion planner continues to expand the tree using a hard constraint on the number of kinematic singularities during the second stage.

[0016] In addition, the objective of some embodiments is to provide a sampling-based motion plan that can enable the movement of a vehicle with improved accuracy. Examples of maneuvers that benefit from improved accuracy include hitching, docking, etc. of vehicles such as trucks. However, even normal parking in a narrow space can benefit from the accurate execution of the parking operation.

[0017] The object 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 an automated tractor-trailer hitch. The object of some embodiments is to provide a modified variant of the bidirectional A-search guide tree (BIAGT) path and motion planning algorithm. When tracking the reference trajectory obtained from the BIAGT algorithm to successfully complete the hitch operation, the object 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 recognition that a vehicle, such as a truck or tractor, steers towards a hitch point where the vehicle's hitch mechanism connects to a trailer while continuously maneuvering forward and in reverse.

[0019] Some embodiments are based on the recognition that, unlike typical automated driving on a highway or in an urban environment, when the vehicle is far from the hitch point but the lateral position error requirement and the heading error requirement near the hitch point are both very strict, the hitch operation is more forgiving with respect to the path tracking error. Therefore, there are specific requirements for an automated hitch operation to avoid damaging the hitch mechanism and to successfully connect to the trailer.

[0020] Some embodiments are based on the understanding that a set of vehicle poses is defined as P ⊂ R 3 and P free ⊂ P represents a set of collision-free poses of the vehicle.

[0021] The object of some embodiments is to calculate a trajectory and control the vehicle to perform a hitch operation from an initial pose p0 ∈ P free at zero velocity to a hitch pose p f ∈ P free while meeting specific requirements.

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

[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 objective of some embodiments is to provide an adjusted modification of the BIAGT algorithm for the motion planning of vehicle-trailer hitch operations.

[0025] The objective 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 objective of some embodiments is to provide the hardware-in-the-loop (HIL) verification results of the planner and controller for an automotive vehicle-trailer hitch.

[0027] Accordingly, one embodiment discloses a control system for controlling the movement of a vehicle using a motion planner. The control system includes 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 state of the vehicle. The plurality of nodes of the graph includes an initial node that defines the initial state of the vehicle and a goal node that defines the goal state of the vehicle, and each pair of nodes in the graph is connected by 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 a plurality of nodes connected through corresponding edges form a first path through the graph that connects the initial node to the goal node by a series of motion primitives for moving the vehicle from the initial state to the goal state. The at least one processor is configured to determine a first number of motion cusps in the series of motion primitives of the first path, and each motion cusp of the first number of motion cusps indicates a switch between forward and reverse motion in the series of motion primitives. If the at least one processor determines that the first number of motion cusps exceeds a threshold, it is configured to expand the graph to add new nodes until an end condition is met. The expansion of the graph is subject to a constraint associated with the total number of motion cusps, and the graph is expanded to form a second path that connects the initial node to the goal node. The second path has a second number of motion cusps that is less than the first number of motion cusps. The at least one processor is configured to control the movement of the vehicle based on the second path.

[0028] The at least one processor is further configured to execute the expansion of the graph until an end condition is met, and if a second path is formed before the end condition is met, control the movement of the vehicle based on the second path.

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

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

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

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

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

[0034] At least one processor is further configured to create a graph forming the first path by repeatedly expanding a node among a plurality of nodes to create a new node or by connecting two existing nodes among the plurality of nodes.

[0035] In some embodiments, to form at least one of the first path or the 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. Further, at least one processor is configured to connect the first tree to the second tree by connecting a node among the first set of nodes to another node among 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. Further, 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 of the 48 RS patterns, and the 8 RS patterns are without cusps.

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

[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 an edge between the child node and the corresponding expandable node includes a number of motion cusps greater than a predetermined threshold.

[0039] In some embodiments, at least one processor is further configured to select expandable nodes for forming a first path without constraints associated with the number of kinematic cusps. Further, at least one processor is configured to select expandable nodes for forming a second path, and the expansion of the graph is subject to constraints associated with the total number of kinematic cusps.

[0040] At least one processor is further configured to initialize a second tree with a root node spaced from a target node having a non-cusped movement to the target node.

[0041] Some embodiments are based on the understanding that the root node is connected to the target node with an edge defining a linear movement.

[0042] Another embodiment discloses a method for controlling the movement of a vehicle. The method includes creating a graph having a plurality of nodes that define the state of the vehicle. The plurality of nodes of the graph includes 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 by 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 series of motion primitives for moving the vehicle from the initial state to the target state. The method includes determining a first number of motion cusps in the series of motion primitives of the first path, where each motion cusp among the first number of motion cusps indicates a switch between forward and backward motion in the series of motion primitives. The method includes expanding the graph to add new nodes until an end condition is met when it is determined that the first number of motion cusps exceeds a threshold. The expansion of the graph is subject to a constraint associated with the total number of motion cusps, and the graph is expanded to form a second path that connects the initial node to the target node. The second path has a second number of motion cusps that is less than the first number of motion cusps. The method includes controlling the movement of the vehicle based on the second path.

[0043] Yet other embodiments disclose a computer-readable storage medium having a program executable by a processor to perform a method for controlling the movement of a vehicle using a motion planner. The method includes creating a graph having a plurality of nodes defining the state of the vehicle. The plurality of nodes of the graph includes an initial node defining an initial state of the vehicle and a goal node defining a goal state of the vehicle, and each pair of nodes in the graph is connected by 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 a number of motion cusps, and a plurality of nodes connected through corresponding edges form a first path through the graph that connects the initial node to the goal node by a series of motion primitives for moving the vehicle from the initial state to the goal state. The method includes determining a first number of motion cusps in the series of motion primitives of the first path, each motion cusp of the first number of motion cusps indicating a switch between forward and backward motion in the series of motion primitives. The method includes expanding the graph to add new nodes until an end condition is met when it is determined that the first number of motion cusps exceeds a threshold. The expansion of the graph is subject to a constraint associated with the total number of motion cusps, and the graph is expanded to form a second path that connects the initial node to the goal node. The second path has a second number of motion cusps that is less than the first number of motion cusps. The method includes controlling the movement of the vehicle based on the second path.

[0044] Some embodiments are based on the understanding that performing the hitch operation of a vehicle to hitch the vehicle to a trailer requires complex, time-consuming, and costly vocational training. In addition, since the visibility of the area associated with the hitch point is limited, the driver of the vehicle may have to make several extra movements (such as forward and reverse movements) in order to accurately hitch the vehicle to the trailer. Due to the limited visibility, the hitch operation may also be prone to causing accidents. However, current path and motion planning systems do not automatically perform such hitch operations of the vehicle. Therefore, an object of the present disclosure is to enable automatic hitching of a vehicle to a trailer. By automation, the time required for hitching and the probability of accidents can be reduced. In this way, such complex hitch operations can be automatically performed.

[0045] Embodiments of the present disclosure will be further described with reference to the accompanying drawings. The drawings shown are not necessarily to scale, but rather focus on explaining the principles of the embodiments of the present disclosure as a whole.

Brief Description of the Drawings

[0046]

Figure 1A

Figure 1B

Figure 1C

Figure 1D

Figure 1E

Figure 2

Figure 3A

Figure 3B

Figure 4A

Figure 4B

Figure 4C

Figure 4D

Figure 5A

Figure 5B

Figure 6A

Figure 6B

Figure 7A

Figure 7B

Figure 8A

Figure 8B

Figure 9

Embodiments for Carrying Out the Invention

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

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

[0049] Specific details are given in the following description for a thorough understanding of the embodiments. However, one of ordinary skill in the art can understand that the embodiments can be practiced without these specific details. For example, the systems, processes, and other elements in the disclosed subject matter may be shown in the form of block diagrams as components, so as not to obscure the embodiments with unnecessary details. In other instances, well-known processes, structures, and techniques may be shown without unnecessary detail to avoid obscuring the embodiments. Further, like reference numerals and designations in the various drawings indicate like elements.

[0050] The objective of some embodiments is to disclose a system and method for controlling the movement of a vehicle using a motion planner. Another objective of some embodiments is to disclose a path and motion planning algorithm and a predictive controller 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 the dynamic model and constraints of the system to be controlled. An example of a path and motion planning algorithm is the bidirectional A-search guide tree (BIAGT). Another objective of some embodiments is to disclose a technique for controlling the movement or operation of a vehicle during hitch operation in an automated manner, i.e., without human intervention. Another objective of some embodiments is to disclose a technique for online calculating a kinematically feasible reference trajectory from an initial pose p0 to an end or goal pose p f up to, while avoiding any collision of the vehicle with any stationary obstacle in the environment.

[0051] Typically, the BIAGT algorithm can calculate a kinematically feasible solution for the parking task of a motor vehicle by forming a graph and searching for a trajectory. During operation, the BIAGT algorithm performs a bidirectional search and can discover the shortest path from the initial node to the goal node by starting the search simultaneously from both directions. In particular, BIAGT can start generating subgraphs from both directions, for example, one starting from the initial node and the other starting 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 end when the two subgraphs intersect. Based on this graph, a trajectory can be determined.

[0052] Therefore, the hitch operation of the vehicle can be regarded as a special parking task with very strict requirements for the errors in the lateral position and the traveling direction angle at the final stage of the operation. To perform the hitch operation of the vehicle, BIAGT is modified. Embodiments of the present disclosure will be described in detail in conjunction with the accompanying drawings.

[0053] FIG. 1A shows an example of a graph showing constraints, hard constraints, and soft constraints for motion primitives for motion planning. According to some embodiments, vehicle path planning plays an important role in a vehicle navigation system. Therefore, trajectory planning is an essential task for autonomous vehicles. Map and sensor-based data form the basis for generating a trajectory that serves as a target value to be tracked by a controller. When generating a target trajectory, in addition to the aspect of comfort, feasibility and possible collisions must be considered.

[0054] During path planning and trajectory generation, it may be necessary to perform constrained optimization. The movement of the vehicle must be planned and adjusted to achieve the driving task while taking into account the constraints introduced by the selected model. The motion planning layer plays the role of calculating a safe, comfortable, and mechanically achievable trajectory from the current configuration or current state of the vehicle to the goal configuration or goal state. The goal configuration may vary depending on the situation. For example, the goal position may be a hitch point where a vehicle such as a tractor can hitch to a trailer to form a tractor-trailer configuration.

[0055] The path planning problem is a problem of finding a path σ(α): [0,1] → X within the configuration space X of a vehicle (or more generally a robot) that starts from an initial state and reaches a target position while satisfying the constraints. By solving the path planning problem, a mechanically achievable path can be generated. Since the robot may have some motion constraints such as non-holonomic states in an under-actuated system, the generated trajectory must be smooth without extreme direction changes. A feasible path planning is a problem of determining a path that satisfies some given problem constraints (i.e., hard constraints) without paying attention to the quality of the solution.

[0056] However, although the generation of a feasible path guarantees obstacle-free navigation, the quality of the generated path may be affected because hard constraints are imposed. For example, the path or trajectory may have sharp direction changes, low quality, a longer route, etc. As shown in FIG. 1, a motion plan with hard constraints 102 on motion primitives may prevent the efficiency of the path.

[0057] Therefore, since the 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 infeasible path that does not efficiently overcome obstacles.

[0058] Therefore, an optimal path plan is necessary. An optimal path plan is a problem of finding a path that optimizes some quality criterion under given constraints. The problem of finding an optimal path is difficult. Additionally, there may not exist an (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 an optimal path.

[0059] Soft constraints are conditions on the trajectory generated by a solution, which may or may not be satisfied, but are prepared to accept not being satisfied due to the cost of satisfying the condition or conflict with other constraints or goals. If there are multiple soft constraints and there is a conflict between them or it is found that the cost of satisfying them is high, it is necessary to determine which of the various constraints should be prioritized.

[0060] According to the present disclosure, in order to guarantee the accuracy of the path, a soft constraint 106 is imposed on the number of motion cusps. Embodiments of the present disclosure will describe imposing soft constraints on path planning based on the number of motion cusps in the generated path.

[0061] FIG. 1B shows an environment in which a control system 100 is implemented according to some embodiments of the present disclosure. The control system 100 includes a motion planner 108, a prediction 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 the vehicle while performing a hitch operation. During the hitch operation, the vehicle may need to continuously perform forward and reverse movement operations while steering towards the hitch point. At the hitch point, the fifth wheel of the vehicle hitch mechanism can be connected to the trailer.

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

[0063] During operation, the prediction controller 110 receives a reference trajectory 105 indicating the desired behavior of the system 120 calculated by the motion planner 108. 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 indicate a time function of the path along which the vehicle should travel to perform an operation such as a hitch operation. For example, the motion planner 108 may generate a path based on, for example, a graph structure. Further, a time dimension may be added to the path to generate the reference trajectory 105.

[0064] In response to receiving the reference trajectory 105, the prediction controller 110 generates a control signal (also referred to as a control input) 111 that functions as an input to the system 120. In response to receiving the control input 111, the system 120 updates the output 103 of the system 120. Based on the measured value 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 tracks the movement of the vehicle to check whether the vehicle is traveling along the reference trajectory 105 using the state of the system 120.

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

[0066] System 120 may be configured to generate a number of controlled output signals 103 (also referred to as output 103). For example, the output 103 may be associated with a physical quantity such as current, flow rate, speed, position indicating a state transition from a previous state to a current state of System 120, etc. In one example, the output 103 may be partially associated with previous output values of System 120 and may be partially associated with previous and current input values. The dependence on previous inputs and previous outputs may be encoded within the state of System 120. During operation of System 120, for example, System 120 may generate an output 103 indicating a change in the state of the vehicle, such as a change in the exact position, orientation, or setting of the vehicle being controlled, based on the movement of the vehicle from one state to another while following a reference trajectory 105. In one example, the control input 111 may also be provided to an actuator of the vehicle to move the vehicle in an automated manner based on the reference trajectory. The output 103 may include a series of output values generated by System 120 following the application of a certain input value.

[0067] A system model 107 may be associated with System 120. The 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 current inputs, previous inputs, and previous outputs. The state of System 120 may correspond to any set of information such as information that varies over time. For example, the state of System 120 may be a subset of current and previous inputs and outputs that, together with the 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. The constraints 109 may be applied to limit the range of the output 103, the input 111, and in some cases, the state of the system 120 in which the system 120 is allowed to operate. In one example, the constraints may correspond to a driving speed limit, a limit on the physical area associated with the environment, etc.

[0069] The prediction controller 110 may be implemented in hardware or as a software program executed on a processor, such as a microprocessor. The prediction controller 110 may receive the 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 movement of the vehicle based on the current and previous inputs and the current and previous outputs of the system 120. Based on the control input 111 provided by the prediction controller 110, the actuator of the vehicle may be controlled to move the vehicle. The actuator may be controlled to drive the vehicle on the reference trajectory 105. The control of the vehicle based on such a control input 111 may cause a change in the state of the vehicle. Thereafter, by obtaining the estimated state 121 based on the output of the system 120, the prediction controller 110 can monitor and track the movement of the vehicle.

[0070] The prediction controller 110 can receive the estimated state 121 and the desired reference trajectory 105. Further, the prediction controller 110 may be configured to obtain an input, such as a control signal or a control input 111, for operating the system 120 to control the movement of the vehicle using the received information. The prediction 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 either the same as or different from the prediction controller 110. The motion planner 108 is configured to receive the estimated state 121 and the desired target state 101 of the system 120 at fixed or variable control cycle sampling intervals. The motion planner 108 is configured to use the received information to obtain a reference trajectory 105 for the prediction 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 either the same as or different from the prediction controller 110 or the motion planner 108. The state estimator 130 may be configured to receive the output 103 of the system 120 at fixed or variable control period sampling intervals. Further, the state estimator 130 may use new and previous output measurements to obtain 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 signal or control input 111 generated by the prediction controller 110. The updated reference trajectory can optimize the operation of tasks such as vehicle maneuvering.

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

[0074] As described above, unlike typical autonomous driving on a trunk road or in an urban environment, the hitch operation is likely to cause a path tracking error when vehicle 153 is far from the hitch point. However, the hitch operation is very strict in both the lateral position error requirement and the traveling direction error requirement in the vicinity of the hitch point. Therefore, damage to the hitch mechanism can be prevented and the trailer 154 can be successfully connected for hitching.

[0075] According to some embodiments of the present disclosure, the automatic hitch operation must be performed based on a reference trajectory 105. The reference trajectory 105 may be calculated by a motion planner 108. For example, the reference trajectory 105 may include a forward movement 151, a subsequent reverse movement 152, and a hitch movement between vehicle 153 and trailer 154. Some embodiments of the present disclosure are based on the recognition that the position and traveling direction errors with respect to the reference trajectory 105 must be small enough towards the end of the reference trajectory 105 at the final stage of the hitch operation, i.e., when the hitch point is very close, to enable the success of the hitch between the fifth wheel of tractor 153 and the hitch system of trailer 154.

[0076] According to some embodiments, a set of vehicle postures of vehicle 153 is P⊂R3 is defined as, P free ⊂ P represents a set of collision - free postures of vehicle 153. For this reason, there are several requirements or constraints for performing an automatic hitch operation. In one example, the requirements for an automatic hitch operation may include automating the vehicle actions of the hitch operation so that no human intervention is involved. Further, the requirements for an automatic hitch operation are that the reference trajectory 105 from the initial posture p0 of vehicle 153 to the hitch posture p f of vehicle 153 can include calculating the reference trajectory 105 online in real - time or near real - time so that it is kinematically feasible while avoiding any collision with any stationary obstacles such as obstacles 158a and 158b in the environment. In one example, the requirements for an automatic hitch operation may also include that, for the success of the hitch and the safety of the hitch mechanism, the lateral position error |e Y | with respect to the preferred hitch orientation must be less than 0.1 meter (m), and the forward - direction error |e Ψ | must be less than 10 degrees (deg). Additionally, the lateral position error and the forward - direction error must be within a set P f ∈ P f centered on the hitch posture p f ⊂ P free which is necessary. Further, the requirements for an automatic hitch maneuver include that when a dynamic obstacle enters the safety set around the current or predicted position of vehicle 153, the real - time trajectory operation and tracking are interrupted by an emergency braking system, and when the dynamic obstacle leaves or exits the environment, the real - time trajectory operation and tracking are resumed.

[0077] In addition, the system for controlling the operation of vehicle 153 for an automatic hitch operation needs to be executed on an automotive - grade embedded system platform and meet real - time requirements. In particular, the motion planner 108 may need to execute a motion plan when stopped and calculate the reference trajectory 105 within a maximum calculation time of 2 - 5 seconds. Further, the prediction controller 110 is T SThere may be a case where continuous trajectory tracking must be performed while the vehicle 153 is moving with a sampling time of = 50 milliseconds (ms).

[0078] In one example, the path and motion planning system may have to control the operation of the vehicle 153 to perform a hitch operation under the assumption of normal driving conditions, i.e., in non-critical operation. Thereafter, the vehicle modeling may be based on a single-track model in which two wheels on each axle of the vehicle 153 are lumped together. Further, if the vehicle 153 has multiple rear axles, these multiple axles may be lumped together into a model with only two wheels, one front wheel and one rear wheel. A dynamic state model based on the balance of forces and torques is generally more accurate than a kinematic model, but the differences at relatively low speeds, which are typical in vehicle-trailer hitch operations, are small and are compensated for by the nature of the feedback of the prediction controller 110.

[0079]

Number

[0080]

Number

[0081] In some embodiments of the present disclosure, one or more road-side units (RSUs) or infrastructure detection devices 165 may be used for the accurate execution of an automated vehicle-trailer hitch operation based on an accurate detection of the current state of the vehicle 153 and the current environment of the path and motion planning system. For example, one or more RSUs 165 may include one or more sensors, such as distance rangefinders, radars, LIDARs, and / or cameras, and sensor fusion techniques to accurately detect the state of the vehicle 153 and the dynamic environment around the vehicle 153.

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

[0083] FIG. 1D shows a schematic diagram of a vehicle 153 including a path and motion planning system 172 that employs the principles of some embodiments. Examples of the vehicle 153 may include, but are not limited to, passenger cars, large vehicles, tractor-trailers, buses, or rovers. In addition, the vehicle 153 may be an autonomous or semi-autonomous vehicle.

[0084] According to some embodiments of the present disclosure, techniques for controlling the movement of the 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 steering system 173 of the vehicle 153. In one embodiment, the steering system 173 is controlled by the path and motion planning system 172. In addition or alternatively, 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 path 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 can include, but are not limited to, distance rangefinders, radar, LIDAR, and cameras. 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 can include, but are not limited to, a global positioning system (GPS), accelerometers, inertial measurement units, gyroscopes, shaft rotation sensors, torque sensors, deflection sensors, pressure sensors, and flow sensors. Sensors 174 and 175 may provide information to the path and motion planning system 172. Vehicle 153 may be provided with a transceiver 177 that enables communication functions of the path and motion planning system 172 through a wired or wireless communication channel.

[0086] For example, the path and motion planning system 172 may control the operation or movement of vehicle 153. In this regard, the path and motion planning system 172 may generate a reference trajectory 105 for vehicle 153 to move along to complete a task. Further, the path and motion planning system 172 may generate motion commands for vehicle 153 based on the reference trajectory 105 to be followed. Further, the path and motion planning system 172 may control components such as the steering system 173 of vehicle 153 to configure vehicle 153 to execute 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] FIG. 1E shows an example of a plot of a path for controlling the movement of a vehicle according to some embodiments. According to 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 pair of corresponding nodes. Nodes 186a, 186b, 186c, 186d, 188a, 188b, and 188c are selected to generate path 182 from a plurality of other nodes such that nodes 186a, 186b, 186c, 186d, 188a, 188b, and 188c are mechanically realizable, that is, to ensure a collision-free path 182. Similarly, nodes 190a, 190b, 190c, 190d, 190e, 192a, and 192b are mechanically realizable nodes.

[0088] For an accurate hitch operation, a path may have to be generated that has a constraint on the total number of motion cusps within the path. Specifically, when the vehicle is near the hitch point, there should be no or few motion cusps. For this reason, path 182 shows, in particular, imposing a hard constraint on the total number of motion cusps towards the end of the path when the vehicle may be near the hitch point. As shown, in path 182, nodes 186a, 186b, 186c, and 186d may not completely eliminate the motion cusps within the corresponding motion primitive, but nodes 188a, 188b, and 188c can satisfy the hard constraint on the number of motion primitives and may not include any number of motion cusps.

[0089] The generation of such a path 182 may be difficult, especially in terms of satisfying hard constraints towards the ends of the path 182. Thus, embodiments of the present disclosure provide techniques that ensure that some of the nodes, such as nodes 192a and 192b of path 184, satisfy the constraints while ensuring that the entire path 184 satisfies some constraints to some extent without degrading the path quality. In this way, soft constraints are imposed on the mechanically realizable path 182 to generate a path 184 that satisfies the constraints associated with the maximum number of motion cusps within the path.

[0090] Aspects 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 in conjunction with the following drawings.

[0091] FIG. 2 shows a schematic diagram 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 sophisticated interconnected sensing and control components. Due to the complexity associated with the development of the software and hardware components of an autonomous vehicle and dynamic driving conditions, the autonomous control of the vehicle is complex.

[0092] According to an embodiment, a path and motion planning system 172 for controlling the movement of a vehicle 153 may include a path and motion planning layer 210 and a vehicle controller layer 220. For example, the path 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 path and motion planning layer 210 calculates a reference trajectory 105 and provides the reference trajectory 105 to the vehicle controller layer 220. Further, the vehicle controller layer 220 calculates a control input 111 for the system 120 to track the reference trajectory 105. The system 120 can update the reference trajectory 105 to optimize vehicle operation. 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, a 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 target. Such a route is then navigated by the vehicle 153 in an automatic or semi-automatic manner. According to the present disclosure, the vehicle 153 may drive on this route in an automatic manner. Additionally, this route may accommodate hitch operations, parking operations, and the like.

[0096] Given a route, the decision-making layer 200 can serve to determine one or more local driving goals (or target goals corresponding to target positions) of the vehicle 153 and corresponding discrete decisions 201. Each of the discrete decisions 201 may be a desired sequence of one or more motion commands. Examples of motion commands can include, but are not limited to, turning right, staying within a lane, turning left, changing lanes, or coming to a complete stop at a specific position. In this regard, some sensing and mapping modules 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 previous map information, to estimate an estimated state from the surrounding portion of the environment related to the system 120 and the vehicle 153 and the current state of the system 120 for a specific driving scenario. The sensing and mapping modules may be made available to one, multiple, or all of the layers of the path and motion planning system 172.

[0097] Based on one or more driving goals and corresponding discrete decisions 201, the path and motion planning layer 210 serves to determine a reference trajectory 105 provided to the vehicle controller layer 220. In some embodiments of the present disclosure, the reference trajectory 105 is a safe, desirable, and mechanically achievable trajectory for the vehicle 153 to follow. The reference trajectory 105 may be determined based on the output from the sensing and mapping module and the system 120. Some embodiments are based on the recognition that an important requirement for the reference trajectory 105 calculated by the path and motion planning layer 210 is that the reference trajectory 105 is collision-free, mechanically achievable, and trackable by the predictive controller 110 within the vehicle controller layer 220. This means that the reference trajectory 105 avoids any collisions with objects in the environment and achieves or reaches one or more target goals while considering the dynamic system model 107 of the controlled system 120, which can be represented by a set of mathematical equations.

[0098] Some embodiments of the present disclosure are based on the recognition that a typical limiting factor in path and motion planning tasks is the non-convexity of constrained dynamic optimization problems. This results in achieving only local optimal solutions that can be very far from the global optimal solution. In some cases, solving the optimization problem can utilize a significant computational load and time just to discover realizable solutions. According to some embodiments, path and motion planning may be performed using sampling-based methods such as rapidly exploring random tree (RRT), or graph search methods such as A*, D*, and other variants.

[0099] As shown in FIG. 2, the vehicle controller layer 220 aims to achieve the reference trajectory 105 by calculating a control signal or control input 111 for operating the system 120 in consideration of the dynamic system model 107 and the constraints 109. The control input 111 may include one or more actuation commands such as values of steering angle, wheel torque, and braking force. In some embodiments of the present disclosure, the vehicle controller layer 220 provides the control input 111 to an additional layer consisting of one or more controllers within 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 trajectory 105 calculated by the path 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 such that it can effectively use future information in the long-term reference trajectory 105 calculated by the path and motion planning layer 210 to achieve the desired movement or operation 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 the 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 the objective function may be non-linear, and / or the dynamic system model 107 equations describing the vehicle state behavior may be non-linear, resulting in a non-linear 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 calculate a relatively long-term and highly predictive motion plan, but typically needs to be executed at a relatively low sampling frequency. In some embodiments of the present disclosure, the path and motion planning layer 210 is executed once for each desired sequence of one or more motion commands. For example, the path and motion planning layer 210 may be executed for the automatic hitch operation shown in FIG. 1B. In some embodiments of the present invention, the path and motion planning layer 210 may be executed multiple times for each desired sequence of one or more motion commands to perform an automatic hitch operation so that replanning is possible, for example, when the surrounding environment of the vehicle 153 changes substantially.

[0103] Some embodiments of the present disclosure are based on the recognition that the predictive vehicle controller layer 220 can track the reference trajectory 105 by calculating the control input 111 over a relatively short prediction range while executing at a relatively high sampling frequency. For example, the vehicle controller layer 220 can use a prediction horizon of 1 to 10 seconds while executing at 10 to 100 times per second. The vehicle controller layer 220 can have high responsiveness, for example, to uncertainties associated with obstacles in the surrounding environment of the vehicle 153 in vehicle state estimation and to local deviations resulting from other uncertainties in the perception and mapping module.

[0104] In some embodiments of the present disclosure, different dynamic vehicle models may be used in different components within the multi-layer path and motion planning system 172 to control the movement of an autonomous vehicle or a semi-autonomous vehicle. For example, a relatively simple but low-computation-cost kinematic model may be used in the path and motion planning layer 210, while a relatively accurate but high-computation-cost dynamic single-track or double-track vehicle model may be used in the vehicle controller layer 220.

[0105] As shown in FIG. 2, for the autonomous or semi-autonomous control of the vehicle 153, information may be shared between different components within the multi-layer path and motion planning system 172. For example, information 205 regarding the map and the vehicle surroundings may be shared between the decision layer 200 and the path and motion planning layer 210. Also, information 215 regarding the map and the vehicle surroundings may be shared between the path 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. Additionally, some embodiments of the present disclosure are based on the recognition that the reliability and safety in the control of the vehicle 153 can be improved by using diagnostic information such as performance metrics of the success and / or failure of an algorithm in one component that can be shared with the components of the multi-layer path 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 tree starting from an initial position (or source position) and a target position. The tree may be composed of a set of nodes indicating states and a set of edges. In this specification, any node or edge of the tree may include the position and heading information of the vehicle 153. The set of nodes and the set of edges may represent kinematically or dynamically realizable transitions between states that are collision - free. Additionally, the set of nodes and the set of edges may represent all possible states in which the vehicle 153 does not overlap with any obstacle in the environment.

[0107] The motion planner 108 is configured to generate a path, and based on this path, a reference trajectory 105 is generated. A method for calculating a path for the motion planner 108 to control the movement of the vehicle 153 to execute a task will be described in detail below.

[0108] FIG. 3A shows an example 300 of a method for generating a first path to control the movement of the vehicle 153 according to some embodiments of the present disclosure. In one example, the movement 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 FIG. 1B, the initial state 157 may correspond to the starting pose of the vehicle 153, and the target state 158 may correspond to the position of the vehicle 153 corresponding to the hitch operation.

[0109] Note that vehicle 153 is equipped with a function to interact with the environment and receive sensor data corresponding to the state of the vehicle and the surrounding environment. Vehicle 153 is configured using a path and motion planning system 172 for controlling the movement of vehicle 153 in an automated manner. For example, the path and motion planning system 172 may include one or more components for determining a first path of vehicle 153 and further controlling the movement of vehicle 153 to perform a specific operation. According to an embodiment of the present disclosure, the operation of vehicle 153 may correspond to a hitch operation. For example, the path and motion planning system 172 may provide a set of motion commands for controlling the movement of vehicle 153 based on the first path. In one example, the path and motion planning system 172 may utilize a BIAGT algorithm to generate a path of vehicle 153 to cause vehicle 153 to perform a hitch operation. Method 300 describes the use of the BIAGT algorithm for generating a path to perform a hitch operation in an effective and efficient manner.

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

[0111] The bidirectional A-search guide tree (BIAGT) path and motion planning algorithm will be understood to be a variant of the A*-based algorithm. The BIAGT algorithm may be capable of efficiently calculating a kinematically realizable solution for an automotive vehicle parking task. According to an embodiment of the present disclosure, the described hitch operation can be regarded as a special parking task with very strict requirements for the final stage of the operation in terms of the lateral position error and the traveling direction angle error.

[0112] The BIAGT algorithm improves the hybrid A* algorithm in two aspects. Given a configuration space P of a set of vehicle postures including position and heading direction, the BIAGT algorithm prioritizes the control actions at each node to balance the optimality of the calculated path and the computational efficiency. In addition, the BIAGT algorithm simultaneously expands two trees or subgraphs, namely, a first tree (also called the initial tree) from the initial node towards the goal node and a second tree (also called the goal tree) from the hitch point in the trailer 154 corresponding to the goal node towards the initial node. In particular, each of the first tree and the second tree estimates the cost-to-go of its own nodes by leveraging the arrival costs of the other tree or other subgraphs. When a feasible path is found, BIAGT may execute a motion planning step to determine a speed profile along the feasible path and output a motion plan.

[0113] In one example, the tree of the graph may be defined as the union of a set of nodes and a set of edges. In this regard, the tree is T = (υ, ε), where υ ⊂ P free , E(X i , X j ) ∈ ε. For this purpose, E(X i , X j ) represents a feasible and collision-free trajectory between the state values X i and X j . Furthermore, P free is implicitly obtained by examining collisions with stationary obstacles such as obstacles 158a and 158b in the environment. Assume that M represents a finite set of motion primitives pre-computed from the available control actions and V max represents the maximum number of nodes allowed in the tree of the graph.

[0114] According to some embodiments of the present disclosure, 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, a modification is made to adjust the BIAGT algorithm to generate a first path.

[0115] Continuing with this example, at 302, a first tree is constructed starting from an 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 a start tree) T starting from X0. S to construct.

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

[0117] At 306, a graph forming the first path is constructed. According to some embodiments, the graph of the first path may be aimed at minimizing the path cost without restricting the motion primitives for traveling from the initial state 157 to the goal 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] FIG. 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 the state X of the vehicle 153 i and X jIt may have a plurality of nodes (shown as nodes 312a to 312i) that define states such as etc. The plurality of nodes 312a to 312i of the graph 310 include an initial node 312a that defines the initial state X0157 of the vehicle 153 and the target state X f of the vehicle 153 may include a target node 312i that defines 156.

[0119] The target state X f 156 may correspond to a hitch point. Each pair of nodes 312a to 312i in the graph 310 is connected by an edge (shown as edges 314a to 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 to 312i may include several motion cusps that represent the number of motion cusps in the path from either 312a or 312i to a particular node. For example, a first path through the graph 310 is generated that connects the initial node 312a to the target node 312i by a series of motion primitives for moving the vehicle 153 from the initial state 157 to the target state or goal or target state 156 using a plurality of nodes 312a to 312i connected through the corresponding edges 314a to 314h.

[0120] In one example, the graph 310 may be created by repeatedly expanding a certain node among the plurality of nodes 312a to 312i to create new nodes, or by connecting two existing nodes among the plurality of nodes 312a to 312i. The new nodes and the expanded nodes 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 the graph 310, the motion planner 108 may adopt the BIAGT algorithm. The BIAGT algorithm can connect two nodes corresponding to the state of the vehicle 153. For example, two states of the vehicle are X i and X jIt can be represented by. Subsequently, two states X i and X j can be connected by an edge indicating a collision-free motion primitive. For example, two states X i and X j 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 a constraint on the number of motion cusps within the connection path. The 48 Reeds-Shepp (RS) patterns are described in detail in connection with FIG. 5B.

[0122] The BIAGT algorithm may simultaneously expand each of the first tree 320 and the second tree 330, i.e., the states in each of the first tree 320 and the second tree 330. In one example, the expansion of the first tree 320 and the second tree 330 may be based on a cost function F(·) that sums 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 towards the corresponding goals of the first tree 320 and the second tree 330 while ignoring the obstacles 158a and 158b. When either the first tree 320 or the second tree 330 approaches the corresponding goal, or when the distance between the two trees 320 and 330 is less than or equal to the threshold ε, BIAGT may connect the two trees 320 and 330 with a mechanically realizable edge 316. If the connection is successful, all the parents of the connected nodes of one tree are added to the other tree to obtain a realizable 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 mechanically executable 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 corresponding edges 314a, 314c, 314e, 316, 314f, and 314h.

[0123] Therefore, since there is no constraint associated with the number of kinematic singularities during the generation of the first path 318, the graph 310 may be formed by connecting different states or nodes of the trees 320 and 330 using any one of 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 hard constraints associated with the number of motion cusps allowed in the connection path between two states. In such a case, the connection path of the motion primitives between two states for generating the second path can have a minimum number of motion cusps. According to some embodiments, the motion primitive between two states X i and X j can represent constituting the shortest distance path with one of the 8 cusp-free RS patterns out of the 48 RS patterns. Such embodiments associated with the generation of the second path will be described in detail in conjunction with FIGS. 4A, 4B, 4C, and 4D below. In some embodiments, the motion primitive to be applied in state X i shall not introduce additional cusps, i.e., the speed of the motion primitive to be applied in X i shall be the same as the speed of the motion primitive reaching X i from its parent.

[0125] When 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 - 312i on both the trees 320 and 330, i.e., the forward tree 320 and the backward tree 330, reaches V max and the trajectory that is the solution, 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 the control of the vehicle 153 may be transferred for manual operation.

[0126] Based on the generated first path 318, the modified BIAGT algorithm adopted by the motion planner 108 may improve the performance of the prediction controller 110 to track the reference trajectory 105 of the vehicle 153 and to enable the success of the hitch operation. The manner in which the motion planner 108 uses the modified BIAGT algorithm to control the movement of the vehicle 153 to efficiently execute the hitch operation is defined in detail below.

[0127] FIG. 4A shows a method 400 as an example for controlling the movement of the vehicle 153 based on a second path according to some embodiments of the present disclosure. For example, the movement 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 the method 400 may be performed after the generation of the first path 318 described in FIG. 3B.

[0128] According to some 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 prediction controller 110 for tracking the resulting reference trajectory 105. The modified BIAGT algorithm is configured to continue the expansion of 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 regarding 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, a first tree 320 from the initial node 312a and a second tree 330 from the goal or target node 312i. The method of constructing the second path is described in this method 400.

[0129] In 402, the first number of motion cusps in a 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 and reverse motions in the series of motion primitives. As used herein, a motion cusp switches a forward motion to a reverse motion, or a reverse motion to a forward motion, in a series of motion primitives that define the first path 318.

[0130] Since conventional motion planners typically attempt to find the shortest path, they provide a path such as the first path 318 that has unnecessary motion cusps. The reason is mainly 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 that the speed of the motion of the vehicle 153 be zero and near zero, and such motions are problematic for the accuracy of the path and motion planning system 172 because the inertia of the vehicle 153 dominates its dynamic behavior. Therefore, each motion cusp in the path of the vehicle 153 amplifies the effect of uncertainty, which accumulates with the number of motion cusps and results in accuracy problems.

[0131] To overcome problems associated with the number of motion cusps, when it is determined at 404 that the first number of motion cusps in the first path 318 exceeds a threshold, the motion planner 108 is configured to continue to expand the graph 310 of the first path 318 while taking into account the number of motion cusps. In particular, when comparing the first number of motion cusps with the threshold and determining that the first number of motion cusps exceeds the threshold, the motion planner 108 may continue to expand the graph 310 and add more new nodes. The threshold is defined as the maximum number of allowable motion cusps within the 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 trajectory 105 based on the first path 318 with higher 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 of the motion cusps, which may impair the accuracy of the path plan and prevent the vehicle 153 from traveling along the reference trajectory 105 based on the first path 318.

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

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

[0134] Continuing further, at 408, the motion planner 108 further expands the graph 310 to generate a second path. In particular, the graph 310 is expanded to add new nodes to the graph 310 until the end condition is met. Such an expansion of the graph 310 is subject to a hard constraint associated with the total number of motion cusps in the graph 310 or the path. The graph 310 is further expanded to form a second path that connects the initial node 157 or 312a to the target node 156 or 312i. The second path may have a second number of motion cusps that is less than the first number of motion cusps within the first path 318.

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

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

[0137] According to some embodiments, the generation of the second path may be initialized based on the search for new nodes, and the selection of nodes is based on imposing a penalty on the change in the speed direction when calculating the arrival cost associated with the nodes. However, in order to balance completeness, computational efficiency, and path quality, the way of imposing such a penalty may substantially increase the time 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] FIG. 4B shows a graph structure 420 as an example for forming a second path according to some 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, although the embodiments of the present disclosure describe controlling the movement of the vehicle 153 during a hitch operation, this should not be construed as a limitation. In other embodiments of the present 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 the second path, the motion planner 108 may utilize a modified BIAGT algorithm. The motion planner 108 may construct the graph 420 using a modified BIAGT algorithm.

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

[0141] For example, the construction of the start tree 422 begins by 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 the 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 includes selecting the best node, such as node 426b, with the lowest cost of moving towards the goal node, i.e., the target or goal state 156. The construction of the start tree 422 further includes expanding the best node by applying motion primitives, which grows the start tree 422 by adding collision-free child nodes and collision-free edges, with each of the collision-free edges connecting the best node 426b to one or more corresponding child nodes. When all possible motion primitives of a particular type that give collision-free child nodes within the start tree are applied to a node, this node is removed from the priority queue.

[0142] Thereafter, the root node 426c of the second tree 330, which is at a distance from the target state 156, is initialized. In other words, rather than starting the second tree 330 or goal tree strictly from the target state X f or the hitch point corresponding to 156, the root node 426c of the second tree 330 is placed at a location at a distance defined by the user from the hitch point in the preferred hitch direction from the target state X f in the trailer. 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 to 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 a straight-line motion. Thereby, the BIAGT algorithm surely calculates a motion plan or a second path that ends at a straight-line segment before the vehicle 153 reaches the vicinity of the hitch point. Thereby, any inhibition to the accuracy of the hitch mechanism can be prevented, and the strict requirements of the hitch operation can be guaranteed. In this way, a movement without motion cusps is guaranteed from at least the root node 426c of the second tree 330 to the target state 156, that is, from the region near the hitch point to the hitch point.

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

[0144] As described with reference to FIGS. 3A and 3B, it will be understood that the feasibility of the first path 318 generated during the first stage is ensured by executing the BIAGT algorithm to discover the first path 318 as quickly as possible. Further, during the second stage, the path quality of the first path 318 is improved by imposing the maximum desired number of motion cusps as a hard constraint during node selection.

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

[0146] In one example, expandable nodes within the first tree 422 and / or the second tree 424 are identified based on the corresponding costs. Further, the expandable nodes are expanded by applying motion primitives and adding child nodes thereto. Such addition of child nodes is based on a determination of whether the child node introduces a number of motion cusps greater than a specified maximum number of motion cusps into the corresponding tree 422 or 424 and / or the associated path. In particular, for any node within the priority queue, when expanding the 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 allowed maximum number of motion cusps, such a node is removed from the priority queue without further expansion. For example, such addition of new nodes for path quality improvement continues until an end condition is met or a second path 428 with a minimum number of motion cusps is found. For example, the end condition may be set based on a time constraint. When the first tree 422 and / or the second tree 424 are expanded based on new nodes and / or previous nodes, the two trees 422 and 424 may be connected using a collision-free connection path having one of eight non-cusp RS patterns out of 48 RS patterns to form the second path 428. In particular, the above eight RS patterns are non-cusp path patterns.

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

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

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

[0150] Similarly, the target tree 424 may be expanded. When the start tree 422 and the goal tree 424 are close, i.e., when the distance between these trees is less than a predetermined threshold value, the goal tree 424 is connected to the start tree 422 to generate a path. The expansion of the start tree 422 and the target tree 424 may continue until the distance between the start tree 422 and the goal or the target tree 424 is less than the threshold value.

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

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

[0153] Thereafter, based on the second path 428, a reference trajectory 105 for controlling the movement of the vehicle 153 is generated. In particular, if the second path 428 is identified or generated before the end condition is satisfied, the reference trajectory 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, which is less than the first number of motion cusps associated with the first path 318. Thus, the reference trajectory 105 for the hitch operation based on the second path 428 may include a minimum number of motion cusps, i.e., may include a switch between the forward driving direction and the reverse driving direction.

[0154] According to some embodiments, the motion planner 108 may generate a reference trajectory 105 for controlling the movement of the vehicle 153 along either the first path 318 or the second path 428. The reference trajectory 105 may be generated as a function of time. For example, the reference trajectory 105 may indicate the type or pattern of motion and the corresponding amount of time during which the pattern of motion is to be executed. Thereafter, the reference trajectory 105 may be provided to a predictive controller 110 that generates a control input 111. The control input may include motion control commands, such as values of steering angle, acceleration, etc., 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 a stop at each motion cusp. In one example, the stop may be for 1 second, 2 seconds, 5 seconds, etc. In one example, the stop at the motion cusp can correspond to the physical time for gear shifting. Additionally, the stop at the motion cusp may improve the tracking performance when the predictive controller 110 is used.

[0156] FIG. 4C shows an example of a method for performing the creation of the graph 420 during the second stage, according to some embodiments. The second path 428 may be generated based on the first path 318 during the second stage.

[0157] At 432, the priority queue is reordered to integrate the hard constraints. In some embodiments, the hard constraint indicates the maximum number of kinematic singularities along the path from the root node of tree 422 or 424 to the first node. Since the number of kinematic singularities for each node is calculated and recorded in the first stage during the generation of the first path 318, the nodes 312a - 312i of graph 310 corresponding to the first path 318 in the priority queue are reordered. Such reordering of nodes 312a - 312i is based on two keys: the number of kinematic singularities 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 singularities have the highest priority to be selected (as node 426 for forming the second path 428) compared to nodes with the same number of singularities. For example, the node with the minimum cost and the minimum number of kinematic singularities is prioritized. In this way, the priority queue is reordered by increasing the priority of nodes with the fewest number of singularities and the lowest cost.

[0158] Once the priority queue is reordered, at 434, the best node, such as node 426b, is removed or selected. At 436, if the number of kinematic singularities included in the best node 426b is more than the maximum allowable kinematic singularity threshold, it may be identified that there is no way to improve the quality of the first path 318. 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. The node expansion may be performed by applying a motion primitive at the best node 426b.

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

[0160] At 442, if the node expansion of the best node 426b leads to a new path and this new path meets the cost target, use this new path as the output for generating the reference trajectory 105. According to one example, the cost target is associated with the total number of cusps in the new path. For example, the number of kinematic cusps in the new path (referred to as the second number of kinematic cusps) is calculated. If the second number of kinematic cusps in the new path is less than the first number of kinematic cusps in the first path 318, the new path is updated as the first path 318. Then, the updated first path is used to calculate the reference trajectory 105. In such a case, another best node may be retrieved for further processing. However, if the second number of kinematic cusps in the new path is less than the threshold of the maximum allowable number of kinematic cusps, the new path is recorded as the second path 428. Next, the second path 428 is output to generate the reference trajectory 105 and control the movement of the vehicle 153.

[0161] Figure 4D shows an example of a method for node expansion according to some embodiments. At 450, the best node 426b of the start tree 422 or the target tree 424 is retrieved. Given the best node 426b, at 452, it is determined whether the best node 426b of a given tree is close to the other tree. If it is close, at 454, an attempt may be made to connect the best node 426b to a specific node on the other tree within a specific distance using a specific type of motion primitive such as the motion primitive 430. If it is not close, at 456, another type of motion primitive such as the motion primitive 425 may be applied to expand the best node 426b and add a corresponding child node shown as the node 426d.

[0162] At 458, a determination is made to check whether the node expansion by the motion primitive 430 or 425 in step 454 or 456 has resulted in an edge connecting the best node 426b of a given tree to the other tree. If neither of the motion primitives 430 and 425 results in a new edge connecting both trees 422 and 424, the node expansion of the best node 426b is terminated. Thereafter, another node such as the child node 426d may be selected for expansion according to the steps of the method described in FIGS. 4A, 4B, 4C, and 4D. Otherwise, at 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 a smaller number of motion cusps than the first number of motion cusps of the first path 318, the new path is updated as the first path. Instead, if the new path generated using the new edge contains a smaller number of motion cusps than the threshold of the maximum number of allowable motion cusps, the new path may be recorded as the second path 428.

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

[0164] FIG. 5A shows an example of a type of motion primitive 425 according to some embodiments. For example, the motion primitive 425 is applied to a node to provide a collision-free child node. It will be understood that the motion primitive is a pre-computed motion that the vehicle 153 can execute. The motion primitive represents a motion by which the vehicle 153 can smoothly transition. For example, the motion primitive may be superimposed on a node of a graph, such as node 426b, to identify its child nodes.

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

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

[0167] The construction of a tree, such as the start tree 422 or the target tree 424, will be understood to include expanding 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 of the collision-free edges connects the best node to a child node.

[0168] According to this example, the node 504 may be the best node of the tree, and the motion planner 108 may expand this tree using motion primitives 425a (defined by {a1(t), t ∈ [0, t f1}) to 425i (a i (t), t ∈ [0, t fi ). The motion primitives 425a to 425i add collision-free edges 506a to 506i. The edges 506a to 506i can connect the node 504 to the corresponding child nodes 502a to 502i. According to some embodiments, the child nodes 502a to 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 the threshold of the maximum number of allowable motion cusps. In addition, the cost of the child node may be lower than the cost of the parent node 504. Such a child node may be added to the graph to generate a second path 428. Next, a particular child node may be selected as the next best node for expansion.

[0169] In one example, the cost of the child nodes corresponding to the states 502a to 502i may be calculated as the sum of the arrival cost and the estimated cost-to-go. The arrival cost represents the cost of driving the 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 the vehicle 153 from the target state (e.g., the state corresponding to the hitch point) to the child node 520.

[0170] FIG. 5B shows an example of the type of motion primitive 430 according to some embodiments. For example, the motion primitive 430 resolves the shortest distance path between any two states, such as X i and X j , without considering obstacles. In one example, the motion primitive 430 is applied to a state X of a tree, such as the start tree 422 i to connect the start tree to another tree, such as the target tree 424. In particular, when states X i and X j are close to each other, the motion primitive 430 is applied to connect the state X of the start tree 422 i to the X of the target tree j .

[0171] Reeds-Shepp has revealed that there are up to 48 solutions to connect any two states, and it will be understood that the 48 solutions allow the patterns shown in Table 510 of FIG. 5B. In this regard, "L" corresponds to a left turn, "S" corresponds to a straight line, and "R" corresponds to a right turn. The superscript characters "+", and "-" of "L", "S", and "R" mean forward movement and backward movement, respectively. The subscript indicates that vehicle 153 needs to follow a circle to change the vehicle's traveling direction angle. The line represents a motion cusp. Note that a motion cusp can indicate a change in the direction of movement, that is, a change from forward direction movement to reverse direction movement, or a change from reverse direction movement to forward direction movement. The motion primitive 430 between two states X i and X j represents the construction of the shortest distance path by one of the 48 patterns 430a

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

[0173] In another embodiment, the motion primitive 430 between states represents the construction of the shortest distance path in one of the first eight non-cusp patterns 430b.

[0174] Unlike the motion primitive 425, the motion primitive 430 is not predefined. Instead, only the pattern or structure of the path (first path or second path) is predefined. Pattern and structure are used interchangeably in the present invention.

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

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

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

[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 indicate the constraints in the physical movement of the corresponding vehicle around it. Further, the prediction controller 110 may be configured to generate an objective function 604 for the MPC prediction controller 110 using the reference trajectory 105 generated by the motion planner 108. In one example, the objective function 604 may indicate 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 indicate 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, the optimal control data of the objective function 604 and the equality and inequality constraints 602 may be used to solve the optimization problem. For example, based on the solution of the optimization problem, a control signal 111 may be generated. In one example, the type of optimization problem may depend on the dynamic system model 107 of the system 120, the system constraints 109, the estimated state 121 of the system 120, and the reference trajectory 105.

[0179] According to some embodiments, the prediction controller 110 may be configured to compute a control solution, such as a solution vector 606 that includes a sequence of future optimal control inputs 111 over the prediction time horizon 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 be in the form of an optimal control structured quadratic program 608.

[0180] In some embodiments, the solution to the optimization problem with inequality constraints, i.e., the optimal control structured quadratic program (QP) 608, uses the state and control values 610 over a prediction time horizon from previous control time steps that can be read from memory. In this way, techniques for warm-starting or hot-starting the optimization problem may be realized. This can significantly reduce the amount of computational effort required for 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 sub-optimal 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 trajectory 105, the predictive controller 110 may compute a control input 111 for controlling and tracking the movement of the vehicle 153 by solving the optimal control structured quadratic program QP608.

[0182]

Number

[0183] Assuming k = 0,..., N, the optimization variables in the optimal control structured QP 608 are the state variables x k and the control input variables u k According to some embodiments of the present disclosure, assuming k = 0,..., N, the dimensions of the state and control variables 610 need not be equal to each other at each discrete time point t k At each sampling time for the predictive controller 110, the optimal control structured QP 608 is formulated using the QP matrix 614 and the QP vector 616. Then, the optimal control structured 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. Further, a new control input 111 is generated based on solving the optimal control structured 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 reference tracking terms 618 that penalize the difference between the sequence 620 of predicted states and / or output values 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 speed and / or acceleration of the vehicle 153, the slip ratio or slip angle, the azimuth angle or angular velocity, the wheel speed, the force, and the torque.

[0187] In various embodiments, the penalty between the reference value corresponding to the reference trajectory 105 determined by the motion planner 108 and the 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. Additionally or alternatively, some embodiments add additional reference tracking terms considered by the prediction controller 110. Such additional reference tracking terms may be related to, for example, driving comfort, speed limits, energy consumption, pollution, etc. These embodiments balance the cost of reference tracking and the additional reference tracking terms.

[0188] According to one example, the additional reference tracking terms of the MPC may be defined 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 reference tracking terms, including the stage cost 622 and the terminal cost matrix 624, may include linear and / or quadratic penalties for one or more combinations of one or more state and / or control input variables. For example, the objective function in the constrained QP608 may include linear or quadratic penalties for longitudinal or lateral speed and / or acceleration of the vehicle, slip ratio or slip angle, yaw 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 the terminal cost matrix 624 are the matrix Q k in the QP matrix 614 k and S k and the gradient values q k in the QP vector 616 k and r

[0189]

Number

[0190]

Number

[0191]

Number

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

[0193] In this way, the equality constraints 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 the QP 608. By solving the QP 608, the prediction controller 110 can generate the value of the control input 111. For example, the value of the control input may include, for example, the longitudinal or lateral speed 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, the angular velocity, the wheel speed, the force, and the 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 is provided to the actuator of the vehicle 153, and the movement of the vehicle may be controlled based on the reference trajectory 105 corresponding to the first path 318 or the second path 428.

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

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

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

[0197] Other embodiments of the present disclosure can solve the non - convex optimal control structured QP 608 using an optimization algorithm for non - linear programming, such as sequential quadratic programming (SQP) or interior point method (IPM). Thereafter, for the optimization problem with inequality constraints at each sampling time of the MPC controller 110, a sub - optimal, locally optimal, or globally optimal control solution can be found.

[0198] [Number]

[0199] Assuming \(k = 0,\ldots,N\), the optimization variables in the optimal control structured NLP 640 are the state variables \(x\) k and the control input variables \(u\). k In some embodiments, assuming \(k = 0,\ldots,N\), the dimensions of the state and control variables 610 may not be equal to each other at each discrete time point \(t\) k . At each sampling time of the predictive controller 110, the optimal control structured NLP 640 is formulated using the reference and weighting matrices in the reference tracking cost 642, and 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 the next iteration. Further, a new control input 111 is generated based on solving the optimal control structured NLP 640.

[0200] [Number]

[0201] The objective function within the constrained optimal control structured NLP 640 that can be solved by the predictive controller 110 includes one or more linear and / or non-linear least squares reference tracking terms 646. The reference 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 the present disclosure, assuming \(k = 0,\ldots,N\), a sequence of weighting matrices \(W\) k is used for the least squares reference tracking terms 646. For this reason, each weighting matrix \(W\) kIt may be adapted in the control cost function 642 based on the reference trajectory 105. Let k = 0,..., N, then the output value y for use in the reference tracking term 646 k (x k , u k ) may be defined as any linear or non - linear function of the state and / or control input variables.

[0203]

Number

[0204] Embodiments of the present disclosure can define additional tracking terms of the MPC cost function 642 in the form of stage cost and / or terminal cost terms 648. These cost terms can all include any combination of linear functions, linear - quadratic functions, or non - linear functions. These additional objective terms can include penalties for functions of the state and / or control input variables. For example, the objective function 644 in the constrained optimal control structured NLP 640 can include linear, quadratic, or non - linear penalties for the longitudinal or lateral speed and / or acceleration of the vehicle, slip ratio or slip angle, yaw angle or angular velocity, wheel speed, force, torque, or any combination of such quantities.

[0205]

Number

[0206] Some embodiments of the present disclosure are based on the recognition that a discrete-time dynamic model 650 for predicting the behavior of 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 a numerical simulation routine 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 speed, acceleration, position and / or orientation of vehicle 153 relative to its surroundings, slip ratio or slip angle, azimuth angle or angular velocity, wheel speed, force, and / or torque. For example, an obstacle avoidance constraint may be implemented in the non-linear prediction controller 110 by defining a set of one or more inequality constraints on a linear or non-linear function of the predicted position, speed, and orientation of vehicle 153 relative to the predicted positions, speeds, and orientations of one or more obstacles 158a and 158b in the surrounding environment of vehicle 153.

[0209]

Number

[0210] Some embodiments of the present disclosure are based on an adjusted optimization algorithm for efficiently solving the constrained optimal control structured NLP 640 at each sampling time of the non-linear predictive controller 110. Such an optimization algorithm can find a solution vector 606 that is either feasible and globally optimal, feasible but locally optimal, or feasible but quasi-optimal with respect to the constraints, or an iterative optimization algorithm can 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, variants of the interior point method and variants of the sequential quadratic programming (SQP) method.

[0211] In particular, some embodiments of the present disclosure use a real-time iteration (RTI) algorithm, which is an online variant of the sequential quadratic programming method (SQP), in combination with a quasi-Newton or generalized Gauss-Newton type positive semi-definite Hessian approximation to solve at least one convex block-sparse QP approximation at each sampling time of the non-linear predictive controller 110.

[0212] Each iteration of the RTI consists of two steps. In the first step, corresponding to the preparation stage, the system dynamics are discretized and linearized, the remaining constraint functions are linearized, and the quadratic objective approximation is evaluated to construct the optimal control structured QP subproblem. In the second step, corresponding to the feedback stage, 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 the present disclosure, the block-sparse optimal control structure in the Hessian and constraint Jacobian matrices may be utilized in one or more of the linear algebra operations of the optimization algorithm to reduce the execution time and memory footprint of the NLP optimization algorithm by attempting to reduce the computational complexity.

[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 This makes it 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 an example, 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 the iterative solver within a primal - active - set algorithm with a dedicated warm - start. Combined with the C code generated by CasADi, the PREAS solver provides an efficient and reliable NMPC solver for solving the optimal control structure NLP640 problem suitable for embedded system platforms.

[0228] Based on the control input 111 generated by the prediction controller 110, the vehicle 153 can be controlled to execute a hitch operation by solving the optimal control structured NLP 640 problem.

[0229] FIG. 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. For this reason, the reference trajectory 105 may include one or more motion cusps, i.e., a transition from a forward motion 702 to a reverse motion 704 or a transition from a reverse motion 704 to a forward motion 702. Some embodiments of the present disclosure are based on the recognition that, due to the time delay period 714 of the gearshift mechanism in the controlled system, the reference trajectory 105 must include a sufficiently large pause period 710 at each of the one or more motion cusps so that the prediction controller 110 can command a gear change with a sufficiently large preview period 712.

[0230] For example, FIG. 7A shows a graph of the reference trajectory 105 versus time 708, including a forward motion 702 with a positive reference speed value, a subsequent stationary pause with a zero reference speed value 706, and a subsequent reverse motion 704 with a negative reference speed value. The pause period 710 is larger than the preview period 712 for the prediction controller 110 to command a gear change. The preview period 712 is larger than the time delay period 714 of the gearshift mechanism in the controlled vehicle 153.

[0231] In particular, the BIAGT motion planning algorithm is modified to introduce a pause at each motion cusp, i.e., at each switch between the forward motion 702 and the reverse motion 704 of the vehicle 153, so as to reach the hitch point at the end of the maneuver. The pause period 710, Δt, during which the vehicle 153 is planned to be stationary at the zero reference speed value 706 pause before the vehicle 153 begins to move in the opposite direction, the preview period 712, Δt previewGear shifting is then started. 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, where 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 the forward motion 702 or the 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 in the forward motion 702, the 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 by solving an optimization problem with constraints for predictive reference tracking at 726 to generate the control input 111 at the current sampling time step t. For example, the objective and constraint function 644 may be related to optimization problems of the prediction controller 110, such as the optimal control structured QP608 and the optimal control structured NLP640. Thus, the presence of the vehicle reverse movement 704 following the vehicle forward movement 702 may correspond to a kinematic cusp. i This may be achieved, for example, by the prediction controller 110 solving an optimization problem with constraints for predictive reference tracking at the current sampling time step t to generate the control input 111. For example, the objective and constraint function 644 may be related to optimization problems of the prediction controller 110, such as the optimal control structured QP608 and the optimal control structured NLP640. Thus, the presence of the vehicle reverse movement 704 following the vehicle forward movement 702 may correspond to a kinematic cusp.

[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 achievable 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, some embodiments are based on the recognition that the hitch operation is performed in an environment that is mostly closed, i.e., an environment 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 current state configuration 808 of the vehicle and / or the safety areas (shown as safety sets 802, 804, and 806) around the predicted state configurations 810, 812 within the environment of vehicle 153. Note that during the operation, vehicle 153 may move from the current state configuration 808 to the predicted state configurations 810, 812.

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

[0242] Some embodiments of the present disclosure are based on the recognition that a safety tube with a volume that changes over time can be constructed around the reference trajectory 105 that vehicle 153 follows for the operation. Additionally, the AEB system may always be activated when 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 detection devices 165 can be used for the accurate execution of an automatic hitch operation based on the accurate detection of the current state and the accurate detection of dynamic obstacles within the environment or safety sets 802, 804, 806 of the controlled vehicle 153. For example, one or more RSUs 165 can include one or more sensors, such as distance rangefinders, radars, LIDARs, and / or cameras, as well as sensor fusion techniques, to accurately detect the state of vehicles and the state of dynamic obstacles within the environment of the controlled system. In some embodiments of the present invention, the communication network 160 may be used for real-time communication between the vehicle 153 and one or more RSUs or infrastructure detection devices 165.

[0244] In one example, when a dynamic obstacle is detected using sensors mounted on the vehicle 153 and / or optionally by communication from one or more RSUs 165, the AEB system is activated. Thus, if there is a dynamic obstacle within the combination of safety sets 802, 804, 806 surrounding the current state configuration 808 or predicted state configurations 810, 812 of the vehicle 153, the prediction controller 110 is interrupted and the AEB system starts to execute a braking operation.

[0245] When the dynamic obstacle moves outside the combination of safety sets 802, 804, 806, the prediction controller is reinitialized and the automatic hitch operation continues. Optionally, since the vehicle 153 may execute the hitch operation at a slow speed, the same motion plan, i.e., the reference trajectory, may be reused to complete the operation, and it may not be necessary to execute the BIAGT algorithm after such an interruption. However, in some cases, it may be necessary to quickly generate a new motion plan or replan a new reference trajectory, and the obstacle avoidance constraints in the optimization problem formulation may change.

[0246] FIG. 8B shows an example of a method for re-initializing the prediction controller 110 according to some embodiments. For example, the movement of the vehicle 153 and the tracking of the prediction controller 110 may be interrupted by a dynamic obstacle. In some cases, it may not be possible to return to the old reference trajectory, for example, when the dynamic obstacle has not moved for a long time or due to changes in the environment of the vehicle 153 caused by the dynamic obstacle.

[0247] At 820, the current state configuration 808 and information about the surroundings of the vehicle 153 are tracked. For example, the surroundings of the vehicle may correspond to the safety sets 802, 804, 806. In one example, based on the tracking of the vehicle 153, the prediction controller 110 may use a detection mechanism to request information for determining whether it is safe to perform an operation such as a hitch operation at each sampling time step t i . For example, the detection mechanism may include using the RSU 165 or one or more other in-vehicle sensors to detect the current state 808 of the vehicle 153 and the surroundings of the vehicle 153.

[0248] At 822, based on the current state 808 and environment of the vehicle 153 and the current reference trajectory 105, a determination is made as to whether each of one or more safety checks associated with the maneuver is satisfied.

[0249] If the safety check is satisfied, at 720, the prediction controller 110 may be implemented by solving a constrained optimization problem for predictive reference tracking of the vehicle 153 based on the reference trajectory 105 and the estimated state 121 at the sampling time step t i . The prediction controller 110 can generate a control input 111 for controlling the vehicle 153 to perform a hitch operation.

[0250] However, if the safety inspection is not satisfied at 822, the automatic emergency braking (AEB) system is activated. At 824, the AEB system is activated to safely stop vehicle 153. In this regard, the AEB control signal is calculated at 826. The generated AEB control signal is applied to the actuator of vehicle 153 to stop vehicle 153. If one or more of the safety inspections are not satisfied, at 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 the predicted state configurations 810 and 812 of vehicle 153, the AEB system may be activated.

[0251] Some embodiments of the present disclosure are based on the recognition that the AEB system continues its execution as long as one or more of the safety inspections are not satisfied at 828.

[0252] Furthermore, at 830, the state and control trajectory re-initialization steps of the prediction controller 110 are executed when it becomes safe to execute the operation again. For example, to ensure a relatively smooth transition after AEB is switched back to the hitch operation, the prediction controller 110 may calculate the closest point within the reference trajectory 105 with respect to the current state configuration 808 of vehicle 153. Thereafter, the prediction controller 110 starts tracking vehicle 153 in the reference trajectory 105 from the current state configuration 808 of vehicle 153, and an updated reference trajectory of the prediction controller 110 can be obtained. For example, the reference trajectory 105 is set to the current state configuration 808 of vehicle 153 with a reference speed value of zero. Thereafter, a new reference state is time-shifted within the horizon considering that the vehicle reaches the reference speed from rest after braking.

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

[0254] The system 900 may include one or a combination of 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 operatively coupled to other components via a connection 920. The connection 920 may include a bus, a line, a fiber, a link, or a combination thereof.

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

[0256] In some embodiments, the sensor 910 can 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, the sensor 910 can convert an optical image into an electronic or digital image and transmit the acquired image to the processor 950. Additionally or alternatively, the image sensor can detect light reflected from a target object in a scene and provide the intensity of the captured light to the processor 950.

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

[0258] In some embodiments, the processor 950 can also receive inputs from the IMU 930. In other embodiments, the IMU 930 can include a three-axis accelerometer, a three-axis gyroscope, and / or a magnetometer. The IMU 930 can provide velocity, orientation, and / or other position-related information to the processor 950. In some embodiments, the IMU 930 can output the measured information in synchronization with the capture 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] The system 900 can 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 captured by the sensor 910, fused images, augmented reality (AR) images, graphical user interfaces (GUIs), motion control instructions, and other program outputs. In some embodiments, the display 980 includes a touch screen and / or can be housed with a touch screen that enables a user to input data through any combination of a virtual keyboard, icons, menus, or other GUIs, user gestures, and / or input devices such as a stylus and other writing utensils. In some embodiments, the display 980 can be implemented using a liquid crystal display (LCD) display, or a light-emitting diode (LED) display such as an organic light-emitting diode (OLED) display. In other embodiments, the display 980 can be a wearable display. In some embodiments, the result of the fusion may be rendered on the display 980 or provided to different applications that may be internal or external to the system 900.

[0260] As an example, system 900 may be modified in various ways to be consistent with the present disclosure, such as by adding, combining, or omitting one or more of the illustrated functional blocks. For example, in some configurations, system 900 does not include IMU 930 or transceiver 970. Further, in some exemplary implementations, system 900 includes various other sensors (not shown) such as ambient light sensors, microphones, acoustic sensors, ultrasonic sensors, laser rangefinders, and the like. In some embodiments, a portion of system 900 takes the form of one or more chip sets.

[0261] Processor 950 can be implemented using a combination of hardware, firmware, and software. Processor 950 can represent one or more circuits configurable to execute at least a portion of a computational procedure or process related to sensor fusion and / or a method for further processing the fused measurements. Processor 950 fetches instructions and / or data from memory 1060. 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 devices (DSPDs), programmable logic devices (PLDs), field programmable gate arrays (FPGAs), controllers, microcontrollers, microprocessors, embedded processor cores, electronic devices, other electronic units designed to perform the functions described herein, or combinations thereof.

[0262] Memory 960 may be implemented within processor 950 and / or external to 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 kind of memory or number of memories, or the type of physical medium on which the memories are stored. In some embodiments, memory 960 holds program code that facilitates the automatic parking or motion planning of tractor-trailer-based vehicle 902.

[0263] For example, memory 960 can store sensor measurements such as still images, depth information, video frames, program results, etc., as well as data provided by IMU 930 and other sensors. Memory 960 can store a memory that stores the geometric shape of vehicle 902, a map of the environment in which the vehicle's operations are performed, a kinematic model of vehicle 902, and a dynamic system model of vehicle 902. Generally, memory 960 can represent any data storage mechanism. Memory 960 can include, for example, primary memory and / or secondary memory. Primary memory can include, for example, random access memory, read-only memory, etc. Although shown as separate from processor 950 in FIG. 9, it should be understood that all or part of the primary memory may be provided inside processor 950, or else in the same location as processor 950, and / or coupled to processor 950.

[0264] The secondary memory can include, for example, memory of the same or a similar type as the primary memory and / or one or more data storage devices or systems, such as flash / USB memory drives, memory card drives, disk drives, optical disk drives, tape drives, solid state drives, hybrid drives, etc. In some implementations, the secondary memory can operatively receive a non-transitory computer-readable medium within a removable media drive (not shown) or, alternatively, can be configured with respect to a non-transitory computer-readable medium. In some embodiments, the non-transitory computer-readable medium forms part of the memory 960 and / or the processor 950.

[0265] The above embodiments of the present invention can be realized in any of a number of ways. For example, the embodiments may be realized using hardware, software, or a combination thereof. When realized in software, the software code can be provided on a single computer or distributed among a plurality of computers and can be executed on any suitable processor or collection of processors. Such processors may be realized as an integrated circuit having one or more processors as components of the integrated circuit. However, the processors may be realized using circuitry in any suitable format.

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

[0267] In a claim, terms that denote an order, such as "first" and "second," which modify an element of a claim, do not themselves imply any superiority, precedence, or order of one element of a claim over another element, or any temporal order of performing acts of a method. Rather, they are used merely as labels to distinguish one element of a claim having a particular name (where an order-denoting term is not used) from another element of the same claim having the same name, to distinguish the elements of the claim.

[0268] Although the present disclosure has been described with reference to exemplary embodiments, it should be understood that various other adaptations and modifications can be made within the spirit and scope of the present invention.

[0269] Accordingly, the aim of the appended claims is to cover all such variations and modifications as fall within the true spirit and scope of the present invention.

Claims

1. A control system for controlling the movement of a vehicle using a motion planner, the control system comprising: at least one processor; and a memory storing instructions, the instructions causing the at least one processor of the control system to: create a graph having a plurality of nodes defining the state of the vehicle, the plurality of nodes of the graph including an initial node defining the initial state of the vehicle and a target node defining the target state of the vehicle, each pair of nodes in the graph being connected by 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 including a number of 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 series of motion primitives for moving the vehicle from the initial state to the target state; execute determining a first number of motion cusps in the series of motion primitives of the first path, each motion cusp of the first number of motion cusps indicating a switch between forward and backward motion in the series of motion primitives; when it is determined that the first number of motion cusps exceeds a threshold, execute expanding the graph to add new nodes until an end condition is met, the expansion of the graph being subject to a constraint associated with the total number of motion cusps, the graph being expanded to form a second path connecting the initial node to the target node, the second path having a second number of motion cusps less than the first number of motion cusps; execute controlling the movement of the vehicle based on the second path. A control system.

2. The instructions further cause the at least one processor of the control system to: execute the expansion of the graph until the end condition is met; The control system according to claim 1, wherein when the second path is formed before the end condition is satisfied, the movement of the vehicle is controlled based on the second path.

3. The command further causes the at least one processor of the control system to control the movement of the vehicle based on the first path when the second path is not formed until the end condition is satisfied, the control system according to claim 2.

4. The command further causes the at least one processor of the control system to compare the first number of the kinematic cusps with the threshold value, and when the first number of the kinematic cusps in the first path exceeds the threshold value, expand the graph to add the new node, the control system according to claim 1.

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

6. The command further causes the at least one processor of the control system to generate a trajectory for the movement of the vehicle along at least one of the first path or the second path as a function of time, and generate a control command for the vehicle to follow the trajectory, the control system according to claim 1.

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

8. The command further causes the at least one processor to determine a safety region for the movement of the vehicle based on the generated trajectory, the safety region including one or more safety sets along the trajectory, check for dynamic obstacles within the safety region, and when the dynamic obstacle within the safety region is identified, cause the automatic emergency braking system to be activated and perform a braking operation to stop the movement of the vehicle along the trajectory, the control system according to claim 6.

9. The command further causes the at least one processor of the control system to create the graph forming the first path by repeatedly expanding a node among the plurality of nodes to create a new node or by connecting two existing nodes among the plurality of nodes, the control system according to claim 1.

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

11. The command further causes the at least one processor to generate equality and inequality constraint functions for the model predictive controller, the model predictive controller having non-linear constraints and a non-linear system model, generate an objective function for the model predictive controller, the objective function indicating the initial state and the target state of the vehicle, and use one or more iterations of sequential quadratic programming to solve an optimization problem associated with the model predictive controller based on the equality and inequality constraint functions and the objective function, the control system according to claim 10.

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

13. To form at least one of the first path or the second path, the instruction further causes the at least one processor of the control system to create a first tree of a first set of nodes starting from the initial node; create a second tree of a second set of nodes starting from the target node; connect the first tree to the second tree by connecting a node among the nodes of the first set to another node among the nodes of the second set. The control system according to claim 1.

14. The instruction further causes the at least one processor of the control system to form the first path by connecting the first tree to the second tree using a collision-free connection path having one of 48 Reeds-Shepp patterns; form the second path by connecting the first tree to the second tree using a collision-free connection path having one of eight Reeds-Shepp patterns out of the 48 Reeds-Shepp patterns. The eight Reeds-Shepp patterns are cusp-free. The control system according to claim 13.

15. To form at least one of the first path or the second path, the instruction further causes the at least one processor of the control system to select an expandable node based on the cost associated with the expandable node from at least one of a first node of the first tree or a second node of the second tree. Adding a child node connected to the expandable node by an edge defined by a collision-free motion primitive and making the cost of the child node less than the cost of the expandable node to expand the graph, where the cost of the child node is the minimum cost to reach the target node from the initial node through the child node, and the cost of the child node includes a first cost of an initial path through the first set of nodes, a second cost of a target path through the second set of nodes, and a third cost of a connection path between the first set of nodes and the second set of nodes. The control system according to claim 13.

16. The instructions further cause 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 series of motion primitives between a root node and a corresponding expandable node includes a number of motion cusps greater than a predetermined threshold. The control system according to claim 15.

17. The instructions further cause the at least one processor of the control system to select the expandable node for forming the first path without the constraint associated with the number of the motion cusps, and select the expandable node for forming the second path, where the expansion of the graph is subject to a constraint associated with the total number of the motion cusps. The control system according to claim 15.

18. The instructions further cause the at least one processor of the control system to initialize the second tree with a root node spaced from the target node by a distance having a non-cusped movement to the target node. The control system according to claim 13.

19. The root node is connected to the target node with an edge defining a linear motion. The control system according to claim 18.

20. A method for controlling an entity, the method comprising: creating a graph having a plurality of nodes defining a state of a vehicle, the plurality of nodes of the graph including an initial node defining an initial state of the vehicle and a target node defining a target state of the vehicle, each pair of nodes in the graph being connected by 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 including some motion cusps, and the plurality of nodes connected through the corresponding edge forming a first path through the graph that connects the initial node to the target node by a series of motion primitives for moving the vehicle from the initial state to the target state, determining a first number of motion cusps in the series of motion primitives of the first path, each motion cusp of the first number of motion cusps indicating 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, expanding the graph to add new nodes until an end condition is met, the expansion of the graph being subject to a constraint associated with the total number of motion cusps, the graph being expanded to form a second path connecting the initial node to the target node, the second path having a second number of motion cusps less than the first number of motion cusps, A method comprising controlling the movement of the vehicle based on the second path.

21. The method further comprises: performing the expansion of the graph until the end condition is met; and when the second path is formed before the end condition is met, controlling the movement of the vehicle based on the second path, or when the second path is not formed until the end condition is met, controlling the movement of the vehicle based on the first path, the method according to claim 20.

22. To form at least one of the first path or the second path, the method further comprises: 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 target node; connecting the first tree to the second tree by connecting a node among the first set of nodes to another node among the second set of nodes; causing the vehicle to follow the trajectory by a control command for the vehicle. The method according to claim 20.

23. To form at least one of the first path or the second path, the method further comprises: selecting an expandable node based on a cost associated with the expandable node from at least one of a first node of the first tree or a second node of the second tree; expanding the graph by adding a child node connected to the expandable node by an edge defined by a collision-free motion primitive, such that a cost of the child node is less than a cost of the expandable node, wherein the cost of the child node is a minimum cost for reaching the target node from the initial node through the child node, and the cost of the child node includes a first cost of an initial path passing through the first set of nodes, a second cost of a target path passing through the second set of nodes, and a third cost of a connection path between the first set of nodes and the second set of nodes. The method according to claim 22.

24. A non-transitory computer-readable storage medium having a program executable by a processor for executing a method, the method comprising: Creating a graph having a plurality of nodes defining a state of a vehicle, the plurality of nodes of the graph including an initial node defining an initial state of the vehicle and a target node defining a target state of the vehicle, each pair of nodes in the graph being connected by 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 including a plurality of motion cusps, the plurality of nodes connected through the corresponding edge forming a first path through the graph that connects the initial node to the target node by a series of motion primitives for moving the vehicle from the initial state to the target state. Determining a first number of motion cusps in the series of motion primitives of the first path, each motion cusp of the first number of motion cusps indicating a switch between forward and backward motion in the series of motion primitives. When it is determined that the first number of motion cusps exceeds a threshold, expanding the graph to add new nodes until an end condition is met, the expansion of the graph being subject to a constraint associated with the total number of motion cusps, the graph being expanded to form a second path connecting the initial node to the target node, the second path having a second number of motion cusps less than the first number of motion cusps. A non-transitory computer-readable storage medium including controlling movement of the vehicle based on the second path.

Images