diff --git a/electives/amr/autonomous-mobile-robots-cheatsheet.pdf b/electives/amr/autonomous-mobile-robots-cheatsheet.pdf index 0a634de..2ae545d 100644 Binary files a/electives/amr/autonomous-mobile-robots-cheatsheet.pdf and b/electives/amr/autonomous-mobile-robots-cheatsheet.pdf differ diff --git a/electives/amr/parts/01_kinematics/02_inverse.tex b/electives/amr/parts/01_kinematics/02_inverse.tex index 7b100a3..d054a39 100644 --- a/electives/amr/parts/01_kinematics/02_inverse.tex +++ b/electives/amr/parts/01_kinematics/02_inverse.tex @@ -4,7 +4,7 @@ \end{wrapfigure} \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)$ +$\theta_1 = \varphi \pm \alpha$, $\theta_2 = \pm(\pi - \beta)$ (Positive for {\color{ForestGreen} Elbow Down}, Negative for {\color{red} Elbow Up}) diff --git a/electives/amr/parts/01_kinematics/03_temporal-models.tex b/electives/amr/parts/01_kinematics/03_temporal-models.tex index c4a29ab..2135a87 100644 --- a/electives/amr/parts/01_kinematics/03_temporal-models.tex +++ b/electives/amr/parts/01_kinematics/03_temporal-models.tex @@ -1,14 +1,10 @@ \subsection{Temporal Models} -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}):\\ +Model \bi{robot dyn} as \bi{Cont-time n.-lin. system of ODE} $\dot{\vec{x}} = \vec{f}_C(\vec{x}(t), \vec{u}(t), \vec{w}(t))$, with meas. $\vec{z}(t) = \vec{h}(\vec{x}(t)) + \vec{v}(t)$.\\ +Linearize 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 \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 +$\mat{H}$ is meas., $\mat{F}_C$ system, $\mat{G}$ input gain, $\vec{w}$ process noise, $\vec{v}$ measurement noise, +both zero-mean \bi{Gaussian White Noise Process}. 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)$ @@ -17,7 +13,7 @@ $\vec{z}_k = \vec{h}(\vec{x}_k) + \vec{v}_k$, $\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} +\shortdefinition[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/amr/parts/01_kinematics/04_rigid-body-imu-kinematics.tex b/electives/amr/parts/01_kinematics/04_rigid-body-imu-kinematics.tex index fc52894..2efc7f5 100644 --- a/electives/amr/parts/01_kinematics/04_rigid-body-imu-kinematics.tex +++ b/electives/amr/parts/01_kinematics/04_rigid-body-imu-kinematics.tex @@ -3,9 +3,9 @@ \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{Velocity} ${_I}\vec{v}_{IB} = {_I}\dot{\vec{t}}_B$ -\bi{Rot. Velocity} ${_I}\vec{\omega}_{IB} = \diff{t} (\alpha)\; {_I}\vec{t}$ +\bi{Rot. Velocity} ${_I}\vec{\omega}_{IB} = \dot{\alpha} \cdot {_I}\vec{n}$ \bi{Velocity point $P$} ${_B}\vec{v}_{IP} = {_B}\vec{v}_{IB} + {_B}\vec{\omega}_{IB} \times {_B}\vec{t}_{P}$ @@ -15,35 +15,45 @@ $\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)$ + \item Constant angular velocity ($\exp{[\Delta \vec{\alpha}]^\times} = \delta \mat{R}(\Delta \vec{\alpha})$)\\ + $\mat{R}_{IB}(t + \Delta t) = \exp{[\Delta \vec{\alpha}]^\times} \mat{R}_{IB}(t) \quad \vec{\alpha} = {_I}\vec{\omega}_{IB} \delta 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} + $\vec{\dot{q}}_{IB} = + \frac{1}{2} + {\scriptsize + \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}$ + $\vec{\dot{q}}_{IB} = + \frac{1}{2} \vec{q}_{IB} \otimes + {\scriptsize + \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{t}}_S = {_W} \vec{v}$; +$\quad \displaystyle \vec{\dot{q}}_{WS} = \frac{1}{2} \vec{q}_{WS} \otimes + {\scriptsize + \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}$ +${_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 modelled time-varying $\dot{b}(t) = \sigma_C n(t)$. diff --git a/electives/amr/parts/01_kinematics/05_rigid-body-dynamics.tex b/electives/amr/parts/01_kinematics/05_rigid-body-dynamics.tex index 74d8e83..fc20fe3 100644 --- a/electives/amr/parts/01_kinematics/05_rigid-body-dynamics.tex +++ b/electives/amr/parts/01_kinematics/05_rigid-body-dynamics.tex @@ -6,4 +6,4 @@ {_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*} -${_B} \vec{v}_{CoM}$ vel. of CoM, ${_B}\omega$ rot. speed; both w.r.t. world frame +${_B} \vec{v}_{CoM}$ vel. of CoM, ${_B}\omega$ rot. speed; both w.r.t. inert. frame diff --git a/electives/amr/parts/01_kinematics/06_wheeled-robot.tex b/electives/amr/parts/01_kinematics/06_wheeled-robot.tex index dae17d3..2eccae0 100644 --- a/electives/amr/parts/01_kinematics/06_wheeled-robot.tex +++ b/electives/amr/parts/01_kinematics/06_wheeled-robot.tex @@ -9,7 +9,7 @@ \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) + speeds: $v_i \div R_i = \Omega$ ($R_i$ = dist. wheel-ICR; $\Omega$: vehicle body rotation rate) \end{itemize} \bi{Maneuverability} @@ -19,7 +19,8 @@ \item Deg. of Maneuverability: $\delta_M = \delta_m + \delta_s$ \end{itemize} -\bi{Wheel Configurations} +\newpage +\shade{ForestGreen}{Wheel Configurations} \includegraphics[width=1\columnwidth]{assets/wheel-config.png} @@ -32,27 +33,27 @@ \end{tabular} \end{scriptsize} -\bi{Differential Drive Kinematics} +\shortdefinition[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{Inputs} $\vec{u} = [\omega_l, \omega_r]^\top$, radius of right (left) wheel $r_r$ ($r_l$), $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}$ % TODO: Consider adding wheel constraints (planar) here as well (from W05 slides) \textit{Straight}: $v = \omega_l r_l = \omega_r r_r$, $\Omega = 0$, $D = v\Delta t$.\\ -$\vec{b}_s = \begin{bmatrix} +$\vec{b}_s = {\scriptsize \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}$ + \end{bmatrix}} + \quad \vec{b}_t = {\scriptsize \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$) +\textbf{Discretized}: $\vec{x}_k = \vec{x}_{k - 1} b_i$ with $i \in \{s, t\}$, respectively