2016 14th International Conference on Control, Automation, Robotics & Vision

Phuket, Thailand, 13-15th November 2016 (ICARCV 2016) Su41.2

Gripping Position Detection of a Box-shaped Object

using a RGB-D Sensor by Tracked Mobile Robot
with Two Manipulators
Toyomi Fujita and Wataru Segawa
Department of Electronics and Intelligent Systems
Tohoku Institute of Technology
Sendai 982-8577, Japan
Email: t-fujita@tohtech.ac.jp

Abstract—We have developed a tracked mobile robot with two
robotic arms which can work by itself in a disaster area. It can
perform handling tasks such as gripping and carrying of a target
object by the two arms as manipulators. To reduce burden of
remote control on an operator for such tasks, this paper presents
a method for detecting gripping positions of a box-shape object
for the two manipulation arms using a RGB-D sensor. The method
is based on the detection of vertices of the object located in front
of the robot. Several experimental results verified the effectiveness
of this method.
I. I NTRODUCTION
In recent years, a robot is expected to perform some actions
related to rescue activity in a disaster area. The robot should
perform not only extract information but also working task
by itself in the area. For such a task, the robot that has
manipulation arm is useful. Several kinds of rescue robots and
mobile for uneven environments robots have been presented
[1][2][3][4]. Some types have had hybrid mechanism with
tracks and legs for advanced locomotion. For example, Hirose
et al. have presented a hybrid tracked mobile robot with four
legs on tracks that can move on rough terrain [5]. However,
handling tasks have not been considered very much for this
robot because it has been mainly focused on movement mech-
anisms and how to obtain information from the surrounding
environment. HELIOS IX [6] is equipped with a manipulator
for rescue operations, but the types of tasks and the size of
target object are restricted because of its single arm.
Based on this consideration, the authors have developed a
robot which has two manipulation arms so that it is able to
perform handling tasks such as removing rubble, carrying a
target object, and so on [7]. This robot has been controlled
manually by a remote operation based on an observation of
images sent from an camera mounted on the robot. In handling
motion, however, it has been difficult for an operator to control
all positions of hand of the arm and the robot. In order to
solve this problem, this study considers that the robot performs
handling tasks autonomously by detecting gripping positions of
the target object using an RGB-D sensor. The RGB-D sensor
is widely used because it is able to obtain a color image and
depth information simultaneously. Thus, we can expect that the
robot detects the position of hand to be gripped by mounting
the sensor on the robot.
In this study, we focus on gripping and lifting up a large
box-shape object as the working tasks. The aim of this study is
to develop the method for detecting the gripping position using
RGB-D sensor. This paper describes how to detect the gripping
position. Some fundamental experiments are employed in
order to evaluate the accuracy of the detection and confirm
autonomous lifting of a box object by presented method.
II. ROBOT S YSTEM
A. Overview of Robot
Figure 1 shows an overview of the tracked mobile robot
with two manipulation arms developed by the authors. The
robot drives using two tracks. Two 4-DOF robotic arms are
attached at the front part of the robot body. The total weight
is 30 kg and maximum velocity is 400 mm/sec.
RGB-D sensor ”Creative Interactive Gesture Camera” by
Intel Co. Ltd. is mounted at the front part of the robot body at
the height of 340 mm by tilting 25 degrees to lower direction.
The resolution of RGB sensor is 640×480 pixels and visual
angles are 60 degrees in width and 45 degrees in height.
The resolution of depth sensor is 320×240 pixels and visual
angles are 80 degrees in width and 60 degrees in height. The
detectable depth distance is from 0.15 to 1 m.
A laptop computer, Mouse Computer LB-C303S-NT-SSD
(Intel Core i5-4200U CPU, 2.6 GHz clock frequency, 8GB
RAM) is mounted on the robot and used as host PC. In
addition to the host PC, the other two micro computers,
NXP semiconductors mbed NXP LPC1768 (32bit Cortex-M3,
96MHz clock frequency, 64KB RAM) and LPC1114FN28
(32bit Cortex-M0, 50MHz clock frequency, 4KB RAM) are
embedded to control the track driving and the motion of robotic
arm.
B. Control System
The control system of the robot is shown in Fig. 2. The
robot is able to capture RGB images by the RGB-D sensor
and the host PC sends the RGB data to the remote PC.
Operator can observe the images on the remote PC and send

978-1-5090-3549-6/16/$31.00 ©2016 IEEE


Fig. 1. Tracked mobile robot with two manipulation arms
Fig. 2. Control system
commands of track movement for remote operation and the
command for autonomous handling task of the robot to the
host PC. In the autonomous handling motion, the host PC
detects the gripping position of target object and send control
commands for the track driving and manipulation tasks to the
LPC1114FN28 and LPC1768 microcomputers respectively. In
the remote operation, the host PC transmits commands for
track movement to the LPC1114FN28 microcomputer. The
LPC1768 microcomputer computes appropriated joint angles
of the manipulation arms and sends control signals for the
angles to joint motors according to received command from
the host PC. The LPC1114FN28 microcomputer sends PWM
signals to motor-driving circuit for two tracks in order for feed-
back control with rotation information obtained from encoder.
The operator moves the robot in the remote operation and
tries to find a target object in the observation of RGB images
which are sent from RGB sensor. When the target object is
found, the operator moves the robot so that it comes the front
face of the object and then sends the command for handling
task to the host PC. After that, the robot starts autonomous
motion; it continuously detects gripping positions for the left
and right hands using RGB-D sensor and moves forward or
backward by tracks so that hand tips come to the obtained
gripping positions. The robot then starts moving the arms for
lifting up the object. The methods for detection of gripping
positions are explained in Section III. With respect to the arm
motions for gripping are shown in the next section.
C. Control of Robotic Arm
Each of the manipulation arms consists of four joints: one
yaw rotation joint (𝐽1 ), two pitch rotation joints (𝐽2 , 𝐽3 ),
and one roll rotation joint (𝐽4 ). The lengths of links are
𝑙0 = 32mm, 𝑙1 = 39mm, 𝑙2 = 215mm, 𝑙3 = 85mm, and
𝑙4 = 60mm.
We considered inverse kinematics computation of the arms
for the gripping motion. Figure 3 shows the kinematic model
of the right arm Arm-1 in the state when the tip of the arm
contacts to the object. We gave a constraint that the third link
has to be parallel to the X-Y plane and the fourth link has
to be parallel to the Y-axis. Based on this constraint, the joint
angles 𝜃1 , 𝜃2 , 𝜃3 , and 𝜃4 to the hand position (𝑥ℎ , 𝑦ℎ , 𝑧ℎ ) can
be computed as
𝑦 ℎ − 𝑙4
𝜃1 = sin−1 ( ), (1)
𝑙23
𝑙1 − 𝑧 ℎ
𝜃2 = sin−1 ( ), (2)
𝑙2
𝜃3 = −𝜃2 , (3)
and
𝜋
𝜃4 = − 𝜃1 , (4)
2
where 𝑙𝑖 (𝑖 = 1, ⋅ ⋅ ⋅ , 4) is the length of the 𝑖-th link and 𝑙23
shows the length of the projected line of the second and third
links on the X-Y plane.
These joint angles are calculated by giving Y and Z
positions of the hand (𝑦ℎ , 𝑧ℎ ) only. The X position, 𝑥ℎ , is
then determined by
𝑥ℎ = 𝑙23 cos 𝜃1 . (5)
This means that we can not give 𝑥ℎ arbitrally. Thus, the robot
needs to move autonomously to stop at the appropriate position
where the distance to the object becomes 𝑥ℎ to satisfy this
constraint so that it handles the object.
III. D ETECTION OF G RIPPING P OSITIONS
A. Overview
We use RGB and depth images obtained by the RGB-D
sensor to obtain gripping positions on the host PC. FAST
[8] is applied to detect feature points and corner points are
extracted from them. The points which are not on the object
are eliminated by extracting region of the object using template
matching from the RGB image. Six vertices of the object are
then obtained from the corner points. The gripping positions
are computed form the vertices.
 Z
∙ the rear-left vertex: The point at which the value of 𝑢
becomes maximum in the points whose value of 𝑢 + 𝑣 is
below a threshold.
∙ the rear-right vertex: The point at which the value of 𝑢
Y

J2 becomes minimum in the points whose value of 𝑢 − 𝑣 is

below a threshold.
䃗2 ∙ the front-left vertex: The point at which the value of 𝑣 −𝑢
䡔 y z
( h, h, h)
l1 l2 J3 becomes maximum.
l3 ∙ the front-right vertex: The point at which the value of
l4
𝑢 + 𝑣 becomes maximum.
J1 䃗1
䃗3 䃗4 X Figure 5 shows the result of the detection of the four
J4 vertices. 𝑃1 and 𝑃2 show the rear-left and rear-right vertices
l23 respectively. 𝑃3 and 𝑃4 show the front-left and front-right
vertices respectively.
𝑃𝑙ℎ and 𝑃𝑟ℎ show front vertices on the lower surface, which
Fig. 3. Kinematic model of manipulation arm for handling motion become boundary points to the ground. How to obtain these
points are described in the next section.

B. Detection of Target Region

The region of the target object is detected by applying
template matching to an RGB image. We assume that the
template images of a target object are given in advance. The
regions the matching rate of which is exceeds a threshold are
extracted as candidates. In the candidates, the region where
the matching rate is maximum is selected as the target region.
The matching rate at a position (𝑢, 𝑣) on a searched image is
computed based on normalized cross correlation by

Σ𝑢′ ,𝑣′ (𝑇 ′ (𝑢′ , 𝑣 ′ )𝐼 ′ (𝑢 + 𝑢′ , 𝑣 + 𝑣 ′ ))

𝑅(𝑢, 𝑣) = √ √
Σ𝑢′ ,𝑣′ 𝑇 ′ (𝑢′ , 𝑣 ′ )2 Σ𝑢′ ,𝑣′ 𝐼 ′ (𝑢 + 𝑢′ , 𝑣 + 𝑣 ′ )2 Fig. 4. Example of detected corners
(6)
where (𝑢′ , 𝑣 ′ ) is a position on the template image and 𝑇 ′ and
𝐼 ′ are the intensity values at each position of the template and D. Detection of Boundary Points
searched image respectively. A model for the detection of boundary points is shown in
As we mentioned in Section II-B, this method is based Fig. 6. The left panel (a) and right panel (b) respectively show
on the assumption that the operator initially drives the robot the Y-Z and X-Z planes in the sensor coordinate system. When
remotely, finds the target object by seeing the images sent the depth value at (𝑢, 𝑣) on a depth image is 𝐿𝑢𝑣 (in Fig. 6, it
from the RGB sensor on the robot, and moves the robot to is described as 𝐿4 as an example for 𝑃4 ), the 3-D measurement
close position to the object so that its front faces the object. point (𝑥, 𝑦, 𝑧) is obtained by
Since the size of the object on the image at that time is not
𝑥 = 𝐿𝑢𝑣 sin 𝜑, (7)
changed widely, the number of the templates is reduced for an
object. 𝑦 = 𝐿𝑢𝑣 cos 𝜑 cos 𝜃, (8)
and
C. Detection of Vertices 𝑧 = 𝐿𝑢𝑣 cos 𝜑 sin 𝜃. (9)
We apply FAST [8] to detect corners in the target region of Let 𝑚 be the line, the length of which is 𝐿𝑢𝑣 , connecting
depth image. FAST is able to detect corners fast using decision between the origin and the measurement point. 𝜃 represents the
tree. Figure 4 shows an example of the detection of feature angle which the projection line of 𝑚 to Y-Z plane and Y axis
points of a box-shape object. The red rectangle shows obtained makes. 𝜑 is the angle which the projection line and 𝑚 makes.
regions by the matching described above. The corner detection Because the resolution of the sensor is 0.25 degrees/pixel as
is performed in this region only. The circle dots in Fig. 4 shows explained in Section II-A and the bottom line of the image
detected corners. indicates 35 degrees, 𝜃 and 𝜑 are 𝜃 = 35 + 0.25(240 − 𝑣)
In the corners, four vertices on the upper surface of the degrees and 𝜑 = 0.25(𝑢−160) degrees respectively. The depth
target object are extracted. Let (𝑢, 𝑣) be the position of the value 𝐷𝑢𝑣 to the ground for (𝑢, 𝑣) is
corner in the depth image coordinate as shown in Fig. 4. The 𝐻
four vertices are determined as follows; 𝐷𝑢𝑣 = (10)
cos 𝜃 cos 𝜑
where 𝐻 is the height of the sensor. Because the measurement
point is not on the ground when 𝐿𝑢𝑣 < 𝐷𝑢𝑣 , the boundary
points can be detected as the first points at which 𝐿𝑢𝑣 = 𝐷𝑢𝑣
by searching from 𝑃3 and 𝑃4 to the downward direction,
position direction of 𝑣, on the depth image. 𝐿4 and 𝐿𝑟ℎ in
Fig. 6 indicate the depth values for 𝑃4 and 𝑃𝑟ℎ respectively.
The boundary points 𝑃𝑙ℎ and 𝑃𝑟ℎ in Fig. 5 were obtained by
searching from 𝑃3 and 𝑃4 respectively by this method.
It is generally difficult to detect the corner points of the
lower surface by FAST because the change of depth value is
small at the boundary area to the ground. The proposed method
described above is therefore useful in such a case. Even if the
𝑃3 and 𝑃4 are detected on the front surface of the object, this
method can be applied. In this case, the search starts from 𝑃3
Fig. 6. Model of gripping position calculation: (a) YZ plane view, (b) XZ
and 𝑃4 to the upward direction and the front right and left plane view
vertices on the upper surface can be detected as the first point
at which 𝑦 value is equal to that at 𝑃1 and 𝑃2 respectively.
the box A, 155×270×150 mm for the box B, 205×220×150
mm for the box C, and 205 × 270 × 100 mm for the box D.
One of these boxes was located in front of the robot staying
same position. The detection of the gripping position was
performed in different distances between the robot and the
target object: 250 mm and 500 mm. We gave 56 images as
the template in which 14 kinds of magnification changed from
35 % to 100 % by 5 % were used for each of four kinds of
object.
2) Result: The results of the detection of gripping position
are shown in Figure 7. The errors in X, Y, and Z directions
Fig. 5. Example of detected vertices with boundary points are respectively described for each target object A, B, C, and
D. These errors are represented by the absolute values in the
sensor coordinate system shown in Fig. 6. The blue bars show
E. Computation of Gripping Positions the result of 250 mm and the red bars shows the result of 500
mm in measurement distance of Z direction.
The vertices and boundary points give the gripping posi-
tions. On the gripping position of the right side, 𝑃𝑟 (𝑥𝑟 , 𝑦𝑟 , 𝑧𝑟 ) From the results of X direction, we can see that enough
in Fig. 6, 𝑥𝑟 and 𝑦𝑟 are determined by the average values of detection was made because of small errors in the both of 250
𝑃4 (𝑥4 , 𝑦4 , 𝑧4 ) and 𝑃𝑟ℎ (𝑥𝑟ℎ , 𝑦𝑟ℎ , 𝑧𝑟ℎ ) on X and Y values as mm and 500 mm distances.
From the result of Y direction, the error was large in the 250
𝑥4 + 𝑥𝑟ℎ
𝑥𝑟 = mm measurement distance. The boundary point of the target
2 object comes bottom part of the depth image when the robot
and comes close to the target object. This may cause the large
𝑦4 + 𝑦𝑟ℎ
𝑦𝑟 = error in 250 mm. However, this error is not so serious for the
2
robot to grip and lift the object because of the error in height
respectively, and 𝑧𝑟 is determined by The average values of
direction.
𝑃2 (𝑥2 , 𝑦2 , 𝑧2 ) and 𝑃4 on Z value as
From the result of Z direction, the error tended to be larger
𝑧2 + 𝑧 4 than that of X and Y directions in the 500 mm measurement
𝑧𝑟 = .
2 distance. This result may be caused by an error of measurement
The left gripping position 𝑃𝑙 is also computed as the same in the depth direction because there are larger quantization
way. errors in the direction as the measurement distance is further.
Especially, the error was the largest for the target object B. In
IV. E XPERIMENTS
this case, it may be difficult for the robot to complete the task.
A. Detection of Gripping Positions However, it may not be so serious to lift up the other objects
1) Experimental Setup: Basic experiments for detecting because the error was about 10 % to the length of 205 mm of
gripping position were employed. Four boxes (A, B, C, and D) the object.
which had different sizes were used as the target objects; the These results indicates that presented method enables the
sizes were 205×270×150 mm in length, width, and height for robot to perform gripping and lifting an object.
 Fig. 7. Result of gripping position detection for four box objects: A, B, C, and D
B. Carrying a Box Object
1) Experimental Setup: An experiment for carrying a box
was employed based on the control described in Section II and
detection of gripping position presented in Section III. The
environment was basically same to the previous experiment
and the box D was used for the target object. The weight of
the target object was 175 g. The robot was initially located
about 4 or 5 m away from the object in a office room having
flat floor.
An operator starts a remote operation of the movement
of the robot to find the target object by seeing images of
RGB camera at the remote PC. After finding the object, the
operator stops the robot to locate in front of it. Then the robot
autonomously detects the gripping positions, moves forward or
backward so that its hand tips come to the obtained positions,
and start gripping and lifting it up.
2) Result: Figure 8 shows the result of the robot motion.
An operator moved the robot to the target object in (a) and
faced in front of it in (b). Then the robot started autonomous
motion in (c). The gripping positions of the object for both
hands were detected and moved forward to appropriate position
to complete gripping. The robot moved the arms so that each
hand comes to the gripping position in (d), gripped the object
in (e), and lifted it up in (f). This result confirmed that the robot
was able to perform a task of carrying a box autonomously by
the presented method.
Fig. 8. Experimental result of gripping and lifting up a box object
V. C ONCLUSIONS
In this study, a method for detecting the left and right
gripping positions of a box-shaped object was presented for
a tracked mobile robot with two manipulation arms. The
experimental results showed that it is valid for gripping and
lifting up motion. The presented system therefore enables the
robot to perform a working task autonomously and make the
operator reduce load of operation even though it is limited
situation or environment. As the future work, nevertheless, we
need to consider the method for detecting gripping positions
according to various cases of object orientation and shape to
be applied more generally.
R EFERENCES
[1] B. Yamauchi, “PackBot: A Versatile Platform for Military Robotics,” in
SPIE Vol. 5422: Unmanned Ground Vehicle Technology VI, 2004.
[2] T. Yoshida, E. Koyanagi, S. Tadokoro, K. Yoshida, K. Nagatani, K. Ohno,
T. Tsubouchi, S. Maeyama, N. Itsuki, T. Osamu et al., “A High Mobility
6-Crawler Mobile Robot ”Kenaf”,” in Fourth International Workshop
on Synthetic Simulation and Robotics to Mitigate Earthquake Disaster
(SRMED 2007), 2007, p. 38.
[3] X. Gao, K. Li, and J. Gao, “A Mobile Robot Platform with Double Angle-
Changeable Tracks,” Advanced Robotics, vol. 23, pp. 1085–1102, 2009.
[4] E. Rohmer, T. Yoshida, K. Ohno, K. Nagatani, S. Tadokoro, and E. Koy-
anagi, “Quince: A Collaborative Mobile Robotic Platform for Rescue
Robots Research and Development,” in International Conference on
Advanced Mechatronics, 2010, pp. 225–230.
[5] S. Hirose, R. Hodoshima, Y. Fukumura, and H. Amano, “Development
of Track-changeable Quadruped Walking Robot TITAN X-Design of Leg
Driving Mechanism and Basic Experiment-,” in IEEE/RSJ International
Conference on Intelligent Robots and Systems (IROS), 2010, pp. 3340–
3345.
[6] M. Guarnieri, I. Takao, P. C. Debenest, K. Takita, E. F. Fukushima,
and S. Hirose, “HELIOS IX Tracked Vehicle for Urban Search and
Rescue Operations:Mechanical Design and First Tests,” in 2008 IEEE/RSJ
International Conference on Intelligent Robots and Systems, 2008, pp.
1612–1617.
[7] T. Fujita and Y. Tsuchiya, “Development of a Tracked Mobile Robot
Equipped with Two Arms,” in The 40th Annual Conference of the IEEE
Industrial Electronics Society (IECON 2014), 2014, pp. 2738–2743.
[8] E. Rosten and T. Drummond, “Machine learning for high-speed
corner detection,” in Proceedings of the 9th European Conference
on Computer Vision - Volume Part I, ser. ECCV’06. Berlin,
Heidelberg: Springer-Verlag, 2006, pp. 430–443. [Online]. Available:
http://dx.doi.org/10.1007/11744023 34