{'fraction_non_alphanumeric': 0.04801010739102969, 'fraction_numerical': 0.012660560117919563, 'mean_word_length': 4.461903392754457, 'pattern_counts': {'":': 0, '<': 9, '<?xml version=': 0, '>': 8, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 1, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
Stochastic receding horizon control of nonlinear stochastic systems with probabilistic state constraints

The paper describes a receding horizon control design framework for continuous-time stochastic nonlinear systems subject to probabilistic state constraints. The intention is to derive solutions that are implementable in real-time on currently available mobile processors. The approach consists of decomposing the problem into designing receding horizon reference paths based on the drift component of the system dynamics, and then implementing a stochastic optimal controller to allow the system to stay close and follow the reference path. In some cases, the stochastic optimal controller can be obtained in closed form; in more general cases, pre-computed numerical solutions can be implemented in real-time without the need for on-line computation. The convergence of the closed loop system is established assuming no constraints on control inputs, and simulation results are provided to corroborate the theoretical predictions.


I. INTRODUCTION
The behavior of robotic systems can be uncertain due to a variety of reasons, including noise in sensor measurements and environmental effects. Such effects are often represented by stochastic models (for example, ocean waves [2], wind guests [3] and uneven terrain [4]). For nonlinear stochastic systems, existing methods for constrained optimal control are too computationally demanding for real-time implementation. Specifically, no real-time solution exists for continuous-time nonlinear stochastic systems with probabilistic state constraints. A receding horizon formulation partially lifts some of the computational burden associated with the nonlinear stochastic optimal control problem, but current state of the art does not allow real-time implementation on processors at the low-end of the frequency scale. This paper proposes a solution through a stochastic receding horizon formulation that is real-time implementable for nonlinear systems of modest dimension, and comes with probabilistic guarantees of convergence and state constraint satisfaction.
Within a predictive control framework, uncertainty can be accounted for by either approximating sets that bound the system's trajectories [5]- [11] or by stochastic models, with the latter having some specific advantages. In particular, while methods based on set-bounded models may result in over-conservative designs since they plan for the worst case, the use of probabilistic constraints in the methods which are based on stochastic models, on the other hand, allows for less conservatism. In addition, stochastic modelbased methods provide some flexibility by allowing one to adjust the probability that problem constraints are violated. These two qualities A portion of this work has been previously presented at International Conference on Robotics and Automation (ICRA) 2012 [1], which dealt with systems without control multiplicative term and unbounded inputs and a linear example. We extend the theory to systems with control multiplicative term. We also comment on bounded input case and explain a recovery strategy. A nonlinear example, numerical solution methods, extra results and simulations are included. enable stochastic model-based methods to offer solutions where setbounded methods may fail.
The structure of the dynamics, whenever it can be exploited, can greatly facilitate the solution of a model predictive control (MPC) problem. When the stochastic dynamics is linear, one may choose to apply a Kalman filter or its variants and solve an iterative LQG problem [12]. Alternatively, for linear stochastic systems, the optimal control problem under probabilistic constraints is tackled within a chance-constrained model predictive control framework [13]- [20]. Chance-constraint formulations are available for linear discrete time systems with Gaussian noise [18], [20]- [29].
While methods exist to enable MPC in linear stochastic systems [18], [20]- [29], for most nonlinear systems, the stochastic receding horizon optimal control problem can not be solved in real-time. For example, a particle filter implementation of chance-constrained model predictive control is available for linear systems with probabilistic noise [19], [30], and it is in principle applicable to nonlinear systems too. However, the approximate solutions obtained using this method depend on the number of particles, and convergence is achieved after a sufficiently large number of particles is used. Alternative (discrete-time) methods combine a hybrid density filter with dynamic programming [31], the latter being the natural discrete formulation of the optimal control problem. In the hybrid systems literature we find reach-avoid formulations of this problem [32], [33], in which the indicator function of hitting goal or obstacle sets appears in the cost of the optimal control problem (similarly to what is done in this paper). Computational complexity currently limits the application of these methods to systems with up to three states [33], while requirements for real-time implementation are not imposed. Invariably, computational complexity and accuracy issues surface in all discrete-time and space methods, either primarily due to the use of filters, or simply due to the resolution required in the time or state-space domains.
Time and space-discretization may be avoided if the problem is formulated in continuous space and time. Continuous-time solutions to stochastic optimal control problems are available for systems affine in control and with state independent and time invariant control transition matrix, and it is based on path integrals [34]. A path integral is essentially the solution to a Hamilton-Jacobi-Bellman (HJB) equation, obtained after the application of a particular transformation [35]. In certain cases, the path integral is computable numerically using Laplace approximations or Monte Carlo sampling. Different applications of path-integral stochastic optimal control have been explored, such as reinforcement learning [36], variable stiffness control (equivalent to automatic tuning of PD gains) [37] and risk sensitive control [38]. The main issue with path integrals is that for most nonlinear systems the solution is computationally demanding and can not be obtained in real-time on existing processors. This limits the application of path integral to real-time receding horizon control on miniature robots.
The main contribution of this paper is to synthesize a real-time design for stochastic (receding horizon) control, following an exit time [39] formulation of the stochastic optimal control problem, instead of one based on path integrals. The proposed formulation yields a time invariant control vector field, which is optimal in terms of actuation utilization. What enables real-time implementation is the fact that the field can be computed off-line and used on-line in a recursive manner. The formulation is based on a combination of deterministic planning with stochastic optimal control, where successive locally optimal stochastic controls are used to steer a system along a deterministic receding horizon reference trajectory, which is conceptually similar to Differential Dynamic Programming [40] and iterative LQG [12]. While such a two-level planning and control strategies has been used successfully in a deterministic setting [41]- [43] there is no stochastic analog yet except our own work [1]. Due to the explicit consideration of stochasticity, the proposed method offers almost sure (with probability one) guarantees of collision avoidance and convergence to a desired region, which are elusive in a deterministic setting.
The work presented in this paper is organized in the following way. Section II states the problem formally followed by an intuitive explanation of our approach in section III. Section IV explains a stochastic optimal control design, which is at the heart of our framework. Section V presents the design of the stochastic receding horizon framework and discusses the existence of solutions for our closed loop system. The convergence properties of the resulting stochastic hybrid system are established in Section VI, and the issue of input saturation is brought up. Section VII offers examples of linear and nonlinear systems, presents simulation results for the cases of unbounded and bounded inputs, and discusses computation methods for complex nonlinear stochastic systems. We conclude in Section VIII.

II. PROBLEM STATEMENT
Consider an uncertain dynamical system evolving within an open bounded region S ⊂ R n . Within S, there is a closed set O ⊂ S which represents forbidden areas (obstacles). In that sense, the system can safely evolve only in the free workspace P S \ O.
The dynamics of the system is given in the form of a stochastic differential equation (SDE) where q ∈ R n is the state, b : R n → R n is the drift term, G : R n → R m is the matrix of control vector fields, u : R n → R m is the control input, and Σ : R n → R m×m is the diffusion term. Let W = {W (t), Ft : 0 ≤ t < ∞} be an m-dimensional Wiener process on the probability space (Ω, F, P), where Ω is the sample space, F is a σ-algebra on Ω, P is the probability measure and {Ft : t ≥ 0} is the filtration (i.e. an increasing family of sub-σ-algebras of F) that is right continuous and F0 contains all P-null sets. 1 In a typical stochastic optimal control problem, one has to find a control sequence to steer the dynamics to a desired configuration, while minimizing the cost functional where the function L is the incremental cost, assumed positive definite.
For general nonlinear systems, global analytic solutions to the above stochastic optimization problem are not available. Numerical solutions can be obtained, but depending on the size of the dynamics and the constraints of the problem, the computation cost can be too high for real-time implementation on processors on the lower side of the frequency scale. This limitation motivates us to seek sub-optimal solutions to the above problem by solving the following relaxation instead.
R ε Γ Fig. 1. Illustration of the modified problem statement. Obtain the solution Γ to a deterministic optimal control problem using the drift part of the dynamics (solid thick curve), and then maintain the full stochastic dynamics (thin sample path) R-close to that reference solution with accuracy ε.
Even in this form, the problem does not lend itself to efficiently computed solutions because of the nonlinear infinite-horizon optimal control problem that needs to be solved to obtain Γ. For this reason, the solution (q * ,û * ) of the deterministic optimal control problem will be approximated by the solution of the receding horizon problem JT (q, u rh ) = min where T is the prediction horizon of the optimization, function L is the same as in (3), and Q : R n → R+ is the terminal cost which approximates the truncated tail of the integral in (3). The idea behind a receding horizon optimization strategy is that one solves the finite horizon optimal control problem and obtains a control law u rh (t) computed for z(0) =q(t0). Control law u rh (t) is applied on (2) for the time interval [t0, t1], t1 < t0 + T , during which time a new control law is computed for z(0) =q(t1), withq(t1) predicted based on (2). At time t = t1, the control law is updated and the process is repeated. It is known [45] that if Q(z) is a control Lyapunov function for (4b), and where η is a class-K function of z , then application of u rh (t) results in z → 0 asymptotically with time. We assume that Q is a control Lyapunov function for (4b) here as well, and that there exists a positive definite function η satisfying (5). In the our modified problem setting, {ui(q)} takes the place of u rh (t) andq(ti) ≡ {γi}.

III. AN INTUITIVE EXAMPLE
Consider a robot moving in a two-dimensional space, and described by single integrator dynamics perturbed by stochastic noise: where q = [x y] is the state vector, u(q) is the control input and W (t) is a two-dimensional Wiener process. The objective is to find a feedback control law u(q) to drive the system ε-close to the origin, while avoiding the boundary of a circle with radius R, centered at the origin.
An obvious control strategy is to just steer the system along a direction toward the origin. A normalized vector pointing to the origin from the current state q is − q q . To satisfy the state constraints, the system should be forced away from the circle with radius R. One way to achieve this is by weighting the control input by a factor 1 R− q . This results in It turns out, this intuitive design yields a stochastic control law which is actually optimal. In fact, (7) minimizes the cost and τ is the first time the state hits either the circle with radius ε or that with radius R. Control law (7) guarantees that the system avoids the R-radius circle boundary with probability one, and consequently hits the ε-radius circle with probability one, because it is known that it almost surely exits the domain {ε < q < R} somewhere (see [44,Lemma 7.4], and the discussion in the section that follows). Sample paths for the given controller are shown in Fig. 2 Assume now that as soon as the system hits the circle of radius ε around the origin, a coordinate transformation occurs which shifts the origin to a point within distance R from its prior location. Then the same controller can be reapplied to drive the system to a εneighborhood of the new origin. An iterative scheme based on this idea can be used to steer the system from point A to point B in a receding horizon manner. A sample trajectory resulting from an implementation of such a receding horizon controller is shown in Fig. 2 While the design of the controller (7) that enables convergence to way-points is simple for the case of the stochastic single integrator of (6), is not the case for general stochastic nonlinear systems. In following sections, we outline a mathematical framework that allows the computation of receding horizon controllers for more complex stochastic nonlinear systems.

IV. STOCHASTIC OPTIMAL CONTROL WITH EXIT CONSTRAINTS
In this section we design stochastic optimal controllers with exit constraints. These controllers guarantee convergence to a given set, and satisfaction of state constraint, both with probability one. Consider the stochastic system (1) which evolves within a bounded domain D ⊆ P with a C 2 boundary ∂D and closure denoted D. Assume that b(q), G(q), Σ(q), and Σ −1 (q) are bounded and Lipschitz continuous on D. The objective is to find the control u(q) that yields where a(q) = Σ(q) Σ (q). We impose an admissibility condition that there exist a set of control inputs u q ∈ U q such that for all initial conditions q and control inputs u q , the cost V (q, τD) < ∞.
The HJB equation associated with (8) is where A the second-order partial differential operator Equation (9) is written in matrix form as follows where tr stands for trace. The optimal control law u * ∈ U q that solves (9) is then given as Substituting (10) in (9) yields Using the logarithmic transformation [35] V (q, t) = − log g(q) , and with substitution in (11) we get − ∂tg(q, t) = −l(q, t) g(q, t) + ∂qg (q, t) b(q) Analytic solutions of the above partial differential equation (PDE) are generally not possible for complex nonlinear systems. However, the Feynman-Kac formula [44] relates a certain PDE with an equivalent SDE, and facilitates the numerical solution of the PDE through numerical simulation of the SDE. Using the Feynman-Kac formula [44], the solution of (12) takes the form where ζ(t) is the Markov process evolving on the same bounded open set D ⊂ R n .
Stochastic Optimal Control with Exit Constraints: Under the assumption min Lemma 7.4]. This means that the system will escape the domain D in finite time with probability one. The assumption that Σ and Σ −1 are bounded, ensures satisfaction of (15).
A guarantee that the system does not exit from a specific portion of the boundary can be obtained by imposing an infinite penalty for touching that surface. Consider a partition of the boundary ∂D in the form N ⊂ ∂D; M = ∂D \ N . Then choose Φ as Φ = +∞ · XM; and Assuming that l(q, t) ≡ 0 and letting t → ∞, the resulting parabolic PDE (12) gives rise to the Dirichlet problem Then (13) suggests that g(q) is in fact the probability that the sample path of (14) from q hits boundary N before M. Function g(q) takes the form and ζ(t) is the Markov process (14). Now if the admissibility condition is satisfied then the optimal control with infinite penalty on exit boundary is equivalent to a constraint (see [39]), Remark 1: The computation of control input (10) requires g(q), which can be found by either by solving (12) analytically, or numerically simulating (14) and computing (17). As Φ imposes an infinite penalty on state trajectories that exit through M, the above construction forces the system to exit through N while avoiding M with probability one. The problem of stochastic optimal control with terminal cost at exit time is discussed in [35], while a specific problem of exit constraints was discussed in [39]. The latter reference also shows that imposing an exit constraint is equivalent to having infinite penalty on exit location used in this section. We use these two results and thus by defining M to be the boundary of state constraint regions, we achieve the guarantees that state constraints are satisfied, and convergence to a desired region is achieved in finite time.

V. STOCHASTIC RECEDING HORIZON CONTROL DESIGN
After the presentation of the continuous-time constrained stochastic optimal control formulation in its general setting, we proceed with the description of the implementation of these techniques inside the receding horizon framework that was outlined in the example of Section III. Out of this process emerges a simple, special case of a gereral stochastic hybrid system (GSHS), for which the existence of solutions has been established in literature [46]. The section concludes with an examination of the closed loop stability and convergence properties of this simplified GSHS, and a discussion on how input saturation affects these properties.

A. Deterministic Planning
We begin by computing a receding horizon path using (4b) Letq * T (t) : [t0, t0 + T ] → R n be the trajectory that, for a prediction horizon T , minimizes the cost functional with functions L and Q as in (4). Define a receding horizon path as Here we adopt the approach of [47] to obtain an approximation ofq * T and consequently compute ΓT . The latter, however, can also be obtained through an array of alternative methodologies, including potential field methods [48], rapidly exploring random trees RRTs [49], or cell decomposition methods [50].

B. Way-point Generation
Let the closed ball of radius ε centered at a point γ is denoted Bγ(ε) {q : q − γ ≤ ε}, and its complement, B c γ (ε). Now consider a sequence of points {γi} N i=0 ∈ ΓT with γ0 := q(t0) and γN :=q * T (t0 + T ), satisfying where γ is the positive definite function in (5). Define domains Di, for i = 1, . . . , N , such that i Di ∩ O = ∅ and Decompose the boundaries of those domains as follows (see Fig. 3): Mi ∂Di \ Ni The domains Di are defined such that Ni is non-empty for all i.

C. Stochastic optimal controllers
The system state is a Markov process q(t) that evolves between way-points according to the SDE where Σ(q), b(q), G(q), Σ −1 (q) satisfy the requirements of Section IV, and together with ui, are all bounded in Di. The latter is the control input responsible for taking the state from Ni−1 to Ni while avoiding Mi. Let ti−1 be the first time instant when q(t) ∈ Ni−1.
When (23) under ui hits Ni at some time ti, it undergoes a forced transition with ui switching to ui+1, and the switch occurs upon the state hitting a part of the boundary Ni. Control law ui gives a solution to the stochastic optimal control problem +Φ q(ti) q(ti−1) = q =: V (q) (24) Notice that by setting now the terminal time to ti allows the value function V to be time-invariant. We define the exit time for the process driven by ui to be τi = ti − ti−1. Function Φ is again chosen in a way that it imposes infinite on the state hitting Mi.
Condition (26) translates into the process q(t) exiting Di in finite time with probability one which is guaranteed by assumption (15). Condition (27) is equivalent to saying that the process q(t) reaches an ε-neighborhood of way-point γi with probability one, before violating any state constraints (see [39]).
Given a receding horizon path ΓT seeded with a sequence of waypoints {γi} N i=0 , the process of transitioning from way-point γi−1 to way-point γi under (25) is repeated. By the time a new way-point is reached, the path ΓT has been recomputed in a receding horizon manner, and the way-point sequence {γi} N i=0 redefined with the initial element γ0 being the way-point just reached. What is important for real-time implementation is that for predetermined domains Di, (25) can be precomputed off-line, numerically in general but also analytically in special cases where b, G and Σ are such that the boundary value problem for PDE (12) can be solved explicitly.

D. The Resulting Stochastic Hybrid System
Closing the loop around (23) by means of a receding horizon strategy gives rise to a switched stochastic hybrid system, where switching is due to ui and occurs as a forced transition whenever q(t) hits a set Ni. The hybrid state here is just (i, q) where q ∈ R n and i ∈ {0, 1, 2, ..., N } =: I are the continuous and discrete states, respectively. This system can be classified as a GSHS, a general modeling framework of which is described in [46]; however, it is a very simplified version of the the general definition of [46], which can be adequately described by defining only the following three components: the continuous dynamics, the discrete dynamics, and the reset condition.
Continuous Dynamics: The continuous state q(t) evolves according to the SDE (23) where we have just replaced ui q(t) with u i, q(t)) to emphasize the explicit dependence of the control input on the discrete state i, making it a function of the hybrid state (i, q): u : I ×R n → R m . The drift b and diffusion Σ terms, along with G, are assumed independent of i. When in discrete state i, the domain of the continuous variable q(t) is Di.
Discrete Dynamics: The (single) discrete state i evolves by means of state-triggered forced transitions, which occur each time the continuous state q hits a guard. In this case the guard is a function from i to R n , sending i → Ni. The time at which the transition is triggered is called stopping time and it is the first time instant ti inf{t > ti−1 | q(t) / ∈ Di}. Then the discrete state changes according to the following-in fact, deterministic-rule: Note that due to the set of discrete states being finite, and the discrete transition map being a bijection, there can only be a finite number of discrete transitions and the system cannot exhibit Zeno behavior.
Reset Condition: During discrete transitions, continuous states are not reset. Essentially, the reset map for the continuous states is simply the identity.
The solution of (28) over i = 1, . . . , N , is a collection of Markov processes truncated at (their) exit time, which can be represented as a Markov string. A Markov string is a hybrid state jump Markov process [46]. Given the existence of solutions for each SDE (23) for fixed i (see [39] for details), and due to the finiteness of the set of discrete states, the solutions for the closed loop stochastic hybrid system are well defined [46].

VI. CONVERGENCE AND STABILITY PROPERTIES
This section presents a proposition that establishes the finite-time convergence properties of the closed loop system to a neighborhood of the origin.
Proposition 1: Consider the switched stochastic system (23) in an open bounded domain S ⊂ R n , where i ∈ I is the switching index, and W (t) is a Wiener process. Let Q(q) be a C 2 , positive definite function in the closure of a bounded domain S which contains the origin. If for every solution q(t) of the stochastic switched system there exist (i) bounded domains Di that satisfy (20)- (22), and (ii) a class-K function η on S together with a sequence of points {γi} N i=0 ∈ S satisfying (19), then the closed-loop switched stochastic system (23)- (25) converges to an ε-neighborhood of origin in finite time.
Proof: It is known [45] that a receding horizon strategy u rh (t) applied on (4) yields a trajectoryq * (t) satisfying limt→∞q * (t) → 0. Hence, with sufficiently large T < ∞, one can find a path ΓT such that ΓT ∩ B0(ε) = ∅. Moreover, condition (5) ensures that for any q(t0) ∈ S, the system will remain within an open bounded set containing the level set of q(t0). This means that for a sufficiently large T , the path ΓT intersects an ε-neighborhood of the origin and remains bounded. Given that this set is bounded, one can only cover it with a finite number of non-overlapping balls with radius ε > 0. Hence, for sufficiently large T < ∞, there is a finite number of waypoints N that satisfy condition (19) with γN at the origin. Then, by induction it is shown in a straightforward way that the system reaches an ε-neighborhood of the origin in finite time.
To this end, set q(t0) = γ0, construct a path ΓT of finite length according to (4), and select a way-point γ1 according to (19). Given that bounded domain D1 satisfies (20)- (22), the application of control law (25) ensures that for all q(t0) ∈ N0, P{q(t1) ∈ N1 | q(t0)} = 1, that is, the state at time t1 is in N1 almost surely (see Section IV and [39]). Condition (15) ensures that the time that this happens is finite. Now, let us assume that a controller u k (q) was applied iteratively, and at some time t k , state q(t k ) ∈ N k . As N k ⊂ D k+1 and given (20), there exists a controller u k+1 (q) to steer the state to the next way-point γ k+1 . Given now that D k+1 also satisfies (20)- (22), the law (25) gives Inductively, since NN := ∂DN ∩ B0(ε), the proof is completed.

A. Convergence under bounded inputs
The control law ui(q) = −a(q) G (q) ∂q{− log g(q)} may require large inputs near the boundary Mi, since g(q) → 0 there. This can be problematic from an implementation standpoint. When these inputs saturate at some u(q) max, the control law that is practically implemented is rather approximated smoothly by The problem is that bounded inputs cannot force exit at Ni with probability one. The probability of success in exiting when bounded inputs are applied can be computed [51], but there there is always a nonzero probability that the system will exit from Mi instead of Ni. Neither convergence to origin nor constraint satisfaction can be guaranteed almost surely.
To recover convergence under bounded inputs, we propose a recovery strategy that uses repeatedly a controller precomputed offline, which steers the system back inside the domain Di. The receding horizon control can be re-initiated after the state is re-enters Di. This recovery controller is not different from (25), and its use is illustrated in an example in Section VII. In the absence of obstacles, and with infinitely large outer domain, the guarantee of convergence can thus be recovered even with bounded inputs.

VII. EXAMPLES
We present two different examples to demonstrate application of our control design. In the first example the stochastic optimal control law can be computed explicitly, and simulation results are presented to demonstrate its function. The effect of input saturation is also investigated. The second example involves a nonlinear system, where the stochastic optimal control laws can not be computed explicitly. There, we show how the application of the Feynman-Kac formula offers numerical controller designs, and we present the results through representative plots.

A. The Stochastic Single Integrator
Problem formulation: Consider the system (23) with the drift term b(q) ≡ 0 and G(q) is identity. This simple drift-less system can be described as a two-dimensional single integrator with stochastic uncertainty as dq(t) = ui(q(t)) dt + Σ(q(t)) dW (t); q(0) = q0 (29) where q = [x y] is the state and W (t) is a 2-dimensional Wiener process. The objective is to find control inputs ui(q(t)) to drive the system to origin, using minimal inputs, avoiding obstacles, and moving along paths of minimal length to its destination. Here the system's workspace is a ball of radius ρ0, containing M spherical obstacles with radii ρj and centers qj, j = 1, 2, . . . , M .
Deterministic Path Planning: The first step is to find a reference trajectory for (29) ignoring noise. The nominal dynamics is jusṫ q = u(q(t)). We use the approach of [47] (other methods are also possible) to find a continuous trajectory minimizing a finite-horizon cost where T is the prediction horizon and c1 and c2 are arbitrary positive constants. The terminal cost Q(q(T )) is selected as a navigation function [52] defined as where k ∈ N + is a sufficiently large positive integer. In (30), the function β : P → [0, ∞) encodes the location and size of obstacles and is expressed as β M j=0 βj with β0 ρ 2 0 − q 2 and βj q − qj 2 − ρ 2 j , for j = 1, . . . , M . Assume that the outcome of this procedure is an obstacle-free continuous state trajectoryq * (t) ∈ P, and the resulting path is Way-point Generation: There exist control way-points {γi} N i=0 ∈ Γ, such that γ0 =q(t0), and γN =q * (T ). Define the sets Bγ i (ε) {q ∈ P : q − γi ≤ ε} and denote their boundary ∂Bγ i (ε). The waypoints we select are chosen to satisfy the following constraint: where ε and Ri are positive constants. The above constraints also help determine the radius Ri, which is the outer radius of the domain of the continuous state Di. There is no unique solution for Ri and one can specify an upper and lower bounds on Ri.
The local domains Di are now defined as where Ni = ∂Bγ i (ε) and Mi = ∂Bγ i (Ri). Conditions (38)- (39) imply that Stochastic optimal controller: The control input ui(q(ti)) for (29) is constructed as shown in Section IV. It achieves The optimal control law is where a(q) = Σ(q)Σ (q), V (q) = − log g(q), and g(q) is the solution of the PDE 1 2 Function g(q) has an analytic expression: which suggests a value function and a control law of the form Control input ui(q) switches to ui+1(q) upon hitting the boundary Ni for i = 1, 2, . . . until the state is in ε-neighborhood of the goal.
Problem instantiation and simulation results: Simulations were performed (taking q ∈ R 2 ) with the overall bounded domain being S = {q ∈ R 2 | q < 10}. The initial condition is q0 = [x, y] = [−3.0, −3.0] . The goal is to drive the system to the origin. The workspace contains two obstacles of radius 0.2 at coordinates [−3.0, −1.0] and [−2.0, −2.0] . Matrix Σ(q) is the 2 × 2 identity, and Ri is chosen to satisfy γi−1 − γi < Ri − 2ε and min{ γi − z , z ∈ O} > Ri with ε = 0.1. A navigation function Q(q) is constructed on R 2 and a trajectory forq =û(q(t)) is generated based on [47]. The simulation of the complete algorithm is shown in the Fig. 4. The navigation function is depicted in the form of a contour plot, while the discrete way-points are center of filled (red) circles. The boundaries Mi are chosen based on (38)- (33) and are marked in the figure by dotted black circles. The effect of input saturation: The following controller is a saturated version of (34): Figure 5 shows a sample path for the bounded input case, and quantifies the norm of the inputs used. As discussed earlier, bounded inputs (35) will not result in success with probability one (i.e. the probability of first hitting ∂Bγ i (ε)) and the probability of success for each local controller can be computed according to [51]. Figure 6 represents the probability of hitting the goal boundary ∂Bγ i (ε), before exiting the domain elsewhere for any given initial condition. It can be seen that there is always a nonzero probability that the system exits from ∂Bγ i (Ri) instead of ∂Bγ i (ε) under bounded inputs, and this probability becomes higher for initial conditions closer to ∂Bγ i (Ri).  6. The probability of first hitting the goal boundary for the system (6) using bounded input (35) with |u(q)|max = 5. The probability of reaching the desired boundary for each local controller can be computed according to [51].
To recover convergence under bounded inputs, we implement the recovery strategy. The implementation is shown in Fig. 7. We observe that the probability of convergence with recovery strategy can be one in absence of obstacles and sufficiently (infinitely) large outer boundary. In the presence of obstacles, the computation of the probability of convergence can only be approximated by a numerical estimation for finite way-points. 3

B. A Nonlinear System
Finding a solution to the PDE (16) is central to the proposed control design. In Section VII-A, such a solution can be obtained explicitly, but with (16) having varying coefficients, this is not true in general.
In this section we demonstrate a solution approach that is based on the Feynman-Kac formula.
Problem formulation: Consider a mobile robot with three omnidirectional wheels (Fig. 8). In Fig. 8, x, y mark the position, with respect an inertial X-Y frame, of the local, body-fixed frame Xm-Ym. The orientation of the local frame with respect to X-Y is given by angle θ. The dynamical system modeling the robot has as state the vector q = [x, y, θ] . The input to the system is a vector u = [U1, U2, U3] of the linear velocities of the three wheels, denoted U1, U2, U3, respectively. Stochastic noise affects all three coordinates x, y and θ. The equations of motion for such a system can be represented by the following SDE Remark 2: Formally, q = [x, y, θ] belongs in the twodimensional special Euclidean group SE(2); it can, however, be embedded in R 4 [50], where the usual metrics can be used. Here, the metric [x1, y1, θ1] = x 2 1 + y 2 1 + (cos θ1 − 1) 2 + (sin θ1) 2 (see [50]) is used.
The goal is to a find control law Ui q(t) to drive (36) to the origin x = y = θ = 0, using inputs of minimal magnitude, following paths of minimal length, and avoiding obstacles along the way. The robot's workspace is a torus, containing a finite number M of torusshaped obstacles at locations qj, j = 1, 2, . . . , M . The robot's outer workspace boundary, and those of the obstacles for i = 1, . . . , M are is defined as Matching (36) to (23) we identify the different terms as follows: Deterministic Path Planning: Using the metric introduces in Remark 2, and the definition of obstacle and outer boundary in (37), we apply the path planning approach of Section VII-A, selecting a fixed R satisfying infz∈O,t>0 q(t) − z > R > 2ε > 0.
Way-point Generation: Here we will select a sequence {γi} N i=0 ∈ ΓT , of waypoints. The objective of stochastic controller for each discrete state i is to make (36) converge ε > 0 close to way-point γi.
Stochastic optimal controller: The PDE (16) is now written as where L is an operator on functions defined as L(·) = 1 2 tr ∂qq( · ) G(q) Σ(q) Σ (q) G (q) . Equation (40) does not admit analytic solutions. Common applicable numerical methods such as finite differences and finite elements have difficulty producing acceptable solutions for instances of problems with dimension larger than three and complex boundary conditions. Alternatively, the Feynman-Kac's formula (see Section IV), relates the PDE to an SDE: which is essentially the unforced system (36). Then, we know that the function g(q) satisfies g(q) = P ξt(ti) ∈ Ni|ξt = q (42) where ti is the first exit time from the domain Di.
Problem instantiation and simulation results: The probability in (42) can be estimated numerically 4 by simulating sufficiently many sample paths of (41) with different initial conditions q. We produce these sample paths using the Euler-Maruyama method [53]. Using the same method, we also obtain sample paths for (36). A 41 × 41 × 41 grid is imposed on the state space, and treating each node as an initial condition, we produce 500 sample paths and estimate (42). With the estimate of (42), the control law is computed numerically as u * i (q) = −Σ(q) Σ (q) G (q) ∂q − log(g(q)) . Figure 9 presents two numerical approximations of g(q) in the form of 2D colormaps with robot orientation set at 0 and π 2 radians, respectively. Equipped with such a map, a numerical gradient can be used to calculate the control input. Figure 10 shows a single sample path for the closed loop version of (36). The time history of individual states x, y and θ are shown in Figs. 11(a)-11(c), indicating the convergence to an ε neighborhood of the origin. Figure 11(d) plots the norm of the control inputs used. Numerical data confirmed that the probability that the closed loop system hits every desired goal boundary ∂Ni is one.

VIII. CONCLUSIONS
The proposed method allows the design of a receding horizon navigation controller for nonlinear systems governed by stochastic differential equations. If a feasible path, optimal or otherwise, is available in the form of a finite sequence of way-points, then an an optimal control law can be found to steer the stochastic system between these way-points, while keeping it close to the path and away from unsafe regions with probability one. In cases where control inputs are forced within upper and lower bounds, and state constraints (obstacles) are imposed, almost-sure convergence and safety is impossible, but it can be achieved with some probability which depends on how severe the input bounds are compared with respect to the magnitude of subjected noise. For nonlinear systems with dynamics not permitting analytic solutions for the resulting PDEs, numerical solutions for dimensions up to 5 or 6 are shown to be well within the reach of currently available computing platforms.