1. 프리뷰 제어(Preview Control)란 무엇인가?

미리 볼 수 있는 정보를 활용해 제어 성능을 극대화하는 기법

Kajita (2003) 논문에서 소개된 이 기법은 LQ 최적 제어 이론에 기반을 두며

향후 일정 시간 구간(NL 스텝)을 “프리뷰 구간”으로 설정하여 그 구간 동안의 참고(reference)값들을 모두 활용할 수 있게끔 설계된다.

이를 통해 보행 로봇의 ZMP(Zero-Moment Point) 추종 성능을 높이고, 결과적으로 안정적이고 유연한 보행 패턴을 실시간에 가깝게 생성.

1.1. 미래 reference zmp 활용해야 하는 이유

SSP 에서 DSP 로 넘어갈 때 ZMP 목표가 갑자기 바뀌는데, 이때 로봇의 중심(CoM)은 ‘갑자기’ 움직이지 못하므로 앞서 일정 시간 전에 미리 준비를 해야 원활하게 넘어갈 수 있다.

앞으로 발생할 참조 ZMP(예: 1초 ~ 2초 후)를 미리 알고 있다는 가정하에, 그 참조값을 feedforward 로 활용하여 정밀한 추종

image

2. ZMP 수식, cart-table model

ZMP 정의

  • 발바닥과 지면 사이의 마찰 및 반작용력 등, 로봇에 작용하는 모든 힘과 모멘트를 종합했을 때, 수직축에 대한 모멘트가 0이 되는 지점.
  • 발바닥 안에 ZMP가 위치하면 넘어지지 않는 동적 안정성을 확보할 수 있어, 2족 로봇 보행 제어에 자주 활용됨.

ZMP의 역할

  • 2족 로봇에서 **“발바닥 면적 내 ZMP 유지”**가 곧 넘어지지 않고 보행하기 위한 핵심 조건.
  • 로봇 무게중심(CoM)을 움직일 때, 그 결과로 생기는 ZMP가 발바닥 안에서 안정적으로 변화하도록 제어할 필요가 있음.
image 1
image 2

3. Pattern generation

ZMP 기준 궤적이 주어졌을 때

  • 우리가 원하는 ZMP 궤적(스텝 순서, 발 디딜 위치)을 먼저 결정하면, 이를 만족하는 로봇 CoM 궤적을 역으로 계산해야 함.
  • 기존에는 FFT(푸리에 변환)나 시간영역 이산화 등 Batch 계산으로 처리했는데, 스텝이 길어지면 매번 계산을 새로 해야 하는 단점이 있음.

서보(Servo) 문제로의 접근

  • ZMP를 “출력”, 로봇 CoM 가속도의 미분 등을 “입력”으로 생각하면, 출력 추종(Tracking) 제어 문제로 재정의 가능.
  • “원하는 ZMP를 계속 따라가도록” 제어기를 구성하면, 보행 패턴을 실시간/온라인에 가깝게 생성 가능.
image 3
image 5

1.5s 에서 지지발이 뒷발에서 앞발로 바뀌는데 원래대로 라면 1.5 s 에서 갑작스럽게 zmp 가 바뀜

ZMP가 1.5초에 ‘뒷발 → 앞발’로 재배치되는 순간, 로봇의 무게중심(CoM)은 갑자기 이동할 수 없다.

즉, “1.5초에 ZMP가 확 바뀔 예정”이라는 ‘미래 정보’를 사전에 알고 있어야, 아직 뒷발 지지 중인 1.5초 이전부터 CoM을 조금씩 앞으로 이동시키며 미리 준비할 수 있다.

그러나 단순한 피드백 제어만으로는 “아직 발생하지 않은 ZMP 변화를 즉시 알 수 없으므로” 선행 동작이 어렵다.

그래서 Preview Control(프리뷰 제어)가 필요.

3.1. Pattern generation by preview control

image 6

제어 입력 ux = d/dt x” 로 설정하면 위 사진처럼 상태방정식 표현

로봇은 sampling time 마다 움직이므로 ‘연속방정식’->’이산방정식’ 형태로 바뀜

image 8
image 7

위와 같은 식을 얻을 수 있다.

image 9

참고

image 10

위에서처럼 A, B 행렬을 정의하면 Preview 가능한 LQ 문제가 된다


image 11

위와 같이 performance index 를 정의

image 12

그렇게 해서 성능지수 J를 최소화하는  u(k)를 수학적으로 풀면,

image 13

순서대로 오차에 대한 적분 제어, 피드백, 미래 ref 의 feedforward

python 코드 구현

1. parameter 생성 코드

import numpy as np
from control.matlab import *
import matplotlib.pyplot as plt
import scipy.io
from control.matlab import ss, c2d, ssdata, dare
import os
zc = 0.22
dt = 0.01
t_preview = 1.2
Qe = 1e-4
R = 1e-6
def get_preview_control_parameter(zc, dt, t_preview, Qe, R):
    g = 9.81
    A = np.matrix([[0, 1, 0],
                   [0, 0, 1],
                   [0, 0, 0]])
    B = np.matrix([[0],
                   [0],
                   [1]])
    C = np.matrix([[1, 0, -zc/g]])
    D = 0;
    
    sys_c = ss(A, B, C, D);            
    sys_d = c2d(sys_c, dt);
    [A_d, B_d, C_d, D_d] = ssdata(sys_d);

    C_d_dot_A_d = C_d*A_d

    C_d_dot_B_d = C_d*B_d

    A_tilde = np.matrix([[1, C_d_dot_A_d[0,0], C_d_dot_A_d[0,1], C_d_dot_A_d[0,2]],
                         [0, A_d[0,0], A_d[0,1], A_d[0,2]],
                         [0, A_d[1,0], A_d[1,1], A_d[1,2]],
                         [0, A_d[2,0], A_d[2,1], A_d[2,2]]])
    B_tilde = np.matrix([[C_d_dot_B_d[0,0]],
                         [B_d[0,0]],
                         [B_d[1,0]],
                         [B_d[2,0]]])
    C_tilde = np.matrix([[1, 0, 0, 0]])

    Q = np.matrix([[Qe, 0, 0, 0],
                   [0, 0, 0, 0],
                   [0, 0, 0, 0],
                   [0, 0, 0, 0]])

    P, _, K  = dare(A_tilde, B_tilde, Q, R);

    Gi = K[0,0:1]
    Gx = K[0,1:]

    N = np.arange(0,t_preview,dt).reshape(1,-1)

    Gp = np.zeros(N.shape)

    Gp[0,0] = -Gi

    Ac_tilde = A_tilde - B_tilde * K;

    I_tilde = np.matrix([[1],[0],[0],[0]])

    X_tilde = -Ac_tilde.T*P*I_tilde;

    for i in range(1, N.shape[1]):
        Gp[0,i] = (R+B_tilde.T*P*B_tilde)**(-1)*B_tilde.T*X_tilde;
        X_tilde = Ac_tilde.T*X_tilde;
        
    return np.array(A_d), np.array(B_d), np.array(C_d), np.array(Gi), np.array(Gx), np.array(Gp)


A_d, B_d, C_d, Gi, Gx, Gp = get_preview_control_parameter(zc, dt, t_preview, Qe, R)

param_dict = {
    'zc': zc,
    'dt': dt,
    't_preview': t_preview,
    'A_d': A_d,
    'B_d': B_d,
    'C_d': C_d,
    'Gi': Gi,
    'Gx': Gx,
    'Gp': Gp
}

scipy.io.savemat('wpg_parameter.mat', param_dict)
print("Saved wpg_parameter.mat in:", os.getcwd())
print("Keys:", param_dict.keys())
image 17

2. pattern 생성 코드

import numpy as np 
import matplotlib.pyplot as plt  
import scipy.io 

def generate_zmp_trajectory(footstep, dt, t_step):
    n_step = len(footstep)
    zmp_x = []
    zmp_y = []
    k = 0
    for i in range(0, n_step*int(t_step/dt)):
        zmp_x.append(footstep[k][0])
        zmp_y.append(footstep[k][1])
        if i != 0 and i%int(t_step/dt) == 0:
            k += 1
    return zmp_x, zmp_y

def calc_preview_control2(zmp_x, zmp_y, dt, t_preview, t_calc, A_d, B_d, C_d, Gi, Gx, Gp):
    x_x = np.array([[0],
                    [0],
                    [0]])
    x_y = np.array([[0],
                    [0],
                    [0]])
    com_x = []
    com_y = []
    integral_e_x = 0  # 오차 적분을 저장할 변수 초기화
    integral_e_y = 0  # 오차 적분을 저장할 변수 초기화

    for i in range(0, int(t_calc/dt)):
        y_x = np.ndarray.item(C_d.dot(x_x))
        y_y = np.ndarray.item(C_d.dot(x_y))
        e_x = zmp_x[i] - y_x
        e_y = zmp_y[i] - y_y
        
        integral_e_x += e_x * dt  # 오차 적분 갱신
        integral_e_y += e_y * dt  # 오차 적분 갱신

        preview_x = 0
        preview_y = 0
        n = 0
        for j in range(i, i+int(t_preview/dt)):
            preview_x += Gp[0, n] * zmp_x[j]
            preview_y += Gp[0, n] * zmp_y[j]
            n += 1

        u_x = np.ndarray.item(-Gi * integral_e_x - Gx.dot(x_x) - preview_x)
        u_y = np.ndarray.item(-Gi * integral_e_y - Gx.dot(x_y) - preview_y)
        
        x_x = A_d.dot(x_x) + B_d * u_x 
        x_y = A_d.dot(x_y) + B_d * u_y
        com_x.append(x_x[0,0])
        com_y.append(x_y[0,0])

    return com_x, com_y


def main():
    print("ZMP Preview Control Simulation")
    wpg_param = scipy.io.loadmat('wpg_parameter.mat')
    A_d = wpg_param['A_d']
    zc = np.ndarray.item(wpg_param['zc'])
    dt = np.ndarray.item(wpg_param['dt'])
    t_preview = np.ndarray.item(wpg_param['t_preview'])
    A_d = wpg_param['A_d']
    B_d = wpg_param['B_d']
    C_d = wpg_param['C_d']
    Gi = np.ndarray.item(wpg_param['Gi'])
    Gx = wpg_param['Gx']
    Gp = wpg_param['Gp']
    
    # footstep = [[0.00, 0.00, 0.00],
    #             [0.25, 0.10, 0.00],
    #             [0.50, -0.10, 0.00],
    #             [0.75, 0.10, 0.00],
    #             [1.00, -0.10, 0.00]]

    footstep = [[0.0, 0.0, 0.0],
                [0.2, 0.06, 0.0], 
                [0.4, -0.06, 0.0],
                [0.6, 0.09, 0.0],
                [0.8, -0.03, 0.0],
                [1.3, 0.09, 0.0], 
                [1.7, -0.03, 0.0],
                [1.9, 0.09, 0.0],
                [2.0, -0.03, 0.0]]

    t_step = 0.6
    zmp_x, zmp_y = generate_zmp_trajectory(footstep, dt, t_step)
    t_calc = 4
    com_x, com_y = calc_preview_control2(zmp_x, zmp_y, dt, t_preview, t_calc, A_d, B_d, C_d, Gi, Gx, Gp)
    plt.title("ZMP VS CoM Trajectory")
    plt.plot(zmp_x, zmp_y, color='green')
    plt.plot(com_x, com_y, 'x', color='red')
    plt.show()

main()
image 16

파라미터 생성 코드에서 t_preview 와 같은 값들을 수정하고 실행하여 파라미터 값이 담긴 mat 파일을 생성하고 이후 pattern 생성 코드를 실행하면 된다.

5. 결과

t_preview = 1

image 14

t_preview = 1.2

image 15

더 미리 보는 느낌

6. 추가 공부

6.1. LQ

참고문헌

Biped walking pattern generation by using preview control of zero-moment point


0 Comments

Leave a Reply