Vehicle control system and control procedures for controlling the movement of a vehicle

Abstract

Control system (299) of a vehicle (101a, 300, 400, 500a, 910) for controlling the movement of the vehicle (101a, 300, 400, 500a, 910) moving on a road shared with a group of moving objects, comprising: a memory (109, 320, 380, 1509) to store a set of equilibrium points defining lateral displacements on the road traveled by the vehicle (101a, 300, 400, 500a, 910) and to store a set of regions of states of lateral dynamics of the vehicle (101a, 300, 400, 500a, 910) corresponding to the set of equilibrium points, wherein each state of lateral dynamics comprises a lateral displacement, a rate of change of lateral displacement, a lateral orientation and a rate of change of lateral orientation, wherein each region defines a control-invariant set of states of lateral dynamics determined for different speeds of the vehicle (101a, 300, 400, 500a, 910) such that the vehicle (101a, 300, 400, 500a, 910), which has a state within a range determined for a speed, is able toto maintain its states within the region during motion at velocity, wherein each region contains a corresponding equilibrium point and overlaps with at least one adjacent region, and wherein each equilibrium point is associated with one or more regions designated for different velocities; , at least one sensor (300) to generate a time series signal indicating a motion trajectory of each object in the group of moving objects on a road shared with the vehicle (101a, 300, 400, 500a, 910); a collision detector (340) to detect a collision of each area determined for a first velocity with at least one object at each time step of the control in a time horizon, if the motion trajectory of the object at the time step of the control intersects a space defined by a longitudinal displacement of the vehicle (101a, 300, 400, 500a, 910) at the time step of the control and a range of values ​​of the lateral displacement of the area to create a data structure indicating collision-free areas at different time steps of the control; a path planner (350) to determine a temporal sequence, defining a collision-free sequence of domains, forming a unity of domains in space and time, connecting a domain comprising an initial displacement with a domain comprising a destination displacement to generate a trajectory within the unity, connecting the initial displacement with the destination displacement; and a control unit (360) for controlling the vehicle (101a, 300, 400, 500a, 910) according to the trajectory.

Description

Technical area

[0001] The invention relates generally to autonomous driving and advanced driver assistance systems, and more particularly to controlling the movement of a vehicle at variable speed while taking into account the risk posed by the movements of other vehicles. Background to the state of the art

[0002] Reliable localization and movement prediction is a key component for autonomous driving and advanced driver assistance systems (ADAS). For example, one component in an autonomous vehicle and ADAS is the movement planner, which gathers information about the environment and calculates a trajectory profile to navigate to a destination, often in the presence of moving obstacles. As another example, ADAS, such as lane-change systems, require accurate information about the location of other vehicles, both at the current time and at some future time.

[0003] To this end, modern vehicles sometimes include hazard assessment and / or collision avoidance systems that employ object detection sensors used to enable collision warning or avoidance and other active safety applications. The object detection sensors can utilize any number of technologies, such as short-range radar, long-range radar, image-processing cameras, laser or LiDAR, ultrasound, etc. The object detection sensors detect vehicles and other objects in a host vehicle's path, and application software uses the object detection information to provide warnings or perform actions as appropriate. In many vehicles, the object detection sensors are integrated directly into the front bumper or other trim parts of the vehicle.

[0004] However, hazard assessment and / or collision avoidance when planning the host vehicle's lateral movement are challenging tasks. For example, a system described in US Pat. No. 8,543,261 B2 takes hazard assessment into account by generating optimal vehicle states and generating a hazard assessment based on these optimal states. However, calculating optimal routes can be computationally prohibitive, especially in complex environments.

[0005] In US 2016 / 0109571 A1, the hazard assessment is based on a predicted trajectory of the host vehicle, using the vehicle's movement dynamics and multiple returned sample points from the detected vehicles, and calculating the risk of each detected vehicle intersecting with the predicted path of the host vehicle. However, it may be computationally prohibitive to predict the intersection of every possible trajectory of the host vehicle's lateral movement with every possible trajectory of other vehicles.

[0006] Accordingly, there is a need for a system and method for controlling the lateral movement of the vehicle, taking into account the risk posed by the movements of other vehicles, in a computationally efficient manner. Summary of the invention

[0007] Some embodiments are based on the understanding that the lateral movement of a vehicle can be controlled according to different trajectories. For example, a lane-change maneuver can be achieved through multiple trajectories with different values ​​of the rate of change of lateral displacement and different forward speeds, hereinafter referred to as velocity. For this purpose, it may be desirable to predict different trajectories for different lateral movement goals and test the feasibility of these trajectories for the current traffic situations while controlling the vehicle's movement in real time.

[0008] However, some embodiments are based on the understanding that predicting and testing these trajectories can still be a computationally demanding task. Specifically, for each lateral movement goal, such as a right-to-left lane change, a left-to-right lane change, a right-to-right overtaking, or a left-to-right overtaking, there can be a vast, if not infinite, number of possible trajectories that take the vehicle from its current position to the position corresponding to the current goal. Furthermore, if an obstacle intersects a trajectory at even a single point, the entire trajectory must be discarded, wasting computational and memory resources.

[0009] Some embodiments are based on the realization that the reason for the abundance of trajectories and the sensitivity of all points of the trajectory to a partial collision lies in the temporal interdependence of all points of the trajectories. Specifically, the trajectories are functions of time that specify not only the sequence of points, but also the transitions between the points as a function of time. These transitions determine the state and dynamics of the vehicle at each point of the trajectory and force the vehicle to move according to the trajectory. The same lateral displacement between two neighboring points can be achieved with different time transitions and different vehicle speeds. Different time transitions, in turn, force different states of the vehicle at these two neighboring points, making each trajectory unique and interdependent.

[0010] Some embodiments are based on the realization that by breaking the temporal interdependence between values ​​of the vehicle's lateral displacement and the vehicle's velocity, the calculation of the collision-free trajectory for the vehicle's lateral movement can be simplified. Some embodiments are based on the realization that different values ​​of lateral displacement can be decoupled from each other by constructing a range for each lateral displacement and vehicle velocity, defining a control-invariant set of states of the lateral dynamics determined such that the vehicle, having a state within a range, is able to maintain its states within the range.

[0011] Some embodiments are based on the realization that, if it is possible to maintain the state of the vehicle within a region, the collisions of the region with different obstacles at different times can be evaluated separately and independently of the collision assessment of other regions. After the evaluation is completed, it is possible to select a time sequence that defines a collision-free sequence of regions forming a unity of regions in space and time according to a desired goal of lateral movement. To put it figuratively, the desired sequence of regions is concatenated on a collision-free time thread. By changing the collision-free time thread, different collision-free trajectories can be formed.

[0012] In some embodiments, a set of ranges of states of the vehicle's lateral dynamics is determined for a given vehicle speed for a corresponding set of points defining the lateral displacements on the road traversed by the vehicle. Each state of the lateral dynamics includes a lateral displacement, a rate of change of the lateral displacement, a lateral orientation, and a rate of change of the lateral orientation of the vehicle. Each range defines a control-invariant set of the states of the lateral dynamics determined such that the vehicle, having a state within a range, is capable of maintaining its states within the range for vehicle speeds within a limited range of speeds.

[0013] In some embodiments, these regions overlap with each other. In this way, the unity of collision-free regions in space and time can be formed without evaluating the transitions outside the regions.

[0014] In some embodiments, the set of regions for the vehicle's lateral dynamics is predetermined without considering the vehicle's longitudinal dynamics, but the vehicle speed is a parameter for the lateral dynamics. Such decoupling allows the same set of regions to be reused for different longitudinal displacements of the vehicle to determine collisions between different regions at different times.

[0015] In alternative embodiments, the group of ranges for the lateral dynamics of the vehicle is predetermined, taking into account the longitudinal dynamics of the vehicle. For example, in one embodiment, different groups of ranges are determined for different values ​​of the vehicle's longitudinal speed. This embodiment allows the group of ranges to be selected according to a current and / or desired longitudinal speed of the vehicle. In one embodiment, the connections between groups of ranges for different speeds are established, taking the longitudinal dynamics into account.

[0016] In various embodiments, the group of regions is stored in a memory of a control system. For example, in one embodiment, the group of regions is precalculated for all possible variations in road conditions. In this embodiment, the variation in the shape of the road that affects the lateral dynamics of the vehicle is handled by a motion control unit.

[0017] In an alternative embodiment, the control system includes a transceiver for transmitting a current location of the vehicle and, in response to the transmission, receiving the set of areas determined for the current shape of the road.

[0018] Accordingly, one embodiment discloses a control system of a vehicle for controlling the movement of the vehicle traveling on a road shared with a group of moving objects. The control system includes a memory for storing a group of equilibrium points defining lateral displacements on the road traversed by the vehicle, and a group of ranges of states of the lateral dynamics of the vehicle corresponding to the group of equilibrium points, each state of the lateral dynamics comprising a lateral displacement, a rate of change of the lateral displacement, a lateral orientation, and a rate of change of the lateral orientation, each range defining a control-invariant group of the states of the lateral dynamics determined for different speeds of the vehicle, such that the vehicle,having a state within a range determined for a speed, capable of maintaining its states within the range while moving at the speed, each range containing a corresponding equilibrium point and overlapping with at least one adjacent range, and each equilibrium point being associated with one or more ranges determined for different speeds; at least one sensor for generating a time series signal indicative of a movement trajectory of each object in the group of moving objects on a road shared with the vehicle; a collision detector for detecting a collision of each range determined for a first speed with at least one object at each time step of the controller in a time horizon when the movement trajectory of the object intersects a space at the time step of the controller,which is defined by a longitudinal displacement of the vehicle at the time step of control and a range of values ​​of the lateral displacement of the region to create a data structure indicating collision-free regions at different time steps of control; a path planner to create a time sequence defining a collision-free sequence of regions, forming a unit of regions in space and time, connecting a region comprising an initial displacement with a region comprising a target displacement to create a trajectory within the unit connecting the initial displacement with the target displacement; a control unit for controlling the vehicle according to the trajectory.

[0019] For example, the collision detector may detect a collision in each zone designated for a second speed if the path planner cannot determine the time sequence defining the collision-free sequence of zones designated for the first speed. In this situation, the path planner determines the collision-free sequence of zones designated for the second speed to control the vehicle.

[0020] For example, in some embodiments, the collision detector may iterate through different speed values ​​until it reaches a speed value for which the collision-free sequence of regions determined for that value is found. For example, the collision detector may select the first speed as a current speed of the vehicle and start the iteration from that current speed. Additionally or alternatively, the collision detector may start the iteration from the speed limit permitted on the road being traveled.

[0021] In some implementations, a range determined for a balance point representing a current vehicle displacement covers a sequence of speeds including the first and second speeds. For example, the speeds for determining ranges are chosen so that the ranges corresponding to different speeds overlap for similar lateral displacements for at least one speed. In this way, the control system can safely change the vehicle's speeds.

[0022] A further embodiment discloses a control method for controlling the movement of the vehicle traveling on a road shared with a group of moving objects, the method using at least one processor coupled to a memory configured to store a group of equilibrium points defining lateral displacements on the road traveled by the vehicle, and to store a group of ranges of states of the lateral dynamics of the vehicle corresponding to the group of equilibrium points, each state of the lateral dynamics comprising a lateral displacement, a rate of change of the lateral displacement, a lateral orientation, and a rate of change of the lateral orientation, each range defining a control-invariant group of states of the lateral dynamics determined for different speeds of the vehicle, such that the vehicle,having a state within a range determined for a speed, capable of maintaining its states within the range while moving at the speed, each range containing a corresponding equilibrium point and overlapping with at least one adjacent range, and each equilibrium point being associated with one or more ranges determined for different speeds, the processor being coupled to stored instructions implementing the method, the instructions, when executed by the processor, performing steps of the method.

[0023] The method comprises: generating a time series signal indicative of a motion trajectory of each object in the group of moving objects on a road shared with the vehicle; detecting a collision of each region determined for a first speed with at least one object at each control time step in a time horizon when the motion trajectory of the object at the control time step intersects with a space defined by a longitudinal displacement of the vehicle at the control time step and a range of values ​​of lateral displacement of the region, to generate a data structure indicative of collision-free regions at different control time steps;Determining a time sequence defining a collision-free sequence of regions, forming a unit of regions in space and time, connecting a region comprising an initial displacement with a region comprising a target displacement to generate a trajectory within the unit connecting the initial displacement with the target displacement; and controlling the vehicle according to the trajectory. Brief description of the drawings [ Fig. 1A] Fig. Figure 1A shows an example situation in which a vehicle is moving in the left lane of a two-lane road. [ Fig. 1B] Fig. Figure 1B is a schematic diagram illustrating principles employed by some embodiments for lateral movement of the vehicles. [ Fig. 2] Fig. Figure 2 shows a flowchart of a method for controlling a vehicle moving on a road shared with a group of moving obstacles. [ Fig. 3A] Fig. 3A shows a block diagram of a control system of a vehicle for controlling movement of the vehicle traveling on a road shared with a group of moving objects, according to some embodiments. [ Fig. 3B] Fig. 3B shows a general structure of the control system according to an embodiment. [ Fig. 4A] Fig. Figure 4A shows a schematic representation of a movement target of a vehicle. [ Fig. 4B] Fig. 4B illustrates benefits of a control-invariant region according to some embodiments. [ Fig. 4C] Fig. 4C shows a schematic representation of an example control-invariant region used by some embodiments. [ Fig. 4D] Fig. 4D illustrates a scenario of moving through a group of control-invariant regions to a target position according to some embodiments. [ Fig. 5A] Fig. 5A shows an example of modeling the vehicle control in the frame of the vehicle according to some embodiments. [ Fig. 5B] Fig. 5B shows a schematic representation of a model of a state of a vehicle used by some embodiments. [ Fig. 5C] Fig. Figure 5C shows an example of the effects of the physical constraints of the system on the control-invariant groups. [ Fig. 5D] Fig. Figure 5D shows an example of the impact of the physical constraints of the system on the control-invariant groups. [ Fig. 5E] Fig. Figure 5E shows an alternative example for determining connections between equilibrium points. [ Fig. 6A] Fig. 6A shows an example of a state transition graph defining possible transitions between equilibrium points corresponding to different regions for different time steps of the controller without considering the obstacles, according to some embodiments. [ Fig. 6B] Fig. Figure 6B shows the graph of Fig. 6A, updated in response to detecting the obstacles to form a data structure indicating the collision-free regions, according to some embodiments. [ Fig. 6C] Fig. 6C shows a schematic representation of a data structure indicating the state transition graph used by some embodiments. [ Fig. 7A] Fig. 7A shows a situation where parts of the safe areas 700a overlap with the predicted obstacle. [ Fig. 7B] Fig. 7B illustrates the scheme of probabilistically determining the collision-free regions according to one embodiment. [ Fig. 8A] Fig. 8A shows an exemplary implementation of the collision detector for determining the data structure indicating collision-free regions, according to one embodiment. [ Fig. 8B] Fig. 8B shows a flowchart of a method for generating a feasible trajectory and determining its probability of intersection with a region, according to some embodiments. [ Fig. 8C] Fig. 8C shows a table listing different intentions of a hypothetical driver, according to some embodiments. [ Fig. 8D] Fig. Figure 8D shows a sequence of state transitions that represent an iteration of the procedure from Fig. 8B illustrates. [ Fig. 8E] Fig. 8E shows a flowchart of an exemplary implementation of determining the set of sampled states according to one embodiment. [ Fig. 8F] Fig. Figure 8F shows a flowchart of a method for updating the probability that the sampled states match the Fig. 8C are consistent with the driver’s intentions. [ Fig. 8G] Fig. Figure 8G shows a simplified schematic representation of the iteration of the procedure that creates a state for each discrete section of the probability density function (PDF). [ Fig. 8H] Fig. Figure 8H shows possible associated probabilities of the five states at the first iteration in Fig. 8G. [ Fig. 8I] Fig. Figure 8I shows a graph of a total probability density function used by one embodiment. [ Fig. 9] Fig. Figure 9 shows an example of possible realizable trajectories of a vehicle. [ Fig. 10A] Fig. 10A shows a time series signal of a steering trajectory of the vehicle's steering wheel and an acceleration trajectory of the vehicle used by some embodiments to determine the intention of a driver of a vehicle. [ Fig. 10B] Fig. 10B shows a flowchart of an example implementation of a method for updating the probabilities of each feasible trajectory to be followed based on the consistency of the feasible trajectories from the subset with the vehicle's intention from the time series signal, according to some embodiments. [ Fig. 11A] Fig. Figure 11A shows a plot of the change in area size at different speeds. [ Fig. 11B] Fig. Figure 11B illustrates the determination of a safe sequence of zones for different speeds. [ Fig. 12A] Fig. Figure 12A illustrates the selection of lateral displacements for different vehicle speeds. [ Fig. 13A] Fig. Figure 13A shows a schematic representation of how to select which areas are to be stored in the memory. [ Fig. 13B] Fig. 13B shows a schematic representation of selecting regions to be stored in a memory according to some embodiments. [ Fig. 14A] Fig. Figure 14A shows a representation of a possible speed arrangement. [ Fig. 14B] Fig. Figure 14B shows a representation of a possible velocity arrangement. [ Fig. 14C] Fig. Figure 14C shows an illustration of how safe transition between different speeds may be ensured according to some embodiments of the invention. [ Fig. 15] Fig. 15 shows a flowchart of a method for controlling a vehicle. [ Fig. 16] Fig. 16 shows an exemplary implementation of a method for determining the data structure indicating collision-free regions. [ Fig. 17] Fig. Figure 17 shows a representation of the choice of the cost function for the state transitions. Description of the embodiments

[0024] Some embodiments of the invention are based on the understanding that the lateral movement of a vehicle can be controlled according to different trajectories, where a vehicle is generally a road-bound vehicle such as a passenger car, a bus, a truck, or the like. For example, a lane change maneuver can be achieved by multiple trajectories with different values ​​of the rate of change of lateral displacement. Depending on the particular traffic situation and the preference of the vehicle occupants, different trajectories may be more suitable than others. For this purpose, it may be desirable to predict different trajectories for different lateral movement goals and to test the feasibility of these trajectories for the current traffic situations while controlling the vehicle's movement in real time.

[0025] Fig. 1A shows an example situation in which a vehicle 101a is moving in the left lane of a two-lane road with road markings 110a and a lane divider 120a. The goal is to change lanes to the right lane 130a. The movement corresponding to a lane change can be any of several movements 140a. Depending on whether other vehicles are in the vicinity, some of the trajectories 140a may not be suitable or even impossible to perform due to an impending collision.

[0026] In general, the number of trajectories 140a can be large. Therefore, predicting the trajectories and testing their feasibility can be a challenging and computationally demanding task. Specifically, for each lateral movement goal, such as a right-to-left lane change, a left-to-right lane change, a right-to-right overtaking, or a left-to-right overtaking, there may be a vast, if not infinite, number of possible trajectories. Furthermore, if an obstacle intersects a trajectory at even a single point, the entire trajectory must be discarded, wasting computational and memory resources.

[0027] Fig. Figure 1B is a schematic diagram illustrating principles used by some embodiments for the lateral movement of the vehicles. Consider, for example, the left part in Fig. 1B, which shows a diagram of the movement from the lateral starting position 110b to the lateral target position 120b in four time steps. For this purpose, the trajectory is composed of movements of the vehicle defined by state transitions 113b between points 111b, to land at 120b after four time steps. Here, a state trajectory includes a position and direction of travel of the vehicle over time, i.e., the vehicle's speed. If another vehicle 121b intersects with the calculated trajectory 113b, the trajectory 113b is no longer valid, and a new trajectory must either be calculated and checked again for collision with the obstacle 121b or taken from a library of pre-calculated trajectories.

[0028] Part of the reason for the abundance of trajectories and the sensitivity of all points of the trajectory to a partial collision lies in the temporal interdependence of all points 111b of the trajectories. Specifically, the trajectories 113b are functions of time, which specify not only the sequence of points 111b, but also the transitions between the points as a function of time. These transitions determine the state and dynamics of the vehicle at each point of the trajectory and force the vehicle to move according to the trajectory. The same lateral displacement between two neighboring points can be achieved with different time transitions. Different time transitions, in turn, force different states of the vehicle at these two neighboring points, making each trajectory unique and interdependent.

[0029] Some embodiments are based on the realization that by breaking 140b the temporal interdependence between values ​​of the vehicle's lateral displacement, the calculation of the collision-free trajectory for the vehicle's lateral movement can be significantly simplified. Some embodiments are based on the realization that different values ​​of the lateral displacement can be decoupled from each other by constructing a range that is determined such that the vehicle having a state within a range is capable of maintaining its states within the range.

[0030] For example, the right part of Fig. 1B illustrates the situation when the goal is to steer the vehicle from the initial lateral displacement 110b to the target lateral displacement 120b. Instead of determining a trajectory 113b that defines points 111b and corresponding state transitions between the points, one embodiment recognizes that the target lateral displacement 120b can be reached from 110b by constructing regions where it is safe for the vehicle to be located. The regions overlap, meaning that it is possible to move within a safe corridor of regions from the initial point 110b to the final lateral displacement 120b, thus avoiding collision.Consequently, by using the areas instead of specific trajectories, safe trajectories can be determined that lie within the safe corridor of the areas, ensuring that if an obstacle 121b overlaps with a specific area 130b in a specific time window, the area is marked as unsafe and other safe areas can be used instead.

[0031] Fig. Figure 2 shows a flowchart of a method for controlling a vehicle moving on a road shared with a group of moving obstacles or objects, hereinafter referred to interchangeably as vehicles. The method utilizes a memory to store information necessary for implementing steps of the method. The method retrieves 110 from the memory 109 a group of equilibrium points defining lateral displacements and the orientation of the vehicle on the road, and a group of ranges of states of the vehicle's lateral dynamics corresponding to the equilibrium points, each state of lateral dynamics comprising a lateral displacement, a rate of change of the lateral displacement, a lateral orientation, and a rate of change of the lateral orientation.Each region defines a control-invariant set of lateral dynamic states determined such that the vehicle having a state within a region is capable of maintaining its states within the region. In some implementations, each region includes a corresponding equilibrium point and overlaps with at least one neighboring region.

[0032] The method generates 120, using information 119 from at least one sensor that either has a direct connection to the vehicle or has a remote connection to the vehicle, a time series signal 121 indicating the movement of each object in the group of moving objects detected by the sensors. The measurements may include camera measurements or laser / LIDAR measurements. The measurements may include measurements from vehicle-to-vehicle communication, and the measurements may include GPS data. The length of the measurement sequence may vary from vehicle to vehicle.

[0033] Using the time series signal 121 and the information 111 retrieved from memory, the method determines 130 a data structure indicating collision-free regions at different control time steps. In the determination 130, a collision detector detects a collision of each region with at least one object at each control time step in a time horizon if a trajectory of the object at the control time step intersects a space defined by a longitudinal displacement of the vehicle at the control time step and a range of values ​​of the region's lateral displacement.Then, using the determined data structure 131 with the aid of a path planner, the method determines 140 a time sequence defining a collision-free sequence of regions, forming a unit of regions in space and time, connecting a region comprising an initial displacement with a region comprising a target displacement, to create a trajectory 141 within the unit, connecting the initial displacement with the target displacement. Finally, the method controls 150 the vehicle to create a vehicle movement 151 according to the trajectory 141.

[0034] Fig. 3A shows a block diagram of a control system 299 for controlling the movement of a vehicle 300 traveling on a road shared with a group of moving objects, according to some embodiments. The vehicle may also receive external inputs 310 that override the commands of the control system 299. In such a case, the vehicle is a semi-autonomous vehicle.

[0035] The control system 299 includes a memory 320 for storing a group of equilibrium points defining the lateral displacements on the road traveled by the vehicle, and for controlling a group of ranges of states of the lateral dynamics of the vehicle corresponding to the group of equilibrium points, each state of the lateral dynamics comprising a lateral displacement, a rate of change of the lateral displacement, a lateral orientation, and a rate of change of the lateral orientation, each range defining a control-invariant group of the states of the lateral dynamics determined such that the vehicle, having a state within a range, is able to maintain its states within the range, each range comprising a corresponding equilibrium point and overlapping with at least one adjacent range.

[0036] The control system includes a sensing system 330 having at least one sensor for generating a time-series signal 331 indicative of the movement of each vehicle in the group of vehicles moving in the environment 370. For example, the sensing system 330 may measure positions using Global Positioning System (GPS) information and / or inertial motion using an inertial measurement unit (IMU). For example, the IMU may include one or more 3-axis accelerometers, 3-axis gyroscopes, and / or magnetometers. The IMU may provide acceleration, velocity, orientation, and / or other position-related information to other components of the control system 299.

[0037] The control system 299 includes a collision detector 340 for detecting a collision of each region stored in memory 320 with at least one object at each control time step in a time horizon when a trajectory of the object at the control time step intersects a space defined by a longitudinal displacement of the vehicle at the control time step and a range of values ​​of the lateral displacement of the vehicle. The collision detector outputs 341 a data structure indicating collision-free regions at different control time steps.The data structure 341 is used by a route planner 350 embedded in the control system 299, which determines a time sequence defining a collision-free sequence of regions, forming a unit of regions in space and time, connecting a region comprising an initial displacement with a region comprising a target displacement, to generate a trajectory within the unit connecting the initial displacement with the target displacement. The determined trajectory 351 is used by at least one control unit 360 of the vehicle to control the vehicle according to the trajectory.

[0038] Fig. Figure 3B shows a general structure of the control system 299 according to one embodiment. The control system 299 includes at least one processor 390 for executing collision detection and course planning modules.The processor 390 is connected 391 to a memory 380 that stores a group of equilibrium points 381 defining lateral displacements and the lateral orientation on the road traveled by the vehicle, and a group of ranges 382 of states of the lateral dynamics of the vehicle corresponding to the group of equilibrium points, each state of the lateral dynamics comprising a lateral displacement, a rate of change of the lateral displacement, a lateral orientation, and a rate of change of the lateral orientation, each range defining a control-invariant group of the states of the lateral dynamics determined such that the vehicle, having a state within a range, is able to maintain its states within the range, each range comprising a corresponding equilibrium point and intersecting with at least one adjacent range.

[0039] Fig. 4A shows a schematic of a vehicle's movement goal, where vehicle 400 initiates a lane change from the left lane to the right lane, with the goal being to end up in the center of the right lane. Initiating the lane change can be done either by the control system or by external inputs to the control system. For example, a driver of the vehicle can initiate the lane change from the vehicle's dashboard. Some embodiments are based on the recognition that there are multiple trajectories that can lead to a final lateral target displacement of the vehicle. The state transitions defining trajectory 419 result in vehicle 400 reaching the center of the right lane at position 420, while trajectory 409 results in reaching the center of the right lane at position 410.Determining that 419 and 409 are collision-free may be computationally prohibitive in an environment with a number of obstacles and a multitude of trajectories for 409 and 419. For example, any position between 410 and 420 leads to a different trajectory, ultimately leading to an infinite number of possible trajectories leading to the desired lateral displacement. However, the exact trajectory leading to the desired lateral displacement of the vehicle is of less importance than the vehicle actually landing at the desired lateral displacement.

[0040] Fig. Figure 4B illustrates advantages of a control-invariant region according to some embodiments. A region 430 has been determined such that a vehicle having a state within a region is able to maintain its states within the region. By considering regions instead of individual trajectories, determining whether an object overlaps with region 430 is sufficient to exclude all trajectories 409 and 419 within that region without determining trajectories 409 and 419.

[0041] Accordingly, one embodiment defines regions associated with a lateral displacement of the vehicle, each region covering a plethora of state transitions and corresponding trajectories that the vehicle could undergo, and the lateral displacement defining an equilibrium point of the vehicle. For example, one embodiment defines the region as a control-invariant set of the states of the vehicle's lateral dynamics associated with a lateral displacement. A control-invariant set is a region associated with an equilibrium, i.e., a stationary point of the vehicle's lateral dynamics, and a control unit for controlling the vehicle's lateral movement. The size of the region depends on the parameters of the vehicle's lateral dynamics and the control unit used to control the vehicle's lateral dynamics.When using a control unit to control the lateral movement of the vehicle, a control-invariant group can be associated with the control unit and the equilibrium point, i.e., the lateral displacement, of the vehicle on the road.

[0042] Fig. 4C shows a schematic representation of an exemplary control-invariant region utilized by some embodiments. The initial state 399c of the vehicle is located within the control-invariant group 410c. The goal, i.e., the objective of the movement or lateral maneuver, is to utilize the controller to steer the vehicle to position 400c. Since the initial state 399c is located within the region 410c defined by the control-invariant group, the trajectory 420c resulting from steering the vehicle to the target displacement 400c is thus guaranteed to be within the region as long as the desired displacement remains unchanged.

[0043] In some embodiments, the lateral displacements are determined such that the corresponding ranges intersect. In this way, a corridor of ranges is constructed such that, if the ranges do not intersect with an object, they provide a safe sequence of lateral displacements and an associated trajectory to move the vehicle from the initial lateral displacement to the target lateral displacement.

[0044] Fig. 4D illustrates a scenario of movement through a group of control-invariant regions to a destination location 440d according to some embodiments. The region 420d assigned to 440d does not intersect with the region 410c covering the starting position 399c. To this end, one embodiment determines a different lateral displacement, illustrated by location 430d, which has the assigned region 410d overlapping with the region 410c and the region 420d. Thus, if the trajectory 450d is within the portion 460d of the region shared by at least two regions, the intermediate destination location 400c may be changed first to 430d and then to the final destination location 440d to create a trajectory embedded within the safe corridor of the region unit.

[0045] Fig. 5A shows an example of modeling the vehicle control in the frame of the vehicle according to some embodiments. Referring to Fig. 5A, it is recognized in one embodiment that by modeling the control of the vehicle 500a in the vehicle frame rather than in the global frame, the expressions are significantly simplified. However, due to modeling in the vehicle's local frame, an equilibrium point may have a lateral orientation in addition to a lateral displacement to account for the inertial effects when transitioning from the global to the local frame. In this example, the motion goal is to move the vehicle from the lateral displacement 500a to the final lateral displacement 510a. For example, modeling the road in the global frame 520a results in control-invariant groups, which lead to regions 540a with complicated shapes that are difficult to express mathematically. Instead, numerical, approximate approaches must be used.However, by modeling in the local frame 530 of the vehicle, the control-invariant groups have simple shapes, such as ellipsoids 550a, which can be expressed exactly and thus more accurately.

[0046] As used by various embodiments, the control-invariant groups are regions in the forward and lateral dimensions of the vehicle. Generally, however, the groups are higher-dimensional and include the state of the vehicle around the equilibrium point, where each state of the lateral dynamics includes a lateral displacement, a rate of change of the lateral displacement, a lateral orientation, and a rate of change of the lateral orientation, where each region defines a control-invariant group of the states of the lateral dynamics determined such that the vehicle, having a state within a region, is capable of maintaining its states within the region. The model of the vehicle can take several forms. In one embodiment, the vehicle model is a kinematic model in which forces are ignored. Such models are suitable under constrained driving conditions.In another embodiment, a vehicle model is expressed as a dynamic system from Newtonian force-mass equilibrium.

[0047] Fig. Figure 5B shows a schematic representation of a model of a state of a vehicle used by some embodiments. The state is modeled, for example, with where e y and e ψ = ψ - ψ d denote the lateral position and the vehicle orientation, respectively, in the road-oriented coordinate frame, and ψ d is the angle of the tangent of the road with respect to the inertial frame 510b. The vehicle model can be written as a linear dynamic system, where the term D e ψ̇ dis a disturbance acting on the vehicle model caused by the change of the coordinate system from the global to the local system. In one embodiment, this disturbance is canceled by the control unit used to construct the control-invariant groups.

[0048] The Fig. 5C and Fig. 5D show examples of the effects of the system's physical constraints on the control-invariant groups. In particular, the control-invariant groups can change their shape and form depending on the system's physical constraints. Fig. For example, Figure 5C on the left shows a control-invariant group 510c at time 0, where the size of the region is limited so that it does not overlap with the obstacles 520c. x=[ey e˙y eψ e˙ψ]Tx˙=Aex+Beδ+Deψ˙d

[0049] Furthermore, the range at time 0 intersects with the range 530c at time 1, which means that it is possible to switch from 509c to 529c in a single step. However, if limits are also set on the vehicle's steering, the control-invariant group can be smaller, since it is not possible to guarantee staying in a large range due to the limited actuation. Thus, as in Fig. As shown in Figure 5D, the control-invariant group 510c has shrunk to 510d and no longer overlaps with 530c. Therefore, it is not possible to find a collision-free trajectory from 509c to 529c.

[0050] In some embodiments, the group of ranges for the lateral dynamics of the vehicle is predetermined without taking the longitudinal dynamics of the vehicle into account. Such decoupling allows the same group of ranges to be reused for different longitudinal displacements of the vehicle to determine collisions of different ranges at different times. In alternative embodiments, the group of ranges for the lateral dynamics of the vehicle is predetermined taking the longitudinal dynamics of the vehicle into account. For example, in one embodiment, different groups of ranges are determined for different values ​​of the longitudinal velocity of the vehicle. This embodiment allows the group of ranges to be selected according to a current and / or desired longitudinal velocity of the vehicle.

[0051] Fig. Figure 5E shows an alternative example for determining connections between equilibrium points. In general, the state is not only maintained in the control-invariant group as time increases, but also converges to the equilibrium point, implying that the effective control-invariant group changes its size over time. Thus, in one embodiment, the determination of the safe corridor is based on the shrinkage of the size of the safe region over time. Fig. For example, in Figure 5E, the goal is to move from 509e to 529e, with the corresponding invariant groups labeled 510e and 530e. In a single control step, the invariant group 530e changes size to 540e, which means that all states contained in 540e can reach the invariant group 530e in a single control step. However, the group 540e does not contain the group 510e, so it is not certain that all states in 510e can reach 530e. However, in a further control step, the group 530e has increased in size to 550e, which covers 510e. Thus, it is possible to reach the invariant group 530e associated with the equilibrium point 529e in two time steps of control from any state in 510e, which implies that it is possible to move from 509e to 529e in two time steps of control.

[0052] Fig. 6A shows an example of a state transition graph defining possible transitions between equilibrium points corresponding to different regions for different time steps of the controller without considering the obstacles, according to some embodiments. In the example of Fig. 6A, the vehicle is in the area corresponding to lateral displacement 600a, and the movement goal is to move to lateral displacement 610a. For example, for each time step of the controller, there are three lateral displacements, equilibrium points, on the road. The possible transitions 620, which represent possible time sequences for achieving the desired lateral displacement, form a state transition graph stored in memory. Fig. 6A, the possible transition to achieve the lateral displacement 610a can be evaluated, and the state time sequence corresponding to 630a is the sequence of equilibrium points that achieve the lateral displacement in at least 2 time steps of the control, resulting in the creation of the trajectory 640a.

[0053] Fig. Figure 6B shows the graph of Fig. 6A, updated in response to detecting the obstacles to form a data structure indicating the collision-free regions, according to some embodiments. Fig. For example, Figure 6B shows the situation where the collision detector detects an obstacle 630b, thereby invalidating the transitions 640b.

[0054] In some embodiments, the state transition graph is predetermined under the assumption that there are no obstacles, wherein the collision detector invalidates at least some transitions on the graph in response to detecting the obstacles, and wherein the path planner searches the valid transitions of the graph to find the unit of regions. For example, the path planner searches for the minimum time sequence 650b, which is now in 3 time steps of the controller. Additionally or alternatively, in some embodiments, the transitions contain uncertainties, so that the state transition graph is a Markov chain.

[0055] In one embodiment, the set of regions is precalculated for all possible variations in road conditions and stored in the control system's memory. In this embodiment, the variation in the shape of the road that affects the vehicle's lateral dynamics is handled by a motion control unit, for example, the motion control unit associated with the control-invariant groups.

[0056] Fig. Figure 6C shows a schematic representation of a data structure indicating the state transition graph used by some embodiments. For example, one embodiment stores the set of equilibrium points across the controller's time steps, indicating the state transition graph as an upper block diagonal matrix, where all elements leading to possible transitions are nonzero, and where all elements leading to non-possible transitions are zero. For example, if the number of controller time steps is N and the number of equilibrium points is M, then the matrix has size MN x MN, with the structure as shown in Fig. 6C, where the value of the non-zero elements is determined by the cost of moving from one equilibrium point to another equilibrium point, as explained by other embodiments of the invention. Each element 600c contains M elements, and there are N elements 600c in total. All elements below the diagonal are zero because an equilibrium point cannot be reached with less than a single step, i.e., the system has temporal causality. The elements up to the right side of the matrix are zero because the invariant groups have a finite size.

[0057] Collision detector 340 estimates the future motion based on the time-series signal indicating the object's motion. In one embodiment, the collision detector determines whether the object's future motion will overlap with each region associated with lateral displacement.

[0058] Fig. 7A shows a situation in which portions 710a of the safe regions 700a overlap with the predicted obstacle 720a, and thus the collision detector invalidates the transitions on the graph corresponding to the regions 710a. In this way, the path planner determines a temporal sequence defining a collision-free sequence of regions, forming a unit of regions in space and time, connecting a region comprising an initial displacement with a region comprising a target displacement to create a trajectory within the unit, connecting the initial displacement with the target displacement. For example, the unit of regions 735a and 745a connects the state or equilibrium points 730a and 740a in space and time to provide a collision-free corridor. Any trajectory within this corridor is collision-free.

[0059] For example, in one embodiment, the path planner determines the trajectory by optimizing a cost function of the vehicle's movement within the unit. Additionally or alternatively, in one embodiment, the course planner determines the shortest time sequence that forms the unit. For example, the path planner may determine the trajectory connecting the equilibrium points that define the initial displacement and the target displacement.

[0060] In some embodiments, the predicted trajectory of the obstacles includes at least a heading, a speed, and an orientation / heading, but may also include other entities such as rotation speeds, accelerations, and steering. Furthermore, the movement may be represented as a probability density function (PDF), which measures the probability that the movement is at a particular location at a particular time.

[0061] In some embodiments, the collision detector estimates a future trajectory of the object in accordance with the time series signal indicative of the object's motion, estimates a probability of intersection of the future trajectory with each region at each control time step, and detects the region as collision-free at the control time step if the probability of intersection is below a threshold.

[0062] Fig. 7B illustrates the scheme for probabilistically determining the collision-free regions according to one embodiment. In Fig. For example, in Figure 7B, the estimated location of an obstacle 700b at a single control step and associated PDF 710b of the location overlaps with the area 720b corresponding to the equilibrium point 730b at the single control step. If the probability of overlapping with the boundary 721b of the safe area is greater than the threshold 740b, the transitions leading to the equilibrium point 730b are invalidated and removed from the path planner's graph search by adjusting the corresponding elements of the corresponding upper block diagonal matrix.

[0063] Some embodiments are based on the understanding that vehicles moving in the environment shared with the host vehicle only pose a threat to the host vehicle if the vehicles are moving on trajectories that intersect the host vehicle's trajectory. However, this statement can be reversed, leading to the understanding that a hypothetical trajectory that intersects the host vehicle's trajectory only poses a threat to the host vehicle if there is another vehicle traveling on that hypothetical trajectory.

[0064] Some embodiments are based on the realization that it is possible and computationally more efficient to assess the danger of the trajectories for the host vehicle, rather than the danger of the movement of other vehicles for the host vehicle. This is because there is only a limited number of feasible trajectories dictated by the drivable area, road traffic, and / or the map of the environment. These feasible trajectories can be pre-calculated in a computationally efficient manner, and it is easier to test whether the vehicle follows the pre-calculated trajectory than to generate all possible trajectories for the moving vehicle. In this way, the problem of generating optimal trajectories is replaced by the assignment and / or classification problem, which is computationally more efficient.

[0065] Some embodiments are based on the realization that the intersection of a feasible trajectory with the trajectory of the host vehicle can be determined probabilistically. In this way, the feasible trajectory can be represented by a probability density function to cover a wider space of a drivable area, thereby reducing the number of feasible trajectories to be generated. Similarly, the assignment of the vehicle to a feasible trajectory can also be determined probabilistically to represent the uncertainty of the movement measurements, the possibility of a rapid change in the driving intention of a driver of the vehicle, and the possibility of movement along segments belonging to multiple trajectories.

[0066] To this end, some embodiments determine a level of risk of a feasible trajectory as a combination of the probability that the feasible trajectory overlaps with the trajectory of the host vehicle and the probability that the feasible trajectory is followed by at least one vehicle.

[0067] Fig. 8A shows an example implementation of the collision detector for determining 130 the data structure indicating collision-free regions, according to one embodiment. The example implementation determines 810 a set of feasible trajectories of hypothetical objects moving within a travel region of the vehicle. Then, the example implementation determines 820 a probability of each feasible trajectory to intersect with each region at each time step of the controller. Then, using the time series signals, the example implementation determines a probability of each feasible trajectory to be followed by at least one object.Finally, a level of risk of each feasible trajectory intersecting with each region at each control time step is determined as a combination of the probability of the feasible trajectory intersecting with each region at each control time step and the probability of the feasible trajectory being tracked by at least one object; and the example implementation concludes detecting a region at a control time step collision-free if the level of risk of all feasible trajectories intersecting with the region at the control time step is below a threshold.

[0068] In some embodiments, the feasible trajectories are determined using a kinematic model of a movement of a hypothetical vehicle, e.g., by generating the feasible trajectories for different speeds of the hypothetical vehicles at different locations in the environment, moving with different driving intentions selected from a group of driving intentions.

[0069] Additionally or alternatively, some embodiments determine the feasible trajectories by iteratively sampling the state space of a hypothetical object and adding a sampled state to a feasible trajectory for an intention of movement of the hypothetical object if the sampled state is consistent with the intention of movement.

[0070] Fig. 8B shows a flowchart of a method 810 for generating a feasible trajectory and determining its probability of intersection with a region, according to some embodiments. The method iteratively determines a sequence of states that specify the movement of the vehicle from a hypothetical initial state of the vehicle to a target lane of the vehicle, e.g., while fulfilling different intentions of a hypothetical driver. In various embodiments, the hypothetical initial state is a state of vehicles observed in the past and / or wherein the hypothetical initial state is the corresponding state determined during a previous iteration of the method.

[0071] Fig. 8C shows a table listing different intentions of a hypothetical driver, according to some embodiments. For example, some embodiments determine the feasible trajectories for each hypothetical vehicle performing different environmentally permissible driving intentions that move the hypothetical vehicle into the host vehicle's travel area. For example, the set of driving intentions includes one or a combination of a left-turn intention, a right-turn intention, a straight-ahead intention, a left-lane-change intention, a right-lane-change intention, a deceleration intention, an acceleration intention, and a maintain-speed intention.

[0072] The movement is defined by the state transitions that connect the vehicle states. Each state includes a location, a speed, and a direction of movement of the vehicle. The movement is determined iteratively until a termination condition is met, e.g., for a period of time, for a predetermined number of iterations, or as long as the feasible trajectory lies within the region of interest. The termination condition can be set relative to the planning horizon in the path planner 350. For a manually operated vehicle, the termination condition can be set relative to the visibility of the road segment.

[0073] Fig. Figure 8D shows a sequence of state transitions that represent an iteration of the procedure from Fig. 8B. The method determines 800b, starting from an initial state 800d, a group of sampled states 820d and 870d and a corresponding group of state transitions 821d such that the state and transition satisfy static and dynamic constraints for the state of the vehicle. For example, the method begins at state 880d, determines state 860d and state transition 881d in Fig. 8D, while avoiding obstacles 890d and fulfilling restrictions 830d for the movement of the vehicle from the environment map.

[0074] Fig. Figure 8E shows a flowchart of an exemplary implementation of determining the set of sampled states 800b according to one embodiment. Determining 800b uses information about the environment, the states determined from previous iterations, and the driver's intentions in Fig. 8C. Examples of the previous states include nodes 880d, 870d, 820d, and the initial state 800d in Fig. 8D.

[0075] The example implementation performs an initial determination of a state 801e. If this is the first iteration of the method, the initial state is the current hypothetical state of the vehicle, with an associated uncertainty. The uncertainty can be in the form of a confidence interval or can be in the form of multiple possible locations. Such a scenario could arise, for example, if the sensing system provides very uncertain measurements or if the map is uncertain. Otherwise, the initial state is determined by previous iterations of the method.

[0076] The method samples 802e a group of predicted states that satisfy constraints on the vehicle's motion and can be predetermined or made adaptive. In some embodiments of the invention, the states 802e are generated from a fictitious noise source of the dynamic system, ie, from, with the nominal input as the mean value of the input. w k For example, it can be calculated from a Gaussian distribution wk∼N(uk,Qk) resulting or chosen as a probability density function (PDF) tailored to the particular intention associated with the feasible trajectory.

[0077] In other embodiments of the invention, the sampled states 802e are determined using the intentions in Fig. 8C. The driver's intentions are known in advance, and the states generated by the noise source of the dynamic system are corrected to better fulfill the intentions. For example, a probabilistic function q(x k+1 |x k , y k+1 ) can be used to generate states, where is a function of the state at time index k+1 given the state at time index and the specification at time index k+1.

[0078] As a particular example, if both and are additive, Gaussian PDFs, q, can be chosen as q(xk+1|xki,Yk+1)=p(xk+1|xki,Yk+1)=N(xk+1|xk+1i,(∑)k+1−1), = where xk+1i=f(xki)+wki+Lki(yk+1−Hkif(xki)),∑k+1i=((Hki)TRk+1−1(Hki)+Qk−1)−1and Lki=(Qk(Hki)T(HkiQk(Hki)T+Rk+1−1)−1), That is, the state can be generated as a random sample from the noise source of the dynamic system, propagated by the mathematical description of the vehicle and modified with a deterministic term to account for the deviation from the intention, and the corresponding state is predicted using this modified term.

[0079] In one embodiment, the generation of the sampled states 802e and the prediction of the corresponding state are performed in a loop, wherein the number of iterations is determined in advance. In another embodiment, the timely generation of the states 802e is based on the time steps of the intended trajectory. For example, the number of iterations can be determined as a fixed number of steps, or the iterations can be determined as a function of the resolution of the sensors of the sampling system 330. If 802e is the executed time step, the states are generated according to all locations corresponding to the intended trajectory from the time index k+1 to the time index k+T, i.e., q(x k+1 |x k , y k+1 , ..., y k+T )

[0080] Fig. Figure 8F shows a flowchart of a method for updating the probability that the sampled states match the Fig. 8C. Specifically, the method 812f determines whether the sampled state is consistent with a state leading to the target state consistent with the driver's intentions, and updates 813f the probability of the state based on the consistency assessment. For example, if the driver's intention is to move to the left lane, the target state may be in the middle of the left lane, and the intermediate states leading to the target state are consistent with the driver's intention, while other states are inconsistent with the driver's intention.

[0081] In some embodiments, determining 812e is performed as a combination of the PDF of the intentions / constraints, p(yk+1|xk+1i) the next state and the probability ωki of the state determined during the previous iteration. For example, if states are generated according to the dynamic model of the vehicle, the probabilities are proportional to the PDF of the specifications, that is, ωk+1i∝p(yk+1|xk+1i)ωki. As another example, if the sampling of the states according to p(xk+1|xk,iYk+1) the probabilities are proportional to the prediction of the PDF of the intended trajectories, ie ωk+1i∝P(yk+1|xki)ωki. In one embodiment, the probabilities are normalized to represent a PDF.

[0082] In one embodiment, in some time steps, non-zero but low probability states are replaced by higher probability states. For example, one embodiment creates a new set of states in such a way that the probability of creating xki,ωki In another embodiment, the replacement is performed whenever the inverse sum of squares of the probabilities is below a predefined threshold. This ensures that only likely good states are used.

[0083] Determining 820b the state can be done in different ways. For example, one embodiment determines states by using a weighted average function to create the state as xk+1=∑i=1Nωk+1ixk+1i. Another embodiment determines the state as the state with the highest probability, that is, i=argmax ωk+1i. Additionally or alternatively, an embodiment determines the state by averaging over a fixed number m < N of sampled states.

[0084] Fig. Figure 8G shows a simplified diagram of the iteration of the method, which generates a state for each discrete section PDF. In this example, states 823g and 824g are selected for a single iteration. Selecting multiple states for at least one iteration results in a set of moves that connect the current state of the vehicle to the target state of the vehicle. To this end, one embodiment selects from the set of moves the move that optimizes a cost function.

[0085] Fig. Figure 8H shows possible associated probabilities of the five states at the first iteration in Fig. 8G. These probabilities 821h, 822h, 823h, 824h, and 825h are reflected in the selection of the sizes of the points representing the states 821g, 822g, 823g, 824g, and 825g.

[0086] Referring back to Fig. 8G, states 823g and 824g become the initial state for the next iteration, which in turn creates five sampled state transitions of the vehicle from the initial states to the next states.

[0087] Fig. Figure 8I shows a graph of a total probability density function used by one embodiment. For example, to determine the probability that a feasible trajectory intersects with each region of the controller's time step, the feasible trajectories include a probability distribution from determining 810b. In one embodiment, with reference to Fig. 8I, the feasible trajectory is determined from the total probability density as the trajectory with the highest probability 810i, and in one embodiment, this is determined as the aggregated states 820i over the density. In one embodiment, the probability is determined by how large the portion 830i of the PDF at a given time is that overlaps with the region at a given time step. For example, if the entire PDF overlaps, the probability is 1.

[0088] The probability that a feasible trajectory overlaps with the intended trajectory of the host vehicle is not sufficient to determine the level of risk of a feasible trajectory because it does not indicate whether any vehicle will follow the feasible trajectory.

[0089] Fig. Figure 9 shows an example of possible realizable trajectories of a vehicle. Fig. 9, in which there are 8 possible trajectories that a vehicle 910 can follow. The hypothetical trajectories 920 are inconsistent with the measured vehicle 910, so the subset of feasible trajectories includes 930. Assume that the probability of an overlap of 930 with the host vehicle's trajectory has been determined. Then, in one embodiment, the level of risk is determined as a combination of the probability that the feasible trajectory overlaps with the host vehicle's trajectory and the probability that the feasible trajectory is followed by at least one vehicle.

[0090] One embodiment classifies the vehicle's time-series signal to create a driving intention of the vehicle and updates the probabilities of each feasible trajectory from the subset based on the consistency of the feasible trajectories from the subset with the vehicle's intention. The idea is that previously observed data can be used to predict future decisions. For example, by learning the characteristics of a lane-changing driver, future lane changes can be predicted.

[0091] Fig. 10A shows a time series signal of a steering trajectory of the vehicle's steering wheel and an acceleration trajectory of the vehicle, which is used by some embodiments to determine the intention of a driver of a vehicle. The driver initiates a lane change at time 1010a. However, it may be observed that a slight increase 1020a in the steering angle occurs for a brief moment before the lane change. This may be due, for example, to the driver of the vehicle looking over their shoulder to see if there are any vehicles in the blind spot. It can also be seen that the acceleration 1030a slowly increases before the lane change is initiated and then is maintained throughout the lane change. Therefore, information from recorded data can be used to predict future intentions.

[0092] Fig. 10B shows a flowchart of an exemplary implementation of a method 1010b for updating the probabilities 830 of each feasible trajectory to be tracked based on the consistency of the feasible trajectories from the subset with the vehicle's intention from the time series signal, according to some embodiments. The method determines 1010b the vehicle's intention. For example, one embodiment considers, as in Fig. 8C exemplifies the following intentions: left turn 810c, right turn 820c, straight ahead 830c, left lane change 840c, right lane change 850c, in addition to braking 860c, accelerating 870c, or maintaining speed 880c. Some embodiments learn how to choose the intention by observing the behavior of many vehicles. For example, one embodiment uses vehicle data with known intentions to train a random forest algorithm based on deep decision trees, another embodiment trains a support vector machine, and a third embodiment trains a neural network.

[0093] Then, method 1020b updates the probability of the feasible trajectory the vehicle should follow. In one embodiment, the probability is updated by the probability 810i of the feasible trajectory generated from 810i, i.e., the probability of the most likely trajectory. In another embodiment, the update is performed by the weighted average 820i over the PDF of the feasible trajectory.

[0094] One embodiment takes the probability of a feasible trajectory intersecting with the probability of a feasible trajectory being followed and weights these probabilities together. For example, one embodiment uses a weighted average of the probabilities.

[0095] Several embodiments of the invention utilize the risk level to adjust the host vehicle's trajectory. For example, in one embodiment, if the risk level is above a certain threshold, the time to collision between the feasible vehicle trajectory and the host vehicle's motion planner trajectory is calculated. One embodiment renders the risk level to the vehicle's driver as a time-to-collision warning tone, a signal on the instrument panel, or both, and then adjusts the trajectory in response to input commands received from the driver. In another embodiment, the risk level above a threshold is communicated to a collision avoidance module of the host vehicle, which adjusts the trajectory in response to input commands received from the collision avoidance module.The collision avoidance module may, for example, be the vehicle's motion planner 350 or another control unit 360. The input commands may include steering wheel and throttle commands.

[0096] In some embodiments, the path planner finds the time sequence of points and the corresponding trajectory by optimizing a cost function of the movement of the vehicle within the unit. For example, as in Fig. As shown in Figure 6A, one cost function is the minimum time required to achieve a lateral displacement. Another cost function may be to optimize the smoothness of the vehicle's movement, for example, due to the need to provide a comfortable ride for the vehicle's occupants.

[0097] The trajectory 640a corresponding to the time sequence 630a can be determined in several ways. In one embodiment, the trajectory is determined by tracking the equilibrium points and switching between the equilibrium points at time steps of the controller determined by the path planner, as shown in Fig. 4D and Fig. 6A.

[0098] In other embodiments, the control system includes a speedometer for measuring a longitudinal speed of the vehicle, and the collision detector determines the longitudinal displacement of the vehicle at the time step of the control based on a product of the longitudinal speed and a time period corresponding to the time of the control. However, one embodiment assumes that there is no speedometer and instead, collision detection is based on a constant speed, estimated, for example, from the commanded longitudinal speed of the vehicle.

[0099] In another embodiment, the vehicle is equipped with a position sensor such as GPS to determine a current location of the vehicle, and in another embodiment, a transceiver is used to transmit the current location of the vehicle and, in response to the transmission, receive the set of areas determined for a shape of the road comprising the current location. Variable speed

[0100] Some embodiments are based on the understanding that the shape of the regions used to form a concatenated collision-free time thread changes with vehicle speed. For example, when a vehicle is traveling at high speed, it is possible to perform lane changes more quickly in time than at low speeds, which implies that the collision-free region is different.

[0101] Fig. Figure 11A shows a representation of the change in area size with different speeds. There are three lateral displacements 1110a, 1120a, and 1130a on the road. For a vehicle speed v1, it is possible to move from lateral displacement 1110a to 1120a by staying in area 1111a. At the same speed, it is not possible to move safely directly from 1110a to 1130a because area 1113a does not cover lateral displacement 1130a. However, for a different vehicle speed v2, it is possible to move from 1110a to both 1120a and 1130a because the areas 1112a and 1114a associated with that speed cover these two lateral displacements.

[0102] Fig. Figure 11B illustrates the determination of a safe sequence of ranges for different speeds using the three lateral displacements 1110a, 1120a, and 1130a. Two obstacles 1110b and 1120b make it impossible to stay at the same lateral displacement 1110a or to move at speed v1 to lateral displacement 1120a because ranges 1111a overlap with an obstacle. At the same time, it is not possible to move at speed v1 to lateral displacement 1130a because range 1113a does not overlap 1130a. However, by switching to vehicle speed v2, it is possible to move to lateral displacement 1130 because range 1114a associated with speed v2 overlaps 1130a. Thus, it is possible to safely avoid a collision with the obstacles 1110b and 1120b by changing the vehicle speed.

[0103] At different speeds, it is possible to incorporate flexibility in how the lateral displacements are placed on the road. For example, since the areas associated with different speeds vary in size, it is possible and may be advantageous to place some lateral displacements closer together for a given vehicle speed.

[0104] Fig. Figure 12A illustrates the selection of lateral displacements for different vehicle speeds. For speed v1, three lateral displacements on the road and three time steps are sufficient to get from displacement 1210a to 1220a. However, for speed v2, four lateral displacements are necessary, otherwise it is impossible to form a collision-free sequence of regions in space and time.

[0105] In one embodiment, the number of lateral displacements is determined by maximizing the distance between the lateral displacements for a given speed, while ensuring that for each lateral displacement other than the final displacement on the road, it is possible to move into at least one adjacent lateral displacement on either side of the current lateral displacement in a predefined number of control time steps. In other words, the placement of the lateral displacements is determined as the solution to an optimization problem that attempts to minimize the number of lateral displacements under the constraint that the displacements are still connected to each other.

[0106] In another embodiment, the number of lateral displacements is determined as the number of lateral displacements that maximizes smoothness for the vehicle's occupants while ensuring that the displacements overlap for the relevant control time steps. Smoothness is determined by minimizing a parameter associated with smoothness. For example, one embodiment models the steering rate as a control input to the vehicle included in the model of the vehicle dynamics and minimizes the control variable such that the control-invariant groups forming the safe regions still form a connection that enables a safe, collision-free sequence of regions.

[0107] Storing all the different ranges for each lateral displacement and each different velocity would be computationally and memory-prohibitive. To this end, some embodiments recognize that it is sufficient to store a subset of the ranges and use the same ranges for different velocities and lateral displacements.

[0108] Fig. 13A shows a diagram of selecting ranges for storage in memory according to some embodiments. In this example, lateral displacement 1310 has four ranges 1320, 1330, 1340, and 1350, each associated with a different vehicle speed. Range 1340 encompasses ranges 1330 and 1320, ensuring that range 1340 can also be used for speeds associated with ranges 1320 and 1330. This reduces the memory required to store the ranges.

[0109] In one embodiment, the same range is used for multiple speeds, and the determination of connectivity between lateral displacements for different control time steps is performed using the same range for different speeds. If an obstacle does not actually intersect range 1340, the obstacle also does not intersect any sub-range of range 1340. However, range 1350 is not contained in any of the other ranges and does not itself cover any other range. Therefore, for the lateral displacement 1310, it is sufficient to use two ranges 1340 and 1350, even though four different ranges were originally present.

[0110] To further reduce the number of regions that must be stored in memory, in one embodiment, an optimization routine is used to find a region such that region 1350 is also included in the plurality of regions. Fig. Figure 13B shows a situation where an additional region 1360b has been determined such that all other regions within that region are covered. In some implementations, this problem is posed as an estimation problem, where the goal is to minimize the volume of region 1360b while simultaneously covering all other regions associated with the other speeds, which involves finding a control unit associated with the region. This approach ensures conservatism, minimizing collisions and minimizing memory requirements.

[0111] When testing the obstacles that intersect region 1340, the obstacle location relative to the vehicle at a given control time step will be different for different vehicle speeds. While using the same regions for different speeds introduces a degree of conservatism, the different relative positions ensure that it is possible to distinguish the different regions for different speeds.

[0112] In some embodiments, the different speeds are ordered in sequence based on their desirability. For example, assuming that the speed limit for a particular road is the desired speed and exceeding the speed limit violates traffic regulations, a natural sequence is to start with the speed limit and work in descending order until a minimum speed is reached. Only then, assuming no collision-free sequence of zones is found, are speeds above the speed limit tested.

[0113] Fig. Figure 14A shows a representation of a possible speed arrangement. The first priority is to maintain the speed limit. If this is not possible, i.e., if there is no sequence of a unit of ranges that forms a collision-free sequence of lateral displacements, one embodiment iterates through the sequence of nominal vehicle speeds 1420 until a solution is found. Referring back to Fig. 12A This ensures that the number of lateral displacements can vary for different vehicle speeds to ensure connectivity for different speeds.

[0114] In one embodiment, the zero speed is in the sequence of speeds to be traversed. Fig. 14B shows another arrangement 1420 that does not include all speeds up to standstill.

[0115] The resolution of the group of nominal speeds, ie the number of nominal vehicle speeds to be tested for a collision-free sequence, may depend on the size of the safe areas. For example, if Fig. 13A, if range 1350 corresponds to the nominal speed v1, range 1340 to the nominal speed v2, range 1330 to the nominal speed v3, and range 1320 to the speed v4, it is not possible to immediately change the speed of v1 to any of v2, v3, or v4, since range 1350 does not cover any of the ranges 1340, 1330, or 1320. Referring to Fig. 5E, by determining the shrinkage of the different regions for different control time steps, it may be possible to achieve the speed v2 corresponding to region 1340 from the speed v1 corresponding to region 1350 in a certain number of control time steps. For example, if the region 530e in Fig. 5E is the area 1350 and the area 510e is the area 1340, by changing the size and shape of the area 530e for different time steps of the control, it is possible to achieve the speed v2 in two time steps of the control, since the area 550e corresponds to the area 530e after two time steps of the control.

[0116] In another embodiment, the degree of resolution or discretization of the vehicle speeds is determined based on smooth driving conditions that ensure a comfortable ride for the vehicle's occupants. For example, one embodiment models the vehicle's longitudinal speed control unit in determining the resolution of nominal vehicle speeds. A maximum and minimum allowable acceleration value is determined, and the speed resolution is determined as the maximum resolution that does not violate the minimum and maximum values.

[0117] Fig. 14C shows an illustration of how safe transition between different speeds can be ensured according to some embodiments of the invention. Multiple ranges 1410c, 1420c, 1430c, and 1440c are associated with the different vehicle speeds 1409c, each range being used for multiple speeds determined by other embodiments of the invention. For example, range 1410c is applicable for speeds v1, v11, v12, and v2. Now assume that the vehicle is traveling at speed v2, and there is no collision-free sequence of ranges for any of the speeds in the range of v1 and v2. However, range 1420c is applicable for speeds v2, v21, v22, and v3.Therefore, if there is a collision-free sequence of regions that leads to a collision-free trajectory, then it is possible to ensure a safe transition from a velocity v2 to one of the velocities v21, v22 and v3.

[0118] In one embodiment, the preferred speed is set by a vehicle occupant, e.g., via voice or the dashboard. In another embodiment, the preferred speed is the speed corresponding to the speed limit.

[0119] In one embodiment, the obstacle locations in the vehicle's coordinate system are determined relative to a nominal speed, such as the preferred speed. The obstacle locations and the subsequent invalidation of the regions relative to a second speed are then determined by translating the obstacle locations for different control time steps, taking into account the difference between the preferred speed and the second speed. This, and the use of nominal speeds, ensures that calculations are reduced.

[0120] Fig. 15 shows a flowchart of a method for controlling a vehicle traveling on a road shared with a group of moving obstacles or objects, hereinafter referred to interchangeably as vehicles. The method utilizes a memory to store information necessary to implement steps of the method. The method retrieves 1510 from memory 1509 a set of desired speeds and a set of equilibrium points defining the lateral displacements and orientation of the vehicle on the road, and a set of ranges of states of the vehicle's lateral dynamics corresponding to the equilibrium points, each state of the lateral dynamics including a lateral displacement, a rate of change of the lateral displacement, a lateral orientation, and a rate of change of the lateral orientation.Each region defines a control-invariant set of lateral dynamic states desired for at least one of the desired speeds, such that the vehicle having a state within a region is capable of maintaining its states within the region. In some implementations, each region includes a corresponding equilibrium point and overlaps with at least one adjacent region.

[0121] Using information 1519 from at least one sensor that has a direct connection to the vehicle or that has a remote connection to the vehicle, the method generates 1520 a time series signal 1521 indicating the movement of each object in the group of moving objects detected by the sensors. The measurements may include camera measurements or laser / LIDAR measurements. The measurements may include measurements from vehicle-to-vehicle communication, and the measurements may include GPS data. The length of the measurement sequence may vary from vehicle to vehicle.

[0122] Using the time series signal 1521 and the information retrieved from memory 1511, the method determines 1530 a data structure indicating collision-free regions at different control time steps. In determination 1530, a collision detector detects a collision of each region with at least one object at each control time step in a time horizon if a trajectory of the object at the control time step intersects a space defined by a longitudinal displacement of the vehicle at the control time step and a range of values ​​of the lateral displacement of the region.Then, using the determined data structure 1531 for at least one reference speed, the method determines 1540 a time sequence defining a collision-free sequence of regions, forming a unit of regions in space and time, connecting a region comprising an initial displacement with a region comprising a target displacement, to create a trajectory 1541 within the unit connecting the initial displacement with the target displacement. Finally, the method 1550 controls the vehicle to create a vehicle movement 1551 according to the trajectory 1541.

[0123] Fig. 16 shows an exemplary implementation of a method for determining 1530 the data structure indicating collision-free regions and for determining 1540 a collision-free sequence of lateral displacements and a corresponding vehicle trajectory according to embodiments of the invention. The method selects 1609 a desired speed of the vehicle. Then, based on the lateral displacements and the associated ranges for the desired speed, the method 1610 checks the prediction of the obstacles that are expected to intersect the regions. The method removes 1620 the corresponding lateral displacements for each time step of the controller that has at least one obstacle intersecting the associated region.Then, using the current position 1628 and a target displacement 1629, the method determines the initial displacement 1630 associated with the current position and attempts to find a solution to the target displacement. In one embodiment, the initial displacement is chosen as the displacement closest to the current position. If a solution is found, the method exits and controls the vehicle according to other embodiments of the invention. If there is no sequence of displacements that results in a collision-free solution, a different desired speed 1609 is selected, and the method performs another iteration.

[0124] In one embodiment, the path planner determines the trajectory by optimizing a cost function of the vehicle's movement within the unit of collision-free areas. For example, in one embodiment, the cost of transitioning between different displacements in control time steps is determined as a combination of the deviation from the center of the lane and the distance to surrounding obstacles.

[0125] Fig.Figure 17 shows an illustration of the choice of the cost function for the state transitions used by one embodiment. For example, the road may be a two-lane, one-way road. The cost of transitioning to a lateral shift, corresponding to a shift between lanes, increases as the control time steps are increased, and the cost of staying near the center of the lanes is kept low. This ensures that the path planner preferentially changes lanes early, increasing the distance to other objects on the road.

[0126] The above-described embodiments of the present invention may be implemented in a variety of ways. For example, the embodiments may be realized using hardware, software, or a combination thereof. When implemented in software, the software code may be executed on any suitable processor or collection of processors, whether provided in a single computer or distributed among multiple computers. Such processors may be implemented as integrated circuits with one or more processors in an integrated circuit component. However, a processor may be implemented using circuitry in any suitable format.

[0127] Additionally or alternatively, the embodiments described above may be implemented as a non-transitory, computer-readable storage medium embodying a processor-executable program for performing a method of different embodiments.

[0128] Furthermore, the various methods or processes described herein may be encoded as software executable on one or more processors using any of a variety of operating systems or platforms. Furthermore, this software may be written using any number of suitable programming languages ​​and / or programming or scripting tools, and may also be compiled as executable machine language code or intermediate code running on a framework or virtual machine. Typically, the functionality of the program modules may be arbitrarily combined or distributed in different embodiments.

[0129] Furthermore, embodiments of the invention may be illustrated as a method, for which an example has been provided. The actions performed within the method may be ordered in any suitable manner. Accordingly, embodiments may be constructed in which actions are performed in a different order than that illustrated, which may include performing some actions simultaneously, even if they are illustrated as sequential actions in illustrative embodiments.

Claims

[1] A control system (299) of a vehicle (101a, 300, 400, 500a, 910) for controlling the movement of the vehicle (101a, 300, 400, 500a, 910) moving on a road shared with a group of moving objects, comprising: a memory (109, 320, 380, 1509) for storing a group of equilibrium points defining lateral displacements on the road traveled by the vehicle (101a, 300, 400, 500a, 910), and for storing a group of ranges of states of the lateral dynamics of the vehicle (101a, 300, 400, 500a, 910) corresponding to the group of equilibrium points, each state of the lateral dynamics comprising a lateral displacement, a rate of change of the lateral displacement, a lateral orientation, and a rate of change of the lateral orientation, each range defining a control-invariant group of the states of the lateral dynamics determined for different speeds of the vehicle (101a, 300, 400, 500a, 910), such that the vehicle (101a, 300, 400, 500a, 910) which has a state within a range determined for a speed, is capable ofto maintain its states within the region while moving at the speed, each region containing a corresponding equilibrium point and overlapping with at least one adjacent region, and each equilibrium point being associated with one or more regions designed for different speeds; at least one sensor (300) for generating a time series signal indicative of a movement trajectory of each object in the group of moving objects on a road shared with the vehicle (101a, 300, 400, 500a, 910); a collision detector (340) for detecting a collision of each area determined for a first speed with at least one object at each time step of the control in a time horizon when the movement trajectory of the object at the time step of the control intersects a space defined by a longitudinal displacement of the vehicle (101a, 300, 400, 500a, 910) at the time step of the control and a range of values of the lateral displacement of the area to create a data structure indicating collision-free areas at different time steps of the control; a path planner (350) for determining a time sequence defining a collision-free sequence of regions forming a unit of regions in space and time connecting a region comprising an initial displacement to a region comprising a target displacement to generate a trajectory within the unit connecting the initial displacement to the target displacement; and a control unit (360) for controlling the vehicle (101a, 300, 400, 500a, 910) according to the trajectory. [2] The system (299) of claim 1, wherein the collision detector (340) detects a collision of each region determined for a second speed when the path planner (350) cannot determine the time sequence defining the collision-free sequence of regions defined for the first speed, and wherein the path planner (350) determines the collision-free sequence of regions determined for the second speed to control the vehicle (101a, 300, 400, 500a, 910). [3] The system (299) of claim 2, wherein the collision detector (340) iterates through different values of the speed until a value of the speed for which the collision-free sequence of regions determined for that value of the speed is found. [4] The system (299) of claim 2, wherein the first speed is a current speed of the vehicle (101a, 300, 400, 500a, 910). [5] The system (299) of claim 2, wherein a range determined for a balance point representing a current displacement of the vehicle (101a, 300, 400, 500a, 910) covers a sequence of speeds comprising the first speed and the second speed. [6] The system (299) of claim 2, wherein the collision detector (340) determines the obstacle locations with respect to the first speed by calculating future states of the vehicle (101a, 300, 400, 500a, 910) moving at the first speed, and determines the obstacle locations with respect to the second speed by adjusting the future states of the vehicle (101a, 300, 400, 500a, 910) moving at the first speed based on a difference between the first speed and the second speed. [7] The system (299) of claim 2, wherein the speeds for determining ranges are selected such that the ranges corresponding to different speeds at similar lateral displacements overlap for at least one speed. [8] The system (299) of claim 2, wherein the speeds are iteratively tested in descending order starting from a maximum speed corresponding to a speed limit. [9] The system (299) of claim 1, wherein the memory (109, 320, 380, 1509) stores a data structure indicative of a state transition graph defining possible transitions between regions for different time steps of control determined for the same and different speeds without consideration of obstacles, and wherein the collision detector (340) invalidates at least some transitions on the graph in response to detecting the obstacles to form the data structure indicative of the collision-free regions, and wherein the path planner (350) searches the valid transitions of the graph to find the unit of the regions. [10] The system (299) of claim 9, wherein the data structure indicating the state transition graph includes an upper block diagonal matrix. [11] The system (299) of claim 1, wherein the collision detector (340) estimates a future trajectory of the object consistent with the time series signal indicative of the object's motion, estimates a probability of intersection of the future trajectory with each region at each time step of the controller, and detects the region as collision-free at the time step of the controller if the probability of intersection is below a threshold. [12] The system (299) of claim 1, wherein the collision detector (340) is configured to Determining a group of feasible trajectories of hypothetical objects moving in a travel area of the vehicle (101a, 300, 400, 500a, 910); Determining a probability of each feasible trajectory to intersect with each region at each time step of the controller; determining, using the time series signals, a probability of each feasible trajectory being followed by at least one object; Determining a level of risk of each feasible trajectory intersecting with each region at each control time step as a combination of the probability of the feasible trajectory intersecting with each region at each control time step and the probability of the feasible trajectory being followed by at least one object; and Detecting an area at a control time step as collision-free if the level of risk of all feasible trajectories intersecting with the area at the control time step is below a threshold. [13] The system (299) of claim 12, wherein the feasible trajectories are determined using a kinematic model of motion of a hypothetical vehicle, comprising: Generating feasible trajectories for different speeds of hypothetical vehicles at different locations in the environment, moving with different driving intentions selected from a set of driving intentions. [14] The system (299) of claim 1, further comprising: a speedometer for measuring a current speed of the vehicle (101a, 300, 400, 500a, 910), wherein the collision detector (340) measures the longitudinal displacement of the vehicle (101a, 300, 400, 500a, 910) at the time step of the control based on a product of the current speed and a time period corresponding to the time step of the control. [15] A control method for controlling a movement of the vehicle (101a, 300, 400, 500a, 910) moving on a road shared with a group of moving objects, the method using at least one processor coupled to a memory (109, 320, 380, 1509) configured to store a group of equilibrium points defining lateral displacements on the road traveled by the vehicle (101a, 300, 400, 500a, 910) and to store a group of ranges of states of lateral dynamics of the vehicle (101a, 300, 400, 500a, 910) corresponding to the group of equilibrium points, each state of lateral dynamics comprising a lateral displacement, a rate of change of the lateral displacement, a lateral orientation and a rate of change of the lateral orientation, where each area defines a control-invariant group of states of lateral dynamics,which is intended for different speeds of the vehicle (101a, 300, 400, 500a, 910), so that the vehicle (101a, 300, 400, 500a, 910), having a state within a range determined for a speed, is capable of maintaining its states within the range while moving at the speed, wherein each range contains a corresponding equilibrium point and overlaps with at least one adjacent range, and wherein each equilibrium point is associated with one or more ranges determined for different speeds, wherein the processor is coupled to stored instructions implementing the method, wherein the instructions, when executed by the processor, carry out steps of the method, comprising: generating a time series signal indicative of a motion trajectory of each object in the group of moving objects on a road shared with the vehicle (101a, 300, 400, 500a, 910); Detecting a collision of each area determined for a first speed with at least one object at each control time step in a time horizon when the movement trajectory of the object at the control time step intersects with a space defined by a longitudinal displacement of the vehicle (101a, 300, 400, 500a, 910) at the control time step and a range of values of the lateral displacement of the area to create a data structure indicating collision-free areas at different control time steps; Determining a time sequence defining a collision-free sequence of regions forming a unit of regions in space and time, connecting a region comprising an initial displacement with a region comprising a target displacement to create a trajectory within the unit connecting the initial displacement with the target displacement; and Controlling the vehicle (101a, 300, 400, 500a, 910) according to the trajectory.

Images