On the integration of vision and CPG based locomotion for pathplanning of a nonholonomic humanoid crawling robot

Sebastien Gay, Sarah Degallier, Ugo Pattachini, Auke Ijspeert and Jose Santos Victor

Abstract— In this paper we present our work on integratinga CPG based locomotion controller with a vision tracker, andan inverse kinematics solver to design a motion planning algo-rithm based on potential fields for a non holonomic crawlinghumanoid robot, the iCub. We study the influence of the variousparameters of the potential field equations on the performanceof the system and prove the efficiency of our framework bytesting it on a realistic robotics environment and partially onthe real iCub.

I. INTRODUCTION

Humanoids have inspired a lot of researchers and sciencefiction authors over the last few decades. Building a machinethat would mimic humans with the same dexterity androbustness is a problem far from solved. The first steptowards a fully functionnal humanoid robot is to enable itto move around its environment autonomously, identifyinggoals while avoiding collisions.

The work we present here is an attempt to integrate vision,locomotion, reaching, and potential field based planningto have a non holonomic humanoid robot, the iCub movearound a simplified environment using no external informa-tion.

A. Locomotion

The locomotion system we developed uses central patterngenerators (CPG), networks of coupled oscillators inspiredfrom the spinal cord of many animals. CPG models areincreasingly used for different kind of robots and typesof locomotion as insect like hexapods and octopods [1],quadrupeds [2] and [3], Swimming [4], and humanoids [5]and [6]. For a more complete review of CPGs and theirapplication in robotics see [7]. The main benefits of CGPsfor locomotion is their stability to perturbations, the ability tosmootly modulate the shape of the oscillations with a simplecontrol signal and the possibility to easily integrate sensoryfeedback. Most of the efforts of the past decades have beendedicated to using CPGs for rhythmic locomotion patterngeneration. Yet, periodic movements do not suit discretetasks like manipulation or reaching. Our system embeds bothrhythmic and discrete motion generation in the same CPG.

B. Path planning

Numerous path planning techniques exist in the literature.Most of them use a geometric description of the environmentand the robot. Grid based approaches overlay a grid onthe map of the environment, reducing the path planningproblem to a graph theory problem . Sampling techniques arecurrently considered the state of the art for a vast majority ofmotion planning problems. Yet, both these methods require

an exhaustive representation of the world to be efficient,which is incompatible with vision based planning. They alsorequire a precise odometry estimation to be able to achievethe computed roadmap, which is very difficult to achieve onour robotic platform. See [8] and [9] for comparative studiesof grid based and sampling planning techniques.

Obstacle avoidance techniques are better suited to par-tial knowledge of the environment. Examples of obstaclesavoidance techniques include vector field histogram [10]which computes a subsets of motion directions and picks thebest according to some heuristics and the dynamic windowapproach [11] which works in a similar way but in thevelocity controls space.

An Alternate method, at the border between path planningand obstacle avoidance techniques, is Artificial potentialfields [12]. The idea is to place artificial positive potentialson obstacles and negative potentials on the goal to attain,and navigate along the gradient of the potential field. Themajor problem of this method is its fragility to local minima,although some harmonic potential field functions have beendeveloped to counter this weakness [13]. This method hasnot been developped specifically for non holonomic robots,and some variants based notably on fluid dynamics theory[14] have been developped to cope with the constraints ofthese particular robots.

The principal advantages of potential fields approachesfor our application are that it is easily extensible to partialdescriptions of the environment, and dynamically changingenvironments and it is computationally inexpensive, anecessary condition for online path planning.

Our approach does not claim to design a new state ofthe art motion planning algorithm. Instead, the goal of thiswork is to study the challenges that emerge when dealingwith real legged non-holonomic robots. In this optic, weintegrated a CPG based crawling locomotion framework,with a vision tracking system exploiting the embeddedcameras of the robot, a high level motion planner basedon artificial potential fields and acting on the CPG, and aninverse kinematics solver for reaching. To the knowledge ofthe authors approaches aiming at integrating all these featuresare very seldom in the litterature. Examples include thework of [quote] on the HRP-2 robot, and This study showsthat online vision based navigation can be efficient even ona legged nonholonomic robot where vision and odometryestimation are strongly perturbed but the specificities of thequadruped gait (rolling effect, complexity of the inversekinematics etc.). It also shows an application of a full control

system based on dynamical systems allowing rythmic anddiscrete movements.

II. PRESENTATION OF THE ARCHITECTURE

A. Hardware and sofware platform

The architecture presented here is based on the one de-scribed in [15].

The iCub is a humanoid robot developed as part of theRobotCub project [16]. It has been designed to mimic thesize and weight of a three and a half years old child(approximately 1m tall). It has 53 degrees of freedom. TheiCub’s eyes have 2 DOF each and are composed of twoDragonly 2 cameras with a 640x480 CCD sensor. The headof the robot embedds a pentium CPU , allowing for fullyautonomous control, and more demanding computation canhappen outside the robot via ethernet communication.

As the rest of the iCub software, our architecture is buildon top of YARP [17]. The iCub architecture is composedof multiple independent modules communicating betweeneach other and devices through YARP ports. These modulescan be run on any computer of a Ethernet network. Usingsuch a modular architecture gives the possibility to use forinstance different vision modules, or planning algorithms,without changing the other modules in any way. It alsoallows us to parallelize the work between different machinesof a cluster, which can be useful for demanding task likeinverse kinematics, or 3D simulation.

A low-level controller for the generation of both discreteand rhythmic movements, based on the concept of centralpattern generators (CPGs), was developed with the mainfocus of implementing an adaptive, closed-loop controllerfor crawling in framework of the RobotCub project. In thisarticle, we combine this low-level architecture with a high-level planner algorithm.

We first briefly present the low-level control and then wediscuss more in details the high-level planner that we de-veloped. For more information on the low-level architecture,please refer to [15] and [18].

B. Locomotion

Our locomotion framework is built on the concept ofcentral pattern generators (CPGs), that we take in the senseof a network of unit generators (UGs) of basic movementscalled motor primitives.

All trajectories (for each joint) are generated through aunique set of differential equations, which is designed toproduce complex movements modeled as periodic move-ments around time-varying offsets. More precisely, com-plex movements are generated through the superimpositionand sequencing of simpler motor primitives generated byrhythmic and discrete unit generators. The dynamics of thediscrete movement is simply embedded into the rhythmicdynamics as an offset.

0 0.5 1 1.5 2 2.5 3 3.5 4

0

2

4

Control Parameters

0 0.5 1 1.5 2 2.5 3 3.5 4−2

0246

Limb Trajectory

Fig. 1. Unit pattern generators. Upper panel. Control commands fordiscrete and rhythmic movements, that is the target position (in blue) andthe amplitudes (in red), the frequency being not shown on the figure. BottomPanel: The resulting discrete and rhythmic movements (resp. in blue and inred) and the trajectory embedding the two dynamics (black).

The discrete UG is modeled by the following system ofequations

hi = d(p− hi) (1)yi = h4

i vi (2)

vi = p4−b2

4(yi − gi)− b vi. (3)

The system is critically damped so that the output yi of Eqs 2and 3 converges asymptotically and monotonically to a goalgi with a speed of convergence controlled by b, whereas thespeed vi converges to zero. p and d are chosen so to ensurea bell-shaped velocity profile; hi converges to p and is resetto zero at the end of each movement.

The rhythmic UG is modeled as Hopf oscillator with theoutput of the discrete system as offset:

xi = a(mi − r2

i

)(xi − yi)− ωizi (4)

zi = a(mi − r2

i

)zi + ωi (xi − yi) +

∑kijzj (5)

ωi =ωdown

e−fzi + 1+

ωupefzi + 1

(6)

where ri =√

(xi − yi)2 + z2i . When mi > 0, Eqs. 4 and 5

describe an Hopf oscillator whose solution xi is a periodicsignal of amplitude

√mi and frequency ωi with an offset

given by gi. A Hopf bifurcation occurs when mi < 0 leadingto a system with a globally attractive fixed point at (gi,0). Theterm

∑kijzj controls the couplings with the other rhythmic

UGs j; the kij’s denote the gain of the coupling between therhythmic UGs i and j and are set here to generate a trot gait.The expression used for ωi allows for an independent controlof the speed of the ascending and descending phases of theperiodic signal, which is useful for adjusting the swing andstance duration in crawling for instance [18].

Qualitatively, by simply modifying on the fly theparameters gi and mi, the system can switch betweenpurely discrete movements (mi < 0, gi 6= cst), purelyrhythmic movements (mi > 0, gi = cst), and combinationsof both (mi > 0, gi 6= cst) as illustrated on Figure1. Different values for the kij’s lead to different phaserelationship between the limb, i.e. different gaits for instance.

C. Vision

For a robot to be able to navigate in an environment, itneeds to be able to percieve its environment ; in our casesee it. The iCub is equipped with two cameras, two eyes,with the same two degrees of freedom as the human eye. Asvisual processing is not our main topic here, we chose to usea very simple marker based tracker, based on the ARToolKitPlus library [19]. Another reason for us to use this tracker isthe fact that is doesn’t use stereo-vision to compute the threedimensional position of a fixed sized marker. It actuallyuses only one of the two cameras. This allows for fastertracking, which is especially important when both the eyesand the head are moving during scanning. The obstacles andthe goals are marked with a different marker. The tracker isable to output the three dimensional position in the camerareference frame and the ID of multiple markers. On the realiCub robot, the tracker is able to detect an 8cm marker andits ID about 1.5m away. It’s also very robust to changes oflightning. The position of the marker is translated to therobot root reference frame frame (attached to the waist)using forward kinematics.

The main limitation of the vision system of the iCub isits relatively small vision field(α ≈ 45◦) which implies avery small map of the environment, and thus may cause tomiss most of the goals and even collide with obstacles whennavigating (see Section II-E). To solve this problem, we havethe robot scan the environment by rotating its head and eyesfrom left to right. The head oscillation is coupled with thelimbs movements to have the scanning speed depend on thelocomotion speed. This scanning process extends the visionfield of the robot to θ ≈ 120◦ (see Figure 3.

D. Reaching

Once the robot approaches a goal near enough it shouldtouch it with its hand. The robot moves its head and eyeswhile approaching the goal to keep it in the center of itsvision field. This will allow him to make sure it doesn’t loosethe goal and to have a better precision on the goal position.The goal position is estimated using the vision trackerdescribed in the previous section. Once the robot reachesa specific distance to the goal, it’s considered ”potentiallyreachable”. Starting this point we use inverse kinematics tocompute the joints angle of the 7-DOFs arm to achieve thetarget position, that is the position of the goal.

An inverse kinematics cartesian solver was designedspecifically for the YARP framework. This solver is based onthe IPOPT (Interior Point OPTimizer) library [20], a libraryfor large scale non-linear optimization. It uses a primal-dualinterior-point algorithm to solve a problem of the form:

minx∈Rn

f(x)

s.t. c(x) = 0xL ≤ x ≤ xU

(7)

where f is the objective function, c are the equalityconstraints and xL and xU are the lower and upper bound

of the variable x.

For out problem, given a desired position xd in R3, thesolver finds the joint configuration q in the 7 dimensionaljoint space Q ∈ R7 that achieves the nearest position Kx(q)of the end effector (i.e. a given part of the arm):

q = argmaxq∈R7

(||xd −Kx(q)||2)

s.t. qL < q < qU

(8)

where qL and qU are the lower and upper joint limits ofthe arm of the robot. For more details about IPOPT and thenon-linear solver see [20].

It is then possible to compute the euclidian distancebetween the desired and achieved positions ρ(x, xd) and seta threshold ε defining the reachability Re(g) of the goal gas:

Re(g) : ρ(x, xd) < ε (9)

Once the goal is ”reachable”, the robot stops, lift itsreaching arm to avoid collisions with the ground and andthen moves it to the computed position q that achieves xusing the dicrete system desribed in Section II-B.

E. Planning

The purpose of the planning module is to have to robotnavigate in a world composed of multiple goals and multipleobstacles. In a metaphor of a real infant, one can think ofthe goals as toys scattered around a room with furnitures andvarious kinds of obstacles. The input of this module is a set of3D positions of goals and obstacles sent by the vision trackerdescribed in Section II-C and expressed in robot coordinates.

1 2 3 4 5 6 7 8 9 101

2

3

4

5

6

7

8

9

10

2 4 6 8 10

5

10

−50

0

50

Fig. 2. An example of map of the environment (top left) with the obstaclesrepresented as red circles and the goals as green ones. The associatedpotential field is represented on the right. The field of action of an attractivepotential is wider than that of a repulsive one due to the maximum distanceof action ρ0 = 2 and kr = 4 of the repulsive potentials (see Equation 10

Every time a head scanning is finished, a partial mapof the environemnt is generated and, attractive potentialsUa(q) are placed on the goals and a repulsive ones Ur(q)

on the obstacles, q

(xy

)being the robot 2D position on

the map. Figure 2 shows an example of a partial map of theenvironment and the potential field associated to it.

Usual potential methods have a unique goal and thusdefine the attractive potential and the correspondingattracting force proportional to the distance to the goal oreven to a power of it. Having multiple goals, we cannotdefine it this way since the robot would keep oscillatingbetween goals without ever reaching one.

Instead, we chose to define the attractive and repulsiveforces (~Fa) and (~Fr) created respectively by the goal andobstacle potentials as:

~Fa = −∇Ua(q) = −ξ 1ρ(q)ka

~u

~Fr = −∇Ur(q) =

{η 1ρ(q)kr

( 1ρ(q) −

1ρ0

)~u if ρ ≤ ρ0,

0 if ρ > ρ0.(10)

where :• ρ(q) is the euclidian distance between the origin of the

potential and the robot.• ρ0 is the maximum distance of influence of a repulsive

potential.• ka and kr are positive factors that determine the curva-

ture of the potential surface.• ξ and η are positive scaling factors.• ~u = ∇ρ(q) is a unit vector oriented away the origin of

the potential and towards the robot.The resulting force ~FΣ that applies on the robot is then

simply:

~FΣ =n∑i=0

~Fai +m∑j=0

~Frj (11)

n being the number of goals and m the number of obstacles.The robot moves then of a small distance following this

resulting force. Its displacement ~D and angle of rotation φcan be defined as :

~D = ∆~FΣ

||~FΣ||φ = atan2(~r⊥.~u, ~r.~u)

(12)

Where where ~r is the current direction of motion of therobot and ∆ is a small distance to be defined and ⊥. is theperp-dot product.

Here we only compute φ explicitly and let ∆ be thedistance achieved by the robot between two refreshing ofthe potential field (between two full scans). Note that theactual rotation angle of the robot corresponds to the torsoroll angle of the robot (see Figure 3

The values of kr, ka and ρ0 in Equation 10 influencestrongly the shape of the potential field. Figure 4 shows thisinfluence for two different values of ρ0. A potential witha low k (kr or ka) has a sligher slope, and thus a largerrange of influence than one with a big k. By variying ρ0,one can explicitly limit the range of influence of an obstacle

ρ0αθ

R

Fig. 3. A snapshot of the Webots world with annotations of the importantquantities. The red pylon is an obstacle, the green cube a goal (notice theARToolKit markers on them). α is the field of view of the robot, θ theextended field of view and R the radius of curvature

potential. Setting a low ρ0 is particulary useful if one wantsthe robot to be able to squeeze in between obstacles. Settinga high kr may have a similar effect, while also changing theslope of the potential field. Section III presents a study ofthe influence of these various parameters on the performanceof robots with different minimum curvature radius.

510

510

0

2

4

k = 1

510

510

0

2

4

k = 4

510

510

0

2

4

k = 1

510

510

0

2

4

k = 4

Fig. 4. Influence of k (kr or ka) on the shape of the potential field forρ0 = 2 and ρ0 = 10. the slope of the potential shape increases with k,while ρ0 allows an explicit limition of the range of influence of the potiential

III. RESULTS

In order to study the influence of the various parametersdescribed in Section II-E on the performance of the motionplanning algorithm with the constraints of the real robotwe used a two stage simulation approach: first using a 2Dsimulator, having enough simplicity and speed to test a widerange of parameters and then using a real robotics simulatorenvironment. The third stage, implementation on the realrobot, has been started and will be discused at the end ofthis section.

We performed a series of systematic test using a simple2D simulator on the following parameters: ka, kn, andρ0 and R, the radius of curvature of the robot. In thissimulator, no vision is involved but the field of view ofthe robot is constrained geometrically. Thus obstacles andgoals are only ”seen” by the planning algorithm if they are

in an area corresponding to a field of view of 120, witha depth of two meters, from the robot position and alongits orientation. These values are coherent with the realrobot properties. We generated 70 corridor-like worlds ofdimension 4 × 40 m, containing 10 goals and 15 obstacleseach, and enclosed by walls of obstacles. The goals andobstacles were randomly position with the only conditionthat the distance between each of them was at least 1m.This is to ensure a rather uniform distribution of goalsand obstacle and avoid worlds with conglomerates thatwould be impossible for any parameters and thus wouldlead to similar scores for all trials. The parameters rangedas follows: kr = [0.5, 1, 2, 4, 6], ka = [0.5, 1, 2, 4, 6],ρ0 = [1, 1.3, 1.5, 2], R = [0.7, 1, 2, 3, 4, 6] (42000 runs).

The results of these systematic tests are presented in Figure5.

0 1 2 3 4 5 60

1

2

3

4

5

6

7

8

R (in m)

num

ber

of g

oals

/obs

tacl

es

goals reachedobstacles collided

0

2

4

6

11.2

1.41.6

1.820

2

4

6

8

10

R (in m)ρ0 (in m)

num

ber

of g

oals

/obs

tacl

es

goals reachedobstacles collided

24

6

123456

0

1

2

3

4

5

6

7

8

kr

R = 1m − ρ0 = 2m

ka

num

ber

of g

oals

/obs

tacl

es

12

34

56

2

4

6

1

1.5

2

2.5

3

3.5

4

4.5

kr

R = 4m − ρ0 = 2m

ka

num

ber

of g

oals

/obs

tacl

es

goals reachedobstacles collided

Fig. 5. Results of the systematic tests using the 2D simulator. Top left:number of reached goals and obstacles collided over the whole pool of tests.Top right: influence of ρ0 on the performance. Bottom left: influence of ka

and kr on the performance for a small radius of curvature (R = 1) forρ0 = 1. Bottom right: influence of ka and kr on the performance for a bigradius of curvature (R = 4) for ρ0 = 2.

The top left graph shows the mean number of reachedgoals and collided obstacles over all runs for each value ofR. As can be expected the smaller the radius of curvatureR the better the performance. The fact that the number ofobstacles collided is lower for R = 6 than for R = 4 isdue to the corridor shape of the world tested. Indeed, forR = 6 the robot moves almost in straight line and thus theprobability to collide with the walls is reduced.

The top right surface plot shows the influence of ρ0 on thenumber of goals reached and obstacles collided. For low R,the value of ρ0 has barely any influence on the performance.

The small radius of curvature of the robot allows it to avoidobstacles even if they influence its motion only very late(ρ0 small). For higer R however, the performance stronglydecreases with ρ0. This time the robot can only avoidobstacles if it can anticipate enough (ρ0 big).

The bottom two graphs show the influence of kr and kaon the performance for a small and a big value of R, and forρ0 = 2. When the radius of curvature is sufficiently small,the values of kr and ka are, like ρ0 in the previous graph,not critical. This independance of the parameters for small Ris a good feature of the planning algorithm for real roboticsapplications, since it means that the system is stable in theparameter space. Very small values of kr and ka lead toslightly lower performace, since the robot cannot get nearenough obstacles to performe quick maneuvers, which wouldbe made possible and safe by its small curvature.

For big R the number of collided obstacles mostlyincreases with the value of kr, since for big kr the influenceof the obstacle potentials decreses rapidly with the distanceand so the robot cannot anticipate enough to cope with itsbig radius of curvature. A less intuitive observation is thatthe number of reached goals decreases for small values ofka. This can be explained by the fact that, where severalgoals are in the field of view of the robot, and ka is small,their influence would mosly balance until one is significantlynearer than the other. At that point however, with a big R,the robot would not be able to turn fast enough to reach thenearest one. This happens in Figure 6 for R = 4 for the 2Dsimulator. At y ≈ 12 the robot passes in between two goalswithout reaching any of them.

These observations are useful to adapt a potential fieldbased planning algorithm to the constraint of a real non-holonomic robot. But first we have to check that the behaviorof a real robot would match that of the 2D simulation,at least concerning curvature radius issues. We used theWebots [21] robotics simulator, which is based on the OpenDynamics Engine for the physics simulation and on OpenGLfor the rendering. It is rather realistic in the sense that isenables to set robot specific contraints such as joints limitsof position, velocity, acceleration and force, as well as theproportional term P of the low level controller. Parameterof the environment like gravity, friction coefficient etc. arealso open. The Webots model of the iCub fully respects theDaenavit-Hartenberg parameters of the real robot as wellas the limits of the joints. The same controller is used inWebots and on the real robot thanks to a YARP interfacefor Webots. Detection of the obstacles and goals is notgeometrical anymore as in the 2D simulator but uses theperspective projection webots cameras, the ”eyes” of therobot, to perform visual processing using the ARToolKitbased marker tracker described in Section II-C. Hence de-tection is not deterministic anymore but subject to noise inthe position extraction of the markers. Locomotion of courseis significantly different since it uses the CPG based systemdescribed in Section II-B and not a simple translation likein the 2D simulator. This also induces noise in the vision

0 1 2 3 4 5 60

1

2

3

4

5

6

7

8

9

R (in m)

num

ber

of g

oals

/obs

tacl

es

goals reachedobstacles collided

1 1.5 2 2.5 3 3.5 41

2

3

4

5

6

7

8

9

R (in m)

num

ber

of g

oals

/obs

tacl

es

goals reachedobstacles collided

−2 0 2

0

5

10

15

20

25

30

35

40

x (in m)

y (in

m)

R=1

−2 0 2

0

5

10

15

20

25

30

35

40

x (in m)

y (in

m)

R=2

−2 0 2

0

5

10

15

20

25

30

35

40

x (in m)

y (in

m)

R=3

−2 0 2

0

5

10

15

20

25

30

35

40

x (in m)

y (in

m)

R=4

Fig. 6. Comparison of the performance of the planning algorithm fordifferent radius of curvature in one world using the the 2D simulator(top left) and Webots (top right). Comparison of trajectories with similarperformance in Webots (blue solid line) and the 2D simulator (black dashedline) for different values of R (bottom figure).

tracker due to movements of the head and a high variancein the potential field generation since markers are constantlyentering and escaping the field of view of the robot, causingthe modifications in the potential field. To cope with theseissues, we performed noise filtering at the vision level andintroduced a short term memory at the planning level. Thismemory introduces damping in the changes of the potentialfield and thus prevents the robot from constantly changingdirection.

Due to the complexity of the simulator + locomotion +vision tracker + planning alorighm system, we only per-formed a limited amount of tests, to prove the efficiencyof the whole framework and show that the result matchthat of the 2D simulation. We chose a world that gavesignificantly different results for different values of R inthe 2D simulations. We run 5 runs for each values of R ≈[1, 2, 3, 4]. We could not find a stable gait leading to R < 1(the robot would not move) or R > 4 (the robot would movein straight line). A significant difference between the waythe radius of convergence is computed in the 2D simulatorand in Webots is worth mentioning. In webots, turning isachieved by changing the torso roll angle (see Figure 3)and modifying the amplitudes of the left and right limbsaccordingly. However, the robot cannot reach its maximum

turn angle at once since it cause a lot of sliding and bigconstraints on the motors. Thus at each step the turn angleincreases by a small amount, and so the radius of curvatureis not constant, unlike in the 2D simulator. The values of Rgiven before are the curvature after the maximum turn anglehas been reached, which may be different from the actualturn angle while navigating.

Figure 6 (top two graphs) shows the performance of theplanning for different values of R in Webots and in the 2Dsimulator. Interestingly the relation between the maximumcurvature and the number of goals reached and obstaclescollided is qualitatively the same as in the 2D simulator.Thus the 2D simulator is a good approximation of theWebots simulation which should be a good approximation ofwhat should happen when implementing on the real robot.However, quantitatively, the values are different, the valuesin the 2D simulator corresponding approximatively to thosein Webots for 2 × R. This is mostly due to the imperfectmatch between the curvature in both simulators, as discusedbefore.

Overall the planning algorithm proved to solve well theplanning problem with the proper parameters. For R = 1 therobot was able to reach 9 goals out of 10 and collide withno obstacle (even reach 10 in the 2D simulator). The fastonline refreshing of the potential field during locomotionallows the robot to handle dynamical enviroments. Thevideo attached with this paper shows the iCub navigating ina Webots world with only one goal moved around manually.The obstacles were also moved during this experiment. Inthe end the iCub was able follow the moving goal whileavoiding the obstacles.

Finally we implemented the crawling, vision and reachingmechanisms on the real iCub robot. We did not yet imple-ment steering and thus did not test the planning algorithmon the iCub. The experiment consisted in having the robotcrawl for a couple of meters, then detect a marker placed onthe ground, follow it with its head and reach it with its rightarm. Crawling proved very stable even though controlled inopen loop, and the robot was able to switch instantly fromrythmic to dicrete movement when reaching. Visual detectionand tracking showed good performance and the robot seldomlost track of the marker before reaching it. The attachedvideo presents this experiment. Figure 7 show the outputof the CPG and the actual trajectories of the four controlledjoints of the right limbs (the left ones are symetrical). Therobot followed very closely the commands sent by the CPGwhen crawling and reaching. The small offset between theencoders and the CPG output when reaching for joint 0 is dueto the velocity limit of this joint. The small oscillation whencrawling for joint 2 is due to a mechanical coupling of thethree shoulder joints, but was not problematic for crawling.

IV. CONCLUSIONS

We have presented in this document a full system toallow a humanoid to navigate in a simplified environmentusing only its vision to get knowledge about its surrounding.

Crawling Reaching

DiscreteSystem

Rythmic Sytem

Fig. 7. Output of the CPG (solid line) and encoders of the four controlledjoints of the right leg and right arm during crawling then reaching. Joint 0:shoulder/hip pitch, joint 1: soulder/hip roll, joint 2: shoulder/hip yaw, joint3: elbow/knee

The low level locomotion mechanism, the Generator, usescoupled non-linear oscillators (CPG), to generate complexlocomotion patterns using simple control inputs. These in-puts are modulated by the Manager to cope with internalconstraints. High level command are sent by the Planner tothe manager. This locomotion framework is able to performrythmic movements, for crawling, and discrete movementsfor reaching. At the highest level, we designed a motionplanning system based on potential field and using thevisual cues provided by a maker based tracker. The wholelocomotion + vision + motion planning + reaching is thusfully autonomous.

We proved the efficiency of our system on a realisticrobotics environment and using a 2D simulator to studythe influence of the various parameters of the potentialfield equations on the performance while respecting theconstraints of non-holonomic robots. We showed that forsmall radius of curvature, the system is very stable to changesin parameters, while for big radius of curvature, setting thevalues of kp and kn low and ρ0 high allows the robot toanticipate more and compensate for its big curvature.

Once specificity of our work worth mentioning is the shapeof the environments tested: corridor like. We suppose that thebehavior of the system would be similar in different shapeof environments but proving it is left for future work.

Implementation on the real iCub showed promising results,the robot beeing able to crawl, track a marker on the groundwhile crawling and finally reach it.

Further work should include more systematic tests of theplanning system in the realistic robotics environment and onthe real robot.

REFERENCES

[1] S. Inagaki, H. Yuasa, T. Suzuki, and T. Arai, “Wave cpg modelfor autonomous decentralized multi-legged robot: Gait generation and

walking speed control,” Robotics and Autonomous Systems, vol. 54,no. 2, pp. 118 – 126, 2006. Intelligent Autonomous Systems.

[2] H. Kimura, S. Akiyama, and K. Sakurama, “Realization of dynamicwalking and running of the quadruped using neural oscillator,” Auton.Robots, vol. 7, no. 3, pp. 247–258, 1999.

[3] J. Buchli, F. Iida, and A. J. Ijspeert, “Finding Resonance: AdaptiveFrequency Oscillators for Dynamic Legged Locomotion,” in Proceed-ings of the IEEE/RSJ International Conference on Intelligent Robotsand Systems (IROS), pp. 3903–3909, IEEE, 2006.

[4] A. J. Ijspeert, A. Crespi, D. Ryczko, and J.-M. Cabelguen, “Fromswimming to walking with a salamander robot driven by a spinal cordmodel,” Science, vol. 315, no. 5817, pp. 1416–1420, 2007.

[5] G. Taga, Y. Yamaguchi, and H. Shimizu, “Self-organized control ofbipedal locomotion by neural oscillators in unpredictable environ-ment,” Biological Cybernetics, vol. 65, pp. 147–159, July 1991.

[6] L. Righetti and A. J. Ijspeert, “Programmable Central Pattern Gener-ators: an application to biped locomotion control,” in Proceedings ofthe 2006 IEEE International Conference on Robotics and Automation,2006.

[7] A. J. Ijspeert, “Central pattern generators for locomotion controlin animals and robots: a review,” Neural Networks, vol. 21, no. 4,pp. 642–653, 2008.

[8] S. M. LaValle and M. S. Branicky, “On the relationship betweenclassical grid search and probabilistic roadmaps,” 2004.

[9] R. Geraerts and M. H. Overmars, “Sampling techniques for proba-bilistic roadmap planners,” 2004.

[10] J. Borenstein, Y. Koren, and S. Member, “The vector field histogram -fast obstacle avoidance for mobile robots,” IEEE Journal of Roboticsand Automation, vol. 7, pp. 278–288, 1991.

[11] D. Fox, W. Burgard, and S. Thrun, “The dynamic window approach tocollision avoidance,” IEEE Robotics and Automation Magazine, vol. 4,1997.

[12] O. Khatib, “Real-time obstacle avoidance for manipulators and mobilerobots,” Int. J. Rob. Res., vol. 5, no. 1, pp. 90–98, 1986.

[13] K. Sato, “Collision avoidance in multi-dimensional space using laplacepotential,” in Proc. 15th Conf. Robotics Soc. Jpn., 1987.

[14] D. Jo and K. Didier, “A reactive robot navigation system based on afluid dynamics metaphor,” in Parallel Problem Solving from Nature,vol. 496 of Lecture Notes in Computer Science, pp. 355–362, SpringerBerlin / Heidelberg, 1991.

[15] S. Degallier, L. Righetti, L. Natale, F. Nori, G. Metta, and A. Ijspeert,“A modular bio-inspired architecture for movement generation for theinfant-like robot iCub,” in Proceedings of the 2nd IEEE RAS / EMBSInternational Conference on Biomedical Robotics and Biomechatron-ics (BioRob), 2008.

[16] RobotCub, “http://www.robotcub.org/.” An Open Framework forResearch in Embodied Cognition.

[17] G. Metta, P. Fitzpatrick, and L. Natale, “Yarp: Yet another robot plat-form,” International Journal of Advanced Robotics Systems, specialissue on Software Development and Integration in Robotics, vol. 3,no. 1, 2006.

[18] L. Righetti and A. J. Ijspeert, “Pattern generators with sensoryfeedback for the control of quadruped locomotion,” in Proceedings ofthe 2008 IEEE International Conference on Robotics and Automation(ICRA 2008), pp. 819–824, 2008.

[19] D. Wagner, “http://studierstube.icg.tu-graz.ac.at/handheld ar/-artoolkitplus.php.” Augmented Reality Tracking Library.

[20] A. Wachter and L. T. Biegler, “On the implementation of an interior-point filter line-search algorithm for large-scale nonlinear program-ming,” Mathematical Programming, vol. 106, pp. 25–57, 2006.

[21] Webots, “http://www.cyberbotics.com.” Commercial Mobile RobotSimulation Software.