Kinematic Calibration

안녕하세요, MoonLight입니다.

​

제가 최근에 가지고 놀고 있는 장난감이 하나 있는데, 광고는 아니고 아래 Link에서 확인할 수 있는 6축 로봇팔입니다.

 

https://www.pybrain.ai/products/so-arm101

 

 

이것 저것 기술을 적용해 보면서 가지고 놀고 있는데, 로봇팔에는 손가락이 달려있는데, 손가락으로 특정 위치에 있는 물건을 집을 수 있도록 할려고 하고 있습니다.

​

막상 알아보니 6개의 모터를 움직여서 임의의 위치로 정확하게 가져가는 작업 자체가 절대 쉬운 과정이 아니더군요.

​

이번 Post에서는 물리적인 로봇팔의 실제 구조를 수학적으로 모델링하는 기법인 'Kinematic Calibration'에 대해서 알아보도록 하겠습니다.

​

 

 

 

1. Kinematic Calibration

 

1. Introduction

이상적인 로봇 모델은 이론적인 설계 매개변수를 기반으로 하지만, 실제 제조 및 조립 과정에서는 필연적으로 기하학적 오차(geometric discrepancies)가 발생합니다.

링크 길이의 미세한 차이, 조인트 축의 정렬 불량 등이 이에 해당하죠.

​

기구학적 교정(Kinematic Calibration )은 이러한 실제 로봇과 모델 간의 구조적 오차를 식별하고 보정하여, Inverse Kinematic(역기구학 모델)의 정확도를 높이는 것을 목표로 합니다.

​

​

※ Forward Kinematics(정기구학, FK)

• 각 관절의 현재 각도를 알 때, 로봇 끝단의 현재 위치와 방향을 계산하는 과정.
• 즉, 상위 관절부터 순서대로 각도를 조절해 끝점(End Effector)의 위치를 결정하는 방식.
• 구체적으로, 모터 각도(Step)를 넣으면 손 끝 위치(x, y, z)가 나오는 계산식이며, 항상 하나의 유일한 해를 가집니다.

​

​

※ Inverse Kinematics(역기구학, IK)

• 목표 지점(Target)을 먼저 정하면 관절들의 각도가 역으로 계산되는 방식.
• 예를 들어, "손을 사과 위치에 둔다."라고 하면 우리 뇌가 자동으로 손의 위치에 맞춰 팔꿈치와 어깨의 각도를 계산하는 것과 동일.
• 특정 위치에 손을 위치시키는 각도와 길이는 다양할 수 있으므로, Inverse Kinematics은 여러 개의 해가 존재할 수 있습니다.
• 목표 위치(x, y, z)를 넣으면 모터 각도(Step)가 계산되는 방식.

​

​

 

2. 방법

다음과 같은 순서로 Kinematic Calibration를 진행합니다.

​

1) 데이터 측정

레이저 트래커, 3D 스캐너, 또는 기타 정밀 측정 시스템과 같은 외부 측정 장비를 사용하여 로봇의 여러 구성(자세)에서의 실제 엔드 이펙터 위치 및 방향 데이터를 수집.

​

2) 모델링

수집된 실측 데이터와 로봇의 공칭(이론적) 매개변수로부터 계산된 위치 사이의 오차를 기반으로, 실제 기구학적 매개변수를 추정하는 오차 모델을 만듭니다.

실제 기구학적 모델은 Forward Kinematics을 사용합니다.

​

3) 최적화

오차 모델을 최소화하는 방식(이번 Project에서는 최소자승법(Least Squares))으로 로봇의 실제 기구학적 매개변수(예: Denavit-Hartenberg(DH) 매개변수, 이번 Project에서는 1~4번 Motor의 Step수와 팔 길이의 오차, 총 7개의 Parameter)를 결정하도록 합니다..

​

​

최적화 순서

① 예측 (Forward Path)

현재 가진 임의의 팔 길이(a2, a3…)와 오프셋(o2, o3…) 값을 기구학 식( cos ,sin 조합)에 넣어 예측 좌표(x′, y′, z′ )를 구합니다.

​

② 잔차(Residual) 계산

실제 측정된 좌표와 예측 좌표의 차이(r = Pmeas - Pcalc)를 구한다.

​

③ 야코비안(Jacobian) 계산

"팔 길이를 1mm 늘리면 좌표가 어디로 얼마나 변할까?"라는 민감도를 모든 파라미터에 대해 계산.

​

④ 업데이트 방향 결정

오차를 줄이기 위해 각 파라미터를 어느 방향으로, 얼마나 수정해야 할지 결정.

​

⑤ 위 과정을 Train 시작시 설정한 조건을 만족할 때까지 반복

​

이 방식은 "수학적인 우아함보다는 컴퓨터의 무식한 힘(Computing Power)을 빌리는 방식"에 가깝습니다.

공학에서는 이를 '수치해석적 방법(Numerical Method)'이라고 부르는데, 사실상 "계산기 성능이 좋으니까 우리가 고생하지 말고

컴퓨터 시키자"는 철학입니다.

​

4) 보정된 모델 적용

이렇게 최적화된 매개변수를 로봇 컨트롤러의 역기구학 모델에 적용하여, 로봇이 보다 정확한 움직임을 수행할 수 있도록 한다.

​

​

​

3. Multi Variable Input / Multi Variable Output ML과의 차이점

딥러닝이 수만 번의 작은 보폭(Learning Rate)으로 Global Minimum을 찾는다면, 이 방식은 함수의 모양(기하학적 구조)을 이미 알고 있기 때문에 훨씬 더 크고 정확한 보폭으로 최적의 파라미터에 빠르게 도달.

​

1) 일반적인 ML (딥러닝)

로봇이 어떻게 생겼는지 모르기 때문에 수만 개의 파라미터(Weight)를 이용해 "이런 입력엔 이런 출력이 나오더라"는 통계적

관계를 바닥부터 학습합니다. 마치 눈 가리고 코끼리를 만져서 형체를 알아내는 것과 비슷하죠.

"이유는 모르겠지만 이 Step일 땐 대충 이 좌표더라"라고 통계적으로 맞추는 것.

​

2) Kinematic Calibration

"이 로봇의 2번 팔은 정확히 112.5mm니까, 어디를 찔러도 이 공식대로 움직일 거야"라고 물리적 실체를 이미 파악하고 있죠.

이미 로봇이 cos,sin 법칙을 따르는 강체(Rigid Body)들의 조합이라는 것을 알고 있습니다.

즉, 전체적인 "함수의 모양"은 이미 알고 있고, 그 안에 들어갈 상수(팔 길이, 오프셋) 몇 개만 모르는 상태.

​

즉, "로봇이라는 특수 상황이라서 답을 구하기 더 쉬운 환경“이라는 의미입니다.

이를 전문 용어로 "Inductive Bias(귀납적 편향)가 매우 강력하다"고 표현합니다.

​

이런 이유로, ML은 로봇의 모든 각도 조합을 다 가르쳐야 하지만, IK 모델은 물리 법칙을 이미 알기 때문에 몇 개의 지점만 측정해도 나머지 영역의 움직임을 완벽하게 예측할 수 있습니다.

​

 

---

 

2. Find Parameters

앞에서 말씀드린 최적화 순서에 대해서 각 항목에서 어떤 계산을 해서 Parameter들을 찾는지 알아보도록 하겠습니다.

 

 

 

 

1. 예측 (Forward Path)

1) 최초의 FK Model 설정

최초의 FK Model은 학습으로 만드는 것이 아니라, 로봇의 설계도를 바탕으로 수학자가 미리 정의하거나 실제 측정한 값을

바탕으로 만듭니다.

이번 Project에서는 기하학적 구조 정의를 실제 측정한 값을 바탕으로 최초의 FK Model을 만들었습니다.

로봇이 1번 모터는 좌우(Yaw)로, 2, 3, 4번은 위아래(Pitch)로 움직인다는 물리적 구조를 보고 sin,cos 공식을 이용하여 미리

짜 놓은 것입니다.

​

​

2) 초기값(Initial Guess)

코드의 x_0 배열([d1 , a2 , a3 , a4 , o2 , o3 , o4])에 들어있는 값들(`d1=0.12, a2=0.11` 등)이 바로 "설계상 이 정도 길이다"라고

넣어준 최초의 예측치입니다.

​

​

3) x0 7개 값들의 의미

• d1 : "(Base Height): 로봇 바닥(지면)에서 첫 번째 관절(회전축)까지의 수직 높이."
• a2 , a3 , a4 : (Link Lengths): 각 관절 사이의 실제 팔 길이.
  • a2 : 2번 관절과 3번 관절 사이 거리
  • a3 : 3번 관절과 4번 관절 사이 거리
  • a4 : 4번 관절에서 최종 손끝(End Effector)까지의 거리
• o2 , o3 , o4 : (Angle Offsets) : 모터의 0점 오차. 소프트웨어적으로 "0도"라고 명령했을 때, 실제 조립 상태나 기어 맞물림 때문에 발생하는 미세한 각도 편차를 의미.

​

이 값들을 바탕으로 실제 로봇의 기하학적 구조를 함수로 나타낸 후에 최적화를 수행합니다.

​

​

4) 측정치(d1 , a2 , a3 , a4)는 물리적으로 정해진 상수인데, Train이 필요한 이유

측정 오차로 인해 이는 정확하게 로봇팔 구조를 표현하지 못하기 때문에 철저히 측정 Data로부터 Model의 물리적 구조를

학습합니다.

​

​

실제 로봇팔은 아래 그림과 같이 구성되어 있습니다.

​

​

 

 

2. 잔차(Residual, 오차) 계산

 

1) least_squares() 함수

Python의 SciPy Package에는 Least_squares() 함수가 있는데, 이 함수를 이용해서 최적화를 수행합니다.

Least_squares()는 오차를 계산하는 함수(residuals())와 초기값, 그리고 기타 train 관련 parameters를 받아서 오차를 최소화하는 IK Model Parameter(7개)를 찾는 역할을 합니다.

​

residuals()에서 FK Model의 예상치와 실제 측정치(CSV에서 읽은 x,y,z값)의 차이를 모읍니다.

​

제가 측정한 전체 측정 data shape이 189 x 7이고, 7개 중에 앞의 4개는 step, 뒤의 3개는 해당 step에서의 x,y,z값으로 구성되어 있습니다.

​

​

2) residual() 함수

3개의 Parameter를 받는다.

• • 로봇팔의 구조를 나타내는 7개의 FK Model Parameter([d1 , a2 , a3 , a4 , o2 , o3 , o4])
  • Train Data중에서 4개 Motor의 Step수([q1, q2, q3, q4])
  • Train Data중에서 4개 Motor의 Step수일 때 실제 위치( x, y, z )

​

3) FK Model에 7개의 FK Model Parameter와 4개 Motor의 Step수를 넘겨서, 현재 FK Model Parameter로 4개 Motor의 Step수 만큼 움직이면 위치가 어떻게 나오는지 계산하여 return합니다.

​

4) Train Data 1개의 Row당 x,y,z 각각 오차를 계산, 즉 1개의 측정 자세 당 3개의 오차값 생성됩니다.

​

5) 총 189개의 row가 있고, 1개의 row당 3개의 오차값이 생기므로, r(오차 Vector) = 567개

​

​

​

 

3. Jacobian(야코비안) 계산

 

1) 야코비안(Jacobian) 행렬(J)

Jacobian 행렬은 한마디로 "민감도 지도"라고 말할 수 있습니다.

"어떤 파라미터를 건드렸을 때, 결과값이 얼마나 민감하게 변하는가?"를 나타내는 행렬입니다.

단순히 a2 하나만 보는 게 아니라, 모든 파라미터([d1 , a2 , a3 , a4 , o2 , o3 , o4])가 x, y, z 좌표에 미치는 영향을 하나의 거대한 표(행렬)로 만듭니다.

이 행렬의 전체를 보면 "어떤 파라미터 조합을 건드려야 오차가 가장 빨리 사라지는가"에 대한 정보를 가지고 있는 거죠.

​

​

2) Parameter 상호간 영향 분산 방법 - "서로에게 미치는 영향"은 어떻게 해결하나?

"파라미터끼리의 종속성" 문제는 least_squares() 내부에서 JT J (Jacobian의 전치 행렬과 원래 행렬의 곱)라는 연산을 통해 해결합니다.

이 연산을 거치면 '파라미터 간의 상관관계'가 수치화됩니다.

예를 들어, "팔 길이 a2를 늘리는 것과 오프셋 o2를 조정하는 것이 서로 비슷한 효과를 낸다”라는 정보가 이 행렬 안에 포함되어 있습니다.

알고리즘은 이를 보고 "둘이 비슷하니까 한쪽만 너무 많이 바꾸지 말고, 적절히 나눠서 수정하자"라고 판단합니다.

즉, 서로 방해하지 않게 '교통정리'를 해줍니다.

​

​

3) Jacobian 행렬(J) 계산 방법

Jacobian 행렬(J) 은 least_squares() 함수에서 계산하며, residual()함수를 이용하여 Jacobian 행렬(J)를 만듭니다.

​

① 초기 필요값

기본 Param 7개 [d1 , a2 , a3 , a4 , o2 , o3 , o4]

Train Data 189개

ε = 0.00000001 ( 아주 작은 값 )

 

② 최초 오차 계산

기본 Param으로 189개의 오차 계산. 결과로 오차값 567개 ( Basic_error )

​

③ 기본 Param 7개중, d1에 ε을 더해서 새로운 오차 계산

[d1 +ϵ, a2 , a3 , a4 , o2 , o3 , o4]

이 계산의 결과로 오차값 567개 ( New_error )를 얻고, 이 오차값은 d1이 각 오차에 얼마나 영향을 주느냐의 정보가

담겨있습니다.

​

④ 변화량 계산

( New_error – Basic_error) / ε

이 값이 d1의 영향력을 나타내고, J의 첫번째 Column이 됩니다.

​

⑤ 위와 같은 과정을 7개 Param에 각각 수행하고, 차례대로 J의 Column으로 추가합니다.

최종적으로 567 x 7의 Jacobian 행렬을 얻을 수 있습니다.

​

그림으로 나태내면 아래와 같다고 볼 수 있겠죠.

​

​

​

​

4. 업데이트 방향 결정

7개의 Parameter([d1 , a2 , a3 , a4 , o2 , o3 , o4])를 얼마나 조절할(𝜟𝒙) 지 결정하는 과정입니다.

아래 수식으로 결정합니다.

​

Δx≈(JT J)-1 JT r

​

​

​

여기에 언급되는 값들의 의미와 Shape을 정리한 것이니 참고하시면 좋겠죠

​

​

​

​

​

Δx≈(JT J)-1 JT r

​

1) JT r

Parameter 7개, 각각의 영향력(J, Jacobian 행렬)을 실체 오차와 곱해서 모두 더한 값.

$J^Tr=\sum _{i=1}^{567}\left(J_i\times r_i\right)$JTr=567∑i=1(Ji​×ri​)​

이 결과의 의미는 "각 파라미터, 너의 영향력이 주는 오차의 합계가 이 정도이니 너를 이 방향으로 고치라는 압력(Force)이 이만큼 강하게 들어왔어.“라는 의미입니다.

​

​

​

Δx≈(JT J)-1 JT r

​

​

2) JT J

​

헤시안 근사 (Hessian)

각 칸의 값은 두 파라미터의 '영향력 벡터'를 내적(Dot Product)한 값이기 때문에 각 파라미터간의 상관관계를 얻을 수 있습니다.

Vector연산에서 내적(Dot Product)의 의미가 두 벡터가 얼마나 같은 방향을 가리키는가(유사도)입니다.

즉, JT J 행렬은 "모든 파라미터 쌍(Pair)끼리 서로 얼마나 비슷한 짓을 하고 있는지"를 수치화한 상관관계 지도(Correlation Map)가 되는 것이다.

​

연산의 모양은 행렬곱이지만, 결과 행렬의 각 값들의 성격은 두 Vector의 내적의 성격을 띄고 있다.

​

J의 Shape이 아래와 같을 때, JT J 연산은 다음과 같이 진행됩니다.

​

​

이제 행렬 곱을 수행합니다. 행렬 곱셈의 규칙은 "앞 행렬의 행(Row)과 뒤 행렬의 열(Column)을 순서대로 곱해서 더한다“ 입니다.

​

​

벡터의 내적(Dot Product)의 정의와 완벽하게 일치합니다.

​

​

​

즉, ＂행렬 곱을 했더니 그 결과물들이 전부 내적 값이더라＂라는 거죠.

​

​

​

3) JT J의 의미

이 연산의 결과는 (7x7) Shape의 상관관계 지도(Correlation Map)이고, 각 Parameter가 다른 Parameter와 얼마나 서로 연관되어 있는지 나타냅니다.

​

​

​

4) Δx 구하기

​

위에 나온 수식을 조금 정리하면,

​

• Δx≈(JT J)-1 JT r

• (JT J)Δx ≈ JT r

​

위 수식에서 우리가 알고싶은 것은 얼마나 Parameter를 움직여야 하는지(Δx)입니다.

위 수식의 Parameter간의 상관관계를 고려(JT J )하여서 얼마만큼 움직(Δx )여야 각 파라미터가 영향을 주는 오차(JT r )만큼 움직일까를 찾는다는 의미죠.

JT J의 역행렬을 양변에 곱해서 Δx 만 좌변에 남깁니다.

​

​

​

 

5. 반복

Δx 를 구한 후 이 값을 이용해서 새로운 x0를 만들고, residual() 함수를 다시 호출하기를 반복하여 오차가 설정한 범위내에 들어오거나 횟수 제한만큼 반복합니다.