Modelo Matemático Humanoide: LIPM + LQR + ZMP + Cinemática Inversa
1. Definición Física del Sistema
El modelo LIPM (Linear Inverted Pendulum Model) representa la dinámica del
centro de masa (COM) de un robot humanoide asumiendo:
- Masa concentrada en el COM
- Altura constante del COM
- Movimiento en eje sagital X
🎥 Videos Educativos
Péndulo Invertido en Carro - Control Systems
2. Origen del parámetro Zc
Zc representa la altura del centro de masa respecto al suelo:
zc = altura del COM
Se obtiene del balance de momentos respecto al suelo (ZMP).
p = x - (zc/g) * x¨
x¨ = (g/zc)(x - p)
3. Modelo Dinámico LIPM
🔹 Geometría Real del Péndulo
Centro de masa:
x = l sin(θ)
z = l cos(θ)
🔹 Energía Cinética
T = ½ m (ẋ² + ż²)
Derivando:
ẋ = l cos(θ) θ̇
ż = -l sin(θ) θ̇
Entonces:
T = ½ m l² θ̇²
🔹 Lagrangiano
L = T − V
L = ½ m l² θ̇² − m g l cos(θ)
🔹 Ecuación de Movimiento (Euler–Lagrange)
d/dt (∂L/∂θ̇) − ∂L/∂θ = τ
Resultado:
m l² θ¨ + m g l sin(θ) = τ
Forma final:
θ¨ = − (g/l) sin(θ) + τ/(m l²)
🔹 Aproximación Pequeña → LIPM
sin(θ) ≈ θ
x ≈ l θ
Entonces:
x¨ = (g / zc) x
🔹 Concepto ZMP
El ZMP es el punto donde la suma de momentos dinámicos es cero.
Permite mantener estabilidad del robot mientras camina.
Ecuación diferencial
x¨ = (g/zc)(x - p)
Definición de estados
X = [ x ; x˙ ]
Forma estado
X˙ = A X + B p
A = [ 0 1 ; g/zc 0 ]
B = [ 0 ; -g/zc ]
4. Control Óptimo LQR
Función de costo
J = ∫ (Xᵀ Q X + uᵀ R u) dt
La ley de control óptima es:
u = -K X
En el sistema humanoide:
ZMP = -K X
5. Sistema en Lazo Cerrado
X˙ = (A - B K) X
El sistema es estable si:
Eigen(A - BK) < 0
6. Trayectoria del Pie (Marcha)
Movimiento Horizontal
x_foot = x_base + Lstep * phase
Altura del pie
z_foot = Hstep sin(π phase)
7. Cinemática Inversa Pierna 2DOF
Geometría
D = (px² + pz² - L1² - L2²) / (2 L1 L2)
Rodilla
θk = atan2( √(1-D²), D )
Cadera
θh = atan2(pz, px) - atan2(L2 sin θk , L1 + L2 cos θk)
Tobillo
θa = -θh - θk
8. Cinemática Directa
Rodilla
xk = xh + L1 sin(θh)
zk = zh - L1 cos(θh)
Tobillo
xa = xk + L2 sin(θh + θk)
za = zk - L2 cos(θh + θk)
9. Interpretación Física
- LIPM aproxima dinámica humanoide real
- LQR estabiliza el COM
- ZMP garantiza estabilidad de contacto
- IK genera movimiento biomecánico
10. Modelo Completo Compacto
X˙ = A X + B(-KX)
X˙ = (A - BK) X
x¨ = (g/zc)(x - p)
11. Nivel Investigación
Este modelo es base para:
- Preview Control Humanoide
- MPC Humanoide
- Divergent Component of Motion (DCM)
- Capture Point Walking