Motion Planning for a Rhombic-Like Vehicle Operating in ITERScenarios

Daniel FonteInstituto Superior Tecnico

Av. Rovisco Pais 1, 1049-001, Lisboa - PortugalEmail: danielalexandrefonte@gmail.com

Abstract— The paper addresses the motion planning problemstated for an autonomous rhombic vehicle that will operatein the buildings of the International Thermonuclear Experi-mental Reactor (ITER). The problem is addressed followingtwo distinct headings. First, a refinement strategy is proposedwhich divides the motion planning problem in three differentand successive stages: the geometric path evaluation, the pathoptimization and the trajectory evaluation. In its turn, thesecond strategy follows a more direct and unified trajectoryplanning philosophy that enables to directly output trajectorysolutions. As a requirement for the latter approach the paperconducts a detailed analysis on the rhombic vehicle motionincluding the development of different vehicle behavior models.Gathered simulated results show the advantage of the refine-ment strategy on handling feasible and reliable trajectories incluttered scenarios such as those found in ITER. The trajectoryplanning strategy reveals some limitations with future perspec-tives directed to a refinement-like strategy.

I. INTRODUCTION

The International Thermonuclear Experimental Reactor(ITER) is a world project on fusion research that will consti-tute the largest tokamak-based research facility. Besides themajor scientific objective of a viable use of fusion energy,ITER aims to demonstrate that the future fusion power plantscan be safely and effectively maintained. After the start ofmachine operation, its main components become activatedpreventing human presence in certain areas and therefore,the ITER maintenance system will mostly depend on reliableand robust Remote Handling (RH) systems.

The Cask and Plug Remote Handling System (CPRHS)is a large transporter, which will provide the means totransfer in-vessel components and RH tools between theTokamak Building (TB) and the Hot Cell Building (HCB),the two main buildings of ITER. Each CPRHS is composedby three subsystems: the cask envelope, the pallet and theCask Transfer System (CTS), as depicted in Fig. 1-right.The largest CPRHS has dimensions 8.5m x 2.62m x 3.62m(length x width x height) and can reach a maximum weight,when loaded, of ∼ 100T . The CTS, acting as a mobilerobot, is issued with an air cushion system to providean high load capacity to the overall transporter and canmove independently from the cask and pallet. From paststudies related to the CTS design [RLAF97], a decisionwas made to adopt this vehicle with a rhombic kinematicconfiguration with two pairs (one for spare purposes) ofdrivable and steerable wheels positioned on the front andrear of the vehicle and two swivel wheels on the sides.

Cask Transfer System (CTS) with Rhombic Configuration

2,16

m

Air Caster

Drivable/Steerable

Wheels

Swivel Wheels 8,5 m

Cask and Plug Remote Handling System (CPRHS)

Cask Envelope

Pallet

Bottom view

3,62

m

High Maneuvering Ability

Fig. 1. Left: Maneuvering ability of the CPRHS. Right: schematic viewof the CPRHS and the CTS with its rhombic configuration and air cushionsystem.

This kinematic configuration endows the transporter withan high maneuvering ability and flexibility, which are keytraits when considering the cluttered nature of the ITERenvironments, as illustrated in Fig.1-left. The geometry ofthe CPRHS and payload vary according to the cask andcomponents to be transported. As a reference, the largestCPRHS dimensions are 8.5m x 2.62m x 3.62m (length xwidth x height). From this time forward, the term “rhombicvehicle” is used whenever the presented considerations canbe used indifferently for the CPRHS or the CTS. This unifieddesignation is clarified later in Subsection II-A with a precisesystem definition.

The CPRHS should move along optimized prescribedpaths according to schedule or unscheduled maintenanceoperations. Considering the large dimensions of the vehicleand the cluttered nature of the environments, each plannedpath shall satisfy the following criteria: (1) maximize theclearance to the obstacles to reduce the risk of collision;(2) minimize the distance traveled to prevent the need torecharge the on-board batteries during the journeys; (3)guarantee smooth transitions to avoid jerky motions andreduce the number of maneuvers.

The use of motion planning techniques [Lat91], [Lav03]appears as a powerful tool to plan and analyze the requiredmotions for these missions. Regardless of their robustness tosolve specified motion queries, these planning techniques canperform poorly when considering optimization requirementssuch those referred above and the feasibility of the plannedsolutions. Hence, seminal forms of general motion planningtechniques may be required to be coupled with optimization

tools or to integrate extended capabilities on an attemptto generate feasible and optimized solutions that can befollowed by the vehicle. In this context, the paper addressesthe motion planning problem inherent to the autonomousnavigation problem of the rhombic vehicle. In addition, theunusual kinematic rhombic configuration adopted for this ve-hicle, which is barely described in the literature, sustained apioneer analysis on aspects related with its motion, includingthe development of simulation models that follow differentlevels of abstraction.

The paper is organized as follows. Section II presents anextended analysis on the motion behavior of the rhombicvehicle, including the development of a kinematic and dy-namic model. Section III discusses the need and interest ofthe motion planning and concretely formulates the motionplanning problem for the rhombic vehicle. Section IV andSec V present the developed planning techniques based ontwo different algorithmic approaches and planning strategies.The remaining sections lay out the obtained results (SectionVI) on ITER and other simulated environments and outlinesome conclusions and directions for future work (SectionVII).

II. MATHEMATICAL MODELING

This section addresses the subject of mathematical model-ing for the rhombic vehicle, a necessary and important stepin simulation studies.

A. System Definition

The goal of mathematical modeling is to provide a mathe-matical description of the relations between the system quan-tities (states) as well the relations between these quantitiesand external inputs. Therefore the system definition is arather important step, specially when leading with complexsystems as it is the case of the CPRHS.

Without loss of generality and to avoid misinterpretationsand tedious specifications, the paper assumes a single systemrepresentation for the modeling process, which is representedin Figure 2. The adopted system definition relies on a setof reference parameters which define (without freezing) thesystem boundaries enabling an easy system interchangeabil-ity or operating conditions (e.g., transported load, Center ofGravity (CoG) position). The chosen inputs for the modelingprocess are the linear velocities on the front and rear wheels(i.e., steering plus the wheel linear speed) and the outputrefers to the evolution of the CoG (G) position and thevehicle orientation (i.e., the vehicle’s pose). Also, considerthe following basic assumptions: the vehicle is considered tobe a solid rigid body. The only movable parts are the wheels,which are considered to be undeformable; The vehicle moveson a hard, flat surface; The vehicle is considered to besymmetric with respect to its longitudinal and lateral axes.

B. Kinematic modeling

The purpose of this kinematic study is to establish themathematical equations describing the relationship betweenthe temporal variations of the vehicle pose and the linear

G

!

Y

yG

xG

F

R

MF

vFvR

M

!F!R !

MR

LW

vF !Front wheel velocityvR !Rear wheel velocity!F !Front wheel steering!R !Rear wheel steering

xG !CoG x-coordinateyG !CoG y-coordinate! ! Vehicle orientation

G !CoG F ! front wheel R! rear wheelL ! vehicle lengthW ! vehicle widthMF !distance of the front wheel to the CoGMR !distance of the rear wheel to the CoG

Geometric Parameters:

Inputs: Outputs:

Dynamic Parameters:

m!mass of the vehicleI ! moment of inertia

Fig. 2. System representation

velocities on the wheels. It consists on a pure geometricalstudy that is carried out without considering vehicle dynamicproperties such as mass, inertia or friction. Also, at this point,forces or torque inputs are not considered.

Under the Rolling Without Slipping (RWS) assumption,the system is subjected to the following two nonholonomic(i.e., non integrable) constraints, one for each wheel:

xF · sin(θ + θF )− yF · cos(θ + θF ) = 0 (1)

xR · sin(θ + θR)− yR · cos(θ + θR) = 0 (2)

with (xF , yF ) and (xR, yR), being the Cartesian coordinatesof the front and rear wheel, respectively. Using the rigidbody geometric constraints, xF and xR can be obtained asfunction of xG and therefore the nonholonomic constraints,(1) and (2), can be written in the following matrix form asfollows

C(q) · q = 0, (3)

where,q = [xG yG θ]T is the chosen vehicle state vectorand C is equal to

C =

[sin(θ + θF ) − cos(θ + θF ) −MF · cos θFsin(θ + θR) − cos(θ + θR) +MR · cos θR

]. (4)

According to [ACB91], where the authors proposed ageneral framework for modeling Wheeled Mobile Robots(WMRs) with different wheel configurations, the homoge-neous system just presented can be used to formulate akinematic model of the rhombic vehicle. In fact, all theadmissible generalized velocities for the rhombic vehicle areconstrained to belong to a reduced subset of solutions, whichis given by the kernel (Ker) or null space of the constraintmatrix C. As provided in [Dan11] the basis for the null spacefor the matrix C can be set equal to:xGyG

θ

=

MR·cos(θ+θF )M

MF ·sin(θ+θR)M

− sin θRM

· vF +

MF ·cos(θ+θR)M

MF ·sin(θ+θR)M

− sin θRM

· vR. (5)

The system of equations in (5) comprises the kinematicmodel of the rhombic vehicle.

C. Dynamic Modeling

The kinematic model in (5) captures the essential of therhombic vehicle kinematics and is well suited for control andsimulation purposes. However, the non-slipping assumptionsin which the derivation of the kinematic model was based ishardly met in reality. In fact, when the vehicle experiences

slippage phenomena, for instance, in braking and accelera-tion phases or in turning maneuvers, the kinematic modelwill not be able to correctly predict the system behaviorproviding a low simulation accuracy. Most part of simulationand motion control related studies use a simple kinematicmodel to describe the movable system motion. The argumentis usually based on the low speeds, low accelerations andthe lightly loaded conditions under which these vehiclesoperate. In fact, the rhombic vehicle will operate underconsiderably low velocities, not exceeding the 0, 2 m·s−1 butit will have to perform heavy duty work, which may signif-icantly increase tire deformation and consequently slippagephenomena. Moreover the rhombic vehicle operations areissued with the typical demanding requirements of nuclearscenarios, which require high performances and accuracy.All these these facts encourage the author of the paper toassess a dynamic modeling approach, where wheel an vehicleslippage as well other issues, for instance actuator dynamicsor load distribution, are taken into consideration as to obtaina more accurate simulation model of the rhombic vehicle.

The vehicle dynamic model herein presented resorts onthe classic mechanics of the rigid body, namely usingthe Newton-Euler approach. In addition to the previously(subsection II-A) presented assumptions, the derivation ofthe dynamic model of the rhombic vehicle is based on thefollowing new or redefined hypotheses:

• The wheels are no longer considered as an unde-formable solid; slippage phenomena is included on thedynamic analysis;

• The adhesion conditions on the rear and front wheelsare considered to be equal;

• The CoG of the vehicle is located along its longitudinalaxis and it is taken as reference point for the vehiclestate vector;

• The vehicle suspension system is not considered. Forthis reason, the effects of rolling, pitching and loadtransfer are not included;

• Both longitudinal and lateral motions are considered toprovide an accurate location of the vehicle;

• Counter-clockwise angles and torques are considered tobe positive;

• The dynamic model is derived following Newton-Eulerformulation.

The vehicle dynamics with the forces acting on it, isrepresented in 3. The derivation of the dynamic model isgiven in the the global coordinate frame (X,Y ), but [Dan11]presents two other models in the the local vehicle frame (q, p)and the vehicle velocity frame (n, t).

the motion of the CoG of the vehicle along the X-axis isgiven by

Fx = m · xG (6)

with m denoting the vehicle mass. Setting up the equilibriumof forces along the X-axis yields,

m · xG = FlF · cos(θ + θF )− FsF · sin(θ + θF )+FlR · cos(θ + θR)− FsR · sin(θ + θR),

(7)

lRF

sRF

lFF

sFF

!F

F

R!R G

v

!"

#" " !

$"

Y

Fig. 3. Dynamic representation of the rhombic vehicle.

where FlF , FlR and FsF , FsR, are the longitudinal and lateralforces on the front and rear wheel of the vehicle.

Similarly, along the Y -axis the vehicle motion is describedas

Fy = m · yG (8)

with the vertical forces summing up to

m · yG = FlF · sin(θ + θF ) + FsF · cos(θ + θF )+FlR · sin(θ + θR) + FsR · cos(θ + θR)

(9)Setting up the equilibrium of momentum about the vertical

axis ate the CoG of the vehicle chassis, we obtain thefollowing

Iz θ = FlF · sin θF · LF + FsF · cos θF · LF−FlR · sin θR · LR − FsR · cos θR · LR

(10)

The equations (7),(9) and (34), can be assembled on amatrix form as follows. xG

yGθ

=

cos(θ+θF )m

cos(θ+θR)m

sin(θ+θF )m

sin(θ+θR)m

sin θF ·LF

Iz− sin θR·LR

Iz

· [ FlFFlR

]+

− sin(θ+θF )m − sin(θ+θR)

mcos(θ+θF )

mcos(θ+θR)

mcos θF ·LF

Iz− cos θR·LR

Iz

· [ FsFFsR

](11)

which forms the dynamic model of the rhombic vehicle.

III. MOTION PLANNING PROBLEM FOR THERHOMBIC VEHICLE

This section impacts the specific motion planning problemof the rhombic vehicle within transfer and other operationsin ITER. The motion problem is completely described andthe solutions proposes in the paper to tackle it, are unveiled.

During ITER lifetime, the rhombic vehicle will be fre-quently asked to move autonomously from its current posi-tion to a desired or goal location. These “motion requests”,from now on referred to as motion queries, will be frequentand might appear in different contexts and scenarios suchas in: nominal cask transfer operations between the VacuumVessel Port Cells (VVPCs) in the TB and the Docking Ports(DPs) in the HCB, parking maneuvers and recovery andrescue missions.

Planning and analyzing these autonomous missions, eitheron the present design stage or later during ITER operation,

are hard and time consuming tasks, if performed manually.Computerized techniques are thus vital to reduce the cost andtime and increase the quality of such operations. Moreover,and providing that Computer-Aided Design (CAD) models ofthe ITER environments are available as well the informationabout the start and goal objectives for the different rhombicvehicle missions, the employment of motion planning tech-niques [Lat91], [Lav03], appear as a powerful tool to planand analyze the required motions.

The problem and the challenges involved are better under-stood if grounded in the common and basic ingredients ofevery motion planning problem, which are presented below.

1) Agent – The agent performer of the plan, i.e., thesolution for the motion query, is the rhombic vehicle.The planning solutions depend directly on the rhombicvehicle properties (e.g. geometric and kinematics).

2) Configuration – The configuration adopted correspondsto the pose of the rhombic vehicle defined with respectto a global fixed frame as the one defined in Figure 4.This configuration comprises the cartesian coordinatesof the vehicle center (C) and the vehicle orientation,i.e., q = [xC yC θ]T .

3) Start and goal configurations – define the start andfinal poses forming the motion query to the vehicle.

4) Geometric model of the World – model of the sce-nario, i.e., the occupied volumes of the scenario wherethe rhombic vehicle has to move and that constituterelevant information for the definition of a collisionfree plans. The motion planning of the rhombic ve-hicle is solved in a Two Dimensional (2D) world forwhich the original CATIA models, which are in ThreeDimensional (3D), must be converted to 2D projectionson the floor level, as depicted in Figure 4 in item 2.

5) Plan - solution to the motion query, this plan mustcorrectly drive the rhombic vehicle from the startingto the goal pose and without collisions. The paper, andwithout loss of generality, it is assumed that this planconsist on a trajectory defined for the vehicle center, orin a complementary way, for each wheel of the vehicle.

6) Optimization Criteria – In addition to the free collisioncriteria, the trajectory plan shall comply with otherrequirements. For instance, the particular nature of themotion problem of the rhombic vehicle, the clutteredenvironment where it has to move and the contaminatednature of its load, requires a smooth and safe trajecto-ries. Also it can be desirable to evaluate trajectorieswith the minimum length possible as to reduce energylosses or guarantee that differential constraints such asmaximum velocities and accelerations are satisfied.

Note that the desired final output is an optimized trajec-tory, which entail that vehicle kinematic and dynamic con-straints are satisfied. One approach is to regard a trajectoryas a decoupled solution composed of a path defining a set ofvehicle configurations or points that should be followed bythe vehicle and associated velocity profile. This option raisesthe possibility of using path planning methodologies to first

!"

!"

#"

$"

%" X Y

xCyCq

!

"

####

$

%

&&&&

&"

Fig. 4. The motion planning problem for the rhombic vehicle: 1- rhombicvehicle, 2- pose, 3- start and goal conditions, 4- geometric model (3D and2D), 5-optimized trajectories for each VVPC in the TB.

achieve a geometric description of the vehicle motion andthen couple it with a time parameterized law that embedsvehicle dynamic assumptions. This strategy is followed inSection IV to solve the motion planning problem of therhombic vehicle, with small improvements that include pathoptimization techniques. This strategy is referred to as adecoupled or refinement planning strategy, since the solu-tion plan is progressively improved. A different strategy ispresented in Section V, which directly evaluates trajectorysolutions that inherit vehicle differential constraints and it isreferred to as trajectory planning.

IV. A REFINEMENT STRATEGY FOR MOTIONPLANNING

This chapter proposes to tackle the motion planningproblem stated for the rhombic vehicle in Section III, byfollowing a decoupled and refinement strategy, which isdepicted in Figure 5 and is composed by the following threestages:

1) Geometric path evaluation – Provided that the geo-metric model of the environment and the initial andgoal configurations of the vehicle are given, an initialcollision-free path connecting the two objective config-urations is obtained;

2) Path optimization – This stage transforms the initial-ization path into an optimized solution that satisfiesdifferent optimization criteria;

3) Trajectory evaluation – The last stage considers howthe vehicle should move along the path in order to meettime and vehicle differential constraints.

The following subsections present two distinct approachesthat handle the first and second stage of this general frame-work. Each approach assumes different assumptions for thepath generation and optimization. In particular, they employtechniques with distinctive algorithmic natures. For thisreason they are presented in separate sections as follows:

• Section IV-A presents a combinatorial based approach,which assumes that the outputted trajectories of theplanning stage shall represent a unique reference to befollowed by both wheels of the rhombic vehicle, as illus-trated in Figure 5-left. The geometric path evaluation iscarried using a point-like robot approach by means of acombinatorial method named of Constrained Delaunay

Desired optimized path

(for both wheels) Path described by the center of

the vehicle

Both wheels on the path

Different paths for each wheel

Fig. 5. Left: the rhombic vehicle following a unique path reference forboth wheel. Right: executed motion by the rhombic vehicle while followingindependent references for each wheel.

Triangulation (CDT). The path optimization algorithmis carried based on the elastic bands concept [QK93a],[QK93b] which guarantees collision free paths for thevehicle, in addition to other optimizing criteria.

• The second approach assumes an increased vehicleflexibility, where the wheels are allowed to followindependent trajectories (see Figure 5-right). The geo-metric path evaluation is performed using the Rapidly-exploring Random Tree (RRT) method [Lav98]. Draw-ing inspiration in the former approach and on rigidbody dynamics, a different path optimization algorithmis developed, which is capable of fully exploring thehigh maneuvering ability of the rhombic vehicle.

A. Combinatorial Approach

1) Geometric Path Evaluation: In the combinatorial, thegeometric path evaluation is performed using the CDT,which yields a triangular Cell Decomposition (CD) andprovides the means to construct a search graph and tosolve the specified motion query. The overall procedure isillustrated in Figure 6 and descried as follows.

(i) For a specified scenario, the CDT is applied to thecorresponding 2D map yielding the triangle CD, asillustrated in Figure 6-a.

(ii) To handle a specific motion query, i.e., to connect theinitial configuration, qI , to the goal configuration, qG,the next step determines which cells contain these twoconfigurations.

(iii) All the possible triangle cell sequences connecting theinitial and goal triangle and composed by consecutivecells that do not share a real edge are evaluated.The desired cells in the sequences are connected byvirtual edges, since feasible solutions cannot cross wallsrepresented by real edges. Due to its complete nature,if such a sequence does not exist, the algorithm statesthat there is no solution for the proposed query.

(iv) The final step converts the shortest cell sequence into anordered sequence of points connected by line segments.

To increase the efficiency of the CDT algorithm the paperproposes the A∗ algorithm [HNR68] in alternative to thebrute-force search performed in step 3.

2) Elastic Bands: The CDT combined with the A∗ isable to efficiently determine a geometric path connectingconnecting qI to qG. However, the feasibility upon executionis not guaranteed as the obstacle avoidance for a rigidbody such as the rhombic vehicle remains an issue. Anotherundesirable effect resulting from this geometric solutionis the curvature discontinuity, which causes the vehicle to

Gq

IqGT

IT

(a)!

(c)! (d)!

(e)!(b)!Fig. 6. Evaluation of the initial geometric path: (a) Triangle CD with initialand goal conditions. (b)−(e) Possible triangle cell sequences. (e) Shortesttriangle cell sequence and corresponding geometric path.

stop every time it mets a path discontinuity. The geometricsolution is thus unfeasible to be followed by the vehicle. Inorder to meet some of the criteria requirements defined inSection III it would be desirable to: guarantee a collision-freepath, by enlarging the minimum distance from the vehicle toobstacles and reduce path looseness in order to get shorterand smoother paths without slacks. To tackle the referredissues, it is proposed the use of an optimization method basedon the elastic bands concept [QK93a], [QK93b].

Following the elastic band approach, the path is modeledas an elastic band which can be compared to a seriesof connected springs subjected to two types of forces asillustrated in Figure 7: Internal or elastic forces (Fe) to allowthe path to stretch; External or repulsive forces (Fr) to keepthe path, and consequently the vehicle, away from obstacles.

pj -1(k) Gq

qI

External or Repulsive Forces Internal or Elastic

Forces p6

p2 p3 p4 p5 pj (k)

pj +1(k)

pj (k+1) Geometric path

interpolated with a B-spline

Fig. 7. Elastic band formulation: internal or elastic and external or repulsiveforces.

When submitted to these artificial forces, the elastic bandis deformed over time becoming a shorter and smoother path,increasing clearance from obstacles.

The elastic force acting on each path point can be evalu-ated by

Fe(j) = Ke · [(pj−1 − pj)− (pj − pj+1)] (12)

with ke being the elastic gain and pj , the jth path point.The path elastic behavior is controlled through Ke; highvalues of Ke stretch the path while smaller values of Ke

provide more flexibility to the path deformation. The repul-sive contributions from the obstacles can be achieved in manydifferent ways. This in fact the main issue when adopting theelastic bands concept. The details are given in [Dan11] whereit is proposed the use of an innovative repulsive schemeimplementation that directly considers the vehicle geometric

constraints and complies with the constraint of a uniquereference for both wheels.

With the elastic and repulsive forces evaluated, the path isthen iteratively updated according to the following equation:

pj,new = pj,old + Ftotal(pj,old) (13)

where the total force contribution is given by

Ftotal(pj,old) = Fe(pj,old) + Fr(pj,old) (14)

B. Randomized Approach

1) Geometric Path Evaluation: To evaluate the geometricpath this new approach uses a randomized method, the RRTalgorithm. The RRT algorithm was first presented in [Lav03]as randomized data structure suitable for a broad class ofmotion planning problems. The adoption of this planningtechnique stems from the fact that it naturally extends to thenonholonomic planning problem later addressed in SectionV). The RRTs explicitly handle the vehicle geometry duringthe search and are thus able to directly generate collision-free paths for rigid bodies (in contrast with point like robotassumption of the previous approach). The seminal formof the RRT algorithm grows a single tree from the initialconfiguration (qI ), until one of its branches reaches the goalconfiguration (qG). Algorithm 1 presents the pseudocode forthis most basic RRT variant while Algorithm 2 displays thedual-tree version of the RRT which grows two trees, rootedat qI and qG, instead of one.

In each iteration, a configuration is randomly sample yield-ing qrand. The growTree in step 11, selects the closest node(or configuration) in the tree to the sampled configurationqrand, using a specified distance metric (ρ). In step 12,an attempt is made to add a new edge to the tree, fromqnear towards qrand. The search process runs until qG isreached (or an ε-boundary of qG) or the time elapsed exceedsa specified constant.

Algorithm 1: single-tree RRT

1 function: singleRRT(qI , qG, C)2 τ ← tree(qI)3 while time ≤ timemax do4 qrand ← randomConfig(C)5 qnew ← growTree(τ , qrand)6 if qnew ∧ ρ(qnew, qG)1 < ζ then7 return extractSol(qnew)2

8 τ ← tree(qI)

9 return failure

10 function: growTree(τ ,qtarget)11 qnear ← nearestNeighbor(τ, qtarget)12 τ ← τ+ newEdge(qnear, qtarget)3 return qnew

2) Path Optimization: The geometric path evaluationstage previously presented is able to generate collision-freepaths that account for the vehicle geometry. However, as forthe case of the combinatorial approach, it is obtained a rough

Algorithm 2: dual-tree RRT

1 τa, τb ← tree(qI),tree(qG)2 while time ≤ timemax do3 qrand ← randomConfig(C)4 qa ← growTree(τa, qrand)5 if qa then6 qb ← growTree(τb, qa)7 if qb then8 if ρ(qa, qb) < ζ then9 return extractSol(qa, qb)

10 τa, τb← τa, τb

11 return failure

and low quality solution, which presents small clearance overthe obstacles and induces jerky motions that do not favortracking purposes. The employment of a path optimizationtechnique is thus required as to achieve a feasible solutionthat can be followed by the rhombic vehicle. Moreover, thelimitations of the former refinement strategy dictated thedevelopment of a new path optimization technique able ofexploring the high maneuverability of the rhombic vehicle.

Taking into consideration all theses aspects, this sectionproposes a novel path optimization technique, which con-serves the idea of path optimization as path deformationproblem from the elastic bands. However, by drawing inspi-ration on the dynamics of rigid bodies and taking advantageof the explicit representation of the rough query path, whichdefines a set of collision-free vehicle configurations from qIto qG, this new method evades the formulation of paths asparticle systems.

This new method is schematized in Figure 8 and can besummarily described as follows. In the path optimizationprocess, each of the consecutive vehicle poses that form therough query path is treated as a rigid body that is connectedwith its adjacent poses like a convoy through internal inter-actions and subjected to external-repulsive forces producedby obstacles in its vicinity. Hence, the path optimizationbecomes a path deformation problem, which relies on theprinciples of rigid body dynamics to iteratively simulatethe evolution of each pose on the optimization process. Inparticular, it is proposed to subject each vehicle pose in thequery path to two types of efforts:

• Internal efforts – Consecutive poses are kept connectedthrough virtual elastic and torsional springs, which sim-ulate the Hooke’s elasticity concept and originate elasticforces and torsional torques. These efforts guaranteesmoothness on deformation and help to shorten the path;

• External efforts – Repulsive forces repel the rigidposes from obstacles, acting as a collision avoidancefeature. Moreover, force eccentricity originates repul-sive rotating torques, which readapt poses orientationmaximizing clearance over the obstacles.

The implementation of this optimization method is de-

!"

Repulsive Forces and Torques

Row query path

Elastic Forces and

Torsional Torques

Torsional spring

Elastic spring

qI

qG

Fig. 8. Optimization based on the rigid body dynamics: the elastic forcesand torsional torques help to smooth and shorten the path while the repulsiveforces and torques maximize clearance from obstacles.

scribed as follows. Let j = {1, . . . , J} be the index of theconsecutive vehicle poses composing the query path, eachdefined by the configuration vector, qj = [pj θj ]

T , wherepj and θj denote the position and the orientation of the poseqj relative to a fixed reference frame, respectively. It is statedthat q1 = qI and qJ = qG.

The elastic force, Fe, and the torsional torque, τt, evalu-ated for the vehicle pose at qj are:

Fe(qj) = Ke · [(pj+1 − pj) + (pj−1 − pj)] (15)

τt(qj) = Kt · [(θj+1 − θj) + (θj−1 − θj)], (16)

where Ke, the elasticity gain and, Kt, the torsional gain,control the elastic and torsional avoidance behavior on thepath deformation, respectively.

The evaluation of the external efforts due to obstacle prox-imity relies on a heuristic-based collision detector module,which is capable of determining the set of i-nearest ObstaclePoints (OPs) to each sampled pose qj . The overall procedureto handle the evaluation of the repulsive forces and torquescan be described as follows:

1) Let i = {1, . . . , I} denote the index of the i-th OPrelative to a specific pose qj . Let uj,i be the vector

uj,i = Vj,i −Oj,i, (17)

taken from each OP, Oj,i, and the corresponding vehi-cle nearest point, Vj,i.

2) To improve clearance during path deformation, distance-dependent repulsive forces are defined, where each pairof points (Oj,i, Vj,i) determines a repulsive contri-bution. For a specific vehicle pose, qj , the repulsivecontributions are defined as

rj,i =uj,i

‖uj,i‖· f(‖uj,i‖) (18)

where,

f(‖uj,i‖) = max(0, Fmax −Fmaxdmax

· ‖uj,i‖). (19)

In (19), a maximum allowable magnitude, Fmax, isassigned to avoid outsized values in the close vicinity

of the obstacles. dmax denotes the distance up to whichthe repulsive force is applied.

3) For each pose qj , the total repulsive force is defined as

Fr(qj) =

I∑i=1

rj,i. (20)

The net repulsive torque around the j-th pose CoG isdefined as

τr(qj) =

I∑i=1

rj,i × ej,i. (21)

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

Ftotal(qj) = Fr(qj) + Fr(qj). (22)

τtotal(qj) = τt(qj) + τr(qj). (23)

Once determined the efforts acting on each pose, the ensuedmotion is evaluated through the principles of rigid bodyyielding

aj =Ftotal(qj)

m−Kdvj (24)

αj =τtotal(qj)

Iz−Kdωj , (25)

which provide the linear and angular accelerations for aspecific pose qj . The last term in the right hand side of(24) and (25) represent damping effects introduced to reducethe oscillatory motion during path deformation. They arecontrolled through KD, herein set equally for both thetranslational and the rotational motion components.

From a starting configuration in the query path, qj , thispose is updated iteratively according to the following set ofequations.

v = v0 + at (26)

ω = ω0 + αt (27)

where v0 and ω0 are the initial linear and angular velocities.

p = p0 + v0t+1

2at2 (28)

θ = θ0 + ω0t+1

2αt2, (29)

where p0 and θ0 are the initial position and orientation ofthe rigid body’s CoG. The stopping criteria is defined bysetting a maximum number of iterations.

C. Trajectory Evaluation

The output of the path optimization module is a collisionfree path suitable for execution. To achieve a realistic plan,it is necessary to determine how the rhombic vehicle shouldmove along the path satisfying dynamic constraints, i.e., theoptimized paths should be parameterized in terms of veloc-ities. The purpose of this section is to present a trajectoryevaluation method that is able to handle vehicle dynamicconstraints and also incorporate safety requirements. Thismethod integrates the last stage of the refinement planningstrategy followed in this chapter and suits any of the two path

planning and optimization approaches previously presented.The input data available for the trajectory evaluation module,are the reference path points pj , with j = {1, ..., J} and Jbeing the number of reference path points. The velocity ineach reference point, is updated according to the acceleration,which, to simplify the computation is kept fixed during thetime interval (∆tj), which is variable and not given as input.

vj+1 = vj + ∆tjaj , (30)

The position of the reference points is updated by theaverage velocity during the time interval (∆tj). Using theTrapezoidal and under the constant acceleration assumption,the average velocity during a time period is the average ofits beginning velocity and ending velocity as follows,

pj+1 = pj + ∆tj(vj + vj+1

2). (31)

By substituting Equation 30 into Equation 31, one definesthe updated position in terms of the previous step position,velocity, and acceleration as,

pj+1 = pj + ∆tjvj +∆t2j

2aj . (32)

At this point, we have all the math necessary to elaboratea routine for generating velocity profiles for the referencepoints. The routine proposed is the following:

1) To include obstacle safety requirements, the speed ateach reference point, here denoted as (sj), is firstlydefined as the linear function of the minimum distance(dj) from the vehicle to the obstacles. A maximumallowable speed (smax) is set to this profile, so tointegrate kinematic constraints;

sj =

smin if dj < dsafα(d) if dsaf < dj < dthsmax if dj > dth

(33)

2) Each reference point is then updated with the corre-sponding velocity vector, which can be easily obtainedthrough the following equation,

vj = sj〈cosφj , sinφj〉; (34)

3) Consider zero initial condition for the velocity an ac-celeration: v0 = 0 and a0 = 0;

4) For j = {2, ..., J−1}, the following steps are iterativelyperformed

a) Evaluate ∆tj through Equation 31;b) With ∆tj from (a), evaluate aj using Equation 30;c) If aj ∈ [amin, amax] (dynamical feasible transition)

proceed to the next iteration from the step (a), other-wise, follow step (d);

d) Using the correct feasible acceleration (amin oramax) re-evaluate ∆tj using Equation 32;

e) Update the new velocity, vj+1, using ∆tj from (d)and Equation 30;

5) The evaluated profile is feasible if vJ = 0. Otherwisethe steps (a) to (e) shall be repeated using a reverse-time approach, i.e., j = {J, ..., 1} and imposing vN tothe zero vector;

With this routine the generated profile may violate thesafety requirements integrated in the speed profile s. Thisis an undesirable situation that can lead the vehicle toprohibitive velocities with respect to the obstacle proximity.To sidestep this issue, it is proposed the following greedysolution; Suppose the existence of D violating points, withD ⊂ J . For each pd ∈ D the routine above describedis repeated in the forward sense from the pd to pJ (j ={d, ..., J}) and in the backward sense from pd to p1 (j ={d, ..., 1}) and assuming that vd = sd〈cosφj , sinφj〉 and vgiven being the last obtained velocity profile. The process isrepeated till D = ∅.

V. MOTION PLANNING UNDER DIFFERENTIALCONSTRAINTS

To address the problem of planning under differentialconstraints, alternatively referred to as trajectory planning,the paper proposes the development and use of RRT-basedplanners which were originally developed for handling dif-ferential constraints. As reported in [Dan11], the algorithms1 and 2 are easily extended to the nonholonomic planninginstance. Despite their robustness, these RRT-based plannersgenerate low quality solutions due to its random nature andusually perform poorly in constrained environments. For thisreason, in [Dan11] it is explored the implementation of boththe RRT-Blossom [KP06], a novel variation of RRT thatis designed to perform well in highly constrained environ-ments, and the Transition-Based RRT (diminished to RRT-T) [JCS08], which allows to conduct a search on continuouscost spaces. This study led to a novel RRT variant, referred toas T-B -RRT which combines both the ideas from the RRT- Blossom and T-RRT. The details of the T-B - RRT aregiven in [Dan11]. Figure 9 provides an example of a searchfor a smooth path, allowing to gain some understanding onthe capacities of this this new planner. Further results arediscussed in [Dan11].

Fig. 9. Top: T-B - RRT searching for a smooth path on a unobstructedenvironment with T-B- - RRT.

VI. EXPERIMENTS AND SIMULATION RESULTS

The set of conducted experiments and provided illustra-tions were developed in the MATLAB platform. In particular,it was used a Matlab-based software tool, the TrajectoryEvaluator and Simulator (TES), which was developed un-der the grant F4E-2008-GRT-016 (MS-RH) [RVR+10] andincludes the planners developed in Section IV and V aswell the simulation models defined in Section II. Since themain concern of the paper is to investigate the ability ofthe developed planners to generate feasible and optimizedtrajectories under the scope of the RH operations in the

ITER, the defined experimental setup gives preference toITER simulated environments.

To evaluate the proficiency of the proposed planningtechniques, three scenarios are presented entailing differentmotions queries, which are depicted in Figure 10 and de-scribed as follows:

• Scenario I: given the starting starting configuration atthe lift in the TB, the vehicle shall dock inside a specificVacuum Vessel Port Cell;

• Scenario II: the rhombic vehicle leaving the lift fromthe TB, must dock in a specified DP with a specificorientation inside the HCB;

• Scenario III: the queried motion consists in a simpleand general operation between two arbitrary configura-tions on the illustrative map.

Scenario I Scenario II Scenario III

Fig. 10. Motion query for: a nominal operation in the TB (left), a dockingprocedure in the HCB (center) and for a general motion between twoarbitrary configurations (right).

A. Refinement Strategy

The experiments are partitioned according to each plan-ning stage (geometric path evaluation, path optimization andtrajectory evaluation) sustaining a comparison between thetwo approaches.

B. Geometric Path Evaluation

The evaluation of the geometric path in the combinatorialapproach does not raise any computational issues due tocomplete nature and computational efficiency of both theCDT and A∗ methods. Therefore no references are givento timing results. Figure 11-right illustrates the triangularCD yielded by the CDT, the shortest triangle cell sequencereturned by A∗ (on the top) and the respective geometricpaths (on the bottom). At this stage the generation ofcollision-free paths is not guaranteed.

The second approach relies in a randomized sampled-based approach, the RRT method, which brings the computa-tional efficiency into play. The experiments performed in thisstudy show that the dual-tree presents a clear advantage overthe simpler single-tree in all the three analyzed scenarios.Figure 11-right shows a possible geometric path retuned bythe dual-tree RRT planner for the motion query of scenarioI. A feature that stands out, is the collision-free status of theobtained geometric path, which contrasts with the results inFigure 11-left.

C. Path Optimization

Both the combinatorial and randomized-based approachesshown to be insufficient to guarantee the generation of

Scenario II Scenario III Scenario I

Fig. 11. Left: Triangular CD (in blue) and shortest cell sequence (inmagenta) evaluated by the A∗ algorithm for Scenario II and III. Right:geometric path yielded by the dual-tree RRT planner for the motion queryof scenario I.

feasible paths that can be followed by the vehicle. Theprovided techniques are good at delivering an initial solution,but they must be coupled with more sophisticated methods asto return higher quality solutions. For this purpose, two pathoptimization methods were developed on a path deformationbasis. To assess the performances of each path optimizationmethod a common data framework was defined in [Dan11]that measures path clearance, smoothness and length.

The optimization process, using the two optimizationmethods, is well represented in Figure 12. The initial roughgeometric path, which may or not be collision-free (depend-ing on the used approach), is continuously deformed (centercolumn) through the action of forces. For the specific caseof scenario I, that refers to the combinatorial approach, thepath deformation was represented through the evolution ofthe path points. This prompts the fact that the deformationacts upon the path points and not on vehicle poses. In itsturn, in the randomized approach, the paths are deformedexplicitly. Notwithstanding, both iterative processes lead toa shorter, smoother and safer path, as illustrated in Figure12 -right.

Geometric path Path optimization Optimized Path

Sce

nario

I S

cena

rio II

Fig. 12. Path optimization with: elastic bands (top) and with the approachbased on the rigid body dynamics (bottom).

D. Trajectory Planning

The final step of the above referred refinement approachestransforms the optimized paths on a trajectory determiningthe velocity profile to be satisfied by the rhombic vehicle

along the path. As referred in Section IV-C this would becarried out in agreement with both obstacle clearance andvehicle dynamic constraints.

Figure 13, nicely illustrates the speed evolution alongan optimized path that was generated using the thecombinatorial-based approach. Apart from the starting ac-celeration and final deceleration phases that inherit slowmotions, it can be seen that the speed is also reduced atthe exit of the lift and entrance of the VVPC due to thespace confinement. This journey is performed by the vehiclein 227 seconds (∼ 3, 7 minutes).

Fig. 13. Speed vehicle map for a journey to the VVPC.

VII. CONCLUSIONS AND FUTURE WORK

The paper addresses the motion planning problem statedfor an autonomous rhombic vehicle that will operate in thebuildings of the ITER.

The two approaches developed under the refinement plan-ning strategy allow to bridge the gap between planning andexecution phases providing, in a off-line instance, optimizedtrajectories that will certainly ease the task of real-timecollision avoidance systems. The main shortcoming of theseapproaches is the fact that they are closely related to projectrequirements. For instance, the second approach, which isstrongly linked to the rhombic capabilities and thereforeits extension to other configurations is compromised. Theformer approach is suitable for other configurations that toouse line guidance approaches, even though the satisfactionof the kinematic constraints must be assessed.

For future developments, the author recommends or fore-sees the necessity of including a dynamic tuning feature thatis capable of optimizing the gains associated with the defor-mation process (Ke, Kr) for the combinatorial approach and(Ke, Kt) for the randomized one, which are required to betuned on time depending on the map and vehicle geometryused. Also, the author proposes the development of a pruningtechnique based on the combination of the RRT with theA∗ algorithm to avoid unnecessary maneuvers during thegeometric path evaluation, in the randomized approach.

In its turn and according to the provided data in [Dan11],the trajectory planning strategy, through the use of RRT-based planners reveal some limitations on handling optimizedtrajectories for the vehicle, ate least in the confined scenarios

presented in the paper. Therefore future perspectives aredirected to a refinement-like strategy [LBL02].

Considering all what was stated above, it can be concludedthat the decoupled and refinement approach is more appeal-ing because it provides much more reliable results in whatconcerns the quality of the solutions and therefore it must beadopted as the main reference for future incoming activitiesconcerning the evaluation of optimized trajectories for therhombic vehicle. Notwithstanding, the results obtained withthe RRTs in the trajectory planning strategy shall not beignored.

In what concerns the mathematical modeling part, a furtherstep involves the accomplishment of effective simulationexperiments integrating the dynamic model of the rhombicvehicle. These experiments will provide a valuable outcometo understand how dynamic phenomena affect the vehicleperformances.

REFERENCES[ACB91] B. D’Andrea-Novel , G.Campion, and G Bastin, “Modelling

and control of nonholonomic Wheeled Mobile Robots”, inProceedings Of the 1991 Institute of Electrical and ElectronicsEngineers (IEEE) International Conference on Robotics andAutomation (ICRA), pp. 1130-1135, 1991.

[Dan11] D.Fonte, “Motion Planning for a Rhombic-Like Vehicle Oper-ating in ITER Scenarios”, Master’s thesis report , 2011.

[HNR68] P. E. Hart, N.J. Nilsson, B. Raphael, “A Formal Basis forthe Heuristic Determination of Minimum Cost Paths”, IEEETransactions on Systems Science and Cybernetics, vol.4, pp.100 - 107, Jul 1968.

[JCS08] L. Jaillet, J. Cortes and T. Simeon, “Transition-based RRTfor Path Planning in Continuous Cost Space”, 2008 IEEE/RSJInternational Conference on Intelligent Robots and Systems,Nice, France, Sept., pp. 22-26, 2008.

[KP06] M. Kaisiak, M. van de Panne, “RRT-blossom: RRT with a localflood-fill behavior”, In proceedings of International Conferenceon Robotics & Automation, ICRA 2006, Orlando, FL, pp. 1237- 1242, 2006.

[Lat91] J-C Latombe, Robot Motion Planning, Kluwer Academic Pub-lishers, Boston, MA; 1991.

[Lav03] S.M. LaValle, Planning Algorithms, Cambridge UniversityPress; 2003.

[Lav98] S.M. LaValle, “Rapidly-exploring Random Trees: A New Toolfor Path Planning”,Technical Report (TR) 98-11, ComputerScience Dept., Iowa State University, 1998.

[LBL02] F. Lamiraux, D. Bonnafous, and O. Lefebvre, “Reactive tra-jectory deformation for nonholonomic systems: application tomobile robots”, In Proceedings of the International Conferenceon Robotics and Automation, ICRA ’02, pp. 3099 - 3104,Washington, USA, 2002.

[QK93a] S. Quinlan and O. Khatib, “Towards real-time execution of mo-tion tasks”, In R. Chatila and G. Hirzinger, editors, ExperimentalRobotics, Springer-Verlag, 1993.

[QK93b] S. Quinlan, and O. Khatib, “Elastic bands: Connecting pathplanning and control”, In Proceedings of the IEEE InternationalConference on Robotics and Automation, vol. 2, pp. 802-807,Atlanta (United States of America (USA)), 1993.

[RLAF97] M.I. Ribeiro, P. Lima, P. Aparıcio, R. Ferreira, “ConceptualStudy on Flexible Guidance and Navigation for ITER Re-mote Handling Transport Casks”, In Proceeding of the 17thIEEE/NPSS Symposium on Fusion Engineering, San Diego(USA), pp. 969-972, 2007.

[RVR+10] M.I. Ribeiro, A. Vale, P. Ruibanys, V. Queral, D. Fonte, F.Valente, C.Reig, E. Gazeau, P. Lima, “Activities related to thedevelopment of an ATS prototype and Cask Transfer SystemVirtual Mock-up”- Final Report – F4E-2008-GRT-016 (MS-RH), Oct. 201