Prediction and planning for mobile robots

Abstract

In the presence of at least one agent, the self-actions of the mobile robot are autonomously planned. In a sampling phase, an objective is sampled for the agent from a set of available objectives based on a probability objective distribution, an observed agent trajectory is used to determine the probability objective distribution. Based on a probability trajectory distribution, an agent trajectory is sampled from a set of possible trajectories associated with the sampled objective, each trajectory in the set of possible trajectories reaching a location of the associated objective. In a simulation phase, a self-action is selected from a set of available self-actions, and based on the selected self-action, the sampled agent trajectory, and a current state of the mobile robot, a behavior of (i) the mobile robot and (ii) a simultaneous behavior of the agent are simulated to evaluate a feasibility of the selected self-action.

Description

Technical Field

[0001] This disclosure relates to the planning and forecasting of autonomous vehicles and other mobile robots. Background Technology

[0002] One rapidly emerging technology is autonomous vehicles (AVs) capable of navigating city roads on their own. These vehicles must not only perform complex maneuvers between humans and other vehicles, but they must do so frequently while rigorously limiting the likelihood of adverse events, such as collisions with other agents in the environment. Autonomous vehicles, also known as self-driving vehicles, are vehicles equipped with sensor systems for monitoring their external environment and control systems capable of making and executing driving decisions autonomously using these sensors. This includes, in particular, the ability to autonomously adapt the vehicle's speed and direction of travel based on perceived input from the sensor systems. Fully autonomous or "driverless" vehicles have sufficient decision-making capabilities to operate without any input from a human driver. However, the term autonomous vehicle used in this paper applies to semi-autonomous vehicles, which have more limited autonomous decision-making capabilities and therefore still require a degree of supervision from a human driver. Other mobile robots are under development, for example, for transporting goods and supplies within and outside industrial areas. Such mobile robots are also unmanned and belong to a class of mobile robots called UAVs (Unmanned Autonomous Vehicles). Autonomous aerial mobile robots (drones) are also under development. Summary of the Invention

[0003] A core problem addressed here is predicting the behavior of the agent so that the possible actions (self-actions) that the mobile robot might take can be assessed. This allows self-actions to be planned in a way that takes into account predictions of other vehicles. A major advantage of the solution taught here is interpretability. The ability to explain and justify the decisions made by autonomous systems is becoming increasingly important, especially when these decisions may be critical to safety (e.g., in autonomous driving). In the context of autonomous vehicles and other mobile robots, increased transparency in autonomous decision-making ultimately means that such systems can be configured, tested, and debugged more robustly, and ultimately operate in a more reliable and predictable manner. This invention uses probabilistic predictions to accommodate uncertainty in the predictions in a way that maintains interpretability.

[0004] A first aspect of the present invention provides a computer-implemented method for autonomously planning self-movements for a mobile robot in the presence of at least one agent (participant), the method comprising:

[0005] The observed trajectory of the receiving agent;

[0006] During the sampling phase:

[0007] Based on a probability target distribution, the proxy targets are sampled from the available target set, and the observed trajectory is used to determine the probability target distribution; and

[0008] Based on a probability trajectory distribution, proxy trajectories are sampled from a set of possible trajectories associated with the sampled target, each trajectory in the set reaching the location of the associated target; and

[0009] During the simulation phase:

[0010] Select a self-action for the mobile robot from the available set of self-actions, and

[0011] Based on the selected self-action, the sampled agent trajectory, and the current state of the mobile robot, simulate (i) the behavior of the mobile robot and (ii) the synchronization behavior of the agent to evaluate the feasibility of the selected self-action in the current state of the mobile robot.

[0012] In an embodiment, during the target identification phase, a target distribution can be calculated by determining the reward difference between (a) a first basic optimal trajectory and (b) a best available trajectory for each target, wherein the first basic optimal trajectory extends from the initial state of the observed trajectory to the target position, and the best available trajectory combines (b1) the observed trajectory from the initial state of the observed trajectory to the current state and (b2) a second basic optimal trajectory from the current state to the target position, wherein the probability trajectory distribution is determined by determining the reward for each trajectory in the set of possible trajectories. That is, the best available trajectory is constrained to match the observed trajectory with the current state.

[0013] The first basic optimal trajectory from the initial state to the target position can be calculated independently of the current state, or at least not constrained to match the observed trajectory with the current state.

[0014] The difference between the reward for each trajectory and the reward for each objective can be determined using a reward function that rewards reduced travel time while penalizing unsafe trajectories. The reward function may also penalize other reward factors, such as lack of comfort (e.g., one or more of longitudinal jerk, lateral jerk, and path curvature).

[0015] The sampling phase and (where applicable) target recognition phase may be “open-loop” because agent trajectories do not attempt to model the agent’s reactive behavior (i.e., they do not explicitly model any reaction of the agent to actions taken by the mobile robot and / or any other agent).

[0016] However, this does not mean that the simulation phase is limited to open-loop simulation. During the simulation phase, sampled agent trajector trajector trajector trajector behavior can be simulated in either open-loop or closed-loop manner. With closed-loop simulation, the simulated closed-loop agent trajectory can be determined based on the sampled agent trajectory (which can be open-loop) and the simulated set of agent observations and / or simulated sensor feedback. Simulated observations / feedback may cause the simulated closed-loop agent trajectory to deviate reactively from the sampled agent trajectory. For example, the closed-loop trajectory can simulate the effects of adaptive cruise control and / or proportional control (these effects do not need to be modeled during the trajectory sampling / target recognition phase).

[0017] Each trajectory can take the form of a sequence of states. States can encode positional information for a specific time instance, but they can also encode motion information, such as one or more of the following: (instantaneous) velocity, heading, acceleration (numerical and / or directional), jerk (the first time derivative of acceleration), etc. More typically, a trajectory can encode spatial information (path components) or motion information (motion components).

[0018] Through closed-loop simulation, motion components can be treated as indicators, allowing the agent's actual simulated motion to deviate from these indicators in a reactive manner.

[0019] The first and second trajectories can be determined during a first search process that seeks to optimize the reward or budget cost of the first and second trajectories. In the described example, a budget cost is used during the trajectory search process, and the budget cost only considers the travel time to the target location, ignoring any other cost factors (although any such other factors can still be considered when determining cost differences and rewards for individual trajectories).

[0020] As will be understood, the terms "minimize cost" and "maximize reward" are entirely synonymous. The described embodiments involve maximizing reward as defined by a reward function, which is entirely equivalent to minimizing cost as defined by a cost function. The choice of terminology does not imply any particular numerical representation of cost / reward—lower cost / higher reward can be represented by lower or higher numerical values.

[0021] The set of possible trajectories can be determined during the second search process, which is free to explore additional trajectories. This may include trajectories that do not reach the target location, but in such cases, trajectories found during the second tree search process that do reach the relevant target location will only be added to the set of possible trajectories.

[0022] The first search process and / or the second search process are performed on a search space of possible agent actions, wherein each trajectory corresponds to a searched agent action or a sequence of searched agent actions within the search space.

[0023] The sampling phase may include sampling current agent actions from a set of possible current agent actions based on a probability distribution of current actions determined for the agent.

[0024] The probability target distribution is determined using the observed trajectory and the sampled current agent action.

[0025] The probabilistic target distribution includes the conditional probability of each target given the observed trajectory and the sampled current agent maneuver.

[0026] The current action distribution is determined by a probabilistic maneuver detector based on one or more observations of the agent, such as one or more states of the observed trajectory.

[0027] The first search process and / or the second search process can be based on the sampled current agent action.

[0028] The simulation phase can include determining the predicted state of the mobile robot after the selected self-action has been completed.

[0029] Multiple iterations of the simulation phase can be performed, where an initial iteration is performed on the current mobile robot state (as above), and each subsequent iteration is performed for the predicted mobile robot state determined in previous iterations, until a termination condition is met (e.g., the simulated behavior is determined to violate a collision avoidance condition, or the mobile robot is determined to have reached a desired mobile robot position; in some embodiments, the additional termination condition may be a limit on the number of iterations that can be performed). In this case, each subsequent iteration would involve selecting a further self-action for the mobile robot and simulating its behavior based on the further self-action and the predicted mobile robot state of the previous iterations to evaluate the feasibility of the further self-action given the predicted self-state—the result is a time series of multiple selected self-actions evaluated over multiple iterations to predict increasingly more distant future mobile robot states.

[0030] Self-actions can be probabilistically selected during the simulation phase (in each iteration), for example, using probabilistic exploration techniques.

[0031] Multiple iterations of the trajectory sampling phase and the simulation phase constitute a single "rollout".

[0032] The method may include performing multiple rollouts, and within each rollout, a sampling phase may be repeated to sample the agent target to be considered in that rollout, the corresponding agent trajectory, and (where applicable) the current agent action. Due to the probabilistic nature of the sampling, this typically results in different agent targets, trajectories, and (where applicable) current actions being considered in different rollouts. Within each rollout, one or more iterations of the simulation phase may be performed, depending on the number of iterations required to reach the termination condition.

[0033] In other words, in each rollup, a single surrogate trajectory can be sampled and used as the basis for all iterations of the simulation phase of that rollup.

[0034] Multiple roll-ups are performed using a probabilistic tree search algorithm, such as Monte Carlo Tree Search (MCTS), where the root node of the search tree represents the current state of the mobile robot, the edges of the tree represent selected ego actions, and the appended nodes represent predicted states. Branches of the tree are explored in each roll-up through the continuous selection of ego actions during iterations in the simulation phase.

[0035] Each action can take the form of a basic maneuver, or it can be a macroscopic action formed by a sequence of multiple basic maneuvers.

[0036] Other aspects of this document provide a computer program that includes one or more computers programmed or otherwise configured to perform any of the methods disclosed herein, and a computer program configured to program a computer system to perform any such methods.

[0037] References are made to international patent applications PCT / EP2019 / 078062 and PCT / EP2019 / 078072 (the earlier applications), each of which is incorporated herein by reference in its entirety.

[0038] PCT / EP2019 / 078062 discloses a form of prediction based on "backward planning." Backward planning refers to a class of prediction methods that assumes the agent will plan its decisions in a predictable manner. Backward planning can be performed on possible maneuvers or behaviors to infer the agent's current maneuver / behavior based on relevant observations (a form of maneuver detection). Backward planning can also be performed on possible targets to infer the agent's possible targets (a form of target recognition). Target recognition and maneuver detection typically operate on different time scales. Target recognition typically considers longer future time periods compared to maneuver detection. PCT / EP2019 / 078072 specifically discloses a mobile robot planning framework based on Monte Carlo Tree Search (MCTS). This provides a structured framework for reasoning about the potential impact of decisions made by a mobile robot planner. Implementations of this framework incorporate predictions from backward planning to simulate ("unfold") the predicted behavior of an external agent within the MCTS planning framework.

[0039] The following describes embodiments incorporating target recognition within a probabilistic programming framework. In particular, the described embodiments consider a form of inverse programming target recognition within the MCTS framework. To some extent, these extend the teachings of earlier applications.

[0040] The preceding text referenced the cost of the trajectory and the budgeted cost in the context of the search process. In the following description, the former is described using the term "reward" (rather than cost), while the term "cost" (rather than budgeted cost) is used in the context of the search process. Attached Figure Description

[0041] To better understand the present invention and to illustrate how embodiments of the invention can be implemented, reference is made to the following figures, in which:

[0042] Figure 1 A sample driving scenario is shown, which includes the vehicle itself and multiple agents near the vehicle.

[0043] Figure 2 A schematic block diagram of the prediction and planning system is shown;

[0044] Figure 3 The average driving time required to complete four representative scenario instances in a simulation-based test is shown.

[0045] Figure 4 A results graph is displayed, demonstrating the system's ability to predict targets for a given agent as the scene evolves over time; the targets are predicted probabilistically, and the graph shows the probability of the correct target, which increases over time in each scene instance;

[0046] Figures 5 to 8 Four scenario examples are illustrated respectively;

[0047] Figure 9 A schematic functional block diagram of the AV runtime stack is shown;

[0048] Figure 10 A flowchart of an example inverse programming method applied to probabilistic target recognition is shown;

[0049] Figure 11 A schematic functional block diagram of a maneuver planning method is shown, which combines target prediction from backward planning; and

[0050] Figure 12 An example of a search tree constructed by the MCTS Mobility Planner is shown. Detailed Implementation

[0051] I. Overview

[0052] The embodiments of the present invention provide an integrated prediction and planning system for autonomous vehicles.

[0053] Figure 2 A schematic block diagram of system 200 is shown, which sequentially includes a target recognition module 202 and a self-planning module 212. Using a probabilistic behavior prediction framework, the system is able to evaluate and plan the actions (self-actions) of a mobile robot in the presence of one or more agents (e.g., other drivers, cyclists, pedestrians, etc.), such as as an autonomous vehicle (self-driving vehicle – abbreviated as EV or AV).

[0054] The target recognition module 202 is shown to include the following (sub)modules: a reverse planning module 204, a surrogate (non-self) target generator 206, and a maneuver detector 208. The maneuver detector 208 uses the Monte Carlo Tree Search (MCTC) algorithm as described later to plan maneuvers.

[0055] The self-planning module 212 is shown as including the following (sub)modules: route planner 214, self-goal generator 216, and maneuver planner 218. For simplicity, the maneuver planner 218 may be referred to as the planner. Unless otherwise stated, the following reference to the planner refers to the maneuver planner 218.

[0056] Agent target generator 206 determines one or more sets of available targets for each of one or more agents in a driving scenario. Available targets are determined based on contextual information, such as road layout. Self-target generator 216 operates in the same manner, determining one or more targets available to the self-vehicle. Targets to be executed by the AV are selected by route planner 214.

[0057] Maneuver planner 218 plans the optimal maneuver to be taken by the AV to reach the selected target location. Maneuver planner 218 uses target probabilities 210 and associated agent trajectory predictions 212 provided by inverse planner 204 to compute maneuver planning 220. Inverse planner 204 is a prediction component that predicts agent behavior based on the assumption that the agents also rationally plan to reach their own desired targets.

[0058] Maneuvering is one form of action considered in this paper. As described in further detail below, the following examples consider basic maneuvers and “macro-movements” as predefined sequences of maneuvers. However, this description is more generally applicable to other forms of action that can be taken and planned accordingly by mobile robots.

[0059] The target recognition module 202 and the self-planning module 214 use a common maneuver library and "macro-actions" (predefined maneuver sequences), and both utilize a velocity smoothing function 224 to smooth the calculated trajectory and a reward function 226 to use a metric-based approach to evaluate the trajectory associated with a specific target, where certain factors (e.g., progress toward the desired target) are rewarded, while others (e.g., lack of safety, comfort, etc.) are penalized. The probability of a specific trajectory relative to a given target is estimated under the assumption that the agent intends to execute an optimal trajectory with respect to these metrics (i.e., maximizing rewards).

[0060] The ability to predict the intentions and driving trajectories of other vehicles is a key issue in autonomous driving. This problem is highly complex because it requires fast and accurate predictions based on limited observational data derived from a dynamic, evolving environment with coupled multi-agent interactions.

[0061] One approach to making predictions tractable in this context is to assume the agent uses one of a finite number of different behaviors. These behaviors typically represent advanced maneuvers such as lane following, lane changing, turning, stopping, etc. Some type of classifier would be used to detect the maneuver currently being performed by the vehicle based on the observed driving trajectory, and then use the current maneuver to predict future trajectories to inform the controlled ego vehicle's planning. The limitation of this approach is that they only detect the current maneuvers of other vehicles, so planners using this prediction method are essentially limited to the time scale of the detected maneuvers.

[0062] Another approach is to assign a finite set of possible targets (e.g., various road exit points) to each other vehicle and plan a complete trajectory to each target based on the local conditions observed by the vehicle. While this approach can generate long-term predictions, it is limited by the fact that the generated trajectories must be followed relatively closely by the vehicle to produce high-confidence predictions.

[0063] Recent deep learning-based methods have shown promising results in trajectory prediction for autonomous driving. Predictive models are trained on large datasets made available through data collection activities involving sensored vehicles traversing urban roads. The resulting sensor information (typically various forms of vision and LiDAR, and sometimes radar) is used for predictions by models ranging from video prediction to more contextualized estimations that take environmental cues into account. Reliable predictions within seconds remain a challenge, partly due to the difficulty in capturing the coupled evolution of traffic. Solving problems such as... Figures 5 to 8 There is still much to be done regarding the scenarios shown. Another significant limitation of some of these methods is the difficulty in incorporating hard constraints into optimization objectives that must be differentiable (e.g., avoiding meaningless predictions for a given situation). Finally, one of the most significant limitations of these methods is the difficulty in extracting interpretable predictions in a form that is easily integrated with planning methods that effectively represent multidimensional and hierarchical task objectives.

[0064] To address the aforementioned problems, the fundamental principle of System 200 described in this paper is that, in order to predict future vehicle maneuvers, some clues are needed about why—that is, for what purpose—the vehicle performs its current and past maneuvers, which will provide information about its intended objectives. Understanding the objectives of other vehicles allows for the prediction of future maneuvers and trajectories related to their objectives. This long-term prediction facilitates planning over longer timescales to capitalize on opportunities that may not otherwise occur, such as... Figure 1 As illustrated in the example, prediction, to some extent, is a high-level maneuver built around the interpretation of the observed trajectory. The target recognition process helps to provide an intuitive explanation for debugging purposes, with a level of detail as shown in the example. Figure 1 As shown.

[0065] Figure 1 The scene depicts a junction with four possible destinations. The ego vehicle, labeled EV, aims to reach location G1. Car 3 travels eastward at a constant speed. Car 2 slows down until it reaches its indicated stopping position. EV might deduce that Car 2 is stopping because it intends to reach G2, but must wait for Car 3 to pass before turning towards G2. This provides the ego vehicle with an opportunity to turn onto the road while Car 2 waits.

[0066] Ultimately, as the world moves towards making autonomous systems more trustworthy, the ability to explain concepts and justify (explain) system decisions becomes increasingly important. In the current context, this applies to goal-based predictions and how these predictions influence autonomous driving decisions.

[0067] To this end, the integrated planning and prediction system 200 takes advantage of the computational advantage of using a finite maneuver space (whose expression is sufficient to explain the observed behavior), but extends the approach to the prediction of planning and maneuver sequences (i.e., planning). This is achieved through a novel integration of rational backward planning to identify targets of other vehicles, and Monte Carlo Tree Search (MCTS) [6] to plan optimal maneuvers for the ego vehicle. Backward planning and MCTS utilize a shared set of predefined maneuvers to construct a plan that can be explained by the principle of rationality, i.e., the plan is optimal with respect to a given metric. Instead of directly matching the plan, system 200 evaluates how rational the observed trajectory is for a given target, providing robustness regarding trajectory variability. The described MCTS algorithm performs a closed-loop forward simulation of vehicle dynamics and their coupled interactions, but by separating control from maneuver, system 200 is able to optimize the velocity curves between maneuvers and can utilize simplified open-loop control to improve the efficiency of backward planning.

[0068] System 200 has been evaluated in simulations of four urban driving scenarios, including Figure 1 The scenarios, including roundabout entry and dense lane merging, demonstrate that System 200 can robustly identify targets of other vehicles and generate near-optimal plans under different scenario initializations. Intuitive explanations can be extracted for the identified targets and maneuver predictions in each scenario, thus validating the system's decisions. Further details of this evaluation are provided in Appendix A below.

[0069] II. Preliminary Explanation and Problem Definition

[0070] It is the set of vehicles that interact with their local neighbors (including other vehicles). At time t, each vehicle... All are in local state Accepting local observation And you can select actions The joint state (of the self-vehicle and the agent) is represented as symbol s a:b Used to represent tuples (s) a ,…,s b ), similarly used Observe via p(o) i |s t Depending on the joint state, the action is transmitted via Depending on the observation. In system 200, the local state includes the vehicle's attitude, speed, and acceleration (where speed and velocity are used interchangeably); the observation includes the attitude and speed of nearby vehicles; and the action controls the vehicle's steering and acceleration.

[0071] Joint state sequence s 1:n The probability of n≥1 is given by the following formula.

[0072]

[0073] Where p(s) t+1 |s t ,a t Joint vehicle dynamics are defined, assuming independent local observations and actions: and Vehicles observe from their local area Respond to other vehicles.

[0074] The planning problem is defined as finding an optimal "strategy" π. * This strategy allows the self-vehicle ε to select actions to achieve a specified goal g. ε Meanwhile, the driving trajectory is optimized through a defined reward function.

[0075] The strategy is to observe the sequence (Observed trajectory) mapped to action The function π: The target can be any (partial) state description However, as an example, the following description focuses on specifying the target location of the object. Formally, define...

[0076]

[0077] in mean Satisfy g ε The second condition in (2) ensures that for any policy π, This also ensures the robustness of neutralization in (3). The problem is to find π. * To satisfy

[0078]

[0079] Among them, R i (s 1:n ) is s 1:n For the reward of vehicle i (see Section III-E below). Intuitively, maximizing (3) requires optimizing the probability of achieving the objective and the reward for generating the trajectory. In the described implementation, (3) is approximated using finite programming intervals (see Section III-G for details).

[0080] Although the experiments detailed in Appendix A used a scenario with a fixed objective, the problem formula described above allows the method to drive the entire route using a route planner module that continuously updates the vehicle's target position g based on the vehicle's current position on the route. ε .

[0081] III. Methods

[0082] A. System Overview

[0083] The described method relies on two assumptions: (1) each vehicle attempts to reach a (unknown) target location from a set of possible targets, and (2) each vehicle follows a plan generated from a finite library of defined maneuvers.

[0084] System 200 Approximate Optimal Strategy π * The process is as follows: For each other vehicle, its possible targets are listed, and backward planning is performed for that vehicle towards each target, giving the probability of the target and the predicted trajectory. The resulting target probabilities and trajectories inform the simulation process of the Monte Carlo Tree Search (MCTS) algorithm to generate the optimal maneuver plan for the vehicle itself. To keep the required search depth shallow and thus remain efficient, both backward planning and MCTS plan macroscopic actions, which flexibly connect maneuvers using contextual information. Figure 2 It provides an overview of the components in System 200.

[0085] The following sections describe in more detail Figure 2 Each component of the system 200 shown.

[0086] B. Maneuver

[0087] Assume that at any given time, each vehicle is performing one of a finite number of maneuvers 220a. In this example, system 200 uses the following maneuvers: lane following, left / right lane changing, left / right turning, yielding, and stopping.

[0088] Each maneuver ω specifies an applicability and termination condition. A maneuver is available in a given state if and only if the state satisfies the applicability condition. For example, a left lane change is only applicable to lanes to the left of the vehicle that have the same direction of travel (space constraints can also be checked). The maneuver terminates if the state satisfies the termination condition.

[0089] If applicable, the local trajectory that the motorized vehicle must follow. The local trajectory includes the reference path in the global coordinate system and the target velocity along the path. For ease of explanation, assume... and s i Using the same representation and index, but this is not usually the case (e.g., It can be indexed by longitudinal position rather than time (interpolation to time index is possible). In example system 200, the reference path is generated by fitting a Bézier curve function to a set of points extracted from the road topology, and the target speed is set using a domain heuristic similar to

[10] . In general, system 200 assumes that the vehicle will attempt to travel at a local speed limit where possible. This target is reduced to the speed of slower vehicles ahead in the same lane (if any) or as a function of the local curvature of the travel path.

[0090] A yield maneuver causes a vehicle to slow to a specified speed as it approaches a given location, typically an intersection or turning point where oncoming vehicles have priority. At that location, the maneuver terminates if the designated lane is clear (allowing the vehicle to continue the next maneuver without coming to a complete stop); otherwise, it comes to a complete stop and terminates once the designated lane is clear. When used as part of system 200, early termination of the yield maneuver is permitted if system 200 predicts safe entry (see Appendix A).

[0091] Lane following and yield maneuvers have open parameters in their termination conditions: for lane following, the parameter specifies the travel distance; for yielding, the parameter specifies the lane that must be monitored for oncoming vehicles. These open parameters are automatically set by the macro actions we define in the next section.

[0092] Compared to methods such as MPDM[9], here the maneuver and control are separated. This separation serves two purposes: First, when constructing the maneuver sequence, each maneuver must optimize its velocity profile for predicting subsequent maneuvers. This is not possible if control is integrated into an independent policy, as is the case in MPDM. Access to the trajectory allows the system to perform velocity-smoothing maneuvers across 200 trajectories, as described in Section III-D. Second, the maneuver can be simulated under different control models (open-loop, closed-loop) in inverse programming and MCTS, which provides different trade-offs between prediction accuracy and simulation efficiency.

[0093] C. Macro-level actions

[0094] Macro actions 222b connect one or more maneuvers in a flexible manner. Using macro actions alleviates the planner's burden in two important ways: they specify common maneuver sequences, and they automatically set free parameters within the maneuver based on contextual information (typically road layout). Table I defines examples of macro actions 222b used in system 200. The applicability conditions of a macro action are given by the applicability conditions of the first maneuver in the macro action, along with optional additional conditions (see Table I). The termination condition of a macro action is given by the termination condition of the last maneuver in the macro action.

[0095] Please note that the macro-movements used in this context do not define a hierarchy of decomposable actions; they simply define a sequence of actions.

[0096] Both backward programming (Section III-F) and MCTS (Section III-G) search for macroscopic actions rather than maneuvers, which significantly improves their efficiency by reducing the search depth.

[0097]

[0098] Table 1: Macroscopic Actions Used in Our System. Each macroscopic action connects to one or more maneuvers and sets their parameters (see Section III-C).

[0099] D. Speed ​​smoothing

[0100] To adapt to the natural changes in driving behavior and obtain a feasible trajectory for vehicle i across maneuvers, a velocity smoothing function 224 is applied to optimize the given trajectory. The target speed. Let... It is the vertical position on the reference path. and This is the corresponding target velocity, for 1 ≤ t ≤ n. The piecewise linear interpolation of the target velocity between points is represented as κ: x → v. Given the time Δt elapsed between two time steps; the maximum velocity and acceleration v. max / a max , and settings A smoothing problem is defined as

[0101]

[0102] x t+1 =x t +v t Δt

[0103] 0 <v t <v max

[0104] v t ≤k(x t )

[0105] Where λ>0 is the weight of the acceleration portion given the optimization objective. Treating the target velocity as the last constraint of the upper limit leads to a more realistic braking behavior in system 200.

[0106] Equation (4) is a nonlinear, nonconvex optimization problem that can be solved, for example, using primal dual interior point methods (e.g., IPOPT

[27] ). Based on the solution to the problem (x... 2:n ,v 2:n The system 200 performs interpolation to obtain the result at the original point. The achievable speed at that point. If for all t, Then system 200 can use this solution to... Perform interpolation. Otherwise, system 200 can be derived from x. n Start solving similar problems and repeat the process until all are solved. All are within the boundaries.

[0107] Velocity smoothing should be handled with care when dealing with zero velocity in the input trajectory, which represents a complete stop. A simple way to achieve this is to break the trajectory into segments separated by stopping events and apply the smoothing function to each segment.

[0108] E. Reward Function

[0109] As stated above, the "reward function" and the "cost function" are two ways of describing the same thing, namely, a function 226 that penalizes (i.e., prevents) certain trajectory features and rewards (i.e. encourages) other trajectory features. How the output of function 226 (the reward or cost for a given trajectory) is represented numerically is not important; the choice of specific terms (reward or cost) does not imply any particular numerical representation. A lower value may represent a lower cost / higher reward or a higher cost / lower reward compared to a higher value, and the choice of terms does not imply any limitation on this. The only requirement is that the numerical representation is consistent with the way any cost / reward optimization is performed.

[0110] In this example, the trajectory s of vehicle i 1:n The reward is defined as the weighted sum of the K reward components.

[0111]

[0112] weight w k >0 and System 200 includes reward components for execution time, longitudinal jerk, lateral jerk, path curvature, and safe distance to the vehicle ahead. Here, R i Represent the target g using mathematical symbols i The reward function is 226.

[0113] F. Target Recognition

[0114] By assuming each vehicle Use planning built from our defined macroscopic actions to seek to reach a finite number of possible target locations. One of them, the backward planner 204, uses a rational backward planning framework to compute the Bayesian posterior distribution of the target for vehicle i at time t (target probability 210).

[0115] p(g i |s 1:t )∝L(s1:t |g i )p(g i (6)

[0116] Wherein, L(s) 1:t |g i ) is i in a given target g i In the case of p(g), the probability of the observed trajectory, and p(g) i Specify target g i The prior probability.

[0117] The probability is calculated as a function of the reward difference between the two plans: from the initial observed state of i after velocity smoothing. To target g i Reward for the optimal trajectory Track the observed trajectory up to time t, then continue to the target g in the optimal manner. i trajectory rewards Smoothing is applied only to trajectories after t. Then, the probability is defined as...

[0118]

[0119] Here, β is a scaling parameter (e.g., β = 1). This definition assumes that the vehicle is rational in achieving its goal through optimized driving, but allows for a certain degree of deviation. If a goal cannot be reached, we set its probability to zero.

[0120] Velocity smoothing does not apply to the portion of the trajectory that has already been observed, i.e., s. 1:t Otherwise, the effect of speed smoothing might wash away evidence suggesting a certain target.

[0121] Algorithm 1 shows the pseudocode used to implement the target recognition algorithm, with more details in the following subsections. This algorithm identifies each vehicle i and target g... i and rewards Use parallel threads to achieve efficient parallel processing.

[0122] Algorithm 1: Target Recognition Algorithm

[0123] Input: Vehicle i, current movement ω i , observations 1:t

[0124] Return: Target probability p(g) i |s 1:t ,ω i )

[0125] 1. From the state Generate possible targets

[0126] 2. Set the prior probability p(g) i (For example, unification)

[0127] 3. For all Do:

[0128] from to g i

[0129] right Smooth application speed

[0130] ←Rewards

[0131] from to g i ,in

[0132] right Smooth application speed

[0133] ←Rewards

[0134]

[0135] Return p(g) i |s 1:t )∝L(s 1:t |g i )p(g i )

[0136] 1) Target Generation: The heuristic function is used to generate a set of possible targets based on the location of vehicle i and contextual information, such as road layout and traffic rules. System 200 defines a target for the end point of the vehicle's current road and multiple targets for the end points of each reachable connecting road, bounded by the vehicle's own field of vision (e.g., ...). Figure 1 (As shown). Infeasible targets are not included, such as locations behind vehicles.

[0137] In addition to static objectives that depend solely on vehicle position and road layout / rules, dynamic objectives can also be defined, such as those that depend on current traffic. For example, in the dense merging scenario considered in Appendix A below, a stop objective is dynamically added to simulate the vehicle's intention to allow ego vehicles to merge in front of it.

[0138] 2) Maneuver Detection: Maneuver detection is used to detect the maneuver currently being performed by a vehicle (at time t), allowing backward planning to complete the maneuver before forward planning. The maneuver detection module of system 200 calculates the probability p(ω) of the current maneuver for each vehicle i. i One option is a Bayesian change point detection algorithm, such as CHAMP

[19] . The motion detection method itself is known and will not be described in detail. In the experiments detailed below, a simulated detector was used.

[0139] Since different current maneuvers may imply different targets, for each possible current maneuver p(ω) i )>0 Perform backward programming. Therefore, each current maneuver generates its own target posterior probability, denoted as p(g) i |s 1:t ,ω i To improve efficiency, backward planning may be limited to certain subsets of maneuvers, such as the most likely maneuvers.

[0140] 3) Reverse planning: Reverse planning is performed using A* search

[12] on macroscopic actions. A* generates the initial trajectory after completion. Current maneuver ω i Then it begins. Each search node q corresponds to a state. The initial node is in state Macro-actions are filtered based on their applicability to s. A* selects the next macro-action, which leads to a target g. i The node q' with the lowest estimated total cost is given by the following formula: f(q') = l(q') + h(q').

[0141] Here, the term "cost" is used to maintain consistency with standard A* terminology and to distinguish the simpler definition of cost used in A* search from the reward function defined in Section III-E. By considering only travel time (see below), the cost h approximates the reward, which is sufficient for the purposes of A* search. However, different forms of cost / reward can be defined in this case.

[0142] The cost l(q') to reach node q' is given by the travel time from i's position in the initial search nodes to its position in q', following the trajectory of the macroscopic action that leads to q'. Estimate the travel time from q' to the target g. i The cost heuristic h(q') for the remaining cost is given by the travel time of i at position q' to reach the target in a straight line at the speed limit. According to A* theory, this definition of h(q') is acceptable, as it ensures the search returns an optimal plan. After finding the optimal plan, the initial segments of the planned maneuvers and the current maneuver are used to determine the optimal plan. Extract the complete trajectory

[0143] To minimize the computational cost of inverse programming, the current implementation uses open-loop control to perform macroscopic actions. This means simulating the trajectory using an idealized driving model that performs the trajectory with a linearly interpolated target speed. Secondly, System 200 operates under the assumption that all other vehicles are not planned after their observed trajectories, using a constant-speed lane-following model. This assumption allows for yield maneuvers and left / right macroscopic actions to predict when traffic will clear. Thirdly, no smoothing is applied during the search process, and an alternative cost definition based solely on approximate driving times is used. Finally, collisions are not checked during inverse programming; due to the use of open-loop control and the constant speeds of other vehicles, collisions may inevitably occur (note that predicted collisions are still detected and interpreted, just not in this particular context—see below). These simplifications result in a level of abstraction that has low computational cost and sufficient information for the target probability. However, it should be understood that this is merely one possible implementation, and other design choices may be employed in other implementations.

[0144] 4) Trajectory Prediction: In this example, system 200 predicts multiple seemingly reasonable trajectories 212 given the vehicle and target, rather than a single optimal trajectory. This is beneficial because in some cases, different trajectories may be (close to) optimal, but may lead to different predictions, which may require different behaviors from the vehicle itself.

[0145] To predict multiple trajectories and their probabilities for a given objective, a second A* search runs for a fixed period of time and is free to compute a set of plans (at most a fixed number) with corresponding rewards. Whenever the A* search finds a node that reaches the objective, the corresponding plan is added to the plan set. The A* search may find trajectories that do not reach the objective, but these trajectories are not added to the plan set considered in the next stage. Given an objective g... i The calculated trajectory set With initial maneuver ω i And related rewards after smoothing Calculate the distribution on the trajectory using the Boltzmann distribution:

[0146]

[0147] Where γ is a scaling factor (e.g., γ = 1) and η is a normalizer. This encodes the assumption that a trajectory closer to the optimal one (in terms of its reward) is more likely.

[0148] G. Self-Vehicle Planning

[0149] To compute the optimal plan for the ego vehicle, target probabilities and trajectory predictions are used to inform the Monte Carlo Tree Search (MCTS) algorithm [6]. MCTS combines the statistical backpropagation operator used in temporal reinforcement learning

[25] with dynamic tree expansion to focus the search on the current state. Algorithm 2 provides pseudocode for the implementation of the MCTS algorithm (which is a “convolute-based” version of MCTS

[16] .

[0150] Algorithm 2: Monte Carlo Tree Search Algorithm

[0151] Return: State s t Optimal maneuver of the vehicle under ε

[0152] Perform D simulation:

[0153] 1. Search node qs←s t (Root node)

[0154] 2. Search depth d←0

[0155] 3. For all Do

[0156] Sample current maneuver ω i ~p(ω i )

[0157] Sampling target

[0158] Sampling trajectory in

[0159] 4. When d <d max Do

[0160] Choose the macroscopic action μ applicable to ε in qs.

[0161] ← Simulate macroscopic movements until they terminate; other vehicles follow their sampled trajectories.

[0162] If your own vehicle is If a collision occurs during this period, then

[0163] r←r coll

[0164] Otherwise, if To achieve your personal goals, then

[0165]

[0166] Otherwise, if d = d max -1, then

[0167] r←r term

[0168] if but

[0169] Backpropagation is performed along the search branch (q,μ,q') of the generated simulation using (9).

[0170] Start the next simulation

[0171] q←q′;d←d+1

[0172] Return s t ,μ∈argmax μ The maneuver of ε in Q(root,μ).

[0173] The algorithm starts from the current state. Start executing many simulations This continues until a fixed search depth is reached or until the target state is reached. At the start of each simulation, the current maneuver is sampled for other vehicles, then for the target, and then for the vehicle trajectory using relevant probabilities (see Section III-F).

[0174] Avoid from mixed posterior Sample g i Because it is possible to construct something that could lead to ω i / g i Examples of sampling incompatibility (i.e., in ω) i After that, g could not be achieved. i ).

[0175] The sampled trajectories will be used to simulate the motion of other vehicles in closed-loop or open-loop modes (see below for details). Similar to A* search, each node q in the search tree corresponds to a state. Furthermore, macroscopic maneuvers are filtered based on their applicability to s. After selecting a macroscopic action μ using some exploration technique (e.g., UCB1[3]), the state in the current search node is simulated forward based on the trajectory generated by the macroscopic action and the sampled trajectories of other vehicles, thereby generating a partial trajectory. and with state The new search node q'.

[0176] right Perform a collision check to see if your vehicle has been involved in a collision; in this case, set the reward to r←r. coll According to the following equation (9), backpropagation occurs, where r coll These are method parameters.

[0177] Otherwise, if the new state Achieve personal goals gε Then the reward for backpropagation is calculated as follows: That is, as a reward for the trajectory calculated according to equation (5); in this case, it is the same reward function 226 used to evaluate the possible trajectories of other agents (assuming that other vehicles reason in the same way as the self-vehicle).

[0178] If the search reaches its maximum depth d without collision or by reaching the target. max The reward is then set to r←r term It can be a constant or based on a heuristic reward estimate similar to A* search.

[0179] The reward r is backpropagated by generating a simulated search branch (q,ω,q') using a 1-step policy update function defined by the following equation.

[0180]

[0181] Where δ is the number of times macroscopic action μ is selected in q. After the simulation is complete, the algorithm starts from the root node argmax. μ Q(root,μ) selects in s t The best macro-level actions to be taken.

[0182] The MCTS algorithm is rerun at a given frequency to take into account new observations since the last MCTS call (without reusing information from past search trees).

[0183] In Algorithm 2, the index τ in line 9 represents the current time point in the generated simulation (not necessarily the current time point in the "real world"). In the first simulation iteration, d = 0, τ ← t (current time), meaning the simulation starts at time t and the macroscopic action ends at time l. For the next iteration, τ ← ι (i.e., the completion time of the previous simulation iteration, etc.).

[0184] The simulation begins at the current time t, therefore past trajectories are not part of the sampled trajectory. The first iteration (d = 0) always begins with the state observed by each agent at time t. To reflect this, the notation for the sampled trajectory in line 6 of Algorithm 2 (and elsewhere) can be written as... in Always equal to (The actual trajectory at time t).

[0185] Two different control modes are used to simulate maneuvering and macroscopic actions. The ego vehicle's motion is always simulated using closed-loop mode, while other vehicles can be simulated in either closed-loop or open-loop mode.

[0186] 1) Closed-Loop Simulation: Closed-loop simulation combines proportional control and adaptive cruise control (ACC). Two independent proportional controllers control the vehicle's acceleration and steering. If another vehicle approaches in front of the controlled vehicle, control is handed over to ACC to maintain a safe distance. Since the combination of P / ACC control achieves approximately smooth control, speed smoothing is not applied. Termination conditions during maneuvers are monitored at each time step based on vehicle observations. ACC itself is known, therefore the details of the ACC algorithm will not be described here.

[0187] 2) Open-loop simulation: Open-loop simulation operates in the same way as A* search (see Section III-F3), by setting the vehicle's position and speed directly according to the trajectory specification. Therefore, there is no automatic distance holding in open-loop control. Speed ​​smoothing is applied to the trajectory to improve the realism of the prediction. Termination conditions in maneuvers, such as "waiting for oncoming traffic to clear," are implemented, for example, as used in yield maneuvers, assuming the uncontrolled vehicle uses a constant-speed lane-following model and waits for predicted traffic to clear.

[0188] System 200 thus integrates planning and prediction over an extended field of view by leveraging the computational advantage of a limited pool of maneuvers. Prediction over the extended field of view can be achieved by identifying targets of other vehicles through a reasonable backward planning process. The evaluations listed in Appendix A show that System 200 can robustly identify targets of other vehicles in different urban driving scenarios, thereby improving decision-making, while allowing intuitive interpretation of the predictions to justify the rationality of the system's decisions. System 200 is general because it uses relatively standard planning techniques that can be replaced by other relevant techniques, such as POMDP-based approximate planners

[23] . Furthermore, while the above examples focus on prediction of other vehicles, the basic principles of System 200 are general and can be extended to include prediction of other traffic participants (e.g., cyclists) or to other areas of interaction between mobile robots and other robots / humans. System 200 can also be extended to address irrational biases in humans.

[0189] Object detection example:

[0190] To further illustrate, an example calculation of the target probability will now be described.

[0191] The following description refers to observation O, including the observed state agent in the above context.

[0192] Figure 10 A schematic flowchart of a backward planning method is shown, which probabilistically infers the goals of external participants from a finite set of available goals based on reward discrepancies. This is an example of a backward planning method implemented by backward planner 204.

[0193] Figure 10 The right side shows an illustrative example of steps applied to a scenario with two available targets:

[0194] 1. G1 - Continue driving along the current road, which is defined as the target location at the visible end of the road (more generally, as a reference point in front of the car on the current road);

[0195] 2.G2 – Right turns are possible, depending on the exit location.

[0196] Given a set of possible targets for the observed vehicle and a sequence of past basic maneuvers performed by the vehicle, the posterior distribution of the targets can be computed using an inverse programming process. As described above, this method, given an observation sequence O (e.g., the observed trajectory τ), n Given the prior distribution P(G) and likelihood function L(O|G) of the target (as in the example above), calculate the Bayesian posterior P(G|O) ~ L(O|G)P(G) of the possible target G.

[0197] The target is defined based on the target location, symbol G. i This can be used to represent the target location within the area. Target location G i It can be a point in space, but it can also be a region or correspond to a specific distance along a road. For example, the target location can be defined as a line perpendicular to the road. In this case, a car is said to have reached the target once it reaches that line (regardless of its lateral position on the road).

[0198] Given target G i The probability L(O|G) ∈ G i The difference between the rewards of the two plans is defined as follows:

[0199] 1. From the car's initial position r t (at time t) to target position G i The optimal planning, i.e., from r t To G i The optimal planning is achieved without considering any behavior of the observed car after time t. This can be performed as a basic maneuver, a macroscopic action, or a sequence of basic maneuvers instead of a macroscopic action. By combining the associated partial trajectories through multiple basic maneuvers, a plan can be provided from the initial position r. t Reaching target G i The optimal complete trajectory (ignoring any actual observed car behavior after time t); and

[0200] 2. "Optimal Availability" Planning – This is defined as the optimal plan for any car behavior observed between time t and time t+Δt, based on r. t To the target location G iThe optimal plan, i.e., from r t To G i The optimal plan is obtained with the additional constraint that it must match the actual observed behavior over subsequent time intervals ΔT. In other words, as the car's initial position r... t To target G i The optimal plan is one that respects the observation O. This assumes the car is more likely to execute the optimal plan to achieve the objective, but allows for a degree of deviation. This can also be performed as a basic maneuver, a macroscopic action, or a sequence of basic maneuvers instead of a macroscopic action. For multiple basic maneuvers, the associated partial trajectories are combined to provide a trajectory from the initial position r. t Reaching target G i The "best available" complete trajectory is determined, but the actual observed behavior of the car within the interval from t to t+Δt is taken into account. The best available trajectory has the observed portion of the interval [t, t+Δt] that matches the actual observed trajectory and the future portion of subsequent time intervals. The selection is made to minimize the overall reward associated with the best available complete trajectory (i.e., the total reward for the observed portion and the future portion).

[0201] This is a form of target identification because it takes into account the complete path to the target (which may be based on multiple partial trajectories associated with multiple maneuvers).

[0202] The rewards assigned to a complete track can take into account various factors, as described below. These include driving time (penalizing tracks that require longer to reach the goal), safety (penalizing unsafe tracks), and comfort (e.g., penalizing excessively bumpy tracks).

[0203] For example, the initial position r of the car t This could be the car's first observation position. A reasonable approach is to define the initial position r using a moving window of past observations defined by the range of the car's sensors. t .

[0204] exist Figure 10 Step 1004 in the above section calculates the optimal program for each objective G1, G2 (see step 1 above). After calculation, this allows the determination of the optimal trajectory for each objective G1, G2, for example, using A* search (see below). Once the optimal trajectory is calculated, the full reward associated with the optimal trajectory can be calculated (also described below). The optimal trajectory is a complete trajectory, i.e., from the initial position r... t To reach the goal discussed.

[0205] exist Figure 10In the example, the trajectory is represented by points that are uniformly distributed along the trajectory over time. Therefore, uniformly distributed points imply a constant speed, while increasing (or decreasing) the distance between points implies acceleration (or deceleration). White circles are used to represent the optimal trajectory points. It can be seen that for target G1, the optimal trajectory is a straight path continuing along the road at a constant speed, while for target G2, the optimal trajectory gradually slows down as the car approaches the turning point near the exit.

[0206] In step 1006, the optimal available plan (above 2) is calculated for each objective G1, G2. As noted, these take into account time t (when the car is in its initial position r). t The actual observations O between (t+Δt) and the current time. These observations O may include the observed low-level trace τ, in Figure 10 Black circles are used to represent this.

[0207] In that case, observations O may alternatively or additionally include the car's current maneuver, meaning the probability of each target can be estimated based on the maneuver the car is currently performing. They may also include maneuvers observed in the past.

[0208] Despite Figure 10 While not explicitly stated, as mentioned above, probabilistic maneuver detection is used to predict the probability distribution of the car's current possible maneuvers. Therefore, based on the distribution p(M|τ) of possible current maneuvers, the current maneuver may not be absolutely known, but rather probabilistic. In this case, during the planning process, at the start of each MCTS rollout, the current maneuver M is first sampled from p(M|τ). j (that is, including M) j Observe O), and then from the current maneuver M j The target is sampled from the target probability distribution p(G|O).

[0209] Based on the optimal availability plan, the optimal availability trajectory can be determined (see below), which in turn allows for the determination of the full reward for the optimal availability trajectory (also described below). This is also from the initial position r t To the target location G i The complete trajectory in the sense of a complete trajectory. The best available trajectory has the observed portion between time t and t+Δt, which matches the actual observed trajectory (i.e., Figure 10 (The black circle in the image), and the optimal usable trajectory also includes the future portion after time t+ΔT, in Figure 10 The shaded circle is used to represent this.

[0210] In the depicted example, it can be seen that both the observed part (black circle) and the future part (diagonal shaded circle) of the best available trajectory for the target G1 quite well match the optimal trajectory (white circle) of that target G1. Thus, the reward difference for the target G1 - that is, the difference between the reward of the optimal trajectory and the reward of the best available trajectory - is relatively low.

[0211] However, for the target G2, the deviation of the observed trajectory (black circle) from the optimal trajectory (white circle) is quite large because the car fails to reach the degree required by the optimal trajectory at time t + Δt. This difference itself does not necessarily result in a significant reward difference (it may or may not depend on the implementation details). However, as a result of the observed behavior, it can be seen that the future part of the best available trajectory (i.e., the part after time t + Δt) must include a sharp braking (which reflects the fact that, given the situation of the car, the highest reward path from the current position of the car to G2 must involve a sharp braking) - which will be penalized by the reward function. This difference from the reward of the optimal trajectory means that the reward difference for the target G2 is higher.

[0212] In step 1008, for each target G1, G2, the target likelihood L(O|G) is calculated based on the reward difference between the optimal plan calculated in step 1004 and the best available plan for that target calculated in step 1006. This in turn allows the calculation of the target posterior P(G|O) based on the target likelihood and the target prior (step 1010).

[0213] The prior P(G) can be used to encode knowledge about the "intrinsic" probability of certain targets. For example, it can be observed that in Figure 10 the scenario where there are relatively few cars turning right, it can be encoded as a prior with P(G2) < P(G1). This will effectively bias the target G1 towards G2. For scenarios without such prior knowledge, it can simply be assumed that each target is equally likely, without any observation of the individual behavior of a particular car, i.e., P(G1) = P(G2), as in the example of Algorithm 2, where a uniform target prior distribution is used.

[0214] It is assumed above that, given a target and the initial position r t of a car, the optimal plan for that target can be determined, and the best available plan for that target can be determined based on the observations in subsequent time intervals ΔT. Additionally, it is assumed that, given an optimal (or best available) plan, an optimal (or best available) trajectory can be determined. One mechanism for mapping a target to a trajectory in this way uses A* search, as described above.

[0215] Current Maneuver and Target Sampling Example:

[0216] Figure 11A schematic diagram of a mechanism is shown in which the results of backward planning can be applied as part of the MCTS rollout by the motor planner 218. This applies the principles described above related to Algorithm 2, where the MCTS is executed multiple times using different samples of probability predictions about other agents.

[0217] The maneuver detector 208 detects the current maneuver probability distribution of the target participant in the manner described above. For each maneuver M... i ∈M (M being a finite set of available maneuvers), which provides the probability that the target actor is currently performing that maneuver, given the observed trajectory τ, i.e.

[0218] P(M|τ)

[0219] Each MCTS rollout begins as follows. For each other's cars:

[0220] 1. Sample the current maneuver from the set of predicted current maneuvers and their probabilities, that is, sample the current maneuver M based on P(M|τ). j Perform sampling (box 1104);

[0221] 2. Sample a target from the target posterior P(G|O) corresponding to the sampled current maneuver, that is, combine the above inverse planning steps with the sampled current maneuver M. j Apply together (box 1106).

[0222] These samples, namely the current maneuver M sampled j and the target G of the sampling i This is used throughout the unwinding process of the MCTS procedure. Steps 1 and 2 above will be performed at the beginning of each MCTS unwinding and will be used throughout the unwinding process using the sample target and trajectory.

[0223] exist Figure 11 In Chinese, the abbreviation P(G|M) j ) and P(G|M k ) are used to represent the given current maneuver M j and M k The target posterior—note that observation O may be broader than simply the current maneuver (e.g., a sequence including previous maneuvers). Using this notation, if in step 1 above, the maneuver M... j Since sampling has been performed, the target posterior P(G|M) will be analyzed in step 2. j Sampling is performed; while if M k If sampled, the target posterior P(G|M) will be used. k ).

[0224] Although Figure 11It is not described in the text, but the trajectory probability will be determined as the target, for example, using A* search in the manner described above.

[0225] Example search tree:

[0226] Figure 12 An example of a Monte Carlo tree that might be constructed when applying Algorithm 2 above is shown.

[0227] As an example, Figure 12 The diagram shows the edges D04a-c from the root node D02 to its three direct child nodes, labeled D06a-c. Each of these edges D04a-c represents a different maneuver that can be performed given the driving scenario state s0 of the node from which it extends (root node D02 in this case). Each of these child nodes represents a subsequent state of the driving scenario - s0, respectively. 1A ,s 1B ,s 1C - Predict the situation when the vehicle performs the corresponding maneuver, starting from state s0.

[0228] For example, edges D04a-c can correspond to "following lane," "changing lane left," and "changing lane right" in a multi-lane driving scenario with respect to parent state s0, respectively. In state s0, the ego vehicle is in the current lane - performs a lane-following maneuver to remain in the current lane for an applicable time interval; changing lane left and changing lane right are performed to attempt to move to the left and right lanes of the current lane, respectively. State s 1A ,s 1B ,s 1C It is obtained by advancing the parent state s0 according to the relevant maneuver, while taking into account the external agent behavior within the relevant time interval.

[0229] Algorithm 2 explores one branch of the tree in each rollout until the relevant termination condition (collision, reaching the goal, or reaching the maximum depth) is met. For example, leaf nodes D8 and D10 at the end of their respective branches are shown, and it can be noted that the leaf nodes are located at the positions where a collision occurs, the goal is reached, or the maximum depth is reached. Each node in a given branch corresponds to the state simulated in the corresponding iteration of the simulation phase of that rollout.

[0230] The aforementioned techniques are implemented in either "in-vehicle" or "out-of-vehicle" contexts. In an out-of-vehicle environment, the technician can implement these techniques in the planner of a mobile robot to plan actions in real time. In this case, observations can be derived from sensor signals captured using its onboard sensors. In an out-of-vehicle environment, the aforementioned techniques can also be implemented in the planner of a simulated runtime stack to test the performance of the runtime stack based on simulated driving scenarios. Simulation is becoming increasingly important in the safety and other performance testing of autonomous vehicles.

[0231] Please note the difference between external vehicle simulation and the simulation inherent in the planning process. Above, multiple simulations are run to evaluate and plan self-movements / maneuvers. This is part of the planning process, and these simulations will be run in either onboard or external vehicle environments. In the onboard environment, the observed agent trajectory will be derived from real-time measurements by real physical sensors. In the external vehicle environment, the observed agent trajectory will be simulated, and planning and prediction processes will be applied to the simulated agent observations for safety or other performance testing, training, and other purposes.

[0232] Example AV stack

[0233] Figure 9 A highly schematic functional block diagram of some functional components of the AV runtime stack A1 is shown, namely the perception component A2, the prediction component A4, and the AV planner A6.

[0234] The perception component A2 receives sensor data from the AV's onboard sensor system A8. The onboard sensor system A8 can take different forms, but typically includes various sensors such as image capture devices (cameras), LiDAR units, satellite positioning sensors (GPS, etc.), motion sensors (accelerometers, gyroscopes, etc.), etc. Together, they provide rich sensor data from which detailed information about the surrounding environment and the status of the AV and other participants (vehicles, pedestrians, etc.) in that environment can be extracted.

[0235] Route Planner 214 Figure 9 As shown, the reverse planner 204 and the MCTS-based maneuver planner 218 are shown as part of the prediction and planning components A4 and A6, respectively.

[0236] However, please note that this technology is not limited to using image data captured by the AV's own onboard optical sensors (image capture devices, LiDAR, etc.). This method can be applied alternatively or additionally using externally captured sensor data, such as CCTV images captured by an external image capture unit located near the AV. In that case, at least some of the sensor inputs used to implement this method can be received by the AV from an external sensor data source via one or more wireless communication links.

[0237] The perception component A2 processes sensor data to extract such information. This typically involves various forms of machine learning (ML) / artificial intelligence (AI) processing. In the current context, the relevant functions of perception component A2 include localization (box A10), object detection (box A12), and object tracking (box A14).

[0238] Localization is performed to provide awareness of the surrounding environment and the AV's position within it. Various localization techniques can be used for this, including visual and map-based localization.

[0239] Object detection is applied to sensor data to detect and locate external objects in an environment, such as vehicles, pedestrians, and other external actors whose behavior requires the AV to respond safely. This can, for example, include a form of 3D bounding box detection where the position, orientation, and size of objects within the environment and / or relative to the vehicle itself are estimated. This can be applied, for example, to (3D) image data such as RGBD (Red, Green, Blue Depth), LiDAR point clouds, etc. This allows the location and other physical properties of such external actors to be determined on a map.

[0240] Object tracking is used to track any movement of detected objects in an environment. The result is the observed trajectory of each agent, determined over time by object tracking. The observed trajectory is the history of a moving object; it captures the object's trajectory over time and can also capture other information such as the object's historical velocity and acceleration at different points in time.

[0241] By combining object detection and object tracking, external participants can be fully located and tracked on a defined map of the environment surrounding the AV.

[0242] Object detection and object tracking are well-known and can be performed in the current context using a variety of publicly available state-of-the-art models.

[0243] By combining localization, object detection, and object tracking, the perception component A2 provides a comprehensive representation of the environment surrounding the vehicular vehicle, including the current state (detectable position, heading, speed, etc.) of any external participants in that environment, as well as the historical traces of such participants that the AV can track. This is continuously updated in real time to provide the latest location and environmental awareness.

[0244] Predictive component A4 uses this information as the basis for predictive analytics, in which it predicts the future behavior of external participants near AV. Examples of suitable predictive methods are described below.

[0245] The AV planner A6 uses extracted information about its surrounding environment and external agents within it, along with behavioral predictions provided by the prediction component A4, as the basis for AV planning. That is, the predictive analysis of the prediction component A4 adds a layer of predictive information on top of the information extracted from sensor data by the data processing component, which in turn is used by the AV planner A6 as the basis for AV planning decisions. This is typically part of a hierarchical planning process, where the AV planner A6 makes various high-level decisions, followed by increasingly lower-level decisions required to execute higher-level decisions. The end result is a series of real-time, low-level action decisions. To execute these decisions, the AV planner A6 generates control signals, which are at least partially input into the AV's drive mechanism A16 to control the vehicle's speed and heading (e.g., through steering, braking, acceleration, gear shifting), etc. Control signals are also generated to perform auxiliary actions, such as signaling.

[0246] Scene extraction component A3 uses the output of perception component A2 to determine the driving scene encountered by the AV. The determined driving scene includes driving scene parameters extracted from captured sensor data. These parameters provide a concise yet sufficiently detailed representation of the real-world scene encountered by the AV, which can be used as the basis for realistic simulation. This is expressed in a structured scene description language, which can be used as the basis for such simulations.

[0247] The simulator A5 receives parameters of the driving scenarios encountered and can run simulations based on these parameters. As mentioned above, these are simulations inherent to the MCTS planning process.

[0248] One function of the prediction component A4 is to model the predicted behavior of external agents to run as part of a simulation. That is, an external agent behavior model is executed to predict the behavior of any external actors in the driving scenario encountered, so that the predicted behavior can be incorporated into the simulation on which the maneuver planning is based.

[0249] In simulated contact outside the vehicle, some or all of the components of runtime stack A1 can operate in exactly the same way, but on simulated inputs, such as simulated agent trajector ...

[0250] The terms "component," "function," and "module" used in this document refer to functional components of a computer system that can be implemented at the hardware level in various ways. This includes... Figure 1 Various components and Figure 2The system includes a target recognition module 202 and a self-planning module 212, as well as various sub-modules thereof. The computer system comprises one or more computers, which may be programmable or non-programmable. Each computer includes one or more processors that perform the functions of the aforementioned components. The processors may take the form of general-purpose processors, such as CPUs (Central Processing Units) or accelerators (e.g., GPUs), or more specialized hardware processors, such as FPGAs (Field-Programmable Gate Arrays) or ASICs (Application-Specific Integrated Circuits). In other words, the processors may be programmable (e.g., instruction-based general-purpose processors, FPGAs, etc.) or non-programmable (e.g., ASICs). Such a computer system can be implemented in in-vehicle or external environments.

[0251] The first aspect of this paper describes a computer-implemented method for autonomously planning self-actions for a mobile robot in the presence of at least one agent, the method comprising: receiving an observed trajectory of an agent; in a sampling phase: sampling targets for the agent from a set of available targets based on a probabilistic target distribution, the observed trajectory being used to determine the probabilistic target distribution; and sampling agent trajector ...

[0252] The method may include, in the target identification phase, calculating a target distribution by determining a reward difference between (a) a first basic optimal trajectory and (b) a best available trajectory for each target, wherein the first basic optimal trajectory extends from the initial state of the observed trajectory to the target position, and the best available trajectory combines (b1) the observed trajectory from the initial state of the observed trajectory to the current state and (b2) a second basic optimal trajectory from the current state to the target position, wherein the probability trajectory distribution is determined by determining the reward for each trajectory in the set of possible trajectories.

[0253] The probability trajectory distribution includes the probability of each available target given the observed trajectory, the probability of each available target being calculated as the product of the prior probability of the target and the likelihood of the observed trajectory given the target, the likelihood being based on an exponent of the reward difference.

[0254] The probability trajectory distribution includes the probability of each of the possible trajectories based on the exponent of its reward.

[0255] The difference between the reward for each trajectory and the reward for each objective can be determined using a reward function that rewards reduced travel time while penalizing unsafe trajectories.

[0256] The reward function can also punish a lack of comfort.

[0257] The sampled surrogate trajectory is open-loop, but the simulation phase is closed-loop. The simulated closed-loop surrogate trajectory is determined based on the sampled open-loop surrogate trajectory. The simulated closed-loop trajectory deviates from the sampled open-loop trajectory in a manner that reacts to the simulated behavior of the mobile robot.

[0258] Alternatively, the sampled proxy trajectory can be open-loop, and the simulation phase can also be open-loop.

[0259] The target recognition stage can be open-loop.

[0260] Each trajectory can have path components and motion components.

[0261] Each trajectory can take the form of a state sequence, with each state encoding spatial and motion information at a specific moment.

[0262] Motion components can be used as indicators, allowing agents to deviate from these indicators reactively during the simulation phase (e.g., using the closed-loop form of the simulation described above).

[0263] The first and second trajectories can be determined in the first search process, which seeks to optimize the reward or budget cost of the first and second trajectories.

[0264] Budgeted costs can be used during the trajectory search process, and budgeted costs can take into account only the travel time to the target location, and can ignore at least one other cost factor that is considered when determining the reward difference between the first trajectory and the best available trajectory.

[0265] The set of possible trajectories can be determined during the second search process, which is free to explore additional trajectories, including those that do not reach the target location. However, trajectories found during the second search process will only be added to the set of possible trajectories if they actually reach the location of the relevant target.

[0266] A first search process and / or a second search process can be performed on the search space of possible agent actions, wherein each trajectory corresponds to the agent action being searched or a sequence of agent actions being searched within the search space.

[0267] The sampling phase may include sampling current agent actions from the set of possible current agent actions based on the probability distribution of current actions determined for the agent.

[0268] The first search process and / or the second search process can be based on the sampled current agent action.

[0269] The distribution of the probabilistic target can be determined using the observed trajectory and the sampled current agent action.

[0270] The probabilistic target distribution can include the conditional probability of each target given the observed trajectory and the sampled current agent maneuver.

[0271] The current action distribution can be determined by a probabilistic maneuver detector based on one or more observations of the agent, such as one or more states of the observed trajectory.

[0272] The simulation phase can include determining the predicted state of the mobile robot after the selected self-action has been completed.

[0273] The simulation phase is performed on the sampled agent trajectory, with an initial iteration based on the current mobile robot state and each subsequent iteration based on the sampled agent trajectory, the further selected self-actions, and the predicted mobile robot state determined in the previous iterations, until a termination condition is met, thereby evaluating the time series of multiple selected self-actions in multiple iterations.

[0274] Multiple iterations of the trajectory sampling phase and the simulation phase constitute a single rollout, and the method includes performing multiple rollouts, wherein in each rollout, the sampling phase is repeated to sample the surrogate target and the corresponding surrogate trajectory for one or more iterations of the simulation phase performed in each rollout.

[0275] The current agent action is sampled in each volume for use in that volume.

[0276] Multiple roll-ups are performed according to a probabilistic tree search algorithm, where the root node of the search tree represents the current state of the mobile robot, the edges of the tree represent selected ego actions, and the additional nodes represent predicted states, wherein branches of the tree are explored in each roll-up by successive selection of ego actions in one or more iterations of the simulation phase performed in the roll-up.

[0277] Self-actions can be probabilistically selected in each iteration of the simulation phase.

[0278] Each action is either a basic maneuver or a macroscopic action formed by a sequence of multiple basic maneuvers.

[0279] This method can be implemented in a planner to plan essentially optimal self-movements or sequences of movements for a mobile robot.

[0280] The essentially optimal self-action or self-action sequence can be determined based on the rewards obtained through backpropagation of the search tree described above. Each reward is calculated for nodes that are determined to have collided, for nodes that are determined to have reached their self-goal without a collision, or for nodes where the branch terminates without a collision and without reaching their self-goal.

[0281] For each node determined to have reached the goal without collision, the reward can be the reward for the simulated ego trajectory computed for the branch containing that node.

[0282] The reward for the simulated self-trajector is calculated using the same reward function as the probability target distribution of the computation agent and the probability trajectory distribution of the set of possible trajectories associated with it.

[0283] The observed agent trajectory can be obtained from physical sensor measurements or from simulated agent trajector ...

[0284] Possible trajectories could be smooth trajectories that have already been applied with velocity smoothing and / or other motion smoothing.

[0285] Velocity smoothing and / or other motion smoothing can be applied to the first and second trajectories, but not to the observed trajectory.

[0286] Other aspects of this document may provide a computer system comprising one or more computers programmed or otherwise configured to implement the methods of the first aspect or any embodiments thereof, and a computer program configured to program the computer system to perform any such methods. The computer system may be implemented in a mobile robot or an off-vehicle simulator (e.g.).

[0287] Appendix A – Assessment

[0288] We evaluate our system in four urban driving scenarios with different initializations. We show that:

[0289] Our method correctly identifies targets from other vehicles.

[0290] Target recognition leads to improved driving behavior

[0291] We can extract intuitive explanations for the identified targets and predictions to demonstrate that the system's decisions are correct.

[0292] Videos showing the prediction and planning steps of our system in each scenario can be found in the supplementary materials.

[0293] A. Scene

[0294] We use the following scenarios:

[0295] S1 T-junction ( Figure 5 Vehicle V1 approaches the T-junction from the west, aiming to reach the east end (blue target). Vehicle V1 is on the same road as Vehicle V1 in the adjacent lane, aiming to exit the road and travel south (purple target), changing lanes ahead of Vehicle V1. Vehicle V2 approaches the T-junction from the south, aiming to travel east (blue target), and V2 must yield at the intersection.

[0296] S2 X intersection ( Figure 6 Vehicle V1 approaches intersection X from the south with its goal of reaching the west end (blue target). Vehicle V2 approaches intersection X from the west with its goal of reaching the east end (yellow target). Vehicle V2 approaches intersection X from the east with its goal of reaching the north end (purple target). V2 must wait for V1 to pass.

[0297] S3 roundabout ( Figure 7 Vehicle V1 approaches the roundabout from the west with the goal of traveling east (green objective). Vehicle V1 is inside the roundabout with the goal of exiting south (orange objective).

[0298] S4 lane change ( Figure 8 ): The ego vehicle approaches the main road from the east, aiming to reach the north end (purple target). Several other vehicles are on the main road, queuing after the red light. Vehicle V1 leaves space for the ego vehicle to merge. Vehicle V2 is traveling south (green target). For each scenario, we generate 100 instances containing an initial longitudinal position with a random offset (offset ~ [-10, +10] meters) and an initial velocity sampled from the range [5, 10] meters / second for each vehicle, including the ego vehicle.

[0299] B. Baseline and Parameters

[0300] We compared the following versions of our system: Full: A complete system using object recognition and MCTS. MAP: Similar to Full, but MCTS only uses the most probable object and trajectory for each vehicle. CVel: MCTS without object recognition, replaced by constant speed lane tracing prediction. Cons: Similar to CVel, but using a conservative yield maneuver, waiting for all oncoming vehicles in the priority lane to pass. All these baselines were simulated using closed-loop MCTS. We also evaluated the full baseline using both closed-loop (Full-CL) and open-loop (Full-OL) simulations.

[0301] Maneuvering, macro-actions, and target generation heuristics are defined earlier in this paper. For other vehicles and generated targets, we generate at most three predicted trajectories. We simulate noise detection of other vehicles' current maneuvers by assigning a probability of 0.95 to correct the current maneuver and distributing the remaining probabilities evenly among other maneuvers. MCTS runs at 1 Hz, performing D = 30 simulations with a maximum search depth of d = 5. Collision rewards and the maximum search depth are set to r. coll =r term = -1. The prior probability of achieving the goal is consistent.

[0302] C. Result

[0303] Figure 5-8 This section shows snapshots of scenario instances at different planning stages of our complete system. The bar charts present the target probabilities of other vehicles associated with their most likely current maneuvers. For each target, we show the most likely trajectory prediction from the vehicle to the target, with the thickness proportional to its probability. We extract intuitive explanations for target identification and maneuver prediction in each scenario, which are given in the accompanying figure descriptions.

[0304] Figure 4 The diagram shows the evolution of the probability of assigning the correct objective over time in four scenario instances. It can be seen that the probability approaches the correct objective when other objectives are excluded by the rationality principle. We observed this behavior in all scenario instances.

[0305] Figure 3The average time (in seconds) and standard deviation required for each baseline to complete the scenario instance are shown. (S1) All baselines switch lanes in response to V1 changing lanes. Due to backward planning, Full and MAP predict V1's deceleration earlier than the other baselines, allowing them to switch lanes slightly earlier. CVel and Cons only switch lanes when V1 has already begun to decelerate and cannot explain V1's behavior. (S2) Cons needs more time to complete the scenario because it waits for V2 to clear the lane, while V2 must wait for V1 to pass. Full and MAP anticipate this behavior, allowing them to enter the road safely earlier. Since V2's speed is zero, CVel produces the same result but cannot fully justify (explain) its decision because it cannot explain V2's waiting behavior. (S3) Both CVel and Cons need more time to complete the scenario. Here, the constant speed prediction in CVel and the actual clearing of the waiting in Cons total the equivalent of the ego vehicle's entry time. Full and MAP are able to enter earlier because they recognize that V1's goal is to exit the roundabout. MAP enters earlier than Full because it is fully committed to V1's most likely target, while Full exhibits more cautious behavior because the remaining uncertainty about V1's target could lead to a crash. (S4)Cons must wait until V1 decides to close the gap before it can enter the road, thus requiring more time. Full and MAP identify V1's target and can enter safely. CVel again produces the same behavior based on V1's constant velocity, but cannot explain V1's waiting behavior.

[0306] Full-CL and Full-OL achieved the same scene instance completion rate (100%) and required the same travel time. We found that, in our specific scenario, OL was sufficient to simulate vehicle interactions. However, densely populated roads would likely benefit from CL simulations. As expected, OL required significantly less computation time than CL, in some cases by up to 50%.

[0307] Related Attached Figures and Descriptions

[0308] Figure 3 : The average driving time (in seconds) required to complete the scenario instance, with standard deviation.

[0309] Figure 4 : The probability evolution of providing the correct target for the selected vehicle in four scenario instances. Note: Linear ratio of S1 / S3 Figure 3 The diagram shown is shorter because the vehicle target changes after the exit point (see [reference]). Figure 5 and 7 We only display the line of the initial target.

[0310] Figure 5S1: Vehicle V1 is on its own lane, and vehicle V2 is approaching from the south. (a) Initially, we attribute the uniform distribution to all the goals of V1 and V2. (b) V1 changes lanes from left to right, causing the ego to predict a bias towards the belief that it will exit, because if V1's goal is to travel east, then changing lanes would be unreasonable. Since exiting would require significant deceleration, the ego decides to change lanes to avoid also being decelerated. (c) / (d) The ego's belief that V1 will exit increases as V1 decelerates as it approaches the intersection, thus encouraging the ego to continue in its lane. (e) V1 exits and the ego can safely continue because V2 will merge into the right lane, its only possible goal.

[0311] Figure 6 S2: Vehicle V1 approaches the intersection from the west, and vehicle V2 approaches the intersection from the east. (a) Initially, we assign all targets of V1 and V2 to a uniform distribution. (b) / (c) As V2 approaches the intersection, slows down, and waits to turn, V2's ego belief that it will turn right increases significantly because stopping would be unreasonable if the target were a left turn or going straight. V1's distribution of targets remains consistent due to the lack of information provided by the execution. (d) Due to V1's constant speed, the ego excludes north targets because a left turn at V1's speed and position would require a sudden, costly (in terms of reward) braking from V1. Meanwhile, V2's presence at the intersection reinforces the ego's belief that V1 intends to go north. (e) Because the ego recognizes that V2's target is north, it predicts that V2 will wait for V1 to pass, giving the ego a chance to enter the road.

[0312] Figure 7 S3: Vehicle V1 is in the roundabout. (a) Initially, we assign all of V1's goals to a uniform distribution. (b) As V1 moves from the inner lane to the outer lane of the roundabout and reduces its speed, it significantly biases the ego's prediction towards the belief that V1 will leave at the next exit, as this is the rational action process for that goal. (c) As V1 approaches the exit and continues to decelerate, the ego's belief that V1 will leave further increases, thus encouraging the ego to enter the roundabout while V1 is still in the roundabout. (d) / (e) V1 exits, at which point the ego has already entered the roundabout.

[0313] Figure 8S4: Two vehicles are stopped at a traffic light at an intersection. Vehicle V1 approaches from behind, and vehicle V2 crosses in the opposite direction. (a) Initially, we distribute all targets of V1 and V2 evenly. (b) Due to the very small change in speed, the target probabilities of the two vehicles remain nearly identical. (c) V1 has begun to decelerate, making it easier to indicate leaving the road, as the target to the north does not need to decelerate early according to the reward function. Therefore, it self-integrates. (d) V1's zero speed reveals a stopped target at its current position, and the distribution will move towards it, as stopping is unreasonable for a north / east target. The explanation is that V1 wants to self-integrate. (e) Given the identified target, self-integrates into the road in front of V1.

[0314] refer to

[0315] The above text references the following content, each of which is incorporated herein by reference in its entirety:

[0316] [3] P. Auer, N. Cesa-Bianchi, and P. Fischer. Finite-time analysis of the multiarmed bandit problem. Machine Learning, 47(2-3): 235-256, 2002.

[0317] [6] C. Browne, E. Powley, D. Whitehouse, S. Lucas, P. Cowling, P. Rohlfshagen, S. Tavener, D. Perez, S. Samothrakis and S. Colton. A survey of Monte Carlo tree search methods. IEEE Transactions on Computational Intelligence and AI in Games, 4(1):1–43, 2012.

[0318] [9] E. Galceran, A. Cunningham, R. Eustice, and E. Olson. Multipolicy decision-making for autonomous driving via changepoint-based behavior prediction: Theory and experiment. Autonomous Robots, 41(6):1367–1382, 2017.

[0319]

[10] CG′amez Serna and Y. Ruichek. Dynamic speed adaptation for pathtracking based on curvature information and speed limits. Sensors, 17(1383), 2017.

[0320]

[12] P. Hart, N. Nilsson, and B. Raphael. A formal basis for the heuristic determination of minimum cost paths. In IEEE Transactions on Systems Science and Cybernetics, volume 4, pages 100–107, July 1968.

[0321]

[16] L. Kocsis and C. Szepesv'ari. Bandit based Monte Carlo planning. In Proceedings of the 17th European Conference on Machine Learning, pages 282–293. Springer, 2006.

[0322]

[19] S. Niekum, S. Osentoski, C. Atkeson, and A. Barto. Online Bayesian changepoint detection for articulated motion models. In IEEE International Conference on Robotics and Automation, IEEE, 2015.

[0323]

[23] A. Somani, N. Ye, D. Hsu, and WSLee. DESPOT: online POMDP planning with regularization. In Advances in Neural Information Processing Systems, pages 1772–1780, 2013.

[0324]

[25] R. Sutton and A. Barto. Reinforcement Learning: An Introduction. MIT Press, 2018.

[0325]

[27] A. W. A. ​​A. A. A. A. B. B. B. B. B. C. On the implementation of an interior-point filter line-search algorithm for large-scale nonlinear programming. Mathematical Programming, 106(1):25–57, 2006.

Claims

1. A computer-based method for autonomously planning self-movements for a mobile robot in the presence of at least one agent, the method comprising: The observed trajectory of the receiving agent; During the sampling phase: Based on a probability target distribution, the proxy targets are sampled from the available target set, and the observed trajectory is used to determine the probability target distribution; and Based on the probability trajectory distribution, proxy trajector trajector trajector trajectories are sampled from a set of possible trajectories associated with the sampled target, each trajectory in the set of possible trajectories reaching the location of the associated target; During the simulation phase: Select a self-action for the mobile robot from the available set of self-actions, and Based on the selected self-action, the sampled agent trajectory, and the current state of the mobile robot, simulate (i) the behavior of the mobile robot and (ii) the synchronization behavior of the agent to evaluate the feasibility of the selected self-action in the current state of the mobile robot; and The target identification phase is performed after the sampling phase: The target distribution is computed by determining the reward difference between (a) a first optimal trajectory and (b) a best available trajectory for each target, wherein the first optimal trajectory is from the initial state of the observed trajectory to the target position, and the best available trajectory combines (bi) the observed trajectory from the initial state of the observed trajectory to the current state and (b.ii) the second optimal trajectory from the current state to the target position, wherein the probability trajectory distribution encodes the assumption that trajectories closer to the optimal are more likely, and the probability trajectory distribution is determined by determining the reward of each trajectory in the set of possible trajectories, wherein the probability of a trajectory for a target is estimated under the assumption that the agent intends to execute the optimal trajectory that maximizes the reward.

2. The method of claim 1, wherein the probability trajectory distribution includes a probability of each available target given the observed trajectory, the probability of each available target being calculated as the product of the prior probability of the target and the likelihood of the observed trajectory given the target, the likelihood being based on an exponent of the reward difference.

3. The method according to claim 1 or 2, wherein, The probability trajectory distribution includes the probability of each of the possible trajectories based on the exponent of its reward.

4. The method of claim 3, wherein the difference between the reward for each trajectory and the reward for each target is determined using a reward function that rewards reduced travel time while penalizing unsafe trajectories.

5. The method according to claim 4, wherein, The reward function also penalizes a lack of comfort.

6. The method according to claim 1 or 2, wherein, The sampled agent trajectory is an open loop, but the simulation phase is a closed loop. The simulated closed loop agent trajectory is determined based on the sampled open loop agent trajectory. The simulated closed loop trajectory deviates from the sampled open loop trajectory in a manner that reacts to the simulated behavior of the mobile robot.

7. The method according to claim 1 or 2, wherein, The sampled proxy trajectory is an open loop, and the simulation phase is also an open loop.

8. The method according to claim 6, wherein, The target recognition stage is an open-loop process.

9. The method according to claim 1 or 2, wherein, Each trajectory has path components and motion components.

10. The method of claim 9, wherein each trajectory is in the form of a state sequence, and each state encodes spatial and motion information at a specific time.

11. The method according to claim 10, wherein, The motion components are used as indicators, and during the simulation phase, the agent is allowed to deviate from the indicators reactively.

12. The method according to claim 1 or 2, wherein, The first optimal trajectory and the second optimal trajectory are determined in a first search process, which seeks to optimize the reward or budget cost of the first optimal trajectory and the second optimal trajectory.

13. The method according to claim 12, wherein, The trajectory search process uses a budget cost, which only considers the travel time to the target location and ignores at least one other cost factor considered when determining the reward difference between the first optimal trajectory and the best available trajectory.

14. The method according to claim 12, wherein, The set of possible trajectories is determined during a second search process, which is free to explore additional trajectories, including those that do not reach the target location. However, trajectories found during the second search process are only added to the set of possible trajectories if they actually reach the location of the relevant target.

15. The method according to claim 12, wherein, The first search process and / or the second search process are performed on a search space of possible agent actions, wherein each trajectory corresponds to a searched agent action or a sequence of searched agent actions within the search space.

16. The method according to claim 15, wherein, The sampling phase includes sampling current agent actions from a set of possible current agent actions based on the probability distribution of current agent actions determined for the agent.

17. The method according to claim 16, wherein, The first search process and / or the second search process are based on the sampled current agent action.

18. The method according to claim 16, wherein, The probability target distribution is determined using the observed trajectory and the sampled current agent action.

19. The method according to claim 18, wherein, The probability target distribution includes the conditional probability of each target given the observed trajectory and the sampled current agent maneuver.

20. The method of claim 16, wherein, The current agent action distribution is determined by a probabilistic maneuver detector based on one or more observations of the agent, wherein the one or more observations are one or more states of the observed trajectory.

21. The method according to claim 17, wherein, The simulation phase includes determining the predicted state of the mobile robot after the selected self-action has been completed.

22. The method according to claim 21, wherein, The simulation phase is performed on the sampled agent trajectory, with an initial iteration based on the current mobile robot state and each subsequent iteration based on the sampled agent trajectory, the further selected self-actions, and the predicted mobile robot state determined in the previous iterations, until a termination condition is met, thereby evaluating the time series of multiple selected self-actions in multiple iterations.

23. The method according to claim 22, wherein, One or more iterations of the sampling phase and the simulation phase constitute a single rollout, and the method includes performing multiple rollouts, wherein in each rollout, the sampling phase is repeated to sample the agent target and the corresponding agent trajectory for one or more iterations of the simulation phase performed in each rollout.

24. The method according to claim 23, wherein, The current agent action is sampled in each rollout for use in that rollout.

25. The method according to claim 23, wherein, The multiple rollouts are performed according to a probabilistic tree search algorithm, wherein the root node of the search tree represents the current state of the mobile robot, the edges of the search tree represent selected self-actions, and the additional nodes represent predicted states, wherein the branches of the search tree are explored in each rollout by the successive selection of self-actions in one or more iterations of the simulation phase performed in the rollout.

26. The method according to claim 22, wherein, The self-action is selected probabilistically in each iteration of the simulation phase.

27. The method according to claim 1 or 2, wherein, Each action is either a basic maneuver or a macroscopic action formed by a sequence of multiple basic maneuvers.

28. The method of claim 25, implemented in a planner, for planning an optimal self-action or sequence of actions for the mobile robot.

29. The method according to claim 28, wherein, The optimal self-action or self-action sequence is determined based on the rewards backpropagated through the search tree, with each reward calculated for nodes determined to have collided, for nodes determined to have reached their self-goal without collision, or for nodes where the branch terminates without collision and without reaching their self-goal.

30. The method according to claim 29, wherein, For each node determined to have reached the goal without collision, the reward is the reward for the simulated ego trajectory calculated for the branch containing the node.

31. The method according to claim 4, wherein, Implemented in the planner to plan the optimal self-movements or sequence of movements for the mobile robot; The optimal self-action or series of self-actions is determined based on rewards backpropagated through a search tree, wherein the root node of the search tree represents the current state of the mobile robot, the edges of the search tree represent selected self-actions, and the additional nodes represent predicted states. Each reward is calculated for nodes determined to have collided, for nodes determined to have reached a self-goal without collision, or for nodes where the branch terminates without collision and without reaching a self-goal. For each node determined to have reached the goal without collision, the reward is the reward for the simulated ego trajectory calculated for the branch containing the node; and The reward for the simulated self-trajector is calculated using the same reward function as the probability target distribution of the computation agent and the probability trajectory distribution of the set of possible trajectories associated with it.

32. The method according to claim 1 or 2, wherein, The observed trajectory originates from physical sensor measurements or from a simulated proxy trajectory generated in an external vehicle simulator.

33. The method according to claim 1, wherein, The possible trajectory is a smoothed trajectory that has been smoothed by applying motion smoothing, specifically velocity smoothing.

34. The method according to claim 33, wherein, The velocity smoothing is applied to the first optimal trajectory and the second optimal trajectory, but not to the observed trajectory.

35. A computer system comprising one or more computers programmed or otherwise configured to implement the method as claimed in any one of claims 1 to 34.

36. The computer system according to claim 35, implemented in a mobile robot or an off-vehicle simulator.

37. A computer program product comprising a computer program that, when executed by a computer system, implements the method according to any one of claims 1 to 34.

Images