Motion Planning of Large Scale Vehicles for Remote Material Transportation

Abstract

The International Thermonuclear Experimental Reactor (ITER) project is a worldwide research experiment that aims to explore nuclear fusion as a viable source of energy for the coming years. Mobile robotics plays an important role in the remote handling systems that perform the maintenance operations in ITER. The Cask and Plug Remote Handling System (CPRHS) is one of the remote handling systems that transports heavy and highly activated in-vessel components between the Tokamak Building and the Hot Cell Building, the two main buildings of the ITER facility. The CPRHS has dimensions similar to an autobus, maximum weight of 100 tons, kinematics of a rhombic like vehicle (two drivable and steerable wheels) and has to move in cluttered environments. The main challenges described in this chapter are the definition of motion planning strategies that cope with the building maps and the cluttered environments. The algorithms were developed and implemented in a standalone application that receives CAD models of the buildings and returns the best trajectories, including reports of the most risky points of collision, and the swept volume of the vehicle along the missions. More than 700 trajectories were computed for different CPRHS types applied in the models of the real scenarios, crucial to proceed with the construction of the Tokamak Building.

1 Introduction

There is a practical need for developing and exploring nuclear fusion as a source of energy for the humankind benefit. The shortage predictions on fossil fuels, especially with the inevitable oil extraction decline, requires an urgent development and exploration of new sources of energy.

The current energy supply policy is mostly based on fossil fuels (oil, coal and natural gas) representing almost 80 % of the total energy consumption [1]. To worsen this scenario, the world population is expected to grow from 6 to 9 billion people until 2050 [2], resulting on an expressive raise of energy demand.

According to [3], no single technology is likely to provide all of the world’s future energy needs and replace the actual oil-based energy infrastructure. It is necessary to achieve a more sustainable mix of fossil fuels and, more importantly, to develop an energy consumption-frame based on new technologies and alternative energies such as solar, geothermal and nuclear, fission and fusion power.

The International Thermonuclear Experimental Reactor (ITER) project is a worldwide research experiment that aims to explore nuclear fusion as a viable source of energy for the coming years. The project is funded by seven member entities: the European Union (EU), India, Japan, China, Russia, South Korea and the United States. The largest experimental tokamak nuclear fusion reactor will be located at the Cadarache facility, in the south of France, as depicted in Fig. 1.

Fig. 1

The ITER Tokamak (left image) and the scientific buildings and facilities that will house the ITER experiments (right image) in Cadarache, south of France

Besides the major scientific objective of exploring the nuclear fusion as a source of energy, the fusion power plants must be safely and effectively maintained through Remote Handling (RH) techniques, due to restrictions on human being in activated areas.

During ITER lifetime, the internal components of the vacuum vessel of the reactor, such as the blanket and divertor modules, will become activated due to exposure to highly energetic neutrons released during the fusion reaction. Additionally, these in-vessel materials might get contaminated with small amounts of radioactive dust. Hence, the components that provide the base functions for the ITER machinery will need to be periodically inspected and upgraded. To manage such operations and provided that human presence will be not authorized in activated areas, the ITER maintenance system will mostly rely on RH devices [4].

1.1 The Scenario

Among the various RH systems that are expected to operate in ITER, this work focus on a large and complex transporter unit that was chosen for the transfer of heavy and contaminated loads between the two main buildings of ITER, the Tokamak Building (TB), lodging the tokamak reactor and with access by vacuum vessel port cells (from this point forward simply identified as “ports”) and the Hot Cell Building (HCB), that will work mainly as a support area. A lift establishes the only interface between the different levels of TB and between the TB and the HCB. Figure 2 represents the interface between the TB and the HCB. It also depicts a mission between one port in one level of TB and a refurbishment docking place in HCB.

Fig. 2

A 3D snapshot of the CAD models from the two main buildings in ITER, emphasizing the reactor, the vehicle and a particular mission between the buildings

The foreseen RH equipment will have a large impact on the design and assembly of the remaining ITER components, namely on building structural aspects and interfaces. Therefore, motion planning studies for the Cask and Plug Remote Handling System (CPRHS) in all of its missions are required for the sake of the feasibility of the ITER buildings design and for the space reservation for the RH missions, carried out by the CPRHS, as described in [5].

In ITER, the environment in all levels of TB and HCB is mostly composed by static and well structured scenarios, as displayed in Figs. 3 and 4 and each level of the buildings can be modeled using a 2D map representation that will be used for motion planning evaluation. The adopted representation for a map is a set of 2D points in the global Cartesian referential defined in the ITER buildings design and a set of line segments. Each line segment connects two different points and it is assumed that there is no crossing between lines. In case of intersection, a 2D point resulted from the intersection is created and each crossed line segment is split in two new line segments, one starting and the other ending in the splitting point, respectively.

Fig. 3

The three levels of Tokamak Building in a split view (second image), in particular the 2D maps of the level B1 (first image) and the level L1 (third image)

Fig. 4

The five levels of Hot Cell Building in a split view (second image), in particular the 2D maps of the level B2 (first image) and the level L1 (third image)

In TB, the vehicles can operate in three levels (from bottom to up): B1, L1 and L2, as illustrated in Fig. 3. In HCB, the vehicles may operate in five levels (from bottom to up): B2, B1, L1, L2 and L3, as in Fig. 4.

The entire work developed in this project is applied to the scenario of ITER buildings. However, the same research and development in terms of mobile robots navigation can be applicable to any other type of scenario as warehouses or office type environments.

1.2 The Vehicle

The CPRHS is a critical element of the ITER remote maintenance system devoted to transportation of components between the TB and HCB. Due to the necessary confinement of contaminated components the CPRHS is defined as Safety Importance Class 1 (SIC-1) plus the mobile nature of the CPRHS brings with it a significant number of complex interfaces with other ITER sub-systems [6]. The geometry of the CPRHS and its payload vary according to the components to be transported and hence, different CPRHS typologies will operate in ITER. As a reference, the largest CPRHS dimensions are 8.5 m $\times$ 2.62 m $\times$ 3.62 m (length $\times$ width $\times$ height), as depicted in Fig. 5, and the total weight with the maximum load can reach up to 100 tons.

Fig. 5

The CAD model of the vehicle (left image) and three different loads during a disassembling operation (right image)

Fig. 6

The rhombic like configuration

A CPRHS is composed by three sub-systems: the cask envelope (container that enclosures the in-vessel components and the RH tools to be transported), the Cask Transfer System (CTS), which acts as a mobile robot and the pallet (interface between the cask and the CTS equipped with an handling platform to support the cask load and to help on docking procedures). When underneath the pallet, the CTS transports the entire CPRHS, but it can also move independently of the pallet and cask. The CTS has a rhombic like configuration provided by two drivable and steerable wheels, identified as “F”ront and “R”ear wheels, as illustrated in Fig. 6. Given this configuration, the CTS has a higher maneuverability in confined spaces than the traditional cars with Ackerman or tricycle configurations [7].

The CTS when operating individually or the CPRHS when carried out by the CTS are, hence, rhombic like vehicles. For simplicity and from this point forward the CPRHS and the CTS when moving alone are identified as the “vehicle”.

As illustrated in Fig. 6, consider the state vector $q = \left[ x_c\, y_c\, \theta \right]$ as a representation of the vehicle pose in the frame {I}, with $(x_c,y_c)$ the coordinates of the center of the vehicle and $\theta$ the orientation of the vehicle. Consider $v$ as the longitudinal speed and $\beta$ the controllable sideslip angle of the vehicle, both defined in {I}. The angles and the velocities of the front and rear wheels represented Fig. 6, i.e., $\theta _F$, $\theta _R$, $v_F$ and $v_R$, are the inputs to control the motion of the vehicle, which is not addressed in this work. A kinematic model for a rhombic like vehicle in {I}, that allows the simulation of the vehicle motion directly through the desired longitudinal speed $v$, instead of imposing an individual linear speed for each wheel, was introduced in [8] as:

$$\begin{aligned} \left[ \begin{array}{c} \dot{x}_c\\ \dot{y}_c\\ \dot{\theta } \end{array} \right] = \left[ \begin{array}{c} \cos (\theta +\beta )\\ \sin (\theta +\beta )\\ \displaystyle {\frac{\cos \beta \cdot [\tan \theta _F - \tan \theta _R]}{M}} \end{array} \right] \cdot v, \end{aligned}$$

(1)

where

$$\begin{aligned} \beta = \arctan \left( \frac{v_F\cdot \sin \theta _F+v_R\cdot \sin \theta _R}{2\cdot v_{R}\cdot \cos \theta _{R}}\right) \end{aligned}$$

(2)

and

$$\begin{aligned} v = \frac{v_F\cdot \cos \theta _F+v_R\cdot \cos \theta _R}{2\cdot \cos \beta }. \end{aligned}$$

(3)

This model entails that the wheels of the vehicle roll without slipping, a constraint inherent to the nonholonomy of rhombic like vehicles, and it also considers a rigid body constraint, common to this type of vehicles, as follows:

$$\begin{aligned} v_F\cos \theta _F=v_R\cos \theta _R. \end{aligned}$$

(4)

Fig. 7

The drivable and steerable wheels of a rhombic like vehicle following the same path, feature identified as line guidance (left image) and following different paths, identified as free roaming (right image). The dotted lines represent the path described by the center of the vehicle

The drivable and steerable wheels of a rhombic like vehicle are able to follow the same path, in a methodology identified in this work as the line guidance approach, or to follow different paths, herein identified as the free roaming approach, as illustrated in Fig. 7. These two approaches will be addressed later in Sect. 2.

1.3 Problem Statement

The maintenance operations of transportation in ITER require the vehicle motion throughout the cluttered environments of the TB and the HCB. There are 46 ports in the three levels of TB and one lift that interfaces the different levels of TB and the different levels of HCB. In addition, there are several docking ports and parking places inside the different levels of HCB. The maintenance operations require a mesh of paths between the target points inside the buildings. For instance, the transportation mission of a load for refurbishment requires a path between a port and the lift in TB and then between the lift and a docking port in HCB. During a mission the vehicle sweeps a given volume when follow its path. This volume is important given the scarce free space available in the scenario, or with other vehicles in operation or parked, as illustrated in Fig. 8. The speed along the path is also relevant not only for the mission execution time, but in particularly because the dynamics of the vehicle, since it can reach up to 100 tons. From all these reasons, each mission requires an optimized trajectory.

Fig. 8

Two missions of remote handling in ITER: from the lift to port 16 in level B1 of TB (left image) and the sequences for moving a parked CPRHS in level B2 of HCB (right image)

The trajectory optimization problem stated for the vehicle consists on evaluating a trajectory, i.e., a geometric path combined with a speed profile, which guarantees that the vehicle, departing from an initial configuration $q_S$, achieves the specified goal, $q_F$, without colliding with obstacles and ensuring a safety margin with its surroundings. Specific optimization criteria such as smoothness, path length and obstacle clearance are also considered during the planning phase as well as the vehicle characteristics (dimensions, kinematic and dynamic constraints) and surrounding scenario.

To solve the trajectory optimization problem for each mission, specific information is required, which defines the inputs to the developed algorithms:

• 1. Environment model: the model of the scenario where the vehicles have to move that constitute relevant information for the definition of a collision free optimal planned solution. From the original CAD models in 3D, it is only used their 2D projection at floor level, as described in Sect. 1.1.

• 2. Vehicle model: the planning solutions depend directly on the vehicle configuration (geometric, kinematic and dynamic), as described in Sect. 1.2.

• 3. Initial and goal conditions: the initial and final poses of the vehicle defines the first and the last points of the path, i.e., $P_i$ and $P_f$.

• 4. Global trajectory(ies): most of the trajectories in TB share a large common path around the Tokamak, which is identified as a “ring” in each level. First, the path of the ring is evaluated and then, each path for a different port is maximized with the path of the ring. The maximization of different paths is addressed later in Sect. 2.1.5.

Together, these informations define a motion query for the specified mission in the ITER scenarios and are fed as inputs into a trajectory planner. This planner generates a path to be carried out by the vehicles, i.e., a set of Cartesian coordinates (for specific vehicle reference points) and corresponding orientations that geometrically describe the vehicle motion. In addition to the geometric feasibility of the solution, which shall guarantee that the vehicle reaches the goal configuration without colliding with obstacles, it is desirable that the planned solution follows specific criteria requirements:

• Path length: whenever possible, find the shortest possible path, so as to minimize the energy consumption of the on-board batteries.

• Path clearance: increase the minimum distance of the vehicle to the surrounding obstacles of the scenario.

• Path smoothness: the planned solution shall be smooth, minimizing steering maneuvers and jerky motions.

• Maximization of common paths: the vehicle journeys may share common paths through the buildings, wherever possible.

The planner outputs a trajectory to guide the vehicle from an initial configuration, $q_S$, to a final configuration, $q_F$, using the 2D map representation, $M$, and the vehicle model. The geometric solution (a path) is combined with a speed profile, which defines how to move the vehicle along the path at various speeds while satisfying the kinematic and dynamic constraints (maximum/minimum velocities and accelerations).

2 Motion Planning

Two main approaches are presented for solving the trajectory optimization problem of rhombic like vehicles: the line guidance and the free roaming (see Fig. 7). The line guidance approach, where both drivable and steerable wheels follow the same path, is used in most of the trajectories. This approach, detailed in Sect. 2.1, outputs an optimized trajectory without maneuvers of alternatively moving forward and backward. In some situations, the resulted trajectory using line guidance is not feasible in the entrances to some ports or to the lift, but may become feasible if including maneuvers. Therefore, an improvement of the line guidance approach using maneuvers is described in Sect. 2.1.4. When the trajectory is not feasible even if using maneuvers, the free roaming approach, detailed in Sect. 2.2, is the last solution adopted. The Sect. 2.3 describes the speed evaluator considered for both approaches, which converts the optimized paths into optimized trajectories. In addition, and in particular for the ITER scenario, specially in the TB, most of the trajectories share a long common part from the single lift in TB to the vicinity of each port of the Tokamak. Therefore, an additional feature was developed for maximizing the common part of different trajectories, which is detailed in Sect. 2.1.5.

The two main approaches, the line guidance and the free roaming, share three stages:

• 1. Geometric path evaluation: given the environment model and the initial and goal objectives, an initial geometric path is found. At this point the aim is to find a path connecting the initial and goal objectives that can act as an initial condition for the next path optimization stage.

• 2. Path optimization: this module receives the preceding geometric solution as input and returns an optimized path. The optimization process first applies a spline interpolation to satisfy weaker differential constraints such as smoothness requirements. Afterwards, a clearance based optimization is carried out to guarantee a collision free path that meets the safety requirements. In this study, a minimum safety distance between the vehicle and the obstacles must be guaranteed.

• 3. Trajectory evaluation: in this final module (described in Sect. 2.3), a velocity function is defined along the optimized path transforming it into a trajectory, which is the output of the proposed planning approach.

The two first stages correspond to the path planning and optimization, while the third stage consists on the trajectory evaluation, including a velocity profile to the path. The two first stages are detailed in the sequel for the line guidance and for the free roaming.

2.1 Line Guidance

From previous work of RH in ITER [9], and for safety purposes, buried wired systems were proposed to implement the paths resulted from line guidance. The vehicles would follow the path, with both wheels following the same path. Given this ITER project requirement, the proposed planning methodology returns directly the path to be followed by the center of the wheels and not the one corresponding to the center of the vehicle. A nominal operation of the vehicle for a specified environment determines a motion between two configurations (2D points with specific orientations). The first step of this planning methodology is to find an initial geometric path, i.e., a set of 2D points, connecting the initial and final configurations.

The environment in all levels of TB is composed by static and well structured scenarios. Therefore, the environments can be modeled using a planar map representation, ensuring good geometric properties, like low dimensionality. This encouraged the use of a combinatorial planning approach, instead of other approaches that require more computational effort, as [10, 11]. To handle this first planning objective, the use of a cell decomposition approach is considered, but other combinatorial approaches [12, 13], could also be used. From the cell decomposition approaches, a triangle cell arrangement was adopted, using the Constrained Delaunay Triangulation (CDT) [14], as illustrated in Fig. 9—left.

The adopted representation for a map, $E$, is a set of 2D points, $\mathbf {p_i}$ in a global Cartesian referential of ITER and a set of line segments, $l_{jk}$, where each line segment connects two different points, $\mathbf {p_k}$ and $\mathbf {p_l}$, i.e.,

$$\begin{aligned} E = \{\mathbf {p_i},l_{jk} | i,j,k=1, \ldots ,E_p\} \end{aligned}$$

(5)

where $E_P$ is the number of points, $\mathbf {p_i} = (x,y)$ and $l_{jk}=\{\mathbf {p_j}+t\cdot \mathbf {p_k} | t\in [0,1]\}$.

Fig. 9

Example of the line guidance approach applied in the trajectory evaluation to port 16 in level B1 of TB: the initial map, the Constrained Delaunay Triangulation and the best sequence of triangles painted in a different color (first image), the poses of the vehicle along the geometrical path extracted from the sequence of triangles (second image), the evaluation of the optimization procedure with the smoothing of the path (third image) and the poses of the vehicle along the optimized path (last image)

The CDT, detailed in Sect. 2.1.1, provides the support to generate an initial path, which has a sharp layout and possibly is not feasible. Then, an optimization algorithm based on elastic bands concept is used to smooth the initial path and to turn it feasible, as detailed in Sect. 2.1.3. In most of the situations, the CDT requires more computational power than the elastic bands, i.e., the initial part of the algorithm is very demanding when compared to the optimization part. Hence, the alternative initialization approach based on Fast Marching Method (FMM), faster than the CDT, is proposed in Sect. 2.1.2. The Sects. 2.1.4 and 2.1.5 details the improvement of line guidance approach with maneuvers and the maximization of common parts of different paths.

2.1.1 Constrained Delaunay Triangulation for Initialization

The overall procedure to determine an initial geometric path can using CDT is described:

• 1. For a specified scenario, the CDT is applied to the corresponding 2D map, $E$, yielding a triangle cell decomposition. Let $C$ denote the set of $N$ triangle cells so obtained, $C = \{C_{n} | n = 1, \ldots , N\}$. Since each triangle has three edges, only some of them may correspond to a wall of the scenario and, therefore, identified as a real edge $l_{jk}$. The other edges, possible all of them, are identified as virtual edges since they exist only for computational purposes;

• 2. To handle a specific motion query, i.e., to connect an initial vehicle configuration, $q_S$, to a final configuration, $q_F$, the next algorithm’s step determines which cells, herein denoted by $C_{I}$ and $C_{F}$, contain these two configurations;

• 3. Using the cell adjacency property, all the possible triangle sequences connecting $C_{I}$ to $C_{F}$ and composed by consecutive cells that do not share a real edge are evaluated. The desired cells in the sequences are connected by virtual edges, since feasible solutions cannot cross walls represented by real edges. Let $S = \{S_{i} | i = 1, \ldots , K\}$ be the set of all cell sequences so obtained. If no one of such sequence exists, the algorithm states that there is no solution for the proposed query;

• 4. The final step converts each cell sequence $S_{i}$ into an ordered sequence of points connected by line segments that can be interpreted as a graph. First, $q_S$ is connected to the middle edge point of the two first cell in each sequence. Then, the middle edge points of two consecutive cells of the sequence are taken as path sample points and are linked by a straight line. Finally, the middle edge point of the last cell in each sequence is connected to $q_F$.

The geometric path evaluation module just presented outputs all the possible geometric paths connecting $q_S$ to $q_F$ and composed by a set of line segments. To determine the best solution, the shortest path is chosen. To increase the efficiency of the CDT algorithm, the $A^*$ algorithm [15], is used in alternative to an exhaustive search. Simulations experiments shown how this algorithm can dramatically fasten the search for the shortest triangle cell sequence in complex scenarios such as the TB, that are composed by numerous triangle cells.

Figure 9—first image illustrates the level B1 of TB, the respective CDT of the map and the best sequence of triangles painted in a different color. Figure 9—second image presents the poses of the vehicle along the geometrical path extracted from the sequence of triangles. This geometrical path is the initial path with a sharp layout and not feasible (clashes with some pillars). The iterative process of the path optimization starting on the initial path is presented in the Fig. 9—third image. A part of the path is zoomed for a better understanding about the modifications along the iterations. The last image in the Fig. 9 presents the poses of the vehicle along the final smooth and feasible path.

2.1.2 Fast Marching for Initialization

The initialization path obtained with CDT methodology presents limitations in terms of path smoothness. In complex scenarios, the geometric representation results in a huge number of triangles with rough initial paths still far from the optimal one, as shown in Fig. 9, yielding a large computational effort to optimize the path. These limitations can be overcome by using the Fast Marching Square (FM$^2$) method [16], providing a better initialization. The FM$^2$ is an alternative approach for the initialization, in terms of computational improvement. At the end of the entire process, the optimal final paths are very similar when using the CDT or FM$^2$ initializations, as shown later in this chapter.

The FMM is a computational algorithm to solve the arrival time of expanding waves in every point of the space. Conceptually, it can be considered as a continuous version of the Dijkstra’s algorithm [17]. It is based on the assumption that the information only flows outwards from the seeding area (wave source). The FMM was proposed by Sethian [18] to approximate the solution of the Eikonal equation, a non-linear partial differential equation encountered in problems of wave propagation [18].

Let assume a 2D map, where $\mathbf {x} = (x,y)$ is a point on the map with coordinates defined in a Cartesian referential, the front wave arrival time function for every point of the map, $T(\mathbf {x})$, and the velocity of the wave propagation $F(\mathbf {x})$ in each point $\mathbf {x}$. Let also assume that a wave starts propagating at $\mathbf {x_0}=(x_0,y_0)$ at time $T(\mathbf {x_0}) = 0$ with velocity $F(\mathbf {x})\ge 0$. The Eikonal equation (6) defines the time of arrival of the propagating frontwave $T(\mathbf {x})$ at each point $\mathbf {x}$ of the map, in which the propagation speed depends on $F(\mathbf {x})$, according to:

$$\begin{aligned} |\nabla T(\mathbf {x})|F(\mathbf {x}) = 1 \end{aligned}$$

(6)

With the discretization of the gradient $\nabla T(\mathbf {x})$ as in [19], it is possible to solve the Eikonal equation at each point $\mathbf {x}$. Using the notation in (7), Eq. (6) can be rewritten as (8):

$$\begin{aligned} \begin{array}{l} T_{1}=\min (T(i-1,j),T(i+1,j))\\ T_{2}=\min (T(i,j-1),T(i,j+1)) \end{array} \end{aligned}$$

(7)

$$\begin{aligned} \left( \frac{T(i,j)-T_1}{\varDelta x}\right) ^2 + \left( \frac{T_(i,j)-T_2}{\varDelta y}\right) ^2 = \frac{1}{F(i,j)^2} \end{aligned}$$

(8)

The FMM consists on solving the Eq. (8) in which all the parameters are known, except $T(i,j)$. The process is iterative, starting at the source point of the wave (or waves) where $T(i_0,j_0)=0$. Each iteration obtains the value $T(i,j)$ for the neighbors of the points evaluated in the previous iteration. Using as an input a binary grid map, in which the velocity $F(i,j)=0$ (black) means obstacle and $F(i,j)=1$ (white) means free space, the output of the algorithm is a map of distances. These distances are concretely the time of arrival of the expanding wave at every point of the map.

FMM can be directly used as a path planner algorithm. By applying gradient descent from any point of the distance map, a path will be obtained with the source of the wave as a goal point. The paths provided by the FMM are optimal in terms of length, but they do not accomplish the smoothness and safety constraints that most of robotic applications require, because they run too close to obstacles and walls and have sharp curves. However, the FM$^2$ method includes smoothness and solves partially the safety constraints.

The FM$^2$ method arises from the application of the FMM twice over the same map. The first time is used to create a map of velocities for the environment, as illustrated in the first image of Fig. 10 and the second time it computes the time of arrival of the wave for every point when the wave is moving at the velocity computed in the previous step. The FM$^2$ method is based on automatically creating maps of velocities, $F(\mathbf {x})$, depending on the environment map in which the velocity of the expanding wave varies depending on the distance to the closest obstacle. The FMM is applied in a first stage to obtain these maps of velocities. In this case, all the obstacles and walls are labeled as wave sources that expand with constant speed. The result is a map of distances in which those cells in the grid that are farther from the obstacles have a higher value, as depicted in Fig. 10—center.

Fig. 10

Map of velocities obtained using all the black points of the grid map as a wave source in the FMM (first image), map of distances obtained after applying FMM to the map of velocities (second image) and the resulted path obtained by applying gradient descent over the map of distances (third image)

Once this map of distances is computed, it is normalized in order to have values between 0 and 1 and interpreted as relative wave expansion speeds (meaning full stopped or full speed of the wave expansion, respectively). The FMM is applied once again with the goal point as the wave source. During the expansion, the wave will propagate with the velocities indicated in the map previously generated. The propagation ends once the initial point of the path is reached. The resulting map of distances is similar to the one obtained with the standard FMM, but slight differences which make the paths smooth when gradient descent is applied, as shown in Fig. 11. In summary, FM$^2$ applies FMM twice without any mathematical modification: the first step creates the map of velocities, $F(\mathbf {x})$, and the second step computes the time of arrival function, $T(\mathbf {x})$, in which gradient descent is applied to find the path. In FMM the map of velocities $F(\mathbf {x})$ is directly the input binary map.

Similar to the CDT, the FM$^2$ does not include the rhombic kinematic constrains and, hence, the resulted initial path could be non feasible. Therefore, the optimization process of elastic bands described in Sect. 2.1.3 is required after using any of the initializations methods, CDT or FM$^2$. However, the path obtained with FM$^2$ is smoother and closer to the final optimal solution when compared to the initial geometric paths obtained with CDT, as illustrated in Fig. 11 (a mission from the lift to the port 12, extracted from the several experiments presented in [20]).

Fig. 11

Initial path computed by the CDT (first image), initial path computed by the FM$^2$ (second image) and the resulted optimized path (third image), which is the same if using any of the previous initializations, but achieved with less iterations if using the second initialization

The FM$^2$ avoids local minimum, completeness (finds a path if it exists and notifies in case of no feasible path) and requires less computation power (the map of velocities is independent of the robot shape and pose). In addition, the FM$^2$ performs a better initialization when compared to the initialization obtained with CDT, as explained in the Sect. 2.1.3. However, FM$^2$ requires a discretization of the environment and the map of distances has to be computed to each mission, since the final poses are different. In both approaches, CDT or FM$^2$, the result is only an initialization path, which is long, with a sharp layout and probably non feasible.

2.1.3 Elastic Bands for Smoothing

The initial path retrieved by the CDT or by the FM$^2$ is not optimized in terms of length, smoothness and most of the times is also non feasible, i.e., results in clashes with some element in the scenario. An optimization is necessary to smooth the path and to turn it feasible starting from the output of the CDT or the FM$^2$ initializations. An optimization methodology was implemented, based on the elastic bands method [21]. The original concept associated with this approach appeared in the computer vision field, with the presentation of the so called “snakes” algorithm [22]. A snake is a deformable curve guided by artificial forces that pull it towards image features such as lines and edges. The solution herein proposed with the elastic bands methodology is similar to the snakes approach. Instead of retracting a curve to image features, in the path planning problem, it repels the path out from obstacles. Following this approach, the path is modeled as an elastic band which can be compared to a series of connected springs subjected to two types of forces:

• Internal forces: the internal contraction force simulates the Hooke’s elasticity concept [23, 24], i.e., the magnitude force is proportional to the amplitude of displacement. This modeling approach allows the simulation of the behavior of a stretched band. This is the reason why the paths become retracted and shorter. From this point on, the term “elastic force”, $F_{e}$, is adopted to refer to this force component;

• External forces: the obstacle clearance is achieved using repulsive forces, to keep the path, and consequently the vehicle, away from obstacles.

When submitted to these artificial forces, the elastic band is deformed over time becoming a shorter and smoother path, increasing clearance from obstacles. Hooke’s law evaluates the elastic force $F_{e}$ applied to path point $P_{i}$ as

$$\begin{aligned} F_{e}(P_{i}) = k_{e}\cdot \left[ (P_{i-1}-P_{i})-(P_{i}-P_{i+1})\right] \end{aligned}$$

(9)

where $k_{e}$ is the elastic gain and $P_{i-1}$ and $P_{i+1}$ are the path points adjacent to $P_{i}$. The elastic band behavior can be controlled through $k_{e}$. The band stretches with high values of $k_{e}$ while low values increase the band flexibility.

Using a collision detector algorithm, the nearest obstacle point (OP) to each vehicle pose might be considered. The use of a single OP as a reference to determine the repulsive forces may not be satisfactory to maintain clearance from obstacles due to vehicle dimensions, and therefore, a larger set of obstacle points, such as the k-nearest (k-OPs), must be considered, as illustrated in Fig. 12. This will lead to a more balanced repulsive contribution ensuring effectiveness on most situations. Henceforth, on this formulation, it is considered the set of references formed by the nearest OP to each of the four vehicle’s faces.

In the collision detector algorithm four situations may occur with the nearest points between the vehicle and the scenario: between a corner of the vehicle and a corner of the scenario, between a corner of the vehicle and a wall, between a corner of the scenario and a face of the vehicle and between a face of the vehicle and a wall (the vehicle is in parallel to a wall). In the three first situations, the result is always a point of the vehicle and point in the scenario, defining, as described later, in a distance and a direction for the repulsive force. In the last situation, when the vehicle is in parallel to a wall, an infinite number of points would be expected. However, any wall or any face of vehicle are line segments. Therefore, when this situation is verified, it is considered the closest points in the boundaries of the line segments.

Fig. 12

Elastic band concept: elastic forces to smooth the path (left image) and repulsive forces generated by the closest obstacles (right image)

The overall procedure to evaluate the repulsive force for each path point $P_{i}$ is the following:

• 1. The initial poses (position and orientation) are determined based on the constraint of both wheels placed over the path. These poses were computed considering a forward direction and a backward direction. In the forward direction the rear wheel is fixed on each $P_i$. Then, the path point $P_j$ closest to the front wheel position along the path is determined, such that $\left\| P_{i} - P_{j} \right\| = M$, where M is the distance between front and rear wheels. The points $P_{i}$ and $P_{j}$ define the CPRHS/CTS pose. In the backward direction, the same procedure is repeated, but fixing the front wheel for each $P_{i}$, as if the vehicle was executing the path moving backwards.

  Let $k =\{1, \ldots , K\}$ denote the index of the $k_{th}$ OP considered on each $P_{i}$ related pose and $l = \{F, B\}$ referring to the forward or backward direction. The $t_{l,k}$ is the vector defined by the $k$ obstacle point ($O_{l,k}$) and each wheel point ($W_{F}$ = rear wheel and $W_{B}$ = front wheel),

  $$\begin{aligned} t_{l,k} = W_{l}-O_{l,k} \end{aligned}$$

  (10)

  This vector defines the repulsive force direction taking into account the position of the wheels. To maintain clearance from obstacles, the force magnitude must vary inversely with the distance of the poses to the obstacles. To carry out this geometric consideration let $u_{l,k}$ denote the vector taken from the $O_{l,k}$ to the vehicle nearest point $V_{l,k}$,

  $$\begin{aligned} u_{l,k} = V_{l,k} - O_{l,k} \end{aligned}$$

  (11)

• 2. Each pair of points ($O_{l,k}$ ,$V_{l,k}$) determines a repulsive contribution defined on $P_{i}$ given by,

  $$\begin{aligned} r_{l,k}(P_{i}) = \left\{ \begin{array}{ll} \frac{t_{l,k}}{\left\| t_{l,k}\right\| }f(\left\| u_{l,k}\right\| )&{} \quad \text{ if } \left\| u_{l,k}\right\| > d_{th}\\ &{} \\ \frac{t_{l,k}}{\left\| t_{l,k}\right\| }f_{th}&{} \quad \text{ if } 0\le \left\| u_{l,k}\right\| \le d_{th} \end{array}\right. \end{aligned}$$

  (12)

  with $f$ denoting a monotonically decreasing function, with a maximum reference value, $f_{th}$, to avoid outsized magnitude values when a threshold distance, $d_{th}$, is exceeded.

• 3. The repulsive force for each $P_{i}$ is determined as a combination of different repulsive contributions, given by (12),

  $$\begin{aligned} F_{r}(P_{i}) = k_{r} \cdot \sum _{l=\left\{ F,B\right\} }\sum _{k=1}^{K}r_{l,k}(P_{i}) \end{aligned}$$

  (13)

  with $k_{r}$ denoting the repulsive gain.

  Once the elastic (9) and the repulsive (13) forces are computed, an update equation procedure that defines the path evolution along each iteration is applied as

  $$\begin{aligned} P_{i,new} = P_{i,old}+k\cdot F_{total}(P_{i,old}) \end{aligned}$$

  (14)

  where $k$ is a normalization factor adding up the total force contribution applied to all points $P_{i,old}$ and the total force contribution is given by

  $$\begin{aligned} F_{total}(P_{i,old})=F_{e}(P_{i,old})+F_{r}(P_{i,old}) \end{aligned}$$

  (15)

  The stopping criteria is defined by detecting that the magnitude changes on $F_{total}$ are smaller than a given threshold and by setting a maximum number of iterations.

The path optimization is thus carried out by a path deformation approach where the computed paths are treated as flexible and deformable bands. Elastic interactions smooth the path by removing any existing slack, whereas repulsive forces improve clearance from obstacles. The algorithm is fully described in [25] where the results of line guidance applied in all levels of TB are presented. In case of not enough space, a feasible path may not be possible and the algorithm returns, with warning, a path with clashes or with the minimum distance below a safety margin in certain places.

The computational efforts to obtain the initial path using CDT or FM$^2$ are similar. However, since the path resulted from FM$^2$ is closer to the final solution, it is expected that the computational time required by the elastic bands to obtain the final solution is less when using the initial path resulted from FM$^2$. The number of iterations to obtain the final path to the port 12 is illustrated in Fig. 13. In this figure, each line represents the variation of each point of the path, i.e., how much the point “moves” in the Cartesian referential when the elastic band is applied. In the first iterations, the points have larger variation, since there are more differences between the elastic forces and repulsive forces. In the last iterations, there is more equilibrium and the variation are minimum corresponding to small oscillations. The algorithm stops when the variation is below a threshold value. The elastic bands algorithm takes less iterations to converge using the FM$^2$ initialization, and thus, less computational time as expected.

Fig. 13

Convergence of the elastic bands algorithm when using the CDT initialization (top image) and when using the FM$^2$ initialization (bottom image), which requires less iterations

2.1.4 Integration of Maneuvers

Due to the confined environment, there are particular situations where the described methodology of line guidance fails to generate feasible solutions, i.e., paths without collisions and above a safety margin. The integration of maneuvers can greatly improve the path planning, by providing a feasible solution where none could be found with the previously methodology and with the advantage of improving the distance to obstacles, as represented in Fig. 14.

Fig. 14

The line guidance algorithm was not able to generate a path to port 17 in level B1 of TB without clashes (left image), but the problem becomes solved with a maneuver (right image)

A maneuver exists when the vehicle stops and changes its motion direction (from/to forward to/from backwards). In this paper, a maneuver splits the path in two sub-paths with the constraint that the final pose of the first sub-path is the initial pose of the next sub-path. In both sub-paths the line guidance methodology is considered.

By taking advantage of the vehicle kinematic configuration, the line guidance algorithm was improved to incorporate one or multiple maneuvers. In case $n$ maneuvers are required, the path is divided in $n+1$ sub-paths and the path optimization is applied to each sub-path. An additional constraint has to be taken into account when considering maneuvers. The path should be common to both wheels. However, when following a path, both wheels cannot follow the entire path. For instance, when following in forward direction, it is expected that the front wheel reaches the end of the path before the rear wheel. Similarly, in backwards direction, the rear wheel reaches the beginning of the path before the front wheel. This gap corresponds to the wheelbase, $M$. Consequently, this constraint should also be taken into consideration when evaluating maneuvers. Between two consecutive sub-paths of a maneuver, there is a coincident segment of both sub-paths with a length greater or equal to the distance between the wheels.

The decision of including maneuvers is taken when a path without maneuver is not feasible, as illustrated in Fig. 14, where there is a collision, or does not fulfill the minimum safety distance to obstacles. The point(s) of maneuver are introduced manually. It is possible to choose if they are fixed or flexible. In this later case the algorithm can adjust its position during the optimization to obtain the final trajectory. The integration of maneuvers is currently available in [26].

2.1.5 Maximization of Common Parts of Different Paths

The geometry of the scenarios in TB and HCB is such that paths for different missions of the CPRHS can, in certain situations, share common parts. In particular, this is noticeable in the galleries around the tokamak where all CPRHS have to travel from/to the lift to/from each of the port cells. The maximization of common parts in different paths minimizes the overall volume required for CPRHS operation, this being a key issue in ITER design and safety. To achieve this goal the line guidance approach described in Sect. 2.1 was improved with a feature to fulfill this objective.

Around the galleries in TB an optimal path to be followed using line guidance navigation is firstly generated, with the initial pose inside the lift and the final pose in the corridor, near the lift, as illustrated in Fig. 15—left. This path is to be used, as much as possible, in all missions from/to the lift to/from each port that are accessible from the gallery. For each of these paths, two issues arise at this stage considering a mission from the lift to a port: (i) where to deviate from this common part of the path to reach a particular port, and (ii) which is the optimal path from this deviation point, to the final goal in the port cell. The last path, i.e., from the deviation point to the final goal, can be optimized using line guidance or free roaming, since the entrance to the port cell is usually critical, given the risk of clashes with the pillars.

Fig. 15

The common trajectory around the tokamak (left image) and the maximization process with the trajectory to port 14 in equatorial level of TB (right images)

The overall procedure to evaluate an optimized path that considers the maximization of common paths is the following:

• 1. Assume as input the path starting at the lift and describing a ring around the galleries. This path can be evaluated using the algorithm described in Sect. 2.1.

• 2. Obtain a second optimized path from the lift to a specific port, using the algorithm of Sect. 2.1.

• 3. Starting from the end point of the second path (obtained in point 2), the most closest point between the two paths is searched and defined as the Closest Point (CP), as illustrated in Fig. 15. From the CP and crawling backwards the length of the vehicle along the first path a Splitting Point (SP) is defined. The common path is defined between the initial point of both paths and the SP.

• 4. The path starting in the SP, where the pose of vehicle is frozen, and finishing in the target goal is optimized following the same procedure of line guidance described in Sect. 2.1 or, if it is not possible, using the free roaming described in Sect. 2.2. This means that the path in point 2 is disregarded at this stage.

The resulted path is finally inputted to the speed evaluator, leading to an optimized trajectory.

2.2 Free Roaming

The line guidance methodology entails that both vehicle wheels should follow the same physical path and therefore the inherent rhombic flexibility is only partially explored. Figure 16 illustrates part of the scenario in TB of ITER where a CPRHS, acting as a rescue vehicle, has to dock in a Vacuum Vessel Port Cell (VVPC) where another CPRHS is already parked. There is a possible trajectory for the first cask using line guidance, as illustrated in Fig. 16 (top). However, if considering the same constraint, the rescue vehicle clashes with the wall, as illustrated in Fig. 16 (bottom left). In several situations as the previous one, no possible trajectory is found for the second cask using the line guidance approach. In case of using independent references for the wheels, i.e., free roaming navigation methodology, a possible path is found, as illustrated in Fig. 16 (bottom right). This solution requires the use of dedicated motion planning techniques, in particular, an efficient path optimization method capable of exploring the high maneuvering ability of the rhombic vehicle.

Fig. 16

Trajectories for port 14 in any level of TB: (top image) the nominal operation is possible with line guidance, (bottom left image) in rescue operation, where a CPRHS is already docked, resulting in collision using the line guidance and (bottom right image) using free roaming results in a feasible trajectory

The proposed free roaming methodology is based on rough paths provided by global planners like the Rapidly-Exploring Random Tree (RRT) [27, 28], or the Probabilistic Roadmap Method (PRM) [10]. The resulted path is optimized using the elastic bands method, but this time, it is redefined, formulating paths as particle systems [21]. Inspired on the rigid body dynamics, consecutive poses along the rough path previously referred are treated as rigid bodies that are repulsion from obstacles through external forces, improving path clearance. Figure 17 illustrates the free roaming process. This formulation allow to explicitly consider the vehicle geometry during the optimization and fully profit from the high maneuverability of rhombic vehicles.

In this approach, the use of rigid body dynamics is restricted to the case of general plane motion, i.e., the particles composing the rigid body move in parallel planes and their motion is neither characterized by pure rotational nor pure translational movements. Therefore, angular variables, such as moments and angular velocities, are scalar quantities.

Fig. 17

The initial path returned by the Rapidly exploring random tree (first image), the poses of the vehicle along the path (second image), the evaluation of the free roaming (third image) and the resulted path (last image)

Consider that a rigid body with a Center of Mass (CoM) denoted by $C$ is acted by $N$ external forces, $F_n$, with $\{n = 1, \ldots , N\}$. Following the Newton’s Second Law and taking the rigid body as a system of particles, the dynamics of $C$, with respect to the inertial frame $OXY$, is given by

$$\begin{aligned} F_{total} =\sum _{n=1}^{N}F_n = m\cdot a, \end{aligned}$$

(16)

where $m$ is the mass of the vehicle (up to 100 tons) and $a$ is the linear acceleration of $C$. The dynamics of the rigid body motion relative to the its body frame, $CX'Y'$, is given by

$$\begin{aligned} \tau _{total} = \sum _{n=1}^{N}F_n \times e_n = I_C\alpha , \end{aligned}$$

(17)

which entails that the resultant torque about $C$, $\tau _{total}$, is a vector with the direction of the angular acceleration, $\alpha$, and magnitude $I_C\alpha$. In (17), $I_C$ is the moment of inertia around the perpendicular axis passing through $C$, whereas $e_n$ corresponds to the position vector of $F_n$ relative to the reference frame $CX'Y'$. For the case of uniformly accelerated motion, which will be adopted in this formulation, $a$ and $\alpha$ assume constant values over time. From the kinetics viewpoint, the general plane motion of the rigid body can be decomposed as the combination of a translation with linear acceleration, $a$, and a rotation about $C$ with angular acceleration, $\alpha$, given by (16) and (17), respectively.

The path optimization, based on a deformation process, refines and improves the quality of a rough solution path provided by a planner. This rough path, which defines the input for the optimization process, is consider to be a set of collision-free motions connecting the queried initial pose, $q_S$, and the final pose, $q_F$. From this time forward the rough path will be referred to as query path, as described in Sect. 2.1.3.

In the path optimization process, each of the consecutive vehicle poses that form the query path is treated as a rigid body that is connected with its adjacent poses like a convoy through internal interactions and subjected to external-repulsive forces produced by obstacles in its vicinity. Hence, the path optimization becomes a path deformation problem, which relies on the principles of rigid body dynamics to iteratively simulate the evolution of each pose on the optimization process. In particular, it is proposed to subject each vehicle pose in the query path to two types of efforts:

• Internal efforts: consecutive poses are kept connected through virtual elastic and torsional springs, which simulate the Hooke’s elasticity concept and originate elastic forces and torsional torques. These efforts guarantee smoothness on deformation and help shorten the path, and

• External efforts: repulsive forces repel the rigid poses from obstacles, acting as a collision avoidance feature. Moreover, force eccentricity originates repulsive rotating torques, which re-adapt poses orientation maximizing clearance over the obstacles.

Loosely following the elastic bands concept proposed by Quinlan and Khatib in [21], this method, by considering each vehicle pose as a rigid body, enables the path deformation to explicitly consider the vehicle geometry and exploits the rhombic vehicle nature, issues considered here as unattended on similar studies.

The implemented path optimization process based on elastic bands concept is described as follows. Let $j=\{1, \ldots ,J \}$ be the index of the consecutive vehicle poses composing the query path, each defined by a configuration vector

$$\begin{aligned} q_j=\left[ \begin{array}{c} s_j\\ \theta _j \end{array}\right] , \end{aligned}$$

(18)

where $s_j$ and $\theta _j$ denote the position and the orientation of the pose $q_j$ relative to a fixed reference frame, respectively. It is stated that $q_1=q_S$ and $q_J=q_F$.

The elastic force, $F_E$, and the torsional torque, $\tau _T$, evaluated for the vehicle pose at $q_j$ are:

$$\begin{aligned} F_E(q_j)=K_E\cdot [(s_{j+1}-s_j)+(s_{j-1}-s_{j})] \end{aligned}$$

(19)

$$\begin{aligned} \tau _T(q_j)=K_T\cdot [(\theta _{j+1}- \theta _j)+(\theta _{j-1}- \theta _{j})], \end{aligned}$$

(20)

where $K_E$, the elasticity gain and, $K_T$, the torsional gain, control the elastic and the torsional avoidance behavior on the path deformation, respectively.

The evaluation of the external efforts due to obstacle proximity relies on a heuristic-based collision detector module, which is capable of determining the set of i-nearest Obstacle Point (OP) to each sampled pose $q_j$. The overall procedure to handle the evaluation of the repulsive forces and torques are described as follows:

• 1. Let $i=\{1, \ldots ,I \}$ denote the index of the $i$th OP relative to a specific pose $q_j$. Let $u_{j,i}$ be the vector

  $$\begin{aligned} u_{j,i}=V_{j,i} - O_{j,i}, \end{aligned}$$

  (21)

  taken from each OP, $O_{j,i}$, and the corresponding vehicle nearest point, $V_{j,i}$.

• 2. To improve clearance during path deformation, distance-dependent repulsive forces are defined, where each pair of points ($O_{j,i}$, $V_{j,i}$) determines a repulsive contribution. For a specific vehicle pose, $q_j$, the repulsive contributions are defined as

  $$\begin{aligned} r_{j,i}=\frac{u_{j,i}}{\Vert u_{j,i}\Vert }\cdot f(\Vert {u_{j,i}}\Vert ) \end{aligned}$$

  (22)

  where,

  $$\begin{aligned} f(\Vert {u_{j,i}}\Vert ) = \max \left( 0,F_{max} - \frac{F_{max}}{d_{max}}\cdot \Vert {u_{j,i}}\Vert \right) . \end{aligned}$$

  (23)

  In (23), a maximum allowable magnitude, $F_{max}$, is assigned to avoid outsized values in the close vicinity of the obstacles, and $d_{max}$ denotes the distance up to which the repulsive force is applied.

• 3. For each pose $q_j$, the total repulsive force is defined as

  $$\begin{aligned} F_R(q_j)=\sum _{i=1}^Ir_{j,i}. \end{aligned}$$

  (24)

  Using (17), the net repulsive torque around the $j$th pose CoM is defined as

  $$\begin{aligned} \tau _R(q_j)=\sum _{i=1}^Ir_{j,i}\times e_{j,i}. \end{aligned}$$

  (25)

The repulsive and elastic forces are combined on a total force contribution as,

$$\begin{aligned} F_{total}(q_j)=F_{R}(q_j)+F_{E}(q_j) . \end{aligned}$$

(26)

Similar approach is valid for the torsional and repulsive torques acting on each pose $q_j$. This leads to the definition of a net torque expressed as,

$$\begin{aligned} \tau _{total}(q_j)=\tau _T(q_j)+\tau _R(q_j). \end{aligned}$$

(27)

Once determined the efforts acting on each pose, the ensued motion is evaluated through the principles of rigid body dynamics. Equations (16) and (17), are rewritten as

$$\begin{aligned} a_j=\frac{F_{total}(q_j)}{m} - K_D\cdot v_j \end{aligned}$$

(28)

$$\begin{aligned} \alpha _j=\frac{\tau _{total}(q_j)}{I_C} - K_D \omega _j, \end{aligned}$$

(29)

which provide the linear and angular accelerations for a specific pose $q_j$. The last term in the right hand side of (28) and (29) represent damping effects introduced to reduce the oscillatory motion during path deformation. They are controlled through $K_D$, herein set equally for both the translational and the rotational motion components. Notice that both $m$ and $I_C$ in (28) and (29), do not refer to real vehicle parameters but rather to simple scalars determining the resistance of each pose to change its configuration. The used values in the parameters are presented in Table 1 of Sect. 3.1.

From a starting configuration in the query path, $q_j$, this pose is updated iteratively assuming the resulted acceleration and torque, where the referred initial conditions are the previous iterated pose in this process. The stopping criteria is defined by setting a maximum number of iterations. The numerical solution is detailed in [29] and presented with results in all levels of TB and HCB in [30].

2.3 From Path to Trajectory

The output of the path optimization module of the planning methodology, using line guidance or free roaming, is a collision free path suitable for execution. To achieve a realistic plan, it is necessary to determine how the vehicle should move along the path satisfying dynamic constraints, i.e., the optimized paths should be parameterized in terms of velocities, converting the paths into trajectories. The definition of the vehicle’s velocity along the path assumes a particular importance in this study. The designed trajectories must guarantee that the vehicle performs its motion in the shortest time period satisfying energy minimization. On the other hand, safety requirements are mandatory and the risk of collision shall be reduced. Given the cluttered environment where the CPRHS/CTS moves, an initial approach might define the vehicle speed profile as a function of the distance to the obstacles. The velocity assumes low values when the vehicle is closer to obstacles. Otherwise, the velocity could be higher, under safety levels. To generate this initial speed profile, the minimum distance of each path point in the optimized path to the closest obstacles is identified, as shown in Fig. 18—left. Each value of minimum distance refers to a specific path point, $P_{i}$, and is measured considering the positioning of the vehicle when in this point (front or rear wheel), as if following the optimized path (see Fig. 18—right).

Fig. 18

Evolution of the minimum distances to obstacles along an optimized path (left image) and the respective path in the scenario, part of level B2 of HCB (right image)

From the evolution of the minimum distance to obstacles, also referred as clearance profile, it is possible to determine an initialization for the vehicle’s speed, which determines the speed at each reference point $P_{i}$, denoted as $s_{i}$, defined as a linear function of the minimum distance, $d_{i}$, included in the clearance profile as follows.

$$\begin{aligned} s_{i} = \left\{ \begin{array}{ll} s_{min} &{} \quad \text{ if } ~ d_{i} < d_{ safe}\\ \alpha (d_{i}) &{} \quad \text{ if } ~ d_{ safe} \le d_{i} < d_{th}\\ s_{max} &{} \quad \text{ if } ~ d_{i} \ge d_{th} \end{array}\right. \end{aligned}$$

(30)

A maximum and minimum allowable speed, $s_{max}$ and $s_{min}$, are set to this profile, in order to integrate kinematic constraints. The safety margin is denoted as $d_{ safe}$ and $d_{th}$ identifies the threshold distance above which $s_{max}$ is considered. The speed profile thus obtained is saturated when $d_{i}$ is above $d_{th}$ or below $d_{ safe}$ and is referred as C-based speed profile [25], as illustrated in Fig. 19—top left. However, the C-based speed profile is unable to handle vehicle dynamics constraints, meaning that the constraints on the admissible accelerations of the vehicle are ignored. To sidestep this issue, a specific routine was developed, which tests each C-based speed profile transitions, checking whether the accelerations are feasible or not. Whenever a dynamic unfeasible transition is found (e.g., the calculated acceleration is higher than the admissible maximum value, $a_{max}$, or lower than the admissible minimum value, $a_{min}$), the routine corrects the speed accordingly. In Fig. 19—top right, the difference between the C-based profile (yellow) and the re-evaluated speed profile based on vehicle dynamics (green) is depicted.

Fig. 19

Flow diagram for the evaluation of a speed profile that is both compliant with safety (from the proximity with obstacles) and dynamic (from the vehicle) constraints

The transition from a clearance based profile to a dynamic feasible profile may lead to a violation of the initial safety restrictions as it is pointed out in Fig. 19—bottom. This means that, in certain situations, the evaluated speed profile may result in values of speed higher than the desired given the obstacle proximity and the maximum level of acceleration/deceleration. When these transgressions are detected, the final profile is generated by considering both safety requirements and vehicle dynamics constraints as illustrated in the bottom right of Fig. 19. The obtained speed profile can be significantly different from the initial C-based speed profile and when combined with the optimized path, the final optimized trajectory is obtained.

A typical optimized trajectory obtained is shown in Fig. 20 along with the speed variation along the path ($a_{max} = 0.01\,\mathrm{m}/\mathrm{s}^2$, $d_{ safe} = 0.3\,\mathrm{m}$, $d_{th} = 1\,\mathrm{m}$).

Fig. 20

CPRHS speed map for a journey to port cell 2 in level L1 of TB. Each pose is painted in a different color corresponding to a speed value according to the gradient, where the values are in meters per second

2.4 Feasibility Analysis

A vehicle spans an occupancy area when it moves along a path evaluated either using the line guidance or the free roaming methodologies, with or without maneuvers, with or without maximization of common parts among different paths. The occupancy area is evaluated after the path optimization process as follows. The poses are distributed along the path. Then, a polygon is build to fit all the poses. That polygon results from an iterative sum of polygons of each pose, using the polygon clip algorithm described in [31]. The resulted polygon of a path computed by the line guidance is illustrated in the second image of Fig. 21, and computed by the free roaming is illustrated in Fig. 22.

The boundary of the occupancy area means that at least one corner of the vehicle can pass over there. To enlarge the occupancy area with a safety margin, according to the requirements of ITER, a new polygon is computed, containing the initial polygon. The minimum distance between the boundary of the new polygon and the boundary of the initial polygon is always constant and defined by a value identified as the safety margin of 30 cm. The resulted polygons with a safety margin are illustrated in the third images of Figs. 21 and 22.

Fig. 21

A generic path computed by the line guidance algorithm (first image), the corresponding occupied area (second image) with a safety margin (third image)

Fig. 22

A generic path computed by the free roaming algorithm (first image), the corresponding occupied area (second image) with a safety margin (third image)

Fig. 23

Examples of the swept volume of the casks in level B2 of HCB, where no other elements can be placed

In terms of 3D, a polygon with the occupancy area is extruded to a swept volume according to the height of the vehicle. This swept volume in the real world results on an additional constrain to the scenario. No other physical elements can be placed in this volume, which must be a free area, as illustrated in Fig. 23. Besides the trajectory optimization, the swept volume is another important output required by the ITER Organization for the buildings design. By integrating the swept volume of a specific mission with new building CAD models it is possible to, immediately, identify if clashes exist and hence, if that mission is feasible.

In addition, combining the occupancy area with the building model, it is possible to identify the closest points between the scenario and the vehicle when it follows a path. The feasibility analysis becomes a geometrical problem to find the closest points between the polygon of the occupancy area and the segment lines of the scenario. As a result, the most critical points of the scenario, i.e., the points with the highest risk of collision are identified, as illustrated in Fig. 24. If there is an intersection with the polygon, a clash exists.

Fig. 24

The most closest points between the scenarios and the paths evaluated by the line guidance (left image) and by the free roaming (right image)

3 Results

The algorithms were implemented in the specially designed software tool Trajectory Evaluator and Simulator (TES), developed under the grants F4E-GRT-016 and F4E-GRT-276-01 of the European Joint Undertaking for ITER and the Development of Fusion Energy. The TES was developed to generate trajectories using line guidance and free roaming approaches, for the evaluation of common parts of different trajectories and for the evaluation of the 3D volume swept by the vehicle in CATIA V5R19 format. The TES receives the models of the buildings and of different vehicles typologies and exports the optimized trajectories and the corresponding 3D swept volume directly to CATIA V5R19.¹ The TES provides also a GUI to preview the trajectory optimization, to manipulate the scenarios (for instance to test modifications in the port doors aperture configuration if necessary), to easily choose the vehicle typology to be used in the simulation and to generate results. Snapshots of TES are presented in Fig. 25, illustrating some features, including the exportation to CATIA V5R19. The output of TES is a set of optimized trajectories, which, under the scope of the previously mentioned grants, were validated by an independent software developed by ASTRIUM SAS [32]. Additionally, TES provides trajectory information that supports the comparison of the different trajectory scenarios, i.e., both wheels following the same path or following different paths, the ability to evaluate the risk of a clash and the time duration of the journey of a mission.

Fig. 25

Snapshots of the Trajectory Evaluator and Simulator (TES) software application

3.1 Trajectories Simulated Results

The results were generated using the maps of the three levels of TB and the four levels of HCB for trajectory optimization (including trajectories for nominal operation of transportation, rescue and parking in HCB). Most of the trajectories are between the lift and a port in TB, and a docking port or a parking place in the HCB. Wherever possible, the line guidance approach is selected to find an optimized trajectory. Figure 26 illustrates an optimized trajectory using line guidance between the lift and the port 2 of level B1 in TB. Figure 26—left illustrates the common path of both wheels, the paths described by the wheels and by the center of the vehicle and a sample of poses along the path. Figure 26—center represents the most critical points of the scenario associated to the path, i.e., the closest obstacles to the vehicle when it crosses each point of the path. The risk level is coded by a gradient color, where red corresponds to the most risky situations. It is noticed that the critical points are most of the times the environment’s corners, in particular the entrances to the lift and to the ports. The third image on the right represents the area swept by the vehicle when following the optimized path, i.e., the reserved area where no other elements are allowed aiming at avoiding collisions. Around the swept area it is also presented an extrusion of 30 cm that represents a safety margin; if an element is placed there, no collision is verified, but the risk of clash is high. In Fig. 27 it is quantified the minimum distances to obstacles along the path and the speed profile for the trajectory presented in Fig. 26.

Fig. 26

Trajectory to port 2 in level B1 of TB using line guidance (from left to right): path and sampling poses, the most closest obstacles and the area spanned by the vehicle along the path, with a safety margin

Fig. 27

The minimum distance to the closest obstacles (left image) and the speed profile (right image) along the trajectory to port 2 in level B1 of TB using line guidance

Fig. 28

Optimized trajectory in the vicinity of the lift using line guidance without maneuver (left image) and with a maneuver (right image)

In some situations, the free space constrains the usage of line guidance as illustrated in Fig. 28—left: the optimized path may not result in collision but the safety margin in the entrance to the lift is not fulfilled. Therefore, a maneuver is adopted, as illustrated in the right side of Fig. 28. The minimum distances to obstacles are larger in a trajectory with a maneuver, as illustrated in Fig. 29, mainly between the points 6 and 12 of the path. Each path is a set of points and the horizontal axis in Fig. 29 represents the indexes of those points.

Fig. 29

The minimum distance to the closest obstacles with and without the maneuver in the vicinity of the entrance to the lift

Fig. 30

Comparison of non feasible trajectory using line guidance (left image) and the feasible trajectory using free roaming (right image) to port 14 in level B1 of TB

Fig. 31

Trajectory to port 1 in level B1 of TB (from left to right): line guidance and the corresponding spanned area, free roaming and the corresponding spanned area

Figure 30 illustrates the example of port 14, where line guidance can not provide a feasible trajectory, even if including maneuvers. The solution is to adopt the free roaming approach. Given the requirement of maximizing the common parts of different trajectories, the free roaming is only adopted in the vicinity of the entrance to the port, while the other part of the trajectory is accomplished using line guidance.

The line guidance is always adopted as the first choice for the trajectory optimization. However, all the feasible trajectories evaluated using the line guidance approach can also be evaluated or improved using the free roaming approach. The line guidance approach can be seen as a particular case of the free roaming, i.e., when the paths of both wheels are coincident free roaming becomes the solution.

In terms of ITER requirements, if a trajectory evaluated using line guidance is feasible for a particular mission, the free roaming is not studied. Even though and for the purpose of this paper. Figure 31 shows the comparison between the two approaches for port 1 in level B1 of TB, where both approaches are feasible. The trajectories are similar, but in the vicinity of the entrances to the lift and to the port, the differences between the paths of each wheel are more relevant. In addition, the space swept by the cask when moving along a path evaluated by the free roaming approach is larger when compared with the line guidance approach. In the example illustrated in Fig. 31, the total swept area using line guidance is $173.3\,\text {m}^2$, while using the free roaming is $182.2\,\text {m}^2$, i.e., 5 % less. However, the major difference in both approaches is the distances to the closest obstacles when the vehicle is moving along the paths. As illustrated in Fig. 32, the free roaming provides a better safety distance when the vehicle is closer to the obstacles. In particular, in the entrance to the port (around the trajectory point 40), the minimum distance to the closest obstacle along the path evaluated by the line guidance approach is closer to the safety margin, while the free roaming approach results in a trajectory with additional 15 cm of safety margin. It is a substantial value when the minimum safety margins allowed in ITER are between 10 and 30 cm.

Fig. 32

Comparison of minimum distance to the closest obstacles between line guidance and free roaming trajectories to pot 1 in level B1 of TB

Fig. 33

Trajectories to ports 12 and 13 in level B1 of TB using line guidance without maximization of common parts: sampling poses (left image) and the waypoints (right image)

The maximization of common parts from different trajectories is more noticeable for long trajectories, as in the examples illustrated in Figs. 33 and 34: trajectories for ports 12 and 13 in level B1 of TB. Figure 33 illustrates the trajectories evaluated separately. The trajectories are quite similar, with the exception in the vicinity of the ports or closer to the pillars. Figure 34 illustrates both trajectories to the same ports, but maximized with the common path around the tokamak. The main difference is visible outside the lift, since the common path around the tokamak took into consideration the other trajectories that are counter clockwise and requires maneuvers (see the example of Fig. 14) and also the trajectory for port 2 (see Fig. 26). The resulted trajectory of maximization the common part seems to have less waypoints when compared with the trajectories that were evaluated individually. The number of points are equal, with the difference that the points are exactly the same for both trajectories maximized with the common part until the splitting point of the first trajectory (to the port 13). The minimum distances along the trajectories to port 12 and 13 obtained with and without the maximization of the common part are plotted in Figs. 35 and 36. The trajectories resulted from the maximization of common parts provide better results since they decrease the effect of the smoothness and increase the clearance. However, the minimum values, which corresponds to the entrance to the ports, remain approximately the same.

Fig. 34

Trajectories to ports 12 and 13 in level B1 of TB using line guidance with maximization of common parts: sampling poses (left image) and the waypoints (right image)

Fig. 35

Comparison of the minimum distances to the closest obstacles using line guidance trajectories to port 12, with and without maximization of common part

Fig. 36

Comparison of the minimum distances to the closest obstacles using line guidance trajectories to port 13, with and without maximization of common part

Table 1 Values used in the experimental results, where $m$ and $I_c$ were assumed as 0.5 and 3.29, respectively

Fig. 37

The optimized trajectories for all ports in level B1 of TB (first image), the respective spanned area (second image) and the paths described by the wheels (third image)

Fig. 38

The optimized trajectories for rescue mission for all ports in level B1 of TB (first image), the respective spanned area (second image) and the paths described by the wheels (third image)

Fig. 39

The optimized trajectories for all ports in level L1 (left image) and in level B2 (right image) of HCB

A total of 536 trajectories were optimized: 304 for the 4 levels in HCB and 233 for the 3 levels in TB, for different cask typologies and using the values presented in Table 1. The 3D CAD models of the occupied volumes were generated for all trajectories. Figure 37 illustrates all the trajectories for the nominal operations and the respective occupied volume in level B1 of TB. The common path is similar to a ring around the tokamak. Figure 38 illustrates all the trajectories for the rescue operations and the respective occupied volumes in level B1 of TB (in rescue operations it is assumed a docked cask inside the port). Figure 39 illustrates all the trajectories for the nominal operations in levels B2 and L1 of HCB.

3.2 Feasibility Analysis Results

The main goal of the entire work is the motion planning inside the ITER buildings. However, throughout the project, a new challenge was proposed. Despite of walls, pillars and doors, the model of the scenario includes a proposed volume where the vehicle is allowed to move. Therefore, the new challenge is to study the feasibility of the proposed volume, i.e., if it is possible to evaluate a path inside the proposed volume.

Fig. 40

The CAD model with the allowed space for the cask navigation (first image), the respective geometric representation (second image) and the optimized path in the allowed space (third image) to the port cell 3 in level B1 of TB

For the sake of the work related with the analysis of volume feasibility, a proposed volume is considered feasible if there is at least one feasible trajectory that fits the volume. It is applicable to line guidance or free roaming. To find a feasible path, it is considered a virtual map that results from the 2D projection of the proposed volume, as illustrated in Fig. 40 for the particular case of the mission from lift to the port cell 2 in level B1 of TB. The first image is a snapshot of the 2D view of the proposed volume in CATIA software, the second image the 2D projection plotted in TES, the last image is an optimized path inside the proposed volume and hence feasible.

When no volume is proposed, there are more free space available in the scenario to find a path, as depicted in Fig. 41. In that case, the path is optimized in terms of smoothness and clearance. The distances along the paths are evaluated against the closest obstacles of the scenario and defines the safety of the mission following that path. When a volume is initially proposed, the space is highly constrained and the goal is only to find a feasible path, as illustrated in Fig. 42. In this case, the distances along the path are evaluated to the proposed volume, leading to lower values.

Fig. 41

The line guidance optimized path to port 12 of level B1 in TB (first image), the respective swept area (second image) and the distance to the closest obstacles along the path (third image)

Fig. 42

The feasibility analysis to port 12 of level B1 in TB (first image), the comparison between the proposed area and the swept area (second image) and the distance between them (third image)

Another important feasibility analysis is the entire trajectory around the Tokamak, called the “ring”, which corresponds to the common part of all trajectories. As example, the Fig. 43 illustrates the proposed volume for level B1 of TB, where a possible path totally inside the proposed volume was identified. The path is represented by the lines described by each wheel and by the center of the vehicle. Figure 44 presents the minimum distances between the vehicle along the path used for the feasibility analysis and the proposed volume that corresponds to the ring. The ring is feasible, since a path was found. However, there are two critical poses where the minimum distance is below 100 mm, which only occurs in the vicinity of the entrance to the lift and slightly above the 100 mm closer to the port 11.

Fig. 43

The best path inside the allowed space for the ring in level L1 of TB and the two critical situations where the minimum distance between cask and the boundary is below a threshold value (left and right images)

Fig. 44

The minimum distances between the cask poses and the boundaries of the allowed volume along the path for the ring in level L1 of TB

Different volumes were proposed for each port in all levels of TB, given the different typologies of the cask expected for operation. As result, a total of 233 trajectories were calculated in TB (3 trajectories for the rings, 46 trajectories for the feasibility analysis of volumes associated to the cask.

4 Open Issues and Future Work

This paper presented the trajectory optimization strategies developed for Remote Handling systems that will operate in ITER, for transporting heavy and highly activated in-vessel components between the Tokamak Building and the Hot Cell Building. Two main approaches were developed for trajectory optimization, providing smooth paths that maximize the clearance to obstacles, taking into account the features of rhombic like vehicles: line guidance (both wheels following the same path) and free roaming (different paths for each wheel). Whenever possible, the line guidance approach with or without maneuvers is adopted as the final solution. If not possible, the free roaming is selected. The trajectories were evaluated maximizing the common paths in the same level of each building. The maximized path is line guidance, since both wheels follows the same path. The branches to each port could also be line guidance or free roaming.

The two main approaches were implemented in a standalone application that receives the 3D CAD models of each level of the buildings, converts them into 2D models and, using the specifications of the missions and the models of the vehicles, returns the best trajectories, including a report of the most risky points of collision and the swept volume of he vehicle along the missions to CATIA V5R19 software. More than 500 hundred trajectories were evaluated, most of them using line guidance, some of them with 1 and few with 2 maneuvers. The most critical points are in the vicinity of the pillars and in the entrance to the lift of ports, where sometimes the free roaming is the only feasible solution. The main conclusions of trajectory optimization were provided to the ITER Organization by the Fusion For Energy and they were crucial to proceed with the construction of the Tokamak Building. For instance the doors aperture profiles (angle and orientation) were adjusted to reduce the risk of clashes.

The required trajectories were computed offline. However, the actual implementation can be reverted into a version to run in real time. The decision of selecting line guidance and free roaming is manually, as similar to the decision to include a maneuver and the respective point of maneuver. In the future, the decision of selecting line guidance or free roaming can be done based on the minimum distances to the obstacles, as the decision of including or not maneuvers. The decision of where to put a point of maneuver remains as an open issue. The line guidance approach is a deterministic process, while free roaming may converge to similar solutions, since the initialization is random given the Rapidly-Exploring Random Tree component.

The velocity profile in the trajectory will be improved taking into account approaches with dynamics and the risk of huge vehicle in cluttered environments, which is the next instance of development and improvement of TES.

The next challenge of RH in ITER scenarios after trajectory optimization is the path following. The first simulated results have already achieved in the path following using modified versions of cutting edge approaches, such as Alonzo Kelly controller, Stanley controller, arc path following controller and also a non linear based controller embodied in the Lyapunov function, all resumed in [33]. The first work done only considers the kinematics model, while the inclusion of dynamics is on progress.

The localization of the vehicle, i.e., its position and orientation are also variables to estimate. Given the rad-hard condition in ITER scenarios, different approaches of implementation have been studied. In particular, a set of laser range finders strategically spread over the ITER rooms with Extended Kalman Filtering estimator, as detailed in [34].

The studies of path following and localization has been addressed on simulation environment and also in a experimental setup with a 1:25 scaled mock-up of half of a floor of the TB, as illustrated in Fig. 45. It includes four laser range finder sensors, one video camera and a rhombic like vehicle remotely controlled by a computer. The first results were already achieved an published.

Fig. 45

Experimental setup with a 1:25 scaled mock-up of half of a floor of the TB with four laser range finder sensors, one video camera and a rhombic like vehicle remotely controlled

An emerging issue in ITER scenarios is the logistics given the number of casks and the reduced space available to park and move them. The first studies to optimize the layout and the sequence of operations to move each cask, including optimized trajectories, have already done and reported in [35].

The TES has been upgraded with new features to endorse the new challenges, namely the path following, the localization, logistics and also multiple vehicles operating simultaneously.