SYSTEM AND METHOD FOR CONTROLLING THE MOTION OF ONE OR MORE DEVICES ALONG A CALCULATED OPTIMUM TRAJECTORY - Patent application

Abstract

The present disclosure provides a system and method for controlling the motion of a device from an initial state to a goal state in an environment having obstacles that form constraints on the motion of the device. The method includes generating an initial trajectory by executing a learned function trained by machine learning to generate feasible or infeasible trajectories that connect the initial state of the device to the goal state of the device while penalizing the degree of violation of at least some of the constraints. The method further includes solving a convex optimization problem subject to constraints to generate an optimal trajectory that minimizes deviations from the initial trajectory, and controlling the motion of the device according to the optimal trajectory.

Description

[Technical Field]

[0001] The present disclosure relates generally to motion planning for devices, and more particularly to systems and methods for controlling the motion of one or more devices from an initial state to a goal state in an environment having obstacles. [Background technology]

[0002] Optimal motion planning for devices to reach specific locations in a workspace containing obstacles while avoiding collisions with obstacles and each other has multiple applications, such as wheeled ground robots, ground-based aircraft, and flying drones. The task in such a motion planning problem includes constraints on the device's motion trajectory. The constraints can encode requirements for each device's target location, collision avoidance with each other, and collisions with other stationary or moving obstacles in the workspace. For example, an airborne drone can be tasked with reaching a specific location to perform a structural inspection while avoiding collisions with other structures and with each other. An aircraft can be tasked with navigating the airport grounds while avoiding other aircraft, carts, and structures to reach an assigned runway or gate. A mobile robot on a factory floor can be tasked with reaching a work location or a location where cargo needs to be unloaded while avoiding work areas, human workers, and other robots. Summary of the Invention [Problem to be solved by the invention]

[0003] Such motion planning problems are difficult because the achievable workspace and the set of device configurations where all task-related constraints are satisfied are typically non-convex. Thus, motion planning problems are non-convex optimization problems. As used herein, a motion planning problem refers to determining a motion trajectory of a device to perform a task. However, solving a non-convex optimization problem requires a significant amount of computational and memory resources.

[0004] Motion planners are used to solve non-convex optimization problems. One example of a non-convex optimization-based motion planner uses a mixed integer programming (MIP) approach. The MIP approach involves solving a constrained optimization problem, where a cost function is minimized or maximized under some constraints, and some optimization variables are real-valued (continuous variables) and others are integer or binary-valued (discrete variables). In particular, mixed integer linear / quadratic programming (MILP / MIQP) is applicable when the trajectory of a device's motion can be described by linear dynamics and obstacles can be represented, possibly in a conservative manner, for example, by polygons (i.e., linear inequalities). MILP / MIQP is known to complete in finite time, and therefore, bounds on the time to compute a solution can be derived. However, the computations are still complex (NP-hard) and can incur significant computational costs. Therefore, a solution is needed that can achieve optimal planning of a device's motion trajectory in a workspace with obstacles while using limited computational and memory resources. [Means for solving the problem]

[0005] An objective of some embodiments is to provide a system and method for controlling the motion of a device in an environment from an initial state to a goal state. Another objective of some embodiments is to determine a motion trajectory of a device to perform a task, such as reaching a goal state. The device state may include one or a combination of the device's position, device orientation, and device velocity. The environment includes obstacles. The device may be an autonomous vehicle, such as a mobile robot, an aircraft, a surface vehicle, or an underwater vehicle. To perform a task, constraints on the device's motion must be satisfied. In other words, task execution is subject to constraints. Examples of such constraints include reaching a target position, avoiding collisions with obstacles, avoiding collisions with other devices, and moving the device so that the device's state (e.g., position) falls within a predetermined set of allowable states for the device. For example, a drone may be tasked with reaching a specific location for inspecting a structure while avoiding collisions with parts of the structure.

[0006] Some embodiments are based on the recognition that the allowable states and the set of device configurations / states where all constraints are satisfied are typically non-convex. Thus, a motion planning problem is a non-convex optimization problem. As used herein, a motion planning problem refers to determining a motion trajectory of a device to perform a task. However, solving a non-convex optimization problem requires a significant amount of computational and memory resources.

[0007] Some embodiments are based on the recognition that the allowable states and the set of device configurations / states where all constraints are satisfied are typically non-convex. Thus, a motion planning problem is a non-convex optimization problem. As used herein, a motion planning problem refers to determining a motion trajectory of a device to perform a task. However, solving a non-convex optimization problem requires a significant amount of computational and memory resources.

[0008] An objective of some embodiments is to use machine learning to reduce the computational burden of constrained non-convex optimization problems. Some embodiments are based on the understanding that various learned functions, such as neural networks, can be trained offline by a training system with sufficient computational requirements and executed online by a controller with limited computational power.

[0009] However, replacing a constrained non-convex optimization problem with such a learned function is problematic due to the nature of machine learning. Specifically, machine learning involves computational statistics, which are inherently probabilistic. As a result, learned functions, such as neural networks, are not configured to manage hard constraints, but are designed to satisfy constraints with some probability. This concept directly replicates the concept of soft constraints, which are similar to the concept of soft constraints, where violation of the constraint is penalized but not prohibited.

[0010] As a result, it is impossible, or at least impractical, to train a neural network that provides a solution to a non-convex optimization problem subject to the constraints as hard constraints. While there is a good chance of finding such a solution, it is conditional on sufficient offline training data that closely approximates the online environment, which is impractical. Because motion planning and control require satisfying constraints, some embodiments recognize that it is possible to use the learned function to determine an initial solution to the non-convex optimization problem, i.e., an initial trajectory for the device subject to soft constraints. However, such an initial trajectory must be further processed to ensure that the final trajectory satisfies the constraints.

[0011] Some embodiments recognize that, given an initial trajectory that satisfies constraints, it is also possible to optimize the initial trajectory to guarantee the constraints. In addition, such optimization is convex. In other words, given an initial trajectory, a non-convex optimization problem subject to hard constraints can be transformed into a convex optimization problem subject to hard constraints if the initial trajectory is feasible. However, initial trajectories generated by learned functions trained under soft constraints do not guarantee feasibility and generally cannot guarantee such a transformation.

[0012] However, some embodiments are based on the recognition that the transformation from a non-convex optimization problem to a convex optimization problem relies on the possibility of convexity around the initial trajectory, which provides the union of a time-varying set of convex regions that contain the goal state. Each convex region is generated by considering a portion of the initial trajectory and a side of the obstacle. Therefore, the initial trajectory does not have to be feasible. Conversely, the initial trajectory may be infeasible as long as the convex region contains the initial state and the goal state.

[0013] Some embodiments are based on the intuitive fact, supported by numerous tests and experiments, that a learned function trained to minimize the violation rate of constraints will generate good initial trajectories for such transformations. One example of such training is reinforcement learning (RL), a branch of machine learning that focuses on how an intelligent device should act in an environment to maximize cumulative reward.

[0014] Furthermore, some embodiments are based on the recognition that there may be probabilistic uncertainties in device dynamics due to nonlinearities, startup mismatches, and unmodeled phenomena, and that there may be probabilistic uncertainties in device and obstacle locations due to detection limitations. In such cases where the device dynamics include probabilistic uncertainties, the constraints are replaced with probabilistic safety constraints (also called chance constraints). The optimization-based safety filter is configured to enforce the probabilistic safety constraints as hard constraints to ensure safety with a high probability. In one embodiment, the optimization-based safety filter enforces the probabilistic safety constraints based on a convex chance approximation of the probabilistic safety constraints.

[0015] Accordingly, one embodiment discloses a controller for controlling motion of a device from an initial state to a goal state in an environment having obstacles that form constraints on the motion of the device, the controller comprising: a processor and a memory having instructions stored thereon that, when executed by the processor, cause the controller to: generate an initial trajectory by executing a learned function trained by machine learning to generate feasible or infeasible trajectories that connect the initial state of the device to a goal state of the device while penalizing the degree of violation of at least some of the constraints; solve a constrained convex optimization problem to generate an optimal trajectory that minimizes deviation from the initial trajectory; and control the motion of the device according to the optimal trajectory.

[0016] Accordingly, another embodiment discloses a method for controlling motion of a device from an initial state to a goal state in an environment having obstacles that form constraints on the motion of the device, the method including generating an initial trajectory by executing a learned function trained by machine learning to generate feasible or infeasible trajectories that connect the initial state of the device to a goal state of the device while penalizing the degree of violation of at least some of the constraints, solving a constrained convex optimization problem to generate an optimal trajectory that minimizes deviation from the initial trajectory, and controlling the motion of the device according to the optimal trajectory.

[0017] Accordingly, another embodiment discloses a non-transitory computer-readable storage medium including a program executable by a processor to perform a method for controlling motion of a device from an initial state to a goal state in an environment having obstacles that form constraints on the motion of the device, the method including: generating an initial trajectory by executing a learned function trained by machine learning to generate feasible or infeasible trajectories that connect the initial state of the device to a goal state of the device while penalizing the degree of violation of at least some of the constraints; solving a constrained convex optimization problem to generate an optimal trajectory that minimizes deviation from the initial trajectory; and controlling motion of the device according to the optimal trajectory.

[0018] The presently disclosed embodiments will be further described with reference to the accompanying drawings, in which the drawings are not necessarily to scale, emphasis instead generally being placed upon illustrating the principles of the presently disclosed embodiments. [Brief explanation of the drawings]

[0019] [Figure 1A] 1 illustrates an example of an environment in which devices are controlled, according to an embodiment of the present disclosure. [Figure 1B] FIG. 1 illustrates a block diagram of a controller for controlling the motion of a device in an environment from an initial state to a goal state, according to an embodiment of the present disclosure. [Figure 1C] FIG. 10 illustrates an example of an initial trajectory, according to an embodiment of the present disclosure. [Figure 1D] FIG. 10 illustrates a non-convex collision-free region determined by implementing an optimization-based safety filter, according to an embodiment of the present disclosure. [Figure 1E] FIG. 10 illustrates a technique for generating convex regions that define non-convex collision-free regions, according to an embodiment of the present disclosure. [Figure 1F] FIG. 1 illustrates multiple half-spaces generated by an optimization-based safety filter as a device navigates through an environment, according to an embodiment of the present disclosure. [Figure 2A] 1 illustrates an environment including multiple devices, each having its own target state, according to an embodiment of the present disclosure. [Figure 2B] 1 shows a schematic diagram of motion planning for multiple devices according to an embodiment of the present disclosure. [Figure 2C] 10 shows a schematic diagram of motion planning for multiple devices according to an alternative embodiment of the present disclosure. [Figure 3] FIG. 1 shows a schematic diagram of a Markov Decision Process (MDP) used in reinforcement learning, according to some embodiments of the present disclosure. [Figure 4] FIG. 1 shows a schematic diagram of a deep neural network used as a predictive model (i.e., a learned function) according to some embodiments of the present disclosure. [Figure 5A] FIG. 10 illustrates a convex approximation of a constraint requiring a device to stay within a predetermined environment polyhedron, according to some embodiments of the present disclosure. [Figure 5B] FIG. 10 illustrates a convex approximation of obstacle avoidance constraints according to some embodiments of the present disclosure. [Figure 5C] FIG. 10 illustrates a convex approximation of inter-device collision avoidance constraints according to some embodiments of the present disclosure. [Figure 5D] FIG. 1 illustrates a block diagram for formulating constraints on the state xt of a device when the state xt is stochastic and non-stochastic, according to some embodiments of the present disclosure. [Figure 6] 1 illustrates an example of a two-dimensional projection of a control invariant set corresponding to a constraint set, according to an embodiment of the present disclosure. [Figure 7] FIG. 1 illustrates a block diagram of a method for controlling the motion of a device from an initial state to a goal state in an environment with obstacles, according to an embodiment of the present disclosure. [Figure 8] FIG. 1 is a schematic diagram illustrating a computing device for implementing the methods and system / controller of the present disclosure. DETAILED DESCRIPTION OF THE INVENTION

[0020] For purposes of explanation, numerous specific details are set forth in the following description to provide a thorough understanding of the present disclosure. However, it will be apparent to those skilled in the art that the present disclosure may be practiced without these specific details. In other instances, devices and methods are shown in block diagram form solely to avoid obscuring the present disclosure.

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

[0022] 1A illustrates an example of an environment 100 in which a device 101 is controlled, according to an embodiment of the present disclosure. An objective of some embodiments is to control the motion of the device 101 in the environment 100 from an initial state 103 to a goal state 105. An objective of some embodiments is to determine a motion trajectory of the device 101 to perform a task, for example, to reach the goal state 105. The state of the device 101 may include one or a combination of the position of the device 101, the orientation of the device 101, and the velocity of the device 101.

[0023] The environment 100 includes obstacles 107 and 109. The device 101 may be an autonomous vehicle, such as a mobile robot, an aircraft, a surface vehicle, or an underwater vehicle. To perform a task, constraints on the motion of the device 101 must be satisfied. In other words, the execution of the task is constrained. Examples of such constraints include reaching a goal state 105, avoiding collisions with the obstacles 107 and 109 (i.e., device-to-obstacle collision avoidance constraints), avoiding collisions with other devices (i.e., device-to-device collision avoidance constraints), moving the device 101 such that its state (e.g., position) is within a predetermined set of allowable states for the device 101 (i.e., keep-in constraints), etc. As an example, a drone may be tasked with reaching a specific location for inspecting a structure while avoiding collisions with parts of the structure.

[0024] To achieve the above objectives, some embodiments provide a controller 111 for controlling the movement of a device 101 in an environment 100 from an initial state 103 to a target state 105 .

[0025] FIG. 1B illustrates a block diagram of a controller 111 according to an embodiment of the present disclosure. The controller 111 includes a processor 113 and a memory 115. The processor 113 may be a single-core processor, a multi-core processor, a computing cluster, or any number of other configurations. In some embodiments, the processor 113 is an embedded processing unit (EPU). The memory 115 may include random access memory (RAM), read-only memory (ROM), flash memory, or any other suitable memory system. Additionally, in some embodiments, the memory 115 may be implemented using a hard drive, an optical drive, a thumb drive, an array of drives, or any combination thereof. In some embodiments, the controller 111 is communicatively coupled to the device 101. By way of example, the controller 111 may be remotely connected to the device 101 via a network. In an alternative embodiment, the controller 111 is embedded in the device 101.

[0026] Some embodiments are based on the recognition that the allowable states and the set of device configurations / states where all constraints are satisfied are typically non-convex. Thus, the motion planning problem is a non-convex optimization problem. As used herein, a motion planning problem refers to determining a motion trajectory of the device 101 to perform a task. However, solving a non-convex optimization problem requires a significant amount of computational and memory resources.

[0027] An objective of some embodiments is to use machine learning to reduce the computational burden of constrained non-convex optimization problems. Some embodiments are based on the understanding that various learned functions associated with machine learning, such as neural networks, can be trained offline by a training system with sufficient computational requirements and executed online by a controller with limited computational power.

[0028] However, replacing a constrained non-convex optimization problem with such a learned function is problematic due to the nature of machine learning. Specifically, machine learning involves computational statistics, which are inherently probabilistic. As a result, learned functions, such as neural networks, are not configured to manage hard constraints, but are designed to satisfy constraints with a certain probability. A hard constraint is one that "must" always be satisfied. However, the probabilistic notion of a constraint is similar to, or even directly replicates, the notion of a soft constraint, where violation of the constraint is penalized but not prohibited.

[0029] As such, it is impossible, or at least impractical, to train a neural network to provide a solution to a non-convex optimization problem subject to the constraints as hard constraints. While such a solution is likely to be found, it is impractical provided there is sufficient offline training data to closely approximate the online environment. Because motion planning and control require satisfying constraints, some embodiments recognize that it is possible to use learned functions to determine an initial solution to the non-convex optimization problem, i.e., an initial trajectory for device 101 subject to soft constraints. However, such an initial trajectory must be further processed to ensure that the final trajectory satisfies the constraints.

[0030] Some embodiments recognize that, given an initial trajectory that satisfies a constraint, it is also possible to optimize the initial trajectory to guarantee satisfaction of the constraint. Moreover, such optimization is convex. In other words, given an initial trajectory, a non-convex optimization problem subject to hard constraints can be transformed into a convex optimization problem subject to hard constraints if the initial trajectory is feasible. However, initial trajectories generated by learned functions trained under soft constraints do not guarantee feasibility and generally cannot guarantee such a transformation.

[0031] However, some embodiments are based on the recognition that the transformation of a non-convex optimization problem into a convex optimization problem relies on the possibility of convexity around the initial trajectory, which provides the union of a time-varying set of convex regions that contain the target state 105. As such, the initial trajectory does not have to be feasible. Conversely, the initial trajectory may be infeasible as long as a convex region contains the initial state 103 and the target state 105.

[0032] FIG. 1C illustrates an example of an initial trajectory 117 according to an embodiment of the present disclosure. The processor 113 (shown in FIG. 1B) is configured to execute a learned function to generate the initial trajectory 117 that connects the initial state 103 to the goal state 105 while penalizing the degree of violation of at least some of the constraints. The learned function may be stored in the memory 115 (shown in FIG. 1B) of the controller 111. According to an embodiment, the learned function is a neural network trained using reinforcement learning based on a reward function. The reward function penalizes the degree of violation of the constraints. The reward function returns a higher value when the initial trajectory 117 satisfies the constraints compared to when the constraints are violated (i.e., when the reward function penalizes constraint violations). In an embodiment, an algorithm such as proximal policy optimization is used to determine the initial trajectory that maximizes the reward function.

[0033] Initial trajectory 117 is an infeasible trajectory because it overlaps obstacle 107. Furthermore, given initial trajectory 117, processor 113 executes an optimization-based safety filter to determine a non-convex collision-free region.

[0034] FIG. 1D illustrates a non-convex collision-free region 119 determined by executing an optimization-based safety filter according to an embodiment of the present disclosure. The optimization-based safety filter may be stored in memory 115 (shown in FIG. 1B) of controller 111. Furthermore, processor 113 (shown in FIG. 1B) solves a convex optimization problem subject to hard constraints to generate an optimal trajectory 121 that is within the non-convex collision-free region 119 and minimizes deviation from initial trajectory 117. Processor 113 controls device 101 according to the optimal trajectory 121. For example, based on the optimal trajectory 121, controller 111 determines control inputs to actuators of device 101 to track the optimal trajectory 121. Because the optimal trajectory is within the non-convex collision-free region 119, device 101 does not overlap obstacles 107 and 109 when controlled according to the optimal trajectory 121. For example, even when device 101 approaches corners 123 and 125 when executing the optimal trajectory 121, device 101 does not overlap obstacle 107.

[0035] According to one embodiment, the non-convex collision-free region 119 is the union of a time-varying set of convex regions, each generated by considering a portion of the initial trajectory 117 and the sides of the obstacles 107, 109.

[0036] 1E illustrates an approach to generating a convex region that defines a non-convex collision-free region 119, according to an embodiment of the present disclosure. According to an embodiment, the convex region is generated by generating a half-space. Specifically, when the device 101 starts at the initial state 103, an optimization-based safety filter projects the current position of the device 101 (i.e., the initial state 103) onto the obstacles 107 and 109, resulting in support half-spaces 127 and 129. The intersection of the half-spaces 127 and 129 forms a convex region 131 that contains the device 101 in the initial state 103. Based on the initial trajectory 117, the processor 113 determines a trajectory that minimizes deviation from the initial trajectory 117 and maintains the device 101 in the convex region 131. As the device 101 moves along the trajectory, the support half-space is continuously updated based on the current state of the device 101. When device 101 reaches position 133, the support half-space of obstacle 107 changes from 129 to 135, and the convex region changes from 131 to 137. Furthermore, when device 101 reaches position 139, the support half-space of obstacle 107 changes from 135 to 141, and the convex region changes from 137 to 143.

[0037] 1F illustrates all half-spaces 127, 129, 135, 141, 145, 147, and 149 generated by the optimization-based safety filter as device 101 navigates within environment 100, according to an embodiment of the present disclosure. The intersections of half-spaces 127, 129, 135, 141, 145, 147, and 149 form convex regions 131, 137, and 143, which together define non-convex collision-free region 119.

[0038] Furthermore, some embodiments are based on the recognition that such a framework for motion planning (based on learned functions and optimization-based safety filters) can be extended for motion planning for multiple devices, where each device has its own target state.

[0039] 2A illustrates an environment 200 including multiple devices, each with its own goal state, according to an embodiment of the present disclosure. Devices 201a, 201b, 201c, and 201d are required to reach their assigned goal states 203a, 203b, 203c, and 203d. Environment 200 includes obstacles 205a, 205b, 205c, 205d, 205e, and 201f. The objective of some embodiments is to determine, for each device, an optimal trajectory that connects each device to its goal state while avoiding collisions with each other and with obstacles 205a, 205b, 205c, 205d, 205e, and 201f.

[0040] In some embodiments, the controller 111 executes on a remote centralized server and is communicatively coupled to each device. In such embodiments, for motion planning of multiple devices, a learned function is formulated for each device to generate an initial trajectory for each device separately. However, rather than solving the convex optimization problem for each device separately, the convex optimization problem is transformed into a joint optimization problem that simultaneously determines an optimal trajectory for each device based on its initial trajectory. Therefore, for motion planning of multiple devices, the controller 111 includes a number of learned functions equal to the number of devices and an optimization-based safety filter that simultaneously determines the optimal motion trajectory for each device by solving the joint optimization problem. Such motion planning of multiple devices is advantageous because a learned function for a single device is easier to train, requires fewer computational resources, and exhibits better performance compared to training a neural network (or model) that simultaneously provides initial trajectories for all devices. Motion planning for multiple devices is described below with reference to FIG. 2B.

[0041] 2B shows a schematic diagram of motion planning for multiple devices, according to an embodiment of the present disclosure. Learned functions 207a-207d generate initial trajectories 209a-209d, respectively. The initial trajectories 209a-209d are associated with devices 201a-201d, respectively. The initial trajectories 209a-209d are applied to an optimization-based safety filter 211, which solves a joint optimization problem to generate optimal trajectories. 213 a to 213d are determined simultaneously. 213 a to 213d are associated with devices 201a to 201d, respectively.

[0042] The controller 111 controls each device based on its optimal trajectory. In particular, the controller 111 controls each device based on a predetermined number of states of its optimal trajectory. When the devices 201a-201d are controlled based on their respective optimal trajectories, the devices 201a-201d reach new states 215a-215d. The new states 215a-215d are fed back (217) to the learned functions 207a-207d, respectively. The learned functions 207a-207d then generate new initial trajectories based on the new states 215a-215d, and the above process is repeated.

[0043] In some alternative embodiments, for motion planning for multiple devices, controller 111 executes on each device, such embodiments being described below in FIG. 2C.

[0044] 2C shows a schematic diagram of motion planning for multiple devices according to an alternative embodiment of the present disclosure. Each of devices 201a-201d includes a controller (similar to controller 111). By way of example, device 201a includes controller 219 including learned function 219a and optimization-based safety filter 219b. Learned function 219a is configured to generate an initial trajectory for device 201a. Optimization-based safety filter 219b is configured to solve a constrained convex optimization problem to generate an optimal trajectory that minimizes deviation from the initial trajectory for device 201a. Controller 219 further controls the motion of device 201a based on the optimal trajectory generated for device 201a.

[0045] Similarly, device 201b includes controller 221 including learned function 221a and optimization-based safety filter 221b, device 201c includes controller 223 including learned function 223a and optimization-based safety filter 223b, and device 201d includes controller 225 including learned function 225a and optimization-based safety filter 225b. Each device's learned function generates an initial trajectory for that device, and then each device's optimization-based safety filter generates an optimal trajectory for the corresponding device.

[0046]

number

[0047]

number

[0048] FIG. 3 shows a schematic diagram of an MDP according to some embodiments of the present disclosure. At each time step, a policy 301 transforms observations 303 (o(k) above) into actions 305 (a(k) above). A reward function 307 is a function of observations 303 and actions 305 defined in (3). A world 309 consists of a system 311, a measurement model 313, and a clock 315. The system 311 reaches the next internal state x(k+1) using a step function defined based on the closed-loop dynamics (1b), the current internal state x(k), the current action a(k), and the current reference control r(k). The measurement model 313 transforms the state into the appropriate observation vector. The clock 315 increments time k to k+1.

[0049] Some embodiments of the present disclosure use a function approximator to characterize the policy 301. As a result, an appropriate predictive model can be constructed that provides an initial trajectory (e.g., initial trajectory 117). The predictive model corresponds to the learned function. The predictive model may be deterministic or probabilistic and may rely on a neural architecture or kernel representation. Examples of deterministic predictive models include multi-layer perceptrons, convolutional neural networks, kernel regression, and support vector machines, etc. Examples of probabilistic predictive models include Bayesian neural networks, neural processes, Gaussian processes, Kriging interpolation, etc.

[0050]

number

[0051]

number

[0052] Some reinforcement learning algorithms are used to train a predictive model (or learned function) of the initial trajectory, and another predictive model of the policy is obtained so that the cumulative sum of the reward function is maximized. The reinforcement learning algorithms may include Q-learning, state-action-reward-state-action, deep Q-network, deep deterministic policy gradient, asynchronous actor-critic algorithm, trust region policy optimization, and / or proximal policy optimization.

[0053] Some embodiments are based on the recognition that constraints may include static constraints, such as building walls, and dynamic constraints, such as moving obstacles whose positions change as a function of time. Some embodiments are based on the recognition that if static constraints are taken into account during training of the learned function, the presence of unknown dynamic constraints can still be managed by a convex optimization problem. This is because the dynamic constraints are less stringent due to the added flexibility in the time domain. As such, the constraints managed by the convex optimization problem may differ from the constraints used during training of the learned function. Thus, the learned function is trained to penalize only violations of the static constraints, and a convex optimization problem is solved under the static and dynamic constraints.

[0054] Furthermore, some embodiments recognize that there may be probabilistic uncertainties in device dynamics due to nonlinearities, startup mismatches, and unmodeled phenomena, and that there may be probabilistic uncertainties in device and obstacle locations due to detection limitations. In such cases where the device dynamics include probabilistic uncertainties, the constraints are replaced with probabilistic safety constraints (also referred to as chance constraints). The optimization-based safety filter is configured to enforce the probabilistic safety constraints as hard constraints to ensure safety with a high probability. In one embodiment, as illustrated in Figures 5A, 5B, and 5C, the optimization-based safety filter enforces the probabilistic safety constraints based on a convex chance approximation of the probabilistic safety constraints.

[0055]

number

[0056]

number

[0057] 5A illustrates a convex approximation of a first constraint that device 101 must stay within a predetermined environment polyhedron 501, according to some embodiments of the present disclosure. The constraint is naturally convex for non-stochastic dynamics of device 101 (1a). On the other hand, for stochastic dynamics of device 101 (1c), device 101 is required to stay within a smaller set 503. Reinforcing environment polyhedron 501 with reinforcement regions 505 results in the formation of smaller sets 503, which arise due to the stochasticity of the dynamics. Additionally, in some embodiments, similar convex approximations may be constructed for additional constraints on the state of device 101, including its velocity.

[0058] FIG. 5B illustrates a N O The region 507 shows a convex approximation of the second constraint of not colliding with stationary obstacles. j 5D illustrates the shape of obstacle j with a linear half-space 511 that is contained within the other half-space of the linear half-space 513 that contains region 507. Half-space 511 is composed of obstacle boundary 515 and normal vector 517, and is augmented by an augmentation region 519 to account for probabilistic uncertainty. The required augmentation is illustrated in FIG. 5D.

[0059]

number

[0060]

number

[0061] As such, some embodiments of the present disclosure are based on the recognition that when the dynamics are non-stochastic, the constraints do not require tightening, and in such an event simplify to a convex approximation of the original.

[0062] Some embodiments recognize that typical model predictions require recursive feasibility constraints to practically realize the controller 111. Specifically, the mean position of the device is required to remain within a well-defined control invariant set so that the device can be safely controlled despite probabilistic uncertainty. An example control invariant set is described below in FIG. 6.

[0063] 6 illustrates an example two-dimensional projection of a control invariant set 603 corresponding to a constraint set 601, according to an embodiment of the present disclosure. In one embodiment, the constraint set 601 may be a multidimensional polyhedron determined by a hyperplane represented by linear inequalities along multiple dimensions corresponding to constraints on the optimal trajectory of the device 101. The constraint set 601 encodes states of the device 101 that are safe. For any state of the device 101 in the control invariant set 603, there exist control commands that maintain the state of the device 101 in the control invariant set 603 relative to known or acceptable future states.

[0064] For example, for any state of device 101 within all possible control commands 617-623 that controller 111 can execute, such as state 615 in control invariant set 603, there exists at least one control command 623 that keeps the state of device 101 within control invariant set 603. On the other hand, state 605 may be feasible for one iteration, but all control commands 607-613 that controller 111 is allowed to obtain during the next iteration may cause the state of device 101 to be outside constraint set 601. Examples of constraint sets 601 include convex approximations of the enterable workspaces discussed in Figures 5A, 5B, and 5C.

[0065] 7 illustrates a block diagram of a method 700 for controlling the motion of a device in an environment with obstacles from an initial state to a goal state, according to an embodiment of the present disclosure. At block 701, the method 700 obtains parameters of a task from the device 101 and / or a remote server. The parameters of the task include the state of the device 101. In some embodiments, the parameters may include constraints that characterize the task, such as one or a combination of the initial state of the device 101, the goal state of the device 101, the geometric configuration of one or more stationary obstacles that define at least a portion of the constraints, and the motion of moving obstacles that define at least a portion of the constraints. Further, the parameters are provided to the learned function.

[0066] At block 703, the method 700 includes executing a learned function trained using machine learning to generate an initial trajectory connecting an initial state of the device to a goal state of the device. According to one embodiment, the learned function is a neural network trained using reinforcement learning based on a reward function that penalizes the degree of violation of constraints. At block 705, the method 700 includes solving a constrained convex optimization problem to generate an optimal trajectory that minimizes deviation from the initial trajectory. In one embodiment, an optimization-based safety filter generates the optimal trajectory by solving a convex optimization problem subject to the constraints as hard constraints. At block 707, the method 700 includes controlling the motion of the device 101 according to the optimal trajectory.

[0067] 8 is a schematic diagram illustrating a computing device 800 for implementing the disclosed method and system / controller. The computing device 800 includes a power supply 801, a processor 803, a memory 805, and a storage device 807, all connected to a bus 809. Furthermore, a high-speed interface 811, a low-speed interface 813, a high-speed expansion port 815, and a low-speed connection port 819 may be connected to the bus 809. Additionally, a low-speed expansion port 817 is connected to the bus 809. Furthermore, an input interface 821 may be connected to an external receiver 823 and an output interface 825 via the bus 809. The receiver 827 may be connected to an external transmitter 829 and a transmitter 831 via the bus 809. An external memory 833, an external sensor 835, a machine 837, and an environment 839 may also be connected to the bus 809. Furthermore, one or more external input / output devices 841 may be connected to the bus 809. A network interface controller (NIC) 843 may be adapted to connect to a network 845 via bus 809, and may, among other things, enable data or other data to be rendered on a third-party display device, a third-party imaging device, and / or a third-party printing device external to computing device 800.

[0068] The memory 805 can store instructions executable by the computing device 800, as well as any data that can be utilized by the methods and systems of the present disclosure. The memory 805 can include random access memory (RAM), read-only memory (ROM), flash memory, or any other suitable memory system. The memory 805 can be one or more volatile memory units and / or one or more non-volatile memory units. The memory 805 can also be another form of computer-readable medium, such as a magnetic disk or an optical disk.

[0069] The storage device 807 may be adapted to store supplemental data and / or software modules used by the computing device 800. The storage device 807 may include a hard drive, an optical drive, a thumb drive, an array of drives, or any combination thereof. Additionally, the storage device 807 may include a computer-readable medium such as a floppy disk device, a hard disk device, an optical disk device, or an array of devices including a tape device, a flash memory or other similar solid-state memory device, or a storage area network or other configuration of devices. The instructions may be stored on an information carrier. When executed by one or more processing devices (e.g., processor 803), the instructions perform one or more methods, such as those described above.

[0070] Computing device 800 may optionally be linked through bus 809 to a display interface or user interface (HMI) 847 adapted to connect computing device 800 to a display device 849 and keyboard 851, which may include, among other things, a computer monitor, a camera, a television, a projector, or a mobile device. In some implementations, computing device 800 may include a printer interface for connecting to a printing device, which may include, among other things, a liquid inkjet printer, a solid ink printer, a large scale commercial printer, a thermal printer, a UV printer, or a dye-sublimation printer.

[0071] The high-speed interface 811 manages bandwidth-intensive operations of the computing device 800, and the low-speed interface 813 manages low-bandwidth-intensive operations. Such an allocation of functionality is merely an example. In some implementations, the high-speed interface 811 can be coupled to memory 805, a user interface (HMI) 847, a keyboard 851, and a display device 849 (e.g., through a graphics processor or accelerator), and can be coupled to a high-speed expansion port 815 that can accept various expansion cards via the bus 809.

[0072] In one implementation, low-speed interface 813 is coupled to storage device 807 and low-speed expansion port 817 via bus 809. Low-speed expansion port 817, which may include various communication ports (e.g., USB, Bluetooth, Ethernet, wireless Ethernet), may be connected to one or more input / output devices 841. Computing device 800 may be coupled to server 853 and rack server 855. Computing device 800 may be implemented in several different forms. For example, computing device 800 may be implemented as part of rack server 855.

[0073] This specification provides only exemplary embodiments and is not intended to limit the scope, applicability, or configuration of the present disclosure. Rather, the following description of exemplary embodiments will provide those skilled in the art with an enabling description for implementing one or more exemplary embodiments. Various changes may be made to the function and arrangement of elements without departing from the spirit and scope of the disclosed subject matter as set forth in the appended claims. Specific details are provided in the following description to provide a thorough understanding of the embodiments. However, those skilled in the art will understand that embodiments may be practiced without these specific details. For example, systems, processes, and other elements in the disclosed subject matter may be shown as components in block diagram form so as not to obscure the embodiments in unnecessary detail. In other instances, well-known processes, structures, and techniques may be shown without unnecessary detail so as not to obscure the embodiments. Furthermore, like reference numbers and names in the various drawings refer to like elements.

[0074] Also, particular embodiments may be described as a process that is depicted as a flowchart, a flow diagram, a data flow diagram, a structure diagram, or a block diagram. While a flowchart may describe operations as a sequential process, many of the operations may be performed in parallel or simultaneously. Additionally, the order of operations may be rearranged. A process may be terminated when its operations are completed, but may have additional steps not discussed or included in the diagram. Moreover, not all operations in any specifically described process may occur in all embodiments. A process may correspond to a method, a function, a procedure, a subroutine, a subprogram, etc. When a process corresponds to a function, the termination of the function may correspond to a return of the function to the calling function or the main function.

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

[0076] Furthermore, embodiments of the present disclosure and the functional operations described herein can be implemented in digital electronic circuitry, in tangibly embodied computer software or firmware, in computer hardware including the structures disclosed herein and their structural equivalents, or in one or more combinations thereof. Furthermore, some embodiments of the present disclosure can be implemented as one or more computer programs, i.e., as one or more modules of computer program instructions encoded on a tangible, non-transitory program carrier for execution by or to control the operation of a data processing apparatus. Still further, the program instructions can be encoded on an artificially generated propagated signal, for example, an electrical, optical, or electromagnetic signal generated by a machine. The propagated signal is generated to encode information that is transmitted to a suitable receiving device for execution by a data processing apparatus. The computer storage medium may be a machine-readable storage device, a machine-readable storage substrate, a random-access or serial-access memory device, or one or more combinations thereof.

[0077] A computer program (which may also be called or described as a program, software, software application, module, software module, script, or code) may be written in any form of programming language, including compiled or interpreted languages, or declarative or procedural languages, and may be deployed in any form, either as a stand-alone program or as a module, component, subroutine, or other unit suitable for use in a computing environment.

[0078] A computer program may, but need not, correspond to a file in a file system. A program can be stored within a file that holds other programs or data, such as one or more scripts stored in a markup language document, a single file dedicated to the program, or multiple coordinated files, such as files that store one or more modules, subprograms, or portions of code. A computer program can be deployed to be executed on one computer or on multiple computers that are located at one site or distributed across multiple sites and interconnected by a communications network.

[0079] A computer suitable for running a computer program may be based, by way of example, on a general-purpose microprocessor, a special-purpose microprocessor, or both, or on any other type of central processing unit. Typically, the central processing unit receives instructions and data from a read-only memory, a random-access memory, or both. The essential elements of a computer are a central processing unit for performing or executing instructions and one or more memory devices for storing instructions and data. Typically, a computer also includes one or more mass storage devices, such as magnetic, magneto-optical, or optical disks, for storing data, or is operatively coupled to such disks to receive data from, transfer data to, or both. However, a computer need not have such devices. Furthermore, a computer may be embedded in another device, such as a mobile phone, a personal digital assistant (PDA), a mobile audio or video player, a game console, a global positioning system (GPS) receiver, or a portable storage device, such as a universal serial bus (USB) flash drive, to name a few.

[0080] To provide for user interaction, embodiments of the subject matter described herein may be implemented on a computer having a display device, e.g., a CRT (cathode ray tube) or LCD (liquid crystal display) monitor, for displaying information to the user, and a keyboard and pointing device, e.g., a mouse or trackball, for allowing the user to provide input to the computer. Other types of devices may also be used to provide for user interaction. For example, feedback provided to the user may be any form of sensory feedback, e.g., visual feedback, auditory feedback, or tactile feedback, and input from the user may be received in any form, including acoustic input, speech input, or tactile input. Additionally, the computer may provide for user interaction by sending documents to and receiving documents from a device used by the user, for example, by sending a web page to a web browser on the user's client device in response to a request received from the web browser.

[0081] Embodiments of the subject matter described herein can be implemented in a computing system that includes a back-end component, e.g., a data server, or includes a middleware component, e.g., an application server, or includes a front-end component, e.g., a client computer having a graphical user interface or web browser that allows a user to interact with an implementation of the subject matter described herein, or includes any combination of one or more such back-end, middleware, or front-end components. The components of the system can be interconnected by any form or medium of digital data communication, e.g., a communications network. Examples of communications networks include local area networks ("LANs") and wide area networks ("WANs"), e.g., the Internet.

[0082] A computing system may include clients and servers. Clients and servers are generally remote from each other and typically interact through a communication network. The relationship of client and server arises by virtue of computer programs running on the respective computers and having a mutual client-server relationship.

[0083] While the present disclosure has been described in terms of certain preferred embodiments, it is to be understood that various other adaptations and modifications can be made within the spirit and scope of the disclosure. It is, therefore, the object of the following claims to cover all such variations and modifications that fall within the true spirit and scope of the disclosure.

Claims

1. 1. A controller for controlling motion of a device from an initial state to a goal state in an environment having obstacles that form constraints on the motion of the device, the controller comprising a processor and a memory having instructions stored thereon, the instructions, when executed by the processor, causing the controller to: generating an initial trajectory by executing a learned function trained by machine learning to generate feasible or infeasible trajectories that connect the initial state of the device to the goal state of the device while penalizing the degree of violation of at least some of the constraints; solving a convex optimization problem subject to the constraints to generate an optimal trajectory that minimizes deviation from the initial trajectory; and controlling the motion of the device according to the optimal trajectory. the environment includes a plurality of devices, each of the plurality of devices being associated with a corresponding goal state, and the learned function being formulated for each device of the plurality of devices; The processor further comprises: for each device of the plurality of devices, executing the learned function to determine an initial trajectory for each device; and solving a joint optimization problem to simultaneously determine an optimal trajectory for each device, wherein the optimal trajectory for each device minimizes a deviation of the optimal trajectory for a corresponding device from the corresponding initial trajectory, and the processor is further configured to: a controller configured to control the motion of each device based on the corresponding optimal trajectory to achieve the corresponding goal state of each device.

2. 1. A controller for controlling motion of a device from an initial state to a goal state in an environment having obstacles that form constraints on the motion of the device, the controller comprising a processor and a memory having instructions stored thereon, the instructions, when executed by the processor, causing the controller to: generating an initial trajectory by executing a learned function trained by machine learning to generate feasible or infeasible trajectories that connect the initial state of the device to the goal state of the device while penalizing the degree of violation of at least some of the constraints; solving a convex optimization problem subject to the constraints to generate an optimal trajectory that minimizes deviation from the initial trajectory; and controlling the motion of the device according to the optimal trajectory. the environment includes a plurality of devices, each of the plurality of devices being associated with a corresponding goal state, and the learned function being formulated for each device of the plurality of devices; The processor further comprises: for each device of the plurality of devices, executing the learned function to determine an initial trajectory for each device; and solving a convex optimization problem for each device of the plurality of devices to determine an optimal trajectory for each device, wherein the optimal trajectory for each device minimizes deviation of the optimal trajectory of a corresponding device from a corresponding initial trajectory, and the processor is further configured to: a controller configured to control the motion of each device based on the corresponding optimal trajectory to achieve the corresponding goal state of each device.

3. 3. The controller of claim 1 or 2, wherein the learned function is a neural network trained using reinforcement learning based on a reward function that penalizes the degree of violation of the constraints.

4. The controller according to claim 1 or 2, wherein the constraints include at least a constraint for reaching the target state of the device, a constraint for avoiding collision between the device and an obstacle, and a constraint for avoiding collision between devices.

5. The constraints include at least static constraints and dynamic constraints; the static constraint is a constraint on an obstacle whose position is fixed; the dynamic constraint is a constraint on an obstacle whose position changes over time; The controller of claim 1 or 2, wherein the learned function is trained to penalize violations of only the static constraints, and the convex optimization problem is solved under the static and dynamic constraints.

6. The controller of claim 1 or 2, wherein the constraints are implemented as hard constraints based on a convex approximation of each constraint.

7. The controller of claim 1 or 2, wherein the constraints are implemented as hard constraints when the dynamics of the device include stochastic uncertainties.

8. The controller of claim 7 , wherein the constraints are implemented as hard constraints based on a convex chance approximation of each constraint.

9. The controller of claim 1 or 2, wherein the device corresponds to at least one of an autonomous vehicle, a mobile robot, an aircraft, a surface vehicle, and an underwater vehicle.

10. 1. A method for controlling motion of a device from an initial state to a goal state in an environment having obstacles that form constraints on the motion of the device, the method comprising: generating an initial trajectory by executing a learned function trained by machine learning to generate feasible or infeasible trajectories that connect the initial state of the device to the goal state of the device while penalizing the degree of violation of at least some of the constraints; solving a convex optimization problem subject to the constraints to generate an optimal trajectory that minimizes deviation from the initial trajectory; controlling the motion of the device according to the optimal trajectory; the environment includes a plurality of devices, each of the plurality of devices being associated with a corresponding goal state, and the learned function being formulated for each device of the plurality of devices; The method further comprises: for each device of the plurality of devices, executing the learned function to determine an initial trajectory for each device; and solving a joint optimization problem to simultaneously determine an optimal trajectory for each device, wherein the optimal trajectory for each device minimizes deviation of the optimal trajectory of a corresponding device from the corresponding initial trajectory, the method further comprising: controlling the motion of each device based on the corresponding optimal trajectory to achieve the corresponding goal state of each device.

11. 1. A method for controlling motion of a device from an initial state to a goal state in an environment having obstacles that form constraints on the motion of the device, the method comprising: generating an initial trajectory by executing a learned function trained by machine learning to generate feasible or infeasible trajectories that connect the initial state of the device to the goal state of the device while penalizing the degree of violation of at least some of the constraints; solving a convex optimization problem subject to the constraints to generate an optimal trajectory that minimizes deviation from the initial trajectory; controlling the motion of the device according to the optimal trajectory; the environment includes a plurality of devices, each of the plurality of devices being associated with a corresponding goal state, and the learned function being formulated for each device of the plurality of devices; The method further comprises: executing the learned function for each device to determine an initial trajectory for each device; and solving a convex optimization problem for each device to determine an optimal trajectory for each device, wherein the optimal trajectory for each device minimizes deviation of the optimal trajectory from the corresponding initial trajectory, the method further comprising: controlling the motion of each device based on the corresponding optimal trajectory to achieve the corresponding goal state of each device.

12. The method according to claim 10 or 11, wherein the constraints include at least a constraint for reaching the goal state of the device, a constraint for avoiding collisions between the device and an obstacle, and a constraint for avoiding collisions between devices.

13. The constraints include at least static constraints and dynamic constraints; the static constraint is a constraint on an obstacle whose position is fixed; the dynamic constraint is a constraint on an obstacle whose position changes over time; The method of claim 10 or 11, wherein the learned function is trained to penalize violations of only the static constraints, and the convex optimization problem is solved under the static and dynamic constraints.

14. The method of claim 10 or 11, wherein the device corresponds to at least one of an autonomous vehicle, a mobile robot, an aircraft, a surface vehicle, and an underwater vehicle.

Images