System and method for controlling motion of one or more devices along a computated optimal track

Abstract

The present disclosure provides a system and method for controlling movement of a device from an initial state to a target state in an environment having an obstacle that forms a constraint on the movement of the device. The method includes executing a learning function trained with machine learning to generate a trajectory connecting an initial state of the device with a target state of the device, while penalty violating at least some constraints to a certain extent to produce an initial trajectory. The method further includes solving a convex optimization problem that meets all the constraints to produce an optimal trajectory that minimizes deviation from the initial trajectory, and controlling motion of the device according to the optimal trajectory.

Description

Technical Field

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

[0002] In a workspace with obstacles, the optimal motion planning for a device to achieve a specific position while avoiding collisions with obstacles and collisions between themselves has a variety of applications, such as wheeled ground robots, aircraft on the ground, and flying drones. The task in this motion planning problem includes constraints on the motion trajectory of the device. Constraints can encode the requirements for the target position of each device, avoiding collisions with each other and with other static / mobile obstacles in the workspace. For example, an aerial drone can be responsible for reaching certain locations to inspect buildings while avoiding collisions with other buildings and collisions between themselves. An aircraft can be responsible for moving on the ground of an airport to reach its assigned exit (runaway) or door while avoiding other aircraft, carts, and buildings. A mobile robot on the factory floor can be responsible for reaching its work location or the location where goods need to be unloaded while avoiding work areas, workers, and other robots. Summary of the invention

[0003] Technical issues

[0004] Such motion planning problems are challenging because the set of equipment configurations that allow the workspace and satisfy all constraints associated with the task are usually non-convex. Therefore, the motion planning problem is a non-convex optimization problem. As used herein, the motion planning problem refers to determining the motion trajectory of the equipment used to perform the task. However, solving non-convex optimization problems requires a lot of computing and storage resources.

[0005] Motion planners are used to solve non-convex optimization problems. An example of a motion planner based on non-convex optimization is to use a mixed-integer programming (MIP) method. The MIP method involves solving a constrained optimization problem in which a cost function is minimized or maximized subject to some constraints, some optimization variables are real-valued (continuous variables), and others are integer or binary-valued (discrete variables). Specifically, when the motion trajectory of the device can be described by linear dynamics and obstacles can be represented in a conservative manner (such as by polygons (i.e., by linear inequalities)), mixed-integer linear / quadratic programming (MILP / MIQP) can be applied. It is known that MILP / MIQP terminates in a finite time, so the time bound for the computational solution can be derived. However, the computation is still complex (NP-hard problem) and may result in significant computational costs. Therefore, a solution is needed that can achieve optimal planning of the motion trajectory of a device in a workspace with obstacles while using limited computing resources and storage resources.

[0006] Solution to the problem

[0007] The purpose of some embodiments is to provide a system and method for controlling the motion of a device from an initial state to a target state in an environment. The purpose of some embodiments is also to determine the motion trajectory of a device that performs a task (e.g., reaches a target state). The state of the device may include one or a combination of the position of the device, the orientation of the device, and the speed of the device. 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. The execution of the task needs to satisfy the constraints on the motion of the device. In other words, the execution of the task is subject to constraints. Examples of such constraints include reaching a target location, avoiding collisions with obstacles, avoiding collisions with other devices, moving the device so that the state of the device (e.g., position) is within a predefined allowed state set of the device, etc. For example, a drone may be responsible for reaching certain locations to inspect a building while avoiding collisions with parts of the building.

[0008] Some embodiments are based on the recognition that the allowed states and the set of device configurations / states that satisfy all constraints are generally non-convex. Therefore, the motion planning problem is a non-convex optimization problem. As used herein, the motion planning problem refers to determining the motion trajectory of a device for performing a task. However, solving non-convex optimization problems requires a lot of computing and storage resources.

[0009] Some embodiments are based on the recognition that the allowed states and the set of device configurations / states that satisfy all constraints are generally non-convex. Therefore, the motion planning problem is a non-convex optimization problem. As used herein, the motion planning problem refers to determining the motion trajectory of a device for performing a task. However, solving non-convex optimization problems requires a lot of computing and storage resources.

[0010] Some embodiments aim to use machine learning to reduce the computational burden of non-convex optimization problems subject to constraints. Some embodiments are based on the understanding that various learning 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.

[0011] However, due to the nature of machine learning, replacing non-convex optimization problems subject to constraints with such learning functions is problematic. Specifically, machine learning is associated with computational statistics that are probabilistic in nature. As a result, learning functions such as neural networks are not equipped to manage hard constraints, but are designed to satisfy constraints with some probability. This concept is an analog or direct copy of the concept of soft constraints, when violation of constraints is penalized but not prohibited.

[0012] 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. Such a solution can be found with high probability but under the condition of sufficient offline training data to closely approximate the online environment, it is impractical. Since motion planning and control require satisfying constraints, some embodiments are based on the recognition that it is possible to use a learning function to determine an initial solution (i.e., an initial trajectory of the device) to a non-convex optimization problem subject to soft constraints. However, such an initial trajectory needs to be further processed to ensure that the final trajectory satisfies the constraints.

[0013] Some embodiments are based on the recognition that given an initial trajectory that satisfies constraints, the initial trajectory can be optimized to also ensure the constraints. In addition, this optimization will be convex. In other words, when the initial trajectory is feasible, having the initial trajectory can transform a non-convex optimization problem subject to hard constraints into a convex optimization problem subject to hard constraints. However, the initial trajectory generated by the learning function trained with soft constraints does not guarantee feasibility, and in general cannot guarantee such a transformation.

[0014] However, some embodiments are based on the recognition that the transformation from a non-convex optimization problem to a convex optimization problem is due to the possibility of convexification around the initial trajectory providing a union of a time-varying set of convex regions that include the target state. Each convex region is generated by considering a portion of the initial trajectory and a small facet of the obstacle. Therefore, the initial trajectory does not have to be feasible. On the contrary, the initial trajectory may be infeasible as long as the convex region includes the initial state and the target state.

[0015] Some embodiments generate initial trajectories suitable for such transformations based on intuition supported by many tests and experiments, where a learning function trained to minimize the rate of violation of constraints generates an initial trajectory suitable for such transformations. An example of such training is reinforcement learning (RL), which is a field of machine learning concerned with how intelligent devices should take actions in an environment in order to maximize cumulative rewards.

[0016] In addition, some embodiments are based on the recognition that random uncertainties may exist in the dynamics of the device due to nonlinearities, actuation mismatches, and unmodeled phenomena, and random uncertainties may exist in the positions of the device and obstacles due to sensing limitations. In this case, where the device dynamics include random uncertainties, the constraints are replaced by probabilistic safety constraints (also called chance constraints). The optimization-based safety filter is configured to implement the probabilistic safety constraints as hard constraints to ensure safety with high probability. In an embodiment, the optimization-based safety filter implements the probabilistic safety constraints based on a convex chance approximation of the probabilistic safety constraints.

[0017] Therefore, one embodiment discloses a controller for controlling the movement of a device from an initial state to a target state in an environment with obstacles that form constraints on the movement of the device. The controller includes: a processor; and a memory having instructions stored thereon, which, when executed by the processor, causes the controller to: execute a learning function trained using machine learning to generate a feasible trajectory or an infeasible trajectory connecting the initial state of the device with the target state of the device, while penalizing violations of at least some of the constraints to a certain extent to produce an initial trajectory; solve a convex optimization problem subject to the constraints to produce an optimal trajectory that minimizes deviations from the initial trajectory; and control the movement of the device according to the optimal trajectory.

[0018] Therefore, another embodiment discloses a method for controlling the motion of a device from an initial state to a target state in an environment with obstacles, wherein the obstacles form constraints on the motion of the device. The method includes the following steps: executing a learning function trained by machine learning to generate a feasible trajectory or an infeasible trajectory connecting the initial state of the device with the target state of the device, while penalizing violations of at least some constraints to a certain extent to produce the initial trajectory; solving a convex optimization problem subject to constraints to produce an optimal trajectory that minimizes deviations from the initial trajectory; and controlling the motion of the device according to the optimal trajectory.

[0019] Therefore, another embodiment discloses a non-transitory computer-readable storage medium, on which a program executable by a processor is embodied, the program for performing a method for controlling the movement of a device from an initial state to a target state in an environment with obstacles, wherein the obstacles form constraints on the movement of the device. The method includes: executing a learning function trained with machine learning to generate a feasible trajectory or an infeasible trajectory connecting the initial state of the device with the target state of the device, while penalizing violations of at least some constraints to a certain extent to produce the initial trajectory; solving a convex optimization problem subject to constraints to produce an optimal trajectory that minimizes the deviation from the initial trajectory; and controlling the movement of the device according to the optimal trajectory.

[0020] The presently disclosed embodiments will be further explained with reference to the accompanying drawings. The drawings shown are not necessarily drawn to scale, with emphasis instead generally being placed upon illustrating the principles of the presently disclosed embodiments. BRIEF DESCRIPTION OF THE DRAWINGS

[0021] [ Figure 1A ]

[0022] Figure 1A An example environment for controlling a device according to an embodiment of the present disclosure is shown.

[0023] [ Figure 1B ]

[0024] Figure 1B A block diagram of a controller for controlling movement of a device from an initial state to a target state in an environment according to an embodiment of the present disclosure is shown.

[0025] [ Figure 1C ]

[0026] Figure 1C An example initial trajectory is shown in accordance with an embodiment of the present disclosure.

[0027] [ Figure 1D ]

[0028] Figure 1D A non-convex collision-free region determined by performing an optimization-based safety filter according to an embodiment of the present disclosure is shown.

[0029] [ Figure 1E ]

[0030] Figure 1E A method for generating a convex region defining a non-convex collision-free region according to an embodiment of the present disclosure is shown.

[0031] [ Figure 1F ]

[0032] Figure 1FMultiple half-spaces generated by an optimization-based safety filter as a device navigates an environment are shown in accordance with an embodiment of the present disclosure.

[0033] [ Figure 2A ]

[0034] Figure 2A An environment including multiple devices, each having a respective target state, is shown according to an embodiment of the present disclosure.

[0035] [ Figure 2B ]

[0036] Figure 2B A schematic diagram showing motion planning of multiple devices according to an embodiment of the present disclosure is shown.

[0037] [ Figure 2C ]

[0038] Figure 2C A schematic diagram illustrating motion planning for multiple devices according to an alternative embodiment of the present disclosure is shown.

[0039] [ Figure 3 ]

[0040] Figure 3 A schematic diagram of a Markov Decision Process (MDP) used in reinforcement learning according to some embodiments of the present disclosure is shown.

[0041] [ Figure 4 ]

[0042] Figure 4 A schematic diagram of a deep neural network used as a prediction model (ie, learning function) according to some embodiments of the present disclosure is shown.

[0043] [ Figure 5A ]

[0044] Figure 5A A convex approximation of a constraint requiring a device to remain within a predetermined environmental polyhedron is shown in accordance with some embodiments of the present disclosure.

[0045] [ Figure 5B ]

[0046] Figure 5B A convex approximation of an obstacle avoidance constraint according to some embodiments of the present disclosure is shown.

[0047] [ Figure 5C ]

[0048] Figure 5C A convex approximation of a device-device collision avoidance constraint according to some embodiments of the present disclosure is shown.

[0049] [ Figure 5D ]

[0050] Figure 5D The method for performing the state x according to some embodiments of the present disclosure is shown. t is random and non-random when the state of the device x t A block diagram for formulating constraints.

[0051] [ Figure 6 ]

[0052] Figure 6 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 is shown.

[0053] [ Figure 7 ]

[0054] Figure 7 A block diagram of a method for controlling the movement of a device from an initial state to a target state in an environment with obstacles according to an embodiment of the present disclosure is shown.

[0055] [ Figure 8 ]

[0056] Figure 8 is a schematic diagram illustrating a computing device for implementing the methods and systems / controllers of the present disclosure. DETAILED DESCRIPTION

[0057] In the following description, for the purpose of explanation, many specific details are set forth in order to provide a thorough understanding of the present disclosure. However, it is apparent to those skilled in the art that the present disclosure can be practiced without these specific details. In other cases, only in order to avoid obscuring the present disclosure, devices and methods are shown in block diagram form.

[0058] As used in this specification and claims, the terms "for example," "for example," and "such as," and the verbs "include," "have," "comprise," and other verb forms thereof, when used in conjunction with a list of one or more components or other items, are each interpreted as open-ended, meaning that the list is not to be viewed as excluding other additional components or items. The term "based on" means based, at least in part, on. In addition, it should be understood that the wording and terminology employed herein are for descriptive purposes and should not be considered limiting. Any headings used in this specification are for convenience only and have no legal or restrictive effect.

[0059] Figure 1AAn example of an environment 100 for controlling a device 101 according to an embodiment of the present disclosure is shown. Some embodiments aim to control the movement of the device 101 from an initial state 103 to a target state 105 in the environment 100. Some embodiments also aim to determine a movement trajectory of the device 101 to perform a task (e.g., to reach the target 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 speed of the device 101.

[0060] Environment 100 includes obstacles 107 and 109. Device 101 can be an autonomous vehicle, such as a mobile robot, an aircraft, a surface vehicle, or an underwater vehicle. The execution of the task needs to satisfy the constraints on the movement of device 101. In other words, the execution of the task is subject to constraints. Examples of such constraints include reaching a target state 105, avoiding collisions with obstacles 107 and 109 (i.e., device-obstacle collision avoidance constraints), avoiding collisions with other devices (i.e., device-to-device collision avoidance constraints), moving device 101 so that the state (e.g., position) of device 101 is within a predefined set of allowed states of device 101 (i.e., remaining under constraints), etc. For example, a drone may be responsible for reaching certain locations to inspect a building while avoiding collisions with parts of the building.

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

[0062] Figure 1B A block diagram of a controller 111 according to an embodiment of the present disclosure is shown. The controller 111 includes a processor 113 and a memory 115. The processor 113 can 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 a random access memory (RAM), a read-only memory (ROM), a flash memory, or any other suitable memory system. In addition, in some embodiments, the memory 115 may be implemented using a hard drive, an optical drive, a thumb drive, a drive array, or any combination thereof. In an embodiment, the controller 111 is communicatively connected to the device 101. For example, the controller 111 can be remotely connected to the device 101 via a network. In an alternative embodiment, the controller 111 is embedded in the device 101.

[0063] Some embodiments are based on the recognition that a set of device configurations / states and allowed states that satisfy all constraints are typically non-convex. Therefore, 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 101 for performing a task. However, solving a non-convex optimization problem requires a large amount of computing and storage resources.

[0064] Some embodiments aim to use machine learning to reduce the computational burden of non-convex optimization problems subject to constraints. Some embodiments are based on the understanding that various learning 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.

[0065] However, due to the nature of machine learning, replacing non-convex optimization problems subject to constraints with such learning functions is problematic. Specifically, machine learning is associated with computational statistics that are probabilistic in nature. As a result, learning functions such as neural networks are not equipped to manage hard constraints, but are designed to satisfy constraints with some probability. A hard constraint is a constraint that "must" always be satisfied. However, the probabilistic concept of a constraint is a direct copy or analog of the concept of a soft constraint, as violating the constraint is penalized but not prohibited.

[0066] 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 hard constraints. Such a solution can be found with high probability, but it is impractical under conditions where sufficient offline training data closely approximates the online environment. Since motion planning and control require constraints to be satisfied, some embodiments are based on the recognition that a learning function can be used to determine an initial solution to a non-convex optimization problem subject to soft constraints (i.e., an initial trajectory of device 101). However, such an initial trajectory needs to be further processed to ensure that the final trajectory satisfies the constraints.

[0067] Some embodiments are based on the recognition that given an initial trajectory that satisfies constraints, the initial trajectory can be optimized to also ensure that the constraints are satisfied. In addition, this optimization will be convex. In other words, when the initial trajectory is feasible, having the initial trajectory can transform a non-convex optimization problem subject to hard constraints into a convex optimization problem subject to hard constraints. However, the initial trajectory generated by the learning function trained with soft constraints does not guarantee feasibility, and in general cannot guarantee such a transformation.

[0068] However, some embodiments are based on the recognition that the transformation from a non-convex optimization problem to a convex optimization problem is due to the possibility of convexification around the initial trajectory providing a union of a time-varying set of convex regions including the target state 105. Therefore, the initial trajectory does not have to be feasible. Instead, the initial trajectory may be infeasible as long as the convex region includes the initial state 103 and the target state 105.

[0069] Figure 1C An example of an initial trajectory 117 according to an embodiment of the present disclosure is shown. The processor 113 ( Figure 1B 105 ) is configured to execute a learning function to generate an initial trajectory 117 that connects the initial state 103 to the target state 105 while penalizing violations of at least some of the constraints to a certain extent. The learning function can be stored in the memory 115 ( Figure 1B ). According to an embodiment, the learning function is a neural network trained using reinforcement learning based on a reward function. The reward function penalizes violating the constraint to a certain extent. The reward function returns a higher value when the initial trajectory 117 satisfies the constraint than when the constraint is violated (i.e., the reward function penalizes when the constraint is violated). In an embodiment, an algorithm such as proximal policy optimization is used to determine the initial trajectory that maximizes the reward function.

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

[0071] Figure 1D FIG. 1 shows 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 ( Figure 1B In addition, the processor 113 (in Figure 1B 1 (shown in FIG. 1 ) solves a convex optimization problem subject to hard constraints to produce an optimal trajectory 121 that minimizes the deviation from the initial trajectory 117 and is located within the non-convex collision-free region 119. 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 the control inputs to the actuators of the device 101 to track the optimal trajectory 121. Since the optimal trajectory is located within the non-convex collision-free region 119, when controlled according to the optimal trajectory 121, the device 101 does not overlap with the obstacles 107 and 109. For example, even when the device 101 forms tight corners 123 and 125 when executing the optimal trajectory 121, the device 101 does not overlap with the obstacle 107.

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

[0073] Figure 1E A method for generating a convex region defining a non-convex collision-free region 119 according to an embodiment of the present disclosure is shown. 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, the current position of the device 101 (i.e., the initial state 103) is projected onto the obstacles 107 and 109 based on the optimized safety filter, and the supporting half-spaces 127 and 129 are obtained. The intersection of the half-spaces 127 and 129 forms a convex region 131 including the device 101 in the initial state 103. Based on the initial trajectory 117, the processor 113 determines the trajectory that deviates from the initial trajectory 117 the least, and keeps the device 101 in the convex region 131. As the device 101 moves along the trajectory, the supporting half-space is continuously updated based on the current state of the device 101. When the device 101 reaches the position 133, the supporting half-space of the obstacle 107 changes from 129 to 135, and the convex region changes from 131 to 137. Furthermore, when device 101 reaches position 139 , the supporting half-space of obstacle 107 changes from 135 to 141 , and the convex area changes from 137 to 143 .

[0074] Figure 1F All half-spaces 127, 129, 135, 141, 145, 147, and 149 generated by the optimization-based safety filter when the device 101 navigates in the environment 100 according to an embodiment of the present disclosure are shown. The intersection of the half-spaces 127, 129, 135, 141, 145, 147, and 149 forms convex regions 131, 137, and 143, which together define the non-convex collision-free region 119.

[0075] Furthermore, some embodiments are based on the recognition that this framework for motion planning, which is based on learning functions and optimization-based safety filters, can be extended for motion planning of multiple devices, where each device has a corresponding target state.

[0076] Figure 2AAn environment 200 including multiple devices according to an embodiment of the present disclosure is shown, wherein each device has a corresponding target state. Devices 201a, 201b, 201c, and 201d need to reach their assigned target states 203a, 203b, 203c, and 203d. Environment 200 includes obstacles 205a, 205b, 205c, 205d, 205e, and 201f. An objective of some embodiments is to determine an optimal trajectory for each device that connects each device to its corresponding target state while avoiding collisions with each other and with obstacles 205a, 205b, 205c, 205d, 205e, and 201f.

[0077] In some embodiments, the controller 111 is executed on a remote centralized server and is communicatively connected to each device. In such an embodiment, for motion planning of multiple devices, a learning function is formulated for each device to generate an initial trajectory for each device, respectively. However, instead of solving the convex optimization problem separately for each device, the convex optimization problem is converted into a joint optimization problem, which simultaneously determines the optimal trajectory for each device based on the corresponding initial trajectory. To this end, for motion planning of multiple devices, the controller 111 includes a plurality of learning functions equal to the number of devices and an optimization-based safety filter, which simultaneously determines the optimal motion trajectory for each device by solving the joint optimization problem. This motion planning of multiple devices is advantageous because a single device learning function is easier to train, requires less computing resources, and exhibits excellent performance compared to training a neural network (or model) that provides initial trajectories for all devices simultaneously. Below in Figure 2B Motion planning for multiple devices is explained in .

[0078] Figure 2B A schematic diagram of motion planning for multiple devices according to an embodiment of the present disclosure is shown. Learning functions 207a-207d generate initial trajectories 209a-209d, respectively. Initial trajectories 209a-209d are associated with devices 201a-201d, respectively. 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.

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

[0080] In some alternative embodiments, for motion planning of multiple devices, controller 111 is executed on each device. Figure 2C Such an implementation is described in .

[0081] Figure 2C A schematic diagram of motion planning of multiple devices according to an alternative embodiment of the present disclosure is shown. Each of devices 201a-201d includes a controller (similar to controller 111). For example, device 201a includes controller 219, which includes learning function 219a and optimization-based safety filter 219b. Learning function 219a is configured to generate an initial trajectory of device 201a. Optimization-based safety filter 219b is configured to solve a convex optimization problem subject to constraints to generate an optimal trajectory that minimizes deviations from the initial trajectory of device 201a. In addition, controller 219 controls the motion of device 201a based on the optimal trajectory generated for device 201a.

[0082] Likewise, device 201b includes controller 221, which includes learning function 221a and optimization-based safety filter 221b, device 201c includes controller 223, which includes learning function 223a and optimization-based safety filter 223b, and device 201d includes controller 225, which includes learning function 225a and optimization-based safety filter 225b. The learning function of each device generates an initial trajectory of its corresponding device, and then the optimization-based safety filter of each device generates the optimal trajectory of the corresponding device.

[0083] The implementation of a learning function (e.g., learning function 219a) is described below. Consider a device (e.g., device 101) with d-dimensional deterministic dynamics.

[0084] x(k+1)=Ax(k)+Bu(k), p(k)=Cx(k), (1a)

[0085] Specify the target position q.

[0086] Define the following low-level controller:

[0087] u(k)=Kx(k)+Fr(k)

[0088] Where K is the stable gain matrix that stabilizes the closed-loop system, and the matrix F satisfies C(I-(A+BK)) -1 BF = Identity(d), and r(k) is the reference position command. Therefore, the following stable device dynamics are obtained:

[0089] x(k+1)=(A+BK)x(k)+BFr(k). (1b)

[0090] Let c j (1≤j≤N O ) represents all N O The obstacles are assumed to have radius γ. j (1≤j≤N O ) circle.

[0091] In some embodiments, the stochastic dynamics of the device 101 are also considered.

[0092] x(k+1)=Ax(k)+Bu(k)+w(k), p(k)=Cx(k)+η(k), (1c)

[0093] Here, process noise w(k) and measurement noise η(k) are random disturbances that affect the dynamics (1a) of the device 101.

[0094] Consider a deterministic Markov decision process (MDP) for the device 101 with the above dynamics. The MDP has the following:

[0095] 1. Observation space: Define the observation vector o(k) as the concatenated vector containing the state x(k), the device’s current position to the target (p(k)-q) and to N O static obstacles (p(k)-c j ) displacement, where c j For all 1≤j≤N O The nominal average position of the obstacles. Therefore, the position of the device p(k) is a linear transformation of the observation vector o(k).

[0096] 2. Action space: An action a(k) defines the reference position as a bounded perturbation a(k) to the target position q such that

[0097] r(k)=q+a(k). (2)3. Step function: Use closed-loop stable dynamics (1b) to obtain the next state x(k+1).

[0098] 4. Reward and punishment function: The instantaneous reward and punishment function is

[0099]

[0100] While the above characterization of the step function of the MDP considers closed-loop deterministic dynamics (1b), stochastic dynamics (1c) can also be used to set up similar step functions.

[0101] Figure 3 A schematic diagram of an MDP according to some embodiments of the present disclosure is shown. At each time step, a policy 301 converts an observation 303 (denoted above as o(k)) into an action 305 (denoted above as a(k)). A reward and punishment function 307 is a function of the observation 303 and the action 305 as defined in (3). A world 309 includes a system 311, a measurement model 313, and a clock 315. The system 311 uses a step function defined based on closed-loop dynamics (1b), the current internal state x(k), the current action a(k), and the current reference control r(k) to reach the next internal state x(k+1). The measurement model 313 converts the state into an appropriate observation vector. The clock 315 increases the time k to k+1.

[0102] Some embodiments of the present disclosure use a function approximator to characterize the strategy 301. Therefore, a suitable prediction model that provides an initial trajectory (e.g., the initial trajectory 117) can be constructed. The prediction model corresponds to a learning function. The prediction model can be deterministic or stochastic and can rely on a neural architecture or kernel representation. Examples of deterministic prediction models include multilayer perceptrons, convolutional neural networks, kernel regression, and support vector machines, etc. Examples of stochastic prediction models include Bayesian neural networks, neural processes, Gaussian processes, Kriging interpolation, etc.

[0103] Figure 4 A schematic diagram of a deep neural network 400 used as a prediction model (i.e., a learning function) according to some embodiments of the present disclosure is shown. From the data collected for training, an input signal 401 is provided to the deep neural network 400 through an input layer 403. The input layer 403 is a feature extraction layer and can be linear or include convolutional filters to extract useful features from the input signal 401. For a linear input layer, the transformation is linear and the feature

[0104]

[0105] Where W0 and w0 form the weights and biases of the input layer 403, and X is the input to the deep neural network 400. Therefore, the features pass through the hidden layer 405 and they are transformed via activation functions such as rectified linear unit (ReLU), sigmoid function or modifications and combinations thereof. The operations occurring in the hidden layer 405 can be represented using a composition of functions, where the output from the final hidden layer is given by

[0106]

[0107] The output layer 407 is used for regression here and can be linear, so the predicted output 409 is

[0108]

[0109] And training the deep neural network 400 involves obtaining good values ​​for the weights and biases in the input layer 403 , the output layer 407 , and each hidden layer 405 .

[0110] Several reinforcement learning algorithms are used to train a prediction model (or learning function) of the initial trajectory, where another prediction model of the policy is obtained so that the cumulative sum of the reward and punishment function is maximized. Reinforcement learning algorithms can 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.

[0111] Some embodiments are based on the recognition that constraints may include static constraints (such as the walls of a building) and dynamic constraints (such as a moving obstacle that changes its position over time). Some embodiments are based on the recognition that if static constraints are considered during the training of the learning function, the presence of unknown dynamic constraints can still be managed by a convex optimization problem. This is because dynamic constraints are less stringent as the flexibility of the time domain increases. For this reason, the constraints managed by the convex optimization problem may be different from the constraints used during the training of the learning function. Therefore, the learning function is trained to penalize violations of only static constraints, and the convex optimization problem is solved subject to both static and dynamic constraints.

[0112] Additionally, some embodiments are based on the recognition that random uncertainties may exist in the dynamics of the device due to nonlinearities, actuation mismatches, and unmodeled phenomena, and that random uncertainties may exist in the positions of the device and obstacles due to sensing limitations. In such cases (where the device dynamics include random uncertainties), the constraints are replaced by probabilistic safety constraints (also called chance constraints). The optimization-based safety filter is configured to implement the probabilistic safety constraints as hard constraints to ensure safety with high probability. In an embodiment, the optimization-based safety filter implements the probabilistic safety constraints based on a convex chance approximation of the probabilistic safety constraints, such as Figure 5A , Figure 5B and Figure 5C Described in .

[0113] According to an embodiment, the convex optimization problem can be formulated as a convex quadratic programming, where for the planning horizon T and the number of devices N A Seeking safe control actions Among them, 1≤i≤N A , k≤t≤k+T. Given an initial trajectory represented by a learning function (given by Represents, where 1≤i≤N A , k≤t≤k+T), convex quadratic programming minimizes the quadratic objective

[0114]

[0115] The quadratic objective is subject to the constraints caused by the dynamics (1a), the control safe (t|k), a constraint on the resulting motion trajectory executing the task within the time horizon T or coming closer to executing the task with a high user-specified likelihood, and a recursive feasibility constraint that ensures the convex quadratic program is also feasible at future time steps.

[0116] Some embodiments of the present disclosure consider the task of ensuring that an entire system with multiple devices is probabilistically jointly safe, and a set of constraints that ensure safe motion planning for multiple devices. Under user-specified probabilities, it is expected that all devices (each with dynamics (1a)) i) remain within predetermined environmental boundaries, ii) do not interact with a device located at c j (1≤j≤N O )(denotes with radius γ j (where 1≤j≤N O ) all N O N at the nominal average position of obstacles O static obstacles, and iii) within a given device radius r A , without colliding with each other.

[0117] Figure 5A A convex approximation of a first constraint according to some embodiments of the present disclosure is shown - the device 101 must remain within a predetermined environmental polyhedron 501. For the non-stochastic dynamics of the device 101 (1a), the constraint is naturally convex. On the other hand, for the stochastic dynamics of the device 101 (1c), the device 101 is required to remain within a smaller set 503. Tightening the environmental polyhedron 501 by tightening the region 505 results in the formation of a smaller set 503, which arises due to the randomness of the dynamics. In addition, in some embodiments, similar convex approximations can be constructed for additional constraints on the state of the device 101 (including its velocity).

[0118] Figure 5B shows a second constraint (not related to N) according to some embodiments of the present disclosure O The region 507 represents a convex approximation with center 509 (denoted by c j The geometry of obstacle j is represented by ( ). Region 507 is avoided by device 101 if the device position is in linear half-space 511, which is contained in the complement of linear half-space 513 that includes region 507. Half-space 511 is constructed by obstacle boundary 515 and normal vector 517, and is tightened by tightening region 519 to account for random uncertainty. Figure 5D The required tightening is explained in .

[0119] Figure 5C A convex approximation of the third constraint (no collision with other devices) according to some embodiments of the present disclosure is shown. For example, it is required that devices 521 and 523 do not collide. A coordinate system defined by axes 525 and 527 is attached to device 521. The dynamics of device 523 in the coordinate system is given by:

[0120] x 523-521 (k) = x 523 (k)-x 521 (k)

[0121] u 523-521 (k) = u 523 (k)-u 521 (k)

[0122] w 523-521 (k) = w 523 (k)-w 521 (k)

[0123] η 523-521 (k) = η 523 (k)-η 521 (k)

[0124] x 523-521 (k+1)=Ax523-521 (k)+Bu 523-521 (k)+w 523-521 (k) (5a)

[0125] p 523-521 (k) = Cx 523-521 (k)+η 523-521 (k), (5b)

[0126] where w 523-521 (k) and η 523-521 (k) are the process and measurement noise in the coordinate system defined by axes 525 and 527. For Gaussian w and η, these noises are Gaussian. To avoid device-device collisions, device 523 should be kept below boundary 529. Boundary 529 is obtained after growing region 531 with tightening region 533. Region 531 contains device geometry 535, where the Minkowski sum of device geometry 535 is considered together with its negative image. The resulting motion 537 stays below boundary 529 and avoids device-device collisions.

[0127] Figure 5D The method for performing the operation in state x according to some embodiments of the present disclosure is shown. t Formulate the state x for random and non-random t At block 541, check the state x t Is it random? If the state x t is non-random, then the 543 constraint is not required to be tightened. On the other hand, if the state x t is random, then constraint 539 becomes a chance constraint 545, making constraint 539 maintain a probability no less than δ. Therefore, deterministic reformulation 547 is used to tighten constraint 539. Here, when applied to the average state When β is a tightening of the constraint 539. t ,

[0128] To this end, some embodiments of the present disclosure are based on the recognition that when the dynamics are non-stochastic, the constraints do not need to be tightened, and they simplify to their original convex approximation in such an event.

[0129] Some embodiments are based on the recognition that in the model prediction paradigm, the practical implementation of the controller 111 requires recursive feasibility constraints. Specifically, it is required that the average position of the device remains within a properly defined set of control invariants so that the device can be safely controlled despite random uncertainties. Figure 6 An example of controlling an invariant collection is described in .

[0130] Figure 6 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 is shown. In an embodiment, the constraint set 601 may be a multi-dimensional polyhedron determined by a hyperplane along multiple dimensions corresponding to the constraints on the optimal trajectory of the device 101, the hyperplane being represented by a linear inequality. The constraint set 601 encodes the state of the device 101 that is safe. At any state of the device 101 within the control invariant set 603, there exists a control command that maintains the state of the device 101 within the control invariant set 603 as a known or allowed future state.

[0131] For example, for any state of device 101, such as state 615 within control invariant set 603 and within all possible control commands 617-623 that controller 111 can execute, there is at least one control command 623 that maintains 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 may take during the next iteration may take the state of device 101 outside constraint set 601. Examples of constraint set 601 include, for example, Figure 5A , Figure 5B and Figure 5C The convex approximation of the allowed workspace is discussed in .

[0132] Figure 7 A block diagram of a method 700 for controlling the motion of a device from an initial state to a target state in an environment with obstacles according to an embodiment of the present disclosure is shown. 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 characterizing the task, for example, one or a combination of the initial state of the device 101, the target state of the device 101, the geometric configuration of one or more stationary obstacles defining at least a portion of the constraints, and the motion of a mobile obstacle defining at least a portion of the constraints. In addition, the parameters are submitted to a learning function.

[0133] At box 703, method 700 includes executing a learning function trained using machine learning to generate an initial trajectory connecting an initial state of the device to a target state of the device. According to an embodiment, the learning function is a neural network trained using reinforcement learning based on a reward and penalty function that penalizes violations of constraints to a certain extent. At box 705, method 700 includes solving a convex optimization problem subject to constraints to generate an optimal trajectory that minimizes deviations from the initial trajectory. In an embodiment, an optimized safety filter solves a convex optimization problem subject to constraints as hard constraints to generate an optimal trajectory. At box 707, method 700 includes controlling the movement of device 101 according to the optimal trajectory.

[0134] Figure 8 800 is a schematic diagram showing a computing device 800 for implementing the method and system / controller of the present disclosure. The computing device 800 includes a power supply 801, a processor 803, a memory 805, a storage device 807, all connected to a bus 809. In addition, 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. In addition, 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. An external memory 833, an external sensor 835, a machine 837, and an environment 839 can also be connected to the bus 809. In addition, 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 , where data or other data may be presented on a third-party display device, third-party imaging device, and / or third-party printing device external to computing device 800 .

[0135] The memory 805 can store instructions executable by the computing device 800 and 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 optical disk.

[0136] The storage device 807 may be suitable for storing supplementary data and / or software modules used by the computing device 800. The storage device 807 may include a hard disk drive, an optical disk drive, a thumb drive, a drive array, or any combination thereof. In addition, 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 a tape device, a flash memory or other similar solid-state storage device, or an array of devices including a storage area network or other configurations. Instructions may be stored in an information carrier. The instructions execute one or more methods, such as the method described above, when executed by one or more processing devices (e.g., processor 803).

[0137] The computing device 800 may be optionally linked via the bus 809 to a display interface or user interface (HMI) 847 adapted to connect the computing device 800 to a display device 849 and a keyboard 851, wherein the display device 849 may include a computer monitor, a camera, a television, a projector, or a mobile device, etc. In some implementations, the computing device 800 may include a printer interface to a printing device, wherein the printing device may include a liquid inkjet printer, a solid ink printer, a mass commercial printer, a thermal printer, a UV printer, or a dye sublimation printer, etc.

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

[0139] In 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 coupled to one or more input / output devices 841. Computing device 800 may be connected 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.

[0140] This specification only provides exemplary embodiments and is not intended to limit the scope, applicability or configuration of the present disclosure. On the contrary, the following description of the exemplary embodiments will provide a description of the implementation for implementing one or more exemplary embodiments to those skilled in the art. Without departing from the spirit and scope of the disclosed subject matter set forth in the appended claims, various changes may be made to the functions and arrangements of the elements. Specific details are given in the following description to provide a thorough understanding of the embodiments. However, it will be appreciated by those of ordinary skill in the art that the embodiments may be practiced without these specific details. For example, the 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 details. In other cases, well-known processes, structures and techniques may be shown without unnecessary details to avoid obscuring the embodiments. In addition, the same reference numerals and numbers in the respective drawings indicate the same elements.

[0141] In addition, various embodiments may be described as processes depicted as flow charts, flow views, data flow diagrams, structure diagrams, or block diagrams. Although a flow chart may describe an operation as a sequential process, many operations may be performed in parallel or concurrently. In addition, the order of the operations may be rearranged. A process may terminate when its operations are completed, but may have additional steps not discussed or included in the figure. In addition, not all operations in any particularly described process may occur in all embodiments. A process may correspond to a method, function, procedure, subroutine, subprogram, etc. When a process corresponds to a function, the termination of the function may correspond to the function returning to a calling function or a main function.

[0142] In addition, embodiments of the disclosed subject matter may be implemented at least in part manually or automatically. Manual or automatic implementation may be performed or at least assisted using a machine, hardware, software, firmware, middleware, microcode, hardware description language, or any combination thereof. When implemented in software, firmware, middleware, or microcode, program code or code segments that perform the necessary tasks may be stored in a machine-readable medium. A processor may perform the necessary tasks.

[0143] In addition, the embodiments of the present disclosure and the functional operations described in this specification can be implemented in digital electronic circuits, in tangibly implemented computer software or firmware, in computer hardware (including the structures disclosed in this specification and their structural equivalents), or in a combination of one or more of them. In addition, some embodiments of the present disclosure can be implemented as one or more computer programs, that is, one or more modules of computer program instructions encoded on a tangible non-temporary program carrier for execution by a data processing device or control of the operation of the data processing device. In addition, the program instructions can be encoded on an artificially generated propagation signal (e.g., a machine-generated electrical signal, optical signal, or electromagnetic signal), which is generated to encode information for transmission to a suitable receiver device for execution by a data processing device. The computer storage medium can be a machine-readable storage device, a machine-readable storage substrate, a random or serial access memory device, or a combination of one or more of them.

[0144] A computer program (which may also be referred to 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 it may be deployed in any form, including as a stand-alone program or as a module, component, subroutine or other unit suitable for use in a computing environment.

[0145] A computer program may, but need not, correspond to a file in a file system. A program may be stored as part of a file that holds other programs or data, such as one or more scripts in a markup language document, in a single file dedicated to the program in question, or in multiple coordinated files (e.g., files storing one or more modules, subroutines, or portions of code). A computer program may be deployed for execution on multiple computers or on one computer located at one site or distributed over multiple sites and interconnected by a communications network.

[0146] Computers suitable for executing computer programs include, for example, central processing units that can be based on general or special microprocessors or both and any other types. Typically, the central processing unit will receive instructions and data from a read-only memory or a random access memory or both. The basic element of a computer is a central processing unit for executing or running instructions and one or more memory devices for storing instructions and data. Typically, a computer will also include or be operably connected to receive data from one or more large-capacity storage devices (e.g., disks, magneto-optical disks or optical disks) for storing data or to transmit data or both. However, a computer does not need to have such a device. In addition, a computer can 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.

[0147] To provide interaction with a user, embodiments of the subject matter described in this specification 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) through which the user can provide input to the computer. Other kinds of devices may also be used to provide interaction with the user; for example, the feedback provided to the user may be any form of sensory feedback, such as visual feedback, auditory feedback, or tactile feedback; and input from the user may be received in any form, including acoustic input, voice input, or tactile input. In addition, the computer may interact with the user 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 in response to a request received from a web browser on the user's client device.

[0148] Embodiments of the subject matter described in this specification may 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 with a graphical user interface or a web browser through which a user can interact with an implementation of the subject matter described in this specification), or any combination of one or more such back-end, middleware, or front-end components. The components of the system may be interconnected by digital data communication (e.g., a communication network) in any form or medium. Examples of communication networks include local area networks ("LANs") and wide area networks ("WANs"), such as the Internet.

[0149] 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 through computer programs running on the respective computers and having a client-server relationship to each other.

[0150] Although the present disclosure has been described with reference to certain preferred embodiments, it should be understood that various other changes and modifications may be made within the spirit and scope of the present disclosure. Therefore, the aspects of the appended claims cover all such changes and modifications that fall within the true spirit and scope of the present disclosure.

Claims

1. A controller for controlling the movement of a device from an initial state to a target state in an environment with obstacles, wherein the obstacles form constraints on the movement of the device, the controller comprising: processor; and a memory having instructions stored thereon, which, when executed by the processor, cause the controller to: executing a learning function trained using machine learning to generate a feasible trajectory or an infeasible trajectory connecting an initial state of the device with a target state of the device while penalizing violations of at least some of the constraints to a certain extent to produce the initial trajectory; solving a convex optimization problem subject to the constraints to produce an optimal trajectory that minimizes deviation from the initial trajectory; as well as The movement of the device is controlled according to the optimal trajectory.

2. The controller according to claim 1, wherein: The learning function is a neural network trained using reinforcement learning based on a reward and penalty function that penalizes violations of the constraints to a certain extent.

3. The controller according to claim 1, wherein: The constraints include at least: a constraint on reaching the target state of the device, a device-obstacle collision avoidance constraint, an inter-device collision avoidance constraint, and a retention constraint.

4. The controller according to claim 1, wherein: The environment includes a plurality of devices having a respective target state associated with each of the plurality of devices, and wherein the learning function is formulated for each of the plurality of devices.

5. The controller according to claim 4, wherein: The processor is further configured to: executing the learning function for each of the plurality of devices to determine an initial trajectory for each device; 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 of the corresponding device from a corresponding initial trajectory; and The motion of each device is controlled based on the corresponding optimal trajectory to achieve the corresponding target state of each device.

6. The controller according to claim 4, wherein: The processor is further configured to: executing the learning function for each of the plurality of devices to determine an initial trajectory for each device; solving a convex optimization problem for each of the plurality of devices to determine an optimal trajectory for each device, wherein the optimal trajectory for each device minimizes a deviation of the optimal trajectory of the corresponding device from a corresponding initial trajectory; as well as The motion of each device is controlled based on the corresponding optimal trajectory to achieve the corresponding target state of each device.

7. The controller according to claim 1, wherein: The constraints include at least: a static constraint with a fixed position and a dynamic constraint with a position that changes over time, wherein the learning function is trained to penalize violations of only the static constraints, and wherein the convex optimization problem is solved subject to the static constraints and the dynamic constraints.

8. The controller according to claim 1, wherein: Each constraint is implemented as a hard constraint based on a convex approximation of the constraint.

9. The controller according to claim 1, wherein: When the dynamics of the device include stochastic uncertainties, the constraints are implemented as hard constraints.

10. The controller according to claim 9, wherein: The constraints are implemented as hard constraints based on the convexity approximation of each constraint.

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

12. A method for controlling the movement of a device from an initial state to a target state in an environment having obstacles, wherein the obstacles form constraints on the movement of the device, the method comprising the following steps: executing a learning function trained using machine learning to generate a feasible trajectory or an infeasible trajectory connecting an initial state of the device with a target state of the device while penalizing violations of at least some of the constraints to a certain extent to produce the initial trajectory; solving a convex optimization problem subject to the constraints to produce an optimal trajectory that minimizes deviation from the initial trajectory; and The movement of the device is controlled according to the optimal trajectory.

13. The method according to claim 12, wherein: The constraints include at least: a constraint on reaching the target state of the device, a device-obstacle collision avoidance constraint, an inter-device collision avoidance constraint, and a retention constraint.

14. The method according to claim 12, wherein: The environment includes a plurality of devices having respective target states, and wherein the learning function is formulated for each of the plurality of devices.

15. The method according to claim 14, wherein: The method further comprises the following steps: executing the learning function for each of the plurality of devices to determine an initial trajectory for each device; 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 corresponding optimal trajectory from the corresponding initial trajectory; and The motion of each device is controlled based on the corresponding optimal trajectory to achieve the corresponding target state of each device.

16. The method according to claim 14, wherein: The method further comprises the following steps: executing the learning function for each device to determine an initial trajectory for each device; solving a convex optimization problem for each device to determine an optimal trajectory for each device, wherein the optimal trajectory for each device minimizes the deviation of the optimal trajectory from the corresponding initial trajectory; and The motion of each device is controlled based on the corresponding optimal trajectory to achieve the corresponding target state of each device.

17. The method according to claim 12, wherein: The constraints include at least: a static constraint with a fixed position and a dynamic constraint with a position that changes over time, wherein the learning function is trained to penalize violations of only the static constraints, and wherein the convex optimization problem is solved subject to the static constraints and the dynamic constraints.

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

19. A non-transitory computer-readable storage medium having a program embodied thereon, the program being executable by a processor to perform a method for controlling the motion of the device from an initial state to a target state in an environment having obstacles, the obstacles constraining the motion of the device, the method comprising: executing a learning function trained using machine learning to generate a feasible trajectory or an infeasible trajectory connecting an initial state of the device with a target state of the device while penalizing violations of at least some of the constraints to a certain extent to produce the initial trajectory; solving a constrained convex optimization problem to produce an optimal trajectory that minimizes deviation from the initial trajectory; and The movement of the device is controlled according to the optimal trajectory.

Images