Skip to content

비선형 \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}는 각각 제어이득을 나타낸다.

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

예제 코드 작성

관절공간 위치제어기

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

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

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

1
2
3
4
5
/***** License Information *****/
#define USERNAME "NEUROMEKA"
#define EMAIL "NEUROMEKA@neuromeka.com"
#define SERIAL "NEUROMEKA1234567890"
/******************************/

제어입력(토크)을 계산하기 위해서 computeControlTorq() 함수를 작성한다. resetHinfController() 는 제어기의 적분에러를 초기화 하는 함수이며 HinfController() 는 뉴로메카에서 제공하는 비선형 최적제어 기반의 위치제어기 이며 setHinfControlGain() 는 제어기의 피드백제어 부분의 제어이득을 조절하는 함수이다.

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
int JointControlComponent::computeControlTorq(ROBOT & robot,
LieGroup::Vector3D const & gravDir, JointVec const & qDesired,
JointVec const & qdotDesired, JointVec const & qddotDesired,
JointVec & torque)
{
    if (reset_ctrl)
    {
        robot.resetHinfController();
        reset_ctrl = false;
    }

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

    return 0;
}

제어기 초기화를 위해 다음과 같이 생성자 및 초기화 함수를 작성한다. initHinfCOntroller(delt) 는 제어기에서 사용되는 수학연산을 수행하기 위한 제어이산시간 설정을 수행한다.

1
2
3
4
5
6
JointControlComponent::JointControlComponent()
: AbstractController(USERNAME, EMAIL, SERIAL)
, _robotNom(NULL)
{
    reset_ctrl = true;
}
1
2
3
4
5
void JointControlComponent::initialize(ROBOT & robot, double delt)
{
    _robotNom = AbstractController::createRobotObj();
    robot.initHinfController(delt);
}

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

1
2
3
4
void JointControlComponent::reset()
{
    reset_ctrl = true;
}

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

1
2
3
4
5
6
7
void JointControlComponent::setGains(JointVec const & kp, JointVec const & kv,
JointVec const & ki)
{
    _kp = kp;
    _kv = kv;
    _ki = ki;
}

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

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
int JointControlComponent::computeGravityTorq(ROBOT & robot,
LieGroup::Vector3D const & gravDir, JointVec & torque)
{
    JointVec resTorq;
    robot.idyn_gravity(gravDir);
    resTorq = robot.tau();

    torque = resTorq;

    return 0;
}

작업공간 위치제어기

기본적인 사용법은 관절공간 위치제어기 사용방법과 동일하다. 단지, 제어기의 입력으로 관절의 각도, 속도, 각속도 대신에 작업공간에서 정의되는 값이 사용된다.

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
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;
}