How can I track a fixed point $P=(x_P, y_P)$ from a moving robot?

Coordinates of $P$ are relative to the state/pose of the robot (x axis looks forward the robot and y axis is positive on the right of the robot). Suppose that the initial robot state/pose is at $S_{R}=(x_R, y_R, \theta_R)$. The next frame (namely after $\Delta t$) with the applied control $(v, \omega)$ the robot is at state $S_{R'}=(x_{R'}, y_{R'}, \theta_{R'})$.

Where (I set the axes as OpenCV):

$x_{R'} = x_R + v cos(\theta_R) \Delta t $

$y_{R'} = y_R + v sin(\theta_R) \Delta t $

$\theta_{R'} = \theta_{R} + \omega\Delta t$

The question is: which are the coordinates $(x_P', y_P')$ of the same point $P$ relative to $S_{R'}$?

enter image description here

As visible in the picture, I know the transformation from the initial state to the next state of the robot and the coordinate of P in reference to the initial state

$$ t = \begin{pmatrix} cos(\theta_{R'}) & -sin(\theta_{R'}) & x_{R'}\\ sin(\theta_{R'}) & cos(\theta_{R'}) & y_{R'}\\ 0 & 0 & 1\\ \end{pmatrix} $$

Please correct me if I made some mistakes!

Thank you, any help is appreciated.

  • $\begingroup$ Please try to rephrase your question. You are talking about some point P = (xp,yp), then talking about the state Sr (using some (xr, yr) point) and then asking for the relative position of the point P which you never told us what it is. If you cannot define it mathematically, at least try to explain it with words, which points are your talking about. Thanks. $\endgroup$ Apr 16 '14 at 16:25
  • $\begingroup$ Thankyou for the feedback, I edited the question, is it a bit clearer now? $\endgroup$ Apr 17 '14 at 8:08

Let's assume that you know the position of P=(xp,yp) in the initial state. Let's also assume that you know the transformation matrix t which transforms the initial state of the robot to the next state. Due to the relative nature of positions, you can say that robot is standing still and that the point P is moving. This is exactly what you need as you wish to view everything from robot's own coordinate system. It is not moving relative to itself, but the point P is.

By that we can say: P'=inverse(T)*P


Edit: I'm not completely sure that the transformation matrix you wrote is perfectly correct, please check it and edit the post if it's incorrect for future reference (it should hold the difference between the values, not ending values). Also, keep in mind that the rotation and translation are not commutative so don't let that give you headaches.


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.