SYSTEM AND METHOD FOR CONTROLLING THE MOTION OF ONE OR MORE DEVICES ALONG A CALCULATED OPTIMAL 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 of devices to reach a specific location in a workspace with obstacles while avoiding collisions with obstacles and with each other has multiple applications, such as wheeled ground robots, ground-based aircraft, and flying drones. The tasks in such motion planning problems include constraints on the motion trajectories of the devices. The constraints can encode requirements for each of the device's target positions, and requirements for avoiding collisions with each other and with other stationary / moving obstacles in the workspace. As an example, an airborne drone can be tasked with reaching a specific location to perform an inspection of a structure while avoiding collisions with other structures and with each other drone. An aircraft can be tasked with navigating the ground of an airport 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 a load needs to be dropped off 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 possible workspace and the set of device configurations where all constraints related to the task are satisfied are typically non-convex. Thus, the motion planning problem is a non-convex optimization problem. As used herein, a motion planning problem means determining the 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) can be applied when the trajectory of the device's motion can be described by linear dynamics and the obstacles can be represented, possibly in a conservative manner, for example, by polygons (i.e., by linear inequalities). MILP / MIQP are known to terminate 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 may incur significant computational costs. Therefore, a solution is needed that can achieve optimal planning of the trajectory of the device's motion 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. Also, an objective of some embodiments is to determine a motion trajectory of a device to perform a task, for example, to reach a goal state. The state of the device may include one or a combination of the device's position, device's orientation, and device's 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, the execution of the task is constrained. Examples of such constraints include reaching a target position, avoiding collisions with obstacles, avoiding collisions with other devices, moving the device such that the state (e.g., position) of the device is included in a predefined set of acceptable states for the device, and the like. As an example, a drone may be tasked with reaching a certain position 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, the motion planning problem is a non-convex optimization problem. As used herein, a motion planning problem means 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, the motion planning problem is a non-convex optimization problem. As used herein, a motion planning problem means 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 realization 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 is related to computational statistics, which are probabilistic in nature. 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. This concept directly replicates the concept of soft constraints, which is similar to the concept of soft constraints, where violation of the constraint is penalized but not prohibited.

[0010] Therefore, 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. Although 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. Since motion planning and control require satisfying constraints, some embodiments are based on the recognition that it is possible to use the learned functions to determine an initial solution to the non-convex optimization problem, i.e., an initial trajectory of the device subject to soft constraints. However, such an initial trajectory needs to be further processed to ensure that the final trajectory satisfies the constraints.

[0011] Some embodiments are based on the recognition that, given an initial trajectory that satisfies a constraint, it is also possible to optimize the initial trajectory to guarantee the constraint. 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 this initial trajectory is feasible. However, an initial trajectory generated by a learned function trained under soft constraints does not guarantee feasibility and generally cannot guarantee such a transformation.

[0012] However, some embodiments are based on the realization that the transformation of a non-convex optimization problem into a convex optimization problem relies on the possibility of convexification around the initial trajectory, which provides a union of a time-varying set of convex regions that contain the goal state. Each convex region is generated by considering a part of the initial trajectory and a side of the obstacle. Thus, the initial trajectory does not have to be feasible. In contrast, 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 rewards.

[0014] Further, 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 there may be probabilistic uncertainties in device and obstacle positions due to detection limitations. In such cases where the device dynamics includes 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 forming 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 a feasible or infeasible trajectory that connects 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 convex optimization problem subject to the constraints to generate an optimal trajectory that minimizes deviations 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 forming 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 connecting 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 convex optimization problem subject to the 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.

[0017] Accordingly, another embodiment discloses a non-transitory computer-readable storage medium including a program executable by a processor to execute a method for controlling motion of a device from an initial state to a goal state in an environment having obstacles forming 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 connecting 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 convex optimization problem subject to the 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.

[0018] The presently disclosed embodiments will be further described with reference to the accompanying drawings, in which: The drawings shown are not necessarily to scale, with emphasis instead generally being placed upon illustrating the principles of the presently disclosed embodiments. [Brief description 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. 1 illustrates an example of an initial trajectory, according to an embodiment of the present disclosure. [Figure 1D] FIG. 1 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. 1 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 within an environment, according to an embodiment of the present disclosure. [Figure 2A] 1 illustrates an environment including multiple devices, each having its own goal state, according to an embodiment of the present disclosure. [Figure 2B] FIG. 1 illustrates a schematic diagram of motion planning for multiple devices according to an embodiment of the present disclosure. [Figure 2C] 13 shows a schematic diagram of motion planning for multiple devices according to an alternative embodiment of the present disclosure. [Diagram 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. 1 illustrates a convex approximation of a constraint requiring a device to stay within a predefined environmental polyhedron, according to some embodiments of the present disclosure. [Figure 5B] FIG. 1 illustrates a convex approximation of obstacle avoidance constraints according to some embodiments of the present disclosure. [Figure 5C] FIG. 1 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. 2 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 PREFERRED EMBODIMENTS

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

[0021] As used in the 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 illustration and should not be considered limiting. Any headings used herein are for convenience only and should not be considered to have any 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. Also, 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 the state (e.g., position) of the device 101 is included in a predefined set of allowable states of the device 101 (i.e., keep-in constraints), etc. As an example, a drone can be tasked with reaching a certain position 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, the motion planning problem means determining the 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 realization 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 is related to computational statistics that are probabilistic in nature. 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 a direct replica of, the notion of a soft constraint, where violation of the constraint is penalized but not prohibited.

[0029] Therefore, 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. Although 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. Since motion planning and control require satisfying constraints, some embodiments are based on the recognition that it is possible to use the learned functions to determine an initial solution to the non-convex optimization problem, i.e., an initial trajectory of the device 101 subject to soft constraints. However, such an initial trajectory needs to be further processed to ensure that the final trajectory satisfies the constraints.

[0030] Some embodiments are based on the recognition that, given an initial trajectory that satisfies a constraint, it is also possible to optimize the initial trajectory to guarantee the 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 this initial trajectory is feasible. However, an initial trajectory generated by a learned function trained under soft constraints does 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 convexification around the initial trajectory, which provides a union of a time-varying set of convex regions that contain the target state 105. Thus, the initial trajectory does not have to be feasible. In contrast, 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 the 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 the constraint violation). 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 with obstacle 107. Furthermore, given the 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 the memory 115 (shown in FIG. 1B) of the controller 111. Furthermore, the 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 the initial trajectory 117. The processor 113 controls the device 101 according to the optimal trajectory 121. For example, based on the optimal trajectory 121, the controller 111 determines control inputs to actuators of the device 101 to track the optimal trajectory 121. Because the optimal trajectory is within the non-convex collision-free region 119, the device 101 does not overlap with the obstacles 107 and 109 when controlled according to the optimal trajectory 121. For example, even if the device 101 approaches corners 123, 125 when executing the optimal trajectory 121, the device 101 does not overlap with the 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 of which is generated by considering a portion of the initial trajectory 117 and the sides of the obstacles 107, 109.

[0036] FIG. 1E illustrates an approach to generate 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 the support half-spaces 127 and 129. The intersection of the half-spaces 127 and 129 forms a convex region 131 that includes the device 101 at the initial state 103. Based on the initial trajectory 117, the processor 113 determines a trajectory that has minimal deviation from the initial trajectory 117 to keep 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 of motion planning (based on learned functions and optimization-based safety filters) can be extended for motion planning of multiple devices, where each device has its own goal state.

[0039] 2A illustrates an environment 200 including multiple devices, each having 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 an optimal trajectory for each device that connects each device to its goal state while avoiding collisions between the devices and with the 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, the convex optimization problem is not solved for each device separately, but rather 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 an 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 less computational resources, and exhibits better performance compared to training a neural network (or model) that provides initial trajectories for all devices simultaneously. Motion planning of multiple devices is described below in FIG. 2B.

[0041] 2B illustrates 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. The optimization-based safety filter 211 solves a joint optimization problem to simultaneously determine optimal trajectories 231a-213d. The optimal trajectories 231a-213d are associated with devices 201a-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. Further, the learned functions 207a-207d 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] FIG. 2C shows a schematic diagram of motion planning for multiple devices according to an alternative embodiment of the present disclosure. Each of the devices 201a-201d includes a controller (similar to the controller 111). As an example, the device 201a includes a controller 219 including a learned function 219a and an optimization-based safety filter 219b. The learned function 219a is configured to generate an initial trajectory for the device 201a. The 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 of the device 201a. Furthermore, the controller 219 controls the motion of the device 201a based on the optimal trajectory generated for the device 201a.

[0045] Similarly, device 201b includes a controller 221 including a learned function 221a and an optimization-based safety filter 221b, device 201c includes a controller 223 including a learned function 223a and an optimization-based safety filter 223b, and device 201d includes a controller 225 including a learned function 225a and an optimization-based safety filter 225b. The learned function of each device generates an initial trajectory for that device, and then the optimization-based safety filter of each device 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 converts observations 303 (o(k) above) into actions 305 (a(k) above). A reward function 307 is a function of the observations 303 and actions 305 defined in (3). A world 309 is composed 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 converts the state into the appropriate observation vector. The clock 315 increments the 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 neural architectures or kernel representations. 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 such 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 the 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. The reason is that the dynamic constraints are less stringent due to the added flexibility in the time domain. Therefore, the constraints managed by the convex optimization problem may be different from the constraints used during training of the learned function. Thus, the learned function is trained to penalize only the violation of the static constraints, and a convex optimization problem is solved under the static and dynamic constraints.

[0054] Further, 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 there may be probabilistic uncertainties in device and obstacle positions due to detection limitations. In such cases where the device dynamics includes 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, as described 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 the device 101 must stay within a predefined environment polyhedron 501, according to some embodiments of the present disclosure. The constraint is naturally convex for non-stochastic dynamics of the device 101 (1a). On the other hand, for stochastic dynamics of the device 101 (1c), the device 101 is required to stay within a smaller set 503. Reinforcing the environment polyhedron 501 with a reinforcement region 505 results in the formation of the smaller set 503, which arises due to the stochasticity of the dynamics. Additionally, in some embodiments, similar convex approximations may be constructed for additional constraints on the state of the device 101, including its velocity.

[0058] FIG. 5B is a schematic diagram of a N O The region 507 has a center 509 (c j 5D illustrates the shape of an obstacle j with a linear half-space 511 that is contained in the other half-space of the linear half-space 513 that contains region 507. The half-space 511 is composed of an obstacle boundary 515 and a 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 are based on the recognition that in typical model prediction cases recursive feasibility constraints are necessary to practically realize the controller 111. In particular, the mean position of the device is required to stay within a well-defined control invariant set so that the device can be safely controlled despite probabilistic uncertainties. An example of a control invariant set is described below in FIG. 6.

[0063] 6 illustrates an example of a two-dimensional projection of a control invariant set 603 corresponding to a constraint set 601, according to an embodiment of the present disclosure. In an 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 an 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 exists a control command that maintains the state of the device 101 in the control invariant set 603 relative to a known or acceptable future state.

[0064] For example, for any state of the device 101 within all possible control commands 617-623 that the controller 111 can execute, such as state 615 in the control invariant set 603, there exists at least one control command 623 that keeps the state of the device 101 within the control invariant set 603. On the other hand, state 605 may be feasible for one iteration, but all control commands 607-613 that the controller 111 is allowed to obtain during the next iteration may put the state of the device 101 outside the 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 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. In 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 a state of the device 101. In some embodiments, the parameters may include constraints characterizing the task, such as one or a combination of the initial state of the device 101, the goal state of the device 101, a geometric configuration of one or more stationary obstacles that define at least a portion of the constraints, and a motion of a moving obstacle that defines at least a portion of the constraints. Additionally, the learned function is provided with the parameters.

[0066] At block 703, the method 700 includes executing a learned function trained by machine learning to generate an initial trajectory that connects 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 the 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 solves a convex optimization problem subject to the constraints as hard constraints to generate the optimal trajectory. 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 can be connected to the bus 809. In addition, a low-speed expansion port 817 is connected to the bus 809. Furthermore, an input interface 821 can be connected to an external receiver 823 and an output interface 825 via the bus 809. The receiver 827 can be connected to an external transmitter 829 and a transmitter 831 via the bus 809. Also connected to the bus 809 can be an external memory 833, an external sensor 835, a machine 837, and an environment 839. Furthermore, one or more external input / output devices 841 can be connected to the bus 809. A network interface controller (NIC) 843 may be adapted to connect to a network 845 via bus 809 to, 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 the 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. The instructions, when executed by one or more processing devices (e.g., the processor 803), perform one or more methods, such as those described above.

[0070] The computing device 800 can optionally be linked through a bus 809 to a display or user interface (HMI) 847 adapted to connect the computing device 800 to a display device 849 and a keyboard 851, which may include, among others, a computer monitor, a camera, a television, a projector, or a mobile device. In some implementations, the computing device 800 may include a printer interface for connecting to a printing device, which may include, among others, 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 one 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, the low speed interface 813 is coupled to storage 807 and a low speed expansion port 817 via bus 809. The 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. The computing device 800 may be coupled to a server 853 and a rack server 855. The computing device 800 may be realized in a number of different forms. For example, the computing device 800 may be realized as part of a 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. It is contemplated that various changes may be made to the function and configuration of elements without departing from the spirit and scope of the disclosed subject matter as set forth in the appended claims. Specific details are given in the following description to provide a thorough understanding of the embodiments. However, those skilled in the art will understand that the embodiments can 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 examples, well-known processes, structures, and techniques may be shown without unnecessary detail so as not to obscure the embodiments. Additionally, like reference numbers and names in the various drawings refer to like elements.

[0074] Also, the individual embodiments may be described as a process that is depicted as a flowchart, a flow diagram, a data flow diagram, a structure diagram, or a block diagram. Although a flowchart may describe operations as a sequential process, many of the operations may be performed in parallel or simultaneously. In addition, 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 process specifically described may occur in all embodiments. A process may correspond to a method, a function, a procedure, a subroutine, a subprogram, or the like. When a process corresponds to a function, the end of the function may correspond to a return of the function to the calling function or to the main function.

[0075] Additionally, 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 through 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, the embodiments of the present disclosure and the functional operations described herein can be realized in digital electronic circuitry, in tangibly implemented computer software or firmware, in computer hardware including the structures disclosed herein and their structural equivalents, or in a combination of one or more of them. Furthermore, some embodiments of the present disclosure can be realized 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 for controlling the operation of a data processing apparatus. Still further, the program instructions can be encoded on a human-generated propagated signal, for example, on a machine-generated electrical, optical, or electromagnetic signal. 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 memory device or a serial access memory device, or one or more combinations thereof.

[0077] Computer programs (which may also be called or described as programs, software, software applications, modules, software modules, scripts, or code) can be written in any form of programming language, including compiled or interpreted languages, or declarative or procedural languages, and can be deployed in any form, whether as stand-alone programs or as modules, components, subroutines, or other units 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 in part of 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 subject program, or a number of 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 executing a computer program may be based on, by way of example, a general-purpose or dedicated microprocessor or both, or any other type of central processing unit. Typically, the central processing unit receives instructions and data from a read-only memory or a random access memory or both. The essential elements of a computer are a central processing unit for implementing 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 said disks to receive data therefrom or transfer data thereto, or both. However, a computer need not have such devices. Furthermore, a computer may be embedded in another device. For example, a computer may be embedded in 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 be used to provide 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, e.g., 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., as 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 of 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 from computer programs running on the respective computers and having a mutual relationship of client and server.

[0083] Although the present disclosure has been described with respect to certain preferred embodiments, it should 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 come 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 that, when executed by the processor, cause 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 in accordance with the optimal trajectory.

2. The controller of claim 1 , 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.

3. The controller of claim 1 , wherein the constraints include at least a constraint for reaching the goal state of the device, a collision avoidance constraint between the device and an obstacle, a collision avoidance constraint between devices, and a keep-in constraint.

4. 2. The controller of claim 1, wherein the environment includes a plurality of devices, each of the plurality of devices being associated with a corresponding target state, and the learned function is formulated for each device of the plurality of devices.

5. The processor further comprises: executing the learned function for each device of the plurality of devices to determine an initial trajectory for each device; and solving a joint optimization problem to simultaneously determine an optimal trajectory for each device, the optimal trajectory for each device minimizing a deviation of the optimal trajectory of a corresponding device from a corresponding initial trajectory, the processor further configured to: The controller of claim 4 , configured to control the motion of each device based on the corresponding optimal trajectory to achieve the corresponding goal state of each device.

6. The processor further comprises: executing the learned function for each device of the plurality of devices 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, the optimal trajectory for each device minimizing a deviation of the optimal trajectory of a corresponding device from a corresponding initial trajectory, the processor further configured to: The controller of claim 4 , configured to control the motion of each device based on the corresponding optimal trajectory to achieve the corresponding goal state of each device.

7. 2. The controller of claim 1, wherein the constraints include at least static constraints where a position is fixed and dynamic constraints where a position changes over time, 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.

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

9. The controller of claim 1 , wherein the constraint is implemented as a hard constraint when the dynamics of the device include stochastic uncertainty.

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

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

12. 1. A method for controlling a motion of a device from an initial state to a goal state in an environment having obstacles forming 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; and controlling movement of the device according to the optimal trajectory.

13. The method of claim 12 , wherein the constraints include at least a constraint of reaching the goal state of the device, a collision avoidance constraint between the device and an obstacle, a collision avoidance constraint between devices, and a keep-in constraint.

14. 13. The method of claim 12, wherein the environment includes a plurality of devices, each of the plurality of devices being associated with a corresponding goal state, and the learned function is formulated for each device of the plurality of devices.

15. The method further comprises: executing the learned function for each device of the plurality of devices to determine an initial trajectory for each device; and solving a joint optimization problem to simultaneously determine an optimal trajectory for each device, the optimal trajectory for each device minimizing deviation of the optimal trajectory of a corresponding device from a corresponding initial trajectory, the method further comprising: The method of claim 14 , comprising controlling the motion of each device based on the corresponding optimal trajectory to achieve the corresponding goal state of each device.

16. 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, the optimal trajectory for each device minimizing deviation of the optimal trajectory from the corresponding initial trajectory, the method further comprising: The method of claim 14 , comprising controlling the motion of each device based on the corresponding optimal trajectory to achieve the corresponding goal state of each device.

17. 13. The method of claim 12, wherein the constraints include at least static constraints where a position is fixed and dynamic constraints where a position changes over time, 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.

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

19. 1. A non-transitory computer-readable storage medium comprising a program executable by a processor for performing a method for controlling a motion of a device from an initial state to a goal state in an environment having obstacles forming 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; and controlling movement of the device according to the optimal trajectory.

Images