ระบบควบคุมโรโบติกส์: แขนกล หุ่นยนต์เคลื่อนที่ และการควบคุมข้อต่อ

A1 Forward kinematics (2-link planar) จลนศาสตร์ตรง

$$x = l_1\cos\theta_1 + l_2\cos(\theta_1 + \theta_2), \qquad y = l_1\sin\theta_1 + l_2\sin(\theta_1 + \theta_2)$$

ใช้ตรีโกณมิติล้วน ๆ · แขนหลายข้อต่อใช้เมทริกซ์แปลงพิกัด (Denavit–Hartenberg) คูณต่อกัน

A2 Inverse kinematics จลนศาสตร์ผกผัน

$$\cos\theta_2 = \frac{x^2 + y^2 - l_1^2 - l_2^2}{2l_1l_2}, \qquad \theta_1 = \operatorname{atan2}(y, x) - \operatorname{atan2}(l_2\sin\theta_2,\ l_1 + l_2\cos\theta_2)$$

$\theta_2 = \pm\cos^{-1}(\cdot)$ ให้สองคำตอบ (ข้อศอกขึ้น/ลง) · ถ้า $|\cos\theta_2| \gt 1$ ปลายแขนเอื้อมไม่ถึง

A3 Jacobian จาโคเบียนและภาวะเอกฐาน

$$\begin{pmatrix} \dot x \\ \dot y \end{pmatrix} = \underbrace{\begin{pmatrix} -l_1s_1 - l_2s_{12} & -l_2s_{12} \\ l_1c_1 + l_2c_{12} & l_2c_{12} \end{pmatrix}}_{J(\boldsymbol\theta)}\begin{pmatrix} \dot\theta_1 \\ \dot\theta_2 \end{pmatrix}, \qquad \det J = l_1l_2\sin\theta_2, \qquad \boldsymbol\tau = J^T\mathbf F$$

$s_{12} = \sin(\theta_1 + \theta_2)$ · $\theta_2 = 0, \pi$ (แขนเหยียดตรงหรือพับสุด) เป็นภาวะเอกฐาน · $J^T$ แปลงแรงที่ปลายแขนเป็นแรงบิดข้อต่อ (ใช้ใน force control) · ดูอินเวอร์สและดีเทอร์มิแนนต์

A4 Manipulator dynamics สมการพลศาสตร์แขนกล

$$M(\mathbf q)\ddot{\mathbf q} + C(\mathbf q, \dot{\mathbf q})\dot{\mathbf q} + \mathbf g(\mathbf q) + \mathbf f(\dot{\mathbf q}) = \boldsymbol\tau$$

$M$ เมทริกซ์ความเฉื่อย (สมมาตร บวกแน่นอน เปลี่ยนตามท่า) · $C$ แรงโคริโอลิสและหนีศูนย์ · $\mathbf g$ แรงโน้มถ่วง · $\mathbf f$ แรงเสียดทาน · ได้จากสมการออยเลอร์–ลากรานจ์ $\frac{d}{dt}\frac{\partial\mathcal L}{\partial\dot q} - \frac{\partial\mathcal L}{\partial q} = \tau$

A5 PD + gravity compensation PD + ชดเชยแรงโน้มถ่วง

$$\boldsymbol\tau = K_P(\mathbf q_d - \mathbf q) - K_D\dot{\mathbf q} + \mathbf g(\mathbf q)$$

ลู่เข้า $\mathbf q_d$ คงที่เสมอสำหรับ $K_P, K_D \succ 0$ ใดก็ได้ (พิสูจน์ด้วยลียาปูนอฟ $V = \tfrac12\dot{\mathbf q}^TM\dot{\mathbf q} + \tfrac12\mathbf e^TK_P\mathbf e$) · ไม่ต้องรู้ $M$ และ $C$ เลย · ไม่มีเทอม I จึงไม่ต้องกลัว windup

A6 Computed torque (inverse dynamics) คอมพิวเต็ดทอร์ก

$$\boldsymbol\tau = M(\mathbf q)\big[\ddot{\mathbf q}_d + K_D\dot{\mathbf e} + K_P\mathbf e\big] + C\dot{\mathbf q} + \mathbf g \ \Rightarrow\ \ddot{\mathbf e} + K_D\dot{\mathbf e} + K_P\mathbf e = \mathbf 0$$

ความผิดพลาดทุกข้อต่อกลายเป็นระบบอันดับสองเชิงเส้นอิสระ เลือก $K_P = \omega_n^2,\ K_D = 2\zeta\omega_n$ · ต้องรู้แบบจำลองแม่น · คือ feedback linearization

A7 Cubic and quintic trajectories วิถีพหุนาม (จากหยุดนิ่งถึงหยุดนิ่ง)

$$q(t) = q_0 + \Delta\left[3\left(\tfrac tT\right)^2 - 2\left(\tfrac tT\right)^3\right], \qquad \dot q_{max} = \frac{3\Delta}{2T}, \qquad \ddot q_{max} = \frac{6\Delta}{T^2}$$

ความเร็วต่อเนื่อง ความเร่งกระโดดที่ต้นและปลาย · พหุนามกำลังห้า $\Delta[10\tau^3 - 15\tau^4 + 6\tau^5]$ ให้ความเร่งต่อเนื่องด้วย (นุ่มกว่า กระตุ้นเรโซแนนซ์น้อยกว่า) · ส่ง $q, \dot q, \ddot q$ ไปเป็น feedforward (K4)

A8 Differential-drive kinematics จลนศาสตร์หุ่นยนต์สองล้อขับ

$$v = \frac r2(\omega_R + \omega_L), \quad \omega = \frac rb(\omega_R - \omega_L), \quad \dot x = v\cos\phi,\ \dot y = v\sin\phi,\ \dot\phi = \omega$$

$r$ รัศมีล้อ $b$ ระยะห่างล้อ · ผกผัน: $\omega_{R,L} = \dfrac{v \pm \omega b/2}{r}$ แล้วใช้ PI คุมความเร็วล้อแต่ละข้าง

A9 Go-to-goal / heading control นำทางไปเป้าหมาย

$$\phi_d = \operatorname{atan2}(y_g - y,\ x_g - x), \qquad \omega = K_\phi\,\operatorname{wrap}(\phi_d - \phi), \qquad v = \min\big(v_{max},\ K_\rho\rho\big)$$

$\rho$ = ระยะถึงเป้า · wrap มุมให้อยู่ใน $(-\pi, \pi]$ · หุ่นเดินตามเส้นใช้ PD กับความคลาดจากเส้นแทน $\phi_d - \phi$