Differences

This shows you the differences between two versions of the page.

--- cs:localization:rotation:start [2017/01/22 01:14]
Brian Moore created
+++ cs:localization:rotation:start [2018/04/29 11:25] (current)
Brian Moore [Equivalent Rotations]
@@ Line 54: / Line 54: @@
 A full 3D rotation includes a roll, pitch, and yaw.  With these three rotations, we can describe any arbitrary orientation.
-Order of operation is important.  To achieve a complete rotation, the vector must be first rolled, then pitched, then yawed.
+Order of operation is important.  The complete $R$ matrix describes the vehicle first yawing around its own z-axis, then pitching along its own y-axis, and then finally rolling about its own x-axis.  As an example, the rotation $R([180,10,30])$ would have the submarine pointed to the left $30^o$, then pitched slightly upwards by $10^o$, and then rolling onto its back at $180^o$.
+However, our rotation matrices do not provide rotations about our vehicle's intrinsic axes.  They rotate vectors about the global, static $x,y,z$ axes.  Thus, to achieve a complete rotation, the vector must be first rolled, then pitched, then yawed, relative to these constant axes.  These matrices are meant to operate on 3x1 column vectors on right right-hand side.  As such, order of operation goes from right to left as more rotations are tacked onto the system.
 $$
-R_{\psi,\phi,\theta} = R_\psi R_\phi R_\theta \\
+R_{\psi,\phi,\theta} = R_\theta R_\phi R_\psi \\
 R =
@@ Line 78: / Line 81: @@
 \end{bmatrix} \\
-R =
+R([\psi,\phi,\theta]) =
 \begin{bmatrix}
 cos(\theta)sin(\phi) & cos(\theta)sin(\phi)sin(\psi) - sin(\theta)cos(\psi) & cos(\theta)sin(\phi)cos(\psi) + sin(\theta)sin(\psi) \\
@@ Line 85: / Line 88: @@
 \end{bmatrix}
 $$
+Additionally, it is important to be able to reverse the process, and identify which combination of roll, pitch, and yaw describes the current orientation.  This will be a non-unique combination of values, as there are an arbitrary number of ways to reach a given orientation.  However, this process will yield consistent results.
+$$
+\theta = \arctan(R_{21},R_{11}),\quad [-180^o,180^o] \\
+\phi = \arctan(-R_{31},\sqrt{R_{32}^2 + R_{33}^2}),\quad [-90^o,90^o] \\
+\psi = \arctan(R_{32},R_{33}),\quad [-180^o,180^o]
+$$
+In the event that $\phi = \pm90^o$ the other values must be determined using the more complicated four elements in the upper-right corner. When pitched by $\pm90^o$ yaw and roll become meaningless as independent values - only their sum or difference remain.  Thus we can describe the orientation vector as $\begin{bmatrix}0 & \phi & \theta^* \end{bmatrix}$ or as $\begin{bmatrix}\psi^* & \phi & 0\end{bmatrix}$. We will go with the first version, reporting the roll parameter as equal to zero.  For an arbitrary rotation including a pitch of $\phi = +90^o$ the yaw is reported as $\theta^* = \theta-\psi$. For an arbitrary rotation including a pitch of $\phi = +90^o$ the yaw is reported as $\theta^* = \theta+\psi$.
+Looking at the formulation for the elements of $R$ above and assuming $\phi = \pm90^o$ we can find how to reverse-calculate our $\theta^*$
+$$
+\theta^* = \arctan(-R_{12},R_{22})
+$$
+===== Inverse Rotation =====
+To reverse the rotation of an $ \begin{bmatrix} x,y,z \end{bmatrix} $ matrix, you multiply it by the inverse of the rotation matrix $R^{-1}$.  The inverse of an orthagonal matrix is equal to its transpose.
+$$
+RU = U^R \\
+\begin{bmatrix}
+R_{11} & R_{12} & R_{13} \\
+R_{21} & R_{22} & R_{23} \\
+R_{31} & R_{32} & R_{33}
+\end{bmatrix}
+\begin{bmatrix}
+x \\
+y \\
+z
+\end{bmatrix}
+=
+\begin{bmatrix}
+x^{R} \\
+y^{R} \\
+z^{R}
+\end{bmatrix} \\
+\\
+R^{\mathrm {T}}RU = R^{\mathrm {T}}U^R = U \\
+\begin{bmatrix}
+R_{11} & R_{12} & R_{13} \\
+R_{21} & R_{22} & R_{23} \\
+R_{31} & R_{32} & R_{33}
+\end{bmatrix}
+^{\mathrm {T}}
+\begin{bmatrix}
+x^R \\
+y^R \\
+z^R
+\end{bmatrix}
+=
+\begin{bmatrix}
+x \\
+y \\
+z
+\end{bmatrix}
+$$
+It is worth stating explicitly that $R^{-1} \neq R(-\psi,-\phi,-\theta)$. If you yaw, then pitch, then roll into an orientation, you //cannot// anti-yaw, then anti-pitch, then anti-roll from that orientation to get back to the origin.  You'd have to anti-roll, then anti-pitch, then anti-yaw.  It must be rotated completely in reverse.  It must be multiplied by its //transpose// $R^{\mathrm {T}}$.
+$$
+U \neq (R_{-\theta} R_{-\phi} R_{-\psi})(R_\theta R_\phi R_\psi) U \\
+U = (R_{-\psi} R_{-\phi} R_{-\theta}) (R_\theta R_\phi R_\psi) U \\
+U = (R_{-\psi} (R_{-\phi} (R_{-\theta} R_\theta) R_\phi) R_\psi) U \\
+U = (R_{-\psi} (R_{-\phi} (I) R_\phi) R_\psi) U \\
+U = (R_{-\psi} (R_{-\phi} R_\phi) R_\psi) U \\
+U = (R_{-\psi} (I) R_\psi) U \\
+U = (R_{-\psi} R_\psi) U \\
+U = (I) U \\
+$$
+The Equivalent yaw/pitch/roll combination for a Rotation Matrix's transpose will not necessarily have any values corresponding to the original roll/pitch/yaw rotation.
+==== Equivalent Rotations ====
+It is often necessary to find an equivalent rotation from a series or rotations, or as the difference between two rotations.
+We often want to calculate where our vector is at after rotating first by $R_1$, then by $R_2$, and finally by $R_3$.  If you recall from above, these complete Rotation matrices will behave exactly as the specific roll, pitch, and yaw matrices.  That is to say, the rotations they perform are all relative to the fixed global $x,y,z$ axes.  Which means that the rotation performed last, $R_3$, must be allowed to act on the vector first.
+Were we to tell our submarine's control system to perform a relative rotation of $R(\psi_1,\phi_1,\theta_1)$, and then once accomplishing it make another relative rotation $R(\psi_2,\phi_2,\theta_2)$, and then finally tell it to make a third relative rotation $R(\psi_3,\phi_3,\theta_3)$, we would calculate the result as
+$$
+R_{123}U = (R_1(R_2(R_3U)))
+$$
+and **NOT**
+$$
+R_{123}U \neq (R_3(R_2(R_1U))
+$$
+Be sure to remember this, or you'll get headaches down the line.
+Now we have a different question.  Say we are at orientation $R_1$ and we want to rotate to another arbitrary rotation $R_2$.  For instance, we are at orientation $R_1(20,-40,7)$ and we tell our control system we want to go to the absolute orientation $R_1(70,10,-40)$.  What rotation will move us between these two orientations?
+Using what we know from above, about the order of operation, we can make an equation.  Recall that $R_{err}$ will rotate us from where we are currently, at $R_1$. Thus we must apply it //before// $R_1$:
+$$
+R_1R_{err}U = R_2U \\
+R_1^{\mathrm {T}}R_1R_{err}U = R_1^{\mathrm {T}}R_2U\\ \\
+R_{err} = R_1^{\mathrm {T}}R_2 \\
+$$
+To find the rotation //between// two rotations, it is helpful to ask the question "What rotation would I need to achieve $R_2$ if $R_1$ was at the origin?" The answer is, of course, just $R_2$. We find our relative motion by first un-rotating $R_2$ by $R_1$
+===== Special Properties =====
+Rotation matrices have some very exploitable properties.  It stands to reason that there is an underlying structure, as we are using 9 elements to represent only 3 unique values.
+=== Othanormal ===
+A Rotation matrix's Transpose is equal to its inverse.
+$$
+=\det(I)=\det(R^{\mathrm {T} }R)=\det(R^{\mathrm {T} })\det(R)=(\det(R))^{2} \\
+\det(R) = \pm1
+$$
+===== Code =====
+To produce a 3x3 rotation matrix from roll $\psi$, pitch $\phi$, and yaw $\theta$ use the following matlab code or it's C++ equivalent:
+<code matlab>
+function [R] = r3D(omega) % omega = [roll; pitch; yaw]
+R = [cosd(omega(3)) -sind(omega(3)) 0;...
+    sind(omega(3)) cosd(omega(3)) 0;...
+0 1] * ...
+    [cosd(omega(2)) 0 sind(omega(2));...
+1 0;...
+    -sind(omega(2)) 0 cosd(omega(2))] * ...
+    [1 0 0;...
+cosd(omega(1)) -sind(omega(1));...
+sind(omega(1)) cosd(omega(1))];
+end
+</code>
+To find an equivalent roll $\psi$, pitch $\phi$, and yaw $\theta$ //given// a 3x3 rotation matrix $R$ use the following code:
+<code matlab>
+function [omega] = ir3D(R)
+   theta = atan2di(R(2,1),R(1,1));
+   psi = atan2di(R(3,2),R(3,3));
+   phi = atan2di(-R(3,1),sqrt(sum(R(3,2:3).^2)));
+   if(abs(phi)==90)
+       theta = atan2di(-R(1,2),R(2,2));
+       psi = 0;
+   end
+   omega = [psi;phi;theta]; %omega = [roll;pitch;yaw]
+end
+</code>

User Tools

Differences

Page Tools

Palouse RoboSub Technical Documentation

Site Tools