Deriving Inverse Kinematics For The Drivetrain

By Mahesh, Cooper, and Ben

Task: Derive Inverse Kinematics For The Drivetrain

Due to having an unconventential drivetrain consisting of two differental wheels and a third swerve wheel, it is crucial that we derive the inverse wheel kinematics early on. These inverse kinematics would convert a desired linear and angular velocity of the robot to individual wheel velocities and an angle for the back swerve wheel. Not only would these inverse kinematics be used during Tele-Op to control the robot through joystick movements, but it will be useful for getting the robot to travel at a desired forward velocity and turning rate for autonomous path following. Either way, deriving basic inverse kinematics for the drivetrain is a necessity prerequisite for most future programming endeavors.

More concretely, the problem consists of the following:
Given a desired linear velocity \(v\) and turning rate/angular velocity \(\omega\), compute the required wheel velocities \(v_l\), \(v_r\), and \(v_s\), as well as the required swerve wheel angle \(\theta\) to produce the given inputs. We will define \(v_l\) as the velocity of the left wheel, \(v_r\) as the velocity of the right wheel, and \(v_s\) as the velocity of the swerve wheel.

To begin, we can consider the problem from the perspective of the robot turning with a given turning radius \(r\) with tangent velocity \(v\). From the equation \(v = \omega r\), we can conclude that \(r = \frac{v}{\omega}\). Notice that this means when \(\omega = 0\), the radius blows up to infinity. Intuitively, this makes sense, as traveling in a straight line (\(\omega = 0\)) is equivalent to turning with an infinite radius.

The main equation used for inverse wheel kinematics is:

\[\displaylines{\vec{v_b} = \vec{v_a} + \vec{\omega_a} \times \vec{r}}\]

Where \(\vec{v_b}\) is the velocity at a point B, \(\vec{v_a}\) is the velocity vector at a point A, \(\vec{\omega_a}\) is the angular velocity vector at A, and \(\vec{r}\) is the distance vector pointing from A to B. The angular velocity vector will point out from the 2-D plane of the robot in the third, \(\hat{k}\), axis (provable using the right-hand rule).

But why exactly does this equation work? What connection does the cross product have with deriving inverse kinematics? In the following section, the above equation will be proven. See section 1.1 to skip past the proof.

To understand the equation, we start by considering a point A rotating around aother point B with turn radius vector \(\hat{r}\) and tangent velocity \(\vec{v}\).

For an angle \(\theta\) around the x-axis, the position vector \(\vec{s}\) can be defined as the following:

\[\displaylines{\vec{s} = r(\hat{i}\cos\theta + \hat{j}\sin\theta) }\]

by splitting the radius vector into its components and recombining them.

To arrive at a desired equation for \(\vec{v}\), we will have to differentiate \(\vec{r}\) with respect to time. By the chain rule:

\[\displaylines{\vec{v} = \frac{d\vec{s}}{dt} = \frac{d\vec{s}}{d\theta} \cdot \frac{d\theta}{dt}}\]

The appropriate equations for \(\frac{d\vec{s}}{d\theta}\) and \(\frac{d\theta}{dt}\) can then be multiplied to produce the desired \(\vec{v}\):

\[\displaylines{\frac{d\vec{s}}{d\theta} = \frac{d}{d\theta} \vec{s} = \frac{d}{d\theta} r(\hat{i}\cos\theta + \hat{j}\sin\theta) = r(-\hat{i}sin\theta + \hat{j}cos\theta) \\ \frac{d\theta}{dt} = \omega \\ \vec{v} = \frac{d\vec{s}}{dt} = \frac{d\vec{s}}{d\theta} \cdot \frac{d\theta}{dt} = r(-\hat{i}sin\theta + \hat{j}cos\theta) \cdot \omega \mathbf{= \omega r(-\hat{i}sin\theta + \hat{j}cos\theta)} }\]

Now that we have an equation for \(\vec{v}\) defined in terms of \(\omega\), \(r\), and \(\theta\), if we can derive the same formula using \(\vec{\omega} \times \vec{r}\), we will have proved that \(\vec{v} = \frac{d\vec{s}}{dt} = \vec{\omega} \times \vec{r}\)

To begin, we will define the following \(\vec{\omega}\) and \(\vec{r}\):

\[\displaylines{ \vec{\omega} = \omega \hat{k} = \begin{bmatrix}0 & 0 & \omega\end{bmatrix} \\ \vec{r} = r(\hat{i}cos\theta + \hat{j}sin\theta) = \hat{i}rcos\theta + \hat{j}rsin\theta = \begin{bmatrix}rcos\theta & rsin\theta & 0\end{bmatrix} }\]

Then, by the definition of a cross product:

\[\displaylines{ \vec{\omega} \times \vec{r} = \begin{vmatrix} \hat{i} & \hat{j} & \hat{k} \\ 0 & 0 & \omega \\ rcos\theta & rsin\theta & 0 \end{vmatrix} \\ = \hat{i}\begin{vmatrix} 0 & \omega \\ rsin\theta & 0\end{vmatrix} - \hat{j}\begin{vmatrix} 0 & \omega \\ rcos\theta & 0\end{vmatrix} + \hat{k}\begin{vmatrix} 0 & 0 \\ rcos\theta & rsin\theta\end{vmatrix} \\ = \hat{i}(-\omega rsin\theta) - \hat{j}(-\omega rcos\theta) + \hat{k}(0) \\ \mathbf{= \omega r(-\hat{i}sin\theta + \hat{j}cos\theta)}}\]

Since the same resulting equation, \(\omega r(-\hat{i}sin\theta + \hat{j}cos\theta)\), is produced from evaluating the cross product of \(\vec{\omega}\) and \(\vec{r}\) and by evaluating \(\frac{d\vec{s}}{dt}\), we can conclude that \(\vec{v} = \vec{\omega} \times \vec{r}\)

1.1

Based on the constants defined and geometry defined in the image below:

The equation can be applied to the left wheel to derive its inverse kinematics:

\[\displaylines{\vec{v_l} = \vec{v} + \vec{\omega} \times \vec{r_l} \\ = \mathbf{v + \omega (r - \frac{l}{2})}}\]

where \(r\) is the turn radius and \(l\) is the width of the robot's front axle. Applied to the right wheel, the equation yields:

\[\displaylines{\vec{v_r} = \vec{v} + \vec{\omega} \times \vec{r_r} \\ = \mathbf{v + \omega (r + \frac{l}{2})}}\]

Applied to the swerve wheel, the equation yields:

\[\displaylines{\vec{v_s} = \vec{v} + \vec{\omega} \times \vec{r_s} \\ = \mathbf{v + \omega \sqrt{r^2 + s^2}}}\]

where \(s\) is the length of the chassis (the distance between the front axle and swerve wheel).

The angle of the swerve wheel can then be calculated like so using the geometry of the robot:

\[\displaylines{\theta = \frac{\pi}{2} - tan^{-1}\left( \frac{s}{r} \right)}\]

Mathematically/intuitively, the equations check out as well. When only rotating (\(v = 0\)), \(r = \frac{v}{\omega} = \frac{0}{\omega} = 0\), so:

\[\displaylines{ v_l = v + \omega(r - \frac{l}{2}) = \omega \cdot -\frac{l}{2} \\ v_r = v + \omega(r + \frac{l}{2}) = \omega \cdot \frac{l}{2} \\ v_s = v + \omega\sqrt{r^2 + s^2} = \omega \cdot \sqrt{ s^2} = \omega \cdot s}\]

In all three cases, \(v = \omega \cdot r\), where \(r\) is the distance from each wheel to the "center of the robot", defined as the midpoint of the front axle. Since rotating without translating will be around a center of rotation equal to the center of the robot, the previous definition for \(r\) can be used.

As for the equation for \(\theta\), the angle of the swerve wheel, it checks out intuitively as well. When only translating (driving straight: \(\omega = 0\)), \(r = \frac{v}{\omega} = \frac{v}{0} = \infty\), so:

\[\displaylines{ \theta = \frac{\pi}{2} - tan^{-1} \left( \frac{s}{r} \right) \\ = \frac{\pi}{2} - tan^{-1} \left( \frac{s}{\infty} \right) \\ = \frac{\pi}{2} - tan^{-1}(0) \\ = \frac{\pi}{2} - 0 \\ = \frac{\pi}{2} }\]

As expected, when translating, \(\theta = 0\), as the swerve wheel must point straight for the robot to drive straight. When only rotating (\(v = 0\)), \(r = \frac{v}{\omega} = \frac{0}{\omega} = 0\), so:

\[\displaylines{ \theta = \frac{\pi}{2} - tan^{-1} \left( \frac{s}{r} \right) \\ = \frac{\pi}{2} - tan^{-1} \left( \frac{s}{0} \right) \\ = \frac{\pi}{2} - tan^{-1}(\infty) \\ = \frac{\pi}{2} - \lim_{x \to +\infty} tan^{-1}(x) \\ = \frac{\pi}{2} - \frac{\pi}{2} \\ = 0}\]

As expected, when only translating (driving straight), \(\theta = \frac{\pi}{2}\), or in other words, the swerve wheel is pointed directly forwards to drive the robot directly forwards. When only rotating, \(\theta = 0\), or in other words, the swerve wheel is pointed directly to the right to allow the robot to rotate counterclockwise.

Next Steps:

The next step is to use PID control to maintain target velocities and angles calculated using the derived inverse kinematic equations. Then, these equations can be used in future motion planning/path planning attempts to get the robot to follow a particular desired \(v\) and \( \omega\).

A distance sensor will also need to be used to calculate \(s\), the distance between the front axle and swerve wheel of the robot.