Robust 3D Walking Using Inverse Dynamics And Footstep Planning With Model Predictive ... · 2019....
Embed Size (px)
Transcript of Robust 3D Walking Using Inverse Dynamics And Footstep Planning With Model Predictive ... · 2019....
Robust 3D Walking Using Inverse Dynamics And Footstep PlanningWith Model Predictive Control
Salman Faraji, Soha Pouya and Auke Jan IjspeertBiorobotics Laboratory, EPFL, Lausanne, Switzerland
Controlling legged robots is challenging due to the fact thatthey should keep balance and perform locomotion. Bipeds areeven more prone to failure since the support region for theirCenter of Mass (CoM) is limited. Although many methodsexist in literature that keep the CoM inside the support regionduring locomotion, the resulting statically-stable walking isnot fast or natural like humans. Dynamically-stable walkinghowever produces faster and more natural motions as it letsthe CoM fall outside the support region, while the robot takesproper steps to maintain balance dynamically and walk. Chal-lenging aspects of this approach are proper prediction of thefuture states, avoiding tilting or rolling of feet, fast motions,using heels and toes for faster walking, keeping knees straightin order to avoid consuming unnecessary energy, human-likeCoM height profile and finally being compliant to softly han-dle impacts produced at each touchdown. These aspects are inaddition to considering physical constraints like torque limits,frictions and self collisions. In this work we want to proposea generic method that needs minimal tuning, covers a widerange of speeds and does not rely on off-line optimizations.We aim at reducing the problem size from joint to Carte-sian space in order to avoid dealing with high dimensionaljoint trajectories and their complicated geometrical consis-tency. The proposed method should be computationally af-fordable and robust against various sources of perturbationsinternally or externally.
2 Our Approach
Here we break down the controller into three layers where wesimplify the algorithm step by step.
First Layer: In this layer we generate actuator torques giventhe desired motion in the Cartesian (task) space. Variousmethods in the literature use kinematic models of the robotto convert Cartesian velocities [4, 3] or forces  to actua-tor variables. For our torque controlled robot Coman ,considering the dynamics-model of the robot however willhelp us calculating the required torques when targeting ag-ile and versatile motions. Although large feedback gains willincrease the tracking accuracy, they make the robot less com-pliant. Adding feed-forward torques will help to achieve thesame preciseness with smaller feedback gains, still being ableto reject perturbations and behaving more compliantly. In-spired from  we use task-space inverse dynamics to gen-
erate the joint torques given Cartesian accelerations. How-ever similar to  we use the whole body optimization ap-proach instead of pseudo-inversion as it enables us to takeinto account the torque limits, the CoP region and frictionpolyhedrals as inequality constraints. In this method, one op-timizes contact forces, joint accelerations and torques at thesame time under aforementioned constraints, together withthe equation of motion and joint/Cartesian space accelera-tion relations as equality constraints. Our problem is ef-ficiently formulated in a fast quadratic optimization librarycalled CVXGEN  which solves it per time-step in less than1.2ms. Introduction of soft constraints makes our methodslightly different from  and  in the sense that we canaccept infeasible input accelerations and still produce a feasi-ble motion.
Second layer: Our whole-body optimization in the first layerhelps us planning trajectories for the CoM and feet in the task-space with reduced dimensions. Thus in the second layer ofour controller, we define arc trajectories for swing foot andtrack them by PD controllers which produce Cartesian ac-celerations. Various methods like ,  and  optimizethese trajectories off-line which makes their method harderto generalize for other environment conditions. However weuse soft transitions and SLERP (spherical linear interpolation)functions to smoothen these trajectories. The input to thislayer is the next footstep location that is determined by thethird layer of the controller.
Third layer: One needs to simplify the robot’s model to pre-dict the future motion with less computational cost during dy-namic walking. A common approach is to use simple invertedpendulums or with springs, ankles or inertia mass. SpringLoaded Inverted Pendulum (SLIP) model is usually used topredict running. Although Mordatch  for example has de-rived a closed form solution for evolution of the CoM statewith polynomial approximations, such nonlinear prediction iscomputationally expensive. For the purpose of walking how-ever, Linear Inverted Pendulum Models (LIPM) appear morefrequently in the literature and they enable simpler or even lin-ear predictions of motion if no inertia is assumed for the mass.Some methods have control over the CoP trajectory during theswing phase to follow the LIPM model [6, 4]. However theyrely on ankle torques to modulate the motion which is lessrobust in case of arbitrary foot shapes, unstructured terrainsor contacts made of soft materials. Similar concept is used in for 1-step prediction. The swing foot touches down in a
location where the energy of the robot is captured after beingpushed and thus going to rest condition.
In , the top level planner uses LIPM and controls boththe CoP trajectory and multiple footstep locations which ismore elaborated and robust, but still being linear and in closedform. This method solves a Model Predictive Control (MPC)problem which optimizes a sequence of footsteps and CoPtrajectories that induce a desired given reference speed. How-ever relying on ankle torques has previously-mentioned draw-backs, in addition to the fact that the low level control methodin  is kinematics-based and less compliant.
Inspired by this work, we have formulated a similar MPCproblem which merely optimizes footstep locations assumingno ankle for the LIPM unlike . Given reference forwardand steering speeds via a joystick, the third layer forms a ref-erence footstep plan. It then optimizes future states of therobot and footsteps that act as discrete inputs to this system,regarding the reference footstep plan and the LIPM model.The first footstep location is then used to calculate swing arctrajectories in the second layer. With such novel linear andclosed form simple formulation which is again implementedin CVXGEN, we are able to adjust future footsteps in everytime-step. Together with the low-level compliant controller,our method is responsive to pushes or other large perturba-tions by taking proper footsteps. It only uses ankle torquesfor smaller perturbations which is done compliantly in thelow-level controller, not in the planning level.
3 Results and Discussions
The proposed controller is able to produce a wide range of for-ward speeds from −0.2m/s to 0.4m/s on our simulated kid-sized robot Coman which has only around 1m height. Thisperformance is comparable with methods like  which pro-duce 1.14m/s on Atlas robot which has a height of around190cm. However we do not have heel touchdown and toe lift-off phases unlike  which restrict our motion geometricallyregarding ankle joint limits. Our robot is also able to rotateabout the yaw axis with maximum speed of ±1rad/s whichis notably more than similar methods like . However walk-ing forward and steering at the same time can not be done atthese maximum speeds due to large swing motions of feet.Our method can dynamically recover from strong pushes of10N lasting 0.3s on the kid-sized robot Coman. It can alsoperform blind walking robustly over slopes of −10o to 15oand an uneven terrain with ±5cm height variations. We havealso verified its robustness against internal perturbations suchas sensor noises up to 3o of standard deviation, communica-tion loops with 10ms delays and various types of model er-rors like shifting the CoM 1cm or making different parts ofthe body 1kg heavier.
In summary, we have transferred the joint control problem totask space in the first layer with our whole-body inverse dy-namics formulation. We have also proposed an on-line simpletrajectory planning method to replace the use of off-line op-timizations. Finally in the third layer which is the novelty
3rd layer:Foot-step planner
Coronal, Sagittal and Steering velocities
Cartesian acc. of CoM, base and feet
Next footstep location
2nd layer:Trajectory pattern generator
1st layer:Whole body optimization
0 0.5 1 1.5 2 2.5 3 3.5 4 4.5−0.2
(A) Push recovery
(B) Model error
normalleft foot +0.5kgleft thigh +1kgtorso backpack15 degrees slope
normal100Hz loopnoisy IMU angles
(D) Rough terrain
0 0.5 1 1.5 2 2.5 3 3.5 4 4.5
Figure 1: Top: The Coman robot and our hierarchical control archi-tecture. Bottom: The walking sequence at 0.2m/s wherethe robot is pushed twice by a 10N force lasting 0.3s. Itrecovers by taking proper correcting footsteps.
of this work, we have formulated a discrete-time MPC prob-lem that optimizes future footsteps of the robot in closed formand linearly. Our controller is compliant and precise whileconsidering various physical constraints in its first layer. Itcovers a wide range of speeds and performs robustly whensubject to various internal or external perturbations. Themethod is generic and we have applied it on another hu-manoid robot (Atlas) in simulations, achieving similar per-formance . All movies of this work could be found onhttp://biorob.epfl.ch/page-99800-en.html.
This project is funded under the European Community’s 7thFramework Programme: FP7-ICT 611832 (WALK-MAN).Cognitive Systems and Robotics: FP7-ICT-2013-10.
References J. Pratt et. al., (2001), I. J. of Robotics Research. S. Feng et. al., (2013), Humanoids Conf. A. Herdt et. al., (2010), IROS Conf. J. Englsberger et. al., (2011), IROS Conf. T. Koolen et. al. (2012), I. J. of Robotics Research. S. Kuindersma et. al., (2013), In arXiv. J. Mattingley et. al., (2012), vol. 13, Springer. I. Mordatch et. al., (2010), ACM T. on Graphics. L. Sentis et. al., (2010), IEEE T. On Robotics. A. Herzog et. al., (2013), In arXiv. B. Stephens et. al., (2010), IROS Conf. E. S. Papastavridis, (2013), IEEE I. C. Mechatronics. S. Faraji et. al., (2014), ICRA Conf.