SYSTEM AND METHOD FOR PARKING AN AUTONOMOUSLY DRIVEN VEHICLE IN A DYNAMIC PARKING ENVIRONMENT - Patent application

Abstract

The present disclosure provides a system and method for parking an autonomous vehicle in a dynamic environment of a parking lot. The method comprises collecting measurements of a state of the dynamic environment, a state of one or more stationary vehicles in the parking lot, and a state of one or more moving obstacle vehicles. The method further comprises executing a path planner configured to generate a trajectory based on the state of the dynamic environment, and executing an environment prediction device configured to predict a path and a motion mode of each of the obstacle vehicles. The method further comprises determining a safety constraint for each of the obstacle vehicles based on the path and the motion mode of each of the obstacle vehicles, and parking the autonomous vehicle based on the parking trajectory and the safety constraint for each of the obstacle vehicles.

Description

[Technical field]

[0001] The present disclosure relates generally to motion planning for parking of autonomous vehicles, and more particularly to a system and method for parking an autonomous vehicle in a dynamic environment of a parking lot. [Background technology]

[0002] Some parking systems are used in autonomous vehicles to automatically park the autonomous vehicle at a parking spot in a parking lot. The parking task can be formulated as determining a path or motion of the autonomous vehicle from an initial state to a goal state that defines a parking spot, and then controlling the actuators of the autonomous vehicle, such as the vehicle's accelerator pedal and steering wheel, to ensure that the autonomous vehicle follows the determined path or motion. Summary of the Invention [Problem to be solved by the invention]

[0003] However, parking lots may contain obstacles, such as moving obstacle vehicles or pedestrians, that the autonomous vehicle must not collide with while parking in the parking lot. Furthermore, autonomous parking may involve motion planning in a narrow space. Furthermore, in contrast to driving on roads or highways, vehicle motion in parking lots does not have a clear set of rules to follow and is highly dependent on the driver's intention or even skill. For these reasons, motion prediction of obstacle vehicles and motion planning of autonomous vehicles for parking are difficult.

[0004] Motion prediction, i.e. predicting the future state of the obstructed vehicle, is crucial to determine the safety constraints that the planning and control modules of the autonomous vehicle must take into account, thus affecting the feasibility and smoothness of the motion plan or path. In particular, accurate short-term motion prediction allows the autonomous vehicle to safely plan and react to the autonomous vehicle, while long-term planning / mode prediction allows the autonomous vehicle to plan efficiently and smoothly. Some approaches use interacting multiple models (IMMs) to predict the short-term trajectory during parking, while others use classifiers that extract information from the motion of the obstructed vehicle to generate short-term motion predictions. However, such data-driven methods are undesirable because their performance is not stable and guaranteed with different datasets, and a poor selection of the dataset can lead to large prediction errors.

[0005] Moreover, general motion planning algorithms are not directly suitable for parking in the presence of obstacles, which requires rapid replanning for complex driving maneuvers, while motion planners specialized for autonomous parking are unable to integrate short-term and long-term range planning or adequately incorporate online path repair when new obstacles appear in the parking lot.

[0006] Therefore, there remains a need for a system and method for parking an autonomous vehicle in the dynamic environment of a parking lot. [Means for solving the problem]

[0007] It is an object of some embodiments to provide a system and method for parking an autonomous vehicle in a dynamic environment of a parking lot. It is further an object of some embodiments to provide a strategic motion planner that changes a control strategy of the autonomous vehicle based on different conditions in the parking lot to park the autonomous vehicle. The parking lot includes parking locations for parking the vehicle. The parking lot may further include stationary vehicles parked in the parking lot and disabled vehicles moving within the parking lot.

[0008] An objective of some embodiments is to park an autonomous vehicle in such a dynamic environment of a parking lot without colliding with stationary vehicles, obstacle vehicles, and other obstacles that may be present in the parking lot. For example, the autonomous vehicle has an initial state and needs to park in a parking location defined by a goal state without colliding with stationary and obstacle vehicles. Each state (e.g., the initial state and the goal state) defines at least a position (x, y) and an orientation θ of the autonomous vehicle in a two-dimensional plane.

[0009] To achieve such a goal, in some embodiments, measurements are first collected of a dynamic environment state in the parking lot indicating available parking locations in the parking lot, the state of each stationary vehicle, and the state of the obstructed vehicle. In one embodiment, the state of the obstructed vehicle may include the position and orientation of the obstructed vehicle, and its geometric information, such as its wheelbase. Further, a path planner is executed to generate a trajectory for parking the autonomous vehicle based on the dynamic environment state in the parking lot.

[0010] Some embodiments are based on the recognition that in order to realize collision-free parking of an autonomous vehicle in a parking lot, the motion of an obstacle vehicle needs to be taken into consideration, and therefore some embodiments aim to predict the path of the obstacle vehicle.

[0011] However, some embodiments are based on the recognition that, considering the dynamic environment of a parking lot, predicting the path of the obstructed vehicle is not enough, and it is also useful to determine the motion mode of the obstructed vehicle. Some embodiments are based on the observation that the obstructed vehicle runs in various modes in the parking lot, such as a cruising mode and a maneuvering mode. In the cruising mode, the steering angle of the obstructed vehicle is small or stable. When the obstructed vehicle enters the parking lot and is approaching a parking spot, or when the obstructed vehicle exits the parking lot, the obstructed vehicle runs in the cruising mode. In the maneuvering mode, the obstructed vehicle frequently changes the steering angle, deviates from the path to park, or exits from a narrow parking spot.

[0012] Therefore, an objective of some embodiments is to predict both the path and the motion mode of the obstructed vehicle. To predict the path and the motion mode of the obstructed vehicle, an environment prediction device configured to predict the path and the motion mode of the obstructed vehicle is executed.

[0013] Furthermore, the safety constraint of the obstacle vehicle is determined based on the predicted path and the motion mode of the obstacle vehicle. For example, when the predicted mode is a cruising mode, the safety constraint is determined around the state of the obstacle vehicle along the path. The safety constraint determined around the state of the obstacle vehicle along the path is called a safety margin. In the cruising mode, the obstacle vehicle has less active steering actions and the motion of the obstacle vehicle can be predicted with relatively high reliability, so the safety margin is determined as the safety constraint of the cruising mode. However, in the steering mode, it is difficult to predict the motion of the obstacle vehicle, and the obstacle vehicle tends to occupy a larger space to adjust its orientation. Therefore, in the steering mode, the safety constraint is determined as a safety range in all driving directions around the state of the obstacle vehicle. Therefore, when the predicted mode is the steering mode, a safety range is determined for the obstacle vehicle. The safety range defines an area that the autonomously driven vehicle should avoid when the obstacle vehicle is in the steering mode.

[0014] Furthermore, the autonomous vehicle is parked by controlling the autonomous vehicle based on the trajectory and the determined safety constraints.

[0015] Some embodiments are based on the recognition that the motion of an autonomous vehicle in a parking lot may be constantly adapted according to changes in the parking lot environment. Thus, even after the trajectory of the autonomous vehicle is determined, the need to adjust the trajectory may arise from time to time. Therefore, to control the parking of the autonomous vehicle, some embodiments use a strategic motion planner that changes the control strategy based on different conditions in the parking lot.

[0016] For example, in some embodiments, the strategic motion planner uses three different strategies based on different situations. The strategic motion planner includes a model predictive control-based safety controller for trajectory tracking, a search-based evacuation planner for finding an avoidance path in emergency, and an optimization-based repair planner for correcting the trajectory path. The strategic motion planner balances safety, plan feasibility, and smooth maneuvering by leveraging the advantages of the optimization-based approach and the search-based approach.

[0017] Thus, one embodiment discloses an integrated system for parking an autonomous vehicle in a dynamic environment of a parking lot. The integrated system includes a processor and a memory having instructions stored thereon, which, when executed by the processor, cause the integrated system to collect measurements of a dynamic environmental state in the parking lot indicating available parking locations in the parking lot, a state of one or more stationary vehicles parked in the parking lot, and a state of one or more obstructing vehicles moving in the parking lot, execute a route planner configured to generate a trajectory for parking the autonomously driven vehicle based on the dynamic environmental state in the parking lot, execute an environment prediction device configured to predict a path and a motion mode including a first motion mode or a second motion mode of each of the obstructing vehicles, determine safety constraints for each of the obstructing vehicles based on the path and motion mode of each of the obstructing vehicles, where if the obstructing vehicle is in the first motion mode, the safety constraints are determined along the path around the state of the obstructing vehicle, and if the obstructing vehicle is in the second motion mode, the safety constraints are determined in all driving directions around the state of the obstructing vehicle, and the instructions further cause the integrated system to park the autonomously driven vehicle based on the trajectory for parking and the safety constraints of each of the obstructing vehicles.

[0018] Accordingly, another embodiment discloses a method for parking an autonomous vehicle in a dynamic environment of a parking lot, the method comprising: collecting measurements of a state of a dynamic environment in the parking lot indicating available parking locations in the parking lot, a state of one or more stationary vehicles parked in the parking lot, and a state of one or more obstacle vehicles moving in the parking lot, executing a path planner configured to generate a trajectory for parking the autonomous vehicle based on the state of the dynamic environment in the parking lot, executing an environment prediction device configured to predict a path and a motion mode including a first motion mode or a second motion mode of each of the obstacle vehicles, and determining a safety constraint for each of the obstacle vehicles based on the path and motion mode of each of the obstacle vehicles, where if the obstacle vehicle is in the first motion mode, the safety constraint is determined along the path around the state of the obstacle vehicle, and if the obstacle vehicle is in the second motion mode, the safety constraint is determined in all driving directions around the state of the obstacle vehicle, the method further comprising parking the autonomous vehicle based on the trajectory for parking and the safety constraint for each of the obstacle vehicles.

[0019] Accordingly, yet another embodiment discloses a non-transitory computer-readable storage medium having embodied thereon a program executable by a processor to perform a method for parking an autonomous vehicle in a dynamic environment of a parking lot. The method comprises collecting measurements of a dynamic environmental state in the parking lot indicating available parking locations in the parking lot, a state of one or more stationary vehicles parked in the parking lot, and a state of one or more obstacle vehicles moving in the parking lot; executing a route planner configured to generate a trajectory for parking the autonomously driven vehicle based on the dynamic environmental state in the parking lot; executing an environment prediction device configured to predict a path and a motion mode including a first motion mode or a second motion mode of each of the obstacle vehicles; and determining safety constraints for each of the obstacle vehicles based on the path and motion mode of each of the obstacle vehicles, where if the obstacle vehicle is in the first motion mode, the safety constraints are determined along the path around the state of the obstacle vehicle, and if the obstacle vehicle is in the second motion mode, the safety constraints are determined in all driving directions around the state of the obstacle vehicle, and the method further comprises storing instructions to park the autonomously driven vehicle based on the trajectory for parking and the safety constraints of each of the obstacle vehicles.

[0020] The presently disclosed embodiments are further described with reference to the accompanying drawings. The illustrated drawings are not necessarily to scale, with emphasis generally being placed upon illustrating the principles of the presently disclosed embodiments. [Brief description of the drawings]

[0021] [Figure 1A] FIG. 1 illustrates an example of a parking lot in accordance with some embodiments of the present disclosure. [Figure 1B] FIG. 1 is a block diagram illustrating a system for parking an autonomous vehicle according to one embodiment of the present disclosure. [Figure 1C] FIG. 1 is a block diagram illustrating a method for parking an autonomous vehicle in a dynamic environment of a parking lot according to one embodiment of the disclosure. [Figure 1D]FIG. 2 is a schematic diagram illustrating different routes a disabled vehicle may traverse to exit a parking lot, according to one embodiment of the present disclosure. [Figure 1E] FIG. 2 is a schematic diagram illustrating a safety margin determined around an obstructing vehicle condition along a route according to one embodiment of the present disclosure. [Figure 1F] FIG. 1 is a schematic diagram illustrating a safety range according to an embodiment of the present disclosure. [Diagram 2] 1 is a schematic diagram illustrating an environment prediction device according to an embodiment of the present disclosure. [Figure 3A] FIG. 2 is a schematic diagram illustrating a motion estimator based on a multi-stage estimation framework, according to an embodiment of the present disclosure. [Figure 3B] FIG. 1 is a schematic diagram for dynamic model based motion estimation according to an embodiment of the present disclosure. [Figure 3C] FIG. 2 is a schematic diagram illustrating a predicted path of an obstructing vehicle using different models according to an embodiment of the present disclosure. [Figure 3D] FIG. 13 is a diagram showing an example of a short-term motion prediction result using a bicycle model and an example of a short-term motion prediction result using a unicycle model according to an embodiment of the present disclosure. [Figure 4A] FIG. 2 is a schematic diagram illustrating a long-term mode predictor for estimating the probability of each mode according to an embodiment of the present disclosure. [Figure 4B] FIG. 1 is a schematic diagram for developing a cost map according to an embodiment of the present disclosure. [Diagram 5] FIG. 1 is a block diagram for determining safety constraints and safety sets according to an embodiment of the present disclosure. [Figure 6] FIG. 1 is a block diagram illustrating a motion prediction and planning framework according to an embodiment of the present disclosure. [Figure 7A] FIG. 1 illustrates an example scenario in which a safety constraint is violated and an evacuation planner is activated, according to an embodiment of the present disclosure. [Figure 7B] FIG. 1 is a block diagram illustrating a method for an evacuation planner to determine a path between a current location and a safe point for evacuation according to one embodiment of the present disclosure. [Figure 7C]FIG. 1 is a schematic diagram illustrating evaluation of node costs according to an embodiment of the present disclosure. [Figure 7D] FIG. 13 is a schematic diagram illustrating an evacuation planner process according to an embodiment of the present disclosure. [Figure 8A] FIG. 1 illustrates an example scenario in which an impaired vehicle stops on the reference trajectory in accordance with an embodiment of the present disclosure. [Figure 8B] FIG. 1 is a block diagram for verifying the feasibility of a restored path according to an embodiment of the present disclosure. [Figure 9] FIG. 1 is a schematic diagram illustrating a system that can be used to implement the systems and methods of the present disclosure. DETAILED DESCRIPTION OF THE PREFERRED EMBODIMENTS

[0022] In the following description, for the sake of convenience, numerous specific details are set forth in order to provide a thorough understanding of the present disclosure. However, it will be apparent to those skilled in the art that the present disclosure may be practiced without these specific details. In other instances, the apparatus and methods are shown only in block diagram form in order to avoid obscuring the present disclosure.

[0023] As used in this specification and claims, the terms "for example," "such as," and "such," as well as the verbs "comprise," "have," "include," and other verb forms thereof, when used with a list of one or more components or other items, are each to be construed as open-ended, meaning that the list should not be considered to exclude other additional components or items. The term "based on" means based at least in part on. Furthermore, it is to be understood that the phraseology and terminology used herein are for purposes of description and should not be considered limiting. Any headings used herein are for convenience only and have no legal or limiting effect.

[0024] 1A illustrates an example of a parking lot 100 according to some embodiments of the present disclosure. The parking lot 100 includes a parking location for parking vehicles, such as location 101. The parking lot 100 further includes one or more stationary vehicles, such as vehicles 103, 105, 107, parked in the parking lot 100, and one or more obstacle vehicles, such as obstacle vehicle 109, moving in the parking lot 100. The obstacle vehicle 109 may exit the parking lot 100 by following a path 111. Furthermore, the parking lot 100 includes static obstacles, such as walls 113 and / or pillars of the parking lot 100.

[0025] An objective of some embodiments is to park the autonomous self-vehicle 115 in such a dynamic environment of the parking lot 100 without colliding with the stationary vehicles 103, 105, 107, the obstacle vehicle 109, the wall 113, and other obstacles that may be present in the parking lot 100. For example, the autonomous self-vehicle 115 has an initial state 117 and needs to park in a parking location defined by a goal state 119 without colliding with the stationary vehicles 103, 105, 107, the obstacle vehicle 109, and the wall 113. Each state, e.g., the initial state 117 and the goal state 119, defines at least a position (x, y) and an orientation θ of the autonomous self-vehicle 115 in a two-dimensional plane.

[0026] To achieve these objectives, some embodiments provide a system for parking an autonomous vehicle 115. A system for parking an autonomous vehicle 115 is illustrated in FIG.

[0027] FIG. 1B is a block diagram illustrating a system 123 for parking an autonomous vehicle 115, according to one embodiment of the present disclosure. The system 123 includes a processor 125 and a memory 127. The processor 125 may be a single-core processor, a multi-core processor, a computing cluster, or any number of other configurations. The memory 127 may include a random access memory (RAM), a read only memory (ROM), a flash memory, or any other suitable memory system. Additionally, in some embodiments, the memory 127 may be implemented using a hard drive, an optical drive, a thumb drive, an array of drives, or any combination thereof. In one embodiment, the system 123 may be integrated into the autonomous vehicle 115. The processor 125 is configured to perform various functions for parking the autonomous vehicle 115, as described in FIG. 1C.

[0028] 1C is a block diagram illustrating a method 129 for parking an autonomous vehicle 115 in a dynamic environment of a parking lot 100 according to one embodiment of the disclosure. In block 131, the processor 125 collects measurements of the state of the dynamic environment within the parking lot 100 indicating available parking locations within the parking lot 100, the state of each of the stationary vehicles 103, 105, 107, and the state of the obstructing vehicle 109. In one embodiment, the state of the obstructing vehicle 109 may include the position and orientation of the obstructing vehicle 109 and geometric information of the obstructing vehicle 109, such as its wheelbase.

[0029] In block 133, processor 125 executes a path planner configured to generate a trajectory for parking autonomous vehicle 115 based on the conditions of the dynamic environment in parking lot 100. For example, processor 125 executes the path planner to generate trajectory 121 shown in FIG. 1A for parking autonomous vehicle 115 at goal state 119. In one embodiment, the path planner may be stored in memory 127.

[0030] Some embodiments are based on the recognition that to achieve collision-free parking of the autonomous vehicle 115 in the parking lot 100, the motion of the obstructing vehicle 109 needs to be taken into account. Therefore, some embodiments aim to predict the path of the obstructing vehicle 109. Figure 1D is a schematic diagram illustrating different paths that the obstructing vehicle 109 may traverse to exit the parking lot 100, according to one embodiment of the present disclosure. The obstructing vehicle 109 may traverse, for example, along path 109a and / or path 109b.

[0031] Some embodiments are based on the recognition that, considering the dynamic environment of the parking lot 100, predicting the path of the obstructed vehicle 109 is not enough, but determining the motion mode of the obstructed vehicle 109 is also useful. Some embodiments are based on the observation that the obstructed vehicle 109 runs in various modes in the parking lot 100, such as a cruising mode and a maneuvering mode. In the cruising mode, the steering angle of the obstructed vehicle 109 is small or stable. The obstructed vehicle 109 runs in the cruising mode when the obstructed vehicle 109 enters the parking lot 100 and is approaching a parking spot, or when the obstructed vehicle 109 is exiting the parking lot 100. The cruising mode is also called the first mode. In the maneuvering mode, the obstructed vehicle 109 frequently changes the steering angle to park off the path or exit a narrow parking spot. The maneuvering mode is also called the second mode.

[0032] Therefore, an objective of some embodiments is to predict both the path and the motion mode of the obstructing vehicle 109. To predict the path and the motion mode of the obstructing vehicle 109, in block 135, the processor 125 executes an environmental prediction device configured to predict the path and the motion mode of the obstructing vehicle 109. In one embodiment, the environmental prediction device may be stored in the memory 127.

[0033] Further, in block 137, the processor 125 determines a safety constraint for the obstructing vehicle 109 based on the predicted path and the motion mode of the obstructing vehicle 109. For example, if the predicted path is path 109b and the predicted mode is a cruise mode, the safety constraint is determined around the state of the obstructing vehicle 109 along the path. The safety constraint determined around the state of the obstructing vehicle 109 along the path is called a safety margin. FIG. 1E is a schematic diagram illustrating a safety margin 141 determined around the state of the obstructing vehicle 109 along path 109b according to one embodiment of the present disclosure. The safety margin 141 is determined as a safety constraint for the cruise mode because in the cruise mode, the obstructing vehicle 109 takes less aggressive steering actions and its motion can be predicted with a relatively high degree of confidence. However, in the steering mode, it is difficult to predict the motion of the obstructing vehicle 109, and the obstructing vehicle 109 tends to occupy a larger space to adjust its orientation. Therefore, in the steering mode, the safety constraint is determined as the safety range in all driving directions around the state of the obstructing vehicle 109.

[0034] Thus, for example, if the predicted path is path 109b and the predicted mode is a steering mode, a safety zone is determined for the obstructing vehicle 109. Figure 1F is a schematic diagram illustrating the safety zones 143a and 143b according to an embodiment of the present disclosure. The safety zones 143a and 143b define an area 145 that the autonomously driven vehicle 115 should avoid if the obstructing vehicle 109 is traversing the path 109b in a steering mode.

[0035] Further, in block 139, the processor 125 parks the autonomous vehicle 115 by controlling the autonomous vehicle 115 based on the trajectory 121 and the determined safety constraints.

[0036]

number

[0037] In some embodiments, the kinematic model may include additional state variables to ensure continuous curvature, while in other embodiments, the kinematic model may include longitudinal velocity, steering angle, and steering angular velocity as additional state variables.

[0038]

number

[0039]

number

[0040]

number

[0041] According to one embodiment, the path of the obstructing vehicle 109 may be determined based on the position and speed of the obstructing vehicle 109. However, some embodiments are based on the recognition that the position and speed are insufficient to estimate the motion mode of the obstructing vehicle 109, since vehicles at the same position and with the same speed at any time instance may be in different modes. Therefore, some embodiments are based on the recognition that for the obstructing vehicle 109 in a cruise mode, the steering angle may be more indicative of the motion mode, since the steering angle is more consistent with the speed and / or heading of the obstructing vehicle 109, compared to the steering angle of the obstructing vehicle 109 in a steering mode.

[0042] Thus, in some embodiments, the environmental prediction device estimates the position, speed, and steering angle of the obstacle vehicle 109 at different time instances, and estimates a path of the obstacle vehicle 109 based on the position and speed of the obstacle vehicle 109 at the different time instances. The environmental prediction device further estimates a motion mode of the obstacle vehicle 109 based on the steering angle of the obstacle vehicle 109 at the different time instances. Additionally or alternatively, in some embodiments, the environmental prediction device determines a motion mode of the obstacle vehicle 109 based on the deviation between the heading and steering angle of the obstacle vehicle 109 at the different time instances. The environmental prediction device used to determine the path and motion mode of the obstacle vehicle 109 is described in detail below in FIG. 2.

[0043] FIG. 2 is a schematic diagram 200 illustrating an environment prediction device according to an embodiment of the present disclosure. Based on the measurements, a motion estimator 201 estimates a current state 203 of the obstructing vehicle 109, and a motion predictor 205 predicts a path 207 over a finite time range. The measurements include one or a combination of a target state 119, map information, a position and orientation of the obstructing vehicle 109, and geometric information of the obstructing vehicle 109. Furthermore, based on the current state 203 and the predicted path 207, a long-term mode predictor 209 determines a probability 211 of each mode to which the obstructing vehicle 109 may belong. Furthermore, based on the current state 203, the predicted path 207, and the mode probability 211, the processor 125 determines a safety constraint 213.

[0044]

number

[0045]

number

[0046] To address such shortcomings, some embodiments provide a motion estimator 201 based on a multi-stage estimation framework, which is described below in FIG.

[0047]

number

[0048]

number

[0049]

number

[0050]

number

[0051]

number

[0052]

number

[0053]

number

[0054]

number

[0055] 3C is a schematic diagram illustrating paths of an obstructing vehicle 109 predicted using different models according to an embodiment of the present disclosure. An example path predicted using the unicycle model (3) is denoted at 309. An example path predicted using the bicycle model (4) is denoted at 311. An example path predicted using the dynamic model (7) is denoted at 313.

[0056]

number

[0057]

number

[0058] 3D illustrates an example of short-term motion prediction results 315 based on a bicycle model and an example of short-term motion prediction results 317 based on a unicycle model, according to one embodiment of the present disclosure. The bounding box footprints are 319 and 321. When using a constant steering provided by the bicycle model, the predicted deviation from the ground truth 323 is smaller compared to using a constant yaw rate provided by the unicycle model. For example, one embodiment uses a motion simulator that takes constant inputs and records state changes over a period of time.

[0059] Some embodiments are based on the realization that the long-term motion of the disabled vehicle 109 in the parking lot 100 can be characterized by possible paths (including 201a and 201b as examples), possible modes (maneuvering or cruising along the path), and the probability that the disabled vehicle 109 will follow a particular mode. Combining the path and mode information, a disabled vehicle 109 with N possible paths can have 2N possible modes. According to one embodiment, the prediction of the long-term motion at time step k corresponds to an estimation of the probability of each mode by the long-term mode predictor 209.

[0060]

number

[0061]

number

[0062]

number

[0063]

number

[0064]

number

[0065]

number

[0066] Some embodiments are based on the recognition that the motion of the autonomous vehicle 115 in the parking lot 100 may be constantly adapted in response to changes in the parking lot environment. Thus, even after the trajectory 121 of the autonomous vehicle 115 has been determined, the need to adjust the trajectory 121 may arise at any time. Therefore, to control the parking of the autonomous vehicle 115, some embodiments use a strategic motion planner that changes the control strategy based on different conditions in the parking lot 100.

[0067] For example, in some embodiments, the strategic motion planner uses three different strategies based on different situations. The strategic motion planner includes a model predictive control-based safety controller for trajectory tracking, a search-based evacuation planner for finding an avoidance path, and an optimization-based repair planner for planning a new trajectory / repair path when the reference trajectory becomes invalid. The strategic motion planner balances safety, plan feasibility, and smooth maneuvering by leveraging the advantages of the optimization-based approach and the search-based approach.

[0068] Additionally, some embodiments are based on the recognition that the strategic motion planner can be integrated with the environmental prediction device to form an integrated motion prediction and planning framework, such as that described below in FIG.

[0069] 6 is a block diagram illustrating a motion prediction and planning framework 600 according to one embodiment of the present disclosure. The motion prediction and planning framework 600 includes a central control unit 601, an environmental prediction unit 603, and a strategic motion planner 605. The environmental prediction unit 603 is the same as the environmental prediction unit 603 described in FIG. 2. In one embodiment, the central control unit 601, the environmental prediction unit 603, and the strategic motion planner 605 are stored in the memory 127.

[0070]

number

[0071]

number

[0072]

number

[0073] Additionally, the strategic motion planner 605 determines whether it is possible to track the updated reference trajectory without stopping the autonomous self-vehicle 115. If it is determined that it is possible to track the updated reference trajectory without stopping the autonomous self-vehicle 115, a model predictive controller (MPC) based safety controller 625 is executed to track the updated reference trajectory. In one embodiment, the MPC based safety controller 625 submits control commands, e.g., steering angle and velocity, to the autonomous self-vehicle 115 to track the updated reference trajectory.

[0074] If the repair planner 623 is unable to operate normally, for example if it is determined that it is not possible to track the updated reference trajectory without stopping the autonomous vehicle 115, the repair planner 623 requests re-planning 627 from the central controller 601 by updating the parking lot map 609 and generating the reference trajectory again.

[0075]

number

[0076]

number

[0077]

number

[0078] In block 707, the processor 125 adds the current position to the priority queue as a root node. In block 709, the processor 125 selects the node with the lowest cost, pops it from the priority queue, and extends it by applying precomputed motion primitives. In addition, in block 711, the processor 125 checks whether all precomputed motion primitives have been applied. If not, in block 713, the processor 125 applies new motion primitives to generate an edge set 715. In block 717, the processor 125 determines whether the edge set 715 is collision-free. If the edge set is not collision-free, new motion primitives are applied again to generate a new edge set. If the edge set is collision-free, in block 719, the processor 125 records the child node where the edge set ends. In addition, in block 721, the processor determines whether the child node meets a safety limit, for example, is a certain distance away. If the child node does not meet the safety margin, then in block 723, processor 125 evaluates the node cost. Furthermore, in block 725, processor 125 pushes the child node onto a priority queue. If the child node meets the safety margin, then in block 727, processor 125 calculates a path from the current location to the child node. The path to the child node corresponds to the evacuation path.

[0079] The evaluation of node costs performed in block 723 is described in more detail below in FIG. 7C.

[0080]

number

[0081]

number

[0082] 7D is a schematic diagram illustrating the process of the evacuation planner 619 according to an embodiment of the present disclosure. The evacuation planner 619 starts with a current position 737 that is likely to result in a collision with the obstructing vehicle 109. Initially, the priority queue contains only the root node, so the evacuation planner 619 selects the root node and expands it. Applying the motion primitives 739 results in a new collision-free node 741a. Repeating the application of the motion primitives on the root node adds four more new nodes 741b, 741c, 741d, and 741e to the tree. The five new nodes 741a, 741b, 741c, 741d, and 741e are all pushed into the priority queue. In the next iteration, X best 743 is identified as the best node since it has the lowest node cost and is selected for expansion. Repeating this process eventually results in a new node 745 that meets the safety margin. The tree construction process stops and the evacuation planner 619 returns the evacuation target, which is node 745, and a path 747 from the root node to node 745.

[0083] FIG. 8A illustrates an example scenario in which the obstructing vehicle 109 stops on the reference trajectory 801, according to an embodiment of the present disclosure. In such a scenario, the MPC-based safety controller 625 commands the autonomous self-vehicle 115 to stop on the reference trajectory 801 if the area ahead is not feasible. Unless a new reference trajectory is received, the MPC-based safety controller 625 stops the autonomous self-vehicle 115 and waits for the obstructing vehicle 109 to leave. However, if the obstructing vehicle 109 stops for a long time, waiting is not preferable. Some embodiments are based on the recognition that the obstructing vehicle 109 can be treated as a static obstacle and the reference trajectory 801 can be updated / repaired by the repair planner 623 to form a repair path 803 that allows the autonomous self-vehicle 115 to bypass the obstructing vehicle 109 and return to the reference trajectory 801.

[0084] According to one embodiment, the repaired path 803 is of the same homotopic class as the reference trajectory 801, making an optimization-based repair planner a viable solution. The optimization-based repair planner takes the original waypoints 805 as initialization and moves them to optimal collision-free positions 807 to form the repaired path 803.

[0085]

number

[0086]

number

[0087]

number

[0088]

number

[0089] Some embodiments are based on the understanding that the repaired path 803 cannot always be followed exactly because, even if collision-free, the repaired path 803 may not satisfy the kinematics or dynamics of the autonomous self-driving vehicle 115, causing the autonomous self-driving vehicle 115 to collide with an obstacle. Therefore, before accepting the repaired path 803, the feasibility of the repaired path 803 needs to be verified.

[0090] FIG. 8B is a block diagram for verifying the feasibility of the repaired path 803 according to one embodiment of the present disclosure. In block 809, an iterative linear quadratic regulator (ILQR) tracking controller is used. ILQR is an optimization-based controller configured to generate a motion plan that tracks the repaired path according to the vehicle's kinematics if the repaired path is close to a kinematically feasible trajectory. If ILQR fails, it means the repair planner fails because the repaired path cannot be followed. ILQR is an extension of LQR control, which determines a control sequence in a given time horizon by solving an optimization problem.

[0091] In block 811, the processor 125 determines whether the restored path 803 can be accurately followed. If the restored path 803 can be accurately followed, in block 813, the processor 125 updates the reference trajectory 801 based on the restored path 803. If it is determined that the restored path 803 cannot be accurately followed, in block 815, the processor 125 requests the central controller 601 to provide a new reference trajectory.

[0092] 9 is a schematic diagram illustrating a system that can be used to implement the systems and methods of the present disclosure. The system 900 includes one or a combination of a transceiver 901, an inertial measurement unit (IMU) 903, a display 905, a sensor(s) 907, a memory 909, and a processor 911 operably coupled to each other via a connection 913. The connection 913 may include a bus, a line, a fiber, a link, or a combination thereof.

[0093] The transceiver 901 may include, for example, a transmitter capable of transmitting one or more signals transmitted over one or more types of wireless communication networks and a transceiver capable of receiving one or more signals transmitted over one or more types of wireless communication networks. The transceiver 901 may enable communication with wireless networks based on various technologies, such as, but not limited to, femtocells, Wi-Fi networks or Wireless Local Area Networks (WLANs), which may be based on the IEEE 802.11 family of standards, Wireless Personal Area Networks (WPANS), such as Bluetooth, Near Field Communication (NFC), networks, which may be based on the IEEE 802.15x family of standards, and / or Wireless Wide Area Networks (WWANs), such as LTE, WiMAX, etc. The system 900 may also include one or more ports for communication over a wired network.

[0094] In some embodiments, the system 900 may include an image sensor, such as a CCD or CMOS sensor, a laser, and / or a camera, hereinafter referred to as "sensor 907." For example, the sensor 907 may convert an optical image into an electronic or digital image and may transmit the captured image to the processor 911. Additionally or alternatively, the sensor 907 may sense light reflected from objects in the scene and transmit the intensity of the captured light to the processor 911.

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

[0096] For example, the sensor 907 may include a depth sensor to provide “depth information.” Depth information may be obtained in various ways with the depth sensor. The term “depth sensor” is used to refer to a functional unit that may be used to obtain depth information independently and / or in combination with some other camera. For example, in some embodiments, the depth sensor and an optical camera may be part of the sensor 907. For example, in some embodiments, the sensor 907 includes an RGBD camera that may capture color (RGB) images as well as per-pixel depth (D) information if the depth sensor is enabled.

[0097] As another example, in some embodiments, the sensor 907 may include a 3D Time Of Flight (3DTOF) camera. In embodiments having a 3DTOF camera, the depth sensor may take the form of a strobe light coupled to the 3DTOF camera that can illuminate objects in the scene and the reflected light can be captured by a CCD / CMOS sensor in the sensor 410. Depth information is obtained by measuring the time it takes for a light pulse to reach the object and return to the sensor.

[0098] As yet another example, the depth sensor can take the form of a light source coupled to sensor 907. In one embodiment, the light source projects a structured or textured light pattern, which may include one or more narrow bands of light, onto objects in the scene. Depth information is obtained by exploiting geometric distortions of the projected pattern caused by the surface shape of the object. In one embodiment, depth information is determined from a stereo sensor, such as a combination of an infrared structured light projector and an infrared camera aligned with an RGB camera.

[0099] In some embodiments, the sensor 907 includes a stereoscopic camera. For example, the depth sensor may form part of a passive stereo vision sensor that may use two or more cameras to obtain depth information of a scene. Pixel coordinates of points common to both cameras in the captured scene may be used together with camera pose information and / or triangulation techniques to obtain per-pixel depth information.

[0100] In some embodiments, the system 900 may be operatively connected to multiple sensors 907, such as dual front cameras and / or front and rear cameras, and may also incorporate a variety of sensors. In some embodiments, the sensor 907 may capture both still and moving images. In some embodiments, the sensor 907 may include an RGBD camera or a stereoscopic video camera capable of capturing images at, for example, 30 frames per second (fps). In one embodiment, the images captured by the sensor 907 may be in a raw, uncompressed format and may be compressed before being processed and / or stored in the memory 909. In some embodiments, the image compression may be performed by the processor 911 using lossless or lossy compression techniques.

[0101] In some embodiments, the processor 911 may also receive input from the IMU 903. In other embodiments, the IMU 903 may include a 3-axis accelerometer(s), a 3-axis gyroscope(s), and / or a magnetometer(s). The IMU 903 may provide velocity, orientation, and / or other position related information to the processor 911. In some embodiments, the IMU 903 may output measured information synchronized with the capture of each image frame by the sensor 907. In some embodiments, the output of the IMU 903 is used in part by the processor 911 to fuse the sensor measurements and / or to further process the fused measurements.

[0102] The system 900 may also include a screen or display 905 that renders images such as color and / or depth images. In some embodiments, the display 905 may be used to display live images captured by the sensor 907, fused images, augmented reality (AR) images, graphical user interfaces (GUIs), and other program output. In some embodiments, the display 905 may include and / or accommodate a touch screen to allow a user to input data via some combination of a virtual keyboard, icons, menus, or other GUIs, user gestures, and / or input devices such as styluses and other writing implements. In some embodiments, the display 905 may be implemented with a liquid crystal display (LCD) display device or a light emitting diode (LED) display such as an organic light emitting diode (OLED) display. In other embodiments, the display 480 may be a wearable display. In some embodiments, the results of the fusion may be rendered on the display 905 or submitted to different applications that may be internal or external to the system 900.

[0103] The example system 900 may also be modified in various ways consistent with this disclosure, such as by the addition, combination, or omission of one or more of the illustrated functional blocks. For example, in some configurations, the system 900 does not include an IMU 903 or a transceiver 901. Additionally, in certain implementations, the system 900 includes various other sensors (not shown), such as an ambient light sensor, a microphone, an acoustic sensor, an ultrasonic sensor, a laser range finder, etc. In some embodiments, portions of the system 400 take the form of one or more chipsets, or the like.

[0104] The processor 911 may be implemented using a combination of hardware, firmware, and software. The processor 911 may represent one or more circuits configurable to execute at least a portion of a computing procedure or process related to sensor fusion and / or a method for further processing the fused measurements. The processor 911 retrieves instructions and / or data from the memory 909. The processor 911 may be implemented using one or more application specific integrated circuits (ASICs), central processing units and / or graphical processing units, 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.

[0105] Memory 909 can be implemented within processor 911 and / or external to processor 911. As used herein, the term "memory" refers to any type of long-term, short-term, volatile, non-volatile, or other memory and is not limited to a particular type or number of memories or the type of physical medium on which the memory is stored. In some embodiments, memory 909 holds program code that facilitates automated parking.

[0106] For example, memory 909 may store sensor measurements such as still images, depth information, video frames, program results, and data provided by IMU 903 and other sensors. Memory 909 may store memory that stores vehicle geometry, maps of parking spaces, motion models of the autonomous vehicle, and dynamic models of the autonomous vehicle. In general, memory 909 may represent any data storage mechanism. Memory 909 may include, for example, primary memory and / or secondary memory. Primary memory may include, for example, random access memory, read-only memory, and the like.

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

[0108] The following description provides only exemplary embodiments and is not intended to limit the scope, applicability, or configuration of the present disclosure. Rather, the following description of exemplary embodiments provides those skilled in the art with an enabling description for implementing one or more exemplary embodiments. Contemplated are various changes that may be made in the function and arrangement of elements without departing from the spirit and scope of the disclosed subject matter as set forth in the appended claims.

[0109] Specific details are given in the following description to provide a thorough understanding of the embodiments. However, those skilled in the art will understand that the embodiments may be practiced without these specific details. For example, systems, processes, and other elements in the disclosed subject matter may be shown as components in block diagram form so as not to obscure the embodiments in unnecessary detail. In other instances, well-known processes, structures, and techniques may be shown without unnecessary detail so as not to obscure the embodiments. Additionally, like reference numbers and names in the various drawings refer to like elements.

[0110] Also, particular embodiments may be described as a process that is depicted as a flowchart, a flow diagram, a data flow diagram, a structure diagram, or a block diagram. Although a flowchart may describe operations as a sequential process, many of the operations may be performed in parallel or concurrently. Additionally, the order of operations may be rearranged. A process may terminate upon completion of its operations, but may have additional steps not discussed or included in the diagram. Additionally, not all operations in a specifically described process may occur in all embodiments. A process may correspond to a method, a function, a procedure, a subroutine, a subprogram, or the like. When a process corresponds to a function, the end of the function may correspond to a return of the function to a calling function or to a main function.

[0111] Furthermore, embodiments of the disclosed subject matter may be implemented, at least in part, either manually or automatically. Manual or automated implementations are performed or at least assisted by the use of machines, hardware, software, firmware, middleware, microcode, hardware description languages, or any combination thereof. When implemented in software, firmware, middleware, or microcode, program code or code segments to perform the necessary tasks may be stored in a machine-readable medium. A processor(s) may perform the necessary tasks.

[0112] The various methods or processes outlined herein may be coded as software executable on one or more processors employing any one of a variety of operating systems or platforms. Moreover, such software may be written using any of a number of suitable programming languages ​​and / or programming or scripting tools, and may be compiled as executable machine code or intermediate code that runs on a framework or virtual machine. The functionality of the program modules may typically be combined or distributed as desired in various embodiments.

[0113]

[0023] Embodiments of the present disclosure may be embodied as methods, examples of which are provided. The acts performed as part of the method may be ordered in any suitable manner. Thus, while acts may be shown in an illustrative embodiment as sequential, embodiments may be constructed in which acts are performed in a different order than shown, including performing some acts simultaneously.

[0114] Although the disclosure has been described with reference to certain preferred embodiments, it is to be understood that various other adaptations and modifications can be made within the spirit and scope of the disclosure. It is therefore the object of the appended claims to cover all such variations and modifications as come within the true spirit and scope of the disclosure.

Claims

1. 1. An integrated system for parking an autonomous vehicle in a dynamic environment of a parking lot, comprising: a processor; and a memory storing instructions that, when executed by the processor, cause the integrated system to: collecting measurements of a state of the dynamic environment within the parking lot indicative of available parking locations in the parking lot, a state of one or more stationary vehicles parked in the parking lot, and a state of one or more obstructing vehicles moving in the parking lot; running a path planner configured to generate a trajectory for parking the autonomous vehicle based on the dynamic environmental conditions at the parking lot; running an environment prediction device configured to predict a path and a motion mode including a first motion mode or a second motion mode of each of the obstacle vehicles; determining safety constraints for each of the obstructing vehicles based on the path and the motion mode of each of the obstructing vehicles, where if the obstructing vehicle is in the first motion mode, the safety constraints are determined along the path around a state of the obstructing vehicle, and if the obstructing vehicle is in the second motion mode, the safety constraints are determined in all travel directions around a state of the obstructing vehicle; and the instructions further include: An integrated system that parks the autonomous vehicle based on the parking trajectory and the safety constraints of each of the obstacle vehicles.

2. The environment prediction device includes: Estimating the position, velocity, steering angle, acceleration, and steering angular velocity of the obstructing vehicle at different time instances; estimating the path of the obstacle vehicle based on the position, the velocity, the steering angle, the acceleration, and the steering angular velocity of the obstacle vehicle at the different time instances; The integrated system of claim 1 , configured to estimate the motion mode of the obstruction vehicle based on the steering angles of the obstruction vehicle at the different time instances.

3. The environment prediction device further comprises: Implementing a Kalman filter that integrates the position measurements of the obstacle vehicle to predict the velocity and acceleration of the obstacle vehicle at the different time instances; The integrated system of claim 2 , configured to integrate the estimates of velocity over time to generate estimates of the steering angle and the steering angular velocity of the obstructing vehicle at the different time instances.

4. The environmental prediction device includes: predicting the position, velocity, and steering angle of the obstructing vehicle at different time instances; estimating the path of the obstructing vehicle based on the position and the velocity of the obstructing vehicle at the different time instances; The integrated system of claim 1 , configured to estimate the motion mode of the obstruction vehicle based on the steering angles of the obstruction vehicle at the different time instances.

5. 5. The integrated system of claim 4, wherein the environmental prediction unit is further configured to determine the motion mode of the obstruction vehicle based on a deviation between an orientation of the obstruction vehicle and the steering angle of the obstruction vehicle at the different time instances.

6. The environment prediction device further comprises: Implementing a Kalman filter that integrates the position measurements of the obstructing vehicle to estimate the velocity of the obstructing vehicle at the different time instances; The integrated system of claim 4 , configured to integrate forward in time the estimates of the velocity at the different time instances to generate estimates of the steering angle at the different time instances.

7. The environment prediction device further comprises: estimating the likelihood that the obstructing vehicle will move along different routes in the parking lot; estimating a belief of the motion mode of the obstructing vehicle for each of the different paths through the parking lot; The integrated system of claim 4 configured to assign the motion mode to the impaired vehicle corresponding to the route that is most likely.

8. The integrated system of claim 7 , wherein the environmental prediction unit is further configured to track the beliefs and the likelihoods of each different route through the parking lot using a Bayesian framework.

9. 2. The integrated system of claim 1, wherein the processor is further configured to execute a strategic motion planner configured to select one of a model predictive controller for tracking a path of the trajectory, a search-based evacuation planner for searching an avoidance path for the autonomous vehicle, and an optimization-based repair planner for modifying the path of the trajectory to park the autonomous vehicle based on the trajectory for parking and the safety constraints of each of the obstacle vehicles.

10. 10. The integrated system of claim 9, wherein the strategic motion planner is further configured to evaluate the trajectory against the safety constraints of each of the obstacle vehicles and execute the model predictive controller if it is possible to track the path of the trajectory without stopping the autonomous vehicle.

11. 10. The integrated system of claim 9, wherein the model predictive controller is further configured to treat the safety constraints as constraints of a trajectory optimization problem, the trajectory optimization problem optimizing at least one feasible smooth trajectory that follows the trajectory.

12. 10. The integrated system of claim 9, wherein the strategic motion planner is further configured to evaluate the trajectory against the safety constraints of each of the impaired vehicles and execute the repair planner to adjust the trajectory if it is not possible to track the path of the trajectory without stopping the autonomous vehicle.

13. The integrated system of claim 12 , wherein the repair planner is configured to move original waypoints of the trajectory to optimal collision-free positions to generate a repair path.

14. 10. The integrated system of claim 9, wherein the strategic motion planner is further configured to evaluate the trajectory against the safety constraints of each of the obstacle vehicles and execute the evacuation planner if a current state of the autonomous vehicle following the trajectory violates the safety constraints.

15. The evacuation planner: Obtaining one or more new nodes by applying one or more precomputed motion primitives to a root node; selecting a node from the one or more new nodes that has the lowest cost and applying one or more pre-computed motion primitives at the selected node; Repeat the selection and application until a safe node that satisfies the safety limit is found or the time runs out. The integrated system of claim 14 , configured to calculate a path to the safe node, or to the node with the lowest cost in the case of a timeout, as an evacuation path.

16. 1. A method for parking an autonomous vehicle in a dynamic environment of a parking lot, comprising: collecting measurements of a state of the dynamic environment within the parking lot indicative of available parking locations in the parking lot, a state of one or more stationary vehicles parked in the parking lot, and a state of one or more obstructing vehicles moving in the parking lot; executing a route planner configured to generate a trajectory for parking the autonomous vehicle based on the state of the dynamic environment in the parking lot; running an environment prediction device configured to predict a path and a motion mode, including a first motion mode or a second motion mode, of each of the obstacle vehicles; determining safety constraints for each of the obstructing vehicles based on the path and the motion mode of each of the obstructing vehicles, where if the obstructing vehicle is in the first motion mode, the safety constraints are determined along the path around a state of the obstructing vehicle, and if the obstructing vehicle is in the second motion mode, the safety constraints are determined in all travel directions around a state of the obstructing vehicle, the method further comprising: parking the autonomous vehicle based on the parking trajectory and the safety constraints for each of the obstacle vehicles.

17. The environmental prediction device includes: Estimating the position, velocity, and steering angle of the obstructing vehicle at different time instances; estimating the path of the obstructing vehicle based on the position and the velocity of the obstructing vehicle at the different time instances; The method of claim 16 configured to estimate the motion mode of the obstruction vehicle based on the steering angles of the obstruction vehicle at the different time instances.

18. 17. The method of claim 16, wherein the environment prediction unit is further configured to determine the motion mode of the obstacle vehicle based on a deviation between an orientation of the obstacle vehicle and the steering angle of the obstacle vehicle at the different time instances.

19. The environment prediction device further comprises: Implementing a Kalman filter that integrates the position measurements of the obstructing vehicle to predict the velocity of the obstructing vehicle at the different time instances; 20. The method of claim 18, configured to integrate the estimates of the velocity at the different time instances forward in time to generate estimates of the steering angle at the different time instances.

20. 1. A non-transitory computer-readable storage medium having embodied thereon a program executable by a processor for performing a method for parking an autonomous vehicle in a dynamic environment of a parking lot, the method comprising: collecting measurements of a state of the dynamic environment within the parking lot indicative of available parking locations in the parking lot, a state of one or more stationary vehicles parked in the parking lot, and a state of one or more obstructing vehicles moving in the parking lot; executing a route planner configured to generate a trajectory for parking the autonomous vehicle based on a dynamic environmental condition at the parking lot; running an environment prediction device configured to predict a path and a motion mode, including a first motion mode or a second motion mode, of each of the obstacle vehicles; determining safety constraints for each of the obstructing vehicles based on the path and the motion mode of each of the obstructing vehicles, where if the obstructing vehicle is in the first motion mode, the safety constraints are determined along the path around a state of the obstructing vehicle, and if the obstructing vehicle is in the second motion mode, the safety constraints are determined in all travel directions around a state of the obstructing vehicle, the method further comprising: parking the autonomous vehicle based on the parking trajectory and the safety constraints for each of the obstacle vehicles.

Images