접촉 작업, 협동 로봇, 안전 제어를 위한 컴플라이언스 기술
전통적인 위치 제어 로봇은 사전에 정의된 경로를 정확히 따라가지만, 환경과의 접촉이 발생하면 과도한 힘이 발생하거나 작업이 실패할 수 있습니다. 힘 제어는 로봇이 환경과 안전하고 유연하게 상호작용할 수 있게 합니다.
| 작업 유형 | 위치 제어 | 힘 제어 |
|---|---|---|
| 조립 (Peg-in-Hole) | 위치 오차로 끼임/파손 | 힘 피드백으로 자동 정렬 |
| 연마/폴리싱 | 불균일한 표면 처리 | 일정 압력 유지 |
| 협동 작업 | 충돌 시 위험 | 안전한 접촉 반응 |
| 유연물 핸들링 | 변형/파손 위험 | 힘 제한으로 안전 취급 |
6축 힘/토크 센서는 로봇 손목에 장착되어 3축 힘(Fx, Fy, Fz)과 3축 토크(Tx, Ty, Tz)를 측정합니다.
import numpy as np
from dataclasses import dataclass
from typing import Tuple
@dataclass
class ForceTorqueReading:
"""6축 힘/토크 센서 측정값"""
force: np.ndarray # [Fx, Fy, Fz] in N
torque: np.ndarray # [Tx, Ty, Tz] in Nm
timestamp: float
class ForceTorqueSensor:
"""힘/토크 센서 인터페이스"""
def __init__(self, sensor_ip: str, sample_rate: int = 1000):
self.ip = sensor_ip
self.sample_rate = sample_rate
self.bias = np.zeros(6) # 바이어스 보정값
# 저역 통과 필터 파라미터
self.filter_alpha = 0.1
self.filtered_reading = np.zeros(6)
def calibrate_bias(self, num_samples: int = 100):
"""바이어스 캘리브레이션 (무부하 상태에서 수행)"""
readings = []
for _ in range(num_samples):
raw = self._read_raw()
readings.append(raw)
time.sleep(1.0 / self.sample_rate)
self.bias = np.mean(readings, axis=0)
print(f"Bias calibrated: {self.bias}")
def read(self) -> ForceTorqueReading:
"""필터링된 측정값 읽기"""
raw = self._read_raw() - self.bias
# 저역 통과 필터 적용
self.filtered_reading = (
self.filter_alpha * raw +
(1 - self.filter_alpha) * self.filtered_reading
)
return ForceTorqueReading(
force=self.filtered_reading[:3],
torque=self.filtered_reading[3:],
timestamp=time.time()
)
def get_wrench_in_world(self, sensor_pose: np.ndarray) -> np.ndarray:
"""센서 측정값을 월드 좌표계로 변환"""
reading = self.read()
wrench_sensor = np.concatenate([reading.force, reading.torque])
# 회전 변환
R = sensor_pose[:3, :3]
wrench_world = np.zeros(6)
wrench_world[:3] = R @ wrench_sensor[:3]
wrench_world[3:] = R @ wrench_sensor[3:]
return wrench_world
class GravityCompensator:
"""공구/그리퍼 중력 보상"""
def __init__(self, tool_mass: float, tool_cog: np.ndarray):
"""
Args:
tool_mass: 공구 질량 (kg)
tool_cog: 센서 기준 무게중심 위치 (m)
"""
self.mass = tool_mass
self.cog = tool_cog
self.gravity = np.array([0, 0, -9.81])
def compensate(self, wrench: np.ndarray,
sensor_rotation: np.ndarray) -> np.ndarray:
"""중력 성분 제거"""
# 센서 좌표계에서의 중력 벡터
gravity_sensor = sensor_rotation.T @ self.gravity
# 중력에 의한 힘
force_gravity = self.mass * gravity_sensor
# 중력에 의한 토크 (τ = r × F)
torque_gravity = np.cross(self.cog, force_gravity)
# 보상
compensated = wrench.copy()
compensated[:3] -= force_gravity
compensated[3:] -= torque_gravity
return compensated임피던스 제어(Impedance Control)는 로봇을 가상의 질량-스프링-댐퍼 시스템으로 모델링하여 원하는 동적 특성을 부여합니다. 위치 오차에 비례하여 힘이 발생하도록 제어합니다.
class ImpedanceController:
"""데카르트 공간 임피던스 제어기"""
def __init__(self,
mass: np.ndarray, # 가상 질량 (6x6)
damping: np.ndarray, # 가상 댐핑 (6x6)
stiffness: np.ndarray): # 가상 강성 (6x6)
"""
임피던스 관계: F = M*ẍ + D*ẋ + K*x
낮은 강성 = 부드러운 응답 (컴플라이언트)
높은 강성 = 딱딱한 응답 (위치 추종)
"""
self.M = mass
self.D = damping
self.K = stiffness
# 상태 변수
self.prev_error = np.zeros(6)
self.prev_velocity = np.zeros(6)
def compute(self,
current_pose: np.ndarray, # 현재 포즈 (4x4)
desired_pose: np.ndarray, # 목표 포즈 (4x4)
external_wrench: np.ndarray, # 외부 힘/토크
dt: float) -> np.ndarray:
"""
임피던스 제어 계산
Returns:
보정된 목표 포즈 (4x4)
"""
# 포즈 오차 계산
error = self._pose_error(desired_pose, current_pose)
# 속도 오차 (수치 미분)
velocity = (error - self.prev_error) / dt
# 가속도 오차 (수치 미분)
acceleration = (velocity - self.prev_velocity) / dt
# 임피던스 방정식: M*ẍ + D*ẋ + K*x = F_ext
# 해: x = M^(-1) * (F_ext - D*ẋ - K*x)
M_inv = np.linalg.inv(self.M)
correction = M_inv @ (
external_wrench - self.D @ velocity - self.K @ error
)
# 보정량을 목표 포즈에 적용
modified_pose = self._apply_correction(desired_pose, correction * dt * dt)
# 상태 업데이트
self.prev_error = error
self.prev_velocity = velocity
return modified_pose
def _pose_error(self, desired: np.ndarray, current: np.ndarray) -> np.ndarray:
"""포즈 오차 계산 (위치 + 회전)"""
# 위치 오차
pos_error = desired[:3, 3] - current[:3, 3]
# 회전 오차 (로그 맵)
R_error = desired[:3, :3] @ current[:3, :3].T
rot_error = self._rotation_to_axis_angle(R_error)
return np.concatenate([pos_error, rot_error])
def _rotation_to_axis_angle(self, R: np.ndarray) -> np.ndarray:
"""회전 행렬을 축-각 표현으로 변환"""
theta = np.arccos(np.clip((np.trace(R) - 1) / 2, -1, 1))
if theta < 1e-6:
return np.zeros(3)
axis = np.array([
R[2, 1] - R[1, 2],
R[0, 2] - R[2, 0],
R[1, 0] - R[0, 1]
]) / (2 * np.sin(theta))
return theta * axis
def set_compliance(self, axis: str, stiffness: float, damping: float):
"""특정 축의 컴플라이언스 설정"""
axis_map = {'x': 0, 'y': 1, 'z': 2, 'rx': 3, 'ry': 4, 'rz': 5}
idx = axis_map[axis]
self.K[idx, idx] = stiffness
self.D[idx, idx] = damping임피던스 파라미터 튜닝: 강성(K)을 낮추면 부드러운 접촉, 댐핑(D)을 높이면 진동 억제, 질량(M)을 높이면 느린 응답이 됩니다.
어드미턴스 제어(Admittance Control)는 임피던스 제어의 역관계입니다. 외부 힘을 입력으로 받아 위치 변화를 출력합니다. 위치 제어 로봇에 더 쉽게 적용할 수 있습니다.
class AdmittanceController:
"""어드미턴스 제어기 (Force → Position)"""
def __init__(self,
mass: np.ndarray,
damping: np.ndarray,
stiffness: np.ndarray):
"""
어드미턴스 관계: x = Y * F
여기서 Y = (Ms² + Ds + K)^(-1)
"""
self.M = mass
self.D = damping
self.K = stiffness
# 적분 상태
self.velocity = np.zeros(6)
self.position_offset = np.zeros(6)
def compute(self,
external_wrench: np.ndarray,
dt: float) -> np.ndarray:
"""
외부 힘에 대한 위치 보정 계산
Args:
external_wrench: 측정된 외부 힘/토크 [Fx, Fy, Fz, Tx, Ty, Tz]
dt: 제어 주기
Returns:
position_offset: 위치 보정량 [x, y, z, rx, ry, rz]
"""
# 가속도 계산: M*a = F - D*v - K*x
M_inv = np.linalg.inv(self.M)
acceleration = M_inv @ (
external_wrench -
self.D @ self.velocity -
self.K @ self.position_offset
)
# 속도 적분
self.velocity += acceleration * dt
# 위치 적분
self.position_offset += self.velocity * dt
return self.position_offset.copy()
def reset(self):
"""제어기 상태 초기화"""
self.velocity = np.zeros(6)
self.position_offset = np.zeros(6)
class HybridForcePositionController:
"""하이브리드 힘/위치 제어기"""
def __init__(self):
# 선택 행렬: 0=위치 제어, 1=힘 제어
self.selection_matrix = np.diag([0, 0, 1, 0, 0, 0]) # Z축만 힘 제어
# 위치 제어기 게인
self.Kp_pos = np.diag([1000, 1000, 1000, 100, 100, 100])
self.Kd_pos = np.diag([100, 100, 100, 10, 10, 10])
# 힘 제어기 게인
self.Kp_force = np.diag([0.001, 0.001, 0.001, 0.0001, 0.0001, 0.0001])
self.Ki_force = np.diag([0.0001, 0.0001, 0.0001, 0.00001, 0.00001, 0.00001])
self.force_error_integral = np.zeros(6)
def compute(self,
current_pose: np.ndarray,
desired_pose: np.ndarray,
current_velocity: np.ndarray,
current_wrench: np.ndarray,
desired_wrench: np.ndarray,
dt: float) -> np.ndarray:
"""
하이브리드 제어 계산
Returns:
제어 토크/힘
"""
# 위치 오차
pose_error = self._compute_pose_error(desired_pose, current_pose)
# 힘 오차
force_error = desired_wrench - current_wrench
self.force_error_integral += force_error * dt
# 위치 제어 출력
S_pos = np.eye(6) - self.selection_matrix
u_pos = S_pos @ (self.Kp_pos @ pose_error - self.Kd_pos @ current_velocity)
# 힘 제어 출력
S_force = self.selection_matrix
u_force = S_force @ (
self.Kp_force @ force_error +
self.Ki_force @ self.force_error_integral
)
# 합성
return u_pos + u_force
def set_force_control_axis(self, axes: list):
"""힘 제어 축 설정"""
self.selection_matrix = np.zeros((6, 6))
axis_map = {'x': 0, 'y': 1, 'z': 2, 'rx': 3, 'ry': 4, 'rz': 5}
for axis in axes:
idx = axis_map[axis]
self.selection_matrix[idx, idx] = 1협동 로봇(Cobot)은 사람과 같은 공간에서 작업하므로 안전이 최우선입니다. ISO 10218-1/2와 ISO/TS 15066 표준을 준수해야 합니다.
from enum import Enum
class SafetyMode(Enum):
NORMAL = "normal"
REDUCED = "reduced" # 감속 모드
PROTECTIVE_STOP = "protective_stop"
EMERGENCY_STOP = "emergency_stop"
class CollaborativeSafetyController:
"""협동 로봇 안전 제어기 (ISO/TS 15066 준수)"""
def __init__(self,
max_tcp_speed: float = 250.0, # mm/s
max_tcp_force: float = 150.0, # N
max_pressure: float = 140.0, # N/cm²
reduced_speed: float = 100.0): # mm/s
"""
ISO/TS 15066 기준값:
- 머리/얼굴: 130 N, 20 N/cm²
- 목: 150 N, 50 N/cm²
- 몸통: 210 N, 70 N/cm²
- 팔/손: 160 N, 140 N/cm²
"""
self.max_tcp_speed = max_tcp_speed
self.max_tcp_force = max_tcp_force
self.max_pressure = max_pressure
self.reduced_speed = reduced_speed
self.safety_mode = SafetyMode.NORMAL
self.contact_detected = False
def check_safety(self,
tcp_speed: float,
tcp_force: float,
human_distance: float = None) -> SafetyMode:
"""
안전 상태 검사
Args:
tcp_speed: 현재 TCP 속도 (mm/s)
tcp_force: 현재 TCP 힘 (N)
human_distance: 사람과의 거리 (mm), None이면 감지 없음
"""
# 힘 한계 초과
if tcp_force > self.max_tcp_force:
self.safety_mode = SafetyMode.PROTECTIVE_STOP
self.contact_detected = True
return self.safety_mode
# 속도 기반 안전 거리 계산 (SSM: Speed and Separation Monitoring)
if human_distance is not None:
safe_distance = self._calculate_safe_distance(tcp_speed)
if human_distance < safe_distance * 0.5:
self.safety_mode = SafetyMode.PROTECTIVE_STOP
elif human_distance < safe_distance:
self.safety_mode = SafetyMode.REDUCED
else:
self.safety_mode = SafetyMode.NORMAL
return self.safety_mode
def _calculate_safe_distance(self, robot_speed: float) -> float:
"""
SSM 기반 안전 거리 계산
S_p = S_h + S_r + S_s + C + Z_d + Z_r
S_h: 사람 이동 거리
S_r: 로봇 정지 거리
S_s: 시스템 반응 거리
C: 침입 거리
Z_d, Z_r: 불확실성 마진
"""
# 간단화된 계산
human_speed = 1600 # mm/s (최대 보행 속도)
reaction_time = 0.1 # s (시스템 반응 시간)
stopping_time = 0.2 # s (로봇 정지 시간)
S_h = human_speed * (reaction_time + stopping_time)
S_r = robot_speed * stopping_time
margin = 100 # mm
return S_h + S_r + margin
def limit_velocity(self, desired_velocity: np.ndarray) -> np.ndarray:
"""안전 모드에 따른 속도 제한"""
if self.safety_mode == SafetyMode.PROTECTIVE_STOP:
return np.zeros_like(desired_velocity)
elif self.safety_mode == SafetyMode.REDUCED:
max_speed = self.reduced_speed
else:
max_speed = self.max_tcp_speed
# 속도 크기 제한
speed = np.linalg.norm(desired_velocity[:3])
if speed > max_speed:
scale = max_speed / speed
desired_velocity[:3] *= scale
return desired_velocity
class PowerAndForceLimit:
"""전력 및 힘 제한 (PFL) 제어"""
def __init__(self,
max_power: float = 80.0, # W
max_momentum: float = 25.0, # kg·m/s
max_quasi_static_force: float = 150.0): # N
self.max_power = max_power
self.max_momentum = max_momentum
self.max_quasi_static_force = max_quasi_static_force
def compute_safe_velocity(self,
reflected_mass: float,
current_force: float) -> float:
"""
충돌 시 안전한 최대 속도 계산
E = 0.5 * m_eff * v² (운동 에너지)
충돌 시 에너지가 안전 한계 이내여야 함
"""
# 에너지 기반 속도 한계
max_energy = 0.5 # J (손 접촉 기준)
v_energy = np.sqrt(2 * max_energy / reflected_mass)
# 운동량 기반 속도 한계
v_momentum = self.max_momentum / reflected_mass
# 힘 기반 속도 한계 (접촉 중)
remaining_force = max(0, self.max_quasi_static_force - current_force)
v_force = remaining_force / (100 * reflected_mass) # 가정된 스프링 상수
return min(v_energy, v_momentum, v_force)협동 로봇 안전 설계는 반드시 리스크 평가를 수행하고 전문가 검증을 받아야 합니다. 이 코드는 교육 목적의 예시입니다.
힘 제어를 활용한 대표적인 산업 응용 사례들입니다.
| 작업 | 제어 방식 | 핵심 파라미터 |
|---|---|---|
| Peg-in-Hole 조립 | 임피던스 + 나선 탐색 | 낮은 XY 강성, 힘 임계값 |
| 연마/폴리싱 | 하이브리드 (법선 힘 + 접선 위치) | 일정 접촉력, 경로 속도 |
| 스크류 체결 | 토크 제어 | 목표 토크, 각도 모니터링 |
| 유연물 핸들링 | 어드미턴스 | 매우 낮은 강성 |
Peg-in-Hole 조립에서 나선형 탐색(Spiral Search)과 임피던스 제어를 결합하면 0.1mm 이하의 클리어런스에서도 성공적으로 삽입할 수 있습니다.