Autonomous and Mobile Robotics Prof. Giuseppe Oriolo MPC-Based Humanoid Gait Generation with application to Pursuit-Evasion (slides prepared by Nicola Scianca and Daniele De Simone)
Introduction • COMANOID: Multi-contact Collaborative Humanoids in Aircraft Manufacturing started on January 1, 2015 2 Oriolo: AMR – MPC Gait Generation for Real-Time Pursuit Evasion
Introduction • Automate the process of printing brackets for wires in the aircraft 3 Oriolo: AMR – MPC Gait Generation for Real-Time Pursuit Evasion
Introduction • Airbus Group ready to deploy COBOT in production • COBOT can work only in the 60% of the environment 4 Oriolo: AMR – MPC Gait Generation for Real-Time Pursuit Evasion
Introduction • Environment too complex for a wheeled robot 5 Oriolo: AMR – MPC Gait Generation for Real-Time Pursuit Evasion
Introduction • Tasks are achieved in constrained or hard postures • Multi-contact situations 6 Oriolo: AMR – MPC Gait Generation for Real-Time Pursuit Evasion
Introduction 7 Oriolo: AMR – MPC Gait Generation for Real-Time Pursuit Evasion
Motivations • Human-robot coexistence • Robots and human workers share their workspace • We need to guarantee safety for both humans and robots 8 Oriolo: AMR – MPC Gait Generation for Real-Time Pursuit Evasion
Topics • Model Predictive Control for gait generation • Linear Inverted Pendulum model • MPC scheme • Stability constraint • Real-time pursuit-evasion between humanoid robots • Constant velocity moving obstacles • Changing velocity obstacles (pursuer) • Pursuit-evasion among fixed obstacles 9 Oriolo: AMR – MPC Gait Generation for Real-Time Pursuit Evasion
Model Predictive Control for Gait Generation Oriolo: AMR – MPC Gait Generation for Real-Time Pursuit Evasion
ሶ ሷ ሶ Linear Inverted Pendulum model The dynamics of the humanoid can be approximated to a Linear Inverted Pendulum (LIP) 𝑦 𝑨 = 𝑦 𝑑 − 1 𝜕 2 ሷ 𝑦 𝑑 where 𝜕 = /ℎ 𝐷𝑝𝑁 , or as a state-space representation 𝑦 𝑑 𝑦 𝑑 0 1 0 = 𝑦 𝑑 + −𝜕 2 𝑦 𝑨 𝜕 2 𝑦 𝑑 𝑨 𝑑 0 and has two associated modes, of which one is stable and the other is unstable 𝑦 𝑨 𝑦 𝑑 11 Oriolo: AMR – MPC Gait Generation for Real-Time Pursuit Evasion
ZMP-based gait generation Strategy: keep the ZMP inside the support polygon 1. Plan the footsteps start goal 12 Oriolo: AMR – MPC Gait Generation for Real-Time Pursuit Evasion
ZMP-based gait generation Strategy: keep the ZMP inside the support polygon 2. Plan a ZMP trajectory that is always inside the support polygon start goal 13 Oriolo: AMR – MPC Gait Generation for Real-Time Pursuit Evasion
ZMP-based gait generation Strategy: keep the ZMP inside the support polygon 3. Compute a CoM trajectory such that the ZMP moves as planned start goal 14 Oriolo: AMR – MPC Gait Generation for Real-Time Pursuit Evasion
ZMP-based gait generation Strategy: keep the ZMP inside the support polygon 4. Track the CoM trajectory start goal 15 Oriolo: AMR – MPC Gait Generation for Real-Time Pursuit Evasion
ZMP-based gait generation In order to ensure balance we need to keep the ZMP within the support polygon The LIP has a positive eigenvalue which pertains to an unstable mode This means that we could have a diverging CoM trajectory even if the ZMP is at all times within the support polygon balance ≠ stability 16 Oriolo: AMR – MPC Gait Generation for Real-Time Pursuit Evasion
Model Predictive Control A widely adopted approach to solving the balance problem employs Model Predictive Control (MPC) MPC can be used to generate a CoM trajectory, which can be tracked with standard Kinematic Control An important feature of MPC is that it allows to impose constraints 17 Oriolo: AMR – MPC Gait Generation for Real-Time Pursuit Evasion
Model Predictive Control MPC is a form of real-time optimal control At each iteration we compute the next input by optimizing over a short prediction horizon 𝑈 ℎ • At the 𝑙 -th iteration look for an optimal control sequence 𝑣 𝑙 , 𝑣 𝑙+1 , … , 𝑣 𝑙+𝑂 over the prediction horizon • Apply the first control input 𝑣 𝑙 , then shift the prediction window forward • Optimize again, this time from 𝑙 + 1 to 𝑙 + 1 + 𝑂 Shorter prediction horizons yield less optimality but faster computation 18 Oriolo: AMR – MPC Gait Generation for Real-Time Pursuit Evasion
Model Predictive Control The problem can be formulated as a minimization of a quadratic cost function 𝑣 + 𝑔 𝑈 ത 𝑣 𝑈 𝐼ത min ത 𝑣 for a linear system ( ത 𝑣 is a vector containing the next 𝑂 control inputs), subject to linear constraints 𝐵 𝑗 𝑣 ≤ 𝑐 𝑗 𝐵 𝑓 𝑣 = 𝑐 𝑓 This kind of optimization problem can be solved very efficiently through Quadratic Programming (QP) In particular, the inequality constraints are very suited for keeping the ZMP within the support polygon This allows for a robust solution, because we are not tracking any specific trajectory, any solution that satisfies the constraints is fine 19 Oriolo: AMR – MPC Gait Generation for Real-Time Pursuit Evasion
ሶ ሶ Model Predictive Control Our control variable is the ZMP velocity, assumed piecewise constant with timestep 𝜀 At this point we assume that the footsteps are preassigned We look for a ZMP trajectory that minimizes the quadratic cost function: 𝑙 2 + 𝑙 2 𝑌 𝑨 𝑍 𝑨 under the following constraints: • ZMP is always inside the support polygon (balance constraint) • CoM trajectory is bounded w.r.t. the ZMP (stability constraint) 20 Oriolo: AMR – MPC Gait Generation for Real-Time Pursuit Evasion
ሶ Balance constraints To maintain balance the ZMP has to be at all times within the support polygon At the predicted instant 𝑢 𝑙 + 𝑗𝜀 we apply the constraint 𝑙+𝑗 𝑘 − 1 𝑘 + 1 𝑙 + 𝜀 𝑚 ≤ 𝑦 𝑔 𝑦 𝑔 2 𝑡 ≤ 𝑦 𝑨 𝑦 𝑨 2 𝑡 𝑚=𝑙+1 predicted ZMP 𝑘 is the 𝑘 -th foot position, and 𝑡 is the size of the robot foot 𝑦 𝑔 21 Oriolo: AMR – MPC Gait Generation for Real-Time Pursuit Evasion
ሶ Stability constraint The LIP model has an unstable mode: it is possible for the ZMP to be within the support polygon while the CoM is diverging Solution: every bounded CoM trajectory 𝑦 𝑑 𝑢 is constrained to the generated ZMP 𝑦 𝑨 𝑢 by this relation ( 𝑢 𝑙 is the current time) ∞ 𝑦 𝑑 𝑢 𝑙 + 1 𝑓 −𝜕(𝜐−𝑢 𝑙 ) 𝑦 𝑨 𝜐 𝑒𝜐 𝑦 𝑑 𝑢 𝑙 = 𝜕 න 𝜕 𝑢 𝑙 Depends upon all future values of the ZMP 22 Oriolo: AMR – MPC Gait Generation for Real-Time Pursuit Evasion
Stability constraint Since we are making a prediction, we can impose this relation as a constraint on such prediction, at least up to 𝑈 ℎ The walking gait has a tendency to periodicity, we compute the integral after 𝑈 ℎ by infinitely replicating the control inputs (although there are other possible choices) Final stability constraint 𝑂−1 1 − 𝑓 −𝜕𝜀 1 𝑦 𝑨 (𝑢 𝑙+𝑗 ) = 𝑦 𝑑 𝑢 𝑙 + ሶ 𝑦 𝑑 𝑢 𝑙 𝑓 −𝑗𝜕𝜀 ሶ 1 − 𝑓 −𝑂𝜕𝜀 − 𝑦 𝑨 𝑢 𝑙 𝜕 𝜕 𝑗=0 23 Oriolo: AMR – MPC Gait Generation for Real-Time Pursuit Evasion
ሶ ሶ ሶ ሶ ሶ Automatic footstep placement No prior knowledge of the footstep positions. The predicted footsteps are included as additional control variables 𝑙 𝑙 𝑙 𝑙 𝑌 𝑨 𝑌 𝑔 𝑍 𝑍 𝑨 𝑔 The new goal is to track a reference velocity, which is done by adding a term to the cost function 𝑙 2 + 𝑙 2 + 2 𝑙 − 𝑤 𝑠𝑓𝑔 𝑌 𝑨 𝑍 𝑌 𝑑 𝑨 • In order to maintain linearity the balance constraints are enforced only in single support phases • The stability constraint is unchanged (depends only on the predicted ZMP trajectory) • Additional constraints on the predicted footsteps to ensure feasibility 24 Oriolo: AMR – MPC Gait Generation for Real-Time Pursuit Evasion
Simulations https://youtu.be/hYegqFoeCJc 25 Oriolo: AMR – MPC Gait Generation for Real-Time Pursuit Evasion
ሷ ሷ ሷ ሷ MPC on a Multi-Mass Model A second mass accounts for the contribution to the ZMP given by the swinging leg −1 1 + 𝑛 𝑨 𝑛 + 𝑦 𝑨,𝑁 + 𝑛 𝑨 𝑛 + 𝑦 𝑨,𝑢𝑝𝑢 = 𝑨 𝑁 + 𝑦 𝑨,𝑛 𝑁 𝑨 𝑁 + 𝑁 • Partially compensates for neglected angular momentum • Measured ZMP is closer to the nominal prediction • More robust to uncertainties 26 Oriolo: AMR – MPC Gait Generation for Real-Time Pursuit Evasion
Simulations https://youtu.be/CwpWX2isypk 27 Oriolo: AMR – MPC Gait Generation for Real-Time Pursuit Evasion
ሶ ሶ MPC for Walk-To Locomotion Instead of tracking a reference velocity, we want the cost function to penalize the distance from the goal 𝑙 2 + 𝑙 2 + 𝑌 𝑑 2 + 𝑍 2 𝑙 − 𝑦 𝑝𝑏𝑚 𝑙 − 𝑧 𝑝𝑏𝑚 𝑌 𝑨 𝑍 𝑨 𝑑 • Optimization needs to generate also footstep orientations • Constraints become nonlinear, need to be approximated ZMP constraint footstep placement constraint 28 Oriolo: AMR – MPC Gait Generation for Real-Time Pursuit Evasion
Simulations https://youtu.be/fmmehMItOGw 29 Oriolo: AMR – MPC Gait Generation for Real-Time Pursuit Evasion
Recommend
More recommend
