Content uploaded by Ashkan Jasour
Author content
All content in this area was uploaded by Ashkan Jasour on Feb 28, 2014
Content may be subject to copyright.
Path Tracking and Obstacle Avoidance for
Redundant Robotic Arms Using Fuzzy NMPC
Ashkan M. Jasour, Mohammad Farrokhi
IRAN University of Science and Technology, TEHRAN, IRAN, amjasour@ee.iust.ac.ir, farrokhi@iust.ac.ir
Abstract— This paper presents a Nonlinear Model Predictive
Control (NMPC) for redundant robotic arms. Using NMPC, the
endeffector of robotic arm tracks a predefined geometry path in
the Cartesian space in such a way that no collision with obstacles
in the workspace and no singular configurations for robot
occurs. Nonlinear dynamic of the robot including actuators
dynamic is also considered. Moreover, the online tuning of the
weights in NMPC is performed using the fuzzy logic. The
proposed method automatically adjusts the weights in cost
function in order to obtain good performance. Numerical
simulations of a 4DOF redundant spatial manipulator actuated
by DC servomotors shows effectiveness of the proposed method.
I. INTRODUCTION
oday, robotic manipulators are increasingly used in many
tasks such as industry, medicine and space. One of the
main reasons for the development of manipulator robots is to
replace human in doing long and repetitive operations and
unhealthy tasks. In particular, these robots are needed to track
a predefined path in such a way that no collision with
obstacles in the environment occurs. High degrees of freedom
for redundant manipulators lead to an infinity of possible joint
positions for the same pose of the endeffector. Hence, for a
given endeffector path in the Cartesian space, the robot can
track it in many different configurations, among these, the
collision free and singular free tracking must be selected.
Finding feasible path for joints of redundant manipulators for
a given endeffector path is called redundancy resolution [1].
Redundancy resolution and obstacle avoidance are already
considered in papers. With gradient projection technique,
redundancy can be solved considering obstacle avoidance [2].
In taskpriority redundancy resolution technique, the tasks are
performed with the order of priority. Path tracking is given the
first priority and obstacle avoidance or singularity avoidance
is given the second priority [3, 4]. This technique is locally
optimal solution that is suitable for realtime redundancy
control but not for large number of tasks. The generalized
inverse Jacobin technique and extended Jacobin technique,
which are used for redundancy solution, are time consuming
[5, 6, 7]. Optimization techniques, which minimize a cost
function subject to constraints, like endeffector path tracking
and obstacle avoidance, are not suitable for online
applications [4].
In this paper, Nonlinear Model Predictive Control (NMPC)
method is presented for redundancy resolution, considering
obstacles and singularity avoidance. Although Model
Predictive Control (MPC) is not a new control method, works
related to manipulator robots using MPC is limited. Most of
the related works are about joint space control and end
effector coordinating. The linear MPC is used in [8, 9, 10] and
NMPC is used in [11, 12, 13, 14] for joint space control of
manipulators.
In this paper, using NMPC, the input voltages of DC
servomotors of joints are obtained in such a way that the end
effector of a redundant manipulator tracks a given path in the
Cartesian space, considering obstacles and singularity
avoidance. Moreover, using fuzzy logic, an automatic
mechanism for the online tuning of the weights for the path
tracking and obstacle avoidance terms in the cost function is
proposed.
This paper is organized as follows: Section II presents
nonlinear dynamic of 4DOF spatial redundant manipulator
including the actuators dynamic. Section III describes the
nonlinear predictive control. In Section IV, NMPC is
implemented for path tracking and obstacle avoidance of a
4DOF manipulator. Section V presents the proposed modified
NMPC using fuzzy logic. Conclusions are drawn in Section
VI.
II. MANIPULATOR ROBOT DYNAMIC
Schematic diagram of a 4DOF spatial redundant
manipulator robot is shown in Fig. 1.
Fig. 1 Schematic of a 4DOF spatial manipulator
According to DenavitHartenberg parameters [15] of the
shown robot in Table I, the position of the endeffector in
T
2009 American Control Conference
Hyatt Regency Riverfront, St. Louis, MO, USA
June 1012, 2009
WeB20.6
9781424445240/09/$25.00 ©2009 AACC 1353
Cartesian space can be calculated in terms of joint angles as
fallows:
TABLE I
DENAVITHARTENBERG PARAMETERS OF ROBOT FIG.1
Link αo a d θo
1 90 0 0 1
θ
2 0 2
l 0 2
θ
3 0 3
l 0 3
θ
4 0 4
l 0 4
θ
⎥
⎥
⎥
⎦
⎤
⎢
⎢
⎢
⎣
⎡
++
++
++
=
⎥
⎥
⎥
⎦
⎤
⎢
⎢
⎢
⎣
⎡
222332344
2223323441
2223323441
)(
)(
slslsl
clclcls
clclclc
z
y
x (1)
The dynamic model of the robot manipulator can be
obtained using the Lagrangian method as follows [15, 16]:
τθθθθθθ
=+++ )()(),()( GDCM &&&& (2)
where
θ
i is the angle of the ith joint, M)(
θ
∈Rn×n is the
symmetric and positive definite inertia matrix, C),(
θθ
&∈Rn is
the centrifugal and coriolis force vector, G)(
θ
∈Rn is the
gravity vector, D)(
θ
&∈ Rn is the vector for joints friction of the
links,
τ
∈R n is the torque vector of joints, and n is the degree
of freedom, which is equivalent to four for the robot
considered in this paper. The above matrix and vectors are
given in Appendix.
Friction for joint i is as follow [15]:
)sgn()( idiV DDiD
θθ
&& += (3)
where Dv is the coefficient of the viscous friction and Dd is
the coefficient of the dynamic friction.
The dynamics of the armaturecontrolled DC servomotors
that drive the links are expressed in the following form [15]:
mEaaaat
mmmmme
aTe
KiLiRV
BJ
iK
θ
τθθτ
τ
&
&
&&&
++=
++=
=
(4)
Where
τ
e∈Rn is the vector of electromagnetic torque,
KT∈Rn×n is the diagonal matrix of the motor torque constant,
ia∈Rn is the vector of armature currents, Jm∈Rn×n is the
diagonal matrix of the moment inertia, Bm∈Rn×n is the
diagonal matrix of torsional damping coefficients,
n
R
mmm ∈
θθθ
&&& ,, denote the vectors of motor shaft positions,
velocities and acceleration, respectively,
τ
m ∈ Rn is the vector
of load torque, Vt∈Rn is the vector of armature input voltages,
Ra∈Rn×n is the diagonal matrix of armature resistance, La∈Rn×n
is the diagonal matrix of armature inductance and KE∈Rn×n is
the diagonal matrix of the back electromotive force (EMF)
coefficients.
In order to apply the DC servomotors for actuating an n
link robot manipulator, a relationship between the robot joint
and the motorshaft can be represented as:
m
m
r
τ
θ
θ
τ
== (5)
where r∈Rn×n is a diagonal positive definite matrix of the
gear ratios for n joints. According to the fact that the armature
inductance is small and negligible, the Eq. (4) can be
expressed as follow [15]:
t
a
T
mm
a
TE
mmm V
R
K
R
KK
BJ =+++
τθθ
&&& )( (6)
Using Eq. (5) to eliminate
θ
m and
τ
m in Eq. (6) and then
substituting for
τ
from Eq. (2), the governed equation of n
link robot manipulator including actuator dynamics can be
obtained as:
t
a
T
a
TE
mm V
R
rK
DGCr
R
KK
BMrJ =++++++ )()()
(
22
θθ
&&& (7)
According to Eq. (7), the armature input voltages are
considering as control effort. The detailed parameters of the
robot manipulator and DC servomotors are given as Table II
and Table III, respectively.
TABLE II
MANIPULATOR ROBOT PARAMETERS
Link 1 2 3 4
l (m) 1 0.5 0.4 0.3
m (kg) 1 0.5 0.4 0.3
TABLE III
DC SERVO MOTORS PARAMETERS
Motor 1 2 3 4
Ra 6.51 6.51 6.51 6.51
KE 0.7 0.7 0.7 0.7
KT 0.5 0.5 0.5 0.5
Bm 4
1064
−
× 4
1064 −
× 4
1064
−
× 4
1064
−
×
Jm 0.2 0.2 0.2 0.2
R 1:100 1:100 1:10 1:10
Vt 24 24 24 24
III. MODEL PREDICTIVE CONTROL
Unlike classical control schemes, in which the control
actions are taken based on the past output of the system, the
MPC is a modelbased optimal controller, which uses
predictions of the systems output to calculate the control law
[17, 18].
At every sampling time k, based on measurements obtained
at time k, the controller predicts the output of the system over
prediction horizon NP in future using model of the system and
determines the input over the control horizon NC≤ NP, such
that a predefined cost function is minimized.
To incorporate feedback, only the first member of the
obtained input is applied to system until the next sampling
time [17]. Using the new measurement at next sampling time,
the whole procedure of prediction and optimization is repeated.
From the theoretical point of view, the MPC algorithm can
be expressed as follow:
1354
))(min(arg kJu u
= (8)
Such that
maxmin
maxmin
0
)(
)1(
))1(()1(
))(),(()1(
,))())(
)(
ukjkuu
xkjkxx
kjkxhkjky
kjkukjkxfkjkx
NjkNkukjku
xkkx
d
d
CC
≤+≤
≤++≤
++=++
++=++
≥+=+
=
(9)
Where j∈[0 , NP1], x and u are states and input of the
system and the notation a(mn) indicates the value of a at the
instant m predicted at instant n, x0 is the initial condition and
fd and hd are the model of the system used for prediction.
[xmin , xmax] and [umin , umax] stand for the lower and the upper
bound of states and input, respectively. The cost function J is
defined in terms of the predicted and the desired output of the
system over the prediction horizon. MPC schemes that are
based on nonlinear model or consider nonquadratic cost
function and nonlinear constrains on the inputs and states are
called Nonlinear MPC [17]. The optimization problem (8)
must be solved at each sampling time k, yielding a sequence
of optimal control law as )}(),...,({ ** kNkukku C
+. For
optimization, the SQP method is used in this paper [19].
IV. PATH TRACKING AND OBSTACLE AVOIDANCE
USING NMPC
The purpose of the path tracking and obstacle avoidance of
robot manipulators is to obtain a control law such that the end
effector tracks a given geometry path in the Cartesian space
and at the same time collision between the endeffector and
links is avoided. To achieve this purpose, the NMPC is
implemented in this section. Block diagram of NMPC is
shown in Fig. 2. According to the NMPC algorithm, an
appropriate cost function must be determined in order to
obtain the control law. For path tracking, the cost function
must have direct relation with the tracking error between the
endeffector coordination and the given path in the Cartesian
space; on the other hand, for obstacles avoidance the cost
function must have inverse relation with the distance between
the obstacle and the manipulator. One of the proper candidates
for the cost function can be introduced as:
∑
=++
+++=
P
N
jOO
PP kjkDkjkD
R
kjkQDkjkD
J
1)()(
)()( (10)
where DP is the Euclidean distance between the end
effector and the geometry path in the Cartesian space, DO is
the minimum Euclidean distance between the manipulator and
obstacles, notation a(mn) indicates the value of a at the
instant m predicted at instant n and Q ≥ 0, R≥ 0 are the
weighting parameters. According to Eq. (10), the path
tracking term of the cost function is described as distance but
the obstacle avoidance term of the cost function is described
as the inverse of distance. Hence, it is important to notice that
the distance is bounded in the workspace, but the inverse of
the distance is unbounded.
Fig. 2 Block diagram of NMPC
Therefore, combination of these two inconsistent terms as a
cost function is not appropriate for an optimization problem.
To tackle this problem, these two terms are normalized to
[0 1] using a nonlinear map. Hence, the modified cost function
takes the following form:
∑
=−−
−+−
+
−
−
+
−
−
=
P
OO
OO
P
P
PP
N
j
DD
DkjkD
D
D
DkjkD
ee
ee
R
ee
ee
Q
J
1
)(
)(
)()(
maxmin
max
min
max
min
(11)
Where [DPmin , DPmax] and [DOmin , DOmax] are the range of
variations for DP and DO, respectively. According to the
length of manipulator links the value for DPmin and DPmax is 0
and 2.4 meter and the value of DOmin and DOmax is 0 and 1.2
meter. Predictive controller discussed in this paper uses a
nonlinear dynamic model of the manipulator in the
optimization of the cost function. Substituting (
θ
(k +1) 
θ
(k))
/T for
θ
& in the dynamic Eq. (7), a onestep ahead prediction
for joints angle can be expressed as:
))(),(()1( kVkfk td
θ
θ
=
+
(12)
where k is the sampling time and T is the sampling rate,
which is equivalent to 0.5 s in this paper. Using forward
kinematics as Eq. (1), a onestep ahead prediction of the end
effector position can be obtained. However, in the predictive
control, multistep predictions are used over prediction
horizon by applying onestep prediction recursively. Next,
constraints in the optimization problem are considered.
Considering the fact that the amplitude of input voltages is
limited, one of the constraints is:
maxmin ttt VVV
≤
≤
(13)
where Vt min and Vt max stand for the lower and the upper
bound of input voltages of servo DC motors, respectively
(24 and 24 as Table III shows). Besides, considering the fact
that in a singular configuration, for the case of limited velocity
for the endeffector, the joint velocities are infinite. Therefore,
the following constraint must be taken into account:
maxmin
θθθ
&&& ≤≤ (14)
where min
θ
&and max
θ
&are the lower and the upper bound of
the joints velocity, respectively, which are 400 and 400
degree/s, considering the robot and motors parameters. By
Future
Reference
Future Obstacles
Position Predictions
Nonlinear
Dynamic
Model
Past
Control
+

Cost
Function
Constraint
Future
Errors
Sensory
Data
Input
Torques
Optimization
NMPC
Manipulator
Robot
(x,y,z)
1355
incorporating constrains (13) and (14) into the cost function,
the optimization problem can be solved. Solving this
optimization problem at each sampling time, the input
voltages of DC servomotors of joints are obtained in such a
way that the endeffector of a redundant manipulator tracks a
given path in the Cartesian space considering obstacles and
singularity avoidance. Simulation results for a rectangular
path in the Cartesian space with obstacles inside the work
space are shown in figures 3 to 7. In this case, NP = 5, NC = 1,
Q = 10 and R = 0.8. Figures 8 to 9 show the case, where the
obstacle is located on the path. In this case, NP = 5, NC = 1.
However, the best results are obtained when Q = 10 and R =
1.3. That is, when the coordinates of obstacles are changed,
the weights in the cost function must be customized
accordingly.
Fig. 3 Desired and actual endeffector path
Fig. 4 Positions of manipulator joints
Fig. 5 Velocities of manipulator joints
Fig. 6 Input voltages of servo DC motors
Fig. 7 Path following of a 4DOF manipulator with obstacles in the workspace
Fig. 8 Desired and actual endeffector path
Fig. 9 Positions of manipulator joints
V. PATH TRACKING AND OBSTACLE AVOIDANCE USING
FUZZY NMPC
In the previous section, it was observed that for different
paths and different positions of obstacles, the weights Q and R
must be changed and finetuned in order to produce
satisfactory results, thereby following the desired path as
closely as possible and avoiding the obstacles at the same
time. To provide a proper solution to this problem, fuzzy logic
is employed in this paper for the online tuning of these
weights. The proposed fuzzy system uses minimum distance
between the manipulator and the obstacle and the rate of
change of this distance as the inputs. The outputs of the fuzzy
system are the weights Q and R. To design the fuzzy system, a
boundary around each obstacle is considered in such a way
that the control algorithm does not care about obstacles unless
the endeffector or any links of the manipulator enter this
boundary region. Parameters of fuzzy systems are tuned in
such a way that when the manipulator is outside the obstacle
regions, R is equal to zero and when the manipulator is inside
this region, R is increased and Q is decreased adaptively.
Fuzzy rules, membership functions, and fuzzy operations are
shown in figures 10 to 13 and Tables IV and V.
Using the proposed fuzzy system, when the distance
between the manipulator and the obstacle is more than 0.2 m,
R = 0 and Q = 10 and for distances less than 0.2 m, 5 ≤ Q < 10
and 0 < R ≤5. In each step in prediction over prediction
1356
horizon, Q and R are changed by fuzzy system and
consequently each terms of cost function are calculated with
different Q and R.
Fig. 10 Membership functions of distance
Fig. 11 Membership functions of distance variation
Fig. 12 Membership functions of weight Q
Fig. 13 Membership functions of weight R
TABLE IV
FUZZY OPERATIONS
And Implication Aggregation Defuzzification
min prod max lom
TABLE V
FUZZY RULES
O
D O
D
& Positive Zero Negative
Very Far Q=VBig
R=VSmall
Q=VBig
R=VSmall
Q=VBig
R=VSmall
Far Q=VBig
R=VSmall
Q=VBig
R=VSmall
Q=Big
R=Small
Medium Q=Big
R=VSmall
Q=Big
R=Small
Q=Medium
R=Medium
Near Q=Big
R=Small
Q=Medium
R=Big
Q=Small
R=Big
Very Near Q=Medium
R=Medium
Q=VSmall
R=VBig
Q=VSmall
R=VBig
Simulation results of the proposed fuzzy NMPC are shown
in figures 14 to 17. As these figures show, the manipulator can
follow the desired path with better accuracy as compared to
the previous case. Moreover, Fig. 17 shows that the fuzzy
system effectively changes the weighting parameters in the
optimization process for better path following and obstacle
avoidance.
Fig. 14 Desired and actual endeffector path
Fig. 15 Positions of manipulator joints
Fig. 16 Path following of a 4DOF manipulator with obstacles in the
workspace
Fig. 17 Tuning of weights Q and R in cost function
VI. CONCLUSION
To achieve better path tracking and obstacle avoidance for
robotic arms, the NMPC method was proposed in this paper.
For this reason, two terms were introduced in the cost function,
one for the tracking problem and the other one for the obstacle
avoidance. Moreover, by introducing constraints to the joints
velocities, singularities were avoided. Furthermore, online
1357
tuning of the weighting factors in NMPC was achieved using
fuzzy logic. The proposed fuzzy system automatically adjusts
the path tracking and obstacle avoidance weights in the cost
function for obtaining better performance. Using the tuning
mechanism, obstacles do not affect performance of the
manipulator unless they enter the predefined boundary regions
around obstacles. Future works in this area include
considering moving obstacles, robustness of the method
against changes in the system parameters and stability analysis
of closed loop system.
REFERENCES
[1] E. S. Conkur, “Path planning using potential fields for highly
redundant manipulators”, Robotics and Autonomous Systems, Vol. 52,
pp. 209–228, 2005.
[2] J. L. Chen, J. S. Liu, W. C. Lee, T. C. Liang, “Online multicriteria
based collisionfree posture generation of redundant manipulator in
constrained workspace“, Robotica, Vol. 20, pp. 625–636, 2002.
[3] P. Chiacchio, S. Chiaverini, L. Sciavicco, B. Siciliano, ”Closedloop
inverse kinematics schemes for constrained redundant manipulators
with task space augmentation and task priority strategy“, International
Journal of Robotics Research, Vol. 10, pp. 410426, 1991.
[4] Y. Nakamuro, Advanced Robotics Redundancy and Optimization,
AddisonWesley Publishing Co., 1991.
[5] C. L. Boddy and J. D. Taylor,”Whole arm reactive collision avoidance
control of kinematically redundant manipulators“, in Proc. of IEEE Int.
Conf. on Robotics and Automation, Vol. 3, pp. 382–387, Atlanta,
Georgia, USA, May, 1993.
[6] T. Yoshikawa, ”Analysis and control of robot manipulators with
redundancy”, in Proc. of the First International Symposium in Robotic
Research , MIT Press, Cambridge, MA, pp. 439446, 1993.
[7] C. Chevallereau, W. Khalil, ”A new method for the solution of the
inverse kinematics of redundant robots“, in Proc. of IEEE Int. Conf.
on Robotics and Automation, pp. 37–42, Washington DC, USA, 1988.
[8] F. Valle, F. Tadeo, T. Alvarez, ”Predictive control of robotic
manipulators”, in Proc. of IEEE Int. Conf. on Control Applications, pp.
203208, Glasgow, Scotland, UK, September, 2002.
[9] J. K. Kim, M. C. Han,”Adaptive robust optimal predictive control of
robot manipulators”, 30th Annual Conf. of the IEEE Industrial
Electronics Society, Busan, Korea, November, 2004.
[10] A. Vivas, V. Mosquera, ”Predictive functional control of a PUMA
robot”, ACSE Conf., CICC, Cairo, Egypt, December, 2005.
[11] W. Wroblewski, ”Implementation of a model predictive control
algorithm for a 6dof Manipulatorsimulation results”, Fourth Int.
Workshop on Robot Motion and Control, Puszczykowo, Poland, 2004.
[12] R. Hedjar, R. Toumi, P. Boucher, D. Dumur, S. Tebbani, ”Finite
horizon non linear predictive control with integral action of rigid link
manipulators”, IEEE Conference on Control Applications, Int. J. Appl.
Math. Comput. Sci., Vol. 15, No. 4, pp. 101113, août, Canada, 2005.
[13] Ph. Poignet, M. Gautier, “Nonlinear model predictive control of a robot
manipulator”, in Proc. of 6th Int. Workshop on Advanced Motion
Control, pp. 401406, Nagoya,Japan, March, 2000.
[14] R. Hedjar, R. Toumi, P. Boucher, D. Dumur, “Feedback nonlinear
predictive control of rigid link robot manipulators”, American Control
Conference, Anchorage, Alaska, May, 2002.
[15] F. L. Lewis, C. T. Abdallah, and D. N. Dawson, Control of Robot
Manipulators Theory and Practice, Marcel Dekker Inc, 2004.
[16] T. Yoshikawa, Foundation of Robotics, Analysis and Control, the MIT
Press, Boston, USA, 1990.
[17] F. Allgower, R. Findeisen, Z. K. Nagy, “Nonlinear model predictive
control: from theory to application”, J. Chin. Inst. Chem. Engrs, Vol.
35, No. 3, pp. 299315, 2004.
[18] R. Findeisen, L. Imsland, F. Allgower, and B. A. Foss, “State and
output feedback nonlinear model predictive control: an overview”,
European journal of control, Vol. 9, pp. 190206, 2003.
[19] R. Fletcher, Practical methods of optimization, John Wiley & Sons,
1987.
APPENDIX
22 32 42 2 33 43 23 44234
3 3 4 3 23 4 4 234
4 4 234
(1,1 ) 0
111
(1, 2 ) m m m c m m c m c
222
11
(3,1) m m c m c
22
1
(4,1) m c (A.1)
2
G
Gglglglglglgl
Gglglgl
Ggl
=
⎛⎞⎛⎞
=+++ + +
⎜⎟⎜⎟
⎝⎠⎝⎠
⎛⎞
=+ +
⎜⎟
⎝⎠
=
()
2222222222
11 22 32 42 2 33 43 23 44 234
3 3 2 4 3 2 23 2 4 4 3 234 23 4 4 2 234 2
22 222 2
22 32 33 43 42 44 442 434 4
33
21 1 1
(1,1 ) c c
33 3 3
2 cc c
11 1
(2,2) c
33 3
M
ml ml ml ml ml ml mlc
mll mll mllc c mll c
M mlml mlmlml mlmll mll
mll
⎛⎞⎛⎞
=+ ++ + + +
⎜⎟⎜⎟
⎝⎠⎝⎠
++ + +
= ++ +++ + +
+
()
24233
22 2
33 43 44 4 34 4
2
44
22 2
33 43 442 44 332 423 3
443 4
2
442 44 443 4
2c
11
(3,3) c
33
1
(4,4) 3
1111
(2,3) (3,2) c
3232
c
111
(2,4) (4, 2) c
232
(3,4 ) (4, 3)
mll
Mmlmlmlmll
Mml
M M ml ml mll ml mll mll
mll
MM mllmlmll
MM
+
=+++
=
⎛⎞
==++++ +
⎜⎟
⎝⎠
+
==++
==
2
44 434 4
11
c
32
(1, 2) (2, 1) 0
(1, 3 ) (3, 1) 0 ( A. 2 )
(1, 4) (4, 1) 0
ml mll
MM
MM
MM
+
==
==
==
22 2 2 2
2 2 22 3 2 22 3 2 3 223 3 3 2233 4 3 2233 4 2 22
12
2
4 2 3 223 4 4 3 22334 4 4 2 2234 4 4 223344
22
3 2 3 23 2 3 3 2233 4 3 2233 4 3 2 23
11
 s s s s s s
33
(1,1) 1
 2 s  s  s  s
3
1
 sc s s 2 s
3
ml ml mll ml ml m l
C
mll mll mll ml
mll ml ml mll
θθ
⎛⎞
⎜⎟
⎜⎟
=⎜⎟
⎜⎟
⎝⎠
+
&&
2 4 3 4 22334
13
2
4 2 4 234 2 4 4 223344
2
4 4 3 234 23 4 4 2 234 2 4 4 223344 1 4
22 2 2
2 2 22 3 2 22 3 2 3 223 3 3 2233 4 3 2233
2
42 22
c  s
1
 s c  s
3
1
ss s
3
111 11
s s s s s
622 62
1
(2,1) s
2
mll
mll ml
mll c mll c ml
ml ml mll ml ml
Cml
θθ
θθ
⎛⎞
⎜⎟
⎜⎟
⎜⎟
⎜⎟
⎝⎠
⎛⎞
+⎜⎟
⎝⎠
++ ++
=+ +
&&
&&
()
()()
2
4 2 3 223 4 4 3 22334 4 2 4 2234 1
2
4 4 223344
22
323 3 4323 3 443 4 4 434 4 2 4
3323 432323 434434
11
s s s
22
1S
6
11
 ss  s s
22
s 2s s
(3,1
mll mll ml l
ml
mll mll mll mll
mll ml l mll
C
θ
θθθθ
θθ θθ
⎛⎞
⎜⎟
⎜⎟
⎜⎟
++
⎜⎟
⎜⎟
⎜⎟
+
⎜⎟
⎝⎠
⎛⎞⎛⎞
+++
⎜⎟⎜⎟
⎝⎠⎝⎠
++
&
&&&&
&& &&
()
22
3 3 2 23 2 3 3 2233 4 3 2233 4 2 3 23 2 4 4 3 22334
2
1
2
4 4 2 234 2 4 4 223344
22
323 3 432 3 2 434 4 2 4 434 4 4
434
111 1
s s s s s
262 2
)11
ss
26
11
s s  s  s
22

mll c ml ml mll c ml l
mll c ml
mll mll mll mll
mlls
θ
θθθ θ
⎛⎞
++ + +
⎜⎟
⎜⎟
=⎜⎟
++
⎜⎟
⎝⎠
⎛⎞ ⎛⎞
++ + +
⎜⎟ ⎜⎟
⎝⎠ ⎝⎠
+
&
&&& &
()
()
434
22 2
4 4 3 234 23 4 4 2 234 2 4 4 223344 1 4 4 3 4 2
2
443 4 3 4434 3 4
111 1
(4,1) s sc s s
226 2
1s s (A.3)
2
C mll c mll ml mll
mll mll
θθ
θθ
θθθ
⎛⎞⎛⎞
=+++
⎜⎟⎜⎟
⎝⎠⎝⎠
⎛⎞
++
⎜⎟
⎝⎠
&&
&&
&&&
where, li and mi (i=1,…,4) are the length and mass of the ith
link, respectively,
θ
i and i
θ
&are the angular position and the
angular velocity of the ith joint, respectively, and ci = cos(
θ
i),
si = sin(
θ
i ), cij = cos(
θ
i +
θ
j), sij = sin(
θ
i +
θ
j), and so forth.
1358