Skip to content

Computed Torque Method(CTM)기반 위치제어

본 예제에서는 아래 개요에서 간략하게 설명 된 Computed Torque Method기반 위치제어를 IndySDK를 이용하여 구현하고 Indy 시뮬레이션을 통해서 실행해 본다.

제어기 개요

먼저 CTM기반 위치제어에 대해서 간략하게 설명한다. 다자유도 로봇의 동역학은 다음과 같이 주어진다. \begin{align} \tau + J^{T}F_{ext} = M(q)\ddot{q} + C(q,\dot{q})\dot{q} + g(q), \end{align} 여기서 M(q), C(q,\dot{q}), g(q)는 각각 로봇의 관성행렬, 코리올리 행렬, 중력 벡터를 나타낸다. F_{ext}는 외력을 \tau는 제어 입력을 나타낸다.

비선형이며 서로 복잡하게 결합된 특성을 가지는 로봇 동역학에 대응하고 고성능의 위치제어기를 구현하기 위해서 다음과 같이 로봇 동역학을 기반으로 하는제어입력을 생성한다.

\begin{align} \tau = M(q)\ddot{q}_{ref} + C(q,\dot{q})\dot{q} + g(q) - J^{T}F_{ext} \end{align}

그러면 다음과 같이 독립적이고 선형의 시스템 동역학을 구현할 수 있다.

\begin{align} \ddot{q} = \ddot{q}_{ref} \end{align}

다음으로 레퍼런스 가속도 \ddot{q}_{ref}를 다음과 같이 정의한다.

\begin{align} \ddot{q}_{ref} = \ddot{q}_{des} + K_{v}\dot{e} + K_{p}e, \end{align}

여기서 e = q_{des} - q이다. 결과적으로 로봇 시스템이 다음과 같이 이차 상미분 방정식을 만족하도록 제어할 수 있다.

\begin{align} \ddot{e} + K_{v}\dot{e} + K_{p}e = 0. \end{align}

지수 안정성을 만족시키며 오차 값이 평형점(equillibrium point)으로 수렴한다는 사실을 알 수 있다 (즉, q_{des} \rightarrow q).

예제 코드 작성

로봇의 동역학 모델을 사용하기 위해서 DynamicAnalysis6D.h 를 선언하고 타입 DynamicAnalysis 를 정의한다.

1
2
#include <Indy/DynamicAnalysis6D.h>
typedef NRMKFoundation::IndySDK::DynamicAnalysis6D DynamicAnalysis;

다음으로 computeControlTorq() 함수에 아래 코드와 같이 작성한다. 여기서 JointDynamics 함수는 M, C, g에 각각 로봇의 관성행렬, 코리올리 행렬 및 중력벡터를 반환한다.

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
JointMat M, C;
JointVec g;

DynamicAnalysis::JointDynamics(robot, M, C, g, LieGroup::Vector3D(0,0,-GRAV_ACC));

//possible to use AbstractRobot6D's library: computeDynamicsParams
//instead of using DynanmicAnalysis's library: JointDynamics
//robot.computeDynamicsParams(robot,M,C,g,LieGroup::Vector3D(0,0,-GRAV_ACC));

JointMat Kp, Kv;
JointVec tauCTM;
JointVec qddot_ref;

for (int i=0; i<JOINT_DOF; ++i)
{
  switch(i)
  {
  case 0:
  case 1:
    Kp(i,i) = 70;
    Kv(i,i) = 55;
    break;
  case 2:
    Kp(i,i) = 40;
    Kv(i,i) = 30;
    break;
  case 3:
  case 4:
    Kp(i,i) = 25;
    Kv(i,i) = 15;
    break;
  case 5:
    Kp(i,i) = 18;
    Kv(i,i) = 3;
    break;
  }
}

qddot_ref = qddotDesired + Kv*(qdotDesired - robot.qdot()) + Kp*(qDesired - robot.q());
tauCTM = M*qddot_ref + C*robot.qdot() + g;

torque = tauCTM;