I Introduction
In the past decades, robotics applications have been increasingly shifting from structured domains, where robots operate with full knowledge of all environmental variables, towards deployment in unstructured scenarios. We now find robots performing tasks as diverse as cargo delivery [1], security [2] and even cinematography [3]. Much of this progress stems from advancements in perception and planning algorithms, as well as hardware and compute capacity. However, ensuring autonomous vehicle safety still remains a central challenge, especially in dynamic navigation settings.
During navigation, robots must cope with disturbance uncertainties and remain safe among obstacles. Engineers often employ simplified vehicle dynamics and external disturbance models to handle the highdimensionality of kinodynamic planning, leaving the burden of trajectoryfollowing for the controller. Consequently, the robot trajectory will display tracking errors, which can eventually lead to crashes despite a nominal collisionfree path. As seen in Fig. 1, our work aims to address this challenge by computing probabilisticallysafe trajectories despite model and disturbance uncertainties.
A traditional strategy to achieve safety is to inflate obstacle borders, or to craft safety tubes around trajectories. Human experts handtune static safety margins for a particular mission based on previous experiences and costly trialanderror [4, 5]. However, this approach works only under low variability of vehicle dynamics and environmental disturbances. A designer can also derive margins given a fixed bounded uncertainty, however this is not a realistic assumption in reallife situations. For example, we can use worstcase margins to account for large disturbances [6, 7, 8, 9], but worstcase guarantees lead to overlyconservative plans and lower navigation performance. Such static paradigm may either be unsafe or overlyconservative for missions where disturbance magnitude is timevarying (e.g. transition between lowwind to highwind regions).
Our work presents a middle ground between handcrafted and overlyconservative margins, capable of achieving good navigation performance without sacrificing safety. We develop adaptive safety margins, which are selected using online disturbance measurements. The key contribution is the introduction of adaptive margins to motion primitive library planning approaches that enables better safety/performance tradeoff under timevarying disturbances. Our approach encompasses uncertainties in the robot’s dynamics model, controller and environmental disturbances, and can additionally operate under timevarying regimes. Our contributions are threefold:

Probabilistic Safety Tube Formulation: First, we formalize the theory behind the design of probabilisticallysafe tubes. (Section III);

Realtime Adaptive Margins: Next, we develop an offlineonline approach to probabilistically guarantee vehicle safety. In the offline step, we employ our formal safety definition to precompute a library of safety margins at varying levels of disturbance uncertainties for a library of motion primitives. Online, we leverage the library of margins and use disturbance measurements to select the least conservative motion primitive that still guarantees safety for realtime planning. The proposed framework is invariant to choice of motion primitives, global planner, vehicle dynamics, controller and underlying disturbance distribution. (Section IV);

Experimental Validation: Finally, we validate our realtime adaptive approach using extensive simulated and realworld flight tests among obstacles and under influence of timevarying disturbances. We show that our method has better performance than static margin baselines, while remaining safe (Section VI).
Ii Related Work
Planning under Uncertainty: Obstacle inflation is often used to achieve safe planning under uncertainty [10]. In practice, many works handdefine static safety buffers [11, 4, 12] to generate safe trajectories, however an expert designer has to opaquely adjust margin sizes to balance between safety and performance. Majority of the path planning work also does not explicitly address external disturbance [10, 13, 14, 15, 16].
Robust Motion Planning: We find works that precompute reachable sets given a high bounded disturbances to guarantee safety, such as, static funnels around a motion primitive library [8], a worstcase tracking error bound [6, 7], or a robust control invariant sets for emergency maneuvers [9]. Another work derives a globally invariant tube that is valid for any dynamically feasible trajectory [17]. However, above works do not adjust their disturbance bounds during a mission.
Kousik et. al [18] learns a tracking error function offline to conservatively approximate reachable set at runtime, with hardware implementation and timevarying uncertainties as future work. Most similar to our problem space, [19] approximates in realtime the reachable set given current disturbance, while our work precomputes margins for faster adaptation.
Robust, Adaptive Control: Alternatively, model mismatch and obstacle avoidance can be cohesively handled in the control layer [20]. Many of these approaches use a form of disturbance estimator/predictor [21] to compensate for learnt model mismatch, however do not handle safety when model is uncertain. We find works [22, 23] that explicitly account for model uncertainty to stay probabilisticallysafe, gaining higher performance with experience, however plan for a short time horizon due to high computation cost.
We highlight that this paper does not propose a new planning method, but serves as a framework to estimate safety margins for motion primitive library methods given current disturbance to maintain probabilistic safety under timevarying disturbance. We take inspiration from disturbance estimation, adaptive control and robust motion planning to introduce adaptive probabilisticallysafe margins for trajectory planning.
Iii Problem Definition
We wish to control a robot under unknown varying disturbance to stay probabilistically safe while navigating as close as possible to a reference trajectory from a highlevel planner. Our objective is to select, from a motion primitive library, the most similar primitive to reference trajectory such that when followed by the robot, it will remain safe under current disturbance given a risk threshold.
To do so, we estimate safety tubes that probabilistically enclose the nominal robot states at discretized disturbance uncertainties. We define a trajectory as probabilistically safe when its associated tube is not in collision.
Iiia Defining Trajectory Safety
Let be the robot state, and be a controller that maps the current state , trajectory , and time to a control command. We consider a robot described by a dynamics function , which we define as the summation of a known prior model and a disturbance .
(1) 
Let , be a trajectory followed by the vehicle, i.e., . We define trajectory safety as a confidence bound that the vehicle remains in free space () when following a trajectory , subject to disturbance and a controller . The timevarying disturbance captures the model mismatch between the nominal model and the true vehicle dynamics.
Definition 1 (Probabilistic Trajectory Safety):
(2)  
Our trajectory safety definition plays an important practical and theoretical role. As described in Sec. I, operators commonly handle robot safety by inflating obstacles using handtuned margins. This process can be viewed as an instance of our safety tube definition, with tube width chosen by the expert before a mission. In contrast, our definition offers a more principled method for defining probabilistically safe trajectories, as we estimate the optimal tube’s shape in realtime.
IiiB Estimating ProbabilisticallySafe Tube
Our key idea is to compute a probabilistic safety margin for Eq. 2 in terms of trajectory tracking error. First, we define the concept of a 3D safety tube , relative to a trajectory . Given an additive disturbance function and a controller , we define as the minimum volume in workspace that encloses the vehicle states when tracking , with a confidence bound . We can describe the shape of the tube with a margin radius that defines a 2D crosssection circle that is centered and perpendicular to the trajectory .
(3) 
IiiC Replanning Objective with Safety Tubes
Our objective is to navigate as close as possible to a reference trajectory , given a similarity metric function . The reference trajectory can be userdefined or from a highlevel planner, for example, a segment between start and goal locations, or a segment from a samplingbased planner. We do not assume that is inherently collisionfree (e.g. a forklift can be partially blocking the reference trajectory). During execution, our vehicle selects a new trajectory from a predefined motion primitive library to follow such that it remains probabilistically safe under current disturbance while minimizing the similarity metric . Eq. 4 defines our replanning objective, with the probabilistic safety constraint represented by trajectoryrelative safety tube (Eq. 3).
(4) 
Iv Approach
This section details the overall approach to solve our replanning objective in Eq. 4.
We propose a offlineonline framework that uses a library of offline precomputed tubes and an online disturbance estimator to select motion primitives based on uncertainty in realtime. Fig. 2 describes the overall process. The framework is generalizable to other mobile robot platforms, where we focus on Unmanned Aerial Vehicles (UAVs) in this paper.
Offline, given a motion primitive library, we generate a lookup table (LUT) of safety margins for each motion primitive at discretised levels of disturbance. For every motion primitive and disturbance level, we estimate a safety tube as in Eq. 3, where we calculate the tube margin by first generating possible trajectories using Monte Carlo simulations (Sec. IVA1), then fitting a tube (Sec. IVA2). Online, we estimate the disturbance variance (Sec. IVB1). Then, given a reference trajectory, the variance is used to select the optimal collisionfree motion primitive with collision check based on the precomputed tubes from LUT (Sec. IVB2). Disturbance mean is set to zero and we use the variance to capture varying disturbances.
Iva Offline: Generating Margin Lookup Table (LUT)
IvA1 Simulating Possible Trajectories
Given a set of disturbance standard deviations
and a motion primitive library , we run Monte Carlo simulations to collect a set of possible vehicle trajectories . We use Monte Carlo simulations as it allows us to generate predictions for arbitrary dynamics model, underlying disturbance model and controller, without any assumptions of their structure. The MonteCarlo simulated vehicle follows a motion primitive with controller whilst under nominal model dynamics with a disturbance sampled from every disturbance period and an initial state distribution defined by an operator times to generate a set .IvA2 Tube Fitting
We find the optimal margin radius that define a minimumvolume tube that encloses predicted states in
. We discretize trajectory to segments by time and fit a zeromean normal distribution to crosstrack error of collected points in each segment. For each segment, we find the fitted interval (e.g.,
) that corresponds to desired confidence bound . The maximum fitted interval over all segments is then added to LUT, which can be queried by .IvB Online: Replanning with Adaptive Margins
IvB1 Temporal Disturbance Modeling
We estimate the current disturbance term by learning a disturbance model .
Disturbance is the difference between predicted and actual dynamics .
In this paper, we set as a moving variance on observed disturbance, and assume all primitives share the same timevarying disturbance. However, any disturbance modeling method that provides a disturbance variance can be used.
IvB2 Margin Lookup and Trajectory Selection
Given estimated disturbance variance , we first query the associated safety margins for each primitive in the library by using the disturbance . The collisionfree (given respective safety margin) primitive with minimum trajectory cost is then chosen, here we use L1 norm.
(5) 
The margin lookup and trajectory selection step is shown in Alg. 1.
IvC Implementation
In this paper, we replan at 5Hz with a motion primitive library of constant angular and linear velocity. Figure 6 shows a sample lookup table used in maze trials. After parallelization, this 22trajectory, 9variance LUT with 1000 Monte Carlo simulations took 230s to produce and uses 1.1kB of space. A position and velocity PID controller computes desired attitude. We estimate bodyx and y disturbance (, ) in acceleration. The larger between and is used for lookup. Our nominal model is modified from [24] to assume linear mapping between desired and normalized thrust, and desired yawrate is actual yawrate.
V Simulated Experiments
We compare the performance of our adaptive margins to baseline static margins under increasing magnitude of disturbances. We validate the transferiblity of our method to realistic wind situations by using wind from the Dryden gust model [25], often used by the Department of Defense.
Fig. 4 shows map and samples of wind sequences. The vehicle is given a straight line path and a trial is successful if vehicle reaches goal line without crashing. We use the same tube library at fixed disturbance variance (Static: Low at , Static: High at ) as our two baselines. For our margin lookup, we use the zerocentered standard deviation of the future 9s sequence of disturbance. Each data point is an averaged result over 10 trials in randomized wind sequences.
Table I shows the performance of different margin methods, under increasing levels of turbulence. At all turbulence level, our adaptive margins dynamically computes best margin given current information, resulting in overall lower cost than Static: High, while maintaining 100% safety. The rightmost column of Table I shows the percentage of time robot is within margin. The results confirm our precomputed margin transfers to realistic wind conditions, with worst withinmargin percentage being at high turbulence, likely due to suddenly higher gusts pushing the vehicle out very briefly. In contrast, Static: Low only stays within margin of time, leaving it more vulnerable to collisions with trials failing.







Low 





Med 





High 




Vi RealWorld Experiments
We test with real flights three hypotheses: Our adaptive margin method can (H1) adjust margins in realtime, (H2) better balance safety and performance than baseline static margins in tight spaces, (H3) and under timevarying disturbances.
Experimental Platform: We use a custombuilt quadrotor with an Intel T265 Tracking Camera for state estimation. All computation is done onboard using a Jetson TX2. An offline map is given for all trials.
E1  Realtime adaptation to disturbance uncertainty (H1):
To validate the realtime nature of our method, we set up an indoor maze environment with tight corridors (1.5m) and hairpin turns, as shown in Fig. 5. The configuration is difficult to navigate with potential crashes around turns due to undercutting or overshooting. The narrow environment also causes very high safety margins to render no solution. A collisionfree 1m/s global plan is given. We show the impact of decreasing margin size,by precomputing margins for two sets of motion primitive library, one at , another at . Fig. 6 shows this experiment’s LUT for the 0.5m/s motion primitive library. As expected, computed margins increase with higher uncertainty and higher angular velocity.
To show effects of varying disturbance uncertainty, the first loop around the maze is initialized with a high disturbance standard deviation at , and the second loop with the first loop’s final standard deviation.
Fig. 5 compares the two loops, with narrower margins, lower completion time and cost as disturbance uncertainty decreases. For example, in the lower right turn, system picked the faster trajectory margins in the second loop as its margins decreased with lower uncertainty, Table III validates realtime performance, with trajectory selection taking 26ms and margin lookup only taking 0.02 ms for a library of 22 motion primitives.







100% 




100% 




30% 




90% 


Procedure  Runtime(ms)  CPU Thread %  




E2  Baseline comparisons in tight spaces (H2): We compare our adaptive approach to static margin methods with the same setup as E1. A trial is successful if the vehicle successfully completes one maze loop without crashing. We use three sets of static margins as baselines: one conservative using high bounded disturbance from [7], and two handcrafted static margins tuned with other global plans. Our adaptive margins are hotstarted at from E1. Table II compares success rate, completion time and cost, with our adaptive margins enabling higherperformance flight than conservative margins, while staying 100% safe whereas handcrafted margins did not. The results display the difficulty of handcrafting safety margins, where a difference of 5cm leads to 60% more crash.
E3  Adapting to Timevarying Disturbance (H1+H3):
We validate the performance of our proposed framework under timevarying disturbances by testing its ability to avoid obstacles in windy environment. The quadrotor is given a straight line path with an obstacle straight ahead and wind blowing laterally (in ydirection) towards the obstacle. Fans are not included in map. Fig. 7 shows experimental setup and our vehicle’s planned trajectories with computed margins when flying through wind. We use a static margin handtuned with no wind as baseline. Red line on ydisturbance plot shows disturbance standard deviation used to lookup margins.
The vehicle starts at a low disturbance at (A).
At (B), when vehicle encounters wind and experience a high ydisturbance, the margins dynamically increase in size to move the quadrotor away from danger. Note, right after (B), the robot moves 30cm to the left which would have been at the edge of previous tube if it had not adapted.
For most of the time quadrotor is in the wind field between (B) and (C), lookup s.d. is , with a jump to as the quadrotor moves towards the last fan while avoiding the obstacle. At (C), vehicle successfully avoids the obstacle and decreases its margin as disturbance has lowered. In contrast, the quadrotor with the fixed margin leaves its tube and crashes.
Our adaptive margins allow us to stay highperformance (smaller margins) when disturbance is low, and safe when disturbance is high. The instantaneous increase of margins to wind disturbance validates the realtime performance.
Vii Conclusion and Discussion
In this paper, we present a realtime framework for adapting safety margins onthefly to compute safe plans without overly sacrificing performance. Central to this approach, is the precomputation of safety margins at multiple disturbance levels for a motion primitive library. We validated with extensive simulated and real flights that our adaptive margins significantly increase performance while maintaining safety in tight spaces and under timevarying disturbances than static margin methods.
We find multiple directions for future work. We are interested in coupling our adaptive framework with SOS optimization as used in [8, 17, 26], to precompute margins for multiple levels of worstcase disturbances. This removes our need to define the Monte Carlo disturbance model and enables better handling of disturbance bias. We can also extend our 1step reactive planning framework to multiple steps for longer horizon planning [27]. Finally, we are working on including rigorous safety guarantee when transitioning between primitives, taking inspirations from [7, 8, 28].
Acknowledgment
This work was supported by the Croucher Foundation, the Office of Naval Research (Grant N0001418RSN05), Department of Energy Grant DEEE0008463, and the Waibel scholarship. The authors would like to thank Christoph Endner and Tino Fuhrmann for construction of the platform, and Brady Moon for Dryden model implementation.
References
 [1] S. Choudhury, K. Solovey, M. Kochenderfer, and M. Pavone, “Efficient largescale multidrone delivery using transit networks,” arXiv:1909.11840, 2019.
 [2] M. DeMiguelMolina, Ethics and Civil Drones: European Policies and Proposals for the Industry. Springer, 2018.
 [3] R. Bonatti, W. Wang, C. Ho, A. Ahuja, M. Gschwindt, E. Camci, E. Kayacan, S. Choudhury, and S. Scherer, “Autonomous aerial cinematography in unstructured environments with learned artistic decisionmaking,” Journal of Field Robotics, 2019.
 [4] F. Gao, W. Wu, W. Gao, and S. Shen, “Flying on point clouds: Online trajectory generation and autonomous navigation for quadrotors in cluttered environments,” Journal of Field Robotics, vol. 36, no. 4, pp. 710–733, 2019.
 [5] A. Spitzer, X. Yang, J. Yao, A. Dhawale, K. Goel, M. Dabhi, M. Collins, C. Boirum, and N. Michael, “Fast and agile visionbased flight with teleoperation and collision avoidance on a multirotor,” in International Symposium on Experimental Robotics. Springer, 2018, pp. 524–535.
 [6] S. L. Herbert, M. Chen, S. Han, S. Bansal, J. F. Fisac, and C. J. Tomlin, “Fastrack: A modular framework for fast and guaranteed safe motion planning,” in 2017 IEEE 56th Annual Conference on Decision and Control (CDC), 2017, pp. 1517–1522.
 [7] D. FridovichKeil, S. L. Herbert, J. F. Fisac, S. Deglurkar, and C. J. Tomlin, “Planning, fast and slow: A framework for adaptive realtime safe trajectory planning,” in 2018 IEEE International Conference on Robotics and Automation (ICRA), 2018, pp. 387–394.
 [8] A. Majumdar and R. Tedrake, “Funnel libraries for realtime robust feedback motion planning,” The International Journal of Robotics Research, vol. 36, no. 8, pp. 947–982, 2017.
 [9] D. Althoff, M. Althoff, and S. Scherer, “Online safety verification of trajectories for unmanned flight with offline computed robust invariant sets,” in 2015 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). Hamburg, Germany: IEEE, Sep. 2015, pp. 3470–3477.
 [10] S. Liu, K. Mohta, N. Atanasov, and V. Kumar, “Searchbased motion planning for aggressive flight in se (3),” IEEE Robotics and Automation Letters, vol. 3, no. 3, pp. 2439–2446, 2018.
 [11] A. E. Spitzer, X. Yang, J. W. Yao, A. Dhawale, K. Goel, M. Dabhi, M. Collins, C. Boirum, and N. Michael, “Fast and agile visionbased flight with teleoperation and collision avoidance on a multirotor,” in Proceedings of the 2018 International Symposium on Experimental Robotics (ISER 2018), November 2018, pp. 524 – 535.
 [12] J. Patrikar, V. Dugar, V. Arcot, and S. Scherer, “Realtime motion planning of curvature continuous trajectories for urban uav operations in wind,” in 2020 International Conference on Unmanned Aircraft Systems (ICUAS), 2020, pp. 854–861.
 [13] Y. Li, Z. Littlefield, and K. E. Bekris, “Sparse methods for efficient asymptotically optimal kinodynamic planning,” in Algorithmic foundations of robotics XI. Springer, 2015, pp. 263–282.
 [14] M. Lan, S. Lai, Y. Bi, H. Qin, J. Li, F. Lin, and B. M. Chen, “Bit*based path planning for micro aerial vehicles,” in IECON 201642nd Annual Conference of the IEEE Industrial Electronics Society. IEEE, 2016, pp. 6079–6084.
 [15] C. Xie, J. van den Berg, S. Patil, and P. Abbeel, “Toward asymptotically optimal motion planning for kinodynamic systems using a twopoint boundary value problem solver,” in 2015 IEEE International Conference on Robotics and Automation (ICRA). IEEE, 2015, pp. 4187–4194.
 [16] R. Allen and M. Pavone, “A realtime framework for kinodynamic planning with application to quadrotor obstacle avoidance,” in AIAA Guidance, Navigation, and Control Conference, 2016, p. 1374.
 [17] S. Singh, M. Chen, S. L. Herbert, C. J. Tomlin, and M. Pavone, “Robust Tracking with Model Mismatch for Fast and Safe Planning: an SOS Optimization Approach,” arXiv:1808.00649 [cs, math], Jul. 2019, arXiv: 1808.00649. [Online]. Available: http://arxiv.org/abs/1808.00649
 [18] S. Kousik, P. Holmes, and R. Vasudevan, “Safe, aggressive quadrotor flight via reachabilitybased trajectory design,” in Dynamic Systems and Control Conference, vol. 59162. American Society of Mechanical Engineers, 2019, p. V003T19A010.
 [19] H. Seo, D. Lee, C. Y. Son, C. J. Tomlin, and H. J. Kim, “Robust trajectory planning for a multirotor against disturbance based on hamiltonjacobi reachability analysis,” in 2019 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). IEEE, 2019, pp. 3150–3157.
 [20] B. Lopez, J. How, and J. Slotine, “Dynamic tube mpc for nonlinear systems,” in 2019 American Control Conference (ACC). IEEE, 2019, pp. 1655–1662.
 [21] C. Ostafew, A. Schoellig, T. Barfoot, and J. Collier, “Learningbased nonlinear model predictive control to improve visionbased mobile robot path tracking,” Journal of Field Robotics, vol. 33, no. 1, pp. 133–152, 2016.
 [22] C. Ostafew, A. Schoellig, and T. Barfoot, “Robust constrained learningbased nmpc enabling reliable mobile robot path tracking,” The International Journal of Robotics Research, vol. 35, no. 13, pp. 1547–1563, 2016.
 [23] D. Fan, J. Nguyen, R. Thakker, N. Alatur, A. Aghamohammadi, and E. Theodorou, “Bayesian learningbased adaptive control for safety critical systems,” arXiv:1910.02325, 2019.
 [24] M. Kamel, T. Stastny, K. Alexis, and R. Siegwart, “Model predictive control for trajectory tracking of unmanned aerial vehicles using robot operating system,” in Robot Operating System (ROS) The Complete Reference, Volume 2, A. Koubaa, Ed. Springer.
 [25] R. W. Beard and T. W. McLain, Small Unmanned Aircraft: Theory and Practice. USA: Princeton University Press, 2012.

[26]
A. Gahlawat, P. Zhao, A. Patterson, N. Hovakimyan, and E. Theodorou, “L1gp: L1 adaptive control with bayesian learning,” ser. Proceedings of Machine Learning Research, A. M. Bayen, A. Jadbabaie, G. Pappas, P. A. Parrilo, B. Recht, C. Tomlin, and M. Zeilinger, Eds., vol. 120. The Cloud: PMLR, 10–11 Jun 2020, pp. 826–837. [Online]. Available:
http://proceedings.mlr.press/v120/gahlawat20a.html  [27] X. Yang and N. Michael, “Assisted mobile robot teleoperation with intentaligned trajectories via biased incremental action sampling,” 2020 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), 2020.
 [28] A. Lakshmanan, A. Gahlawat, and N. Hovakimyan, “Safe feedback motion planning: A contraction theory and adaptive control based approach,” 2020.
Comments
There are no comments yet.