본 연구를 위해서 HILS에 채택된 산업용 로봇은 KUKA kr250 r2700 모델이다. 먼저 위성 동작 구현을 위해서 위성에 설정된 좌표계에서 동작을 정의한다. 로봇 끝단에 힘/토크(F/T) 센서를 부착하고 좌표계 {s}를, 위성의 무게중심에 좌표계 {b}를 [Fig. 1]에서 제시된 것처럼 표시한다. 이 두 좌표계를 표기하기 위한 참조 좌표계(reference frame)로는 고정 좌표계 {w}를 활용한다. 고정 좌표계에서부터 로봇 끝단에 부착된 F/T센서까지의 변환 행렬식^[6]은 식 (1)과 같이 표현된다:

[Fig. 1] 
KUKA kr250 r2700 model and the satellite model given as hexapod shape, where the end-effector frame of the Kuka robot equipped with F/T sensor is denoted as {s} while the satellite frame is denoted by {b}

여기서 M_ws∈SE(3)는 고정 좌표계로부터 F/T센서까지 zero-configuration HT, S_i∈R⁶는 각 관절 별 스크류 축(screw axis), R_ws∈SO(3)로 고정 좌표계로부터 F/T센서까지 회전 행렬, p_ws∈R³는 고정 좌표계로부터 F/T센서까지 위치 벡터를 의미한다.

또한, F/T센서로부터 위성 무게중심까지의 변환 행렬은 식 (2)과 같이 표현되며 상수 행렬이다.

여기서 R_sb∈SO(3)로 F/T센서로부터 위성까지 회전행렬, p_sb∈R³는 F/T센서로부터 위성까지 위치벡터를 의미하며 모두 상수이다.

우주 환경에서 위성 사이의 접촉 시 생성되는 6차원 접촉 wrench로 인해 위성들의 움직임은 변하는데, 이를 구현하기 위해서는 접촉 wrench 및 접촉점을 산출하여 접촉 동역학(contact dynamics)를 계산하고 접촉 후의 위성의 동작을 생성하여 이를 로봇의 끝단의 움직임으로 인가하여야 한다. 이를 위해 지상 실험장치 전체 위성에 F/T센서를 부착하는 것이 어렵다. 그래서 본 논문에서는 F/T센서를 산업용 로봇의 끝단에 부착하였고, 여기에서 힘과 모멘트를 측정한다. 즉 접촉지점이 아닌 F/T센서 좌표계 {s}에서 힘과 모멘트를 측정한다. 그러나 F/T센서에서 측정된 힘과 모멘트는 위성의 중력에 기인한 wrench가 함께 나타난다. 이러한 이유로, F/T센서로부터 얻은 데이터를 위성의 무게중심 좌표계인 {b}로 변환하고, F/T센서로부터 측정되는 위성 자체의 중력 wrench인 F_b,g를 제거해 주어야 한다. 이를 위한 표현식은 다음 식 (3)과 같다.

여기서, F_s는 F/T센서에서 측정되는 wrench, F_b,g는 {b}좌표계에서 표현되는 위성 질량(mass)에 기인한 중력 wrench이며, F_b는 {b}좌표계에서 표현되는 중력 wrench를 제거한 순수한 contact wrench이다. 또한 Ad_Tsb∈R^6×6는 {s}좌표계에서 {b}좌표계로 변환시켜는 변환 행렬이며 식 (4)와 같이 표현된다.

F/T센서는 센서 특성상 노이즈가 포함되어 있으므로, 이를 줄이기 위한 저주파통과필터의 기능을 가지는 칼만 필터가 설계/적용되었다.

먼저, 목표 HT T^des와 위성의 실제 HT T_wb사이의 상대변환행렬 T~∈SE3를 다음과 같이 정의한다.

여기서 T_wb = T_wsT_sb이며, T_ws, T_sb는 식(1)과 (2)을 사용한다. T^des∈SE(3)는 원하는 위성 목표(desired) HT로서 목표 회전행렬과 목표 위치벡터를 포함한 행렬이다. 6차원 추적 오차(tracking error)를 산출하기 위하여 T~에 행렬로그함수(matrix logarithm)를 적용하여 지수좌표(exponential coordinate)를 산출한다.

위의 식(8)을 이용하여 지수좌표(exponential coordinate) λ = [η_x,η_y,η_z,ξ_x,ξ_y,ξ_z]^T∈R⁶를 얻을 수 있다. 4×4 행렬 λ∈se3는 리 대수(Lie algebra)이며, ⋅은 R⁶의 벡터와 리 대수 se(3) 사이의 관계를 1:1로 연결시키는 연산자이다. ξ =[ξ_x,ξ_y,ξ_z]^T∈R³는 회전 지수좌표 3차원 벡터이며, η =[η_x,η_y,η_z]^T∈R³는 선형 지수좌표의 3차원 벡터이다. 또한 상대변환행렬 T~을 사용하여 상대 트위스트(relative twist)를 얻는 방법은 2가지가 있는데, right multiplication을 사용하여 얻어지는 상대 트위스트는 {w} 좌표계에서의 표현되는 6차원 속도이며, left multiplication을 사용하여 얻어지는 twist는 {b}좌표계에서 얻어진 속도이다. 우리는 모든 표현을 인공위성의 무게중심 좌표계 {b}로 표현하기 위하여 left multiplication을 이용하여 상대 트위스트(relative twist) V~∈R6를 다음과 같이 얻는다.

여기서 Vdes=Tdes-1T˙des, V=Twb-1T˙wb이다. 위의 4x4 행렬 식 (7)를 6차원 속도 벡터식으로 변환하기 위해서 Adjoint transformation 수식 (4)를 이용하여 다음과 같이 표현할 수 있다.

여기서 V^des∈R⁶는 원하는 위성의 목표 트위스트 6차원 속도 벡터이고, V∈R⁶는 현재 위성의 트위스트 6차원 속도 벡터이다. 또한 상대 트위스트로부터 지수좌표 속도오차로의 변환관계를 찾아야 한다. 이는 지수함수의 미분을 취하여 얻을 수 있다. 이를 differential of exponential map이라 부르며, dexp로 나타낸다. 이는 참고문헌^[7]에서 찾을 수 있다. 따라서 상대 트위스트로부터 지수좌표 속도오차(λ˙∈R6)로의 변환 과정은 식 (9)과 같이 얻어진다.

여기서 dexp-λ-1∈R6×6는 상대 트위스트와 지수좌표 속도 오차 사이의 선형 변환을 나타내는 행렬이다.

다른 한편으로 위성 사이의 RvD 작업 구현을 위해서는 위성에서 목표 wrench(힘/모멘트)을 구현해야 하는데, 이를 위해서 실제 wrench와의 차이인 wrench 오차를 다음 식 (10)과 같이 정의한다.

여기서 F^des(t)∈R⁶로 원하는 목표 힘/모멘트를 포함하는 wrench이다. 또한 0.5가 붙은 이유는 power conservation에서 유도된다.

힘 제어 모델을 관성 행렬 A∈R^6×6과 감쇠 행렬 B∈R^6×6을 사용하여 설계할 경우 식 (11)과 같이 설계할 수 있다.

여기서 V~˙∈R6는 상대 트위스트의 시간에 대한 미분으로, 상대 트위스트의 시간변화율을 나타낸다. 따라서 다음 힘 제어 모델을 구현하기 위해서는 V~˙을 산출해야 하는데, 이를 위해서는 상대 트위스트 V~를 수치 미분을 해야 하며, 이경우 원하지 않는 노이즈들이 추가될 수 있다. 이러한 이유로 식 (11)를 한번 적분하여 다음 식 (12)과 같이 차수를 낮추어 힘 제어 모델을 재설계한다.

여기서 힘 제어 모델은 스프링-댐퍼 모델인 감쇠 행렬 D∈R^6×6와 강성 행렬 K∈R^6×6을 사용한다고 가정한다.

식 (12)를 지수 좌표계로 표현하기 위해 상대 트위스트를 식 (9)를 이용하여 지수좌표 속도오차로 변환하여 다음 식으로 표현한다.

산업용 로봇은 속도제어 입력을 사용하므로 어드미턴스 제어 입력을 속도항에 인가하고 reference 속도제어입력이라 표기한다.

여기서 이득 행렬들은 다음과 같이 표현된다.

또한 λ˙ref∈R6는 지수 좌표계에서의 참조(reference) 속도를 나타내는 6차원 벡터이며, K_p,K_γ∈R^6×6는 지수 좌표계에서 참조 속도를 생성하기 위한 이득 행렬이다. 여기서 dexp_-λλ = λ라는 성질을 이용하여 K_p를 정의한다. 식 (14) 중 K_pλ는 추적 오차에 비례하여 로봇을 목표 위치로 이동시키기 위한 지수 좌표 속도 제어 입력을 생성한다. Kγ∫F~는 wrench 오차의 적분에 비례하여 발생하는 지수 좌표 속도 제어 입력 생성하여, 힘의 변화에 민감하게 반응한다. 따라서 추적 오차와 비교하여 wrench 오차가 클 경우 로봇은 wrench 오차를 줄일 것이며, 추적 오차와 비교하여 wrench의 오차가 작을 경우 로봇은 추적 오차를 안정화 하려고 할 것이다. 이는 wrench 오차와 지수 좌표계의 추적오차 사이의 trade-off를 조절하여 산업용 로봇이 원하는 작업을 수행할 수 있도록 하고, 외부환경과 상호 작용할 수 있다는 장점을 가진다.

얻어진 어드미턴스 제어입력 (14)를 다시 식 (8)과 (9)를 이용하여 reference 트위스트로 다음과 같이 변환한다.

Reference 트위스트를 로봇의 속도 역기구학을 이용하여 목표 관절속도로 변환한다. 이 때 속도기구학을 사용해서 생성되는 수치 오차를 보상하기 위하여 closed-loop inverse kinematics(CLIK)를 이용한다.

여기서 J^-1(q)는 J(q)의 역행렬이다. f(q)는 정기구학을 나타내며, K는 CLIK의 비례이득 행렬, p_d 목표 위치를 의미한다. qdes,q˙des∈R6은 위성을 들고 있는 산업용 로봇의 각 관절별 각도 입력와 각속도 입력을 나타내며, q∈R⁶ 실제 로봇 관절각을 나타내며, q^prev∈R⁶ 이전 시간에 대한 로봇 관절각을 나타낸다.

[Fig. 2] 는 본 논문에서 제안하는 어드미턴스 제어의 블록 다이어그램을 보여준다. 위성간 접촉이 없을 때는 어드미턴스 제어 모델 식 (14)에서 F~가 0이 되어 지수좌표에 대한 PD제어가 구현되며, 위성간 접촉이 있을 때는 어드미턴스 힘 제어 모델 식 (14)가 구현된다.

[Fig. 2] 
Entire block diagram for realization of admittance control

시뮬레이션을 통한 실험한 결과는 [Fig. 3]과 [Fig. 4]과 같으며, 시뮬레이션 조건은 위성에 x-방향으로 500 N의 힘을 인가하기를 원한다고 가정하였다. 다시 말해서 목표 wrench인 F^des = [-500,0,0,0,0,0]로 설정하였다. 실험은 5.5 초간 진행되었으며 2.8 초에 위성간 접촉이 발생한다. [Fig. 3] 은 추적오차를 지수좌표로 표현한 결과이며, 그래프에서 xi는 ξ =[ξ_x,ξ_y,ξ_z]^T로 회전 지수좌표를, Eta는 η =[η_x,η_y,η_z]^T로 선형 지수 좌표를 x,y,z축으로 나타낸 것이다. [Fig. 4] 는 각 wrench에 해당하는 힘 3차원 벡터 (f =[f_x,f_y,f_z]^T)와 모멘트 3차원 벡터 (τ =[τ_x,τ_y,τ_z]^T)에 대한 측정 wrench와 목표 wrench를 비교한 그래프이다. [Fig. 4] 에서 보이듯이 제시된 식 (14)의 어드미턴스 제어 모델이 x-방향 500 N의 접촉 힘을 잘 추종하는 것을 확인할 수 있다.

[Fig. 3] 
Exponential coordinates (tracking errors) calculated from the relative transformation and relative twist

[Fig. 4] 
Desired wrench and measuring wrench (wrench error) when admittance control is applied to industrial robots

[Fig. 3]와 [Fig. 4]의 시뮬레이션을 위하여 사용된 어드미턴스 제어 모델의 감쇠이득 및 강성이득 행렬은D=300×diag(1,1,1,1,1,1), K=200×diag(1,1,1,1,1,1)로 설정하였다. [Fig. 4] 를 통해 어드미턴스 제어의 힘 추종이 잘 되는 것을 확인하였지만, [Fig. 3]에서는 추적 오차가 계속해서 누적되다가 경로 생성이 중지될 때 오차가 안정화되는 것을 확인할 수 있었다. 이때 지수 좌표계 이득 행렬은 [Fig. 5(a)], [Fig. 5(b)]와 같다. [Fig. 5(a)] 는 식 (14)에서 K_pλ를 나타내는 그래프이며 추적 오차의 결과를 보여준다. [Fig. 5(b)] 는 식 (14)에서 Kγ∫F~에 대한 그래프로, wrench오차의 누적에 상응하는 결과를 나타낸다. Wrench의 오차가 추적 오차에 비해 상대적으로 크기 때문에 힘 제어 작업이 주로 이루어진다. 따라서 [Fig. 5(a)]는 힘 제어는 주로 wrench 오차를 줄이는데 중점을 두고 있어, 추적 오차가 증가하는 결과를 보이고, [Fig. 5(b)]는 힘의 누적 오차가 증가함에 따라 민감하게 그의 오차를 줄이는 것을 확인할 수 있다. 자연스러운 결과로서 동일한 방향으로의 힘제어와 위치제어는 서로 병립할 수 없기 때문이다.

[Fig. 5] 
The gain matrix shown in the exponential coordinate system when the admittance control is applied to the industrial robot according to the desired force

제안된 어드미턴스 제어 모델의 성능을 검증하기 위하여 목표 wrench와 감쇠이득 및 강성이득 행렬을 변경하여 다시 실험을 진행하였다. [Fig. 6(a)]는 목표 wrench인 F^des = [-200,0,-200,0,0,0]으로, 감쇠이득 및 강성이득 행렬은 각각 D = 300 × diag(1,1,1,1,1,1), K= 700 × diag(1,1,1,1,1,1)설정한 시뮬레이션 결과를 나타낸다. 이때 결과는 힘 3차원 벡터 (f =[f_x,f_y,f_z]^T)를 보여준다. 접촉이 발생하고, 어드미턴스 제어를 통해 안정화되어 위성들이 서로 붙어 있으나 서로 힘을 인가하지 않고 있는 것을 확인할 수 있다. [Fig. 6(b)] 는 실험 4.1과 동일한 감쇠 이득 및 감성이득을 사용하였고, 목표 wrench는 F^des = [-200,0,0,0,0,0]로 설정하여 나온 시뮬레이션 결과이다. 이를 통해 제안된 어드미턴스 제어 모델은 힘 제어를 잘 수행할 수 있다는 것을 확인하였다. [Fig. 7] 은 [Fig. 6(b)] 의 결과에서 접촉 순간을 확대한 것으로, 접촉 현상이 불연속적으로 일어나고 있는 것을 확인할 수 있다. 초기 접촉 직후 위성들은 서로 반발력으로 멀어졌다가 다시 접촉을 하고, 다시 멀어졌다 가를 반복한 후 안정적인 접촉이 이루어짐을 확인할 수 있다. [Fig. 6(b)] 에서 보이듯이 정상상태에 도달할 수록 wrench 오차는 0으로 수렴한다.

[Fig. 6] 
Desired force and measured force (force error) when applying admittance control according to the desired force for the industrial robot

[Fig. 7] 
Magnifying the transient period after contact of [Fig. 6(b)]

제안된 어드미턴스 스프링-댐퍼 모델을 통해 로봇의 끝단에 가해지는 외부 힘과 모멘트에 따라 로봇의 움직임을 제어할 수 있다. 이로 인해 목표 wrench를 적절하게 설계하면 로봇이 외부 힘에 민감하게 반응하는 것을 줄일 수 있고, 로봇의 움직임이 부드럽고 안정적으로 보이게 된다. 그러나 목표 wrench가 너무 큰 경우 로봇 시스템에 과도한 응답을 가져올 수 있어서, 로봇과 주변 환경에 미치는 영향을 신중하게 고려가 필요하다.