로봇공학 및 자동제어
Robotics
로봇공학 2단계: 칼만 필터, 기구학, 동역학 — 불완전한 세상에서 정확하게 움직이기
▌ 학습에 들어가기 전에
먼저 1단계에서 배운 것을 복기하자. 1단계에서 너는 PID 제어기로 오차를 자동 교정하고, 상태 공간(State Space) 표현으로 시스템 내부 상태를 행렬로 기술했으며, IMU 센서와 모터 드라이버를 직접 연결해봤다. 2단계는 그 위에 세워지는 건물이다. 1단계가 "어떻게 제어할 것인가?"를 다뤘다면, 2단계는 두 가지 질문을 동시에 던진다: "제어 대상의 현재 상태를 얼마나 정확히 알 수 있는가?" 그리고 "로봇이 원하는 곳으로 어떻게 움직여야 하는가?"
PART 1: 이론적 기초 — 세상은 노이즈로 가득 차 있다
불확실성과 확률: 세계는 가우시안으로 이루어졌다
아마 너는 물리 시간에 "마찰이 없다면", "공기 저항을 무시하면"이라는 말을 수도 없이 들었을 것이다. 그 이상적인 세계에서 모든 값은 정확하다. 하지만 실제 로봇 공학에서 센서는 절대로 완벽한 값을 주지 않는다. GPS는 수 미터의 오차가 있고, 1단계에서 직접 다뤄본 IMU는 시간이 지날수록 **적분 드리프트(Integration Drift)**가 쌓인다. 각속도를 계속 적분해 각도를 구하면 오차가 눈덩이처럼 불어나는 그 현상이다. 이것이 2단계가 필요한 이유다.
센서 오차를 수학적으로 다루기 위해 우리는 **확률(Probability)**을 도구로 사용한다. 어떤 센서가 실제 온도 20°C를 측정할 때 항상 정확히 20°C를 출력한다면 이상적인 센서다. 현실 센서는 18.7°C, 21.2°C, 19.9°C처럼 실제 값 주위로 흩어진 값들을 뱉는다. 이 흩어짐의 패턴이 가우시안 분포(Gaussian Distribution), 즉 정규분포다.
[노트 기록] 가우시안 분포: N(μ, σ²) — μ(평균, 참값에 해당)와 σ²(분산, 불확실성의 크기)로 완전히 정의된다. 칼만 필터는 세상의 모든 불확실성이 가우시안 분포를 따른다고 가정하며, 이 가정이 성립할 때 칼만 필터는 수학적으로 **최적(Optimal)**이다.
왜 가우시안인가? 이건 단순한 편의가 아니다. **중심 극한 정리(Central Limit Theorem)**에 의하면, 아주 많은 독립적인 작은 오차들의 합은 원래 분포가 무엇이든 간에 결국 가우시안에 수렴한다. 수십 개의 전자 잡음, 기계적 진동, 온도 변화가 합쳐진 센서 오차가 가우시안에 가까운 건 우연이 아니다.
행렬과 좌표 변환: 로봇의 언어
로봇 팔이 움직인다는 것은 수학적으로, **공간 상의 점(Point)**이나 **물체의 자세(Pose)**가 변한다는 뜻이다. 고등학교 수학에서 벡터와 행렬을 배웠을 것이다. 로봇 공학에서 이 행렬은 **좌표 변환(Coordinate Transformation)**의 핵심 도구가 된다. 2D에서 점 (x, y)를 각도 θ만큼 회전시키면:
x' = x·cos(θ) - y·sin(θ)
y' = x·sin(θ) + y·cos(θ)
행렬로 쓰면 [x'; y'] = R(θ) · [x; y] 이고, 이 R이 **회전 행렬(Rotation Matrix)**이다. 3D에서는 x, y, z축 회전이 각각 존재하며, 이것들을 조합해 SE(3) — 3차원 특수 유클리드 군(Special Euclidean Group)이라 불리는 변환 그룹을 만든다. 더 편리한 표현을 위해 회전과 이동(Translation)을 하나의 4×4 행렬로 합치는 **동차 변환 행렬(Homogeneous Transformation Matrix)**을 사용한다:
T = [ R t ] (R: 3×3 회전, t: 3×1 이동)
[ 0 1 ]
[노트 기록] 3D 공간에서 물체의 **자세(Pose)**는 위치(Position) 3개 + 방향(Orientation) 3개 = 총 **6 자유도(6 DOF)**로 표현된다. 1단계의 상태 공간 행렬처럼, 이 T 행렬 하나가 로봇 관절 하나의 "움직임"을 완전히 기술한다.
강체 운동: 물체가 어떻게 움직이는가
뉴턴의 F=ma는 질점(Point Mass)에 대한 법칙이다. 하지만 로봇 팔은 강체(Rigid Body) — 내부 형태가 변하지 않는 물체다. 강체는 움직일 때 위치 변화(병진)와 방향 변화(회전)가 동시에 일어난다. 특히 중요한 개념은 **관성 모멘트(Moment of Inertia)**다. 질량이 선형 가속도에 저항하는 것처럼, 관성 모멘트는 **각가속도(Angular Acceleration)**에 저항한다. 같은 토크로도 긴 봉과 짧은 봉이 다르게 회전하는 이유가 여기 있다. 3D에서 관성 모멘트는 스칼라가 아니라 관성 텐서(Inertia Tensor), 즉 3×3 행렬이 되며, 이것이 나중에 동역학 방정식에서 **질량 행렬(Mass Matrix)**로 등장한다.
PART 2: 칼만 필터와 센서 데이터 융합
베이즈 추론: "내 예측"과 "센서 값"을 어떻게 섞을까?
잠깐 생각해보자. 눈을 감고 방 안을 걷는다면 위치를 어떻게 알 수 있을까? 걸음 수를 세고 방향을 기억해 "지금쯤 여기 있을 것"이라고 예측할 수 있다. 그런데 발에 벽이 닿았다. 이때 걸음 수 예측과 벽의 위치를 합쳐서 "내가 여기 있구나"라고 업데이트한다. 바로 이 과정이 **베이즈 추론(Bayesian Inference)**의 직관이다. 수식으로는 P(상태|측정값) ∝ P(측정값|상태) × P(상태) — 이것이 **베이즈 정리(Bayes' Theorem)**다. P(상태)는 사전 믿음(Prior), P(측정값|상태)는 우도(Likelihood), P(상태|측정값)는 사후 믿음(Posterior)이다. 칼만 필터는 이 베이즈 추론을 가우시안 분포로 제한했을 때 얻어지는 닫힌 형태(Closed-form)의 최적 해법이다. Rudolf Kálmán은 1960년 논문 *"A New Approach to Linear Filtering and Prediction Problems"*에서 이를 발표했다.
상태 공간 모델: 세상을 방정식으로
1단계에서 배운 상태 공간 표현 ẋ = Ax + Bu 를 기억하는가? 칼만 필터는 동일한 프레임 위에 노이즈를 추가한다. 프로세스(시스템) 모델: x_k = F·x_{k-1} + B·u_k + w_k. 측정 모델: z_k = H·x_k + v_k. 여기서 x_k는 상태 벡터(State Vector) (예: 위치와 속도), F는 상태 전이 행렬(State Transition Matrix), z_k는 측정 벡터(Measurement Vector), H는 측정 행렬(Measurement Matrix) — "상태 벡터에서 센서가 실제로 측정하는 부분이 무엇인가"를 정의한다. 그리고 두 노이즈: **w_k ~ N(0, Q)**는 프로세스 노이즈, **v_k ~ N(0, R)**는 측정 노이즈다. Q와 R은 **공분산 행렬(Covariance Matrix)**이며, 이 값들을 얼마나 잘 설정하느냐가 칼만 필터 성능을 결정한다.
[노트 기록] 두 핵심 노이즈의 직관: Q(프로세스 노이즈)가 클수록 "내 물리 모델을 믿지 말고 측정값을 더 신뢰해라"는 뜻이고, R(측정 노이즈)이 클수록 "센서가 형편없으니 내 예측을 더 믿어라"는 뜻이다.
칼만 필터 알고리즘: Predict → Update
칼만 필터는 두 단계를 계속 반복한다. 마치 "예측하고, 확인하고, 수정하는" 과학적 방법론처럼. **예측 단계(Predict)**에서는 이전 상태와 물리 모델로 현재 상태를 예측한다 — 눈 감고 걸음 수 세는 것. 불확실성(P)은 당연히 커진다.
예측된 상태: x̂_{k|k-1} = F · x̂_{k-1|k-1} + B · u_k
예측된 오차 공분산: P_{k|k-1} = F · P_{k-1|k-1} · F^T + Q
**갱신 단계(Update)**에서는 실제 센서 측정값 z_k를 받아 예측을 수정한다 — 발이 벽에 닿는 순간.
혁신(Innovation): ỹ_k = z_k - H · x̂_{k|k-1}
혁신 공분산: S_k = H · P_{k|k-1} · H^T + R
칼만 게인: K_k = P_{k|k-1} · H^T · S_k^{-1}
상태 업데이트: x̂_{k|k} = x̂_{k|k-1} + K_k · ỹ_k
공분산 업데이트: P_{k|k} = (I - K_k · H) · P_{k|k-1}
[노트 기록] 칼만 게인(Kalman Gain) K의 핵심: 분자는 예측의 불확실성(P), 분모는 전체 불확실성(H·P·H^T + R). 예측이 불확실할수록 K가 커져 측정값에 더 의존한다. K → 0이면 측정 무시, K → H^{-1}이면 예측 버리고 측정만 신뢰. 이 가중치가 가우시안 분포 가정 하에서 수학적으로 최적임이 증명된다.
센서 융합: IMU + GPS를 하나로
실제 로봇에서는 여러 센서를 함께 쓴다. GPS는 절대 위치를 알려주지만 느리고(10Hz) 실내에서는 작동 안 한다. IMU는 빠르지만(1000Hz) 드리프트가 심하다. 칼만 필터에서 H 행렬을 적절히 설계하면 서로 다른 종류의 센서들을 수학적으로 통합할 수 있다 — 이것이 **센서 융합(Sensor Fusion)**이다. 선형 시스템에만 적용되는 칼만 필터가 비선형 시스템에서는 직접 적용되지 않는다. 이때 등장하는 **확장 칼만 필터(Extended Kalman Filter, EKF)**는 비선형 함수를 현재 추정점 주변에서 **야코비안(Jacobian)**으로 선형화한다. 야코비안은 잠시 후 기구학 부분에서 다시 등장한다 — 같은 수학 도구가 전혀 다른 문제에 똑같이 쓰인다는 점이 흥미롭다.
PART 3: 로봇 기구학 — 관절 각도와 끝점 위치의 변환
관절 공간과 작업 공간
로봇 팔을 제어하는 방법은 두 가지다. "1번 관절 30도, 2번 관절 45도로 설정"하는 것(관절 공간)과, "손을 (x=0.5, y=0.3, z=0.8) 위치로 옮겨"라고 명령하는 것(작업 공간). 인간의 뇌는 자연스럽게 작업 공간에서 목표를 설정하지만, 실제 모터에게는 관절 각도를 줘야 한다. **관절 공간(Joint Space)**은 모든 관절 각도 θ = [θ1, θ2, ..., θn]^T의 집합이고, **작업 공간(Task Space, Cartesian Space)**은 로봇 손(End-Effector)의 위치와 방향(최대 6D)이다. 두 공간 사이의 변환이 **기구학(Kinematics)**이며, 관절 → 위치 방향이 순기구학(Forward Kinematics, FK), 위치 → 관절이 **역기구학(Inverse Kinematics, IK)**이다.
순기구학: DH 파라미터와 변환 행렬의 연쇄
6축 로봇 팔은 각 관절마다 좌표계를 가진다. 이 좌표계들을 체계적으로 붙이는 방법이 Denavit-Hartenberg(DH) 파라미터다. Jacques Denavit과 Richard Hartenberg가 1955년에 발표한 이 체계는 4개의 파라미터로 인접한 두 관절 사이의 변환을 완전히 기술한다.
[노트 기록] DH 파라미터 4가지: θ_i (관절 회전 각도 — 회전 관절의 변수), d_i (z축 방향 이동 거리), a_i (링크 길이, x축 이동), α_i (링크 비틀림 각도, Twist). 각 관절의 변환 행렬: T_{i-1}^i = Rot_z(θ_i) · Trans_z(d_i) · Trans_x(a_i) · Rot_x(α_i). 전체 순기구학: T_0^n = T_0^1 · T_1^2 · ... · T_{n-1}^n.
2-DOF 평면 팔(링크 길이 L1, L2, 관절 각도 θ1, θ2)의 순기구학을 변환 행렬 연쇄로 계산하면 끝점 위치는:
x = L1·cos(θ1) + L2·cos(θ1 + θ2)
y = L1·sin(θ1) + L2·sin(θ1 + θ2)
T_0^1과 T_1^2를 직접 곱해서 이 결과가 나오는지 스스로 확인해볼 수 있겠는가?
역기구학: 해는 하나가 아니다
역기구학에는 두 가지 근본적인 어려움이 있다. 첫째, **해가 여러 개(Multiple Solutions)**다. 2-DOF 팔에서 같은 끝점 위치에 도달하는 방법이 두 가지다 — 팔꿈치가 위로 향하는(Elbow-Up) 자세와 아래로 향하는(Elbow-Down) 자세. 6-DOF 로봇에서는 무한히 많은 해가 있을 수 있다. 어떤 해를 선택할 것인가는 수학 문제가 아니라 최적화(Optimization) 문제다. 둘째, 해가 없을 수도 있다 — 목표가 로봇의 작업 공간(Workspace) 밖이거나 관절 각도 한계를 초과하면 물리적으로 불가능하다.
2-DOF 팔의 역기구학을 코사인 법칙으로 유도하면, x² + y² = L1² + L2² + 2L1L2·cos(θ2) 라는 관계로부터:
cos(θ2) = (x² + y² - L1² - L2²) / (2·L1·L2)
θ2 = ±atan2(√(1 - cos²(θ2)), cos(θ2))
±가 Elbow-Up/Down의 두 해다. θ2를 구했으면:
θ1 = atan2(y, x) - atan2(L2·sin(θ2), L1 + L2·cos(θ2))
[노트 기록] atan2(y, x): 일반 arctan과 달리 4사분면을 모두 구별하는 2-인수 역탄젠트. 결과는 (-π, π] 범위. 로봇 공학에서 각도 계산의 기본 도구다.
6-DOF 이상에서 해석적 해를 구하기 어렵거나 불가능할 때는 **수치적 방법(Numerical Methods)**을 사용한다. 그 핵심이 야코비안이다.
야코비안: 관절 속도와 끝점 속도의 연결
야코비안은 **미분 기구학(Differential Kinematics)**의 핵심이다. 관절 각속도 θ̇와 끝점 속도 ẋ 사이의 선형 변환: ẋ = J(θ) · θ̇. 2-DOF 팔에서:
J = [ ∂x/∂θ1, ∂x/∂θ2 ] = [ -L1·sin(θ1) - L2·sin(θ1+θ2), -L2·sin(θ1+θ2) ]
[ ∂y/∂θ1, ∂y/∂θ2 ] [ L1·cos(θ1) + L2·cos(θ1+θ2), L2·cos(θ1+θ2) ]
이 야코비안을 역(Inverse)으로 사용하면 수치적 역기구학이 된다: θ̇ = J^{-1}(θ) · ẋ_desired. 원하는 끝점 속도를 주면 필요한 관절 속도를 즉시 계산할 수 있다. 그런데 J가 **특이 행렬(Singular Matrix)**일 때 — 즉 det(J) = 0일 때 — 역행렬이 존재하지 않는다. 이것이 **특이점(Singularity)**이다. 로봇이 특정 자세에서 특정 방향으로 움직일 수 없는 구성이다. 이를 다루기 위해 유사 역행렬(Pseudo-inverse), 댐핑 최소자승법(Damped Least Squares, DLS) 등이 사용된다. 그리고 앞서 언급한 EKF의 야코비안과 정확히 같은 "비선형 함수의 미분으로 선형화"라는 수학 구조가 여기서도 등장한다.
PART 4: 동역학과 궤적 계획
로봇 동역학의 기본 방정식
기구학이 "어떤 위치에 있는가"를 다뤘다면, **동역학(Dynamics)**은 "얼마만큼의 힘/토크로 그 운동을 만드는가"를 다룬다. 다관절 로봇의 운동 방정식은:
M(q)·q̈ + C(q, q̇)·q̇ + G(q) = τ
여기서 **M(q)**는 관성 행렬(Inertia Matrix) (n×n, 항상 양정치 대칭) — 현재 자세 q에서 링크들의 관성을 표현. C(q, q̇)·q̇는 코리올리/원심력 항 — 관절들이 함께 움직일 때 발생하는 상호작용력으로 속도의 제곱에 비례. **G(q)**는 중력 벡터 — 현재 자세에서 중력이 각 관절에 가하는 토크. τ는 관절 토크 벡터 — 실제 모터가 내야 하는 힘.
[노트 기록] 직관: F=ma의 다관절 버전이다. M(q)·q̈가 "관성 × 가속도", C·q̇가 "관절 간 상호작용 힘", G(q)가 "중력 보상". τ를 제어하면 q̈를 원하는 대로 만들 수 있다.
이 방정식을 유도하는 두 가지 방법이 있다. **뉴턴-오일러법(Newton-Euler)**은 각 링크에 대한 힘/모멘트 평형을 재귀적으로 계산하며 계산 효율이 높다. **라그랑지안법(Lagrangian)**은 시스템의 운동 에너지 T와 퍼텐셜 에너지 V로부터 L = T - V를 정의하고 오일러-라그랑주 방정식 d/dt(∂L/∂q̇_i) - ∂L/∂q_i = τ_i를 적용한다. 이론 분석에는 라그랑지안이 더 선호된다.
1단계에서 배운 PID 제어는 오차를 보고 사후에 교정하는 피드백 제어다. 동역학 방정식으로부터 τ_ff = M(q)·q̈_d + C(q, q̇)·q̇ + G(q)처럼 원하는 가속도를 달성하는 토크를 미리 계산하는 것이 피드포워드(Feedforward) 제어다. 고성능 로봇은 피드포워드 + PID 피드백을 함께 사용한다.
궤적 계획: 두 점 사이를 어떻게 갈까
**궤적(Trajectory)**은 경로(Path, 어디를 통과하는가) + 시간 정보(언제 어디 있어야 하는가)다. 물리적 제약이 있다 — 관절 속도, 가속도, 토크 한계. 그리고 제어 품질 요건 — 갑작스러운 가속도 변화인 **저크(Jerk)**는 기계적 충격과 진동을 유발한다.
가장 기본적인 3차 다항식(Cubic Polynomial) 궤적: q(t) = a0 + a1·t + a2·t² + a3·t³. 경계 조건 4개 — q(0)=q0, q(tf)=qf, q̇(0)=0, q̇(tf)=0 — 으로 계수를 구하면:
a0 = q0, a1 = 0
a2 = 3(qf - q0)/tf²
a3 = -2(qf - q0)/tf³
[노트 기록] 왜 3차인가? 4개의 경계 조건을 만족시키려면 최소 3차 다항식이 필요하다. 하지만 3차 다항식은 가속도(q̈)가 t=0과 t=tf에서 불연속이다 — 저크가 불연속적으로 급변한다.
**5차 다항식(Quintic Polynomial)**은 가속도 경계 조건 q̈(0)=0, q̈(tf)=0를 추가해 더 부드럽다: q(t) = a0 + a1·t + a2·t² + a3·t³ + a4·t⁴ + a5·t⁵. 경계 조건 6개 → 계수 6개의 선형 시스템으로 풀린다. Flash와 Hogan(1985)은 "The coordination of arm movements" (Journal of Neuroscience)에서 인간 팔 움직임이 저크를 최소화한다는 것을 발견했으며, 이것이 자연스러운 로봇 동작 생성의 기준이 됐다. 이 **최소 저크 궤적(Minimum Jerk Trajectory)**이 바로 위의 5차 다항식과 같은 형태로 표현된다.
여러 경유점(Via Points)을 통과할 때는 각 구간 다항식을 연결하는 **스플라인(Spline)**을 사용한다. 연결점에서 위치, 속도, 가속도의 **연속성(Continuity)**이 보장되어야 부드러운 전체 궤적이 만들어진다.
PART 5: 프로젝트 — 직접 만들고 증명하라
이제 배운 것을 직접 구현할 시간이다. 각 프로젝트는 문제만 제시한다. 직접 수식을 유도하고, 코드를 짜고, 결과를 분석하라. 막히는 지점이 있다면 해당 수식으로 돌아가 다시 읽어라 — 답은 이미 위의 본문 안에 있다.
Project 1: 1D 칼만 필터로 위치 추정하기
배경: 1D 직선 위를 등속으로 움직이는 로봇이 있다. 실제 속도는 정확히 1.0 m/s, 시간 간격 Δt = 0.1s. 위치를 측정하는 센서에 가우시안 노이즈가 있다. 상태 벡터는 x = [위치; 속도].
주어진 값: Q = [[0.001, 0], [0, 0.001]], R = [[1.0]], x̂_0 = [0; 1.0], P_0 = I (2×2 단위 행렬).
측정 데이터 생성 방법: k = 0, 1, ..., 99 타임스텝에서 실제 위치 p_true(k) = 1.0 × k × 0.1. 측정값 z_k = p_true(k) + N(0, 1.0)의 난수.
문제 1-1: 등속 운동 모델에서 상태 전이 행렬 F (2×2)와, 센서가 위치만 측정한다는 측정 행렬 H (1×2)를 명시적으로 써라. (힌트: p_k = p_{k-1} + v·Δt, v_k = v_{k-1})
문제 1-2: 100 타임스텝 동안 칼만 필터 Predict → Update를 반복 수행하는 코드를 작성하라 (Python/NumPy 권장). 각 타임스텝의 추정값 x̂를 저장하라.
문제 1-3: 실제 위치(True), 노이즈 있는 측정값(Measurement), 칼만 필터 추정값(Estimated)을 같은 그래프에 그려라. 칼만 필터가 노이즈를 얼마나 줄이는가?
문제 1-4 (탐구): R을 0.1로 줄이면(센서 더 정확) 추정이 어떻게 바뀌는가? Q를 10으로 늘리면(모델 더 불확실) 어떻게 바뀌는가? 그 이유를 칼만 게인 K의 변화 관점에서 설명하라.
Project 2: 2-DOF 로봇 팔 기구학 구현
배경: L1 = 1.0m, L2 = 0.8m인 2-DOF 평면 로봇 팔. 관절 각도 범위 θ1, θ2 ∈ [-π, π].
문제 2-1 (순기구학): θ1 = 30°, θ2 = 45°일 때 끝점 (x, y)를 수식으로 계산하고, 코드로 검증하라.
문제 2-2 (역기구학 유도): 끝점 목표 (x_d, y_d)가 주어졌을 때 θ2와 θ1을 구하는 공식을 코사인 법칙으로부터 스스로 유도하라. Elbow-Up(+), Elbow-Down(-) 두 가지 해를 모두 도출하라.
문제 2-3 (역기구학 검증): 목표 위치 (0.8, 0.6)에 대해 두 해를 수치적으로 계산하고, FK(IK 결과)가 원래 목표와 일치하는지 확인하라. 일치하지 않으면 어디가 틀렸는가?
문제 2-4 (야코비안): θ1 = 30°, θ2 = 45°에서 J를 계산하라. 이 자세에서 끝점을 ẋ = [0.1, 0.0] m/s로 움직이려면 관절 속도 [θ̇1, θ̇2]^T가 얼마여야 하는가?
문제 2-5 (작업 공간 시각화): θ1을 0°~180°, θ2를 -180°~180°까지 고르게 샘플링해 각 조합의 끝점 (x, y)를 플롯하라. 이것이 이 로봇의 **작업 공간(Workspace)**이다.
문제 2-6 (특이점 탐구): 끝점이 (1.8, 0.0) 근처에 있을 때(두 링크를 거의 펴는 자세), θ2 ≈ 0 근방에서 det(J)를 수치적으로 계산해보라. det(J) = 0이 되는 조건이 무엇인가? 그 자세에서 물리적으로 어떤 일이 생기는지 설명하라.
Project 3: 관절 공간 궤적 계획
배경: 위의 2-DOF 팔. θ1: 0° → 90°, θ2: 0° → −45°. 이동 시간 tf = 2.0초.
문제 3-1 (3차 다항식): θ1과 θ2 각각에 대해 3차 다항식 계수 a0~a3을 본문의 공식으로 계산하라. tf = 2.0초, 시작/끝 속도 모두 0.
문제 3-2 (5차 다항식): θ̈(0)=0, θ̈(tf)=0 조건을 추가해 5차 다항식을 구하는 6×6 선형 시스템을 세워라. 이를 np.linalg.solve(A, b)로 풀어라. (힌트: 6개의 경계 조건을 6×6 행렬 방정식으로 표현하라. A의 각 행은 t=0 또는 t=tf를 대입한 다항식 계수 벡터.)
문제 3-3 (비교 시각화): 3차와 5차 궤적을 같은 그래프의 3개 서브플롯 — (1) 각도 q(t), (2) 속도 q̇(t), (3) 가속도 q̈(t) — 으로 비교하라. 가속도 그래프에서 두 다항식의 차이가 보이는가?
문제 3-4 (끝점 궤적): 각 타임스텝에서 순기구학으로 끝점 (x(t), y(t))를 계산하고 플롯하라. 3차와 5차에서 끝점 경로(공간 상의 형태)가 다른가? 왜 그런가(또는 왜 그렇지 않은가)?
문제 3-5 (동역학 토크 계산): 중력 없음(G=0), 각 링크 질량 m1=m2=1 kg, 질량이 중점에 집중. 아래 관성 행렬 공식을 사용해 5차 다항식 궤적의 각 타임스텝에서 필요한 관절 토크 τ = M(q)·q̈ + τ_coriolis를 계산하고 플롯하라. 최대 토크는 얼마인가?
M11 = (m1+m2)L1² + m2L2² + 2m2L1L2cos(θ2)
M12 = M21 = m2L2² + m2L1L2cos(θ2)
M22 = m2L2²
τ_coriolis = [ -2m2L1L2sin(θ2)θ̇1θ̇2 - m2L1L2sin(θ2)θ̇2² ]
[ m2L1L2sin(θ2)θ̇1² ]
Project 4 (종합): 칼만 필터 + 기구학 통합
배경: 관절 각도 센서(인코더)에 표준편차 0.05 rad의 가우시안 노이즈가 있다. 실제 관절 각도는 Project 3-1의 3차 다항식 궤적을 따른다.
문제 4-1: 상태 벡터 x = [θ1; θ̇1; θ2; θ̇2] (4×1)으로 하는 칼만 필터를 설계하라. F는 4×4 등속 모델(각 관절이 일정한 각속도로 움직인다고 가정), H는 위치만 측정(4×4 중 위치 부분만 1).
문제 4-2: 100 타임스텝 동안 칼만 필터를 실행해 θ1, θ2를 추정하라.
문제 4-3: (a) 노이즈 있는 센서 측정값 직접 사용 vs. (b) 칼만 필터 추정값 사용 — 두 경우에 순기구학으로 끝점 위치를 계산하고, 실제 끝점 궤적과의 RMSE(Root Mean Square Error) = sqrt(mean((x_estimated - x_true)² + (y_estimated - y_true)²))를 계산해 정량적으로 비교하라.
평가 기준 안내
이 프로젝트들은 다음 기준으로 평가된다. 추정 정확도(40점): 칼만 필터가 노이즈를 얼마나 효과적으로 제거하는가, Q/R 설정의 근거 있는 선택, Project 4에서 RMSE 개선 정량화. 궤적 추종 성능(40점): 기구학 구현의 정확성(FK-IK 검증 통과 여부), 3차/5차 다항식의 경계 조건 만족 여부, 동역학 토크 계산 정확성. 기술 발표(20점): 각 수식의 물리적 의미를 말로 설명할 수 있는가, 시각화와 분석 결과를 논리적으로 전달할 수 있는가.
이제 시작하라. 첫 번째 프로젝트에서 막히는 지점이 어디인지가 네 이해도를 정직하게 보여줄 것이다.