Planning Persistent Monitoring Trajectories in Spatiotemporal Fields using Kinodynamic RRT*

Transcription

1 Planning Persistent Monitoring Trajectories in Spatiotemporal Fields using Kinodynamic RRT* Xiaodong Lan, Cristian-Ioan Vasile, and Mac Schwager Abstract This paper proposes a sampling based kinodynamic planning technique for planning persistent monitoring trajectories for a sensing robot in a spatiotemporal environmental field. The robot uses a Kalman-Bucy filter to estimate the spatiotemporal field. Since the error covariance matrix of the Kalman-Bucy filter evolves according to the nonlinear Riccati differential equation, this requires planning a trajectory under a difficult differential constraint. The paper adapts kinodynamic RRT* to deal with this differential constraint. By properly choosing the state space, the sampling space, the stopping criterion, and a approximate procedure for the optimal steering of RRT*, the proposed algorithm finds trajectory for the robot that tries to minimize a metric on the error covariance matrix of the field estimate. Numerical simulations and experimental trials with an m3pi robot demonstrate the effectiveness of the algorithm. I. INTRODUCTION In this paper we consider the problem of planning a trajectory for a robotic sensing agent to estimate a dynamic spatiotemporal field. We introduce a sampling based trajectory planning algorithm inspired by Kinodynamic RRT* [1], [2], where the objective is to find a trajectory which tries to minimize the expected error variance in an estimate of the spatiotemporal field. Consider, for example, an aerial vehicle equipped with an infra-red camera flying over a lake to measure the surface temperature of the water. We want the aerial vehicle to fly over the areas where the uncertainty about the water temperature is greatest, such that by the end of the flight the estimate of the temperature field has lower uncertainty as little uncertainty as possible. In many problems relevant to Earth and Environmental sciences, it is crucial to estimate spatiotemporal scalar fields of many types, for example, the density and type of vegetation coverage in a forest, the ice coverage over northern or southern seas, the concentration of a particular pollutant over a lake, and other similar spatiotemporal quantities. In an urban setting, we may want to know the density of traffic over a city, or the density of criminal activity, or commercial activity. In a disaster relief setting, for example, we may want to know the radiation levels or concentration of a toxic chemical over a disaster site, the density of survivors trapped in rubble, or the likelihood of collapse of various parts of an unstable This work was supported in part by ONR grant N We are grateful for this support. X. Lan is with the Department of Mechanical Engineering, Boston University, Boston, MA 22, USA, xlan@bu.edu. C.-I. Vasile is with the Division of Systems Engineering, Boston University, Boston, MA 22, USA, cvasile@bu.edu. M. Schwager is with the Department and Mechanical Engineering and the Division of Systems Engineering, Boston University, Boston, MA, 22, USA, schwager@bu.edu. structure. All of these applications require us to plan a trajectory for a sensing agent to follow in order to estimate the time-changing spatial field of interest. This paper is related to the recent work on persistent monitoring and sampling based kinodynamic planning. Persistent monitoring considers collecting information about a spatiotemporal field of interest over a long time horizon. Because the field is time-varying, the sensing robot must repeatedly visit areas of the environment in order to maintain a good estimate of the field. In this paper, we regard persistent monitoring as a long-term task, such that planning is performed over a finite time horizon much longer than the time-scale of the environment. The infinite horizon case is treated in [3], which proposes a sampling-based algorithm called Rapidlyexploring Random Cycles (RRC) to plan periodic trajectories for sensing agents to do persistent monitoring. Similar in spirit to our work, but using a deterministic field model, [4] uses linear optimization techniques to optimize the speed of a sensing robot along a fixed closed trajectory. Also using a deterministic field model, the approach in [] employs optimal control techniques and infinitesimal perturbation analysis to find trajectories for a sensing vehicle to minimize an uncertainty metric in a 1-D mission space. In [6], we use the matrix version of the Pontryagin s minimum principle to give a set of necessary conditions the optimal trajectory has to satisfy in order to minimize the trace of the error covariance. Work in [7] proposes a topology-guided search strategy to discovery the topological features of a priori unknown differentiable random fields. The work in [13] uses sampling-based planning algorithms to gather information, assuming a known, static information landscape, which is quite different from our time-varying field evolving under the nonlinear Riccati differential equation as dictated by the Kalman-Bucy filter. Our work also uses tools from Randomized kinodynamic planning, which employs sampling-based techniques to find trajectories for vehicles with differential constraints. In [8], the authors propose an algorithm called kinodynamic RRT, which can return a feasible trajectory satisfying the differential constraints, but lacks optimality guarantees. To deal with this, the authors in [2] propose one variant called kinodynamic RRT*, which can guarantee almost-sure convergence to optimal trajectories given a cost metric. In order to guarantee optimality, RRT* requires that two states must be connected exactly by an optimal trajectory. This requirement implies that the kinodynamic system must be controllable. To get this optimal trajectory connecting two states, we usually need to solve a two point boundary value problem (BVP).

2 4 High value Low value 4 High value Low value 4 High value Low value 4 High value Low value For most dynamical systems, this is very difficult and no analytical solutions exist [9]. But for some special systems and special cost metric, closed-form solutions do exist. For example, in [], the authors use LQR control to find the optimal trajectory between two nodes in the random tree. In [11], the authors consider a special cost function which penalizes both the duration of the trajectory and the expended control effort, so an analytical solution exists to optimally steer the system from one state to another. In these special cases, since we can connect two states optimally, kinodynamic RRT* can return an asymptotically optimal trajectory between the initial state and the goal state. In general cases, to avoid solving the two point boundary value problem, approximate methods are often used to find an approximate trajectory between two states. This approximation will sacrifice the optimality, but by choosing the approximation properly, it will be computationally efficient and return a suboptimal trajectory very quickly. For example, the shooting method and the piecewise-constant input method are used in [12] to return an approximate trajectory between two nodes in the random tree. In this paper we first construct a basis function representation of the spatiotemporal field. Then we use the Kalman- Bucy filter to estimate the field. This filter has a covariance matrix that evolves according to the Riccati differential equation. The robot trajectory is planned in order to control the evolution of this Riccati differential equation so that at the end of the time horizon the variance on the estimate of the spatiotemporal field can be minimized. Our previous work [6] gives a set of necessary conditions the optimal trajectory has to satisfy in oder to minimize the estimation variance. But as discussed in [6], based on these necessary conditions, we have to solve a boundary value problem to get the optimal trajectory, which is known to be very difficult [9]. Also, the work in [6] does not consider the presence of obstacles. This paper uses kinodynamic RRT* to deal with these problems. One reason to use this method is that it can check obstacle collisions effectively. The second reason is that it can return a feasible and quite good trajectory very quickly, at least better than some benchmark trajectories, see Section IV. Besides, if we can find an analytical optimal solution to the Steer procedure, kinodynamic RRT* can return the asymptotically optimal trajectories. But because of the nonlinear Riccati differential constraint, finding a closed-form solution to the Steer procedure is intractable. To circumvent this, we propose an approximation method to the Steer procedure. Specifically, we consider three motion primitives to connect two states and choose the one which causes the lowest cost between the two states. To adapt kinodynamic RRT* to our persistent monitoring setting, we propose some extensions to the classical kinodynamic RRT* procedures, including extended state space, task space sampling and repropagation. By our algorithm, we can find a trajectory which outperforms a benchmark trajectory: the TSP tour. Finding the optimal trajectories to estimate a spatiotemporal field still remains an open question. The rest of this paper is organized as follows. The envi- Fig. 1. Example of a dynamic spatio-temporal field φ(q, t) as modeled in this paper. The figures show snapshots of the field at times increasing from left to right. ronment model, robot model, and trajectory cost metric is described in Sec. II. The algorithm is introduced in Sec. III. Results of simulations and experiments are given in Sec. IV, and we give our conclusions in Sec. V. II. PROBLEM FORMULATION A. Spatiotemporal Field and Robot Model We seek to estimate the value of a spatially and temporally changing field over a continuous spatial domain Q R 2. Let the field value at a point q Q and a time t R be given by the function φ(q, t). This could represent the surface temperature in a patch of the ocean, the density of traffic over a city, etc. Furthermore, we suppose that this field is analytic, so that it can be written as a series of basis functions and coefficients, φ(q, t) = C(q)x(t) where C(q) = [c 1 (q) c n (q)] is a vector of spatial basis functions c i (q), and x(t) = [x 1 (t) x n (t)] T is a vector of time-varying weights, x i (t). This is a common way to represent environmental fields [14], [], [16]. Although this looks limiting, any physical field will have an approximate decomposition of this sort. For example, one could take a 2D Fourier expansion of the field, so that C(q) is the vector of 2D Fourier bases, and x(t) is the vector of time-varying Fourier coefficients, or one could take a Taylor expansion, so that C(q) are the 2D polynomials and x(t) is the vector of time-varying Taylor coefficients. We assume that we know the basis functions, but not the weights, as would typically be the case. In this paper we choose Gaussian radial basis functions as the bases, so that c i (q) = Ke q qi 2 /2σ 2 c, where q i is the center position of the basis function. Fig. 1 shows an example of a spatiotemporal field modeled in this way. In oder to model the time-varying nature of the field, we let the unknown weights vary according to the linear stochastic dynamics ẋ(t) = Ax(t) + w(t), (1) where A is an n n matrix and w(t) is a Gaussian white noise process. We assume that A is known, and that the covariance matrix Q on the white noise process w(t) is known. Furthermore, we suppose that we have an initial guess that the field weights are Gaussian distributed with mean x(t ) and covariance matrix Σ at the initial time t. A sensing robot carries a point sensor, moves along a path in the environment and takes noisy measurements of the field. We assume that the sensor can only take a continuous, noisy measurement of the field s value at the robot s location. y(t) = φ ( p(t), t ) + v(t) = C ( p(t) ) x(t) + v(t)

3 where p(t) is the position of the robot, v(t) is a Gaussian white noise process, uncorrelated with w(t), and with known variance R. This sensor may be, for example, a temperature probe, a chemical concentration sensor, or any other point sensor appropriate for the field. Our goal is to drive the sensing robot to estimate φ(q, t) using the sensing signal y(t), while minimizing some measure of the estimation uncertainty. B. Trajectory Quality In order to measure the estimation performance from moving along a trajectory, an estimation performance metric must be defined. We choose the Kalman-Bucy filter for estimation which is known to be the optimal estimator for this model in the sense that it minimizes the mean squared estimation error [17]. Denote the estimate of x(t) by ˆx(t) and the estimation error of x(t) by e x (t) = ˆx(t) x(t). Denote the error covariance matrix by Σ(t) = E[e x (t)e x (t) T ]. From the Kalman-Bucy filter, we know that the error covariance evolves according to the matrix Riccati differential equation, Σ(t) = AΣ(t) + Σ(t)A T Σ(t)C(p(t)) T R 1 C(p(t))Σ(t) + Q. (2) We choose the spectral radius (i.e. the largest eigenvalue) of the error covariance matrix, ρ(σ(t)), as the objective function. The spectral radius is an indication of the worst estimation error variance over the whole domain of interest. One can also choose the trace or the determinant of the covariance matrix as the objective function. Our goal is to design a trajectory for the agent, σ = {p(t) t t t f }, such that when it moves along this trajectory the spectral radius of the error covariance matrix at the final time t f will be minimized. Define the cost of a trajectory starting at t 1 and ending at t 2 (t 2 t 1 ) as J(σ) = ρ(σ σ (t 2 )) ρ(σ σ (t 1 )), (3) where σ denotes a trajectory in time horizon [t 1, t 2 ], Σ σ (t 2 ) and Σ σ (t 1 ) are the error covariance matrices attained at the final time t 2 and initial time t 1 along the trajectory σ, respectively. We stress that our goal is to control the evolution of the error covariance matrix dynamics (2) to minimize the estimation uncertainties in space and time. Therefore (2) is the nonlinear kinodynamic constraint for our system, and this is why we must use kinodynamic RRT*. The dynamics of the robot itself are inconsequential, as they do not effect the evolution of the covariance matrix. More accurately, one can view the robot position p(t) as the control input for our dynamical system (2). Therefore, so as not to unnecessarily complicate the problem, we represent the robot s motion as a simple integrator, ṗ(t) = u(t), u(t) u max, (4) where u(t) is the robot s control input, and p is the initial position of the robot. Next we describe the optimal kinodynamic persistent monitoring problem. By (4) and (2), the dynamical system can be described as follows: ṗ(t) = u(t), u(t) u max, p(t ) = p (a) Σ(t) = AΣ(t) + Σ(t)A T Σ(t)C(p(t)) T R 1 C(p(t))Σ(t) + Q, Σ(t ) = Σ (b) n(n+1) 2+ where (p(t), Σ(t)) X and u(t) U, X R 2 and U R 2 are the state space and the control input space, respectively. Denote any state in the state space by z(t) = (p(t), Σ(t)) X. Denote the obstacle region and the obstacle-free region in the state space by X obs and X free := X \ X obs, respectively. A feasible trajectory in the state space is σ : [t, t f ] X free, that is, σ(t) X free for t [t, t f ]. Denote by X all the feasible trajectories in X free. We now give a formal statement of the optimal kinodynamic persistent monitoring problem (PMP). Problem 1 (Optimal Kinodynamic PMP): Given a state space X, an obstacle region X obs, an initial position p and initial covariance matrix Σ, a dynamical system () with cost J(σ) given by (3), find a control u(t) U in time [t, t f ] such that the unique feasible trajectory σ X that satisfies Equation () for all t [t, t f ], avoids obstacles, that is, σ(t) X free, t [t, t f ], minimizes the cost functional J(σ) = ρ(σ σ (t f )) ρ(σ σ (t )). In Problem 1, we only consider feasible persistent monitoring problems. That is, the covariance matrix will not grow unbounded. As discussed in [18], as long as (A, C(p(t))) is detectable, Σ(t) will be bounded. III. KINODYNAMIC RRT* FOR PERSISTENT MONITORING The RRT* [1] algorithm is an extension of RRT algorithm [8], which guarantees asymptotic optimality. Before we describe our algorithm, we briefly introduce the primitive procedures used in RRT*. SampleFree returns uniformly distributed samples from X free. In Fig. 3, it returns z rand. Given a graph G = (V, E) and a state z, Nearest(G, z) returns a vertex in V from which z can be reached with the lowest cost. In Fig. 3, it returns z nearest. Given two states z and z, the Steer(z, z ) procedure returns the locally optimal feasible trajectory σ z,z starting from z and reaching z in the presence of obstacles. Near(G, z) procedure returns a set of vertices in V from which z can be reached with a bounded cost. Given a trajectory σ, the ObstacleFree(σ) procedure returns true if and only if σ lies entirely in the obstacle-free state space X free. A. Extensions to Kinodynamic RRT* This subsection proposes some extensions to the RRT* algorithm such that it can be used to plan trajectories for persistent monitoring problems.

4 Fig. 2. The figure illustrates that in task space sampling, the covariance matrix depends on the trajectory. Although the robot ends up in the same location by two different trajectories (left), the resulting covariance matrix will be different for each of these two trajectories (right). This is accounted for with a repropagation step in our algorithm. 1) Extended State Space: In (), the system state is n(n+1) 2+ (p(t), Σ(t)) and the state space is X R 2. While it is possible to operate RRT* in this state space, the stopping criterion can not be defined clearly if we use this state space. Since our goal is to find a trajectory in time horizon [t, t f ], we hope that RRT* can stop whenever it finds a trajectory such that traveling along this trajectory takes time (t f t ). So we include time t in the state and we have an extended n(n+1) 1+2+ state z(t) = (t, p(t), Σ(t)) R 2. If we initialize the tree by z = (t, p, Σ ), then RRT* will stop when the final time of a trajectory σ(t) is greater than or equal to t f. 2) Task Space Sampling: If we use the extended state (t, p(t), Σ(t)), we need to sample t, p(t) and Σ(t). While it is possible to sample t in [t, t end ](t end t f ) and sample p(t) in the bounded domain of interest, we need to specify a bounded subset of the covariance matrix space to sample Σ(t) uniformly. Since we only know Σ(t ) and do not know Σ(t f ), it is hard to specify such a bounded subset. Also, the dimension of the extended state space will blow up if the number of bases in the environment is very large. And since the system () is uncontrollable, we may not connect two states in the extended state space exactly, which will influence the optimality of RRT*. Hence, to effectively deal with the high dimensionality of the extended state space and the uncontrollability of system (), like [12], we propose a task space sampling strategy. Denote the task space by T. One can choose (t, p(t)) R 1+2 as the task space state, or choose p(t) R 2 as the task space state. But choosing (t, p(t)) still has the problem of uncontrollability. For example, when we need to connect two vertices (t 1, p(t 1 )) and (t 2, p(t 2 ))(t 1 < t 2 ) inside the tree, if the Euclidean distance p(t 2 ) p(t 1 ) > u max (t 2 t 1 ), then there is no trajectory that can connect them. So we choose p(t) T as the state in the task space. And we let SampleFree return z rand := (t, p rand, Σ ). But there is a trade-off between task space sampling and extended state space sampling. While it is hard to connect two states in extended state space, the covariance matrix becomes history dependent in task space sampling. That is, if connecting two vertices by different trajectories in the task space, the resulting covariance matrices will also be different, see Fig. 2. 3) Nearest Node: Given a graph G = (V, E) and a random state z rand, the Nearest(G, z rand ) procedure returns a vertex z nearest in V from which z rand can be reached with the lowest cost. In persistent monitoring, this cost is no Fig. 3. The main steps in the Kinodynamic RRT* algorithm for persistent monitoring are illustrated here, along with the resulting tree structure. longer the Euclidean distance. It is given by J(σz,z rand ) = ρ(σ σ z rand ) ρ(σ σ z ), see Fig. 3. 4) New Random Node z new : In Line 6 of Algorithm 2, a new random node is generated by the trajectory between z nearest and z rand. Typically, z new can be any point on the trajectory, we can even let z new = z rand. This does not affect the asymptotic optimality of RRT* [11]. But in most cases, z new is set as the endpoint of a partial trajectory of a specified maximum cost from z nearest, because the partial trajectory is more likely to avoid obstacles. In this paper, we let z new be the endpoint of a partial trajectory, but this partial trajectory is not the one with the specified maximum cost from z nearest. Since our stopping criterion is related to final time t f, we let the partial trajectory be the one with the specified maximum traveling time from z nearest. Another reason doing this is that there may be no point on the trajectory with specified maximum cost from z nearest since the cost can be negative or positive. Therefore, z new = argmax z σnew {Time(z nearest, z) Time(z nearest, z) t max, σ new = Steer(z nearest, z rand )}, where Time(z nearest, z) is the traveling time from z nearest to z along σ new, see Fig. 3. ) Near Nodes: In Line of Algorithm 2, we need to find a set of near vertices. Near(G, z new ) = { z V J(σz,z new ) l( V ) }, ( ) we choose l( V ) as follows: l( V ) = γ log V 1/2, V where γ is a constant, see Fig. 3. 6) Rewire Check: In the rewire procedures, we try to connect z new to each vertex in Z near to check whether this rewiring will incur less cost than the current cost of the near nodes. In these procedures, besides checking collision of the rewiring, we also need to check whether z near and z new are in the same path and whether z near is the root of the tree, because in these two cases rewiring will return cycles which should be avoided. For example, in Fig. 3, if we do rewiring and connect z new to z i, it will return a cycle since z new and z i are in the same path. Hence, procedure RewireCycleCheck returns true if rewiring incurs no cycle, otherwise, it returns false. Also, when rewiring occurs, we need to check whether the stopping criterion is satisfied since rewiring will update the time in the extended state (t, p(t), Σ(t)). 7) Repropagation: Since RRT* operates in the task space, when rewiring occurs, the state of the descendant nodes of

5 the rewired vertex should be updated, see Fig. 3. Before discussing the repropagation algorithm, some definitions are stated below. Given a graph G = (V, E) maintained by RRT*, for any vertex z inside the tree, Children(z) denotes the set of all children vertices of z, that is, Children(z) := {z V (z, z ) E}. Denote by Branch(z) a subtree of G consisting of z and all the descendants of z. Define UpdateState(z 1, z 2, σ z1,z 2 ) as follows. If z 1 = (t 1, p(t 1 ), Σ(t 1 )) and z 2 = (t 2, p(t 2 ), Σ(t 2 )), starting from z 1, integrating the matrix Riccati differential equation (2) along σ z1,z 2, reaching z 3 = (t 3, p(t 3 ), Σ(t 3 )), then UpdateState(z 1, z 2, σ z1,z 2 ) returns z 3 with p(t 3 ) = p(t 2 ). That is, z 2 is updated by z 3 with t 2 changed to t 3, p(t 2 ) = p(t 3 ), and Σ(t 2 ) changed to Σ(t 3 ). So repropagation means traversing Branch(z) and updating the extended state (t, p(t), Σ(t)) in each node in Branch(z). Specifically, t and Σ(t) should be updated in these vertices, while p(t) does not change. Since p(t) does not change, the updating process can be evaluated quickly by integrating the matrix Riccati differential equation along the edges in Branch(z). And the Steer procedure does not need to be re-run. Also, when doing the repropagation procedure, we should check whether the stopping criterion is satisfied after each UpdateState step. The repropagation procedure Repropagate(z, Branch(z)) has three inputs z and Branch(z) and t f. It can be implemented recursively or iteratively. The iterative repropagation procedure is given in Algorithm 1. It uses depth first search (DFS) algorithm to traverse Branch(z). Algorithm 1 The Iterative Repropagation Procedure Input: z, Branch(z); 1: CurrentNode z; 2: while 1 do 3: Z child Children(CurrentNode); 4: if CurrentNode == z and Z child == then : Break loop and stop; 6: end if 7: if Z child == then 8: CurrentNode Parent(CurrentNode); 9: else : z Z child ; 11: UpdateState(CurrrentNode, z, σ CurrentNode,z ); 12: t z = (t, p, Σ ); 13: if t t f then 14: Break while loop; : Stop kinodynamic RRT* algorithm; 16: return σ(z, z ); 17: end if 18: Children(CurrentNode) Z child \ z ; 19: CurrentNode z ; : end if 21: end while In our implementation, we use the iterative repropagation procedure since the recursive one consumes lots of storage space and may exceed available stack space assigned for the recursion. 8) Approximate Procedure: In the primitive procedure of RRT*, in order to guarantee optimality, Steer(z, z ) must return an optimal trajectory that starts from z and (a) Three kinds of trajectory connecting p(t ) and p(t ) to approximate the optimal trajectory returned by Steer(z, z ) procedure. σ 1 (t) is straight line trajectory, σ 2 (t) is upper semicircular arc trajectory and σ 3 (t) is lower semicircular arc trajectory. Fig. 4. (b) Color map stands for the estimation uncertainties. We choose the motion primitive which can minimize the estimation uncertainties to approximate the steer procedure. Approximate procedure. reaches z exactly. But finding such an optimal trajectory may be computationally challenging, since this is equal to solving a two-point boundary value problem for an ordinary differential equation. Analytical solutions to boundary value problems only exist for some specific systems and specific cost, for example, finding the minimum length trajectory to connect two states in the 2D plane for Dubins vehicle [19], []. For most dynamical systems, an analytical solution does not exist [9]. Hence, to avoid solving the boundary value problem, we propose one method ApproximateSteer to approximate the Steer procedure. We use three kinds of trajectories to approximate the optimal trajectory between z and z : straight line, upper semicircular arc and lower semicircular arc, see Fig. 4(a). We can view them as the motion primitives between two states. We will choose the one to connect two states if it can collect more information and minimize the estimation uncertainties. For example, in Figure 4(b), we will use the upper semicircular arc to connect the two states since it goes through the area which has higher uncertainties. If the mobile sensor moves along this upper semicircular arc, it can take measurements and minimize the high uncertainties in that area. In Fig. 4(a), if the robot moves along these paths with maximum speed u max, then the expressions for the three kinds of trajectories are as follows. σ 1(t) : p 1 (t) = p(t ) + p(t ) p(t ) p(t ) p(t ) umax (t t ) (6) " # σ 2(t) : p 2 (t) = p(t ) + p(t ) cos(π umax (t t ) + r r + ϕ) (7) 2 sin(π umax (t t ) r + ϕ) # σ 3(t) : p 3 (t) = p(t ) + p(t ) 2 + r " cos( umax (t t ) r π + ϕ) sin( umax (t t ) r π + ϕ) where r := p(t ) p(t ) 2 and p(t ) p(t ) is the Euclidean distance between p(t ) and p(t ), ϕ is the four quadrant arctangent angle between (p(t ) p(t )) and the positive horizontal axis. We follow MATLAB notation and write ϕ := atan2 (p y (t ) p y (t ), p x (t ) p x (t )). When the robot moves along these paths, it takes measurements and updates the covariance matrix according to (2). After it reaches p(t ), it gets a covariance matrix for (8)

6 these three different paths. Denote these covariance matrices by Σ 1 (t 1 ), Σ 2 (t 2 ), Σ 3 (t 3 ). Note that t 1, t 2, t 3 may not equal t and Σ 1 (t 1 ), Σ 2 (t 2 ), Σ 3 (t 3 ) may not equal Σ(t ). Then the cost for these three different paths is J(σ 1 ) = ρ(σ 1 (t 1 )) ρ(σ(t )), J(σ 2 ) = ρ(σ 2 (t 2 )) ρ(σ(t )), and J(σ 3 ) = ρ(σ 3 (t 3 )) ρ(σ(t )). We compare these cost and choose the one with the lowest cost as the approximation to the optimal trajectory. B. Kinodynamic RRT* for Persistent Monitoring The kinodynamic RRT* algorithm for persistent monitoring works in the following way. The algorithm is initialized with V = {z }, E = and t f. Then a random node z rand is sampled in the free state space (Line 4). The nearest vertex z nearest to the random node z rand inside the tree is found by the Nearest procedure. Then a new random node z new is returned by using the ApproximateSteer procedure (Line - 6). z new is usually chosen as one point on the trajectory between z nearest and z rand, since the trajectory between z nearest and z rand is likely to collide with obstacles and we choose the part of the trajectory which is collision-free. If no collision is found between z nearest and z new, the Near procedure will calculate a set of vertices Z near close to z new (Line ). For each vertex in Z near, RRT* will compare the cost of these vertices by extending them to z new. Then it sets the vertex in Z near that reaches z new with the lowest cost as z min (Line 11-17) and adds the edge between z min and z new to the tree (Line 18). After this, the algorithm checks whether the stopping criterion is satisfied (Line 19-23). For any vertex in Z near \ {z min }, the algorithm tries to connect z new to z near with a collision-free trajectory. If this connection incurs a cost less than the current cost of z near, then z new is made the new parent of z near. These procedures are called rewire. At this step, the algorithm will also check whether the stopping criterion is satisfied and update the states of the children of z near. This is called repropagation (Line 24-36). C. Computational Complexity This section analyzes the computational complexity of RRT* with the proposed extensions. We will analyze the computational complexity of the primitive procedures first, then discuss the computational complexity of the whole algorithm. As discussed in [1], the CollisionFree procedure can be executed in O(log d M) time, M is the number of obstacles in the environment and d is the dimension of the environment. As for the procedure Nearest and Near, both of them have time complexity O(log V ), where V is the number of vertices. In the Repropagate procedure, we use depth-first search (DFS) to traverse the subtree. For DFS in a tree, the time complexity is O( V ) [21]. Hence, after N iterations, the time complexity of RRT* with the proposed extensions is O(N(log d M + log V + log V + V )). Since d and M are fixed, and V = N, the time complexity is O(N 2 ). With the Repropagate procedure, the kinodynamic RRT* algorithm for persistent monitoring becomes O(N 2 ) compared with O(N log N) for RRT. Algorithm 2 The Kinodynamic RRT* for Persistent Monitoring 1: V z = (t, p, Σ ); E ; Stopping time: t f ; 2: while 1 do 3: G (V, E); 4: z rand SampleFree; : z nearest Nearest(G, z rand ); 6: σ new ApproximateSteer(z nearest, z rand ); z new σ new; 7: if ObstacleFree(σ new) then 8: V V z new; 9: z min z nearest; J min Cost(z new); : Z near Near(G, z new); 11: for z near Z near do 12: σ ApproximateSteer(z near, z new); 13: if ObstacleFree(σ) and Cost(z near) + J(σ) < J min then 14: J min Cost(z near) + J(σ); : z min z near; 16: end if 17: end for 18: E E {(z min, z new)}; 19: Update t new and Σ new: z new = (t new, p new, Σ new) ApproximateSteer(z min, z new); : if t new t f then 21: Break while loop; 22: return σ(z, z new); 23: end if 24: for z near Z near \ {z min} do : σ ApproximateSteer(z new, z near); 26: if ObstacleFree(σ) and Cost(z new) + J(σ) < Cost(z near) and RewireCycleCheck then 27: z parent Parent(z near); 28: E E \ {(z parent, z near)}; 29: E E {(z new, z near)}; : Update t near and Σ near σ; 31: if t near t f then 32: Break current for loop; 33: Break while loop; 34: return σ(z, z near); : end if 36: Repropagate(z near, Branch(z near), t f ); 37: end if 38: end for 39: end if : end while IV. SIMULATIONS AND EXPERIMENTS This section presents numerical simulations of the kinodynamic RRT* algorithm doing persistent monitoring. Since finding the optimal trajectory for the Steer procedure is equal to solving a two-point boundary value problem, which is computationally very expensive, we use the approximation procedure proposed above to approximate the optimal trajectory between two vertices. So the trajectory returned in this simulation is an approximation to the optimal trajectory doing persistent monitoring. In this simulation, our goal is to find a trajectory in time horizon [t, t f ] such that moving along this trajectory the estimation uncertainties can be minimized everywhere in the environment and in time. We choose a time horizon as follows: t =, t f = 67s. The algorithm will stop as long as it finds a trajectory such that the end time of the trajectory is greater than or equal to t f. The envi-

7 ronment is chosen as a square region with two rectangular obstacles. We use 4 radial basis functions to represent the field in the environment, and they are given by C(p(t)) = [c 1 (p(t)),..., c 4 (p(t))], where c i (p(t)) := Ke p(t) qi 2 /2σ 2 c, i = 1,..., 4, K =, σc = 6, and the basis positions are q 1 = [, 4] T, q 2 = [, 4] T, q 3 = [, ] T, q 4 = [, ] T. The coefficients of these basis functions are changing according to the linear stochastic dynamics (1). The system matrix is set to be A = (.) I 4, the process covariance matrix is Q = I 4, where I n is the n n identity matrix. By such an A matrix, the field is stable, since all the real parts of the eigenvalues are smaller than and the system is stabilizable and detectable. Therefore, the matrix Riccati differential equation (2) will not grow unbounded [18]. For the sensor, the measurement covariance matrix is R =. For the robot, u max =. Its initial position in the environment is p(t ) = [, ] T, and the initial covariance matrix is Σ(t ) = 3 I 4. The RRT* parameter γ =.. The approximate trajectory for the sensing robot monitoring the environment is shown in Fig.. In this figure, the black blocks are obstacles and green squares denote the positions of the basis functions. Blue curves are rapidlyexploring random trees and the solid red curve is the approximate trajectory returned by RRT* with the approximation procedure for the Steer function. The robot will move from its initial position, avoid obstacles and get close to the basis functions quickly, because the field close to the basis functions changes faster and the estimation uncertainties there are higher. Then the sensing robot will move around in that area, take measurements and try to minimize the estimation uncertainties in the whole environment. What is interesting is that the algorithm smartly returns a trajectory leading to the area between the basis centers, because the estimation uncertainties in that area are higher than other areas and the goal is to balance the estimation uncertainties everywhere in the environment and in time. The robot will not move to the basis centers exactly since the field is correlated. So even though the robot can only measure the field at its position, that measurement contains information about the field in the neighborhood of its position. We then compare the kinodynamic RRT* trajectory with a trajectory which visits each basis center successively. We call this trajectory a tour trajectory (see Fig. 6). It starts with the same initial position at t, and ends when the end time of the trajectory reaches t f. We choose this simple trajectory to compare with the trajectory generated by the proposed algorithm since there are no existing planning algorithms for the considered problem. From Fig. 6, the cost of our trajectory is much lower than the cost of the tour trajectory. That is, our trajectory can keep the estimation uncertainties in the environment much lower than that of the tour trajectory. Fig. 7 shows 4 screen shots from an experiment video showing how estimation uncertainties change when an m3pi robot moves along the trajectory returned by kinodynamic RRT*. We denote the estimation uncer- Fig.. Numerical results of kinodynamic RRT* algorithm doing persistent monitoring. Black blocks: obstacles; green squares: basis function centers; blue curves: rapidly-exploring random tree; solid red curve: trajectory returned by kinodynamic RRT* starting from t = and ending at time t f = 67s. Trajectory cost Kinodynamic RRT* trajectory Tour trajectory 6 7 Time(s) Fig. 6. A tour through all the basis function centers (shown on the left) is used as a baseline trajectory for comparison. The trajectory using our algorithm outperforms the baseline tour trajectory (shown on the right). tainty at one point by [ its variance Var(p(t)). Hence, ] we have Var(p(t)) = E (C(p(t))e x (t)) (C(p(t))e x (t)) T = tr(σ(t)c(p(t)) T C(p(t))). In this figure, the environment is discretized into a grid. We calculate the variance of all the points in the grid and use a colormap to represent the variance. Red color stands for high variance while blue color stands for low variance. The environment is projected on the ground. We use a computer to control the robot to move along the kinodynamic RRT* trajectory. When the robot moves and takes measurements, it tries to minimize the estimation uncertainties everywhere in the environment. We simulate the measurement process by using a motion capture system (OptiTrack) to get the position of the robot and use the position data to update the evolution of the covariance matrix, that is, update the estimation uncertainties in the whole environment, which are represented by the colormap. The estimation uncertainties are correlated in the environment. Therefore, when the robot moves, the uncertainty in the neighborhood of the robot will decrease since the robot takes measurement at its position. The goal for the robot is to find an optimal trajectory in a fixed time horizon to move in the environment and minimize the estimation uncertainties everywhere in the environment.

8 (a) Time t = s (b) Time t = s (c) Time t = s (d) Time t = 6s Fig. 7. An m3pi robot executing a trajectory returned by kinodynamic RRT* in the fixed time horizon [, 67]. The black blocks represent obstacles, the red curve shows the path of robot, and the colormap indicates the field estimation uncertainties. Red hot stands for high uncertainty while dark blue stands for low uncertainty. The robot moves around in the environment, takes measurements and tries to minimize the estimation uncertainties everywhere in the environment (the measurement process is simulated using a motion capture system, OptiTrack). V. CONCLUSIONS AND FUTURE WORK In this paper, we applied kinodynamic RRT* to a persistent monitoring problem. That is, a sampling-based technique is used to find a trajectory for a sensing robot to estimate a dynamic spatial field of interest over an environment. We aim to minimize the spectral radius of the estimation covariance matrix when performing Kalman filtering along this trajectory. We proposed an extended state space to deal with the stopping criterion of RRT*. We also proposed a task space sampling procedure to avoid the uncontrollability of the kinodynamic system and a repropagation procedure which is used in the rewire operation. Then we deal with the computational difficulty of finding the optimal trajectory for the Steer procedure by proposing an approximation method. The performance of our algorithm is demonstrated in simulations and hardware experiments. Future work will be dedicated to finding the optimal trajectory for the Steer procedure. We also plan to investigate sampling based methods for persistent monitoring with multiple robots. ACKNOWLEDGMENTS We would like to thank our labmate Dingjiang Zhou for the support in our experiment implementation. REFERENCES [1] S. Karaman and E. Frazzoli, Sampling-based algorithms for optimal motion planning, International Journal of Robotics Research, vol., pp , June 11. [2] S. Karaman and E. Frazzoli, Optimal kinodynamic motion planning using incremental sampling-based methods, in IEEE Conference on Decision and Control (CDC), (Atlanta, GA), December. [3] X. Lan and M. Schwager, Planning periodic persistent monitoring trajectories for sensing robots in gaussian random fields, in 13 IEEE International Conference on Robotics and Automation, 13. [4] S. L. Smith, M. Schwager, and D. Rus, Persistent robotic tasks: Monitoring and sweeping in changing environments, IEEE Transactions on Robotics, vol. 28, pp , April 12. [] C. Cassandras, X. Lin, and X. Ding, An optimal control approach to the multi-agent persistent monitoring problem, Automatic Control, IEEE Transactions on, vol. 8, no. 4, pp , 13. [6] X. Lan and M. Schwager, A variational approach to trajectory planning for persistent monitoring of spatiotemporal fields, in 14 American Control Conference (ACC), 14. [7] D. Baronov and J. Baillieul, Decision making for rapid information acquisition in the reconnaissance of random fields, Proceedings of the IEEE, vol., no. 3, pp , 12. [8] S. M. LaValle and J. J. Kuffner, Randomized kinodynamic planning, The International Journal of Robotics Research, vol., no., pp. 378, 1. [9] A. E. Bryson Jr, Optimal control-19 to 198, Control Systems, IEEE, vol. 16, no. 3, pp , [] G. Goretkin, A. Perez, R. Platt Jr, and G. Konidaris, Optimal sampling-based planning for linear-quadratic kinodynamic systems, in Robotics and Automation (ICRA), 13 IEEE International Conference on, IEEE, 13. [11] D. J. Webb and J. v. d. Berg, Kinodynamic rrt*: Asymptotically optimal motion planning for robots with linear dynamics, in Robotics and Automation (ICRA), 13 IEEE International Conference on, IEEE, 13. [12] J. H. Jeon, S. Karaman, and E. Frazzoli, Anytime computation of time-optimal off-road vehicle maneuvers using the rrt*, in Decision and Control and European Control Conference (CDC-ECC), 11 th IEEE Conference on, pp , IEEE, 11. [13] G. A. Hollinger and G. S. Sukhatme, Sampling-based robotic information gathering algorithms, The International Journal of Robotics Research, pp , 14. [14] J. Cortés, Distributed Kriged Kalman filter for spatial estimation, IEEE Transactions on Automatic Control, vol. 4, no. 12, pp , 9. [] N. Cressie, The origins of kriging, Mathematical Geology, vol. 22, no. 3, pp , 199. [16] N. Cressie, Statistics for Spatial Data. New York: Wiley, [17] R. Kalman, A new approach to linear filtering and prediction problems, Journal of basic Engineering, vol. 82, no. Series D, pp. 4, 196. [18] F. Callier and J. Willems, Criterion for the convergence of the solution of the riccati differential equation, Automatic Control, IEEE Transactions on, vol. 26, no. 6, pp , [19] L. E. Dubins, On curves of minimal length with a constraint on average curvature, and with prescribed initial and terminal positions and tangents, American Journal of mathematics, vol. 79, no. 3, pp , 197. [] D. P. Bertsekas, D. P. Bertsekas, D. P. Bertsekas, and D. P. Bertsekas, Dynamic programming and optimal control, vol. 1. Athena Scientific Belmont, 199. [21] T. H. Cormen, C. E. Leiserson, R. L. Rivest, and C. Stein, Introduction to Algorithms. The MIT Press, 1.