計測自動制御学会東北支部 第 218 回研究集会（2004.10.9）
資料番号 218-22

Groping Locomotion of a Humanoid Robotin Environments with Obstacles○Y. Hanafiah, Mitsuru Endo, Mitsuhiro Yamano, Kazuhisa Mitobe,Yasuo Nasu, Shinichiro Kaneko, Kazumi Oikawa
Yamagata University
キーワード：Humanoid robot, groping locomotion, obstacle avoidance, correction of robot orientation,groping front wall, groping side wall, autonomous single arm groping
連絡先：〒992-0038 米沢市城南 4-3-16 山形大学工学部機械システム工学科
渡辺克己 研究室 Y. Hanafiah
Tel.: (0238) 26-3237, Fax : (0238) 26-3205, E-mail : t03146@dipfr.dip.yz.yamagata-u.ac.jp__________________________________________________________________________________
direction. We also propose a method of obstacle

1. INTRODUCTION
newavoidance by applying suitable algorithm to thedimension of interaction between human andhumanoid robot’s system. Previous study has

robot. Humanoid robot is expected to coexist

reported a method of obstacle avoidance forand collaborate with humans in environment

humanoid robot which applies vision sensor’s

where human work and live, e.g., in homes,

images to avoid the obstacle by walking andoffices, hospitals, because the design of acreeping [1]. Basically, application of sensor ishumanoid robot is to resemble human physicalnecessary in order for humanoid robot tocharacteristics. Humanoid robot is also expectedacknowledge its surrounding. However, someto replace human activities in environment

type of sensor, such as vision sensor, ultrasonicwhere involvement directly of human is highly

wave sensor, etc, sometimes cannot performrisk and dangerous, e.g., in hazard site, accident

well due to interruption of environment factors,site, etc. These demands make humanoid robot

e.g., noise, heat, dust, smoke, etc. Therefore, inmust be able to acknowledge and respond to its

this study we apply force sensor at humanoidsurrounding, and create suitable trajectory to

robot’s arms, which directly touch with existingattain sophisticate locomotion.

object to detect force which allows robot toHumanoidrobot

research

hasopenIn this study, we propose groping locomotion
for humanoid robot. It consists of basic research
forhumanoidrobot

toacknowledge

itssurrounding by touching and groping on wall’s
flat surface, and responds by performing
correction on its orientation and locomotion-1-

acknowledge the existing of wall or obstacle.

2.

Overview of Groping Locomotion

The main purpose of this study is to develop agroping locomotion method for humanoid robot
in order to operate autonomously in closed room
environment with existence of obstacles. Thismethod required a reliable algorithm for robot toSearch front wall

perform locomotion correctly and safely from

Wall not foundWall foundstart point to the desired end point. Fig.1 shows
simulation of room with wall and obstacle toGroping
front wall

overview path planning of proposed method.

Search side wall
Wall foundWall notfound

Gropingside wall

Side-walkto right

Robot will start the locomotion by searching
Correctrobot
distancenearest wall using right arm. If no wall is found,robot will perform side-walk to right direction
and repeating searching process until the wall isCorrect robot
distancefound. When the wall is found, robot arm willCheckobstacle toleft

grope on the wall surface to obtain wall position
and angle so that robot can correct its orientation

Obstacle foundto walk parallel with the wall surface. Before

Obstacle
avoidance

proceed with correction of angle, robot’s left
arm will check the existence of obstacle at theNo obstacle
Correct
robot angle

Fig.2 Algorithm of groping locomotion.

correction area. If the obstacle is detected, the3.

Searching and Groping Motion ofRobot Arm

to avoid the obstacle, before proceed to walk

3.1

Searching wall and checking obstacle

forward. The robot will repeat the process untilItisreach to the desired target point. Fig.2 shows

surrounding by find nearest wall or obstacle. In

algorithm of groping locomotion applied in thethis study, we propose an autonomous single arm

humanoid robot system. In this study, we apply

groping method. This method is consists ofdegree 5 polynomial equations in solving

sequent motion of right arm and left arm during

interpolation to generate motion trajectory.

groping locomotion and obstacle avoidance.

robot will perform obstacle avoidance trajectory

importanttoascertain

therobot’s

Searching and groping to wall only perform byObstacle
Wallsingle arm, which decided in this study as rightarm. Left arm is function to check obstacle inorder to navigate robot either to proceed
correction of its orientation or to performEnd

obstacle avoidance. Application of this method
has given reliable relation between groping

Avoid
obstacle

locomotion and obstacle avoidance.
During searching for wall and checking
obstacle, when arm’s end effector touches theStart

Search andgroping wall

wall, force sensor will detect force and send thedata to robot’s system in order for robot toacknowledge the wall. In this study, maximum

Fig.1 Path planning in groping locomotion.

-2-

force control parameter is fixed earlier by results

force and position control of arm’s end effectorof experiment, so that motion will stop if force

during groping front wall and side wall can bevalue detected is over the maximum fixed value.

describe as equations (2) and (3).

At this moment, encoders will record each arm’s
joints angle. By solving the direct kinematics
calculation of arm’s joints angle, the touch
position could be obtained.
3.2

Groping wall surface

The purpose of groping is to obtain end effector
position data which can be calculate to result
distance and angle between humanoid robot andwall, in order for humanoid robot to makecorrection on its locomotion direction. During
groping, the position and motion of end effector
are controlled by calculations which apply
values of maximum force Fmax , minimum force

Fmin and arm’s end effector shift distance in 1
sampling time l , as a parameter values.

(1)

For groping front wall: P − l   cur    0  (F 〉 Fmax )   0   0  Pref =   Pcur − l  (Fmin ≤ F ≤ Fmax ) 0    Pcur + l   Pcur − l  (F 〈 Fmin )   0  (2)For groping side wall:Fig.3 shows motion range of right arm during
searching wall. If wall is detected at range (a),robot will perform groping which can bedescribed as ‘groping front wall’. If wall isdetected at range (b), robot will perform groping
which can be described as ‘groping side wall’.
X(b)

(a)

Pref = Pcur + lY   Pcur − l   P + l  (F 〉 F )max  cur   0   
  Pcur − l Pref =   0  (Fmin ≤ F ≤ Fmax )   0     0 
  P − l  (F 〈 F )min  cur   0  (3)These formulations provide smooth motion toLeft

Robot Top
view

Right

robot’s arm during groping on wall surface. AtFig.3 Motion range of robot’s right arm during
searching wall.

this moment, series of end effector’s position

Here, end effector’s position P can bedistance and angle between robot and wall.

described as equation (1). From this equation,-3-

data are taken, which can be compute to results

4.
4.1

Correction of Robot’s Orientation

position of wall that facing the robot creates

Formulation

possibility for collision during correction ofDistance between robot to wall L and angle

robot’s orientation. Therefore, correction of

between robot and wall φ can be defined byrobot’s distance simply performs by generating

using end effector’s position data obtained
during groping, to be calculate with least-square
method calculation. The calculation gives results
to variable factors a and b in linear equation
as shown in equation (4). Here, distance L isthe shortest distance from robot’s reference
coordinate origin at right arm shoulder to linear

trajectory for legs to walk backwards. In the caseof groping side wall, correction of the robot’sdistance involves trajectory generation of legs towalk to side direction, like crab. Here, step size
is required to be defined. Base on the distance
between robot to the wall, which described as

L and reference distance between robot to theaswall, which described as Lb , the correction ofperpendicular line. Angle between robot anddistance can be defined as Lb − L . The sidewall φ , which described as groping angle is an

step size, described as S can be defined withangle from the perpendicular line to X-axis.

following formulations.

equation(4),whichFormulations ofcanL and φbedrawn

equations (5) and (6), respectively.
(4)

b(5)

a +12

 1
 a

φ = tan −1  − (6)From equation (6), correction of robot’slocomotion direction angle can be defined as

90° − φ , so that robot’s orientation is parallel(Lb − L ≤ 0)α
S=(Lb − L )sin φ (Lb − L〉 0 )(8)From equation (8),

(( Lb − L) sin φ 〉ϕ )βS =(9)(L − L )sin φ (( L − L) sin φ ≤ ϕ )b b

α , βandϕ are parameter values

considering maximum step size of humanoidwith the wall.
4.2(7)

Here, from equation (7),y = ax + b
L=S = (Lb − L )sin φis shown inCorrection of robot’s orientation ingroping front wall and side wall

Correctionofrobot

orientationreferstrajectoryof

bothlegs

performing

bytointerpolation of joint’s angle and position of fore
leg’s center point by applying the values of Land φ . Degree 5 polynomial equations arerobot legs to walk to side direction. Above
formulations allows robot to correct distance atany value of L and φ , where 0 ≤ φ ≤ 90° .
Figs.4 and 5 shows respectively a correctionof the robot orientation in groping front wall andgroping side wall, geometrically. Axis X-Y andX’-Y’ are robot’s orientation during groping andconsists in solving the trajectory generation forafter

both legs. In the case of groping front wall,respectively.

-4-correction

ofdistanceandangle,

correction of angle or to avoid obstacle. IfXφobstacle detected, mode of ‘whirl back left leg’

Wallwill avoid the obstacle by changing the robot’s
orientation facing the ostacle. Mode of ‘confirm

YCorrect angle

Correctdistance90 − φX’

obstacle’ will recheck the obstacle before robot
proceeds to walk forward direction or to walk toleft side direction. Fig.8 shows geometricalanalysis of robot center coordinates afterY’
Fig.4 Correction of robot distance and angle ingroping front wall.

groping front wall to avoid obstacle. Axis X-Y,X’-Y’ and X’’-Y’’ shows orientation of robot
during groping, after whirl back left leg and afterside walk to left, respectively.

LbL

WallX

90°−φ

Correct angle

90 − φX

X’Checkobstacle

φY
Y’
Correct distance

YFig.5 Correction of robot’s distance and angle ingroping side wall.

5.

Obstacle Avoidance

The concept of obstacle avoidance’s algorithm
is based on trajectory generation of robot’s legs
by manipulation of leg’s joints rotation angle. It
is assist by force sensor to detect existing ofLeft

Groping
front wall

Gropingside wall

Correct robot
distanceCorrect robot
distance

Check obstacle
to left

range angle of left arm is equal to the correction
angle 90° − φ , as shown in fig.6. Any object

No obstacle

detected

Correct robot
angle

thisdetection

rangeisRight

Fig.6 Motion range of robot’s left arm during
checking obstacle.

obstacles on robot’s left side. The detection

withinRobot Topview

Obstacle found
Whirl back left
leg

considered as an obstacle. Fig.7 shows thealgorithm of obstacle avoidance method. The
algorithm consist 3 important mode; ‘checking

Walk forward
No obstacle

obstacle to left’, ‘whirl back left leg’ and‘confirm obstacle’. The result of robot left armin ‘checking obstacle to left’ after correction ofdistance will decide either robot will proceed-5-Walk forward

Confirm
obstacle
ObstaclefoundSide-walk toleft

Fig.7 Algorithm of obstacle avoidance.

Obstacle

XφYaｗZWall

Y
90 - φRoll
X

X’

Pitch
YX’’
Fig.9 Photograph of humanoid robot“Bonten-MaruⅡ” and distribution of DOFs.

Y’Y’’

Table.1 Range of joint rotation angle

Fig.8 Correction of robot’s orientation
to avoid obstacle after groping front wall.

6.
6.1

Experiment

Humanoid robot “Bonten-MaruⅡ”In this study, we use “Bonten-Maru Ⅱ ”
humanoid robot [2]. The robot has total of 21
Degree Of Freedom (DOF). 6-DOF for each leg,
3-DOF for each arm, 1-DOF for waist and2-DOF for head. Fig.9 shows the photograph ofhumanoid

robot“Bonten-Maru Ⅱ ”

Range of rotation
angle (deg)Neck (roll)-90 ~ 90
NeckHumanoid(pitch) robotBonten-MaruⅡ-90 ~and90
Fig.8
Shoulder (pitch)-180 ~ 120
distributionof DOF
Right shoulder (roll)-135 ~ 30
Left shoulder (roll)-30 ~ 135
Elbow (roll)-90 ~ 90
Waist (yaw)-45 ~ 45
Hip (yaw)-90 ~ 90
Right hip (roll)-90 ~ 25
Left hip (roll)-25 ~ 90
Hip (pitch)-130 ~ 45
Knee (pitch)-20 ~150
Ankle (pitch)-90 ~ 60
Right ankle (roll)-90 ~ 20
Right ankle (roll)-20 ~ 90
Axis

anddistribution of DOFs. The height, wide and total
mass are 1250 [mm], 540 [mm] and 32.5 [kg],respectively.
Each joint has relatively wide range ofrotation angle, as shown in table 1. Especially

PCReceiverInterface

Counter

D/Afor both leg’s hip yaw which permitting both leg
to rotate in wide angle during obstacle avoidance.
Pulse
encoder

Every joint is driven by DC servomotor with arotary encoder and harmonic drive reduction

Motor velocity
command
Motor
DriverCurrent

system, and PC with Linux (CPU: Caleron2.4GHz) is utilized for control. The motor driver,the PC and the power supply are placed outsideof the robot. Fig.10 shows the layout of controlsystem.

Force sensor

RotaryencoderDCservomotor

Humanoid Robot “Bonten-MaruⅡ”

Fig.10 Control system of Humanoid Robot“Bonten-MaruⅡ”.

-6-6.2

robot’s system. During experiment, the robot

Force sensor

In order to measure the force applied duringperforms groping locomotion and obstacle

groping locomotion and obstacle avoidance,avoidance

force sensor are attached at both arms. It

robot’s system calculates end effector’s position

positioned between arm and end effector as

data using formulations of equations (4) ~ (6), toshown in fig.11. In this study, we use 6-axis

results robot distance and groping angle to theforce sensor IFS-67M25A50-I40 developed bywall. Robot system responds to the defined

Nitta Corporation. It consist 3 elements of force

results by performing either correction of robot

and 3 elements of moment at each axis X-Y-Z.

orientation or obstacle avoidance.

Maximum load at X and Y axis are 400 [N],6.3

while at Z axis is 200 [N]. Fig.12 shows aFig.13 (a) and (b) shows graph of end effector’s

photograph of force sensor used in this study.

position data during groping front wall and sideautonomously.

Duringgroping,Result and discussion

wall, respectively. In both cases, useful data onlyArm

considered at area 2 until 3. Area 1 until 2 shows
unreliable data due to reflex force of robot rigid

Force sensor

body when end effector touches the wall, whichEnd
effectorcauses robot‘s origin to shift from its original
position.
The data at area 2 until 3 are calculated using

Fig.11 Assembly of force sensor at robot arm.

least-square method. Calculation of the data
gives values to variable factors a and b ,which solve equations (4) ~ (6) to generate
results of robot distance L and groping angle

Zφ . Linear equation of equation (4) can be drawn
as shown in fig.13 (a) and (b).
In this experiment, during groping front wall,Y

respective values of a and b are 4.395676

Xand -1498.91, which results L = 389.7 [mm]and φ = 29.1 [deg]. Meanwhile, for groping

Fig.12 Photograph of force sensor.

6.3 Experiment methodside wall, respective values of a and b are

In this study, we performed 3 types of groping

0.223005 and -284.075, which results L =locomotion experiment; (a) groping front wall,

343.7 [mm] and φ = 62.9 [deg].

(b) groping side wall and (c) obstacle avoidance,with humanoid robot “Bonten-MaruⅡ”.
In

thisexperiment,

seriesofFig.14 (a), (b) and (c) shows series of actual
robot’s locomotion capture photographs duringmotion’sexperiment of groping front wall, groping side

programs are created initially and saved inwall and obstacle avoidance, respectively. Fig.14-7-

(a) and (b) shows reliable results where robot1performed correction of distance and angle
according to theoretical calculation. Appliedalgorithm of obstacle avoidance also shows
good performance as shown in fig.14 (c).
12

2

3
3

Fig.13 End effector position data during groping

1. Groping wall.

2. Correct distance

3. Check obstacle.

4. Correct angle.

5. Final orientation.

4. Correct angle.

5. Final orientation.

(a) Groping front wall.

1. Groping wall.

2. Correct distance.

3. Check obstacle.

(b) Groping side wall.

1. Search and touchobstacle.

2. Whirl back.

3. Confirm obstacle.

4. Side walk.

(c) Avoid obstacle.
Fig.14 Photograph of robot locomotion.

-8-

5. Reconfirm obstacle. 6. Walk forward.

[2]7. Conclusion and Future Works

Yasuo Nasu, Genci Capi, Shim-ichiro

In this study, we proposed groping locomotion

Kaneko, Mitsuhiro Yamano:

and obstacle avoidance for a humanoid robot.

Development

We apply a reliable algorithm for both cases andhumanoid robot,

perform simple force and position control toBull. Yamagata Univ. (Eng) Vol.28 No.1

fulfill the desired purpose in this study. We also

Feb.2004.

apply an autonomous single arm groping method
to relate humanoid robot’s motion between
groping locomotion and obstacle avoidance.
The result shows that by applying proposed
groping locomotion and obstacle avoidance
method, the robot is able to acknowledge andresponds to its surrounding condition. The
results

also

shows

thatbyapplying

autonomous

singlearm

groping

anmethod,humanoid robot is able to perform reliable
trajectory between groping locomotion andobstacle avoidance.
In the future, we going to improve force andposition control by applying impedance control
method. We believe groping locomotion andobstacle avoidance proposed in this study will bea good basic platform for humanoid robot
researches in the future.

References[1]

Y. Nasu, S.Kaneko, M.Yamano. G.. Capi,
S. Nahavandi:
CORBA-based Humanoid Robot Systemfor Accident Site Inspection trough theInternet,
7th WSES International Conference onSystems, CD-ROM Proceedings, 6 pages,
Corfu Island, Greece, July 7-10,2003.
Computational Methods in Circuits andSystems Aplications, WSEAS Press,
pp. 177-184.
-9-of“Bonten-Maru”