Velocity Obstacles (VO) methods form a paradigm for collision avoidance strategies among moving obstacles and agents. While VO methods perform well in simple multi-agent environments, they do not guarantee safety and can show overly conservative behavior in common situations. In this paper, we propose to combine a VO strategy for guidance with a Control Barrier Function approach for safety, which overcomes the overly conservative behavior of VOs and formally guarantees safety. We validate our method in a baseline comparison study, using second-order integrator and car-like dynamics. Results support that our method outperforms the baselines with respect to path smoothness, collision avoidance, and success rates.

In this paper we show how to improve the performance of backward chained behavior trees (BTs) that include policies trained with reinforcement learning (RL). BTs represent a hierarchical and modular way of combining control policies into higher level control policies. Backward chaining is a design principle for the construction of BTs that combines reactivity with goal directed actions in a structured way. The backward chained structure has also enabled convergence proofs for BTs, identifying a set of local conditions to be satisfied for the convergence of all trajectories to a set of desired goal states. The key idea of this paper is to improve performance of backward chained BTs by using the conditions identified in a theoretical convergence proof to configure the RL problems for individual controllers. Specifically, previous analysis identified so-called active constraint conditions (ACCs), that should not be violated in order to avoid having to return to work on previously achieved subgoals. We propose a way to set up the RL problems, such that they do not only achieve each immediate subgoal, but also avoid violating the identified ACCs. The resulting performance improvement depends on how often ACC violations occurred before the change, and how much effort, in terms of execution time, was needed to re-achieve them. The proposed approach is illustrated in a dynamic simulation environment.

Multi agent coverage and robot navigation are two very important research fields within robotics. However, their intersection has received limited attention. In multi agent coverage, perfect navigation is often assumed, and in robot navigation, the focus is often to minimize the localization error with the aid of stationary features from the environment. The need for integration of the two becomes clear in environments with very sparse features or landmarks, for example when a group of Autonomous Underwater Vehicles (AUVs) are to search a uniform seafloor for mines or other dangerous objects. In such environments, localization systems are often deprived of detectable features to use that could increase their accuracy. In this paper we propose an algorithm for doing navigation aware multi agent coverage in areas with no landmarks. Instead of using identical lawn mower patterns, we propose to mirror every other pattern to enable the agents to meet up and make inter-agent measurements and share information regularly. This improves performance in two ways, global drift in relation to the area to be covered is reduced, and local coverage gaps between adjacent patterns are reduced. Further, we show that this can be accomplished within the constraints of very limited sensing, computing and communication resources that most AUVs have available. The effectiveness of our method is shown through statistically significant simulated experiments.

Air Combat is a high-risk activity carried out by trained professionals operating sophisticated equipment. During this activity, a number of trade-offs have to be made, such as the balance between risk and efficiency. A policy that minimizes risk could have very low efficiency, and one that maximizes efficiency may involve very high risk.In this study, we use Reinforcement Learning (RL) to create Control Barrier Functions (CBF) that captures the current risk, in terms of worst-case future separation between the aircraft and an enemy missile. CBFs are usually designed manually as closed-form expressions, but for a complex system such as a guided missile, this is not possible. Instead, we solve an RL problem using high fidelity simulation models to find value functions with CBF properties, that can then be used to guarantee safety in real air combat situations. We also provide a theoretical analysis of what family of RL problems result in value functions that can be used as CBFs in this way.The proposed approach allows the pilot in an air combat scenario to set the exposure level deemed acceptable and continuously monitor the risk related to his/her own safety. Given input regarding acceptable risk, the system limits the choices of the pilot to those that guarantee future satisfaction of the provided bound.

In this paper, we show how a planning algorithm can be used to automatically create and update a Behavior Tree (BT), controlling a robot in a dynamic environment. The planning part of the algorithm is based on the idea of back chaining. Starting from a goal condition we iteratively select actions to achieve that goal, and if those actions have unmet preconditions, they are extended with actions to achieve them in the same way. The fact that BTs are inherently modular and reactive makes the proposed solution blend acting and planning in a way that enables the robot to effectively react to external disturbances. If an external agent undoes an action the robot reexecutes it without re-planning, and if an external agent helps the robot, it skips the corresponding actions, again without replanning. We illustrate our approach in two different robotics scenarios.

In this paper, we study the problem of automatically synthesizing a successful behavior tree (BT) in ana prioriunknown dynamic environment. Starting with a given set of actions, a reward function, and sensing in terms of a set of binary conditions, the proposed algorithm incrementally learns a switching structure, in terms of a BT, that is able to handle the situations encountered. Exploiting the fact that BTs generalizeand–or-trees and also provide very natural chromosome mappings for genetic programming, we combine the long-term performance of genetic programming with a greedy element and use theand–oranalogy to limit the size of the resulting structure. Finally, earlier results on BTs enable us to provide certain safety guarantees for the resulting system. Using the testing environment Mario AI, we compare our approach to alternative methods for learning BTs and finite state machines. The evaluation shows that the proposed approach generated solutions with better performance, and often fewer nodes than the other two methods.

This paper proposes a new Kalman filter based online framework to estimate the spatial wireless connectivity in terms of received signal strength (RSS), which is composed of path loss and the shadow fading variance of a wireless channel in autonomous vehicles. The path loss is estimated using a localized least squares method and the shadowing effect is predicted with an empirical (exponential) variogram. A discrete Kalman Filter is used to fuse these two models into a state-space formulation. The approach is unique in a sense that it is online and does not require the exact source location to be known apriori. We evaluated the method using real- world measurements dataset from both indoors and outdoor environments. The results show significant performance improvements compared to state-of-the- art methods using Gaussian processes or Kriging interpolation algorithms. We are able to achieve a mean prediction accuracy of up to 96% for predicting RSS as far as 20 meters ahead in the robot's trajectory.

Mobile robots, be it autonomous or teleoperated, require stable communication with the base station to exchange valuable information. Given the stochastic elements in radio signal propagation, such as shadowing and fading, and the possibilities of unpredictable events or hardware failures, communication loss often presents a significant mission risk, both in terms of probability and impact, especially in Urban Search and Rescue (USAR) operations. Depending on the circumstances, disconnected robots are either abandoned, or attempt to autonomously back-trace their way to the base station. Although recent results in Communication-Aware Motion Planning can be used to effectively manage connectivity with robots, there are no results focusing on autonomously re-establishing the wireless connectivity of a mobile robot without back-tracing or using detailed a priori information of the network. In this paper, we present a robust and online radio signal mapping method using Gaussian Random Fields, and propose a Resilient Communication-Aware Motion Planner (RCAMP) that integrates the above signal mapping framework with a motion planner. RCAMP considers both the environment and the physical constraints of the robot, based on the available sensory information. We also propose a self-repair strategy using RCMAP, that takes both connectivity and the goal position into account when driving to a connection-safe position in the event of a communication loss. We demonstrate the proposed planner in a set of realistic simulations of an exploration task in single or multi-channel communication scenarios.

In this paper we study the problem of synthesizing correct-by-construction Behavior Trees (BTs) controlling agents in adversarial environments. The proposed approach combines the modularity and reactivity of BTs with the formal guarantees of Linear Temporal Logic (LTL) methods. Given a set of admissible environment specifications, an agent model in form of a Finite Transition System and the desired task in form of an LTL formula, we synthesize a BT in polynomial time, that is guaranteed to correctly execute the desired task. To illustrate the approach, we present three examples of increasing complexity.

A reliable wireless connection between the operator and the teleoperated unmanned ground vehicle (UGV) is critical in many urban search and rescue (USAR) missions. Unfortunately, as was seen in, for example, the Fukushima nuclear disaster, the networks available in areas where USAR missions take place are often severely limited in range and coverage. Therefore, during mission execution, the operator needs to keep track of not only the physical parts of the mission, such as navigating through an area or searching for victims, but also the variations in network connectivity across the environment.In this paper, we propose and evaluate a new teleoperation user interface (UI) that includes a way of estimating the direction of arrival (DoA) of the radio signal strength (RSS) and integrating the DoA information in the interface. The evaluation shows that using the interface results in more objects found, and less aborted missions due to connectivity problems, as compared to a standard interface.The proposed interface is an extension to an existing interface centered on the video stream captured by the UGV. But instead of just showing the network signal strength in terms of percent and a set of bars, the additional information of DoA is added in terms of a color bar surrounding the video feed. With this information, the operator knows what movement directions are safe, even when moving in regions close to the connectivity threshold.

Behavior trees (BTs) are a way of organizing the switching structure of a hybrid dynamical system (HDS), which was originally introduced in the computer game programming community. In this paper, we analyze how the BT representation increases the modularity of an HDS and how key system properties are preserved over compositions of such systems, in terms of combining two BTs into a larger one. We also show how BTs can be seen as a generalization of sequential behavior compositions, the subsumption architecture, and decisions trees. These three tools are powerful but quite different, and the fact that they are unified in a natural way in BTs might be a reason for their popularity in the gaming community. We conclude the paper by giving a set of examples illustrating how the proposed analysis tools can be applied to robot control BTs.

Teleoperation of a mobile robot manipulating and exploring an object shares many similarities with the manipulation of virtual objects in a 3D design software such as AutoCAD. The user interfaces are however quite different, mainly for historical reasons. In this paper we aim to change that, and draw inspiration from the 3D design community to propose a teleoperation interface control mode that is identical to the ones being used to locally navigate the virtual viewpoint of most Computer Aided Design (CAD) softwares. The proposed mobile manipulator control framework thus allows the user to focus on the 3D objects being manipulated, using control modes such as orbit object and pan object, supported by data from the wrist mounted RGB-D sensor. The gripper of the robot performs the desired motions relative to the object, while the manipulator arm and base moves in a way that realizes the desired gripper motions. The system redundancies are exploited in order to take additional constraints, such as obstacle avoidance, into account, using a constraint based programming framework.

Behavior Trees (BTs) is a way of organizing the switching structure of a control system, that was originally developed in the computer gaming industry but is now also being used in robotics. The Teleo-Reactive programs (TRs) is a highly cited reactive hierarchical robot control approach suggested by Nilsson and And-Or-Trees are trees used for heuristic problems solving. In this paper, we show that BTs generalize TRs as well as And-Or-Trees, even though the two concepts are quite different. And-Or-Trees are trees of conditions, and we show that they transform into a feedback execution plan when written as a BT. TRs are hierarchical control structures, and we show how every TR can be written as a BT. Furthermore, we show that so-called Universal TRs, guaranteeing that the goal will be reached, are a special case of so-called Finite Time Successful BTs. This implies that many designs and theoretical results developed for TRs can be applied to BTs.

We study the problem of robot interaction with mechanisms that afford one degree of freedom motion, e.g., doors and drawers. We propose a methodology for simultaneous compliant interaction and estimation of constraints imposed by the joint. Our method requires no prior knowledge of the mechanisms' kinematics, including the type of joint, prismatic or revolute. The method consists of a velocity controller that relies on force/torque measurements and estimation of the motion direction, the distance, and the orientation of the rotational axis. It is suitable for velocity-controlled manipulators with force/torque sensor capabilities at the end-effector. Forces and torques are regulated within given constraints, while the velocity controller ensures that the end-effector of the robot moves with a task-related desired velocity. We give proof that the estimates converge to the true values under valid assumptions on the grasp, and error bounds for setups with inaccuracies in control, measurements, or modeling. The method is evaluated in different scenarios involving opening a representative set of door and drawer mechanisms found in household environments.

Teleoperated Unmanned Ground Vehicles (UGVs) are expected to play an important role in future search and rescue operations. In such tasks, two factors are crucial for a successful mission completion: operator situational awareness and robust network connectivity between operator and UGV. In this paper, we address both these factors by extending a new Free Look Control (FLC) operator interface with a graphical representation of the Radio Signal Strength (RSS) gradient at the UGV location. We also provide a new way of estimating this gradient using multiple receivers with directional antennas. The proposed approach allows the operator to stay focused on the video stream providing the crucial situational awareness, while controlling the UGV to complete the mission without moving into areas with dangerously low wireless connectivity. The approach is implemented on a KUKA youBot using commercial-off-the-shelf components. We provide experimental results showing how the proposed RSS gradient estimation method performs better than a difference approximation using omnidirectional antennas and verify that it is indeed useful for predicting the RSS development along a UGV trajectory. We also evaluate the proposed combined approach in terms of accuracy, precision, sensitivity and specificity.

In the future mobile dual-arm robots are expected to perform many tasks. Kinematically, the configuration of two manipulators that branch from the same common mobile base results in a serial-to-parallel kinematic structure, which makes inverse kinematic computations non-trivial. The motion of the base has to be decided in a trade-off, taking the needs of both arms into account. We propose to use a Virtual Kinematic Chain (VKC) to specify the common motion of the parallel manipulators, instead of using the two manipulators kinematics directly. With this VKC, we formulate a constraint based programming solution for the robot to respond to external disturbances during task execution. The proposed approach is experimentally verified both in a noise-free illustrative simulation and a real human robot co-manipulation task.

This paper presents a mathematical framework for performance analysis of Behavior Trees (BTs). BTs are a recent alternative to Finite State Machines (FSMs), for doing modular task switching in robot control architectures. By encoding the switching logic in a tree structure, instead of distributing it in the states of a FSM, modularity and reusability are improved. In this paper, we compute performance measures, such as success/failure probabilities and execution times, for plans encoded and executed by BTs. To do this, we first introduce Stochastic Behavior Trees (SBT), where we assume that the probabilistic performance measures of the basic action controllers are given. We then show how Discrete Time Markov Chains (DTMC) can be used to aggregate these measures from one level of the tree to the next. The recursive structure of the tree then enables us to step by step propagate such estimates from the leaves (basic action controllers) to the root (complete task execution). Finally, we verify our analytical results using massive Monte Carlo simulations, and provide an illustrative example of the results for a complex robotic task.

This paper presents a unified framework for Behavior Trees (BTs), a plan representation and execution tool. The available literature lacks the consistency and mathematical rigor required for robotic and control applications. Therefore, we approach this problem in two steps: first, reviewing the most popular BT literature exposing the aforementioned issues; second, describing our unified BT framework along with equivalence notions between BTs and Controlled Hybrid Dynamical Systems (CHDSs). This paper improves on the existing state of the art as it describes BTs in a more accurate and compact way, while providing insight about their actual representation capabilities. Lastly, we demonstrate the applicability of our framework to real systems scheduling open-loop actions in a grasping mission that involves a NAO robot and our BT library.

Behavior Trees (BTs) have become a popular framework for designing controllers of in-game opponents in the computer gaming industry. In this paper, we formalize and analyze the reasons behind the success of the BTs using standard tools of robot control theory, focusing on how properties such as robustness and safety are addressed in a modular way. In particular, we show how these key properties can be traced back to the ideas of subsumption and sequential compositions of robot behaviors. Thus BTs can be seen as a recent addition to a long research effort towards increasing modularity, robustness and safety of robot control software. To illustrate the use of BTs, we provide a set of solutions to example problems.

This paper addresses the problem of robot interaction with objects attached to the environment through joints such as doors or drawers. We propose a methodology that requires no prior knowledge of the objects' kinematics, including the type of joint - either prismatic or revolute. The method consists of a velocity controller which relies on force/torque measurements and estimation of the motion direction, rotational axis and the distance from the center of rotation. The method is suitable for any velocity controlled manipulator with a force/torque sensor at the end-effector. The force/torque control regulates the applied forces and torques within given constraints, while the velocity controller ensures that the end-effector moves with a task-related desired tangential velocity. The paper also provides a proof that the estimates converge to the actual values. The method is evaluated in different scenarios typically met in a household environment.

In this paper, we combine navigation functionlike potential fields and constraint based programming to achieve obstacle avoidance in formation. Constraint based programming was developed in robotic manipulation as a technique to take several constraints into account when controlling redundant manipulators. The approach has also been generalized, and applied to other control systems such as dual arm manipulators and unmanned aerial vehicles. Navigation functions are an elegant way to design controllers with provable properties for navigation problems. By combining these tools, we take advantage of the redundancy inherent in a multi-agent control problem and are able to concurrently address features such as formation maintenance and goal convergence, even in the presence of moving obstacles. We show how the user can decide a priority ordering of the objectives, as well as a clear way of seeing what objectives are currently addressed and what are postponed. We also analyze the theoretical properties of the proposed controller. Finally, we use a set of simulations to illustrate the approach.

In this paper, the multi pursuer version of the pursuit evasion problem in polygonal environments is addressed. This problem is NP-hard, and therefore we seek good enough, but not optimal solutions. By modeling the problem as a Boolean Control Network, we can efficiently keep track of which regions are cleared, and which are not, while the input nodes of the network are used to represent the motion of the pursuers. The environment is partitioned into a set of convex regions, where each region correspond to a set of nodes in the network. The method is implemented in ANSI C, and efficiently solves complex environments containing multiple loops and requiring so-called recontamination. The provided examples demonstrate the effectiveness of the method in terms of computational time.

In this paper, we address the multi pursuer version of the pursuit evasion problem in polygonal environments. It is well known that this problem is NP-hard, and therefore we seek efficient, but not optimal, solutions by relaxing the problem and applying the tools of Mixed Integer Linear Programming (MILP) and Receding Horizon Control (RHC). Approaches using MILP and RHC are known to produce efficient algorithms in other path planning domains, such as obstacle avoidance. Here we show how the MILP formalism can be used in a pursuit evasion setting to capture the motion of the pursuers as well as the partitioning of the pursuit search region into a cleared and a contaminated part. RHC is furthermore a well known way of balancing performance and computation requirements by iteratively solving path planning problems over a receding planning horizon, and adapt the length of that horizon to the computational resources available. The proposed approach is implemented in Matlab/Cplex and illustrated by a number of solved examples.

Unmanned ground vehicles (UGVs) equipped with surveillance cameras present a flexible complement to the numerous stationary sensors being used in security applications today. However, to take full advantage of the flexibility and speed offered by a group of UGV platforms, a fast way to compute desired camera locations that cover or surround a set of buildings e.g., in response to an alarm, is needed. In this paper we focus on two problems. The first is how to create a line-of-sight perimeter around a given set of buildings with a minimal number of UGVs. The second problem is how to find UGV positions such that a given set of walls are covered by the cameras while taking constraints in terms of zoom, range, resolution and field of view into account. For the first problem we propose a polynomial time algorithm and for the second problem we extend our previous work to include zoom cameras and furthermore provide a theoretical analysis of the approach itself. A number of examples are presented to illustrate the two algorithms.

Model based robot control approaches are often designed to allow the verification of certain system properties such as safety or goal convergence. However, designing such controllers is often very time consuming, and most of the time it is not possible to add additional control objectives without jeopardizing the previously proved system properties.

A new distributed coordination algorithm for multi-vehicle systems is presented in this paper. The algorithm combines a particular choice of navigation function with Voronoi partitions. This results not only in obstacle avoidance and motion to the goal, but also in a desirable geographical distribution of the vehicles. Our algorithm is decentralized in that each vehicle needs only to know the position of neigh boring vehicles, but no other inter-vehicle communication or centralized control are required. The algorithm gives a natural priority to safety, goal convergence, and formation keeping, in that (1) collision avoidance is guaranteed under all circumstances, (2) the vehicles will move toward the goal as long as a given optimization problem is feasible, and (3) if prior criteria admit, the vehicles tend to a desirable lattice formation. These theoretical properties are discussed in the paper and the performance of the algorithm is illustrated in simulations with realistic models of twenty all-terrain vehicles. Planned experimental evaluation using customized miniature cars is also briefly described.

The dynamic window approach (DWA) is a well-known navigation scheme developed by Fox et al. and extended by Brock and Khatib. It is safe by construction, and has been shown to perform very efficiently in experimental setups. However, one can construct examples where the proposed scheme fails to attain the goal configuration. What has been lacking is a theoretical treatment of the algorithm's convergence properties. Here we present such a treatment by merging the ideas of the DWA with the convergent, but less performance-oriented, scheme suggested by Rimon and Koditschek. Viewing the DWA as a model predictive control (MPC) method and using the control Lyapunov function (CLF) framework of Rimon and Koditschek, we draw inspiration from an MPC/CLF framework put forth by Primbs to propose a version of the DWA that is tractable and convergent.

In this paper, we study a scenario where a set of vehicles having different origins and/or destinations move in a common region. The goal is to have the vehicles join and leave formations in a completely decentralized manner. When a vehicle traveling along its own path finds itself moving close to another vehicle it automatically switches into follower mode. The vehicle stays in follower mode as long as the path of the other vehicle is beneficial to it. If, at some point, the leader is not moving towards the destination of the follower, the follower leaves the leader and head of on its own. We address this problem for a group of dynamic unicycle robots. Incorporating the split and join capability into a Receding Horizon Control approach to obstacle avoidance we are able to show safety as well as convergence of all vehicles to their destinations under general nonconvex obstacle assumptions. We illustrate the method with a simulation example.

In this paper, we present an approach to obstacle avoidance for a group of unmanned vehicles moving in formation. The goal of the group is to move through a partially unknown environment with obstacles and reach a destination while maintaining the formation. We address this problem for a class of dynamic unicycle robots. Using Input-to-State Stability we combine a general class of formation-keeping control schemes with a new dynamic window approach to obstacle avoidance in order to guarantee safety and stability of the formation as well as convergence to the goal position. An important part of the proposed approach can be seen as a formation extension of the configuration space obstacle concept. We illustrate the method with a challenging example.

The dynamic window approach is a well known navigation scheme developed by Fox et. al. (1997) and extended by Brock and Khatib (1999). It is safe by construction and has been shown to perform very efficiently in experimental setups. However, one can construct examples where the proposed scheme fails to attain the goal configuration. What has been lacking is a theoretical treatment of the algorithm's convergence properties. A first step towards such a treatment was previously presented by the authors (2002). Here we continue that work with a computationally tractable algorithm resulting from a careful discretization of the optimal control problem of the previous paper and a way to construct a continuous navigation function. Inspired by the similarities between the dynamic window approach and the control Lyapunov function and receding horizon control synthesis put forth by Primbs et. al. (1999) we propose a version of the dynamic window approach that is tractable and provably convergent.

In this paper, the multiagent coordination problem is studied. This problem is addressed for a class of robots for which control Lyapunov functions can be found. The main result is a suite of theorems about formation maintenance, task completion time, and formation velocity. It is also shown how to moderate the requirement that, for each individual robot, there exists a control Lyapunov function. An example is provided that illustrates the soundness of the method.

A solution to the trajectory tracking problem for mobile manipulators is proposed, that allows for the base to be influenced by a reactive, obstacle avoidance behavior. Given a trajectory for the gripper to follow, a tracking algorithm for the manipulator is designed, and at the same time the base motions are generated in such a way that the base is coordinated with the gripper. Furthermore, it is shown that the method allows arbitrary upper and lower bounds on the gripper-base distance to be set and this can be achieved without introducing deadlocks into the system. The solution also ensures that the control effort, spent on slow base motions, is kept small.