Skip to content

관절공간 임피던스 제어

시스템 및 제어기 구성

관절에 작용하는 마찰 및 로봇의 모델 불확실성을 극복하고 명목로봇으로 동작하도록 한다. 이 후, 명목 로봇에 임피던스 제어기를 적용하여 안정하면서도 외력에 유연하게 대처할 수 있도록 한다. 시스템의 구성은 엔드이펙터에 F/T가 장착된 협동로봇이며 모든 상호작용은 엔드에펙터를 통해서 발생한다고 가정한다 (F/T 센서 설치에 관한 내용은 FT 센서를 이용한 마찰보상 예제를 참고).

본 예제를 통해서 다음을 살펴볼 수 있다.

  • F/T센서의 좌표계 변환방법
  • 로봇의 동역학 모델의 사용

Note

안전상의 이유로 위치제어오차가 일정수준 이상일 때 실제로봇은 비상정지 되도록 되어있다. 즉, 작성된 알고리즘이 시뮬레이션 환경에서는 제어이득에 관계없이 잘 작동하지만 실제로봇에서는 비상정지 상황이 발생할 수 있다.

예제 코드 작성

관절 제어 컴포넌트(Joint Control Component)를 생성한 후, 동역학 시뮬레이션 함수와 공유메모리를 사용하기 위해서 다음의 헤더파일을 추가하고 필요한 코드를 작성한다.

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
//for dynamic simulation of nominal robot
#include <Indy/DynamicAnalysis6D.h>
typedef NRMKFoundation::IndySDK::DynamicAnalysis6D DynamicAnalysis;

//for the use of shared memeory
#include <NRMKFramework/Indy/Sharedmemory/SharedData.h>
NRMKFramework::ShmemManager * indyShm = NULL;
NRMKIndy::SharedData::ExtraIOData extioData; // extra input-output data
NRMKIndy::SharedData::RobotControlSharedData ctrData; // control data
NRMKIndy::SharedData::RobotControlStatusSharedData ctrStatusData; // control status data

임피던스 제어기를 설계하기 위해 필요한 다음의 함수와 변수들을 관절 제어 컴포넌트의 헤더파일에 작성한다.

 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
// type definition
typedef typename ROBOT::JointMat JointMat;
typedef typename ROBOT::TaskPosition TaskPosition;
typedef typename ROBOT::TaskVelocity TaskVelocity;
typedef typename ROBOT::TaskJacobian TaskJacobian;

// To calculate the control input for impedance control
int computeImpedanceControlTorq(JointVec const & qdes, JointVec const & q,
JointVec const & qdot, JointVec const & tauGrav, JointVec & tauImp);

void setBias() {_ftBias = _ftSensor;}
void readFTSensor(ROBOT & robot);

public:
    double _delT;
    double _t;
    int key;
    int flag_bias;

private:
    ROBOT * _robotNom;
    bool init_robotNom;

    JointVec tauGrav, tauGrav_nom;
    JointVec fext, tauExt;

    JointMat M, C, D, Mhat;
    JointVec g;

    TaskPosition _tpos;
    TaskVelocity _tvel;
    TaskPosition _tpos_nom;
    TaskVelocity _tvel_nom;
    TaskJacobian _J, _Jdot, _J_nom, _Jdot_nom;

private:
    // with respect to ft sensor
    LieGroup::Wrench    _ftBias;
    LieGroup::Wrench    _ftSensor;
    LieGroup::Rotation  _ft_sensor_orientation;
    LieGroup::Wrench    _ftTransformed, _ftTransformed_filtered;

공유 메모리에서 F/T센서 정보를 획득하기 위해서 다음과 같이 생성자에서 포인터 변수에 메모리를 할당한다 (자세한 내용은 IndyFramework 또는 F/T센서 이용 참고).

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
19
20
JointControlComponent::JointControlComponent()
: AbstractController(USERNAME, EMAIL, SERIAL)
, _robotNom(NULL)
{
    // initialize shared memory
    // INDY_SHM_NAME: "indySHM"
    // INDY_SHM_LEN: 0x1000000 16MB == 0x000000~0xFFFFFF (~16777216)
    indyShm = new NRMKFramework::ShmemManager(INDY_SHM_NAME, INDY_SHM_LEN);
    for (int i=0; i<6; i++) _ftBias[i] = 0;

    // initialize control time: 4kHz control frequency
    _delT = 0.00025;

    // initialize variables
    D.setZero();
    _J.setZero();
    _Jdot.setZero();
    Mhat.setZero();
    tauExt.setZero();
}

다음으로 종료 시 할당된 메모리를 해제하기 위해서 다음과 같이 공유 메모리와 명목 로봇에 할당된 메모리를 해제한다.

1
2
3
4
5
6
7
8
9
JointControlComponent::~JointControlComponent()
{
    // destroy nominal robot
    if (_robotNom != NULL) delete _robotNom;

    // destroy shared memory
    if (indyShm != NULL)
        delete indyShm;
}

본 예제에서는 관절공간 임피던스 제어기를 구현하기 위해 computeImpedanceControlTorq() 함수를 정의하여 비례-미분(Proportional-Derivative)형태의 강성-댐퍼 제어기를 사용한다.

 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
43
44
int JointControlComponent::computeImpedanceControlTorq(JointVec const &
qdes, JointVec const & q, JointVec const & qdot, JointVec const & tauGrav,
JointVec & tauImp)
{
    // initialize parameters
    JointMat kp, kd;    //p- and d-gains

    kp.setZero();
    kd.setZero();

    // set pd control gains
    for (int i=0; i < JOINT_DOF; i++)
    {
        switch(i)
        {
        //for 1st and 2nd joints
        case 0:
        case 1:
            kp(i,i) = 70;
            kd(i,i) = 5;
            break;
        //for 3rd joint
        case 2:
            kp(i,i) = 25;
            kd(i,i) = 3;
            break;
        //for 4th and 5th joints
        case 3:
        case 4:
            kp(i,i) = 25;
            kd(i,i) = 3;
            break;
        //for 6th joint
        case 5:
            kp(i,i) = 18;
            kd(i,i) = 1.5;
            break;
        }
    }

    tauImp = kp*(qdes - q) - kd*qdot;

    return 0;
}

위에서 도입한 비례-미분 형태의 제어기는 실제로봇의 마찰이나 중력에 의해서 제대로 동작하지 않는다. 그러므로 마찰을 보상하고 로봇의 안정성을 보장하는 범위에서 로봇의 질량을 수정하기 위해서 computecontrolTorq() 함수에 다음과 같이 강인제어기를 구현한다.

  • 외력을 인지하기 위해서 공유메모리를 이용해서 F/T센서를 통해서 외력을 측정한다. F/T센서 이용 및 그리퍼 구동에서 언급한 바와 같이 F/T센서 값을 획득하기 위해서 ExtraIODataControlData 둘 중 어느 구조체를 사용해도 된다.

  • F/T센서 값은 (힘, 모멘트)로 측정되지만 IndySDK에서 정의되는 Wrench는 (모멘트, 힘) 순으로 정의되기 떄문에 위치를 바꿔주도록 한다.

  • 작업이 새로 시작될 때 명목로봇의 기구학적 정보를 실제로봇의 것으로 초기화 한다. 관절 각도 및 속도를 대입하고 상태변수 업데이트 및 속도기구학 정보 업데이트를 수행한다.

  • 명목로봇과 실제로봇의 순기구학을 풀고 자코비안을 계산한다. 외력, 중력, 그리고 임피던스 제어 입력으로 부터 명목 로봇의 가속도를 계산한다.

  • 명목로봇과 실제로봇의 차이로 부터 마찰 및 모델 불확실성을 보상하기 위한 보조 입력을 계산한다.

  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
 43
 44
 45
 46
 47
 48
 49
 50
 51
 52
 53
 54
 55
 56
 57
 58
 59
 60
 61
 62
 63
 64
 65
 66
 67
 68
 69
 70
 71
 72
 73
 74
 75
 76
 77
 78
 79
 80
 81
 82
 83
 84
 85
 86
 87
 88
 89
 90
 91
 92
 93
 94
 95
 96
 97
 98
 99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
int JointControlComponent::computeControlTorq(ROBOT & robot,
LieGroup::Vector3D const & gravDir, JointVec const & qDesired,
JointVec const & qdotDesired, JointVec const & qddotDesired,
JointVec & torque)
{
    // wait until servo is On
    NRMKIndy::SharedData::getControlStatusData(*indyShm, ctrStatusData);
    if(!ctrStatusData.isReady)
    {
        robot.idyn_gravity(gravDir);
        torque = robot.tau();

        return 0;
    }

    // update time
    _t += _delT;

    // read ft sensor value; the function is defined below
    readFTSensor(robot);

    // update initial states of nominal robot with those of real robot
    if (init_robotNom == true)
    {
        //update target coordinates
        _robotNom->setTtarget(robot.Ttarget().R(), robot.Ttarget().r());
        _robotNom->setTReference(robot.Tref().R(), robot.Tref().r());
        _robotNom->q() = robot.q();
        _robotNom->qdot() = robot.qdot();
        _robotNom->update();
        _robotNom->updateJacobian();

        init_robotNom = false;
    }

    static bool sim_mode = true;
    // calculate real and nominal robot dynamics
    //DynamicAnalysis::JointDynamics(*_robotNom, M, C, g,
    //                          LieGroup::Vector3D(0, 0, -GRAV_ACC));
    _robotNom->computeDynamicsParams(gravDir, M, C, g);
    Mhat = M;
    for (int i=0; i<JOINT_DOF; i++)
    {
        switch(i)
        {
        case 0:
        case 1:
            if(sim_mode) Mhat(i,i) = M(i,i);
            else Mhat(i,i) = M(i,i) + 40;
            break;
        case 2:
            if(sim_mode) Mhat(i,i) = M(i,i);
            else Mhat(i,i) = M(i,i) + 20;
            break;
        case 3:
        case 4:
        case 5:
            if(sim_mode) Mhat(i,i) = M(i,i);
            else  Mhat(i,i) = M(i,i) + 10;
            break;
        }
    }

    robot.idyn_gravity(LieGroup::Vector3D(0, 0, -GRAV_ACC));
    robot.computeFK(_tpos, _tvel);
    robot.computeJacobian(_tpos, _tvel, _J, _Jdot);

    _robotNom->idyn_gravity(LieGroup::Vector3D(0, 0, -GRAV_ACC));
    _robotNom->computeFK(_tpos_nom, _tvel_nom);
    _robotNom->computeJacobian(_tpos_nom, _tvel_nom, _J_nom, _Jdot_nom);

    // ** controller
    JointVec tauImp, tauAux;
    JointVec qddot_nom, err, err_dot;
    tauImp.setZero();
    tauAux.setZero();
    qddot_nom.setZero();

    tauExt = _J.transpose()*_ftSensor;
    tauGrav = robot.tau();
    tauGrav_nom = _robotNom->tau();

    // set control gain
    JointVec K, KC, KD;
    if(sim_mode == true)
    {
        K << 1.0, 1.0, 1.0, 1.0, 1.0, 0.02;
        KC << 80, 80, 40, 25, 25, 25;
        KD << 55, 55, 30, 15, 15, 15;
    }else
    {
        K << 5.0, 5.0, 5.0, 5.0, 5.0, 5.0;
        KC << 80, 80, 40, 25, 25, 25;
        KD << 55, 55, 30, 15, 15, 15;
    }

    // calculate impedance control input
    computeImpedanceControlTorq(qDesired, _robotNom->q(), _robotNom->qdot(), tauGrav, tauImp);
    qddot_nom = Mhat.inverse()*(-(C + 3.0*D)*_robotNom->qdot() + K.cwiseProduct(tauImp + tauExt));
    // update nominal states
    _robotNom->qdot() += (float) _delT*qddot_nom;
    _robotNom->q() += (float) _delT*_robotNom->qdot();

    _robotNom->update();
    _robotNom->updateJacobian();

    // calcualte error
    err = _robotNom->q() - robot.q();
    err_dot = _robotNom->qdot() - robot.qdot();

    // calculate auxiliary control input
    tauAux = 4.0*KC.cwiseProduct(err) + 4.0*KD.cwiseProduct(err_dot);

    // calculate final control input
    torque = tauGrav - tauExt + K.cwiseProduct(tauImp + tauAux + tauExt);

    return 0;
}

readFTSensor 함수는 F/T 센서 데이터를 획득하고 변환하기 위해 사용된다.

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
19
20
21
void JointControlComponent::readFTSensor(ROBOT & robot)
{
    NRMKIndy::SharedData::getControlData(*indyShm, ctrData);
    TaskPosition _tposT;
    TaskVelocity _tvelT;
    LieGroup::Wrench _ftSensorTemp;
    _ftSensorTemp.setZero();

    robot.computeFK(_tposT, _tvelT);

    for (int i=0; i<6; i++) _ftSensorTemp[i] = ctrData.Fext[i];
    _ftSensorTemp = _ftSensorTemp - _ftBias;

    LieGroup::HTransform T_ft_target = robot.Ttarget().icascade(robot.Tft());
    LieGroup::Wrench _ftSensorTemp2 = T_ft_target.itransform(_ftSensorTemp);
    LieGroup::Wrench _ftSensorTransformed =
        LieGroup::Wrench(_tposT.R()*_ftSensorTemp2.f(), _ftSensorTemp2.n());

    for (int i=0; i<3; i++) _ftSensor[i] = _ftSensorTransformed[i+3];
    for (int i=0; i<3; i++) _ftSensor[i+3] = _ftSensorTransformed[i];
}

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

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
void JointControlComponent::reset()
{
    // get ft sensor values from shared memory
     NRMKIndy::SharedData::getControlData(*indyShm, ctrData);

     for (int i=0; i<6; i++) _ftSensor[i] = ctrData.Fext[i];

     // compensate ft sensor bias
     setBias();

    init_robotNom = true;
}

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

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
int JointControlComponent::computeGravityTorq(ROBOT & robot, LieGroup::Vector3D const & gravDir, JointVec & torque)
{
    // compute gravitational torque
    JointVec tauGrav;
    robot.idyn_gravity(LieGroup::Vector3D(0, 0, -GRAV_ACC));
    tauGrav = robot.tau();

    // update torque control input
    torque = tauGrav;

    return 0;
}