Introduction

In FIRST Tech Challenge (FTC), kinematics are used all of the time—they’re essential to robot operation. However, few teams understand where the relationships come from or even acknowledge them explicitly, especially for holonomic drives. This paper intends to demystify the role and derivation of drive kinematics with a unified approach.

Drive Kinematics

This section systematically derives the forward and inverse kinematics for a variety of drives found in FTC (i.e., differential, mecanum, and swerve). The forward kinematics describe how the robot moves with specific wheel velocities. The inverse kinematics describe the opposite—how the wheels move during a given robot motion.

Robots as Rigid Bodies

Most FTC robots have a robust structure that holds all of the components together into one coherent system. When acted upon by a force, the robot does not materially deform, and its constituent parts remain essentially fixed relative to each other. Objects with this property are called rigid bodies, and their kinematics can be summarized with a single translational and angular velocity vector pair. The translational velocity describes the motion of the axis or tation, while the angular velocity magnitude points along the axis of rotation and has magnitude equal to the angular speed.

Consider a point in the body with position \(\vec{r}\). This position can be written as the sum \(\vec{R} + \vec{r}^{\,\prime}\) where \(\vec{R}\) points from the origin to the axis of rotation and \(\vec{r}^{\,\prime}\) extends perpendicular to \(\vec{\omega}\). Differentiation¹ gives the velocity relationship \[\begin{aligned} \frac{d\vec{r}}{dt} &= \frac{d\vec{R}}{dt} + \frac{d\vec{r}^{\,\prime}}{dt} \\ &= \vec{V} + \vec{\omega} \times\vec{r}^{\,\prime}. \end{aligned}\] As FTC robots are generally confined to a plane, we have \[\begin{aligned} \vec{r}^{\,\prime} &= x \, \hat{\imath} + y \, \hat{\jmath}\\ \vec{V} &= v_x \, \hat{\imath} + v_y \, \hat{\jmath}\\ \vec{\omega} &= \omega \, \hat{k}. \end{aligned}\] In this special case, the velocity is \[\vec{v} = \frac{d\vec{r}}{dt} = (v_x - y \, \omega) \, \hat{\imath} + (v_y + x \, \omega) \, \hat{\jmath}.\]
Now for a given configuration \((v_x, v_y, \omega)\) it is possible to compute the two-dimensional velocity of any point on the robot.

Differential Drives

Perhaps the simplest practical mobile robot drive configuration is a differential drive with two normal wheels. These wheels have radius \(R\) and are spaced \(2l\) units apart. Let the center of rotation (i.e., the midpoint of the wheel positions) be the origin with the wheel axles laying parallel to the y-axis. With this setup, the wheel center positions are \((\pm l, 0)\).

Using the results of the previous subsection, the desired velocities are \[\begin{aligned} \vec{v}_l &= (v_x - l \, \omega) \, \hat{\imath} + v_y \, \hat{\jmath}\\ \vec{v}_r &= (v_x + l \, \omega) \, \hat{\imath} + v_y \, \hat{\jmath}. \end{aligned}\] Furthermore, the orientation of the wheels gives us the tangential velocities in terms of the wheel angular velocities \[\begin{aligned} \vec{v}^{\,\prime}_l &= \omega_l R \, \hat{\imath}\\ \vec{v}^{\,\prime}_r &= \omega_r R \, \hat{\imath}. \end{aligned}\] For traction wheels, the corresponding velocities must exactly match. This yields the equations \[\begin{aligned} v_x - l \, \omega &= \omega_l R \\ v_x + l \, \omega &= \omega_r R \\ v_y &= 0 \\ v_y &= 0. \end{aligned}\] From this we can solve to obtain the forward and inverse kinematics \[\begin{aligned}[c] v_x &= \frac{R}{2}(\omega_l + \omega_r) \\ v_y &= 0 \\ \omega &= \frac{R}{2l}(\omega_r - \omega_l), \end{aligned} \qquad\qquad \begin{aligned}[c] \omega_l &= \frac{v_x - l \, \omega}{R} \\ \omega_r &= \frac{v_x + l \, \omega}{R}. \end{aligned}\] In matrix form, these relations are \[\begin{bmatrix} v_x \\ v_y \\ \omega \end{bmatrix} = \frac{R}{2} \begin{bmatrix} 1 & 1 \\ 0 & 0 \\ -\frac{1}{l} & \frac{1}{l} \end{bmatrix} \begin{bmatrix} \omega_l \\ \omega_r \end{bmatrix}, \qquad\qquad \begin{bmatrix} \omega_l \\ \omega_r \end{bmatrix} = \frac{1}{R} \begin{bmatrix} 1 & 0 & -l\\ 1 & 0 & l \end{bmatrix} \begin{bmatrix} v_x \\ v_y \\ \omega \end{bmatrix}.\] These same kinematics can be applied to differential drives with multiple wheels per side. However, you’ll quickly realize that it is no longer possible to satisfy all the constraints. To rotate properly, the wheels must overcome static friction and slide parallel to the wheel axis. This slippage can be mathematically accounted for by treating this scrub direction as a passive degree of freedom (like an omni wheel) as we’ll see in the next section. With this assumption, the kinematics are the same as above with all motors on each side sharing the same velocity.

Mecanum Drives

Mecanum drives consist of four mecanum wheels with rollers at \(45\degree\) angles with respect to the axle positioned in a rectangle. Looking from above, the main diagonal wheels have counterclockwise-rotated rollers, while the alternate diagonal wheels have clockwise-rotated rollers. The careful combination of these two different directions enables omnidirectional movement. The track width (distance between opposing wheels) is \(2l\) and the wheelbase (distance between adjacent wheels) is \(2b\). Place the center of rotation at the origin as before with wheel center positions \((\pm l, \pm b)\).

Using the same procedure as before, we obtain \[\begin{aligned} \vec{v}_{fl} &= (v_x - l \, \omega) \, \hat{\imath} + (v_y + b \, \omega) \, \hat{\jmath}\\ \vec{v}_{bl} &= (v_x - l \, \omega) \, \hat{\imath} + (v_y - b \, \omega) \, \hat{\jmath}\\ \vec{v}_{br} &= (v_x + l \, \omega) \, \hat{\imath} + (v_y - b \, \omega) \, \hat{\jmath}\\ \vec{v}_{fr} &= (v_x + l \, \omega) \, \hat{\imath} + (v_y + b \, \omega) \, \hat{\jmath}. \end{aligned}\] Computing the tangential velocities from the angular velocities is bit trickier for mecanum wheels. The tangential force acts \(45\degree\) from the roller, resulting in a speed reduction of \(\sqrt{2}\). The direction of the velocity points perpendicular to the roller axis (viewed from above). Thus, the velocities are \[\begin{aligned} \vec{v}^{\,\prime}_{fl} &= \frac{R \, \omega_{fl}}{2} (\hat{\imath} - \hat{\jmath})\\ \vec{v}^{\,\prime}_{bl} &= \frac{R \, \omega_{bl}}{2} (\hat{\imath} + \hat{\jmath})\\ \vec{v}^{\,\prime}_{br} &= \frac{R \, \omega_{br}}{2} (\hat{\imath} - \hat{\jmath})\\ \vec{v}^{\,\prime}_{fr} &= \frac{R \, \omega_{fr}}{2} (\hat{\imath} + \hat{\jmath}). \end{aligned}\] The vectors can’t be directly equated for non-square configurations due to the passive motion of the rollers. Instead we equate the portion of each vector in the tangential velocity direction. For example, the front left constraint is \[\vec{v}_{fl} \cdot \vec{v}^{\,\prime}_{fl} = \vec{v}^{\,\prime}_{fl} \cdot \vec{v}^{\,\prime}_{fl}.\] The final system is \[\begin{aligned} \frac{R \, \omega_{fl}}{2}(v_x - l \, \omega - v_y - b \, \omega) &= \frac{R^2\omega_{fl}^2}{2}\\ \frac{R \, \omega_{bl}}{2}(v_x - l \, \omega + v_y - b \, \omega) &= \frac{R^2\omega_{bl}^2}{2}\\ \frac{R \, \omega_{br}}{2}(v_x + l \, \omega - v_y + b \, \omega) &= \frac{R^2\omega_{br}^2}{2}\\ \frac{R \, \omega_{fr}}{2}(v_x + l \, \omega + v_y + b \, \omega) &= \frac{R^2\omega_{fr}^2}{2}. \end{aligned}\] which yields the matrix kinematics \[\begin{bmatrix} v_x \\ v_y \\ \omega \end{bmatrix} = \frac{R}{4} \begin{bmatrix} 1 & 1 & 1 & 1 \\ -1 & 1 & -1 & 1 \\ -\frac{1}{l+b} & -\frac{1}{l+b} & \frac{1}{l+b} & \frac{1}{l+b} \end{bmatrix} \begin{bmatrix} \omega_{lf} \\ \omega_{lb} \\ \omega_{rb} \\ \omega_{rf} \end{bmatrix} \qquad\qquad \begin{bmatrix} \omega_{lf} \\ \omega_{lb} \\ \omega_{rb} \\ \omega_{rf} \end{bmatrix} = \frac{1}{R} \begin{bmatrix} 1 & -1 & -(l + b) \\ 1 & 1 & -(l + b) \\ 1 & -1 & (l + b) \\ 1 & 1 & (l + b) \end{bmatrix} \begin{bmatrix} v_x \\ v_y \\ \omega \end{bmatrix}\]

Swerve Drives

Unlike the previous drives, swerve drives have actuated wheel directions. Nevertheless, swerve kinematics can still be derived within the same framework. Let each wheel have coordinates \((x_i, y_i)\), orientation \(\phi_i\), tangential velocity \(\vec{v}_i\), and angular velocity \(\omega_i\).

For simplicity, we will assume the swerve wheels do not slip and directly equate the tangential velocities: \(v_x - y_i \, \omega = R \, \omega_i \operatorname{cos} \phi_i\) and \(v_y + x_i \, \omega = R \, \omega_i \operatorname{sin} \phi_i\). This gives the following inverse kinematics²: \[\begin{aligned} \phi_i &= \operatorname{atan2} \big( v_y + x_i \, \omega, \; v_x - y_i \, \omega \big)\\ \omega_i &= \frac{1}{R} \sqrt{(v_x - y_i \, \omega)^2 + (v_y + x_i \, \omega)^2} \end{aligned}\]

Forward kinematics for a collection of multiple wheels can be computed by solving the corresponding overdetermined system.

Odometry

The previous section focused on finding the relationship between configuration and wheel velocities. This by itself is sufficient to employ the inverse kinematics for sending the appropriate control signals. However, the forward kinematics cannot be directly used for odometry. To localize the robot, the local velocities must be integrated into positions and transformed into the global frame.

Constant Velocity Odometry

For simplicity, this method assumes constant translational and rotational velocity over each measurement period. In practice, this is a good assumption so long as measurements are frequent enough (additionally, estimating acceleration or other higher order derivatives robustly from wheel position data is nontrivial).

Without loss of generality, we take the robot heading \(\theta\) to be \(0\) initially. A measurement is then taken \(\Delta t\) time later. During this period the robot’s global velocity is \[\begin{bmatrix} \dot{x}_G \\ \dot{y}_G \\ \dot{\theta}_G \end{bmatrix} = \begin{bmatrix} \operatorname{cos} \theta & -\operatorname{sin} \theta & 0 \\ \operatorname{sin} \theta & \operatorname{cos} \theta & 0 \\ 0 & 0 & 1 \end{bmatrix} \begin{bmatrix} \dot{x}_R \\ \dot{y}_R \\ \dot{\theta}_R \end{bmatrix}.\] Integrating this over time gives \[\begin{bmatrix} \Delta x_G \\ \Delta y_G \\ \Delta \theta_G \end{bmatrix} = \begin{bmatrix} \frac{\operatorname{sin} \Delta \theta_R}{\Delta \theta_R} & -\frac{1 - \operatorname{cos} \Delta \theta_R}{\Delta \theta_R} & 0 \\[4pt] \frac{1 - \operatorname{cos} \Delta \theta_R}{\Delta \theta_R} & \frac{\operatorname{sin} \Delta \theta_R}{\Delta \theta_R} & 0 \\[4pt] 0 & 0 & 1 \end{bmatrix} \begin{bmatrix} \Delta x_R \\ \Delta y_R \\ \Delta \theta_R \end{bmatrix}.\] This can then be rotated and added to the previous estimate to complete the update \[\begin{bmatrix} x_{G,\,t+1} \\ y_{G,\,t+1} \\ \theta_{G,\,t+1} \end{bmatrix} = \begin{bmatrix} x_{G,\,t} \\ y_{G,\,t} \\ \theta_{G,\,t} \end{bmatrix} + \begin{bmatrix} \operatorname{cos} \theta_{G,\,t} & -\operatorname{sin} \theta_{G,\,t} & 0 \\ \operatorname{sin} \theta_{G,\,t} & \operatorname{cos} \theta_{G,\,t} & 0 \\ 0 & 0 & 1 \end{bmatrix} \begin{bmatrix} \frac{\operatorname{sin} \Delta \theta_R}{\Delta \theta_R} & -\frac{1 - \operatorname{cos} \Delta \theta_R}{\Delta \theta_R} & 0 \\[4pt] \frac{1 - \operatorname{cos} \Delta \theta_R}{\Delta \theta_R} & \frac{\operatorname{sin} \Delta \theta_R}{\Delta \theta_R} & 0 \\[4pt] 0 & 0 & 1 \end{bmatrix} \begin{bmatrix} \Delta x_R \\ \Delta y_R \\ \Delta \theta_R \end{bmatrix}.\]

Tracking Wheels

Tracking wheels are passive omni wheels intended solely for odometry. Each wheel has an arbitrary position \((x_i, y_i)\) and orientation \(\phi_i\) in the robot frame. Similar to swerve, the tangential velocities of each wheel are \(\vec{v}_i = (v_x - y_i \, \omega) \, \hat{\imath} + (v_y + x_i \, \omega) \, \hat{\jmath}\) and \(\vec{v}^{\,\prime}_i = R \, \omega_i (\operatorname{cos} \phi_i \, \hat{\imath} + \operatorname{sin} \phi_i \, \hat{\jmath})\). \[\omega_i = \frac{1}{R}\Big[v_x \operatorname{cos} \phi_i + v_y \operatorname{sin} \phi_i + \omega \, (x_i \operatorname{sin} \phi_i - y_i \operatorname{cos} \phi_i)\Big]\] If placed properly, three tracking wheels are sufficient to determine the \((v_x, v_y, \omega)\) configuration: \[\begin{bmatrix} \omega_1\\ \omega_2\\ \omega_3 \end{bmatrix} = \frac{1}{R} \begin{bmatrix} \operatorname{cos} \phi_1 & \operatorname{sin} \phi_1 & x_1 \operatorname{sin} \phi_1 - y_1 \operatorname{cos} \phi_1 \\ \operatorname{cos} \phi_2 & \operatorname{sin} \phi_2 & x_2 \operatorname{sin} \phi_2 - y_2 \operatorname{cos} \phi_2 \\ \operatorname{cos} \phi_3 & \operatorname{sin} \phi_3 & x_3 \operatorname{sin} \phi_3 - y_3 \operatorname{cos} \phi_3 \\ \end{bmatrix} \begin{bmatrix} v_x\\ v_y\\ \omega \end{bmatrix}\] \[\begin{bmatrix} \Delta \theta_1\\ \Delta \theta_2\\ \Delta \theta_3 \end{bmatrix} = \frac{1}{R} \begin{bmatrix} \operatorname{cos} \phi_1 & \operatorname{sin} \phi_1 & x_1 \operatorname{sin} \phi_1 - y_1 \operatorname{cos} \phi_1 \\ \operatorname{cos} \phi_2 & \operatorname{sin} \phi_2 & x_2 \operatorname{sin} \phi_2 - y_2 \operatorname{cos} \phi_2 \\ \operatorname{cos} \phi_3 & \operatorname{sin} \phi_3 & x_3 \operatorname{sin} \phi_3 - y_3 \operatorname{cos} \phi_3 \\ \end{bmatrix} \begin{bmatrix} \Delta x_R\\ \Delta y_R\\ \Delta \theta_R \end{bmatrix}\] For example, take the common configuration of two tracking wheels placed parallel to the robot’s \(x\)-axis at \((0,\pm y_0)\) and a third placed parallel to the robot’s \(y\)-axis at \((x_0,0)\). For simplicity, all wheels point in the positive direction of their corresponding axis. This gives the following matrices: \[\begin{bmatrix} \Delta \theta_1\\ \Delta \theta_2\\ \Delta \theta_3 \end{bmatrix} = \frac{1}{R} \begin{bmatrix} 1 & 0 & -y_0 \\ 1 & 0 & y_0 \\ 0 & 1 & x_0 \\ \end{bmatrix} \begin{bmatrix} \Delta x_R\\ \Delta y_R\\ \Delta \theta_R \end{bmatrix}\] \[\begin{bmatrix} \Delta x_R\\ \Delta y_R\\ \Delta \theta_R \end{bmatrix} = \frac{R}{2y_0} \begin{bmatrix} y_0 & y_0 & 0\\ x_0 & -x_0 & 2y_0\\ -1 & 1 & 0\\ \end{bmatrix} \begin{bmatrix} \Delta \theta_1\\ \Delta \theta_2\\ \Delta \theta_3 \end{bmatrix} \quad\leftrightarrow\quad \begin{aligned}[c] \Delta x_R &= \frac{R}{2}(\Delta \theta_1 + \Delta \theta_2)\\ \Delta y_R &= R\bigg[\frac{x_0}{2y_0}(\Delta \theta_1 - \Delta \theta_2) + \Delta \theta_3\bigg]\\ \Delta \theta_R &= \frac{R}{2y_0}(\Delta \theta_2 - \Delta \theta_1) \end{aligned}\] Two tracking wheels and a heading sensor is also sufficient for localization. \[\begin{bmatrix} \Delta \theta_1\\ \Delta \theta_2\\ \Delta \theta_3 \end{bmatrix} = \frac{1}{R} \begin{bmatrix} \operatorname{cos} \phi_1 & \operatorname{sin} \phi_1 & x_1 \operatorname{sin} \phi_1 - y_1 \operatorname{cos} \phi_1 \\ \operatorname{cos} \phi_2 & \operatorname{sin} \phi_2 & x_2 \operatorname{sin} \phi_2 - y_2 \operatorname{cos} \phi_2 \\ 0 & 0 & R\\ \end{bmatrix} \begin{bmatrix} \Delta x_R\\ \Delta y_R\\ \Delta \theta_R \end{bmatrix}\]