3
$\begingroup$

I'm reading Robotics, Vision and Control, on page 108 it has following equations:

$$\rho=\sqrt{\Delta^2_x+\Delta^2_y}$$ $$\alpha = \tan^{-1}\frac{\Delta_y}{\Delta_x}-\theta$$ $$\beta = -\theta-\alpha$$

How does it go from the above to the following equations?

$$\pmatrix{ \dot{\rho}\\ \dot{\alpha}\\ \dot{\beta}} = \pmatrix{ -\cos \alpha & 0\\ \frac{\sin \alpha}{\rho} & -1\\ -\frac{\sin \alpha}{\rho} & 0 }\pmatrix{v \\ \omega }\text{, if } \alpha \in \left(-\frac{\pi}{2}, -\frac{\pi}{2}\right] $$

$$v=k_p\rho$$

$$\omega=k_\alpha \alpha+k_\beta \beta$$

$$\pmatrix{ \dot{\rho}\\ \dot{\alpha}\\ \dot{\beta}} = \pmatrix{ -k_\rho \cos \alpha\\ k_\rho \sin\alpha - k_\alpha \alpha -k_\beta \beta\\ -k_\rho \sin \alpha } $$

Shouldn't $\dot{\rho} = -k_\rho \rho \cos\alpha$ ?

$\endgroup$
  • $\begingroup$ Is that the 2nd edition of Robotics, Vision and Control? The page numbering doesn't seem to match my copy. $\endgroup$ – sempaiscuba Apr 5 '19 at 0:41
  • $\begingroup$ @sempaiscuba yes 2nd $\endgroup$ – drerD Apr 5 '19 at 0:48
  • 3
    $\begingroup$ On Robotics we are fortunate enough to have MathJax support enabled, allowing you to easily create subscripts, superscripts, fractions, square roots, greek letters and more. This allows you to add both inline and block element mathematical expressions in robotics questions and answers. For a quick tutorial, take a look at How can I format mathematical expressions here, using MathJax? $\endgroup$ – Mark Booth Apr 5 '19 at 10:39
6
$\begingroup$

OK, I'm working from the 1st edition of the book, so section 4.2.4 of my edition starts on page 75, but the text appears otherwise to be the same.


Initially, the book re-writes the equations of motion of the robot, given earlier as Eq. 4.2:

$ \dot x = \nu cos \theta $

$ \dot y = \nu sin \theta $

$ \dot \theta = \frac {\nu} {L} tan \gamma $

in matrix form:

$ \begin{pmatrix} \dot x \\ \dot y \\ \dot \theta \end{pmatrix} = \begin{pmatrix} cos \theta & 0 \\ sin \theta & 0 \\ 0 & 1 \end{pmatrix} \begin{pmatrix} \nu \\ \gamma \end{pmatrix} $


These equations are then transformed into polar coordinate form using the notation from the following figure (figure 4.12 in my edition, yours may vary):

Polar corordinate notation for bicycle model

From this figure, we obtain your first set of equations:

$ \rho = \sqrt {\Delta ^{2} _{x} + \Delta ^{2} _{y}} $

$ \alpha = tan ^{-1} \frac {\Delta _{y}} {\Delta _{x}} - \theta $

$ \beta = - \theta - \alpha $


Next, we apply the chain rule. Multiply both sides of Eq 4.2 by the Jacobian of the coordinate transformation, J.

There are a couple of ways to obtain the Jacobian, J. Either differentiate the forward map to get

$ J = \begin{pmatrix} {\frac {\Delta _{x}} {\sqrt {\Delta ^{2} _{x} + \Delta ^{2} _{y}}}} & {\frac {\Delta _{y}} {\sqrt {\Delta ^{2} _{x} + \Delta ^{2} _{y}}}} & 0 \\ -{\frac {\Delta _{y}} {{\Delta ^{2} _{x} + \Delta ^{2} _{y}}}} & {{\frac {\Delta _{x}} {{\Delta ^{2} _{x} + \Delta ^{2} _{y}}}}} & -1 \\ {\frac {\Delta _{y}} {{\Delta ^{2} _{x} + \Delta ^{2} _{y}}}} & -{{\frac {\Delta _{x}} {{\Delta ^{2} _{x} + \Delta ^{2} _{y}}}}} & 0 \end{pmatrix} = \begin{pmatrix} -\cos\beta & \sin\beta & 0 \\ -{\frac {\sin \beta} {\rho}} & -{\frac {\cos \beta} {\rho}} & -1 \\ {\frac {\sin \beta} {\rho}} & {\frac {\cos \beta} {\rho}} & 0 \end{pmatrix} $

Alternatively, since we can write the inverse as:

$ \Delta _{x} = -\rho\cos(-\beta) = -\rho\cos\beta $

$ \Delta _{y} = -\rho\sin(-\beta) = \rho\sin\beta $

$ \theta = -(\alpha + \beta) $

we could write this in matrix form and differentiate to get:

$ \begin{pmatrix} -\cos \beta & 0 & \rho \sin \beta \\ \sin\beta & 0 & \rho \cos \beta \\ 0 & -1 & -1 \end{pmatrix} $

and then invert to obtain J:

$ J = \begin{pmatrix} -\cos\beta & \sin\beta & 0 \\ -{\frac {\sin \beta} {\rho}} & -{\frac {\cos \beta} {\rho}} & -1 \\ {\frac {\sin \beta} {\rho}} & {\frac {\cos \beta} {\rho}} & 0 \end{pmatrix} $


$ J \begin{pmatrix} cos \theta & 0 \\ sin \theta & 0 \\ 0 & 1 \end{pmatrix} \begin{pmatrix} \nu \\ \gamma \end{pmatrix} = \begin{pmatrix} -\cos \beta \cos \theta + \sin \beta \sin \theta & 0 \\ - \frac {1} {\rho} (\cos \beta \sin \theta + \sin \beta \cos \theta) & -1 \\ \frac {1} {\rho} (\cos \beta \sin \theta + \sin \beta \cos \theta) & 0 \end{pmatrix} \begin{pmatrix} \nu \\ \gamma \end{pmatrix} $

Simplifying and then substituting $ \theta = -(\alpha + \beta) $ gives:

$ = \begin{pmatrix} - \cos (\beta + \theta) & 0 \\ - \frac {1} {\rho} \sin (\beta + \theta) & -1 \\ \frac {1} {\rho} \sin (\beta + \theta) & 0 \end{pmatrix} \begin{pmatrix} \nu \\ \gamma \end{pmatrix} = \begin{pmatrix} - \cos \alpha &0 \\ {\frac {\sin \alpha} {\rho}} & -1 \\ - {\frac {\sin \alpha} {\rho}} & 0 \end{pmatrix} \begin{pmatrix} \nu \\ \gamma \end{pmatrix} $


So, we can write:

$ \begin{pmatrix} \dot \rho \\ \dot \alpha \\ \dot \beta \end{pmatrix} = \begin{pmatrix} -cos \alpha & 0 \\ \frac {sin \alpha} {\rho} & -1 \\ -\frac {sin \alpha} {\rho} & 0 \end{pmatrix} \begin{pmatrix} \nu \\ \gamma \end{pmatrix}, \text { if } \alpha \in \begin{pmatrix} -\frac {\pi} {2} , \frac {\pi} {2} \end{pmatrix} $

Which is your second equation.


The equations

$ \nu = k _{\rho} \rho $

$ \gamma = k _{\alpha} \alpha + k _{\beta} \beta $

are the linear control law which, when applied to the equation we just derived gives us:

$ \begin{pmatrix} \dot \rho \\ \dot \alpha \\ \dot \beta \end{pmatrix} = \begin{pmatrix} - k _{\rho} \rho \cos \alpha \\ k _{\rho} \rho \sin \alpha - k _{\alpha} \alpha - k _{\beta} \beta \\ -k _{\rho} \sin \alpha \end{pmatrix} $

for the closed-loop system.


And yes, it looks like there may be a typo in the book (on page 76 for the first edition, presumably on page 109 in yours?)


A list of errata for RVC can be found on Peter's website, and this doesn't appear to be listed for either edition. I've emailed him (using the address on the errata page) to let him know about it with a link to your question.

Alternatively, Peter has recently started using Robotics:SE, so he may see it here (I've suggested an edit to the title to include the name of the book).

| improve this answer | |
$\endgroup$
  • $\begingroup$ Why is $J\begin{pmatrix} \dot x \\ \dot y \\ \dot \theta \end{pmatrix} = \begin{pmatrix} \dot p \\ \dot \alpha \\ \dot \beta \end{pmatrix}$? $\endgroup$ – drerD Apr 5 '19 at 17:04
  • $\begingroup$ J is the Jacobian for the change of variables from Cartesian coordinates. See, for example, Changing Coordinates for a step-by-step explanation with worked examples. $\endgroup$ – sempaiscuba Apr 5 '19 at 17:29
4
$\begingroup$

Thanks for the question and great derivation by @sempaiscuba. I first learnt about this kind of controller from the book by Siegwart etal. but the source is

Exponential Stabilization of a Wheeled Mobile Robot Via Discontinuous Control, A. Astolfi, Journal of Dynamic Systems, Measurement, and Control, March 19999, Vol 121, pp. 121-126.


Yes, there is an error and it should indeed be $\dot{\rho} = -k_p \rho \cos \alpha$.

There is an error in the first edition, and fixed in the second, where I mistakenly have the control inputs as $(v, \gamma)^T$, as used in @sempaiscuba's solution, rather than $(v, \omega)^T$. This control approach assumes a unicycle rather than a bicycle kino-dynamic model. The first equation on page 107 of the second edition mistakenly has $\dot{\omega}$ where it should have $\dot{y}$ is the left-hand side. I will get it right for the third edition...

I have updated the errata for the book.

| improve this answer | |
$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service, privacy policy and cookie policy

Not the answer you're looking for? Browse other questions tagged or ask your own question.