왜 선형이 중요한가
좌변이 \(\dot q\)에 대해 선형이므로, 이 조건은 QP(이차계획법) 안의 선형 부등식 제약 하나로 그대로 들어간다. 매 제어 주기마다 실시간으로 풀 수 있다.
연속시간 제어 시스템에서 안전 제약을 수학적으로 보장하는 방법, 그리고 dexterous hand retargeting에 붙이는 방식.
먼저 "안전하다"는 것을 함수로 표현한다. 시스템 상태 \(x \in \mathbb{R}^n\)에 대해, 연속 미분 가능한 함수 \(h(x)\)가 안전 제약을 인코딩한다. 이 함수가 0 이상인 영역이 곧 안전 집합 \(\mathcal{C}\)다.
예를 들어 로봇 손가락이 서로 충돌하면 안 된다면, \(h(x)\)를 두 링크 사이의 거리 마진으로 잡는다. 거리가 양수면 안전, 0이면 막 닿기 직전, 음수면 이미 충돌이다. 핵심은 제약을 하나의 스칼라 함수로 압축한다는 점이다.
안전 집합을 정의했다면, 다음 목표는 시스템이 그 집합 안에 영원히 머무르게(forward invariance) 하는 것이다. 1차 시스템에서는 다음 조건을 시스템 동역학에 부과한다.
여기서 "차수(order)"는 제어 입력이 안전 함수 h에 영향을 주기까지 h를 몇 번 미분해야 하는가를 가리킨다. 정확히는 relative degree(상대 차수)라는 개념이다.
1차 시스템은 상태의 1차 미분이 입력에 의해 직접 결정되는 경우다. \(\dot x = f(x) + g(x)\,u\) 형태로, 입력 \(u\)가 곧바로 "속도"를 만든다. 로봇 맥락에서는 관절 속도 \(\dot q\)(또는 증분 \(\Delta q\))를 직접 명령하는 velocity-level 제어가 여기에 해당한다. 이 경우 \(h\)를 한 번만 미분해도 \(\dot h\) 안에 입력이 나타나므로, 식 (3)의 \(\dot h + \alpha h \ge 0\)이 곧바로 입력에 대한 부등식이 된다. 이 자료의 retargeting이 \(\dot q\)를 다루기 때문에 1차 시스템 가정이 자연스럽다.
2차 시스템은 입력이 가속도를 만드는 경우다. \(\ddot x = f(x) + g(x)\,u\) 형태로, 힘/토크 제어가 대표적이다. 위치로 정의된 \(h\)를 한 번 미분한 \(\dot h\)에는 아직 입력이 없고, 두 번 미분한 \(\ddot h\)에서야 입력이 등장한다. 이때는 식 (3)을 그대로 쓸 수 없고 High-Order CBF(HOCBF)나 Exponential CBF 같은 확장이 필요하다.
요약하면 1차 시스템은 "입력 = 속도, relative degree = 1"인 가장 단순한 경우이고, 그래서 CBF 조건이 단 한 번의 미분으로 입력에 대한 선형 부등식까지 깔끔하게 떨어진다.
이 조건을 읽는 방법이 직관의 핵심이다. \(h(x)\)가 크면(경계에서 멀면) \(\dot{h}\)가 꽤 음수여도 부등식이 성립한다. 즉 안전 여유가 많을 때는 경계 쪽으로 빠르게 움직여도 된다. 반대로 \(h(x)\)가 0에 가까워지면(경계에 붙으면) \(\dot{h} \geq 0\)이 강제된다. 경계에서는 안전 마진이 더 이상 줄어들 수 없도록 변화율 자체가 막히는 것이다. 이것이 \(h(x)=0\) 표면을 절대 통과하지 못하게 하는 메커니즘이다.
관절 위치 \(q\)를 상한 \(q_{max}=90°\) 아래로 유지하고 싶다고 하자. 안전 함수를 h(q) = q_max − q 로 잡으면, 1차 시스템 \(\dot q = u\)에서 CBF 조건 \(\dot h + \alpha h \ge 0\)은 곧 u ≤ α·(q_max − q) 가 된다. 즉 허용되는 최대 속도가 경계에 다가갈수록 선형으로 0까지 줄어든다. 슬라이더로 직접 확인해 보자.
최적화 기반 제어에서 CBF 조건은 제어 입력에 대한 부등식 제약으로 바뀐다. 로봇 시스템의 속도 단계(velocity-level) 또는 증분(increment) 제어에 적용하면, 관절 속도 \(\dot q\)에 대한 선형 제약 형태가 된다.
좌변이 \(\dot q\)에 대해 선형이므로, 이 조건은 QP(이차계획법) 안의 선형 부등식 제약 하나로 그대로 들어간다. 매 제어 주기마다 실시간으로 풀 수 있다.
"원하는 동작에 최대한 가깝게 \(\dot q\)를 정하되, 이 부등식을 위반하지 마라"는 형태로 푼다. 안전이 보장된 가장 가까운 명령을 매번 계산하는 셈이다.
CBF가 retargeting에 잘 맞는 이유는 위 세 가지가 맞물리기 때문이다. 안전 제약을 스칼라 함수 \(h(x)\)로 압축하고(Eq. 2), 경계를 넘지 않는 조건을 부과하고(Eq. 3), 그것이 관절 속도에 대한 선형 제약으로 떨어진다(Eq. 4). 결과적으로 사람 손 동작을 로봇 손에 매핑하는 빠른 루프 안에 형식적 안전 보장을 직접 끼워 넣을 수 있다.
관절 한계까지의 마진, 손가락 링크 간 자기충돌(self-collision) 거리, 손바닥/물체와의 침투 방지 거리 등을 \(h(x)\)로 잡으면, 텔레오퍼레이션 입력이 위험한 자세를 요구해도 제어기가 경계 직전에서 명령을 부드럽게 깎아낸다. 하드 클리핑과 달리 \(\alpha\)로 반응 강도를 조절할 수 있다는 점이 실용적 장점이다.
정리하면, CBF는 "안전 영역을 정의하고 → 그 경계를 forward invariant하게 만드는 조건을 세우고 → 그것을 실시간 풀 수 있는 선형 제약으로 환원"하는 세 단계로 요약된다. 제어 성능(추종 정확도)과 안전(제약 위반 방지)을 하나의 최적화 문제 안에서 동시에 다룰 수 있다는 것이 CBF의 핵심 가치다.
QP(식 7)는 작업 목표와 물리적 제약을 고주파 제어 아래에서 통합적으로 다루지만, 그 자체로는 실행 내내 안전 제약이 지켜진다는 보장을 주지 못한다. 기존의 비선형 최적화 기반 retargeting은 보통 거리/충돌 관련 페널티를 목적함수에 더하는 방식으로 충돌을 회피한다. 문제는 이것이 본질적으로 soft constraint라는 점이다.
충돌 위험을 목적함수의 가중치로 누른다. 안전 성능이 heuristic한 weight 튜닝에 크게 의존하고, 결과적으로 충돌을 확률적·경험적으로 완화할 뿐 형식적 보장을 주지 못한다.
안전 집합의 forward invariance를 증명 가능한 형태(provable)로 강제한다. 가중치가 아니라 hard inequality 제약이므로, 실행 내내 \(h(q) \ge 0\)이 수학적으로 유지된다.
그래서 우리는 CBF를 retargeting 정식화에 직접 결합한다. 목표는 실행 중 안전 집합 \(\mathcal{S}\)의 forward invariance를 증명 가능하게 보장하는 것이다.
로봇 링크와 환경 장애물을 기하 프리미티브(geometric primitive)의 집합으로 근사한다. 구체적으로는 캡슐(capsule) 기반 충돌 모델을 쓴다. 충돌 가능성이 있는 두 물체 \(A\), \(B\)에 대해, 각자의 골격 선분(skeletal segment) 위에서 가장 가까운 점(witness point) \(p_A(q),\, p_B(q) \in \mathbb{R}^3\)을 잡는다. 안전 함수는 두 witness point 사이 거리에서 두 캡슐 반지름의 합을 뺀 값으로 정의된다.
장애물 B를 회전시키면 두 골격 선분 사이의 최근접점(witness point) p_A, p_B가 선분 위(끝의 둥근 부분 포함)를 이동한다. 그에 따라 ‖p_A − p_B‖가 바뀌고, 표면 간격 h(q) = ‖p_A − p_B‖ − (r_A + r_B)도 함께 변한다. B를 가까이 옮긴 뒤 회전시키면 h가 0을 지나 음수(충돌)로 떨어지는 지점을 직접 만들 수 있다.
이제 \(\mathcal{S}\)의 forward invariance를 보장하기 위해 조건 \(\dot h(q) \ge -\gamma\, h(q)\)를 부과한다. 여기서 \(\gamma > 0\)는 시스템을 안전 경계에서 얼마나 빠르게 밀어내는지를 조절한다.
식 (3)에서 네 단계 변형으로 그대로 나온다. 새로운 가정은 없고, 일반 상태 \(x\)를 관절 형상 \(q\)로 특수화하고 상수 이름만 바꾼 것이다.
여기서 \(x \to q\)는 의미상의 치환이다. 식 (3)의 상태 \(x \in \mathbb{R}^n\)가 이 문제에서는 곧 관절 형상 \(q \in \mathbb{R}^n\)이고, \(h\)는 그 위에 정의된 안전 함수다. \(\alpha\)와 \(\gamma\)는 둘 다 "경계에서 얼마나 세게 밀어낼지"를 정하는 양수 상수로 역할이 완전히 같아서, 기호만 바꿔 쓴 것이다.
그럼 왜 하필 "지금" 부과하는가. 섹션 06에서 충돌을 뜻하는 구체적인 \(h(q)\)(캡슐 표면 거리, 식 8)와 안전 집합 \(\mathcal{S}\)를 정의했다. 이제 목표는 그 \(\mathcal{S}\)를 실행 내내 벗어나지 않게(forward invariant) 만드는 것이고, 그 도구가 바로 일반 CBF 조건이다. 거기에 우리의 \(h(q)\)를 끼워 넣은 결과가 위 식이다. 추상적인 식 (3)에 충돌 회피라는 구체적 의미를 입힌 셈이다.
읽는 법은 섹션 02와 똑같다. 경계(\(h=0\), 표면이 막 닿기 직전)에서는 \(\dot h \ge 0\)이 강제되어 거리가 더 줄 수 없다(충돌 불가). 멀리 있을 때(\(h\)가 큼)는 \(\dot h\)가 꽤 음수여도 부등식이 성립해 자유롭게 다가갈 수 있다. \(\gamma\)가 클수록 경계에서 더 일찍·세게 감속한다.
위 조건을 강제하려면 \(h(q)\)의 시간 미분이 필요하다. 이는 미분 기구학(differential kinematics)과 Danskin 정리로 얻는다. 핵심 결과부터 말하면, \(\dot h\)는 관절 속도 \(\dot q\)에 대해 선형이며, 그 계수가 곧 distance Jacobian \(J_{dist}\)다.
자코비안은 "관절을 움직이면 어떤 점이 어디로 움직이는가"를 환산해 주는 표다. 관절 속도 \(\dot q\)(여러 개)를 넣으면 그 점의 공간 속도 \(\dot p\)(3차원)가 나온다. 곧 \(\dot p = J\,\dot q\). 다이얼 여러 개를 돌릴 때 바늘이 어느 방향으로 얼마나 움직이는지 알려주는 변환기라고 보면 된다. 식 (2)·(3)의 point Jacobian \(J_{v,A}, J_{v,B}\)가 바로 이것이다.
distance Jacobian \(J_{dist}\)는 거기서 한 걸음 더 간다. 두 점의 3차원 속도 중 "두 물체가 가까워지거나 멀어지는 성분"만 뽑는다. 그래서 출력이 3차원이 아니라 숫자 하나(거리 변화율 \(\dot h\))이고, \(J_{dist}\)는 \(1 \times n\) 짜리 행벡터가 된다. 각 성분은 "이 관절을 단위 속도로 돌리면 표면 간격이 초당 얼마나 변하는가"를 뜻한다. 예를 들어 \(i\)번째 성분이 \(+2\,\mathrm{cm/rad}\)이면, \(i\)번 관절을 \(+1\,\mathrm{rad/s}\)로 돌릴 때 간격이 초당 2cm씩 늘어난다(멀어진다)는 의미다.
"가까워지는 성분만 뽑는다"가 곧 법선 \(\hat n\)에 대한 투영 \(\hat n^{\top}(\dot p_A - \dot p_B)\)이다. 두 최근접점을 잇는 직선 방향의 움직임만 거리를 바꾸고, 그에 수직인(옆으로 스치는) 움직임은 1차로는 거리를 바꾸지 않기 때문이다. 위 회전 다이어그램에서 B를 옆으로 스치듯 돌릴 때보다 정면으로 다가갈 때 \(h\)가 더 빠르게 줄어든 것이 같은 이야기다.
세 단계를 하나씩 풀어 보자. 목표는 줄곧 하나다 — \(\dot h\)를 \(\dot q\)에 대한 선형식으로 만드는 것이다.
1단계 · 왜 단위 법선부터 구하나. \(h(q) = \lVert p_A - p_B\rVert - (r_A+r_B)\)에서 상수 \((r_A+r_B)\)는 미분하면 0이라, \(h\)의 변화율은 곧 두 점 사이 거리 \(\lVert p_A - p_B\rVert\)의 변화율과 같다. 그런데 노름(거리)을 미분하면 단위 벡터가 튀어나온다. \(\partial \lVert v\rVert / \partial v = v/\lVert v\rVert\)이고, 여기서 \(v = p_A - p_B\)이므로 그 단위 벡터가 바로 \(\hat n\)이다. 즉 \(\hat n\)은 "\(p_A\)를 어느 방향으로 밀면 거리가 가장 빨리 늘어나는가"를 가리키는 거리의 gradient 방향이고, 다음 단계에서 속도를 투영할 축이 된다.
2단계 · point Jacobian이 \(\dot q\)를 끌어들인다. witness point \(p_A\)는 로봇 링크 위의 한 점이고, 관절이 움직이면 공간에서 함께 움직인다. 그 점의 위치가 형상 \(q\)의 함수이므로 시간으로 미분하면 \(\dot p_A = \dfrac{\partial p_A}{\partial q}\dot q = J_{v,A}\,\dot q\)가 된다. 이 \(J_{v,A} \in \mathbb{R}^{3\times n}\)가 point Jacobian이다. 행 3개\((x,y,z)\), 열 \(n\)개(관절)이고, \(j\)번째 열은 "\(j\)번 관절만 단위 속도로 움직일 때 점 A가 공간에서 어디로 가는가"를 담는다. 결정적인 점은 \(\dot p_A\)가 \(\dot q\)에 대해 선형이라는 것이다. Jacobian은 현재 \(q\)에서 값이 고정된 행렬이고, 거기에 \(\dot q\)만 곱해지기 때문이다. (장애물 B가 정지해 있으면 \(J_{v,B}=0\), B도 움직이는 링크면 자기 Jacobian을 가진다.)
3단계 · 연쇄법칙으로 합치면 행벡터 하나가 남는다. 구하려는 것은 \(\dot h = \dfrac{d}{dt}h(q(t)) = \dfrac{\partial h}{\partial q}\dot q\)다. 거리의 시간 미분을 1단계의 노름 미분 공식으로 쓰면 \(\dfrac{d}{dt}\lVert p_A - p_B\rVert = \hat n^{\top}(\dot p_A - \dot p_B)\)이다(\(\hat n\)이 투영 축으로 등장). 여기에 2단계 결과 \(\dot p_A = J_{v,A}\dot q,\ \dot p_B = J_{v,B}\dot q\)를 대입하면 \(\dot h = \hat n^{\top}(J_{v,A} - J_{v,B})\,\dot q = J_{dist}\,\dot q\). 1단계의 투영 축과 2단계의 속도 사상이 합쳐져, \(\dot q\) 앞에 곱해지는 \(1\times n\) 행벡터 \(J_{dist} = \hat n^{\top}(J_{v,A}-J_{v,B})\) 하나로 압축된 것이다.
결국 \(\dot h = J_{dist}\,\dot q\)는 \(\dot q\)에 대해 완전히 선형이다. 이것이 이 모든 과정의 목적이다. 선형이어야 CBF 조건 \(\dot h \ge -\gamma h\)가 \(\dot q\)(이산화 후엔 증분 \(\Delta q\))에 대한 선형 부등식이 되고, 그래야 다음 섹션처럼 convex QP에 그대로 들어가 실시간으로 풀린다. 만약 \(\dot h\)가 \(\dot q\)의 비선형 함수였다면 이 접근 전체가 무너진다.
여기서 한 가지 짚을 점이 있다. \(h(q)\)는 사실 최소화 문제의 해(value function)다. 두 골격 선분 위의 모든 점쌍 중 가장 가까운 거리를 고른 값이기 때문이다.
문제는 미분이다. \(p_A, p_B\)가 \(q\)에 의존하므로, \(\dot h\)를 구할 때 원칙적으로는 두 항을 모두 미분해야 한다. 하나는 물체가 움직여 생기는 항이고, 다른 하나는 최근접점 자체가 선분 위에서 미끄러져 생기는 항(argmin의 이동, \(\partial p / \partial q\))이다. 뒤쪽 항은 닫힌 형태로 쓰기 어렵고 계산도 무겁다.
한 줄 요약 — 최솟값 함수를 미분할 때, 최소점(witness point)이 \(q\)에 따라 움직이는 효과는 무시하고 그 점을 고정된 것처럼 미분해도 결과가 같다는 보장. 포락선 정리(envelope theorem)의 한 형태다.
성립하는 이유는 1차 최적성(first-order optimality)이다. 최근접점에서 점을 자기 선분 방향으로 미세하게 밀어도 거리는 1차로 변하지 않는다(그 방향이 거리의 극소점이라 기울기가 0이기 때문). 즉 witness point의 "선분 위 미끄러짐"은 거리에 1차로 기여하지 않고, 두 물체의 강체 운동만 법선 \(\hat n\) 방향으로 투영되어 남는다. argmin 이동 항이 통째로 사라지는 것이다.
형식으로 쓰면, 최소점에서의 편미분을 그대로 평가하는 것으로 충분하다.
덕분에 \(p_A, p_B\)를 각 물체에 고정된 물질점으로 보고 point Jacobian으로 속도만 구한 뒤 \(\hat n\)에 투영하면 끝난다. 매 스텝 무거운 argmin 미분을 풀 필요 없이 \(J_{dist}\)가 닫힌 형태로 나온다.
주의할 점도 있다. Danskin 정리는 최소점이 유일할 때 깔끔하게 성립한다. witness point가 한 끝점에서 다른 곳으로 갑자기 바뀌거나(끝점 전환) 최소점이 여러 개인 형상에서는 \(h(q)\)가 그 순간 미분 불가능해지고 \(J_{dist}\)가 불연속이 될 수 있다. 캡슐 모델에서는 부드러운 근접 거리나 약간의 안전 마진을 두어 이런 특이점을 실용적으로 회피한다.
고주파 텔레오퍼레이션에서는 1차 이산화 \(\dot q \approx \Delta q_t / \Delta t\)를 적용한다. 그러면 forward invariance 조건이 결정변수 \(\Delta q_t\)에 대한 선형 부등식 하나로 떨어진다.
지금까지는 시간이 연속으로 흐른다고 보고 순간 속도 \(\dot q\)를 다뤘다. 하지만 실제 제어기는 매 프레임마다(예: 1ms 간격 \(\Delta t\)) 한 번씩 명령을 내린다. 그래서 순간 속도를 "이번 프레임에 관절을 얼마나 움직일까"인 증분 \(\Delta q_t\)로 바꿔 쓴다. \(\dot q \approx \Delta q_t / \Delta t\)가 그 환산이고, 매 프레임 풀어야 할 미지수가 바로 이 \(\Delta q_t\)다.
이 치환을 \(\dot h \ge -\gamma h\)에 넣으면 그대로 \(\Delta q_t\)에 대한 부등식이 된다. 연속시간의 \(\gamma\)는 \(\tilde\gamma = \gamma\,\Delta t\)로 흡수될 뿐, 의미는 같다(경계에서 얼마나 세게 밀어낼지).
유도는 섹션 06·07의 결과를 이어 붙이는 것이다. 연속시간 CBF 조건에서 출발해 \(\dot h\)를 \(J_{dist}\dot q\)로 바꿔치우고, 이산화 한 번이면 \(\Delta q_t\)에 대한 affine 부등식이 나온다.
여기서 한 가지를 짚자. \(J_{dist}\)와 \(h\)를 직전 형상 \(q_{t-1}\)에서 평가한다는 점이다. 시각 \(t\)에서 모르는 것은 이번 증분 \(\Delta q_t\)뿐이고, 현재 자세 \(q_{t-1}\)는 이미 측정·계산되어 있다. 그래서 \(J_{dist}(q_{t-1})\)과 \(h(q_{t-1})\)은 그 순간 상수가 되고, 부등식은 미지수 \(\Delta q_t\)에 대해서만 1차로 남는다. 이것이 "affine"의 정체다 — 계수가 상수인 1차 부등식.
마이너스 부호의 정체부터 보자. QP의 부등식 제약은 보통 \(A\,\Delta q_t \le b\)(작거나 같음) 꼴로 표준화한다. 그런데 위 affine 제약은 \(\ge\) 방향이라, 양변에 \(-1\)을 곱해 뒤집는다 — \(-J_{dist}(q_{t-1})\,\Delta q_t \le \tilde\gamma\, h(q_{t-1})\). 그래서 stacked 행렬의 CBF 줄이 \(-J_{dist}\)로 들어가는 것이다. 부호는 "\(\ge\)를 \(\le\)로 바꾼" 흔적일 뿐 의미는 그대로다.
관절 한계 줄 \(A_{jl}, b_{jl}\)도 같은 식으로 만든다. 한계 \(q_{\min} \le q_{t-1} + \Delta q_t \le q_{\max}\)를 미지수 \(\Delta q_t\)에 대한 상·하한 \(\le\) 부등식으로 옮기면 두 줄이 나온다(\(\Delta q_t \le q_{\max}-q_{t-1}\), \(-\Delta q_t \le q_{t-1}-q_{\min}\)). 마지막으로 관절 한계 줄과 CBF 줄을 위아래로 쌓아 \(A = [A_{jl};\,-J_{dist}]\), \(b = [b_{jl};\,\tilde\gamma\, h(q_{t-1})]\)로 묶으면, 매 프레임 푸는 \(A\,\Delta q_t \le b\) 한 덩어리가 완성된다.
QP(이차계획법, Quadratic Program) — 2차식 목적함수를 최소화하면서 여러 선형 부등식을 모두 만족시키는 최적화 문제. 여기서 목적함수는 "사람 손이 원하는 동작에서 로봇 손이 얼마나 벗어났는가"의 제곱이고, 부등식은 관절 한계와 CBF 안전 제약이다. 즉 위반 없이 가장 가까운 동작을 매 프레임 찾는 문제다.
affine(아핀) — \(\Delta q_t\)에 대해 1차(직선/평면) 형태라는 뜻으로, 제곱이나 곱 같은 비선형 항이 없다. CBF 조건이 affine으로 떨어졌기 때문에 QP의 깔끔한 선형 부등식으로 그대로 들어갈 수 있다.
convex(볼록) — 실행 가능 영역이 "그릇 모양"이라는 뜻이다. 그릇 안에서는 국소 최소가 곧 전역 최소라 풀이가 헤매지 않고 빠르고 확실하게 수렴한다. affine 부등식 하나가 만드는 영역(반평면)은 항상 convex이고, 그것들의 교집합도 convex다. 그래서 제약을 아무리 쌓아도 문제가 convex QP로 유지된다.
stack(적층) — 행렬을 위아래로 쌓는 것은 단순히 "이 부등식들을 전부 동시에 지켜라"를 한 줄 \(A\,\Delta q_t \le b\)로 묶는 표기다. \(A_{jl}\)은 관절 한계 줄, \(-J_{dist}\)는 CBF 줄이다. 새 제약이 생기면 줄을 더 쌓기만 하면 된다.
목적함수와 모든 제약이 convex로 유지되므로, 결과는 표준 convex QP다. 실행 가능 집합(feasible set)은 convex 집합들의 교집합이 된다. 이 정식화가 주는 것은 기하학적 안전을 형식적으로 보장하면서도 실시간 풀이 가능성을 유지한다는 점이다.
affine 제약 → convex QP. 고주파 실행에서도 실시간으로 풀리고, 기하학적 안전이 증명 가능하다.
CBF 제약을 비선형 정식화에 직접 넣으면 고차원 비선형 부등식이 생긴다. 계산 비용이 폭증해 고주파 실행이 비현실적이 된다.