A receding horizon approach to generating ... - Semantic Scholar
A receding horizon approach to generating dynamically feasible plans for vehicles that operate over large areas Daniel J. Stilwell and Aditya S. Gadre and Andrew J. Kurdila Abstract— We present an approach to planning dynamically feasible vehicle trajectories for applications where the vehicle operates in very large environments and for which a kinematic model is poor approximation of the vehicle’s dynamics. Using a standard receding horizon control framework, we compute a sequence of short-horizon optimal trajectories that can be computed in real-time. We also compute a global path that is updated only occasionally. Because the environment is very large, we presume that the global path cannot be updated in real-time. By selecting a terminal state cost in the finite-time optimal control problem that approximates the cost-to-go, we are able to propose a formal condition under which the sequence of dynamically feasible trajectories is guaranteed to converge to a desired goal location. Moreover, we show that the global path need be updated only when the formal condition is not satisfied, which allows us to delay update of the global path.
I. I NTRODUCTION We address the problem of planning vehicle trajectories over very large areas that are potentially poorly mapped and for which an accurate map is acquired in real-time. We also address the case that vehicle dynamics are not adequately approximated by a kinematic model. Our approach is to compute a global path that does not respect vehicle dynamics and a sequence of local dynamically feasible trajectories. Information that describes the global path is utilized when computing the sequence of dynamically feasible trajectories. Our approach ensures that the sequence of trajectories converge to a desired goal location. Although local trajectories are computed continuously using the most recent map, the global path is computed infrequently in our approach. Since the environment is potentially very large, we presume that no global path planner is able to compute the global path in real-time. In practice, most global path updates are local and can be computed quickly. However, even with the availability of fast real-time planners, such as D∗ and E∗ , among many others (see [10] and [13]), it is possible for the map to change in such a way that the global plan cannot be updated in real-time. We address this challenge by delaying the computation of a global path until it is absolutely necessary. Our approach yields a formal test on when global path must be updated. Speaking loosely, the global path must be updated only when the global path and the local trajectory are inconsistent in This work was supported by the Office of Naval Research via grant N00014-10-1-0185 D. Stilwell and A. Gadre are with the Bradley Department of Electrical and Computer Engineering, Virginia Tech, Blacksburg, VA, USA
[email protected], [email protected] A. Kurdila is with the Mechanical Engineering Department, Virginia Tech, Blacksburg, VA, USA [email protected]
a meaningful way. We find via simulation studies that the global path is updated infrequently using our approach. At occasions when the global path must be updated, the vehicle may need to slow down or even stop in order to meet realtime planning constraints. Our work is inspired by applications involving guidance and control of an unmanned surface vessel (USV) that operates in large tropical riverine systems. This application highlights the lack of an accurate prior map and the importance of vehicle dynamics. Tropical riverine systems can be poorly mapped due to changes in the river topography, such as flooding, and the existence of a multi-layer canopy that can obscure the waterway in satellite imagery. Thus we presume that only an approximate map is available prior to the start of a mission, and that an accurate map can only be acquired by the vehicle in real-time. Furthermore, a USV can exhibit significant side-slip, especially at lowspeeds. In extreme cases, side-slip can be up to ninety degrees, and thus a kinematic model that presumes zero sideslip is a poor approximation of USV dynamics. Although our work is motivated by a specific application, we believe that our results can be applied to other vehicle guidance applications where a kinematic model of the vehicle is a poor approximation of the vehicle’s motion. For example, ground vehicles operating at high-speed can exhibit significant sideslip while turning. There is a rich literature on planning and trajectory generation for vehicle systems, including recent work described in [3], in which trajectories are computed in real-time for a kinematic vehicle model. The work that is most closely related to ours is reported in [8], in which a receding horizon approach is adopted and a specific terminal state cost is proposed that approximates the cost-to-go in an obstacle filled environment. We adopt a similar receding horizon approach, although our cost-to-go is different, and our formal use of a matching condition related to the cost-to-go allows us to derive a test for delaying global path updates. This paper is organized as follows. In Section II, we briefly describe our approach to computing a global path. In Section III, we describe how a sequence of dynamically feasible trajectories are computed in a receding horizon framework. In Section IV, we derive a so-called matching condition and show that the sequence of dynamically feasible trajectories is guaranteed to converge to a desired goal location if the matching condition is satisfied. An illustrative example is presented in Section V.
We assume that the vehicle operates in an environment Ω ⊂ R2 , and that the map is represented by the function g : Ω 7→ R ≥ 0. The vehicle starts at location zo ∈ Ω and travels to the goal location ξ ∈ Ω. We assume that the map g is zero at the goal location. The value of the map represents risk of traversal. For the USV application, locations that are identified as land or other hazards to navigation correspond to a high risk of traversal. Locations that are identified as water have a low risk of traversal. In practice, we compute initial maps from topological maps or from satellite imagery. For example, a satellite image of a small section of an operating area is shown in Figure 1. The prior map is created from satellite imagery by marking areas thought to be water with very low risk and areas that are thought to be land with very high risk.
shown in Figure 2 is the global path computed from a location on the map to the goal location. Recall that the direction of the global path is the negative gradient of the level set function. The global path in Figure 2 clearly displays that the level set is highly discretized in our application.
1200
1000
800 North(m)
II. M APPING AND GLOBAL PATH PLANNING
600
400
200
200
Fig. 2.
400
600
800 East(m)
1000
1200
1400
Level set function for the wrong map of the operating area.
Although we have utilized a level-set approach to path planning, we conjecture that any planning method that produces a cost-to-go will work within our framework. Examples of such algorithms include Dijkstra’s algorithm and path planners that use NF1 and NF2 algorithms over a potential field (see e.g. [1] and [2]). However, a formal analysis of this conjecture is on-going. III. L OCAL TRAJECTORY GENERATION Fig. 1.
Satelline imagery of an operating area.
We utilize the level set approach described in [14] to compute global paths, although this approach is nearly identical to the E∗ algorithm in [10]. The level set function Q : Ω 7→ R, which is computed using a fast marching method, has two important properties for our work. The direction of the globally optimal path at any point z ∈ Ω is −∇Q(z), and the value of the level set function Q(z) represents the cost-to-go from any location z to the goal location ξ. We do not directly make use of the global path in our work. Rather, we use the level set function Q(z) as the terminal cost when computing local trajectories. For illustration, the level set function corresponding to the map in Figure 1 is shown in Figure 2. Note that the map is purposefully incorrect. A large portion of the shore in approximately the center of the satellite image is removed in order to simulate, in Section V, an incomplete map available to the USV. The level set in Figure 2 is computed for a specific goal location at north = 85m and east = 1550m. The value of the level set function is indicated by shading. Lighter areas indicate smaller cost-to-go values and darker areas indicate higher cost-to-go values, respectively. Also
We assume that the dynamic model of the vehicle can be expressed x(t) ˙ = f (x(t), u(t)),
x(ti ) = xo ,
t ≥ ti
(1)
where x(t) ∈ X ⊂ Rn is the state of the vehicle and u(t) ∈ U ⊂ Rm is corresponding control signal. The sets X and U represent bounds on the vehicle state and actuator, respectively. We note that a subset of the elements of x correspond to position variables, and we may formally write [ ] x x= a xb where xa ∈ Ω ⊂ R2 represents the position of the vehicle. In the sequel we make use of the matrix L ∈ R2×n that is defined such that xa = Lx. (2) To generate a control signal for the vehicle, we compute a sequence of dynamically feasible state and control signal trajectories. For the interval t ∈ [ti , ti + T ], we compute a control signal that minimizes the cost function ∫ ti +T JT∗ (ti , xo ) = min q(τ, x(τ ), u(τ ))dτ +Wi (x(ti +T )) u∈U
ti
(3)
subject to the vehicle dynamics in (1). We define u∗i (t) with support [ti , ti + T ] to be a control signal that minimizes (3) and x∗i (t) to be the corresponding state trajectory. The integral cost q(t, x, u) is positive everywhere except when the vehicle is at the goal location ξ and the control signal is zero. Thus q(t, x, u) = 0 when ξ = Lx and u = 0. The terminal state cost Wi (x) is time-invariant over each optimization interval [ti , ti + T ], but we allow that Wi (x) can be different during each optimization interval. In our application, Wi (x) is the level set function that is used to generate a global path. Since the level set function is occasionally updated, the terminal state cost Wi (x) may also be occasionally updated. Only a portion of the control signal u∗i is applied to the vehicle system, and the remaining portion of the control signal is discarded. Specifically, we implement a control signal u ¯i (t) for t ∈ [ti , ti + δ] where the implementation horizon δ is less than the optimization horizon T . At time ti+1 := ti + δ we compute a new optimal control u∗i+1 (t) for the interval t ∈ [ti+1 , ti+1 + T ]. Thus we discard the portion of the suboptimal control signal corresponding to t ∈ [ti + δ, ti + T ). This results in a control signal and corresponding state trajectory u ¯(t) = u∗i (t), x ¯(t) = x∗i (t),
ti ≤ t < ti+1 ti ≤ t < ti+1
Note that the control signal is not required to be continuous at ti . Convergence of the system (1) when driven by the suboptimal control signal u ¯(t) is not guaranteed without imposing additional requirements on the optimization problem. It is generally recognized that the terminal state penalty Wi (x) plays an import role (e.g., see [6] and [9]). In our approach, the terminal state penalty is the key to combining global information with local trajectory generation. Indeed, we require the following, Assumption 3.1: The integral cost q(t, x(t), u(t)) and the terminal state cost Wi (x(t)) in (3), satisfy ˙ i (x(t)) ≤ 0 q(t, x(t), u(t)) + W
(4)
for all t ∈ [ti + T − δ, ti + T ]. Since we are addressing control of vehicle systems, it is not necessarily appropriate to consider convergence of the state x(t) to zero. Some states may correspond to position and velocity, while other states may correspond to orientation. If the objective of the control signal is to drive the system to a desired location, then we may desire that position and velocity states converge to zero, but that the steady-state value of the orientation state is arbitrary. Thus we do not seek to drive the state to zero. Rather, we seek to drive q(t, x, u) to zero. Theorem 3.2: If the integral cost q(t, x, u) and the terminal state penalty Wi (x) satisfy Assumption 3.1, then the
control signal u ¯(t) drives the system (1) so that lim q(t, x ¯(t), u ¯(t)) = 0. (5) t→∞ For continuity of exposition, the proof of Theorem 3.2 appears in the Appendix. IV. M ATCHING CONDITION The fundamental concept in our approach is to use the level set function Qi (z) from the global path planner as the terminal cost function Wi (x) in (3). Note that the level set function is indexed with the subscript i to indicate that it is occasionally updated to reflect changes to the map. Since the domain of level set function is Ω ∈ R2 , we use instead Qi (Lx) where L is defined in (2). Note that if we assign W (x) := Qi (Lx), then ˙ i (x) = ∇Qi (Lx) · L dx W dt = ∇Qi (Lx) · Lf (x, u) = ∇Qi (Lx) · x˙ a where xa is the position of the vehicle and x˙ a is the velocity of the vehicle. The inequality (4) is expressed q(t, x(t), u(t)) ≤ −∇Q(Lx(t)) · x˙ a
(6)
for all t ∈ [ti + T − δ, ti + T ]. Definition 4.1: We refer to the inequality in (6) as the matching condition. If the matching condition (6) is satisfied at the end of each trajectory interval, then the sequence of trajectories are such that q(t, x, u) converges to zero. In other words, we require that the matching condition is satisfied in order for the vehicle to converge to the goal location. If the matching condition is not satisfied, then we can update the level set function and compute the trajectory again. In addition, we can increase the local trajectory horizon T and recompute the trajectory. The matching condition is useful in our work by providing a formal test for when the level set function Qi (z) must be updated. So long as the matching condition is satisfied, a level set function can be used even if it is based on map data that is no longer accurate. If the matching condition is not met, then the following actions are taken: (i) the level set function is updated using the most recent map, (ii) the optimal trajectory is re-computed using the updated level set as terminal cost, and (iii) the matching condition is checked again. If the matching condition is not met when using the up-to-date level set, then the optimization horizon T can be increased, and the process started again. Note that Theorem 3.2 does not require that T is constant. However, in our work, we have focused on using the matching condition to determine when the level set function need be updated, and we have not addressed changing the optimization horizon T . The matching condition (6) possesses an useful physical interpretation. The right-hand side is related to the angle between the direction of the global path (−∇Qi (Lx)) and the direction of travel of the USV from the optimal trajectory (x˙ a ). Thus the matching condition can be interpreted as a test
V. I LLUSTRATIVE EXAMPLE We illustrate our guidance concepts with a computer simulation of an unmanned surface vehicle (USV) operating in a riverine system. The dynamic model of the USV, which appears in [12], is 1 K 0 − τβ 0 − τββ β˙ β ψ˙ = (7) 0 0 1 ψ + 0 Kr r r˙ 0 0 − τ1 τr r
where β is the side-slip angle, ψ is the yaw angle and r is the yaw rate of the USV, and Kβ , Kr , τβ and τr are gains and time constants that depend on the speed of the USV. For this simulation, we choose Kβ = 0.96, Kr = 1.44, τβ = 2.13 and τr = 2.22, which corresponds to 1.5 m/s in [12]. Position of the USV in north-east-down (Earth-fixed) coordinate system is computed by s˙ = ν cos (ψ + β) e˙ = ν sin (ψ + β)
(8)
where s is the north coordinate, e is the east coordinate and ν is the speed of the USV. Dynamically feasible local trajectories are computed by first transforming the optimal control problem (3) into a nonlinear programming (NLP) problem by approximating the control signal as piecewise constant over finite subintervals in the planning horizon [7], and then solving the resulting NLP using the Hooke-Jeeves algorithm [5]. The output of the Hookes-Jeeves algorithm is the optimal control sequence and the corresponding state trajectory for the USV. The optimization horizon is set to T = 60 seconds. The implementation horizon is set to δ = 6 seconds. The integral cost q(t, x, u) in (3) is very simple for the illustrative example presented herein. We choose q(t, x, u) = 0 if x corresponds to water, q(t, x, u) = 1 × 106 if x corresponds to land, and q(t, x, u) = 1 × 103 if the location x is within 10 meters of land. The latter cost ensures that the USV prefers not to be too close to land. When evaluating q(t, x, u), the up-to-date local map is used that includes all sensed information locally acquired by the USV. The map is implemented as a cellular decomposition of the environment where a scalar value that represents risk of traversal is attached to each cell. New sensor data
is integrated into the map using a standard Dempster-Shafer approach [11]. We note, however, that the level set function Qi (z) is based on an out-of-date map unless the matching condition is violated and an updated level set function is required. In Figure 3, we have simulated a shoreline that is found by the USV, but that does not appear in the prior map. New sensor data generates a thin shoreline that is appended to the original wrong map in real-time. This is similar to how a laser line scanner would perceive shoreline. However, this updated map information is not immediately reflected in the level set function. Figure 3 shows the dynamically feasible local trajectory (thick red line). The thin blue line is the global path starting at the location of the USV. It is not used directly in our approach, and it is shown only for illustration. The newly found shoreline is superimposed on the old level set function. It can be seen towards the end of the local trajectory that the direction of negative gradient of the level set function (blue arrow), which is the direction of the global path, diverges significantly from the direction of the local trajectory. This results in failure of the matching condition, and it denotes a significant mismatch between the map and the level set function. 650
600
550 North(m)
on the angle between the global path and the dynamically feasible local trajectory evaluated at the end of the trajectory. If the angle is small, then we expect the matching condition to be satisfied. Since the matching condition need only be satisfied during the last δ seconds of the trajectory, obstacle avoidance is implicitly addressed. The local dynamically feasible trajectory is permitted to steer clear of local obstacles. Finally, we note that the sequence of dynamically feasible local trajectories does not track the global path. Indeed, doing so might be a poor choice. Instead, the sequence of local trajectories need only approximate the direction of the global path at the end of every optimization interval.
500
450
400
350
450
500
Fig. 3.
550
600 650 East(m)
700
750
800
Failed matching condition.
Since the matching condition is not satisifed, the USV does not follow the local trajectory show in Figure 3. Failure of the matching condition triggers an update of the level set function so that the level set function reflects the most recent map data. Once the level set function is updated, a new dynamically feasible local trajectory is computed and the matching condition is re-evaluated using the updated level set function as a terminal state cost. The new trajectory is shown in Figure 4 (thick red line) along with the negative gradient of the level set (blue line) at the end of the trajectory. Recall that the negative gradient of the level set function is the direction of the global path. Now that the level set function represents newly sensed shoreline, local trajectory and the global path are approximately aligned at the end of the local trajectory, and the matching condition is satisfied.
indicated by red squares, allowing the USV to travel over a long distance without requiring frequent computationally expensive level set updates. This simulation demonstrates efficient planning of dynamically feasible local trajectories, as well as proposed scheme of delayed computation of the global path through level set function update.
650
600
500
450
1200
400
1000
350
Fig. 4.
800 450
500
550
600 650 East(m)
700
750
800
North(m)
North(m)
550
600
Dynamically feasible trajectory on updated level set function. 400
It is important to note that not all newly sensed changes to the map result in failure of the matching condition. Figure 5 shows such a case in which the local trajectory successfully circumvents newly found shoreline using an up-to-date map but an old level set function. Since the gradient of the level set function and the direction of the computed trajectory at the end of the trajectory do not diverge significantly, the matching condition is satisfied and no update of the level set function is required.
600
550 North(m)
0
0
500
1000
1500
East(m)
Fig. 6.
Path planning run in riverine environment.
VI. C ONCLUDING REMARKS We have described an approach to computing dynamically feasible trajectories using concepts from receding horizon control. The principal benefit of our approach is that global plans need be updated only infrequently, which means that the vast majority of computations are local and fast. The success of our approach is due to selection of the terminal state cost in (3). In general, the terminal state cost should be the cost-to-go. We have shown that the level set function is a suitable estimate of cost-to-go. Others, such as [8], have recognized the important role played by the terminal state cost, and have selected other functions to approximate the cost-to-go. On-going work seeks to identify necessary conditions on the terminal state cost.
650
500
450
400
350
200
450
Fig. 5.
500
550
600 650 East(m)
700
750
VII. ACKNOWLEDGMENTS
800
Dynamically feasible trajectory on updated map.
Figure 6 shows the result of using our approach to travel through the simulated river. In the simulation, the USV started at the position indicated by the red triangle. Obstacles shown in gray do not appear in the original map, and thus they are not accounted for by the initial level set function. As the USV travels closer to an obstacle, the obstacle becomes visible to the USV, thus simulating map update based on new sensor data. The USV does not update the level set function if the matching condition is satisfied. Along the entire run, the level set function is updated only twice, at positions
The authors gratefully acknowledge support of the Office of Naval Research via grant N00014-10-1-0185. VIII. A PPENDIX Proof of Theorem 3.2: Our approach is based on the proof in [4], although we choose a different form of the value function that results in a slightly different cost function. We seek to show that the value function JT −δ (ti , x ¯(ti )) := ∫ ti +T −δ q(τ, x ¯(τ ), u ¯(τ ))dτ + Wi (x(ti + T − δ)) ti
is monotonically decreasing with respect to increasing index i. For arbitrary index i, consider the difference JT −δ (ti+1 , x ¯(ti )) − JT −δ (ti , x ¯(ti )) ∫ ti +T = q(τ, x ¯(τ ), u ¯(τ ))dτ + Wi (¯ x(ti + T )) ∫ −
ti +δ ti +T −δ
(9)
it must be that q(τ, x ¯(τ ), u ¯(τ ))dτ − Wi (x(ti + T − δ))
ti +T −δ
+ Wi (x∗ (ti + T )) − Wi (x∗ (ti + T − δ)) ≤ 0
and thus Wi (x∗ (ti + T )) − Wi (x∗ (ti + T − δ)) ≤ ∫ ti +T − q(τ, x∗ (τ ), u∗ (τ ))dτ ti +T −δ
From (9) JT −δ (ti+1 , x ¯(ti )) − JT −δ (ti , x ¯(ti )) ∫ ti +T ∫ ti +T −δ ≤ q(τ, x ¯(τ ), u ¯(τ ))dτ − q(τ, x ¯(τ ), u ¯(τ ))dτ ti +δ
∫ ≤−
∫
tk
q(τ, x ¯(τ ), u ¯(τ ))dτ t0
where we have recalled that ti+1 = ti + δ. From the ˙ i (x∗ (t)) ≤ 0 for requirement that q(t, x∗ (t), u∗ (t)) + W t ∈ [ti + T − δ, ti + T ], we note that ∫ ti +T q(τ, x∗ (τ ), u∗ (τ ))dτ
−
t0
JT −δ (t0 , x∗ (t0 ), u∗ (t0 ))
ti
∫
By recalling that q(t, x, u) ≥ 0 and rearranging (10) ∫ tk JT −δ (tk ), x∗ (tk )) + q(τ, x ¯(τ ), u ¯(τ ))dτ ≤
ti ti +T
q(τ, x∗ (τ ), u∗ (τ ))dτ
ti +T −δ ti +δ
q(τ, x ¯(τ ), u ¯(τ ))dτ ti
Now we show that this property holds as the difference interval is increased. Note that for arbitrary positive integer k, JT −δ (tk ), x∗ (tk )) − JT −δ (t0 , x∗ (t0 ), u∗ (t0 )) = JT −δ (tk , x∗ (tk ), u∗ (tk )) − JT −δ (tk−1 , x∗ (tk−1 ), u∗ (tk−1 )) + JT −δ (tk−1 , x∗ (tk−1 ), u∗ (tk−1 ))− JT −δ (tk−2 , x∗ (tk−2 ), u∗ (tk−2 )) · · · + JT −δ (t1 , x∗ (t1 ), u∗ (t1 )) − JT −δ (t0 , x∗ (t0 ), u∗ (t0 )) Thus JT −δ (tk ), x∗ (tk )) − JT −δ (t0 , x∗ (t0 ), u∗ (t0 )) ≤ k−1 ∑ ∫ ti +δ q(τ, x ¯(τ ), u ¯(τ ))dτ − i=0
ti
and we conclude that JT −δ (tk ), x∗ (tk )) − JT −δ (t0 , x∗ (t0 ), u∗ (t0 )) ≤ ∫ tk − q(τ, x ¯(τ ), u ¯(τ ))dτ t0
(10)
is bounded for all k. Thus q(τ, x ¯(τ ), u ¯(τ )) → 0 as k → ∞. R EFERENCES [1] J Barraquand, JC Latombe, 1991, Robot Motion Planning: A Distribted Representation Approach, in The International Journal of Robotics Research, Vol. 10, No. 6, pp.628–649. [2] J Barraquand, B Langlois, JC Latombe, 1992, Numerical Potential Field Techniques for Robot Path Planning, in IEEE Transactions on Systems, Man, and Cybernetics, Vol. 22. No. 2, pp.224–241. [3] D. Dolgov, S. Thrun, M. Montemerlo, J. Diebel, 2010, Path Planning for Autonomous Vehicles in Unknown Semi-structured Environments, in The International Journal of Robotics Research, 29(5):485-501. [4] F. A. A. C Fontes, 2001, A general framework to design stabilizing noninear model predictive controllers, Systems and Control Letters, 42:127–143. [5] R Hooke, TA Jeeves, 1961, Direct Search Solution of Numerical and Statistical Problems, in Journal of the ACM, Vol. 8, No. 2, pp.212–229. [6] A Jadbabaie, J Yu, and J Hauser, 1999, “Stabilizing receding horizon control of nonlinear systems: a control Lyapunov function approach,” Proc. of the American Control Conference, pp. 15351539. [7] D Kraft, 1985, On Converting Optimal Control Problems into Nonlinear Programming Problems, in Nato ASI Series, Vol. 15, pp.261–280. [8] Y Kuwata, A Richards, T Schouwenaars, JP How, 2006,“Decentralized Robust Receding Horizon Control for Multi-Vehicle Guidance,” in Proc. American Control Conference, Minneapolis, MN, pp. 2047– 2052. [9] DQ Mayne, JB Rawlings, CV Rao and POM Scokaert, 2000, “Survey Paper: Constrained model predictive control: Stability and optimality”, Automatica, 36, pp. 789-814. [10] R Philippsen, R Siegwart, 2005, An Interpolated Dynamic Navigation Function, in Proceedings of the IEEE International Conference on Robotics and Automation, pp.3782–3789, Barcilona, Spain. [11] G Shafer, 1976, A Mathematical Theory of Evidence, Princeton University Press, New Jersey, 1976. [12] C Sonnenburg, A Gadre, D Horner, S Kragelund, A Marcus, DJ Stilwell, CA Woolsey, 2010, Control-oriented planar motion modeling of unmanned surface vehicles, Proc. IEEE/MTS OCEANS, Seattle, WA. [13] A Stentz, 1993, Optimal and Efficient Path Planning for Unknown and Dynamic Environments, in International Journal of Robotics and Automation, Vol. 10, pp.89–100. [14] B Xu, DJ Stilwell, AJ Kurdila, 2009, Efficient computation of level sets for path planning, in IEEE/RSJ Int’l Conf. on Intelligent Robots and Systems, St. Louis, MO.
I. I NTRODUCTION We address the problem of planning vehicle trajectories over very large areas that are potentially poorly mapped and for which an accurate map is acquired in real-time. We also address the case that vehicle dynamics are not adequately approximated by a kinematic model. Our approach is to compute a global path that does not respect vehicle dynamics and a sequence of local dynamically feasible trajectories. Information that describes the global path is utilized when computing the sequence of dynamically feasible trajectories. Our approach ensures that the sequence of trajectories converge to a desired goal location. Although local trajectories are computed continuously using the most recent map, the global path is computed infrequently in our approach. Since the environment is potentially very large, we presume that no global path planner is able to compute the global path in real-time. In practice, most global path updates are local and can be computed quickly. However, even with the availability of fast real-time planners, such as D∗ and E∗ , among many others (see [10] and [13]), it is possible for the map to change in such a way that the global plan cannot be updated in real-time. We address this challenge by delaying the computation of a global path until it is absolutely necessary. Our approach yields a formal test on when global path must be updated. Speaking loosely, the global path must be updated only when the global path and the local trajectory are inconsistent in This work was supported by the Office of Naval Research via grant N00014-10-1-0185 D. Stilwell and A. Gadre are with the Bradley Department of Electrical and Computer Engineering, Virginia Tech, Blacksburg, VA, USA
[email protected], [email protected] A. Kurdila is with the Mechanical Engineering Department, Virginia Tech, Blacksburg, VA, USA [email protected]
a meaningful way. We find via simulation studies that the global path is updated infrequently using our approach. At occasions when the global path must be updated, the vehicle may need to slow down or even stop in order to meet realtime planning constraints. Our work is inspired by applications involving guidance and control of an unmanned surface vessel (USV) that operates in large tropical riverine systems. This application highlights the lack of an accurate prior map and the importance of vehicle dynamics. Tropical riverine systems can be poorly mapped due to changes in the river topography, such as flooding, and the existence of a multi-layer canopy that can obscure the waterway in satellite imagery. Thus we presume that only an approximate map is available prior to the start of a mission, and that an accurate map can only be acquired by the vehicle in real-time. Furthermore, a USV can exhibit significant side-slip, especially at lowspeeds. In extreme cases, side-slip can be up to ninety degrees, and thus a kinematic model that presumes zero sideslip is a poor approximation of USV dynamics. Although our work is motivated by a specific application, we believe that our results can be applied to other vehicle guidance applications where a kinematic model of the vehicle is a poor approximation of the vehicle’s motion. For example, ground vehicles operating at high-speed can exhibit significant sideslip while turning. There is a rich literature on planning and trajectory generation for vehicle systems, including recent work described in [3], in which trajectories are computed in real-time for a kinematic vehicle model. The work that is most closely related to ours is reported in [8], in which a receding horizon approach is adopted and a specific terminal state cost is proposed that approximates the cost-to-go in an obstacle filled environment. We adopt a similar receding horizon approach, although our cost-to-go is different, and our formal use of a matching condition related to the cost-to-go allows us to derive a test for delaying global path updates. This paper is organized as follows. In Section II, we briefly describe our approach to computing a global path. In Section III, we describe how a sequence of dynamically feasible trajectories are computed in a receding horizon framework. In Section IV, we derive a so-called matching condition and show that the sequence of dynamically feasible trajectories is guaranteed to converge to a desired goal location if the matching condition is satisfied. An illustrative example is presented in Section V.
We assume that the vehicle operates in an environment Ω ⊂ R2 , and that the map is represented by the function g : Ω 7→ R ≥ 0. The vehicle starts at location zo ∈ Ω and travels to the goal location ξ ∈ Ω. We assume that the map g is zero at the goal location. The value of the map represents risk of traversal. For the USV application, locations that are identified as land or other hazards to navigation correspond to a high risk of traversal. Locations that are identified as water have a low risk of traversal. In practice, we compute initial maps from topological maps or from satellite imagery. For example, a satellite image of a small section of an operating area is shown in Figure 1. The prior map is created from satellite imagery by marking areas thought to be water with very low risk and areas that are thought to be land with very high risk.
shown in Figure 2 is the global path computed from a location on the map to the goal location. Recall that the direction of the global path is the negative gradient of the level set function. The global path in Figure 2 clearly displays that the level set is highly discretized in our application.
1200
1000
800 North(m)
II. M APPING AND GLOBAL PATH PLANNING
600
400
200
200
Fig. 2.
400
600
800 East(m)
1000
1200
1400
Level set function for the wrong map of the operating area.
Although we have utilized a level-set approach to path planning, we conjecture that any planning method that produces a cost-to-go will work within our framework. Examples of such algorithms include Dijkstra’s algorithm and path planners that use NF1 and NF2 algorithms over a potential field (see e.g. [1] and [2]). However, a formal analysis of this conjecture is on-going. III. L OCAL TRAJECTORY GENERATION Fig. 1.
Satelline imagery of an operating area.
We utilize the level set approach described in [14] to compute global paths, although this approach is nearly identical to the E∗ algorithm in [10]. The level set function Q : Ω 7→ R, which is computed using a fast marching method, has two important properties for our work. The direction of the globally optimal path at any point z ∈ Ω is −∇Q(z), and the value of the level set function Q(z) represents the cost-to-go from any location z to the goal location ξ. We do not directly make use of the global path in our work. Rather, we use the level set function Q(z) as the terminal cost when computing local trajectories. For illustration, the level set function corresponding to the map in Figure 1 is shown in Figure 2. Note that the map is purposefully incorrect. A large portion of the shore in approximately the center of the satellite image is removed in order to simulate, in Section V, an incomplete map available to the USV. The level set in Figure 2 is computed for a specific goal location at north = 85m and east = 1550m. The value of the level set function is indicated by shading. Lighter areas indicate smaller cost-to-go values and darker areas indicate higher cost-to-go values, respectively. Also
We assume that the dynamic model of the vehicle can be expressed x(t) ˙ = f (x(t), u(t)),
x(ti ) = xo ,
t ≥ ti
(1)
where x(t) ∈ X ⊂ Rn is the state of the vehicle and u(t) ∈ U ⊂ Rm is corresponding control signal. The sets X and U represent bounds on the vehicle state and actuator, respectively. We note that a subset of the elements of x correspond to position variables, and we may formally write [ ] x x= a xb where xa ∈ Ω ⊂ R2 represents the position of the vehicle. In the sequel we make use of the matrix L ∈ R2×n that is defined such that xa = Lx. (2) To generate a control signal for the vehicle, we compute a sequence of dynamically feasible state and control signal trajectories. For the interval t ∈ [ti , ti + T ], we compute a control signal that minimizes the cost function ∫ ti +T JT∗ (ti , xo ) = min q(τ, x(τ ), u(τ ))dτ +Wi (x(ti +T )) u∈U
ti
(3)
subject to the vehicle dynamics in (1). We define u∗i (t) with support [ti , ti + T ] to be a control signal that minimizes (3) and x∗i (t) to be the corresponding state trajectory. The integral cost q(t, x, u) is positive everywhere except when the vehicle is at the goal location ξ and the control signal is zero. Thus q(t, x, u) = 0 when ξ = Lx and u = 0. The terminal state cost Wi (x) is time-invariant over each optimization interval [ti , ti + T ], but we allow that Wi (x) can be different during each optimization interval. In our application, Wi (x) is the level set function that is used to generate a global path. Since the level set function is occasionally updated, the terminal state cost Wi (x) may also be occasionally updated. Only a portion of the control signal u∗i is applied to the vehicle system, and the remaining portion of the control signal is discarded. Specifically, we implement a control signal u ¯i (t) for t ∈ [ti , ti + δ] where the implementation horizon δ is less than the optimization horizon T . At time ti+1 := ti + δ we compute a new optimal control u∗i+1 (t) for the interval t ∈ [ti+1 , ti+1 + T ]. Thus we discard the portion of the suboptimal control signal corresponding to t ∈ [ti + δ, ti + T ). This results in a control signal and corresponding state trajectory u ¯(t) = u∗i (t), x ¯(t) = x∗i (t),
ti ≤ t < ti+1 ti ≤ t < ti+1
Note that the control signal is not required to be continuous at ti . Convergence of the system (1) when driven by the suboptimal control signal u ¯(t) is not guaranteed without imposing additional requirements on the optimization problem. It is generally recognized that the terminal state penalty Wi (x) plays an import role (e.g., see [6] and [9]). In our approach, the terminal state penalty is the key to combining global information with local trajectory generation. Indeed, we require the following, Assumption 3.1: The integral cost q(t, x(t), u(t)) and the terminal state cost Wi (x(t)) in (3), satisfy ˙ i (x(t)) ≤ 0 q(t, x(t), u(t)) + W
(4)
for all t ∈ [ti + T − δ, ti + T ]. Since we are addressing control of vehicle systems, it is not necessarily appropriate to consider convergence of the state x(t) to zero. Some states may correspond to position and velocity, while other states may correspond to orientation. If the objective of the control signal is to drive the system to a desired location, then we may desire that position and velocity states converge to zero, but that the steady-state value of the orientation state is arbitrary. Thus we do not seek to drive the state to zero. Rather, we seek to drive q(t, x, u) to zero. Theorem 3.2: If the integral cost q(t, x, u) and the terminal state penalty Wi (x) satisfy Assumption 3.1, then the
control signal u ¯(t) drives the system (1) so that lim q(t, x ¯(t), u ¯(t)) = 0. (5) t→∞ For continuity of exposition, the proof of Theorem 3.2 appears in the Appendix. IV. M ATCHING CONDITION The fundamental concept in our approach is to use the level set function Qi (z) from the global path planner as the terminal cost function Wi (x) in (3). Note that the level set function is indexed with the subscript i to indicate that it is occasionally updated to reflect changes to the map. Since the domain of level set function is Ω ∈ R2 , we use instead Qi (Lx) where L is defined in (2). Note that if we assign W (x) := Qi (Lx), then ˙ i (x) = ∇Qi (Lx) · L dx W dt = ∇Qi (Lx) · Lf (x, u) = ∇Qi (Lx) · x˙ a where xa is the position of the vehicle and x˙ a is the velocity of the vehicle. The inequality (4) is expressed q(t, x(t), u(t)) ≤ −∇Q(Lx(t)) · x˙ a
(6)
for all t ∈ [ti + T − δ, ti + T ]. Definition 4.1: We refer to the inequality in (6) as the matching condition. If the matching condition (6) is satisfied at the end of each trajectory interval, then the sequence of trajectories are such that q(t, x, u) converges to zero. In other words, we require that the matching condition is satisfied in order for the vehicle to converge to the goal location. If the matching condition is not satisfied, then we can update the level set function and compute the trajectory again. In addition, we can increase the local trajectory horizon T and recompute the trajectory. The matching condition is useful in our work by providing a formal test for when the level set function Qi (z) must be updated. So long as the matching condition is satisfied, a level set function can be used even if it is based on map data that is no longer accurate. If the matching condition is not met, then the following actions are taken: (i) the level set function is updated using the most recent map, (ii) the optimal trajectory is re-computed using the updated level set as terminal cost, and (iii) the matching condition is checked again. If the matching condition is not met when using the up-to-date level set, then the optimization horizon T can be increased, and the process started again. Note that Theorem 3.2 does not require that T is constant. However, in our work, we have focused on using the matching condition to determine when the level set function need be updated, and we have not addressed changing the optimization horizon T . The matching condition (6) possesses an useful physical interpretation. The right-hand side is related to the angle between the direction of the global path (−∇Qi (Lx)) and the direction of travel of the USV from the optimal trajectory (x˙ a ). Thus the matching condition can be interpreted as a test
V. I LLUSTRATIVE EXAMPLE We illustrate our guidance concepts with a computer simulation of an unmanned surface vehicle (USV) operating in a riverine system. The dynamic model of the USV, which appears in [12], is 1 K 0 − τβ 0 − τββ β˙ β ψ˙ = (7) 0 0 1 ψ + 0 Kr r r˙ 0 0 − τ1 τr r
where β is the side-slip angle, ψ is the yaw angle and r is the yaw rate of the USV, and Kβ , Kr , τβ and τr are gains and time constants that depend on the speed of the USV. For this simulation, we choose Kβ = 0.96, Kr = 1.44, τβ = 2.13 and τr = 2.22, which corresponds to 1.5 m/s in [12]. Position of the USV in north-east-down (Earth-fixed) coordinate system is computed by s˙ = ν cos (ψ + β) e˙ = ν sin (ψ + β)
(8)
where s is the north coordinate, e is the east coordinate and ν is the speed of the USV. Dynamically feasible local trajectories are computed by first transforming the optimal control problem (3) into a nonlinear programming (NLP) problem by approximating the control signal as piecewise constant over finite subintervals in the planning horizon [7], and then solving the resulting NLP using the Hooke-Jeeves algorithm [5]. The output of the Hookes-Jeeves algorithm is the optimal control sequence and the corresponding state trajectory for the USV. The optimization horizon is set to T = 60 seconds. The implementation horizon is set to δ = 6 seconds. The integral cost q(t, x, u) in (3) is very simple for the illustrative example presented herein. We choose q(t, x, u) = 0 if x corresponds to water, q(t, x, u) = 1 × 106 if x corresponds to land, and q(t, x, u) = 1 × 103 if the location x is within 10 meters of land. The latter cost ensures that the USV prefers not to be too close to land. When evaluating q(t, x, u), the up-to-date local map is used that includes all sensed information locally acquired by the USV. The map is implemented as a cellular decomposition of the environment where a scalar value that represents risk of traversal is attached to each cell. New sensor data
is integrated into the map using a standard Dempster-Shafer approach [11]. We note, however, that the level set function Qi (z) is based on an out-of-date map unless the matching condition is violated and an updated level set function is required. In Figure 3, we have simulated a shoreline that is found by the USV, but that does not appear in the prior map. New sensor data generates a thin shoreline that is appended to the original wrong map in real-time. This is similar to how a laser line scanner would perceive shoreline. However, this updated map information is not immediately reflected in the level set function. Figure 3 shows the dynamically feasible local trajectory (thick red line). The thin blue line is the global path starting at the location of the USV. It is not used directly in our approach, and it is shown only for illustration. The newly found shoreline is superimposed on the old level set function. It can be seen towards the end of the local trajectory that the direction of negative gradient of the level set function (blue arrow), which is the direction of the global path, diverges significantly from the direction of the local trajectory. This results in failure of the matching condition, and it denotes a significant mismatch between the map and the level set function. 650
600
550 North(m)
on the angle between the global path and the dynamically feasible local trajectory evaluated at the end of the trajectory. If the angle is small, then we expect the matching condition to be satisfied. Since the matching condition need only be satisfied during the last δ seconds of the trajectory, obstacle avoidance is implicitly addressed. The local dynamically feasible trajectory is permitted to steer clear of local obstacles. Finally, we note that the sequence of dynamically feasible local trajectories does not track the global path. Indeed, doing so might be a poor choice. Instead, the sequence of local trajectories need only approximate the direction of the global path at the end of every optimization interval.
500
450
400
350
450
500
Fig. 3.
550
600 650 East(m)
700
750
800
Failed matching condition.
Since the matching condition is not satisifed, the USV does not follow the local trajectory show in Figure 3. Failure of the matching condition triggers an update of the level set function so that the level set function reflects the most recent map data. Once the level set function is updated, a new dynamically feasible local trajectory is computed and the matching condition is re-evaluated using the updated level set function as a terminal state cost. The new trajectory is shown in Figure 4 (thick red line) along with the negative gradient of the level set (blue line) at the end of the trajectory. Recall that the negative gradient of the level set function is the direction of the global path. Now that the level set function represents newly sensed shoreline, local trajectory and the global path are approximately aligned at the end of the local trajectory, and the matching condition is satisfied.
indicated by red squares, allowing the USV to travel over a long distance without requiring frequent computationally expensive level set updates. This simulation demonstrates efficient planning of dynamically feasible local trajectories, as well as proposed scheme of delayed computation of the global path through level set function update.
650
600
500
450
1200
400
1000
350
Fig. 4.
800 450
500
550
600 650 East(m)
700
750
800
North(m)
North(m)
550
600
Dynamically feasible trajectory on updated level set function. 400
It is important to note that not all newly sensed changes to the map result in failure of the matching condition. Figure 5 shows such a case in which the local trajectory successfully circumvents newly found shoreline using an up-to-date map but an old level set function. Since the gradient of the level set function and the direction of the computed trajectory at the end of the trajectory do not diverge significantly, the matching condition is satisfied and no update of the level set function is required.
600
550 North(m)
0
0
500
1000
1500
East(m)
Fig. 6.
Path planning run in riverine environment.
VI. C ONCLUDING REMARKS We have described an approach to computing dynamically feasible trajectories using concepts from receding horizon control. The principal benefit of our approach is that global plans need be updated only infrequently, which means that the vast majority of computations are local and fast. The success of our approach is due to selection of the terminal state cost in (3). In general, the terminal state cost should be the cost-to-go. We have shown that the level set function is a suitable estimate of cost-to-go. Others, such as [8], have recognized the important role played by the terminal state cost, and have selected other functions to approximate the cost-to-go. On-going work seeks to identify necessary conditions on the terminal state cost.
650
500
450
400
350
200
450
Fig. 5.
500
550
600 650 East(m)
700
750
VII. ACKNOWLEDGMENTS
800
Dynamically feasible trajectory on updated map.
Figure 6 shows the result of using our approach to travel through the simulated river. In the simulation, the USV started at the position indicated by the red triangle. Obstacles shown in gray do not appear in the original map, and thus they are not accounted for by the initial level set function. As the USV travels closer to an obstacle, the obstacle becomes visible to the USV, thus simulating map update based on new sensor data. The USV does not update the level set function if the matching condition is satisfied. Along the entire run, the level set function is updated only twice, at positions
The authors gratefully acknowledge support of the Office of Naval Research via grant N00014-10-1-0185. VIII. A PPENDIX Proof of Theorem 3.2: Our approach is based on the proof in [4], although we choose a different form of the value function that results in a slightly different cost function. We seek to show that the value function JT −δ (ti , x ¯(ti )) := ∫ ti +T −δ q(τ, x ¯(τ ), u ¯(τ ))dτ + Wi (x(ti + T − δ)) ti
is monotonically decreasing with respect to increasing index i. For arbitrary index i, consider the difference JT −δ (ti+1 , x ¯(ti )) − JT −δ (ti , x ¯(ti )) ∫ ti +T = q(τ, x ¯(τ ), u ¯(τ ))dτ + Wi (¯ x(ti + T )) ∫ −
ti +δ ti +T −δ
(9)
it must be that q(τ, x ¯(τ ), u ¯(τ ))dτ − Wi (x(ti + T − δ))
ti +T −δ
+ Wi (x∗ (ti + T )) − Wi (x∗ (ti + T − δ)) ≤ 0
and thus Wi (x∗ (ti + T )) − Wi (x∗ (ti + T − δ)) ≤ ∫ ti +T − q(τ, x∗ (τ ), u∗ (τ ))dτ ti +T −δ
From (9) JT −δ (ti+1 , x ¯(ti )) − JT −δ (ti , x ¯(ti )) ∫ ti +T ∫ ti +T −δ ≤ q(τ, x ¯(τ ), u ¯(τ ))dτ − q(τ, x ¯(τ ), u ¯(τ ))dτ ti +δ
∫ ≤−
∫
tk
q(τ, x ¯(τ ), u ¯(τ ))dτ t0
where we have recalled that ti+1 = ti + δ. From the ˙ i (x∗ (t)) ≤ 0 for requirement that q(t, x∗ (t), u∗ (t)) + W t ∈ [ti + T − δ, ti + T ], we note that ∫ ti +T q(τ, x∗ (τ ), u∗ (τ ))dτ
−
t0
JT −δ (t0 , x∗ (t0 ), u∗ (t0 ))
ti
∫
By recalling that q(t, x, u) ≥ 0 and rearranging (10) ∫ tk JT −δ (tk ), x∗ (tk )) + q(τ, x ¯(τ ), u ¯(τ ))dτ ≤
ti ti +T
q(τ, x∗ (τ ), u∗ (τ ))dτ
ti +T −δ ti +δ
q(τ, x ¯(τ ), u ¯(τ ))dτ ti
Now we show that this property holds as the difference interval is increased. Note that for arbitrary positive integer k, JT −δ (tk ), x∗ (tk )) − JT −δ (t0 , x∗ (t0 ), u∗ (t0 )) = JT −δ (tk , x∗ (tk ), u∗ (tk )) − JT −δ (tk−1 , x∗ (tk−1 ), u∗ (tk−1 )) + JT −δ (tk−1 , x∗ (tk−1 ), u∗ (tk−1 ))− JT −δ (tk−2 , x∗ (tk−2 ), u∗ (tk−2 )) · · · + JT −δ (t1 , x∗ (t1 ), u∗ (t1 )) − JT −δ (t0 , x∗ (t0 ), u∗ (t0 )) Thus JT −δ (tk ), x∗ (tk )) − JT −δ (t0 , x∗ (t0 ), u∗ (t0 )) ≤ k−1 ∑ ∫ ti +δ q(τ, x ¯(τ ), u ¯(τ ))dτ − i=0
ti
and we conclude that JT −δ (tk ), x∗ (tk )) − JT −δ (t0 , x∗ (t0 ), u∗ (t0 )) ≤ ∫ tk − q(τ, x ¯(τ ), u ¯(τ ))dτ t0
(10)
is bounded for all k. Thus q(τ, x ¯(τ ), u ¯(τ )) → 0 as k → ∞. R EFERENCES [1] J Barraquand, JC Latombe, 1991, Robot Motion Planning: A Distribted Representation Approach, in The International Journal of Robotics Research, Vol. 10, No. 6, pp.628–649. [2] J Barraquand, B Langlois, JC Latombe, 1992, Numerical Potential Field Techniques for Robot Path Planning, in IEEE Transactions on Systems, Man, and Cybernetics, Vol. 22. No. 2, pp.224–241. [3] D. Dolgov, S. Thrun, M. Montemerlo, J. Diebel, 2010, Path Planning for Autonomous Vehicles in Unknown Semi-structured Environments, in The International Journal of Robotics Research, 29(5):485-501. [4] F. A. A. C Fontes, 2001, A general framework to design stabilizing noninear model predictive controllers, Systems and Control Letters, 42:127–143. [5] R Hooke, TA Jeeves, 1961, Direct Search Solution of Numerical and Statistical Problems, in Journal of the ACM, Vol. 8, No. 2, pp.212–229. [6] A Jadbabaie, J Yu, and J Hauser, 1999, “Stabilizing receding horizon control of nonlinear systems: a control Lyapunov function approach,” Proc. of the American Control Conference, pp. 15351539. [7] D Kraft, 1985, On Converting Optimal Control Problems into Nonlinear Programming Problems, in Nato ASI Series, Vol. 15, pp.261–280. [8] Y Kuwata, A Richards, T Schouwenaars, JP How, 2006,“Decentralized Robust Receding Horizon Control for Multi-Vehicle Guidance,” in Proc. American Control Conference, Minneapolis, MN, pp. 2047– 2052. [9] DQ Mayne, JB Rawlings, CV Rao and POM Scokaert, 2000, “Survey Paper: Constrained model predictive control: Stability and optimality”, Automatica, 36, pp. 789-814. [10] R Philippsen, R Siegwart, 2005, An Interpolated Dynamic Navigation Function, in Proceedings of the IEEE International Conference on Robotics and Automation, pp.3782–3789, Barcilona, Spain. [11] G Shafer, 1976, A Mathematical Theory of Evidence, Princeton University Press, New Jersey, 1976. [12] C Sonnenburg, A Gadre, D Horner, S Kragelund, A Marcus, DJ Stilwell, CA Woolsey, 2010, Control-oriented planar motion modeling of unmanned surface vehicles, Proc. IEEE/MTS OCEANS, Seattle, WA. [13] A Stentz, 1993, Optimal and Efficient Path Planning for Unknown and Dynamic Environments, in International Journal of Robotics and Automation, Vol. 10, pp.89–100. [14] B Xu, DJ Stilwell, AJ Kurdila, 2009, Efficient computation of level sets for path planning, in IEEE/RSJ Int’l Conf. on Intelligent Robots and Systems, St. Louis, MO.
