NEPTUNE: Nonentangling Trajectory Planning for Multiple Tethered Unmanned Vehicles

Interestingly, most of the early works on the tethered robot planning problem focus on multiple robots rather than a single robot. Sinden [12] investigated the scheduling of tethered robots to visit a set of pre-defined locations in turns so that none of the cables crosses each other during the motion of the robots. A bipartite graph is constructed, with colored edges representing the ordered cable configurations. In [13, 14], the authors addressed the path planning for tethered robots with specified target cable configurations. A directed graph is used to represent the motion constraints and the ordering of movements. The output is a piece-wise linear path for each robot and waiting time at some specified location. Zhang et al.[15] extended the results of [13] by providing an analysis of a more efficient motion profile where all robots move straight and concurrently. These works are very different from our work in terms of problem formulation and approach: (1) they consider taut cables forming straight lines between robots and bases, whereas we consider slack cables that are allowed to slide over one another; (2) static obstacles are not taken into account in these works; (3) their approaches are offline and centralized while our work presents a decentralized online approach; (4) the outputs of these algorithms are piece-wise linear paths whereas our approach provides dynamically feasible trajectories.

The development of planning algorithms for a single tethered robot typically focuses on navigating the robot around obstacles to reach a goal while satisfying the cable length constraint. Early work [16] and its recent derivative [17] find the shortest paths in a known polygonal environment by tracing back along the previous path to look for turning points in a visibility graph-like construction. Recent developments of homotopic path planning using graph-search techniques [18, 19, 20, 21] provide the foundation for a series of new works on tethered robot path planning. Particularly, Kim et al.[8] use a homotopy invariant (h-signature) to determine the homotopy classes of paths by constructing a word for each path (Section III). A homotopy augmented graph is built with the graph nodes carrying both a geometric location and the homotopy class of a path leading to the location. Then graph search techniques can be applied to find the optimal path subject to grid resolution. Kim et al.[9] and Salzman et al.[10] improve the graph search and graph building processes of [8] respectively through applying a multi-heuristic A* [11] search algorithm and replacing the grid-based graph with a visibility graph. McCammon et al.[22] extend the results to a multi-point routing problem. The homotopy invariant in these works, which is for a 2-D static environment, is insufficient to represent the complex interactions when multiple tethered robots are involved (a justification will be provided in Section III-B). Furthermore, these works use a curve shortening technique to determine whether an expanded node satisfies the cable length constraint, which is computationally expensive and leads to slow graph expansion. It is thus a common practice to construct the augmented graph in advance. Bhattacharya et al.[21] proposed a homotopy invariant for multi-robot coordination, which can be potentially applied to the centralized planning for tethered multi-robot tasks. However, considering the high dimensions of the graph and the complexity related to identifying homotopy equivalent classes (the word problem), it is time-consuming to build a graph even for a simple case (more than 30s for $3$ robots in a $7\times 7$ grid). Compared with these works, we develop an efficient representation of homotopy for multiple tethered robots, thus facilitating the computation of the required cable length. Therefore, the graph expansion and search can be executed online for on-demand targets. Teshnizi et al.[23] proposed an online decomposition of workspace into a graph of cells for single-robot path searching. A new cell is created when an event of cable-cable crossing or cable-obstacle contact is detected. However, infinite friction between cable surfaces is assumed, which deviates from the practical cable model. Furthermore, a large number of cells can be expected during the graph search in an obstacle-rich environment. The reason is that a new cell is created for each visible vertex while no heuristics are provided for choosing the candidate cells.

Several studies have been recently performed which leverage Braid groups to characterize the topological patterns of robots’ trajectories and facilitate the trajectory planning for multiple robots [24, 25, 26]. Despite promising results, these works have not been extended to the trajectory planning for tethered robots.

Most of the existing works on the tethered robot planning problem generate piece-wise linear paths. In this section, we review some smooth trajectory generation techniques for decentralized multi-robot planning, while focusing on methods applied to UAVs. In general, decentralized multi-robot trajectory generation includes synchronous and asynchronous methods. Synchronous methods such as [27, 28] require trajectories to be generated at the same planning horizon for all robots, whereas asynchronous methods do not have such a restriction and hence are more suitable for online application. In [3], the authors presented a search-based multi-UAV trajectory planning method, where candidate polynomial trajectories are generated by applying discretized control inputs. Trajectories that violate the collision constraint (resulting in a non-empty intersection between robots’ polygonal shapes) are discarded. In [4], the collision-free requirement is enforced as a penalty function in the overall objective function of the nonlinear optimization problem. In [5], a similar approach is taken, but a newly developed trajectory representation [6] is employed (as compared to B-spline in [4]) which enables concurrent optimization of both the spatial and the temporal parameters. In [7], robots’ trajectories are converted into convex hulls. The collision-free constraint is guaranteed by optimizing a set of planes separating the convex hulls of the collaborating robots and those of the planning robot. To save computational resources and ensure short-term safety, an intermediate goal is chosen, which is the closest point to the goal within a planning radius.

As revealed by the comparisons in [7, 28], centralized and offline trajectory generation approaches typically require much longer computation time to generate a feasible solution than the online decentralized approaches, and the difference grows with the number of robots involved. Furthermore, the absence of online replanning means that the robots cannot handle in-flight events, such as the addition of a new robot into the fleet or the appearance of non-cooperative agents. Hence, it is believed that a decentralized planner with online replanning is more suitable for practical applications. Furthermore, the framework presented in this paper has the flexibility to integrate new features such as the prediction and avoidance of dynamic obstacles and non-cooperative agents. Similar to [7], our approach uses an asynchronous planning strategy with a convex-hull representation of trajectories. However, the greedy strategy used by [7] results in inefficient trajectories. The reason is that an intermediate goal may be selected near a large non-traversable region, where the robot has to make abrupt maneuvers to avoid the obstacles. In contrast, our approach selects the intermediate goals that are derived from a feasible, goal-reaching, and efficient (based on some evaluation criteria such as length) trajectory generated by our front-end kinodynamic trajectory finder.

In this paper, $\|\mathbf{x}\|$ denotes the $2$-norm of vector $\mathbf{x}\in\mathbb{R}^{n}$, $\mathbf{x}^{(i)}$ denotes the $i$-th order derivative of vector $\mathbf{x}$ and $\mathcal{I}_{n}$ denotes the set consisting of integers $1$ to $n$, i.e., $\mathcal{I}_{n}=\{1,\dots,n\}$. The notation frequently used in this paper is shown in Table I. More symbols will be introduced in the paper.

We briefly review the concepts related to homotopy. Consider a workspace of arbitrary dimension consisting of obstacles. A path or a cable in the workspace can be represented as a curve. Two curves in this workspace, sharing the same start and end points, are homotopic (or belong to the same homotopy class) if and only if one can be continuously deformed into another without traversing any obstacles.

A homotopy invariant is a representation of homotopy that uniquely identifies the homotopy class of a curve. In a 2-dimensional workspace consisting of $m$ obstacles, there exists a standard procedure to compute a homotopy invariant [29] (Figure 2). First, a set of non-intersecting rays $o_{1},o_{2},\dots,o_{m}$ are constructed, each emitting from a reference point $\bm{\zeta}_{i}$ in each obstacle. Then, a representation (word) is obtained by tracing the curve from the start to the goal and adding the corresponding letters of the crossed rays. Right-to-left crossings are distinguished by a superscript ‘-1’. Then, the word is reduced by canceling consecutive crossings of the same ray with opposite crossing directions. This reduced word is called the h-signature and is a homotopy invariant. Indeed, two curves with the same start and goal belong to the same homotopy class if and only if they have the same h-signature.

The h-signature introduced above records ray-crossing events that indicate robot-obstacle interactions. However, it is not capable of recording key events of cable-cable interaction for identifying entanglements. To illustrate this, an h-signature is obtained for the path taken by each robot in Figure 1. Each robot has taken a path that crosses the ray emitted from the other robot exactly once. Hence, the resulting h-signature records only one ray-crossing event revealing no potential entanglements.

A tethered robot following a path from its base to a goal should have its cable configuration homotopic to the path. Hence, a shorter homotopy-equivalent curve can be computed to approximate the cable configuration and calculate the required cable length to reach the goal. In [8, 9, 10], a common curve shortening technique is used, which requires tracing back the original path to obtain a list of turning points, i.e., points on the shortened path where the path changes direction. Each of these points’ visual line of sight to its previous point does not intersect any obstacles. An example of this shortened curve is shown in Figure 2.

To compute the representation of homotopy for the planning robot, a set of virtual line segments of the collaborating robots and the obstacles are created. Crossing one of these segments indicates an interaction with the corresponding obstacle or robot. To obtain the virtual segments of the static obstacles, we first construct $m$ non-intersecting lines $o_{j}$, $\forall j\in\mathcal{I}_{m}$, such that each of them passes through the interior of an obstacle $O_{j}$. Each line $o_{j}$ is separated into three segments by two points on the surface of the obstacle, $\bm{\zeta}_{j,0}$ and $\bm{\zeta}_{j,1}$. The virtual segments, labelled as $o_{j,0}$ and $o_{j,1}$, are the two segments outside $O_{j}$, as shown in Figure 5. To build the virtual line segments of the collaborating robots, we use the positions and the contact points obtained from the communication network. The shortened cable configuration of robot $j$, labelled as $r_{j,1}$, is a collection of $\gamma_{j}+1$ line segments constructed by joining in sequence its base point $\mathbf{b}_{j}$, its contact points $\mathbf{c}_{j,1},\mathbf{c}_{j,2},\dots,\mathbf{c}_{j,\gamma_{j}}$ and finally its position $\mathbf{p}_{j}$. The extension of $r_{j,1}$ beyond the robot position $\mathbf{p}_{j}$ until the workspace boundary is labelled as $r_{j,0}$. We call $r_{j,1}$ and $r_{j,0}$ the cable line and extension line of robot $j$, respectively. Figure 5 displays the actual cable configuration and the corresponding line segments of robot $j$. By constructing the virtual line segments of all robots and obstacles, a word of a path is obtained by adding the letter corresponding to the line segment crossed in sequence, regardless of the direction of crossing. An example is shown in Figure 5, where the word for the black dashed path for robot $i$ is $o_{1,0}o_{2,0}o_{2,0}o_{2,0}r_{j,1}o_{3,1}$.

In an online implementation, the word, denoted as $h(k)$, can be updated iteratively based on the incremental movements of the robots. To ensure that each robot starts with an empty word $h(0)=$‘ ’, we make the following assumption:

The procedure for updating the word at every discretized time $k$ is shown in Algorithm 2. There are two cases where a letter can be appended to the word:

1. 1.

   an active crossing case (line 2-2), where robot $i$ crosses a virtual line segment during its movement from $\mathbf{p}(k-1)$ to $\mathbf{p}(k)$, or in the case when a collaborating robot $j$ is moving, robot $i$ is swept across by a robot $j$’s virtual segment $r_{j,f}$, $f\in\{0,1\}$, as shown in Figure 6;

2. 2.

   a passive crossing case (line 2-2), where the extension line of a collaborating robot, $r_{j,0}$, $j\in\mathcal{I}_{n}\backslash i$, sweeps across robot $i$’s base, $\mathbf{b}_{i}$.

The second case is needed to ensure consistency of representation regarding different starting positions of robots. As shown in Figure 6, both positions $\mathbf{p}_{j}(k-2)$ and $\mathbf{p}_{j}(k)$ are valid starting positions of robot $j$, i.e. they can be chosen as $\mathbf{p}_{j}(0)$ because they satisfy Assumption V.1 with $\mathbf{p}_{i}(k)$ chosen as the initial position of robot $i$. Therefore, both positions should induce an empty word for robot $i$. This can be ensured only by appending the letter $r_{j,0}$ twice when robot $j$ moves from $\mathbf{p}_{j}(k-2)$ to $\mathbf{p}_{j}(k)$, one time when $r_{j,0}$ sweeps across robot $i$ and the other time when $r_{j,0}$ sweeps across $\mathbf{b}_{i}$. Then a reduction procedure cancels the consecutive same letters (Section V-B), resulting in an empty word.

A critical step for establishing a homotopy invariant for a topological space is to identify a set of words that are topologically equivalent to the empty word and remove them from the original hotomotpy representation. These words correspond to the elements of the fundamental group mapped to the identity element [21]. For example, in [29], the same consecutive letters, corresponding to crossing and then uncrossing the same segment, can be removed from the original word to obtain a reduced word. In our approach, we identify and remove not only the identical consecutive letters but also combinations of letters representing paths or sub-paths that loop around the intersections of virtual segments. For example, in Figure 5, the dashed blue path that loops around the intersection of $r_{j,1}$ and $o_{1,1}$ can be topologically contracted to a point (it is null-homotopic). Therefore, its word $o_{1,1}r_{j,1}o_{1,1}r_{j,1}$ can be replaced by an empty string (for further understanding of the topological meaning of null-homotopic loops, readers may refer to Appendix A). The reduction procedure is shown in Algorithm 3, which consists of the following steps:

1. 1.

   Given an unreduced homotopy representation $h(k)$ at time $k$, we check whether it contains a pair of identical letters $\chi_{j,f}$, $\chi\in\{o,r\}$, $f\in\{0,1\}$, and extract the string $\chi_{j,f}\chi^{1}\chi^{2}\dots\chi^{\omega}\chi_{j,f}$ from $h(k)$. Here $\omega$ is the number of letters between the pair.

2. 2.

   We check if the virtual segment $\chi_{j,f}$ intersects with all of the segments $\chi^{1}$, $\chi^{2}$, $\dots$, $\chi^{\omega}$ at time $k$. If so, remove both letters $\chi_{j,f}$ from $h(k)$.

3. 3.

   After a removal, go to step 1) and start checking again.

The reason behind the procedure is that, in a static environment, the string $\chi_{j,f}\chi^{1}\chi^{2}\dots\chi^{\omega}\chi_{j,f}$ constitutes a part of the loop if the segment $\chi_{j,f}$ intersects with all of the segments in between, $\chi^{1}$, $\dots$, $\chi^{\omega}$. The loop is in the form of $\chi_{j,f}\chi^{1}\chi^{2}\dots\chi^{\omega}\chi_{j,f}\diamond\Lambda(\chi^{1}\chi^{2}\dots\chi^{\omega})$, where $\Lambda(\cdot)$ denotes a particular sequence of the input letters. Figure 7 shows two particular forms of the loop for $\omega=3$. As a loop is an identity element in the fundamental group, we can write $\chi_{j,f}\chi^{1}\chi^{2}\dots\chi^{\omega}\chi_{j,f}=\Lambda(\chi^{1}\chi^{2}\dots\chi^{\omega})^{-1}$. Assuming that $\Lambda(\chi^{1}\dots\chi^{\omega})$ takes the form shown in Figure 7(a), i.e., $\Lambda(\chi^{1}\dots\chi^{\omega})=\chi^{\omega}\chi^{\omega-1}\dots\chi^{1}$, we can establish a relation $\chi_{j,f}\chi^{1}\chi^{2}\dots\chi^{\omega}\chi_{j,f}=\chi^{1}\chi^{2}\dots\chi^{\omega}$, thus removing both letters $\chi_{j,f}$. As a special case, although virtual segments $r_{j,0}$ and $r_{j,1}$ share a common point at robot $j$’s position, they are considered non-intersecting. Otherwise, any loop around the robot (e.g., the dashed green path in Figure 5) will be reduced to a single letter which reflects a different topological meaning.

It should be noted that the reduction rules introduced here are insufficient to generate a true homotopy invariant for the topological space considered in our scenario, which requires a much more involved construction in a 4-dimensional (X-Y-Z-Time) space and is beyond the scope of this paper. However, the reduced homotopy representation captures important topological information about the robot to check for the entanglement of cables. The condition to identify entanglement can be stated as a two-entry rule: a robot is considered to be risking entanglement at time $k$, if its reduced homotopy representation contains two or more letters corresponding to the same collaborating robot, i.e. $h(k)$ consists of two or more entries of $r_{j,f}$, $j\in\mathcal{I}_{n}\backslash i$, $f\in\{0,1\}$. This rule can be justified as follows:

For operations using $3$ and more robots, any pair-wise entanglements involving only two robots can be detected using the same argument as Proposition V.3. Figure 9 illustrates a more complicated $3$-robot entanglement, where robot $i$’s motion is hindered due to the cables of both robot $j$ and $l$ (removing either robot $j$ or $l$ releases robot $i$ from the entanglement). This scenario is realized by the sequence of movements where robot $i$ crosses (1) the cable line of robot $j$, (2) the extension line $r_{l,0}$ of robot $l$, (3) the cable line of robot $j$ again. The homotopy representation of robot $i$ is $h_{i}=r_{j,1}r_{l,0}r_{j,1}$. Since segments $r_{l,0}$ and $r_{j,1}$ do not intersect, $h_{i}$ is nonreducible. Therefore, the entanglement can be detected using the two-entry rule.

Considering obstacle-ridden environments, Figure 8 illustrates an entanglement caused by two robots following the intended paths sequentially. Since the word of robot $j$, $h_{j}=r_{i,1}o_{1,1}r_{i,1}$, is nonreducible, the entanglement can also be identified using the two-entry rule. Intuitively, the two-entry rule can be used to identify instances when a robot partially circles around another robot (Figure 1), and instances when a robot circles around a part of the cable of another robot in a topologically non-trivial way (Figure 8(c) and Figure 9).

The list of contact points is updated at every iteration, along with updating and reducing the word. Intuitively, a contact point addition (or removal) occurs when a new bend of the cable is created at (or an existing bend is released from) the surface of the obstacles passed by the robot. For efficiency, we adopt a simplification of the obstacle shapes, such that each obstacle $j$ is only represented as a thin barrier $\mkern 1.5mu\overline{\mkern-1.5mu\bm{\zeta_{j,0}}\bm{\zeta_{j,1}}\mkern-1.5mu}\mkern 1.5mu$. The detailed procedure is described in Algorithm 4, in which the indication of the timestamp $(k)$ in the expressions $\mathbf{c}_{1}(k)$, …, $\mathbf{c}_{\gamma_{i}(k)}(k)$ is omitted for brevity. This algorithm checks if the robot has crossed any lines linking a contact point to the surface points of the obstacles, or any lines linking the two consecutive contact points; if such crossing happens, contact points are added or removed accordingly. Figure 10 illustrates Algorithm 4.

We make the following assumption to ensure the correct performance of the algorithm:

Assumption V.4 prevents incorrect identification of surface points as contact points. In Figure 10(c), the robot moving from $\mathbf{p}(k+2)$ to $\mathbf{p}(k+3)$ will result in $\bm{\zeta}_{3,0}$ added as a contact point, but no contact between the cable and $O_{3}$ is possible in this case. Under Assumption V.4, the only case where multiple contact points should be added or removed in one iteration is when multiple coincident lines are crossed. Such a case can be handled by the iterative intersection check in Algorithm 4 following the sequence of the obstacles added to $h(k)$ (line 4 to 4), and the reverse sequence of the contact points added to $\mathcal{C}_{i}(k)$ (line 4 to 4). In practice, Assumption V.4 holds true if the displacement of the robot is sufficiently small between consecutive iterations, which can be achieved by a sufficiently high iteration rate.

We have the following statement on the property of the shortened cable configuration obtained using Algorithm 4:

The length of the shortened cable configuration is a lower bound of the actual length of the shortest cable due to the use of simplified obstacle shapes. In Section VI, this length is compensated with an extra distance to the surface of the obstacles, to better approximate the required cable length. As will be shown in Section IX, the use of Algorithm 4 enables more efficient feasibility checking than the expensive curve shortening algorithm used in [8, 9].

We use the recently published MINVO basis [33] to convert the polynomial coefficients to the control points, which form convex hulls that entirely encapsulate the trajectory and its derivatives. $\mathcal{Q}_{l}$, $\mathcal{V}_{l}$ and $\mathcal{A}_{l}$ represent the set of 2-D position, velocity, and acceleration control points of a candidate node with index $l$, respectively. To ensure feasibility, we check whether the following inequalities are satisfied:

Due to friction, a UAV flying at a fast speed may have its cable trailing behind, as shown in Figure 11. We model the range of cable that may stay in the air as $\xi z$ for a UAV flying at altitude $z$. $\xi$ is a constant to be determined empirically. Therefore, to guarantee non-collision with robot $j$ and its cable, the planning UAV needs to maintain a distance more than $\rho_{j}+\rho_{i}+\xi\Delta z_{l}$, where $\rho_{j}$ and $\rho_{i}$ are the radii of UAVs, and $\Delta z_{l}$ is the maximum possible altitude difference during the planning interval $l$, which can be obtained from their Z-axis trajectories. Eventually, we need to check whether the following equality holds:

where $\text{hull}(\cdot)$ represents the convex hull enclosing the set of control points, $\mathcal{Q}_{j,l}$ is the minimum set of MINVO control points containing the trajectory of robot $j$ during the planning interval $l$, $\mathbf{B}_{j,l}$ is the axis-aligned bounding box whose side length is $\rho_{j}+\rho_{i}+\xi\Delta z_{l}$, and $\oplus$ is the Minkowski sum. We use the Gilbert–Johnson–Keerthi (GJK) distance algorithm [34] to efficiently detect the collision between two convex hulls. Checking the collision with static obstacles can be done similarly by checking the intersections with the inflated convex hulls containing the obstacles.

A tethered robot should always operate within the length limit of its cable and never over-stretch its cable. We check whether a trajectory with index $l$ satisfies this by approximating the required cable length at a position $\mathbf{p}_{i}$ using the list of active contact points:

where $\bar{z}_{l}$ is the maximum altitude of the robot during planning interval $l$ and $\mu_{\alpha}>0$ is an added length to compensate for the underestimation of cable length using contact points only. For safety, we can set $\mu_{\alpha}$ as $2$ times the longest distance from the contact point to any surface points of the same obstacle.

Algorithm 5 incrementally predicts the planning robot’s homotopy representation by getting sampled states of the robots (line 5) and updating the representation using Algorithm 1 (line 5). After each predicted update, it checks the cable length constraints (line 5) and the non-entanglement constraint (line 5-5). We discard trajectories that fail the entanglement check only due to a crossing action incurred in the current prediction cycle (line 5). In this way, we allow the planner starting from an unsafe initial homotopy representation to continue searching and find a trajectory escaping the unsafe situation.