A Generalized Mixed-Integer Convex Program for Multilegged Footstep Planning on Uneven Terrain  Bernardo Aceituno-Cabezas,   Student Member, IEEE , Jos ́ e Cappelletto, Juan C. Grieco, and Gerardo Fern ́ andez-L ́ opez,   Member, IEEE  Abstract — Robot footstep planning strategies can be divided in   two   main   approaches:   discrete   searches   and   continuous optimizations. While discrete searches have been broadly ap- plied, continuous optimizations approaches have been restricted for humanoid platforms. This article introduces a generalized continuous-optimization strategy for multilegged footstep plan- ning which can be adapted to different platforms, regardless the number and geometry of legs. This approach leverages  Mixed-Integer Convex Programming   to account for the non- convex constraints that represent footstep rotation and ob- stacle avoidance. The planning problem is formulated as an optimization problem, which considers robot geometry and reachability with linear constraints and can be efficiently solved using optimization software. To demonstrate the functionality and adaptability of the planner, a set of tests are performed on a BH3R hexapod and a LittleDog quadruped on scenarios which can’t be easily handled with discrete searches, such tests are solved efficiently in fractions of a second. This work represents, to the knowledge of the authors, the first successful implementation of a continuous optimization-based multilegged footstep planner.  Index Terms — Multilegged Robots, Motion and Path Plan- ning, Mixed-Integer Convex Programming.  I. INTRODUCTION Motion planning for walking robots has become an active area of research on the recent years. One particular and well- established method is based in the separation of the contact sequences and body motion plans as different problems. This consists in specifying a sequence of contacts in the form of footsteps for the robot to follow, and then solve for body motions separately. Such method requires powerful planning tools to obtain optimal and feasible footstep plans for the platform. As noted by Deits and Tedrake [1], footstep planning approaches can be broadly divided in two areas:  discrete searches and continuous optimizations . While dis- crete searches have been thoroughly studied for multilegged robots, the entirety of continuous optimization approaches have been restricted to bipedal platforms. The main issue when adapting such approach to multilegged platforms, is that representing the geometry of a multilegged robot and its workspace in a continuous optimization footstep planner results in the introduction of non-convex constraints. This results in a nonlinear program which is difficult to solve and can’t guarantee an optimal result.  This research was partially funded by the Universidad Sim ́ on Bol ́ ıvar Research and Development Deanship B. Aceituno-Cabezas, J. Cappelletto, J. C. Grieco and G Fern ́ andez- L ́ opez are with the Mechatronics Research Group, Sim ́ on Bol ́ ıvar Uni- versity, 1080 Sartenejas, Venezuela   { 12-10764,   cappelletto, jcgrieco,   gfernandez } @usb.ve  In the past, several graph-search algorithms such as A* or RRT have been successfully applied to multilegged footstep planning: Zucker et al. [2] use ARA* with learned cost maps to obtain plans for a LittleDog robot walking on challenging terrain, Satzinger et al. [3] introduced a planner based on a modified version of A* to obtain footstep plans for a quadruped platform given a pre-computed foothold set to follow during the DARPA Robotics Challenge (DRC) finals, Winkler et al. [4] build cost maps based on the complexity of terrain, and then apply ARA* to obtain footsteps for a Quadruped. However, these methods require   pre-computed foothold sets   and terrain cost maps in order to provide a good heuristic for the search. These are difficult to compute without a prior knowledge of the desired path and are limited to a   discrete   version of the environment. On the other hand, continuous optimization approaches have been restricted for humanoid platforms. Herdt et al. [5] introduced a Quadratic Programming (QP) formulation that can produce optimal footstep placements and controls for a walking robot model when footstep orientations are fixed and obstacles in the environment are ignored. Fallon et al. [6] implemented a nonlinear programming algorithm that can plan footsteps while considering orientation, but couldn’t guarantee optimality, nor generate plans around obstacles. Deits and Tedrake [1] generalized the previous approach by formulating the problem as a   Mixed-Integer Quadratically Constrained Quadratic Program   (MIQCQP) for the DRC Finals, constraining every step to lie within a safe convex region, and using a   piecewise linear approximation   of the  sin   and   cos   functions to plan orientations. Such approach follows a similar idea as the work of Richards et al. [7], where safe UAV trajectories are obtained by using binary variables to restrict the plan within a set of   safe convex regions   represented as the intersection of the half-planes of the obstacles faces. This paper introduces a generalized continuous optimiza- tion footstep planner for multilegged platforms, which can be formulated as a   Mixed-Integer Quadratic Program   (MIQP), solved   efficiently   using   commercial   convex   optimization software, and can be easily adapted to multilegged robots with different geometries. The remaining of this paper is organized as follows: Section II describes the formulation of the program and its constraints, Section III presents the results obtained from a implementation of the planner in two different multilegged platforms (a LittleDog quadruped and a BH3R circular hexapod) and Section IV discusses and concludes on the contributions of this work.  arXiv:1612.02109v2 [cs.RO] 4 Jan 2017
II. APPROACH The goal of the planner is to solve for a sequence of   N  footsteps represented as:  f   = (   f x   f y   f z   θ   )   (1) where   f x ,   f y ,   f z   and   θ   are the   xyz   coordinates of the foot and the approximated yaw angle of the body for that step. The planner is then formulated as a continuous optimization problem over the footstep sequence, subject to the constraints that: 1) Every footstep must be reachable from the previous position of the foot. 2) All Footsteps must lie within a obstacle-free convex region. Where the geometry of the robot and the safe convex regions are given.  A. Kinematic Reachability  Since multilegged robots can have very different geome- tries, which determines the configurations that can be reached by each foot and the distribution of the leg workspace, it is necessary to restrict each step according such geometry. For this, every step is constrained around a nominal position. The  Center of Contacts   (CoC or   p ) of a footstep is defined as the geometric center of each   n legs   configuration of footsteps finishing in such step, algebraically:  p i   =   ∑ i k = i − n legs + 1   f k  n legs  (2) Then, given the CoC for a certain footstep and a nominal distance   L leg   between both, the nominal leg position   r nom   is defined as:  r nom   =  ( p x   +   L leg cos ( φ   )  p y   +   L leg sin ( φ   )  )  (3) Where   φ   is the angle between the approximate body orientation   θ   and the nominal leg position. Then, the step is required to land within a reference region   ℜ   centered on the reference position. In this case the region is chosen to be square shaped, making it rotation invariant in the trajectory (as shown in figure 1):  f ı   ∈   ℜ   ⇔   f ı   ∈  ( r nomx   ±   l bnd  r nomy   ±   l bnd  )  (4) Where   l bnd   is the side of the square that bounds the reference region. The presence of the   sin   and   cos   functions in equation (3) makes the problem non-convex. In order to solve this problem, these functions are represented using a piecewise linear approximation, thus maintaining the convexity of the problem. It is important to remark that the nominal length   L leg  should vary when the roll and pitch angles of the body differ significantly from 0. One solution is to represent this variable  Fig. 1.   Geometric constraint in   f i   and   ℜ  with the difference of height on the   n legs   footstep configura- tion as a bilinear constraint, which can be approximated with McCormick Envelopes or Multiparametric Disaggregation Techniques. In this work   L leg   is fixed as a constant, making the geometric constraints linear. For a feasible plan, every footstep needs to be reachable from its previous configuration. Since the workspace of each leg often results in a non-convex set, it is necessary to approximate it in a way such that it can be represented as a convex constraint. For this, every footstep is restricted to lie in a polygon inscribed within the biggest circle centered at  r nom   and contained in the workspace. This allows to represent the workspace of each step in a general way that will always be convex, regardless of the configuration of the robot (as shown in figure 2). Therefore, the following linear constraint is introduced:  f ı   ∈  ( r nom ( ı − n legs ) x   ±   d lim  r nom ( ı − n legs ) y   ±   d lim  )  (5) where   d lim   represent the limits of the convex linear ap- proximation as derived by Rojas et al. in [8].  Fig. 2.   Convex linear approximation of the kinematic reachability  The center of the workspace is located at the foot nominal position in order to account for changes in the workspace during the base movement. Additionally, every upward and downward step is restricted to be reachable. This is repre- sented by the following linear inequality constraint:  |   f ız   −   f ( ı − n legs ) z | ≤   ∆ z max   (6) Where   ∆ z max   is step high variation possible.
Other formulations of reachability were also considered: Perrin et al. [9] represents the workspace as a circle centered on nominal position of the leg, which can be introduced as   quadratic   constraints   on   the   program.   This   approach was found to yield similar results as the presented below. However, while this representation is invariant to the rotation of the robot body it also increases the complexity of the problem by adding quadratic constraints.  B. Obstacle Avoidance  Constraining the plan to avoid obstacle collisions, or to consider uneven terrain makes the problem non-convex. A solution to this is to restrict the footstep plan to lie within a set of   obstacle-free convex regions . These regions can be easily computed by perception algorithms or by running the IRIS algorithm [10] which uses Semidefinite Programming [11] to compute large safe convex regions in the environment. Each safe region   R   is then represented as the following convex hull:  R   =   { x   ∈   R 3   |   A r x   ≤   b r }  The assignment of footsteps to these regions can be done by introducing mixed- { 0 ,   1 }   variables. To do this a binary matrix   H   ∈ { 0 ,   1 } N × N r   is defined, where   N r   is the number of safe regions. Then each footstep is assigned to a single safe region with the following linear constraint:  N r  ∑  r = 1  H ır   =   1   (7) While the previous doesn’t mean that these regions can’t intersect, it reduces the complexity of the problem by only assigning one of these regions to each step. Every footstep is then restricted to lie in a safe region by adding the following mixed-integer constraint:  H ır   ⇒   A r   f ı   ≤   b r   (8) where the   ⇒   (implies) operator can be modeled in Mixed Integer Programming solvers as a linear constraint by using big-M formulation [12]. This constraint ensures that footstep  ı   lies with region   r .  C. Body orientation  As previously explained, this approach uses   piecewise lin- ear approximations   of the   sin   and   cos   functions to maintain convexity in the problem (as shown in figure 3). As in [1], two variables   s   and   c   are defined in order to account for sin ( θ   )   and cos ( θ   )   along with two binary matrices   S   and  C   that assign a line segment of the approximation to each variable. This is expressed with the following mixed-integer constraints:  S ık   ⇒  {  ψ k − 1   ≤   θ ı   ≤   ψ k + 1  s ı   =   m k θ ı   +   n k  (9)  C ık   ⇒  {  γ k − 1   ≤   θ ı   ≤   γ k + 1  c ı   =   m k θ ı   +   n k  (10) where the angles   ψ   and   γ   represent the boundaries be- tween each linear segment and   m   and   n   represent its slope and intersection. Then, it is enforced that every piecewise lin- ear approximation lies within a single line segment, therefore for each step   f ı :  N s  ∑  s = 1  S ıs   =   1   (11)  N s  ∑  s = 1  C ıs   =   1   (12) where   N s   is the total number of segments in the piecewise linear approximation.  Fig. 3.   Piecewise linear approximation of the   sin   and   cos   functions as shown in [1]  Since   the   introduction   of   binary   variables   makes   the problem NP-complete [13], and the presence of multiple legs   increases   the   number   of   segments   significantly,   it becomes desirable to reduce the size of the   C   and   S  matrices. The solution chosen for this is to constrain the orientation   variable   to   be   the   same   ( θ   )   in   every   n legs  configuration of the plan, and then add a known offset when computing   φ   for the   CoC   on each leg.  D. Plan length  Finally, since the necessary number of steps can’t be known a priori, the chosen solution to reduce the plan size is to simply set a maximum plan length   N , and then trim those unused steps. For this, a binary variable   t   is introduced to determine if the step must be trimmed, and then add the following mixed-integer constraint:  t ı   ⇒   f ı   =   g leg ( ı )   (13) where   leg (   f   )   :   Z   → { 1 ,   2 , . . . ,   n legs }   is   a   function   that returns the final step index for the leg of footstep   k .  E. Objectives  Given the constrains stated above, the problem is formu- lated as a minimization program, where the objectives to minimize are: 1) Final distance to the goal 2) Number of footsteps in the plan 3) Relative distance between configurations The first objective introduces a quadratic cost over the difference between the final   n legs   footsteps   f g   and the goal configuration   g , formulated as:
(   f g   −   g ) T   Q goal   (   f g   −   g )   (14) where   Q goal   is a weight cost matrix over the distance between each final step and the goal. Then, in order to minimize the number of footsteps in the plan it is chosen to maximize the amount of trimmed steps. Which can be achieved by assigning a negative cost over the sum of each trimming variable, and can be written as:  N  ∑  k = 1  q t   t k   (15) where   q t   is a negative weight over each trimming variable. In order to minimize the relative distance between config- urations a quadratic cost over the distance between the CoC of each configuration pair is introduced, this can be written as the following weighted sum:  N  ∑  k = n legs  ( p k   −   p k − n legs   ) T   Q r ( p k   −   p k − n legs   )   (16) where   Q r   is positive a weight to the cost of relative displacement between configurations. In this implementation the biggest weight is on the dis- tance to the goal, followed by weights the footstep trim and the relative displacement.  F. Complete formulation  The complete MIQP   footstep planner   with   all of the constraints and objectives can be written as: minimize  f   , t ,   H   ,   S   ,   C  (   f g   −   g ) T   Q g (   f g   −   g ) +   ∑ N k = 1   q t   t k   +  ∑ N k = n legs   ( p k   −   p k − n legs   ) T   Q r ( p k   −   p k − n legs   )  subject to:  •   Geometric constraints: p i   =   ∑ i − 1  k = i − n legs + 1   f k  n legs   −   1  r nom   =  ( p x   +   L leg c φ  p y   +   L leg s φ  )  f ı   ∈  ( r nomx   ±   l bnd  r nomy   ±   l bnd  )  •   XYZ reachability: f ı   ∈  ( r nom ( ı − n legs ) x   ±   d lim  r nom ( ı − n legs ) y   ±   d lim  )  |   f ız   −   f ( ı − n legs ) z | ≤   ∆ z max  •   Safe region assignment:  N r  ∑  r = 1  H ır   =   1  H ır   ⇒   A r   f ı   ≤   b r  •   Piecewise linear sin-cos approximation:  S ık   ⇒  {  φ k − 1   ≤   θ ı   ≤   φ k + 1  s k   =   m k θ ı   +   n k  C ık   ⇒  {  γ k − 1   ≤   θ ı   ≤   γ k + 1  c k   =   m k θ ı   +   n k  •   Trimming constraints: t ı   ⇒   f ı   =   g leg ( ı )  where   f g   = [   f N   ,   f N − 1 , . . . ,   f N − n legs + 1 ]   are the final   n legs  steps of the plan and   leg (   f   )   :   Z   → { 1 ,   2 , . . . ,   n legs }   is a function that receives a footstep index and returns its cor- responding leg number.  Fig. 4.   A   BH 3 R   hexapod following a generated footstep plan on a stepping stones track.  III. RESULTS The functionality of the planner is validated by performing simulations in MATLAB R2015a [14] within the Drake Toolbox   for   planning   and   control   [15],   and   using   the optimization software Gurobi [16] as a solver for the MIQP. Since the complexity of the problem increases significantly with the number of mixed-integer variables the tests are per- formed computing sets of 4   n legs   plans, and then concatenate them to obtain the final plan.
A. Hexapods  All   tests   are   initially   performed   on   a   BH3R   circular hexapod using a upper bound of 72 footsteps. The first test to perform is on a stepping stones track with 13 safe regions and a goal located 2 m from the initial position with a 45 ◦  rotation of the base. The problem is solved returning the plan shown in figure 4.  Fig. 5.   BH 3 R   following a footstep plan with 90 ◦   rotation  The planner is also tested with a goal rotated 90 ◦   from the robot, resulting in the plan shown in figure 5. Finally the planner is tested on a tilted terrain course with a goal located 2 m from the robot, resulting in the plan shown in figure 6.  Fig. 6.   BH 3 R   following a footstep plan on tilted terrain  B. Quadrupeds  To demonstrate the generality of the approach the planner is adapted to a LittleDog quadruped and perform tests on the same environments as for hexapods. Figure 8 shows the resulting plan over the tilted terrain using a 36 upper bound for footsteps. Then a planner is tested with a tilted terrain course, using the same specifications as before with a goal located 2 m ahead of the robot. A plan is generated and is shown in figure 7.  C. Performance  All of the tests were performed on a low-end commercial 2.4 GHz   Intel Core 2 Quad   computer with Ubuntu 15.04,  Fig. 7.   LittleDog   following a generated footstep plan for a stepping stones course  Fig. 8.   LittleDog   following a generated footstep plan for a tilted terrain course  similar to   onboard processors . As noted above the complex- ity of the problem increases significantly with the number of integer variables on the problem and therefore it can result more efficient to plan considering fewer safe regions or smaller rotation ranges.  TABLE I P ERFORMANCE OF THE   MIQP   FOR DIFFERENT SCENARIOS  Footsteps   Mixed-Integer Variables   Solving Time (s) 36   828   3.68 24   552   0.44 12   312   0.08  To contrast the impact this results have in the performance of the planner Table I presents a comparison of the solving
time in different scenarios were the planner is challenged. It is important to note the variation in the complexity of the problem when the number of mixed-integer variables increases. IV. CONCLUSION This work introduces a generalized continuous optimiza- tion approach to footstep planning; the proposed approach leverages Mixed-Integer Programming to replace non-convex constraints and can be easily adapted to multilegged robots with different geometries. This approach was successfully implemented on Drake   [15] and   represents   a successful implementation of a generalized continuous optimization footstep planner for different multilegged robots. The results obtained show how this approach handles complex rotations successfully; even on cluttered environ- ments and rough terrain without a significant increase of the complexity of the problem. The raw planner can return average sequences of footsteps in matter of seconds, on easy mockups; it can return solutions in few minutes when the planning is done on complex environments. When concate- nating shorter plans it can be solved for the same sequences, in fragments of a second, using low-end processing.  A. Future work  The performance of the planner represents an opportunity to develop   online footstep planners   based on continuous optimization by working with short sequences of footsteps and recomputing offboard for changes in the environment, similar to the work shown in [17]. This would require a set of local goals in order to avoid local minima (for instance, a wall between the robot and the goal), possibly by running a discrete search over a sampled space of the convex safe regions using RRT* or ARA*. Another opportunity is to integrate novel robust walk- ing planning approaches on multilegged platforms. New optimization-based approaches have been successfully im- plemented for humanoid platforms using robust optimization to maximize a universal stability margin based on   Contact Wrench Cones   as shown in [18]. Furthermore, the planner itself still has room for improve- ment, since the workspace of a multilegged robot changes significantly when performing dynamic motions. So, it be- comes necessary to account for these changes when repre- senting the geometric constraints. Adding this consideration and including footstep sequence within the planner, assigning a binary variable to the moving legs, would allow to obtain different locomotion modes and gaits.  B. Source code  Following the same spirit as [1] the authors of this work have made the entire source code publicly available on github 1 .  1 https://github.com/baceituno  ACKNOWLEDGMENT The authors would like to thank Hongkai Dai for his advise and expertise in the subject, and Maureen Rojas for her support on the simulations stage of this project. This work was partially funded by the Universidad Sim ́ on Bol ́ ıvar Research and Development deanship. R EFERENCES [1]   R.   Deits   and   R.   Tedrake,   “Footstep   planning   on   uneven   terrain with   mixed-integer   convex   optimization,”   in   2014   IEEE-RAS International Conference on Humanoid Robots .   IEEE, 2014, pp. 279– 286.   [Online].   Available:   http://groups.csail.mit.edu/robotics-center/ public papers/Deits14a.pdf [2]   M.   Zucker,   J.   A.   Bagnell,   C.   G.   Atkeson,   and   J.   Kuffner,   “An optimization approach to rough terrain locomotion,” in   Robotics and Automation (ICRA), 2010 IEEE International Conference on .   IEEE, 2010, pp. 3589–3595. [3]   B. W. Satzinger, C. Lau, M. Byl, and K. Byl, “Tractable locomo- tion planning for robosimian,”   The International Journal of Robotics Research , p. 0278364915584947, 2015. [4]   A. W. Winkler, C. Mastalli, I. Havoutis, M. Focchi, D. G. Caldwell, and C. Semini, “Planning and execution of dynamic whole-body locomotion for a hydraulic quadruped on challenging terrain,” in   2015 IEEE International Conference on Robotics and Automation (ICRA) . IEEE, 2015, pp. 5148–5154. [5]   A. Herdt, H. Diedam, P.-B. Wieber, D. Dimitrov, K. Mombaur, and M. Diehl, “Online walking motion generation with automatic footstep placement,”   Advanced Robotics , vol. 24, no. 5-6, pp. 719–737, 2010. [6]   M. Fallon, S. Kuindersma, S. Karumanchi, M. Antone, T. Schneider, H. Dai, C. P. D’Arpino, R. Deits, M. DiCicco, D. Fourie,   et al. , “An architecture for online affordance-based perception and whole-body planning,”   Journal of Field Robotics , vol. 32, no. 2, pp. 229–254, 2015. [7]   A. Richards, J. Bellingham, M. Tillerson, and J. How, “Coordination and control of multiple uavs,” in   AIAA guidance, navigation, and control conference, Monterey, CA , 2002. [8]   M. Rojas, N. Certad, J. Cappelletto, and J. Grieco, “Foothold planning and gait generation for a hexapod robot traversing terrains with forbid- den zones,” in   12th Latin American Robotics Symposium LARS2015 and 2015 Third Brazilian Symposium on Robotics , 2015. [9]   N. Perrin, C. Ott, J. Englsberger, O. Stasse, F. Lamiraux, and D. G. Caldwell, “Continuous legged locomotion planning,”   IEEE Transac- tions on Robotics , vol. PP, no. 99, pp. 1–6, 2016. [10]   R.   Deits   and   R.   Tedrake,   “Computing   large   convex   regions of   obstacle-free   space   through   semidefinite   programming,”   in  Algorithmic Foundations of Robotics XI .   Springer, 2015, pp. 109– 124.   [Online].   Available:   http://groups.csail.mit.edu/robotics-center/ public papers/Deits14.pdf [11]   S. Boyd and L. Vandenberghe,   Convex Optimization .   Cambridge University Press, 2004. [Online]. Available: https://books.google.co. ve/books?id=IUZdAAAAQBAJ [12]   A. Richards and J. How, “Mixed-integer programming for control,” in  Proceedings of the 2005, American Control Conference, 2005.   IEEE, 2005, pp. 2676–2683. [13]   R. M. Karp, “Reducibility among combinatorial problems,” in   Com- plexity of computer computations .   Springer, 1972, pp. 85–103. [14]   MATLAB,   version 8.5.1 (R2015a) .   Natick, Massachusetts: The MathWorks Inc., 2015. [15]   R.   Tedrake,   “Drake:   A   planning,   control,   and   analysis   toolbox for   nonlinear   dynamical   systems,”   2014.   [Online].   Available: http://drake.mit.edu [16]   I. Gurobi Optimization, “Gurobi optimizer reference manual,” 2015. [Online]. Available: http://www.gurobi.com [17]   P. Karkowski and M. Bennewitz, “Real-time footstep planning using a geometric approach,” in   2016 IEEE International Conference on Robotics and Automation (ICRA) .   IEEE, 2016, pp. 1782–1787. [18]   H. Dai and R. Tedrake, “Planning robust walking motion on uneven terrain via convex optimization,” in   2016 IEEE-RAS International Conference on Humanoid Robots .   IEEE, 2016. [Online]. Available: http://groups.csail.mit.edu/robotics-center/public papers/Dai16a.pdf