[논문리뷰] A fast, iterative solver for the Inverse Kinematcis Problem

A fast, iterative solver for the Inverse Kinematcis Problem

​

저자 : Andreas Aristidou, Joan Lasenby

논문 : http://andreasaristidou.com/FABRIK.html

작성 : benthebear93

​

Background and Motivation

​

IK 문제에 대한 가장 많이 사용되는 접근법은 자코비안 행렬을 계산하여 선형 근사하는 방법이다. 자코비안 해는 로봇 끝단(End-effector)의 움직임을 링크의 선형 움직임과 조인트 각도에 대해 선형적인 모델을 뜻한다. 쉽게 말해 자코비안이 로봇이 움직일 때 링크와 EE의 관계를 선형적으로 표현한다는 것이다.

​

이런 자코비안을 계산하고 근사하는 데 다양한 방법론들이 있다.

1. Jacobian Transpose
2. Damped Least Squares(DLS)
3. Damped Least Squares with Singular Value Decomposition(SVD-DLS)
4. Selectively Damped Least Squares(SDLS)

하지만 위의 대부분의 방식들은 Computing power가 많이 필요하다. 자코비안 자체가 행렬이 매우 복잡해지고, 싱귤러에 대한 고려까지 들어가게 되면 더욱 더 행렬 연산이 복잡해진다. 이를 위해서 IK 문제를 Control prospective 접근으로 푸는 방법도 나왔다(SDLS). 이는 계산량은 줄어들고 싱귤러 문제도 없게 된다.

​

IK Solver의 또 다른 형태는 Newton 방식이다. 이 알고리즘은 최소화 문제의 형태로 접근한다. 따라서 불규칙적인 끊어짐 없는 모션이 생성된다. 이런 형태의 알고리즘으로 가장 유명한 것은 Powell 방식과 BFGS 방식이다. 하지만 Netwon 방식은 기본적으로 복잡하고, 구현하기가 어렵고 iteration 마다 계산량이 많다.

​

유명한 IK 방식으로 Cyclic Coordinate Descent (CCD) 알고리즘이 있다. 이는 휴리스틱 반복 방식으로 행렬을 막 바꾸고 하지 않아도 계산량이 적은 형태이다. 해는 매우 빠르게 찾을 수 있는데, 불규칙적인 끊어짐 현상으로 인해 현실적으로 사용하기 어려운 알고리즘이다. 또한 다중의 로봇 끝단을 가진 로봇에는 적용하기 어렵다 ( 휴머노이드 같은 느낌).

​

Sequential Monte Carlo Method와 파티클 필터 방식도 연구 됐다.

​

The articulated body model

multibody system은 링크와 조인트들의 조합으로 이뤄진 강체이다. 제약 조건이 잘 들어간다면 링크들의 자세들을 특정 집합(자세를 제한한다는 뜻)의 형태로 나타낼 수 있다. 이때 Multibody system은 root에서 시작하여 링크와 조인트로 이루어진 chain 형태이다.

​

FABRIK : a new heuristic IK solution

FABRIK 의 기본적인 원리는 제일 최근에 계산 된 조인트 위치를 사용하여 앞뒤 반복 모드에서 업데이트를 찾는 것이다. FABRIK은 조인트 각도를 하나씩 조절하면서 시스템 오차를 최소화 하는 방식이 들어가 있다.

​

FABRIK : 기본 알고리즘 설명

​

1. 제일 먼저 마지막 조인트에서 시작하여 root 안쪽으로 들어가면서 각 조인트들 조금씩 조절한다.

2. 다시 뒤로 돌아가면서 동일하게 조인트들을 조절한다.

​

가장먼저 root 기준으로 target 위치의 거리를 계산하게 된다. 이 거리가 각 링크들의 길이보다 작으면 Reachable하다고 판단하여 iteration 을 시작한다. 1회 iteration은 2개의 스테이지로 이뤄진다.

가장먼저 조인트 P4의 위치를 t로 옮긴 후 P4'으로 만들어 준다.

P4'와 P3를 만나는 직선 L3를 구하고, 그 위에 d3만큼 떨어진 P3'을 만든다.

P3'에서 P2를 지나는 직선 L2를 구하고 그 위에 d2만큼 떨어진 P2'을 만든다.

P2'에서 P1을 지나는 직선 L1을 구하고 그 위에 d1만큼 떨어진 P1'을 만든다.

​

이렇게 되면 root가 달라지기 때문에 다시 P1'을 P1자리로 옮겨주는 작업을 해야한다.

이를 위해 2번째 스테이지가 실행된다.

이번에는 root부터 시작하여 EE로 진행된다.

P1'을 root 혹은 b(base) 위치로 옮겨서 P1''으로 만든다.

동일하게 P1''와 P2'을 지나가는 직선 L1을 구하고 그 위에 P2''을 d1만큼 거리에 위치한다.

P2''와 P3'을 지나가는 직선 L2를 구하고 그 위에 P3''을 d2만큼 거리에 위치한다.

P3''와 P4'을 지나가는 직선 L3를 구하고 그 위에 P3''을 d3만큼 거리에 위치한다.

​

이런 한번의 iteration이 끝나면 거의 모든 경우에 EE의 위치가 target과 가까워진다. 이를 EE의 위치가 target과 동일하거나 충분히 가까울때까지 하면 된다. 제약 조건이 없는 FABRIK은 어떤 chain이던 goal이던 도달하게 된다.

​

여기서 몇가지 최적화가 Conformal Geometric Algebra를 통해 이뤄질 수 있다. 이를 적용하면 더욱 빠르게 수렴하는 모습을 확인할 수 있다. CGA는 구의 형태나, 라인, 평면, 원형 형태를 대수적으로 표현함으로서 이득이 있다. 정확한 방법론에 대해서는 잘 모르겠지만, 결국 링크들 간의 관계가 Geometric하게 표현될 수 있고 이를 통해 약간의 예측을 할 수 있으므로 시간 단축이 된다는 뜻. 다른 최적화 방법론에는 target이 닿을 수 없을 때 target을 향하는 line을 구성하는 것이 있다.

(해당 최적화 관련 참고 논문 Clifford Algebra to Geometric Calculus: A Unified Language for Mathematics and Physics

, Geometric Algebra for Physicists)

​

이 방법론은 결국 포인트, 직선, 거리만 사용하기 때문에 복잡한 행렬 연산이 필요없게 된다. 따라서 매우 수렴속도가 빠르고, 싱귤러 문제 또한 발생하지 않으며 불규칙적인 끊어짐이 없이 부드러운 모션을 생성할 수 있게 된다.

​

FABRIK / FABRIK-R : Joint limit 설정

기본적으로 FABRIK에서는 Ball and socket 조인트를 가정하고 방법론을 제시한다. 즉 3자유도(rotation 2 자유도, twist 형태의 orientation 1자유도)의 형태 조인트를 선언하는 데 이는 Robotics manipulator에 적용하기는 어려움이 있다. 이를 해결하고자 나온 논문이 FABRIK-R이다.

​

FABRIK-R에서의 Joint limit에 대해 이야기하기 전에, 기본 FABRIK에서의 Joint limit에 대해서 이야기 해보자. 기본적으로 FABRIK는 반복적으로 실행되기 때문에 Joint 제약은 각 step에서 적용 가능한 영역에 orientation이 머물게 하는 걸로 만족시킬 수 있다. 이렇게 강제하는 게 실제로 target에 도달하는 FABRIK의 근본적인 방식에는 영향을 미치지 않는다.

​

이 방식에 기본 원리는 가능한 영역에서 Position과 orientation을 재조정하는 것이다. 결국 위에서 말한 위치를 조절하는 것과 동일하게 Joint limit을 만족하는 orientation에만 들어오게 조절하면 된다. 특히 다른 방법들과 다른 점은 FABRIK는 3D 문제를 2D 문제로 변환하여 진행한다는 것이다. 즉, 복잡성과 계산량이 줄어든다는 사실.

​

논문에서는 Ball-and-Socket 조인트를 먼저 예시로 든다.

먼저 Orientation 제한을 rotor(회전자) R로 표현하고, rotational 제한을 theta1~4로 표현하자.

Joint limit의 영역은 이 theta에 따라서 불규칙한 꼬깔콘 모양이 된다. limit을 만족하기 위해서는 joint limit boundary와 평면이 만드는 conic section에서 target과 가장 가까운 점으로 조정 된다. 이때 conic section의 형태는 위 사진에서 보는 것처럼 Circle, Ellipsoidal, Parabolic 형태가 있다. 결국 orientation을 표현하는 rotor R에 의해 joint 위치를 확인했을 때 boundary 안에 있으면 되고 그렇지 않으면 재조정한다는 이야기.

Orientation을 조정하는 방법에 대해서,

먼저 조정 스테이지 1번에 있을 때, P4'이 새롭게 만들어졌고, P3'을 만들려고 한다고 하자. 이때 P4'과 P3로 만들어지는 rotor를 만들었을 때 rotation이 limit보다 클 때는 P3의 orientation을 재 조정한다. 이를 반복하여 limit 안쪽으로 들어오게 되면 끝낸다.

Orientation이 조정 됐으니, rotation을 조정한다.

theta1~4로 표현된 rotational limit을 사용한다. 직선 L1위로 target을 projection 했을 때 나오는 O를 찾는다.

Joint와 O 사이의 거리 S를 계산하고, 각 theta에 대한 q1~q4 값을 S*tan(theta1~4)로 계산한다.

​

이 평면의 타원식을 theta값과 q 값을 통해 계산하고, target과 가장 가까운 타원의 위치를 찾는다.

가장 가까운 타원 위의 위치는 타원 식과 타원위의 점에서 직교하는 tangent line의 식을 동시에 Newton-Raphson 방식으로 불면 된다.

(참고논문 : Least-squares orthogonal distances fitting of circle, sphere, ellipse, hyperbola, and parabola)

​

참고할 점은 모든 타원 영역을 풀 필요는 없다는 거다. 결국 Target과 가까운 부분의 타원식만 풀면 된다.

​

Limiting Hinge Joint

FABRIK은 사실 다양한 조인트들에 대한 설정이 어려운데, 그나마 Hinge 형태의 Joint에 대한 적용은 가능하다. FABRIK이 기본적으로 Joint 좌표계에서 반복적으로 위치를 조절하면서 알고리즘이 이뤄지기 때문에, Joint 를 orientation의 평면에 projection 시키는 것으로 Joint limit을 만족하게 강제할 수 있다.

평면은 root와 target 위치로 정의 된다.

​

속도 비교

실제 자코비안 방식들에 비해서 완전 빠른걸 확인할 수 있다.

​

한계

​

실제로 논문에 "구현하면서 심각한 한계는 찾지 못했다" 라고 나와있다 ㅎㅎ

일단 조인트 타입에 대한 지원 정도가 있다. 다만 이에 대한 해결책도 어느정도 제시하고 있다.

​

 

한창 서울대 인턴할 때 연구실에서 읽고 구현해보라고 던져줬던 논문인데, 지금봐도 쉽지는 않다.

이번 논문은 사실 로보틱스 분야보다는 애니메이션 분야에 더 적합한 내용이다. 특히 Single Endeffector만 존재하는 로봇 팔과 달리 사람 형상이나 핸드 형상을 애니메이션 하는 경우에 사용되는 알고리즘이다. (언리얼 엔진의 IK Solver로 들어가 있다) 실제로 단순 로봇 팔에 적용하기에는 조금 복잡함이 있을 수 있어서 그런가 FABRIK-R이라는 논문도 있다. 다음 논문리뷰는 FABRIK-R에 대해 리뷰해보겠다. 그리고 구현도 함께