Force-and-moment-based Model Predictive Control for Achieving Highly Dynamic Locomotion on Bipedal Robots

Junheng Li and Quan Nguyen

Abstract— In this paper, we propose a novel framework on force-and-moment-based Model Predictive Control (MPC) for dynamic legged robots. In specific, we present a formulation of MPC designed for 10 degree-of-freedom (DoF) bipedal robots using a simplified rigid body dynamics with input forces and moments. This MPC controller will calculate the optimal inputs applied to the robot, including 3-D forces and 2-D moments at each foot. These desired inputs will then be generated by mapping these forces and moments to motor torques of 5 actuators on each leg. We evaluate our proposed control design on physical simulation of a 10 degree-of-freedom (DoF) bipedal robot. The robot can achieve fast walking speed up to 1.6 m/s on rough terrain, with accurate velocity tracking. With the same control framework, our proposed approach can achieve a wide range of dynamic motions including walking, hopping, Fig. 1: 10-DoF Bipedal Robot. The bipedal robot walking through rough and running using the same set of control parameters. terrain with velocity vxd = 1.6 m/s. Simulation video: https:// youtu.be/Z2s4iuYkuvg. I.INTRODUCTION The motivation of studying bipedal robot is widely pro- motions that may cause instability due to under-actuation and moted by commercial and sociological interests [1]. The stabilize the system by solving for optimal force inputs to desired outcomes of bipedal robot applications range from the robot based on predictions . replacing human in hazardous operations, in which it requires MPC has been also utilized in the control of bipedal highly dynamic robots in unknown complex terrains, to the robot through various approaches. The control framework development of highly functional bipedal robot applications proposed in [8] applies MPC to minimize the values of in medical field and rehabilitation processes such as recent rapidly exponentially stabilizing control Lyapunov function development in research of powered lower-limb prostheses (RES-CLF) in a Human-Inspired Control (HIC) approach. for the disabled [2]. Reference [9] presents a revisited ZMP Preview Control Building effective force-based controller remains of the scheme, in which it attempts to solve an optimal control most important cornerstones of achieving dynamic motions problem by MPC that finds an optimal sequence of jerks of while keeping balance for legged robots, because bipedal robot center of mass (CoM). However, these approaches are robot has inherently less stable dynamical system, compared either based on position control to track an joint trajectory to other sub-classes of legged robots such as quadruped resulting from optimization; or tend to address the step- robots [1]. There are many controller design and choices by-step planning problem. In this work, we focus on a that are employed in bipedal balance control, including con- real-time feedback control approach that can handle a wide trollers that utilize popular Zero-Moment-Point (ZMP) and range of walking gaits, without relying on offline trajectory arXiv:2104.00065v1 [cs.RO] 31 Mar 2021 spring-loaded inverted pendulum (SLIP) model [3], [4], [5]. optimization. Both have great success in maintaining a stable locomotion Inspired by the successful force-based MPC approach for of bipedal robots (e.g. [3], [5]). Hybrid Zero Dynamics quadruped robots presented in [7], in this paper, we propose a method is another control framework that is employed in new control framework that utilizes MPC to solve for optimal bipedal robot ATRIAS, the method utilizes input-output ground reaction forces and moments to achieve dynamic linearization, a non-linear feedback controller, with virtual motion on bipedal robots. We investigate different models constraints that allows precise foot placement in periodic can be used for the MPC framework and introduce the walking gait [6]. Recently, force-based MPC control is intro- formulation that works most effectively on our bipedal robot duced for dynamic quadruped robots [7], allowing the robot model. Our proposed approach allows a 10-DOF bipedal to perform a wide range of dynamic gaits with robustness robot to perform high-speed and robust locomotion on rough to rough terrains. One advantage of the MPC framework in terrain. We implement and validate our controller design dynamic locomotion is that the controller can predict future in a high-fidelity physical simulation that is constructed in MATLAB and Simulink with the software dependency of Junheng Li and Quan Nguyen are with the Department of Aerospace and Mechanical Engineering, University of Southern California, Los Angeles, Spatial v2. CA 90089. email:[email protected], [email protected] The main contribution of the paper are as follows: • We proposed a new framework of force-and-moment- based MPC for 10-DoF bipedal robot locomotion. • We investigate different models of rigid body dynamics that can be used for the MPC framework. The most effective model is then used in our proposed approach. • The proposed MPC framework allows 3-D dynamic lo- comotion including fast-walking with accurate velocity tracking. • This MPC has success in hopping motion and potential to extend gait types to more dynamic and aggressive motion such as running. • Thanks to using the force and moment based control inputs, our approach is robust to rough terrain. We have Fig. 2: Robot Configuration. An overview of 10 DoF bipedal robot CAD successfully demonstrated the problem of fast walking model. with the velocity of 1.6 m/s on rough terrain. The rough terrain consists stairs with maximum height of 0.075 m and maximum difference of 0.055 m between two consecutive stairs. The rest of the paper is organized as follows. Section II introduces the model design and physical parameters of the bipedal robot. Simulation method and control architecture is also provided in this section. Section III presents the dynam- ics and controller choices, design, and formulation including both MPC force-and-moment-based control of stance leg and PD Cartesian space control of swing leg. Some result highlights are presented in Section IV along with analysis on controller performance in various dynamic motions.

II.BIPEDAL ROBOT MODELAND SIMULATION Fig. 3: Leg Configuration. The link and joint configuration of bipedal robot A. Robot Model left leg. In this section, we present the robot model that will be used throughout the paper. A 10 DoF bipedal robot a test bench for many forthcoming controller designs and consists of 5 joint actuation each leg. Commonly, a 10 DoF optimization implementations (see Fig. 4: simulation block bipedal robot has abduction (ab) and hip joints that allow diagram). 3-D rotation and ankle joints that allow double-leg-support The simulation software requires user to input desired standing (e.g. [10], [11]). states at the beginning of the simulation. The desired states The design of our bipedal consists robot body, ab link, hip form a column vector that consist desired body center of link, thigh link, calf link, and foot link (see Fig. 3 for leg mass (CoM) position p , desired body CoM velocity p˙ , configuration and Table I for physical parameters). The robot d d desired rotation matrix R (resized to 9×1 vector), and body is taken directly from Unitree A1 robot, in a vertical d desired angular velocity ω of robot body. configuration (see Fig. 2). The joint actuator modelled in d The simulation model shown in Fig. 4 provides a platform this robot design is the Unitree A1 torque motor. A1 motor for controller implementation with the advantages of fast is a low-weight yet powerful torque motor that is suitable simulation time, simplicity in modification and debugging, for joint actuation for medium-sized legged robot. Pairing and reliability. with the A1 robot body, the motor in use also has direct compatibility (see Table II for parameters of joint actuator). III.DYNAMICSAND CONTROL B. Simulation A. Simplified Dynamics The bipedal robot simulation is built primarily in MAT- In this Section, we investigate different dynamic models LAB and Simulink. The dependent software Spatial v2 is an that can be used in our MPC control framework. While implementation of spatial vector arithmetic and dynamics al- the whole-body dynamics of the bipedal robot is highly gorithms that is available in MATLAB scripts. The software non-linear, we are interested in using simplified rigid-body employs Roy Featherstone’s book Rigid Body Dynamics dynamics so that we can guarantee our linear MPC control Algorithms and provides a series of accessible MATLAB can be solved effectively in real-time. In addition, in order functions for robotic dynamics simulations [12]. The scope to guarantee the capability of absorbing frequent and hard of the simulation construction is to build easy-to-use and impacts from dynamic locomotion, design of bipedal robots reliable MATLAB and Simulink models that can be used as also requires light-weight limbs and connections. This has TABLE I: Robot Physical Parameters Parameter Symbol Value Units Mass m 11.84 kg 2 Body Inertia Ixx 0.0443 kg · m 2 Iyy 0.0535 kg · m 2 Izz 0.0214 kg · m Body Length lb 0.114 m Body Width wb 0.194 m Body Height hb 0.247 m Thigh and Calf Lengths l1, l2 0.2 m Foot Length ltoe 0.09 m lheel 0.05 m

Fig. 4: Simulation Block Diagram. The simplified block diagram and TABLE II: Joint Actuator Parameters control architecture of this bipedal simulation in Simulink. Parameter Value Units Max Torque 33.5 Nm Max Joint Speed 21 Rad/s follows:  D   m(p¨ + g)  1 F = c (1) D2 H˙ where allowed the weight and rotation inertia of each link part to   be very small compared to the body weight and rotation D1 = I3 I3 03 03 (2) inertia. Hence, the effect of link parts in the robot dynamics   may be neglected, to form a simplified dynamic of only D2 = (p1 − pc)× (p2 − pc)× LL (3) robot body that has a linear relationship [13], [14]. This is   also a common assumption in many legged robots’ controller 0 0 0 designs [15], [16]. L =  0 1 0  (4) 0 0 1 There are three simplified dynamics model that we have considered and tested, shown in Fig. 5. The main difference The term (pi − pc) denotes the distance vector from the between these three options is the number of contact points robot body CoM location to the foot i position in the world on each foot, contact location, and contact force and moment coordinate. And (pi − pc)× represents the skew-symmetric formation at each contact point. Model 1 resembles the matrix representing the cross product of (pi − pc)×Fi. H˙ MIT cheetah 3 simplified dynamics choice [14], with 4 can be approximated as H˙ = IGω˙ (as discussed in [13]), contact points exerting 3-D contact forces. However, with where IG stands for the centroid rotation inertia of robot this dynamics model applied to our 10-DoF bipedal robot, body and ω˙ represents the angular velocity of robot body under rotation motions testings in simulation, the robot is [14], [16]. unable to perform pitch motion. Model 2 is improved and further simplifies the contact points. With this simplified dynamics model, in simulation validation process, the robot is unable to perform roll motion correctly. Hence, model 3 excluded external moment around x-axis, with only 3-D contact forces and 2-D moments around y and z-axis. This model has allowed the robot to perform all 3-D rotations. Hence, model 3 is chosen to be the final simplified dynamics design in this framework. The validation of this decision process is shown in Section IV. The detailed derivation of the model 3 dynamics is presented as follows. (a) Model 1 (b) Model 2 (c) Model 3 The bipedal model in this paper has legs that both consists Fig. 5: Three Simplified Dynamics Models. Yellow dots on the figures of 5 DoF. Commonly, the external contact forces applied to represents the contact point locations in simplified dynamics a) 2 contact points at the toe and heel of each robot foot, 3-D forces applied b) 1 contact the robot are only limited to 3-D forces in many legged- point at the middle of the each foot, 3-D force and 3-D moments applied c) robot dynamics (e.g.[10], [13]). However, thanks to the 1 contact point at the middle of the each foot, 3-D force and 2-D moments additional hip and ankle joint actuation, the external applied applied. Model 3 is the final choice to be used in our proposed approach. moments can also be included in the robot dynamics, forming a linear relationship between robot body’s acceleration p¨c, Equation (1) to (4) describes the simplified dynamics rate of change in angular momentum H˙ about CoM [16], model 3. This model, by our choice, enforces the external T and contact force and moments F = [F1, F2] , where applied moment around x-axis Mix = 0, and thus the T Fi = [Fix, Fiy, Fiz, Mix, Miy, Miz] , i = 1, 2 , as force and moment input of each robot foot has the form T T F = [F1, F2] , where Fi = [Fix, Fiy, Fiz, 0, Miy, Miz] . The linearized dynamics in (11) can be represented in Hence only 5 force and moment inputs are directly mapped discrete-time form at each time step i to 5 additional joint torques in each leg. x[i + 1] = Aˆ[i] + Bˆ[i]u[i] (12) We use rotation matrix R as a state variable to represent ˆ the orientation of the robot body. R can be also directly Matrix A is a constant matrix computed from Ac(ψ) using converted to Euler Angles. Reference [7] presents the ap- a average yaw value during entire reference trajectory. Bˆ proximation of the angular velocity in terms of Euler angle matrix is computed from Bc([p1 − pc], [p2 − pc], ψ), using Θ = [φ, θ, ψ]T , where φ is roll angle, θ is pitch angle, and ψ the desired values of average yaw and foot location. The only is yaw angle, the angular velocity ω in the world coordinate exception is that at the first time step, Bˆ[1] is computed from can be expressed as current states of the robot instead of reference trajectory. A MPC problem with a finite horizon length k can be  cos θ cos ψ −sin ψ 0  φ˙ written in the standard form ω = cos θ sin ψ cos ψ 0 θ˙ (5)     k−1 −sin θ 0 1 ψ˙ X T min (xi+1 − xi+1 ) Qi(xi+1 − xi+1 ) + kuikRi x,u ref ref With small non-zero pitch angle and small roll angle, i=0 equation (5) can be approximated and rewritten as (13)  ˙   φ cos ψ sin ψ 0 s.t. x[i + 1] = Aˆ[i] + Bˆ[i]u[i], i = 0 . . . k − 1 (14) θ˙ ≈  − sin ψ cos ψ 0  ω (6) ψ˙ 0 0 1 − + ci ≤ Ciui ≤ ci , i = 0 . . . k − 1 (15) Hence the rotation matrix for small roll and pitch angles is obtained Diui = 0, i = 0 . . . k − 1 (16) φ˙ In (13), xi and ui are system states and control inputs at ˙ θ ≈ Rz(ψ)ω (7) time step i. Qi and Ri are diagonal matrices defining the ˆ ˆ ψ˙ weights of each controlled variable. A and B in (14) are the discrete-time system dynamic constraints from (12). c−,c+, Combing the approximated orientation matrix with small i i and C in (15) represents the inequality constraints of the roll and pitch and translational dynamics, the simplified i MPC problem. D in (16) represents the equality constraints. dynamics can be written as i In this problem, the equality constraint governs the optimal control input from MPC controller is a zero vector for swing  ˆ   ˆ    Θ Θ 0 foot. d pˆc pˆc 0   = A   + BF +   (8) The MPC controller solves the most optimal ground con- dt ωˆ  ωˆ  0 tact force and moment, therefore, the inequality constraints ˙ ˙ pˆc pˆc g govern the contact forces in x and y direction are within the where friction pyramid (17), where µ is the friction coefficient. The   contact forces in z-direction should also fall within the upper 03 03 Rz(ψ) 03 and lower bounds of force (18), where lower bound should 03 03 03 I3  A =   (9) be greater than zero. It is also important to restrict the joint 03 03 03 03 torques to be under the threshold of maximum torque which 03 03 03 03 the motor can generate, shown in (19).   03 03 03 03 −µFz ≤ Fx ≤ µFz  03 03 03 03  B =  −1 −1 −1 −1  −µFz ≤ Fy ≤ µFz (17) IG (p1 − pc)× IG (p2 − pc)× IG L IG L I3/m I3/m 03 03 0 < Fmin ≤ Fz ≤ Fmax (18) (10) By assigning gravity as additional state variable, now state τi ≤ τmax (19) xˆ = [Θˆ , pˆ , ωˆ, pˆ˙ , g]T will allow the dynamics in (8) to c c C. QP Formulation be rewritten into a linear state-space form With the linear dynamics in Section III-A and the MPC xˆ˙ (t) = A (ψ)xˆ(t) + B ([p − p ], [p − p ], ψ)u(t) (11) c c 1 c 2 c formulation in Section III-B, our controller can be formulated The linearized dynamics in (11) is suitable for convex as a quadratic program (QP) that can be solved effectively MPC formulation [7]. in real-time. Firstly, the dynamic constraints (11) for the entire MPC B. MPC Formulation prediction horizon can be written as: This section presents the formulation of MPC problem X = A x + B U (20) based on the standard MPC form (13-16) and constraints qp 0 qp applied to the system. where X is a column vector containing system states for the next k horizons, x[i + 1], x[i + 2] ... x[i + k] and U is a With Cartesian space PD control, the swing leg can move column vector containing optimal control inputs of current and be controlled to follow desired foot placement trajectory. state u[i] and next k − 1 horizons, u[i + 1], u[i + 2] ... u[i + The gait generator decides either the robot leg is in stance k − 1] at time step i. The QP problem can be written in the phase or swing phase in a fixed gait cycle and assign form appropriate controller to corresponding leg. Now the robot 1 has both swing and stance leg control, it is ready to test the min U T hU + U T f (21) U 2 MPC in simulation. s.t. CU ≤ d (22) IV. SIMULATION RESULTS In this section, we present numerical validation of our AeqU = beq (23) proposed approach for different dynamic locomotion. The where C and d are inequality constraint matrices, Aeq and simulation video for this paper is given in Fig. 1. For our beq are equality constraint matrices, and simulation, the bipedal robot model and ground contact model is setup in MATLAB with Spatial v2 software. The h = 2(B T MB + K) (24) qp qp MPC sampling frequency is set to 0.03s while the simulation is run at 1 kHz. One gait cycle, or 10 horizons are predicted f = 2B T M(A x − y) (25) qp qp 0 at each time step in MPC, in which each gait cycle is fixed Diagonal matrices K and M are weights for rate of change at 0.30 s. of state variables and force/moment magnitude. The resulting controller input from QP problem U = T [F1, F2] is mapped to joint torques of each leg by T τi = Ji Fi (26) where Ji is the Jacobian matrix of ith leg   Jv Ji = (27) Jω

Jv and Jω are the linear velocity and angular velocity (a) Snapshot of Model 1 in (b) Snapshot of Model 3 in Double-leg Stance Double-leg Stance components of Ji. Pitch Motion Comparison D. Swing Leg Control 10 5 As discussed earlier in this section, due to equality con- strains, the robot leg that is under swing phase does not exert 0

Pitch (Deg.) Pitch -5 ground contact forces, and therefore is not under control Model 1 Response Model 3 Response of force-and-moment-based MPC. In order to control the -10 Desired Input leg and foot position in each gait cycle, the desired foot 0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1 trajectory is under control in the Cartesian space with PD time (s) position controller. We obtain the current foot location by (c) Pitch Motion Comparison Forward Kinematics (FK). Foot velocity is computed by Fig. 6: Comparison of Model 1 and Model 3 in Pitch Motion Simulation T a) Snapshot at the end of simulation with model 1 b) Snapshot at the end of p˙ = J q˙ ◦ footi i i (28) simulation with model 3 c) Pitch motion response comparison with a 10 desired pitch input. where q˙i is the joint velocity state feedback of each leg at time step i. The swing leg force can be computed by treating the foot A. Validation of Simplified Dynamics attached to a virtual spring-damper system [17]. The foot First, we present the simulation results of simple rotation weight is reasonable to be neglected since it is very small motions during standing with both legs on the ground to compared to the robot body [13]. Following the PD control validate the claim in Section III that for the simplified law, the foot force can be written as dynamics used for control design, model 3 is a superior ˙ ˙ Fswingi = KP (pfootd − pfooti ) + KD(pfootd − pfooti ) choice over model 1 and model 2. (29) As mentioned in Section III, the simplified dynamics where KP and KD are PD control gains, or spring stiffness model 1 is unable to perform pitch motion. It is shown in and damping coefficient of the virtual spring-damper system. Fig. 6, a pitch motion comparison between using simplified ˙ dynamics model 1 and model 3. The later one is what we pfootd and pfootd are desired foot placement and velocity, respectively. ultimately chose to use in MPC formulation. It is observed Similar to (26), the joint torque can be computed by that the simulation result with model 1 does not response to desired pitch input, whereas model 3 can perform pitch τ = J T F swingi v swingi (30) motion. Velocity Tracking 1.2

1

0.8

0.6

0.4

0.2

(a) Snapshot of Model 2 (b) Snapshot of Model 3 in (m/s) X velocity CoM in Double-leg Stance Double-leg Stance 0 Actual Velocity Desired Velocity Roll Motion Comparison -0.2 10 0 0.5 1 1.5 2 2.5 3 3.5

5 Time (s) 0 Fig. 8: Velocity Tracking. Comparison of desired velocity input and actual

Roll (Deg.) Roll -5 velocity response in x-direction. The desired velocity curve keeps constant -10 from t = 0 s to t = 0.6 s and from t = 1.5 s to t = 3 s at vxd = 0 m/s 0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1 and vxd = 0.6 m/s, respectively. From t = 0.6 s to t = 1.5 s and from time (s) t = 3 s to t = 3.9 s, vxd increases linearly. 10

5 0 formed by stairs with various heights and lengths. The stair Model 2 Response Yaw (Deg.) Yaw -5 Model 3 Response heights range from 0.020 m to 0.075 m with a maximum -10 Desired Input 0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1 height difference of 0.055 m between two consecutive stairs. time (s) A snapshot of this simulation animation is provided in Fig. (c) Roll Motion Comparison 1. Fig. 7: Comparison of Model 2 and Model 3 in Roll Motion Simulation To validate the feasibility and potential of MPC loco- a) Snapshot at the end of simulation with model 2 b) Snapshot at the end motion through rough terrain, the robot is commanded to ◦ of simulation with model 3 c) Roll motion response comparison with a 10 follow a high desired velocity v = 1.6 m/s. Plots of desired roll input. xd CoM location, velocity, body orientation, and joint positions and velocities are shown in Fig. 9, Fig. 10, and Fig. 11, Also mentioned in Section III, we further simplified model respectively. It can be observed that the CoM location during 1 and added 3-D moment to each contact point to form this simulation is within a reasonable error region. The simplified dynamics model 2. However, in the roll motion position curves of the joints shows that the joint position test, the response with model 2 is incorrect to desired roll responses are smooth under the control of MPC and PD input and it also shows deviation in yaw angle, shown in Fig. Cartesian control. The MPC controller input is presented in 7. With model 3, the robot simulation succeed in roll motion Fig. 12. The force in the y-direction Fy and moment Mz has test. Therefore, we decide to use model 3 for our proposed the largest variation between the two legs. Hence it yields a approach. Following are simulation results for walking and slight y-direction displacement at the end of the simulation. hopping motion using MPC on model 3. As can be seen in CoM y-direction location in Fig. 9, the final y-direction location of body CoM is 0.0087 m at t = 1.8 s. B. Velocity Tracking The motor torque and angular velocity limits are shown in In this simulation, we test the MPC performance in for- Table II. The joint torques (shown in Fig. 13) and angular ward walking motion(positive x-direction) with time-varying velocities (shown in Fig. 11) during this entire simulation desired speed and a desired CoM height of 0.5 m. The are in the reasonable ranges and satisfy these constraints. velocity tracking plot is shown in Fig. 8, the actual response D. Bipedal Hopping curve with MPC shows a good tracking performance. The velocity response has a maximum deviation of 0.076 m/s On top of rotation and walking simulations presented compared to the desired input. The control framework with earlier in this section, we have also implemented other gaits MPC we present enables the bipedal robot to have dynamic such as hopping. The hopping gait consists of a double motions in 3-D. Besides walking forward, we also have support phase and a flight phase. A hopping gait illustration successful simulation results and demonstration in walking is shown in Fig. 14. It can be observed that during hopping sideways and diagonally. The simulation results can be found motion, the robot during the last quarter of each gait is in in the video (URL is under Fig. 1) associated with this paper. flight phase. To validate the feasibility in hopping motion with the current MPC formulation, we test hopping forward

motion with velocity vxd = 0.5 m/s. The CoM position C. High-velocity Walking in Rough Terrain and velocity plots are shown in Fig. 15. It is observed that In this simulation, we test the controller performance in the hopping motion can be performed with current MPC rough terrain locomotion. In specific, the robot is com- formulation, with the trade-off of a certain degree of error manded to walk through a 2.4-meter-long rough terrain in CoM velocity and height tracking. We plan to optimize Body CoM Position Body CoM Velocity Joint Position Joint Velocity 3 20 1.5 20 0 0 2 1 -20

-20 Ab joint (Deg.) joint Ab 1 0.5 0 0.5 1 1.5 (Rad/s) joint Ab 0 0.5 1 1.5 Time (s) Time (s)

0 0 CoM position X (m) X position CoM

CoM velocity X (m/s) X velocity CoM 20 0 0.5 1 1.5 0 0.5 1 1.5 20 Time (s) Time (s) 0 0 0.05 0.5 -20

-20 Hip joint (Deg.) joint Hip 0 0.5 1 1.5 (Rad/s) joint Hip 0 0.5 1 1.5 Time (s) Time (s) 0 0 20 50 -0.5 0

-0.05 CoM position Y (m) Y position CoM 0 0.5 1 1.5 (m/s) Y velocity CoM 0 0.5 1 1.5

Time (s) Time (s) 0 -20 Thigh joint (Deg.) joint Thigh Thigh joint (Deg.) joint Thigh 0 0.5 1 1.5 0 0.5 1 1.5 0.6 Time (s) Time (s) 20 0 -50 0.55 0 -0.5 -100

-20 Calf joint (Deg.) joint Calf -1 0 0.5 1 1.5 (Rad/s) joint Calf 0 0.5 1 1.5

0.5 CoM position Z (m) Z position CoM 0 0.5 1 1.5 (m/s) Z velocity CoM 0 0.5 1 1.5 Time (s) Time (s) Time (s) Time (s) 80 20 60 0 Fig. 9: Plots of Body CoM Position and Velocity in Rough Terrain 40 Simulation. 20 -20

Ankle joint (Deg.) joint Ankle 0 0.5 1 1.5 0 0.5 1 1.5 Ankle joint (Rad/s) joint Ankle Time (s) Time (s) Left Leg Right Leg Body Orientation in Euler Angles 2 Fig. 11: Plots of Joint Position an Velocity in Rough Terrain Simulation.

0 Roll (Deg.) Roll -2 MPC Forces and Moments 0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 0 time (s)

-50

(N) x

F -100 2 0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 0 Time (s) 50 Yaw (Deg.) Yaw -2

(N) 0 y

0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 F time (s) -50 0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 Time (s) 2 0

-200

0 (N) z

F -400 Pitch (Deg.) Pitch -2 0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 Time (s)

time (s) 0 -20

(Nm) -40 Fig. 10: Plots of Robot Orientation in Rough Terrain Simulation. y

M -60

0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 Time (s) the MPC formulation in the future work to enable faster and 10

5 (Nm) more aggressive hopping motion. z 0 M -5 0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 V. CONCLUSIONS Time (s) Left Leg Right Leg In conclusion, we introduced an effective approach of force-and-moment-based Model Predictive Control to Fig. 12: Plots of MPC Force and Moment in Rough Terrain Simulation. achieve highly dynamic locomotion on rough terrains for 10 degrees of freedom bipedal robots. Our framework also al- lows the robot to achieve a wide range of 3-D motions using the same control framework with the same set of control Simulink with Spatial v2 software dependency. We explore parameters. The convex MPC formulation can be translated and find the most suitable dynamics model for the 10-DoF into a Quadratic Program problem and solved effectively in bipedal robot and we have presented successful walking real-time of less than 1ms. Working in conjunction with simulations with time-varying velocity input, rough-terrain Cartesian PD control for swing leg, we validate the MPC locomotion with high velocity and results in hopping motion. performance in a simulation that is built in MATLAB and Simulation results have indicated that the control perfor- Joint Torque with MPC Body CoM Position Body CoM Velocity 1 2 20 0

-20 0.5 0

Ab Torque (Nm) Torque Ab 0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 Time (s)

0 -2 CoM position X (m) X position CoM 20 0 0.5 1 1.5 2 (m/s) X velocity CoM 0 0.5 1 1.5 2 0 Time (s) Time (s) -20 0.05 2

Hip Torque (Nm) Torque Hip 0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 Time (s) 0 0 20 0 -0.05 -2

-20 (m) Y position CoM 0 0.5 1 1.5 2 (m/s) Y velocity CoM 0 0.5 1 1.5 2 0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6

Thigh Torque (Nm) Torque Thigh Time (s) Time (s) Time (s) 2 20 0.5 0 0.48 0 -20 0.46

Calf Torque (Nm) Torque Calf 0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6

Time (s) 0.44 -2 CoM position Z (m) Z position CoM

0 0.5 1 1.5 2 (m/s) Z velocity CoM 0 0.5 1 1.5 2 20 Time (s) Time (s) 0 -20 Fig. 15: Plots of CoM Location and Velocity in Hopping Simulation

0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 Ankle Torque (Nm) Torque Ankle Time (s) Left Leg Right Leg [4] S. Kajita, M. Morisawa, K. Harada, K. Kaneko, F. Kanehiro, K. Fu- Fig. 13: Plots of Joint Torques in Rough Terrain Simulation. jiwara, and H. Hirukawa, “Biped walking pattern generator allowing auxiliary zmp control,” in 2006 IEEE/RSJ International Conference on Intelligent Robots and Systems, pp. 2993–2999, IEEE, 2006. [5] P. Holmes, R. J. Full, D. Koditschek, and J. Guckenheimer, “The dynamics of legged locomotion: Models, analyses, and challenges,” SIAM review, vol. 48, no. 2, pp. 207–304, 2006. [6] Q. Nguyen, A. Agrawal, X. Da, W. C. Martin, H. Geyer, J. W. Grizzle, and K. Sreenath, “Dynamic walking on randomly-varying discrete terrain with one-step preview.,” in Robotics: Science and Systems, vol. 2, 2017. [7] J. Di Carlo, P. M. Wensing, B. Katz, G. Bledt, and S. Kim, “Dynamic locomotion in the mit cheetah 3 through convex model-predictive control,” in 2018 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), pp. 1–9, IEEE, 2018. Fig. 14: Illustration of Bipedal Hopping in Simulation [8] M. J. Powell, E. A. Cousineau, and A. D. Ames, “Model predictive control of underactuated bipedal robotic walking,” in 2015 IEEE Inter- national Conference on Robotics and Automation (ICRA), pp. 5121– 5126, IEEE, 2015. mance in velocity tracking test has a maximum deviation [9] P.-B. Wieber, “Trajectory free linear model predictive control for stable of 0.076 m/s compared to desired input. And in the rough walking in the presence of strong perturbations,” in 2006 6th IEEE- terrain test, the robot is able to walk through a terrain with RAS International Conference on Humanoid Robots, pp. 137–142, IEEE, 2006. various heights while maintaining a high forward walking [10] G. T. Levine and N. M. Boyd, “Blackbird: Design and control of a velocity at 1.6 m/s. The control framework can be extended low-cost compliant bipedal robot,” to many other dynamic motions such as bipedal running [11] Y. Gong, R. Hartley, X. Da, A. Hereid, O. Harib, J.-K. Huang, and J. Grizzle, “Feedback control of a cassie bipedal robot: Walking, and hopping. We are working on improving performance standing, and riding a segway,” in 2019 American Control Conference and modifying MPC formulation for each type of motion, (ACC), pp. 4559–4566, IEEE, 2019. to achieve better results. This control framework and MPC [12] R. Featherstone, Rigid body dynamics algorithms. Springer, 2014. [13] Q. Nguyen, M. J. Powell, B. Katz, J. Di Carlo, and S. Kim, “Optimized design is also expected to be extended to a physical bipedal jumping on the mit cheetah 3 robot,” in 2019 International Conference robot for testing and validation in the near future. on Robotics and Automation (ICRA), pp. 7448–7454, IEEE, 2019. [14] G. Bledt, M. J. Powell, B. Katz, J. Di Carlo, P. M. Wensing, and S. Kim, “Mit cheetah 3: Design and control of a robust, dynamic REFERENCES quadruped robot,” in 2018 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), pp. 2245–2252, IEEE, 2018. [1] E. R. Westervelt, J. W. Grizzle, C. Chevallereau, J. H. Choi, and [15] M. Focchi, A. Del Prete, I. Havoutis, R. Featherstone, D. G. Caldwell, B. Morris, Feedback control of dynamic bipedal robot locomotion. and C. Semini, “High-slope terrain locomotion for torque-controlled CRC press, 2018. quadruped robots,” Autonomous Robots, vol. 41, no. 1, pp. 259–272, [2] H. Zhao, J. Horn, J. Reher, V. Paredes, and A. D. Ames, “First 2017. steps toward translating robotic walking to prostheses: a nonlinear [16] B. J. Stephens and C. G. Atkeson, “Push recovery by stepping for optimization based control approach,” Autonomous Robots, vol. 41, humanoid robots with force controlled joints,” in 2010 10th IEEE- no. 3, pp. 725–742, 2017. RAS International conference on humanoid robots, pp. 52–59, IEEE, [3] A. D. Ames, E. A. Cousineau, and M. J. Powell, “Dynamically stable 2010. bipedal robotic walking with nao via human-inspired hybrid zero [17] G. Chen, S. Guo, B. Hou, and J. Wang, “Virtual model control for dynamics,” in Proceedings of the 15th ACM international conference quadruped robots,” IEEE Access, vol. 8, pp. 140736–140751, 2020. on Hybrid Systems: Computation and Control, pp. 135–144, 2012.