2.1 접촉제약을 반영한 동역학

로봇이 작업을 수행할 때, 벽과 같이 말단장치가 어느 방향에 대해서 더 이상 움직이지 못하게 하는 접촉이 발생한 경우를 생각해보자. 매니퓰레이터의 말단장치와 환경이 접촉하는 경우, 로봇은 접촉으로 인한 동작의 제약이 발생하게 되며 이는 작업을 수행함에 있어 반드시 고려되어야 한다. 먼저, 다음과 같은 환경과의 접촉이 있는 상황에서 n개의 자유도를 갖는 매니퓰레이터의 동역학을 생각하자.

여기에서 $q$$\in$$\mathbb{R}$$^{n\times 1}$, $A := A(q)$$\in$$\mathbb{R}$$^{n\times n}$, $B := B(q,\:\dot q)$$\in$ $\mathbb{R}$$^{n\times n}$, $g := g(q)$$\in$$\mathbb{R}$$^{n\times 1}$ 는 각각 관절각, 관절 공간 관성 행렬, 코리올리-원심력 행렬(Coriolis and centrifugal matrix), 그리고 중력 보상 벡터이다. $\Gamma$$\in$$\mathbb{R}$$^{n\times 1}$, $J_{c}:=J_{c}(q)$$\in$$\mathbb{R}$$^{m\times n}$, $f_{c}$$\in$$\mathbb{R}$$^{m\times 1}$은 각각 토크 벡터, 접촉 자코비안(Contact jacobian), 그리고 접촉힘을 나타낸다. 접촉에 대한 기하학적 정보를 모두 알고 있다면 접촉에 대한 자코비안을 얻을 수 있다. 접촉에 대한 자코비안은 매니퓰레이터 말단의 접촉 방향에 대한 속도와 관절 각속도의 관계이다.

식 (1)의 양변에 $J_{c}A^{-1}$을 곱하고 말단 장치에 대한 정기구학 식 $\dot x_{c}= J_{c}\dot q$을 사용하면 매니퓰레이터 말단의 접촉 공간에 대한 동역학은 아래와 같다.

식 (2)는 직교 좌표계에서의 접촉 방향 가속도, 힘, 그리고 관절 토크 간의 관계를 나타낸다. 식 (2)의 양변에 $(J_{c}A^{-1}J_{c}^{T})^{-1}$을 곱하면 아래와 같이 나타낼 수 있다.

여기서 $\Lambda_{c}$, $\mu_{c}$, $p_{c}$, $\bar{J}_{c}$는 다음과 같다.

식 (3)에서 로봇의 말단 장치가 강체와의 접촉을 하는 상황을 고려해보자. 이때, $\dot x_{c,\:}\ddot x_{c}= 0$을 만족하며 접촉힘 $f_{c}$에 대하여 다음과 같이 정리할 수 있다.

식 (5)를 식 (1)의 $f_{c}$에 대입하면 다음과 같이 접촉 제약 동역학을 얻을 수 있다.

여기서 $h_{c},\:P_{c}$는 각각 $J_{c}^{T}(\mu_{c}\dot q +p_{c}),\: J_{c}^{T}\bar{J}_{c}^{T}$로 계산되며 환경과의 접촉에 의해 생기는 토크이다.

로봇의 말단 장치가 강체와 접촉하는 상황은 접촉에 의해 자유도가 줄어들 수 있는 상황이다. 강체와의 접촉에 의해 자유도가 줄어들었을 때, 로봇 말단장치의 정기구학 식은 다음과 같이 정의할 수 있다.

여기서 $x_{o}$$\in$$\mathbb{R}$$^{z\times 1}$, $J_{o}:= J_{o}(q)$$\in$$\mathbb{R}$$^{z\times n}$ 는 각각 작업 공간에서의 매니퓰레이터 말단 장치의 위치와 작업 자코비안이며, $z$는 접촉에 의해 줄어든 말단 장치의 자유도를 의미한다.

이제, 식 (7)의 작업 자코비안을 사용하여 식 (6)을 작업 공간으로 투영하면 아래와 같은 식을 유도할 수 있다.

여기에서 $\Lambda_{o}$, $\mu_{o}$, $p_{o}$, $\bar{J}_{o}$는 다음과 같다.

시스템 모델을 알고 있다고 가정하면 식 (8)을 사용하여 강체와의 접촉에 의해 제약받고 있는 상황에서의 토크 입력을 구할 수 있다.

2.2 운동량 기반 외란관측기

외란관측기를 사용하면 시스템에 가해지는 덩어리 외란을 추정하여 보상할 수 있다. 특히, 운동량 기반 외란관측기는 운동량을 이용하여 덩어리 외란을 추정하고 이를 보상한다⁽¹⁴⁾. 이 절에서는 운동량 기반 외란관측기를 사용한 강인제어기를 소개한다. 외란을 반영한 매니퓰레이터 동역학은 다음과 같다.

여기에서 $\Gamma_{d}$$\in$$\mathbb{R}$$^{n\times 1}$는 각 관절에 가해지는 외란 토크 벡터이다. 시스템에 실제로 인가되는 토크는 각 관절의 액추에이터의 출력 토크에서 외란 토크를 제외한 나머지가 가해지게 된다. 제어 알고리즘에서 제어 입력은 매니퓰레이터의 공칭 모델을 사용하여 계산한다. 매니퓰레이터의 공칭 모델은 다음과 같이 정의한다.

여기에서 $\bar{A,\:}\bar{B,\:}\bar{g}$는 각각 $A,\: B,\: g$에 대한 공칭 모델이며 $\Gamma_{r}$은 공칭 모델을 사용하여 계산한 시스템 제어 입력, 즉 입력 토크이다. 이 시스템 입력 토크를 실제 시스템 식 (10)에 가하는 경우, 실제 모델과 공칭모델간의 차이인 모델 불확실성에 의한 토크가 발생한다.

이 모델 불확실성에 의한 토크와 외란을 아래와 같은 하나의 덩어리 외란 $d$로 생각하자.

시스템의 불확실성과 외란에 의해 생긴 덩어리 외란을 보상하기 위해 그림 1과 같이 외란관측기 모델을 사용하였다. 그리고 Q필터를 사용하여 운동량과 토크 출력을 추정하였다⁽¹⁵⁾.

여기서 $\zeta ,\:\chi$는 각각 운동량 $A\dot q$과 제어 입력 토크 $\Gamma$의 추정 값이며, $\alpha$와 $\mu$는 1차 저역 통과 필터의 설계인자이다. 이 값은 시스템에 가해지는 외란의 범위에 따라 결정하였다.

로봇 시스템 동역학에서 코리올리-원심력 행렬은 유일하지 않다. 알려진 코리올리-원심력 행렬을 계산하는 방법 중 한 가지는 관성 행렬을 사용하는 것이다⁽¹⁶⁾. 이러한 경우 관성 행렬의 미분과 코리올리-원심력 행렬은 반대칭 행렬의 특성($\dot A = B+B^{T}$)을 가지게 되며 운동량의 미분에 대하여 다음과 같이 구할 수 있다.

이를 덩어리 외란에 대해 정리하고, 식 (13)에 대입하면 외란 추정 값은 다음과 같다.

따라서 시스템에 인가되는 제어입력을 덩어리 외란을 보상하여 아래와 같이 설계할 수 있다.

3.1 접촉 방향의 힘 강인 제어

먼저, 그림 2에서 접촉방향에 대해 시스템 외란이 가해지는 상황을 고려해보자.

환경과의 접촉을 강체 접촉이라고 가정하는 경우 $\dot x_{c,\:}\ddot x_{c}= 0$ 이므로 접촉 방향에 대한 일반화된 운동량과 그 미분은 항상 0이 된다. 이러한 이유로, 운동량의 변화량을 추정하여 사용하는 운동량 외란관측기를 접촉 방향에 대해 부적합하다. 따라서 접촉 방향에 대한 강인 제어는 매니퓰레이터의 말단장치에 부착된 힘/토크 센서를 사용하여 접촉힘을 측정하고 측정된 값과 목표 값의 차이를 외란으로 보고 보상하는 방법을 사용한다.

힘/토크 센서의 값에는 큰 잡음이 존재하며 이는 접촉힘 제어의 성능을 감소시키거나 제어기의 발산을 유발할 수 있다. 접촉힘 제어의 성능 저하 및 진동 발생을 억제하기 위해 접촉힘과 제어 입력 각각에 대해 Q필터를 아래와 같이 설계한다.

여기서 $\zeta_{c}$, $\chi_{c}$는 각각 접촉 토크 $\Gamma_{c}$와 상위 제어기 토크 $\Gamma_{c,\:r}$를 추정하는 값이다. 접촉에 사용된 자유도를 $m$이라 할 때 $\alpha_{c}$와 $M_{c}$$\in$$\mathbb{R}$$^{m\times m}$는 Q필터 설계인자이며 $M_{c}$의 경우 각 관절에 대한 지연시간 설계인자 $\mu_{c,\:1,\:}\mu_{c,\:2,\:}\cdots ,\:\mu_{c,\:m}$를 대각으로 나열한 행렬이다.

식 (18)의 $\zeta_{c}$, $\chi_{c}$를 사용하여 접촉 방향에 대한 외란 추정 값을 다음과 같이 정의한다.

이제 상위 제어기 출력 토크 $\Gamma_{c,\:r}$에 추정한 외란 $\hat d_{c}$을 보상하는 제어 입력을 계산하고 이를 시스템에 인가하도록 한다.

3.2 작업 방향의 위치 강인 제어

접촉을 유지하면서 움직이려면 작업 공간에서의 동작이 접촉공간에 영향을 주지 않으면서 수행되어야 한다. 작업 공간이 접촉 공간의 영공간에 존재한다면, 즉 접촉을 유지하면서 주어진 작업을 수행하기 위한 조인트 공간의 해가 존재한다면 다음과 같이 접촉 제약 동역학 식 (6)을 사용하여 접촉을 유지하는 작업 방향 동역학을 유도할 수 있다. 먼저, 외란을 반영한 접촉 제약 동역학을 고려해보자. 식 (17)에서 $\dot x_{c,\:}\ddot x_{c}= 0$이므로 이를 사용해 외란을 포함하는 접촉 제약 동역학으로 정리할 수 있다.

여기서 $h_{c},\:P_{c}$는 각각 $J_{c}^{T}(\mu_{c}\dot q +p_{c}),\: J_{c}^{T}\bar{J}_{c}^{T}$로 계산되며 환경과의 접촉에 의해 생기는 토크이고 $\Gamma_{o,\:r}$는 작업 방향에 대한 상위제어기의 출력 토크를 의미한다.

식 (7)의 작업 자코비안을 사용하여 식 (22)을 작업 공간으로 투영하여 접촉 제약 작업 방향 동역학을 아래와 같이 얻을 수 있다.

여기서 $x_{o}$$\in$$\mathbb{R}$$^{z\times 1}$는 작업 공간에서의 매니퓰레이터 말단 장치의 위치를 의미하며 $\Lambda_{o}$, $\mu_{o}$, $p_{o}$, $\bar{J}_{o}$는 식 (9)와 같이 얻을 수 있다.

이제 시스템에 가해지는 외란을 추정하기 위해 시스템의 현재운동량과 상위 제어기 출력을 추정하고 이 차이를 계산해야 된다. 작업 방향에 대한 운동량과 상위 제어기 출력 힘을 Q필터를 사용하여 추정하였다.

여기서 $\zeta_{o}$, $\chi_{o}$는 각각 $\Lambda_{o}\dot x_{o}$와 $\bar{J}_{o}^{T}\Gamma_{o,\:r}$를 추정하는 값이며, $\alpha_{o}$와 $M_{o}$$\in$$\mathbb{R}$$^{z\times z}$는 Q필터 설계인자이며 $M_{o}$의 경우 각 관절에 대한 지연시간 설계인자 $\mu_{o,\:1,\:}\mu_{o,\:2,\:}\cdots ,\:\mu_{o,\:z}$를 대각으로 나열한 행렬이다.

만약 $\zeta_{o}$, $\chi_{o}$가 각각 $\Lambda_{o}\dot x_{o}$와 $\bar{J}_{o}^{T}\Gamma_{o,\:r}$를 충분히 추종하였다고 할 때 작업 방향에 대한 외란 추정 값은 아래와 같이 구할 수 있다.

외란 추정 값을 사용하여 상위 제어기 출력에서 외란을 보상한 제어 입력 식 (27)을 계산하고 이를 작업 공간, 즉 접촉 공간의 영공간에 투영하여 시스템에 인가한다. 그림 3은 본 논문에서 제안하는 하이브리드 위치-힘 강인 제어 블록도이다.

5.1 매니퓰레이터 모델

그림 5는 수치실험에 사용된 관절 토크 제어가 가능한 7 자유도 매니퓰레이터 Franka Emika Panda를 보여준다. 매니퓰레이터의 말단장치에 사각의 판이 부착하였으며 사각판과 말단장치 사이에 힘/토크 센서를 부착하였다. 본 논문은 수치실험을 위한 프로그램으로 V-REP을 사용하였다⁽¹⁷⁾. 또한, 물리 엔진으로는 Bullet 2.78 엔진이 사용되었다⁽¹⁸⁾.

5.2 하이브리드 위치-힘 강인 제어기를 사용한 수치 실험 결과

이 절에서는 3장에서 제안한 하이브리드 위치-힘 강인 제어에 대한 수치실험 결과에 대해 설명한다. 그림 6은 하이브리드 위치-힘 제어 수치실험 블록도이다. 여기에서, 빨간색 점선은 강인 제어가 적용된 경우에 활성화되는 블록을 의미한다. PD 제어기를 사용하여 말단의 목표 가속도를 계산하고 이를 식 (8)을 사용해 작업 방향의 제어 입력을 얻는다. 수치실험에 사용한 설계인자는 $K_{p}$, $K_{d}: =diag(250,\:250,\:1100)$ $M_{c}:=10^{-1}I^{3\times 3}$, $M_{o}: = 10^{-2}diag(1,\:1,\:0.5)$, $\alpha_{c}$, $\alpha_{o}:=1$으로 설정하였으며 제어기의 최소, 최대 출력 가속도는 -20 [$m/s^{2}$]과 20 [$m/s^{2}$]으로 설정하였다. 그림 7은 작업 방향에 대한 로봇팔 말단장치의 위치 제어 수치실험 결과를 보여준다. 그리고 표 1은 PD제어만 사용하였을 때와 PD제어기와 강인제어를 같이 사용하였을 때의 말단 장치의 위치 오차 노름 값을 보여준다. 작업 방향, 즉 X, Y 방향에 대해서 제안한 강인 제어기를 사용하는 경우 오차가 줄어드는 것을 확인할 수 있다. 따라서 제안하는 제어기가 작업 방향에 대한 외란을 잘 보상하고 있음을 알 수 있다.

5.3 외란 관측 모델 예측 제어기를 사용한 수치 실험 결과

이 절에서는 4장에서 제안한 외란 관측 모델 예측 제어에 대한 수치실험을 진행하고 5.2절의 시뮬레이션 결과와 비교한다. 그림 8은 외란 관측 모델 예측 제어 시뮬레이션 블록도이다. 여기에서, 빨간색 점선은 강인 제어가 적용된 경우에 활성화되는 블록을 의미한다. QP 계산기로 qpOASES를 사용하였다⁽¹⁹⁾.

모델 예측 제어기 출력을 식 (8)을 사용해 작업 방향 제어 입력을 얻는다. 사용한 제어 설계인자는 $Q$, $S := diag(2000I_{z},\: 10^{-4}I_{z})$, $R:=10^{-3}I_{z}$이며 다른 설계인자는 5.2절과 동일하다. 그림 9와 표 2는 작업 방향에 대한 로봇팔 말단의 외란 관측 모델 예측 제어의 결과이다. 표 1과 표 2를 통해 PD 제어, PD 강인 제어, 모델 예측 제어, 외란 관측 모델 예측 제어 순으로 추적 목표와 가까운 것을 확인 할 수 있다. 따라서 시뮬레이션을 통하여 PD제어 기반의 하이브리드 위치-힘 제어기만 사용하는 경우 보다 제안하는 외란관측기를 사용하는 경우가 지령 궤적과의 오차가 작으며, 제안하는 하이브리드 위치-힘 제어를 위한 외란 관측 모델 예측 제어기를 사용하는 경우에 가장 오차가 적은 결과를 얻었다.