IROS2019国际学术会议论文集1001

Online Relative Footstep Optimization for Legged Robots Dynamic Walking Using Discrete-Time Model Predictive Control Songyan Xin, Romeo Orsolino, Nikos Tsagarakis AbstractWe present a unifi ed control framework that generates dynamic walking motions for biped and quadruped robots with online relative footstep optimization. The footstep optimization is formulated as a discrete-time Model Predictive Control problem which determines future footstep locations. The framework has a hierarchical structure consisting of three layers: footstep planner, trajectory generator and whole-body controller. The footstep planner plans next footstep position based on Linear Inverted Pendulum (LIP) model. Relative footstep optimization is proposed to enable automatic footstep planning without the use of any predefi ned footstep sequences. The trajectory generator will generate CoM and feet trajectory given the next footstep placement. In order to generalize to quadruped robots, “virtual leg” concept has been used to coordinate leg pair movement. The whole-body inverse dynamic controller calculates joint torques to track given Cartesian reference trajectories. To include under-actuation into consid- eration, contact vertices formulation of ground reaction forces (GRFs) has been adopted. Generalized whole-body controller can handle biped robot with line feet as well as quadruped robots with point feet walking with dynamic gaits. Several simulations have been performed to demonstrate the robustness and generality of the proposed framework. I. INTRODUCTION Legged robots move from one place to another through contacts. However, the ground reaction forces (GRFs) is severely limited by unilateral constraints and friction con- straints. Not only that, the GRFs are not constantly available throughout all phases due to the hybrid nature of legged locomotion. With restricted actuation, it is more diffi cult to handle unexpected perturbations and environment un- certainties. Model Predictive Control (MPC), also known as Receding Horizon Control, is a general control scheme specifi cally designed to deal with such constrained dynamical systems, to enable generating adaptive behavior for a wide range of situations 1. MPC is basically solving model- based optimization problem iteratively by both considering system current state and anticipating its future evolution. The choice of model to be optimized on is crucial for successful implementation. Too complicated model might results in slow convergence or local minima which could potentially limit its use for real application. Over simplifi ed model could lead to unrealistic results. Trajectory Optimization (TO) is a general and effective method to generate motions for dynamical systems while considering physical constraints 2 3 4 5. The motion planning problem is usually transcribed into a Nonlinear Department of Advanced Robotics, Istituto Italiano di Tecnologia, via Morego, 30, 16163 Genova, Italy. Email: name.surnameiit.it Fig. 1: Legged robots walking motion can be generated by a unifi ed controller framework in CoM level. Programming Problem (NLP) with fi nite number of decision variables and constraints. However, the NLP often takes time to solve and can hardly meet real-time requirement. Linear Inverted Pendulum (LIP) model 6 is one of the simplest model that has been widely used to generate walking motion for biped robots 7 and quadruped robots 8. The motion generation usually starts with a set of predefi ned support polygons. Then CoM trajectory is generated while maintaining the Zero Moment Point (ZMP) 9 inside the support areas. To enable the robot walking freely without the use of predefi ned footsteps, an online walking motion generation scheme with automatic footstep placement has been proposed 10. In the work, quadratic program (QP) problem is formulated to optimize both footstep locations and ZMP trajectories at the same time. Since ZMP trajectories involved, the ankle torque is implicitly considered in the planning. In order to modulate ZMP, polygonal shaped contact area is required. This planar feet assumption makes it diffi cult to consider those under-actuated cases such as line feet or point feet. Since continuous ZMP trajectories has been considered inside the optimization, we call it continuous version of “walking without thinking about it” 11. Discrete-time MPC formulation that optimizes footstep placements has been proposed in 12 13 14. The prob- lem formulation is similar to the continuous one but the optimization variable differs. Instead of footstep locations and ZMP trajectories, only the previous one is considered 2019 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS) Macau, China, November 4-8, 2019 978-1-7281-4003-2/19/$31.00 2019 IEEE513 in discrete-time formulation. Without considering ZMP tra- jectories in the planning phase actually releases the planar feet assumption. As a result, it enables the consideration of biped robots with line feet or quadruped robots with point feet. The underlying assumption becomes point feet during the planning. For point foot, the ZMP coincide with foot tip. However, all the previous works introduce some kind of reference footsteps to guide the actual steps optimization. A position similarity minimization term has been included in the optimization objectives to penalize the deviation of the optimized footsteps from the reference ones. Although footsteps can be generated from a single desired average velocity input, it is not truly automatic. The result is biased by the footstep pre-generation plan. A. Contribution In this paper, two contributions have been made and they are listed here: A general controller framework that can generate dy- namic walking motions for both biped and quadruped robots. Every layer of the framework has to be general- ized considering both types of robots. Leg coordination mechanism based on virtual leg concept is developed in trajectory generator. The vertex forces GRFs representa- tion is adopted to generalize the whole-body controller to cover both fully-actuation and under-actuation cases. A relative footstep optimization formulation is proposed allowing the robot to generate walking footsteps auto- matically without the use of any pre-generation plan. It can be seen as a discrete version of the automatic footstep strategy proposed in 10. It leads to a unique reactive walking mode which keeps the robot stepping in place even when its upper-body has been blocked by barrier. B. Paper Organization The paper is organized based on the hierarchical structure of the control framework as shown in Fig. 2. The top level user command is a simple average reference velocity for the robot to track. It consists of two components: forward speed, lateral speed. Given this velocity, the footstep planner plans few future footsteps based on current robot state and the goals as described in Section II. The fi rst optimized footstep will be sent to the trajectory generator to generate trajectories for CoM and swing foot. Details about Carte- sian trajectory generation as well as the leg coordination mechanism for quadruped robots are given in Section III. The whole-body controller serves as a trajectory stabliser which calculates joint torques to allow the robot track these trajectories as close as possible. The problem formulation and its generalization are presented in Section IV. Section VI gives discussions and conclusions. II. FOOTSTEP PLANNER In this part, the proposed formulation for optimizing future footsteps is introduced. The footstep planner is implemented in a MPC fashion and each iteration is formulated as a QP Fig. 2: Hierarchical control framework. problem. The goal of the footstep planner is to optimize next few foot positions so that the CoM tracks user given reference velocity v= v x,vyT. A time driven state machine is defi ned inside the planner to handle the rhythm of step motion as well as MPC re-planning. Double support is not necessary for dynamic motion, so no double support phase has been assumed in this work. As a result, the robot switches between left support and right support immediately at touchdown moment and this greatly simplifi es the planner formulation. A simple two states state machine is used to synchronize walking motion. For quadruped robots, a leg coordination mechanism has been introduced in Section III so that this state machine could also be re-used to generate periodic rhythm for dynamic gaits such as trot, pace and bound. Fixed phase duration Ts has been assumed so all gaits in this paper switch between support and swing phase periodically. For the MPC part, higher re-planning frequency is necessary so that the robot could be more responsive during the step. Tmpchas been set to trigger MPC re-planning. It is preferred that Ts/Tmpcis an integer number so that re-planning at support switching (or touchdown) moment is ensured, which is normally required. LIP model is used to predict system future evolution. Point foot is assumed so that the planner can be extended to under- actuation cases. During single support phase, the LIP model is simply following its passive dynamics. The CoM has been pushed away by its prismatic leg while keeping constant height. The horizontal acceleration of the CoM is related to the difference between the foot and CoM position: x = g z0 (x px)(1) where x and pxrefer to the positions of CoM and stance foot in world frame, z0is the CoM height and g is the gravity constant. Similar relationship exists for y direction. The state 514 of CoM in x direction is x = x, xT, starting from initial state x0, after t time, it becomes: x(t) = A(t)x0+ B(t)px(2) where A(t) and B(t) are time dependent matrices: A(t) = 0.5 ? et+ et etet (et et)et+ et ? B(t) = ? 1 0.5(et+ et) 0.5 (et et) ? , (3) with w = pg/z 0. At any moment of swing phase, given the remaining du- ration of the current swing phase t0, the current support foot position p0and current estimated CoM state x = x, xT, the CoM state at touchdown moment x0could be calculated from (2): x0= A(t0)x + B(t0)px,0(4) The current support foot position px,0 cannot be modifi ed for the whole remaining period of the swing phase, but the next several future steps can be optimized. With no double support assumption, the take off state is the same as the previous touchdown state. Therefore, N future steps can be estimated given fi xed step time Ts: x1= A(Ts)x0+ B(Ts)px,1 x2= A(Ts)x1+ B(Ts)px,2 . xN= A(Ts)xN1+ B(Ts)px,N (5) where N is the number of steps to be optimized which should be at least 1 (N 1). In this formulation, footstep positions vector px= px,1px,2. px,NTis the optimization variable. The CoM states X = xT 1 xT 2 . xT N T is a function of chosen footstep positions. For each MPC iteration, only the fi rst step position px,1will be given to next layer for trajectory generation. The cost function basically consists of two terms: CoM ve- locity tracking term which minimizes tracking error between reference velocity v x and future CoM states X, footsteps regulation term that regulates optimization variable px. Here in this paper, the proposed cost function is: min px N X i=1 1 2Q( xi v x) 2 + 1 2Rp 2 x,i (6) where px,i= px,i px,i1is the difference between two adjacent steps, this proposed footsteps regulation term does not require any absolute references, it is doing relative min- imization. From another point of view, minimizing the foot placement difference is equivalent to minimizing average CoM velocity. Theoretical analysis shows that minimizing any derivative of the motion of CoM always results in stable online walking motion 15 and this supports the viability of the proposed regulation term. The minimization objectives in y direction is defi ned as: min py N X i=1 1 2Q( yi v y) 2 + 1 2R(py,i s (1)id)2(7) where d is desired inter-feet clearance distance (the hip width could be used). s indicates the support phase the robot is in, s = 1 for left support and s = 1 for right support. The relative distance similarity regularization term is introduced to keep the feet away from each other to avoid self-collision. Here, only relative feet distances have been enforced so it keeps the formulation free from absolute reference foot placement anchoring. Getting rid of absolute footstep anchoring results in an unique reactive walking mode which could help the robot keeping walking in place even when its upper-body has been blocked by barrier, it will be demonstrated in Section V. In fact, the more general cost function could be written as: min p N X i=1 1 2Q(vi v)2+ 1 2R(pi p i) 2 (8) where vi= xi, yiTis the velocity of CoM at the end of the ith step. pi= pipi1is the difference between two step positions. p i = p x,i, py,i is the desired footstep difference. Previously mentioned (6) and (7) are just a special case of this general cost function. Normally, zero reference velocity and non-zero footstep differences will be given to footstep planner in lateral direc- tion. If zero values have been set for footstep differences, the feet of the robot tend to get closer and closer in the lateral plane due to CoM velocity minimization. This is not desirable due to self collisions. Footstep differences guiding is the soft way to solve this problem. Another way is to defi ne hard constraints on the footstep differences. For example, in order to keep lateral footsteps away from each other, following constraint can be defi ned: s(1)ipy,i= s(1)i(py,ipy,i1) d(i = 1,.,N), (9) this constraint remains linear as long as the robot walks without turning. Without considering constraints, steering capability can be incorporated into footstep optimization with absolute reference footstep anchoring 13, similar idea could be applied to our proposed relative footstep optimization by including steering rate into the generation of desired footstep differences p. Normally, the steering control is coupled with footstep optimization due to the non-zero inter-feet clearance d. Exception exists for quadruped robots trot gait. As will be explained in the next section, quadruped trot gait can be transformed into its underlying biped gait with virtual leg substitution. The resulted virtual biped gait takes zero inter-feet clearance d and this leads to decoupled steering control. It means that the heading of a trotting quadruped robot can be controlled independently from its CoM motion. Until now, the footstep planning is only based on simple template model, there is no specifi c robot kinematic confi g- uration or foot geometry information involved. The output of this planner is just next footstep position. In the next part, we are going to explain how this planner is applied to biped robots as well as quadruped robots walking motion generation. 515 III. TRAJECTORY GENERATOR In this section, details about Cartesian trajectory genera- tion will be given. In order to generalize to quadruped, a leg coordination mechanism is also presented. The input of this layer is the next footstep position given by footstep planner. The output of this layer is reference trajectories for CoM and swing foot. As discussed in previous section, the point foot LIP model used in planner implies that an actuated planar support foot is not necessary, the robot recovers from disturbances and uncertainties by taking steps instead of relying on ankle torque. However, for a robot with large planar feet, the robot could still take advantage of that by considering it in the whole-body controller for perturbation rejection. Here, we consider the robot following the passive dynamics defi ned by LIP model during single support phase, the CoM trajectories could be calculated with (2). For swing foot, cubic spline is used to generate the swing trajectory. Given its initial position and goal position, a third way point is needed to defi ne an arc to raise the foot. It can be defi ned from the middle point but with the height raised by hs. The second derivative at the takeoff point and fi rst derivative at touchdown point have been set to zeros as boundary conditions for the cubic spline. The zero touchdown velocity is for minimizing impact between swing foot and the ground. But zero takeoff velocity is not desirable so zero acceleration has been chosen. A quadruped robot normally moves multiple legs during swing phase. Multiple swing legs have to be coordinated to achieve certain gait pattern. Three typical types of dynamic quadruped gaits such as trot, pace and bound can be mapped to a underlying biped walking gait using virtual leg concept 16. The virtual leg is developed with respect to the trunk of the robot in the original work. Here, we extend it to the abstract CoM level, in such a way, the virtual leg is completely decoupled from the robot morphology structure. The basic idea of virtual leg is that when two legs are coordinated to act in a pair, they can be represented by a functionally equivalent virtual leg. Specifi cally, diagonal pairs of legs are used for trot gait, lateral pairs for pace gait and front/rear pairs for bound gait. The leg paring scheme is specifi ed in TABLE I. TrotPaceBound Pair ALF/RHLF/LHLF/RF Pair BRF/LHRF/RHLH/RH (LF: Left Front, RF: Right Front, RH: Right Hind and LH: Left Hind.) TABLE I: Leg paring scheme for quadruped dynamic gaits. With this pairing scheme, quadruped gaits have been transformed into a common underlying virtual biped gait as shown in Fig. 3. Therefore, the footstep planner developed for biped robots can be adopted to produce quadruped gaits. One fact of the trot gait is that the robot does not suffer from self-collision in planning since the virtual legs are imaginary. After transferring to biped gait, the cross-step Fig. 3: Quadruped leg pairs defi nition for trot (top-left), pace (top-right) and bound (d