frc971/control_loops/swerve/swerve_notes.tex

\documentclass{article}
\usepackage[letterpaper, margin=1in]{geometry}
\usepackage{graphicx}
\usepackage{amsmath}
\usepackage[backend=bibtex]{biblatex}
\usepackage[hidelinks, linktoc=all]{hyperref}
\usepackage{tikz,tikz-3dplot}
\usepackage[hypcap=false, margin = 3cm]{caption}
\usepackage{booktabs}

\DeclareMathOperator{\Ell}{\mathcal{L}}
\newcommand{\harpoon}{\overset{\rightharpoonup}}

\usetikzlibrary{calc}

\bibliography{references}

\title{Swerve Model}
\author{Justin Turcotte, Austin Schuh}
\date{May 2024}

\begin{document}

\maketitle
\tableofcontents
\newpage
\section{Swerve Model}
\subsection{Single Tire}
This section describes the physics behind a single tire model where all variables are in reference to the wheel. The full robot model is in the next section.
\subsubsection{Definitions}
\begin{center}
    \captionof{table}{Symbol definitions for a Single Tire Model}
    \begin{tabular}{l c c}
        \toprule
        Description                                                           & Symbol     & Units                  \\
        \hline
        Effective Radius of Wheel                                             & $r_e$      & m                      \\
        Longitudinal Velocity                                                 & $v_x$      & m/s                    \\
        Lateral Velocity                                                      & $v_y$      & m/s                    \\
        Angular Velocity of Wheel                                             & $\omega$   & rad/s                  \\
        Angle the velocity vector makes with the longitudinal axis of vehicle & $\theta$   & rad                    \\
        Angle the tire makes with the longitudinal axis of vehicle            & $\delta$   & rad                    \\
        Cornering Tire Stiffness                                              & $C_\alpha$ & N/rad                  \\
        Longitudinal Tire Stiffness                                           & $C_\sigma$ & N                      \\
        Longitudinal Slip Ratio                                               & $\sigma$   & Dimensionless          \\
        Slip Angle                                                            & $\alpha$   & rad                    \\
        Mass                                                                  & m          & kg                     \\
        Gear Ratio(Typically less than 1)                                     & G          & Dimensionless          \\
        External Force in Longitudinal Direction                              & $F_{dx}$   & N                      \\
        External Force in Lateral Direction                                   & $F_{dy}$   & N                      \\
        Current through Motor                                                 & $I$        & A                      \\
        Lateral Force From Tire                                               & $F_a$      & N                      \\
        Longitudinal Force From Tire                                          & $F_o$      & N                      \\
        Motor Torque Constant                                                 & $K_t$      & Nm/A                   \\
        Moment of Inertia of motor to wheel                                   & $J$        & kgm\textsuperscript{2} \\
        \bottomrule
    \end{tabular}
\end{center}
\subsubsection{Tire Forces}
Longitudinal Slip Ratio and Slip angle are defined as follows
\begin{gather}
    \sigma  =  \frac{r_e\omega - v_x}{|v_x|} \\ % add breaking/accelerating and figure out whether we need the abs value
    \alpha = \delta-\theta
\end{gather}
Both of these are empirically proven to be approximately linearly proportional to force around zero. \supercite{tire_basics} Thus
\begin{gather}
    F_o = C_\sigma \sigma \label{f_o}\\
    F_a = C_\alpha \alpha \label{f_a}
\end{gather}
We can also say that
\begin{gather}
    \tan(\theta) = \frac{v_y}{v_x}
\end{gather}
and thus assuming no steering angle, $\delta$, we can say
\begin{gather}
    \alpha = -atan2\left(\frac{v_y}{v_x}\right)
\end{gather}
\subsubsection{Motor Forces}
A DC motor can be modeled by the following two equations
\begin{gather}
    V = IR + \frac{\omega}{K_v}\\
    \tau_{motor} = K_tI \label{motor_torque}
\end{gather}
\subsubsection{Equations of Motion}
We can encapsulate our whole system in terms of $v_x$, $v_y$, and $\omega$(of wheel), thus the total forces in each direction is
\begin{gather}
    \Sigma F_x = F_o + F_{dx}\\
    \Sigma F_y = F_a + F_{dy}\\
    \Sigma \tau = \frac{\tau_{motor} - GF_o r_e}{G}
\end{gather}
The Gear ratios constants are required to shift to and from torque at wheel and motor.
Thus, the equations of motion are
\begin{gather}
    \dot{v}_x = \frac{C_\sigma \frac{r_e \omega - v_x}{v_x} + F_{dx}}{m}\\
    \dot{v}_y = \frac{-C_\alpha atan2\left(\frac{v_y}{v_x}\right) + F_{dy}}{m}\\
    \dot{\omega} = \frac{-GF_or_e + K_tI}{GJ} \label{wheel_omega}
\end{gather}

\subsection{Swerve Model}
\subsubsection{Definitions}

\tdplotsetmaincoords{0}{90}
\begin{center}
    \begin{tikzpicture} [scale=2, tdplot_main_coords, axis/.style={->,blue,thick},
            vector/.style={-stealth,black,very thick},
            vector guide/.style={dashed,black,thick}]

        \coordinate (O) at (0,0);

        \draw[axis] (0,0) -- (0,1) node[anchor=north west]{$\mathbf{v_x}$};
        \draw[axis] (0,0) -- (-1,0) node[anchor=south west]{$\mathbf{v_y}$};

        \draw[thick,tdplot_main_coords] (2,2)-- (2,-2) -- (-2,-2)--(-2,2)--cycle;

        \draw[thick,tdplot_main_coords] (2, 2) node[anchor=south east]{module 3};
        \draw[thick,tdplot_main_coords] (2, -2) node[anchor=south west]{module 2};
        \draw[thick,tdplot_main_coords] (-2, -2) node[anchor=north west]{module 1};
        \draw[thick,tdplot_main_coords] (-2, 2) node[anchor=north east]{module 0};

        \newcommand{\circAr}%
        {  \draw[axis] (0,0, 0) +(0:.25)[->] arc(0:360:.25);
        }

        \begin{scope}[tdplot_main_coords]
            \circAr
        \end{scope}
        \draw[blue, thick](0.35,0,0)node[anchor=west]{$\mathbf{\theta}$};

    \end{tikzpicture}
    \captionof{figure}{Shows the module numbering scheme(like Quadrants) and the axes of the robot}

    \vspace{1cm}

    \tdplotsetmaincoords{0}{90}
    \begin{tikzpicture} [scale=3, tdplot_main_coords, axis/.style={->,blue,thick},
            vector/.style={-stealth,black,thick},
            vector guide/.style={dashed,black,thick},
            ref axis/.style={->,dashed, magenta, thick}]

        %standard tikz coordinate definition using x, y, z coords
        \coordinate (O) at (0,0,0);

        \draw[axis] (0,0,0) -- (0,1,0) node[anchor= west]{$\mathbf{v_x}$};
        \draw[axis] (0,0,0) -- (-1,0,0) node[anchor=west, yshift=-0.15cm]{$\mathbf{v_y}$};
        % \draw[axis] (0,0,0) -- (0,0,1);

        \draw[thick, tdplot_main_coords] (0,0,0) -- (-2, 2, 0) node[midway, anchor = north west]{$\mathbf{\harpoon{d}_{mod}}$};
        \coordinate (perp) at ($ (-2,2,0)!0.15!-90:(0,0,0) $);

        \draw [ref axis] (-2,2,0) -- (perp) node[very near end, anchor = north east]{$\mathbf{\dot{\theta} \times \harpoon{d}_{mod}}$};
        % \draw[thick,tdplot_main_coords] (2,2,0)-- (2,-2,0) -- (-2,-2,0)--(-2,2,0)--cycle;

        \draw[vector guide] (-2,0,0) -- (-2,2,0) node[midway, above]{};
        \draw[vector guide] (0,0,0) -- (-2,0,0) node[midway, above, right, pos=0.65]{};
        \draw[vector guide] (-2,2,0) -- (-2+0.375,2,0);

        \coordinate (Shift) at (-2,2,0);
        \tdplotsetrotatedcoordsorigin{(Shift)}

        \draw[ref axis, tdplot_rotated_coords] (0,0,0) -- (0,0.5,0) node[anchor=north west]{$\mathbf{F_x/v_{mod_x}}$};
        \draw[ref axis, tdplot_rotated_coords] (0,0,0) -- (-0.5,0,0) node[anchor=south]{$\mathbf{F_y/v_{mod_y}}$};
        \draw[ref axis, tdplot_rotated_coords] (0,0,0) -- (0,0,0.5);

        \tdplotsetrotatedcoords{-30}{0}{0}

        \draw[axis, tdplot_rotated_coords] (0,0,0) -- (0,0.5,0) node[anchor= west]{$\mathbf{F_{wy}/v_{patch_y}}$};
        \draw[axis, tdplot_rotated_coords] (0,0,0) -- (-0.5,0,0) node[anchor= west]{$\mathbf{F_{wx}/v_{patch_x}}$};
        \tdplotdrawarc[blue, thick, tdplot_rotated_coords]{(0,0,0)}{0.25}{-60+90}{90}{anchor=north west}{$\mathbf{\psi}$}

        % \newcommand{\circAr}%
        % {  \draw[axis] (0,0, 0) +(0:.25)[->] arc(0:360:.25);
        % }

        % \newcommand{\translatepoint}[1]%
        % {   \coordinate (mytranslation) at (#1);
        % }

        % \translatepoint{0,0,0.5}
        % \begin{scope}[tdplot_main_coords,shift=(mytranslation)]
        %     \circAr;
        % \end{scope}
        % \draw(0,0,0.7)node[anchor=west]{$\psi$};

    \end{tikzpicture}
    \captionof{figure}{Shows the swerve axes one of the module axes, and variables defining the shift in coordinates}

    \captionof{table}{Symbol definitions for the Swerve Model}
    \begin{tabular}{l c c}
        \toprule
        Description                                                   & Symbol                 & Units                  \\
        \hline
        velocity of the module                                        & $\harpoon{v}_{mod}$    & m/s                    \\
        angle of the robot with respect to the field                  & $\theta$               & rad                    \\
        location of the module with respect to a center point         & $\harpoon{d}_{mod}$    & m                      \\
        angular velocity of the robot                                 & $\omega$               & rad/s                  \\
        velocity of the robot                                         & $\harpoon{v}$          & m/s                    \\
        velocity of the contact patch of the wheel                    & $\harpoon{v}_{patch}$  & m/s                    \\
        steer angle of the module                                     & $\theta_{mod_s}$       & rad                    \\
        caster of the wheel                                           & $d_{caster}$           & m                      \\
        angular velocity of steer of a module                         & $\omega_{mod_s}$       & rad/s                  \\
        velocity of the module with respect to the ground             & $\harpoon{v}_{ground}$ & m/s                    \\
        effective radius of the wheel                                 & $r_w$                  & m                      \\
        angular velocity of drive of a module                         & $\omega_{mod_d}$       & rad/s                  \\
        slip ratio of a module                                        & $\sigma_{mod}$         & Unitless               \\
        slip angle of a module                                        & $\alpha_{mod}$         & rad                    \\
        self righting moment of a module                              & $M_{mod_s}$            & m/s                    \\
        force exerted by a module on the robot                        & $\harpoon{F}_{mod}$    & N                      \\
        force of the tire on the module in the Longitudinal direction & $F_{wx}$               & N                      \\
        force of the tire on the module in the lateral direction      & $F_{wy}$               & N                      \\
        mass of the robot                                             & $m$                    & kg                     \\
        moment of inertia of the robot                                & $J_{robot}$            & kgm\textsuperscript{2} \\
        external force on the robot                                   & $\harpoon{F}_d$        & N                      \\
        external torque on the robot                                  & $\tau_d$               & Nm                     \\
        \bottomrule
    \end{tabular}
    \vspace{0.1cm}\\
    Note: Dots on top of symbols represent the time derivative of that symbol. The subscript $mod$ signifies the variable is per module
    \captionof{table}{Symbol definitions for a module of the Swerve Model used in Section 1.2.3 and 1.2.4}
    \begin{tabular}{l c c}
        \toprule
        Description                                                               & Symbol        & Units                  \\
        \hline
        sun angle                                                                 & $\beta$       & rad                    \\
        carrier angle                                                             & $\psi$        & rad                    \\
        planetary angle                                                           & $\gamma$      & rad                    \\
        pitch radius of the sun                                                   & $r_s$         & m                      \\
        pitch radius of the planetary                                             & $r_p$         & m                      \\
        torque on the sun                                                         & $\tau_\beta$  & Nm                     \\
        torque on the carrier                                                     & $\tau_\psi$   & Nm                     \\
        torque on the planetary                                                   & $\tau_\gamma$ & Nm                     \\
        force acting between the sun and planetary                                & $F_1$         & N                      \\
        force acting between the bevel gears                                      & $F_2$         & N                      \\
        force acting on the left axle contact point                               & $F_l$         & N                      \\
        force acting on the right axle contact point                              & $F_r$         & N                      \\
        effective radius of the vertical, smaller bevel gear                      & $r_{b1}$      & m                      \\
        effective radius of the horizontal, larger bevel gear                     & $F_{b2}$      & m                      \\
        offset of the pitch circle of the bevel gear from the center of the wheel & $w_b$         & m                      \\
        distance between the center of the wheel and each axle contact points     & $w_w$         & m                      \\
        torque constant of the motor                                              & $K_t$         & Nm/A                   \\
        current applied to the steer motor                                        & $I_s$         & A                      \\
        current applied to the drive motor                                        & $I_d$         & A                      \\
        moment of inertia of the steer motor                                      & $J_{ms}$      & kgm\textsuperscript{2} \\
        gear ratio from the wheel to steer motor                                  & $G_s$         & Unitless               \\
        gear ratio from the wheel to drive motor                                  & $G_d$         & Unitless               \\
        moment of inertia of the drive motor                                      & $J_{md}$      & Nm                     \\
        gear ratio from the motor to sun                                          & $G_{d1}$      & Unitless               \\
        gear ratio of the sun to planetary gear                                   & $G_{d2}$      & Unitless               \\
        gear ratio of the bevel gears                                             & $G_{d3}$      & Unitless               \\
        angle of the module                                                       & $\theta_s$    & rad                    \\
        angle of the wheel                                                        & $\theta_d$    & rad                    \\
        angle of the wheel motor                                                  & $\theta_{md}$ & rad                    \\
        torque caused by the steer motor on the steer coordinate                  & $\tau_s$      & Nm                     \\
        \bottomrule
    \end{tabular}
\end{center}
\subsubsection{Module Velocities and Forces}
Due to the circular motion, we use a cross product added to linear velocities
\begin{gather}
    \harpoon{v}_{mod} = \left(\omega \times R\left(\theta\right)\harpoon{d}_{mod}\right) + \harpoon{v}\\
    \harpoon{v}_{patch} = \harpoon{v}_{mod} +  \left(\omega + \omega_{mod_s}\right) \times R\left(\theta + \theta_{mod_s}\right)
    \begin{bmatrix}
        d_{caster} \\
        0
    \end{bmatrix}
\end{gather}
where all functions R are rotation matrices expressed as the following
\begin{gather}
    R(\psi) =
    \begin{bmatrix}
        \cos(\psi) & -\sin(\psi) \\
        \sin(\psi) & \cos(\psi)  \\
    \end{bmatrix}
\end{gather}
We then express this in the coordinate system of the wheel which is needed for the tire forces
\begin{gather}
    \harpoon{v}_{ground} = R(-(\theta + \theta_{mod_s}))\harpoon{v}_{patch}
\end{gather}
We then can use those velocities to calculate the slip ratios which are slightly modified
\begin{gather}
    \sigma_{mod} = \frac{r_w\omega_{mod_d} - \harpoon{v}_{ground_x}}{max(0.02, |\harpoon{v}_{ground_x}|)}\\
    \alpha_{mod} = -atan2(\frac{\harpoon{v}_{ground_y}}{\harpoon{v}_{ground_x}})
\end{gather}
The lateral and longitudinal forces are given by equations \eqref{f_o} and \eqref{f_a}.
A tire also has a self aligning torque due to having more forces and the far side of the contact path due to a greater deflection. The equation for this assuming a constant weight distribution is found in \textcite{tire_basics} where $a$ is half the length of the contact patch
\begin{gather}
    M_{mod_s} = -\frac{aF_{wy}}{3}
\end{gather}
\subsubsection{Module Behavior}
{\footnotesize Note: For the following section(until 1.2.4) use symbol table 3 in addition to 2}\\
This section aims to find the static relation, since the inertia is negligible, between the sun gear and the wheel and its relation to the rotation of the whole module
\begin{center}
    % TODO(Justin):A nice latex diagram would be nice at some point intead of an image

    % \tdplotsetmaincoords{60}{100}
    % \begin{center}
    %     \begin{tikzpicture} [scale=2, tdplot_main_coords, axis/.style={blue,thick},
    %             vector/.style={-stealth,black,very thick},
    %             vector guide/.style={dashed,black,thick}]

    %         \coordinate (O) at (0,0);

    %         \draw[axis] (0,0,2) arc (0:360:1);
    %         \draw[axis] (0,2,2) arc (0:360:1);

    %     \end{tikzpicture}
    %     \captionof{figure}{Shows the module numbering scheme(like Quadrants) and the axes of the robot}
    % \end{center}
    \begin{figure}[h]
        \includegraphics[width=\textwidth]{module_diagram.jpg}
    \end{figure}
    \begin{tikzpicture} [axis/.style={blue, very thick},
            vector/.style={black,thick},
            vector guide/.style={dashed,black}]

        \draw[vector guide] (0.5,0) arc (0:60:0.5) node[midway, anchor = south west]{$\psi$};
        \draw[vector] (2,0) arc (0:360:2) node[midway, anchor = east]{sun};
        \draw[vector] (60:3) ++(1,0) arc (0:360:1) node[anchor = west]{planetary};
        \draw[vector guide] (0,0) --(2,0);
        \draw[axis] (0,0) --(60:3) node[vector, pos=0.45, sloped, above]{carrier};
        \draw[axis] (0,0) -- (130:2);
        \draw[vector guide] (0.4,0) arc (0:130:0.4) node[very near end, anchor = south]{$\beta$};
        \draw[vector guide] (60:3) -- +(1,0);
        \draw[axis] (60:3) -- +(110:1);
        \draw[vector guide] (60:3) ++(0.5, 0) arc (0:110:0.5) node[midway, above]{$\gamma$};
    \end{tikzpicture}
    \captionof{figure}{Each blue line represents a rotational axis where the carrier rotates the planetary gear itself}
\end{center}
From the figure, we can say the following
\begin{gather}
    F_2 = \frac{r_p}{r_{b1}}F_1\\
    F_2 = \frac{\tau_\gamma}{r_{b1}}\\
    F_w = \frac{r_{b2}}{r_w}F_2\label{fw_to_f2}\\
    \tau_\psi +F_lw_w-F_rw_w + \left(r_s+r_p\right)\left(F_2-F_1\right)= 0\label{the_eq}\\
    F_w - F_l -F_r + F_2 = 0
\end{gather}
which we can express as
\begin{gather}
    \left(1+\frac{r_{b1}}{r_w}\right)F_2 - F_l -F_r = 0
\end{gather}
Thus, we can express $F_l$ and $F_r$ in terms of $F_2$
\begin{gather}
    F_l = \frac{1}{2}\left(1+\frac{r_{b1}}{r_w}-\frac{w_b}{w_w}\right)F_2\\
    F_r = \frac{1}{2}\left(1+\frac{r_{b1}}{r_w}+\frac{w_b}{w_w}\right)F_2
\end{gather}
We then plug these values of $F_l$ and $F_r$ into \eqref{the_eq} to express everything in terms of $F_2$ which we can then express in terms of $F_w$ with \eqref{fw_to_f2}
\begin{gather}
    \tau_\psi = \left(w_b + \left(r_s+r_p\right)\left(1-\frac{r_{b1}}{r_p}\right)\right)F_2\\
    = \left(w_b + \left(r_s+r_p\right)\left(1-\frac{r_{b1}}{r_p}\right)\right)\frac{r_w}{r_{b2}}F_w
\end{gather}
\subsubsection{Full Module Kinematics}
We add up all the torques/forces in each dimension to give the following equations. The variables used here are separate per module
\begin{gather}
    M_s + \tau_s + \tau_\psi = J_{total}\ddot{\theta_s}\\
    \tau_md + \tau_d = -F_{wx}r_w\\
    \dot\theta_d = G_{d3}\left(\left(G_{carrier} - 1\right)\dot\theta_s - \dot\theta_{md}G_{motor-to-planet}\right) \label{the_one}
\end{gather}
The $-1$ in equation \eqref{the_one} comes from the additional contirbution of the wrapping around the gear with the change of coordinates.
We can expand this into
\begin{gather}
    M_s + \frac{K_tI_s}{G_s} + \left(-w_b+\left(r_s+r_p\right)\left(1-\frac{r_{b1}}{r_p}\right)\right)\frac{r_w}{r_{b2}}\left(-F_{wx}\right) = \left(J_s + \frac{J_{ms}}{G^2_s}\right)\ddot\theta_s\\
    \frac{J_{md}\ddot\theta_{md}}{G_d}+\frac{K_{t}I_d}{G_d} = F_{wx}r_w \label{md_ddot}\\
    \dot\theta_d = G_{d3}\left(\left(\frac{r_p+r_s}{r_p}-1\right)\dot\theta_s - \dot\theta_{md}G_{d1}G_{d2}\right)
\end{gather}
where each G represents a separate gear ratio.
\subsubsection{Robot Equation of Motion}
We then rotate back the module forces by
\begin{gather}
    \harpoon{F}_{mod} = R(\theta + \theta_{mod_s})\begin{bmatrix}
        F_{wx} \\
        F_{wy}
    \end{bmatrix}
\end{gather}
The linear velocity differential equations are relatively simple as it's just adding all the forces by the module and an arbitrary external force
\begin{gather}
    \dot{\harpoon{v}} = \frac{\Sigma \harpoon{F}_{mod} + \harpoon{F}_{d}}{m}
\end{gather}
However, the angular velocity differential equation is more complicated as the torque added per module varies in sign
\begin{gather}
    \ddot{\theta} = \frac{\Sigma\left(\harpoon{r} \times \harpoon{F}_{mod}\right) + \tau_{d}}{J_{robot}}
\end{gather}
\subsection{Simplified longitudinal dynamics}

The time constants involved with the longitudinal dynamics are significantly faster than the time constants for accelerating a robot.
This makes the equations pretty stiff and hard to work with.
Ignoring the mass of the wheel, the time constant for a Kraken is around $4ms$.
This also makes it hard on the MPC solver.  Therefore, it is useful to have a simplified version of the physics which solves for the acceleration force directly as a function of drive current.

\eqref{md_ddot} then simplifies to 
\begin{gather}
\frac{K_{t}I_d}{G_d} = -F_{wx}r_w\\
\end{gather}

TODO(austin): Need to document how the position dynamics work for an EKF.

\newpage
\printbibliography
\end{document}