1. 프리뷰 제어(Preview Control)란 무엇인가?
미리 볼 수 있는 정보를 활용해 제어 성능을 극대화하는 기법
Kajita (2003) 논문에서 소개된 이 기법은 LQ 최적 제어 이론에 기반을 두며
향후 일정 시간 구간(NL 스텝)을 “프리뷰 구간”으로 설정하여 그 구간 동안의 참고(reference)값들을 모두 활용할 수 있게끔 설계된다.
이를 통해 보행 로봇의 ZMP(Zero-Moment Point) 추종 성능을 높이고, 결과적으로 안정적이고 유연한 보행 패턴을 실시간에 가깝게 생성.
1.1. 미래 reference zmp 활용해야 하는 이유
SSP 에서 DSP 로 넘어갈 때 ZMP 목표가 갑자기 바뀌는데, 이때 로봇의 중심(CoM)은 ‘갑자기’ 움직이지 못하므로 앞서 일정 시간 전에 미리 준비를 해야 원활하게 넘어갈 수 있다.
앞으로 발생할 참조 ZMP(예: 1초 ~ 2초 후)를 미리 알고 있다는 가정하에, 그 참조값을 feedforward 로 활용하여 정밀한 추종
2. ZMP 수식, cart-table model
ZMP 정의
- 발바닥과 지면 사이의 마찰 및 반작용력 등, 로봇에 작용하는 모든 힘과 모멘트를 종합했을 때, 수직축에 대한 모멘트가 0이 되는 지점.
- 발바닥 안에 ZMP가 위치하면 넘어지지 않는 동적 안정성을 확보할 수 있어, 2족 로봇 보행 제어에 자주 활용됨.
ZMP의 역할
- 2족 로봇에서 **“발바닥 면적 내 ZMP 유지”**가 곧 넘어지지 않고 보행하기 위한 핵심 조건.
- 로봇 무게중심(CoM)을 움직일 때, 그 결과로 생기는 ZMP가 발바닥 안에서 안정적으로 변화하도록 제어할 필요가 있음.
3. Pattern generation
ZMP 기준 궤적이 주어졌을 때
- 우리가 원하는 ZMP 궤적(스텝 순서, 발 디딜 위치)을 먼저 결정하면, 이를 만족하는 로봇 CoM 궤적을 역으로 계산해야 함.
- 기존에는 FFT(푸리에 변환)나 시간영역 이산화 등 Batch 계산으로 처리했는데, 스텝이 길어지면 매번 계산을 새로 해야 하는 단점이 있음.
서보(Servo) 문제로의 접근
- ZMP를 “출력”, 로봇 CoM 가속도의 미분 등을 “입력”으로 생각하면, 출력 추종(Tracking) 제어 문제로 재정의 가능.
- “원하는 ZMP를 계속 따라가도록” 제어기를 구성하면, 보행 패턴을 실시간/온라인에 가깝게 생성 가능.
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
제어 입력 ux = d/dt x” 로 설정하면 위 사진처럼 상태방정식 표현
로봇은 sampling time 마다 움직이므로 ‘연속방정식’->’이산방정식’ 형태로 바뀜
위와 같은 식을 얻을 수 있다.
참고
위에서처럼 A, B 행렬을 정의하면 Preview 가능한 LQ 문제가 된다
위와 같이 performance index 를 정의
그렇게 해서 성능지수 J를 최소화하는 u(k)를 수학적으로 풀면,
순서대로 오차에 대한 적분 제어, 피드백, 미래 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())
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()
파라미터 생성 코드에서 t_preview 와 같은 값들을 수정하고 실행하여 파라미터 값이 담긴 mat 파일을 생성하고 이후 pattern 생성 코드를 실행하면 된다.
5. 결과
t_preview = 1
t_preview = 1.2
더 미리 보는 느낌
6. 추가 공부
6.1. LQ
참고문헌
Biped walking pattern generation by using preview control of zero-moment point
0 Comments