Skip to content

작업공간 PD 제어

개요

본 장에서는 시뮬레이션 환경에서 Indy의 작업공간 PD 제어를 구현하며 다음을 살펴본다.

  • 저역통과 필터와 필터 미분의 사용방법
  • 순 기구학과 역 기구학의 사용방법
  • Indy의 동역학 파라미터를 획득하는 방법

예제 코드 작성

Eclipse에서 Task Control Component 를 생성하고 아래와 같이 사용할 헤더파일들을 포함시키고 타입들을 선언한다.

  • Indydml 동역학 파라미터를 획득하기 위한 DynamicAnalysis6D.h
  • 저역필터 및 필터 미분을 사용하기 위한 Filter.h

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
//use Indy7's dynamic parameters
#include <Indy/DynamicAnalysis6D.h>

//use filters such as lowpass filter and filtered derivative
#include <Util/Filter.h>

//type definition for simplicity
typedef NRMKFoundation::IndySDK::DynamicAnalysis6D DynamicAnalysis;
typedef NRMKFoundation::FilterLowPass<double> LowPassFilter;
typedef NRMKFoundation::FilterDerivative<double> FilteredDerivative;
typedef typename ROBOT::JointVec JointVec;
typedef typename ROBOT::JointMat JointMat;

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

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
enum
{
    JOINT_DOF = AbstractRobot6D::JOINT_DOF
};
private:
    const double _dim_ratio;
    double _Kp;
    double _Kd;
    TaskVec _e, _edot;
    LowPassFilter _qdot_filtered[ROBOT::JOINT_DOF];
    LowPassFilter _qddot_filtered[ROBOT::JOINT_DOF];
  • dim_ratio: 직선이동과 회전이동간의 제어이득 비율을 위해 사용
  • K_{p}, K_{d}: PD제어기의 제어 이득 값들

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

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

initTaskErr() 함수에 샘플링 타임을 설정한다. 해당 함수는 직교공간에서 오차를 정의하기 위해서 사용된다.

1
2
3
4
void UserTaskController::initialize(ROBOT & robot, double delt)
{
    robot.initTaskErr(delt);
}

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
57
58
59
60
61
62
63
64
65
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);

    // define matrices for calculating robot dynamics
    JointMat M,C;
    JointVec g;

    //use DynamicAnalysis's library
    DynamicAnalysis::JointDynamics(robot, M, C, g, LieGroup::Vector3D(0,0,-GRAV_ACC));

    //use AbstractRobot6D's library
    //robot.computeDynamicsParams(robot, M, C, g, LieGroup::Vector3D(0,0,-GRAV_ACC));


    // calculate Task-space PD control
    JointMat Kp = JointMat::Zero();
    JointMat Kd = JointMat::Zero();

    // calculate errors in the task domain
    // this function is especially for rotation error calculations
    robot.computeTaskErr(pos, vel, posDeisred, 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 제어