M06.03 · 기하학적 IK·Jacobian·감쇠 최소제곱 IK

목표에서 관절값을 되찾는다

역기구학(IK, Inverse Kinematics)은 목표 말단 위치나 자세를 만족하는 관절값을 찾는 문제다. 해가 없거나 여러 개일 수 있다. 여기서는 M06.02와 같은 평면 2R, L1=0.3 m와 L2=0.2 m를 사용하며 목표는 베이스 좌표의 위치 (x_d,y_d)뿐이다. 두 회전각 q1,q2는 rad이며 관절 범위·충돌은 먼저 생략한다. 도구 방향 φ까지 임의로 추가하면 다른 제약 문제가 된다.

그림 원본 크기로 보기

그림 M06.03-1. 본문의 목표 (0.3,0.2) m를 만족하는 두 기하 분기를 겹쳐 보였다. 끝점이 같아도 팔꿈치·경로는 다르다. 관절 제한·충돌·도구 방향은 이 기하 해에 포함되지 않는다. 직접 제작.

베이스·팔꿈치·목표점의 삼각형에서 길이 제곱을 전개하면 다음 코사인 관계가 나온다. c2는 cos q2의 약칭이다.

$$c_2=\frac{x_d^2+y_d^2-L_1^2-L_2^2}{2L_1L_2}.$$

|c2|≤1이어야 실수 각도가 가능하다. 범위 안이면 sin q2에 양/음 두 분기를 둘 수 있다. atan2(y,x)는 부호와 사분면을 반영한 각도를 돌려주는 함수로, 단순 atan(y/x)처럼 x=0에서 바로 나누지 않는다.

$$s_2=\pm\sqrt{1-c_2^2},\qquad q_2=\operatorname{atan2}(s_2,c_2),$$ $$q_1=\operatorname{atan2}(y_d,x_d)-\operatorname{atan2}(L_2s_2,L_1+L_2c_2).$$

목표 (0.3,0.2) m에서는 분자가 0이라 c2=0이다. 양의 분기는 q2=90도, q1=atan2(0.2,0.3)−atan2(0.2,0.3)=0이다. 음의 분기는 q2=−90도, q1≈67.380도다. 두 해를 FK에 넣으면 같은 위치를 얻지만 도구 방향과 팔꿈치 위치는 다르다. 범위와 충돌까지 검사해야 실제 후보가 된다. 목표 (0.6,0) m에서는 c2=(0.36−0.13)/0.12≈1.917이라 이 모델에서 불가다. 큰 범위 초과를 무조건 1로 잘라 가짜 경계 해를 만들지 않는다. 반올림 수준의 오차를 다룰 허용치도 문서화해야 한다.

Jacobian은 현재 자세에서의 작은 변화다

위치 함수 p=f(q)를 관절 벡터 q=(q1,q2)에서 미분한 행렬이 위치 Jacobian J다. J의 각 열은 해당 관절각을 조금 바꿀 때 x,y가 얼마나 변하는지 나타낸다. 여기서는 각도를 rad로 수치화하고 길이는 m로 고정하여 J의 민감도 단위를 m/rad로 읽는다. 첫 행을 예로 들면 x=L1 cos q1+L2 cos(q1+q2)를 미분하여 얻는다.

$$J=\begin{bmatrix} -L_1\sin q_1-L_2\sin(q_1+q_2)&-L_2\sin(q_1+q_2)\\ L_1\cos q_1+L_2\cos(q_1+q_2)&L_2\cos(q_1+q_2) \end{bmatrix}.$$

현재 q=(0,π/2)에서는 위치가 (0.3,0.2) m이고 J의 행은 (−0.2,−0.2)와 (0.3,0)이다. 작은 관절 증분 Δq=(0.01,0) rad를 넣으면 Δp≈JΔq=(−0.002,0.003) m다. 이것은 국소 선형 예측이며 큰 각도 변화에 같은 J를 그대로 쓰는 정확한 FK가 아니다. q를 갱신하면 다시 FK와 J를 계산한다.

목표 잔차를 e=p_d−f(q)로 정의한다. 부호가 반대이면 업데이트 방향도 바뀐다. J가 정방이라도 특이하면 역행렬이 없고, 거의 특이하면 역행렬을 이용한 증분이 매우 커질 수 있다. DLS(Damped Least Squares, 감쇠 최소제곱)는 선형 잔차와 증분의 크기를 함께 줄이려는 방법이다. λ>0은 감쇠 계수다. 이 위치 문제에서는 λ를 m/rad의 민감도 스케일로 읽고 m와 rad의 수치 기준을 고정한다. 그러면 목적함수의 두 항은 모두 m² 스케일이다. 스케일이나 위치/각도 가중치가 바뀌면 λ의 의미도 재검토해야 한다.

$$\min_{\Delta q}\;\|J\Delta q-e\|^2+\lambda^2\|\Delta q\|^2.$$

계산은 역행렬을 직접 만드는 대신 선형계를 풀어도 된다. I는 위치 차원인 2×2 단위행렬이고, z는 중간 계산 벡터로 이 단위 표기에서는 rad²/m 스케일이다.

$$(JJ^T+\lambda^2 I)z=e,\qquad \Delta q=J^Tz.$$

한 번 풀고 실제 잔차를 다시 본다

현재 자세 q=(0,π/2), 목표 (0.31,0.20) m면 e=(0.01,0) m다. λ=0.1로 정한 수치 예에서 JJᵀ+λ²I의 행은 (0.09,−0.06)와 (−0.06,0.10)이다. 두 식은 0.09z1−0.06z2=0.01과 −0.06z1+0.10z2=0이다. 둘째 식에서 z2=0.6z1이고 첫째에 넣으면 0.054z1=0.01이다. 따라서 z≈(0.185185,0.111111), Δq≈(−0.003704,−0.037037) rad다.

한 단계 크기를 그대로 쓰는 예의 q_new=q+Δq다. 선형 예측 변화는 약 (0.008148,−0.001111) m이고, 새 FK는 약 (0.308144,0.198723) m다. 새 잔차는 (0.001856,0.001277) m로 0이 아니다. 감쇠는 잔차를 정확히 한 번에 없애기보다 증분 크기를 줄이는 절충이다. 정확한 비선형 잔차와 선형 예측의 차이도 남는다. 더 작은 단계 계수나 증분 상한을 쓸 때에는 적용한 값을 별도로 기록한다.

반복에서는 잔차 허용치·관절 범위·충돌·최대 반복 수·작은 증분만 남는 정체를 확인한다. λ를 키우면 대체로 증분을 줄이는 효과가 있지만 모든 목표의 도달과 안전을 보장하지 않는다. 한 초기값에서 수렴하지 않았다고 전체 작업공간에서 해가 없다는 증명도 아니다. 반대로 기하적으로 불가한 목표를 DLS가 도달 가능하게 만들지는 않는다. 펼친 자세에서는 x방향 Jacobian 행이 0이어서 x잔차만 있는 선형 갱신이 0으로 멈출 수도 있다. 잔차가 남았는데 Δq만 작다는 이유로 성공을 선언하지 않는다. 기존 3.3의 IK 코드로 확장할 때도 위치와 회전 잔차의 좌표·스케일 및 실제 관절 조건을 별도로 맞춘다.

입력은 본문의 0.3/0.2 m 평면 2R다. 관절 한계와 충돌은 일단 없으며 계산 뒤 추가 검사 대상으로 남긴다. 현재 자세는 (0,π/2), DLS 목표는 (0.31,0.20) m, λ=0.1이고 단계 계수는 1이다.

1. 기하 목표 (0.3,0.2) m의 c2와 양/음 두 분기를 구한다. (0.6,0) m 목표의 도달 조건도 검사한다.
2. 현재 Jacobian을 FK 미분식에서 구하고 Δq=(0.01,0)의 선형 변화를 계산한다. 이어 DLS의 두 선형식을 풀어 z와 관절 증분을 얻는다.
3. 갱신 뒤 제시된 FK (0.308144,0.198723) m에서 잔차를 계산한다. 잔차 허용치를 각 성분 0.001 m 이하로 정하면 성공인지 판정하고, 다음 반복 및 중단에서 확인할 조건을 제안한다.

기본 활동은 종이 계산이며 실제 관절 명령을 보내지 않는다. 막히면 먼저 e의 부호와 J의 행/열 의미를 적는다. c2가 크게 범위를 벗어나면 sqrt를 억지로 계산하지 않고 도달 검사를 먼저 끝낸다.