Vision sensors are extensively used for localizing a robot's pose, particularly in environments where global localization tools such as GPS or motion capture systems are unavailable. In many visual navigation systems, localization is achieved by detecting and tracking visual features or landmarks, which provide information about the sensor's relative pose. For reliable feature tracking and accurate pose estimation, it is crucial to maintain visibility of a sufficient number of features. This requirement can sometimes conflict with the robot's overall task objective. In this paper, we approach it as a constrained control problem. By leveraging the invariance properties of visibility constraints within the robot's kinematic model, we propose a real-time safety filter based on quadratic programming. This filter takes a reference velocity command as input and produces a modified velocity that minimally deviates from the reference while ensuring the information score from the currently visible features remains above a user-specified threshold. Numerical simulations demonstrate that the proposed safety filter preserves the invariance condition and ensures the visibility of more features than the required minimum. We also validated its real-world performance by integrating it into a visual simultaneous localization and mapping (SLAM) algorithm, where it maintained high estimation quality in challenging environments, outperforming a simple tracking controller.

Unsupervised skill discovery in reinforcement learning (RL) aims to learn diverse behaviors without relying on external rewards. However, current methods often overlook the periodic nature of learned skills, focusing instead on increasing the mutual dependency between states and skills or maximizing the distance traveled in latent space. Considering that many robotic tasks—particularly those involving locomotion—require periodic behaviors across varying timescales, the ability to discover diverse periodic skills is essential. Motivated by this, we propose Periodic Skill Discovery (PSD), a framework that discovers periodic behaviors in an unsupervised manner. The key idea of PSD is to train an encoder that maps states to a circular latent space, thereby naturally encoding periodicity in the latent representation. By capturing temporal distance, PSD can effectively learn skills with diverse periods in complex robotic tasks, even with pixel-based observations. We further show that these learned skills achieve high performance on downstream tasks such as hurdling. Moreover, integrating PSD with an existing skill discovery method offers more diverse behaviors, thus broadening the agent’s repertoire. Our code and demos are available at https://jonghaepark.github.io/psd

Safety verification in motion planning is known to be computationally burdensome, despite its importance in robotics. In this paper, we investigate the behavior of safe receding horizon motion planners when the update interval becomes infinitesimal. By requiring the trajectory parameters to evolve continuously in time, the trajectory optimization problem is reformulated into a time-derivative form, whose decision variables are their rate of change. This results in a quadratic programming problem which directly provides safe input, and can be regarded as a real-time safety filter. The input expressivity is also enhanced by leveraging the differentiable structure of the parameter space. The proposed safety filter is experimentally validated using a wheeled ground robot in obstacle-cluttered environments. The result shows that the safety filter is capable of generating safe inputs in real-time, while addressing hundreds of constraints simultaneously.

Autonomous aerial manipulation requires the capability to handle inevitable dynamic changes during physical interaction. Previously, very few studies have addressed the stability and transient performance of the scenarios involving abrupt changes in dynamics. This paper proposes a hybrid controller enhancing transient performance for an aerial manipulator extracting an object wedged in a static structure. This task incurs a significant jump in the interaction force on the end-effector so that the analysis using the concept of hybrid dynamical systems is required. To demonstrate the dynamic characteristics of the object-extracting aerial manipulator, we derive the dynamic equations for two flight modes, i.e., free-flight and object-extracting, and the rule of state jumps. Also, we design control strategies which enhance the transient performance during flight mode transition. Then, the stability of the proposed control law is proven, and the overshoot reduction after the object extraction is analyzed. To show the improved performance, we conduct plug-pulling experiments with a quadrotor-based aerial manipulator using the proposed controller and two different existing controllers. The comparative results confirm that our controller enables the aerial manipulator to maintain its stability after the flight mode transition and shows the best transient performance in overshoot minimization among three controllers.Note to Practitioners—The motivation for this article is the desire to prevent unexpected collisions between obstacles and an aerial manipulator after the vehicle extracts a wedged object from a static structure. To resolve this problem, we present a hybrid control method for such tasks while avoiding an excessive overshoot after the extraction. This method can be utilized in tasks involving abrupt changes in the dynamic model such as retrieving a device attached to a tall structure, reclaiming an object in disaster recovery, or pulling a plug out of a socket. Unlike existing methods, the proposed method simultaneously considers the stabilityandthe initial overshoot right after pulling the object out of the structure, by employing a disturbance observer (DOB) in a hybrid control structure. It can be utilized for the aerial manipulator to avoid collision in a narrow space or to keep it in a safe operation envelope even though the task requires producing a relatively large pulling force.

The motion planning problem for multiple unstop-pable agents is of interest in many robotics applications, for example, autonomous traffic management for multiple fixed-wing aircraft. Unfortunately, many of the existing algorithms cannot provide safety for such agents, because they require the agents to be able to brake to a complete stop for safety and feasibility insurance. In this paper, we present a distributed multi-agent motion planner that guarantees collision avoidance and persistent feasibility, which can be applied to a team of homogeneous mobile vehicles that cannot stop. The planner is built on top of the idea that a collision-free trajectory in form of a loop can safely accommodate multiple unstoppable agents, while avoiding collisions among them and static obstacles. At every time step, in a distributed manner, the agents generate trajectory-manipulating actions that preserve the loop structure. Then, a deconfliction process selects a conflict-free subset of the generated actions, which are applied at the next time step. Through simulation using an unstoppable Dubins car model, we show that the proposed motion planner is able to provide persistent safety guarantees for such agents in obstacle-cluttered space in real-time.

This paper presents a decentralized multi-agent trajectory planning (MATP) algorithm that guarantees to generate a safe, deadlock-free trajectory in an obstacle-rich environment under a limited communication range. The proposed algorithm utilizes a grid-based multi-agent path planning (MAPP) algorithm for deadlock resolution, and we introduce the subgoal optimization method to make the agent converge to the waypoint generated from the MAPP without deadlock. In addition, the proposed algorithm ensures the feasibility of the optimization problem and collision avoidance by adopting a linear safe corridor (LSC). We verify that the proposed algorithm does not cause a deadlock in both random forests and dense mazes regardless of communication range, and it outperforms our previous work in flight time and distance. We validate the proposed algorithm through a hardware demonstration with ten quadrotors.

This article presents an online distributed trajectory planning algorithm for a quadrotor swarm in a maze-like dynamic environment. We utilize a dynamic linear safe corridor to construct the feasible collision constraints that can ensure interagent collision avoidance and consider the uncertainty of moving obstacles. We introduce mode-based subgoal planning to resolve deadlock faster in a complex environment using only previously shared information. For dynamic obstacle avoidance, we adopt heuristic methods such as collision alert propagation and escape point planning to deal with the situation where dynamic obstacles approach the agents clustered in a narrow corridor. We prove that the proposed algorithm guarantees the feasibility of the optimization problem for every replanning step. In an obstacle-free space, the proposed method can compute the trajectories for 60 agents on average 7.66 ms per agent with an Intel i7 laptop and shows the perfect success rate. Also, our method shows 64.5$\%$shorter flight time than buffered Voronoi cell and 34.6$\%$shorter than with our previous work. We conduct the simulation in a random forest and maze with four dynamic obstacles, and the proposed algorithm shows the highest success rate and shortest flight time compared to state-of-the-art baseline algorithms. In particular, the proposed algorithm shows over 97$\%$success rate when the velocity of moving obstacles is below the agent's maximum speed. We validate the safety and robustness of the proposed algorithm through a hardware demonstration with ten quadrotors and two pedestrians in a maze-like environment.

Safety guarantee prior to the deployment of robots can be difficult due to unexpected disturbances in runtime. This article presents a real-time receding-horizon robust trajectory planning algorithm for nonlinear closed-loop systems, which guarantees the safety of the system under unknown but bounded disturbances. We characterize the forward reachable sets (FRSs) of the system based on the Hamilton–Jacobi reachability analysis as a means for safety verification. For the online computation of the FRSs, we approximate nonlinear systems as LTV systems with linearization errors and compute ellipsoids that encompass the FRSs in continuous time. Using the proposed ellipsoidal approximation of the FRSs, we formulate a computationally tractable robust planning problem that can be solved online. Consequently, the proposed method enables real-time replanning of a reference trajectory with safety guarantees even when the system encounters unexpected disturbances in runtime. The flight experiment of obstacle avoidance in a windy environment validates the proposed robust planning algorithm.

Hierarchical Reinforcement Learning (HRL) has made notable progress in complex control tasks by leveraging temporal abstraction. However, previous HRL algorithms often suffer from serious data inefficiency as environments get large. The extended components, $i.e.$, goal space and length of episodes, impose a burden on either one or both high-level and low-level policies since both levels share the total horizon of the episode. In this paper, we present a method of Decoupling Horizons Using a Graph in Hierarchical Reinforcement Learning (DHRL) which can alleviate this problem by decoupling the horizons of high-level and low-level policies and bridging the gap between the length of both horizons using a graph. DHRL provides a freely stretchable high-level action interval, which facilitates longer temporal abstraction and faster training in complex tasks. Our method outperforms state-of-the-art HRL algorithms in typical HRL environments. Moreover, DHRL achieves long and complex locomotion and manipulation tasks.

This paper presents an approach that generates the overall trajectory of mobile manipulators for a complex mission consisting of several sub-tasks. Parametric dynamic movement primitives (PDMPs) can quickly generalize the online motion of robot manipulation by learning multiple demonstrations in offline. However, regarding complex missions consisting of multiple sub-tasks, a large number of demonstrations are required for full generalization, which is impractical. In this paper, we propose a framework that reduces the number of demonstrations for a complex mission. In the proposed method, complex demonstrations are segmented into multiple unit motions representing sub-tasks, and one PDMP is formed per each segment, resulting in multiple PDMPs. The phase decision process determines which sub-task and associated PDMPs to be executed online, allowing multiple PDMPs to be autonomously configured within an integrated framework. In order to generalize the execution time and regional goal in each phase, the Gaussian process regression (GPR) is applied. Simulation results from two different scenarios confirm that the proposed framework not only effectively reduces the number of demonstrations but also improves generalization performance. The actual experiments also demonstrate that the mobile manipulators effectively perform complex missions through the proposed framework. Note to Practitioners—This paper presents an approach of learning from demonstration (LfD) to generalize complex movements of robots. Parametric dynamic movement primitives (PDMPs) compute styles of movements from multiple demonstrations. However, the complexity of the PDMP increases as the mission involves more sub-tasks. In this paper, we resolve this issue by segmenting the complex mission into multiple sub-tasks and configuring multiple PDMPs. This work effectively reduces the number of required demonstrations for PDMPs, moderates the complexity of the algorithm. Also, the proposed approach allows flexible sub-task sequencing. It enables the mission in an unlearned sequence or a new combination of sub-tasks. The proposed approach is validated in both simulation and experimental results. Our approach is applicable for complex missions whose sub-tasks are clearly identified

In this paper, an autonomous aerial manipulation task of pulling a plug out of an electric socket is conducted, where maintaining the stability and robustness is challenging due to sudden disappearance of a large interaction force. The abrupt change in the dynamical model before and after the separation of the plug can cause destabilization or mission failure. To accomplish aerial plug-pulling, we employ the concept of hybrid automata to divide the task into three operative modes, i.e, wire-pulling, stabilizing, and free-flight. Also, a strategy for trajectory generation and a design of disturbance-observer-based controllers for each operative mode are presented. Furthermore, the theory of hybrid automata is used to prove the stability and robustness during the mode transition. We validate the proposed trajectory generation and control method by an actual wire-pulling experiment with a multirotor-based aerial manipulator.

Motion planners for mobile robots in unknown environments face the challenge of simultaneously maintaining both robustness against unmodeled uncertainties and persistent feasibility of the trajectory-finding problem. That is, while dealing with uncertainties, a motion planner must update its trajectory, adapting to the newly revealed environment in real-time; failing to do so may involve unsafe circumstances. Many existing planning algorithms guarantee these by maintaining the clearance needed to perform an emergency brake, which is itself a robust and persistently feasible maneuver. However, such maneuvers are not applicable for systems in which braking is impossible or risky, such as fixed-wing aircraft. To that end, we propose a real-time robust planner that recursively guarantees persistent feasibility without any need of braking. The planner ensures robustness against bounded uncertainties and persistent feasibility by constructing a loop of sequentially composed funnels, starting from the receding horizon local trajectory’s forward reachable set. We implement the proposed algorithm for a robotic car tracking a speed-fixed reference trajectory. The experiment results show that the proposed algorithm can be run at faster than 16 Hz, while successfully keeping the system away from entering any dead end, to maintain safety and feasibility.

Automation of excavation tasks requires real-time trajectory planning satisfying various constraints. To guarantee both constraint feasibility and real-time trajectory re-plannability, we present an integrated framework for real-time optimization-based trajectory planning of a hydraulic excavator. The proposed framework is composed of two main modules: a global planner and a real-time local planner. The global planner computes the entire global trajectory considering excavation volume and energy minimization while the local counterpart tracks the global trajectory in a receding horizon manner, satisfying dynamic feasibility, physical constraints, and disturbance-awareness. We validate the proposed planning algorithm in a simulation environment where two types of operations are conducted in the presence of emulated disturbance from hydraulic friction and soil-bucket interaction: shallow and deep excavation. The optimized global trajectories are obtained in an order of a second, which is tracked by the local planner at faster than 30 Hz. To the best of our knowledge, this work presents the first real-time motion planning framework that satisfies constraints of a hydraulic excavator, such as force/torque, power, cylinder displacement, and flow rate limits.

This paper presents a new efficient algorithm which guarantees a solution for a class of multi-agent trajectory planning problems in obstacle-dense environments. Our algorithm combines the advantages of both grid-based and optimization-based approaches, and generates safe, dynamically feasible trajectories without suffering from an erroneous optimization setup such as imposing infeasible collision constraints. We adopt a sequential optimization method with dummy agents to improve the scalability of the algorithm, and utilize the convex hull property of Bernstein and relative Bernstein polynomial to replace non-convex collision avoidance constraints to convex ones. The proposed method can compute the trajectory for 64 agents on average 6.36 seconds with Intel Core i7-7700 @ 3.60GHz CPU and 16G RAM, and it reduces more than 50% of the objective cost compared to our previous work. We validate the proposed algorithm through simulation and flight tests.