PCD에서 로봇 본딩 경로 생성하기
Step 2 경로 검출과 Step 2-1 정밀 보정 알고리즘 정리
핵심 키워드
1. 들어가며
이번 글에서는 안경 프레임의 PCD(Point Cloud Data)에서 얇은 본딩 면을 검출하고, 로봇이 접착제를 도포할 수 있는 3차원 중앙 경로를 만드는 과정을 정리한다.
전체 알고리즘은 크게 두 단계로 구성된다.
Step 2: 원본 PCD에서 본딩 면의 Outer/Inner 경계를 검출하고 기본 중앙 경로 생성Step 2-1: 기본 경로의 짧은 Wave와 이상점을 제거하고 실제 PCD 단면을 이용해 정밀 보정
Step 2의 구현 순서는 PCD의 XY 격자 투영, Outer/Inner 경계 검출, 중앙 경로 계산, 필터링, 일정 간격 재샘플링 및 결과 저장으로 구성된다.
Step 2-1은 안경 프레임의 큰 형상은 유지하면서 국부적인 경계 돌출, 중앙선 진동, 면 폭의 급격한 변화를 제거한다. 실제 로봇의 안전 높이, 즉 World (+Z) 방향 Offset은 Step 2나 Step 2-1이 아니라 Step 3에서만 적용한다.
2. 전체 데이터 흐름
AI_Glass_Front_0.010mm.pcd
│
▼
Step 2: generate_global_path.py
│
├─ XY Top-envelope 생성
├─ Outer/Inner edge 검출
├─ 얇은 본딩 면 추출
├─ 기본 중앙 경로 생성
└─ 0.5 mm 간격 재샘플링
│
├─ step2_thin_bonding_face.pcd
├─ step2_global_path.csv
├─ step2_global_path.pcd
└─ step2_global_path.npz
│
▼
Step 2-1: smooth_global_path.py
│
├─ 이상점 제거
├─ Y/Z 경로 평활화
├─ 실제 PCD 단면 재측정
├─ 중앙 경로 재보정
├─ 면 폭 및 방향 재구성
└─ 최종 접선 재계산
│
├─ step2_1_accurate_global_path.csv
├─ step2_1_accurate_global_path.pcd
├─ step2_1_accurate_global_path.npz
└─ step2_1_accuracy_metrics.json
│
▼
Step 3: Quaternion trajectory 및 IK수학적으로는 다음과 같이 표현할 수 있다.
P → Rasterization ( A , Z top ) → Edge Detection ( O , I ) → Centerline C → Regularization C ^ \mathcal{P} \xrightarrow{\text{Rasterization}} (A,Z_{\text{top}}) \xrightarrow{\text{Edge Detection}} (\mathbf O,\mathbf I) \xrightarrow{\text{Centerline}} \mathbf C \xrightarrow{\text{Regularization}} \hat{\mathbf C} P Rasterization ( A , Z top ) Edge Detection ( O , I ) Centerline C Regularization C ^ 여기서 각 기호는 다음을 의미한다.
기호 의미 (\mathcal P) 원본 PCD 점군 (A) XY 격자의 점 존재 여부 (Z_{\text{top}}) XY 위치별 윗면 높이 (\mathbf O) Outer edge (\mathbf I) Inner edge (\mathbf C) Step 2 기본 중앙 경로 (\hat{\mathbf C}) Step 2-1 최종 보정 경로
3. 기본 수학 표현
원본 PCD는 다음과 같은 3차원 점들의 집합이다.
\mathcal P ========== \left{ \mathbf p_n =========== \begin{bmatrix} x_n\ y_n\ z_n \end{bmatrix} \right}_{n=1}^{N}
최종적으로 생성하고 싶은 경로는 경로 거리 (s)에 따른 3차원 곡선이다.
C ( s ) = = = = = = = = = = = = [ x ( s ) y ( s ) z ( s ) ] \mathbf C(s) ============ \begin{bmatrix} x(s)\ y(s)\ z(s) \end{bmatrix} C ( s ) = = = = = = = = = = = = [ x ( s ) y ( s ) z ( s ) ] 본딩 면의 양쪽 경계를 각각 다음과 같이 정의한다.
O ( s ) = = = = = = = = = = = = [ O x ( s ) O y ( s ) O z ( s ) ] \mathbf O(s) ============ \begin{bmatrix} O_x(s)\ O_y(s)\ O_z(s) \end{bmatrix} O ( s ) = = = = = = = = = = = = [ O x ( s ) O y ( s ) O z ( s ) ] I ( s ) = = = = = = = = = = = = [ I x ( s ) I y ( s ) I z ( s ) ] \mathbf I(s) ============ \begin{bmatrix} I_x(s)\ I_y(s)\ I_z(s) \end{bmatrix} I ( s ) = = = = = = = = = = = = [ I x ( s ) I y ( s ) I z ( s ) ] 양 경계의 중앙, 폭, 방향은 다음과 같이 계산할 수 있다.
중앙 경로
C ( s ) = = = = = = = = = = = = O ( s ) + I ( s ) 2 \mathbf C(s) ============ \frac{\mathbf O(s)+\mathbf I(s)}{2} C ( s ) = = = = = = = = = = = = 2 O ( s ) + I ( s ) 본딩 면의 폭
w ( s ) = = = = ∣ O ( s ) − I ( s ) ∣ 2 w(s) ==== \left| \mathbf O(s)-\mathbf I(s) \right|_2 w ( s ) = = = = ∣ O ( s ) − I ( s ) ∣ 2 Outer 방향 단위벡터
d ( s ) = = = = = = = = = = = = O ( s ) − I ( s ) ∣ O ( s ) − I ( s ) ∣ 2 \mathbf d(s) ============ \frac{ \mathbf O(s)-\mathbf I(s) }{ \left| \mathbf O(s)-\mathbf I(s) \right|_2 } d ( s ) = = = = = = = = = = = = ∣ O ( s ) − I ( s ) ∣ 2 O ( s ) − I ( s ) 경로 단위 접선
t ( s ) = = = = = = = = = = = = d C ( s ) / d s ∣ d C ( s ) / d s ∣ 2 \mathbf t(s) ============ \frac{ d\mathbf C(s)/ds }{ \left| d\mathbf C(s)/ds \right|_2 } t ( s ) = = = = = = = = = = = = ∣ d C ( s ) / d s ∣ 2 d C ( s ) / d s 이 정보는 이후 Step 3에서 노즐의 위치와 회전 자세를 계산할 때 사용된다.
4. Step 2: PCD에서 기본 경로 생성
4.1 PCD를 XY Raster로 변환하는 이유
PCD는 점들이 불규칙한 간격으로 존재하는 비정형 데이터이다.
이 상태에서 바로 경계 검출이나 미분을 수행하면 점 밀도와 분포에 따라 결과가 크게 달라질 수 있다. 따라서 XY 평면을 일정한 크기의 격자로 나누고, 각 셀에 대표 높이를 저장한다.
현재 Raster 해상도는 다음과 같다.
Δ = 0.02 mm \Delta=0.02\text{ mm} Δ = 0 . 0 2 mm 점 ((x_n,y_n))이 속하는 격자 인덱스는 다음과 같이 계산한다.
u n = = = ⌊ x n − x min Δ ⌋ u_n === \left\lfloor \frac{x_n-x_{\min}}{\Delta} \right\rfloor u n = = = ⌊ Δ x n − x m i n ⌋ v n = = = ⌊ y n − y min Δ ⌋ v_n === \left\lfloor \frac{y_n-y_{\min}}{\Delta} \right\rfloor v n = = = ⌊ Δ y n − y m i n ⌋ Raster 해상도가 (0.02\text{ mm})이므로 셀 중심 기준 좌표 양자화 오차는 이론적으로 약 다음 범위에 존재한다.
ϵ x , ϵ y ≈ ± 0.02 2 = = = = = = = = = = = = = = = = = ± 0.01 mm \epsilon_x,\epsilon_y \approx \pm\frac{0.02}{2} ================= \pm0.01\text{ mm} ϵ x , ϵ y ≈ ± 2 0 . 0 2 = = = = = = = = = = = = = = = = = ± 0 . 0 1 mm 4.2 Occupancy Map과 Top-envelope
각 XY 셀에 점이 존재하는지를 다음과 같이 정의한다.
A ( v , u ) = = = = = = { 1 , 셀 내부에 PCD 점이 존재 0 , 셀 내부에 PCD 점이 없음 A(v,u) ====== \begin{cases} 1, & \text{셀 내부에 PCD 점이 존재}\ 0, & \text{셀 내부에 PCD 점이 없음} \end{cases} A ( v , u ) = = = = = = { 1 , 셀 내부에 PCD 점이 존재 0 , 셀 내부에 PCD 점이 없음 그리고 각 셀에서 물리적으로 가장 위쪽에 있는 점을 대표 높이로 저장한다.
현재 Isaac 좌표 변환에서는 Local Z 방향이 물리적 높이 방향과 반대로 정의되어 있기 때문에, Local Z가 가장 작은 점이 실제 윗면에 해당한다.
Z top ( v , u ) = = = = = = = = = = = = = = = = = = = min p n ∈ cell ( v , u ) z n Z_{\text{top}}(v,u) =================== \min_{\mathbf p_n\in\text{cell}(v,u)}z_n Z top ( v , u ) = = = = = = = = = = = = = = = = = = = p n ∈ cell ( v , u ) min z n 코드에서는 PCD를 Chunk 단위로 읽고, 각 셀의 점 존재 여부와 최소 Local Z를 저장한다.
실제 Raster 인덱스 계산과 최소 Z 저장은 다음 처리에 대응한다.
px = floor( ( x - x_min) / raster)
py = floor( ( y - y_min) / raster)
occupied[ py, px] = True
min_z[ py, px] = min ( min_z[ py, px] , z) 구현에서는 np.minimum.at()을 이용해 셀별 최소 Z를 계산한다.
이 결과는 완전한 3차원 모델이라기보다 다음 형태의 2.5D Height Map 이다.
즉, 하나의 XY 위치에 하나의 대표 높이를 저장하는 방식이다.
4.3 비어 있는 셀의 높이 보완
중앙 경로의 XY 위치에서 PCD 셀이 비어 있을 수 있다.
이 경우 현재 셀 주변의 (5\times5) 영역을 확인하고, 유효 높이들의 중앙값을 사용한다.
\hat Z_{\text{top}}(u,v) ======================== \operatorname{median} \left{ Z_{\text{top}}(u+i,v+j) \right}
i , j ∈ − 2 , − 1 , 0 , 1 , 2 i,j\in{-2,-1,0,1,2} i , j ∈ − 2 , − 1 , 0 , 1 , 2 평균이 아니라 중앙값을 사용하는 이유는 일부 이상 높이점의 영향을 줄이기 위해서이다.
코드에서도 비어 있는 위치를 주변 (5\times5) 패치의 유효 높이 중앙값으로 보완한다.
5. Outer edge 검출
각 X 열에서 Occupied 상태인 Y 좌표 중 가장 큰 값을 Outer edge로 선택한다.
O_y(x_j) ======== \max \left{ y_k \mid A(y_k,x_j)=1 \right}
즉, 위에서 바라본 XY Occupancy Map에서 (+Y) 방향으로 가장 바깥쪽에 있는 윤곽을 찾는 방식이다.
Y 증가 방향
↑
│ Outer edge
│ ●────────●
│ │ 본딩 면 │
│ ●────────●
│ Inner edge
└──────────────────→ X이 방식에는 다음 가정이 포함되어 있다.
안경 프레임의 바깥쪽 방향이 현재 좌표계에서 (+Y) 방향이다.
제품의 방향이 반대로 배치되면 max Y가 아니라 min Y를 사용해야 한다.
6. Inner edge 검출
6.1 검색 범위 설정
Inner edge는 Outer edge에서 안쪽으로 다음 범위에서 탐색한다.
d min = 0.35 mm d_{\min}=0.35\text{ mm} d m i n = 0 . 3 5 mm d max = 2.80 mm d_{\max}=2.80\text{ mm} d m a x = 2 . 8 0 mm 따라서 검색 영역은 다음과 같다.
O y ( x j ) − 2.80 ≤ y ≤ O y ( x j ) − 0.35 O_y(x_j)-2.80 \le y\le O_y(x_j)-0.35 O y ( x j ) − 2 . 8 0 ≤ y ≤ O y ( x j ) − 0 . 3 5 이 범위는 Inner edge가 존재할 것으로 예상되는 공간적 사전정보이다.
6.2 Y 방향 Z 프로파일
특정 X 위치 (x_j)에서 Y 방향 높이 분포를 다음과 같이 정의한다.
z j ( y ) = = = = = = Z top ( x j , y ) z_j(y) ====== Z_{\text{top}}(x_j,y) z j ( y ) = = = = = = Z top ( x j , y ) Inner edge는 본딩 윗면과 홈 사이에서 높이가 급격히 바뀌는 지점이다.
하지만 원본 프로파일에는 PCD 노이즈와 Raster 계단이 존재하므로 미분 전에 Gaussian filter를 적용한다.
6.3 Gaussian Filter
Gaussian 함수는 다음과 같다.
G σ ( τ ) = = = = = = = = = = = = = = 1 2 π σ exp ( − τ 2 2 σ 2 ) G_\sigma(\tau) ============== \frac{1}{\sqrt{2\pi}\sigma} \exp \left( -\frac{\tau^2}{2\sigma^2} \right) G σ ( τ ) = = = = = = = = = = = = = = 2 π σ 1 exp ( − 2 σ 2 τ 2 ) 평활화된 높이 프로파일은 다음과 같다.
z ~ j ( y ) = = = = = = = = = = = = = ( G σ ∗ z j ) ( y ) \tilde z_j(y) ============= (G_\sigma*z_j)(y) z ~ j ( y ) = = = = = = = = = = = = = ( G σ ∗ z j ) ( y ) 현재 사용되는 물리적 표준편차는 다음과 같다.
σ = 0.03 mm \sigma=0.03\text{ mm} σ = 0 . 0 3 mm Raster 간격이 (0.02\text{ mm})이므로 셀 단위 표준편차는 약 다음과 같다.
\sigma_{\text{cell}} ==================== # \frac{0.03}{0.02} 1.5
즉, 약 1.5셀 범위의 짧은 측정 노이즈를 완화한다.
6.4 높이 미분으로 경계 찾기
Gaussian filter를 적용한 높이를 Y 방향으로 미분한다.
g j ( y ) = = = = = = d z ~ j ( y ) d y g_j(y) ====== \frac{d\tilde z_j(y)}{dy} g j ( y ) = = = = = = d y d z ~ j ( y ) 이산 데이터에서는 다음 중앙 차분과 유사한 방식으로 계산할 수 있다.
g j ( y k ) ≈ z ~ j ( y k + 1 ) − − − − − − − − − − − − − − − − − − − z ~ j ( y k − 1 ) 2 Δ g_j(y_k) \approx \frac{ \tilde z_j(y_{k+1}) ------------------- \tilde z_j(y_{k-1}) }{ 2\Delta } g j ( y k ) ≈ 2 Δ z ~ j ( y k + 1 ) − − − − − − − − − − − − − − − − − − − z ~ j ( y k − 1 ) 현재 Local Z가 물리 높이와 반대이기 때문에 가장 음의 방향으로 강하게 변화하는 위치를 선택한다.
k ∗ = = = arg min k g j ( y k ) k^* === \arg\min_k g_j(y_k) k ∗ = = = arg k min g j ( y k ) I y ( x j ) = = = = = = = = y k ∗ I_y(x_j) ======== y_{k^*} I y ( x j ) = = = = = = = = y k ∗ 경계 강도는 다음과 같이 정의된다.
E j = = = − g j ( y k ∗ ) E_j === -g_j(y_{k^*}) E j = = = − g j ( y k ∗ ) 실제 구현에서도 Gaussian filter 이후 np.gradient()로 미분하고, argmin() 위치를 Inner edge로 선택한다.
6.5 유효 경계 판정
Outer와 Inner 사이의 폭은 다음과 같다.
w j = = = O y ( x j ) − I y ( x j ) w_j === O_y(x_j)-I_y(x_j) w j = = = O y ( x j ) − I y ( x j ) 검출된 경계는 다음 조건을 만족해야 한다.
0.35 ≤ w j ≤ 2.80 0.35 \le w_j \le 2.80 0 . 3 5 ≤ w j ≤ 2 . 8 0 E j ≥ 0.15 E_j \ge 0.15 E j ≥ 0 . 1 5 폭이나 경계 강도 조건을 만족하지 않는 X 열은 무효 처리한 뒤, 인접한 유효 열을 이용해 보간한다.
또한 전체 X 열 중 유효 경계쌍이 80% 미만이면 경로 생성에 실패한 것으로 판단한다.
7. 경계 평활화
검출된 Outer/Inner edge에는 Raster 계단과 국부적인 돌출점이 포함될 수 있다.
따라서 다음 순서로 평활화한다.
검출 경계
│
▼
Median Filter
│
▼
Savitzky-Golay Filter
│
▼
평활화된 경계길이 (2K+1)의 Median filter는 다음과 같다.
y ^ i = = = = = = = = median ( y i − K , … , y i , … , y i + K ) \hat y_i ======== \operatorname{median} \left( y_{i-K}, \ldots, y_i, \ldots, y_{i+K} \right) y ^ i = = = = = = = = m e d i a n ( y i − K , … , y i , … , y i + K ) 예를 들어 다음과 같은 데이터가 있다고 가정하자.
1.01, 1.00, 1.02, 1.85, 1.01, 1.00여기서 1.85는 국부적인 이상점이다.
평균 필터는 1.85의 영향을 받지만 Median filter는 주변의 정상값을 기준으로 이상점을 제거할 수 있다.
7.2 Savitzky-Golay Filter
Savitzky-Golay filter는 주변 데이터를 단순 평균하지 않고 저차 다항식으로 근사한다.
3차 다항식을 사용하면 다음과 같다.
p i ( τ ) = = = = = = = = = a 0 + a 1 τ + a 2 τ 2 + a 3 τ 3 p_i(\tau) ========= a_0+a_1\tau+a_2\tau^2+a_3\tau^3 p i ( τ ) = = = = = = = = = a 0 + a 1 τ + a 2 τ 2 + a 3 τ 3 각 중심점 주변에서 다음 최소제곱 문제를 해결한다.
min a 0 , a 1 , a 2 , a 3 ∑ k = − K K [ y i + k − − − − − − − p i ( k Δ ) ] 2 \min_{a_0,a_1,a_2,a_3} \sum_{k=-K}^{K} \left[ y_{i+k} ------- p_i(k\Delta) \right]^2 a 0 , a 1 , a 2 , a 3 min k = − K ∑ K [ y i + k − − − − − − − p i ( k Δ ) ] 2 평활화된 중심값은 다음과 같다.
\hat y_i ======== # p_i(0) a_0
이 방식은 이동평균보다 곡선의 경사와 곡률을 잘 유지한다.
코드에서는 Median filter 이후 3차 Savitzky-Golay filter를 적용한다.
7.3 Step 2 필터 파라미터
적용 대상 Median 창 SG 창 다항식 차수 Outer/Inner Y 0.50 mm 3.00 mm 3 중앙 표면 Z 0.40 mm 1.20 mm 3
경계와 높이에 서로 다른 필터 창이 설정되어 있다.
XY 경계에는 비교적 긴 창을 사용해 Raster wave를 충분히 제거한다.
반면 Z에 긴 창을 적용하면 실제 급경사 형상이 평탄해질 수 있으므로 더 짧은 창을 사용한다.
8. 중앙 경로 계산
평활화된 Outer와 Inner의 Y 좌표를 이용해 중앙 Y를 계산한다.
C x ( x j ) = x j C_x(x_j)=x_j C x ( x j ) = x j C y ( x j ) = = = = = = = = O y ( x j ) + I y ( x j ) 2 C_y(x_j) ======== \frac{ O_y(x_j)+I_y(x_j) }{2} C y ( x j ) = = = = = = = = 2 O y ( x j ) + I y ( x j ) 중앙 Z는 Outer와 Inner의 Z를 단순 평균하지 않는다.
중앙 XY 위치에서 Top-envelope 높이를 다시 측정한다.
C z ( x j ) = = = = = = = = Z top ( x j , C y ( x j ) ) C_z(x_j) ======== Z_{\text{top}} \left( x_j,C_y(x_j) \right) C z ( x j ) = = = = = = = = Z top ( x j , C y ( x j ) ) 따라서 중앙점은 다음과 같다.
C j = = = = = = = = = = = [ x j O y ( x j ) + I y ( x j ) 2 Z top ( x j , C y ( x j ) ) ] \mathbf C_j =========== \begin{bmatrix} x_j\ \dfrac{O_y(x_j)+I_y(x_j)}{2}\ Z_{\text{top}}\left(x_j,C_y(x_j)\right) \end{bmatrix} C j = = = = = = = = = = = [ x j 2 O y ( x j ) + I y ( x j ) Z top ( x j , C y ( x j ) ) ] 코드에서도 center_y=(outer_y+inner_y)/2를 계산한 후 해당 중앙 위치의 Top-envelope Z를 읽어 사용한다.
이 방식의 핵심은 다음과 같다.
XY는 양쪽 경계의 중앙을 사용하고, Z는 그 중앙 위치에서 측정한 실제 윗면 높이를 사용한다.
9. 얇은 본딩 면 PCD 추출
모든 Top-envelope 점을 사용하는 것이 아니라, 양쪽 경계 사이의 실제 얇은 윗면만 별도의 PCD로 추출한다.
첫 번째 조건은 점이 Inner와 Outer 사이에 존재하는 것이다.
I y ( x ) ≤ y ≤ O y ( x ) I_y(x) \le y \le O_y(x) I y ( x ) ≤ y ≤ O y ( x ) 두 번째 조건은 점의 높이가 중앙 높이와 (0.15\text{ mm}) 이내에 존재하는 것이다.
∣ z − C z ( x ) ∣ ≤ 0.15 mm \left| z-C_z(x) \right| \le 0.15\text{ mm} ∣ z − C z ( x ) ∣ ≤ 0 . 1 5 mm 따라서 얇은 면 점군은 다음과 같다.
\mathcal F ========== \left{ (x,y,z) \in \mathcal P_{\text{top}} ;\middle|; I_y(x)\le y\le O_y(x), \left|z-C_z(x)\right|\le0.15 \right}
코드에서도 경계 사이에 존재하고 중앙 Z에서 (\pm0.15\text{ mm}) 이내인 점만 얇은 본딩 면 PCD로 저장한다.
이 조건은 홈 내부나 옆면처럼 높이가 다른 점을 제거하는 역할을 한다.
10. 양 끝 직각 전환부 제거
안경 프레임 양 끝에는 본딩 경로가 아닌 수평 탭이나 급격한 직각 전환부가 존재할 수 있다.
이를 제거하기 위해 인접 경로점의 XY 이동량과 Z 변화량을 계산한다.
Δ r x y , i = = = = = = = = = = = = = = = ( Δ x i ) 2 + ( Δ y i ) 2 \Delta r_{xy,i} =============== \sqrt{ (\Delta x_i)^2+ (\Delta y_i)^2 } Δ r x y , i = = = = = = = = = = = = = = = ( Δ x i ) 2 + ( Δ y i ) 2 Δ z i = = = = = = = = = = z i + 1 − z i \Delta z_i ========== z_{i+1}-z_i Δ z i = = = = = = = = = = z i + 1 − z i XY 평면 기준 경사각은 다음과 같다.
θ i = = = = = = = = atan2 ( ∣ Δ z i ∣ , ( Δ x i ) 2 + ( Δ y i ) 2 ) \theta_i ======== \operatorname{atan2} \left( |\Delta z_i|, \sqrt{ (\Delta x_i)^2+ (\Delta y_i)^2 } \right) θ i = = = = = = = = a t a n 2 ( ∣ Δ z i ∣ , ( Δ x i ) 2 + ( Δ y i ) 2 ) 각도를 Degree로 변환하면 다음과 같다.
θ i , deg = = = = = = = = = = = = = = = θ i 180 π \theta_{i,\deg} =============== \theta_i \frac{180}{\pi} θ i , d e g = = = = = = = = = = = = = = = θ i π 1 8 0 기본적으로 다음 조건을 만족하는 첫 구간과 마지막 구간을 탐색한다.
θ i , deg ≥ 5 5 ∘ \theta_{i,\deg} \ge 55^\circ θ i , d e g ≥ 5 5 ∘ 이 (55^\circ)는 안경의 설계각이 아니다.
로봇 자세가 갑자기 변하는 양 끝 전환부를 검출하기 위한 임계값이다.
CLI 실행에서는 --no-endpoint-trim을 사용하지 않는 한 양 끝 전환부 제거가 활성화된다.
11. 일정 간격 경로 재샘플링
경계에서 계산한 중앙점들은 X Raster 간격으로 생성되었기 때문에 3차원 실제 거리 간격은 일정하지 않다.
따라서 먼저 3차원 누적 거리를 계산한다.
인접 경로점 사이의 거리는 다음과 같다.
ℓ i = = = = = = ∣ C i − C i − 1 ∣ 2 \ell_i ====== \left| \mathbf C_i-\mathbf C_{i-1} \right|_2 ℓ i = = = = = = ∣ C i − C i − 1 ∣ 2 누적 거리는 다음과 같다.
s i = = = ∑ k = 1 i ℓ k s_i === \sum_{k=1}^{i}\ell_k s i = = = k = 1 ∑ i ℓ k 최종 경로 길이는 다음과 같다.
목표 경로 간격은 다음과 같다.
Δ s = = = = = = = = 0.5 mm \Delta s ======== 0.5\text{ mm} Δ s = = = = = = = = 0 . 5 mm 목표 Station은 다음과 같이 생성한다.
s j ∗ = = = = = j Δ s s_j^* ===== j\Delta s s j ∗ = = = = = j Δ s j = 0 , 1 , … , ⌊ L Δ s ⌋ j = 0,1,\ldots, \left\lfloor \frac{L}{\Delta s} \right\rfloor j = 0 , 1 , … , ⌊ Δ s L ⌋ 11.1 선형 보간
목표 거리 (s_j^*)가 다음 범위에 존재한다고 가정한다.
s i ≤ s j ∗ ≤ s i + 1 s_i \le s_j^* \le s_{i+1} s i ≤ s j ∗ ≤ s i + 1 보간 비율은 다음과 같다.
α = = = = = = s j ∗ − s i s i + 1 − s i \alpha ====== \frac{ s_j^*-s_i }{ s_{i+1}-s_i } α = = = = = = s i + 1 − s i s j ∗ − s i 보간된 경로점은 다음과 같다.
C ( s j ∗ ) = = = = = = = = = = = = = = = = ( 1 − α ) C i + α C i + 1 \mathbf C(s_j^*) ================ (1-\alpha)\mathbf C_i + \alpha\mathbf C_{i+1} C ( s j ∗ ) = = = = = = = = = = = = = = = = ( 1 − α ) C i + α C i + 1 코드에서는 중앙선의 3차원 누적 거리를 기준으로 X, Y, Z를 각각 선형 보간하여 기본 (0.5\text{ mm}) 간격의 경로를 만든다.
12. 접선 벡터 계산
연속적인 곡선의 접선은 다음과 같다.
d C d s \frac{d\mathbf C}{ds} d s d C 이산 경로에서는 중앙 차분으로 근사할 수 있다.
v i ≈ C i + 1 − C i − 1 s i + 1 − s i − 1 \mathbf v_i \approx \frac{ \mathbf C_{i+1}-\mathbf C_{i-1} }{ s_{i+1}-s_{i-1} } v i ≈ s i + 1 − s i − 1 C i + 1 − C i − 1 접선의 크기를 1로 만들기 위해 정규화한다.
t i = = = = = = = = = = = v i ∣ v i ∣ 2 \mathbf t_i =========== \frac{ \mathbf v_i }{ |\mathbf v_i|_2 } t i = = = = = = = = = = = ∣ v i ∣ 2 v i 따라서 다음 조건을 만족한다.
∣ t i ∣ 2 = 1 |\mathbf t_i|_2=1 ∣ t i ∣ 2 = 1 코드에서는 누적 거리 기준으로 np.gradient()를 적용하고, 각 미분 벡터를 정규화한다.
이 접선은 Step 3에서 로봇 Tool의 진행 방향을 구성할 때 사용된다.
13. KD-tree를 이용한 경로 검증
생성된 중앙 경로가 실제 얇은 면 PCD 위에 위치하는지를 검증한다.
각 중앙점 (\mathbf C_i)에서 얇은 면 PCD까지의 최근접 거리를 다음과 같이 계산한다.
d i = = = min q ∈ F ∣ C i − q ∣ 2 d_i === \min_{\mathbf q\in\mathcal F} \left| \mathbf C_i-\mathbf q \right|_2 d i = = = q ∈ F min ∣ C i − q ∣ 2 모든 PCD 점과 직접 거리를 계산하면 연산량이 크기 때문에 KD-tree를 사용한다.
코드에서도 cKDTree를 이용해 각 중앙점에서 얇은 면 PCD까지의 최근접 거리를 계산한다.
해석 방법은 다음과 같다.
(d_i)가 작다: 경로가 실제 추출 면 위에 잘 위치함
(d_i)가 크다: 경로가 표면에서 이탈했거나 PCD가 희박함
평균값만 보는 것보다 평균, P95, 최댓값을 함께 확인하는 것이 좋다.
14. Step 2-1: 정밀 보정
Step 2에서 생성된 경로에는 다음 오차가 남을 수 있다.
Raster 격자에 의한 짧은 계단
Inner edge 순간 오검출
PCD 이상점에 의한 중앙선 돌출
Y 방향의 짧은 Wave
Z 방향의 국부적인 측정 진동
본딩 면 폭의 급격한 변화
Step 2-1은 전체 안경 형상을 바꾸지 않고 이러한 국부적인 문제만 제거하는 단계이다.
15. 필터 길이를 샘플 수로 변환
Step 2-1의 필터 크기는 샘플 개수가 아니라 mm 단위로 설정된다.
평균 경로 간격을 (\Delta s), 원하는 물리적 필터 길이를 (W)라고 하면 필요한 샘플 수는 다음과 같다.
N = round ( W Δ s ) N = \operatorname{round} \left( \frac{W}{\Delta s} \right) N = r o u n d ( Δ s W ) Savitzky-Golay filter는 중심 대칭 창을 사용하므로 (N)은 홀수여야 한다.
따라서 (N)이 짝수이면 다음과 같이 보정한다.
N ← N + 1 N\leftarrow N+1 N ← N + 1 코드에서는 실제 경로의 중앙 간격을 계산한 뒤 mm 단위 창을 홀수 샘플 창으로 변환한다.
경로 간격을 약 (0.5\text{ mm})라고 가정하면 다음과 같다.
처리 대상 물리적 창 예상 샘플 수 Median 기준선 1.5 mm 약 3개 Y 경로 평활화 7.0 mm 약 15개 Z 경로 평활화 3.5 mm 약 7개 면 폭 평활화 9.0 mm 약 19개 폭 방향 평활화 7.0 mm 약 15개 PCD 재중앙화 7.0 mm 약 15개
실제 샘플 수는 경로의 실제 간격과 전체 점 개수에 따라 달라질 수 있다.
16. Robust Low-pass Filter
Step 2-1의 핵심 평활화 과정은 다음과 같다.
원본 신호
│
▼
1.5 mm Median 기준선
│
▼
Residual 계산
│
▼
MAD 기반 이상치 제한
│
▼
Savitzky-Golay Low-pass입력 신호를 (v_i)라고 하면 Median 기준선은 다음과 같다.
m_i === \operatorname{median} \left{ v_{i-K},\ldots,v_{i+K} \right}
16.2 Residual 계산
원본 신호와 기준선의 차이를 계산한다.
r i = = = v i − m i r_i === v_i-m_i r i = = = v i − m i 짧은 돌출점은 큰 Residual을 만든다.
16.3 MAD 기반 Robust Sigma
Residual의 중앙값은 다음과 같다.
r c = = = median ( r i ) r_c === \operatorname{median}(r_i) r c = = = m e d i a n ( r i ) Median Absolute Deviation은 다음과 같다.
MAD = = = = = = = = = = = = = = = = = = median ( ∣ r i − r c ∣ ) \operatorname{MAD} ================== \operatorname{median} \left( |r_i-r_c| \right) M A D = = = = = = = = = = = = = = = = = = m e d i a n ( ∣ r i − r c ∣ ) 정규분포의 표준편차와 유사한 척도로 변환하기 위해 다음 계수를 사용한다.
σ ^ = = = = = = = = = = 1.4826 ⋅ MAD \hat\sigma ========== 1.4826 \cdot \operatorname{MAD} σ ^ = = = = = = = = = = 1 . 4 8 2 6 ⋅ M A D 일반적인 표준편차는 큰 이상치의 제곱에 영향을 많이 받는다.
반면 MAD는 Median 기반이므로 소수의 큰 PCD 이상점에 강하다.
16.4 (3.5\sigma) 이상치 제한
Residual의 허용 범위는 다음과 같다.
r c − 3.5 σ ^ ≤ r i ≤ r c + 3.5 σ ^ r_c-3.5\hat\sigma \le r_i \le r_c+3.5\hat\sigma r c − 3 . 5 σ ^ ≤ r i ≤ r c + 3 . 5 σ ^ 범위를 벗어난 Residual은 다음과 같이 제한한다.
r i clip = = = = = = = = = = = = = = = = = clip ( r i , r c − 3.5 σ ^ , r c + 3.5 σ ^ ) r_i^{\text{clip}} ================= \operatorname{clip} \left( r_i, r_c-3.5\hat\sigma, r_c+3.5\hat\sigma \right) r i clip = = = = = = = = = = = = = = = = = c l i p ( r i , r c − 3 . 5 σ ^ , r c + 3 . 5 σ ^ ) 이상치가 제거된 신호는 다음과 같다.
v i despike = = = = = = = = = = = = = = = = = = = = m i + r i clip v_i^{\text{despike}} ==================== m_i+r_i^{\text{clip}} v i despike = = = = = = = = = = = = = = = = = = = = m i + r i clip 마지막으로 Savitzky-Golay filter를 적용한다.
v ^ i = = = = = = = = SG ( v i despike ) \hat v_i ======== \operatorname{SG} \left( v_i^{\text{despike}} \right) v ^ i = = = = = = = = S G ( v i despike ) 코드에서는 Median 기준선, Residual, MAD 기반 Sigma, (3.5\sigma) Clipping, SG filter 순서로 구현되어 있다.
17. Y와 Z에 서로 다른 필터 적용
경로의 Y와 Z는 서로 다른 특성을 가진다.
Y에는 Raster 및 경계 검출에 의한 짧은 좌우 Wave가 주로 존재한다.
Z에는 실제 안경 형상의 높이 변화와 급경사가 포함된다.
따라서 다음과 같이 서로 다른 창을 사용한다.
C ^ y ( s ) = = = = = = = = = = = RobustLP 7.0 ( C y ( s ) ) \hat C_y(s) =========== \operatorname{RobustLP}_{7.0} \left( C_y(s) \right) C ^ y ( s ) = = = = = = = = = = = R o b u s t L P 7 . 0 ( C y ( s ) ) C ^ z ( s ) = = = = = = = = = = = RobustLP 3.5 ( C z ( s ) ) \hat C_z(s) =========== \operatorname{RobustLP}_{3.5} \left( C_z(s) \right) C ^ z ( s ) = = = = = = = = = = = R o b u s t L P 3 . 5 ( C z ( s ) ) Step 2-1의 주요 파라미터는 다음과 같다.
파라미터 값 Median window 1.5 mm Y lateral window 7.0 mm Z height window 3.5 mm Width window 9.0 mm Direction window 7.0 mm Outlier threshold 3.5 sigma Polynomial order 3
이 값들은 AccuracyConfig에 정의되어 있다.
Z의 창을 Y보다 짧게 설정한 이유는 실제 급경사 형상을 최대한 유지하기 위해서이다.
18. 원본 경로 대비 보정량 제한
평활화를 과도하게 적용하면 실제 안경 형상이 바뀔 수 있다.
따라서 원본 경로에서 최종 보정 경로까지의 이동량을 다음 범위로 제한한다.
M = 0.30 mm M=0.30\text{ mm} M = 0 . 3 0 mm 원본 경로와 보정 후보 경로의 차이를 다음과 같이 정의한다.
\Delta\mathbf C_i ================= ## \mathbf C_i^{\text{candidate}} \mathbf C_i^{\text{raw}}
Scale은 다음과 같다.
α i = = = = = = = = min ( 1 , M ∣ Δ C i ∣ 2 ) \alpha_i ======== \min \left( 1, \frac{M}{ |\Delta\mathbf C_i|_2 } \right) α i = = = = = = = = min ( 1 , ∣ Δ C i ∣ 2 M ) 최종 제한 경로는 다음과 같다.
C i limited = = = = = = = = = = = = = = = = = = = = = = = = = = = = C i raw + α i Δ C i \mathbf C_i^{\text{limited}} ============================ \mathbf C_i^{\text{raw}} + \alpha_i \Delta\mathbf C_i C i limited = = = = = = = = = = = = = = = = = = = = = = = = = = = = C i raw + α i Δ C i 따라서 항상 다음 조건을 만족한다.
∣ C i limited − − − − − − − − − − − − − − − − − − − − − − − − − − − − C i raw ∣ 2 ≤ 0.30 mm \left| \mathbf C_i^{\text{limited}} ---------------------------- \mathbf C_i^{\text{raw}} \right|_2 \le 0.30\text{ mm} ∣ ∣ ∣ C i limited − − − − − − − − − − − − − − − − − − − − − − − − − − − − C i raw ∣ ∣ ∣ 2 ≤ 0 . 3 0 mm 코드에서도 보정 방향은 유지하면서 보정 벡터의 길이만 최대 (0.30\text{ mm})로 제한한다.
19. 실제 PCD 단면 재측정
Step 2-1에서는 Step 2가 계산한 경계만 믿지 않고, 각 경로점 주변의 얇은 면 PCD를 다시 측정한다.
경로점 (\mathbf C_i) 주변에서 다음 조건을 만족하는 PCD 점을 선택한다.
∣ x n − C x , i ∣ ≤ 0.08 mm |x_n-C_{x,i}| \le 0.08\text{ mm} ∣ x n − C x , i ∣ ≤ 0 . 0 8 mm ∣ z n − C z , i ∣ ≤ 0.08 mm |z_n-C_{z,i}| \le 0.08\text{ mm} ∣ z n − C z , i ∣ ≤ 0 . 0 8 mm ∣ y n − C y , i ∣ ≤ 2.00 mm |y_n-C_{y,i}| \le 2.00\text{ mm} ∣ y n − C y , i ∣ ≤ 2 . 0 0 mm 선택된 점들의 Y 좌표 집합을 다음과 같이 정의한다.
\mathcal Y_i ============ \left{ y_n ;\middle|; \begin{aligned} &|x_n-C_{x,i}|\le0.08\ &|z_n-C_{z,i}|\le0.08\ &|y_n-C_{y,i}|\le2.00 \end{aligned} \right}
이 단면은 경로 접선에 수직인 회전 단면이 아니라, X·Y·Z 좌표축에 정렬된 직육면체 영역이다.
즉, 현재 알고리즘은 다음을 가정한다.
경로의 주 진행 방향은 대체로 X 방향
본딩 면의 폭 방향은 대체로 Y 방향
19.1 Percentile을 사용하는 이유
선택된 Y 값의 최솟값과 최댓값을 사용하면 노이즈 점 하나가 전체 면 폭을 크게 왜곡할 수 있다.
따라서 다음 두 Percentile을 사용한다.
q_{0.02,i} ========== Q_{2%}(\mathcal Y_i)
q_{0.98,i} ========== Q_{98%}(\mathcal Y_i)
측정된 면 중앙은 다음과 같다.
C y , i PCD = = = = = = = = = = = = = = = = = = = = q 0.02 , i + q 0.98 , i 2 C_{y,i}^{\text{PCD}} ==================== \frac{ q_{0.02,i} + q_{0.98,i} }{2} C y , i PCD = = = = = = = = = = = = = = = = = = = = 2 q 0 . 0 2 , i + q 0 . 9 8 , i 측정된 면 폭은 다음과 같다.
w_i^{\text{PCD}} ================ ## q_{0.98,i} q_{0.02,i}
코드에서는 단면에 최소 10개 이상의 점이 있어야 해당 측정을 사용한다. 또한 전체 경로점 중 최소 80%에서 유효한 단면이 확보되어야 한다.
유효하지 않은 일부 구간은 인접한 유효 측정값으로 보간한다.
20. PCD 기반 중앙 경로 재보정
현재 평활화 경로와 실제 PCD 단면 중앙의 Y 차이를 계산한다.
e_i === ## C_{y,i}^{\text{PCD}} \hat C_{y,i}
이 오차에 다시 Robust low-pass를 적용한다.
e ˉ i = = = = = = = = RobustLP 7.0 ( e i ) \bar e_i ======== \operatorname{RobustLP}_{7.0}(e_i) e ˉ i = = = = = = = = R o b u s t L P 7 . 0 ( e i ) 단일 재중앙화량은 다음 범위로 제한한다.
− 0.35 ≤ e ˉ i ≤ 0.35 -0.35 \le \bar e_i \le 0.35 − 0 . 3 5 ≤ e ˉ i ≤ 0 . 3 5 즉,
e ˉ i clip = = = = = = = = = = = = = = = = = = = = = = clip ( e ˉ i , − 0.35 , 0.35 ) \bar e_i^{\text{clip}} ====================== \operatorname{clip} \left( \bar e_i, -0.35, 0.35 \right) e ˉ i clip = = = = = = = = = = = = = = = = = = = = = = c l i p ( e ˉ i , − 0 . 3 5 , 0 . 3 5 ) 경로 Y는 다음과 같이 이동한다.
C ^ y , i ← C ^ y , i + e ˉ i clip \hat C_{y,i} \leftarrow \hat C_{y,i} + \bar e_i^{\text{clip}} C ^ y , i ← C ^ y , i + e ˉ i clip 이후 원본 경로 대비 최종 3차원 이동량을 다시 (0.30\text{ mm})로 제한한다.
여기서 두 제한값의 역할은 다르다.
제한값 의미 (\pm0.35\text{ mm}) PCD 중앙으로 이동시키는 Y 보정량 제한 (0.30\text{ mm}) 원본 경로 대비 최종 3차원 보정량 제한
따라서 최종 결과는 항상 원본 경로에서 3차원 거리 기준 (0.30\text{ mm}) 이내에 존재한다.
21. 면 폭과 방향 재구성
원본 Outer와 Inner의 차이를 폭 벡터로 정의한다.
r i = = = = = = = = = = = O i − I i \mathbf r_i =========== \mathbf O_i-\mathbf I_i r i = = = = = = = = = = = O i − I i 원본 폭은 다음과 같다.
w i raw = = = = = = = = = = = = = = = = ∣ r i ∣ 2 w_i^{\text{raw}} ================ |\mathbf r_i|_2 w i raw = = = = = = = = = = = = = = = = ∣ r i ∣ 2 원본 폭 방향 단위벡터는 다음과 같다.
d i raw = = = = = = = = = = = = = = = = = = = = = = = = r i ∣ r i ∣ 2 \mathbf d_i^{\text{raw}} ======================== \frac{ \mathbf r_i }{ |\mathbf r_i|_2 } d i raw = = = = = = = = = = = = = = = = = = = = = = = = ∣ r i ∣ 2 r i 21.1 폭 평활화
Percentile 단면에서 측정한 폭에 (9.0\text{ mm}) Robust low-pass를 적용한다.
w ^ i = = = = = = = = RobustLP 9.0 ( w i PCD ) \hat w_i ======== \operatorname{RobustLP}_{9.0} \left( w_i^{\text{PCD}} \right) w ^ i = = = = = = = = R o b u s t L P 9 . 0 ( w i PCD ) 폭이 0 또는 음수가 되지 않도록 최소 폭을 설정한다.
w ^ i ≥ 0.05 mm \hat w_i \ge 0.05\text{ mm} w ^ i ≥ 0 . 0 5 mm 21.2 폭 방향 평활화
폭 방향 벡터의 X, Y, Z 성분을 각각 평활화한다.
d ~ i = = = = = = = = = = = = = = = = = = = [ LP ( d x , i ) LP ( d y , i ) LP ( d z , i ) ] \tilde{\mathbf d}_i =================== \begin{bmatrix} \operatorname{LP}(d_{x,i})\ \operatorname{LP}(d_{y,i})\ \operatorname{LP}(d_{z,i}) \end{bmatrix} d ~ i = = = = = = = = = = = = = = = = = = = [ L P ( d x , i ) L P ( d y , i ) L P ( d z , i ) ] 성분별 필터링 이후에는 벡터의 크기가 1이 아닐 수 있으므로 다시 정규화한다.
d ^ i = = = = = = = = = = = = = = = = = d ~ i ∣ d ~ i ∣ 2 \hat{\mathbf d}_i ================= \frac{ \tilde{\mathbf d}_i }{ |\tilde{\mathbf d}_i|_2 } d ^ i = = = = = = = = = = = = = = = = = ∣ d ~ i ∣ 2 d ~ i 21.3 Outer와 Inner 재구성
최종 중앙 경로를 기준으로 폭의 절반만큼 양쪽으로 이동한다.
O ^ i = = = = = = = = = = = = = = = = = C ^ i + w ^ i 2 d ^ i \hat{\mathbf O}_i ================= \hat{\mathbf C}_i + \frac{\hat w_i}{2} \hat{\mathbf d}_i O ^ i = = = = = = = = = = = = = = = = = C ^ i + 2 w ^ i d ^ i \hat{\mathbf I}_i ================= ## \hat{\mathbf C}_i \frac{\hat w_i}{2} \hat{\mathbf d}_i
이 구조에서는 항상 다음 관계가 성립한다.
O ^ i + I ^ i 2 = = = = C ^ i \frac{ \hat{\mathbf O}_i + \hat{\mathbf I}_i }{2} ==== \hat{\mathbf C}_i 2 O ^ i + I ^ i = = = = C ^ i 그리고 재구성된 경계 사이의 거리는 다음과 같다.
∣ O ^ i − − − − − − − − − − − − − − − − − I ^ i ∣ 2 = = = = = = = = = w ^ i \left| \hat{\mathbf O}_i ----------------- \hat{\mathbf I}_i \right|_2 ========= \hat w_i ∣ ∣ ∣ ∣ O ^ i − − − − − − − − − − − − − − − − − I ^ i ∣ ∣ ∣ ∣ 2 = = = = = = = = = w ^ i 코드에서도 PCD 단면 폭과 평활화된 원본 폭 방향을 이용해 Outer와 Inner를 중앙 경로 기준으로 대칭 재구성한다.
22. 최종 접선 재계산
경로의 Y와 Z가 변경되었으므로 Step 2의 접선을 그대로 사용할 수 없다.
먼저 보정된 경로의 누적 거리를 다시 계산한다.
s ^ i = = = = = = = = ∑ k = 1 i ∣ C ^ k − − − − − − − − − − − − − − − − − C ^ k − 1 ∣ 2 \hat s_i ======== \sum_{k=1}^{i} \left| \hat{\mathbf C}_k ----------------- \hat{\mathbf C}_{k-1} \right|_2 s ^ i = = = = = = = = k = 1 ∑ i ∣ ∣ ∣ ∣ C ^ k − − − − − − − − − − − − − − − − − C ^ k − 1 ∣ ∣ ∣ ∣ 2 이 거리 기준으로 경로를 미분한다.
v ^ i = = = = = = = = = = = = = = = = = d C ^ d s ^ \hat{\mathbf v}_i ================= \frac{ d\hat{\mathbf C} }{ d\hat s } v ^ i = = = = = = = = = = = = = = = = = d s ^ d C ^ 단위 접선은 다음과 같다.
t ^ i = = = = = = = = = = = = = = = = = v ^ i ∣ v ^ i ∣ 2 \hat{\mathbf t}_i ================= \frac{ \hat{\mathbf v}_i }{ |\hat{\mathbf v}_i|_2 } t ^ i = = = = = = = = = = = = = = = = = ∣ v ^ i ∣ 2 v ^ i 코드에서도 보정된 경로의 누적 거리와 접선을 다시 계산한 뒤 정규화한다.
이 접선이 Step 3의 Quaternion trajectory 계산에 전달된다.
23. Accuracy Metric 해석
Step 2-1은 단순히 경로를 저장하는 것뿐 아니라 보정 효과를 수치로 평가한다.
23.1 중앙 경로 보정량
각 점의 보정 크기는 다음과 같다.
c i = = = ∣ C ^ i − − − − − − − − − − − − − − − − − C i raw ∣ 2 c_i === \left| \hat{\mathbf C}_i ----------------- \mathbf C_i^{\text{raw}} \right|_2 c i = = = ∣ ∣ ∣ ∣ C ^ i − − − − − − − − − − − − − − − − − C i raw ∣ ∣ ∣ ∣ 2 확인할 값은 다음과 같다.
Maximum은 설정상 (0.30\text{ mm})를 초과하지 않아야 한다.
23.2 PCD 면 중앙 오차
보정 전 중앙 오차는 다음과 같다.
e_i^{\text{before}} =================== ## C_{y,i}^{\text{PCD}} C_{y,i}^{\text{raw}}
보정 후 중앙 오차는 다음과 같다.
e_i^{\text{after}} ================== ## C_{y,i}^{\text{PCD}} \hat C_{y,i}
Signed mean은 전체 경로가 어느 방향으로 치우쳐 있는지를 나타낸다.
e ˉ = = = = = = 1 N ∑ i e i \bar e ====== \frac1N \sum_i e_i e ˉ = = = = = = N 1 i ∑ e i 절댓값 P95는 전체 경로점 중 95%의 중앙 오차가 어느 범위 안에 있는지를 나타낸다.
P 95 ( ∣ e i ∣ ) P_{95} \left( |e_i| \right) P 9 5 ( ∣ e i ∣ ) 23.3 고주파 RMS
경로 신호에서 긴 SG 추세선을 제거한다.
h i = = = y i − y ˉ i h_i === y_i-\bar y_i h i = = = y i − y ˉ i 고주파 RMS는 다음과 같다.
RMS H F = = = = = = = = = = = = = = = = = = = = = = = 1 N ∑ i h i 2 \operatorname{RMS}_{HF} ======================= \sqrt{ \frac1N \sum_i h_i^2 } R M S H F = = = = = = = = = = = = = = = = = = = = = = = N 1 i ∑ h i 2 보정 후 RMS가 감소하면 짧은 Y 방향 Wave가 줄었다는 의미이다.
코드에서도 신호와 Savitzky-Golay 추세의 차이에 대한 RMS를 계산한다.
23.4 XY Heading Total Variation
각 경로 구간의 XY 진행각은 다음과 같다.
ψ i = = = = = = atan2 ( y i + 1 − y i , x i + 1 − x i ) \psi_i ====== \operatorname{atan2} \left( y_{i+1}-y_i, x_{i+1}-x_i \right) ψ i = = = = = = a t a n 2 ( y i + 1 − y i , x i + 1 − x i ) 각도를 Unwrap한 뒤 총 변화량을 계산한다.
T V ψ = = = = = = = ∑ i ∣ ψ i + 1 − ψ i ∣ TV_\psi ======= \sum_i \left| \psi_{i+1}-\psi_i \right| T V ψ = = = = = = = i ∑ ∣ ψ i + 1 − ψ i ∣ 이 값이 감소하면 로봇 진행 방향의 좌우 흔들림이 줄었다는 의미이다.
23.5 본딩 면 폭 통계
면 폭 평균은 다음과 같다.
w ˉ = = = = = = 1 N ∑ i w i \bar w ====== \frac1N \sum_iw_i w ˉ = = = = = = N 1 i ∑ w i 면 폭 표준편차는 다음과 같다.
σ w = = = = = = = = 1 N ∑ i ( w i − w ˉ ) 2 \sigma_w ======== \sqrt{ \frac1N \sum_i (w_i-\bar w)^2 } σ w = = = = = = = = N 1 i ∑ ( w i − w ˉ ) 2 보정 후 표준편차가 줄어들면 순간적인 폭 변화가 제거되었다는 의미이다.
다만 실제 제품의 폭이 위치에 따라 변하는 구조라면 표준편차가 지나치게 작아지는 것도 과평활화일 수 있다.
코드에서는 보정량, PCD 중앙 오차, 고주파 RMS, Heading 변화량 및 폭 통계를 JSON으로 저장한다.
24. 간단한 계산 예제
어떤 X 위치에서 Outer와 Inner가 다음과 같이 검출되었다고 가정한다.
O i = = = = = = = = = = = [ 10.0 20.8 − 3.25 ] mm \mathbf O_i =========== \begin{bmatrix} 10.0\ 20.8\ -3.25 \end{bmatrix} \text{ mm} O i = = = = = = = = = = = [ 1 0 . 0 2 0 . 8 − 3 . 2 5 ] mm I i = = = = = = = = = = = [ 10.0 19.4 − 3.25 ] mm \mathbf I_i =========== \begin{bmatrix} 10.0\ 19.4\ -3.25 \end{bmatrix} \text{ mm} I i = = = = = = = = = = = [ 1 0 . 0 1 9 . 4 − 3 . 2 5 ] mm 중앙점은 다음과 같다.
C i = = = = = = = = = = = O i + I i 2 = = = = [ 10.0 20.1 − 3.25 ] mm \mathbf C_i =========== \frac{ \mathbf O_i+\mathbf I_i }{2} ==== \begin{bmatrix} 10.0\ 20.1\ -3.25 \end{bmatrix} \text{ mm} C i = = = = = = = = = = = 2 O i + I i = = = = [ 1 0 . 0 2 0 . 1 − 3 . 2 5 ] mm 폭은 다음과 같다.
w_i === # |\mathbf O_i-\mathbf I_i|_2 1.4\text{ mm}
다음 경로점이 다음과 같다고 가정한다.
C i + 1 = = = = = = = = = = = = = = = [ 10.5 20.12 − 2.95 ] \mathbf C_{i+1} =============== \begin{bmatrix} 10.5\ 20.12\ -2.95 \end{bmatrix} C i + 1 = = = = = = = = = = = = = = = [ 1 0 . 5 2 0 . 1 2 − 2 . 9 5 ] 변화량은 다음과 같다.
Δ x = 0.5 , Δ y = 0.02 , Δ z = 0.30 \Delta x=0.5,\qquad \Delta y=0.02,\qquad \Delta z=0.30 Δ x = 0 . 5 , Δ y = 0 . 0 2 , Δ z = 0 . 3 0 XY 이동량은 다음과 같다.
Δ r x y = = = = = = = = = = = = = 0. 5 2 + 0.0 2 2 ≈ 0.5004 mm \Delta r_{xy} ============= \sqrt{ 0.5^2+0.02^2 } \approx 0.5004\text{ mm} Δ r x y = = = = = = = = = = = = = 0 . 5 2 + 0 . 0 2 2 ≈ 0 . 5 0 0 4 mm 경사각은 다음과 같다.
θ = = = = = = atan2 ( 0.30 , 0.5004 ) ≈ 30.9 4 ∘ \theta ====== \operatorname{atan2} \left( 0.30, 0.5004 \right) \approx 30.94^\circ θ = = = = = = a t a n 2 ( 0 . 3 0 , 0 . 5 0 0 4 ) ≈ 3 0 . 9 4 ∘ 따라서 (55^\circ)보다 작으므로 양 끝 직각 전환부로 판정되지 않는다.
24.1 Percentile 단면 예제
PCD 단면 측정 결과가 다음과 같다고 가정한다.
Y_{2%} ====== 19.45\text{ mm}
Y_{98%} ======= 20.75\text{ mm}
측정 중앙은 다음과 같다.
C y PCD = = = = = = = = = = = = = = = = 19.45 + 20.75 2 = = = = 20.10 mm C_y^{\text{PCD}} ================ \frac{ 19.45+20.75 }{2} ==== 20.10\text{ mm} C y PCD = = = = = = = = = = = = = = = = 2 1 9 . 4 5 + 2 0 . 7 5 = = = = 2 0 . 1 0 mm 측정 폭은 다음과 같다.
w^{\text{PCD}} ============== # 20.75-19.45 1.30\text{ mm}
현재 평활화된 경로 Y가 (20.18\text{ mm})라면 중앙 오차는 다음과 같다.
e = # 20.10-20.18 -0.08\text{ mm}
따라서 경로는 Y 음의 방향으로 약 (0.08\text{ mm}) 이동하게 된다.
25. 결과 그림 해석
25.1 XY Top View
위에서 본 그림에서는 다음 요소를 확인한다.
높이에 따라 색상이 지정된 얇은 본딩 면 PCD
보정된 Outer edge
보정된 Inner edge
최종 중앙 경로
Robust 경계 밖에 존재하는 원본 PCD 점
2%와 98% Percentile을 사용하므로 소수의 원본 점이 보정 경계 밖에 남는 것은 정상이다.
최종 시각화 코드에서도 Robust 경계 내부의 점은 높이 색상으로 표시하고, 경계 밖의 점은 회색으로 분리한다.
25.2 XZ Side View
XZ 그림에서는 경로 높이와 구간별 경사각을 확인한다.
각 구간의 경사각은 다음과 같다.
θ i = = = = = = = = atan2 ( ∣ Δ Z i ∣ , ( Δ X i ) 2 + ( Δ Y i ) 2 ) \theta_i ======== \operatorname{atan2} \left( |\Delta Z_i|, \sqrt{ (\Delta X_i)^2+ (\Delta Y_i)^2 } \right) θ i = = = = = = = = a t a n 2 ( ∣ Δ Z i ∣ , ( Δ X i ) 2 + ( Δ Y i ) 2 ) 경로 선분의 색상이 경사각을 나타내므로 다음을 확인할 수 있다.
급경사 구간의 위치
좌우 끝단 전환부
평활화 이후 실제 높이 형상이 유지되었는지
경로 중간에 비정상적인 경사 급변이 존재하는지
최종 시각화에서도 XZ 경로의 선분 색상을 XY 평면 기준 경사각으로 표시한다.
26. 주요 파라미터 튜닝 가이드
파라미터 역할 너무 작을 때 너무 클 때 Raster 0.02 mm XY 높이 맵 해상도 계산량과 메모리 증가 경계 계단 및 위치 오차 증가 Inner search 0.35~2.80 mm Inner edge 탐색 범위 실제 경계 누락 다른 홈을 경계로 오검출 Gaussian sigma 0.03 mm 미분 전 노이즈 완화 미분 노이즈 증가 경계가 지나치게 퍼짐 Edge strength 0.15 최소 경계 기울기 약한 노이즈도 경계로 검출 완만한 실제 경계 누락 Edge SG 3.0 mm XY 경계 평활화 Raster wave 잔존 실제 XY 곡률 왜곡 Height SG 1.2 mm Step 2 높이 평활화 Z 진동 잔존 급경사 형상 완화 Top-face tolerance 0.15 mm 얇은 면 높이 범위 면 PCD가 부족해짐 홈과 옆면 점 포함 Output spacing 0.5 mm Waypoint 간격 IK 계산량 증가 곡선 재현 성능 저하 Y window 7.0 mm Step 2-1 좌우 Wave 제거 짧은 Wave 잔존 실제 Y 곡률 평탄화 Z window 3.5 mm Step 2-1 높이 진동 제거 Z 노이즈 잔존 실제 급경사 손실 Width window 9.0 mm 폭 변화 안정화 폭 급변 잔존 실제 폭 변화 손실 Outlier 3.5 sigma 이상치 제한 정상 변화까지 제한 큰 돌출점 잔존 Recenter ±0.35 mm 단면 중앙 추종 한계 큰 초기 오차 미보정 잘못 측정한 단면을 과도하게 추종 Final correction 0.30 mm 원본 형상 보존 충분한 재보정이 어려움 원본 경로에서 크게 이탈 가능
27. 알고리즘의 주요 가정과 한계
27.1 하나의 XY 위치에 하나의 표면만 존재
Top-envelope는 하나의 XY 위치에 하나의 Z만 저장한다.
따라서 Overhang처럼 하나의 XY 위치에 여러 층의 표면이 존재하는 구조는 정확하게 표현하기 어렵다.
27.2 최소 Local Z가 실제 윗면이라는 가정
현재 Isaac 좌표 변환에서는 최소 Local Z를 윗면으로 사용한다.
좌표 변환이 바뀌어 Local Z 방향이 물리 높이 방향과 같아지면 다음과 같이 변경해야 할 수 있다.
Z top = = = = = = = = = = = = = = max z Z_{\text{top}} ============== \max z Z top = = = = = = = = = = = = = = max z 27.3 Outer edge가 (+Y) 방향이라는 가정
현재 Outer edge는 각 X 열의 최대 Y이다.
O y ( x ) = = = = = = max Y O_y(x) ====== \max Y O y ( x ) = = = = = = max Y 제품이 회전하거나 반대로 놓이면 올바른 경계를 검출하지 못할 수 있다.
27.4 Inner edge 미분 부호 의존성
현재는 다음 위치를 Inner edge로 선택한다.
arg min d Z d Y \arg\min \frac{dZ}{dY} arg min d Y d Z 좌표계가 반전되면 다음 방식이 필요할 수 있다.
arg max d Z d Y \arg\max \frac{dZ}{dY} arg max d Y d Z 또는
arg max ∣ d Z d Y ∣ \arg\max \left| \frac{dZ}{dY} \right| arg max ∣ ∣ ∣ ∣ ∣ d Y d Z ∣ ∣ ∣ ∣ ∣ 27.5 축 정렬 단면 사용
Step 2-1의 PCD 단면은 경로 접선에 수직인 단면이 아니다.
현재는 다음과 같은 고정 좌표축 기준 단면이다.
X = ±0.08 mm
Z = ±0.08 mm
Y = ±2.00 mm경로 방향이 X축에서 크게 회전하는 형상이라면 접선 기반 회전 단면을 사용하는 것이 더 일반적이다.
폭 방향 (\mathbf d_i)를 접선 (\mathbf t_i)에 직교화하면 다음과 같다.
b i = = = = = = = = = = = d i − − − − − − − − − − − ( d i ⋅ t i ) t i ∣ d i − − − − − − − − − − − ( d i ⋅ t i ) t i ∣ 2 \mathbf b_i =========== \frac{ \mathbf d_i ----------- (\mathbf d_i\cdot\mathbf t_i)\mathbf t_i }{ \left| \mathbf d_i ----------- (\mathbf d_i\cdot\mathbf t_i)\mathbf t_i \right|_2 } b i = = = = = = = = = = = ∣ d i − − − − − − − − − − − ( d i ⋅ t i ) t i ∣ 2 d i − − − − − − − − − − − ( d i ⋅ t i ) t i 이 (\mathbf b_i) 방향으로 PCD를 투영하면 경로 방향이 바뀌더라도 일관된 폭을 측정할 수 있다.
현재 코드에는 이 방식이 직접 적용되어 있지 않다.
27.6 Step 2-1에서 Z를 Percentile로 재측정하지 않음
Step 2-1의 단면 재측정으로 얻는 값은 다음 두 가지이다.
Z는 PCD Percentile로 다시 계산하지 않고, Step 2의 Z 경로를 Robust low-pass로 평활화한다.
따라서 Step 2에서 Z가 구조적으로 잘못 검출된 경우 Step 2-1만으로 완전히 수정하기는 어렵다.
28. Step 3과의 연결
Step 2-1은 최종 위치와 접선을 제공한다.
하지만 접선 하나만으로는 3차원 회전 자세를 완전히 결정할 수 없다.
접선에 수직인 회전 방향이 무한히 존재하기 때문이다.
따라서 Step 3에서는 폭 방향 또는 표면 Normal과 같은 추가 방향이 필요하다.
폭 방향을 접선에 직교화하면 다음과 같다.
b i = = = = = = = = = = = d ^ i − − − − − − − − − − − − − − − − − ( d ^ i ⋅ t ^ i ) t ^ i ∣ d ^ i − − − − − − − − − − − − − − − − − ( d ^ i ⋅ t ^ i ) t ^ i ∣ 2 \mathbf b_i =========== \frac{ \hat{\mathbf d}_i ----------------- (\hat{\mathbf d}_i\cdot\hat{\mathbf t}_i) \hat{\mathbf t}_i }{ \left| \hat{\mathbf d}_i ----------------- (\hat{\mathbf d}_i\cdot\hat{\mathbf t}_i) \hat{\mathbf t}_i \right|_2 } b i = = = = = = = = = = = ∣ ∣ ∣ ∣ d ^ i − − − − − − − − − − − − − − − − − ( d ^ i ⋅ t ^ i ) t ^ i ∣ ∣ ∣ ∣ 2 d ^ i − − − − − − − − − − − − − − − − − ( d ^ i ⋅ t ^ i ) t ^ i 표면 Normal은 외적으로 계산할 수 있다.
n i = = = = = = = = = = = t ^ i × b i \mathbf n_i =========== \hat{\mathbf t}_i \times \mathbf b_i n i = = = = = = = = = = = t ^ i × b i 이 세 축으로 회전행렬을 만들 수 있다.
R i = = = [ t ^ i b i n i ] R_i === \begin{bmatrix} \hat{\mathbf t}_i& \mathbf b_i& \mathbf n_i \end{bmatrix} R i = = = [ t ^ i b i n i ] 이후 회전행렬을 Quaternion으로 변환하고, 로봇의 연속 IK를 수행한다.
Step 2에서는 Local Z Offset을 기본적으로 0으로 유지하며, 실제 위쪽 안전 높이는 Step 3의 surface_offset_mm에서 World (+Z) 방향으로 적용한다.
최종 로봇 위치는 개념적으로 다음과 같다.
P i robot = = = = = = = = = = = = = = = = = = = = = = = = = = T world ← local ( C ^ ∗ i ) + h ∗ safe [ 0 0 1 ] world \mathbf P_i^{\text{robot}} ========================== T_{\text{world}\leftarrow\text{local}} \left( \hat{\mathbf C}*i \right) + h*{\text{safe}} \begin{bmatrix} 0\ 0\ 1 \end{bmatrix}_{\text{world}} P i robot = = = = = = = = = = = = = = = = = = = = = = = = = = T world ← local ( C ^ ∗ i ) + h ∗ safe [ 0 0 1 ] world 여기서 (h_{\text{safe}})가 Step 3의 surface_offset_mm이다.
29. 실행 방법
전체 파이프라인 실행
cd /home/happy/modelsoultion
./lee_ws/step2_pcd_path/run_step2_pipeline.sh저장된 결과를 이용해 PNG만 다시 생성
python3 lee_ws/step2_pcd_path/make_step2_result_figures.py단계별 개별 실행
python3 lee_ws/step2_pcd_path/generate_global_path.py
python3 lee_ws/step2_pcd_path/smooth_global_path.py
python3 lee_ws/step2_pcd_path/make_step2_result_figures.py30. 출력 파일 정리
Step 2 출력
step2_global_path.csv
step2_global_path.pcd
step2_global_path.npz
step2_thin_bonding_face.pcd
step2_global_path_metadata.jsonStep 2 코드에는 기본 경로 CSV, PCD, NPZ, 얇은 면 PCD 및 Metadata 출력 파일이 정의되어 있다.
Step 2-1 출력
step2_1_accurate_global_path.csv
step2_1_accurate_global_path.pcd
step2_1_accurate_global_path.npz
step2_1_accuracy_metrics.jsonStep 2-1 코드에서도 보정 경로와 Accuracy metric 출력 파일이 별도로 정의되어 있다.
31. 공부해야 할 핵심 이론
이번 알고리즘은 단순한 경로 생성 코드가 아니라 여러 분야의 개념이 함께 사용된 구조이다.
Point Cloud Processing
Point Cloud
↓
ROI
↓
Rasterization
↓
Occupancy Map
↓
Top-envelope관련 개념은 다음과 같다.
PCD 데이터 구조
ROI
Rasterization
Height Map
Occupancy Map
KD-tree 최근접 탐색
Digital Signal Processing
다음 데이터들은 1차원 신호로 볼 수 있다.
z ( y ) , y ( s ) , z ( s ) , w ( s ) z(y),\qquad y(s),\qquad z(s),\qquad w(s) z ( y ) , y ( s ) , z ( s ) , w ( s ) 관련 개념은 다음과 같다.
Gaussian Low-pass Filter
Median Filter
Savitzky-Golay Filter
수치 미분
고주파와 저주파
필터 창 길이
위상 지연
Robust Statistics
이번 알고리즘에서는 평균과 표준편차만 사용하는 대신 다음 개념을 사용한다.
Median \operatorname{Median} M e d i a n MAD \operatorname{MAD} M A D Q_{2%},\quad Q_{98%}
이러한 통계량은 소수의 큰 PCD 이상점에 강하다.
Differential Geometry
경로를 3차원 곡선으로 바라보면 다음 개념이 중요하다.
t ( s ) = = = = = = = = = = = = d C / d s ∣ d C / d s ∣ 2 \mathbf t(s) ============ \frac{ d\mathbf C/ds }{ |d\mathbf C/ds|_2 } t ( s ) = = = = = = = = = = = = ∣ d C / d s ∣ 2 d C / d s θ = = = = = = atan2 ( ∣ d Z ∣ , d X 2 + d Y 2 ) \theta ====== \operatorname{atan2} \left( |dZ|, \sqrt{dX^2+dY^2} \right) θ = = = = = = a t a n 2 ( ∣ d Z ∣ , d X 2 + d Y 2 ) Step 3에서는 접선, 폭 방향, Normal을 사용해 로봇의 3차원 자세를 계산한다.
32. 마무리
이번 Step 2와 Step 2-1 파이프라인의 핵심은 다음과 같이 정리할 수 있다.
Step 2
원본 PCD에서 본딩 면의 기본 중앙 경로를 검출 \boxed{ \text{원본 PCD에서 본딩 면의 기본 중앙 경로를 검출} } 원본 PCD 에서 본딩 면의 기본 중앙 경로를 검출 핵심 수식은 다음과 같다.
Z top ( x , y ) = = = = = = = = = = = = = = = = = = = min z Z_{\text{top}}(x,y) =================== \min z Z top ( x , y ) = = = = = = = = = = = = = = = = = = = min z O_y(x) ====== \max \left{ y \mid occupied(x,y) \right}
I y ( x ) = = = = = = arg min y d z ~ ( x , y ) d y I_y(x) ====== \arg\min_y \frac{ d\tilde z(x,y) }{ dy } I y ( x ) = = = = = = arg y min d y d z ~ ( x , y ) C y ( x ) = = = = = = O y ( x ) + I y ( x ) 2 C_y(x) ====== \frac{ O_y(x)+I_y(x) }{2} C y ( x ) = = = = = = 2 O y ( x ) + I y ( x ) C z ( x ) = = = = = = Z top ( x , C y ( x ) ) C_z(x) ====== Z_{\text{top}} \left( x,C_y(x) \right) C z ( x ) = = = = = = Z top ( x , C y ( x ) ) Step 2-1
경로의 국부 진동을 제거하고 실제 PCD 면 중앙으로 제한 보정 \boxed{ \text{경로의 국부 진동을 제거하고 실제 PCD 면 중앙으로 제한 보정} } 경로의 국부 진동을 제거하고 실제 PCD 면 중앙으로 제한 보정 핵심 수식은 다음과 같다.
σ ^ = = = = = = = = = = 1.4826 median ( ∣ r i − median ( r ) ∣ ) \hat\sigma ========== 1.4826 \operatorname{median} \left( |r_i-\operatorname{median}(r)| \right) σ ^ = = = = = = = = = = 1 . 4 8 2 6 m e d i a n ( ∣ r i − m e d i a n ( r ) ∣ ) C_y^{\text{PCD}} ================ \frac{ Q_{2%}(Y)+Q_{98%}(Y) }{2}
w^{\text{PCD}} ============== Q_{98%}(Y)-Q_{2%}(Y)
∣ C ^ i − − − − − − − − − − − − − − − − − C i raw ∣ 2 ≤ 0.30 mm \left| \hat{\mathbf C}_i ----------------- \mathbf C_i^{\text{raw}} \right|_2 \le 0.30\text{ mm} ∣ ∣ ∣ ∣ C ^ i − − − − − − − − − − − − − − − − − C i raw ∣ ∣ ∣ ∣ 2 ≤ 0 . 3 0 mm O ^ = = = = = = = = = = = = = = = C ^ + w ^ 2 d ^ \hat{\mathbf O} =============== \hat{\mathbf C} + \frac{\hat w}{2} \hat{\mathbf d} O ^ = = = = = = = = = = = = = = = C ^ + 2 w ^ d ^ \hat{\mathbf I} =============== ## \hat{\mathbf C} \frac{\hat w}{2} \hat{\mathbf d}
결론적으로 이 알고리즘은 다음 기술이 결합된 경로 생성 파이프라인이다.
Point Cloud Processing
+
Edge Detection
+
Digital Signal Processing
+
Robust Statistics
+
3D Curve Geometry
+
Robot Path PlanningStep 2는 PCD에서 기하학적인 초기 경로를 생성하고, Step 2-1은 실제 PCD 단면을 이용해 해당 경로를 강건하게 보정한다.
이후 Step 3에서는 최종 중앙 경로와 접선을 이용해 Quaternion trajectory와 연속 IK를 계산하고, 실제 로봇이 본딩 면을 따라 이동할 수 있는 자세 궤적을 생성한다.
#PointCloud #PCD #RobotPathPlanning #Robotics #SavitzkyGolay #RobustStatistics #Quaternion #InverseKinematics
