diff --git a/electives/others/amr/assets/rigid-body-6d.png b/electives/others/amr/assets/rigid-body-6d.png new file mode 100644 index 0000000..ca62308 Binary files /dev/null and b/electives/others/amr/assets/rigid-body-6d.png differ diff --git a/electives/others/amr/assets/wheel-config.png b/electives/others/amr/assets/wheel-config.png new file mode 100644 index 0000000..ea97e67 Binary files /dev/null and b/electives/others/amr/assets/wheel-config.png differ diff --git a/electives/others/amr/assets/wheel-constraints.png b/electives/others/amr/assets/wheel-constraints.png new file mode 100644 index 0000000..57dce4d Binary files /dev/null and b/electives/others/amr/assets/wheel-constraints.png differ diff --git a/electives/others/amr/autonomous-mobile-robots-cheatsheet.pdf b/electives/others/amr/autonomous-mobile-robots-cheatsheet.pdf index 9114004..d8ac866 100644 Binary files a/electives/others/amr/autonomous-mobile-robots-cheatsheet.pdf and b/electives/others/amr/autonomous-mobile-robots-cheatsheet.pdf differ diff --git a/electives/others/amr/parts/01_kinematics/02_inverse.tex b/electives/others/amr/parts/01_kinematics/02_inverse.tex index c75f80a..7b100a3 100644 --- a/electives/others/amr/parts/01_kinematics/02_inverse.tex +++ b/electives/others/amr/parts/01_kinematics/02_inverse.tex @@ -2,13 +2,13 @@ \begin{wrapfigure}[10]{r}{0.4\columnwidth} \includegraphics[width=0.4\columnwidth]{assets/inverse-kinematics.png} \end{wrapfigure} -Option: Solve Forward Kinematics for angles.\\ -Better: Law of cosine with polar coordinates. Compute angle using cosine rule,\\ +\bi{Option}: Solve Forward Kinematics for angles.\\ +\bi{Better}: Law of cosine with polar coordinates. Compute angle using cosine rule,\\ $\theta_1 = \phi \pm \alpha$, $\theta_2 = \pm(\pi - \beta)$ (Positive for {\color{ForestGreen} Elbow Down}, Negative for {\color{red} Elbow Up}) -Extension to 6R: +\bi{Extension to 6R}: 1. Waist: spherical coords (2 sol.)\\ 2. 2 sols from 2R for shoulder + elbow\\ 3. Solve for wrist joints (no influence on pos) diff --git a/electives/others/amr/parts/01_kinematics/03_temporal-models.tex b/electives/others/amr/parts/01_kinematics/03_temporal-models.tex index 40ad617..c4a29ab 100644 --- a/electives/others/amr/parts/01_kinematics/03_temporal-models.tex +++ b/electives/others/amr/parts/01_kinematics/03_temporal-models.tex @@ -1,18 +1,23 @@ \subsection{Temporal Models} -Often use cont-time n.-lin. system of ODE $\dot{\vec{x}} = \vec{f}_C(\vec{x}(t), \vec{u}(t))$, with measurements $\vec{z}(t) = \vec{h}(\vec{x}(t)) + \vec{v}(t)$. +For \bi{Cont-time n.-lin. system of ODE} $\dot{\vec{x}} = \vec{f}_C(\vec{x}(t), \vec{u}(t))$, with measurements $\vec{z}(t) = \vec{h}(\vec{x}(t)) + \vec{v}(t)$.\\ Need linearised (around $\vec{f}_C(\vec{\overline{x}}, \vec{\overline{y}}) = 0$, at \bi{equilibrium}):\\ $\delta \vec{\dot{x}}(t) = \vec{f}_C(\vec{\overline{x}}, \vec{\overline{u}}) + \mat{F}_C \delta \vec{x}(t) + \mat{G}_C \delta \vec{u}(t) + \mat{L}_C \vec{w}(t)$\\ $\delta \vec{z}(t) = \mat{H} \delta \vec{x}(t) + \vec{v}(t)$. Herein, $\mat{H}$ is measurements, $\mat{F}_C$ system, $\mat{G}$ input gain, $\vec{w}$ process noise, $\vec{v}$ measurement noise, both zero-mean \bi{Gaussian White Noise Process}. -For n-lin. cont-time system: +For \bi{n-lin. cont-time system}: $\vec{\dot{x}}(t) = \vec{f}_C(\vec{x}(t), \vec{u}(t), \vec{w}(t))$\\ $\vec{z}(t) = \vec{h}(\vec{x}(t)) = \vec{v})(t)$, linearization is the same -To discretize, integrate from $t_{k - 1}$ to $t_k$: +To \bi{discretize}, integrate from $t_{k - 1}$ to $t_k$:\\ $\vec{x}_k = \vec{f}(\vec{x}_{k - 1}, \vec{u}_k, \vec{w}_k)$ $\vec{z}_k = \vec{h}(\vec{x}_k) + \vec{v}_k$, -linearised:\\ +\bi{linearised}:\\ $\delta \vec{x}_k = \vec{f}(\vec{\overline{x}}, \vec{\overline{u}}) + \mat{F} \delta \vec{x}_{k - 1} + \mat{G}_k \delta \vec{u}_k + \mat{L}_k \vec{w}_k$; $\delta \vec{z}_k = \mat{H}_k \delta \vec{x}_k$ + +\bi{Trapezoidal num. int} +$\Delta \vec{x}_1 = \Delta t \vec{f}_C (\vec{x}_{k - 1}, \vec{u}_{k - 1}, t_{k - 1})$\\ +$\Delta \vec{x}_2 = \Delta t \vec{f}_C (\vec{x}_{k - 1} + \Delta \vec{x}_1, \vec{u}_{k}, t_{k})$, then:\\ +$\vec{x}_k = \vec{x}_{k - 1} + 0.5 \cdot (\Delta \vec{x}_1 + \Delta \vec{x}_2)$ diff --git a/electives/others/amr/parts/01_kinematics/04_rigid-body-imu-kinematics.tex b/electives/others/amr/parts/01_kinematics/04_rigid-body-imu-kinematics.tex index b4fb2ed..e002bc5 100644 --- a/electives/others/amr/parts/01_kinematics/04_rigid-body-imu-kinematics.tex +++ b/electives/others/amr/parts/01_kinematics/04_rigid-body-imu-kinematics.tex @@ -1 +1,50 @@ +\newpage \subsection{Rigid body \& IMU kinematics} +\begin{wrapfigure}[5]{r}{0.3\columnwidth} + \includegraphics[width=0.3\columnwidth]{assets/rigid-body-6d.png} +\end{wrapfigure} +\bi{Velocity} ${_I}\vec{v}_{IB} = \diff{t} ({_I}\vec{t}_B)$ + +\bi{Rot. Velocity} ${_I}\vec{\omega}_{IB} = \diff{t} (\alpha)\; {_I}\vec{t}$ + +\bi{Velocity point $P$} ${_B}\vec{v}_{IP} = {_B}\vec{v}_{IB} + {_B}\vec{\omega}_{IB} \times {_B}\vec{t}_{P}$ + +\bi{Rotation Matrices} +\begin{itemize} + \item For left pertubing\\ + $\mat{\dot{R}}_{IB} = [{_I} \omega_{IB}]^\times \mat{R}_{IB}$ + \item For right pertubing + $\mat{\dot{R}}_{IB} = \mat{R}_{IB} [{_I} \omega_{IB}]^\times$ + \item Constant angular velocity ($\exp{[\Delta \alpha]^\times} = \delta \mat{R}(\Delta \alpha)$)\\ + $\mat{R}_{IB}(t + \Delta t) = \exp{[\Delta \alpha]^\times} \mat{R}_{IB}(t)$ +\end{itemize} + +\bi{Quaternions} +\begin{itemize} + \item For left pertubing + $\displaystyle \vec{\dot{q}}_{IB} = \frac{1}{2} \begin{bmatrix} + {_I}\vec{\omega}_{IB} \\ + 0 + \end{bmatrix} + \otimes \vec{q}_{IB}$ + \item For right pertubing + $\displaystyle \vec{\dot{q}}_{IB} = \frac{1}{2} \vec{q}_{IB} \otimes \begin{bmatrix} + {_B}\vec{\omega}_{IB} \\ + 0 + \end{bmatrix}$ +\end{itemize} + +\bi{IMU} (Outputs {\color{blue} ${_S}\vec{\tilde{a}}$} (accel.), {\color{red} ${_S}\vec{\tilde{\omega}}$} (rot. accel.))\\ +${_W}\vec{\dot{t}}_S = {_W} \vec{v}$, +$\displaystyle \vec{\dot{q}}_{WS} = \frac{1}{2} \vec{q}_{WS} \otimes + \begin{bmatrix} + {\color{red}{_S}\vec{\tilde{\omega}}} {\color{gray} + \vec{w}_g - \vec{b}_g} \\ + 0 + \end{bmatrix}$ + + ${_W}\vec{\dot{v}} = \mat{R}_{WS}\; ({\color{blue}{_S}\vec{\tilde{a}}} {\color{gray} + \vec{w}_a - \vec{b}_a}) + {_W}\vec{g}$ + where {\color{gray} gray parts} only IRL (in theor. models, leave out), with $\vec{\dot{b}}_g = \vec{w}_{b_g}$ and $\vec{\dot{b}}_a = \vec{w}_{b_a}$ + +\bi{IMU Sensor Model}: $\vec{\tilde{z}} = \vec{b}_C + s\mat{M}\vec{z} + \vec{b} + \vec{n} + \vec{o}$ +where bias $\vec{b}$ and scale $s$ often modled as time-varying state $\dot{b}(t) = \sigma_C n(t)$. +$\vec{b}_C$ const. calib. and $\vec{n}$ the model. diff --git a/electives/others/amr/parts/01_kinematics/05_rigid-body-dynamics.tex b/electives/others/amr/parts/01_kinematics/05_rigid-body-dynamics.tex index 176d086..6b83d2a 100644 --- a/electives/others/amr/parts/01_kinematics/05_rigid-body-dynamics.tex +++ b/electives/others/amr/parts/01_kinematics/05_rigid-body-dynamics.tex @@ -1 +1,11 @@ \subsection{Rigid Body Dynamics} +\inlinedefinition[Newton II] For fin. body w/ mass $m$ and inertia mat. $I$, with force $\vec{F}$ and torque $\vec{T}$ on \bi{Centre of Mass} (CoM), expressed in body frame: + +\rmvspace[1.5] +\begin{align*} + {_B}\vec{F} &= \sum {_B}\vec{F}_i = m({_B} \vec{\dot{v}}_{CoM}) + m_B \vec{\omega} \times {_B}\vec{v}_{CoM} \\ + {_B}\vec{T} &= \sum {_B}\vec{T}_i = \mat{I}({_B} \vec{\dot{\omega}}) + {_B} \vec{\omega} \times \mat{I}_B\vec{\omega} +\end{align*} + +\rmvspace +${_B} \vec{v}_{CoM}$ vel. of CoM, ${_B}\omega$ rot. speed; both w.r.t. world frame diff --git a/electives/others/amr/parts/01_kinematics/06_wheeled-robot.tex b/electives/others/amr/parts/01_kinematics/06_wheeled-robot.tex index 87f490f..ac5ee1d 100644 --- a/electives/others/amr/parts/01_kinematics/06_wheeled-robot.tex +++ b/electives/others/amr/parts/01_kinematics/06_wheeled-robot.tex @@ -1 +1,57 @@ \subsection{Wheeled robot Kinematics} +\begin{wrapfigure}[7]{r}{0.2\columnwidth} + \includegraphics[width=0.2\columnwidth]{assets/wheel-constraints.png} +\end{wrapfigure} +\bi{Non-holonomic} systems \textbf{not integrable}, no inst. move in every direct. + +\bi{Wheel constraints} $v_i = \omega_i r_i$ + +\begin{itemize} + \item \textit{Driving straight} all $\vec{v}$ equal + \item \textit{Turning} Wheel axis must intersect the \bi{Instant Centre of Rotation} (ICR), + speeds: $v_i \div R_i = \Omega$ ($R_i$ dist. wheel-ICR, $\Omega$, vehicle body rotation rate) +\end{itemize} + +\bi{Maneuverability} +\begin{itemize} + \item Deg. of Mobility: $\delta_m = 3 - $\#constrained directions + \item Deg. of Steerability: $\delta_s = $\#steerable wheels + \item Deg. of Maneuverability: $\delta_M = \delta_m + \delta_s$ +\end{itemize} + +\bi{Wheel Configurations} + +\includegraphics[width=1\columnwidth]{assets/wheel-config.png} + +\begin{scriptsize} + \begin{tabular}{llllll} + Bicyle & Tricycle & Ackermann & Diff. Drive & Two-Steer & Three-Steer \\ + $\delta_m = 1$ & $\delta_m = 1$ & $\delta_m = 0$ & $\delta_m = 2$ & $\delta_m = 1$ & $\delta_m = 0$ \\ + $\delta_s = 1$ & $\delta_s = 1$ & $\delta_s = 2$ & $\delta_s = 0$ & $\delta_s = 2$ & $\delta_s = 3$ \\ + $\delta_M = 2$ & $\delta_M = 2$ & $\delta_M = 2$ & $\delta_M = 2$ & $\delta_M = 3$ & $\delta_M = 3$ \\ + \end{tabular} +\end{scriptsize} + +\bi{Differential Drive Kinematics} + +\bi{State vec} $\vec{x} = [x_1, x_2, \theta]^\top$, +\bi{Inputs} $\vec{u} = [\omega_l, \omega_r]^\top$, $r_r$ radius of right wheel, $w$ width of robot + +\bi{Gen. eq. of Motion} $\dot{x}_1 = v\cos(\theta)$, $\dot{x}_2 = v\sin(\theta)$, $\dot{\theta} = \Omega$, +with $v = 0.5\cdot(\omega_l r_l + \omega_r + r_r)$, $\Omega = \frac{\omega_r r_r - \omega_l r_l}{w}$ + +\textit{Straight}: $v = \omega_l r_l = \omega_r r_r$, $\Omega = 0$, $D = v\Delta t$.\\ +$\vec{b}_s = \begin{bmatrix} + D \cos(\theta) \\ + D \sin(\theta) \\ + 0 + \end{bmatrix}$ +$\vec{b}_t = \begin{bmatrix} + R(\sin(\Delta \theta + \theta) - \sin(\theta))\\ + -R(\cos(\Delta \theta + \theta) - \cos(\theta))\\ + \Delta \theta +\end{bmatrix}$ + +\textit{Turning}: $\Omega = (\omega_l r_l) / R_l\! =\! (\omega_r r_r) / R_r$, $R\! =\! v / \Omega$, $\Delta \theta\! =\! \Omega \Delta t$ + +\textbf{Discretized}: $\vec{x}_k = \vec{x}_{k - 1} b_i$ with $i \in \{s, t\}$. ($\int \ldots \dx \Delta t$)