diff --git a/electives/amr/autonomous-mobile-robots-cheatsheet.pdf b/electives/amr/autonomous-mobile-robots-cheatsheet.pdf index 44b8893..9ad48b1 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/autonomous-mobile-robots-cheatsheet.tex b/electives/amr/autonomous-mobile-robots-cheatsheet.tex index bb7c32f..0963adc 100644 --- a/electives/amr/autonomous-mobile-robots-cheatsheet.tex +++ b/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/} 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 8678e5d..74d8e83 100644 --- a/electives/amr/parts/01_kinematics/05_rigid-body-dynamics.tex +++ b/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 diff --git a/electives/amr/parts/05_planning-control/00_feedback-control/00_siso-mimo.tex b/electives/amr/parts/05_planning-control/00_feedback-control/00_siso-mimo.tex index 61166a7..a7c4b87 100644 --- a/electives/amr/parts/05_planning-control/00_feedback-control/00_siso-mimo.tex +++ b/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) diff --git a/electives/amr/parts/05_planning-control/00_feedback-control/01_pid.tex b/electives/amr/parts/05_planning-control/00_feedback-control/01_pid.tex new file mode 100644 index 0000000..1a43537 --- /dev/null +++ b/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}$. diff --git a/electives/amr/parts/05_planning-control/00_feedback-control/02_lqr.tex b/electives/amr/parts/05_planning-control/00_feedback-control/02_lqr.tex new file mode 100644 index 0000000..d334864 --- /dev/null +++ b/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)$ diff --git a/electives/amr/parts/05_planning-control/00_feedback-control/03_mpc.tex b/electives/amr/parts/05_planning-control/00_feedback-control/03_mpc.tex new file mode 100644 index 0000000..b5d85c3 --- /dev/null +++ b/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.