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 를 정의한다.

#include <Indy/DynamicAnalysis6D.h>
//type definitaion
typedef typename ROBOT::JointMat JointMat;
typedef NRMKFoundation::IndySDK::DynamicAnalysis6D DynamicAnalysis;

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

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;

Kp.setZero();
Kv.setZero();

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;