3.3 DLS 역기구학

3.3.1 선형화와 감쇠

현재 pose에서 작은 관절 변화 $\Delta q$ 에 대한 작업 공간 변화는

\Delta x\approx J(q)\Delta q

이다. 단순 의사역행렬은 특이값이 0에 가까울 때 큰 관절 변화를 만들 수 있다. DLS는

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

를 풀어 다음 갱신을 얻는다.

\Delta q=J^T(JJ^T+\lambda^2I)^{-1}e.

$\lambda$ 가 커지면 관절 변화는 작아지지만 목표 추종 오차가 늘어난다. Wampler는 자코비안 기반 IK가 특이점에서 겪는 문제를 DLS 문제로 재정식화해 모든 자세에서 유용한 보정량을 얻는 방법을 제시했다.[Wampler, 1986]

이해 도움말 — DLS의 감쇠는 ‘브레이크’에 가깝다

특이점 근처에서는 엔드이펙터를 아주 조금 움직이기 위해 일부 관절을 매우 크게 움직여야 한다. 이때 $\lambda$ 는 관절 증분이 폭주하지 않도록 제동을 건다. $\lambda$ 가 작으면 목표를 더 적극적으로 추종하지만 노이즈와 모델 오차에도 민감하고, 크면 움직임은 안정적이지만 목표에 덜 정확하게 접근한다. 따라서 $\lambda$ 는 “정확도 설정값”이 아니라 추종 오차와 관절 움직임 사이의 절충(trade-off)이다.

3.3.2 반복 알고리즘

현재 $q_k$ 에서 FK로 $x_k$ 를 구한다.
오차 $e_k=x^*-x_k$ 를 계산한다.
$J(q_k)$ 와 DLS 증분 $\Delta q$ 를 구한다.
$\|\Delta q\|_\infty$ 를 한 스텝 최대값으로 제한한다.
$q_{k+1}=\operatorname{clip}(q_k+\Delta q,q_{min},q_{max})$ 로 관절 한계를 적용한다.
허용 오차, 최대 반복 수, 진전 부족(stagnation)을 검사한다.

np.linalg.inv로 역행렬을 직접 만들지 말고 np.linalg.solve를 사용한다. 목표가 작업 공간 밖이면 “가장 가까운 자세”가 성공처럼 보일 수 있으므로 수렴 여부와 최종 오차를 함께 반환한다.

3.3.3 5-DoF 과업 설계

SO-101에 6차원 pose 오차를 같은 가중치로 강제하면 서로 양립할 수 없는 목표 때문에 진동할 수 있다. 다음 전략을 선택할 수 있다.

위치 3차원만 정확히 추종하고 손목 각도는 별도 명령한다.
$W=\operatorname{diag}(1,1,1,w_r,w_r,w_r)$ 에서 $0<w_r\ll1$ 로 방향을 부드럽게 추종한다.
주 과업으로 위치를 풀고 영공간(null space) 또는 남는 자유도에서 관절 중앙화 비용을 최적화한다.
접근 방향 1~2개 축만 제약하고 축 주위 롤은 자유롭게 둔다.