1
$\begingroup$

I have the following system here:

https://i.stack.imgur.com/DKIDk.jpg 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.

$\endgroup$
1
  • $\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, 2015 at 18:01

1 Answer 1

3
$\begingroup$

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

$\endgroup$
14
  • $\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, 2015 at 4:03
  • $\begingroup$ Also, doesn't R change wrt time as well. $\endgroup$
    – raaj
    Oct 25, 2015 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, 2015 at 4:20
  • $\begingroup$ Hmm okay thank you, also the final equation, not sure what the L refers to there. $\endgroup$
    – raaj
    Oct 25, 2015 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, 2015 at 4:31

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge that you have read and understand our privacy policy and code of conduct.

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