2.1 상대 자코비안

그림 1과 같이, 양팔 로봇의 두 말단장치 사이에 강체를 두고 상호작용하는 상황을 고려하자. 이 때, 강체는 양팔 로봇의 한쪽 매니퓰레이터의 말단장치에 고정 되어있다고 가정한다. 그림 1에서, $\Sigma_{0}$는 전역 기준 좌표계(global inertial frame)의 원점(origin), $\Sigma_{A}$, $\Sigma_{B}$는 각각 매니퓰레이터A와 B의 기준 좌표계의 원점, $\Sigma_{G}$, $\Sigma_{T}$, $\Sigma_{obj}$는 각각 매니퓰레이터 A와 B의 말단장치와 강체의 질량중심에 고정된 좌표계이다. $^{i}r_{j}$가 $i$ 기준 좌표계에서 $j$ 기준 좌표계까지의 상대 벡터를 나타낸다고 하면, $\Sigma_{0}$에서 $\Sigma_{T}$ 까지의 상대 벡터 $^{0}r_{T}$는 다음과 같이 얻을 수 있다.

여기서 $R_{i}^{j}\in R^{3\times 3}$는 $i$ 기준 좌표계와 $j$ 기준 좌표계 간의 회전변환을 나타내는 회전행렬이다. 식 (1)을 $^{obj}r_{T}$에 대한 식으로 정리하면 다음을 얻는다.

위 식에서 $\theta ,\: q$는 각각 매니퓰레이터 A와 B의 관절 각도이다. 또한, 각 매니퓰레이터의 관절 각속도와 말단 장치의 속도($^{A}\dot r_{G}$, $^{B}\dot r_{T}$) 및 각속도($^{A}\omega_{G}$, $^{B}\omega_{T}$) 간의 관계는 다음과 같이 얻어진다.

이제, 식 (2)를 미분함을 통해 $\Sigma_{obj}$에서 바라본 매니퓰레이터 B의 말단 장치 속도 벡터 $^{obj}\dot r_{T}$를 다음과 같이 얻는다.

여기서 연산자 $\hat(\bullet)$는 의 요소를 3차원 반대칭 행렬(Skew-symmetric matrix)의 공간에 매핑하는 연산자이며, 임의의 벡터 $a:=[a_{1},\:a_{2},\:a_{3}]^{T}$ $\in$에 대하여 $\hat a$는 다음과 같이 계산된다.

따라서 $\hat R^{A}_{BA}(\theta),\:\hat R^{A}_{T}(\theta ,\:q),\:\hat R^{A}_{G}(\theta)$는 다음과 같다.

또한, $\Sigma_{0}$에서 바라본 매니퓰레이터 B의 말단 장치 각속도 $^{0}\omega_{T}$는 다음과 같은 관계를 만족하며

위의 식 (7)로부터 $^{obj}\omega_{T}$는 아래와 같이 얻어진다.

이제, 식 (4)와 식 (8)으로부터 $^{obj}\dot r_{T}$과 $^{obj}\omega_{T}$를 $\dot q$와 $\dot\theta$에 대하여 정리를 하면 다음과 같은 행렬식을 얻을 수 있다.

여기서 $J_{R}$은 상대 자코비안이고, 이것의 각 요소들은 다음과 같다.

\begin{align*} J_{11}= & R_{G}^{obj}R_{G}^{A}(\theta)^{T}R_{0}^{A}R_{B}^{0}J_{Br}(q)\\ J_{12}= &-R_{G}^{obj}[R_{G}^{A}(\theta)^{T}J_{Ar}(\theta)+\hat R_{BA}^{A}(\theta)^{T}R_{G}^{A}(\theta)^{T}J_{A\omega}(\theta)\\ & +\hat R_{T}^{A}(\theta ,\:q)R_{G}^{A}(\theta)^{T}J_{A\omega}(\theta)-\hat R_{G}^{A}(\theta)R_{G}^{A}(\theta)^{T}J_{A\omega}(\theta)]\\ J_{21}= &R_{G}^{obj}R_{G}^{A}(\theta)^{T}R_{0}^{A}R_{B}^{0}J_{B\omega}(q)\\ J_{22}= &-R_{G}^{obj}R_{G}^{A}(\theta)^{T}J_{A\omega}(\theta). \end{align*}

추가적으로, 해석적 자코비안(Analytic Jacobian)$J_{A}(q)$을 사용하면 오일러각 회전속도와 관절 속도 간의 관계를 나타낼 수 있으며, 해석적 자코비안과 기하적 자코비안 사이의 관계는 다음과 같은 관계를 가진다.

여기서 $I_{3}$는 3차원 단위행렬을 나타내며, 오일러 각이 요(Yaw, $\psi $)-피치(Pitch, $\phi$)-롤(Roll, $\rho$)의 변환 순서를 따르는 경우 $T_{eu\le}r$는 다음과 같이 계산된다.

본 논문에서는 매니퓰레이터 A의 말단 장치에서 바라본 매니퓰레이터 B의 말단 장치의 오일러 각을 지령치로 받는 상황을 고려하며, 따라서 기하적 자코비안과 해석적 자코비안 사이의 변환 행렬을 곱한 해석적 상대 자코비안을 사용한다.

2.2 상대 임피던스 제어

상대 임피던스 제어는 양팔 로봇 사이의 상대 모션이 그림 2와 같이 가상의 질량$(M_{Rd})$-댐퍼$(B_{Rd})$-스프링$(K_{Rd})$시스템이 있다고 가정하고, 필요한 접촉력을 갖도록 제어하는 것이다. 상대 임피던스 제어에서는 두 말단 장치 사이의 상대 움직임을 정의하기 위해 상대 자코비안을 사용한다. 그림 2에서처럼 가상의 질량-댐퍼-스프링을 고려하여, 매니퓰레이터 B의 말단 장치에서의 힘을 다음과 같이 얻을 수 있다.

여기서 $f_{R}$은 질량-댐퍼-스프링에 의한 가상의 힘 나타내며, $x_{R}$과 $x_{Rd}$는 각각 매니퓰레이터 B의 말단 장치와 질량-댐퍼-스프링 시스템의 기준 좌표계간의 상대 벡터와 그 지령치를 의미한다.

식 (12)를 말단 장치의 가속도 $\ddot x_{R}$에 대해 정리를 하면 다음과 같다.

이제, 말단 장치에 대한 위치 지령치와 실제 말단 장치에 대한 오차를 $e=x_{Rd}-x_{R}$로 정의하고, 계산 토크 제어 기법을 사용하면, 필요한 관절 토크 $\tau$를 다음과 같이 얻을 수 있다.

여기서 $J_{R}$은 상대 자코비안, $N(q,\:\dot q)$ 코리올리 힘 및 중력 벡터를 나타내며 연산자 $(\bullet)^{+}$는 무어-펜로즈 의사 역행렬(Moore-Penrose Pseudo Inverse Matrix)을 의미한다. 또한, $J_{R}^{T}f_{R}$은 환경과의 접촉력에 의해 발생하는 토크이다.

외부 외란이 없는 경우 일반적인 로봇팔의 동역학 방정식은 다음과 같이 나타낼 수 있다.

여기서 $\tau_{e}$는 환경과의 접촉력에 의해 발생하는 토크를 나타낸다.

식 (14)와 식 (15)으로부터 평형점(equilibrium point)에서의 접촉력은 다음과 같이 유도된다.

식 (12)에서 말단장치의 가속도에 대한 오차 $\ddot e$가 무시할 정도로 작다고 가정하면 접촉력 $f_{R}$이 오차동역학 $B_{Rd}\dot e_{i}+K_{Rd}e_{i}$ 의 크기가 될 수 있도록 $K_{Rd}$, $B_{Rd}$ 선정할 수 있다. 또한, 식 (16)에서 $x_{Rd}$는 지령치이다. 즉, 외란 및 환경에 의한 불확실성을 효과적으로 보상할 수 있다면 적절한 지령치를 선정하여 접촉력 $f_{R}$이 임의의 값을 갖도록 제어 할 수 있다.

3.1 외란관측기

이 절에서는 외란 및 시스템 불확실성을 포함하는 덩어리 외란을 고려하고 이를 추정하기 위한 기본적인 형태의 외란관측기를 소개한다. 이 논문에서는 역공칭모델을 이용한 외란관측기가 사용되었으며, 상태공간(state-space)에서 설계되었다^(12,16,17).

먼저, 다음과 같은 공칭 매니퓰레이터 모델을 생각하자.

여기서 $\bar{M}(q)$는 공칭 모델의 질량 행렬이고, $\bar{N}(q,\:\dot q)$는 공칭 모델의 코리올리힘과 중력 항을 포함한 항이며, $\tau_{r}$은 지령 토크이다. 시스템 불확실성이 있는 실제 시스템은 공칭 시스템과 다른 거동을 보일 수 있으며 시스템 불확실성은 다음과 같이 표현할 수 있다.

이제, 시스템 불확실성, 외부 외란 토크 $\tau_{d}$, 환경과의 접촉력 $\tau_{e}$을 한데 모아, 아래와 같이 하나의 덩어리 외란 $d$를 생각하자.

만약 덩어리 외란 $d$를 알 수 있다면 이를 보상함으로써 시스템 불확실성과 외란이 존재하는 실제 시스템이 공칭시스템처럼 거동하도록 할 수 있으며, 이 논문에서는 이러한 외란 보상을 위해 외란관측기를 사용한다.

그림 3은 기본적인 외란관측기의 블록도이다. 여기서 $P(s)$는 실제 시스템을 나타내며 $P_{n}(s)$는 실제 시스템에 대한 공칭 시스템이다. $Q(s)$는 $Q$-필터로써 저역 통과 필터를 의미한다. 외란관측기는 덩어리 외란 $d$를 추정하여 보상함으로써 실제 시스템과 공칭 시스템의 차이 및 외부 외란을 제거하여 실제 시스템이 공칭 시스템처럼 동작하도록 한다.

즉, 실제 시스템 $P(s)$의 출력을 $y(s)$라고 하면,

를 얻을 수 있고, 위 식 (20)으로부터 입력 토크 $\tau_{r}(s)$과 외부 외란 토크 $\tau_{d}(s)$로부터 출력 $y(s)$까지의 전달함수를 각각 구하면 다음과 같다.

이제, $Q$-필터를 $Q(s)=\dfrac{\alpha}{\mu s+\alpha}$와 같이 1차 저역 통과 필터의 형태로 설계했다고 하자. 이 때, 직류이득(DC-gain)은 $1$이며 ($\lim _{s \rightarrow 0} Q(s)=1$), 식 (21)으로부터 다음을 얻는다.

위 식으로부터 $s$가 $0$으로 수렴할수록, 시스템의 출력은 $y(s)\approx P_{n}(s)\tau_{r}(s)$이 되고 $H_{d}(s)\approx 0$이므로, 시스템은 공칭 모델처럼 동작하며 외란에 의한 영향은 출력에 나타나지 않는다.

3.2 제안하는 방법: 덩어리 외란을 보상하는 강인 제어기 설계 기법

본 논문에서 제안하는 제어기는 상대 임피던스 제어를 통해 양팔 로봇을 제어한다. 이때, 시스템의 불확실성 및 외란을 외란관측기를 통해 보상한다. 만약 외란관측기가 적절히 설계되어 외란, 시스템 불확실성, 환경과의 접촉력을 충분히 보상한다면, 실제 시스템이 공칭 시스템처럼 거동하게 됨을 기대할 수 있다.

대상 시스템이 비선형 모델이므로 상태 공간에서의 외란관측기 설계법이 적용되었다 ^(4,7). 그림 4는 본 논문에서 제안하는 제어기의 구조를 나타낸다. $\bar{M}$은 공칭 시스템의 질량 행렬, $\tau_{r}$은 외부 제어기 제어 입력이다. 시스템 출력으로부터 나온 $\dot q$과 시스템에 인가되는 $\tau$를 이용하여 외란관측기 루프를 통해 다음과 같은 식을 얻을 수 있다.

여기서 $\zeta$와 $\chi$는 외란관측기의 상태변수이며 각각 $\dot q$와 $\bar{M}^{-1}\tau$를 추종하도록 설계되었고, $\alpha$, $\mu$는 1차 저역 통과 필터의 설계인자이다. 만약 $\dot\zeta$이 $\dot q$를 충분히 추종했다면, 입력외란과 시스템 불확실성이 포함된 토크의 추정치 $\hat\tau_{P}$는 다음과 같이 표현할 수 있다.

이제, 입력 외란 및 환경에 의한 외란, 시스템 불확실성이 포함된 토크 추정치 $\hat\tau_{P}$에 시스템에 인가한 토크 $\tau$를 빼서 덩어리 외란의 추정치 $\hat d$를 구할 수 있다. 즉,

따라서 시스템에 인가되는 제어 입력 토크를 다음과 같이 설계할 수 있다.

4.1 양팔 로봇 모델

그림 5는 수치 실험에 사용된 양팔 로봇 모델을 보여준다. 수치 실험은 물리 엔진 기반 가상 환경 시뮬레이터인 Gazebo ⁽⁹⁾에서 수행되었다. 물리 엔진으로는 Open Dynamics Engine(ODE) ⁽¹⁰⁾가 사용되었다. 힘/토크 센서는 사각 판이 부착되어 있는 매니퓰레이터 A의 말단 장치에 부착하였으며 각 축마다 그림 5에서 표현된 임피던스 모델로 제어하였다. 이때, 매니퓰레이터 B의 말단장치의 지령 궤적은 매니퓰레이터 A의 말단장치에 부착되어있는 사각 판에 원을 그리는 작업을 수행하도록 설계되었다. 사용한 매니퓰레이터 모델은 Universal Robots사의 UR10 ⁽¹¹⁾이며,

각 링크와 관절의 물성치 및 연결관계가 표현된 Unified Robot Description Format(URDF)을 사용하여 Gazebo에서 모델을 로드하였다. 또한, 강체 동역학 라이브러리로 Rigid Body Dynamics Library(RBDL)을 사용하여 로봇의 순/역 운동학 (Forward/Inverse Kinematics) 및 코리올리 힘 등을 계산하였다.

4.2 시스템 불확실성이 없는 경우 수치 실험 결과

시스템 불확실성이 없는 경우에 대해, 매니퓰레이터 B의 말단 장치 궤적이 매니퓰레이터 A의 말단장치에 부착되어있는 판에 원을 그릴 수 있도록 지령 궤적을 주었다. 이때 사용한 상대 임피던스 매개변수 $M_{Rd}$, $B_{Rd}$와 $K_{Rd}$는 각각 1 kg, 20 Ns/m, 100 N/m이다.

시스템 불확실성이 없는 경우 상대 임피던스 제어기의 지령 궤적 추종 성능은 그림 6에 나와 있다. 그림 6(a)의 $X_{Rx,\:z}$와 $X_{Rdx,\:z}$는 각각 매니퓰레이터A의 말단장치에 부착된 판의 좌표계에서 매니퓰레이터 B의 말단장치가 지나가는 X, Z 성분의 궤적과 지령 궤적을 의미한다. 원점에서부터 원까지 상대 임피던스 게인을 설정한 대로 감쇠비 1의 거동을 보였다.

그림 6(b)의 $X_{Ry}$와 $X_{Rdy}$는 접촉이 발생하는 Y축에서 매니퓰레이터 B의 말단장치의 Y 성분의 궤적과 지령 궤적을 의미한다. 시스템의 불확실성이 없는 경우에 상대 임피던스 제어를 사용하면 0.02보다 작은 오차를 갖으며 지령 궤적을 추종하는 것을 볼 수 있다.

4.3 시스템 불확실성이 있는 경우 수치 실험 결과

표 1과 같이 각 링크의 질량을 변경하여 시스템의 불확실성을 주었다. 그림 7은 시스템의 불확실성이 있는 상황에서 상대 임피던스 제어기를 적용한 결과를 보여준다. 시스템의 불확실성이 있는 상황에서 외란관측기를 사용한 상대 임피던스 제어기를 적용한 결과는 그림 8에 나와 있다. 상대 임피던스 매개변수 $M_{Rd}$, $B_{Rd}$와 $K_{Rd}$는 각각 1 kg, 20 Ns/m, 100 N/m이며 외란관측기 매개변수 $\alpha$와 $\mu$는 1과 0.001으로 설정하였다.

표 2는 각각 시스템의 불확실성이 있는 경우 외란관측기를 사용한 경우와 사용하지 않은 경우에 대한 위치에 대한 평균 제곱근(Root Mean Square, RMS) 오차를 보여준다. 외란관측기를 사용할 경우 평균제곱근 오차에 대해 두 배 이상 줄어드는 결과를 얻었다. 이 결과를 통해 모델을 아는 경우 외란관측기를 사용한 상대 임피던스 제어를 적용하면 시스템 불확실성에 대해 강인한 것을 볼 수 있다.