로봇 자세 불확실성을 고려한 재투영 기반 AXB=YCZD 이중로봇 시스템 캘리브레이션

3.1 이중로봇 캘리브레이션 모델

본 연구에서는 기존 AXB=YCZ 형태로 정의된 캘리브레이션 문제에 센서 하나를 추가로 도입하고 외부의 캘리브레이션 객체를 활용한 캘리브레이션 체인을 정의한다^[8]. 이를 그림으로 나타내면 [Fig. 1]과 같으며, 파란색은 사전에 알고 있는 관측 변환, 빨간색은 캘리브레이션의 대상이 되는 미지 변환이고 식으로 나타내면 (1)과 같다.

[Fig. 1]

Dual-robot system AXB=YCZD calibration chain. Blue: known measurement data. Red: unknown parameters

미지 변환 $$ X_{c1}^{e1}$$는 로봇1의 엔드이펙터 E₁에서 센서1 C₁간의 동차 변환 행렬이고, $$ Y_{b2}^{b1}$$는 로봇1의 베이스 B₁에서 로봇2의 베이스 B₂간의 동차 변환 행렬이며, $$ Z_{c2}^{e2}$$는 로봇2의 엔드이펙터 E₂에서 센서2 C₂ 간의 동차 변환 행렬이다. 측정 데이터 $$ A_{e1}^{b1}$$는 로봇1의 베이스 B₁에서 로봇1의 엔드이펙터 E₁간의 동차 변환 행렬이고, $$ B_o^{c1}$$는 센서1에서 관측한 캘리브레이션 객체 O의 자세이며, $$ C_{e2}^{b2}$$는 로봇2의 베이스 B₂에서 로봇2의 엔드이펙터 E₂간의 동차 변환 행렬이고, $$ D_o^{c2}$$는 센서2 C₂에서 관측한 캘리브레이션 객체 O의 자세이다. 이때 $$ B_o^{c1}$$과 $$ D_o^{c2}$$는 Perspective-n-Points (PnP) 알고리즘을 통해 계산된 값이며, 본 연구에선 알고리즘에서 발생하는 불확실성을 고려하지 않는다.

본 연구는 캘리브레이션 보드(Calibration board)를 활용한 타겟 기반의 캘리브레이션 방법을 다루며, 캘리브레이션 타겟 좌표계에서 정의된 3차원 점들을 로봇에 부착된 센서로 투영시키는 투영 함수를 이중로봇 시스템에서 구성한다. 캘리브레이션 데이터는 두 로봇의 서로 다른 n_r개의 자세 쌍으로 취득되었으며 보드 좌표계(O) 위의 n_w개의 3차원 보드 점들을 $$ p_k\left(k=1,\dots ,n_w\right)$$라 하고, 각 점이 이미지로 투영된 2차원 점들을 p_j,k라 하면 로봇 자세 j에서 보드 좌표계에 존재하는 3차원 점의 센서1 이미지를 향한 투영 모델을 식 (2)와 같이 구성할 수 있다.

여기서 $$ \pi \left(p_c,i\right)$$는 투영 연산을 의미하며 본 연구에선 핀홀(Pinhole) 모델^[11]과 디비전 왜곡 모델(Division distortion model)^[12]을 결합한 투영 모델을 사용하였다. 반대로 센서2 이미지를 향한 투영은 식 (3)과 같이 표현할 수 있다.

본 논문에선 기본적으로 $$ p_{\left(j,k\right)}^{\left(1\right)}$$를 관측으로 사용한다. 이후 추정 모델에서 각 동차 변환은 오일러(Euler) 각도로 표현된 회전 행렬 $$ R=R_x\left(\alpha \right)R_y\left(\beta \right)R_z\left(\gamma \right)$$, 병진 벡터 $$ t={\left(t_x, t_y, t_z\right)}^T$$와 함께 $$ e=\left(\alpha ,\beta ,\gamma ,t_x,t_y,t_z\right)$$형식으로 파라미터화 된다. 파라미터화 된 캘리브레이션 데이터 쌍 $$ {\left\{A_{e1, j}^{b1}, B_{o, j}^{c1}, C_{e2, j}^{b2}, D_{o, j}^{c2}\right\}}_{j=1}^{n_r}$$를 각각 $$ {\left\{e_{A,j},e_{B,j},e_{C,j},e_{D,j}\right\}}_{j=1}^{n_r}$$, 미지 변환 $$ X_{c1}^{e1}, Y_{b2}^{b1},Z_{c2}^{e2}$$를 $$ e_X,e_Y,e_Z$$로 표기한다.

3.2 추정 모델

관측된 로봇 자세를 0-평균의 가우시안(Gaussian) 분포로 가정하여 확률모델을 통해 추정하는 것이 유효함이 보고되었다^[10]. 본 절에선 이를 이중로봇 시스템으로 확장한 프레임워크를 제안하고자 하며 알고리즘의 전체 흐름은 [Fig. 2]에, 단계별 요약은 [Table 1]에 정리하였다. 로봇 자세의 불확실성을 고려한 가우스-마르코프(Gauss-Markov) 모델을 구성하는 아이디어는 로봇 자세를 관측이자 미지 변환으로 보는 가상 관측(Fictitious observation) 개념을 기반으로 한다. 식 (2)와 같이 정의된 이미지 픽셀 관측함수 이외에 $$ f\left(\cdot \right)$$를 항등함수로 두고, 센서로부터 측정된 로봇 자세 e_A,j, e_C,j는 확률모델의 관측 벡터에 e_A,j = f(e_A,j) 형태로 추가된다. 따라서 이중로봇 시스템에서의 관측 벡터 l의 구성은 투영된 이미지 픽셀, 로봇1의 자세, 로봇2의 자세로 확장되며 관측 벡터 l과 가중치 행렬 Q_ll은 식 (4), (5)와 같이 구성된다.

[Fig. 2]

Pseudocode for Gauss-Markov based parameter estimation and variance component estimation

[Table 1]

Summary of the parameter and VCE estimation procedure

이때 n_i는 투영된 픽셀의 수, $$ {\sigma }_i^2,{\sigma }_{a,1}^2,{\sigma }_{t,1}^2,{\sigma }_{a,2}^2,{\sigma }_{t,2}^2$$는 각 파라미터에 해당하는 분산 성분이며 rep(A, n) 표기는 A라는 요소가 n번 반복되어 대각 행렬의 성분을 구성함을 나타낸다. 이중로봇 시스템에 따라 각 로봇에 대해 개별적으로 분산을 설정하였는데, 이는 로봇의 반복 정밀도, 위치 정밀도 등의 성능에 따른 상대적인 가중치를 반영하기 위함이다. 로봇 자세가 가상 관측으로 추가된 최적화 파라미터 벡터 x는 식 (6)과 같이 구성된다. 본 논문에선 이중로봇 시스템의 미지 변환 X, Y, Z 그리고 각 로봇의 자세를 최적화 파라미터로 삼으며 왜곡 계수 및 카메라 계수를 포함하는 내부 파라미터 캘리브레이션에 대한 내용은 다루지 않는다.

이후 식 (7)과 같이 정의된 $$ \left(2n_i+12n_r\right)\times n_x$$ 크기의 자코비안(Jacobian) A 행렬을 유도하게 되며, 가상 관측의 도입으로 재투영오차와 관련된 부분은 투영 모델의 연쇄법칙으로 유도되며, 로봇 자세의 오차와 관련된 부분은 단위행렬로 구성되어 있는 형태를 확인할 수 있다.

파라미터 추정 단계에선 확률 모델의 잔차(Residual)를 $$ \Delta l=l-f\left(x^{\left(0\right)}\right)$$, 가중치 행렬을 $$ {\mathit{P}}_{\mathit{l}\mathit{l}}={\mathit{Q}}_{\mathit{l}\mathit{l}}^{-1}$$라고 할 때, 식 (8)을 바탕으로 갱신된 $$ x^{\left(1\right)}=x^{\left(0\right)}+\Delta \widehat{x}$$을 얻을 수 있으며, $$ \Delta \widehat{x}$$가 충분히 0에 가까워질 때까지 반복적으로 진행한다.

식 (2)에 정의된 투영 함수의 자코비안은 단계별 편미분을 연쇄법칙으로 결합하여 구성한다. 투영 체인이 확장됨에 따라 자코비안 유도 방식도 수정되었으며, 본 연구에선 좌표계 변환의 역방향(Inverse) 자코비안을 명시적으로 사용하는데, 이는 두 로봇 자세의 불확실성 모델을 동일 기준 프레임에서 일관되게 유지하기 위함이다. 상기 정의한 오일러 변환 순서에 대응되는 3차원 좌표 p의 좌표계 변환에 대한 정방향(Forward) 자코비안과 역방향 자코비안을 [Table 2]과 같이 정리할 수 있으며 이때 $$ {\left[\cdot \right]}_{\times }$$는 $$ {\left[a\right]}_{\times }b=a\times b$$를 만족하는 스큐(Skew) 연산자를 의미한다. 이후 자코비안 구현 시 식 (2)에서 정의한 투영 체인의 변환 순서에 맞게 $$ D_{o,j}^{c2}, Z_{c2}^{e2}, C_{e2,j}^{b2}, Y_{b2}^{b1}$$ 변환은 정방향 자코비안을, $$ {\left(A_{e1,j}^{b1}\right)}^{-1}, {\left(X_{c1}^{e1}\right)}^{-1}$$ 변환은 역방향 자코비안을 통해 유도되며, 카메라 좌표계로 변환된 이후의 투영 과정에서의 자코비안 유도는 선행 연구^[10]를 따라 전개되었다.

[Table 2]

Forward and inverse Jacobian of transformation

제안된 프레임워크는 각 그룹의 분산을 가중치로 활용한다. 그러나 단위가 서로 다른 각 관측 그룹의 분산을 단일 전역 분산 계수로 표현하는 것은 정확한 추정이 어렵다. 따라서 각 관측 그룹에 그룹별 분산을 추정을 위해 가우스-마르코프 모델에서 유도되는 리던던시 행렬(Redundancy matrix) 개념을 이용한 분산 성분 추정(Variance Component Estimation, VCE)^[10]를 채택한다. 본 연구에선 이중로봇 시스템에서 로봇이 추가됨에 따라 두 로봇 자세의 분산 성분을 독립적으로 분리하여 각 로봇이 가지는 통계적 특성에 부합하는 분산을 추정할 수 있도록 유도하였다. 추정된 상태 $$ \widehat{x}$$에서의 잔차를 $$ \widehat{v}=l-f\left(\widehat{x}\right)$$라고 할 때, 관측이 갖는 중복도를 정량화 하기 위해 리던던시 행렬 $$ \mathit{R}=\mathit{I}-\mathit{A}{\left({\mathit{A}}^T{\mathit{P}}_{\mathit{l}\mathit{l}}\mathit{A}\right)}^{-1}{\mathit{A}}^{\mathbf{\top }}{\mathit{P}}_{\mathit{l}\mathit{l}}$$을 유도한다. 그룹 g에 속한 관측의 인덱스 집합이 I_g라고 할 때, 그룹별 중복도는 R_j를 j번째 열과 행의 대각 성분이라고 할 때 $$ {\sum }_{j\in I_g}{R}_j$$로 정의한다. 본 논문에서 사용하는 그룹은 $$ g\in \left\{i, a_1, t_1, a_2, t_2\right\}$$로, 식 (10), (11)을 따라 각 그룹의 분산 $$ {\sigma }_g^2$$을 보정 계수 $$ {\widehat{\sigma }}_{0,g}^2$$를 곱하여 갱신한다.

여기에서 $$ {\widehat{v}}_g$$는 그룹 g에 해당하는 잔차의 부분 벡터, $$ {\mathit{P}}_{\mathit{l}\mathit{l},\mathit{g}}$$는 동일 인덱스에 대응하는 부분 가중 행렬이다. 이중로봇 시스템에서는 독립적인 로봇 그룹이 추가됨에 따라 두 로봇 자세의 초기 분산 $$ \left({\sigma }_{a_1}^2, {\sigma }_{t,1}^2\right)$$과 $$ \left({\sigma }_{a,2}^2, {\sigma }_{t,2}^2\right)$$을 각 로봇의 성능을 반영한 상대적 크기로 설정할 필요가 있다. 본 연구에선 선행 연구^[10]에 기반하여 두 로봇의 초기 분산 비율을 실험적으로 설정하였으며, 최적의 초기 분산 설계 방법을 제안하지는 않는다. 이후 로봇 별 개별 분산 추정 방법이 수렴 성능에 유의미한 영향을 미침을 4.2절의 시뮬레이션 실험을 통해 분석하였다.

분산 추정은 식 (8)로 유도되는 파라미터 추정이 수렴됐을 때 진행하며, 분산 추정으로 갱신된 $$ {\mathit{Q}}_{\mathit{l}\mathit{l}}$$, $$ {\mathit{P}}_{\mathit{l}\mathit{l}}$$을 사용해 파라미터 추정을 다시 반복한다. 각 그룹에 해당하는 보정 계수가 1.0에 충분히 근접하면 분산 성분 추정이 수렴한 것으로 간주한다.

4.1 초기화 전략 및 비교 프레임워크

본 연구에서는 두 센서가 동일한 보드를 관측한다는 가정 하에 초기에 정의했던 문제를 AXBD^-1=YCZ로 등가 변형할 수 있다는 점을 이용하여, 기존 이중로봇 캘리브레이션의 Kronecker 기반 초기화 절차를 채택한다^[5]. 이렇게 얻은 초기 변환 X, Y, Z는 이후 모든 알고리즘에서 동일하게 사용된다. 성능 비교를 위해, 제안된 방법과 유사한 프레임워크를 지니며 이중로봇 캘리브레이션에서 높은 수준의 정확도를 보고하고 있는 Lie 미분 기반의 최적화 기법^[5]을 동일 프레임워크에 맞춰 구현하고, 성능 비교를 진행하였다.

4.2 시뮬레이션 실험

본 연구에서는 시뮬레이션 기반 캘리브레이션을 위해 기준(ground truth) 변환 X, Y, Z를 임의로 설정하였으며 [Table 3]에 나타내었다. 또한 센서의 내부 파라미터는 사전 캘리브레이션 되어 고정된 것으로 가정하였으며, 설정 기준은 선행연구^[10]를 참고하여 [Table 4]에 나타내었다.

[Table 3]

Rigid transformation matrix of X, Y, Z

[Table 4]

Ground truth and pre-calibrated intrinsics

이후 1m³ 크기의 작업공간을 갖는 두 로봇에 대해 임의의 로봇 자세 $$ A_{e1,i}^{b1},{ C}_{e2,i}^{b2}$$를 생성하고, 임의 위치에 정의된 보드 좌표계를 기준으로 두 센서에서 보드까지의 변환 $$ B_{o,i}^{c1},{ D}_{o,i}^{c2}$$를 구성하였다. 생성된 변환에 대해 가시성 검사를 수행하여, 캘리브레이션 타겟의 모든 점이 이미지 프레임 내로 투영되는 경우에 한해 잡음이 없는 관측 데이터 쌍과 각 센서의 이미지 데이터를 수집하였다. 이후 실험에서 관측 데이터를 캘리브레이션에 사용할 땐 모든 잡음을 0-평균 정규분포로 가정하여 파라미터화 된 $$ A_{e1,i}^{b1}, C_{e2,i}^{b2}$$에는 $$ {\sigma }_a={0.1}^{\circ }$$와 σ_t = 1 mm의 잡음을 각 성분에 부여하였고, 이미지 측정치에는 σ_t = 0.1 px의 잡음을 부여하였다. 카메라와 보드 간의 변환 $$ B_{o,i}^{c1}, D_{o,i}^{c2}$$는 잡음이 추가된 이미지로부터 새롭게 추정하여 고정된 값으로 사용하였다.

제안된 방법의 유효성을 이중로봇 시스템에서 확인하기 위해, 4.1절의 비교 프레임워크 하에서 시뮬레이션에서 수렴 정확도를 평가하였다. 실험 데이터는 20개부터 50개까지 5개씩 늘려가며 실험을 진행했고, 각 조건을 25회 반복하여 평균을 나타내었다. 초기 가중치는 두 로봇에서 동일하게 회전 잡음 0.1°와 병진 잡음 1 mm를 가우시안 분포로 가정하였다. 회전 오차는 상대 변환 $$ T_{err}=T_{gt}{T}_{est}^{-1}$$의 회전 부분 R_err에 대해 $$ \Delta \theta =‖{\cos }^{-1}\left(\frac{tr\left(R_{err}\right)-1}{2}\right)‖$$로 정의하고, 병진 오차는 상대 오차의 병진 부분을 추출하여 $$ \Delta t=‖{\left[T_{err}\right]}_{1:\mathrm{3,4}}‖$$로 정의하였으며, 이는 이후 모든 실험에서 동일하게 적용된다. 실험 결과는 [Fig. 3]에 그래프로 나타냈으며 제안된 방법은 UA-Dual, 4.1절에 기술된 초기 변환과 Lie 미분 기반 최적화 기법을 각각 Initial, Lie로 표기하였다. 결과를 분석하면 제안된 방법이 모든 구간에서 일관되게 좋은 수치를 보였으며, 특히 데이터가 50개인 구간에서 제안된 방법이 Lie 방법에 비해 회전과 병진에서 각각 평균적으로 15.7%, 7.5% 개선을 보였다. UA-Dual은 모든 데이터 구간에서 평균적으로 VCE 반복 2~3회 이내에서 수렴에 성공하였다. 모든 실험은 Intel Core i9-10900 CPU, 32GB RAM 데스크톱 환경에서 진행되었으며 데이터 개수가 50개인 경우 평균 수렴 시간은 14.05초가 소요되었다. 이는 제안된 방법의 실시간 적용을 위해서는 향후 연산 병렬화와 같은 추가적인 개선이 필요함을 의미한다.

[Fig. 3]

Hand-eye calibration results with different amounts of data. Blue: Initial value. Green: Lie based method. Orange: proposed uncertainty aware dual method. Error bars indicate 95% confidence intervals

또한, 이중로봇 시스템에서 분리된 분산 추정 방법의 효과를 검증하기 위해 두 로봇의 분산을 달리하여 시뮬레이션 환경을 구성하고 실험을 진행하였고 결과를 [Fig. 4], [Fig. 5]에 나타내었다. 시뮬레이션 데이터 생성 시 로봇1은 회전 잡음 0.1°와 병진 잡음 1 mm를 가우시안 분포로, 로봇2는 회전 잡음 0.2°와 병진 잡음 2 mm를 가우시안 분포로 부여하였다. 두 로봇을 단일 그룹으로 묶어 초기 가중치 (0.1°, 1 mm)를 적용해 공동으로 추정했을 때를 UA-Dual(joint), 두 로봇의 그룹을 분리하여 로봇 별 초기 가중치 (0.1°, 1 mm)와 (0.2°, 2 mm)를 독립 적용한 분리 추정 방법을 UA-Dual로 표기하여 그 결과를 비교하였다.

[Fig. 4]

Hand-eye calibration results with different amounts of data. Blue: Initial value. Orange: proposed uncertainty aware dual method. Magenta: uncertainty aware dual method with joint VCE. Error bars indicate 95% confidence intervals

[Fig. 5]

Robot-pose calibration results with different amounts of data. Blue: Initial value. Orange: proposed uncertainty aware dual method. Magenta: uncertainty aware dual method with joint VCE. Error bars indicate 95% confidence intervals

수렴 이후 분리 추정은 미지 변환 X, Y, Z 추정 정확도에서 약 2% 정확도의 개선을 보였으며, 특히 로봇 자세 추정에서 두 로봇 모두 일관적인 개선을 보였다. 반면, 동시 추정 방법은 로봇1에서 초기값 대비 성능 저하가 관찰되었다. 이는 확장된 이중로봇 시스템에서 두 로봇의 분산을 동시 추정하게 되면 두 로봇이 가지는 잡음을 타협하는 지점으로 수렴하며 일부 로봇의 자세가 왜곡되는 반면, 독립적인 분산 성분을 추정, 적용하면 잡음 특성이 적절히 반영되어 각 로봇의 자세를 일관되게 복원할 수 있음을 시사한다. 이러한 결과는 제안된 프레임워크가 이중로봇 시스템에서 유효성을 가짐과 동시에 다중로봇 시스템으로의 확장 가능성을 뒷받침한다.

4.3 현실 실험

제안된 확장 모델의 실현 가능성을 평가하기 위해 현실 환경에서 이중로봇 시스템에 제안된 캘리브레이션 방법을 적용하여 실험을 수행하였다. 환경 구성에 사용된 로봇은 Franka Research 3(반복정밀도 0.1 mm)이며, 센서는 Microsoft Azure Kinect DK, 캘리브레이션 타겟으로는 차루코보드(ChArUco board)를 사용하였다. 실험 환경은 [Fig. 6]에 나타나 있다. 캘리브레이션 타겟은 로봇 외부에 고정하였고 로봇 자세를 다양하게 바꿔가며 로봇에 부착된 센서가 캘리브레이션 객체를 항상 바라보도록 유지하였다. 센서와 타겟 간 변환 데이터는 PnP 알고리즘을 통해 취득하였다. 총 70개의 데이터셋을 취득하였으며, 50개의 캘리브레이션 데이터셋, 20개의 테스트 데이터셋으로 구성된 현실 데이터셋을 구성하였다. 초기 가중치는 두 로봇에서 동일하게 (0.1°, 1 mm)를 적용하였다. 현실에서 구성된 환경은 기준 변환 X, Y, Z를 모르는 상황이므로 캘리브레이션 결과는 식 (13)과 식 (14)로 정의된 기하학적 오차로 표현한다. 또한 식 (2), (3)을 통해 유도된 두 센서의 예측 픽셀을 $$ p_{j,k}^{\left(\mathrm{1,2}\right)}$$, 관측된 픽셀을 $$ p_{j,k}^{-\left(\mathrm{1,2}\right)}$$라고 할 때 평균 재투영오차를 Dual Reprojection Root Mean Square Error(DRRMSE)로 식 (15)와 같이 정의하여 평가 지표로 활용하였다.

[Fig. 6]

Real experiment setting

실험 결과는 로봇 자세 캘리브레이션 결과를 함께 평가하기 위해 캘리브레이션에 사용된 캘리브레이션 데이터셋과 테스트 데이터셋에 대해 모두 검증되었으며 각 결과를 [Fig. 7]과 [Fig. 8]에 순서대로 나타내었다. 캘리브레이션 데이터에서 UA-Dual 방법은 회전 오차 0.22°, 병진 오차 2.95 mm, 평균 재투영오차 3.785 px로 초기값 대비 모든 지표에서 개선을 보였고, Lie 방법은 회전 오차는 0.677°로 증가하였으나, 병진 오차가 8.33 mm, 평균 재투영오차는 9.323 px로 개선되었다. 캘리브레이션 세트에서 나타난 큰 폭의 개선으로 보정된 로봇 자세가 전체 잔차 감소에 미친 영향을 확인할 수 있다. 테스트 세트에서 UA-Dual은 회전 오차는 0.582°로 증가되었으나, 병진 오차 7.206 mm, 평균 재투영오차 8.053 px로 초기값과 Lie 대비 더 낮은 수치를 기록하여 제안법이 캘리브레이션 데이터에 과적합 된 결과가 아닌 일반화된 최적화 성능을 보여주었다.

[Fig. 7]

Rotation, translation, reprojection error on calibration data

[Fig. 8]

Rotation, translation, reprojection error on test data