ENGI 9940 : Course Project

Color Based Humanoid and Wheeled Robot Path Planing

Thumeera R. Wanasinghe Arachchige

November 21, 2011

ENGI 9940 : Course Project

Background Objectives Implementation Demo: Example Run Simulation Next Step

ENGI 9940 : Course Project Background

Background

Humanoid robot has several advantages compared with wheeled and multi-legged mobile robots;
step onto object [1], climb up and down stairs [2, 3, 4], step over obstacle [5], crawl underneath object [6, 7].

ENGI 9940 : Course Project Background

Background

Due to the higher number of DOF, motion planing of humanoid becomes more complex and study under three categories; [8];
global path planing, local motion planing, task planing,

ENGI 9940 : Course Project Background

Global Path Planing

Start

Target

Figure: Global Path Planing [9]

ENGI 9940 : Course Project Background

Local Motion Planing

Figure: Local motion planing [10]

ENGI 9940 : Course Project Background

Task Planing

Figure: Humanoid task planing [10]

ENGI 9940 : Course Project Objectives

Objectives

Implement a color based global path planer for mobile robot, Compare performance of humanoid over wheeled mobile robot.

ENGI 9940 : Course Project Implementation

Planer Segmentation
Find a planer surface to place foot step is essential for humanoid [11]. Some of planer segment approaches use in global path planing are;
Expectation maximization; Triebel et al. [12] Thrun et al. [13]. Random sample consensus (RANSA); Rasu et al. [14]. Edge detection; Cupec et al. [15]. Randomize Hough Transformation; Okada et al. [16]. Scan-line grouping; Gutmann et al. [17]. Color segmentation; Kitaoka et al. [18].

ENGI 9940 : Course Project Implementation

Assumptions
We made following assumptions to dene the scope of the project;
2D color image of the terrain is provided, Humanoid can place its feet only on planer surfaces, The world is partitioned in to oor and obstacle, Humanoid is capable of estimate obstacle height and their relative position using its sensors (stereo vision and laser range nder).

ENGI 9940 : Course Project Implementation

Color
We need only three color to represent complex environment.(oor, obstacle and non-planer surfaces). However we used 7 colors and a texture in order to provide height information for the simulation.
1. 2. 3. 4. 5. 6. 7. 8. Boaders Bumps Obstacle with low height Obstacle with large height Step 1 Step 2 Step 3 Backbround texture

ENGI 9940 : Course Project Demo: Example

Step 1
Load the terrain

Figure: 2D color image of the terrain

ENGI 9940 : Course Project Demo: Example

Step 2
Identify oor, obstacle and non-planer areas

Figure: Color based segmented image of the terrain

ENGI 9940 : Course Project Demo: Example

Step 3
3D occupancy grid
3D occupancy grid is a function of 3D position to probabilities.
Camera/Laser (C)

Ray trace

Humanoid (R) C C C

Global frame (G)

Cells under the ray trace Cn Obstacle (O)

Figure: Generation of 3D occupancy grid

ENGI 9940 : Course Project Demo: Example

Step 3
3D occupancy grid
3D occupancy grid is a function of 3D position to probabilities. P (c) P c | piG = P piG | c P (c) P piG | c P(c) + P piG | c (1 P (c)) (1)

where, sensor is modeled from P piG | c and P piG | c We used P piG | c = 0.8 for obstacle P piG | c = 0.6 for oor. Initially OG (x, y , z) = 0.5.

ENGI 9940 : Course Project Demo: Example

Step 3
3D occupancy grid

Figure: Generated 3D occupancy grid

ENGI 9940 : Course Project Demo: Example

Step 4
Generate oor height map
Related to 2D position to oor hight. Floor (x, y ) h Initially Floor (x, y ) = 1. It updates as follows; Floor (x, y ) = Floor (x, y ) if Poor (x,y ) > Pocc 1 invalid (3) (2)

where, PFloor (x,y ) = OG (x, x, Floor (x, y )) and Pocc is a threshold value for detection (Pocc = 0.6)

ENGI 9940 : Course Project Demo: Example

Step 4
Generate oor height map

Figure: Generated oor height map

ENGI 9940 : Course Project Demo: Example

Step 5
Generate oor obstacle grid
Update obstacle height in 3D grid base on a probability for being occupied. Obstacle (x, y , z) = max
OG (x,y ,z)>Pocc

(4)

Update oor and obstacle grid as follows; (obstacle, h ) if h > hf + FOG (x, y , z) = (oor , hf ) else if hf = invalid (unknown, invalid) otherwise (5)

ENGI 9940 : Course Project Demo: Example

Step 5
Generate oor and obstacle height map

Figure: Floor and obstacle height map

ENGI 9940 : Course Project Demo: Example

At the end of step 5

Now robot knows several information about its terrain

Position of planer surfaces and non-planer surfaces, Location of obstacles, Height information of obstacle.

ENGI 9940 : Course Project Demo: Example

Step 6

Built conguration world

In conguration world, a robot represent as point object, To ensure collision free path from start to goal, region growing for obstacle and non-planer areas is required, We used 100x100 image to represent terrain and assumed a single cell clearance is needed for collision free path planning,

ENGI 9940 : Course Project Demo: Example

Step 7

Region growing for wheeled robot

Occupancy grid is used to identify free and occupied areas of terrain. We used 3x3 mask for region growing, If any of near by cell is occupied current cell will update as occupied.

ENGI 9940 : Course Project Demo: Example

Step 7
Region growing for wheeled robot

Figure: Region growing for wheeled robot

ENGI 9940 : Course Project Demo: Example

Step 7
Region growing for wheeled robot

Figure: Region growing for wheeled robot

ENGI 9940 : Course Project Demo: Example

Step 7
Region growing for humanoid
Initially PosibleArea(x, y ) = 1 for all (x, y ), Used 3x3 mask to generate Step Map from Obstacle Grid, If a given cell is occupied, possible area also mark as occupied (PosibleArea(x, y ) = 0), Otherwise,
Calculate height dierent for 8 connected cells (hi i = 1, 2, 3, 4, 5, 6, 7, 8 ) If hi > hstep where hstep is a stable, step up or step down height that humanoid can executes, then the cell will redene as occupied.

ENGI 9940 : Course Project Demo: Example

Step 7
Region growing for wheeled robot

Figure: Region growing for humanoid robot

ENGI 9940 : Course Project Demo: Example

Step 8

Path planing
Used A path planning algorithm. Started with A search for path planning tutorial by Paul Premakumar [19] and made minor modication to his Matlab implementation which is covered by the BSD License. None of the modication violates any term in BSD license agrement.

ENGI 9940 : Course Project Demo: Example

Final Step
Generate the command word for wheeled robots
We assume ve movement actions for wheeled robots,

Move forward, Turn left by 45 Turn left by 90 Turn right by 45 Turn right by 90

ENGI 9940 : Course Project Demo: Example

Final Step

Generate the command word for humanoid robots

Humanoid also can execute all ve footsteps actions mentioned earlier, Moreover, they can step onto object, climb up and down stairs, step over object and crawl underneath object, We simulated step onto object and climb up and down stairs.

ENGI 9940 : Course Project Demo: Example

Final Step

The next action that robot need to be executed is depend on four factors
Robot previous location, Robot current location, Robot next location, Floor height dierent between current location to next location.

ENGI 9940 : Course Project Demo: Example

Final Step
Forward and rotation

Figure: Action plan matrix

ENGI 9940 : Course Project Run Simulation

Run Simulation

ENGI 9940 : Course Project Next Step

Next Step

There are three things yet to be complete

Step over the obstacle, Do not allow to step onto stairs or bump across the corner of the stair or bump, Do not allow transverse very close to edge of bump and stairs when robot move on top of them.

ENGI 9940 : Course Project Bibliography

J. J. Kuner, S. Kagami, K. Nishiwaki, M. Inaba, and H. Inoue, Dynamically-stablemotion planning for humanoid robots, Autonomous Robots, vol. 12, no. 1, pp. 105118, Jan 2002. J. S. Gutmann, M. Fukuchi, and M. Fujita, Stair climbing for humanoid robots using stereo vision, Interligent robots and system, vol. 2, pp. 14071413, Sep 2004. S. H. Kim and Y. Sankai, Stair climbing task of humanoid robot by phase composition and phase sequence, Robot and human interactive communiaction, pp. 531536, Aug 2005. T. H. S. Li, Y.-T. Su, C.-H. Kuo, C.-Y. Chen, C.-L. Hsu, and M.-F. Lu, Stair-climbing control of humanoid robot using force and accelerometer sensors, SICE, Annual conference, pp. 21152120, Sep 2007.

ENGI 9940 : Course Project Bibliography

Y. Guan, K. Yokoi, N. E. Sian, and K. Tanie, Feasibility of humanoid robots stepping over obstacles, Interligent robots and system, vol. 1, pp. 130135, Feb 2005. Z. Shiller, K. Yamane, and Y. Nakamura, Planning motion patterns of human gures using a multi-layered grid and the dynamics lter, Robotics and Automation, vol. 1, pp. 18, 2001. F. Kanehiro, H. Hirukawa, K. K. S. Kajita, K. Fujiwara, K. Harada, and K. Yokoi, Locomotion planning of humanoid robots to pass through narrow spaces, Robotics and Automation, vol. 1, pp. 604609, Jul 2004. J. Kuner, K. Nishiwaki, S. Kagami, M. Inaba, and H. Inoue, Motion planning for humanoid robots, International symposium of robotics reserch, pp. 110, 2003.

ENGI 9940 : Course Project Bibliography

J. steen Gutmann, M. Fukuchi, and M. Fujita, Real-time path planning for humanoid robot navigation, in in Proc. of the Int. Joint Conf. on Articial Intelligence (IJCAI05, 2005, pp. 12321237. W. U. ROBOCASA Project, Emotion expression humanoid robot we-4rii, Online: http://www.robocasa.net/projects.php?lang=en&subpage=1, accessed: Nov 18, 2011. S. Owald, J.-S. Gutmann, A. Hornung, and M. Bennewitz, From 3D point clouds to climbing stairs: A comparison of plane segmentation approaches for humanoids, in Proc. of the IEEE-RAS International Conference on Humanoid Robots (Humanoids), Bled, Slovenia, October 2011.

ENGI 9940 : Course Project Bibliography

R. Triebel, W. Burgard, and F. Dellaert, Using hierarchical em to extract planes from 3d range scans, in In Int. Conf. on Robotics and Automation (ICRA), 2005, pp. 44374442. S. Thrun, C. Martin, Y. Liu, D. Hhnel, R. Emery-Montemerlo, a D. Chakrabarti, and W. Burgard, A real-time expectation-maximization algorithm for acquiring multi-planar maps of indoor environments with mobile robots. IEEE Transactions on Robotics, vol. 20, no. 3, pp. 433443, 2004. R. B. Rusu, A. Sundaresan, B. Morisset, K. Hauser, M. Agrawal, J. C. Latombe, and M. Beetz, Leaving atland: Ecient real-time three-dimensional perception and motion planning, in Journal of FieldRobotics, vol. 26, no. 10, 2009, pp. 841862. R. Cupec, G. Schmidt, and O. Lorch, Experiments in vision-guided robot walking in a structured scenario, in

ENGI 9940 : Course Project Bibliography

Proceedings of the IEEE International Symposium on Industrial Electronics (ISIE 2005), June 2005, pp. 15811586. K. Okada, S. Kagami, M. Inaba, and H. Inoue, Plane segment nder: Algorithm, implementation and applications, in In Int. Conf. on Robotics and Automation (ICRA 2001), May 2001, pp. 21202125. J. S. Gutmann, M. Fukuchi, and M. Fujita, 3d perception and environment map generation for humanoid robot navigation, in International Journal of Robotics Research (IJRR), vol. 27, no. 10, 2008, pp. 11171134. M. Kitaoka, A. Yamashita, , and T. Kaneko, Obstacle avoidance and path planning using color information for a biped robot equipped with a stereo camera system, in Proc. of the 4th Asia Int. Symp. on Mechatronics, 2010, pp. 3843.

ENGI 9940 : Course Project Thank You

P. Premakumar, a search for path planning tutorial, Online: http://www.mathworks.com/matlabcentral/leexchange/ 26248-a-a-star-search-for-path-planning-tutorial, accessed: Oct 20, 2011.

ENGI 9940 : Course Project Thank You

Thank You