On Provably Safe Obstacle Avoidance for Autonomous Robotic Ground Vehicles

Transcription

1 On Provaly Safe Ostacle Avoidance for Autonomous Rootic Ground Vehicles Stefan Mitsch, Khalil Ghoral and André Platzer Carnegie Mellon University, Computer Science Department 5000 Fores Avenue, Pittsurgh PA 15213, USA, WWW home page: Astract Nowadays, roots interact more frequently with a dynamic environment outside limited manufacturing sites and in close proximity with humans. Thus, safety of motion and ostacle avoidance are vital safety features of such roots. We formally study two safety properties of avoiding oth stationary and moving ostacles: i) passive safety, which ensures that no collisions can happen while the root moves, and ii) the stronger passive friendly safety in which the root further maintains sufficient maneuvering distance for ostacles to avoid collision as well. We use hyrid system models and theorem proving techniques that descrie and formally verify the root s discrete control decisions along with its continuous, physical motion. Moreover, we formally prove that safety can still e guaranteed despite location and actuator uncertainty. I. INTRODUCTION With the increased introduction of autonomous rootic ground vehicles as consumer products such as autonomous household appliances [7] or driverless cars on regular Californian roads 1 we face an increased need for ensuring safety not only for the sake of the consumer, ut also the manufacturer. Since those roots are designed for environments that occupy stationary as well as moving ostacles, motion safety and ostacle avoidance are vital safety features for such roots [3, 22, 25, 27]. In this paper, we prove safety for the ostacle avoidance control algorithm of a root. One of the conceptual difficulties is what safety means for an autonomous root. We would want it to e collision-free, ut that usually requires other vehicles to e sensile, e. g., not actively try to run into the root when it is just stopped in a corner. One way of doing that is to assume stringent constraints on the ehavior of ostacles see, e. g., [3, 12]). In this paper, we want to refrain from doing so and allow aritrary ostacles with an aritrary continuous motion with a known upper ound on their velocity. Then our root is safe, intuitively, if no collision can ever happen where the root is to lame. The first notion we consider is passive safety [14]. Passive safety can e guaranteed with minimal assumptions aout ostacles. It requires that the root does not actively collide, i. e., if a collision occurs at all then only while the root is stopped and the moving) ostacle ran into the root. The difficulty with that notion is that it still allows the root to go kamikaze and stop in unsafe places, creating unavoidale collision situations in which an ostacle has no control choices left that would prevent a collision. The second notion we consider is passive friendly safety [14], which aims 1 r=0 for more careful root decisions that respect the features of moving ostacles e. g., their raking capailities). Thus, a passive friendly root not only ensures that it is itself ale to stop efore a collision occurs, ut it also maintains sufficient maneuvering room for ostacles to avoid a collision as well. Motion safety and ostacle avoidance lead to interesting cognitive rootics questions: how much does the root have to know aout the goals and constraints of other vehicles so as not to e considered to lame? In this paper, we successively construct models and proofs that increase the level of assumed knowledge and explicitly expressed uncertainty. We start with i) passive safety, which assumes a known upper ound on the velocity of ostacles. Then we extend to ii) passive friendly safety for a known lower ound of an ostacle s raking power and consider an upper ound on the reaction time that we allow an ostacle to start collision avoidance attempts. Note that all our models use symolic ounds so they hold for all choices of the ounds. As a result, we can account for uncertainty in several places e. g., y instantiating upper ounds on acceleration or time with values including uncertainty). We additionally show how further uncertainty that cannot e attriuted to such ounds in particular location and actuator uncertainty) can e modeled and verified explicitly. We use the well-known dynamic window algorithm [9] and verify passive safety and passive friendly safety. Unlike existing work on ostacle avoidance e. g., [1, 17, 24 26]), we use hyrid models and verification techniques that descrie and verify the root s discrete control choices along with its continuous, physical motion. In summary, our contriutions are i) hyrid models of navigation and ostacle avoidance control algorithms of roots, and ii) proofs that they guarantee passive and passive friendly safety in the presence of stationary and moving ostacles despite sensor and actuator uncertainty. The models and proofs of this paper are undled with KeYmaera. 2 The paper is organized as follows. In the next section, we discuss related work on navigation and ostacle avoidance of roots that focuses on verification. Section III recalls differential dynamic logic that we use as a modeling formalism for the hyrid dynamics of a root and the safety constraints. In Sect. IV, we introduce models of ostacle avoidance control and physical motion, and prove that they guarantee passive safety and passive friendly safety with stationary as well as moving ostacles. We then model uncertainty explicitly and prove that motion is still safe. Sect. V concludes the paper. 2 The author Pulished In Proceedings of Rootics: Science and Systems, DOI: /RSS.2013.IX.014

2 II. RELATED WORK Isaelle has recently een used to formally verify that a C program implements the specification of the dynamic window algorithm [25]. This is interesting, ut the algorithm itself and its impact on motion of the root was considered in an informal pen-and-paper argument only. We, instead, formally verify the correctness of the dynamic window algorithm using a hyrid verification technique. Thus, we complement the work in [25] in a twofold manner: First, we create hyrid models of the control and the motion dynamics of the root and formally verify correctness of the dynamic window algorithm control for the comined dynamics. Second, we model stationary as well as moving ostacles and prove two versions of safety. These complementary results together present a strong safety argument from concept this paper) to implementation [25]. PASSAVOID [3] is a navigation scheme, which avoids raking inevitale collision states i. e., states that regardless of the root s trajectory lead to a collision) to achieve safety in the presence of moving ostacles. Since PASSAVOID is designed to operate in completely unknown environments, it ensures that the root is at rest when a collision occurs passive safety). The motion dynamics of the root have only een considered in simulation. We prove the stronger passive friendly safety using a hyrid verification technique i. e., algorithm and motion dynamics), which ensures that the root does not create unavoidale collision situations y stopping in unsafe places. For the purpose of guaranteeing infinite horizon safety, velocity ostacle sets [27] assume unpredictale ehavior for ostacles with known forward speed and maximum turn rate i. e., Duin s cars). The authors focus only on the ostacle ehavior; the root s motion is explicitly excluded from their work. We complement their work and show that a root, which has a known upper ound on its reaction time and considers discs as velocity ostacle sets i. e., known forward speed and unknown turn rate, as allowed y [27]), moves safely. Hyrid models of driver support systems in cars [12, 16] have een verified with a model of the continuous dynamics of cars. That points out interesting safety conditions for vehicles on straight lines, ut not in the general two-dimensional case that we consider in this work. Safety of aircraft collision avoidance maneuvers in the two-dimensional plane was verified for constant translational velocity and a rotational velocity that stays constant during the maneuver [13, 22]. Our models include acceleration for oth translational and rotational velocity. LTLMoP contains an approach [23] to guarantee highlevel ehavior for map exploration when the environment is continuously updated. The approach synthesizes and resynthesizes plans, expressed in linear temporal logic, of a hyrid controller, when new map information is discovered. This work focuses on preserving the state and task completion history, and thus on guaranteeing that the root will follow a high-level ehavior e. g., visit all rooms) even when the controller is re-synthesized, not on safe ostacle avoidance. Pan et al. [17] propose a method to smooth the trajectories produced y sampling-ased planners in a collision-free manner. Our paper proves that such trajectories are indeed safe when considering the control choices of a root and its continuous dynamics. LQG-MP [26] is a motion planning approach that takes into account the sensors, controllers, and motion dynamics of a root while working with uncertain information aout the environment. The approach assesses randomly generated paths y the approximated proaility of a collision with an ostacle. One goal is to select paths with low collision proaility; however, guaranteeing collision-free motion is not their focus, since a collision-free path may not even have een generated. Althoff et al. [1] use a proailistic approach to rank trajectories according to their collision proaility. To further refine such a ranking, a collision cost metric is proposed, which derives the cost of a potential collision y considering the relative speeds and masses of the collision ojects. Seward et al. [24] try to avoid potentially hazardous situations y using Partially Oservale Markov Decision Processes. Their focus, however, is on a user-definale tradeoff etween safety and progress towards a goal. Hence, safety is not guaranteed under all circumstances. In summary, this paper addresses safety of root ostacle avoidance in the following manner. Unlike [3, 23 25, 27], we study comined models of the hyrid dynamics in terms of discrete control and differential equations for continuous physical motion of the root as well as the ostacles, not discrete control alone or only the ehavior of ostacles. Unlike [12, 13, 16, 22, 25], we verify safety in the two-dimensional plane not one-dimensional space and do not assume constant translational and rotational velocity, ut include accelerations for oth, as needed for ground vehicles. Unlike [1, 17, 23, 24, 26], we produce formal, deductive proofs in a formal verification tool. Note, that in [25] the correctness of the implementation w.r.t. the algorithm s specification was formally verified, ut not the motion of the root. Unlike [17, 23, 25], we verify safety even in the presence of moving ostacles. Unlike [3, 17, 23, 25], we verify passive friendly safety, which is important ecause passive non-friendly) safe roots may cause unavoidale collisions y stopping in unsafe places so that ostacles will collide with them. Unlike [3, 12, 22, 23], we consider sensor and actuator uncertainty in our verification results. Unlike [1, 24, 26], we do not minimize or proailistically minimize collisions, ut prove that collisions can never occur as long as the root system fits to the model). III. PRELIMINARIES: DIFFERENTIAL DYNAMIC LOGIC A root and the moving ostacles in its environment form a hyrid system: they make discrete control choices e. g., compute the actuator set values for acceleration, raking, or steering), which in turn influence their actual physical ehavior e. g., slow down to a stop, move along a curve). Hyrid systems have een considered as joint models for oth components, since verification of either component alone does not capture the full ehavior of a root and its environment.

3 TABLE I: Hyrid program representations of hyrid systems. Statement Effect α; β sequential composition, first run α, then β α β nondeterministic choice, following either α or β α nondeterministic repetition, repeats α n 0 times x := θ assign value of term θ to variale x discrete jump) x := assign aritrary real numer to variale x x 1 = θ1,..., evolve xi along differential equation system x i = θi x n = θn & F ) restricted to maximum evolution domain F In order to verify safe ostacle avoidance, we use differential dynamic logic dl [18, 20, 21], which has a notation for hyrid systems as hyrid programs. We use hyrid programs for modeling a root that follows the dynamic window algorithm as well as for modeling the ehavior of moving ostacles. dl allows us to make statements that we want to e true for all runs of the program safety) or for at least one run liveness). Both constructs are necessary to verify safety: for all possile control choices and entailed physical motion, our root must e ale to stop, while at the same time there must e at least one possile execution in which the ostacle is ale to stop without collision as well. Tale I summarizes the syntax of hyrid programs together with an informal semantics. Sequential composition α; β says that β starts after α finishes e. g., first let the root choose acceleration, then steering angle). The nondeterministic choice α β follows either α or β e. g., let the root decide nondeterministically etween remaining stopped or accelerating). The nondeterministic repetition operator α repeats α zero or more times e. g., let the root repeatedly control and drive). Assignment x := θ instantaneously assigns the value of the term θ to the variale x e. g., let the root choose maximum raking), while x := assigns an aritrary value to x e. g., let the root choose any acceleration). x = θ & F descries a continuous evolution of x within the evolution domain F e. g., let the velocity of the root change according to its acceleration, ut never lower than zero). The test?f checks that a particular condition F holds, and aorts if it does not e. g., test whether or not the distance to an ostacle is large enough). A typical pattern that involves assignment and tests is to limit the assignment of aritrary values to known ounds e. g., limit an aritrarily chosen acceleration to the physical limits of the root, as in x := ;?x A, which says x is any value less or equal A). The set of dl formulas is generated y the following EBNF grammar where {<,, =,, >} and θ 1, θ 2 are arithmetic expressions in +,,, / over the reals): φ ::= θ 1 θ 2 φ φ ψ φ ψ φ ψ xφ [α]φ α φ Further operations, such as Euclidian norm θ and infinity norm θ of a vector θ, are definale. To specify the desired correctness properties of hyrid programs, dl formulas of the form F [α]g mean that all executions of the hyrid program α, which start at a state in which formula F is true, lead to states in which formula G is true. Dually, formula F α G expresses that there is a state reachale y the hyrid program α that satisfies formula G. Differential dynamic logic comes with a verification technique to prove those correctness properties. We did all our proofs in the verification tool KeYmaera, which implements this verification technique [18 20]. KeYmaera supports hyrid systems with nonlinear discrete jumps, nonlinear differential equations, differential-algeraic equations, differential inequalities, and systems with nondeterministic discrete or continuous input. This makes KeYmaera more readily applicale to rootic verification than other hyrid system verification tools, such as SpaceEx [10], which focuses on piecewise linear systems. KeYmaera implements automatic proof strategies that decompose hyrid systems symolically [20]. This compositional verification principle helps scaling up verification, ecause KeYmaera verifies a ig system y verifying properties of susystems. Strong theoretical properties, including relative completeness results, have een shown aout dl [18, 21] indicating how this composition principle can e successful. IV. ROBOTIC GROUND VEHICLE NAVIGATION The rootics community has come up with a large variety of root designs, which differ not only in their tool equipment, ut also and more importantly for the discussion in this paper) in their kinematic capailities. We focus on wheel-ased vehicles. In order to make our models applicale to a large variety of roots, we use only limited control options e. g., do not move sideways to avoid collisions). We consider roots that drive forward non-negative translational velocity) in sequences of arcs in two-dimensional space. 3 Such trajectories can e realized y roots with single wheel drive, differential drive, Ackermann drive, synchro drive, or omni drive [4]. Many different navigation and ostacle avoidance algorithms have een proposed for such roots, e. g. dynamic window [9], potential fields [11], or velocity ostacles [8]. For an introduction to various navigation approaches for moile roots, see [2, 6]. In this paper, we focus on the dynamic window algorithm [9], which is derived from the motion dynamics of the root and thus discusses all aspects of a hyrid system models of discrete and continuous dynamics). Other approaches, such as velocity ostacles [8], are interesting for further work on provaly safe moving ostacle avoidance. We want to prove motion safety of a root that avoids ostacles y dynamic window navigation. Under passive safety [14], the vehicle is in a safe state if it is ale to come to a full stop efore making contact with an ostacle i. e., the vehicle does not itself collide with ostacles, so if a collision occurs at all then while the vehicle was stopped). Passive safety, however, puts the urden of avoiding collisions mainly on other ojects. The safety condition that we want to prove additionally is the stronger passive friendly safety [14]: we want to guarantee that our root will come to a full stop safely under all circumstances, ut will also leave sufficient maneuvering room for moving ostacles to avoid a collision. 4 We consider oth models and oth safety properties to show the differences etween the assumed knowledge and the safety guarantees that can e made. The verification effort and complexity difference is quite instructive: passive safety can e guaranteed y proving safety of all root choices, whereas passive friendly safety additionally requires liveness proofs for 3 If the radius of such a circle is infinite, the root drives forward) on a straight line. 4 The root ensures that there is enough room for the ostacle to stop efore a collision occurs. If the ostacle decides not to, the ostacle is to lame and our root is still considered safe.

4 v r 2 1 v r w r p r x 10 5 pr y v r p r x p r x w r t pr y a) Position p x r, py r ), translational velocity v r and rotational velocity ω r for positive acceleration on a circle pr y ) Position p x r, py r ), translational velocity v r and rotational velocity ω r for raking to a complete stop on a circle. w r c) Position p x r, py r ), translational velocity v r and rotational velocity ω r for translational acceleration on a spiral d) p x r, p y r ) motion plot for acceleration a) e) p x r, p y r ) motion plot for raking ). Fig. 1: Trajectories of the root. f) p x r, p y r ) motion plot for c). the ostacle. In the following sections, we discuss models and verification of the dynamic window algorithm in detail. A. Passive Safety of Ostacle Avoidance The dynamic window algorithm is an ostacle avoidance approach for moile roots equipped with synchro drive [9] ut can e used for other drives too [5]. It uses circular trajectories that are uniquely determined y pairs of translational and rotational velocity v, ω). The algorithm is roughly organized into two steps: i) The range of all possile pairs of translational and rotational velocities is reduced to admissile ones that result in safe trajectories i. e., avoid collisions since those trajectories allow the root to stop efore it reaches the nearest ostacle). The admissile pairs are further restricted to those that can e realized y the root within a short time frame the so-called dynamic window). If the set of admissile and realizale velocities is empty, the algorithm stays on the previous safe curve such curve exists unless the root started in an unsafe state). ii) Progress towards the goal is optimized y maximizing a goal function. For safety verification, we can omit the second step and verify the stronger property that all choices that are fed into the optimization are safe. a) Modeling: We develop a model of the principles in the dynamic window algorithm as a hyrid program in dl. The dynamic window algorithm safely astracts the root s shape to a single point, since other shapes reduce to adjusting the virtual) shapes of the ostacles cf. [15] for an approach to attriute root shape to ostacles). We also use this astraction to reduce the verification complexity. The root has state variales descriing its current position p r = p x r, p y r), translational velocity v r 0, translational acceleration a r, a direction vector d r = cos θ, sin θ), 5 and angular velocity θ = ω r. 6 The translational and rotational velocities are linked w.r.t. the rigid ody planar motion y the formula p r p c ω r = v r, where the radius p r p c is the distance etween the root and the center of its current curve p c = p x c, p y c). Following [19], we encode sine and cosine functions in the dynamics using the extra variales d x r = cos θ and d y r = sin θ to avoid undecidale arithmetic. The continuous dynamics for the dynamic window algorithm [9] can e descried y the differential equation system of ideal-world dynamics of the planar rigid ody motion p x r = v r d x r, p y r = v r d y r, v r = a r, p r p c ω r ) = a r, d x r = ω r d y r, d y r = ω r d x r ) where the condition p r p c ω r ) = a r encodes the rigid ody planar motion p r p c ω r = v r that we consider. The dynamic window algorithm assumes direct control of the translational velocity v r. We, instead, control acceleration a r and do not perform instant changes of the velocity. Our model is closer to the actual dynamics of a root, which cannot really change its velocity instantly. The realizale velocities, then, follow from the differential equation system. Figure 1a) depicts the position and velocity changes of a root accelerating on a circle around a center point p c = 2, 0). The root starts at p r = 0, 0) as initial position, with v r = 2 as initial translational velocity and ω r = 1 as initial rotational velocity; Figure 1d) shows the resulting circular trajectory. Figure 1) and Figure 1e) show the resulting 5 As stated earlier, we study unidirectional motion: the root moves along its direction, that is the vector d r gives the direction of the velocity vector. 6 The derivative with respect to time is denoted y superscript prime ).

5 curve when raking the root rakes along the curve and comes to a complete stop efore completing the circle). If the rotational velocity is constant ω r = 0), the root drives an Archimedean spiral with the translational and rotational accelerations controlling the spiral s separation distance a r /ω 2 r). The corresponding plots are shown in Figures 1c) and 1f). As in the dynamic window algorithm, we assume ounds for the acceleration a r in terms of a maximum acceleration A 0 and raking power > 0, as well as a ound Ω on the rotational velocity ω r. We use ε to denote the upper ound for the control loop time interval e. g., sensor and actuator delays, sampling rate, and computation time). That is, the root may react as quickly as it wants, ut it can take no longer than ε. Notice that, without such a time ound, the root would not e safe, ecause its control might never run. In our model, all these ounds will e used as symolic parameters and not concrete numers. Therefore, our results apply to all values of these parameters and can e enlarged to include uncertainty. An ostacle has vector state variales descriing its current position p o = p x o, p y o) and velocity v o = v x o, v y o). The ostacle model is very lieral. The only restriction aout the dynamics is, that within ε time units, they move continuously with ounded velocity v o V. Note, that the dynamic window algorithm considers a special case V = 0 ostacles are stationary). Depending on the relation of V to ε, moving ostacles can make quite a difference, e. g., when fast ostacles meet communication-ased virtual sensors as in RooCup. 7 In order to determine admissile velocities, the dynamic window algorithm requires the distance to the nearest ostacle for every possile curve. In the presence of moving ostacles, however, all ostacles must e considered and tested for safety e. g., in a loop). To capture this requirement, our model nondeterministically picks any ostacle p o :=, ) and tests its safety, which includes safety for the worst-case ehavior V of the nearest ostacle ties are included) and is thus safe for all possile ostacles. In the case of non-point ostacles, p o denotes the ostacle perimeter point that is closest to the root this fits naturally to ostacle point sets delivered y radar sensors, from which the closest point will e chosen). At each active step of the root, the position p o is updated again nondeterministically to summarize all ostacles). In this process, the root may or may not find another safe trajectory. If it does, the root can follow that safe trajectory w.r.t. any nondeterministically chosen ostacle again, V ensures that all other ostacles will stay more distant than the worst case of the nearest). If not, the root can still rake on the previous trajectory, which is known to e safe. Model 1 represents the common controller-plant model: it repeatedly executes control choices followed y dynamics, cf. 1). For the sake of clarity we restrict the study to circular trajectories with constant positive radius, that is p r p c > 0, which also covers straight line trajectories if the radius is infinite. Thus, the condition p r p c ω r ) = a r can e a r p r p c rewritten as ω r =. We have undled adjusted dynamics for spinning ehavior p r p c = 0, ω r 0) and Archimedean spiral ω r = 0, a r 0) with KeYmaera. The control of the root is executed in parallel to that of the ostacle, cf. 2). The ostacle itself may choose any velocity 7 Model 1 Dynamic window with passive safety dw ps ctrl; dyn) 1) ctrl ctrl o ctrl r 2) ctrl o v o :=, );? v o V 3) ctrl r a r := ) 4)?v r = 0; a r := 0; ω r := 0) 5) a r := ;? a r A; 6) ω r := ;? Ω ω r Ω; 7) p c :=, ); d r :=, ); 8) p o :=, );?curve safe) 9) curve p r p c > 0 ω r p r p c = v r 10) d r = p r p c ) p r p c ) ) safe p r p o > v2 r A A ε2 + εv r 11) + V ε + v ) r + Aε 12) dyn t := 0; p x r = v r d x r, p y r = v r d y r, 13) d x r = ω r d y r, d y r = ω r d x r, 14) p x o = vo x, p y o = vo, y 15) v r = a r, ω r a r = p r p c, t = 1 16) & v r 0 t ε) 17) in any direction up to the worst-case velocity V assumed aout ostacles v o V ), cf. 3). This uses the modeling pattern from Section III: we assign an aritrary value to the ostacle s velocity v o :=, )), which is then restricted to any value up to the worst-case velocity using a susequent test? v o V ). The root is allowed to rake at all times since 4) has no test. If the root is stopped, it may choose to stay in its current spot, cf. 5). Finally, the root may choose a new safe curve in its dynamic window: it chooses any acceleration within the ounds of its raking power and acceleration 6), and any rotational velocity in the ounds, cf. 7). This corresponds to testing all possile acceleration and rotational velocity values at the same time. An actual implementation would use loops to enumerate all possile values and all ostacles and test each pair v r, ω r ) separately w.r.t. every ostacle. The admissile pairs would e stored in a data structure as e. g., in [25]). The curve is determined y the root following a circular trajectory around the rotation center p c, starting in direction d r with angular velocity ω r, cf. 8). The distance to the nearest ostacle on that curve is measured in 9). The trajectory starts at p r tangential to the circle centered at p c which has positive radius with translational velocity v r and rotational velocity ω r as defined y ω r p r p c = v r in 10). 8 A circular trajectory around the rotation center p c ensures passive safety if it allows the root to stop efore it collides with the nearest ostacle. Consider the worst case where the 8 d r = pr pc) p r p c means component-wise equality of the orthonormal vector, normalized y the radius, as in d x r = py r py c p r p c dy r = px r px c p r p c.

6 center p c is at infinity and the root travels on a straight line. In this case, the distance etween the root s current position p r and the nearest ostacle p o must account for the following components: First, the root needs to e ale to rake from v 2 r 2 = its current velocity v r to a complete stand still, cf. vr/ v 0 r t)dt in 11). Second, the root may not react efore ε time units. Thus, we must additionally take into account the distance that the root may travel w.r.t. the worst-case acceleration A and the distance needed for compensating its potential acceleration of A during that time with raking power, cf. A + 1) ) A ε 2 ε2 + εv r = 0 v r+at)dt+ Aε/ v 0 r +Aε t)dt in 11). Third, during the worst-case time ε+ vr+aε ) entailed y 11), the ostacle may approach the root in the worst case on a straight line with maximum velocity V, cf. 12). To simplify the proof, we measure the distance etween the root s position p r and the ostacle s position p o in the infinitynorm p r p o i. e., oth p x r p x o and p y r p y o must e safe) and thus over-approximate the Euclidean norm distance p r p o 2 = p x r p x o) 2 + p y r p y o) 2 y a factor of at most 2. Finally, the continuous dynamics of the root and the ostacle are defined in 13) 17). ) Verification: We verify the safety of the control algorithm modeled as a hyrid program in Model 1, using a formal proof calculus for dl [18 20]. The root is at a safe distance from the ostacle, if it is ale to rake to a complete stop at all times efore the approaching ostacle reaches the root. The following condition captures this requirement as a property that we prove to hold for all executions: φ ps v r = 0) p r p o > v2 r 2 + V v ) r. 18) The formula 18) states that the root is stopped or the root and the ostacle have different positions safely apart. This accounts for the root s raking distance v2 r 2 while the ostacle is allowed to approach the root with its worst case travel distance V vr. We prove that the property 18) holds for all executions of Model 1 under the assumption that we start in a state satisfying the following conditions: 9 ψ ps φ ps p r p c > 0) d r = 1). 19) The first condition of the conjunction formalizes the fact that we are starting in a position that is passively safe, that is φ ps holds. The second conjunct states that the root is not initially spinning. The last conjunct d r = 1 says that the direction d r is a unit vector. Theorem 1 Passive safety): If the root starts in a state where ψ ps 19) holds, then the control model dw ps Model 1) always guarantees the passive safety condition φ ps ), as expressed y the provale dl formula: ψ ps [dw ps ]φ ps. We proved Theorem 1 for circular trajectories, spinning, and spiral trajectories using KeYmaera, a theorem prover for hyrid systems. In the next section, we explore the stronger requirements of passive friendly safety, where the root will not only safely stop itself, ut also allow for the ostacle to stop efore a collision occurs. 9 The formal proof uses the additional constraints stated earlier, v r 0, A 0, V 0, Ω 0, > 0, and ε > 0, which we leave out for simplicity. B. Passive Friendly Safety of Ostacle Avoidance Passive friendly safety, as introduced aove, requires the root to take careful decisions that respect the features of moving ostacles. The definition of Maček et al. [14] requires that the root respects the worst-case raking time of the ostacle. In our model, the worst-case raking time is represented as follows. We assume an upper ound τ on the reaction time of the ostacle and a lower ound o on its raking capailities. Then, τ V is the worst-case distance that the ostacle can travel efore actually reacting and V 2 2 o is the distance for the ostacle to stop from the worst-case velocity V with an assumed minimum raking capaility o. c) Modeling: Model 2 uses the same ostacle avoidance algorithm as Model 1. The essential difference reflects what the root considers to e a safe distance to an ostacle. As shown in 21) the distance not only accounts for the root s own raking distance, ut also for the raking distance V 2 2 o and reaction time τ of the ostacle. The verification of passive friendly safety is more complicated than passive safety as it accounts for the ehavior of the ostacle discussed elow. Model 2 Dynamic window with passive friendly safety dw pfs ctrl; dyn) see Model 1 for details) 20) safe p r p o > v2 r 2 + V 2 + τv 21) ) 2 o ) A A ε2 + εv r + V ε + v ) r + Aε d) Verification: We verify the safety of the root s control choices as modeled in Model 2. Unlike the passive safety case, the passive friendly safety property φ pfs should guarantee that if the root stops, moving ostacles cf. Model 3) still have enough time and space to avoid a collision. This requirement can e captured y the following dl formula: η pfs v r = 0) p r p o > V 2 ) ) + τv 0 v o V ) 2 o ostacle p r p o > 0) v o = 0)). 22) Equation 22) says that, once the root stops v r = 0), there exists an execution of the hyrid program ostacle, existence of a run is formalized y the diamond operator ostacle ), that allows the ostacle to stop v o = 0) efore a collision occurs p r p o > 0). Passive friendly safety is now stated as φ pfs ϕ ps v r = 0) η pfs, where the property ϕ ps is analogue to the passive safety property φ ps w.r.t. Model 2: ϕ ps p r p o > v2 r 2 + V 2 2 o + V vr + τ ) We study the passive friendly safety with respect to the initial feasile states satisfying the following property: ψ pfs ϕ ps p r p c > 0) d r = 1). 23) Oserve that, in addition to the condition η pfs, the difference with the passive safety condition is reflected in the special treatment of the case v r = 0. Indeed, if the root would start.

7 with a null translational velocity which is passive safety) while not satisfying ϕ ps, then we cannot prove the passive friendly safety as the ostacle may e unsafely close already initially. Besides, we can see in φ pfs that we are required to prove η pfs even when the root comes to a full stop. In Model 2 the hyrid program ctrl o is a coarse model given y equation 3), which only constrains its non-negative velocity to e less than or equal to V. Such an ostacle could trivially prevent a collision y stopping instantaneously. In η pfs, we consider a more interesting refined ostacle ehavior modeled y the hyrid program given in Model 3. Model 3 Refined ostacle controls acceleration ostacle ctrl or ; dyn or ) 24) ctrl or d o :=, );? d o = 1; 25) a o := ;?v o + a o ε o V 26) dyn or t := 0; p x o = v o d x o, p y o = v o d y o, 27) v o = a o, t = 1 & t ε o v o 0) The refined ostacle may choose any direction as descried y the unit vector d o in 25) and any acceleration a o, as long as it does not exceed the velocity ound V, cf. 26). The dynamics of the ostacle are straight ideal-world translational motion in the two-dimensional plane, see 27). Theorem 2 Passive friendly safety): If the property ψ pfs 23) holds initially, then the control model dw pfs Model 2), guarantees the passive friendly safety condition φ pfs in presence of ostacles per Model 3, as expressed y the provale dl formula: ψ pfs [dw pfs ]φ pfs. We verified Theorem 2 in KeYmaera. The symolic ounds on velocity, acceleration, raking, and time in the aove models represent uncertainty implicitly e. g., one may instantiate the raking power with the minimum specification of the root s rakes, or with the actual raking power achievale w.r.t. the current terrain). Dually, whenever knowledge aout the current state is availale, the ounds can e instantiated more aggressively to allow efficient root ehavior e. g., in a rare worst case we may face a particularly fast ostacle, ut right now there are only slow-moving ostacles around). Theorems 1 and 2 are verified for all those values. Other aspects of uncertainty need explicit changes in the models and proofs, as discussed in the next section. C. Safety of Ostacle Avoidance Despite Uncertainty Roots have to deal with uncertainty in almost every aspect of their interaction with the environment, ranging from sensor inputs e. g., inaccurate localization, distance measurement) to actuator effects e. g., wheel slip depending on the terrain). In this section, we show two examples of uncertainty explicitly handled y the models. First, we allow localization uncertainty the root knows its position only approximately). We then consider imperfect actuator commands raking and acceleration) considered to e within an interval. Such intervals are instantiated, e. g., according to sensor or actuator specification e. g., GPS error), or w.r.t. experimental measurements Instantiation with proailistic ounds means that the symolically guaranteed safety is traded for a safety proaility. Model 4 Safety despite uncertain position measurements dw ups locate; ctrl; dyn) see Model 1 for details) 28) locate p r :=, );? p r p r U p 29) curve p r p c > 0 ω r p r p c = v r 30) d r = p r p c ) p r p c ) ) safe p r p o > v2 r A A ε2 + εv r + V ε + v ) r + Aε + U p 31) Model 4 introduces location uncertainty. It adds a location measurement p r efore the control decisions are made. This location measurement may deviate from the real position p r y no more than symolic parameter U p, cf. 29). The measured location p r is used in all control decisions of the root e. g., in 30) to compute the curve s center point and rotational velocity), while the root s physical motion is still computed on the symolic real position p r. We prove in KeYmaera that the root is still safe passive safety), if it accounts for the location uncertainty as stated in the safety constraint 31). Model 5 Safety despite actuator uncertainty dw uas ctrl; act; dyn) see Model 1 for details) 32) act u m := ; ã r := u m a r ;?0 < U m u m 1 33) ) ) safe p r p o > v2 r A A U m U m 2 ε2 + εv r + V ε + v ) r + Aε 34) U m dyn with a r replaced y ã r Model 5 introduces uncertainty in actuation with an actuator perturation etween control and dynamics, cf. 32). Actuator perturation affects the acceleration y a damping factor u m, known to e within [U m, 1], cf. 33). Note, that this damping factor can change aritrarily often, ut is assumed to e constant for ε time units. In the worst case, the root has full acceleration u m = 1) ut fully reduced raking u m = U m ). For instance, the root accelerates on perfect terrain, ut is on slippery terrain whenever it rakes. The root considers this worst case scenario during control in its safety constraint 34). Using KeYmaera, we prove that the root is still safe passive safety) when it follows Model 5. V. CONCLUSION AND FUTURE WORK Roots are hyrid systems, ecause they share continuous physical motion with complicated computer algorithms controlling their ehavior. We demonstrate that this understanding also helps proving roots safe. We develop hyrid system models of the dynamic window algorithm for autonomous ground vehicles and prove that the algorithm guarantees oth passive safety and passive friendly safety in the presence of

8 moving ostacles. All the proofs were achieved automatically with some manual guidance using the verification tool KeYmaera. We manually provided inductive and differential invariants crucial), as well as reduced the numer of terms and variales with simple interactions to reduce arithmetic complexity. 85% of the proof steps were automatic. Most interactive steps were simple arithmetic simplifications that KeYmaera s current proof strategies do not yet automate. We augment these models and safety proofs with roustness for localization uncertainty and imperfect actuation. We oserve that incremental revision of models and proofs helps reducing the verification complexity and understanding the impact of uncertainty on the ehavior of the root. Further, note that all symolic ounds in our models such as those for maximum ostacle velocity and sensor/actuator uncertainty can e instantiated with proailistic models e. g., assume the 2σ confidence interval of the distriution of ostacle velocities as maximum ostacle velocity). In this case, our verified safety guarantees translate into a safety proaility. Future work includes refined kinematic capailities e. g., going sideways with omni-drive) and additional sources of uncertainties, such as distance measurements. Also, to derive less conservative safety ounds for specific environments, one could introduce specific ostacle models e. g., pedestrians). ACKNOWLEDGMENTS This material is ased upon work supported y NSF CAREER Award CNS and NSF EXPEDITION CNS , y DARPA FA , and y US DOT UTC TSET award # DTRT12GUTC11. This work was also supported y the Austrian BMVIT under grant FIT-IT , FFG BRIDGE , and FFG Basisprogramm REFERENCES [1] Daniel Althoff, James J. Kuffner, Dirk Wollherr, and Martin Buss. Safety assessment of root trajectories for navigation in uncertain and dynamic environments. Autonomous Roots, 32: , [2] Francisco Bonin-Font, Alerto Ortiz, and Gariel Oliver. Visual Navigation for Moile Roots: A Survey. J. Intelligent Rootics Systems, 533): , [3] Sara Bouraine, Thierry Fraichard, and Hassen Salhi. Provaly safe navigation for moile roots with limited field-of-views in dynamic environments. Autonomous Roots, 323): , [4] Thomas Bräunl. Driving roots. In Emedded Rootics: Moile Root Design and Applications with Emedded Systems, pages Springer, [5] Oliver Brock and Oussama Khati. High-speed navigation using the gloal dynamic window approach. In Rootics and Automation, pages IEEE, [6] Howie Choset, Kevin Lynch, Seth Hutchinson, George Kantor, Wolfram Burgard, Lydia Kavraki, and Seastian Thrun. Principles Of Root Motion. MIT Press, [7] Paolo Fiorini and Erwin Prassler. Cleaning and Household Roots: A Technology Survey. Autonomous Roots, 9: , [8] Paolo Fiorini and Zvi Shiller. Motion Planning in Dynamic Environments Using Velocity Ostacles. J. Rootics Research, 177): , [9] Dieter Fox, Wolfram Burgard, and Seastian Thrun. The dynamic window approach to collision avoidance. Rootics Automation Magazine, IEEE, 41):23 33, [10] Goran Frehse, Colas Le Guernic, Alexandre Donzé, Scott Cotton, Rajarshi Ray, Olivier Leeltel, Rodolfo Ripado, Antoine Girard, Thao Dang, and Oded Maler. SpaceEx: Scalale verification of hyrid systems. In S. Qadeer G. Gopalakrishnan, editor, CAV, LNCS. Springer, [11] Oussama Khati. Real-time ostacle avoidance for manipulators and moile roots. In Rootics and Automation, pages , [12] Sarah M. Loos, André Platzer, and Ligia Nistor. Adaptive Cruise Control: Hyrid, Distriuted, and Now Formally Verified. In FM, pages Springer, [13] Sarah M. Loos, David Renshaw, and André Platzer. Formal verification of distriuted aircraft controllers. In HSCC. ACM, [14] Kristijan Maček, Dizan Alejandro Vasquez Govea, Thierry Fraichard, and Roland Siegwart. Towards Safe Vehicle Navigation in Dynamic Uran Scenarios. Automatika, 503 4): , [15] Javier Minguez, Luis Montano, and José Santos-Victor. Astracting Vehicle Shape and Kinematic Constraints from Ostacle Avoidance Methods. Autonomous Roots, 201):43 59, [16] Stefan Mitsch, Sarah M. Loos, and André Platzer. Towards formal verification of freeway traffic control. In ICCPS, pages IEEE, [17] Jia Pan, L. Zhang, and D. Manocha. Collision-free and smooth trajectory computation in cluttered environments. J. Rootics Research, 3110): , [18] André Platzer. Differential Dynamic Logic for Hyrid Systems. J. Autom. Reas., 412): , [19] André Platzer. Differential-algeraic Dynamic Logic for Differential-algeraic Programs. J. Log. Comput., 201): , [20] André Platzer. Logical Analysis of Hyrid Systems: Proving Theorems for Complex Dynamics. Springer, [21] André Platzer. The Complete Proof Theory of Hyrid Systems. In LICS, pages IEEE, [22] André Platzer and Edmund M. Clarke. Formal Verification of Curved Flight Collision Avoidance Maneuvers: A Case Study. In FM, pages Springer, [23] Shahar Sarid, Bingxin Xu, and Hadas Kress-Gazit. Guaranteeing High-Level Behaviors while Exploring Partially Known Maps. In Rootics: Science and Systems, [24] Derek Seward, Conrad Pace, and Rahee Agate. Safe and effective navigation of autonomous roots in hazardous environments. Autonomous Roots, 22: , [25] Holger Täuig, Udo Frese, Christoph Hertzerg, Christoph Lüth, Stefan Mohr, Elena Voroev, and Dennis Walter. Guaranteeing functional safety: design for provaility and computer-aided verification. Autonomous Roots, 32: , [26] Jur van den Berg, Pieter Aeel, and Ken Golderg. LQG-MP: Optimized Path Planning for Roots with Motion Uncertainty and Imperfect State Information. In Rootics: Science and Systems, [27] Alert Wu and Jonathan P. How. Guaranteed infinite horizon avoidance of unpredictale, dynamically constrained ostacles. Autonomous Roots, 32: , 2012.