younghwanjoo1608

ㅎㅇ

3 minute read


5.1.1 Path and Trajectory


Path란, 지점 $A$에서 $B$까지의 길을 말한다. 즉 $A$에서 $B$를 잇는 점들의 set. 이때 우리는 path를 따르는 과정에서의 속도는 고려하지 않는다. 즉 time과 무관.


Trajectory는 path에 $A$에서 $B$까지의 시간(& 속도)를 함께 고려한 것.


5.1.2 Polynomial Trajectory


Smooth Function은 trajectory 조각의 시작과 끝에서 boundary condition을 만족하고, 연속이어야 한다(위치, 속도, 가속도 등).


예를 들어 $5^{th}$ order(quintic) polynomial은 다음 6개 boundary condition을 만족한다.

\[s_0, \dot{s_0}, \ddot{s_0} \qquad t=0 \\ s_T, \dot{s_T}, \ddot{s_T} \qquad t=T \\\] \[s(t) = At^5 + Bt^4 + Ct^3 + Dt^2 + Et + F \qquad t \in [0,T] \\ \dot{s}(t) = 5At^4 + 4Bt^3 + 3Ct^2 + 2Dt + E \\ \ddot{s}(t) = 20At^3 + 12Bt^2 + 6Ct + 2D\]

$0$과 $T$에서 boundary condition을 고려하자. 그러면

\[s(0) = F \\ \dot{s}(0) = E \\ \ddot{s}(0) = 2D\] \[s(T) = AT^5 + BT^4 + CT^3 + DT^2 + ET + F \\ \dot{s}(T) = 5AT^4 + 4BT^3 + 3CT^2 + 2DT + E \\ \ddot{s}(T) = 20AT^3 + 12BT^2 + 6CT + 2D\]

계수를 구하기 위해 방정식을 풀어야 한다.

\[\begin{bmatrix} A\\B\\C\\D\\E\\F \end{bmatrix} = \begin{bmatrix} 0&0&0&0&0&1\\ T^5&T^4&T^3&T^2&T&1\\ 0&0&0&0&1&0\\ 5T^4 &4T^3 &3T^2 &2T & 1&0\\ 0&0&0&2&0&0\\ 20T^3 & 12T^2 & 6T & 2&0&0 \end{bmatrix}^{-1} \begin{bmatrix} s_0 \\ s_T \\ \dot{s_0} \\ \dot{s_T} \\ \ddot{s_0} \\ \ddot{s_T} \end{bmatrix}\]


Robotics Toolbox - Polynomial Trajectory


tpoly 함수는 quintic polynomial trajectory를 만든다.


5.1.3 Trapezoidal Profile


가능한 빨리 최고 속도에 도달하고 싶다. 그 속도에서 가능한 오래 유지되었으면 한다. 그 다음에 느려졌으면 한다. 이 조건을 만족하려면 velocity profile이 사다리꼴 형태를 하고 있어야 한다.

Accelerate + Constant velocity + Decelerate. Robotics Toolbox에서 lspb를 사용하면 다음과 같은 profile을 그릴 수 있다.

이제 trajectory profile은 직선과 포물선이 합쳐진 형태가 된다.

\[\textrm{For straight line,} \\ \begin{align*} \dot{s} &= b \\ s &= bt + c \end{align*}\] \[\textrm{For parabola,} \\ \begin{align*} \dot{s} &= at+b \\ s &= at^2 + bt + c \end{align*}\]

그러나 이 경우 acceleration이 continuous하지 않다. 따라서 smooth하지 않다.


위 관찰을 토대로 trapezoidal profile을 구성해보자. Smooth curve를 구성하기 위해 linear function이 parabolic과 섞여 있는 형태를 생각하자.

일단 직선 양쪽의 두 parabolic이 같은 duration 동안 섞여 있다고 가정하자. 이로써 양쪽에 같은 constant acceleration(물론 부호는 다르지만)이 적용된다.

이러한 profile의 해는 여러가지 있을 수 있으나, 적어도 중간 속도 $t_h$와 중간 position $\theta_h$에 대칭이라고 예상할 수 있다. Parabolic의 끝부분 속도는 linear region의 속도와 같아야 하므로,

\[\ddot{\theta}t_b = \frac{\theta_h - \theta_b}{t_h - t_b}\]

여기서 $\theta_b$는 blend region의 끝부분 $\theta$값이고 $\ddot{\theta}$는 blend region 동안의 가속도. $\theta_b$의 값은 다음과 같이 주어진다.

\[\theta_b = \theta_0 + \frac{1}{2}\ddot{\theta}t_b^2\]

$t=2t_h$인 점을 생각하고 위 식들을 정리하면,

\[\ddot{\theta}t_b^2 - \ddot{\theta}tt_b + (\theta_f-\theta_0) = 0\]

자, 이렇게 $\theta_f,\theta_0,t$가 주어졌을 때 위 식을 만족하는 $\ddot{\theta}, t_b$를 결정해서 path를 그릴 수 있다.

보통은 가속도 $\ddot{\theta}$가 결정된 상태에서 위 식을 $t_b$에 관하여 푼다.


가속도가 충분히 크게 결정되지 안흥면 해가 존재하지 않을 수 있다. $t_b$에 관한 이차식의 해는

\[t_b = \frac{t}{2}-\frac{\sqrt{\ddot{\theta}^2t^2-4\ddot{\theta}(\theta_f-\theta_0)}}{2\ddot{\theta}}\]

그러므로 해가 존재하기 위한 가속도 constraint는

\[\ddot{\theta} \geq \frac{4(\theta_f-\theta_0)}{t^2}\]

등호가 성립하면 linear region은 영으로 수축하고 path는 두 parabolic이 연결된 형태가 된다. 극단적으로 무한한 가속도를 가질 때, path는 단순히 linear interpolation 형태.


5.1.4 Multiple Segments


임의의 개수의 via point에 대해 이를 확장하자. 그러나 직선과 포물선의 blend로는 실제로 via point를 지나게 할 수는 없다.

$t_{ACC}$가 작으면 via point에 더 가까이 가지만, 가속도는 커진다.

$t_{ACC}$가 커지면, 가속도는 작아지지만 via point에서는 더 멀어진다.


정리해 보면,

[Given]

  • Positions : $\theta_i, \theta_j, \theta_k, \theta_l, \theta_m$
  • Desired time durations : $t_{dij}, t_{djk}, t_{dkl}, t_{dlm}$
  • Magnitude of accelerations : $\vert \ddot{\theta}_i \vert, \vert \ddot{\theta}_j \vert, \vert \ddot{\theta}_k \vert, \vert \ddot{\theta}_l \vert$

[Compute]

  • Blends time : $t_i, t_j, t_k, t_l, t_m$
  • Straight segment time : $t_{ij}, t_{jk}, t_{kl}, t_{lm}$
  • Slope(velocities) : $\dot{\theta}{ij}, \dot{\theta}{jk}, \dot{\theta}{kl}, \dot{\theta}{lm}$
  • Signed accelerations


Interior Path Joints


안쪽부터 시작하자. 그림을 보면 간단하다.

\[\dot{\theta}_{jk} = \frac{\theta_k - \theta_j}{t_{djk}} \\ \ddot{\theta}_k = \textrm{sgn}(\dot{\theta}_{kl} - \dot{\theta}_{jk})\vert \ddot{\theta}_k \vert \\ t_k = \frac{\dot{\theta}_{kl} - \dot{\theta}_{jk}}{\ddot{\theta}_k} \\ t_{jk} = t_{djk} - \frac{1}{2}t_j - \frac{1}{2}t_k\]


First Segment


초기 지점의 blend time $t_1$을 구하기 위해 linear phase 동안의 속도에 대한 관계식을 이용한다.

\[\frac{\theta_2 - \theta_1}{t_{d12}-\frac{1}{2}t_1} = \ddot{\theta}_1t_1\]

이 식을 $t_1$에 대해 풀면 된다. 나머지는 유사하다.

\[\ddot{\theta}_1 = \textrm{sgn}(\dot{\theta}_{2} - \dot{\theta}_{1})\vert \ddot{\theta}_1 \vert \\ t_1 = t_{d12} - \sqrt{t_{d12}^2-\frac{2(\theta_2 - \theta_1)}{\ddot{\theta_1}}} \\ \dot{\theta}_{12} = \frac{\theta_2 - \theta_1}{t_{d12}-\frac{1}{2}t_1} \\ t_{12} = t_{d12} - t_1 - \frac{1}{2}t_2\]


Last Segment


위와 비슷하다.

\[\frac{\theta_{n-1} - \theta_n}{t_{d(n-1)n}-\frac{1}{2}t_n} = \ddot{\theta}_nt_n\]

로부터,

\[\ddot{\theta}_n = \textrm{sgn}(\dot{\theta}_{n-1} - \dot{\theta}_{n})\vert \ddot{\theta}_n \vert \\ t_n = t_{d(n-1)n} - \sqrt{t_{d(n-1)n}^2-\frac{2(\theta_n - \theta_{n-1})}{\ddot{\theta_n}}} \\ \dot{\theta}_{(n-1)n} = \frac{\theta_n - \theta_{n-1}}{t_{d(n-1)n}-\frac{1}{2}t_n} \\ t_{(n-1)n} = t_{d(n-1)n} - t_n - \frac{1}{2}t_{n-1}\]

You may also enjoy