drone-tracking-robot-preview.mp4
컴퓨터 비전 AI 모델을 활용한 드론 추적 로봇 개발 및 PID(Proportional Integral Derivative), SMC(Sliding Mode Control), MPC(Model Predictive Control) 전략 성능 비교
-
뎁스카메라로 픽셀좌표와 드론까지의 거리 정보를 얻습니다.
-
카메라 캘리브레이션을 통해 카메라 내부 파라미터를 구합니다.
-
기구학적인 정보를 통해 원점과 카메라의 위치 및 회전 관계 (카메라 외부 파라미터) 를 구합니다.
-
아래와 같은 수식을 통해 드론의 3차원 좌표를 구합니다.
-
inverse kinematics 를 통해 드론을 가리킬 수 있도록 각 관절이 이동해야할 각도를 구합니다.
-
5번에서 구한 각도로 이동하기 위한 Trajectory 를 구성합니다. (Trapezoidal Velocity Profili)
-
매니퓰레이터의 현재 각도, 각속도와 6번의 Trajectory 를 입력으로 폐루프 제어를 실행합니다.
-
뎁스카메라로 새로운 드론이 탐지되면 1번부터 다시 반복합니다.
Link Length
|
$$d_1$$ |
$$a_2$$ |
$$a_3$$ |
$$a_3'$$ |
| $$Length (m)$$ |
$$0.07$$ |
$$0.03$$ |
$$0.055$$ |
$$0.085$$ |
Link Mass
|
$$m_1$$ |
$$m_2$$ |
| $$Mass (kg)$$ |
$$0.075$$ |
$$0.316$$ |
아래부터 순서대로 { ${base}$ }, { ${1}$ }, { ${2}$ }, { ${tool}$ } 의 local coordinate 이며 빨간축이 $x$축, 초록축이 $y$축, 파란축이 $z$축 입니다.
| $$i$$ |
$$\alpha_{i-1}$$ |
$$a_{i-1}$$ |
$$d_i$$ |
$$\theta_i$$ |
| $$1$$ |
$$0$$ |
$$0$$ |
$$d_1 + a_2$$ |
$$\theta_1$$ |
| $$2$$ |
$$\pi/2$$ |
$$0$$ |
$$0$$ |
$$\theta_2 + \pi/2$$ |
| $$Tool (Laser)$$ |
$$\pi/2$$ |
$$a_3$$ |
$$0$$ |
$$\pi/2$$ |
DH Parameter 는 위의 표와 같습니다.
{ ${base}$ } 계에서 { ${tool}$ } 계를 표현하고자 할 때
DH Parameter 표를 이용해서 $i = 1$ 부터 $tool$ 까지 곱으로 표현할 수 있으며 아래 식과 같습니다.
$${}_{base}^{tool} T = \begin{bmatrix}
\sin \theta_1 & \sin \theta_2 \cos \theta_1 & \cos \theta_1 \cos \theta_2 & -a_3 \sin \theta_2 \cos \theta_1 \\
-\cos \theta_1 & \sin \theta_1 \sin \theta_2 & \sin \theta_1 \cos \theta_2 & -a_3 \sin \theta_1 \sin \theta_2 \\
0 & -\cos \theta_2 & \sin \theta_2 & a_2 + a_3 \cos \theta_2 + d_1 \\
0 & 0 & 0 & 1
\end{bmatrix}$$
드론의 위치를 $x, y, z$ 라고 할 때, $\theta_1$ 과 $\theta_2$ 는 아래와 같이 기하학적인 방식으로 유도할 수 있습니다.
$$\theta_1 = \arctan\left(\frac{y}{x}\right)$$
$$\theta_2 = \arccos\left(L_2 A + \sqrt{(L_2 A)^2 + (A^2 + B)(B - L_2^2)}\right)$$
$$where, \quad A = z - L_1, \quad B = x^2 + y^2, \quad L_1 = d_1 + a_2, \quad L_2 = a_3$$
두 지점을 이동할 때 안정적으로 제어하기 위해서 출발지점과 도착지점은 속도가 0 이 되도록 설계하는 것이 좋습니다.
Trapezoidal Velocity Profile 방식으로 구현하였습니다.
빨간점을 각각의 질점으로 가정하여 위치에너지, 운동에너지를 구할 수 있습니다.
$$ E_p = m_1 g l_1 + m_2 g ( l_1 + l_2 \cos \theta_2 + l_3 \sin \theta_2 ) $$
$$ E_k = \frac{1}{2} m_2 \dot{\theta}_1^2 \left( l_3^2 \cos^2 \theta_2 + l_2^2 \sin^2 \theta_2 - 2 l_2 l_3 \cos \theta_2 \sin \theta_2 \right) + \frac{1}{2} m_2 \dot{\theta}_2^2 \left( l_2^2 + l_3^2 \right) $$
운동에너지와 위치에너지의 차로 라그랑지안을 정의하고 라그랑주 방정식으로 운동방정식을 유도할 수 있습니다.
$$ \frac{d}{dt} \frac{\partial L}{\partial \dot{q}_i} - \frac{\partial L}{\partial q_i} = \tau \quad (i = 1, 2)$$
$$ M(q) \ddot{q} + V(q, \dot{q}) + G(q) = \tau $$
$$ \therefore
\begin{bmatrix}
m_2 A^2 & 0 \\
0 & m_2 (l_2^2 + l_3^2)
\end{bmatrix}
\begin{bmatrix}
\ddot{\theta}_1 \\
\ddot{\theta}_2
\end{bmatrix}
+
\begin{bmatrix}
2m_2 AB \dot{\theta}_1 \dot{\theta}_2 \\
-m_2 AB \dot{\theta}_1^2
\end{bmatrix}
+
\begin{bmatrix}
0 \\
-m_2 A g
\end{bmatrix}
= \begin{bmatrix}
\tau_1 \\
\tau_2
\end{bmatrix} $$
$$ where, \quad A = l_2 \sin \theta_2 - l_3 \cos \theta_2 \quad \text{and} \quad B = l_2 \cos \theta_2 + l_3 \sin \theta_2 $$
로봇의 PID제 어 시스템은 입력된 현재 각도와 각속도에 대한 정 보를 피드백 받고, 목표하는 각도와 각속도의 값을 비교하여 오차 값을 계산합니다.
오차 값은 적절한 이득 값들과 곱해져서 τ'(토크 수정 값)을 생성합니다. 이후, 현재 각도 정보는 질량 매트릭스를 통해 처 리되고, 이 결과는 τ' 와 곱해져 최종토크 τ 를 형성합니다.
차이를 에러 각(e)으로 두고, 폐루프 시스템의 특성을 이용하면 오차방정식을 구할 수 있습니다.
$$ MV(t) = K_p e(t) + K_i \int_{0}^{t} e(t) , dt + K_d \frac{de}{dt} $$
상태공간좌표계에 상태값들이 0 으로 수렴하도록 설계된 슬라이딩표면을 따라가도록 불연속적인 제어입력을 가합니다.
사용된 상태 공간좌표계는 모터의 참조값, 현재값의 차이 각도오차, 각속도 오차를 사용하고 있으며 제어기의 게인을 통해서 오차수렴속도, 채터링, 입력크기 등을 조절하는데 사용할 수 있습니다.
동역학 모델링으로부터 상태공간 방정식을 도출한 식은 아래와 같습니다.
$$ x_1 = \theta_1, \quad x_2 = \dot{\theta}_1, \quad x_3 = \theta_2, \quad x_4 = \dot{\theta}_2 $$
$$ \dot{x}_2 = -\frac{2B}{A} x_2 x_4 + \frac{1}{m_2 A^2} \tau_1 $$
$$ \dot{x}_4 = \frac{AB}{C} x_2^2 + \frac{Ag}{C} + \frac{1}{m_2 C} \tau_2 $$
$$ where, \quad A = l_2 \sin \theta_2 - l_3 \cos \theta_2 \quad B = l_2 \cos \theta_2 + l_3 \sin \theta_2 \quad C = l_2^2 + l_3^2 $$
슬라이딩표면 설계는 아래와 같습니다.
$$ s_1 = c_1 e_1 + e_2, \quad s_2 = c_2 e_3 + e_4 $$
$$ where, \quad c_1 > 0, \quad c_2 > 0 $$
$$ e_1 = \theta_{d1} - \theta_1 $$
$$ e_2 = \dot{\theta}_{d1} - \dot{\theta}_1$$
$$ e_3 = \theta_{d2} - \theta_2$$
$$ e_4 = \dot{\theta}_{d2} - \dot{\theta}_2 $$
RMSE로 계산하였을 때 SMC가 XY Plane 0.809, YZ Plane 0.493으로 가장 적은 RMSE를 가져 다른 제어기보다더 좋은 추적성능을 보여주는 것을 확인할 수 있었습니다. 그러나 전력소모량이 가장 높은 것을 알 수 있습니다.
구간별 추적 성능으로 보았을 SMC는 경로에서 벗어나지 않으며 정확하게 추적하는 성능을 보이고 PID와 MPC 제어기에서는 경로에서 벗어나는 것을 보입니다.
성능적인 측면에서 SMC가 가장 우수하지만, 경제적인 측면을 고려하였을 때 전력소모량이 적고 비교적 추적오차가 적은 PID가 우수하다고 볼 수 있습니다.