[ ARTICLE ]

The Journal of Korea Robotics Society - Vol. 17, No. 3, pp.303-307

ISSN: 1975-6291 (Print) 2287-3961 (Online)

Print publication date 31 Aug 2022

Received 05 Mar 2022 Revised 06 Apr 2022 Accepted 06 Apr 2022

DOI: https://doi.org/10.7746/jkros.2022.17.3.303

이차 계획법을 활용한 부드러운 궤적 생성 방법

성민창¹

; 최영진^†

Smooth Trajectory Generation Method Using Quadratic Programming Method

Minchang Sung¹

; Youngjin Choi^†

1Ph.D. Candidate, Department of Electrical and Electronic Engineering, Hanyang University, Ansan, 15588, Korea wdrac331@hanyang.ac.kr

Correspondence to: ^†Professor, Corresponding author: Department of Electrical and Electronic Engineering, Hanyang University, Ansan, 15588, Korea ( cyj@hanyang.ac.kr)

Abstract

This paper proposes a method that can generate a smooth trajectory from the discontinuous trajectory in kinematic, dynamic, and task-space trajectory constraints. The problem is defined as the minimization of kinetic energy, and then the simulation is performed by using the MATLAB. Kinematic and inverse kinematic equations are derived for the simulation of the 6-DOF robotic arm. The simulation results showed that the trajectory of each joint is generated while satisfying the constraints without any discontinuity. There are small errors in the Cartesian trajectory, but unnecessary deceleration and acceleration can be eliminated. In addition, it is possible to quickly switch between the robotic tasks by applying the proposed method.

Keywords:

Quadratic Programming, Trajectory Generation

1. 서 론

로봇은 4차 산업의 발전과 함께 자동화 공정뿐만 아니라 스마트 팩토리(smart factory), 스마트 팜(smart farm) 등 사회 곳곳에서 활용되고 있다. 산업용 로봇의 경우 제조 및 생산 분야에서 고정밀, 고반복성의 장점을 이용해 많은 산업체에서 활용되고 있다^[1]. 제조 및 생산 과정에서 빠른 로봇 속도가 요구되며 이는 다양한 로봇제어 방법 및 궤적(trajectory)생성방법을 이용하여 달성하고 있다. 일반적인 부드러운 궤적 생성방법으로는 LSPB (Linear Segment Parabolic Blends)^[2,3]와 유사한 사다리꼴 형태의 가속도 생성방법과 가속도의 미분인 저크 부분을 삼각함수 형태로 생성하는 방법^[4,5]들이 있다. QP (Quadratic Programming)를 이용한 궤적생성 방법으로는 GOMP^[6]와 같이 시작점의 작업 공간(task-space) 자세, 끝점의 자세 두 가지의 제한조건만을 이용한 부드러운 경로생성방법이 존재한다. 다만 이 경우 처음과 끝 이외에는 작업 공간 경로추종에서 오류를 발생시키게 된다. 본 논문에서는 모든 작업 공간 경로에서 제한조건을 만족하며 운동에너지를 최소화하는 관절공간 경로생성을 다룬다.

일반적으로 로봇의 궤적생성과 경로생성은 각각 구분되지만^[7], 본 논문에서는 경로(path)와 궤적(trajectory)을 나누지 않고 시간에 대한 함수로 표현되는 위치, 속도, 가속도를 궤적이라고 표현할 것이다. 궤적은 관절공간 궤적, 작업 공간 궤적, 원형 궤적 등으로 구분할 수 있으며, 작업시간의 단축을 위해 경로와 경로 간의 결합을 진행하여 속도 영역까지 부드러운 궤적생성을 진행하는 방법이 있다^[8]. 이 경우 궤적과 궤적이 결합하는 순간 가속도 불연속이 발생하며 로봇의 진동을 야기한다. 본 논문에서는 이러한 궤적 간 결합에서 발생하는 불연속적인 궤적을 [Fig. 1]과 같이 부드러운 궤적으로 재생성하는 문제를 이차 계획법을 통해 해결한다.

[Fig. 1]

Concept of the optimal smooth trajectory generation computed by QP method, where the resultant trajectory can remove the discontinuity between Task-space PtoP(Point to Point) trajectory and Cartesian C (Circular) trajectory.

2. 이차 계획법을 활용한 궤적생성

2.1 이차 계획법(Quadratic Programming, QP) 문제

먼저 n 개의 축을 갖는 로봇의 각 관절 위치 값을 q ∈ Rⁿ, 속도 값을 $q ˙ ∈ R n$ , 가속도 값을 $q ¨ ∈ R n$ 이라고 정의하고, 로봇 궤적이 N개의 구간으로 나누어진다고 가정해보자. 궤적의 초기 위치, 속도, 가속도를 $q 0, q ˙ 0, q ¨ 0$ 라 하고 궤적의 종단 위치, 속도, 가속도를 $q N - 1, q ˙ N - 1, q ¨ N - 1$ 라고 나타내면, 상태변수 x ∈ R^3nN를 다음과 같이 정의할 수 있다.

x = [q 0 T, ⋯, q N - 1 T, q ˙ 0 T, ⋯, q ˙ N - 1 T, q ¨ 0 T, ⋯, q ¨ N - 1 T] T

(1)

속도의 경우 운동에너지와 밀접한 연관이 있으며 운동에너지를 최소화하는 움직임은 많은 로봇제어 방법에 사용된다. 따라서 속도를 정해진 제한조건 아래에서 최소화하도록 하는 이차 계획법 문제로 정의하면 다음과 같이 부등식 조건을 갖는 목적함수를 정의할 수 있다.

min x 12 x T P x s . t . A x ≤ b

(2)

여기서 P ∈ R^3nN×3nN는 속도 궤적의 제곱합(sum of squaring)을 의미하도록 다음과 같은 행렬로 설정한다.

P = 0 n N × n N 0 n N × n N 0 n N × n N 0 n N × n N I n N × n N 0 n N × n N 0 n N × n N 0 n N × n N 0 n N × n N

식 (2)에서 A와 b는 제한된 조건 하에서 최적상태를 구하기 위한 제한 값이며 2.3절에서 자세히 설명한다.

2.2 로봇 기구학(Robot Kinematics)

로봇의 순기구학(forward kinematics)은 관절 값 q에서 동차행렬변환(homogeneous transform) X ∈ SE(3)로 매핑되는 함수 f라 표현할 수 있으며, 이것의 역변환을 역기구학(inverse kinematics)이라 하고 다음과 같이 표현할 수 있다.

X = f(q) forward kinematics
q = f^-1 (X) inverse kinematics

양변을 시간에 대하여 미분하여 관절 속도값 $q ˙$ 에서 twist V ∈ R⁶ 대한 표현으로 바꾼 것을 속도 기구학(velocity kinematics)라 하고 다음과 같이 표현할 수 있다.

$V = J q ˙$ forward velocity kinematics
$q ˙ = J + V$ inverse velocity kinematics

여기서 J ∈ R^6×n는 자코비안(Jacobian matrix)이라 하며 관절 속도값 $q ˙$ 가 변화함에 대한 twist V의 변화의 선형 민감도(linear sensitivity)와 관련있는 매핑행렬이다^[3].

2.3 제한 조건

본 논문에서는 2.1절에서 정의한 이차 계획법 문제를 해결할 때, 다음과 같이 기구학적, 동역학적, 작업 공간 궤적 제한조건의 세 가지 조건으로 최적 궤적을 찾았다.

2.3.1 기구학적 제한 조건

기구학적 제한조건은 각 관절의 위치, 속도, 가속도의 최솟값을 $q min, q ˙ min, q ¨ min$ 라 하고 최댓값 $q max, q ˙ max, q ¨ max$ 라 하면 다음과 같이 표현할 수 있다.

q min ≤ q ≤ q max q ˙ min ≤ q ˙ ≤ q ˙ max q ¨ min ≤ q ¨ ≤ q ¨ max

(3)

위의 식을 Ax ≤ b 형태로 바꾸면 다음과 같은 형태로 표현할 수 있다.

A q min x = I n N × n N 0 n N × n N 0 n N × n N x ≤ b q min A q ˙ min x = 0 n N × n N I n N × n N 0 n N × n N x ≤ b q ˙ min A q ¨ min x = 0 n N × n N 0 n N × n N I n N × n N x ≤ b q ¨ min A q max x = I n N × n N 0 n N × n N 0 n N × n N x ≤ b q max A q ˙ max x = 0 n N × n N I n N × n N 0 n N × n N x ≤ b q ˙ max A q ¨ max x = 0 n N × n N 0 n N × n N I n N × n N x ≤ b q ¨ max

(4)

2.3.2 동역학적 제한 조건

동역학적 제한조건은 i - 1 번째 궤적의 위치, 속도, 가속도 $q i - 1, q ˙ i - 1, q ¨ i - 1$ 라 하고, i번째 궤적의 위치, 속도. 가속도 $q i, q ˙ i, q ¨ i$ 라 할 때, 시간 간격 t_step에 대해서 두 궤적간 동역학적 관계를 다음과 같이 표현할 수 있다.

q i = q i - 1 + t s t e p q ˙ i - 1 + 12 t s t e p 2 q ¨ i - 1

(5)

q ˙ i = q ˙ i - 1 + t s t e p q ¨ i - 1

(6)

식 (5)을 Ax ≤ b 형태로 변형하면 i = 0, ⋯, N - 1에서

A q i x = 0 n × i, - I n × n, I n × n, 0 n × n × N - i - 1, 0 n × i, - t s t e p I n × n, 0 n × n × N - i, 0 n × i, - 12 t s t e p 2 I n × n, 0 n × n × N - i x ≤ b q i

(7)

식 (7)과 같이 변형할 수 있다. 동일하게 식 (6)을 변형하면

A q i ˙ x = 0 n × N, 0 n × i, - I n × n, I n × n, 0 n × n × N - i - 1, 0 n × i, - t s t e p I n × n, 0 n × n × N - i x ≤ b q ˙ i

(8)

식 (8)과 같이 변형할 수 있다.

2.3.3 작업 공간 궤적 제한 조건

작업 공간 궤적 제한조건의 경우 i번째 상태에서의 X_i ∈ SE(3)가 i번째 상태에서의 목표 X_{d_i} ∈ SE(3)와 같거나 허용할 수 있는 작은 차이만 존재해야 한다. V_{d_i} ∈ R⁶를 목표 값과 현재 값의 차이라고 정의하고 다음과 같이 표현할 수 있다.

V d i = log X i - 1 X d i V d i + J i q i - ϵ ≤ J i q i ≤ V d i + J i q i + ϵ

(9)

여기서 S ∈ R⁶가 로봇 기준점에서의 screw axis라고 했을 때 log : X ∈ SE(3)→[S]q ∈ se(3)이며, [v]는 임의 벡터 v에 대한 skew-symmetric matrix변환을 의미한다^[2].

이제 Ax ≤ b 형태로 변형하면

A X i x = 0 n × i, J i, 0 n × n × N - i x ≤ b X i = J i q i + V d i + ϵ

(10)

이다.

2.4 최적궤적시간(Optimal trajectory time)

이차 계획법을 통한 최적 값을 구할 때 상태 x는 각 관절의 위치, 속도, 가속도 값을 의미하며 t_step에 의존적이다. t_step의 값을 0에서부터 반복적으로 늘려 목적함수의 해가 최초로 존재하는 경우를 찾아 최적 궤적 시간을 계산할 수 있다. 다음 [Algorithm 1]은 최적 궤적 값과 t_step을 구하는 과정을 나타낸다.

[Algorithm 1]

An algorithm of smooth trajectory generation

3. 실험결과

실험은 뉴로메카社의 6자유도 협동로봇인 Indy7 모델에 대한 시뮬레이션을 통해 진행하였다. 홈페이지에 게시된 관절 최소 및 최대 위치, 속도를 이용했으며^[9] 관절 가속도의 경우 최대 및 최소 값이 제시되어있지 않아서, 20[rad/s²]라고 가정하고 시뮬레이션을 진행하였다.

[Table 1]은 Indy7 모델의 기구학적 제한조건을 표기하고 있으며, MATLAB R2021b (Mathworks, Natick, MA, USA), OSQP^[10]를 이용하여 시뮬레이션을 수행하였다.

[Table 1]

Kinematic constraints for simulation

3.1 초기궤적생성

초기 궤적의 경우 역기구학을 이용하여 5차 다항식 형태의 작업 공간 PtoP (Point to Point) 궤적과, 작업공간 C (Circular) 궤적을 생성하였다. 생성된 관절 위치 궤적의 미분을 통해 속도 값과 가속도 값을 초기궤적으로 선택하고 최적화를 진행하였다. PtoP 궤적과 C 궤적의 결합을 통한 궤적의 변화를 확인하기 위해, 먼저 PtoP 궤적을 생성하고 그 중간 지점에 C궤적을 삽입하여 불연속점을 생성하였다.

3.2 시뮬레이션 결과

시뮬레이션은 N = 201, t_ϵ = 0.0001[s]로 진행했으며 초기 t_step = 0.0001[s]으로 시작했다. 총 반복횟수는 24회 진행하였으며, 계산된 최적 시간 간격은 t_step = 0.0024[s], 최종 궤적 시간은 t_end = 0.0024(N - 1) = 0.48[s]이다.

궤적의 경우 N의 값이 커지면 특정 시점에서의 궤적과 다음 시점에서의 궤적 간의 동역학적 제한조건이 보다 낮아지게 된다. 따라서 해를 구할 가능성이 높아지게 되나 행렬의 크기가 증가하여 해를 구하는 시간이 증가한다. 반대로 N이 작아지게 되면 동역학적 제한조건이 높아져 해를 구할 가능성이 낮아지게 되며, 행렬의 크기가 줄어들어 해를 구하는 시간이 감소하게 된다.

3.2.1 관절 공간 궤적

[Fig. 2]는 시뮬레이션을 통해 생성된 관절의 위치, 속도, 가속도 값들을 비교한 그림이며 초기 값의 경우 궤적과 궤적이 연결되는 시간에 위치, 속도, 가속도 그래프 모두에 불연속성이 나타나게 된다. 하지만 본문에서 제안한 방식대로 얻은 최적궤적의 경우 위치, 속도, 가속도 제한조건을 만족하며 부드러운 궤적을 생성할 수 있다. 또한 기구학과 직접적으로 연관된 위치 값은 불연속성이 발생한 중간지점 외에는 큰 차이가 발생하지 않는다.

[Fig. 2]

Pressure (P) response to voltage input (V)

3.2.2 작업 공간 궤적

[Fig. 3]은 시뮬레이션을 통해 생성된 관절 위치를 순기구학을 통해 작업 공간 위치로 변환한 후 기존에 생성된 PtoP 궤적과 C 궤적과 비교하는 그림이며 불연속이 발생하는 중간지점 근방을 확대하였다. 작업 공간 회전은 오일러각 roll, pitch, yaw값을 이용하고, 작업 공간 위치는 x, y, z값을 이용하여 비교하였다. Roll값의 경우 결과 값이 목표 값에서 발생한 불연속을 보상하기 위해 오차가 발생했음을 확인할 수 있으며, x, y, z 값에서도 동일하게 불연속 지점에 오차가 발생함을 확인할 수 있었다.

[Fig. 3]

Comparison between the target and Cartesian space QP results, where red dots and blue lines denote initial values and optimal values, respectively, and right axis expresss the error (black dots) yielded by using optimal values

4. 결 론

본 논문에서는 6자유도 로봇의 위치, 속도, 가속도의 제한조건 아래에서 이차 계획법을 활용한 부드러운 궤적생성 방법을 설명하고 뉴로메카의 Indy7 모델을 이용한 시뮬레이션을 수행하였다. 이차 계획법을 통해 운동에너지를 최소화하며, 동시에 기구적인 제한 조건을 만족하고, 동역학적 불연속이 없는 궤적을 생성할 수 있었다. 본 논문의 방법을 통해 목표로 하는 기존 궤적보다는 오차가 존재하지만 관절 공간 궤적의 오버 슛을 줄여 로봇제어 기능을 향상하고, 예상치 못한 가속도 및 저크로 인한 작업의 불확실성을 줄일 수 있을 것으로 기대되며 산업용 로봇의 경우 작업과 작업 간 감속 및 가속을 줄여 보다 빠른 작업이 가능할 것으로 기대된다.

Acknowledgments

This work was supported in part by the National Research Foundation of Korea (NRF) grant funded by the Korea government (MSIT) (2019R1A2C1088375), and in part by the Technology Innovation Program funded by the Korean Ministry of Trade, industry and Energy, (20008908), Republic of Korea

References

C.-H. Lee, “Industrial Robot,” Institute for Information Technology Advancement, Jincheon, Korea, [Online], https://scienceon.kisti.re.kr/srch/selectPORSrchReport.do?cn=TRKO200500019526, .
D. Constantinescu and E. A. Croft, “Smooth and time‐optimal trajectory planning for industrial manipulators along specified paths,” Journal of Robotic Systems, vol. 17, no. 5, pp 233-249, 2000. [https://doi.org/10.1002/(SICI)1097-4563(200005)17:5<233::AID-ROB1>3.0.CO;2-Y]
J. E. Bobrow, S. Dubowsky, and J. S. Gibson, “Time-optimal control of robotic manipulators along specified paths,” The International Journal of Robotics Research, vol. 4, no. 3, pp. 3-17, 1985. [https://doi.org/10.1177/027836498500400301]
A. Y. Lee, G. Jang, and Y, Choi, “Infinitely differentiable and continuous trajectory planning for mobile robot control,” 2013 10th International Conference on Ubiquitous Robots and Ambient Intelligence (URAI), Jeju, Korea, 2013. [https://doi.org/10.1109/urai.2013.6677386.]
A. Y. Lee and Y. Choi, “Smooth trajectory planning methods using physical limits,” Institution of Mechanical Engineers, Part C: Journal of Mechanical Engineering Science, vol. 229, no. 12, pp. 2127-2143, 2015. [https://doi.org/10.1177/0954406214553982]
J. Ichnowski, M. Danielczuk, J. Xu, and V. Satish, “GOMP: Grasp-optimized motion planning for bin picking,” 2020 IEEE International Conference on Robotics and Automation (ICRA), Paris, France, 2020. [https://doi.org/10.1109/ICRA40945.2020.9197548]
K. M. Lynch and F. C. Park, “Trajectory Generation,” Modern Robotics, 1st ed., Cambridge University Press, 2017, ch. 9, sec. 1, pp. 325-326. [https://doi.org/10.1017/9781316661239]
J. Kim, S.‐R. Kim, S.‐J. Kim, and D.‐H. Kim, “A practical approach for minimum‐time trajectory planning for industrial robots,” Industrial Robot, vol. 37, no. 1, pp. 51-61, 2010. [https://doi.org/10.1108/01439911011009957]
Neuromeka-Indy7, [Online], https://s3.ap-northeast-2.amazonaws.com/landing.neuromeka.com/upload/Neuromeka_catalogue_generic_ko.pdf, , Accessed: March 1, 2022.
B. Stellato, G. Banjac, P. Goulart, A. Bemporad, Bartolomeo, and S. Boyd, “OSQP: An operator splitting solver for quadratic programs,” 2018 UKACC 12th International Conference on Control (CONTROL), Sheffield, UK, 2018. [https://doi.org/10.1109/CONTROL.2018.8516834]

성 민 창

2019 한양대학교 ERICA 전자공학부(공학사)

현재 한양대학교 전자공학과(석박사통합과정 수료)

관심분야: 로봇 제어, 컴퓨터 비젼

최 영 진

2002 포항공과대학교 기계공학과(공학박사)

2005 한국과학기술연구원 지능로봇연구센터 선임연구원

2005~현재 한양대학교 ERICA 전자공학부 교수

관심분야: 로봇 제어, 생체신호처리

[Fig. 1]

[Fig. 2]

Pressure (P) response to voltage input (V)

[Fig. 3]

[Algorithm 1]

An algorithm of smooth trajectory generation

Require: X_{d_i} pose at ith step(i = 0,...,N ),
　 t_step the time step between ith and i+1th,
　

q min, q ˙ min, q ¨ min

q max, q ˙ max

and

q ¨ max

the kinematic
　constraints,
　t_ϵ small constant value.
1: while
2:

q 0 ← f - 1 X d 0, q ˙ 0 ← 0, q ¨ 0 ← 0

3: for i←0, 1, ..., N - 1 do
4:

q i ← f - 1 X d i

q ˙ i ← q i - q i - 1 / t s t e p

q ¨ i ← q ˙ i - q ˙ i - 1 / t s t e p

7: end for
8:

x 0 = q 0 T, ⋯, q N - 1 T, q ˙ 0 T, ⋯, q ˙ N - 1 T, q ¨ 0 T, ⋯, q ¨ N - 1 T

x ← arg min x 12 x T P x, s . t . A x ≤ b

  10:     if QP is solved then
  11:          return x^* ← x
  12:     else
  13:          t_step←t_step + t_ϵ
  14:     end if
  15:  end while

[Table 1]

Kinematic constraints for simulation

	Position[rad]		Velocity[rad/s]		Acceleration[rad/s²]
	Min	Max	Min	Max	Min	Max
Joint 1	-3.0543	3.0543	-2.6180	2.6180	-20	20
Joint 2	-3.0543	3.0543	-2.6180	2.6180	-20	20
Joint 3	-3.0543	3.0543	-2.6180	2.6180	-20	20
Joint 4	-3.0543	3.0543	-2.6180	2.6180	-20	20
Joint 5	-3.0543	3.0543	-2.6180	2.6180	-20	20
Joint 6	-3.1415	3.1415	-2.6180	2.6180	-20	20