Motion Planning and Control with Multi-Stage Construction of Invariant Sets

Abstract

A system and/or a method for controlling the movement of a vehicle in a constrained environment subject to a disturbed vehicle model including uncertainty on the dynamics governing the movement of the vehicle, collects a feedback signal indicative of a state of the vehicle and a setpoint for controlling the vehicle according to a task and determine a robust invariant set centered on the setpoint for the operation of the vehicle in an unconstrained environment using the disturbed vehicle model. The robust invariant set is inflated equally in all directions until a termination condition defined by the constraint environment is met to produce a safe invariant set enabling control of the operation of the vehicle according to the task while maintaining the state of the vehicle within the safe invariant set.

Claims

1. A method for controlling the movement of a vehicle in a constrained environment subject to disturbed vehicle model including uncertainty on the dynamics governing the movement of the vehicle, wherein the method uses a processor coupled with stored instructions implementing the method, wherein the instructions, when executed by the processor, performs the steps of the method, comprising: collecting a feedback signal indicative of a state of the vehicle and a setpoint for controlling the vehicle according to a task; determining a robust invariant set centered on the setpoint for the operation of the vehicle in an unconstrained environment using the disturbed vehicle model; inflating the robust invariant set equally in all directions until a termination condition defined by the constraint environment is met to produce a safe invariant set; and controlling the operation of the vehicle according to the task while maintaining the state of the vehicle within the safe invariant set.

2. The method of claim 1, wherein the robust invariant set is the smallest set of a predetermined shape characterized by a Lyapunov function and a level of the Lyapunov function for the vehicle corresponding to the disturbed vehicle model.

3. The method of claim 2, wherein the robust invariant set is inflated by one or a combination of changing a level of the Lyapunov function and scaling parameters of the Lyapunov function.

4. The method of claim 2, wherein the predetermined shape of the robust invariant set is ellipsoidal or polyhedral.

5. The method of claim 1, wherein the operation of the vehicle is done by closed-loop feedback control, wherein the robust invariant set is determined to have an ellipsoidal volume with geometry computed in the parameters of the disturbed vehicle model.

6. The method of claim 5, wherein the vehicle is controlled by a controller with unknown gains, and wherein the robust invariant set is determined for a set of possible controllers that describe how the dynamical system toward the setpoint, wherein the set of possible controllers describe the gains of the disturbed vehicle model and are defined with polytopic uncertainty.

7. The method of claim 6, wherein the ellipsoidal volume is determined by solving optimization problem that includes bounds on the maximum rotation angle of the attitude tracking error in the closed-loop vehicle control system, bounded input disturbances of the disturbed vehicle model, and the polytopic uncertainties in the gains of the disturbed vehicle model.

8. The method of claim 1, wherein the control is performed using a model predictive controller over a prediction horizon, such that the safe invariant set is determined for each time step, along each point of the prediction horizon or both.

9. The method of claim 1, further comprising: collecting a differentiable trajectory defining the operation of the dynamical system to perform the task, wherein the differentiable trajectory is not guaranteed to be safe or feasible; determining a differential equation having a solution defining a setpoint trajectory, using the safe invariant set, unsafe differentiable trajectory, and the state of the vehicle; integrate the differential equation for each time step of the control to produce the setpoint trajectory; and controlling the vehicle according to the computed setpoint trajectory.

10. The method of claim 9, wherein the differentiable trajectory is determined using an optimization method that minimizes a cost function including a total variation of the movement of the vehicle and its derivatives subject to constraints of define by the constrained environment.

11. The method of claim 9, wherein the differentiable trajectory is determined using a navigation field computed in simplified world geometry having obstacles of the constraint environment represented as spheres, wherein the simplified world geometry is mapped to the obstacles in the constrained environment using one or more diffeomorphisms.

12. The method of claim 11, wherein the diffeomorphisms are partitioned into a first diffeomorphism that scales the world geometry, and a second diffeomorphism that maps the scaled geometry to the simplified world geometry using obstacle and boundary influence functions computed as solutions to an optimization problem.

13. The method of claim 1, further comprising: computing a set of setpoints and a set of safe invariant sets centered on the corresponding set points; and constructing a graph having vertices defined by the set of setpoints; and determining connectivity of the graph based on the safe invariant sets, such that a connection is established if the robust invariant set of one node is contained in the safe invariant set of another; finding a solution path of vertices connecting an initial vertex in the graph to a terminal vertex in the graph corresponding to the specified task; and controlling the vehicle according to the solution on the graph by switching the setpoints based on the Lyapunov function and its size in relation to the safe invariant sets of the nodes along the vertices in the solution path.

14. The method of claim 13, wherein the solution path on the graph is a time-agnostic sequence of setpoints, wherein the controlling uses a switching logic connecting the time-agnostic sequence of setpoints in time based on the errors induced by the disturbances acting on the vehicle.

15. The method of claim 14, wherein the switching logic is defined by checking if the Lyapunov function associated with the robust invariant set of the next node in the solution path is smaller than the safe invariant set of the same node in the path.

16. The method of claim 15, wherein the motion trajectory is computed in simulation and used to control the vehicle along a set-point trajectory defined in time.

17. The method of claim 1, wherein the vehicle is a drone operating in an indoor environment and the robust invariant set is computed in six-dimensional space comprising the positions and velocities of the drone.

18. The method of claim 1, wherein the dynamical system (or a vehicle) is a drone, wherein the constraint environment includes an indoor environment having obstacles defining the constraints.

19. A feedback controller for controlling a movement of a vehicle in a constrained environment subject to disturbed vehicle model including uncertainty on the movement of the vehicle, comprising: at least one processor; and a memory having instructions stored thereon that, when executed by the at least one processor, cause the feedback controller to: collect a feedback signal indicative of a state of the vehicle and a setpoint for controlling the vehicle according to a task; determine a robust invariant set centered on the setpoint for the operation of the vehicle in an unconstrained environment using the disturbed vehicle model; inflate the robust invariant set equally in all directions until a termination condition defined by the constraint environment is met to produce a safe invariant set; and control the operation of the vehicle according to the task while maintaining the state of the vehicle within the safe invariant set.

20. A non-transitory computer readable storage medium embodied thereon a program executable by a processor for performing a method, the method comprising: collecting a feedback signal indicative of a state of the vehicle and a setpoint for controlling the vehicle according to a task; determining a robust invariant set centered on the setpoint for the operation of the vehicle in an unconstrained environment using the disturbed vehicle model; inflating the robust invariant set equally in all directions until a termination condition defined by the constraint environment is met to produce a safe invariant set; and controlling the operation of the vehicle according to the task while maintaining the state of the vehicle within the safe invariant set.

Description

BRIEF DESCRIPTION OF THE DRAWINGS

[0023] FIG. 1A illustrates an example environment where a motion plan is computed according to an embodiment of the present disclosure.

[0024] FIG. 1B illustrates how the obstacles and free space is represented as polyhedral sets according to one embodiment of the invention.

[0025] FIG. 1C demonstrates how a feasible motion plan is computed in free space, connecting a starting region with a goal region in an embodiment of the invention.

[0026] FIG. 1D shows how an unsafe planner computes a motion plan based on mission information, and how this plan is modified by a safety mechanism.

[0027] FIG. 1E details how this safety mechanism is constructed using robust and safe invariant sets, according to one embodiment of the present disclosure.

[0028] FIG. 1F illustrates the polytopic uncertainty set characterizing the possible responses of the disturbed vehicle model.

[0029] FIG. 2A contains a block diagram detailing how the robust invariant sets are computed in some embodiments.

[0030] FIG. 2B illustrates an environment in which a sensor is measuring a wind field, which is an exemplar disturbance that may be used in the invention.

[0031] FIG. 2C details how the robust invariant sets can be inflated to compute safe invariant sets, according to one embodiment of the invention.

[0032] FIG. 2D details how the safe invariant sets relate to the obstacles in the environment, centered on the intended path of the vehicle.

[0033] FIG. 3A contains a block diagram depicting how the safety mechanism is designed in one embodiment of the invention.

[0034] FIG. 3B illustrates an unsafe trajectory computed using optimization-based methods, used as input in one embodiment of the safety mechanism.

[0035] FIG. 3C shows a block diagram indicating how the reference can be computed using a navigation function, used in one embodiment of the invention.

[0036] FIG. 3D illustrates the shape of the obstacle functions and their resulting influence functions in one embodiment of the present disclosure.

[0037] FIG. 3E shows how a polyhedral workspace with polyhedral obstacles can be mapped to a spherical world with spherical obstacles which is used to constructing the navigation field in one embodiment.

[0038] FIG. 3F shows a navigation field in the spherical world, and how this translates into the coordinates of the original polyhedral world.

[0039] FIG. 3G illustrates how a controlled vehicle tracks a modified reference with new missions being sent to the controlled system at different points in time.

[0040] FIG. 3H shows an embodiment in which the invariant sets are used in a model predictive control framework to continuously update the reference trajectory.

[0041] FIG. 4A contains a block diagram depicting how the safety mechanism is designed in one embodiment of the invention.

[0042] FIG. 4B shows how to define the connectivity of the graph from the robust and safe invariant sets to form a path from initial vertex with a terminal vertex.

[0043] FIG. 4C shows how the connectivity of the graph is determined according to one embodiment of the present disclosure.

[0044] FIG. 4D shows how candidate the set-points are sampled in space using a lattice-based construction, used in one embodiment.

[0045] FIG. 4E illustrates how the set-points are updated based on a switching condition used in one embodiment of the present disclosure.

[0046] FIG. 4F shows how the safe path is tracked by a vehicle in an indoor environment, which is one exemplar of the invention.

[0047] FIG. 4G how the errors of the system remain within their designated sets when switching between different set-points in time, as done in one embodiment.

[0048] FIG. 5A shows a schematic of the communication and structure for an embodiment in which the vehicles perform remote sensing of the disturbance.

[0049] FIG. 5B shows a sketch of how the invention may be modularized as a service for a generic vehicle that can request a motion plan.

[0050] FIG. 6A is a schematic illustrating a computing device for implementing the methods and systems of the present disclosure.

DETAILED DESCRIPTION

[0051] In the following description, for purposes of explanation, numerous specific details are set forth to provide a thorough understanding of the present disclosure. It will be apparent, however, to one skilled in the art that the present disclosure may be practiced without these specific details. In other instances, apparatuses and methods are shown in block diagram only to avoid obscuring the present disclosure.

[0052] As used in this specification and claims, the terms for example, for instance, and such as, and the verbs comprising, having, including, and their other verb forms, when used in conjunction with a listing of one or more components or other items, are each to be construed as open ended, meaning that that the listing is not to be considered as excluding other, additional components or items. The term based on means at least partially based on. Further, it is to be understood that the phraseology and terminology employed herein are for the purpose of the description and should not be regarded as limiting. Any heading utilized within this description is for convenience only and has no legal or limiting effect.

[0053] Specific details are given in the following description to provide a thorough understanding of the embodiments. However, understood by one of ordinary skill in the art can be that the embodiments may be practiced without these specific details. For example, systems, processes, and other elements in the subject matter disclosed may be shown as components in block diagram form in order not to obscure the embodiments in unnecessary detail. In other instances, well-known processes, structures, and techniques may be shown without unnecessary detail to avoid obscuring the embodiments. Further, like reference numbers and designations in the various drawings indicated like elements.

[0054] FIG. 1A illustrates an example with an environment 100a contained in a boundary 101a where a vehicle 102a is to move and execute a task, according to an embodiment of the present disclosure. It is an objective of some embodiments to compute a motion plan for the vehicle 102a from an initial state in a set 103a to a target state in a set 104a, both defined in the environment 101a. In one embodiment, the vehicle is provided the task of reaching the target state 104a, in other embodiments the vehicle is to move from state to the target state 104a and back while exploring a designated part of the obstacle free environment 100a. The state of the vehicle 102a may include one or a combination of a position of the vehicle 102a, an orientation of the vehicle 102a, a velocity of the vehicle 102a, or other states relating to its actuation.

[0055] The environment 100a includes obstacles 111a, 112a, 113a, 114a, 115a, and 116a. Some of these obstacles are convex, such as 112a, and others are non-convex, such as 111a. The vehicle 102a may be an autonomous vehicle, such as a differentially driven mobile robot, an aerial vehicle, a water surface vehicle, or an underwater vehicle. The task's performance is contingent on satisfaction of constraints on the vehicle's 102a motion relating to the environment 100a. In other words, the performance of the task is subject to the constraints. Examples of such constraints include reaching the target region 104a, avoiding collision with the obstacles 111a-116a (i.e., a vehicle-obstacle collision avoidance constraint), moving the vehicle 102a such that the state of the vehicle 102a (e.g., position) is within a pre-defined set of admissible states of the vehicle 102a (i.e., a keep-in constraint), and the like. For instance, an airborne vehicle such as a drone can be tasked to reach certain positions to inspect a structure while avoiding collisions with parts of the structure.

[0056] FIG. 1B illustrates how the environment 100B may be decomposed into sets of free space, in which the vehicle 101b is free to operate and move. In one embodiment, this is done by computing a mesh comprised of polyhedral sets 120b over the entire environment, before subsequently combining these smaller sets into larger sets 111b-119b. It is understood that this operation can be done in many ways, for instance using submodular coverage optimization or simpler heuristics that iteratively remove edges from the collection of edges in the mesh. The result of any such operation is a collection of sets. The union of these sets covers all the free space in some embodiments, and most of the free space in others. Similarly, the non-convex obstacles such as 121b can be decomposed into a set of convex obstacles. It is understood that some such decompositions may result in convex polyhedral obstacles, convex ellipsoidal obstacles, or other convex shapes whose union contain the non-convex obstacle in its interior.

[0057] In FIG. 1C, a motion plan is depicted wherein a vehicle 101c is to move from a start region 102c to the terminal region 103c while passing through connected sets of free space 111c-115c and always remaining in a safe set 106c. In some embodiments, the segments are constrained to reside on a designated subset of free space. For instance, segment 121c is confined to the set 111c. In other embodiments, the motion is constrained to the intersection of two connected sets of free space, such that the end point of segment 111c is constrained to the intersection of the sets 121c and 122c. In yet other embodiments, the motion is characterized by modifying a reference such that the vehicle remains in a safe invariant set 106c such that it never intersects with the obstacle region 120c.

[0058] FIG. 1D illustrates how information describing a mission 110d is used to compute an unsafe motion plan 120d which is represented as a set-point or trajectory 121d. It is understood that some motion planners that do not compute trajectories are safe given disturbances and uncertainties in the model parameters. Consequently, the safety mechanism 130d modifies the set-point or trajectory 121d to make it safe given assumptions on the disturbances and uncertainties of the model and knowledge of the geometry of the world. The modified set-point or trajectory 121d is subsequently used to control the vehicle 140d, ensuring that its motion remains safe.

[0059] The mission 110d may comprise of single waypoints or a set of waypoints that the UAV should visit in sequence, or more abstract information such as tasks depending on the embodiment of the unsafe planner. Such tasks may include landing, taking off, acquiring sensory information, among other tasks. Provided that the unsafe trajectory can take the mission and compute a reference trajectory or set points for the vehicle, several types of missions are used either in isolation or conjunction.

[0060] In the following, it is understood that a vehicle that is to track a set-point has one or more controllers operating on a stabilization or tracking error to steer the vehicle towards the desired set-point. The model that takes the set-point as input and models the closed-loop behavior of the vehicle subject to this setpoint and disturbances is known as the disturbed vehicle model. This is a dynamical system with several parameters, hereinafter referred to as gains.

[0061] FIG. 1E details how the set-point or trajectory is modified to produce a safe reference using robust invariant sets. The mission 110e is passed to the unsafe planner 120e to compute a set-point or trajectory 121e which is used to compute a robust invariant set 140e using a vehicle model 130e that includes assumptions on how the disturbances interact affect the vehicle and a set to which they are confined, and structured uncertainties in the dynamics describing the time-evolution of the vehicle. In one embodiment, the vehicle mode 130e is a second-order differential equation with polytopic uncertainty in the parameters of the differential equation. In yet other embodiments, the disturbance is an additive input disturbance with a known bound. In yet other embodiments, the structured uncertainties are induced by errors in the attitude tracking control system of the vehicle, responsible for controlling its rotation. In another embodiment, the model has saturations limiting the maximum and minimum accelerations of the vehicle. In other embodiments, the disturbed vehicle model includes either one or more of the polytopic uncertainty in the parameters, bounded input disturbances, attitude tracking control errors, and input constraints. One such disturbed vehicle model for disturbed unmanned aerial vehicles is

[00001]$\stackrel{.Math.}{p}=-\tilde{R}{K}_p\left(p-r\right)-\tilde{R}{K}_{V}v+,$

where the disturbance is characterized by

[00002]$=m^{-1}f+g\left(I-\tilde{R}\right)e_3,$

and the disturbing force f is bounded as ff.sub.max. In this embodiment, the disturbed vehicle model is characterized by a set of possible controller gains Co({(K.sub.p.sup.i, K.sub.v.sup.i)}N.sub.i=1.sup.N)=K, and it is assumed that the controller gains that best describe the response of the controlled vehicle resides within this set. In this case the true system response characterized by K=(K.sub.p, K.sub.v)K, and only known in approximate sense with the set K spanned by N known elements (K.sub.p.sup.i, K.sub.v.sup.i). Furthermore, the embodiment includes a maximal bound on the attitude tracking error in the sense that

[00003]$=\underset{.Math.u.Math.=1}{\max }\arccos \left(u^T\tilde{R}u\right){}_{\max }.$

It is understood that all of the parameters f.sub.max, a.sub.max,{(K.sub.p.sup.i, K.sub.p.sup.i)}.sub.i=1.sup.N, N, m, g need not be known, and that some or all of these parameters can be identified or learned from data either online or offline.

[0062] The disturbed vehicle model is used to compute a robust set 131e, in which the closed-loop control system will reside for all future times subject to the disturbed vehicle model 130e. In some embodiments, the robust set 131e is ellipsoidal, and in others it is polyhedral. Based on the geometry of the world in which the vehicle operates 150e safe robust sets are computed 160e. Any vehicle that starts in a safe robust set 161e remains in the set at all future times, and the safe robust sets are computed such that they do not intersect with the obstacles in the world. In some embodiments, the obstacles are convex or non-convex polytopes, in others they are ellipsoidal volumes, and in yet others they are a combination of both.

[0063] Based on the inflated safe sets, a new set-point 171e is evaluated using a safety mechanism 170e based on the inflated obstacles 161e. The modified set-point is sent to the closed loop control system and used to control the vehicle.

[0064] FIG. 1F illustrates the polytopic uncertainty set characterizing the possible responses of the disturbed vehicle model. The uncertainty set 110f is defined as Co({(K.sub.p.sup.i, K.sub.v.sup.i)}.sub.i=1.sup.N)=K and spanned by a set of possible controller gains, {(K.sub.p.sup.i, K.sub.v.sup.i)}.sub.i=1.sup.N 111f, 112f, 113f, 114f, 115f, wherein each element of this set is associated with a particular system response. For example, the gains 111f are associated with fast and slightly under-damped response 121f, while the gains 112f are associated with a more damped and slower response 122f. The true gain that the controlled vehicle is best represented by K=(K.sub.p, K.sub.v) 120f resides somewhere in this set, that is KK. This gain 120f is not known, and associated with a system response 123f that is not as fast and under-damped as the response 121f yet not as slow and damped as the response 122f. It is understood that this is a simplified illustration of this set.

Computing Invariant Sets

[0065] FIG. 2A shows how the robust invariant sets can be computed by solving an optimization problem. A large set of possible controllers 210a, measurements by which they can be computed 220a, or both are used to compute a smaller set of possible controllers 230a. In one embodiment, the output of these computations are a set {(K.sub.p.sup.i, K.sub.v.sup.i)}.sub.i=1.sup.N defining the K and the bounds on the disturbances a.sub.max and f.sub.max 231a. In other embodiments, only the bound max is computed. Based on this information, an optimization problem is defined to minimize a linear objective c.sup.T x subject to linear matrix inequality constraints F(x)0 with decision variables x.

[0066] In some embodiments, this optimization problem is defined with decision variables x=(P, ) which characterize an ellipsoidal set. Specifically, the problem is:

[00004]$\underset{\begin{array}{c}P{S}_{++}^{66}\\ >0\end{array}}{\min }\left(\right)$

subject to
where

[00005]$\left[\begin{array}{cc}{A}_{i}P+{\mathrm{PA}}_i^T+P& \mathrm{PB}\\ B^{T}P& -I\end{array}\right]0,i=1,.Math.,N,$$PI,$

[00006]$A_i=\left[\begin{array}{cc}0& I\\ -K_p^i& -K_v^i\end{array}\right],$$B=\left[\begin{array}{c}0\\ I\end{array}\right].$

[0067] In other embodiments where structured uncertainties are included, the optimization problem is defined with the decision variables x=(P, P, ), and formulated:

[00007]$\underset{\begin{array}{c}P{S}_{++}^{66}\\ \stackrel{\_}{K}{S}_{++}^{66}\\ >0\end{array}}{\min }\left(\right)$

subject to

[00008]$\left[\begin{array}{ccc}{A}_{i}P+{\mathrm{PA}}_i^T+P+\stackrel{\_}{K}& \mathrm{PB}& \sqrt{}\mathrm{PB}\\ B^{T}P& -I& 0\\ \sqrt{}{B}^{T}P& 0& -I\end{array}\right]0,i=1,.Math.,N,$$PI,$$\left[\begin{array}{cc}\stackrel{\_}{K}& K_i^T\\ K_i& I\end{array}\right]0,$

where

[00009]$K_i=[\begin{array}{cc}{K}_p^i& K_v^i],\end{array}$$=\sqrt{2\left(1-\cos \left({}_{\max }\right)\right).}$

[0068] It is understood that the latter formulation manages to incorporate the structured uncertainties generated by nonperfect attitude tracking. In both cases, the procedure results in a quadratic Lyapunov function

[00010]$V\left(p,r,v\right)={.Math.\left[\begin{array}{c}p\\ v\end{array}\right]-\left[\begin{array}{c}r\\ 0\end{array}\right].Math.}_p^2.$

[0069] Where the level set V(p, r, v).sub.max.sup.2=V.sub.min is forward invariant and characterizes an inner approximation of the robust invariant set. In the context of the quadrotor UAV dynamics used in this exemplar, this corresponds to a six-dimensional hyper ellipsoid. It is understood that subject to the disturbed vehicle model, a controlled vehicle that starts tracking a set-point r will remain in the robust invariant set at all future times as long as the set-point remains constant. It is also understood that other representations and approximations of the robust invariant sets are possible, such as through zonotopes or polytopic representations.

[0070] Having formulated the optimization problem 250a it is solved 260a and the solution in terms of the Lyapunov matrix P and the bound 261a are used to compute the robust invariant set O.sub.min 270a.

[0071] FIG. 2B shows a schematic of a remote sensing of wind over a complex terrain 230b used by some embodiments. The LiDAR sensor 232b arranged at a point, e.g., at a top of a hill, performs series of line-of-sight measurements on a cone including measurements 234b, 238b, 242b along the surface of the cone, and measurements along the center line 240b. The measurements are taken for different altitudes corresponding to different planes 201b. In such a manner, for a set of altitudes corresponding to planes 244b, the measurements on the cone are measurements on a circle including multiple measurements of the radial velocities in different angular directions measured at different line-of-site points on a circumference of the circle and one measurement of the radial velocity in a vertical direction measured at a center of the circle.

[0072] Some such sensors aim to determine a horizontal velocity of the wind flow for each altitude. Given these measurements, an estimate of the horizontal velocity can be determined from the measurements of the radial velocity using a geometrical relationship and assuming that the wind velocity is homogenous on each plane. Here, is the estimated velocity of the wind flow based on homogenous assumption. Some LiDAR sensors are based on recognition that, for complex terrains, such as the terrain 200B, the homogenous velocity assumption leads to a bias in LiDAR estimation of horizontal velocity. The main error is due to variation of the vertical velocity, e.g., along the hill. To that end, some sensors are based on the realization that the homogeneous velocity assumption in sensing wind flow passing over the complex terrain can be corrected using a horizontal derivative of vertical velocity. Such measurements can be used to inform the bounds of the disturbances of the controlled vehicle system.

[0073] FIG. 2C describes how the safe invariant sets can be determined by solving a quadratic program according to one embodiment of the invention. A set of obstacles are determined 220c from the problem geometry 210c. The geometry can either be estimated or provided in terms of a mesh in formats such as a digital asset exchange file (DAE) or the Standard Tessellation Language (STL) and is then converted into a set of convex polytopes and ellipsoids, collectively referred to as the set of obstacles 221c. It is understood that individual obstacles from the set of obstacles 221c may be static or moving, or that some of the obstacles may be static and others may be moving. For each obstacle in the set of obstacles 221c, and a given robust invariant set 230c computed as described in FIG. 2B, the robust invariant set 230c is inflated to produce a candidate safe invariant set defined by the Lyapunov matrix and a level in the set of levels {.sub.i}.sub.i=1.sup.M 241c. The set 241c is computed by solving a quadratic program, a method of projection, or geometric heuristics to find a level set to the Lyapunov function associated with the robust invariant set 230c, such that all trajectories that start in the safe set remain in the safe set for all future times without colliding with an obstacle in the set of obstacles 221c. One such level set is computed for each obstacle in the set of obstacles 221c. Furthermore, given the disturbed vehicle model and the robust invariant set 230c, an additional level .sub.0 can be computed for the input constraints of the disturbed vehicle model 250c, such that all trajectories that start in the associated invariant set always satisfies the input constraints. The set of levels 241c and the level 261c are then combined to compute the largest safe set in which all geometric and input constraints are satisfied. This is done by taking the smallest level in the set of levels 241c combined with the level 261c.

[00011]$V_{\max }^k=\underset{i\left[0,M\right]}{\min }{}_i.$

[0074] Based on this smallest level, the safe robust set is computed and reported 280c, as

[00012]$O_{\max }^k=\left\{x{R}^{2n}|{\left(x_e^k\right)}^{T}P{x}_e^k{V}_{\max }^k,x_e^k=x-{\left(r_k^T,0\right)}^T\right\}.$

[0075] It is recognized that for vehicles such as aerial vehicles, input constraints such as the maximum thrust used by the disturbed vehicle model can be considered in the context of a Lyapunov matrix P by solving an optimization problem

[00013]$\underset{\left({}_{11},{}_{12},{}_{22}\right)R^3}{\min }\left(\right)$

subject to

[00014]$\left[\begin{array}{cc}P& \mathrm{diag}\left(K_p^i,K_v^i\right)\\ \mathrm{diag}\left(K_p^i,K_v^i\right)& \end{array}\right]0,i=1,.Math.,N,$$\left[\begin{array}{cc}{}_{11}& {}_{12}\\ {}_{12}& {}_{22}\end{array}\right]=,$${}_{11}+2{}_{12}+{}_{22}=,$

and subsequently computing the additional level .sub.0 as follows

[00015]${}_0={\left(f_{\max }-mg\right)}^2{m}^{-2}{}^{-1}.$

[0076] FIG. 2D illustrates how the set of levels 241c are computed with respect to three obstacles in two-dimensional space. The robust invariant sets are computed for the set-points 201d, 202d, 203d, and is here illustrated for the set-point 202d with the ellipse 211d. By scaling this robust invariant set and its associated level, the set 201d is computed with respect to the ellipsoidal obstacle 2021d characterized by a level .sub.1 in the set of levels 241c, the set 212d is computed with respect to the polyhedral obstacle 222d characterized by a level .sub.2 in the set of levels 241c, and the set 213d is computed with respect to the half-plane or wall constraint 223d characterized by a level .sub.3 in the set of levels 241c. Of these, the candidate safe set 212d associated with the polyhedral obstacle 222d is contained in the other two candidate safe sets 211d, 213d. Thus, the safe robust set is determined as the invariant set 212d and is found by taking the minimum of the levels in the set of levels 241c associated with the candidate safe robust sets 211d, 212d, 213d. The nature of the safe invariant set 212d is that all trajectories that start within it remain in the set at all future times, and eventually converges to the smaller robust invariant 202d set in its interior. It is recognized that similar computations can be done using level sets represented as zonotopes or polyhedra, which represent separate embodiments of the present invention.

[0077] These sets can be used to ensure safety in several ways, and two such methods are described as part of the disclosure, differing in the way in which the set-points are updated. In the first, the set-point is updated continuously as the solution to an ordinary differential equation. In the second, the set-points are updated discontinuously, forming a piece-wise constant reference trajectory for the controlled system to follow. We start by describing the continuous embodiments, then detail the discontinuous embodiments, before giving implementation details.

Continuous Embodiments

[0078] FIG. 3A details a block diagram of a safety mechanism where the set-points are updated in a continuous manner. The unsafe set-point time derivative 310a is provided to the safety mechanism and is used in the integration of the differential equations 320a governing the time-evolution of the safe set-point 321a. This differential equation is defined by the dynamic margin 330a, which is computed using the safe invariant sets from 200C and 200D centered at the current safe set-point 321a. In one embodiment related to aerial vehicle control, the controlled vehicle 340a is a quadrotor and its states contain the positions and velocities of the system in three dimensions. In this embodiment, the dynamic margin is computed by comparing the Lyapunov function evaluated in the current state of the vehicle to the level of the associated safe invariant set. The difference between the two constitutes the dynamic margin 330a. In one embodiment of the invention, the dynamic margin is computed as

[00016]$M\left(p,v,r_k\right)=V_{\max }^k-V\left(p,v,r_k\right),$

where V.sub.max.sup.k is the level associated with the safe invariant set computed at r.sub.k. The differential equation governing the time-evolution of the set-point is:

{dot over (r)}.sub.k=M(p,V,r.sub.k)r{dot over (r)}.sub.k

where is a positive gain. This differential equation can be integrated in time to update the set-point. It is understood that the initial condition of this differential equation is critical to ensuring safety, and that it can be set from on the initial state of the controlled vehicle 340a.

[0079] FIG. 3B shows segments of the path/motion plan of the vehicle illustrated in one dimension of the flat output space of the controlled vehicle 340a, according to some embodiments of the present disclosure. the motion of the vehicle is parameterized by the sequence of splines as parameterized curves, and wherein the parametrized curves include one or a combination of polynomials, Bezier curves, B-splines, truncated Fourier series, and Legendre polynomials. It is understood that any other representation of the path that is linear in motion parameters can be used in place of these parametrizations. The path includes m segments, wherein each segment is defined with a duration T.sub.i=.sub.i-.sub.i-1, defined in the end times 331b, 332b, 333b, 334b, 335b. In FIG. 3B, a position trajectory (0th derivative) 111f and its first three derivatives: velocity (1st derivative) 112f, acceleration (2.sup.nd derivative) 113f and jerk (3rd derivative) are depicted.

[0080] In some embodiments, such trajectories are computed by minimizing objective functions expressing the total variation of the position trajectory, indicated by 315b for the first segment of the trajectory. In others, the trajectory is computed by a weighted sum of the total variation of the position and all higher derivatives. In yet others, the trajectory is computed to minimize a cost combining a total variation objective with a cost functional in the forces, torques and other signals present in the closed-loop vehicle control system. In other embodiments, the optimization minimizes the total time .sub.i T.sub.i. In some embodiments, the optimization objective comprises all or some of these costs, and optimizes the trajectory subject to endpoint constraints 321b, 322b and continuity constraints. It is understood that an optimization of this kind is computed for a vehicle model in an idealized setting and may result in trajectories that are not safe when considering input disturbances and obstacles given a disturbed vehicle model. In the potentially unsafe motion plan depicted in FIG. 3B, the velocity set-point trajectory is continuous in time and can be evaluated. In some embodiments, this unsafe position set-point time derivative is used as input to the safety mechanism 301a depicted in FIG. 3A.

[0081] In yet other embodiments, the set-point time derivative is encoded as a vector field expressed as a function of the positional space herein referred to as a navigation function.

[0082] FIG. 3C depicts how such a navigation field can be constructed and evaluated to form the unsafe position set-point time derivative that is used as input to the safety mechanism 301a depicted in FIG. 3A. Based on the problem geometry 320c, obstacle and boundary functions can be determined 330c. The obstacle and boundary functions associate each point in space with a positive value that increases when moving away from each individual obstacle 111a, 112a, 113a, 114a, 115a, and the boundary of the world 101a, respectively. From these functions, obstacle and boundary influence functions are computed 340c. The influence functions 341c associate each point in space with a value that increases when moving toward the obstacle or boundary. From these influence functions, and with knowledge of the goal location provided by the mission information 310c, a diffeomorphism can be computed 350c that associates each point in the world with polyhedral obstacles 320c to a world in which the obstacles are spheres, henceforth referred to as the sphere world.

[0083] A diffeomorphism is a structure-preserving mapping between two smooth manifolds of the same type. It is an invertible function that maps one differentiable manifold to another, in this case the original geometry and the sphere world, such that both the function and its inverse are continuously differentiable. Given that such a diffeomorphism 351c exists, and based on the mission information 310c that contains a goal location, a navigation field can be constructed 360c in the sphere world where all trajectories converge to the goal location provided by the mission without intersecting any of the obstacles in the sphere world. The gradient of this navigation field can subsequently be evaluated through the diffeomorphism to produce a vector field in the original geometry. From this navigation vector field, the position set-point time-derivative can be evaluated at any point in space.

[0084] The computation of a navigation field is an alternative to the optimization-based trajectory computation in FIG. 3C, and similarly produces a positional set-point time derivative that is used as input to the safety mechanism 310a in some embodiments of the invention. Next, we provide additional details on how the obstacle functions and diffeomorphisms are computed.

[0085] FIG. 3D demonstrates how the obstacle and boundary functions 331c can be computed and subsequently converted into obstacle and boundary influence functions 341c. The obstacles are represented as polyhedral sets 301d, 302d, 301d, 304d, 305d, and the obstacle function computed for obstacle 305d is shown as 301d. It is recognized that this obstacle function can be computed in many ways, for example by numerical optimization in solving a quadratic program (QP) to find the nearest point to the obstacle and taking the Euclidean distance as the obstacle function

[00017]${}_i\left(r\right)=\underset{q{S}_i}{\min }.Math.q-r.Math..$

[0086] This function is defined in the exterior of any obstacle, and one such function is computed for each obstacle. From these obstacle functions, we can construct a set of obstacle influence functions 330d. It is understood that one way of doing this is as follows

[00018]${}_i\left(r\right)=\frac{{}_k\left(r\right){\stackrel{}{}}_i\left(r\right)}{{}_k\left(r\right){\stackrel{}{}}_i\left(r\right)+{}_i\left(r\right)},$

where

[00019]${\stackrel{}{}}_i\left(r\right)=\underset{j=0\mathrm{or}ji}{\overset{M}{.Math.}}{}_j\left(r\right)$${}_k\left(r\right)={.Math.r-r_G.Math.}^{2k}$

[0087] The obstacle influence function computed for obstacle 305d is shown as 302d.

[0088] Similarly, it is recognized that the boundary function 303d for the boundary 310d can be defined in terms of a distance from a point in its interior to the boundary. The boundary function is thus defined only in the interior of the boundary. This function can similarly be converted 331d using the above expressions to a boundary influence function 304d. Using these functions, we next detail how to compute the diffeomorphism, which is henceforth split into two components. The first component is a scaling diffeomorphism, and the second is the mapping from the scaled polyhedral world to the sphere world.

[0089] FIG. 3E illustrates how the first differmorphisms map points in space from a polyhedral unscaled world to a scaled polyhedral world 320e, here viewed from above, and how this transformed polyhedral word is mapped via the second differomorphism to the sphere world 340e. In the polyhedral worlds, the obstacles 311e, 312e, 313e, 314e, 315e, 331e, 332e, 333e, 334e, 335e are polyhedral obstacles of different size in the scaled and unscaled worlds. These are mapped to spherical obstacles 351e, 352e, 353e, 354e, 355e in the sphere world. The first diffeomorphism is a simple linear scaling, chosen such that the unscaled polyhedral world fits into a unit sphere. This map is defined by

h.sub.a(r)=ar,

[0090] For a positive scaling parameter a. The second differomorphism is computed based on the obstacle influence functions as follows

[00020]$h_{}\left(r\right)=\left(1-\underset{i=0}{\overset{M}{.Math.}}{}_i\left(r\right)\right)r_G+\underset{i=0}{\overset{M}{.Math.}}{}_i\left(r\right)T_i\left(r\right),$

where

[00021]$T_i\left(r\right)=\sqrt{1+{}_i\left(r\right)}\frac{r-w_i}{.Math.r-w_i.Math.}{r}_i+w_i,$$T_i\left(r\right)=\sqrt{1-{}_0\left(r\right)}\frac{r-w_0}{.Math.r-w_0.Math.}{r}_0+w_0.$

[0091] The parameters {(r.sub.i, w.sub.i)}.sub.i=0.sup.M determine the shape of the obstacles and boundary in the target sphere world. When passing points in the unscaled polyhedral world first through h.sub.a 320e and then through h.sub. 330e, these points are mapped to unique points in the sphere world 350e, as shown in FIG. 3E.

[0092] FIG. 3F shows how a navigation field in the sphere world is mapped to a navigation field in the original scaled polyhedral world using the inverse of the two diffeomorphisms. The navigation function is defined such that all solutions moving in the negative gradient of the field converge to the goal position 301f, while avoiding the spherical obstacles 311f, 312f, 313f, 314f, 315f when moving orthogonality to the level sets 303f in the direction of the negative gradient. It is understood that a suitable navigation field can be computed as

[00022]$\stackrel{}{}\left(r;r_G\right)=\frac{{.Math.r-r_G.Math.}^2}{{\left({.Math.r-r_G.Math.}^{2k}-{.Math.}_{i=0}^M{}_i\left(r\right)\right)}^{1/k}},$

[0093] When transformed back to the original polyhedral world 320f using a composition of the first diffeomorphism 320e and the second diffeomorphism 330e, the resulting navigation field 332f is such that all solutions moving in the negative gradient of the field converge to the goal position 302f, while avoiding the polyhedral obstacles 331f, 312f, 313f, 314f when moving orthogonality to the level sets 333f in the direction of the negative gradient. Consequently, specifying the goal location in the mission information, and knowing the obstacles, the navigation function of the sphere world and its gradient can be evaluated at any point in the interior of the world boundary, excluding the interior of the obstacles. It is understood that the resulting gradient can be taken as the set-point time derivative used as the unsafe reference to the safety mechanism, with

[00023]$\stackrel{.}{\stackrel{\_}{r}}=-\frac{\left(r;r_G\right)}{.Math.\left(r;r_G\right).Math.},$

where

(r;r.sub.G)=({circumflex over ()}h.sub.h.sub.a)(r;r.sub.G).

[0094] FIG. 3G illustrates how different goal locations are provided to the safety mechanism, here piece-wise constant, and how the set-point trajectory computed by the safety mechanism in the x, y, and z dimensions, 310g, 311g and 311g, respectively. The actual positions of the quadrotor lags the intended set-point, but that the set-point is modified in such a way that this tracking error never becomes unsafe. The speed of the quadrotor varies depending on how close it is to an obstacle, and it safely traverses the space while satisfying input constraints, despite the original trajectory produced by the navigation field not being safe.

[0095] This represents one embodiment of the safety mechanism, but it is understood that the unsafe reference time derivative can be exchanged for other motion plans, such as the optimization-based motion plan used in other embodiments of the invention.

[0096] FIG. 3H shows a schematic illustrating an embodiment where the disturbed vehicle model is controlled subject to constraints formulated as a non-linear optimization program and solved in a receding horizon framework, also known as model predictive control. The optimization problem is formed such that a safe setpoint trajectory r(t) 312h is computed in order to modify an original unsafe positional setpoint trajectory r(t) 311h such that the position p(t) 313h of the disturbed vehicle model resides in a safe invariant set at all times. In this embodiment, the safe invariant sets can be enforced as nonlinear constraints along each point of the prediction horizon. In this case, it is understood that solving the nonlinear optimization problem entails that the disturbed vehicle model resides in a safe invariant set at all times. In this exemplar, a constraint associated with the maximum thrust of an aerial vehicle 331h and a maximum tilt of the aerial vehicle 340h are shown to reside within their bounds at all times.

Discontinuous Embodiments

[0097] Another embodiment of the invention is to update the set-point trajectory in a discontinuous manner, where the ordinary differential equation used to integrate the position set-point is replaced by a sequence of constant set-points and logic to switch between the set-points.

[0098] FIG. 4A discloses an embodiment where a discrete solution path 410a is provided as a sequence of position set-points, whereupon the position set-point is updated 420a while controlling the vehicle 440a based on checking an error margin 430a, similar to the dynamic margin in FIG. 3A. To compute the error margin which dictates when it is safe to switch set-points, the system requires knowledge of the set-point 421a and the states of the system 441a. In one embodiment, the controlled vehicle is a quadrotor, and the states of the system 441a comprise its position and velocity. In this embodiment, the set-points are three-dimensional points in space. In other embodiments, the set-points 421a are two or one-dimensional points in space, or discrete configurations in a higher-dimensional configuration manifold.

[0099] Next, we detail how safety can be ensured when switching the set-point 420a using the robust invariant sets 230c and safe invariant set 212d computed in FIGS. 2A and 2C, respectively. We then disclose how the solution path 410a can be computed and describe this in terms of a graph containing a set of vertices ={}.sub.i=1.sup.L and a set of edges .Math. connecting some of the vertices. Each vertex in the graph is associated with a set-point in space, r.sub.k, a robust invariant set 230c to which all safe motions converge given the disturbed vehicle model, and a safe invariant set 212d that contains the robust invariant set. The nature of safe invariant set 212d is that all solutions initialized in its interior remains safe for all future times, and eventually converge to the robust invariant set 230c.

[0100] FIG. 4B explains how the graph is constructed in the discrete embodiment and the error margin used when switching set-points. A sequence of set-points is computed from the initial vertex 401b to the terminal vertex 403b, as provided by the mission information and the initial location of the system. The intermediary vertex 402b is a candidate in this solution path. It is understood that, if it exists, we have previously computed a robust invariant set 411b and a safe invariant set associated with this set-point in space given the geometry of the world, the disturbed vehicle model, and the position set-point associated with the vertex. Then, if the robust invariant set 411b associated with the vertex .sub.k 402b is contained in the safe invariant set 112b of another vertex .sub.k+1 404b, then any.sup.AT trajectory started outside the robust invariant set 411b of the vertex .sub.k 402.sub.b will, if it remains safe, converge the robust invariant set 411b if the controlled vehicle system is tracking its associated set-point. If so, the system will eventually converge to the interior of the safe invariant set 112b of another vertex .sub.k+1 404b, at which point it is safe to switch set-points.

[0101] A robust invariant set contained in the interior of the safe invariant set 112b of another vertex thus implies that a transition between the two vertices will be possible in that direction and can be used to define a directed edge in the graph. The construction of this graph will be explained shortly, and with it a solution path connecting the initial vertex 401b and the terminal vertex 403b can be computed.

[0102] Furthermore, if the controlled vehicle is steered toward a set-point associated with the intermediary vertex 402a, then as soon as it enters the safe invariant set 112b of the next vertex 404a in the path, it will be safe to switch the set-points. The distance between the current state of the vehicle and the safe invariant set of the next vertex in the path is referred to as the error margin 430a and dictates how the discrete set-points are updated in 420a. We start by describing how the graph can be constructed and searched, before detailing the switching logic and computation of the error margin.

[0103] FIG. 4C details how the graph is constructed and searched according to one embodiment of the present disclosure. A set of candidate set-points 411c are generated in the space that the vehicle is to operate in 410c. It is understood that this can be done using a lattice-based construction generating a set of candidate set-points uniformly in space, as discussed in relation to FIG. 4D, or by sampling candidate set-points randomly in space. In some embodiments, regions close to obstacles are given a higher likelihood of containing a set-point if the sampling is not uniform. The set of candidate set points {r.sub.l}.sub.l=1.sup.L are then used to compute 420c robust invariant sets 230c and safe invariant sets 212d as described previously in relation to FIGS. 2A, 2C, and 2D. The sets are then used to compute 450c a large vertex set {.sub.i}.sub.i=1.sup.L 460c containing {(r.sub.l, V.sub.min.sup.l, V.sub.max.sup.l)}.sub.l=1.sup.L. For each pair (.sub.i, .sub.j) of candidate vertices in the vertex set {.sub.i=1}.sup.L, it is checked if the robust invariant set associated with .sub.i is contained in the safe invariant set of vertex .sub.j. If this is true, a cost is evaluated 450c and associated with an edge between .sub.i and .sub.j that is added to the graph. This procedure is repeated until all possible pairs of vertices have been considered.

[0104] It is understood that the cost associated with the edges of the graph can be evaluated in many ways. In some embodiments, it is taken as a function of the positions of the set-points associated with the two vertices of the edge. This function can for example be a norm of the difference between the set-points. In another embodiment, the cost is computed as the shortest largest distance between the boundary of the robust invariant set of the vertex .sub.i and the sale invariant set of the vertex .sub.j. In yet other embodiments, the cost is taken to be a function expression of the worst-case tracking errors and control effort necessary for the disturbed system to go from the robust invariant set of the vertex .sub.i to the next robust invariant set of the vertex .sub.j. In yet other embodiments, the cost is associated with expected sensing quality in the region of space containing the safe invariant set of the vertex .sub.i, the vertex .sub.j, or both. In other embodiments, the cost is set equal in all the edges. Finally, it is understood that the cost can also be expressed using any combination of the above-mentioned methods.

[0105] Once the graph has been computed, a graph-search algorithm is used to compute 460c the optimal path connecting the starting vertex 401b with the terminal vertex 403b given the chosen cost. It is understood that many different graph-search methods can be used for this purpose, such as the Dijkstra or A-star algorithms. It is also recognized that the cost to go in the A-star algorithm can be computed in several ways, and that it can be adapted based on the cost used when constructing the edges of the graph. Once the graph-search method has computed a path 460c, this path 470c is provided as input to the safety mechanism 410a.

[0106] FIG. 4D shows a lattice-based sampling of the candidate set-points in space. A large number of set-points are sampled and associated with the candidate vertices. Candidate set-points 410d, 402d, are sampled in space 403d and the edges between the vertices are determined when construction the graph based on the relationship between the robust invariant sets 230c and safe invariant sets 212d.

[0107] FIG. 4E illustrates an implementation of the safety mechanism when the set-points are updated to produce a piece-wise constant and safe reference trajectory. The states of the controlled system are received 420e along with the discrete solution path 410e computed as in FIG. 4C. The Lyapunov function associated with the robust and safe invariant sets is evaluated in the current next set-point along the path and compared to the level associated with the safe invariant set at the next set-point in the path 430e. If the error is less than the safety limit, the set-point is updated 431e, 432e. The index of the current vertex in the solution path is returned to the computation of the error margin at the next time step 433e. Finally, the updated set-point is updated 450e and sent to the controlled system 460e.

[0108] It is understood that this method of switching ensures that the controlled vehicle always remains in a safe invariant set, thus ensuring safety, and that the controlled system will reach the robust invariant set associated with the terminal vertex in finite time, of there exists a path from the initial to the terminal vertex in the graph.

[0109] FIG. 4F illustrates one embodiment of the discontinuous safety mechanism, in which an aerial vehicle is initialized with a state in the safe invariant set associated with the initial vertex 406f in the outer corridor 402f and tasked with navigating to the robust invariant set associated with the terminal vertex 407f in the inner room 403f while avoiding the obstacles 401f shown in gray. The two-dimensional projections of the safe and robust invariant sets are shown in 404f and 405f, respectively. The resulting motions from several hundreds of different initial conditions and disturbances are shown with the black lines 408f which avoid the obstacles and reach the robust invariant set associated with the terminal vertex 407f. This is one exemplar of how the discontinuous safety mechanism can be employed for challenging indoor aerial navigation tasks with missions specifying a target location in space.

[0110] FIG. 4G illustrates how the Lyapunov function defined with a set-point associated to vertices in the optimal path of vertices remains below the levels characterizing associated safe invariant set 402g. The size of the safe invariant sets differs depending on how close the set-point is to the obstacles in the world, and the difference between the safe level and the Lyapunov function is always positive, indicating that the trajectory is safe. Many switches are done 401g, one for each vertex in the solution path of vertices, and the controlled vehicle converges to the safe invariant set of the terminal vertex, as indicated by the Lyapunov function 402g being below the level associated with the robust positive invariant sets 404g.

Implementation Examples

[0111] FIG. 5A shows an embodiment where a planner is implemented on a computing server 510a in which all computations are done. The server transmits updated set-points 540a to the client 520a, in this case the vehicle or robot, which tracks the updated set-points and transmits data 550a on its own state and in some embodiments also the environment back to the server. In some embodiments, all communication is done once prior to starting the mission, and in other embodiments, the set-points 540a are computed and refined continuously as additional information is gathered on the disturbance and the environment. This information may be gathered by the vehicle itself, or by any third-party sensor 531a, 532a, 533a. This sensor may be a physical sensor, such as a LiDAR measuring wind speeds, or a non-physical sensor, such as meteorological forecasts of wind speeds or ocean currents, depending on the vehicle considered.

[0112] FIG. 5B shows yet another embodiment of the planner implemented on a computing server 510b in which all planning computations are done. The communication is the same as in FIG. 5A, where 541b, 542b, 543b contain either the unsafe motion plan, the mission information, or the continuously updated modified set-points for each vehicle. In contrast to the previous embodiment, the vehicles 521b, 522b, 523b now contain sensory equipment whereby disturbances and environment information can be measured. As such, the communication to the planner 551b, 552b, 553b now contains additional measurements. In this embodiment, there is no need for any additional external sensing, but some embodiments combine external sensors 531a, 532a, 533a with onboard sensing 521b, 522b, 523b. In other embodiments, the planner is run physically on the vehicle as a component of its larger control system.

[0113] FIG. 6A is a schematic illustrating a computing device 600a for implementing the methods and systems/controllers of the present disclosure. The computing device 600a includes a power source 601a, a processor 603a, a memory 605a, a storage device 607a, all connected to a bus 609a. Further, a high-speed interface 611a, a low-speed interface 613a, high-speed expansion ports 615a and low speed connection ports 617a, can be connected to the bus 609a. In addition, a low-speed expansion port 619a is in connection with the bus 609a. Further, an input interface 621a can be connected via bus 609a to an external receiver 623a and an output interface 625a. A receiver 627a can be connected to an external transmitter 629a and a transmitter 631a via the bus 609a. Also connected to the bus 609a can be an external memory 633a, external sensors 635a, machine(s) 637a, and an environment 639a. Further, one or more external input/output devices 541 can be connected to the bus 609a. A network interface controller (NIC) 643a can be adapted to connect through the bus 609a to a network 645a, wherein data or other data, among other things, can be rendered on a third-party display device, third party imaging device, and/or third-party printing device outside of the computing device 600a.

[0114] The memory 605a can store instructions that are executable by the computing device 600a and any data that can be utilized by the methods and systems of the present disclosure. The memory 605a can include random access memory (RAM), read only memory (ROM), flash memory, or any other suitable memory systems. The memory 605a can be a volatile memory unit or units, and/or a non-volatile memory unit or units. The memory 605a may also be another form of computer-readable medium, such as a magnetic or optical disk.

[0115] The storage device 607a can be adapted to store supplementary data and/or software modules used by the computer device 600a. The storage device 607a can include a hard drive, an optical drive, a thumb-drive, an array of drives, or any combinations thereof. Further, the storage device 607a can contain a computer-readable medium, such as a hard disk device, an optical disk device, or a tape device, a flash memory or other similar solid-state memory device, or an array of devices, including devices in a storage area network or other configurations. Instructions can be stored in an information carrier. The instructions, when executed by one or more processing devices (for example, the processor 603a), perform one or more methods, such as those described above.

[0116] The computing device 600a can be linked through the bus 609a, optionally, to a display interface or user Interface (HMI) 647a adapted to connect the computing device 600a to a display device 649a and a keyboard 651a, wherein the display device 649a can include a computer monitor, camera, television, projector, or mobile device, among others.

[0117] The high-speed interface 611a manages bandwidth-intensive operations for the computing device 600a, while the low-speed interface 613a manages lower bandwidth-intensive operations. Such an allocation of functions is only an example. In some implementations, the high-speed interface 611a can be coupled to the memory 605a, the user interface (HMI) 648a, and to the keyboard 651a and the display 649a (e.g., through a graphics processor or accelerator), and to the high-speed expansion ports 615a, which may accept various expansion cards via the bus 609a.

[0118] In an implementation, the low-speed interface 613a is coupled to the storage device 607a and the low-speed expansion ports 617a, via the bus 609a. The low-speed expansion ports 517, which may include various communication ports (e.g., USB, Bluetooth, Ethernet, wireless Ethernet) may be coupled to the one or more input/output devices 641a. The computing device 600a may be connected to a server 653a and a rack server 655a. The computing device 600a may be implemented in several different forms. For example, the computing device 600a may be implemented as part of the rack server 655a.

[0119] The description provides exemplary embodiments only and is not intended to limit the scope, applicability, or configuration of the disclosure. Rather, the following description of the exemplary embodiments will provide those skilled in the art with an enabling description for implementing one or more exemplary embodiments. Contemplated are various changes that may be made in the function and arrangement of elements without departing from the spirit and scope of the subject matter disclosed as set forth in the appended claims. Specific details are given in the following description to provide a thorough understanding of the embodiments. However, understood by one of ordinary skill in the art can be that the embodiments may be practiced without these specific details. For example, systems, processes, and other elements in the subject matter disclosed may be shown as components in block diagram form in order not to obscure the embodiments in unnecessary detail. In other instances, well-known processes, structures, and techniques may be shown without unnecessary detail to avoid obscuring the embodiments. Further, like reference numbers and designations in the various drawings indicated like elements.

[0120] Also, individual embodiments may be described as a process which is depicted as a flowchart, a flow diagram, a data flow diagram, a structure diagram, or a block diagram. Although a flowchart may describe the operations as a sequential process, many of the operations can be performed in parallel or concurrently. In addition, the order of the operations may be re-arranged. A process may be terminated when its operations are completed but may have additional steps not discussed or included in a figure. Furthermore, not all operations in any particularly described process may occur in all embodiments. A process may correspond to a method, a function, a procedure, a subroutine, a subprogram, etc. When a process corresponds to a function, the function's termination can correspond to a return of the function to the calling function or the main function.

[0121] Furthermore, embodiments of the subject matter disclosed may be implemented, at least in part, either manually or automatically. Manual or automatic implementations may be executed, or at least assisted, using machines, 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 in a machine-readable medium. A processor(s) may perform the necessary tasks.

[0122] Further, embodiments of the present disclosure and the functional operations described in this specification can be implemented in digital electronic circuitry, in tangibly embodied computer software or firmware, in computer hardware, including the structures disclosed in this specification and their structural equivalents, or in combinations of one or more of them. Further, some embodiments of the present disclosure can be implemented as one or more computer programs, i.e., one or more modules of computer program instructions encoded on a tangible non-transitory program carrier for execution by, or to control the operation of, data processing apparatus. Further still, program instructions can be encoded on an artificially generated propagated signal, e.g., a machine-generated electrical, optical, or electromagnetic signal, which is generated to encode information for transmission to suitable receiver apparatus for execution by a data processing apparatus. 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.

[0123] A computer program (which may also be referred to or described as a program, software, a software application, a module, a software module, a script, or code) can be written in any form of programming language, including compiled or interpreted languages, or declarative or procedural languages, and it can 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.

[0124] A computer program may, but need not, correspond to a file in a file system. A program can be stored in a portion of a file that holds other programs or data, e.g., one or more scripts stored in a markup language document, in a single file dedicated to the program in question, or in multiple coordinated files, e.g., files that store one or more modules, sub programs, 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 communication network.

[0125] Computers suitable for the execution of a computer program include, by way of example, can be based on general or special purpose microprocessors or both, and any other central processing unit. Generally, a central processing unit will receive 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 performing or executing instructions and one or more memory devices for storing instructions and data. Generally, a computer will also include, or be operatively coupled to receive data from or transfer data to, or both, one or more mass storage devices for storing data, e.g., magnetic, magneto optical disks, or optical disks. However, a computer need not have such devices. Moreover, a computer can be embedded in another device, e.g., a mobile telephone, 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, e.g., a universal serial bus (USB) flash drive, to name just a few.

[0126] To provide for interaction with a user, embodiments of the subject matter described in this specification can be implemented on a computer having a display device, e.g., an LCD (liquid crystal display) monitor, for displaying information to the user and a keyboard and a pointing device, e.g., a mouse or a trackball, by which the user can provide input to the computer. Other kinds of devices can be used to provide for interaction with a user as well; for example, feedback provided to the user can be any form of sensory feedback, e.g., visual feedback, auditory feedback, or tactile feedback; and input from the user can be received in any form, including acoustic, speech, or tactile input. In addition, a computer can interact with a user by sending documents to and receiving documents from a device that is used by the user; for example, by sending web pages to a web browser on a user's client device in response to requests received from the web browser.

[0127] Embodiments of the subject matter described in this specification can be implemented in a computing system that includes a back end component, e.g., as a data server, or that includes a middleware component, e.g., an application server, or that includes a front end component, e.g., a client computer having 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 system's components can be interconnected by any form or medium of digital data communication, e.g., a communication network. Examples of communication networks include a local area network (LAN) and a wide area network (WAN), e.g., the Internet.

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

[0129] Although the present disclosure has been described with reference to certain preferred embodiments, it is to be understood that various other adaptations and modifications can be made within the spirit and scope of the present disclosure. Therefore, it is the aspect of the appended claims to cover all such variations and modifications as come within the true spirit and scope of the present disclosure.