I have the following system here:

http://imgur.com/UTqswOi enter image description here

Basically, I have a range finder which gives me $R_s$ in this 2D model. I also have the model rotate about the Centre of Mass, where I have angular values and velocities Beta ($\beta$) and BetaDot ($\dot{\beta}$).

I can't see, for the life of me, how to figure the formula for the angular velocity in the Range Finder frame. How am I supposed to do this? I have all the values listed in those variables. The object there doesn't move when the vehicle/system pitches. It's stationary.

  • $\begingroup$ Is your diagram missing an angle reported by the laser? You have $\dot{\beta}$, but should that arrow also point to some angle (say $\alpha$)? $\endgroup$ Oct 24 '15 at 18:01

I think your diagram is missing an angle for the laser angle with respect to the vehicle body -- I'm going to call that angle $\alpha$, see this diagram for clarity:

enter image description here

Since it seems you are tracking an object with your laser, I imagine the point of this is to predict the angular velocity of that object in the vehicle frame. So your laser is scanning in 2D, providing range at a number of angles, and you want to predict what angle the object will be at in the next time step.

In that case, you need to consider to motion of the system to figure out the motion of the target object in the laser frame. Assume the target object has position $\hat{p}$ in the world frame and $\hat{p}_L$ in the laser frame. Although it sounds like this is for a manipulator that is fixed at the base, we will still consider a moving centre of mass with position $p_0$ in the world frame. The position of the target in the world frame is then:

$\hat{p} = p_0 + A_{\beta} \begin{bmatrix} L_z \\ -L_y \\ \end{bmatrix} + A_{\beta} R \begin{bmatrix} \cos \alpha \\ -\sin \alpha \\ \end{bmatrix} $

Where $A_{\beta} = \begin{bmatrix} \cos \beta & \sin \beta \\ -\sin \beta & \cos \beta \\ \end{bmatrix}$

Then we differentiate that to get the velocity -- which should be zero assuming your object is stationary in the world frame.

$0 = \dot{p}_0 + \dot{\beta} J_{\beta} \begin{bmatrix} L_z \\ -L_y \\ \end{bmatrix} + R \dot{\beta} J_{\beta} \begin{bmatrix} \cos \alpha \\ -\sin \alpha \end{bmatrix} + R \dot{\alpha} A_{\beta} \begin{bmatrix} -\sin \alpha \\ -\cos \alpha \end{bmatrix} + \dot{R} A_{\beta} \begin{bmatrix} \cos \alpha \\ -\sin \alpha \end{bmatrix}$

Where $J_{\beta} = \begin{bmatrix} -\sin \beta & \cos \beta \\ -\cos \beta & -\sin \beta \\ \end{bmatrix}$

This then defines two equations (the z- and y-components of the above equation) in two unknowns ($\dot{R}$ and $\dot{\alpha}$).

However, what you really need is to simply find an expression for the predicted range and laser angle ($\tilde{R}$ and $\tilde{\alpha}$) in terms of your states, the vehicle pose, ($z$,$y$,$\beta$) and target position ($\hat{z}$,$\hat{y}$).

$\tilde{R} = \sqrt{\left( \hat{z} - \left( z + L_z \cos \beta \right) \right)^2 + \left( \hat{y} - \left( y + L_y \sin \beta \right) \right)^2}$

$\tilde{\alpha} = \tan^{-1} \left( \frac{\hat{y} - \left( y + L_y \sin \beta \right)}{\hat{z} - \left( z + L_z \cos \beta \right) } \right) - \beta$

Then you can apply this in an EKF according to the chat discussion. Note I have switched the axes labels and sign conventions to something that I am more used to.

enter image description here

enter image description here

  • $\begingroup$ Hi, thanks so much for your answer. I'm a little stuck on where you have mentioned R in all your equations. I assume you refer to that has a vector wrt the laser frame? So it's X and Y in the laser frame for the object $\endgroup$
    – raaj
    Oct 25 '15 at 4:03
  • $\begingroup$ Also, doesn't R change wrt time as well. $\endgroup$
    – raaj
    Oct 25 '15 at 4:16
  • $\begingroup$ R is the laser range as you have indicated in the original question, it is scalar. You are correct that $\dot{R}$ should be accounted for, I overlooked that! The second method I mention is probably better anyway, I will edit my answer accordingly. $\endgroup$ Oct 25 '15 at 4:20
  • $\begingroup$ Hmm okay thank you, also the final equation, not sure what the L refers to there. $\endgroup$
    – raaj
    Oct 25 '15 at 4:23
  • $\begingroup$ Sorry, that should be $L_z$ and $L_y$, I have edited the equation. Do you get the overall approach here? You need to get an expression for the z-y position of the target object in the laser frame, then apply arctan to that position to get the angle. Once you have the expression, just differentiate to get the angle rate. $\endgroup$ Oct 25 '15 at 4:31

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.