1
$\begingroup$

Room coordinates are following my walls, to use the guidance system I build the position from various other sensors & built a GPS position from it.

As I also need the a "fake" compass I'm trying to interface a moving robot with a sensor I made.

Robot expect compass to send him the values of a 3-axis magnometer. As my sensor gives me the orientation pitch & roll I have this formula:

$\text{Orientation}=\text{atan2}( (-\text{ymag}*\cos(\text{Roll}) + \text{zmag}*\sin(\text{Roll}) ) , (\text{xmag}*\cos(\text{Pitch}) + \text{ymag}*\sin(\text{Pitch})*\sin(\text{Roll})+ \text{zmag}*\sin(\text{Pitch})*\cos(\text{Roll})))$

as I've 3 unknown variables & one equation I need more equations. But I'm stuck, there should be a way based on Orientation values to get constraints (i.e in $\text{atan2}(y,x) = \arctan(y/x)$ if $x > 0$, etc.) but I can translate those relations to equations.

Am I missing something or is it impossible?

What Im trying to do:

-get Xmag,Ymag and Zmag, those are the expected output of the fake compass.

-Known variables are: Orientation (Yaw) Pitch & Roll, on the robot system (X: right of robot, Y: front of robot, Z: going up) Yaw is the rotation on Z in reference to a "North" arbitrary selected, Pitch the rotation on X and Roll the rotation on Y.

$\endgroup$
  • $\begingroup$ I've read the question a few times and I'm not exactly sure what you are trying to do. Could you be a little more specific as to what exactly you want to calculate? $\endgroup$ – Sriotchilism O'Zaic Oct 11 '17 at 2:02
  • $\begingroup$ Not sure if I understand the question, is this correct? You want to calculate xmag, ymag, zmag (to provide to the robot) given orientation, pitch and roll (which you get from the sensor)? Is (xmag, ymag, zmag) a vector giving the direction of the robot? Do orientation, pitch determine the direction of the robot, while roll increases as the robot rotates around its axis? $\endgroup$ – stewbasic Oct 11 '17 at 2:03
  • $\begingroup$ I indeed try to get xmag,ymag, zmag. Orientation, Pitch, Roll are known (Yaw = orientation, to my "fake" north). Im updating the question. $\endgroup$ – A.albin Oct 11 '17 at 2:10
0
$\begingroup$

You will need approximations.

Disclaimer this is going to be a more practical answer, and not a theorical & real mathematic answer, use with cautions.

Assuming your distance to your North axe being relatively small, and your Pitch and Roll being really close to 0 rad (i.e. flat or under 5 degrees).

Your Orientation (Or) is then:

Or = atan2(-Ymag;Xmag) (1)

As your robot will still process your Zmag even if your Pitch and Roll are close to zero, you can neutralize this by setting:

Zmag = 0

Using:
Atan2(Y;X) = 2* arctan(Y/X) with X > 0

Lets also set X to a covenient value; From your equation Xmag is going to interact with your pitch, and we assume pitch to be close to 0 rad during the whole process. so Xmag * cos(pitch) is going to tend to Xmag.

For simplicity let's set:

Xmag = 1.

Then (1) become:

Or = arctan(-Ymag); Ymag = - tan(Or).

Thus for an orientation Or your mag values are (1, - tan(Or), 0).

sanity check:

Pitch = Roll = 0 & Yaw = 0.7 rad then Mag(1, -0.84228838046 ,0).

and Or = atan2(-Ymag; Xmag) = 2 arctan(-Ymag / (sqrt(Ymag^2+Xmag^2) + Xmag)) [1]

Or = 2 arctan(-Ymag / [sqrt(Ymag^2 + 1)+1] = 2 * -0.35

Or = 0.7 rad.

I know its "dirty" but in your specific case its enough (Pitch & Roll close to 0), and even if your pitch and roll are a bit bigger your error should be negligible.

So, once again sorry for the non-mathematical answer.

[1] as far as I know this is the most accurate atan2 formula when you have to use float values.

$\endgroup$
  • 2
    $\begingroup$ Welcome to the site! Please use MathJax for formatting your math. Also, you should ask yourself why approximations are necessary (if you know, you should share this - if you don't, you probably shouldn't attempt an answer). Cheers. $\endgroup$ – Bobson Dugnutt Oct 11 '17 at 13:20
  • $\begingroup$ Unless you can solve 3 unknown variables with a single equation, you'll need approximations. And as the use case in robotics easily tolerate those approximation, its actually the fastest way to solve the problem for this use case. $\endgroup$ – Ebya Oct 11 '17 at 14:19
  • $\begingroup$ Even if not a math approach it indeed work as long as I was on flat. Even if I use a robot indoor, my robot is sadly able to do roll up to 45 degrees. But I've to admit that it actually work, and was quite simple to use. Thank you for stalking me ;) $\endgroup$ – A.albin Oct 12 '17 at 6:48
0
$\begingroup$

I don't know how a 3-axis magnometer works, but based on the equation you gave I'm assuming it measures the components of the $x,y$ and $z$ unit vectors relative to some "north" direction. To save some typing I'll denote orientation, pitch and roll by $\theta$, $\phi$ and $\rho$ respectively.

I am also not sure how your axes are oriented. Your formula

$\text{Orientation}=\text{atan2}( (-\text{ymag}*\cos(\text{Roll}) + \text{zmag}*\sin(\text{Roll}) ) ,$ ...

suggests that Roll is a rotation in the $yz$-plane, ie around the $x$-axis (note that the Roll terms all multiply $y$ or $z$). This is inconsistent with

Roll the rotation on Y.

If roll is around the $y$-axis, then the rotation that takes your robot from its resting position (pointing along the $y$-axis of the room) to its current position is $$ \begin{bmatrix}\cos\theta&\sin\theta&0\\-\sin\theta&\cos\theta&0\\0&0&1\end{bmatrix} \begin{bmatrix}1&0&0\\0&\cos\phi&\sin\phi\\0&-\sin\phi&\cos\phi\end{bmatrix} \begin{bmatrix}\cos\rho&0&-\sin\rho\\0&1&0\\\sin\rho&0&\cos\rho\end{bmatrix} $$ $$ =\begin{bmatrix} \cos\theta\cos\rho+\sin\theta\sin\phi\sin\rho& \sin\theta\cos\phi& -\cos\theta\sin\rho+\sin\theta\sin\phi\cos\rho\\ -\sin\theta\cos\rho+\cos\theta\sin\phi\sin\rho& \cos\theta\cos\phi& \sin\theta\sin\rho+\cos\theta\sin\phi\cos\rho\\ \cos\phi\sin\rho&-\sin\phi&\cos\phi\cos\rho \end{bmatrix} $$ Assuming your "fake north" is the room's $y$-axis, the fake compass output is the second row, namely $$ x_\mathrm{mag}=-\sin\theta\cos\rho+\cos\theta\sin\phi\sin\rho, $$ $$ y_\mathrm{mag}=\cos\theta\cos\phi, $$ $$ z_\mathrm{mag}=\sin\theta\sin\rho+\cos\theta\sin\phi\cos\rho. $$ See this geogebra demo.

On the other hand, if roll is around the $x$-axis and "fake north" is the $x$-axis, then $$ x_\mathrm{mag}=\cos\theta\cos\phi, $$ $$ y_\mathrm{mag}=-\sin\theta\cos\rho+\cos\theta\sin\phi\sin\rho, $$ $$ z_\mathrm{mag}=\sin\theta\sin\rho+\cos\theta\sin\phi\cos\rho. $$ This agrees with your formula, since $$ -y_\mathrm{mag}\cos\rho+z_\mathrm{mag}\sin\rho=\sin\theta, $$ $$ x_\mathrm{mag}\cos\phi+y_\mathrm{mag}\sin\phi\sin\rho +z_\mathrm{mag}\sin\phi\cos\rho=\cos\theta. $$ See this geogebra demo.

$\endgroup$
  • $\begingroup$ Your assumptions seems correct on my use case. I'll try to send that to the robot & check its output. thank you $\endgroup$ – A.albin Oct 11 '17 at 3:04
  • $\begingroup$ As clarification a 3-axis magnometer measure the earth magnetic field on 3 axis. the magnometer axis are the same as the robot's. $\endgroup$ – A.albin Oct 11 '17 at 3:27
  • $\begingroup$ @A.albin OK, makes sense. The only thing I don't understand is why the equation in your question has $x,y$ switched compared to mine; are you sure it's right? If $\phi=\theta=0$ then the robot points along the $y$-axis and we should have $x_\mathrm{mag}=z_\mathrm{mag}=0$, $y_\mathrm{mag}=1$, right? $\endgroup$ – stewbasic Oct 11 '17 at 4:48
  • $\begingroup$ if Phi = 0 (only this condition is needed) the robot is facing the False North of the system, as Phi is the orientation/yaw and thus the rotation on Z(going down actually if we keep the sanity of the coordinates system) being 0 means the Robot coordinate system doesn't require a rotation in the plan (Xrob,Yrob) to be transposed to Xroom,Yroon $\endgroup$ – A.albin Oct 11 '17 at 6:50
  • $\begingroup$ I just came home & tried it with some numbers, and it seems wrong; assuming Pitch and Roll at 0 rad(i.e. robot is flat on ground). then orientation is atan2(-Ymag; Xmag) & from your formula Xmag = -sin(Theta). Choosing Ymag = 1 & Xmag = 2 then Yaw = arctan(-0.5) = -0.463 rad giving thus Xmag = -0.99 $\endgroup$ – A.albin Oct 11 '17 at 11:44

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.