Skip to content

관절경로 보간

삼차 스플라인을 이용한 보간

IndySDK에서는 저수준의 제어기 뿐만이 아니라 로봇의 모션을 생성하는데 있어서 사용자가 직접 궤적을 수정할 수 있다. 본 예제에서는 삼차 스플라인 기법을 이용해서 관절경로를 보간하고 일정한 시간후에 다음 경로점에 도달하도록 관절궤적을 구한다. 삼차 스플라인은 다음 4개의 조건을 만족시키도록 함수 S_{i}의 계수를 구하는 문제이다.


  • S_{j}(x_{j}) = f(x_{j}), S_{j}(x_{j+1})=f(x_{j+1}), \forall j=0,1,\cdots n-1
  • S_{j+1}(x_{j+1}) = S_{j}(x_{j+1}), \forall j=0,1,\cdots n-2
  • S_{j+1}^{'}(x_{j+1}) = S_{j}^{'}(x_{j+1}), \forall j=0,1,\cdots n-2
  • S_{j+1}^{''}(x_{j+1}) = S_{j}^{''}(x_{j+1}), \forall j=0,1,\cdots n-2

다음 둘 중 하나의 양 끝 쪽에서의 조건을 이용하면 삼차 스플라인의 계수를 계산할 수 있다.

  • S^{''}(x_{0}) = S^{''}(x_{n}) = 0
  • S^{'}(x_{0}) = f^{'}(x_{0}), S^{'}(x_{n}) = f^{'}(x_{n})

예제 코드 작성

관절 궤적 보간 컴포넌트(Joint Trajectory Interpolation component)를 생성하고 다음과 같은 함수 및 변수들을 헤더파일에 선언한다.

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
19
20
21
22
//interpolate paths
void Interpolate();

// coefficients: q(s) = a0*s^3 + a1*s^2 + a2*s + a3
Eigen::MatrixXd a0;
Eigen::MatrixXd a1;
Eigen::MatrixXd a2;
Eigen::MatrixXd a3;

double  _t0;
double  _s;
double  _nSegment;
double  _segTime;
VecType _readyPos;

bool    _isTargetReached = false;
bool    _isTrajSet = false;

std::vector<VecType> _Waypoint;
int nPts;
int nSplines;
int dim;

관절 궤적 보간 컴포넌트는 크게 setPath 함수와 traj 함수로 구성되어 있다. setPath 는 콘티 (또는 다른 인터페이스)를 통해서 입력된 웨이포인트를 기반으로 경로점을 지정하는 함수이다. traj 는 경로점을 기반으로 위치제어기가 매 제어주기 마다 추종할 수 있는 궤적을 생성하는 함수이다.

벡터형 포인터의 타입을 통해서 전달되는 웨이포인트들을 이용하여 다음과 같이 setPath 함수를 작성한다.

 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
void JointTrajectoryInterpolationComponent::setPath(const VecType *path,
int len, double * maxDeviation)
{

    std::vector<VecType> wp_angle;

    _readyPos = path[0];
    wp_angle.push_back(path[0]);

    for (int i = 1; i< len; i++)
    {
        wp_angle.push_back(path[i]);
    }

    _Waypoint = wp_angle;
    nPts = wp_angle.size();
    nSplines = wp_angle.size()-1;
    dim = 6;

    // interpolation
    a0.resize(dim, nSplines);
    a1.resize(dim, nSplines);
    a2.resize(dim, nSplines);
    a3.resize(dim, nSplines);

    Interpolate();

    _nSegment = len-1;

    printf("set Path\n");
}

본 예제에서는 삼차 스플라인을 이용하여 보간을 수행하기 때문에 아래와 같은 방법으로 스플라인의 계수들을 도출한다. 주석으로 표시되었듯이 각각의 묶음은 처음에 개요에서 언급한 조건들을 표현한 것이다.

 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
void JointTrajectoryInterpolationComponent::Interpolate()
{
    Eigen::MatrixXd coeff_A;
    Eigen::MatrixXd coeff_b;
    Eigen::MatrixXd coeff;
    for (int j=0; j<dim; j++){
        coeff_A = Eigen::MatrixXd::Zero(4*nSplines,4*nSplines);
        coeff_b = Eigen::MatrixXd::Zero(4*nSplines,1);
        coeff = Eigen::MatrixXd::Zero(4*nSplines,1);
        for (int i=0; i<nSplines; i++){
            // N  = nSplines
            // N conditions: x = f_i(s_i)
            coeff_A(i,i*4+0) = pow(i,3);
            coeff_A(i,i*4+1) = pow(i,2);
            coeff_A(i,i*4+2) = i;
            coeff_A(i,i*4+3) = 1.0;
            coeff_b(i,0) = _Waypoint[i][j];

            // N conditions: x = f_i(s_(i+1))
            coeff_A(nSplines+i,i*4+0) = pow(i+1,3);
            coeff_A(nSplines+i,i*4+1) = pow(i+1,2);
            coeff_A(nSplines+i,i*4+2) = i+1;
            coeff_A(nSplines+i,i*4+3) = 1.0;
            coeff_b(nSplines+i,0) = _Waypoint[i+1][j];

            // 2*(N-1) conditions: f_i'(s_(i+1)) = f_(i+1)'(s_(i+1)), f_i''(s_(i+1)) = f_(i+1)''(s_(i+1))
            if(i<nSplines-1){
                coeff_A(2*nSplines+i, i*4+0) = 3.0*pow(i+1,2);
                coeff_A(2*nSplines+i, i*4+1) = 2.0*(i+1);
                coeff_A(2*nSplines+i, i*4+2) = 1.0;
                coeff_A(2*nSplines+i,(i+1)*4+0) = -3.0*pow(i+1,2);
                coeff_A(2*nSplines+i,(i+1)*4+1) = -2.0*(i+1);
                coeff_A(2*nSplines+i,(i+1)*4+2) = -1.0;
                coeff_b(2*nSplines+i,0) = 0.0;

                coeff_A(3*nSplines-1+i, i*4+0) = 6.0*(i+1);
                coeff_A(3*nSplines-1+i, i*4+1) = 2.0;
                coeff_A(3*nSplines-1+i,(i+1)*4+0) = -6.0*(i+1);
                coeff_A(3*nSplines-1+i,(i+1)*4+1) = -2.0;
                coeff_b(3*nSplines-1+i,0) = 0.0;
            }

             // 2 conditions: f_0''(s_0) = 0, f_n''(s_(n+1)) = 0;
             coeff_A(4*nSplines-2, 0*4+0) = 6.0*0;
             coeff_A(4*nSplines-2, 0*4+1) = 2.0;
             coeff_b(4*nSplines-2, 0) = 0.0;

             coeff_A(4*nSplines-1, (nSplines-1)*4+0) = 6.0*nSplines;
             coeff_A(4*nSplines-1, (nSplines-1)*4+1) = 2.0;
             coeff_b(4*nSplines-1, 0) = 0.0;
         }

         coeff = coeff_A.inverse()*coeff_b;
         for (int i = 0; i<nSplines; i++){
             a0(j,i) = coeff(4*i);
             a1(j,i) = coeff(4*i+1);
             a2(j,i) = coeff(4*i+2);
             a3(j,i) = coeff(4*i+3);
         }
     }
}

다음으로 궤적을 생성하기 위해 필요한 파라미터들을 setTraj 함수에 정의한다. 본 예제에서는 모든 경로점간 이동을 10초로 설정하기 위해 다음과 같이 파라미터들을 설정한다.

1
2
3
4
5
6
7
8
void JointTrajectoryInterpolationComponent::setTraj(double t0)
{
    _t0 = t0;
    _segTime = 10.0;

    _isTargetReached = false;
    _isTrajSet = true;
}

다음으로 traj 함수에 보간을 통해서 생성된 계수들을 기반으로 실시간 경로생성을 수행한다. 경로를 생성할 떄 경로점에서의 스플라인의 값을 임의의 정수(s=0, 1, 2, ...)로 설정하고 계수들을 계산하였기 때문에 10초후에 다음 경로점에 도달하게 하기 위해서 경로를 1/10의 기울기를 가지는 1차 함수로 설정하였다.

 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
void JointTrajectoryInterpolationComponent::traj(double time, VecType &
posDes, VecType & velDes, VecType & accDes)
{
    if (time <= _t0)
    {
        posDes = _readyPos;
        velDes = VecType::Zero();
        accDes = VecType::Zero();
        return;
    }

    _s = (time-_t0)/_segTime;

    double sdot = 1.0/_segTime;
    double sddot = 0.0;

    if (_s > nSplines) _s = nSplines;

    int idx;
    if (_s < nSplines)
    {
        idx = (int)floor(_s);
    }else
    {
        idx= nSplines-1;
    }

    for (int i=0; i< dim; i++)
    {
        posDes[i] = a0(i, idx)*pow(_s, 3) + a1(i, idx)*pow(_s, 2)
                    + a2(i, idx)*_s + a3(i, idx);
        velDes[i] = (3.0*a0(i, idx)*pow(_s, 2) + 2.0*a1(i, idx)*_s
                    + a2(i, idx))*sdot;
        accDes[i] = (3.0*a0(i, idx)*pow(_s, 2) + 2.0*a1(i, idx)*_s
                    + a2(i, idx))*sddot + (6.0*a0(i, idx)*_s
                    + 2.0*a1(i, idx))*sdot*sdot;
    }

    if (_t0 + _nSegment*_segTime <= time)
    {
        _readyPos = posDes;
        _isTrajSet = false;
        _isTargetReached = true;
        _s = 0.0;
    }
}

마지막으로 궤적설정을 위해 필요한 파라미터 및 로봇제어에 필요한 파라미터들을 제어기에서 역으로 이용할 수 있도록 다음의 함수들을 작성하여 준다.

  • setInitialTime 는 제어기에서 측정한 궤적의 시작 시점을 궤적생성에 반영할 수 있도록 한다.
1
2
3
4
void JointTrajectoryInterpolationComponent::setInitialTime(double t0)
{
    _t0 = t0;
}
  • getDuration 는 생성된 궤적을 이동하는데 필요한 총 시간을 제어기가 호출할 수 있도록 한다.
1
2
3
4
double JointTrajectoryInterpolationComponent::getDuration()
{
    return _nSegment*_segTime;
}
  • getNTotalSegment 는 경로점의 개수를 제어기가 호출할 수 있도록 한다.
1
2
3
4
int JointTrajectoryInterpolationComponent::getNTotalSegment()
{
    return _nSegment;
}
  • isTargetReached 는 목표 경로점에 도착했는지의 여부를 제어기가 호출할 수 있도록 한다.
1
2
3
4
bool JointTrajectoryInterpolationComponent::isTargetReached()
{
    return _isTargetReached;
}