Locomotion cơ bản: Từ ZMP đến CPG

Vì sao Locomotion là bài toán khó nhất trong Robotics?

Nếu bạn hỏi bất kỳ roboticist nào rằng module nào khó nhất để làm việc, câu trả lời thường là locomotion -- điều khiển robot di chuyển. Lý do đơn giản: di chuyển đòi hỏi sự phối hợp đồng thời của hàng chục joints, cân bằng động lực học phức tạp, và tương tác liên tục với mặt đất -- một hệ thống mà mọi sai số nhỏ đều có thể khiến robot ngã.

Trước khi reinforcement learning (RL) thống trị như ngày nay, các kỹ sư đã phát triển nhiều phương pháp classical để giải quyết bài toán này. Trong bài viết đầu tiên của series Locomotion từ Zero đến Hero, chúng ta sẽ đi sâu vào 3 phương pháp nền tảng: ZMP, CPG, và Inverse Kinematics -- để hiểu tại sao chúng được phát triển, chúng giải quyết vấn đề gì, và tại sao chúng dần bị thay thế.

ZMP -- Zero Moment Point

Lịch sử và nguồn gốc

Zero Moment Point (ZMP) là một trong những khái niệm quan trọng nhất trong lịch sử robot locomotion. Khái niệm này được giới thiệu lần đầu bởi Miomir Vukobratovic và Davor Juricic vào năm 1968 tại Đại hội Cơ học Lý thuyết và Ứng dụng toàn Liên Xô ở Moscow. Thuật ngữ "Zero Moment Point" chính thức được đặt tên trong các công trình từ 1970 đến 1972.

Minh hoạt động thực tế đầu tiên của ZMP diễn ra tại Nhật Bản năm 1984, tại phòng thí nghiệm của Ichiro Kato (Đại học Waseda), trên robot WL-10RD thuộc dòng WABOT -- robot cân bằng động đầu tiên trên thế giới.

ZMP là gì?

ZMP là điểm trên mặt đất mà tại đó tổng moment của các lực phản lực (reaction forces) theo phương ngang bằng 0. Nói đơn giản hơn: đó là điểm mà robot "cân bằng" được -- nếu ZMP nằm trong vùng support polygon (hình chiếu của bàn chân xuống đất), robot sẽ không bị ngã.

Support Polygon (vung tiep xuc chan):
┌──────────┐
│          │
│   ZMP*   │  ← ZMP nam trong vung nay = ON DINH
│          │
└──────────┘

Neu ZMP vuot ra ngoai → robot THAT CAN BANG → nga

Công thức ZMP đơn giản:

# ZMP calculation (2D simplified)
# M: tong khoi luong, g: gia toc trong truong
# x_com, z_com: vi tri center of mass
# x_ddot, z_ddot: gia toc center of mass

def compute_zmp(x_com, z_com, x_ddot, z_ddot, g=9.81):
    """
    ZMP x-coordinate
    Khi ZMP nam trong support polygon → robot on dinh
    """
    zmp_x = x_com - z_com * x_ddot / (z_ddot + g)
    return zmp_x

Honda ASIMO -- ZMP thành công nhất

Honda ASIMO (ra mắt năm 2000) là ứng dụng thành công nhất của ZMP control. Honda phát triển hệ thống Predictive Movement Control dựa trên lý thuyết ZMP:

1. Ground Reaction Force Control: Đo và điều chỉnh lực phản lực mặt đất theo thời gian thực
2. Model ZMP Control: Dự đoán ZMP trajectory và điều chỉnh trước khi mất cân bằng
3. Foot Landing Position Control: Điều chỉnh vị trí đặt chân để giữ ZMP trong vùng an toàn

Với công nghệ này, ASIMO có thể đi trên sàn không bằng phẳng, leo dốc, và thậm chí chạy với tốc độ 9 km/h. ASIMO được coi là robot humanoid ảnh hưởng nhất trong lịch sử, định hình toàn bộ hướng nghiên cứu robot đi bộ sau này.

Honda chính thức ngừng phát triển ASIMO vào năm 2022 để tập trung vào các ứng dụng robotics có tiềm năng thương mại hơn như thiết bị hỗ trợ đi lại và phục hồi chức năng.

Hạn chế của ZMP

• Chỉ hoạt động trên mặt phẳng: ZMP giả định robot luôn tiếp xúc với mặt phẳng -- không xử lý được địa hình gồ ghề, đá, cát
• Conservative: Để giữ ZMP trong support polygon, robot phải đi chậm, chân ngắn -- rất khác với cách con người đi
• Tính toán nặng: Cần mô hình động lực học chính xác của toàn bộ robot, tính toán ZMP theo thời gian thực
• Không adaptive: Khi gặp địa hình mới, cần re-plan từ đầu thay vì tự điều chỉnh

CPG -- Central Pattern Generator

Cảm hứng từ sinh học

Trong tự nhiên, động vật không "suy nghĩ" về từng bước đi. Một con mèo có thể chạy, nhảy, leo cây mà không cần "tính toán" quỹ đạo của từng chân. Bí mật nằm ở Central Pattern Generator (CPG) -- các mạch thần kinh ở tủy sống có thể tạo ra các pattern vận động nhịp nhàng mà không cần tín hiệu từ não.

Auke Ijspeert (EPFL) là nhà nghiên cứu hàng đầu về CPG trong robotics. Bài review kinh điển của ông "Central Pattern Generators for Locomotion Control in Animals and Robots" tổng kết cách CPG hoạt động và cách áp dụng vào robot.

CPG hoạt động như thế nào?

CPG sử dụng coupled oscillators -- các bộ dao động được kết nối với nhau -- để tạo ra pattern di chuyển. Mỗi oscillator điều khiển một nhóm cơ (hoặc một joint của robot), và chúng đồng bộ với nhau thông qua coupling weights.

Mô hình oscillator phổ biến nhất là Matsuoka oscillator:

import numpy as np

class MatsuokaOscillator:
    """
    Matsuoka CPG oscillator -- tao rhythm cho locomotion
    Moi oscillator co 2 neurons uc che lan nhau
    """
    def __init__(self, tau=0.28, tau_prime=0.45, beta=2.5, w=2.0):
        self.tau = tau          # time constant
        self.tau_prime = tau_prime  # adaptation time constant
        self.beta = beta        # adaptation coefficient
        self.w = w              # mutual inhibition weight
        
        # State variables
        self.x1, self.x2 = 0.1, -0.1  # neuron activities
        self.v1, self.v2 = 0.0, 0.0    # adaptation variables
        self.y1, self.y2 = 0.0, 0.0    # outputs
    
    def step(self, dt, external_input=1.0):
        """One integration step"""
        self.y1 = max(0, self.x1)
        self.y2 = max(0, self.x2)
        
        dx1 = (-self.x1 - self.w * self.y2 - self.beta * self.v1 + external_input) / self.tau
        dx2 = (-self.x2 - self.w * self.y1 - self.beta * self.v2 + external_input) / self.tau
        dv1 = (-self.v1 + self.y1) / self.tau_prime
        dv2 = (-self.v2 + self.y2) / self.tau_prime
        
        self.x1 += dx1 * dt
        self.x2 += dx2 * dt
        self.v1 += dv1 * dt
        self.v2 += dv2 * dt
        
        return self.y1 - self.y2  # output signal

# 4 oscillators cho quadruped (moi chan 1 oscillator)
cpg_network = [MatsuokaOscillator() for _ in range(4)]

# Coupling: trot gait (chan cheo dong pha)
# Front-left + Rear-right dong pha, Front-right + Rear-left dong pha

Các gait pattern cơ bản

CPG có thể tạo ra nhiều kiểu di chuyển (gait) khác nhau chỉ bằng thay đổi phase relationship giữa các oscillator:

Ưu điểm của CPG

• Robustness: CPG tự nhiên chống lại nhiễu (noise) -- oscillator tự ổn định về rhythm
• Nhẹ, nhanh: Không cần mô hình động lực học phức tạp, chỉ cần oscillator equations
• Chuyển gait mượt mà: Thay đổi tham số coupling = chuyển từ walk sang trot sang gallop một cách liên tục
• Biological plausibility: Sát với cách động vật thực sự điều khiển vận động

Hạn chế của CPG

• Không tối ưu: CPG tạo ra pattern "đủ tốt" nhưng không phải tối ưu cho từng địa hình cụ thể
• Tham số tuning khó: Chọn đúng tau, beta, w, coupling weights đòi hỏi nhiều thực nghiệm
• Không học từ kinh nghiệm: CPG là open-loop -- không tự điều chỉnh khi gặp địa hình mới

Inverse Kinematics cho Legs

Từ vị trí chân đến góc joints

Inverse Kinematics (IK) giải quyết bài toán: cho trước vị trí mong muốn của bàn chân, tìm ra góc của các joints (hip, knee, ankle) để đạt được vị trí đó.

Đối với locomotion, IK thường được dùng kết hợp với một foot trajectory planner -- bạn vạch ra quỹ đạo bàn chân muốn đi theo (footswing trajectory), rồi dùng IK để tính góc joints tại mỗi thời điểm.

import numpy as np

def leg_ik_2d(x_foot, z_foot, L1=0.25, L2=0.25):
    """
    Inverse Kinematics cho chan robot 2 khop (2D)
    L1: chieu dai dui (hip -> knee)
    L2: chieu dai ong chan (knee -> foot)
    Returns: (theta_hip, theta_knee) in radians
    """
    # Khoang cach tu hip den foot
    d = np.sqrt(x_foot**2 + z_foot**2)
    
    if d > L1 + L2:
        raise ValueError("Vi tri ngoai tam voi!")
    
    # Cosine rule
    cos_knee = (L1**2 + L2**2 - d**2) / (2 * L1 * L2)
    theta_knee = np.arccos(np.clip(cos_knee, -1, 1))
    
    # Hip angle
    alpha = np.arctan2(x_foot, -z_foot)
    beta = np.arccos(np.clip((L1**2 + d**2 - L2**2) / (2 * L1 * d), -1, 1))
    theta_hip = alpha - beta
    
    return theta_hip, theta_knee

# Vi du: foot o vi tri (0.1, -0.4) tuong doi voi hip
hip_angle, knee_angle = leg_ik_2d(0.1, -0.4)
print(f"Hip: {np.degrees(hip_angle):.1f} deg, Knee: {np.degrees(knee_angle):.1f} deg")

IK + Footswing Trajectory

Một pipeline locomotion điển hình dùng IK:

Gait Planner → Foot Trajectory → IK → Joint Commands
                                         ↓
                                      PD Controller → Robot

Raibert Heuristic (Marc Raibert, MIT Leg Lab, 1986) là một trong những foot placement strategies cổ điển nhất: đặt chân ở vị trí mà triệu tiêu vận tốc ngang của COM (Center of Mass). Đơn giản nhưng hiệu quả đáng kinh ngạc -- vẫn được sử dụng làm baseline đến ngày nay.

Hạn chế của IK-based locomotion

• Cần foot trajectory planner tốt: IK chỉ giải bài toán "đặt chân đúng vị trí" -- vẫn cần ai đó thiết kế quỹ đạo chân
• Không xử lý được contact forces: IK là hình học thuần túy, không biết gì về lực và động lực học
• Brittle: Sai số nhỏ trong mô hình hoặc sensing -> sai số lớn trong thực tế

Bảng so sánh tổng hợp: ZMP vs CPG vs IK

Tại sao các phương pháp classical bị thay thế?

Vào khoảng 2019-2020, một sự chuyển đổi lớn xảy ra trong locomotion research. Reinforcement Learning bắt đầu vượt qua toàn bộ các phương pháp classical, và lý do chính là:

1. Sim-to-real transfer đã sẵn sàng

Với Isaac Gym (2021) và domain randomization, việc train RL policy trong simulation và deploy lên robot thật trở nên khả thi. Paper "Learning to Walk in Minutes Using Massively Parallel Deep Reinforcement Learning" (arXiv:2109.11978) của ETHZ chứng minh rằng chỉ cần vài phút GPU training là có policy đi được.

2. End-to-end learning > hand-crafted pipeline

Các phương pháp classical đòi hỏi hàng trăm tham số cần tuning thủ công: ZMP trajectory parameters, CPG oscillator weights, IK gain tuning, foot placement heuristics... RL học tất cả từ scratch, chỉ cần định nghĩa reward function.

3. Emergent behaviors

RL policy phát triển các hành vi mà không ai thiết kế -- tự động nâng chân cao khi gặp vật cản, nghiêng người khi rẽ, điều chỉnh gait khi địa hình thay đổi. Những hành vi này emerge từ optimization, không thể hand-code được.

4. Generalization

Một RL policy được train với domain randomization có thể hoạt động trên nhiều địa hình khác nhau mà không cần re-plan. ZMP/CPG cần re-design cho từng scenario.

Nhưng classical knowledge vẫn quan trọng

Đừng hiểu nhầm -- ZMP, CPG, IK không "vô dụng". Chúng vẫn được sử dụng:

• Reward shaping: Hiểu ZMP giúp thiết kế reward function tốt hơn cho RL (penalty khi ZMP ra ngoài support polygon)
• CPG + RL: Nhiều paper mới kết hợp CPG oscillators làm action space structure cho RL, như CPG-RL (arXiv:2211.00458) của Bellegarda & Ijspeert
• IK cho initialization: IK cung cấp standing pose để initialize RL training
• Safety constraints: ZMP constraints được dùng như safety monitor trong production systems

Tiếp theo trong series

Đây là Part 1 của series Locomotion từ Zero đến Hero. Bạn đã nắm được nền tảng lý thuyết -- ZMP, CPG, và IK -- cùng lý do tại sao RL đang thay thế chúng.

Trong bài tiếp theo, chúng ta sẽ đi vào chủ đề nóng nhất:

• Part 2: RL cho Locomotion: PPO, reward shaping và curriculum -- Cách RL được áp dụng cho locomotion từ reward design đến training pipeline

Bài viết liên quan

• RL cho Locomotion: PPO, reward shaping và curriculum -- Part 2 của series
• Quadruped Locomotion: legged_gym đến Unitree Go2 -- Part 3: hands-on training và deploy
• Walk These Ways: Adaptive locomotion một policy -- Part 4: paper analysis
• RL cơ bản cho Robotics: Từ Markov đến PPO -- Nền tảng RL trước khi đọc series này
• Simulation cho Robotics: MuJoCo vs Isaac Sim vs Gazebo -- So sánh các simulator dùng cho locomotion training