비선형 $\mathcal{H}_\infty$ 최적 위치제어기

제어기 개요

IndySDK 사용자는 뉴로메카에서 제공하는 위치제어기를 사용할 수 있다. 본 예제에서는 $\mathcal{H}_\infty$ 최적제어기를 각각 관절공간과 작업공간에서 사용하는 방법에 대하여 설명한다. $\mathcal{H}_\infty$ 최적 제어기는 로봇의 모델 값으로 부터 계산되는 레퍼런스 속도 ($\dot{q}_{ref}$)에 대한 피드포워드제어 부분과 외란 및 모델 불확실성을 $\mathcal{H}_\infty$ 측면에서 최적화 하는 피드백제어 부분으로 구성된다.

$$\begin{align} \tau = \widehat{M}(q)\ddot{q}_{ref} + \widehat{C}(q,\dot{q})\dot{q}_{ref} + \widehat{g}(q) + K\dot{e}_{ref}, \end{align}$$

여기서, $\widehat{M}$, $\widehat{C}$, $\widehat{g}$는 각각 계산된 로봇의 관성질량, 코리올리 행렬, 중력벡터를 나타낸다. 또한 $\dot{e}_{ref} = \dot{q}_{ref} - \dot{q}$,

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

$e = q_{des} - q$ 이며 $q_{des}$는 원하는 위치궤적을 나타낸다. $K$, $K_{v}$, $K_{p}$는 각각 제어이득을 나타낸다.

더욱 자세한 사항은 아래의 참고문헌을 참고하기 바란다.

• Design of a Robust $\mathcal{H}_\infty$ PID Control for Industrial Manipulators

예제 코드 작성

관절공간 위치제어기

관절 제어 컴포넌트(Joint Control Component)를 생성한 후, 제어기 리셋 및 제어이득 설정을 수행 할 수 있도록 다음과 같이 헤더파일에 변수들을 추가한다.

private:
    bool reset_ctrl;         // for controller reset
    JointVec _kp, _kv, _ki;  // for control gain settings

크로스 컴파일(Cross Compile)된 관절 제어 컴포넌트를 로봇제어기(Control Box)에서 정상적으로 사용하기 위해서 다음과 같이 컴포넌트의 cpp파일 상단에 위치한 라이센스 정보를 기입한다. (IndySDK 구매 시 발급된 라이센스 정보를 사용, 자세한 정보는 ''시작하기'' 참조).

/***** License Information *****/
#define USERNAME "NEUROMEKA"
#define EMAIL "NEUROMEKA@neuromeka.com"
#define SERIAL "NEUROMEKA1234567890"
/******************************/

다음으로 computeControlTorq() 함수를 구현한다. 해당 함수는 제어 (토크)입력을 계한하기 위한 함수이며 아래 세 가지의 함수들이 제어 입력의 계산과정에서 사용 될 것이다.

• resetHinfController() 함수는 제어기의 적분 동자을 리셋하기 위해 사용된다.
• HinfController() 는 뉴로메카에서 제공하는 비선형 최적 위치제어기이다.
• setHinfControlGain() 는 제어 이득 값들을 조정하기 위해 사용된다.

먼저 아래의 코드를 생성된 cpp파일의 computeControlTorq() 함수에 추가한다.

    if (reset_ctrl)
    {
            robot.resetHinfController();
            reset_ctrl = false;
    }

    robot.setHinfControlGain(_kp, _kv, _ki);
    robot.HinfController(gravDir, qDesired, qdotDesired, qddotDesired, torque);

이제 생성자와 파괴자에 다음의 코드를 추가한다. 해당 코드들은 제어기를 초기화하고 리셋하기 위해 사용된다. 함수 initHinfController() 는 시간에 대한 알고리즘 연산을 수행하기 위해 제어기에 샘플링 시간을 설정한다.

다음과 같이 생성자와 파괴자를 수정한다.

JointControlComponent::JointControlComponent()
: AbstractController(USERNAME, EMAIL, SERIAL)
, _robotNom(NULL)
{
    reset_ctrl = true;
}

void JointControlComponent::initialize(ROBOT & robot, double delt)
{
    _robotNom = AbstractController::createRobotObj();
    robot.initHinfController(delt);
}

경로점간 이동작업을 완료하고 새로운 경로점으로의 이동을 위해 재구동 시 제어기 리셋을 수행한다. 다음과 같이 reset() 함수를 작성한다.

void JointControlComponent::reset()
{
    reset_ctrl = true;
}

뉴로메카에서 제공하는 교시장치 Conty를 이용하여 제어이득을 간편하게 수정하기 위해서 다음과 같이 setGains() 함수를 작성한다. Conty 의 Gain tuning > Manual tuning 메뉴에서 제어이득을 조절할 수 있다.

void JointControlComponent::setGains(JointVec const & kp, JointVec const & kv,
JointVec const & ki)
{
    _kp = kp;
    _kv = kv;
    _ki = ki;
}

마지막으로 관절 제어 컴포넌트에는 직접교시모드 사용에 대한 추가적인 정의를 하도록 되어있다. 본 예제에서는 다음과 같이 간단한 중력보상제어를 적용하도록 한다.

int JointControlComponent::computeGravityTorq(ROBOT & robot,
LieGroup::Vector3D const & gravDir, JointVec & torque)
{
    JointVec resTorq;
    robot.idyn_gravity(gravDir);
    resTorq = robot.tau();

    torque = resTorq;

    return 0;
}

작업공간 위치제어기

기본적인 사용법은 관절공간 위치제어기 사용방법과 동일하다. 단지, 아래에서 보는바와 같이 제어기의 입력으로 관절의 각도, 속도, 각속도 대신에 작업공간 값인 엔드이펙터의 위치, 속도, 가속도 값이 사용된다.

int TaskControlComponent::computeControlTorq(ROBOT & robot,
LieGroup::Vector3D const & gravDir, TaskPosition const & posDesired,
TaskVelocity const & velDesired, TaskAcceleration const & accDesired,
JointVec & torque)
{
 if (reset_ctrl)
 {
   robot.resetHinfController();
   reset_ctrl = false;
 }

 robot.setHinfControlGain(_kp, _kv, _ki);
 robot.HinfController(gravDir, posDesired, velDesired, accDesired, torque);


 return 0;
}