2 wheel robot kinematics -- transformation of coordinates

Question

I am working my way through the Mobile Robot Kinematics book from CMU, available at http://www.cs.cmu.edu/~rasc/Download/AMRobots3.pdf. I am having a hard time deriving the derivative of the robot motion in polar coordinates (equation 3.53).

Given a robot traveling forward at linear speed $v$ and angular speed $\omega$, its position and orientation can be described in cartesian coordinates $(\Delta x, \Delta y, \theta)$. Its position and orientation can also be described in polar coordinates as $(\rho, \alpha, \beta)$

where:

$\rho$ = the distance from the robot to the origin
$\alpha$ = the angle between the robot's orientation and the origin
$\beta$ is the angle between the robot's position and the x axis

That is,

$\rho = \sqrt{\Delta x^2 + \Delta y^2}$
$\alpha = - \theta + atan(\Delta y / \Delta x)$, and
$\beta = - \theta - \alpha$.

I can follow the math through equation 3.48, for instance, to derive $\dot x = v$ cos $\theta$, but I am flummoxed how they derived the polar derivatives in equation 3.53, namely:

$\dot \rho = - v$ cos $\alpha$
$\dot \alpha = v$ sin $(\alpha) / \rho - \omega$
$\dot \beta = - v$ sin $(\alpha) / \rho$

I can sort of see how $\dot \rho = - v$ cos $\alpha$ is saying that the $\rho$ component of the distance the robot travels along the line $\rho$ in unit time is $v$ cos $\alpha$, but the derivation of $\dot \alpha$ and $\dot \beta$ completely baffle me. I can't even conceptualize what the function $\beta(t)$ would look like, so that I could then take $d \beta / dt$.

Can somebody help me derive $\dot\rho$, $\dot\alpha$, and $\dot\beta$? This is one exercise I wish they hadn't left to the reader!

Answer 1

A straightforward application of the chain rule does the trick here: multiply both sides of equation (3.48) by the Jacobian of the coordinate transformation. Computing this Jacobian is pretty much like the same exercise for the standard Cartesian to polar transformation, but you have to take a bit of care to get the signs right.

To reduce clutter and help manage the signs, I’ll use the coordinates $x$ and $y$ in the inertial frame instead of the slightly confusing $\Delta x$ and $\Delta y$. It will also be useful to refer to the inverse coordinate map $$x = -\rho\cos(-\beta) = -\rho\cos\beta \\ y = -\rho\sin(-\beta) = \rho\sin\beta \\ \theta = -(\alpha+\beta).$$ Differentiating the forward map, we get $$J = \begin{bmatrix}{x\over\sqrt{x^2+y^2}} & {y\over\sqrt{x^2+y^2}} & 0 \\ -{y\over x^2+y^2} & {x\over x^2+y^2} & -1 \\ {y\over x^2+y^2} & -{x\over x^2+y^2} & 0\end{bmatrix} = \begin{bmatrix} -\cos\beta & \sin\beta & 0 \\ -{\sin\beta\over\rho} & -{\cos\beta\over\rho} & -1 \\ {\sin\beta\over\rho} & {\cos\beta\over\rho} & 0 \end{bmatrix}.$$ Alternatively, differentiate the inverse map to get $$\begin{bmatrix} -\cos\beta & 0 & \rho\sin\beta \\ \sin\beta & 0 & \rho\cos\beta \\ 0&-1&-1 \end{bmatrix}$$ and then invert this matrix.

Using basic trigonometric identities and $\beta+\theta=-\alpha$, we then have for the right-hand side of the transformed kinematic equation $$J \begin{bmatrix}\cos\theta&0\\\sin\theta&0\\0&1\end{bmatrix} \begin{bmatrix}v\\\omega\end{bmatrix} = \begin{bmatrix} -\cos\beta\cos\theta+\sin\beta\sin\theta & 0 \\ -\frac1\rho\left(\cos\beta\sin\theta+\sin\beta\cos\theta\right) & -1 \\ \frac1\rho\left(\cos\beta\sin\theta+\sin\beta\cos\theta\right) & 0 \end{bmatrix} \begin{bmatrix}v\\\omega\end{bmatrix} = \begin{bmatrix}-\cos(\beta+\theta)&0\\-\frac1\rho\sin(\beta+\theta)&-1\\\frac1\rho\sin(\beta+\theta)&0\end{bmatrix} \begin{bmatrix}v\\\omega\end{bmatrix} = \begin{bmatrix}-\cos\alpha&0\\{\sin\alpha\over\rho}&-1\\-{\sin\alpha\over\rho}&0\end{bmatrix} \begin{bmatrix}v\\\omega\end{bmatrix}.$$

Answer 2

The authors define $\rho$ as a function of $\Delta x$ and $\Delta y$, and there is some fussiness involving modulo $\pi/2$ in the definition of $\alpha$, but it seems more straightforward to define $\rho:=\sqrt{x^2+y^2}$ and to take $\alpha$ as originally described: $\alpha$ is the angle from the reference $X_R$ axis to the vector $\hat x$ joining the robot to the goal.

Write $\phi:=\theta+\alpha$, the angle from the inertial $X_I$ axis to the vector $\hat x$. (So $\phi$ is the opposite of $\beta$.) Check that $\sin\phi=-y/\rho$ and $\cos\phi=-x/\rho$; this is true when $(x,y)$ are in the pictured 3rd quadrant, but also true everywhere else. Using $\alpha=\phi-\theta$ compute $$ \rho\cos\alpha=\rho\cos(\phi-\theta)=\rho(\cos\phi\cos\theta+\sin\phi\sin\theta)=-x\cos\theta-y\sin\theta\tag1 $$ and $$ \rho\sin\alpha=\rho\sin(\phi-\theta)=\rho(\sin\phi\cos\theta-\cos\phi\sin\theta)=-y\cos\theta + x\sin\theta.\tag2 $$ Now differentiate implicitly the relation $\rho^2=x^2+y^2$ with respect to $t$ to obtain $$ \rho\dot\rho = x\dot x+y\dot y.\tag3 $$ Plug in $\dot x=v\cos\theta$ and $\dot y = v\sin\theta$ into (3) and simplify using (1): $$ \rho\dot\rho = x v\cos\theta + y v\sin\theta\stackrel{(1)}=-v\rho\cos\alpha. $$ This yields $$\bbox[5px,border:1px solid black]{\dot\rho=-v\cos\alpha.}$$ To compute $\dot\beta$, which is the same as $-\dot\phi$, differentiate both sides of the relation $$\tan\phi = \frac yx$$ to obtain $$ \frac1{\cos^2\phi}\dot\phi = \frac{x\dot y-y\dot x}{x^2}= \frac{xv\sin\theta - yv\cos\theta}{x^2}\stackrel{(2)}= \frac{v\rho\sin\alpha}{\rho^2\cos^2\phi}, $$ and simplify to get $$\bbox[5px,border:1px solid black]{\dot\beta = -\frac{v\sin\alpha}\rho.}$$

Finally compute $\dot\alpha=-\dot\beta-\dot\theta=-\dot\beta-\omega$.