A uniﬁed motion planning method for a multifunctional underwater robot
Koichiro Shiraishi and Hajime KimuraDept. of Maritime Engineering
Graduate School of Engineering, Kyushu University
744 Motooka, Nishi-ku,Fukuoka, 819-0395 Japank-shiraishi@system.nams.kyushu-u.ac.jp and kimura@nams.kyushu-u.ac.jp
Abstract
This paper deals with motion planning for a multifunctional underwater robot which can accomplish
various missions, e.g., swimming, walking and grasping objects. Authors have developed a uniﬁed motion
planning method which can generate motion planning
for a variety of task by a single algorithm. In thismethod, motion planning problems are modeled as ﬁnite horizon Markov decision processes. The optimum
motion planning is obtained by Dynamic Programming, however Dynamic Programming is sometimes
thought to be of limited applicability because of thecurse of dimensionality. To avoid the curse of dimensionality, authors applied a random network as a statetransition network. The explosion of the number ofstates can be suppressed by using the random network.
The eﬀectiveness of the proposed method is demonstrated through numerical simulations of two types oftasks for multifunctional robots. One is a reachingtask, the other is a generating thrust force task.
Keywords: Mutifunctional underwater robot, Fluid
Drag Force, Reaching task, Generating thrust force
task, Dynamic Programming, Random network

1

Introduction

The ﬁeld of underwater robotics is currently enjoying a period of high interest and growth. Applications
for such robots include exploration of deep sea environment, cable maintenance, monitoring of subsurface
structures, and biological surveys [1]. Authors havebeen developing a multifunctional underwater robot
which can accomplish various missions, e.g., swimming, walking and grasping objects [2]. The robot isequipped with a redundant degree of freedom manipulator. It has two excellent features; One is robustness
for troubles, the other is multifunction, that is, it canuse not only as an arm, but also as a ﬁnger for gripping

objects, or a ﬁn for swimming.
It is necessary for a robot to generate motion planning according to various missions. The problem isthat we must develop the corresponding motion planning algorithms for each individual task. Authors havedeveloped a uniﬁed motion planning method that cangenerate motion planning for a variety of task by a single algorithm. In the method, motion planning problems are modeled as ﬁnite horizon Markov decision
processes. The optimum motion planning can be acquired by using Dynamic Programming.
This paper presents a new approach which has ability to avoid curse of dimensionality. The number ofstate increase exponentially with the number of statevariables is called the curse of dimensionality. The
conventional Dynamic Programming is to discretize
state space with full grid points. Due to the exponential growth in the full grid discretization as the numberof state variables increases, Dynamic Programming is
still commonly considered to be computationally intractable. The proposed method generates a random
state transition network by discretizing the state spaceat random. The random network can prevent fromexponentially increasing of the number of states. The
optimum motion planning can be acquired by using
the Dynamic Programming in the random network.
The eﬀectiveness of the proposed method is demonstrated through numerical simulations of two types oftasks for multifunctional robots. One is a reachingtask that is composed of a manipulator posture planning, so that the robot keeps to a minimum of energy
consumption [3]. The other is a generating thrust force
task that is driving the robot forward by a ﬂuid dragforce that acts on a manipulator. The experimental
results show that the proposed method could generatereasonable motion planning at two problems of motion
planning by a single algorithm. The proposed method
used a random network could apply to a high dimension problem, and enabled the computation time to be
shortened.

2

Motion Planning Problem

ulator posture, which is vector composed of angles ofthe links, given by

In this paper, authors dealt with two motion planning problems of a multifunctional underwater robot.
The one is a reaching task which is a path planningtask for a robotic manipulator. The other is a generating thrust force task for a multifunctional underwater
robot. The reaching task is to obtain an optimum
sequence of postures in ﬂuid. In the reaching task,
the optimum planning minimizes energy consumption
caused by the ﬂuid drag force. Authors consider amovable underwater robot equipped with a manipulator in the generating thrust force task. This robot
generates ﬂuid drag force to move in front by moving
the manipulator. In this task, the objective function
is to maximize the advancing distance.
Uniform Flow

End Effecter

Fluid Drag

Thrust Force

Manipulator
TargetS = (θ1 , θ2 , · · · , θi , · · · , θn ).

A point of the nth link’s tip becomes the position of
the end eﬀecter. The point is given by
xT ip =

2.1

Coordinate System of a Manipulator

Figure 2 shows the coordinate system and parameters in the manipulator. x and y are horizontal, vertical directions, respectively. li is the length of the ithlink θi is the angle of the ith link. The shape of thelinks are assumed to be columns. Joints are assumed
to be small enough compared with the links, so thatﬂuid drag forces which act on joints are able to beomitted in the calculation. S is deﬁned as a manip-n∑yT ip =

n∑li cos θi .

(2)

i=1

Coordinates of other links can be provided by using
the equation (2). The notation P is a sequence of manipulator postures become target posture from initial
posture, It is given by
P = (Sinit , S2 , · · · , Sj , · · · , Send ),(3)

where Sinit is the initial posture where the manipulator begins to move is an initial posture, Send is thetarget posture which the end eﬀecter reaches target
coordinates, j is the number of posture changes, and
Sj is jth manipulator posture.

The Fluid Drag ForceThe Fluid drag force is given by
1π
duf = Cd ρDu|u| + Cm ρ D2 ,
2
4
dt

(4)

where D is a diameter of the column, Cd is a dragcoeﬃcient, Cm is an added mass coeﬃcient, u is avelocity of a link through the ﬂuid. ρ is a density of
the ﬂuid. Since the velocity of the link is small enough,the second term at the right of the equation (4) canbe omitted. The ﬂuid drag force acting on each link isshown as follows by the use of the equation (4)yθ3li sin θi ,

i=1

2.2
Figure 1: Reaching task and Generating thrust force
task

(1)

Tip

Fx,i = Fx,i+1 + fi cos θi(5)Fy,i = Fy,i+1 − fi sin θi(6)

Joint_3

θ2

l3

Joint_2

Mi = Mi+1 + Ti + Fx,i+1 li cos θi − Fy,i+1 li sin θi , (7)Link_3l2

θ1

Link_2where fi and Ti are given by

Joint_1

l1
Link_1

x

∫ li
fi

0

1
Cd ρDi ui |ui |dri ,2

0

1
Cd ρDi ui |ui |ri dri .
2

=
∫ li

Figure 2: Conﬁguration of three-link manipulator

Ti

=

y

2.4

u

θi r iθi+1

In this section, authors describe the motion planning problem of generating thrust force for a movable
underwater robot. The robot is able to move in front
by using the ﬂuid drag force that acts on the manipulator. In this paper, it is assumed that the body of
the robot is constrained by the rail. For a preliminary
experiment, the one dimentional motion is considered.
For simplicity, the eﬀect of the added mass which isconcerned with the ﬂuid is assumed to be negligible.
Consider that ﬂuid drag forces that acts on main bodyand each links. The motion equation of the body isgiven byn∑
M ẍ = Fbody +Fx,i ,
(11)

θi
θ

x

Mi+1

θi-1

Ti

fi

Fi+1

Figure 3: Forces acting on a link

2.3

A Generating Thrust Force Task

A Reaching Task

i=1

Multifunctional underwater robot is operated under the subsea environment. It is necessary to consider an inﬂuence of ﬂuid drag force caused by sea current. Thus, authors consider that a motion planningproblem which minimizes energy consumption causedby the ﬂuid drag force. The energy is determined bytorque which acts on the joints and the angles of thelinks moved in a period. In the motion planning problem, the cost function is formulated by using the consumed energy. When a manipulator posture changes
from SA to SB , the cost function is shown approximately in the following,
Cost(Sa , Sb ) =n ∫ θS∑b,ii=1

Mi dθi + ∆E,

(8)

θSa,i

where n is the number of the links, θSi is the angle ofthe ith link in the manipulator posture S, ∆E denotes
energy consumption caused by a mechanical friction.
A total cost is provided by the use of the equation (8)as|P |−1

T otal Cost(P ) =∑Cost(Si , Si+1 ).

(9)

i=1

In the reaching task, the motion planning problem isformulated as an optimization problem of minimizing
a total cost of the equation (9).
Optimum P lanning = min T otal Cost(P )P ∈PAll(10)

where PAll denotes sets of all executable motion planning in the motion planning problem. In this paper,
the positive energy obtained from the current is notconsidered.

where M is a mass of the movable underwater robot.
Fbody denotes the ﬂuid drag force that acts on the
body of the robot except for the force on the links.
Fx,i denotes the ﬂuid drag force of x axial component
that acts on the ith link. n is a number of links of themanipulator. In this problem, the optimum planning
is a motion to maximize advancement distance of therobot.

3

A Uniﬁed Motion Planning Method

The motion planning problem of the multifunctional
underwater robots is modeled as ﬁnite horizon Markov
decision processes. Optimum motion planning for various tasks is obtained by using Dynamic Programming.

3.1

Markov Decision Processes

State transitions of the robot is represented as a
network showing Fig.4.
In the network, a node denotes a manipulator posture. A direction of an arrow denotes a direction of
the posture change. f (si , si+1 ) is an objective function, when the robot posture changes from si to si+1 .
In reaching task, an objective function is consumed energy caused by ﬂuid drag force. On the other hand, anobjective function is an advancement distance in generating thrust force task. Thus, the motion planningproblem is considered as a graph search problem.
Assuming that Markov property is approved, themotion planning problem is considered as a Markov
decision process. Markov Decision Processes framework for planning is rich in capturing the essence of
purposeful activity in various situations. A Markov
decision process is deﬁned by its state and action sets

can be obtained by using Dynamic Programming withthis network.

f(2,3)

f(1,2)

f(3,2)

f(2,1)
state2f(1,0)

f(0,3)

state1f(6,1)

f(3,0)
f(3,4)

f(0,1)
f(1,6)

4state3f(4,3)

f(0,4)

f(6,0)
state0f(4,0)

f(0,6)

f(4,5)

f(6,5)
state6f(5,4)

f(5,6)

Experiment and Results

In order to conﬁrm the validity of proposed method,
several experiments of two tasks for the multifunctional underwater robots were made. One is the reaching task, the other is the generating thrust force task.
Figure 5 shows settings of the multifunctional underwater robot in two tasks.

state4

Initial Posturestate5Initial Posture

Figure 4: State transition network
Sea Current

and by the one-step dynamics of the environment [4][5]. A state s is deﬁned as a manipulator posture. An
action a is motion of each link in the posture change.
A reward R(s′ |s, a) is a value of the objective functionin the state transition. A value function V (s) denotes
the expected total reward starting at state s according to the strategy. In Markov decision process, theBellman equation is shown by∑V ∗ (s) = maxp(s′ |s, a) (R(s′ |s, a) + γV ∗ (s′ )) , (12)
a∈A

s′ ∈S

where S is state sets, A is action sets, and γ is a
discount factor. The optimum policy is obtained bysolving the equation (12) by using the standard Dynamic Programming. The policy is the optimum motion planning for various tasks In this paper, a standard value iteration method is used for solving the
Bellman equation.

3.2

Discretization of State SpaceIn this section, authors describe a random network
used as a state transition network. State space should
be discretized in standard Dynamic Programming. In
conventional method, the state space is discretized likea lattice. However, this method is suﬀered from thecurse of dimensionality. The curse of curse of dimensionality is a phenomenon that a number of states increases in exponential. A random network which discretizes the state space at random, is developed as amethod to avoid the curse of dimensionality [6] [7]. In
this paper, the state transition network is produced by
the random network. The optimum motion planning

Target Posture

Figure 5: Settings of two Experiments

4.1

Experiment 1: Reaching Task

The aim of Experiment 1 is to verify whether the
proposed method could obtain the motion planningminimizing the consumed energy by ﬂuid drag. In
the experiment, the 4 D.O.F. manipulator was used.
The parameter of the experiment is set as follows:
li = 0.7[m], Di = 0.2[m], Cd = 1.17, ρ = 1.023, |u| =2.0[m/s], and θlimit = ±1.571[rad], where θlimit is theangle where the links of the manipulator can rotate.
In order to compare results, three motion planningwere used: Planning 1 is to reach a target posturein the shortest time, Planning 2 is generated by theuniﬁed motion planning with the lattice network, andPlanning 3 is generated by the uniﬁed motion planning with the random network. The lattice network
was provided by discretization of the state space likea lattice. Each dimension of the state space is divided evenly into 11 parts. The number of nodes is
114 = 14641, the number of links per a node is 80 in
the lattice network. On the other hand, the random
network was provided by discretization of the state
space at random. The number of nodes is 8000, thenumber of links per a node is 50 in the random network. The simulation is executed 10 times and theperformances are evaluated by the average of the results.
Figure 6, 7, and 8 show the motion planning obtained by simulation of the reaching task. All planningcould obtain the motion to reach from the initial posture to the target posture. Table 1 show the cost andthe computation time on experiment 1. Planning 2 has
the smallest cost among the three motions, although
the computation time is longest. The computation
time of Planning 3 is a half compared with Planning
2. Planning 3 is the most eﬀective solution, considering two points of the computation time and the cost.
Authors could notice that the motion of the best solution shown in Figure 7 or Figure 8 takes advantageof the sea current.

Figure 8: Motion obtained by proposed method withrandom network (Planning 3)

Table 1: Simulation results of experiment 1

Planning 1
Planning 2
Planning 3

Cost[J]

Computation Time[s]0.93044 ± 00.00073 ± 00.01326 ± 0.0088

178.0 ± 0661.3 ± 0344.5 ± 20.5

Figure 6: Motion to reach in the shortest time (Planning 1)

force motion. In the experiment, the movable underwater robot equipped with the 3 D.O.F manipulator
is used. The parameter of the experiment is set as
follows : li = 0.5[m], Di = 0.2[m], Cd = 1.17, M =5.0[kg], ρ = 1.023, θlimit = ±1.047[rad], and θ̇ = 0.034[rad/s], where θ̇ is the velocity of the links of the manipulator. The height and the width of the body is
0.2[m] and 0.3[m]. In the lattice network, the number of nodes is 113 = 1331, the number of links pera node is 26. In the random network, the number ofnodes is 600, the number of links per a node is 50. In
order to compare results, two motion planning wereused : Planning 4 is the motion planning generated
by the uniﬁed motion planning with a lattice network
and Planning 5 is the motion planning generated by
the uniﬁed motion planning with a random network.
Figure 10 and 11 show the motion planning obtained by simulation of the generating thrust force
task. In the both motion the motion of the manipulator is a ﬂutter kick for swimming. Table 2 show
the average velocity and the computation time on experiment 2. Planning 4 is superior to Planning 5 in
the average velocity. The computation time of Planning 5 is a half compared with Planning 4. Planning
5 is the most eﬀective solution, considering two pointsof the average velocity and the computation time.
Table 2: Simulation results of experiment 2

Figure 7: Motion obtained by proposed method withlattice network (Planning 2)

Planning 4
Planning 5

5
4.2

Experiment 2: Generating Thrust Force
Task

The aim of the experiment is to verify whether the
proposed method could obtain the generating thrust

Average velocity [m/s]Computation Time [s]0.0271 ± 0
0.0235 ± 0.0052

510 ± 0248 ± 34.5

Summary and Conclusions

In this paper, authors developed the uniﬁed motion
planning for multifunctional underwater robots. The
motion planning problems are modeled as Markov decision processes and optimum motion planning is ob-[m]random networks approach has the variance of the solution. The method is an algorithm for ﬁnding thenearest local minimum of a function of variables which
presupposes that the gradient of the function can becomputed. The locations of the nodes in the solution
are modiﬁed by the conjugate gradient method, so thatthe quality of the solution is improved. Therefore, thisapproach could reduce the variance of the solution in
the random network.

3
2.5

Planning 42
1.5
1Planning 5
0.5
0
0

10

20

30

40

50

60

70

80

90

100

[s]

Acknowledgements
Figure 9: Time series graph for x coordinate value ofa movable underwater robot

This work partly was supported by the Sasakawa
Scientiﬁc Research Grant from The Japan Science Society.

References

Figure 10: Motion obtained by proposed method withlattice network (Planning 4)[1] G. Antonelli, Underwater Robots: Motion andForce Control of Vehicle-Manipulator Systems,
Springer, 2003.
[2] K. Shiraishi and H. Kimura, “On a Uniﬁed Motion Planning for a Multifunctional Underwater
Robot,” Proceedings of the 26th Annual Conference of the Robotics Society of Japan, (inJapanese) 2008.

Figure 11: Motion obtained by proposed method withrandom network (Planning 5)

tained by standard Dynamic Programming. The proposed method can generate motion planning for a variety of tasks by a single algorithm. In addition, theproposed method used random networks enabled thecomputation time to be shortened. The method could
avoid the curse of dimensionality, since the number ofnodes is independent on the number of state variables.
The validity of the method was conﬁrmed through two
experiments.
The drawback of the random network approach isthat the quality of the best solution has large variancebecause of the random location of the state node. Thatis, the solution is depending on the initial design ofthe random network. It is necessary to develop the
method to suppress the variance.
In the future work, authors will utilize the conjugate gradient method to solve the problem that the[3] S. Ishibashi, M. Ito, and E. Shimizu, “Autonomous Motion Planning for a Manipulator
Equipped on AUV in a Workspace Divided into
Cubes,” Proceedings of the 13th International
Oﬀshore and Polar Engineering Conference, pp.
231-238, 2003.
[4] D. P. Bertsekas and J. N. Tsitsiklis, NeuroDynamic Programming, Athena Scientiﬁc, 1996.
[5] Abhijit Gosavi, Simulation-Based Optimization,
Kluwer Academic Publishers, 2003.
[6] C. G. Atkeson and B. J. Stephens, “Random Sampling of States in Dynamic Programming,” IEEE
Transactions on Systems, Man, and Cybernetics,
Vol. 38, pp. 924-929, 2008.
[7] S. M. LaValle, M. S. Branicky, and S. R. Lindemann, “On the Relationship Between ClassicalGrid Search and Probabilistic Roadmaps,” International Journal of Robotics Research, Vol. 23,
pp. 673-692, 2004.