Skip to content

관절공간 임피던스 제어

시스템 및 제어기 구성

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

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

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

예제 코드 작성

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

1
2
3
4
5
6
7
8
9
//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

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

 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
// To calculate the control input for impedance control
int computeImpedanceControlTorq(JointVec const & qdes, JointVec const & q,
JointVec const & qdot, JointVec const & tauGrav, JointVec & tauImp);

public:
    double _ftBias[6];
    double _delT;
    double _t;
    int key;
    int flag_bias;

private:
    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;

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

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

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

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
45
46
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
    JointVec tauPD;

    kp.setZero();
    kd.setZero();
    tauPD.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
int JointControlComponent::computeControlTorq(ROBOT & robot,
LieGroup::Vector3D const & gravDir, JointVec const & qDesired,
JointVec const & qdotDesired, JointVec const & qddotDesired,
JointVec & torque)
{
    // update time
    _t += _delT;

    // read shared memory
    NRMKIndy::SharedData::getExtraIOData(*indyShm, extioData);
    NRMKIndy::SharedData::getControlData(*indyShm, ctrData);

    // read FT sensor
    for (int i=0; i <JOINT_DOF/2; i++) fext[i+3] = ctrData.Fext[i];
    for (int i=0; i <JOINT_DOF/2; i++) fext[i] = ctrData.Fext[i+3];

    // 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.Ttarget().R(), robot.Ttarget().r());
        _robotNom->q() = robot.q();
        _robotNom->qdot() = robot.qdot();
        _robotNom->update();
        _robotNom->updateJacobian();
    }

    // calculate real and nominal robot dynamics
    DynamicAnalysis::JointDynamics(*_robotNom, M, C, g,
                                LieGroup::Vector3D(0, 0, -GRAV_ACC));

    for (int i=0; i<JOINT_DOF; i++)
    {
        switch(i)
        {
        case 0:
        case 1:
            Mhat(i,i) = M(i,i) + 40;
            break;
        case 2:
            Mhat(i,i) = M(i,i) + 20;
            break;
        case 3:
        case 4:
        case 5:
            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();
    qddot_nom.setZero();

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

    // set control gain
    JointVec K, KC, KD;
    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();

    // 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;
}

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

1
2
3
4
void JointControlComponent::reset()
{
    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;
}