Footstep Planning On Uneven Terrain With Mixed . - MIT

2y ago
43 Views
2 Downloads
3.50 MB
8 Pages
Last View : 1d ago
Last Download : 2m ago
Upload by : Kaleb Stephen
Transcription

Footstep Planning on Uneven Terrain with Mixed-Integer ConvexOptimizationRobin Deits1 and Russ Tedrake2Abstract— We present a new method for planning footstepplacements for a robot walking on uneven terrain with obstacles, using a mixed-integer quadratically-constrained quadraticprogram (MIQCQP). Our approach is unique in that it handlesobstacle avoidance, kinematic reachability, and rotation offootstep placements, which typically have required non-convexconstraints, in a single mixed-integer optimization that can beefficiently solved to its global optimum. Reachability is enforcedthrough a convex inner approximation of the reachable spacefor the robot’s feet. Rotation of the footsteps is handled by apiecewise linear approximation of sine and cosine, designed toensure that the approximation never overestimates the robot’sreachability. Obstacle avoidance is ensured by decomposing theenvironment into convex regions of obstacle-free configurationspace and assigning each footstep to one such safe region. Wedemonstrate this technique in simple 2D and 3D environmentsand with real environments sensed by a humanoid robot. Wealso discuss computational performance of the algorithm, whichis currently capable of planning short sequences of a few steps inunder one second or longer sequences of 10-30 footsteps in tensof seconds to minutes on common laptop computer hardware.Our implementation is available within the Drake MATLABtoolbox [1].I. INTRODUCTIONThe purpose of a footstep planner is to find a list offootstep locations that a walking robot can follow safely toreach some goal. Footstep planning is a significant simplification of motion planning through contact, one in which thewhole-body kinematics and dynamics are typically coarselyapproximated or ignored in order to produce a tractableproblem. The challenge of footstep planning thus consistsof finding a path through a constrained environment to agoal while respecting constraints on the locations of anddisplacements between footsteps. An example of such a planis shown in Fig. 1.Broadly speaking, there exist two families of approaches tofootstep planning: discrete searches and continuous optimizations. The discrete search approaches have typically involveda precomputed action set: either represented as a set ofpossible displacements from one footstep to the next or a setof possible footholds in the environment. Chaining actionstogether forms a tree of possible footstep plans, which canbe explored using existing discrete search methods like A and RRT. Action set approaches using pre-computed stepThis work was supported by the Fannie and John Hertz Foundation, theMIT Energy Initiative, MIT CSAIL, and the DARPA Robotics Challenge.1 Robin Deits is with the Computer Science and ArtificialIntelligence Laboratory, MIT, Cambridge, MA 02139, USArdeits@csail.mit.edu2 Russ Tedrake is with the Faculty of Electrical Engineering and ComputerScience, MIT, Cambridge, MA 02139, USA russt@csail.mit.eduFig. 1. Two examples of the output of our MIQCQP footstep planner.Above: An Atlas biped planning footsteps across a set of stepping stones.Below: With one stepping stone removed, Atlas must take the longer detouraround the stepping stones. The gray rectangles are the boundaries of convexregions of obstacle-free configuration space generated by IRIS [2] projectedinto the xy plane.displacements have been used by Michel [3], Baudouin [4],Chestnutt [5], [6], and Kuffner [7], [8]. Similarly, Shkolniket al. used a precomputed set of dynamic motions, ratherthan foot displacements, and an RRT search to find motionsfor a quadruped [9]. The fixed foothold sets have beenused by Bretl for climbing robots [10] and by Neuhaus forthe LittleDog quadruped [11]. These approaches can easilyhandle obstacle avoidance by pruning the tree of actionswhen a particular action would put a foot in collision with anobstacle [7], [8], [4], including obstacle avoidance in the costfunction evaluated at each leaf of the tree [5], [3], or adaptingthe set of actions when a collision is detected [6]. However,they have also tended to suffer from the tradeoff between asmall action set, which reduces the branching factor of thesearch tree, and a large action set, which covers a larger set

of the true space of foot displacements but is much harder tosearch [4]. In addition, applying A or other informed searchmethods to our problem is complicated by the difficulty indefining a good heuristic for partial footstep plans: we cannotgenerally know how many additional footsteps a partialplan will need in order to reach the goal without actuallysearching for those steps [5].The continuous optimization approaches, on the otherhand, operate directly on the poses of the footsteps as continuous decision variables. This avoids the restriction to a smallset of fixed actions and thus allows more possible footstepplans to be explored, but correctly handling rotation andobstacle avoidance turns out to be difficult in a continuousoptimization. Both footstep rotation and obstacle avoidancegenerally require non-convex constraints to enforce them,since the set of rotation matrices and the set of points outsidea closed obstacle are non-convex. We typically cannot findguarantees of completeness or global optimality for suchnon-convex problems [12]. We have presented a non-convexcontinuous optimization for footstep planning, used by TeamMIT during the DARPA Robotics Challenge 2013 Trials[13], but this optimization could not guarantee optimalityof its solutions or find paths around obstacles. Alternatively,footstep rotation and obstacle avoidance can simply beignored: Herdt et al. fix the footstep orientations and donot consider obstacle avoidance. This allows them to forma single quadratic program (QP) which can choose optimalfootstep placements and control actions for a walking robotmodel [14].We choose to use a mixed-integer convex program (specifically, a mixed-integer quadratically constrained quadraticprogram) to provide a more capable continuous footstepplanner. Such a program allows us to perform a continuousoptimization of the footstep placements, while using integervariables to absorb any non-convex constraints. We handleorientation of the footstep placements by approximating thetrigonometric sin and cos functions with piecewise linearfunctions, using a set of integer variables to choose theappropriate approximation. We also avoid the non-convexconstraints inherent in avoiding obstacles by instead enumerating a set of convex obstacle-free configuration spaceregions and using additional integer variables to assignfootsteps to those regions. The presence of integer constraintssignificantly complicates our formulation, but a wide varietyof commercial and free tools for mixed-integer convex programming exist, all of which can provide globally optimalsolutions or proofs of infeasibility, as appropriate [15], [16],[17], [18]. Thus, we can solve an entire footstep planningproblem to optimality while ensuring obstacle avoidance,a task that, to our knowledge, has not been accomplishedbefore.This is not unlike the work of Richards et al., whoconstructed a mixed-integer linear program to plan UAVtrajectories while avoiding obstacles [19]. They representedeach (convex, 2D) obstacle as a set of linear constraints, eachof which generates a pair of half-spaces, one containing theobstacle and one not. They assigned a binary integer variableFig. 2.Four randomly generated 2D environments demonstrating theMIQCQP footstep planner. The gray squares are the randomly placedregions of safe terrain; the black circle is the goal location, with a lineindicating the desired orientation of the robot; and the green and yellowmarkers are the locations of the right and left footsteps, respectively, witharrows showing the orientation of each step. The foot is assumed to be apoint for these examples.

to every pair of half-spaces and required that, for everyobstacle, the vehicle’s location must be in at least one of thenon-obstacle half-spaces. Instead of adding binary variablesfor every single face of every obstacle, we precomputeseveral convex obstacle-free regions, then assign a singlebinary variable to each region and require that every footstepis assigned to one such region, which dramatically reducesthe number of binary variables required. Our ability togenerate these convex obstacle-free regions relies on ourrecent development of IRIS, an algorithm for computinglarge convex regions of obstacle-free space [2].II. TECHNICAL APPROACHOur task is to determine the precise x, y, z and θ (yaw)positions of N footsteps, subject to the constraints that1) Each step does not intersect any obstacle2) Each step is within some convex reachable regionrelative to the position of the prior stepTo accomplish this, we invert the non-convex problem ofavoiding every obstacle and instead reformulate the problemas one of assigning each footstep to some pre-computedconvex region of obstacle-free terrain. We then add quadraticconstraints to ensure that each footstep is reachable from theprior step, using a piecewise linear approximation of sin andcos to handle step rotation.The implies operator in (1) can be converted to a linearconstraint using a standard big-M formulation [20] or handled directly by mixed-integer programming solvers such asIBM ILOG CPLEX [17]. The constraint (2) requires thatevery footstep be assigned to exactly one safe terrain region.B. Ensuring ReachabilityWe choose to approximate the reachable set of footstep positions as an intersection of circular regions in the xy plane,with additional linear constraints on footstep displacementsin yaw and z. The reachable set defined by the intersectionof two circular regions is shown in Fig. 3b. Other approacheshave typically used polytope representations of the reachableset [13], [14], but such an approach results in a non-convexconstraint when rotation is allowed, even under our piecewiselinear approximation of sin and cos.Setting aside the question of footstep rotation for themoment, we can describe our reachable region with a set ofconvex quadratic constraints. For each footstep j, we requirethat xjxj 1cos θj 1 sin θj 1 p1 d1yjyj 1sin θj 1cos θj 1(3) xjxj 1cos θj 1 yjyj 1sin θj 1A. Assigning Steps to Obstacle-Free RegionsOur first task is to decompose the 3D environment into aset of convex regions in which the foot can safely be placed.The IRIS algorithm, first presented in [2] and designed forthis particular task, quickly generates obstacle-free convexregions in the x, y, and θ (yaw) configuration space of therobot’s feet. Each of these obstacle free terrain regions isrepresented as a set of linear constraints defining a polytopein x, y, θ. Additionally, for each region we fit a plane in x, y,and z to the local terrain and add additional linear constraintsto force footsteps in that region to lie on the plane. We labelthe total number of convex regions as R and for each regionr we identify the associated linear constraints with the matrixAr and vector br such that if footstep fj is assigned to regionr, thenAr fj brwhere xj yj fj zj and r {1, . . . , R}θjTo describe the assignment of footsteps to safe regions,R Nwe construct a matrix of binary variables H {0, 1},such that if Hr,j 1 then footstep j is assigned to region r:RXr 1Hr,j Ar fj br(1)Hr,j 1 j 1, . . . , N(2) sin θj 1p2 d2cos θj 1(4)where p1 , p2 are the centers of the circles, expressed in theframe of footstep j 1 and d1 , d2 are their radii. Whenθj 1 is fixed, constraints (3) and (4) are convex quadraticconstraints, but including θ as a decision variable makesthe constraint non-convex. We will handle this problem byintroducing two new variables for every footstep: sj andcj , which will approximate sin θj and cos θj , respectively.Constraints (3,4) thus become: xjxj 1cj sj d1(5) p1yjyj 1sj cj xjxj 1c jyjyj 1sj sjp2 d2 .cj(6)Since p1 , p2 , d1 , d2 are fixed, this is still a convex quadraticconstraint.Our work is not yet complete, however, since we nowmust enforce that sj and cj approximate sin and cos withoutintroducing non-convex trigonometric constraints. We chooseinstead to create a simple piecewise linear approximation ofsin and cos and a set of binary variables to determine whichpiece of the approximation to use. We construct binary matrixL NS {0, 1}, where L is the number of piecewise linearsegments and add constraints of the form(φ θj φ 1S ,j (7)sj g θj h LX 1S ,j 1 j 1, . . . , N(8)

where g and h are the slope and intercept of the linearapproximation of sin θ between φ and φ 1 . We likewiseadd piecewise linear constraints of the same form for cj .The particular choice of g and h turns out to be quiteimportant: we must ensure that our approximation neveroverestimates the reachable space of foot placements. Wecan verify this empirically for the approximation shown inFig. 3a by checking that the intersection of constraints (5,6)is contained within the intersection of constraints (3,4) forall values of θ. The reachable sets for footsteps at a varietyof orientations are shown in Fig. 3c.1sin θcos θApprox.0 1ππ20θ(a) Piecewise linear approximation of sine and cosineC. Determining the Total Number of FootstepsIn general we cannot expect to know a priori the totalnumber of footsteps which must be taken to bring the robotto a goal pose, so we need some method for determining Nefficiently. We could certainly just repeat the optimizationfor different values of N, performing a binary search to findthe minimum acceptable number of steps, but for efficiency’ssake we would prefer to avoid the many runs of the optimizerthat this would require.We might attempt to determine the number of requiredsteps by setting N sufficiently large and simply adding acost on the squared distance from each footstep to the goalpose, which will stretch our footstep plan towards the goal. Ifthe footsteps reach the goal before N steps have been taken,then we can trim off any additional steps at the end of theplan. However, this approach allows the footstep planner toproduce strides of the maximum allowable length for everyfootstep, even on obstacle-free flat terrain. During experiments leading up to the DARPA Robotics Challenge trials,we determined that a forward stride of 40 cm was achievableon the Atlas biped, but that a nominal stride of approximately20 cm was safer and more stable. We would thus like toexpress in our optimization a preference for a particularnominal stride length while still allowing occasional longerstrides needed to cross gaps or clear obstacles. We can tryto create this result by adding additional quadratic costs onthe relative displacement between footsteps, but this requiresvery careful tuning of the weights of the distance-to-goal costand the relative step cost for each individual step in order toensure that the costs balance precisely at the nominal steplength for each step.Instead, we choose a much simpler cost function, with aquadratic cost on the distance from the last footstep to thegoal and identical cost weights on the displacement fromeach footstep to the next. To control the number of footstepsused in the plan, and thus the length of each stride, we add asingle binary variable to each footstep, which we will labelas tj (for ‘trim’). If tj is true, then we require that step j befixed to the initial position of that same foot:(f1 if j is oddtj fj (9)f2 if j is even.Note that f1 and f2 are the fixed current positions of therobot’s two feet.2π3π2p1d1yleft footxright footd2p2(b) Approximation of the reachable set of locations for the right foot, given theposition of the left foot. The gold arrow shows the position and orientationof the left foot, viewed from above. The shaded region shows the set ofreachable poses for the right foot in the xy plane, defined as the intersectionof constraints (5,6) at orientation of θj 0, for which our approximation ofsin and cos is exact. One possible future pose of the right foot is shown forreference.yx(c) A simple footstep plan with 2D reachable regions shown. The goal isthe black circle in the top right, and each arrow shows the position andorientation of one footstep. For each step, we draw the shaded region definedby constraints (5,6) into which the next step must be placed. Note that thefeasible region shrinks when the step orientation is not a multiple of π/2, asour approximation of sin and cos becomes inexact.

Since each footstep for which tj 1 is fixed to thecurrent position of the robot’s feet, the number of footstepswhichPare actually used to move the robot to the goal isNN j 1 tj . By assigning a negative cost value to eachbinary variable tj , we can create an incentive to reduce thenumber of footsteps used in the plan. To tune the nominalstride length, we can simply adjust the cost assigned to thetj . Increasing the magnitude of this cost will lengthen thenominal stride uniformly, and decreasing the magnitude willshorten the stride. Thus, we have a single value to tune inorder to set the desired stride length, while still allowingstrides which exceed this length. After the optimization iscomplete, we can remove any footsteps at the beginning ofthe plan for which tj is true.D. Complete FormulationPutting all of the pieces together gives us the entirefootstep planning problem:minimizef1 ,.,fj ,S,C,H,t1 ,.,tj(fN g) Qg (fN g) NXqt tjj 1 N 1X(fj 1 fj ) Qr (fj 1 fj ))j 1subject to, for j 1, . . . , Nsafe terrain regions:Hr,j Ar fj brr 1, . . . , Rpiecewise linear sin θ:(φ θj φ 1S ,j sj g θj h piecewise linear cos θ:(φ θj φ 1C ,j cj g θj h 1, . . . , L 1, . . . , Lapproximate reachability: xjxj 1c sj jpi di i 1, 2yjyj 1sj cjfix extra steps to initial pose:(f1 if j is oddtj fj f2 if j is even.RXr 1Hr,j LX 1S ,j LXC ,j 1 1Hr,j , S ,j , C ,j , tj {0, 1}bounds on step positions and differences:fmin fj fmax fmin (fj fj 1 ) fmaxwhere g R4 is the x, y, z, θ goal pose, Qg S4 andQr S4 are objective weights on the distance to the goaland between steps, qt R is an objective weight on trimmingunused steps, and fmin , fmax , fmin , fmax R4 are boundson the absolute footstep positions and their differences,respectively. We also fix f1 and f2 to the initial poses ofthe robot’s feet.E. Solving the ProblemWe have implemented this approach in MATLAB [21],using the commercial solver Gurobi [15] to solve theMIQCQP itself. Typical problems involving 10 to 20 footsteps and 10 convex safe terrain regions can be solved in afew seconds to one minute on a Lenovo laptop with an Inteli7 clocked at 2.9 GHz. Smaller footstep plans involving justa few steps can be solved in well under one second on thesame hardware, so this method is capable of providing shorthorizon footstep plans at realtime rates while walking orlonger footstep plans involving complex path planning whilestationary. The Mosek and CPLEX optimizers [16], [17]were also capable of solving the problem, but we generallyfound that Gurobi found optimal solutions or proofs ofinfeasibility more quickly in our experiments.III. RESULTSTo demonstrate the MIQCQP footstep planning algorithm,we first generated a collection of random 2D environments.Each environment consisted of 10 square regions of safeterrain, 9 of which were uniformly randomly placed withinthe bounds of the environment, and 1 of which was placeddirectly under the starting location of the robot to representthe robot’s currently occupied terrain. A goal pose wasuniformly randomly placed within the xy bounds of theenvironment with a desired orientation between π2 relativeto the robot’s starting orientation. Several such environmentsand the resulting footstep plans are shown in Fig. 2. Allthe footstep plans shown in this paper are the result ofconvergence to within 0.1% or less of the globall

Intelligence Laboratory, MIT, Cambridge, MA 02139, USA rdeits@csail.mit.edu 2Russ Tedrake is with the Faculty of Electrical Engineering and Computer Science, MIT, Cambridge, MA 02139, USA russt@csail.mit.edu Fig. 1. Two examples of the output of our MIQCQP footstep planner. Above: An Atlas biped planning footsteps

Related Documents:

Prices shown are for one item excluding VAT *Price per metre is for estimating purposes only Soil Systems - 100 Solvent Size mm Angle Col Pack/ Box Code Price Each Spigot socket bends Long tail. 82 92½ kcG 5/40. Price . Terrain. Terrain. Terrain. Terrain. Terrain. Terrain. Terrain.

till date [1]. Mechanical footstep power generator is one such system. Power generation through footstep does not rely on any conventional sources of energy which is one of the advantages of this system and this system can easily be installed in any public places to obtain maximum output in the form of electrical energy. The only source of .

A number of types of terrain may appear on the map. clear terrain (flat, firm going, devoid of any important features, including gentle hills) broken terrain (moderately marshy or rocky terrain, wooded terrain, rivers and frozen lakes, low brush) *Difficult terrain wo

DESIGN AND FABRICATION OF MECHANICAL FOOTSTEP POWER GENERATOR . utilization of waste energy of foot power with human locomotion is very relevant in populated countries like India where roads, railway stations, bus stands, temples, etc. are overcrowded and millions of people move around. .Author: Shivendra Nandan, Rishikesh Trivedi

This system introduces power generation using non-conventional energy which does not need any input to generate electrical output. In this conversion of force energy into electrical energy takes place. Fig 1: Schematic representation of the working model . for every footstep and the footstep count.

The working of the footstep power generation system involves 1. Interface and transducing 2. Processing 3. Storage 4. This wastage of energy can be converted to usable form using the help of a piezoelectric sensor. 5. The piezoelectric sensor is a device that can convert pressure into voltage.

The concept of the footstep power generation system is shown below. Fig. 1 block diagram of the project The overall system design would follow the stipulated block flow diagram as shown in fig.1 in terms of component placement as well as power flow. The footstep power generation design consists of different modules which include; i.

Double Concept Modal Modal Concept Examples Shall (1) Educated expression Offer Excuse me, I shall go now Shall I clean it? Shall (2) Contractual obligation The company shall pay on January 1st Could (1) Unreal Ability I could go if I had time Could (2) Past Ability She could play the piano(but she can’t anymore) Can (1) Present Ability We can speak English Can (2) Permission Can I have a candy?