제3부 동역학

3.5 효율적 동역학 알고리즘: CRBA·ABA·공간 벡터 대수

3.5.1 이 장에서 배우는 것 / 선수 지식 / 특히 중요한 독자

이 장은 로봇 동역학을 빠르게 계산하는 세 알고리즘, 즉 역동역학의 뉴턴-오일러 재귀(RNEA), 질량행렬을 만드는 복합 강체 알고리즘(composite rigid body algorithm, CRBA), 질량행렬 없이 순동역학을 $O(n)$에 푸는 관절체 알고리즘(articulated body algorithm, ABA)을 공간 벡터 대수로 구현하고 확인한다. 공간 벡터 대수는 회전과 병진을 6차원 벡터와 $6\times6$ 행렬로 한 번에 다루는 표기이며, 운동 벡터·힘 벡터·변환 $X$·외적 $\times$, $\times^{*}$·공간 관성으로 이루어진다. 이 장은 항등식 7종, 6R 팔에서 3.4의 자코비안 합 구현과의 일치, 12관절 분기 트리에서 $M$의 희소 구조, 곱셈 횟수로 센 연산량($n=6\sim384$), 6개 라이브러리와의 대조와 호출당 시간을 계산한다. 강체 하나의 관성과 공간 관성은 3.1, RNEA의 유도는 3.2, 라그랑주 유도는 3.3, 세 항의 성질은 3.4, 라이브러리와 도함수는 3.6, 구속 동역학은 3.11, 부유 기저는 3.13에서 다룬다.

배우는 것:

• 공간 벡터 대수 — 운동·힘 벡터, 변환 $X$와 $X^{*}=X^{-T}$, 외적 $v\times$·$v\times^{*}$, 공간 관성, 이 책의 $(\omega,v)$ 순서와 Modern Robotics의 $\mathrm{Ad}$·$\mathrm{ad}$와의 대응.
• CRBA — 복합 관성, $M_{ij}$를 조상 방향 전파로 채우는 $O(nd)$ 알고리즘(사슬은 $O(n^{2})$).
• ABA — 관절 가속도를 말단에서 기저로 소거하는 3회 순환, 관절체 관성, $O(n)$.
• 희소성과 연산량 — 분기 트리의 $M$ 희소 구조, 알고리즘별 곱셈 횟수와 실행 시간.

선수 지식: 1.10(트위스트·렌치·수반), 3.1(공간 관성), 3.2(RNEA), 3.4(운동방정식의 성질).

특히 중요한 독자: 시뮬레이터·동역학 라이브러리를 구현하거나 선택하는 개발자, 전신 제어에서 $M(q)$와 순동역학을 매 주기 계산하는 제어 엔지니어, 알고리즘 복잡도와 실행 시간을 비교하려는 연구자.

출처 방침. 기준 시점은 2026-11-05이다. 직접 읽은 자료: Lynch·Park, Modern Robotics(사전 인쇄본 2017-05판) 8.3~8.5절(식 8.50~8.54의 재귀, 식 8.81 순동역학, 질량행렬을 역동역학 $n$회로 구성하는 서술). MuJoCo 문서의 계산 개관(관절공간 관성 $M$은 CRB로 계산해 희소하게 저장하고 희소 분해를 계산한다는 서술, 편향력은 가속도 0의 RNE). Pinocchio 4.1.0의 aba·rnea·computeMinverse·cholesky.decompose 독스트링(computeMinverse는 "관절체 알고리즘의 확장"으로 $M^{-1}$을 계산한다고 적혀 있다). RBDL 2.6.0 소스의 Dynamics.h 주석(ForwardDynamics는 관절체 알고리즘으로 $O(n_{dof})$, ForwardDynamicsLagrangian은 CRBA로 만든 $H$로 선형계를 푼다). 서지만 확인한 것(Crossref로 실재를 확인했고 본문은 읽지 못했다): Featherstone(2008, Rigid Body Dynamics Algorithms; 장 제목만 Crossref로 확인: 공간 벡터 대수·역동역학·순동역학의 관성 행렬 방법·순동역학의 전파 방법), Featherstone(1983, 관절체 관성을 이용한 로봇 동역학 계산), Walker·Orin(1982, 로봇 기구의 효율적 동역학 시뮬레이션), Featherstone(2010, 6차원 벡터 입문 1·2부). 두 알고리즘의 식은 이 문헌을 읽지 않고 이 장이 뉴턴-오일러 식에서 직접 유도하고 3.4의 독립 구현과 대조해 확인했다. 방법 이름의 기원과 문헌의 연산량 표는 확인하지 못했다 (확인 필요). 팔·트리·사슬의 제원은 모두 이 장이 정한 가정이다.

3.5.2 개념 — 같은 운동방정식을 목적에 따라 세 가지로 계산한다

결론부터 적으면 다음과 같다. 직렬·분기 연쇄의 동역학은 링크마다 6차원 공간 벡터의 선형 연산으로 나타낼 수 있고, 그 재귀 구조가 알고리즘의 복잡도를 정한다. 관절 가속도에서 토크를 구하는 RNEA는 링크당 일정한 연산이어서 $O(n)$이다. 질량행렬 $M(q)$가 필요하면 CRBA가 말단 방향에서 부분 트리의 관성을 합쳐($I^{c}_{i}$) 모든 성분을 조상 방향으로 채우며 연산량은 트리 깊이 $d$에 대해 $O(nd)$이다. 토크에서 관절 가속도를 구하는 순동역학은 $M$을 만들어 푸는 방식($O(n^{3})$의 분해 포함)과 ABA의 방식이 있다. ABA는 관절 가속도를 말단에서 기저로 소거해 각 링크의 관절체 관성 $I^{A}_{i}$를 만들고 기저에서 말단으로 가속도를 복원하므로 $O(n)$이다. ABA는 $M$을 만들지 않으므로 $M$이 필요한 곳(작업공간 질량, 전신 제어의 QP, 구속 힘)에는 CRBA가 필요하다.

이 장의 계산 결과를 미리 적으면 다음과 같다. ① 공간 벡터 대수의 항등식 7종이 $2.3\times10^{-15}$ 안에서 성립하고 $X$·$v\times$가 Modern Robotics의 $\mathrm{Ad}$·$\mathrm{ad}$(4×4 행렬 켤레·교환자)와 같다. ② 6R 팔에서 CRBA의 $H$가 3.4의 $M$과 $2.7\times10^{-15}$, ABA가 3.4의 순동역학과 $1.3\times10^{-15}$ 안에서 같다. ③ 12관절 분기 트리의 $M$은 비영 성분 80개가 조상-후손 관계로 예측한 80개와 일치하고(50개 자세) 다른 가지의 관절 사이 성분은 정확히 0이다. ④ 곱셈 횟수가 $n=6\to384$에서 RNEA $1.00$·ABA $1.02$·CRBA $1.67$($n=192\to384$에서 $1.92$)·CRBA 경로 순동역학 $1.88$($2.60$)의 로그-로그 기울기이고 $n=384$에서 CRBA 경로가 ABA의 $42$배이다. ⑤ Pinocchio·MuJoCo·RBDL·Drake·modern_robotics·Robotics Toolbox가 $M$·$\ddot q$를 $1.5\times10^{-13}$ 이하로 재현하고, Pinocchio의 호출당 시간은 $n=192$에서 aba $76$ μs, CRBA 경로 $1388$ μs이다.

3.5.3 원리

3.5.3.1 공간 벡터와 변환

운동 벡터는 $v=(\omega,\,v_0)$(각속도, 기준점의 선속도), 힘 벡터는 $f=(n,\,f_0)$(기준점에 대한 모멘트, 힘)이며 이 책의 트위스트·렌치 순서와 같다. 좌표계 $A$에서 $B$로 옮기는 변환은 다음이다($E={}^{B}R_{A}$, $r$은 $B$ 원점의 $A$ 좌표).

$$ {}^{B}X_{A}=\begin{pmatrix}E&0\\-E\,\tilde r&E\end{pmatrix},\qquad {}^{B}X_{A}^{*}=({}^{B}X_{A})^{-T}=\begin{pmatrix}E&-E\,\tilde r\\0&E\end{pmatrix}.$$

${}^{B}X_{A}$는 Modern Robotics의 $\mathrm{Ad}_{T_{BA}}$와 같다. 운동 벡터는 $X$로, 힘 벡터는 $X^{*}$로 변환되며 $X^{*}$를 사용하면 일률 $v\cdot f$가 좌표계에 무관하다. 외적은 다음과 같고 $v\times$는 Modern Robotics의 $\mathrm{ad}_V$와 같다.

$$v\times=\begin{pmatrix}\tilde\omega&0\\\tilde v_0&\tilde\omega\end{pmatrix},\qquad v\times^{*}=-(v\times)^{T}.$$

이 장은 $X$의 역, 일률 불변, 켤레 $(Xv)\times=X(v\times)X^{-1}$, 외적의 야코비 항등식을 임의 입력에서 확인했다. 6차원 벡터의 성분 순서는 문헌과 라이브러리마다 다르다. Pinocchio의 Motion은 선속도가 먼저이다(Motion(linear, angular).vector가 선형 3성분 뒤에 각 3성분을 준다는 것을 직접 확인했다). 이 장의 라이브러리 대조는 관절공간 양($M$·$\tau$·$\ddot q$)만 비교하므로 성분 순서의 영향을 받지 않는다.

3.5.3.2 공간 관성과 운동방정식

링크 프레임 원점 기준의 공간 관성은 다음이고 운동에너지는 $\tfrac12v^TIv$이다($m$ 질량, $h=mc$, $I_O$ 원점 기준 관성 텐서).

$$I=\begin{pmatrix}I_O&\tilde h\\-\tilde h&m\,1\end{pmatrix},\qquad f=I\,a+v\times^{*}I\,v .$$

이 $I$는 3.4의 운동에너지 쌍일차 형식과 $9\times10^{-16}$ 안에서 같다. 관절 $i$는 운동 부분공간 $S_i$를 가진다. 회전 관절은 $S_i=(a_i,0)$이다(축이 프레임 원점을 지난다). 링크의 속도·가속도는 $v_i={}^{i}X_{\lambda}v_{\lambda}+S_i\dot q_i$, $a_i={}^{i}X_{\lambda}a_{\lambda}+S_i\ddot q_i+v_i\times(S_i\dot q_i)$이고($\lambda$는 부모), 관절이 링크에 전달하는 힘 $f_i$에서 구동 토크는 $\tau_i=S_i^Tf_i$이다. 중력은 기저 가속도 $a_0=(0,-\gamma)$로 넣는다(3.2와 같다).

3.5.3.3 RNEA — 두 번의 순환

전방 순환이 $v_i$, $a_i$, $f_i=I_ia_i+v_i\times^{*}I_iv_i$를 구하고 후방 순환이 $f_\lambda\mathrel{+}={}^{i}X_\lambda^{T}f_i$로 자식의 힘을 부모에 더하며 $\tau_i=S_i^Tf_i$를 얻는다. 링크마다 같은 연산이므로 $O(n)$이다. $q$·$\dot q$·$\ddot q$가 주어진 역동역학이며 $\ddot q=0$이면 편향력이다(3.2). 이 장은 RNEA를 CRBA 경로 순동역학의 편향항과 ABA의 검증에 사용한다.

3.5.3.4 CRBA — 복합 강체와 조상 방향 전파

관절 $i$보다 먼 관절을 모두 고정하면 $i$를 뿌리로 하는 부분 트리가 하나의 강체로 움직인다. 그 공간 관성이 복합 관성이다.

$$I^{c}_{i}=I_i+\sum_{j:\ \lambda(j)=i}{}^{j}X_{i}^{T}\,I^{c}_{j}\,{}^{j}X_{i}.$$

관절 $i$가 단위 가속도로 움직일 때 부분 트리가 받아야 하는 힘은 $F=I^{c}_{i}S_i$이고 $M_{ii}=S_i^TF$이다. 이 힘을 부모 방향으로 옮기며($F\leftarrow{}^{j}X_{\lambda}^{T}F$) 조상 $j$마다 $M_{ij}=M_{ji}=S_j^TF$를 얻는다. 한 링크에서 올라가는 횟수가 깊이 $d$이므로 연산량은 $O(nd)$이고 사슬에서는 $O(n^{2})$이다. 조상-후손 관계가 아닌 $(i,j)$는 전파 경로에 나타나지 않으므로 $M_{ij}=0$이다(3.5.3.7).

3.5.3.5 ABA — 관절 가속도의 소거

말단 링크 하나를 보자. 링크에 걸리는 힘은 $f=I(a'+S\ddot q)+p$이다($a'={}^{i}X_\lambda a_\lambda+c$, $c=v\times(S\dot q)$는 속도 항, $p=v\times^{*}Iv$). $\tau=S^Tf$를 $\ddot q$에 대해 풀면 $\ddot q=(u-U^Ta')/D$이고 $U=IS$, $D=S^TU$, $u=\tau-S^Tp$이다. 이를 대입하면 $f=\hat Ia'+\hat p$로 가속도 $a'$에만 선형인 꼴이 된다.

$$\hat I=I-\frac{UU^{T}}{D},\qquad \hat p=p+\frac{U\,u}{D}.$$

자식이 있는 링크 $\lambda$는 자식 $j$가 전달하는 힘 $f_j=\hat I_j({}^{j}X_{\lambda}a_\lambda+c_j)+\hat p_j$를 외력으로 받는다. 그러면 $\lambda$의 식도 같은 꼴이 되며 $I$·$p$ 자리에 관절체 관성 $I^{A}_{\lambda}$와 관절체 바이어스 힘 $p^{A}_{\lambda}$가 들어간다.

$$I^{A}_{\lambda}=I_\lambda+\sum_{j}{}^{j}X_{\lambda}^{T}\hat I_j\,{}^{j}X_{\lambda},\qquad p^{A}_{\lambda}=p_\lambda+\sum_{j}{}^{j}X_{\lambda}^{T}\bigl(\hat p_j+\hat I_jc_j\bigr),$$

여기서 모든 링크 $i$에 대해 $U_i=I^{A}_{i}S_i$, $D_i=S_i^TU_i$, $u_i=\tau_i-S_i^Tp^{A}_{i}$, $\hat I_i=I^{A}_{i}-U_iU_i^T/D_i$, $\hat p_i=p^{A}_{i}+U_iu_i/D_i$이다. 기저에서 말단으로 가속도를 복원한다: $a'_i={}^{i}X_\lambda a_\lambda+c_i$, $\ddot q_i=(u_i-U_i^Ta'_i)/D_i$, $a_i=a'_i+S_i\ddot q_i$. 세 순환(속도·바이어스 / 관절체 관성 / 가속도)이 모두 링크당 일정한 연산이므로 $O(n)$이다. 이 소거는 $M$에 대한 블록 가우스 소거와 같으며 $I^{A}_{i}$는 링크 $i$ 아래 부분 트리의 관절이 모두 자유로울 때 링크 $i$가 보이는 관성이다(관절 $i$ 자신은 부모로 넘길 때 소거된다).

3.5.3.6 연산량과 계산 목적

RNEA는 $O(n)$, CRBA는 $O(nd)$(사슬 $O(n^{2})$), CRBA 경로 순동역학은 $M$의 분해가 밀집일 때 $O(n^{3})$, ABA는 $O(n)$이다. $M$을 RNEA $n$회로 만드는 Modern Robotics 8.5절의 방법은 $O(n^{2})$이다. $M^{-1}$이 필요하면 ABA를 $g=0$, $\dot q=0$, $\tau=e_j$로 $n$회 호출해 열을 얻을 수 있다(이 장은 $10^{-14}$로 확인했다). Pinocchio는 computeMinverse가 ABA의 확장으로 $M^{-1}$을 계산한다고 독스트링에 적었다. MuJoCo는 $M$을 CRB로 계산해 희소 분해하는데, 접촉 구속이 $M^{-1}J^{T}$를 요구하기 때문이며 이 이유는 문서 개관에서 직접 확인하지 못했다 (추정). 계산 목적별 선택은 3.5.6.

3.5.3.7 트리 구조와 희소성

트리에서 $\lambda(i)$는 $i$의 부모이다. CRBA의 전파는 $i$에서 뿌리까지의 경로를 따르므로 $M_{ij}\ne0$이려면 $i$가 $j$의 조상이거나 후손이거나 같아야 한다. 서로 다른 가지(예: 한쪽 팔과 다른 팔)의 관절 사이는 $M_{ij}=0$이며 이 구조는 희소 저장과 희소 분해에 이용된다(MuJoCo 문서가 $M$의 희소 저장과 희소 분해를 서술한다). 관련 쌍이라도 기하가 대칭이면 우연히 0일 수 있다(이 장의 예시 트리는 질량중심을 축에서 어긋나게 두어 피했다). 부유 기저는 뿌리에 6자유도 관절($S=1_{6}$)을 두는 것이며 3.13에서 다룬다. 이 장은 부유 기저를 시험하지 않았다.

3.5.4 알고리즘

CRBA(tree, q):       X_i, S_i ← 변환·운동 부분공간;  I^c_i ← I_i
                     i = n..1:  I^c_λ(i) += X_i^T I^c_i X_i
                     i = 1..n:  F ← I^c_i S_i;  H_ii ← S_i·F;  j ← i
                                j가 뿌리가 아닌 동안:  F ← X_j^T F;  j ← λ(j);  H_ij = H_ji ← S_j·F
ABA(tree, q, q', τ): [1 전방] v_i ← X_i v_λ + S_i q'_i;  c_i ← v_i × S_i q'_i;  p^A_i ← v_i ×* I_i v_i;  I^A_i ← I_i
                     [2 후방] i = n..1:  U_i ← I^A_i S_i;  D_i ← S_i·U_i;  u_i ← τ_i − S_i·p^A_i
                                         Ia ← I^A_i − U_i U_i^T / D_i;  pa ← p^A_i + Ia c_i + U_i u_i / D_i
                                         I^A_λ += X_i^T Ia X_i;  p^A_λ += X_i^T pa
                     [3 전방] a'_i ← X_i a_λ + c_i;  q''_i ← (u_i − U_i·a'_i)/D_i;  a_i ← a'_i + S_i q''_i      (a_기저 = (0, −γ))
CRBA 경로 순동역학:  h ← RNEA(q, q', 0);  H ← CRBA(q);  H = L L^T;  q'' ← (L L^T)^-1 (τ − h)

3.5.5 풀이 예제 — 항등식, 6R 팔, 분기 트리, 연산량, 라이브러리

결론부터 적으면 다음과 같다. ① 공간 벡터 대수의 항등식이 기계 정밀도로 성립한다. ② CRBA·RNEA·ABA가 3.4의 독립 구현과 6R 팔에서 일치한다. ③ 분기 트리의 $M$ 희소 구조가 조상-후손 관계와 정확히 같다. ④ ABA의 연산량은 $n$에 선형이고 CRBA 경로는 $n$이 커질수록 $n^{3}$에 가까워진다. ⑤ 라이브러리 6종이 일치하고 Pinocchio·RBDL의 실행 시간이 같은 추세를 보인다.

구현. crm·crf·xform·inertia6·tf_all·rnea·crba·aba·chol_solve·fd_crba. 아래 코드는 1.1의 matmul·solve_lu와 3.4의 skew·rodrigues를 사용한다. 트리·사슬 생성, 곱셈 횟수 계수(count_ops), 표·그림은 스크립트에 있다.

from math import sqrt

Z3 = [[0.0] * 3 for _ in range(3)]

def tr(A):
    return [list(r) for r in zip(*A)]
def mv(A, x):
    return [sum(a * b for a, b in zip(r, x)) for r in A]
def mm(A, B):
    return matmul(A, B)
def outer(a, b, s=1.0):
    return [[s * x * y for y in b] for x in a]
def madd(A, B, s=1.0):
    return [[x + s * y for x, y in zip(r, t)]
            for r, t in zip(A, B)]
def neg(A):
    return [[-x for x in r] for r in A]
def blocks(A, B, C, D):
    return ([A[i] + B[i] for i in range(3)]
            + [C[i] + D[i] for i in range(3)])

def crm(v):
    """운동 벡터 v=(ω, v0)의 외적 v× (6×6)"""
    W, V = skew(v[:3]), skew(v[3:])
    return blocks(W, Z3, V, W)
def crf(v):
    """힘 벡터용 외적 v×* = −(v×)^T"""
    return neg(tr(crm(v)))
def xform(E, r):
    """^B X_A = [[E, 0], [−E r~, E]]
    (E = ^B R_A, r = B 원점의 A 좌표)"""
    return blocks(E, Z3, neg(mm(E, skew(r))), E)
def inertia6(m, h, I):
    """공간 관성 [[I_O, h~], [−h~, m 1]]
    (h = m c, 링크 프레임 원점 기준)"""
    H = skew(h)
    M1 = [[m * (i == j) for j in range(3)] for i in range(3)]
    return blocks(I, H, neg(H), M1)

def tf_all(tree, q):
    """X[i] = ^iX_λ(i)(q_i), S[i] = (a_i, 0)"""
    X, S = [], []
    for d, a, qi in zip(tree['d'], tree['a'], q):
        X.append(xform(tr(rodrigues(a, qi)), d))
        S.append(list(a) + [0.0, 0.0, 0.0])
    return X, S

def rnea(tree, q, qd, qdd, gam=(0.0, 0.0, -9.81)):
    n, par = len(q), tree['par']
    X, S = tf_all(tree, q)
    G = [inertia6(*P) for P in tree['P']]
    a0 = [0.0, 0.0, 0.0] + [-g for g in gam]  # 중력
    v, a, f = [0] * n, [0] * n, [0] * n
    for i in range(n):
        vp = v[par[i]] if par[i] >= 0 else [0.0] * 6
        ap = a[par[i]] if par[i] >= 0 else a0
        vJ = vsc(qd[i], S[i])
        v[i] = vadd(mv(X[i], vp), vJ)
        a[i] = vadd(vadd(mv(X[i], ap), vsc(qdd[i], S[i])),
                    mv(crm(v[i]), vJ))
        f[i] = vadd(mv(G[i], a[i]),
                    mv(crf(v[i]), mv(G[i], v[i])))
    tau = [0.0] * n
    for i in reversed(range(n)):
        tau[i] = dot(S[i], f[i])
        if par[i] >= 0:
            f[par[i]] = vadd(f[par[i]], mv(tr(X[i]), f[i]))
    return tau

def crba(tree, q):
    """복합 강체: 후방 1회(복합 관성) + 조상 전파"""
    n, par = len(q), tree['par']
    X, S = tf_all(tree, q)
    Ic = [inertia6(*P) for P in tree['P']]
    for i in reversed(range(n)):
        if par[i] >= 0:
            Ic[par[i]] = madd(Ic[par[i]],
                              mm(tr(X[i]), mm(Ic[i], X[i])))
    H = [[0.0] * n for _ in range(n)]
    for i in range(n):
        F = mv(Ic[i], S[i])
        H[i][i] = dot(S[i], F)
        j = i
        while par[j] >= 0:
            F = mv(tr(X[j]), F)
            j = par[j]
            H[i][j] = H[j][i] = dot(S[j], F)
    return H

def aba(tree, q, qd, tau, gam=(0.0, 0.0, -9.81)):
    """관절체: 전방(속도) - 후방(관성) - 전방(가속도)"""
    n, par = len(q), tree['par']
    X, S = tf_all(tree, q)
    IA = [inertia6(*P) for P in tree['P']]
    a0 = [0.0, 0.0, 0.0] + [-g for g in gam]
    v, c, pA = [0] * n, [0] * n, [0] * n
    for i in range(n):
        vp = v[par[i]] if par[i] >= 0 else [0.0] * 6
        vJ = vsc(qd[i], S[i])
        v[i] = vadd(mv(X[i], vp), vJ)
        c[i] = mv(crm(v[i]), vJ)
        pA[i] = mv(crf(v[i]), mv(IA[i], v[i]))
    U, D, u = [0] * n, [0.0] * n, [0.0] * n
    for i in reversed(range(n)):
        U[i] = mv(IA[i], S[i])
        D[i] = dot(S[i], U[i])
        u[i] = tau[i] - dot(S[i], pA[i])
        p = par[i]
        if p >= 0:
            UU = outer(U[i], U[i], 1 / D[i])
            Ia = madd(IA[i], UU, -1.0)
            pa = vadd(vadd(pA[i], mv(Ia, c[i])),
                      vsc(u[i] / D[i], U[i]))
            IA[p] = madd(IA[p], mm(tr(X[i]), mm(Ia, X[i])))
            pA[p] = vadd(pA[p], mv(tr(X[i]), pa))
    a, qdd = [0] * n, [0.0] * n
    for i in range(n):
        ap = a[par[i]] if par[i] >= 0 else a0
        ai = vadd(mv(X[i], ap), c[i])
        qdd[i] = (u[i] - dot(U[i], ai)) / D[i]
        a[i] = vadd(ai, vsc(qdd[i], S[i]))
    return qdd

def chol_solve(H, b):
    """H = L L^T 와 전·후진 대입"""
    n = len(b)
    L = [[0.0] * n for _ in range(n)]
    for i in range(n):
        for j in range(i + 1):
            s = H[i][j] - dot(L[i][:j], L[j][:j])
            L[i][j] = sqrt(s) if i == j else s / L[j][j]
    y = [0.0] * n
    for i in range(n):
        y[i] = (b[i] - dot(L[i][:i], y[:i])) / L[i][i]
    x = [0.0] * n
    for i in reversed(range(n)):
        col = [L[k][i] for k in range(i + 1, n)]
        x[i] = (y[i] - dot(col, x[i + 1:])) / L[i][i]
    return x

def fd_crba(tree, q, qd, tau):
    """CRBA 경로의 순동역학: M q'' = τ − RNEA(q, q', 0)"""
    h = rnea(tree, q, qd, [0.0] * len(q))
    b = [t - x for t, x in zip(tau, h)]
    return chol_solve(crba(tree, q), b)

① 공간 벡터 대수의 항등식.

변환의 역, 일률 불변, 켤레, 야코비 항등식이 성립하고 $X$·$v\times$가 4×4 행렬 켤레 $T[V]T^{-1}$와 교환자 $[V][W]-[W][V]$(Modern Robotics의 $\mathrm{Ad}$·$\mathrm{ad}$ 정의)와 같다. 공간 관성의 이차 형식이 3.4의 $M$ 구성에 사용한 운동에너지 형식과 같다.

② 6R 팔 — 3.4과의 대조.

CRBA의 $H$가 3.4의 자코비안 합 $M$과, RNEA가 3.4의 역동역학과, ABA가 3.4의 순동역학과 같다. 세 알고리즘은 서로 다른 계산 경로(자코비안 합 / 공간 벡터 재귀 / 소거)이므로 일치가 구현 오류를 가려내는 시험이 된다. $M^{-1}$을 ABA의 열로 만든 결과가 $MM^{-1}-1$에서 $1.3\times10^{-14}$이다. 오차가 $10^{-14}$ 수준인 것은 이 팔의 $M$의 조건수($1.6\times10^{4}$, 3.4)에 기인한다고 추정한다 (추정).

③ 분기 트리의 희소성.

비영 성분이 80개로 조상-후손 관계로 예측한 값과 같고 50개 자세 모두에서 예측과 다른 성분이 없다. 목·왼팔·오른팔의 관절 사이 성분이 정확히 0이다(전체 144칸 중 $44\,\%$). ABA와 CRBA 경로가 트리에서도 $1.3\times10^{-15}$ 안에서 같고, RNEA로 만든 $\tau$를 ABA에 넣으면 $\ddot q$가 $3.6\times10^{-14}$ 안에서 복원된다.

④ 연산량.

RNEA와 ABA의 곱셈 횟수는 $n$에 선형이다(링크당 약 $285$회·약 $800$회: $n=384$에서 $109{,}404/384$, $307{,}044/384$). CRBA는 사슬에서 $n$의 약 2제곱($n=192\to384$ 기울기 $1.92$)이고 CRBA 경로 순동역학은 촐레스키 분해가 지배하여 기울기가 $2.60$까지 올라 3에 접근한다. $n=6$에서는 CRBA 경로가 ABA의 $1.16$배로 거의 같고 $n=96$에서 $5.5$배, $n=384$에서 $42$배이다. $M$을 RNEA $n$회로 만들면 $n=96$에서 $96\times27{,}324=2{,}623{,}104$회로 CRBA($239{,}184$회)의 $11$배이다. 이 횟수는 블록 구조·희소성을 이용하지 않은 밀집 곱이므로 실제 라이브러리의 절대 연산량이 아니라 $n$에 대한 증가율을 보이는 값이다. 스칼라 연산·전치·덧셈은 세지 않았다.

⑤ 라이브러리 대조(실행 2026-11-05). 같은 6R 팔과 12관절 트리를 URDF(Pinocchio·Drake)·MJCF(MuJoCo)·같은 숫자의 텍스트 입력(RBDL C++ 하니스)·공간 나사축과 질량중심 프레임(modern_robotics)·Link 객체(Robotics Toolbox)로 옮겨 비교했다. Pinocchio 4.1.0의 crba·rnea·aba·computeMinverse·cholesky, MuJoCo 3.17.0의 mj_fullM·mj_forward, modern_robotics 1.1.1의 MassMatrix·ForwardDynamics, Robotics Toolbox 1.5.0의 inertia·accel, Drake 1.58.0 MultibodyPlant, RBDL 2.6.0(g++로 빌드한 C++ 하니스)을 사용했다. 관절 순서는 이름으로 대응시켰다. 스크립트는 robotics/fig-src/3.5-libcheck.py·3.5-libcheck-drake.py·3.5-libcheck-rbdl.py(+ 3.5-libcheck-rbdl.cc·3.5-libcheck-model.py)이고 출력은 robotics/fig-src/data/3.5-libcheck-output.txt에 있다.

모든 대조가 $3.1\times10^{-13}$ 이하(질량행렬은 $4.0\times10^{-15}$ 이하)에서 같고, 네 라이브러리(Pinocchio·MuJoCo·RBDL·Drake)의 $M$에서 비영 성분 위치도 이 장과 같다(다른 칸 0). Pinocchio의 $M$은 상삼각만 채워 대칭화했다. 다음은 같은 사슬에서 잰 호출당 시간이다. 값은 이 실행의 최솟값이며 컴퓨터·컴파일러·시점에 따라 달라진다.

두 라이브러리에서 ABA·RNEA의 시간은 $n$에 거의 선형이다($n=24\to192$, 8배일 때 Pinocchio $5.9\sim6.1$배·RBDL $6.9\sim8.0$배이며 Pinocchio는 호출 고정 비용 때문에 작은 $n$에서 선형보다 낮게 증가한다). CRBA는 $n=96\to192$에서 Pinocchio $4.3$배·RBDL $3.6$배로 증가하고 CRBA 경로(Pinocchio)와 ForwardDynamicsLagrangian(RBDL)은 $5.6$배로 증가한다. $n=6$에서 CRBA 경로는 ABA의 $1.3$배(Pinocchio $3.9$ 대 $2.9$ μs)·$1.6$배(RBDL $4.14$ 대 $2.60$ μs)이고 $n=192$에서는 $17.5$배(Pinocchio)·$28$배(RBDL)이다. 절대 시간은 Pinocchio가 파이썬 호출 비용을 포함하고 RBDL이 C++ 하니스의 평균이므로 두 라이브러리를 비교하는 값이 아니다. 두 라이브러리의 알고리즘 구현 방식(희소성 이용 등)은 소스를 읽지 않았다 (확인 필요).

한계. 6R 팔·12관절 트리·사슬의 회전 관절에 한정하고 직동 관절·구면 관절·부유 기저·구속(폐연쇄)은 시험하지 않았다. 곱셈 횟수는 구조를 이용하지 않은 밀집 계산의 증가율이고 실제 시간은 구현·메모리 접근·컴파일러에 의존한다. 두 알고리즘의 식은 문헌의 서술이 아니라 이 장의 유도이며 수치 일치로 확인했다.

견해: 순동역학만 필요한 시뮬레이션과 예측 모델에서는 ABA를, $M(q)$ 자체가 필요한 작업공간 제어·전신 제어(12.6·12.7)와 접촉 구속과 결합하는 시뮬레이터(16.1)에서는 CRBA를 기본으로 두고, 자유도가 $6\sim12$ 정도로 작으면 둘의 시간 차이가 수 μs여서 필요한 양에 따라 고른다고 본다. 근거는 이 장의 계산에서 ① ABA의 곱셈 횟수와 Pinocchio·RBDL의 시간이 $n$에 거의 선형이고 CRBA 경로는 $n=192$에서 ABA의 $17.5$배(Pinocchio)·$28$배(RBDL)이며, ② $n=6$에서는 CRBA 경로가 ABA의 $1.3$배(Pinocchio $3.9$ 대 $2.9$ μs)·$1.6$배(RBDL)에 그친다는 점이다. 이 비교는 사슬·한 컴퓨터·회전 관절에 한정되고 $M$ 이외의 산출물(구속 힘·도함수)의 비용은 비교하지 않았다.

3.5.6 설계 트레이드오프 — 어느 알고리즘으로 무엇을 계산하는가

필요한 산출물에 따라 다음 중에서 고른다.

• ① RNEA. 토크(역동역학)·편향력·중력항. $O(n)$이고 $M$을 만들지 않는다(3.2).
• ② CRBA + 분해. $M(q)$ 자체(작업공간 질량, 전신 제어의 QP, 구속 결합)와 순동역학. $O(nd)$로 $M$을, 밀집 분해로 순동역학을 $O(n^{3})$에 얻는다.
• ③ ABA. 순동역학만. $O(n)$이고 $M$을 만들지 않는다. $M^{-1}$은 $n$회 호출로 얻는다.
• ④ RNEA $n$회. $M$을 열마다 구성하는 가장 단순한 방식(Modern Robotics 8.5절). $O(n^{2})$이다.

견해: 라이브러리를 선택할 때 순동역학의 알고리즘 이름보다 필요한 산출물(질량행렬·$M^{-1}$·도함수·구속 힘)과 모델 형식(URDF·MJCF) 지원을 먼저 본다고 보며, 이 장의 라이브러리 6개는 6R 팔에서(Pinocchio·MuJoCo·Drake·RBDL은 분기 트리에서도) $M$·$\ddot q$를 $2\times10^{-13}$ 이하로 같은 값을 내 정확도로는 구별되지 않았다. 도함수의 비교는 3.6이다.

3.5.7 응용 축 비교

이 표는 이 책의 정리이며 장별 요구는 다르다. 응용별 성능 수치는 이 장이 확인하지 않았다.

3.5.8 관련 규격·프로토콜

동역학 알고리즘 자체를 규정한 표준은 확인하지 못했다 (확인 필요).

3.5.9 관련 규제·인증 요구와의 연결

동역학 알고리즘을 규정하는 법령·인증 요건은 확인하지 못했다. 간접 연결은 실시간 안전 기능이 모델 기반 계산(충돌 평가에 사용되는 유효 질량, 3.4)에 의존할 때 계산 시간의 상한이 안전 기능의 응답 시간에 포함된다는 점이다(26.7). 이 장은 계산 시간의 최악값을 측정하지 않았고 규정의 조항을 확인하지 않았다 (확인 필요).

3.5.10 한계·유의점·미해결

• 관절 종류. 회전 관절 트리만 시험했다. 직동·구면·부유 기저 관절은 $S$의 열 수가 다르며 3.13의 주제이다.
• 연산량의 의미. 구조를 이용하지 않은 밀집 곱셈 횟수이며 실제 라이브러리의 연산량·시간과 다르다. 시간은 한 컴퓨터의 최솟값이다.
• 문헌 대조 부재. Featherstone의 알고리즘 표·연산량과 이 장의 식·횟수를 대조하지 못했다 (확인 필요).
• 희소 분해. 트리의 희소 구조를 이용한 분해(MuJoCo)의 이득은 측정하지 않았다.
• 수치 안정성. 손목 관성이 작은 팔의 큰 조건수($10^{4}$)에서 CRBA 경로와 ABA의 오차 차이는 $10^{-15}\sim10^{-13}$이었고 더 나쁜 조건수는 시험하지 않았다.
• 라이브러리. 6개 중 알고리즘 내부는 소스를 읽지 않았다. 시간은 Pinocchio(파이썬 호출 포함)와 RBDL(C++)만 잰다.

3.5.11 체크포인트·연구 질문·출처

체크포인트:

1. ${}^{B}X_{A}^{*}=({}^{B}X_{A})^{-T}$이면 일률 $v\cdot f$가 좌표에 무관함을 보이고 $v\times^{*}=-(v\times)^{T}$를 유도하라.
2. $M_{ij}=S_j^T\,{}^{j}X_{i}^{*}\cdots I^{c}_{i}S_i$ 꼴의 전파가 조상-후손 쌍에만 나타나는 이유를 설명하라.
3. 말단 링크의 식 $f=I(a'+S\ddot q)+p$와 $\tau=S^Tf$에서 $\hat I=I-UU^T/D$를 유도하라.
4. ABA가 $M$에 대한 블록 가우스 소거와 같은 이유를 두 링크 사슬에서 보여라.
5. 사슬에서 CRBA가 $O(n^{2})$이고 분해가 $O(n^{3})$인 이유를 적고 표의 기울기($1.92$, $2.60$)와 비교하라.
6. $M^{-1}$을 ABA $n$회로 얻는 방법의 연산량을 구하고 Pinocchio computeMinverse와 비교하는 시험을 설계하라.
7. 12관절 트리에서 $M$의 비영 성분 수를 부모 배열만으로 세는 식을 적어라.

연구 질문:

• 부유 기저($S=1_{6}$)와 접촉 구속을 포함한 트리에서 ABA와 CRBA의 상대 비용은 어떻게 바뀌는가(3.13·3.11)?
• 트리의 희소 구조를 이용한 분해가 휴머노이드(분기가 많은 트리)의 순동역학에서 ABA보다 빠른 자유도 범위는 어디인가?
• GPU·SIMD 병렬 실행에서 ABA의 세 순환과 CRBA의 전파 중 어느 쪽이 병렬화에 유리한가(16.5)?

1차 출처(조회 2026-11-05).

• K. M. Lynch, F. C. Park, Modern Robotics: Mechanics, Planning, and Control, Cambridge University Press, 2017 — 사전 인쇄본(2017-05) 8.3~8.5절을 읽었다.
• R. Featherstone, Rigid Body Dynamics Algorithms, Springer, 2008, doi:10.1007/978-1-4899-7560-7 — 서지만 확인(장 제목: 공간 벡터 대수, 역동역학, 순동역학—관성 행렬 방법, 순동역학—전파 방법을 Crossref로 확인).
• R. Featherstone, "The calculation of robot dynamics using articulated-body inertias," The International Journal of Robotics Research 2(1), 13–30, 1983, doi:10.1177/027836498300200102 — 서지만 확인.
• M. W. Walker, D. E. Orin, "Efficient dynamic computer simulation of robotic mechanisms," Journal of Dynamic Systems, Measurement, and Control 104(3), 205–211, 1982, doi:10.1115/1.3139699 — 서지만 확인.
• R. Featherstone, "A beginner's guide to 6-D vectors (part 1)," IEEE Robotics & Automation Magazine 17(3), 83–94, 2010, doi:10.1109/MRA.2010.937853, (part 2) 17(4), 88–99, doi:10.1109/MRA.2010.939560 — 서지만 확인.
• J. Carpentier 외, "The Pinocchio C++ library," 2019 IEEE/SICE International Symposium on System Integration, 614–619, doi:10.1109/SII.2019.8700380 — 서지만 확인.
• M. L. Felis, "RBDL: an efficient rigid-body dynamics library using recursive algorithms," Autonomous Robots 41(2), 495–511, 2017, doi:10.1007/s10514-016-9574-0 — 서지만 확인.
• MuJoCo 문서 "Computation"(mujoco.readthedocs.io) — 계산 개관을 읽었다. RBDL 2.6.0 소스 include/rbdl/Dynamics.h 주석을 읽었다.
• Pinocchio 4.1.0, MuJoCo 3.17.0, Drake 1.58.0, RBDL 2.6.0(PyPI pyrbdl 0.0.2 소스 배포본에 포함), modern_robotics 1.1.1, Robotics Toolbox for Python 1.5.0, numpy 2.5.4.

다음 장. 3.6은 동역학 라이브러리(Pinocchio·RBDL·Drake MultibodyPlant·MuJoCo)의 구조와 동역학의 도함수, 즉 해석적 도함수와 자동 미분을 다룬다. 이 장의 RNEA·ABA를 $q$·$\dot q$·$\tau$에 대해 미분하는 문제이다.