2
$\begingroup$

I have a sequence of measurements $\{(x_i, y_i, \theta_i)\}_{i = 1 \:...\: N}$. Each of these represents the new pose of the robot seen from the previous pose, i.e. a measurement $(x', y', \theta')$ means that the robot moved of $x'$ and $y'$ units in the $x$ and $y$ axis of the previous coordinates system and rotated of an angle $\theta'$ afterward. Therefore, the value of $\theta$ represents the rotation of the new coordinates system w.r.t. the previous.

How can I compute the absolute path (the sequence of all poses expressed in the absolute frame)?

$\endgroup$
1
$\begingroup$

Assuming frame $0$ is the 'absolute frame', if we let $^j P_i$ be the $i^{th}$ position/orientation expressed in the $j^{th}$ coordinate frame, then what you're asking for is the sequence $$\{(^0P_i)_{i=1 ... N}\},$$ correct?

Using your sequence of measurements $\{(x_i, y_i, \theta_i)_{i=1 ... N}\}$, it's straightforward to compute the $i^{th}$ 2-D transformation matrix,

$$^i_{i+1} T = \begin{pmatrix} \cos \theta_i & -\sin \theta_i & x_i\\ \sin \theta_i & \cos \theta_i & y_i \\ 0 & 0 & 1 \end{pmatrix}$$

And since you have all of them, use the fact that $$^0_i T = ^0_1T ^1_2 T ... ^{i-1}_i T,$$ then just iterate through to list to compute each translation matrix and compute the data you're interested in. Note that you have to append a $1$ to each $P_i$ to make the 3x3 multiplcations work out (as noted in the reference):

$$^0P_{i+1} = ^0_i T ^i P_{i+1}$$

Hopefully that's helpful. Here's a video explaining frame transformations, that I often refer back to. It's really easy to get lost in notation.

EDIT: To compute the orientation of the $i^{th}$ coordinate, you can use the rotational component of the $^0_iT$ translation matrix, since you have the cos and sin of the angle in entries (1,1) and (2,1), just use the atan2 function to pull out $\theta$. Make sense?

| improve this answer | |
$\endgroup$
1
$\begingroup$

Each step can be represented by its transformation matrix, $$ \begin{bmatrix} \cos{\theta'_{i}} & -\sin{\theta'_{i}} & x'_{i}\\ \sin{\theta'_{i}} & \phantom{-}\cos{\theta'_{i}} & y'_{i} \\ 0 & 0 & 1 \end{bmatrix}. $$ The world position at each step $i$ is the rightward-propagating product of the transforms up to that point, $$ \begin{bmatrix} \cos{\theta_{i}} & -\sin{\theta_{i}} & x_{i}\\ \sin{\theta_{i}} & \phantom{-}\cos{\theta_{i}} & y_{i} \\ 0 & 0 & 1 \end{bmatrix} = \prod_{1}^{i} \begin{bmatrix} \cos{\theta'_{i}} & -\sin{\theta'_{i}} & x'_{i}\\ \sin{\theta'_{i}} & \phantom{-}\cos{\theta'_{i}} & y'_{i} \\ 0 & 0 & 1 \end{bmatrix}. $$

(This is the same math as is used for the forward kinematics of a robot arm -- each step is a link with geometry $x',y'$ followed by a joint rotation $\theta'$.)

| 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.