Archive

Journal of Korea Robotics Society - Vol. 11 , No. 3

[ ARTICLE ]
Journal of Korea Robotics Society - Vol. 11, No. 3, pp. 183-192
Abbreviation: J. Korea Robot. Soc
ISSN: 1975-6291 (Print) 2287-3961 (Online)
Print publication date Aug 2016
Received 30 May 2016 Revised 8 Jul 2016 Accepted 18 Jul 2016
DOI: https://doi.org/10.7746/jkros.2016.11.3.183

이족 로봇의 무게 중심 수평 위치 고속 이동을 위한 실시간 힘 제어 기법
이이수1 ; 박재흥

Real-Time Force Control of Biped Robot to Generate High-Speed Horizontal Motion of Center of Mass
Yisoo Lee1 ; Jaeheung Park
1Graduate School of Convergence Science and Technology, Seoul National University (howcan11@snu.ac.kr)
Corresponding Author : Graduate School of Convergence Science and Technology, Seoul National University, Daehak-Dong, Gwanak-Gu, Seoul, Korea (park73@snu.ac.kr)


© Korea Robotics Society. All rights reserved.

Abstract

Generating motion of center of mass for biped robots is a challenging issue since biped robots can easily lose balance due to limited contact area between foot and ground. In this paper, we propose force control method to generate high-speed motion of the center of mass for horizontal direction without losing balancing condition. Contact consistent multi-body dynamics of the robot is used to calculate force for horizontal direction of the center of mass considering balance. The calculated force is applied for acceleration or deceleration of the center of mass to generate high speed motion. The linear inverted pendulum model is used to estimate motion of the center of mass and the estimated motion is used to select either maximum or minimum force to stop at goal position. The proposed method is verified by experiments using 12-DOF torque controlled human sized legged robot.


Keywords: Biped robot, Whole-body control, Operational space control, Center of mass control

1. 서 론

서비스, 산업, 재난 현장 등에서 보행이 가능한 이족 로봇(Biped robot)의 수요가 증가하고 활발히 개발되고 있다[1-4]. 하지만 많은 연구 개발의 노력에도 불구하고 이 족 로봇의 실용화는 아직 어려운 상황이다. 이족 로봇의 실용화가 어려운 이유는 여러가지가 있지만, 무엇보다 부유형 기저(Floating base) 로봇으로서 고정되지 않고 변동되는 지면 접촉 상태를 가지고 있기 때문에 균형 조 건을 고려한 제어가 필요하다는 점이 이족 보행 연구가 어려운 대표적인 원인이다. 따라서 고속의 동작 중에도 균형 유지가 가능한 동작 생성 방법이 중요한 기술 중 하 나로 여겨진다.

이족 로봇의 균형 유지를 위해서 ZMP (Zero Moment Point) 조건을 사용하는 것이 대표적이다[5]. ZMP는 압력 중심(Center of Pressure, 이하 CoP)과 같은 개념이므로 본 논문에서는 CoP라고 하겠다. 균형 유지를 위해 CoP가 지면과 접촉중인 발바닥 면 안에 포함 되도록 제어해야 한 다. 일반적으로 이족 로봇의 균형을 고려한 이동 기술에서 는 무게 중심(Center of Mass, 이하 CoM)이 주요 제어 대 상이 되고, CoM이 임의의 계획된 이동 경로를 추종하도록 제어하면서 다물체동역학(Multi-Body Dynamics)기반 균 형 제어기[6-10] 또는 F/T센서 피드백 제어 기반 균형 제어 기[11]로 CoP를 제어하기 위해서 CoM경로 수정 또는 로 봇 자세 변형을 하여 균형을 잃지 않고 목적지로의 이동을 수행할 수 있도록 한다.

하지만 짧은 시간 동안 큰 가속도가 요구되는 동작의 경우 균형 제어기로 단기적으로 균형 유지가 되더라도 CoM이 목적지에서 정지하지 못하는 상황[12]에 처해 로봇 이 균형을 잃고 넘어지거나 지지발의 위치를 옮겨야 하는 상황을 초래 할 수 있다. CoM에 큰 가속도가 필요한 대표 적인 고속 이동 사례가 이족 로봇의 보행인데, 일반적으로 보행에서는 고속으로 이동하면서 넘어지지 않기 위하여 CoP를 제어하는 한편 시간에 따른 CoM 운동 상태와 CoP간의 관계를 선형역진자(Linear Inverted Pendulum Model) 모형을 사용하여 예측하여 주어진 CoP 경로를 따 르도록 할 수 있는 CoM 경로를 생성한다[13,14]. 하지만 이 러한 보행 방법들은 CoP가 주요 제어 대상이므로 원하는 임의의 CoM의 목적지에 도달하게 하는 것은 어렵다. 만 약 CoM이 목적지로 고속 이동 하도록 제어 가능하다면 보행 외에도 자세 제어를 위한 CoM의 위치 제어 등 일반 적인 상황에서도 활용이 가능하므로, 본 연구에서는 활용 가능한 최대의 가속/감속도를 사용한 CoM의 고속 이동을 위한 방법을 제안한다.


2. 연구 개요

본 연구는 CoM이 최단 시간에 목적지에 도달하도록 하 기 위하여 발생 가능한 최대 크기의 가속도와 감속도만 사 용하여 이동하는 것을 목적으로 한다. 이를 위하여 이족 로봇의 CoM의 가속/감속도 중 로봇의 균형 유지 조건에 해당하는 CoP 조건과 마찰력 조건을 모두 만족하는 최대 크기의 값을 사용한다. 이는 주어진 조건 속에서 가장 빨리 원하는 위치로 이동 후 정지하는 것으로, 생성 가능한 최고 속도로 목적지로 이동하는 것과 같으며 이를 본 논문에서는 고속 이동이라고 한다.

CoP 조건과 마찰력 조건은 Fig. 1과 같이 이족 로봇의 지지발(Supporting foot)이 사각형의 형태라고 봤을 때, 지 면과 접촉하고 있는 발바닥 면의 중심 위치에 존재하는 좌 표계에서 x는 시상(sagittal), y는 측면(lateral), z는 수직 (vertical)에 해당하는 방향이라고 하고 발의 x방향 길이 는 lx , y방향 길이는 ly라 하면, 아래와 같이 나타낼 수 있다.


Fig. 1 

Biped robot and frame



x 방향 Cop 조건:lx/2<My/Fz<lx/2y 방향 Cop 조건:ly/2<Mx/Fz<ly/2x 방향 마찰력 조건 :μs<|Fx/Fz|μsy 방향 마찰력 조건:μs<|Fy/Fz|μs

여기서 Fx , Fy , Fz는 접촉 지점에서의 각 방향 별 힘을 의미하고, Mx , My는 접촉 지점에서의 각 축에 대한 회전 모멘트, μs는 정지 마찰계수를 의미한다. 위의 조건들은 접촉 지점의 힘과 모멘트로 구성되는 접촉력과 관련된 구속 조건이므로 접촉력과 CoM 사이의 관계를 안다면 CoM이 발생 가능한 최대 크기의 가속/감속도를 알 수 있다. 이 관계는 로봇 동역학을 계산하여 얻을 수 있다.

본 연구에서는 역할에 따라서 두 가지 동역학 모형이 사용하는데, 원하는 CoP를 생성하기 위한 CoM의 힘을 계산하기 위해서 접촉을 고려한 로봇의 전신 다물체동 역학을 사용하고, 시간의 흐름에 따른 CoM의 상태를 예측하기 위해서 선형역진자 모형을 사용한다. 접촉을 고려한 로봇의 전신 다물체동역학을 사용할 경우 순간 에 대해서 비교적 정확한 관계를 계산할 수 있기 때문에 원하는 CoP를 생성할 수 있는 CoM의 가속도를 계산할 수 있으므로 본 연구에서 원하는 CoP가 정해 졌을 때, 전신 다물체동역학을 사용하여 필요한 CoM의 가속도를 계산 후 힘 제어에 사용한다. 하지만 이 방법은 시간의 흐름에 따른 변화를 예측하기 어렵다는 단점이 있다. CoM의 위치를 제어할 때, 원하는 위치에서 CoM이 정지 하기 위해서는 가속 후 감속으로 전환해야 하며, 전환 시점에 대한 예측이 필요하다. 선형역진자 모형은 선형 2차 미분 방정식 형태로 CoM의 수평 방향 동작과 접촉 모멘트 사이의 관계를 표현하므로 시간에 따른 CoM의 움직임을 예측할 수 있는 장점이 있으므로 예측 모형으 로 선형역진자 모형을 사용한다. 하지만 수직 방향 동작 을 구속시키고, CoM 수평 동작 외 로봇의 다른 동작들은 반영되지 않으며, 지면에 대한 수평 방향 접촉 힘을 고려 하지 않아 마찰력 조건을 반영할 수 없으므로 본 연구에 서도 CoM의 수평 동작만 생성하고, CoP 조건만 고려한 최대 가속/감속도를 계산한다.

본 연구에서 고속으로 CoM을 이동하여 원하는 위치 에 도달 후 정지 할 수 있도록 하기 위하여 처음에는 목표 위치로 도달할 수 있는 힘을 내기 위한 최대 크기의 CoP를 사용하여 가속을 하고, 현재 CoM 위치와 속도 정보를 바탕으로 선형역진자 모형을 사용해서 감속하기 위한 최대 크기의 CoP로 전환해야 하는지에 대한 여부 를 실시간으로 판단한다. 만약 감속을 위한 CoP로의 전 환이 결정되었다면 실제 CoM이 정지할 때까지 감속을 위한 CoP를 사용한다. 이와 관련된 내용이 3장에서 소개 된다. 요구 CoP가 결정 되었다면 접촉을 고려한 다물체 동역학 기반 전신제어기(Whole-Body Controller)[15]를 사용하여 원하는 CoP 생성을 위해서 CoM에 작용해야 하는 수평 방향 힘의 크기를 실시간으로 계산하고 생성 한다. 로봇의 전신 동역학을 고려하여 필요한 힘을 계산 하고, 계산된 힘을 적분하여 위치, 속도 경로를 얻어 간 접적인 힘 제어를 하는 방식이 아니라 관절 토크 제어를 통해 직접적으로 필요한 힘을 생성하므로 상대적으로 정밀한 CoP 계산 및 제어가 가능하다. 이 내용은 4장에 정리되었다. 5장에서는 토크 제어가 가능한 12 자유도의 이족 로봇[16]을 사용하여 실험으로 제안한 방법을 검증 하였고, 마지막으로 6장에서 결론을 정리하였다.


3. 선형역진자 모형 기반의 CoP 결정 방법

여기서는 각 방향 별 CoM의 목표 위치 xgoal, ygoal가 정해졌다면 목표 위치에서 CoM이 정지하도록 제어하는 방법을 제안한다. 이를 위해 목표 위치로의 가속 단계, CoP 전환을 통해 가속에서 감속 단계로의 전환 단계, 감속을 통해 정지하는 감속 단계로 나누게 된다.

3.1. 가속을 위한 CoP 선택

정해진 목표 위치 xgoal, yg로 이동하기 위한 속도를 가지도록 하는 가속도를 생성해야 한다. 선형역진자 모 형에 따르면 CoM에 생성되는 힘은 지면과 접촉하고 있 는 발의 CoP와의 관계로 표현이 된다. 이는 Fig. 2에서 보듯이 CoM 보다 뒤에 CoP가 있다면 앞 방향으로 가속 하는 힘을 받고 CoM 보다 앞에 CoP가 있다면 뒤 방향으 로 가속하는 힘을 받게 된다. 따라서 가속도를 발생시키 기 위한 x방향 CoP를 xcop, acc라고 했을 때, 만약 xgoal>x˜com이면 양수 방향의 가속도가 필요하므로 xcop, acc = -lx/2 가 되고, xgoal<x˜com이면 음수 방향의 가속도가 필요하 므로 xcop, acc = lx/2가 된다. 여기서 x˜com은 CoM의 현재 위치를 의미한다. xcop, acc가 정해졌다면 CoP 요구 값 xcop, d = xcop, acc로 결정한다. y방향은 x방향과 동일한 방 식을 사용하므로 생략한다.


Fig. 2 

Relation between CoP and direction of applied force on CoM



3.2. 감속을 위한 CoP로의 전환 및 정지

CoP 전환을 하는 순간은 가속을 멈추고 감속을 시작 해야 하는 때이다. 감속을 시작하는 시간은 CoP 전환 후 감속을 했을 때 원하는 목표 위치에서 CoM이 정지할 수 있을 것으로 예측되는 순간이어야 한다. 정지했을 때 위치를 예측하기 위하여 선형역진자 모형을 사용했을 때, CoM과 CoP의 관계는 직교 좌표계에서 아래의 식으 로 표현된다.

x¨comghxcom=ghxcopy¨comghycom=ghycop(1) 

여기서 xcom, ycom은 각 방향의 CoM의 위치를 나타내고, xcop , ycop는 각 방향의 CoP의 위치를 나타내며 g는 중력 가속도의 크기, h는 CoM의 수직 방향 높이로 상수에 해당한다. 미분 방정식 (1)을 풀면 아래와 같이 시간에 관한 위치와 속도 식을 유도하여 CoM의 거동을 예측할 수 있다.

xcom(t)=(x˜comxcop)cosh(wt)+x˙˜comwsinh(wt)+xcop(2) 

x˙com(t)=w(x˜comxcop)sinh(wt)+x˙˜comcosh(wt)(3) 

여기서 w=g/h 이다. y방향은 동일한 식의 형태이므 로 생략한다. 감속 후 정지했을 때의 시간 tx, stop은 식 (3)에서 x˙com(tx,stop)=0이라고 했을 때 아래와 같이 표 현된다.

tx,stop=1warc tanh(x˙˜comw(x˜comxcop,dcc))(4) 

여기서 xcop, dcc는 감속을 위한 경계 위치의 CoP를 의미하 므로 xcop,dcc=sign(x˙˜com)lx/2가 된다. 식 (4)를 식 (2)에 대입하면 정지했을 때의 위치 xcom(tx,stop)을 계산할 수 있다. 만약 abs(xgoalxcom(tstop))<σ 가 된다면 xcop,d=xcop,dcc 로 전환한다. 여기서 σ는 임계값(Threshold)으로 xgoalxcom(tstop)가 일정 이상 가까워지면 감속을 시작 한다. 이와 같이 CoP 전환 여부를 판단하는 과정은 이동 이 시작된 후 실시간으로 항상 수행한다. y방향은 x방 향과 독립적이며 동일한 식의 형태이므로 생략한다.

CoP 전환을 했다면 앞에서 xcop, d = xcop, dcc라고 정한 CoP 요구 값을 사용하여 감속을 지속하여 x˙˜com0이 되어 정지 상태라고 판단할 수 있을 때까지 유지한다.


4. CoP 제어를 위한 CoM 힘 제어 기법

3장에서 결정된 CoP를 생성하기 위해서 접촉을 고려 한 로봇 전신 다물체동역학을 바탕으로 한 로봇의 전신 제어기를 사용한다. 로봇 전신 동역학을 사용할 경우 선 형역진자 모형을 사용한 방법보다 정확하게 CoP와 CoM의 관계를 계산할 수 있고, 이를 바탕으로 전신 제어 기로 원하는 크기의 CoP를 생성할 수 있는 장점이 있는 데 전신 동역학을 고려한 CoP 예측 값의 정확성은[9]의 실험에서도 확인할 수 있다. 따라서 접촉을 고려한 부유 형 기저 로봇의 작업 공간 힘 제어가 가능한 전신 제어기 [15]를 바탕으로 원하는 CoP를 생성하기 위한 CoM 수평 방향 힘 제어 기법을 정리한다. 3장과 같이 지지발의 중 심에 위치한 직교 좌표계를 전역 좌표계로 사용하였다.

4.1. 접촉을 고려한 전신 제어기의 구성

접촉을 고려한 전신 제어기에 따르면 제어 토크 Γa는 작 업공간의 힘 벡터 F 와의 관계는 아래와 같이 표현된다.M5

Γa=J¯TST¯F=J˜TF(5) 

여기서 J는 작업 공간에 대한 자코비안(Jacobian) 행렬 이고, J 는 자코비안 행렬의 Generalized Inverse를 의미 한다. 선택 행렬(Selection matrix) S 는 부유형 기저의 동 작을 나타내는 가상의 6개의 관절을 제외한 실제 작동하 는 관절을 선택하는 역할을 한다. 본 연구에서 Γa는 다시 아래와 같이 두 개의 성분으로 분리해서 고려하는데,

Γa=Γ1+N˜1TΓ2=J˜1TF1+N˜1TJ˜2TF2(6) 

여기서 Γ1은 CoM의 수평 방향의 선형 동작에 해당하는 제어 토크, J1Γ1에서의 제어 대상의 자코비안 행렬이 다. 본 연구에서 J1은 CoM의 x, y방향 성분이 각각 1행과 2행을 구성하도록 하였다고 가정한다. F1J1에 의해 제 어되는 대상에 요구되는 2×1크기의 힘 벡터로 x, y방향에 대한 무게 중심에서의 힘을 의미한다. Γ2는 CoM의 수평 방향을 제외한 제어 대상에 대한 제어 토크다. N˜1TJ1에 의한 작업 공간에 대한 영공간(Null Space) 투영 행렬이고 N˜1T=IJ˜1T¯J˜1T으로 정의된다. J2Γ2에서의 제어 대 상의 자코비안 행렬이다. F2J2에 의해 제어되는 대상 에 요구되는 힘 벡터가 되는데 선형역진자 모형의 가정에 의해서 최소한 CoM의 수직방향 높이를 일정하게 유지하 기 위한 제어가 포함되어야 하고 이와 같이 미리 계획된 값을 따르도록 결정된 값이다. N˜1T의 사용으로 Γ2Γ1에 의한 제어에 영향을 주지 않는다. 실제 로봇 관절에는 제 어 토크 외에 중력 보상 토크 Γp와 코리올리/원심력 보상 토크 Γμ가 함께 인가된다.

4.2. CoP 제어를 위한 CoM 제어 힘 계산

제어 토크 명령치를 실제 관절 토크 제어기에 인가하기 전에 지면 접촉지점에서 발생할 것으로 예상되는 접촉력 (Fc)을 계산할 수 있는데 로봇 동역학에 의해서 아래와 같이 계산 가능하다.M7

Fc=J¯cTST(Γa)μcpc(7) 

여기서 Jc는 접촉 지점에 대한 자코비안 행렬 μc, pc는 각각 접촉 지점 공간에서의 코리올리/원심력 벡터, 중력 벡터를 나타낸다. 단발 지지에서 Fc는 지지발의 x방향 힘, y방향 힘, z방향 힘, z축 회전 모멘트, y축 회전 모멘 트, z축 회전 모멘트 순서로 구성된 6×1벡터가 되고, 양 발 지지 상황에서는 오른발의 힘과 모멘트, 왼발의 힘과 모멘트로 순서로 구성된 12×1벡터가 된다. 특히 양 발 지지 상황에서는 각 발의 접촉력을 모두 고려해야 하기 때문에 계산의 편의를 위해 Fig. 3과 같이 푸른 색으로 표현한 두 발의 외곽 경계 선 내에 존재하는 영역 안에 녹색 직사각형으로 이뤄진 하나의 지지면으로 고려한다. 이 영역은 선형역진자 모형에서 가정한 지면 접촉면과 동일하게 취급한다.


Fig. 3 

Boundary for double support phase



양 발 각각의 접촉력을 새로 구성한 지지발 O좌표계를 중심으로 6×1접촉력인 oFc로 재구성 할 필요가 있다.M8

oFc=oKFc=[oR03×3oR03×3oRoRoRP^loR]Fc(8) 

여기서 oR 는 임의의 좌표계에서 표현되던 오른발과 왼 발의 접촉력을 O좌표계로 변환해주는 회전 행렬을 의미 하고, P^rP^l은 각각 오른발과 왼발의 접촉 지점 좌표 계에서 O좌표계로 향하는 거리 벡터를 반대칭(skew symmetric) 행렬로 변환한 것이다. 단발 지지의 경우 이 런 과정 없이 oFc = Fc라고 할 수 있다. 마찬가지로 μc, pcO좌표계 기준으로 변환한 벡터 oμc, opc를 사용한다.

로봇의 접촉력 oFc를 통해 예측 CoP는 아래의 식으로 계산 된다.

xcop=oMy/oFzycop=oMx/oFz(9) 

여기서 xcop, ycop는 예측한 CoP의 x, y방향 성분이고, oFz, oMy , oMx는 각각 접촉력 oFc구성 성분 중 수직방향 힘, y축 회전 모멘트, x축 회전 모멘트를 의미한다. 원하 는 CoP 생성을 위하여 F1[xcopycop]T의 관계를 알 아야 한다. oFc성분은 서로 독립적인 4개의 성분oFc,1=J¯cTSTΓ1,oFc,2=J¯cTSTN˜1TΓ2,oμc,opc로 분리해서 생 각할 수 있다. 여기서 다시 oFc,com=oFc,1opc,oFc,bias=oFc,2oμc,로 2개로 나눠서 생각할 수 있는데, oFc, com는 CoM의 수평 방향 제어와 로봇의 하중에 의한 접촉력을 나타내고, 선형역진자 모형 역시 CoM의 수평 방향 선형 운동과 중력만 고려하므로 둘이 동일한 접촉력 성분을 표현한다고 할 수 있다. 따라서 식 (9)는 선형역진자 모 형으로 표현되는 CoP와 나머지 성분에 의해 표현되는 CoP로 분리하여 아래와 같이 다시 표현할 수 있다.

xcop=oMy,COMoFzoMy,biasoFzycop=oMx,CoMoFzoMx,biasoFz(10) 

oFz=oFz,1+oFz,2+oFz,μ+oFz,p라 할 수 있는데, 여기 서 oFz,1,oFz,2,oFz,μ,oFz,p는 각각 순서대로 F1 , F2 , 코리 올리/원심력, 로봇의 무게에 의해 발생하는 수직 방향 접촉 힘을 의미한다. oFz,1은 CoM의 수평 방향 동작에 의해 발생하는 수직 접촉 힘이므로 전체 수직 방향 접촉 힘 oFz에 미치는 영향이 나머지 접촉력 성분에 비해 상대 적으로 매우 적다고 가정하게 되면, oFz,1+oFz,2+oFz,μ+oFz,poFz,2+oFz,μ+oFz,p라 할 수 있다. 식 (10)에서 우변의 첫번째 항은 선형역진자 모형에서 계산한 CoP와 같다고 표현할 수 있으므로, 3장에서 계산하여 얻은 xcop,d,ycop,d는 아래와 같이 표현 할 수 있다.

xcop,d=oMy,CoMoFz,2+oFz,μ+oFz,pycop,d=oMx,CoMoFz,2+oFz,μ+oFz,p(11) 

식 (10)의 우변 두 번째 항과 같이 oFc, bias에 의한 CoP 가 존재하는데, 이 값으로 인해 전체 CoP가 지면 접촉면 밖으로 벗어나는 상황을 방지하기 위하여 지면 접촉면 의 lx , ly는 안전 마진(Margin)을 설정하여 실제 접촉면 의 각 방향 길이보다 작게 설정해야 한다. 그 안전 마진 범위 안에 CoP 값 oMy,bias/oFz,oMx,bias/oFz가 존재하도 록 Γ2에 제한을 둔다.

식 (11)의 관계식을 통해서 xcop, d, ycop, d를 만들기 위해 필요한 접촉 모멘트 oMx,1 , oMy,1을 다음과 같이 얻을 수 있다.M13

oMy,1=(oFz,2+oFz,μ+oFz,p)xcop,doMy,poMx,1=(oFz,2+oFz,μ+oFz,p)ycop,doMx,p(12) 

여기서 oMx,1 , oMy,1, oMx, p , oMy, p는 순서대로 F1에 의한 x축 회전 모멘트와 y축 회전 모멘트, 로봇 무게에 의한 x축 회전 모멘트와 y축 회전 모멘트를 나타낸다. CoM 의 수평 방향 힘 생성만 사용하여 필요한 접촉 모멘트 값을 생성하기 위하여 F1과 접촉 모멘트 oMx,1 , oMy,1의 관계를 아래와 같이 유도한다.

[oMx,1oMy,1]=SfoKJ¯cTSTΓ1=(SfoKJ¯cTSTJ˜cT)F1=QF1(13) 

Sf는 6개의 힘과 모멘트 성분 중 oMx,1, oMy,1를 선택하 기 위한 2×6크기의 행렬로 여기서는 아래와 같다.M14

Sf=[000100000010](14) 

결과적으로 F1 은 아래와 같이,M15

F1=Q1[oMx,1oMy,1](15) 

가 된다. 여기서 Q가 2×2정방 행렬이고 이를 식 (6)을 통해 Γ1을 제어한다면 원하는 CoP를 생성하기 위해서 CoM의 수평 동작만 이용하고 다른 움직임은 생성하지 않는다. Sf와 같은 선택 행렬을 사용한 것은 CoM의 수 평 힘 F1을 통해 oMx,1 , oMy,1만을 원하는 값으로 결정하 고자 함이다.


5. 실 험

제안한 연구 내용이 실제 로봇에서 사용이 가능한지 검증하기 위하여 12 자유도의 토크 제어가 가능한 이족 로봇 Dyros Red (Fig. 4)[16]를 사용하여 실험하였다. 로봇 의 높이는 약 1.425 (m) 고 무게는 약 54.6 (kg)이다. 발 바닥의 길이는 0.3 (m), 폭은 0.15 (m)이다. 제어 주기는 2 kHz였으며, 로봇의 역동역학 연산은 약 500 Hz 주기로 갱신되었다. 로봇의 양 발목에 6축 F/T센서가 설치되어 있으며 CoP 측정에만 사용되고 제어에 활용 되지는 않 았다. 실험할 때 로봇의 자세는 Fig. 4와 같았으며 CoM 의 수직 높이는 약 0.6 (m)였다.


Fig. 4 

Snapshot of a biped leg robot “Dyros Red”



5.1. 목표 위치 추종 실험

제안한 방법을 사용하여 앞뒤 방향에 해당하는 x방향 제어와, 좌우 방향에 해당하는 y방향 제어를 각각 따로 수행하였다. 양 발 지지 상황에 대한 실험이며 지지면은 Fig. 4의 주황색 점선과 같이 양 발을 둘러싼 직사각형으 로 가정하였으며 안전 마진이 적용된 직사각형의 범위 lx = 0.1(m), ly = 0.19(m)가 되고 좌표계는 직사각형의 중앙에 위치한다. 실제 실험 시 lxly는 안전을 위한 마진을 두고 실제 발의 크기보다 작게 설정했고, 마진을 포함한 직사각형의 범위는 lx = 0.15(m), ly = 0.3(m)로 전체 면적의 약 30% 면적이 마진으로 적용되었다. Γ2에 서 제어한 대상은 CoM의 수직 높이, 상체 몸통의 3축 회전 방향이며 모두 초기값을 유지하도록 F2=Λ2kpxe를 적용하여 비례 제어기로 제어하였다. 여기서 Λ2는 작업 공간에서의 관성 행렬이고, kp는 제어 이득 값, xe는 제 어 위치 오차를 의미한다.

제안한 방법을 사용하여 x 방향에 대한 CoM 제어 결 과는 Fig. 5에서 확인할 수 있다. 푸른색 선이 목표 위치 를 나타내고, 붉은색 선이 실제 실험한 로봇의 CoM 위치 를 나타낸다. 0.04 (m) 이동 하도록 명령하였을때, 약 0.71 (sec)에 걸쳐서 목적지로 이동 후 정지 하였고 정지 후 위치 오차는 약 0.0047 (m)였다. 이때의 x방향 CoP 결과는 Fig. 6에서 보여진다. 푸른색 선은 CoM 수평 방 향 제어를 위한 Γ1에 의한 CoP 기대 값이 되고 이는 3장에서 제안한 방법에 의한 요구 CoP와 같은 값이다. lx/2 = 0.05(m)이므로 -0.05 (m)와 0.05 (m)가 가속과 감 속을 위해 선택되었고 약 0.48 (sec) 때 전환됨을 확인할 수 있다. 붉은색 선은 로봇의 전신 동작이 반영된 CoP 기대 값으로 Γ1Γ2 모두가 적용 되었을 때 예측되는 CoP로서 푸른색 선과 다른 것은 CoM의 수직 방향 제어 와 상체 몸통 방향 제어로 인한 영향이다. 노란색 선은 F/T 센서로 측정한 값으로 붉은색 선과 전체적으로 유사 하나 CoP가 급변하는 구간 직후 진동하는 현상이 관찰 되는데 이는 CoP의 급변으로 관절 제어 토크 역시 순간 적으로 많이 변하게 되고 이 과정에서 로봇 하드웨어에 진동이 발생 한 것으로 판단된다. 측정한 CoP가 진동할 때 CoM 역시 진동하는 경향이 Fig. 5에서 확인이 가능한 데 이는 CoP가 급변할 때 발생하는 진동에 의한 영향이 선형역진자 모형에 의한 예측 값과 차이가 나는 원인이 되어 CoM 제어 위치 오차의 원인 중 하나로 작용하는 것으로 판단된다.


Fig. 5 

Control result of CoM for x-direction




Fig. 6 

Desired CoP and result during CoM control for x-direction



같은 방법으로 y방향 제어를 한 결과는 Fig. 7과 Fig. 8에서 확인할 수 있다. 이동하도록 명령한 거리는 0.1 (m)였으며 실험 결과 이동에 소요된 시간은 약 0.8 (sec), 최종 위치 오차는 약 0.0066 (m)였다. x방향 제어와 비교 했을 때 CoM의 거동이나 측정한 CoP가 유사한 경향의 결과를 보였으며 좀 더 먼 거리를 이동하지만 CoP 가용 범위가 넓으므로 더 큰 가속 및 감속이 가능하여 이동 시간은 약 0.09 (sec) 차이만 난다.


Fig. 7 

Control result of CoM for y-direction




Fig. 8 

Desired CoP and result during CoM control for y-direction



5.2. 제자리 걸음 실험

본 연구에서 제안한 방법으로 목표 위치를 연속적으 로 바꿔서 CoM이 좌우 왕복 운동을 하도록 하여 이족 로봇의 제자리 보행 동작 생성에도 응용할 수 있는데 이를 실험으로 확인했다. 지지면은 5.1장의 실험과 동일 하다. 제자리 보행 실험을 위하여 -0.05 (m)와 0.05(m) 지점을 목표로 반복하여 이동하도록 하였으며, CoM이 한쪽 목적지에 도달하여 정지하면 목적지를 곧바로 바 꾸는 방식으로 두 지점을 왕복하도록 하였다. 제자리 보 행 동작 중 발은 수평 방향으로의 동작 없이 수직 방향으 로 약 0.01 (m) 들었다가 내렸다. Fig. 9에서 정지 상태에 서 시작해서 한차례 좌우 왕복하는 실제 실험 모습을 확인 할 수 있다. Fig. 9에서 보여지는 것은 1.4 (sec) 동안의 모습이지만 이어서 같은 동작이 반복되는 실험 을 하였고 그 결과는 Fig. 10과 Fig. 11에서 확인 가능하 다. 약 5.1 (sec) 동안 4번의 왕복 운동을 했을 때 CoM의 이동 결과가 Fig. 10에서 보여진다. 한번 왕복하는데 약 1.28 (sec)가 소요 되었다. 최종 위치 오차는 첫 번째 목적 지 -0.05 (m)에서 가장 큰 차이가 약 0.0091 (m)였고, 이후 반복되어 바뀌는 7번의 목적지에 대한 위치 오차를 포함한 평균 오차는 약 0.0037 (m)가 발생했다. 실험 중 CoP에 대한 결과는 Fig. 11에서 확인 가능한데, 5.1장에 서의 실험 결과와 유사한 경향을 보인다. 실험 결과들을 통해서 지면 접촉 발이 일정한 상황뿐만 아니라 접촉 면이 변하는 상황에서도 제안한 방법을 적용하여 연속 적인 목적지 변화가 가능함을 보이고 검증하였다.


Fig. 9 

Snapshot of the robot during walking experiment




Fig. 10 

Control result of CoM for y-direction for walking




Fig. 11 

Desired CoP and result during CoM control for y-direction for walking




6. 결 론

본 연구에서는 CoM의 수평 방향 고속 이동을 위한 힘 제어 방법을 제안하였다. 고속 이동을 위하여 항상 최대 또는 최소의 CoP를 사용하여 균형을 잃지 않으면 서도 큰 크기의 가속도를 얻었다. 이를 위하여 선형역진 자 모형을 CoM 거동 예측 모형으로 사용하여 요구 CoP 값을 최대로 할 것인지 최소로 할 것인지 결정 하였다. 다물체동역학을 바탕으로 한 로봇의 전신 동역학을 고 려하여 원하는 CoP를 생성하는 CoM 힘 제어를 하였기 접촉력에 대한 피드백 제어가 없었음에도 비교적 정확 하게 원하는 CoP 생성을 할 수 있었다. 그리고 제안한 방법은 실제 이족 로봇으로 실험하여 검증하였으며 단 순 CoM의 고속 이동 외에도 제자리 걸음을 위한 CoM 위치 제어에도 활용이 가능함을 보였다. 선형역진자 모 형의 사용으로 인한 한계는 추후 더 정밀한 예측 모형 개발을 통해 개선해야 할 부분으로 생각된다.


Acknowledgments

This work was supported by the National Research Foundation of Korea (NRF) grant funded by the Korea government (MSIP) (No. NRF-2015R1A2A1A10055798) and the Technology Innovation Program (10060081) funded by the Ministry of Trade, industry & Energy (MI, Korea).


References
1. Ott, C, Dietrich, A, Leidner, D, Werner, A, Englsberger, J, Henze, B, Eiberger, O, “From torque-controlled to intrinsically compliant humanoid robots”, Mechanical Engineering, (2015), 137(6), p7-11.
2. Kaneko, K, Morisawa, M, Kajita, S, Nakaoka, S I, Sakaguchi, T, Cisneros, R, Kanehiro, F, “Humanoid robot HRP2-Kai improvement of HRP-2 towards disaster response tasks”, (2015), IEEE-RAS International Conference on Humanoid Robots, Seoul, Korea, p132-139.
3. Stentz, A, Herman, H, Kelly, A, Meyhofer, E, Haynes, G C, Stager, D , , “CHIMP, the CMU highly intelligent mobile platform”, Journal of Field Robotics, (2015), 32(2), p209-228.
4. Radford, N A, Strawser, P, Hambuchen, K, Mehling, J S, Verdeyen, W K, Donnan, A S , , “Valkyrie: NASA’s first bipedal humanoid robot”, Journal of Field Robotics, (2015), 32(3), p397-419.
5. Vukobratovi´c, M, Borovac, B, “Zero-moment point -thirty five years of its life”, International Journal of Humanoid Robotics, (2004), 1(1), p157-173.
6. Kajita, S, Kanehiro, F, Kaneko, K, Fujiwara, K, Harada, K, Yokoi, K, Hirukawa, H, “Resolved momentum control: Humanoid motion planning based on the linear and angular momentum”, (2003), IEEE/RSJ International Conference on Intelligent Robots and Systems, Las Vegas, USA, p1644-1650.
7. Lee, S-H, Goswami, A, “A momentum-based balance controller for humanoid robots on non-level and nonstationary ground”, Autonomous Robots, (2012), 33(4), p399-414.
8. Kajita, S, Morisawa, M, Miura, K, Nakaoka, S, Harada, K, Kaneko, K, Kanehiro, F, Yokoi, K, “Biped walking stabilization based on linear inverted pendulum tracking”, (2010), IEEE/RSJ International Conference on Intelligent Robots and Systems, Taipei, Taiwan, p4489-4496.
9. Lee, Y, Hwang, S, Park, J, “Balancing of humanoid robot using contact force/moment control by task-oriented whole body control framework”, Autonomous Robots, (2016), 40(3), p457-472.
10. Lee, Y, Park, J, “Velocity tracking algorithm in forward walking using estimated ZMP by whole-body control framework”, (2013), IEEE-RAS International Conference on Humanoid Robots, Atlanta, USA, p94-99.
11. Kajita, S, Yokoi, K, Saigo, M, Tanie, K, “Balancing a humanoid robot using backdrive concerned torque control and direct angular momentum feedback”, (2001), IEEE International Conference on Robotics and Automation, Seoul, Korea, p3376-3382.
12. Hof, A L, Gazendam, M G J, Sinke, W E, “The condition for dynamic stability”, Journal of Biomechanics, (2005), 38(1), p1-8.
13. Kajita, S, Kanehiro, F, Kaneko, K, Yokoi, K, Hirukawa, H, “The 3D linear inverted pendulum mode : A simple modeling for a biped walking pattern generation”, (2001), IEEE/RSJ International Conference on Intelligent Robots and Systems, Maui, Hawaii, p239-346.
14. Kajita, S, Kanehiro, F, Kaneko, K, Fujiwara, K, Harada, K, Yokoi, K, Hirukawa, H, “Biped walking pattern generation by using preview control of zero-moment point”, (2003), IEEE International Conference on Robotics and Automation, Taipei, Taiwan, p1620-1626.
15. Park, J, Khatib, O, “Contact consistent control framework for humanoid robots”, (2006), IEEE International Conference on Robotics and Automation, Orlando, USA, p15-19.
16. Schwartz, M, Hwang, S, Lee, Y, Won, J, Kim, S, Park, J, “Aesthetic design and development of humanoid legged robot”, (2014), IEEE-RAS International Conference on Humanoid Robots, Madrid, Spain, p13-19.

이 이 수

2008 서울대학교 조선해양공학과 (공학사)

2010 서울대학교 조선해양공학과 (공학석사)

2010 ~ 현재 서울대학교 융합과학기술대학원 박사과정

관심분야: 인간형 로봇 제어

박 재 흥

1995 서울대학교 항공우주공학과(공학사)

1999 서울대학교 항공우주공학과(공학석사)

2006 Stanford University., Department of Aeronautics & Astronautics(공학박사)

2009 ~ 현재 서울대학교 융합과학기술대학원 교수

관심분야: 로봇 전신제어 프레임워크