Pranav_Shah_Report

Optimal path planning under sensing and motionuncertainty

Masters report

By

Pranav R Shah

A report submitted in partial satisfaction of therequirements for the degree of

Masters of Science, Plan II

in

Mechanical Engineering

at the

University of California at Berkeley

Fall 2010

Committee in Charge:

Professor Pieter Abbeel

Professor Karl Hedrick

Abstract

The traditional approach to motion planning and control has been to usepath planners like rapidly-exploring random trees (RRT) to find a collisionfree path and then delegate the task of following this trajectory to the con-troller. With this approach the controller is expected to overcome uncertain-ties, possibly using feedback from sensors, while following the trajectory. Inthis report, we propose an approach which takes into account various uncer-tainties at the motion planning stage. This enables the motion planner tochoose a trajectory which maximizes the probability of achieving the task.We illustrate this approach by simulating a 7-DOF Barrett WAM performingthe task of object pick up and insertion into a hole.

Acknowledgments

I would first like to thank Pieter for his guidance throughout this work. Hehas always made himself available for any discussions I needed to have andhas been a constant source of motivation and support. I have learnt a lotfrom his dedication to work and consider the opportunity I got to work withhim as a privilege.

I would also to thank my colleague and friend Kevin for working togetherwith me on this project through the summer. It was great exchanging ideaswith him on every aspect of this work which in turn lead to significant learn-ing. I hope we stay in touch for days to come.

Thank you to Professor Hedrick for reading through my report and agree-ing to serve on my committee. I would also like to thank him for makinghimself available for a discussion whenever necessary and his regular support.

Finally, I thank all my friends in Berkeley for the great moments we have hadtogether. Berkeley would not have been fun without you guys. Go Bears!

1

Contents

Acknowledgments 1

1 Introduction 41.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . 41.2 Problem Addressed . . . . . . . . . . . . . . . . . . . . . . . . 61.3 Related Work . . . . . . . . . . . . . . . . . . . . . . . . . . . 71.4 Organization of the Report . . . . . . . . . . . . . . . . . . . . 8

2 Preliminaries 92.1 Probability and Random Variables . . . . . . . . . . . . . . . 9

2.1.1 Probability space . . . . . . . . . . . . . . . . . . . . . 92.1.2 Random variables and density function . . . . . . . . . 102.1.3 Random vectors . . . . . . . . . . . . . . . . . . . . . . 102.1.4 Conditional density and Bayes rule . . . . . . . . . . . 10

2.2 Gaussian distribution and Kalman filtering . . . . . . . . . . . 102.2.1 Gaussian random vector . . . . . . . . . . . . . . . . . 102.2.2 Conditional Gaussian distribution . . . . . . . . . . . . 112.2.3 Kalman filter . . . . . . . . . . . . . . . . . . . . . . . 112.2.4 Extended Kalman Filter (EKF) . . . . . . . . . . . . . 12

2.3 Configuration Space and Motion Planning . . . . . . . . . . . 132.3.1 Configuration space . . . . . . . . . . . . . . . . . . . . 13

2.4 Motion Planning . . . . . . . . . . . . . . . . . . . . . . . . . 132.4.1 Rapidly-Exploring Random Tree (RRT) . . . . . . . . 13

2.5 Vision System . . . . . . . . . . . . . . . . . . . . . . . . . . . 14

3 Motion Planning under Uncertainty - Evaluating Trajecto-ries 173.1 Robot and Environment Setting . . . . . . . . . . . . . . . . . 18

2

3.2 Visual Servoing for Object Pickup . . . . . . . . . . . . . . . . 183.3 Motion Planning with Object in Hand . . . . . . . . . . . . . 19

3.3.1 State-space and dynamics model . . . . . . . . . . . . 203.3.2 Observation model . . . . . . . . . . . . . . . . . . . . 213.3.3 Object-pose using EKF . . . . . . . . . . . . . . . . . . 23

3.4 Trajectory Generation and Evaluation . . . . . . . . . . . . . 243.4.1 Trajectory generation . . . . . . . . . . . . . . . . . . . 243.4.2 Trajectory evaluation . . . . . . . . . . . . . . . . . . . 25

4 Simulations & Results 284.1 OpenRAVE - Motion Planning Simulation Environment . . . . 284.2 Procedure for Key Insertion . . . . . . . . . . . . . . . . . . . 294.3 Importance of continuous estimation to overcome uncertainties 32

4.3.1 Maximal uncertainties for parameters . . . . . . . . . . 334.3.2 Example of parameter estimation . . . . . . . . . . . . 34

4.4 Results from Trajectory Evaluation . . . . . . . . . . . . . . . 374.4.1 Simulation results - Key grasped in initial state . . . . 374.4.2 Simulation results - Key not grasped in initial state . . 39

5 Conclusion and Future Work 425.1 Conclusion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 425.2 Future Work . . . . . . . . . . . . . . . . . . . . . . . . . . . . 43

3

Chapter 1

Introduction

1.1 Introduction

Figure 1.1: Robot in an assembly line

Robotics is a field of study of computer-controlled devices which interactwith and manipulate objects in the physical world. Today, robotics andautomation is widely used in structured environments like manufacturingassembly lines to improve productivity and performance (figure 1.1). Therehave been efforts to extend the utility of robots in environments like homes,offices, etc but the bottle-neck to widespread use in these environments isthe ability of the robot to deal with their unstructured nature. Robots need

4

to accommodate the enormous uncertainty that exists in the physical worldbefore they can be widely deployed for tasks like mobility in homes and office,performing daily chores like washing dishes, doing laundry, etc (figure 1.2).

Figure 1.2: Robot loading a dishwasher

The uncertainty arises from multiple sources. Firstly, environments inplaces like homes are highly unpredictable and dynamic. Secondly, the sen-sors utilized by robots for perception involve noise and uncertainties in them.Errors in sensing could arise due to limited range and resolution of sensors,imperfect calibration or presence of statistical noise in their measurementoutput. Uncertainty also arises due to imperfect modeling of the robot andits environment as all models of the real world are simplified and approxi-mated.

Robots operating in such unstructured environments with imperfectionsin perception are required to take decisions with limited information it ob-tains from models and sensors rather than making optimal decisions with

5

absolute certainty. Thus, accounting for uncertainty in decision making is avery important step towards making robotic systems robust.

1.2 Problem Addressed

The focus of this report is to describe methodologies accounting for sens-ing and motion uncertainties in the path planning process and subsequentlychoosing paths which are optimal in some sense. The specific task that welooked into for this work was the problem of object pickup and insertion intoa hole using a 7-DOF robot. Motivation for this problem arises from taskslike performing furniture assembly, opening room doors using a physical key,etc. A task like furniture assembly would typically involve multiple subtasksfor tight fit insertion of one part into another. These tasks involving inser-tion usually have to be performed with high precision as there is very littlemargin for error to achieve the task successfully. Hence, if such tasks have tobe automated, accounting for sensor and motion uncertainty becomes evenmore vital.

In our simulation studies, we used a 7 degree of freedom(DOF) Barrett-WAM robot. We looked into the task of picking up a rectangular object andinserting into a hole with dimensions marginally bigger than dimensions ofthe block. We assumed the robot is equipped with a joint angle measurementencoder for each of its joints in order to get the state of the robot. The envi-ronment was also fitted with two cameras for perceiving object and the hole.Various kinds of uncertainties were introduced in simulation. We assumedthe camera parameters, namely focal length and camera optical center, hada fixed unknown offset. Additionally we assumed that camera measurementoutput in terms of pixels was corrupted with additive Gaussian noise. Wealso assumed additive Gaussian noise in joint angle encoder measurementsas well as in joint angle motion.

The presence of uncertainties described above manifests itself in the formof imperfect robot state information as well as imperfect grasp of the objectwhich could ultimately lead to task failure. In our approach, we simulatedmultiple trajectories for the robot hand to move the grasped object from theinitial configuration to a configuration close to the hole. The trajectorieswere generated by applying RRT algorithm in three dimensional space forthe object position. The orientation of the object along the trajectory wasdetermined by doing a linear interpolation between the initial and final ori-

6

entations. This was done to ensure trajectories generated were smooth. Wepropagate variance in object position and choose that trajectory for whichvariance is least at the end point as the optimal one. The “magnitude” ofthe propagated variance is a measure of the cumulative information that therobot has gained while following the trajectory. For a task like object in-sertion, the precision in position of the object in the plane parallel to planeof the key hole and the orientation along the longitudinal axes are very im-portant. Specifically, in our case we looked at the weighted combination ofthe sum of the variances in the two dimensions of the parallel plane as wellas the orientation along the longitudinal axis.

Simulation studies for a Barrett-WAM 7 DOF robot were performed un-der two scenarios to judge the efficacy of the approach above. In the firstset of simulations, the insertion task was performed without accounting foruncertainty in motion planning and determining the uncertainty limits forsuccessful task completion. In the second set of simulations, we performedthe same task but by accounting for uncertainties along the trajectories andthen evaluating them. We found that by using the approach described abovein our experiments we could handle 10 to 30 times more uncertainty in sens-ing and motion and still perform the task successfully.

1.3 Related Work

A substantial body of work has addressed uncertainty in motion planning.Three type of uncertainties are typically approached: motion uncertainty,sensing uncertainty, and uncertainty about the environment. The approachpresented in this report focuses largely on the first two.

Usually a path planner takes into consideration perfect knowledge of thestate and assumes that the controller will deal with all uncertainties. Plan-ners that specifically take into account motion uncertainty include [8], [9],[14]. These planners plan paths that avoid rough terrain. In [7] and [2] mo-tion uncertainty is included in the path planning for a robot manipulator,but the partial observability and sensing uncertainty is not considered. AlsoMarkov Decision Processes, for instance, can be used with motion uncertaintyto optimize probability of success [15].

Existing planners that are most directly related to the work take intoaccount available sensing capability to maximize the probability of arrivingat the goal or to minimize expected cost [5], [6], [10], [11]. However, these

7

algorithms implicitly assume that they receive maximum-likelihood measure-ments from the sensors, which does not result in the true probability distri-butions of the state of the robot, but rather a measure of how well one willbe able to infer the state.

Besides sensors, [1] also takes into account the controller that will be usedfor executing the path, and computes the true a-priori probability distribu-tions of the state of the robot along its future path. The approach describedin this report gains its motivation from [1] and extends by applying it to amore complicated application with a kinematic chain robot. In this reportthe state of the system includes camera’s intrinsic and extrinsic parametersand the identification of the grasp and allows a more exhaustive estimationof the uncertainties. In addition occlusion and object collision are taken intoconsideration.

1.4 Organization of the Report

The report henceforth is organized as follows. Chapter 2 discusses certainpreliminaries needed to understand the rest of the report. An advancedreader may skip this chapter and use it only as a reference. Chapter 3describes the formulation and the methodology of our motion planning ap-proach which takes uncertainty into account. Chapter 4 discusses the resultsof applying this approach to the task of object pickup and insertion using a7-DOF Barrett WAM robot. Finally, Chapter 5 concludes with a summaryand possible directions for future work.

8

Chapter 2

Preliminaries

This chapter introduces some preliminary material useful for subsequentchapters. An advanced reader may choose to skip this chapter.

2.1 Probability and Random Variables

2.1.1 Probability space

Consider an experiment E which is conducted many times. Each trial of Eresults in some outcome ω. The set of all possible outcomes is called samplespace and denoted by Ω.

Subsets of the sample space Ω are called events. The set of all events isdenoted byA. A probability space is a triplet (Ω,A,Prob) where Prob : A →R is a function which satisfies the following axioms:

1. 0 ≤ Prob(A) ≤ 1 for all A ∈ A

2. Prob(Ω) = 1

3. If A ∩B = Φ then Prob(A ∪B) = Prob(A) + Prob(B)

4. If An∞1 is a sequence of events such that An → A thenProb(An)→ Prob(A)

9

2.1.2 Random variables and density function

Given a probability space (Ω,A,Prob), a random variable X is a functionX : A→ R. The Probability distribution function associated with X is

PX(x) = ProbX ≤ x

In the case where PX(x) is differentiable everywhere, we define the probabilitydensity function as

pX(x) =d

dxPX(x)

2.1.3 Random vectors

Given random variables X1, X2 . . . Xn, we define X = [X1X2 . . . Xn]′ to be arandom vector with probability distribution

PX(x) = ProbX1 ≤ x1, X2 ≤ x2, . . . , Xn ≤ xn

and we define joint probability density as

pX(x) =∂n

∂x1∂x2 . . . ∂xnPX(x)

2.1.4 Conditional density and Bayes rule

The Bayes rule helps get distribution of a random variable given instantia-tions of other random variables with which it is jointly distributed. Accordingto Bayes rules, given Y = y0, the density function of the random variable Xis given by

pX|Y=y0 =pX,Y (x, y0)

pY (y0)

2.2 Gaussian distribution and Kalman filter-

ing

2.2.1 Gaussian random vector

A random vector X = [X1X2 . . . Xn]′ is said to have Gaussian distributionif there exists a non-singular matrix Σxx such that the joint density can

10

expressed as

pX(x) =1

[(2π)ndet(Σxx)]1/2exp(−(x−mx)

′Σ−1xx (x−mx)

2

)In the above equation m ∈ Rn and Σxx ∈ Rn×n are the mean and covari-ance of the random vector respectively and we denote the distribution byN (mx,Σxx)

2.2.2 Conditional Gaussian distribution

Theorem 1 Let X ∈ Rn and Y ∈ Rm be two random vectors with jointdistribution given by

Z =

[XY

]∼ N

([mX

mY

],

[ΣXX ΣXY

ΣY X ΣY Y

])Then, X|Y = y0 ∼ N

(mx|y0 ,ΣXX|Y=y0

)where

mx|Y=y0 = mx + ΣXY Σ−1Y Y (y0 −mY ) (2.1)

Σx|Y=y0 = ΣXX − ΣXY Σ−1Y Y ΣT

XY (2.2)

The proof of the above theorem can found in [12]

2.2.3 Kalman filter

Consider a linear discrete-time dynamical system

xt+1 = Axt +Buut +Bwwt

yt = Cxt + vt

where xt ∈ Rn is the state, ut ∈ Rn is the control input, yt ∈ Rp is theobserved output and wt ∈ Rn and vt ∈ Rp are process noise and measurementnoise respectively. We make the following assumptions:

• The initial state x0 ∼ N (x0,Σ0)

• x0, w0, w1, . . . , v0, v1, . . . are jointly Gaussian

• wt are IID with Ewt = 0, EwtwTt = W

11

2.3 Configuration Space and Motion Planning

2.3.1 Configuration space

The environment of a robot usually has a number of obstacles which it isexpected to avoid collision with while in motion. Thus, in order to movea robot, a decision has to be made for a collision-free path from the initialto the final robot state. This path is chosen in a space in which each ele-ment corresponds to a unique state of the robot. Such a space is called aconfiguration space.

More formally, we define configuration of a robot system as a completespecification of the position of every point of that system. The space ofall position configurations of the system is called a configuration space. Thedimension of the configuration space equals the number of degrees of freedomof the robot system. For a mobile-robot constrained to move on a 2D planewithout rotating, the dimension of the configuration space is two. On theother hand, for a robot which is free to rotate the dimension is three.

We denote the subset of the configuration space which is collision free byQfree.

2.4 Motion Planning

Given the collision free subset of configuration space, Qfree, the task of amotion planning algorithm is to decide on a path in Qfree between the ini-tial and final state of the robot. A number of techniques exist to find sucha collision free path. A family of these techniques are based on probabilis-tic sampling of the configuration space and these are called sampling-basedmotion planners. They differ among themselves through strategies for gen-erating samples and connecting samples with paths to obtain solution to thepath-planning problem.

The specific technique we utilize as part of this work is the rapidly-exploring random tree (RRT).

2.4.1 Rapidly-Exploring Random Tree (RRT)

RRT is a single-query planning algorithm that efficiently covers the spacebetween the initial configuration qinit and final configuration qfinal. The

13

algorithm works by incrementally extending a tree of configurations fromthe initial state towards the goal state. We denote this expandable tree byT. At each iteration, a random configuration qrand is sampled uniformly inQfree. The nearest configuration, qnear, to qrand in T is found and an attemptis made to move a distance stepsize along the straight line joining qnear toqrand. If the newly generated configuration qnew is collision free then it isadded to the vertices of T and the edge (qnear, qnew) is added to the edgesof T. Figure 3.3 illustrates the extension of RRT for a point robot in a 2Dworkspace.

Figure 2.1: Extension of RRT [3]

The parameter stepsize is usually chosen dynamically based on the dis-tance qnear and qrand. For further details on how this parameter affects thealgorithm performance one may refer to [3].

2.5 Vision System

Vision systems are widely used in robotics as they provides with an enormousamount of information about the environment and enables rich, intelligentinteraction in dynamic environments. Computer stereo vision is the extrac-tion of 3D information from digital images obtained from several cameraperspectives. A common mathematical model used to explain behavior ofthese cameras is the pinhole camera model.

In pinhole camera model, points in the 3D space are mapped onto animage plane via affine projection. As shown in figure 2.2, point P in the

14

Figure 2.2: Affine Projection

3D world is projected onto the 2D image plane with point O as the center.Mathematically, this is expressed as:

~Y =

[Y1

Y2

]∼

fx1 0 00 fx2 00 0 1

︸︷︷︸

C

X1

X2

X3

(2.3)

where C is called the projection matrix. The parameter fx1 and fx2 are thefocal lengths of the lens in the X1 and X2 directions respectively.

The output of a charged-coupled device(CCD) camera system is in termsof 2D pixel coordinates expressed relative to the corner of an image plane.Coordinates for a representative image plane are shown in figure 2.3. In thisfigure the parameters cx and cy denote the optical center in pixel coordinates.The two parameters along with parameters fx and fy are called intrinsicparameters. Accounting for this coordinate change, the projection model

15

Figure 2.3: Figure showing camera intrinsic parameters

can be expressed as

~Y ∼

fx 0 cx0 fy cy0 0 1

︸︷︷︸

C

X1

X2

X3

~Y is in pixel coordinates (2.4)

In stereo vision, the pixel output from multiple cameras is used to locatethe object in the 3D space using a process called as triangulation [13]. Theaccuracy of this process in locating the object in 3D depends critically onknowing the intrinsic parameters accurately. The techniques for camera cal-ibration do help in identifying these parameters but any kind of uncertaintyin knowing them leads to errors in estimating 3D pose of the object andthis could adversely affect the ability of the robot in performing the taskaccurately.

The motion planning approach discussed in this report takes this uncer-tainty into account while deciding on a trajectory to achieve the task. Theapproach chooses that trajectory which minimizes the effect of these uncer-tainties in achieving the task specified.

In the next chapter we provide a formal problem statement and alsodiscuss details about the simulation environment used to demonstrate theapproach.

16

Chapter 3

Motion Planning underUncertainty - EvaluatingTrajectories

In order to achieve complex tasks in unstructured environments, robots areequipped with multiple sensors like cameras, laser range finders, etc. Thesesensors help the robot perceive its environment and plan safe paths for itsmotion. However, these sensors invariably suffer from calibration errors anduncertainty introduced from electrical noise. Therefore it is important totake these into account while planning safe paths for the robot.

In this chapter, we give details of our motion planning approach whichtakes into account uncertainty that exists in sensing and motion. We beginby giving a physical view of the problem by describing the robot and its envi-ronment (section 3.1). We then describe the visual servoing approach to pickup the object when the cameras used to perceive them have fixed offset errorsin their intrinsic parameters (section 3.2). We then mathematically state theproblem describing the state-space representation of the system (section 3.3).In the final section 3.4 we describe our motion planning approach in detail.

All work in this report has been done for the Barrett-WAM robot insimulation. However, the approach is general enough to be applicable tomany other robots and to a wide range of tasks.

17

3.1 Robot and Environment Setting

The BarrettWAM used in simulations for this report is a 7 degree of free-dom(DOF) robot with a reach of 1m. This robot arm is connected to aBarrettHAND which has 3 fingers with 2 joints in each of them allowing forgood dexterity. In our simulations, we simulate each of the 7 joints of thearm with rotary encoders fitted. This helps us know the state of the robot,though it is not known exactly because of assumed additive noise in each ofthese. We also assume there are optical features on the robot hand whichenable the hand to be tracked using cameras.

The environment is fitted with a static stereo vision system. This camerasystems helps track the robot hand, estimate the initial position of the objectand also position of the hole. We assume a pin-hole projection model for thecamera. We also account for marker occlusions as well as field of view whensimulating camera output.

As mentioned in 2.5, focal length and optical center are important pa-rameters for the camera system. There are camera calibration schemes toestimate these parameters. However, some amount of uncertainty is alwaysexpected in their estimate and this could adversely affect the 3D estimate ofan object obtained using cameras. Therefore, it is important for the robotto account for these uncertainties when planning its motions.

The next section describes the approach of visual servoing which is usedto pick up the object.

3.2 Visual Servoing for Object Pickup

An error in knowing the focal length and optical center manifests itself inincorrect estimates of the object’s initial position. However, having an accu-rate estimate of the relative distance and orientation of the object from therobot hand is very important for successful object pickup. This is achievedusing the idea of visual servoing which is incorporating visual feedback andthe approach we used is described below.

The object position estimate obtained using camera output is not accuratebecause of error in camera parameters. Performing inverse kinematics on thisobject position estimate gives the joint angles necessary at the final position.However, at these joint angles the robot may not necessarily be at the rightposition to grasp the object due to error in the estimate. Therefore, before

18

grasping the object we get the robot hand close to the object and ensureat least some features on the hand become visible in the cameras. Inversekinematics on hand position estimated from this camera output gives jointangles which are close to the actual joint angles but not the same, againbecause of error in camera parameters. The control input to then move therobot from close to the object position to object grasping is given by thedifference between these two joint angle estimates obtained above.

The basic principle of the approach is to make decisions of control inputfrom camera output such that the effect of errors in camera parameters iscompensated. The underlying assumption in the above procedure describedis that errors made in estimating the joint angle values in positions close tothe object before grasp and at the grasp position are the same and thereforewe can compensate for it by taking difference of the joint values to findcontrol input. This assumption is expected to hold well at positions close toobject grasping.

3.3 Motion Planning with Object in Hand

The quality of pickup of the object depends on how well the visual servoingperforms. However, it is very unlikely under practical conditions that thegrasp is exactly as expected. An imprecise grasp if not identified can leadto failure in the task of insertion. Therefore, it is very important to identifygrasp of the object with the least amount of uncertainty.

The ’quality’ of grasp estimate depends on the trajectory followed bythe robot with the object held by the end-effector. Along some trajectoriesfeatures on robot hand and/or object could be occluded or out of field of viewof the camera. The grasp estimate obtained along such trajectories could bepoor. On the other hand, there could be trajectories along which featuresare always visible and this would lead to much better estimates. Thus, inthis approach we seek to find a trajectory along which the estimate obtainedis ’best’ or in mathematical terms - it has the least variance.

We use the Kalman filter approach to incorporate information availablefrom all sensors into estimating the grasp. We include grasp into the statein our state space model of the robot system which is described next.

19

3.3.1 State-space and dynamics model

We describe configuration of the robot using the joint angles and we thereforeinclude them as part of the state. As discussed above, we also need to capturegrasp of the object in our state. We do this by including the deviation of theactual grasp from the ideal grasp as part of the state vector. We express thismathematically as Gx

Gy

Gz

= GactualPos −GidealPos

where GactualPos and GidealPos are positions of actual and ideal grasps respec-tively, measured relative to the robot hand. Similarly, the error in grasporientation is expressed as Gψ

Gϑ

Gφ

= TR2RPY (G−1idealOrt ∗GactualOrt)

where TR2RPY is a function which converts rotation matrices to Euler an-gles. We also include camera intrinsic - focal lengths f1,f2, optical centercx,cy - and extrinsic parameters (x1, y1, z1, x2, y2, z2, ψ1, ϑ1, φ1, ψ2, ϑ2, φ2) aspart of the state vector.

x = (θ1, θ2, θ3, θ4, θ5, θ6, θ7, f1, f2, Px, Py, x1, y1, z1, . . .. . . x2, y2, z2, ψ1, ϑ1, φ1, ψ2, ϑ2, φ2, Gx, Gy, Gz, Gψ, Gϑ, Gφ)

where θi is the i-th joint angle. The above is a 29-dimensional vector.The control inputs to the robot are the changes in joint angles that are

at each instant in time. Thus,

~u =

∆θ1

∆θ2

∆θ3

∆θ4

∆θ5

∆θ6

∆θ7

To account for uncertainty or noise in motion, we include independent addi-tive Gaussian noise with zero mean:

20

~m =

m1

m2

m3

m4

m5

m6

m7

∼ N (0, σ2

θI)

Assuming that the grasp does not change with time, we can express thestate space model as:

~x(t+ 1) = A~x(t) +B~u(t) +Bm ~m(t)

where the matrices A and B are as follows:

A = I29×29 B =

[I7×7 0

0 0

]Bm = B

3.3.2 Observation model

The robot receives feedback from two different feedback sensors: camerasand joint angle encoders. We model encoders as measuring the value of thejoint angle corrupted by additive Gaussian measurement noise. We expressthis model mathematically as:

henc(~x) = Cenc~x+ nencoders

whereCenc =

[I7×7 023×7

]nencoders ∼ N (0, σ2

neI)

Two camera track the position of the end effector and the object usingfeatures like checker boards, optical markers or identifiable edges or corners.Hence, the measurement ~z is a 2·n-D vector (where n is the number of visiblefeatures) consisting of the pixel coordinates of each visible features on theimage planes of both cameras, corrupted by measurement noise ncamera ∼N (0, σ2

ncI).

21

If we assume all features are visible (no occlusions) and within field ofview, then we have the following non-linear observation model:

hcm(~x) =

g1,1r

g1,1c

g1,2r

g1,2c

g2,1r

g2,1c

g2,2r

g2,2c...gn,1r

gn,1c

gn,2r

gn,2c

+ ncamera

where gi,jr is the row pixel value of feature i = 1, 2, . . . , n in camera j =1, 2. Similarly, gi,jc is the column pixel value. It should be noted thateach of these g’s are functions of state of the robot and therefore functionsof the state vector ~x. These functions indirectly capture the use of forward-kinematics to transform joint angles and grasp in the state vector ~x to handand object position and then projections of features on them from 3D spaceonto the 2D image plane using pin-hole projection model described as insection 2.5. These functions are expected to be non-linear and therefore theuse of Extended-Kalman filter is necessary to estimate state (section 2.2).

For EKF equations, the observation matrix C can be obtained as

Ccm =∂hcm∂~x

∣∣∣∣xt|t

where xt|t is the posterior estimate of state ~x. Note that the Ccm matrix is ofsize 2n× 29 where n is the number of features captured on camera. As dis-cussed above, function hcamera indirectly depends on the forward kinematicsas well as the camera projection model utilizing camera intrinsic and extrin-sic parameters. The way we obtain these partial derivatives is by numericallydifferentiating these functions about the state estimate xt|t.

If the marker is not visible due to occlusion or out of field of view, themeasurement vector is shortened by eliminating elements of the non visible

22

markers from the observation vector. This is due to the fact that an occludedmarker is not providing any information to the system.

3.3.3 Object-pose using EKF

The above state-space model along with the observation model can be putthrough an EKF to give an estimate of the state of the robot and the grasp atany point in time. The position of the robot hand depends on the joint anglestate vector and the grasp is relative to the hand position. Thus, knowingan estimate of the entire state vector ~x helps us get an estimate of absolutepose of the object.

Given a trajectory to be followed by the end effector from the state wherethe object has been grasped in its initial position to the final state where theobject is inserted or very close to the hole, we can use a combination of EKFand simple control to move the robot. The EKF gives an estimate of stateat each time step. The controller then utilizes this estimate to take actionto move the robot. This scheme is pictorially shown in figure 3.1. The state

Path planner

C Σ

Estimator

Θdes(t)

ΔΘ(t)

Θest(t)

d(t) np(t) nΘ(t)

Pm(t)

Θm(t) - + + + + +

+ +

Figure 3.1: EKF and controller for the robot system.

estimate and its variance at the last point of the trajectory can be utilizedto obtain estimate and variance of the absolute position of the object. Inour approach, we do this for multiple trajectories and choose trajectory forwhich this variance at the end point is least. Such a trajectory is expected

23

to succeed at the task of insertion with a high probability. We give furtherdetails on how this evaluation is done in sections 3.4 and section 4.4. However,before that we give details on how we generate multiple trajectories for robotmotion.

3.4 Trajectory Generation and Evaluation

3.4.1 Trajectory generation

To efficiently explore the high-dimensional space of the robot manipulatora random path planner is used. For this purpose the Random Rapidly-exploring Tree (RRT) algorithm, as discussed in section 2.4 is used to gen-erate a large set of candidate paths. The RRT algorithm can be used togenerate paths in both the configuration space or work space of the robot.Both variants were implemented. In figure 3.2 an example of a path plannedin the 7-dimensional configuration space of the robot and projected in the3-Dimensional work space is depicted.

As can be seen from the figure, the paths planned in the configurationspace are not very smooth in the work space. Hence a variant of the pathplanned in the work space is chosen as it generates a smoother path. Anexample of path planned in the work space can be seen in figure 3.3.

The path generated is the path that the end effector will follow in thework space. Every new random generated point is a 3-Dimensional pointin the work space. For every new randomly generated point the collisionchecker is called once to see if this end-effector position is in collision. Ifthis end-effector position is not within reach of the robot or if the robot is incollision with any obstacle or with itself, a new random point is generated.Furthermore, if the robot collides during the motion from one point to thenext the edge is eliminated and a new random point is generated. Thisguarantees a collision free path which can be followed by the robot.

The RRT is only responsible for the position of the end effector. Theorientation is computed in the following way. A linear interpolation betweenthe final and the actual orientation is calculated. The difference in orientationbetween the final and the initial orientation is divided by the same numberof steps that the RRT algorithm computes for the position. At each step afraction of this linear rotation interpolation is added. So the end effector’sorientation changes smoothly from the initial to the final orientation. This

24

−0.4

−0.2

0

11.11.21.31.41.5

0.8

0.85

0.9

0.95

1

x−axis

Start!

y−axis

Goal!

z−axis

Figure 3.2: Example of a possible path computed with the RRT algorithmin the configuration space.

prevents the end effector from making jagged motions.An example of path a computed in the working space of the robot is

displayed in figure 3.3. The blue lines are the edges among the nodes of thetree. These are visible as they are the path of the end effector in the workspace. The path is highlighted in red and goes from the start node (in green)to the final node (in black). The RRT algorithm is implemented in Matlaband to generate it takes 9.2 second to compute 100 trajectories.

3.4.2 Trajectory evaluation

As discussed before, in our approach we generate multiple robot trajectoriesfrom start to the end point and choose the one which is most optimal. Inthis section we discuss how we evaluate each of the generated trajectoriesand our procedure to arrive at the most optimal one.

There are multiple ways to optimize the trajectory of a 7 DOF robot ma-nipulator. The path could be optimized for speed or efficiency or a path canbe found that stays away from obstacles. For the application considered inthis work trajectory is optimized for the execution of tasks where high preci-

25

−0.4−0.3

−0.2−0.1

00.1 1

1.11.2

1.31.4

1.5

0.7

0.75

0.8

0.85

0.9

0.95

1

1.05

1.1

Start!

y−axisx−axis

Goal!

z−ax

is

Figure 3.3: Example of a possible path computed with the RRT algorithm.

Figure 3.4: Relative position and orientation of the object compared to thehole.

26

sion is needed. The robot is required to position the held object in a desiredposition and orientation with high precision close to the hole. The precisionof this positioning governs the probability of success at object insertion.

Error in knowing the camera parameters - focal length and optical center- leads to imprecise knowledge of the position and orientation of the holetoo. However, in order to achieve object insertion, the quantity of interestto us is the relative position and orientation of the object relative to thehole and therefore we focus on choosing a trajectory for which the relativeobject position and orientation variance is minimized at the final point ofthe trajectory close to the hole.

Suppose we orient the coordinate axes as shown in figure 3.4. Here,the z-axis coincides with the hole’s longitudinal axis. The x and y-axisspan the plane perpendicular to the hole’s longitudinal axis. The parametersthat govern failure or success of object insertion are the errors in x and ydirections and the rotation about the z-axis. We thus include a weighted sumof the variances corresponding to these three variables only in our trajectoryevaluation criteria.

We further discuss our evaluation criteria in the next chapter where wediscuss results of applying this approach to a simulation example.

27

Chapter 4

Simulations & Results

In this chapter, we demonstrate the effectiveness of our motion planningapproach which takes into account influence of various uncertainties in mo-tion planning. We do this by showing results from two types of simulationexperiments that we performed. In the first experiment, we demonstratehow sensing using cameras joint angle encoders helps overcome uncertain-ties better than otherwise (section 4.3). The second simulation experimentdemonstrates how the approach of generating multiple trajectories and eval-uating them helps choose the trajectory with a significantly higher rate ofsuccess at performing the task (section 4.4).

We begin by giving details of the simulation environment in section 4.1and then giving a detailed step-by-step procedure followed to for key pickupand insertion in section 4.2

4.1 OpenRAVE - Motion Planning Simula-

tion Environment

OpenRAVE - Open Robotics Automation Virtual environment helps com-bine vision, perception, planning and control into one coherent frameworkto create intelligent and autonomous robots [4]. With OpenRave, developerscan concentrate on algorithm design and implementation without explicitlymanaging the details of robot kinematics and dynamics and collision detec-tion.

One of the powerful features of this package is its interface with Mat-lab programming environment. This helps to control and monitor the demo

28

and environment state using Matlab by use of functions which enable com-munication between OpenRAVE and Matlab. Also, all higher level code toimplement Kalman filter and trajectory evaluation was written in Matlab forthis report. Thus, the estimator and the controller is implemented in Matlaband the decisions made are communicated to OpenRAVE so that the controlaction can be implemented on the robot. The manner in which tasks aredivided between these two programs are shown in table 4.1

Matlab OpenRAVE

Main programming environment Set up and visualization of robot,and framework objects and environmentTrajectory generation with RRT Forward and inverse kinematicsTrajectory evaluation and computation Collision checkerof success/failureTriangulation from camera images Integration of cameras and sensorsVisual servoing feedback controlExtended Kalman filteringPlotting and storing the results

Table 4.1: Assignment of the software to each task.

Our simulation environment consists of the 7-degree of freedom BarrettWAM along with a virtual hole with cross sectional dimensions of 3 by 5centimeters. It is designed to fit a simulated rectangular object of dimensions2 by 4 centimeters in cross-section. A snapshot of the environment is shownin figure 4.1. The object is colored red and the hole is colored blue and red.OpenRave has an in-built collision detection module too which helps withplanning of collision free paths.

The environment is also fitted with two cameras right above the robotbase and oriented in the positive z direction. This enables them to have alarge view of the work space. They are marked with blue pyramids in figure4.1.

4.2 Procedure for Key Insertion

This section lists the various steps followed for grasping the object and andinserting it into the hole. While doing this we also mention the way the

29

Figure 4.1: The Barrett WAM in the start configuration.

estimator proceeds with its state update along the trajectory. The wholeprocess can be divided into following the steps:

1. The robot is in its starting configuration, the object is ready to begrasped (RTG). The setup is as shown in figure 4.1.

2. The position and orientation of the object is estimated using the imagesof the cameras.

3. The robot moves the end effector close to the object using this estimate.This motion of the robot as seen by the cameras provides informationwhich can utilized to update camera parameters. This is done usingKalman Estimation as discussed in 2.2.

4. Once the gripper is close enough to the object, visual servoing (3.2)guides the robot to the RTG position, with the fingers around the key.This step is depicted in figure 4.2.

5. Once the RTG pose is reached, the gripper closes its fingers and graspsthe object.

30

6. The position and orientation of key hole is estimated using images fromthe cameras.

7. The RRT algorithm computes a path that brings the end effector fromthe current estimated position to the desired position close to the hole.While moving along this path, the estimator uses camera measurementsto estimate the grasp state.

8. Once the object is close enough to the hole visual servoing is usedto position the object in front of the hole with its longitudinal axisaligned with the hole’s longitudinal axis. This configuration, with theend effector close to the hole is depicted in figure 4.3.

9. When the object is in the ready to insert (RTI) position and orientation,the variance of the object position and orientation is noted and is usedto evaluate the trajectory as mentioned in section 3.4.2 and further insection 4.4.

10. The end effector is them moved straight in the direction of key hole’slongitudinal axis and an attempt is made to insert the key. If the tip ofthe key is inserted the trajectory is considered a success, if the key isoutside the the key hole or collides with it, the trajectory is considereda failure. Figure 4.4 shows an example of a perfectly inserted key.

11. Steps 3-10 are repeated for multiple trajectories starting from the robotin its starting configuration to the final position with the object graspedand close to the key hole. Along each trajectory we propagate statevariance and ultimately note variance of the object position at the finalpoint.

12. We then classify the trajectory with the least variance as the most op-timal. Specifically, we calculated the trajectory variance as a weightedsum of the variances in object position in the plane parallel to theplane of the key hole and in orientation along the longitudinal axis ofthe keyhole as described in section 3.4.2.

31

Figure 4.2: Gripper in the ready tograsp configuration

Figure 4.3: Hand with the object closeto the hole

Figure 4.4: The Barrett WAM in the start configuration.

4.3 Importance of continuous estimation to

overcome uncertainties

In order to highlight the importance of our approach we perform a comparisonof our approach involving parameter estimation to the case where parameter

32

update is not done. For the purpose of this test, we assume that the objectgrasp is perfect and the same robot trajectory is executed multiple timesunder two kinds of scenarios. In the first scenario parameters are kept fixedat their initial estimates and the sensor output is not used to update them.In the second scenario sensor output at every time step is used to update theparameters. While performing this experiment we hold all parameters fixedexcept one which is updated along the trajectory. The list of parametersis shown in tables 4.2 and 4.3. In each case, the objective is to find themaximum levels of uncertainties which can be present in each parameter,one at a time, after which the success rate for object insertion falls below90%.

4.3.1 Maximal uncertainties for parameters

This experiment is simulated without using Kalman estimation and thus noparameter estimates are updated. The robustness of the controller is testedby introducing uncertainties. To test the robustness of an imperfect graspthe grasping position and orientation is artificially modified and a small erroris introduced. In table 4.2 the maximal value of the uncertainty the robotcan deal with while successfully inserting the key is displayed.

Uncertainty Maximal value

Motion noise [deg] 0.1Pixel noise [pixel] 0.1Encoder noise [deg] 0.15Focal length [% of focal length] -1/+1Center pixel [pixel] -4/+4Camera position [m] ±(0.002; 0.002; 0.002)Camera orientation [deg] ±(0.01; 0.005; 0.01)Grasp position [m] ±(0.005; 0.005; 0.005)Grasp orientation [deg] ±(8.5; 8.5; 8.5)

Table 4.2: Maximal uncertainties with no parameter estimation and a singlesource of uncertainty per experiment.

Now, an identical experiment is performed, however with estimation ofthe uncertainties. The results are displayed in table 4.3.

33

Uncertainty Maximal value

Motion noise [deg] 1.2Static joint angle noise [deg] 10Pixel noise [pixel] 1Encoder noise [deg] 0.5Focal length [% of focal length] -15/+15Center pixel [pixel] -50/+50Camera position [m] ±(0.1; 0.1; 0.1)Camera orientation [deg] ±(10; 10; 10)Grasp position [m] ±(0.05; 0.05; 0.05)Grasp orientation [deg] ±(60; 60; 60)

Table 4.3: Maximal uncertainties with parameter estimation and uncertain-ties added singularly.

The maximal values are visibly higher compared with the case where thereis no parameter estimation. The difference is particularly visible in the caseof the camera parameters and of the grasp estimation. Thus, we clearly seethe role of sensing in helping overcome uncertainties better.

4.3.2 Example of parameter estimation

In this section we give a demonstration of how the initial uncertainty in pa-rameters is overcome as more sensor information becomes available while fol-lowing the trajectory. We specifically demonstrate convergence of the cameraposition to its true value. The estimation of all the other parameters workssimilarly.

At the start of the algorithm, the estimator takes in initial mean valueand variance for every element of the state vector. The initial mean value isset to the best a-priori guess. If the parameter does not have to be updatedthe initial variance can be set to zero in which case the estimator does notupdate it. Otherwise, a high positive value can be assigned to the initialvariance if little is known about the true value of the parameter.

The simulation can start once these initial conditions are set. Now, thegreater is the information available from sensors along the trajectory, thebetter would be the state estimate. However, not only the quantity but alsothe quality of information is important. If no or very few features are visible

34

in the camera the estimation is expected to stay poor. Also, certain directionsof motion of the robot hand could result in not very useful information likemotion along the z-axis in our case. Since the cameras are pointed in thez-axis, motion along the z-axis cannot be captured accurately using theseprojective cameras. Furthermore, seeing different features at different timesteps can be of an advantage compared to always seeing the same feature.

1 2 3 4 5 6 7 8−0.2

0

0.2

X [

m]

1 2 3 4 5 6 7 81.3

1.5

1.7

Y [

m]

1 2 3 4 5 6 7 8−0.2

0

0.2

Number of steps

Z [

m]

Figure 4.5: Estimate of camera posi-tion

1 2 3 4 5 6 7 8−0.2

0

0.2

X [

m]

1 2 3 4 5 6 7 81.3

1.5

1.7

Y [

m]

1 2 3 4 5 6 7 8−0.2

0

0.2

Number of steps

Z [

m]

Figure 4.6: Estimate of camera posi-tion along a better trajectory

Figure 4.5 shows a plot of the estimation of the position of camera numberone. The pixel noise is set to 1 pixel. The initial estimate of the mean positionis set at [x, y, z]T = [0, 1.49, 0]Tm. The initial standard deviation is set to0.05m for all three dimensions. The blue line represents the true value of thecamera’s position, [x, y, z]T = [0, 1.5, 0]Tm. The x-axis indicates the numberof steps during the estimation. The red lines on either side are the 3σ limitsabout the mean value calculated from the standard deviation. The mean ofthe estimate is therefore always expected to be in this envelope. Also, thefact that the envelope always encloses the blue line is a testimony to the factthat the estimate converges to the true value over time. In this case it canbe seen that around step number 7 the estimation has already reached a verygood value and it does not improve considerably henceforth.

In the example of figure 4.6 the initial conditions were identical to theexperiment of figure 4.5. However the path was different and the qualityof the information was superior, i.e. more markers were visible. Hence theparameters converge much faster to the correct value. Furthermore, theestimated standard deviation is smaller and the estimation is much moreprecise.

35

1 2 3 4 5 6 7 8−0.2

0

0.2X

[m

]

1 2 3 4 5 6 7 81.3

1.5

1.7

Y [

m]

1 2 3 4 5 6 7 8−0.2

0

0.2

Number of steps

Z [

m]

Figure 4.7: Estimation of camera position with less sensor noise

The EKF is also influenced by the magnitude of measurement and mo-tion noise. The lower the noise, the more precise the estimation will be.Additionally, the speed of convergence will be higher. The more precisethe measurements are, higher is the confidence in the estimated value. Wedemonstrate this in figure 4.7.

The initial condition were the same as in the previous examples. Theonly difference is that the camera’s pixel standard deviation was set very lowat 0.1 pixel. This is only 10% of the value used in the two previous exper-iments. The difference is immediately noticeable. The parameters convergeconsiderably faster.

We demonstrated through these experiments how convergence of a pa-rameter to its true value depends on the initial variance as well as the noisevariance in sensor output. The next section focuses on mathematically eval-uating trajectories for the quality of sensor information received along them.

36

Uncertainty Initial error Initial standard deviation [σ]

Joint angles [deg] 0.1 (static error) 2Focal length [% of focal length] 20 100Center pixel [pixel] 10 50Camera position [m] 0.02 0.1 (on x axis)Camera orientation [deg] 0.2 (on y axis) 1Grasp position [m] 0 0.1Grasp orientation [deg] 0 5

Table 4.4: Initial mean values and standard deviations for the simulationover 20 trajectories.

Noise Value

Motion noise [deg] 0.05Pixel noise [pixel] 1Encoder noise [deg] 0.1

Table 4.5: Noise applied to the simulation over 20 trajectories.

4.4 Results from Trajectory Evaluation

As described in section 3.4.2, the precision in position and orientation of theobject at the final point of the trajectory is very important in order to achievesuccessful insertion. The precision in object position and orientation dependson how well the joint angles and the grasp are estimated. The estimation ofboth these quantities including other parameters depends on the trajectoryfollowed by the robot and the quality of sensor information gathered alongthem. Thus, the trajectories for which object insertion has a high probabilityof success is the one for which the variance of object position and orientationat the final point is lower.

4.4.1 Simulation results - Key grasped in initial state

In this set of experiments, we generate a set of 20 random trajectories usingRRT as in section 2.4 with the key already grasped by the robot. We assumethat the grasp is perfectly known and thus we sets its initial variance to zero.Along each trajectory we propagate the state variance (section 2.2) and notethe variance in object position at the final point of the trajectory. The choice

37

10−4

10−3

10−2

10−1

1

0

Variance of the relative position at the end point

Su

cces

s o

r fa

ilu

re

Figure 4.8: Success/failure with respect to variance.

for initial mean and variances for state variables is listed in table 4.4 and thelevel of sensing and motion noise is listed in table 4.5.

The results of these simulations are displayed in figure 4.8. The x-axisis the weighted sum of the position and orientation variances as describedin section 3.4.2. Note that the scale is logarithmic. The y-axis denoteswhether the robot succeeded or failed at the task of object insertion alonga specific trajectory. Trajectories along which insertion was achieved aredisplayed in blue in the upper part of the graph and the ones along which itfailed are marked red in the lower part. As we observe, there is a correlationbetween the variance at the final point and the outcome of the task in termsof success or failure. Also, as expected the final variance for the unsuccessfultrajectories is larger than for successful trajectories.

The difference between successful and unsuccessful trajectories is visiblebut there are still some successful trajectories with a relatively big varianceand trajectories that fail even though they have a small variance. This over-lapping distribution of the variances is mainly due to the random valuesof the motion noise in the last step. Motion noise can make insertion faileven though the robot was positioned correctly in front of the key hole, orcan make it succeed even though the controller did not manage to place itperfectly in from of the key hole.

38

To confirm the performance of the algorithm, we simulate the best and theworst trajectories multiple times. From the 20 trajectories above, we choosethe ones with the lowest and highest variances. Each of these trajectoriesare simulated 10 times and the success rate of key insertion is noted. Thetrajectory with the smallest variance shows an extremely high probability ofsuccess of 90% and the one with the highest variance a rate of success of10%. This clearly shows a relation between the magnitude of the varianceand the probability of success.

The trajectory with the smallest variance has 22 visible features along thepath. The worst trajectory has 2 visible features and the average number ofvisible features among the 20 randomly generated trajectories is 9. Figure4.9 shows a configuration where key is visible in the camera view, whereas infigure 4.10 key is occluded in camera view. The camera’s position is shown inthe image by transparent blue pyramids. Thus, the trajectory with smallervariance is clearly preferred over the one with higher variance.

4.4.2 Simulation results - Key not grasped in initialstate

In this set of experiments, we start with the key in the ungrasped state. Now,due to uncertainties in camera parameters, the computation of the positionand orientation of the key and the key hole are imprecise. This leads toan imperfect grasp. However the grasp estimation will correct the imperfectgrasp by estimating the true grasp position and orientation. Furthermore, thevisual servoing feedback controller compensates the imprecise computationof the key and key hole’s position and orientation.

For these simulations, the robot arm starts from the neutral position,with all the joint angles value set to zero. Once the object is grasped, thegrasp state is estimated along the trajectory using EKF. The initial meanvalue of the grasping position and orientation are set to zero whereas theinitial standard deviation is set to a high non-zero value indicating that thegrasp state is uncertain.

The same process as in the last experiment is used; 20 different trajecto-ries are first generated by the RRT-algorithm and the probability of successof the set is computed. We use the same values for the initial mean andstandard deviation as in the previous experiment. Out of 20 runs with ran-dom generated trajectories a success rate of 40 % is achieved. The results

39

Figure 4.9: Robot state with good visibility of the object

Figure 4.10: Robot state with key invisible

40

10−3

10−2

10−1

100

1

0

Variance of the relative position at the end point

Su

cces

s o

r fa

ilu

re

Figure 4.11: Variance plot for successful and failed trajectories with graspidentification

are plotted in figure 4.11. The variance in this case is also the weighted sumof the variances in x, y position and orientation along the longitudinal axisof the hole as described in section 3.4.2.

Again from this plot, we see the correlation between magnitude of thevariance and success or failure in object insertion. It is also noticeable thatthe variances are larger compared to the example without grasp. For in-stance, the smallest variance is 8.5 · 10−4 compared to 2 · 10−4 from theprevious experiment set. The introduction of the estimation of the graspintroduces additional uncertainty and increases the overall variance.

The dependency of success or failure in the task on variance magnitudeis even more visible when the trajectories with the best and with the worstvariance are executed 10 times each to compute the probability of success.The best trajectory, with a final variance of 8.5 · 10−4, has a high rate ofsuccess of 90 % and the worst trajectory with a final variance of 1.7 fails atalmost every trial and has close to 3 % success rate.

Thus, we see that accounting for uncertainties at the motion planningstage helps in choosing trajectories which have a considerably higher proba-bility of achieving the task under the influence of these uncertainties.

41

Chapter 5

Conclusion and Future Work

5.1 Conclusion

Robotic systems today are expected to perform increasingly complex tasksin unstructured environments like cleaning dishes in a kitchen or foldinglaundry from a pile of clothes, etc. In order to enable robots to accomplishthis robots are fitted with multiple sensing systems like cameras, laser-rangefinders, etc. This helps the robot perceive its environment and make betterdecisions. However, the output from any practical sensor is always affectedby issues of calibration or random electronic noise in its output. This canadversely influence decision making of the robot if these uncertainties are notmodeled.

In this report, we have presented an approach which takes into accountuncertainties while making motion planning decisions. Specifically, we con-sider uncertainties arising from camera calibration, camera pixel noise, smallcamera position and orientation uncertainty and joint-angle measurementnoise. We apply our motion planning approach to the task of picking up anobject and inserting it into a hole. Tasks involving insertion need a preciseknowledge of how the object has been grasped. We therefore estimate graspof the object in our approach using available sensor information.

We use the framework of an extended-Kalman filter(EKF) to incorporateuncertainty in decision making. We include the robot kinematics modelas well as all the system parameters including camera intrinsic parame-ters,extrinsic parameters and the grasp as part of the state-space model.Using the EKF we propagate the variance in the state estimate at each point

42

along the trajectory and use this to evaluate each trajectory.In order to evaluate our approach, we performed simulation experiments

with a 7-DOF Barrett WAM robot assigned the task of object pickup and in-sertion. As our performance objective to evaluate each trajectory we lookedat variance of the object position at the final point of the trajectory. We per-formed an experiment where 20 trajectories were generated using an RRTplanner and the best and the worst were selected based on the above objec-tive. We then demonstrated that the best trajectory had a success rate of90% compared to the worst trajectory with a success rate of 3%.

These demonstrations help us conclude that under sensing and motionuncertainty, the trajectory followed by the robot greatly influences outcomeof the task. Specifically, for the task of object insertion, we find that aweighted combination of the position and orientation variance at the finalpoint of the trajectory correlates well with the outcome in terms of success orfailure with object insertion. Moreover, the framework of EKF to incorporatesensor information for state estimation is quite general and can be used inmotion planning related tasks not just for object insertion but of a generalvariety.

5.2 Future Work

In our approach for motion planning, our focus has been on evaluating trajec-tories based on the quality of information gained by sensors when followingthe trajectory. Using control terminology, we can state that our objective isto maximize the observability of the system. However, for tasks like objectinsertion, depending on the shape of the object certain angles of insertionmight be more favorable than the other. For example, inserting a key side-onhas a better probability of success than inserting head on because of bettercontrol. One possible approach to capture this criteria would be using theideas of controllability and then use it in trajectory evaluation.

The approach proposed in this report has been verified in simulationon a 7 DOF-Barrett WAM. However, the approach is generic enough to beapplicable to any robot and for a much broader range of tasks. However, anextensive test on a real robotic system either like the Barrett WAM or WillowGarage’s PR2 would be the ultimate test of the approach. This would helpin identifying which uncertainties are critically important for successful taskcompletion and which ones can be neglected.

43

Bibliography

[1] Jur Berg, Pieter Abbeel, and Ken Goldberg. Lqg-mp: Optimized pathplanning for robots with motion uncertainty and imperfect state infor-mation, 2010.

[2] D. Braganza, W. E. Dixon, D. M. Dawson, and B. Xian. Trackingcontrol for robot manipulators with kinematic and dynamic uncertainty.23(2):117, 2008. ID: 228618975.

[3] Howie Choset, Kevin M. Lynch, Seth Hutchinson, George A. Kantor,Wolfram Burgard, Lydia E. Kavraki, and Sebastian Thrun. Principlesof Robot Motion: Theory, Algorithms, and Implementations. MIT Press,Cambridge, MA, June 2005.

[4] Rosen Diankov. Automated Construction of Robotic Manipulation Pro-grams. PhD thesis, Carnegie Mellon University, Robotics Institute, Au-gust 2010.

[5] J.P. Gonzalez and A. Stentz. Using linear landmarks for path planningwith uncertainty in outdoor environments. In Intelligent Robots and Sys-tems, 2009. IROS 2009. IEEE/RSJ International Conference on, pages1203 –1210, 2009.

[6] Vu Anh Huynh and N. Roy. iclqg: Combining local and global opti-mization for control in information space. In Robotics and Automation,2009. ICRA ’09. IEEE International Conference on, pages 2851 –2858,May 2009.

[7] L. E. Kavraki, P. Svestka, J-C Latombe, and M. H. Overmars. Prob-abilistic roadmaps for path planning in high-dimensional configurationspaces. IEEE transactions on robotics and automation : a publication

44

of the IEEE Robotics and Automation Society., 12(4):566, 1996. ID:86956926.

[8] G. Kewlani, G. Ishigami, and K. Iagnemma. Stochastic mobility-basedpath planning in uncertain environments. In Intelligent Robots and Sys-tems, 2009. IROS 2009. IEEE/RSJ International Conference on, pages1183 –1189, 2009.

[9] P.E. Missiuro and N. Roy. Adapting probabilistic roadmaps to han-dle uncertain maps. In Robotics and Automation, 2006. ICRA 2006.Proceedings 2006 IEEE International Conference on, pages 1261 –1267,May 2006.

[10] R. Pepy and A. Lambert. Safe path planning in an uncertain-configuration space using rrt. In Intelligent Robots and Systems, 2006IEEE/RSJ International Conference on, pages 5376 –5381, 2006.

[11] S. Prentice and N. Roy. The belief roadmap: efficient planning in linearpomdps by factoring the covariance. In Proceedings in InternationalSymposium of Robotics Research, 2008.

[12] Dan Simon. Optimal State Estimation: Kalman, H Infinity, and Non-linear Approaches. Wiley & Sons, August 2006.

[13] Richard Szeliski. Computer vision : algorithms and applications.Springer, New York; London, 2010. ID: 462920910.

[14] Russ Tedrake, Ian Manchester, Mark Tobenkin, and John Roberts. Lqr-trees: Feedback motion planning via sums-of-squares verification. TheInternational Journal of Robotics Research, 29(8):1038–1052, 2010. ID:644181179.

[15] Sebastian Thrun, Wolfram Burgard, and Dieter Fox. Probabilisticrobotics. MIT Press, Cambridge, Mass., 2005. ID: 58451645.

45

Pranav_Shah_Report

Documents

Transcript of Pranav_Shah_Report