[AMR] Intro to planning and control section

electives/amr/autonomous-mobile-robots-cheatsheet.tex

@@ -81,7 +81,9 @@
 
 \section{Planning \& Control}
 \input{parts/05_planning-control/00_feedback-control/00_siso-mimo.tex}
-% \input{parts/05_planning-control/00_feedback-control/}
+\input{parts/05_planning-control/00_feedback-control/01_pid.tex}
+\input{parts/05_planning-control/00_feedback-control/02_lqr.tex}
+\input{parts/05_planning-control/00_feedback-control/03_mpc.tex}
 \input{parts/05_planning-control/01_motion-planning-exploration/00_intro.tex}
 % \input{parts/05_planning-control/01_motion-planning-exploration/}
 

electives/amr/parts/01_kinematics/05_rigid-body-dynamics.tex

@@ -1,11 +1,9 @@
 \subsection{Rigid Body Dynamics}
+\label{sec:rigid-body-dynamics}
 \shortdefinition[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

electives/amr/parts/05_planning-control/00_feedback-control/00_siso-mimo.tex

@@ -1,2 +1,6 @@
 \subsection{SISO \& MIMO}
-\bi{S{\scriptsize ingle-}I{\scriptsize nput-}S{\scriptsize ingle-}O{\scriptsize utput}}: $r$ Reference
+\bi{S{\scriptsize ingle-}I{\scriptsize nput-}S{\scriptsize ingle-}O{\scriptsize utput}}: $r$ Reference (e.g. speed), $u$ Sys. Input (e.g. gas pedal pos),
+$z$ Sys. Out. (e.g. speed of car)
+
+\bi{M{\scriptsize ultiple-}I{\scriptsize nput-}M{\scriptsize ultiple-}O{\scriptsize utput}}: $r$ Reference (typ: trajectory), $u$ Sys. Input (e.g. 4 rotor speeds),
+$x$ internal states (pos, orient, speed, rot. speed), $z$ Sys. Output (e.g. speed of car)

electives/amr/parts/05_planning-control/00_feedback-control/01_pid.tex

@@ -0,0 +1,4 @@
+\subsection{Proportional-Integral-Differential (PID)}
+$u(t) = k_p e(t) + k_i \int_{t_0}^{t} e(\tau) \dx \tau + k_d \frac{\dx e(t)}{\dx t}$, where params $k_p$ (curr), $k_i$ (long-term), $k_d$ (trend) reduce corresp. errors
+
+\bi{Drone Control} ${_B}\vec{F}$ and ${_B}\vec{M}$ as in sect. \ref{sec:rigid-body-dynamics}, use $\vec{u}' = {_B}\vec{M}$.

electives/amr/parts/05_planning-control/00_feedback-control/02_lqr.tex

@@ -0,0 +1,14 @@
+\subsection{Linear Quadratic Regulator (LQR)}
+For lin. cont.-time dyn. $\dot{\vec{x}}(t) = \vec{F}_c \vec{x}(t) + \vec{G}_c \vec{u}(t)$.
+
+We try to minimise cost functional (for $\vec{u}(t) = -\mat{K}\vec{x}(t)$)
+\[
+    J = \int_{t}^{\8} \vec{x}(\tau)^\top \vec{Q}\vec{x}(\tau) + \vec{u}(\tau)^\top \vec{R} \vec{u}(\tau) \dx \tau
+\]
+Solution: $\mat{K} = \mat{R}^{-1} (\mat{G}_c^\top \mat{P})$, with $\mat{P}$ found from
+\[
+    \mat{F}_c^\top \mat{P} + \mat{PF}_c - (\mat{PG}_c) \mat{R}^{-1} + \mat{Q} = \mat{0}
+\]
+Finding $\mat{K}$ is expensive, but \textit{offline}, at runtime only $\vec{u}(t)$.
+
+\bi{Non-Lin}: Approx, $\delta\dot{\vec{x}}(t) = \vec{F}_c \delta \vec{x}(t) + \mat{G}_c \delta \vec{u}(t)$

electives/amr/parts/05_planning-control/00_feedback-control/03_mpc.tex

@@ -0,0 +1,16 @@
+\subsection{MPC}
+\bi{Cost function} ($p(\vec{x}_N)$ \textit{terminal cost}, sum the \textit{stage cost})
+\[
+    J_{0 \rightarrow N}(\vec{x}_0, \vec{u}_0, \ldots, \vec{u_{N - 1}}) = p(\vec{x}_N) + \sum_{k = 0}^{N - 1} q(\vec{x}_k, \vec{u}_k)
+\]
+We minimize the above s.t. for $k \in \{ 0, \ldots, N - 1 \}$ we have $\vec{x}_{k + 1} = \vec{f}(\vec{x}_k, \vec{u}_k)$, $\vec{g}(\vec{x}_k, \vec{u}_k) \leq \vec{0}$
+and $\vec{x}_N \in \cX_f$ and $\vec{x}_0 = \vec{x}(0)$
+
+\bi{Finite-Horizon Lin-Quad Control}
+Quad. Cost:
+\[
+    J_{0 \rightarrow N}(\vec{x}_0, \vec{u}_0, \ldots) =
+    \vec{x}_N^\top \vec{P} \vec{x}_N + \sum_{k = 0}^{N - 1} \vec{x}_k^\top \mat{Q} \vec{x}_k + \vec{u}_k^\top \mat{R} \vec{u}_k
+\]
+without constraints, \bi{State Feedback Law} $\vec{u}^*_0 = -\mat{K}\vec{x}(0)$.
+With constraints, minimize as above.