Removal of Debris Blocking an Entryway: Inverse Kinematic Control and Balancing Controller Design for Humanoid

Journal of Institute of Control, Robotics and Systems (2014) 20(10):1063-1066 http://dx.doi.org/10.5302/J.ICROS.2014.14.8017 ISSN:1976-5622 eISSN:2233-4335

휴머노이드 로봇의 입구 통로를 막고 있는

잔해 제거를 위한 역 기구학 제어와 자세 제어기 설계 Removal of Debris Blocking an Entryway: Inverse Kinematic

Control and Balancing Controller Design for Humanoid

이 인 호^*, 김 인 혁, 오 준 호 (In-Ho Lee^1,*, Inhyeok Kim², and Jun-Ho Oh¹)

1Mechnical Engineering, Korea Advanced Institute of Science and Technology

2Rainbow Dynamics Inc.

Abstract: The humanoid robot, DRC-HUBO is developed from the KHR (KAIST Humanoid Robot) series to meet the requirements of the DARPA Robotics Challenge. DARPA Robotics Challenge was a competition to develop semi-autonomous humanoid robot so that dispatched in dangerous environments in place of humans like the Fukushima nuclear accident. In this paper, we introduce DRC- HUBO briefly and a methodology to remove debris blocking an entryway. The methodology includes inverse kinematics for DRC- HUBO and stabilization controller based on ZMP. Proposed inverse kinematics is robust, and pelvis-related tasks improve the manipulability and workspace of the arms. The controller improves the damping characteristic of the system and mitigates the instability during removal of debris. For given position and orientation of the debris, DRC-HUBO generates motion to reach the debris and lift up while stabilizing itself. Many experimental results verify our proposed methodology.

Keywords: DRC-HUBO, inverse kinematic control of humanoids, DARPA robotics challenge, removal of debris

I. 서론

연구자들은 인간의 편의를 위해 로봇을 개발해 왔다. 많은 형태의 로봇들이 산업, 의료, 교육 등 실제 여러 현장에서 사 용되고 있다. 하지만 일본의 후쿠시마 원전 사고에 투입된 많은 로봇들이 제 임무를 완수 하지 못하였다. 통신 장애, 무 너진 지형, 시각 장애 등이 예상되는 재난상황 때문 이었다.

또한 그러한 어려움을 거친 이후 밸브를 잠그고 사다리를 옮 기는 등의 복잡한 작업을 해야 하므로 투입된 로봇은 인간과 같은 높은 자유도를 가져야 한다. 휴머노이드 로봇은 인간과 비슷한 외형을 가지고 있다. 이러한 관점에서 휴머노이드의 궁극적 목표는 재난상황에서 인간 대신에 임무를 수행하는 것이다.

DRC-HUBO는 KAIST 에서 개발한 KHR (KAIST Humanoid Robot) 버전의 최신 버전으로서 HUBO 로 알려진 KHR-3 버 전보다 긴 팔, 다리를 가지고 있다. 또한 13 kg의 무게도 들 수 있는 강한 손을 가지고 있다.

재난 상황에서는 작업을 하고 있는 휴머노이드 로봇에게 불안정한 상황을 연출 시킨다. 작업 도중 외부 환경에 충돌 이 있을 수 있으며, 기울어진 지형으로 인해 역 진자로 모델

링 되는 휴머노이드가 넘어질 수 있다. 따라서 휴머노이드는 작업을 위한 팔 궤적 추종과 안정화를 위한 무게 중심 추종 을 동시에 만족하는 역 기구학 해법이 필요하다. 본 논문에 서는 DRC-HUBO 를 위한 역 기구학 해법을 제시하고, 안정 화를 위한 ZMP 제어기를 제안한다.

II. DRC-HUBO 를 위한 역 기구학 제어

휴머노이드와 같이 복잡한 외형을 가진 로봇은 역 기구학 이 필요하다. 또한 휴머노이드는 팔 궤적 추종과 무게중심 추종을 동시에 만족해야 하므로 task-priority strategy 를 사용 하였다[1]. Task 들의 충돌은 높은 priority 를 가진 task 들의 null space 에서 낮은 priority 의 task 수행시 일어난다. 일반적 인 framework는[2]에서 제안 되었으며, 우리는 이를 DRC- HUBO 에 맞게 수정하여 구성하였다[3]. 다음 수식 관계는 task-priority strategy 의 리뷰이다.

우리는 i 번째 priority의 task에 대한 기구학 식을 세운다면

i i( )

x = f q (1)

과 같다. x R_i∈ ^mi는 i 번째 task vector 이며 q R∈ ⁿ는 joint position vector 이다. 이를 두 번 미분하면 다음과 같다.

Task-consistent

Jacobian Inverse ¹

1



Forward Kinematics

ி௢௢௧ௗ

ு௔௡ௗௗ

஼ைௌ

௉௘௟௩௜௦ௗ

ு௜௣ௗ

ௐ௔௜௦௧ௗ

ா௟௕௢௪ௗ

௏  ௉

ݍ ݍሶ

 ݍሷ

 Reachability

Limiter Joint Limiter

그림 1. DRC-HUBO 역기구학 2차 폐쇄 루프 제어.

Fig. 1. Second order closed-loop inverse kinematics of DRC- HUBO.

Copyright© ICROS 2014

* Corresponding Author

Manuscript received July 7, 2014 / revised September 10, 2014 / accepted September 15, 2014

이인호, 오준호: 한국과학기술원 기계공학과 ([email protected]/[email protected]) 김인혁: ㈜레인보우([email protected])

※ 본 연구는 KAIST Hubo Lab에 의하여 연구되었음.

※ This research was supported by the Engineering Fee Project funded by the Ministry of Trade, Industry and Energy. [10045693, Development of humanoid robot technology for disaster tasks].

※ 이 논문은 2014 제 29회 ICROS 학술대회에 초안이 발표 되었음.

이 인 호, 김 인 혁, 오 준 호 1064

표 1. DRC-HUBO 의 Task 우선순위에 관한 표.

Table 1. Task priorities and task dimensions.

Priority Task Dimension

1(Highest) Foot position and orientation 12 2 Hand position and orientation 12 3 Horizontal position of center-of-mass 2 4

Height of pelvis Attitude of pelvis Waist joint angle

1 3 1 5(Lowest) Angle between the elbow and the torso 2

Total 33

i i

h J q=  (2)

( )/ ^{m ni}

i i

J = ∂f q ∂ ∈q R ^× 는 Jacobian 행렬이다. hi는 다음과 같 이 정의된다.

i i i

h x J q= −  (3)

i 번째 task 의 null space projection operator Ni는 다음과 같다.

i i i

N = −I J J⁺ (4)

‘+’ 는 Moore-Penrose 가 제안한 행렬의 pseudo-inverse 이다.

만약 i 번째 task 와 i-1 번째 의 task 의 관계식을 가속도 계열에서 나열하면,

1 ( 1),

i i i i i i

q q = − +J h J q⁺ − − (q₁=J h_{1 1}⁺ ) (5) 과 같다.

A1

i i i

J =J N₋ (6)

1 ,

A A

i i i i

N =N− −J J⁺ (N₀^A=I) (7) 수식 (5)에 의하듯, qi는 i-1 번째 우선순위의 가속도 q i−1

와 식 (6)의 i 번째 task 의 null space projection operator N 로 i

구해진 task-consistent Jacobian J 의 역 행렬로 결정된다. i

Table 1은 DRC-HUBO 의 task priority 를 나타낸 표로서 가 장 높은 우선순위는 발의 카르테시안 스페이스의 위치 궤적 과 방향 궤적이며, 팔꿈치와 몸통의 여유 각도가 가장 하위 순위의 task priority 를 가진다.

III. 도립 진자 모델링과 ZMP 제어기

휴머노이드 자세제어의 어려움 중 하나는 그가 도립 진자 이기 때문이다. 도립 진자 모델은 non-minimum phase 시스템 이며, 불안정 zero 를 갖기 때문이다. 많은 연구자들이 휴머 노이드 로봇의 자세 제어를 위해 도립 진자 모델링을 하였다.

도립 진자 상태 중 하나는 ZMP (Zero Moment Position) [4]이다.

ZMP 는 지면 지지 다각형(발)을 가진 휴머노이드의 자세를 증명하는 상태 인자 이다[5]. 우리는 작업을 수행하는 휴머노 이드를 보행 상태 중의 double-support phase 로 가정하였다.

일반적으로 포지션 제어기반의 휴머노이드 [6]는 낮은 감 쇠 특성을 가짐과 동시에 non-minimum phase 라는 역 진자 모델의 특이점 때문에 ZMP 추종 제어기의 게인을 높게 결 정 할 수 없는 문제점이 있다. 본 논문에서 제안하는 자세 제어기는 이러한 낮은 감쇠 특성을 보완하는 “Damped

system”과 ZMP 추종을 위한 “Compensation” 으로 구성된다.

“Damped system” 은 시스템 특성을 주파수 기반으로 밝히고 극 배치 기법을 통해 보다 높은 감쇠 성능을 가지도록 한다.

“Compensation” 은 일반적인 PI (Proportional position and Integral action) 보상 기법이다. 본 제어기의 입력 값은 ZMP 추종 위 치 이며 제어 결과는 위 역 기구학의 무게중심 위치 (position of center-of-mass)로 쓰인다.

Fig 2 모델에 의하여 질량 중심에 대한 뉴토니안 식은,

ml2θ τ= −bθ−mglθ (8) 이다. ,m ,l θ 는 각각 휴머노이드의 질량 역 진자의 길이,

실제 기울어진 각도를 의미한다. 시스템에 가해지는 외부 토 크 τ 는 실제 기울어진 각도 θ 와 position reference uθ에 의 하여 τ= −K(θ−uθ) 으로 결정된다. K 와 b 는 각각 시스 템의 강성과 감쇠계수이다. 우리는 상태 벡터를

[ , ]θ θ ^T

Θ =  (9)

와 같이 정의 하였다. 이로 인해 system equation 은 다음과 같이 결정된다.

2 2 2

0 1 0

d 1

K b K u

dt g ml ml ml

θ

⎡ ⎤ ⎡ ⎤

⎢ ⎥ ⎢ ⎥

Θ =⎢ − ⎥Θ +⎢ ⎥

⎢ ⎥

⎢ ⎥ ⎣ ⎦

⎣ ⎦

(10)

K b K

y ZMP u

mg mg mg ^θ

⎡ ⎤ ⎡ ⎤

= =⎢ ⎥Θ + −⎢ ⎥

⎣ ⎦ ⎣ ⎦ (11)

휴머노이드 로봇 DRC-HUBO 의 발에서 측정한 ZMP 를

X

Z Y

g

τ O^robot

X

Z Y

offset g θ

K l

τ

b

robot

O m u COM= ref=u lθ

u_θ

m l

(a) (b) 그림 2. (a) 시상면 (b) 측면 에서 본 DRC-HUBO의 역 진자

모델.

Fig. 2. Inverted pendulum model of DRC-HUBO in (a) lateral and (b) sagittal plane.

C D Robot

G(s)

-K Estimator





y ZMP= x

ˆx

ZMPref u COM= ref

1 s KP

KI

e



 







Compensation Damped System

그림 3. 장애물을 치우는 휴머노이드의 자세제어기 블록 선도.

Fig. 3. Block diagram of balancing controller for humanoid.

In-Ho Lee, Inhyeok Kim, and Jun-Ho Oh

휴머노이드 로봇의 입구 통로를 막고 있는 잔해 제거를 위한 역 기구학 제어와 자세 제어기 설계 1065

기준 ZMP 위치에 수렴시키기 위해서 ZMP 값을 출력 값으 로 결정하고 다음 전달함수를 정의 하였다.

2

2 2

2

2 2

( )

2 n n

K s g

ZMP mg l a

b g K

u s s s s

ml l ml

θ ζω ω

− +

= =

+ +

+ + −

(12)

여기에서 a =38.3256, ζ =0.07649 and ω =n 5.342. 이 결 과는 주파수 기반의 system identification 을 통해 구해진 값이 며 Fig 4는 극 배치 방법을 이용해 고유의 감쇠 성능을 보완 한 모습이다.

IV. 실험 결과

우리는 실험을 통해 본 논문에서 제안한 제어기를 검증하 였다. Fig 5, 6에서 보이듯, 12초에 휴머노이드의 팔은 잔해에 접촉한다. 접촉으로 인해 팔의 힘 센서 값과 ZMP 값이 변화 한다. 제어기는 변화된 ZMP를 PI 제어기로 유지하려 한다.

팔 센서값 변화를 보듯, 22초부터 휴머노이드는 잔해를 들어 올린다. 32초 까지 휴머노이드는 잔해를 이동시키고, 37 초에 잔해를 놓는다. 놓음으로서 ZMP 는 큰 진동을 가지지만 제 어기의 보완된 감쇠 특성으로 인해 이내 곧 안정하다.

Fig 7은 2013 DRC trials 에서 DRC-HUBO 는 장애물 치우기 task 의 중계 방송에 대한 스냅샷 이다. 본 연구에서 제안된 역 기구학 제어와 ZMP 자세 제어기를 이용하여 DRC-HUBO 는 장애물을 안정히 치우며 전진하였다.

V. 결론

본 연구는 휴머노이드의 진행 경로와 입구 통로를 막고 있 는 장애물을 치우기 위한 관절 제한조건 하의 역기구학 제어 와 자세 제어기를 제안하였다. 실제 인간크기의 로봇 DRC- HUBO 에 적합한 역기구학 제어기를 설계하였고 주파수 기 반의 system identification 을 통해 자세 제어기를 설계하였다.

DRC-HUBO 는 진행 경로의 장애물을 외부 충격에 관계없이 안정되게 치우는 모습을 실험으로서 보인다.

12 sec

27 sec 17 sec

32 sec 22 sec

37 sec Reach debris

Lifting

Release Remove

그림 5. 장애물을 치우는 휴머노이드의 스냅샷.

Fig. 5. Snap shot of humanoid removal debris motion.

5 10 15 20 25 30 35 40 45

-30 -20 -10 0 10 20 30

time(sec) Fz

(N)

5 10 15 20 25 30 35 40 45

-0.02 0 0.02 0.04 0.06

time(sec) X(m

)

ZMP input

(a)

(b)

Release Lifting

Reach debris

Unknown External forces

그림 6. 장애물을 치우는 휴머노이드의 측정 ZMP 결과와 제 어 입력 값.

Fig. 6. Experiment result of measured ZMP and controller input.

그림 7. 2013 DRC trials 에서 DRC-HUBO 의 장애물 치우는 모습.

Fig. 7. Removal motion of DRC-HUBO in DRC trials 2013.

-10 -8 -6 -4 -2 0

-15 -10 -5 0 5 10 15

0.19 0.27 0.36 0.5

0.66 0.88

2 4 6 8 10 12 14

2 4 6 8 10 12 14 0.06 0.12 0.19 0.27 0.36 0.5

0.66

0.88

0.06 0.12 Root Locus

Real Axis (seconds^-1) Imag

inar y A xis (sec onds -1)

Original system pole -0.40855.3244

Damped system pole -6  3.2

그림 4. 휴머노이드의 고유의 pole 값과 극 배치 방법으로 나 아진 감쇠 성능의 pole 값.

Fig. 4. Pole of original system and pole of improved damping characteristic by pole placement.

Removal of Debris Blocking an Entryway: Inverse Kinematic Control and Balancing Controller Design for Humanoid

이 인 호, 김 인 혁, 오 준 호 1066

REFERENCES

[1] B. Siciliano, “A closed-loop inverse kinematic scheme for on- line joint-based robot control,” Robotica, vol. 8, no. 3, pp. 231- 243, 1990.

[2] B. Siciliano and J.-J. E. Slotine, “A general framework for managing multiple tasks in highly redundant robotic systems,”

Advanced Robotics, 1991. 'Robots in Unstructured Environments', 91 ICAR., Fifth International Conference on.

IEEE, 1991.

[3] I. Kim and J.-H. Oh, “Inverse Kinematic control of humanoids under joint constraints,” International Journal of Advanced Robotic Systems, 2013.

[4] M. Vukobratović and B. Borovac, “Zero-moment point—thirty five years of its life,” International Journal of Humanoid Robotics, vol. 1, no. 01, pp. 157-173, 2004.

[5] J. Y. Kim, I. W. Park, J. Lee, M. S. Kim, B. K. Cho, and J. H.

Oh, “System design and dynamic walking of humanoid robot KHR-2,” Robotics and Automation, 2005. ICRA 2005. Proc. of the 2005 IEEE International Conference on. IEEE, 2005.

[6] I.-W Park, et al., “Mechanical design of humanoid robot platform KHR-3 (KAIST humanoid robot 3: HUBO),”

Humanoid Robots, 2005 5th IEEE-RAS International Conference on. IEEE, 2005.

[7] M. Gienger, H. Janssen, and C. Goerick, “Task-oriented whole body motion for humanoid robots,” Humanoid Robots, 2005 5th IEEE-RAS International Conference on, IEEE, 2005.

[8] I. H. Lee, I. H. Kim, H. I. Bae, H. B. Jeong, G. G. Lee, and J. H.

Oh, “Removal of debris blocking an entryway: inverse kinematic control of humanoid robot, DRC-HUBO,” Proc. of 2014 29th ICROS Annual Conference (ICROS 2014) (in Korean), Daegu, Korea, pp. 67-68, 2014.

이 인 호

2009년 한국과학기술원 학부 졸업. 2011 년 한국과학기술원 석사 졸업. 2011년~

현재 한국과학기술원 박사과정.

김 인 혁

2004년 연세대학교 학부 졸업. 2006년 한국과학기술원 석사 졸업. 2013년 한국 과학기술원 박사 졸업. 2013년~현재 ㈜ 레인보우.

오 준 호

1977년 연세대학교 학부 졸업. 1979년 연세대학교 석사 졸업. 1985년 미국 버 클리대학 박사 졸업. 1985년~현재 한국 과학기술원 기계공학과 교수. 2005년 인 간크기의 휴머노이드 로봇 HUBO 개발.