Skip to content

작업공간 PD 제어

본 장에서는 작업공간 PD 제어를 예제로 소개합니다. 예제의 목표는 다음과 같습니다.

  • 저역통과 필터와 필터 미분의 사용방법을 학습합니다.
  • 순 기구학과 역 기구학의 사용방법을 학습합니다.
  • Indy의 동역학 파라미터를 사용하는 방법을 학습합니다.

1) Eclipse에서 Task Control Component 를 생성합니다.

2) 아래와 같이 사용할 헤더파일을 포함시키고 타입들을 선언합니다.

1
2
3
4
5
6
7
8
9
#include <Indy/DynamicAnalysis6D.h>
#include <Util/Filter.h>

typedef NRMKFoundation::IndySDK::DynamicAnalysis6D DynamicAnalysis;
typedef NRMKFoundation::CompositeHTransformTaskController TaskController;
typedef NRMKFoundation::FilterLowPass<double> LowPassFilter;
typedef NRMKFoundation::FilterDerivative<double> FilteredDerivative;
typedef typename ROBOT::JointVec JointVec;
typedef typename ROBOT::JointMat JointMat;

3) 제어기 설계와 필터의 사용을 위해서 다음과 같이 변수들을 선언합니다.

1
2
3
4
5
6
7
8
private:
    const double _dim_ratio;
    double _Kp;
    double _Kd;
    TaskVec _e, _edot;
    LowPassFilter _qdot_filtered[ROBOT::JOINT_DOF];
    LowPassFilter _qddot_filtered[ROBOT::JOINT_DOF];
    TaskController _taskController;

4) UserTaskController.cpp 의 생성자에서 다음과 같이 변수들을 초기화 합니다.

1
2
3
4
5
6
UserTaskController::UserTaskController()
:_dim_ratio(1.0/30.0)
{
    _Kp = 500;
    _Kd = 80;
}

5) Task Controller(_taskController)의 제어주기를 설정합니다. Task Controller는 직교공간에서 에러를 정의하기 위해서 사용됩니다.

1
2
3
4
void UserTaskController::setPeriod(double delt)
{
    _taskController.setPeriod(delt);
}

6) computeControlTorq() 함수에 아래 코드와 같이 작업공간 PD 제어기를 작성합니다. computeFK는 순 기구학을 computeJacobian은 자코비안 행렬을 계산합니다. 동역학 계산 및 필터와 관련 된 부분은 주석을 참조하십시오.

 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
int UserTaskController::computeControlTorq(ROBOT & robot, TaskPosition const & posDesired, TaskVelocity const & velDesired, TaskAcceleration const & accDesired, JointVec & torque)
{
    // calculate task kinematics
    TaskPosition pos;
    TaskVelocity vel;
    TaskAcceleration acc;
    TaskJacobian J;
    TaskJacobian Jdot;
    robot.computeFK(pos,vel);
    robot.computeJacobian(pos,vel,J,Jdot);

    // calculate dynamics
    JointMat M,C;
    JointVec g;
    DynamicAnalysis::JointDynamics(robot, M, C, g, LieGroup::Vector3D(0,0,-GRAV_ACC));

    // calculate Task-space PD control
    JointMat Kp = JointMat::Zero();
    JointMat Kd = JointMat::Zero();
    _taskController.error(pos, vel, posDesired, velDesired, _e, _edot);

    // set control gains
    for (int i=0; i<ROBOT::JOINT_DOF;i++)
    {
        if(i<3)
        {
            Kp(i,i) = _Kp*_dim_ratio;
            Kd(i,i) = _Kd*_dim_ratio;
        }
        else
        {
            Kp(i,i) = _Kp;
            Kd(i,i) = _Kd;
        }
    }

    // calculate filtered values
    double temp_vel, temp_acc;
    JointVec qdot, qddot;
    temp_vel = 0; temp_acc = 0;
    for (int i=0; i<JOINT_DOF; i++)
    {
        // lowpass filter
        _qdot_filtered[i].filter(robot.qdot()[i],temp_vel);
        // filtered derivative
        _qddot_filtered[i].filter(robot.qdot()[i],temp_acc);

        qdot(i,0) = temp_vel;
        qddot(i,0) = temp_acc;
    }
    // PD control input
    JointVec forcePD = Kp*_e + Kd*_edot;
    torque = J.transpose()*forcePD + M*qddot + C*qdot + g;

    return 0;
}

다축 로봇 매니퓰레이터를 위한 중력보상제어와 작업공간 PD 제어