See discussions, stats, and author profiles for this publication at: https://www.researchgate.

net/publication/300416787

Resolved motion control of a humanoid robot for coordinated manipulation

Conference Paper · December 2015

DOI: 10.1109/ROBIO.2015.7418997

CITATIONS READS

0 160

5 authors, including:

Wenfu Xu Lei Yan

Harbin Institute of Technology The University of Edinburgh
166 PUBLICATIONS   1,967 CITATIONS    21 PUBLICATIONS   84 CITATIONS   

SEE PROFILE SEE PROFILE

Some of the authors of this publication are also working on these related projects:

Space Robotics View project

Robotic Bird/ Flapping Wing Aerial Vehicle View project

All content following this page was uploaded by Lei Yan on 24 November 2016.

The user has requested enhancement of the downloaded file.

 Proceedings of the 2015
IEEE Conference on Robotics and Biomimetics
Zhuhai, China, December 6-9, 2015
Resolved Motion Control of a Humanoid Robot for Coordinated
Manipulation*
Jiming Shao, Huaiwu Zou, Wenfu Xu*, Member, IEEE, Liang Han and Lei Yan
Abstract—When applied for some tasks, such as payload
handling, assemble, repairing, and so on, the two arms of a
humanoid robot will form closed kinematic chain. It makes the
motion planning and control for dual-arm coordination very
complex and difficult. In this paper, we presented two types of
resolved motion control methods for a humanoid robot during
coordinated manipulation. They are respectively position-level
and velocity-level resolved motion control method. For the
former, the position-level trajectory of the payload is firstly
planned. Then the desired pose (position and attitude) of the
two arms are simultaneously determined by resolving the
constraints on their end-effectors. For the latter, the velocity of
the payload is planned. Then, the each end-effectors’ velocities
are resolved according to the constraints on the closed-chain
system. The joint variables of each arm are then calculated
using the inverse kinematics equations, at position-level or
velocity-level corresponding to the two cases above. Finally, a
dynamic modeling and simulation platform is established
based on ADAMS. The proposed methods are verified by
typical cases.
I. INTRODUCTION
With the rapid development of space technology, the
building and application of space station is very important
for the world. The most known space station is the
international space station (ISS), which serves as a
microgravity laboratory in which crew members conduct
experiments in biology, human biology, physics, astronomy,
meteorology and other fields. During the constructing and
maintenance of ISS, the space manipulators have being
playing important roles [1]. Nowadays, the most advanced
and dexterous robot launched to space is Robonaut2 [2].
China is also carrying out ambitious space exploration
programs, including Chinese space station plan and
Moon/Mar landing and sampling plans. Humanoid robots
with high flexibility and mobility are under development to
meet the complex space missions [3]. The coordinated
motion planning is the key for its applications.
* This work was supported in part by Shanghai Rising-Star Program
(09QA1402900), the National Natural Science Foundation of China
(61175098) and the Basic Research Program of Shenzhen
(JCYJ20140417172417095, JCYJ20140417172417129).
Jiming Shao and Huaiwu Zou are with the Institute of Aerospace
System Engineering Shanghai, 201109, China (e-mail:
Shaojiming0422@163.com; shanshuijun@163.com;
icebergh@126.com).
Wenfu Xu, Liang Han and Lei Yan are with the School of Mechanical
Engineering and Automation, Harbin Institute of Technology Shenzhen
Graduate School. They are also with the Shenzhen Engineering Lab of
Industrial Robot and System, Shenzhen, 518055, China. Z. Wenfu Xu is
the corresponding author (e-mail: wfxu@hit.edu.cn).
978-1-4673-9674-5/15/$31.00 © 2015 IEEE
For the motion generation of a humanoid robot, a
computational method using simple pin-and-drag interface
to generate whole-body motion is proposed in [4]. Neo et al.
[5] developed a framework that integrates command of
operator’s and autonomous function in whole-body motion
generation. Only the necessary points are selected by the
operator. Yoshida et al. [6] proposed a whole-body motion
generation method driven by the task. In order to achieve the
goal task, the proposed method reshapes the support
polygon when the task beyond the ability of the robot. A
whole-body reaching motion planning for collision
avoidance is proposed in [7], which takes the time spent for
the planning and the priority of the environment model into
account.
In the process of dual arms co-operation [8], such as
load handling, demolition and installation process, there
will be a closed kinematic chain. The closed kinematic chain
is more complex than the open chain in dynamics, path
planning and control correspondingly when there will be
contact force with environment [9]. So, it has become a hot
topic. Jia et al [10] proposed a dynamics-based adaptive
control approach for a planar dual-arm space robot in the
presence of closed-loop constraints and uncertain inertial
parameters of the payload. Hu and Vukovich [14] have
studied the force-position hybrid control of a closed
kinematic chain of multi-arm space robot system. Agrawal
[15] presented an algorithm for motion planning of planar
free-floating closed-chain space robot, based on the inverse
position kinematics of the mechanism. Papadopoulous has
explored multi-arm space robot tracking and capturing
target in the dynamics and control [11]. Moosavian and
Papadopoulos have proposed centroid vector method, direct
path method [12] and multi-arm compliant control method
[13].
In this paper, we studied the resolved motion control of a
humanoid robot for coordinated manipulation. Two types of
control methods are presented at different levels:
position-level and velocity-level. They are respectively
called resolved position control and resolved rate control.
Moreover, a dynamic modeling and simulation platform is
established based on ADAMS. The proposed methods are
verified by typical cases.
II. MODELING OF THE DUAL-ARM ROBOT WITH CLOSED
CHAIN
A. Model of a Humanoid Robot
A dual-arm humanoid robot has four branches including
1584
two feet and two arms. Each branch has 7 DOFs. The D-H where, fkine() means the forward kinematics function.
coordinate systems and the corresponding parameters are (b) Velocity-level kinematic equations
shown as Figure 1 and in Table I, respectively. In this paper, The linear and angular velocities of manipulator relative
the values of the structural parameters are as follows: to the base are as follows:
a3=0.054m, a7=0.19m, d1=d3=d5=0.35m. The  vea 
homogeneous transformation matrixes of the base ω   J a  Θa  Θa (5)
 ea 
frames of the two arms with respect to the body frame
(fixed on the body of the humanoid robot) are as follows:  veb 
ω   J b  Θb  Θb (6)
-0.9659 0 0.2588 0  eb 
 -0.2588 0 -0.9659 0
T0 L  
. . .
Re f  (1) xeb zea xea
 0 yL
1 0 0 zeb
  xL
 0 0 0 1 zL L
J bnb Bana
B b
nb
Capture Target
J ana
0.9659 0 -0.2588 0 Bbj y bj nb-DOF a
na-DOF Bi y a
 0.2588 0 0.9659 0 Arm_b Arm_a
i
xia
Re f
T0 R   (2) J b3 x bj
r J a

 0 1 0 0 Space Robot Base i zia B3a2

. . .
Bb2
  z bj B0 y0a B1a
y0b zB
 0 0 0 1 b
J2 oa
B1b J b a x
o yB
z0b b x0b z0a J1 0a
1
x7 x B

y7
190mm

x5 z7
θ7 x6 Figure 2 Constraints on a dual-arm robot
θ6 z5 y6
z6 (1)Position-level Kinematics Equations
y5
350mm

θ5 x3
First of all, considering position constraints of the dual
θ4 z3 x4 z 4 arms, the position and orientation of the gripped load is
y3 y4
denoted as coordinate {XLYLZL}. The other coordinate
350mm

θ3
x1 a3 x2
systems are defined as follows and shown in Figure 3.
z2
θ2 {XBYBZB}: The body frame of the humanoid robot.
z1 y2 {X0aY0aZ0a}: The base frame of Arm-a, fixed to the body
350mm

y1
θ1
z0
of the humanoid.
y0
x0
{XeaYeaZea}: The end-effector frame of Arm-a.
{X0bY0bZ0b}: The base frame of Arm-b, fixed to the
Figure 1 The redundant manipulator and its D-H parameters body of the humanoid.
Table I D-H Parameters
{XebYebZeb}: The end-effector frame of Arm-b.
Link i i / i / ai / m di / m From the geometric relationship shown in Figure 3, we
1 90 -90 0 d1 can calculate the pose of the payload with respect to the
2 0 90 0 0 body frame, according to the kinematic equations of Arm-a,
3 0 -90 -a3 d3 i.e.:
4 0 90 a3 0 B
TL  BTOa Oa TEa Ea Τ L (7)
5 0 -90 0 d5
6 -90 90 0 0 On the other hand, we can calculate the pose of the
7 0 0 a7 0 payload with respect to the body frame, according to the
kinematic equations of Arm-b.
B. Kinematics Equations of Dual-arm Robot with Closed
Chain
B
TL  BTOb Ob TEb Eb Τ L (8)
A general closed kinematic chain model forming by the where, B
TOa and BTOb are only dependent on the
two arms are shown as Figure 2. When the two arms grasp a
common object, the position and velocity of the two arms installation pose. So they are constant matrix and
E E
satisfy certain constraints. Each arm has the following determined in advance; a Τ L and b Τ L denote the pose of
kinematic relations: the payload with respect to the end-effectors of arm-a and
(a) Position-level kinematic equations arm-b, respectively; Oa TEa and Ob TEb are determined by the
The forward kinematics equations of the two arms are
written as: forward kinematics equations:
Te  0T1a 1T2a
0 a
Tn  fkine Θa 
n 1 a
(3)
Oa
TEa  0Tea  fkine Θa  (9)

Teb  0T1b 1T2b

0 n 1
Tnb  fkine Θb  (4)
Ob
TEb  0Teb  fkine Θb  (10)

1585
 When the desired orientation BTL are given, equations veb   E   rb 


1
v L 
(11) and (12) can be obtained by (7) and (8).      (19)
ωeb   O E  ωL 
   
1 1
Oa
TEa  B
TOa B
TL Ea
ΤL (11)
On the one hand, considering (5) and (16), we can get
vL   E   ra  


   
1 1
TEb   J a  Θa  Θa
Ob B B Eb
TOb TL ΤL (12) ω   (20)
 L   O E 
The position constraint equations needed to be satisfied
between Arm-a and Arm-b is as follows: Otherwise, combining (6) with (17), we can have
vL   E   rb  

B
TOa Oa TEa Ea Τ L  BTOb Ob TEb Eb Τ L (13)
ω     J b  Θb  Θb (21)
i.e.  L   O E 
B
TOa f kine Θa  Ea
Τ L  BTOb f kine Θb  Eb
ΤL (14) vea
veb veL

. . . . . .
Eb
TEL Ea
TEL z ωeb ωeL
xeb zeb xL ea xea ωea
EL
ra ra
Eb zL
Ea
EL
Eb Ea
Ob Oa
TEb TEa

Figure 4 The velocity constraints of the robot forming a closed chain

z0b
. y0b
x0b . zB

xB
yB
.
y0a

z0a
x0a
III. RESOLVED MOTION CONTROL

B
TOb
B
TOa A. Coordinated Resolved Motion Position Control
The key to the coordinated resolved motion lies in
Figure 3 The position constraints on a closed-chain system resolving the position constraint equation, as shown in
(2)Velocity-level Kinematics Equations Figure 5. The main steps of resolved motion position control
When the dual-arm form a closed chain and manipulate (called RMPC) are as follows:
the target load, the velocity relations are shown in Figure 4, a) Planning the load trajectory: The manipulation
where: tasks are first depicted as the trajectory of load (the common
vea , ωea are the linear and angular velocity of the body grasped by the two arms). The position and orientation
of the load are expressed in the function about time, using a
Arm-a’s end-effector; veb , ωeb are the those of Arm-b’s
certain planning method, such as 3rd polynomial, 5th
end-effector respectively; vL , ωL are the linear and angular polynomial or spline functions.
velocity of the load; ra , rb are respectively the position b) Determining the pose of load at time t: For time t
vectors from the ends of Arm-a, Arm-b to the origin of the (0ttf), the position and orientation of the payload at time t,
payload frame. i.e. pL  t  ,Ψ L  t  , are determined based on the planning
There exists the following relationship between Arm-a function. Correspondingly, the homogeneous
and the payload:
transformation matrix TL  t  is constructed.
B

vL  vea  ωea  ra

 (15) c) Resolving the velocity constraint equations at time t:
ωL  ωea According to TL  t  , the end pose matrix Oa TEa , ObTEb of
B

Equation (15) can be written as follows: Arm-a and Arm-b can be calculated by (11) and (12)
vL   E   ra   vea 

respectively.
ω      (16) d) Resolving the position-level kinematics at time t:
 L   O E  ωea 
The joint angle corresponding to Oa TEa and Ob TEb can be
Similarly, the motion relationship between Arm-b and
the payload can be derived as: derived by resolving the inverse kinematics of Arm-a and
Arm-b respectively.
vL   E   rb   veb 


 e) Joint control at time t: The determined Θa  t  and

ω     (17)
 L   O E  ωeb  Θb  t  are used as the desired values of joint controllers to
According to (16)and (17), we get: realize the robot control.
1 f) t=t+t.
vea   E   ra   v L 


     (18) g) if t<tf, return to b); otherwise, end.

ωea   O E  ωL 

1586
 Closed Chain Decomposition： pe  t   ped  t  t   pe  t  (26)
Positon Decomposition
Opened Chain Opened Chain
1
Oe   ne  t   ned  t  t  
2 (27)
PL  t  , ψ L  t  oe  t   oed  t  t   ae  t   aed  t  t 
Oa
TEa  t  Arm-a Oa
TEa  t 
Arm-b
PID
Position
PID
Position
where, ped  t  t  is the desired position at last sample
time; and pe  t  is the practical position at the current time;
Control Control

ned  t  t  , oed  t  t  , aed  t  t  and

 ne  t  , oe  t  , ae  t  are respectively the desired attitude
rotation matrix at last sample time, and the practical attitude
rotation matrix at the current moment.
Figure 5 Coordinated position decomposition for a dual-arm manipulator
e) Resolving the velocity-level kinematics at time t:
B. Coordinated Resolved Motion Rate Control vea  t  
According to (20) and (21), we can control the The joint angular velocity corresponding to   and
ωea  t  
coordinated movement of Arm-a and Arm-b using the
resolved motion rate control (called RMRC) strategy. The veb  t  
main steps are as follows:   can be determined by resolving the inverse
a) Planning the load velocity: A certain functions about ωeb  t  
time t, such as 3rd polynomial, 5th polynomial or spline kinematics of Arm-a and Arm-b respectively, i.e.:
functions, is used to described the linear and angular 1  v  t  
velocity of load, according to the task to be performed; Θa  t    J a  Θa  t  t     ea  (28)
b) Determining the load generalized velocity at time t: ωea  t  
For time t (0ttf), the corresponding linear and angular 1  v  t  

velocities vL  t  , ωL  t  are determined according to the Θb  t    J b  Θb  t  t     eb  (29)

ωeb  t 
planning function. The corresponding generalized velocity
Correspondingly, we can obtain the joint angles as
v L  t   follows:
is constructed as  .
ωL  t   Θa  t   Θa  t  t   Θa  t  t (30)
c) Resolving the velocity constraint equations at time t:
Θb  t   Θb  t  t   Θb  t  t (31)
v L  t  
According to   , the desired end velocity of Arm-a
ωL  t   f) Joint control at time t: The calculated Θa  t  , Θa  t 
and Arm-b can be derived by (18) and (19), i.e.: and Θb  t  , Θb  t  are used as the desired values of joint
1
vea   E   ra   vL 
 controllers to realize the robot control.
     (22) g) t=t+t.
ωea   O E  ωL 
h) if t<tf, return to b); otherwise, end.
1
veb   E   rb   vL 

IV. SIMULATION STUDY
     (23)
ωeb   O E  ωL 
A. Adams/Simulink Co-Simulation System
d) Compensating the end position error at time t The co-simulation system of the Humanoid robot is
To avoid the cumulative error, the following equations established to test the proposed algorithm under typical
are used to compensate the velocities determined by (22) coordinated tasks. The dynamic model is created in Adams
and (23), i.e.: software, and the algorithms are realized in
vea  t   vea  t   K  pea  t   Matlab/Simulink environment.
     (24)
ωea  t  ωea  t  t  Oea  t  B. Coordinated Resolved Motion Position Control
Assumed that the payload is a cube, whose dimensions
veb  t   veb  t   K  peb  t   and mass are as follows:
     (25)
ωeb  t  ωeb  t   t  Oeb  t  
Dimensions : 400×400×400mm
Mass : 10 kg
where, pe  t  and Oe  t  are the position and Inertia Tensor: Ixx = Iyy = Izz = 1.3418×104 (kgmm2) ,
Ixy = Iyz = Izx =0
attitude error at the current time; they can be calculated as Initially, the joint angles of each manipulator are known
follows:

1587
as (the unit is degree): errors by analyzing the simulation results. The maximum
Θa 0  76.33， 59.29， 89.06， 124.63， 51.24 deviations in X, Y and Z direction are 0.005, 0.0169, and
T
138.13， 131.49，
4.566 mm respectively. The average error is 1.063 mm. The
largest attitude errors of Z-Y-X Euler angles are 0.025o,
Θb 0  96.21， 57.39，
102.42， 125.34， 138.00， 131.64， 62.33
T
0.046o and 0.012 o respectively.
The force and torque at of the end-effector are also
The initial position of the geometry center of load is detected by the six dimensional force/torque sensors
(600mm, 0, 272mm), and attitude angles are (0, 0, 0) (x-y-z installed at the end of the two arms manipulator. They are
Euler angles) with respect to the body frame of the robot. shown in Figure 9 and Figure 10. Further analysis shows
The homogeneous transformation matrix is as follow: that the force along the Z axis of each arm counteract the
1 0 0 600  gravity of load; the force along X axis is generated from the
0 1 0 0  grasping force of the three-fingers end-effector, the force
Re f
TPL _ 0    (32) along Y axis is close to 0. The torque components along
0 0 1 272 
  each axes are close to 0.
0 0 0 1 
The desired pose of the payload are denoted as:
1 0 0 600 
0 1 0 300 
Re f
TPL _ f    (33)
0 0 1 222 
 
0 0 0 1 
The P2P Cartesian trajectory of the geometry center of
the payload is planned with the movement time tf=10s, using
the 5th polynomial functions. Then the two arms are Figure 8 The movement process under resolved motion position control
controlled to move the payload from the initial pose to the
desired pose, under the resolved motion position control
strategy. During the simulation, the joint trajectories of the
two arms are shown in in Figure 6 and Figure 7 respectively.

Figure 9 The end force/torque of left arm

Figure 6 The joint angles of left arm

Figure 10 The end force/torque of left arm

C. Coordinated Resolved Motion Rate Control

Figure 7 The joint angles of right arm
The whole simulation process is shown in Figure 8. The
motion of the two arms is very smooth, showing that the
control strategy is effective. We also evaluate the pose
The initial and final pose of the payload with respect to
the body frame of the robot are respectively (600m,0mm,
272mm; 0o, 0o, 0o) with respect to the reference coordinate,
the final pose of load is (800mm, 0mm,0mm; 5o,10o,0o).
Similar with the simulation above, the P2P Cartesian

1588
 trajectory of the geometry center of the payload is planned
with tf=10s, using the 5th polynomial functions. The
resolved motion rate control method is then used for the
humanoid robot. Then, the desired velocity of the
end-effectors of the two arms can be derived from equation
(22) and (23).
According to the simulation results, the trajectories of
each joint of the two arms are shown in Figure 11 and Figure
12. The movement process during the simulation is shown
in Figure 13. The maximum position of X, Y and Z-axis are
respectively [-0.5120mm, 0.2169mm, 5.560mm]. The
largest attitude errors of X-Y-Z Euler angles are
[-0.2712o,-0.5312o, 0.0123 o].
Figure 11 Joint angles of left arm
Figure 12 Joint angles of right arm
Figure 13 The movement process under resolved motion rate control
V. CONCLUSION
The two arms of a humanoid robot will form closed
kinematic chain during manipulation, making the motion
planning and control for dual-arm coordination very
complex and difficult. In this paper, we presented
position-level and velocity-level resolved motion control
1589
View publication stats
methods. The desired poses and/or velocities of the payload
are first planned according to the requirement of the task.
Then, the poses and/or velocities of the two arms’
end-effectors are determined by resolving constrains on the
two arms, which are connected by the payload and form a
closed chain. The joint variables of each arm are then
calculated using the inverse kinematics equations, at
position-level or velocity-level corresponding to the two
cases. Finally, a dynamic modeling and simulation platform
is established based on ADAMS. The proposed methods are
verified by typical cases. In the future, we will experiment
the presented methods on a practical humanoid robot.
REFERENCES
[1] E. Coleshill, L. Oshinowo, R. Rembala, B. Bina, D. Rey and S.
Sindelar, "Dextre: Improving Maintenance Operations on the
International Space Station," Acta Astronautica, vol.64, no.9-10, pp.
869-874, 2009.
[2] M. A. Diftler, J. S. Mehling, M. E. Abdallah, N. A. Radford, L. B.
Bridgwater and A. M. Sanders, et al., "Robonaut 2 - The First
Humanoid Robot in Space," in Proc. IEEE International Conference
on Robotics and Automation, Shanghai, China, pp. 2178-2183, 2011.
[3] Z. Jiang, S. Liu, L. Hui, D. Que, X. Chen and Q. Huang, "Mechanism
Design and System Control for Humanoid Space Robot Movement
Using a Simple Gravity-Compensation System," International
Journal of Advanced Robotic Systems, vol.10, no.389, pp. 1-13, 2013.
[4] K. Yamane and Y. Nakamura, "Synergetic CG choreography through
constraining and deconstraining at will," in Proc. IEEE International
Conference on Robotics and Automation, Washington, DC, USA., pp.
855-862, 2002.
[5] E. S. Neo, K. Yokoi, S. Kajita and K. Tanie, "Whole-Body Motion
Generation Integrating Operator's Intention and Robot's Autonomy in
Controlling Humanoid Robots," IEEE Transactions on Robotics,
vol.23, no.4, pp. 763-775, 2007.
[6] E. Yoshida, O. Kanoun, C. Esteves and J. P. Laumond, "Task-driven
Support Polygon Reshaping for Humanoids," in Proc. The 6th
IEEE-RAS International Conference on Humanoid Robots, Genova,
pp. 208-213, 2006.
[7] F. Kanehiro, E. Yoshida and K. Yokoi, "Efficient reaching motion
planning and execution for exploration by humanoid robots," in Proc.
IEEE/RSJ International Conference on Intelligent Robots and
Systems, Vilamoura, pp. 1911-1916, 2012.
[8] W. Xu, Y. Liu and Y. Xu, "The Coordinated Motion Planning of a
Dual-arm Space Robot for Target Capturing," Robotica, vol.30, no.5,
pp. 755-771, 2012.
[9] D. Van Phong and N. Q. Hoang, "Singularity-free simulation of closed
loop multibody systems by using null space of Jacobian matrix,"
Multibody System Dynamics, vol.27, pp. 487-503, 2012.
[10] Y. Jia, Q. Hu and S. Xu, "Dynamics and adaptive control of a
dual-arm space robot with closed-loop constraints and uncertain
inertial parameters," Acta Mechanica Sinica, vol.30, no.1, pp.
112-124, 2014.
[11] E. Papadopoulous, S. Evangelos and S. A. Moosavian, "Dynamics
and Control of Multi-arm Space Robots During Chase and Capture
Operations," in Proc. IEEE International Conference on Intelligent
Robots and Systems, pp. 1554-1561, 1994.
[12] S. A. A. Moosavian and E. Papadopoulos, "On the Kinematics of
Multiple Manipulator Space Free-Flyers and Their Computation,"
Journal of Robotic Systems, vol.15, no.4, pp. 207-216, 1998.
[13] R. Rastegari and S. A. A. Moosavian, "Multiple Impedance Control
of Space Free-flying Robots via Virtual Linkages," ActaAstronautica,
vol.66, pp. 748-759, 2010.
[14] Y. Hu and G. Vukovich, "Dynamic Control of Free-floating
Coordinated Space Robots," Journal of Robotic Systems, vol.15, no.4,
pp. 217-230, 1998.
[15] S. K. Agrawal, R. Garimella and G. Desmier, "Free-floating
closed-chain planar robots: Kinematics and path planning," Nonlinear
Dynamics, vol.9, no.1-2, pp. 1-19, 1996.