An Online Path-Planning Strategy for an Unmanned Aerial Vehicle Crossing Mobile Narrow Passages

Featured Application

The proposed approach has promising applications in critical scenarios where UAVs must operate efficiently and safely in unstructured environments. For example, it can be utilized in post-disaster search and rescue missions, where drones navigate through rubble-strewn areas to locate survivors while avoiding hazardous obstacles. Additionally, this methodology is valuable for inspecting complex industrial sites, such as offshore oil platforms or wind farms, where geometric constraints and moving obstacles pose significant challenges. By ensuring stable and secure operation in these dynamic conditions, this work contributes to advancing UAV capabilities in mission-critical tasks requiring high precision and reliability.

Abstract

Aerial inspection missions over long distances are guided by planning paths with minimum energy consumption, which generally leads to routes with shorter distances. However, these conditions cannot always be met due to the environment’s geometric constraints, especially in the most unexpected (unsafe, dangerous, and risky) scenarios. The aim of this work is to improve the efficiency, stability, and security of unmanned aerial vehicles (UAVs) during inspection missions in unstructured and unpredictable environments. Traditional path-planning techniques that prioritize minimum energy consumption often lead to routes with shorter distances, but these can be hindered by environmental geometric constraints and risky scenarios. To address this challenge, a new path-planning algorithm is proposed that ensures safe passage through challenging moving locations while respecting the physical limitations of the robot and the shape of narrow passages. The algorithm is combined with a controller design for obstacle avoidance. Experimental results with different motion restrictions are presented, demonstrating the effectiveness of the proposed approach.

1. Introduction

With the rapid evolution of aircraft technologies, computational resources, actuators, sensors, and communication, unmanned aerial vehicles (UAVs) have been increasingly adopted as a suitable tool for several applications [1]. UAVs are playing important roles not only in urban environments, but also in hazardous areas that are not easily reachable, such as coastal zones, caves, and forests [2,3,4]. In recent years, UAVs have been extensively researched for various applications, including search and rescue operations [5,6,7], monitoring and inspection [8,9,10,11,12], package delivery [13,14,15,16,17,18], and underground mining [19,20,21]. These applications have garnered attention from academic, scientific, and industrial communities worldwide, requiring the development of autonomous flight technologies that guarantee the safety of the equipment and interaction with the robot’s workspace, which can include static and moving objects.

Autonomous navigation is related to the accurate and safe control of the UAV flight, even in unexpected and risky environments. This requires advanced robot cognition and deliberation for navigation, which is common in inspection tasks [22,23,24]. Most of these tasks involve narrow sections and constrained structures/spaces for the UAVs navigate through. A key challenge lies in generating precise motion planning to ensure the safe inclusion of service UAVs in urban environments. Additionally, motion planning research and development serve academic and educational objectives, including UAV racing and competitions [25,26,27,28].

The challenging question is: “How to develop feasible solutions to search for an optimal or quasi-optimal, safe, and time-efficient path between two locations (source and destination) in a 3D map to overcome an obstacle in the scenario?” [29,30,31]. To ensure safe operation, a UAV must possess the ability to perceive its surrounding environment and respond accordingly to avoid collisions with both stationary and moving obstacles. Additionally, it should be capable of navigating through narrow passages encountered during its mission. An effective approach involves equipping UAVs with range sensors, cameras, or global positioning systems (GPS) to capture relevant information, enabling autonomous decision-making. In essence, the UAV needs to determine a viable path and autonomously control its movements to follow that path.

From a practical point of view, this work addresses the challenge of UAV navigation in environments with narrow passages, such as forests with tree branches, minefields with varying pit sizes, or disaster-affected areas with irregularly shaped structures. These scenarios create conditions where UAVs must navigate through narrow and unpredictable spaces, requiring adaptive path-planning strategies. The motivation for this study stems from the need to develop approaches capable of dynamically adjusting navigation routes in such environments.

In this work, a method is proposed to find a feasible path between two locations while safely navigating considering obstacles/narrow passages in the environment, while satisfying the optimization objective of minimizing the distance to cross a circular hoop. The planned path ensures collision-free motion and respects the physical/kinematic constraints of the UAV as well. Figure 1 illustrates the proposed approach, which is described in detail in the following. In the scenario considered in this work, the UAV has a primary mission considering an obstacle-free workspace, since the environment is unfamiliar. During navigation, the UAV must be able to locate itself and map the environment identifying possible objects in its path and making appropriate and safe decisions to avoid obstacles. It is important to point out that identifying obstacles and mapping the environment are outside the scope of this work, which focuses on navigation and evasion strategies.

In addition to providing context, Section 1.1 reviews similar works and strategies of the literature, formulating the problem as a problem of motion planning under constraints.

1.1. Background and Related Works

Researchers have proposed various methods for generating spatial references to guide the movement of UAVs in three-dimensional (3D) space, classified as global or local planning for single or multiple agents. The key distinction between these techniques lies in the level of prior knowledge about the environment [32]. Global path planning focuses on finding a path in a known environment, whereas local path planning becomes more challenging when the UAV operates in a partially or fully unknown environment.

Global path-planning techniques play a crucial role in enhancing the autonomy of UAVs, encompassing three key aspects. Firstly, a comprehensive map or model of the operating environment, such as a complete 3D map or graph representation, is required. Secondly, a search algorithm is employed to determine a sub-optimal path with minimal computational effort. Lastly, secondary navigation objectives, such as energy efficiency, smoothness, turns, or continuity, are analyzed during the path search process using heuristic or metaheuristic functions. Various algorithms, including potential field [33,34,35], A* [36,37], Dijkstra [38,39], and improved versions like LPA* [40,41], have been widely adopted for efficient and cost-effective path search.

To address the significant computational demands of these approaches, Majeed and Hwang [42] introduced a method that focuses on path-finding within regions with a high probability of providing an optimal or sub-optimal collision-free path. This approach utilizes Constrained Polygonal Space (CPS) and an Extremely Sparse Waypoint Graph (ESWG), restricting path exploration to areas with a higher probability of containing the optimal trajectory. Additionally, Song et al. [43] present a Reinforcement Learning (RL)-based framework capable of generating highly aggressive trajectories for autonomous UAV racing. The strategy is able to identify the gates/narrow passages and navigate achieving speeds of up to 60 km h⁻¹ with a physical UAV, while performing aggressive maneuvers. Also, in terms of UAV racing, navigation into gates is crucial in some phases, as depicted in [43]. Rojas-Perez and Martinez-Carranza [44] have developed a Deep Learning (DL) solution called DeepPilot for gate detection and localization relative to a UAV. DeepPilot utilizes a Convolutional Neural Network (CNN) that processes camera images to predict flight commands, particularly attack angle references for the UAV’s body frame. The framework assumes that the subsequent gate comes into view right after the current gate is passed. Similarly, Cui and Wang [32] introduce a multi-layered planning framework based on RL incorporating distinct layers for global and local planning.

A smooth, closed path that ensures to safely pass through challenging locations is proposed in [45], where waypoints in a Bèzier-based curve guarantee smoothness and continuity in curvature and torsion. A drawback is that this strategy requires a known map. In turn, in [46], a reactive avoidance algorithm considering only static (known as well as unknown) obstacles is proposed. The path-planning approach is based on the wavefront algorithm.

Combining local and global path planning, Bai et al. [47] propose a optimum path search strategy while ensuring accomplishing security and speed requirements. The A* algorithm process the map to find an obstacle-free path according to the motion constraints. Thereafter, the DWA algorithm performs a local optimization. Therefore, the key points of the global path are constructed as the subtarget points of the local path planning selected by DWA, reducing the planning time and the path length. Tang et al. [48] integrate the enhanced Particle Swarm Optimization (PSO) algorithm, the refined artificial potential algorithm, a path exploration switching strategy, and an energy-based task scheduling mechanism to develop a unified global and local path planning optimization approach. This method adapts the path planning mode based on the global and local obstacle conditions while incorporating site-specific details to assign a surveillance task to the UAV effectively.

In this context, the present work primarily concentrates on deterministic, geometry-based path planning methods, aiming to ensure secure and safe maneuvers for UAVs operating in environments with motion restrictions and various challenges. The proposed concept is designed to be dependable and efficient, providing a path that minimizes complexity by adopting a linear curve shape.

1.2. Aims and Work Contributions

In this context, the proposed approach involves utilizing waypoints along linear curves, determined by the geometry of narrow passages, their current states and velocities, and the UAV’s current pose, to ensure safe motion planning in confined spaces. The primary contribution is the generation of a smooth, continuous, and oriented path that can be time-parameterized for trajectory-tracking purposes. Despite its simplicity in terms of geometry and complexity, this method offers a reliable option for predictable robot behavior in constrained environments. More specifically, the key contribution lies in the development of a strategy for planning paths through mobile narrow passages and controlling the UAV to navigate them, using knowledge of obstacle positions. Object detection within the environment is outside the scope of this work, and is suggested as a potential avenue for future research.

To address the topics of interest, this manuscript is, hereinafter, split into a few sections. Firstly, Section 2 provides a detailed explanation of the proposed mission planning strategy. Subsequently, Section 2.2 elucidates the primary path-planning strategy employed to navigate a mobile passage on a moving Unmanned Ground Vehicle (UGV), taking into account the constraints imposed by the environment. Section 3 discusses the dynamic model of the robots, as well as the controllers used to guide them. Section 4 presents and analyzes the results of an experiment conducted using a Pioneer 3-DX unicycle mobile platform (representing the UGV) and a Parrot Bebop 2 quadrotor (tasked with passing through a vertically positioned hollow ring mounted on the UGV), showcasing the outcomes in detail. Lastly, Section 5 summarizes the main conclusions drawn from the research conducted.

2. Mission Planning and the Strategy to Cross Moving Narrow Passages

The ability to navigate through narrow passages is a crucial skill for aerial robots in many applications, such as structure inspection and surveillance in urban environments, for the necessity of managing the restrictions imposed by the local environmental boundaries (e.g., to avoid buildings and sidewalks). However, this task presents meaningful challenges, especially when the passage through which the UAV should navigate is in motion. The scenario described here presents a challenging task of path planning in dynamic environments, which is an active and evolving research area. Numerous strategies have been proposed to tackle this problem effectively.

This section introduces a path-planning strategy specifically designed for aerial robots, with a primary focus on enabling navigation through moving narrow passages. This strategy incorporates object detection, empowering the aerial robot to detect and respond to moving obstacles in real-time.

2.1. Mission Planning

The tasks assigned to the robot for a given mission are planned, and the control references to be reached or followed are generated. Task scheduling and allocation determine the execution order and assign responsibility to the appropriate agent. The robots follow an 8-shaped trajectory, known as the lemniscate of Bernoulli, depicted as the dashed green line in Figure 1, with the green arrow indicating their movement direction. Mathematically, the robots track the 8-shaped trajectory in opposite directions, described by the desired position,

$$\begin{array}{cc}\hfill x_d& =x_o+r_x\sin \left(\omega t\right),\hfill \\ \hfill y_d& =y_o+r_y\sin \left(2\omega t\right),\hfill \\ \hfill z_d& =z_o,\hfill \end{array}$$

where $(x_o,y_o,z_o)$ represents a constant offset value, $r_x$ and $r_y$ are the movement radii, and $\omega$ is the angular velocity. In this work, $(x_o,y_o,z_o)=(0,0.5,0.75)\mathrm{m}$, $r_x=r_y=1\mathrm{m}$, and $\omega =1/60\mathrm{s}$.

In certain scenarios, however, the UAV encounters a moving narrow passage that coincides with its current calculated reference. To avoid a collision with the moving object, the UAV must either deviate or cross the passage, provided that it ensures safe crossing. This determination depends on the UAV’s dimensions, the narrow passage’s size, and the estimation of its state and velocity relative to the UAV’s dynamics and speed limits. Accurate information is essential for avoiding collisions during the crossing.

After checking that flying across the passage is possible, the strategy proposed here is to change the main mission assigned to the UAV to a path-planning algorithm that prioritizes crossing the moving narrow passage safely, resuming the original mission after leaving such passage behind. Thus, the robot navigates reactively, prioritizing its mission until facing a static/moving obstacle.

The procedure for determining a path through a narrow passage starts by searching a waypoint or constructing a visibility graph from the robot’s current position upon detecting the moving obstacle and progressing the data until the target is located, i.e., the opposite side of the narrow passage. The effectiveness of a path-planning method usually depends on selecting waypoints from a given graph that contributes to an optimal or near-optimal path with minimizing the computation effort. This work addresses the UAV path-planning problem, aiming to reduce the complexity without degrading the path quality. Accordingly, a novel approach is proposed to incorporate the robot’s state and its surroundings into the spatial motion planning process.

2.2. Spatial Path-Planning

Instead of relying on traditional obstacle avoidance techniques, this approach incorporates the geometry, as well as the estimated pose and velocity of the narrow passage, as additional information for carefully planning the UAV’s motion. As illustrated in Figure 2a, a plane tangent to the section of a passage (defined by the green and blue vectors) is considered. By considering its instantaneous normal unit vector $\widehat{\mathbf{n}}$ (depicted as the red arrow), located at the centroid ${\mathbf{c}}_p$ of such movement-restrainers, once assured that this passage is large enough to be crossed by the robot, the pair $[{\mathbf{c}}_p,\widehat{\mathbf{n}}]$ can be adopted as the desired pose. This pose is the rotated by 90° around the estimated z-axis (depicted as the blue arrow) to enable the UAV to navigate through complex static or dynamic segments in its path. The simplicity of a line segment connecting two points is used to achieve this motion planning.

Once the pair $[{\mathbf{c}}_p,\widehat{\mathbf{n}}]$ is known, a safety distance $d_s$ from movement-restrainers is set based on the dimensions of the robot, to calculate

$${\mathbf{c}}_{p_{-,+}}={\mathbf{c}}_p\pm d_s\widehat{\mathbf{n}},$$

(1)

thus computing a line segment from ${\mathbf{c}}_{p_{-}}$ to ${\mathbf{c}}_{p_{+}}$, passing through ${\mathbf{c}}_p$ with the same orientation as $\widehat{\mathbf{n}}$, guaranteeing a path through generic narrow passages, as shown in Figure 2b. In this work, a proof of concept is demonstrated in order to elucidate the application of the proposed strategy. However, in practical terms, the structures found in real environments are generally more complex and have specific curvatures. In this sense, the proposed strategy can be extended by a more elaborate curve, with smoothly and continuously connected line segments, for example, based on Bézier curves.

Once the UAV identifies an oriented path through all movement constraints, it must determine on how and when to transition from its current pose to ${\mathbf{c}}_{p_{-}}$ in a safe and smooth manner, considering its potentially restricted surroundings. Additionally, it is essential to establish the point at which the UAV successfully traverses the narrow passage, allowing it to resume its primary mission. Specifically, upon reaching ${\mathbf{c}}_{p_{+}}$, the task scheduling system should instruct the UAV to continue its main task.

In summary, the whole process of tracing line segments is repeated for each restrictive passage in the environment, as summarized in Algorithm 1. Subsequently, a control strategy should be developed to benefit from the properties of this path-planning method, ensuring that different UAVs can effectively apply it to navigate narrow passages.

Algorithm 1 Path-planning for moving narrow passages.

1: Initialize Parameters: $r,d_s,{\delta }_{min},v_x$ 2: while $t<t_{max}$ & Battery voltage level > 10% do 3:     Determines the nearest passage 4:     Estimates ${\mathbf{c}}_p$ and the passage’s posture 5:     Calculates the mobile spherical region containing the obstacle 6:     if the UAV is inside the spherical region then 7:         Compute line segment to cross current passage 8:         Overlays the original mission 9:     end if 10: end while

2.3. Path-Following

The path-following algorithm is based on the carrot-chasing strategy, which has already been demonstrated to be both stable and effective [49]. To enhance control adaptability for various UAVs, the reference velocity is introduced as ${\mathbf{v}}_{ref}$, composed of two components. The first is tangent to the path, denoted as ${\mathbf{t}}_p$ and illustrated by the yellow arrows in Figure 3. The second is tangent to the positioning error between the UAV and the nearest point on the path, denoted as ${\mathbf{t}}_{\tilde{\mathbf{x}}}$ and represented by the orange arrows in the same figure.

As the robot approaches the path, the component ${\mathbf{t}}_{\tilde{\mathbf{x}}}$ reduces, because the position and the path-following error also reduce. As the robot moves closer to the path, the resulting reference velocity tends to approach the component tangent to the path, until only ${\mathbf{v}}_{ref}={\mathbf{t}}_p$ remains. At this point, the robot crosses the moving passage in the opposite direction of its movement. The idea behind this approach is to generate an attractive force to make the robot approach the path as quickly as possible, i.e., to approach the line with origin in the geometric center of the moving obstacle. Once the path is reached, the tangential component of the error between the robot and the nearest point on the path becomes small enough, and the robot can track the path and cross the obstacle safely.

Describing the path as a succession of equally spaced k waypoints, ${\mathbf{c}}_k$, along the coordinates from ${\mathbf{c}}_{p_{-}}$ to ${\mathbf{c}}_{p_{+}}$, we can define the position error as $\tilde{\mathbf{x}}={\mathbf{c}}_i-\mathbf{x}$, where i denotes the point on the path that is closest to the robot at a given time instant. Thus, we can define a vector tangent to the position error as ${\mathbf{t}}_{\tilde{\mathbf{x}}}={\mathbf{K}}_p\tilde{\mathbf{x}}$, where ${\mathbf{K}}_p$ is a gain matrix, denoting the aggressiveness that the robot approaches the path. The path tangent direction at a given waypoint, ${\mathbf{t}}_{p,i}$, can be directly obtained as the unit vector along the segment from ${\mathbf{c}}_i$ to ${\mathbf{c}}_{i+1}$.

Finally, assuming as an additional control objective, the reference velocity to be sent to the UAV, which is given by

$${\mathbf{v}}_{ref}=\left\{\begin{array}{cc}{v}_x{\mathbf{t}}_{p,i}+{\mathbf{t}}_{\tilde{x}},\hfill & \mathrm{if}\left|\right|\tilde{\mathbf{x}}\left|\right|<{\delta }_{min}\hfill \\ {\mathbf{t}}_{\tilde{x}},\hfill & \mathrm{otherwise}.\hfill \end{array}\right.,$$

(2)

where $v_x$ is a user-defined scalar that is set as a value lower than the maximum velocity that the UAV can achieve. On the other hand, ${\delta }_{min}$ denotes the minimum distance required for the robot to be considered as having reached the path.

Assuming that the exact/estimated current UAV position ($\mathbf{x}$) and the position of the centroid of the narrow passage (${\mathbf{c}}_p$) are known, one can define the path waypoints and the limited region in which the robot reference becomes the straight line segment generated by the path-planning algorithm to cross the moving narrow passage. Given that the primary control objective is to accurately track the predefined trajectory, the UAV leaves its regular mission only when necessary. Specifically, the UAV deviates from its intended path upon entering a spherical region centered around the estimated position of the narrow passage section’s centroid, as shown in Figure 2c. The moving obstacle is accompanied by a spherical region that surrounds it. This region has a radius larger than the predefined security distance $d_s$, with an additional term r to ensure that the entire passage is contained within this region. However, note that the UAV only proceeds with this step after confirming that it is indeed feasible to safely cross the passage.

3. Robots Dynamic Modeling and Control

In this work, the proof of concept of the proposal is validated through experiments with UAVs and Unmanned Ground Vehicles (UGVs) carrying cylinders, which represent mobile narrow passages. In order to apply the proposed path planning strategy, a previously validated kinematic models was applied alongside with dynamic compensation towards computing valid control signals for the UGV and the UAV, in simulations and experiments. Parameters needed to compute the dynamic compensation were identified in the literature, and are described in the following.

3.1. The UGV Model and Control

In this work, we used the Pioneer P3-Dx, a unicycle like mobile robot, for which the kinematic model can be described as shown in Figure 4. Considering that navigation occurs only in a horizontal plane, the robot pose is $\mathbf{x}={\left[xy\psi \right]}^{⊺}$, in which x and y are the coordinates of the control point located at the center of the virtual axis that joins its wheels $\langle r\rangle$, and $\psi$ is its heading with respect to its x axis, based on the inertial frame $\langle w\rangle$. Also, u and $\omega$ are the linear and angular velocities of the robot, respectively, and $\mathbf{u}={\left[u\omega \right]}^{\top }$ is the vector composed of this velocities. The non-zero constant a is the distance from the origin of the body axes (the virtual line linking the two driven wheels) to the point of interest for control purposes. Note also that the mobile robot is seeking the goal/target, with specific orientation $\theta$, represented by the reference in $\langle g\rangle$.

The system of kinematics equations describing the motion of the robot over time in the Cartesian plane is presented in [50,51]. The UGVs used in this work are a unicycle-type mobile robot, whose kinematic model is given by

$$\left[\begin{array}{c}\dot{x}\\ \dot{y}\\ \dot{\psi }\end{array}\right]=\left[\begin{array}{cc}\cos \psi & -a\sin \psi \\ \sin \psi & a\cos \psi \\ 0& 1\end{array}\right]\left[\begin{array}{c}u\\ \omega \end{array}\right],$$

(3)

in which $\dot{x}$ and $\dot{y}$ represent the x and y components of the robot’s current linear velocity represented in the inertial frame $\langle g\rangle$, $\dot{\psi }$ is its angular velocity. Additionally, u and $\omega$ are the control inputs—namely, the linear and the angular velocity, respectively.

Representing the robot navigation in polar coordinates, we have

$$\begin{array}{cc}\hfill \rho & =\sqrt{{(x_d-x)}^2+{(y_d-y)}^2}\hfill \\ \hfill \theta & =\arctan {\displaystyle \frac{y_d-y}{x_d-x}}\hfill \\ \hfill \alpha & =\theta -\psi ,\hfill \end{array}$$

(4)

in which $\rho$ is the robot-destination distance, $\theta$ is the orientation of reference in the target point, and $\alpha$ is the heading error to the destination.

In this work, the navigation circuit consists of a set of waypoints without any time constraints. Thus, the robot executes a regulation (positioning) task, where the temporal derivatives of the reference positions are zero. With this understanding, taking the first time derivative of (4) and replacing (3) yields the kinematic model expressed in polar coordinates.

$$\left[\begin{array}{c}\dot{\rho }\\ \dot{\alpha }\end{array}\right]=\left[\begin{array}{cc}-\cos \alpha & -a\sin \alpha \\ {\displaystyle \frac{\sin \alpha }{\rho }}& -a{\displaystyle \frac{\cos \alpha }{\rho }}-1\end{array}\right]\left[\begin{array}{c}u\\ \omega \end{array}\right],\mathrm{or}\mathrm{in}\mathrm{the}\mathrm{compact}\mathrm{form}\dot{\mathbf{h}}=\mathbf{Bu},$$

(5)

in which $\dot{\theta }=\dot{\alpha }+\omega$.

Assuming that the sequence of waypoints prioritizes task-oriented arrival, we design a controller without a specified final orientation. In this scenario, $\theta$ is treated as a free variable, constrained only to remain bounded at the final state. Using Lyapunov theory as a foundation, we propose the following radially unbounded candidate function:

$$V\left(\mathbf{h}\right)={\displaystyle \frac{1}{2}}{\mathbf{h}}^{⊺}\mathbf{h}>0.$$

(6)

Applying the feedback linearization technique, we can propose the kinematic control signal,

$$\mathbf{u}=-{\mathbf{B}}^{-1}\mathbf{G}\tanh \mathbf{h},$$

(7)

in which $\mathbf{G}$ is a positive defined gain matrix.

After applying its first time derivative in (6), and replacing (5) and (7), we obtain

$$\begin{array}{c}\dot{V}\left(\mathbf{h}\right)={\mathbf{h}}^{⊺}\dot{\mathbf{h}}={\mathbf{h}}^{⊺}\mathbf{Bu}=-{\mathbf{h}}^{⊺}\mathbf{G}\tanh \mathbf{h}<0.\end{array}$$

(8)

This establishes the global asymptotic convergence of $\mathbf{h}\to \mathbf{0}$ as $t\to \infty$, indicating that the robot will reach its target in an obstacle-free environment. Furthermore, as a direct result, $\mathbf{u}\to \mathbf{0}$ is also achieved as $t\to \infty$.

The same control law can be applied to trajectory-tracking tasks. For our UGV, good performances at low-speed are achieved just assuming perfect velocity tracking, i.e., $u\equiv u_d$ and $\omega \equiv {\omega }_d$. However, in most applications, especially those performed at high speeds, the robot’s kinematic model alone is not able to track and follow trajectories and/or paths efficiently and accurately. Thus, for the design of motion controllers, it is of paramount importance to consider the robot’s dynamic modeling. This UGV is equipped with an embedded low-level controller that operates the vehicle’s two motors, allowing the implementation of the previously mentioned high-level linear and angular velocity commands, $\mathbf{u}$. To deal with the dynamic effects caused by inertia and friction, a dynamic model first introduced in [52] and updated in [53] was used.

The robot dynamic model in the compact form is presented by [54] as

$$\mathbf{M}\dot{\mathbf{u}}+\mathbf{C}\mathbf{u}={\mathbf{u}}_{ref},$$

(9)

where $\mathbf{M}$ and $\mathbf{C}$ are the Inertia and Coriolis matrices, respectively.

A positioning control described in the Cartesian space can be achieved using inverse dynamics and considering a desired trajectory. The reference velocity command can be defined as

$${\mathbf{u}}_d={\mathbf{K}}^{-1}(\tanh \left(\dot{\tilde{\mathbf{x}}}\right)+{\mathbf{K}}_p\tanh \left(\tilde{\mathbf{x}}\right)),$$

(10)

where ${\mathbf{K}}_p$ is a positive definite gain matrix and $\mathbf{K}$ is the direct kinematics matrix defined in [50,54].

To contemplate applications in which the robot needs to navigate under high-speed movements, and to evaluate a more realistic scenario, a dynamic compensation module is adopted. Considering (9), the adopted dynamic compensation control law is given by

$${\mathbf{u}}_{ref}=\mathbf{M}\left({\dot{\mathbf{u}}}_d+\kappa ({\mathbf{u}}_d-\mathbf{u})+\mathbf{C}\mathbf{u}\right)$$

(11)

in which the gain $\kappa$ is a positive definite matrix and $\tilde{\mathbf{u}}=({\mathbf{u}}_d-\mathbf{u})$ is the velocity tracking error. The control signal ${\dot{\mathbf{u}}}_u$ is obtained by numeric differentiation considering the sampling time of the control loop.

The saturation of the physical actuators introduces unpredicted non-linearities in the system, which may cause instability. Although a non-linearity is inserted in the control law, the term $\tanh (·)$ is used in (10), saturating the control signals for large position errors. This non-linearity is considered in the system stability analysis. The advantage of using it is to prevent the saturation of the robot actuators, guaranteeing good behavior, even for large errors $\dot{\tilde{\mathbf{x}}}$ and $\tilde{\mathbf{x}}$.

Now, the use of the proposed dynamic model and its properties is illustrated via the design of an dynamic compensation controller. It receives the desired control signals ${\mathbf{u}}_d$ from the kinematic controller and generates a pair of linear and angular velocity references ${\mathbf{u}}_{ref}$ for the robot servos, as shown in the diagram block of Figure 5.

Furthermore, the choice of $\tanh (·)$ over other saturation functions, such as $\mathrm{sign}(·)$, is because $\tanh (·)$ is differentiable and smooth, avoiding fast commutations and chattering effects.

3.2. The UAV Model and Control

The Bebop 2 quadcopter, produced by Parrot Inc., is a small UAV with an onboard stabilization system and an ad hoc network communication interface for the delivery of various information. Among the most transparent data to the user, we have the following variables: vehicle altitude relative to the surface, the longitudinal and lateral translation speeds in the vehicle body, and the Tait–Bryan angles in the global referential. For control purposes, the vector of such states is given by $\mathbf{h}={\left[\begin{array}{cccccc}z& v_x^b& v_y^b& \varphi & \theta & \psi \end{array}\right]}^{\top }$.

The mathematical model to be adopted for navigation and control proposals is the one described in [55,56], referring to the description presented in Figure 6. The translational coordinates of the quadrotor are $\mathbf{\xi }={\left[\begin{array}{ccc}x& y& z\end{array}\right]}^{\top }$, and its attitude is described by the vector $\eta ={\left[\begin{array}{ccc}\varphi & \theta & \psi \end{array}\right]}^{\top }$, which contains the roll, pitch, and yaw Tait–Bryan angles, both related to the inertial reference frame $\langle w\rangle$. The Newton–Euler dynamics correspondent to the used quadrotor, and its simplified form is presented in [54]. Such a simplification performs well for roll and pitch angles up to approximately 30°, corresponding to linear velocities of approximately 1.5–2 m/s [57].

A near-hover model for the quadrotor can be written in the linear form

$$\mathbf{u}={\left(\mathbf{R}{\mathbf{K}}_u\right)}^{-1}({\ddot{\mathbf{x}}}_{ref}+{\mathbf{K}}_{\dot{x}}\dot{\mathbf{x}}),$$

(12)

in which $\mathbf{x}={\left[\begin{array}{cccc}x& y& z& \psi \end{array}\right]}^{\top }$ is the pose vector (position in frame $\langle b\rangle$, and orientation, namely the yaw heading). Meanwhile, the vector of control signals $\mathbf{u}={\left[\begin{array}{cccc}{u}_{\theta }& u_{\varphi }& u_{\dot{z}}& u_{\dot{\psi }}\end{array}\right]}^{\top }\in {\mathbb{R}}^4$ contemplates the commands $u_{\theta }$ referring to the pitch angle and $u_{\varphi }$ to the rolling one, while $u_{\dot{z}}$ and $u_{\dot{\psi }}$ relate to the rate of change in linear and angular velocity along the axis $z^b$, respectively, all in the interval [−1, +1].

The matrices ${\mathbf{K}}_u$ and ${\mathbf{K}}_{\dot{x}}$ are diagonal matrices containing, respectively, the dynamic and drag parameters for the model, identified through an identification procedure [56]. The vector ${\mathbf{x}}_{ref}$ contains the reference control signal for a desired trajectory. $\mathbf{R}$ is a simplified transformation matrix, also known as the kinematic model, relating the vehicle’s $\langle w\rangle$ and the global $\langle b\rangle$ coordinate systems, only dependent of $\psi$,

$$\mathbf{R}=\left[\begin{array}{cccc}\cos \psi & -\sin \psi & 0& 0\\ \sin \psi & \cos \psi & 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right].$$

(13)

The reference signal ${\ddot{\mathbf{x}}}_{ref}$ can be obtained using a PD feedback compensator plus a feedforward term, which corresponds to ${\ddot{\mathbf{x}}}_{ref}={\ddot{\mathbf{x}}}_d+{\mathbf{K}}_d\tanh \dot{\tilde{\mathbf{x}}}+{\mathbf{K}}_p\tanh \tilde{\mathbf{x}}$. In this formulation, ${\mathbf{x}}_d$ is the defined task, which can be trajectory tracking or positioning. $\tilde{\mathbf{x}}={\mathbf{x}}_d-\mathbf{x}$ is the control error, and ${\mathbf{K}}_d$ and ${\mathbf{K}}_p$ are positive definite gain matrices.

So, we can rewrite the control law in (12) as

$${\mathbf{u}}_d={\left(\mathbf{F}{\mathbf{K}}_u\right)}^{-1}({\ddot{\mathbf{x}}}_d+{\mathbf{K}}_d\tanh \dot{\tilde{\mathbf{x}}}+{\mathbf{K}}_p\tanh \tilde{\mathbf{x}}+{\mathbf{K}}_{\dot{x}}\dot{\mathbf{x}})$$

(14)

The diagram block describing the implemented controller is shown in Figure 7.

3.3. Stability Analysis of the Controllers

For the successful experimental evaluation of the proposed high level task allocation planner, all utilized robots must have the following prerequisite skills: dynamic stability for the preselected set of tasks, ability to localize themselves, plan and execute obstacle free trajectories, as well as the infrastructure through which they can share information and negotiate task allocation. The controllers presented for the UAV, (14), and UGV, (11), were designed using the feedback linearization technique, which assumes a perfect knowledge of the models parameters. Under these assumptions, the proposed controllers make the system asymptotically stable when closing the loop, as presented in [55].

In order to analyze the stability of the equilibrium of the rotorcraft system when using the proposed control law, one can write, in closed-loop, the equation that describes the dynamics of the posture errors,

$$\ddot{\tilde{\mathbf{x}}}+{\mathbf{K}}_d\tanh \dot{\tilde{\mathbf{x}}}+{\mathbf{K}}_p\tanh \tilde{\mathbf{x}}=\mathbf{0},$$

(15)

The candidate Lyapunov’s function

$$V(\tilde{\mathbf{x}},\dot{\tilde{\mathbf{x}}})={\mathbf{K}}_p\ln \cosh \left(\tilde{\mathbf{x}}\right)+{\displaystyle \frac{1}{2}}{\dot{\tilde{\mathbf{x}}}}^{\top }\dot{\tilde{\mathbf{x}}}>0,$$

(16)

is proposed. Taking its first-time derivative and using (15), the closed-loop equation, one has

$$\begin{array}{cc}\hfill \dot{V}(\tilde{\mathbf{x}},\dot{\tilde{\mathbf{x}}})& ={\dot{\tilde{\mathbf{x}}}}^{\top }{\mathbf{K}}_p\tanh \left(\tilde{\mathbf{x}}\right)+{\dot{\tilde{\mathbf{x}}}}^{\top }\ddot{\tilde{\mathbf{x}}}\hfill \\ \hfill & =-{\dot{\tilde{\mathbf{x}}}}^{\top }{\mathbf{K}}_d\tanh \left(\dot{\tilde{\mathbf{x}}}\right)<0,\forall \dot{\tilde{\mathbf{x}}}\ne \mathbf{0}\hfill \end{array}$$

(17)

from where a sufficient condition to guarantee that $\dot{V}$ is negative semi-definite is ${\mathbf{K}}_d$ and ${\mathbf{K}}_p$ are positive definite.

Now, applying the La Salle theorem for autonomous systems, taking into account the dynamics of the system given by (15) and disregarding the disturbances, one can note that the largest invariant set $\mathbf{M}$ contained in the set $\mathsf{\Omega }=\left\{{\left[\begin{array}{cc}\tilde{\mathbf{x}}& \dot{\tilde{\mathbf{x}}}\end{array}\right]}^{\top }|\dot{V}(\tilde{\mathbf{x}},\dot{\tilde{\mathbf{x}}})=0\right\}$ is ${\left[\begin{array}{cc}\tilde{\mathbf{x}}& \dot{\tilde{\mathbf{x}}}\end{array}\right]}^{\top }={\left[\begin{array}{cc}\mathbf{0}& \mathbf{0}\end{array}\right]}^{\top }$. This demonstrates that the system (and, consequently, the tracking/regulation error origin) is asymptotically stable. Mathematically, $\dot{\tilde{\mathbf{x}}}\left(t\right),\tilde{\mathbf{x}}\left(t\right)\to \mathbf{0}$ for $t\to \infty$.

In this work, we assume that the map of the environment is a priori known, and that each vehicle knows its exact position. In practice, this is difficult to achieve without utilization of self-localization and map-building algorithms. We leave this part of the system for future work and development, but for now, we can achieve this through a combination of motion tracking system and a priori known environment.

4. Experiments, Results, and Discussion

After presenting the theoretical formulation, to assess the effectiveness and performance of the proposed motion planning strategy, a series of experiments was conducted. The experiments aimed to evaluate the strategy’s capability to navigate through narrow passages in dynamic environments. For this purpose, a circular hoop was mounted on a moving unicycle robot, serving as a representation of the narrow passages, as depicted in Figure 8.

The Bebop Parrot 2 is the UAV used in our experiments, whose dimensions are inscribed in a $330\times 390\times 90\mathrm{m}\mathrm{m}$ prism. It is worth emphasizing that the UAV successfully traverses the circular hoop while maintaining a safety margin.

Figure 9 shows an overview of the setup used to run the experiment. The positions of the two robots were obtained using an installation of the motion capture system OptiTrack, composed of eight infrared cameras detecting and tracking the robots (and, thus, the passage) in a $3\times 4\times 3\mathrm{m}$ indoor environment. As one can see in the experimental setup, two computers, a Windows 10 desktop and a Linux laptop, were used. The first runs the OptiTrack and Motive 3.1.0 software for motion capture and a MATLAB^® script, which computes the path-planning algorithm and generates the control signals. As for the Linux laptop, it contains the ROS nodes necessary to send/receive data from/for the Bebop 2. The two robots used are also shown in the figure, with the reflective markers used in connection with the OptiTrack system. Notice that the Pionner 3-DX has a circular hoop on its top, across which the UAV should fly, whose diameter is 700 mm, which represents a particularly challenging geometric constraint, especially when in motion.

Both vehicles were programmed to perform a trajectory tracking task, with a trajectory shaped as a lemniscate of Bernoulli. A video of the experiment is available at https://youtu.be/KrNmOUjahy8 (accessed on 1 January 2025), which allows one to better understand the experiment. Snapshots showing the behavior of the UAV according to the proposed strategy are presented in Figure 10. Both robots initiate the experiment from a stationary position, and are tasked with following the same trajectory. However, they move in opposite directions, creating challenging position conditions where they cross each other.

When the UAV detects that it is near the narrow mobile passage, i.e., an obstacle that is not part of its original mission, it checks the possibility of crossing the object. The path-following stage begins when the UAV enters the virtual sphere encompassing the obstacle. Therefore, the planning algorithm overlays the reference originated from the original UAV mission with the path planned to cross the passage, so that the UAV momentarily abandons its original task and prioritizes a safe crossing. A snapshot of the moment when the UAV crosses the hula hoop can be observed in Figure 10g–i.

Notice that the UAV effectively navigates through the passage while maintaining a safety margin. The path planning algorithm calculates the UAV’s trajectory, ensuring that its intersection with the plane corresponding to the hula hoop is as close as possible to the geometric center of the hoop. This strategy enables the UAV to cross the passage accurately and safely. After crossing the moving obstacle, the UAV resumes its original mission, restarting the trajectory-tracking task, as can be seen in Figure 10j–l.

The dashed line in the figures in the left column of Figure 10 (see Figure 10a,d,g,j) represents the route planning for the UAV, which means that during the execution of its mission, the final planned route is not exactly the tracking of the trajectory defined as its main mission. Thus, the result observed in Figure 11a is not exactly a Bernoulli lemniscate, as expected. As a mater of fact, it presents sections where the robot abandoned its main mission to follow the proposed path, which makes it possible to cross the mobile passage safely. It is also important to note that the abrupt change in the type of planned task (from trajectory-tracking to path-following, and vice versa) also affects the robot behavior in the z-axis direction, as it can be seen in Figure 12a.

Observing Figure 12a, it is possible to identify the exact moments when the robot started the path-planning algorithm to cross the mobile passage, thus overlapping the reference given by the planned mission. The time-span when the robot performed the crossings during its route is indicated by the rectangular regions with a gray background. Note that, at the beginning and end of each of these regions, both the trajectory-tracking errors and path-following errors show spikes in their curves. This is due to the abrupt change in the motion reference calculated by the task planning and sent to the control system. Again, it is important to emphasize that the implemented control law is capable of tracking the proposed trajectory with zero steady-state error, as observed in the white regions in the graph. The behavior of the UAV is asymptotically stable, due to the applied control law, as expected. Furthermore, it is observed that the control error increases abruptly at each moment when the UAV starts/ends the path following, i.e., at the exact instants when it enters/exits the virtual sphere that comprises the obstacle.

At the first moment when the UAV identifies the circular passage and ensures a safe crossing, it initializes the process of generating a safe path to cross the platform. The straight-line path is moving along with the circular passage and, therefore, at the moments when the UAV meets the hula hoop, the route executed by the UAV until crossing the passage may present a different shape from a straight line in space. This can be easily observed in Figure 12a, where it is noted that in the moments of crossing (sections marked with gray background), the desired and effectively executed position profiles approach a straight line in the intervals of 10 to 20 and 40 to 50 s, when the robots cross in the linear section of the planned trajectory. However, in the sections between 25 and 35 s, as well as between 55 and 65 s, the planned path moves in such a way that the robot needs to follow a rounded route to ensure a safe passage, as the orientation of the hula hoop is changing as it moves along the trajectory.

Finally, we would like to highlight the observed velocity profile during the experiment, as shown in Figure 12b. It is evident that the velocity tracking error exhibits significant oscillations when the robot is on the path. This behavior arises from the fact that the robot receives a constant reference velocity magnitude, while both the UAV and the path are in motion. As a result, the robot accelerates to match the path’s speed and aligns itself with the hoop for successful passage. However, it needs to decelerate to attain the desired speed $v_x$ at each point on the path. During deceleration, the velocity error increases due to the hoop moving in the opposite direction of the UAV, prompting the UAV to accelerate once again to correct the error. The intervals of approximately constant velocity correspond to the planned velocities along the path.

5. Concluding Remarks

In summary, we presented a closed and smooth path that ensures safe motion in such scenarios, based on the physical limitations of the robot and its environment, taking advantage of the known geometric properties. Experimental results demonstrated that the UAV-UGV system effectively planned a safe path and successfully navigated through the moving obstacle with minimal control errors. The system showcased its potential for cooperative UAV-UGV applications, including search and rescue, surveillance, and inspection tasks. Future research endeavors could focus on assessing the scalability of the proposed system and its performance in more intricate and challenging environments. One potential avenue for refinement is the integration of Visual Simultaneous Localization and Mapping (VSLAM) techniques and advanced object detection algorithms to enhance the autonomy of the UAV in both motion and planning tasks. These advancements would contribute to improving the system’s adaptability and effectiveness in various applications.