As discussed previously, to represent a 3D rotation, one often uses a rotation matrix that is a member of the Lie groupSO(3). This has been recognized to be the most consistent, secure and unique approach. However, in many application cases, we wish to perform a rotation for a given frame by rotating itself with a certain angle φ about an instantaneously fixed unit vector k, instead of using a successive rotation sequence that is a product of up to three elementary rotation matrices from the basis (2.2). Such a rotation approach is intuitively more natural, effective and easier for sampling in robotic path planning applications because the multiplication of matrices in a successive rotation process is not commutable in general.
Let a unit vector k =
⎛
⎝k1 k2 k3
⎞
⎠ be referred and projected onto a given frame to be performed for a rotation. Clearly, k2 =k21+k22+k23 = 1. Its corresponding skew-symmetric matrix is given by
S(k) =K=
⎛
⎝ 0 −k3 k2
k3 0 −k1
−k2 k1 0
⎞
⎠.
It can be shown without difficulty that for this 3 by 3 unity skew-symmetric matrix,
1. K is skew-symmetric with tr(K) = 0;
2. K2is symmetric with tr(K2) =−2;
3. K3=−K.
With the above interesting properties owned by the unity skew-symmetric matrix K, we now develop a realistic rotation process, called thek–φ pro- cedure. First, let φk ∈ R3 be defined as a 3 by 1 vector. Substituting its skew-symmetric matrixφK∈so(3) into the exponential mapping (2.6) yields
exp(φK) =R∈SO(3). (2.7)
The result of the exponential mapping is a rotation matrix that should be equivalent to the rotation byφabout thek-axis.
Expanding the left-hand side of (2.7) into Taylor series and noticing the above properties ofK, we obtain
exp(φK) =I+φK+φ2
2!K2+φ3
3!K3+φ4 4!K4ã ã ã
=I+ φ−φ3 3! +ã ã ã
K+ φ2 2! −φ4
4! +ã ã ã
K2
=I+ sinφK+ (1−cosφ)K2=R, (2.8) where the well-known Taylor expansions have been applied:
sinφ=φ−φ3 3! +φ5
5! − ã ã ã and cosφ= 1−φ2 2! +φ4
4! − ã ã ã. Equation (2.8), commonly called the Rodrigues formula, provides a means as a forward direction to determine the equivalent rotation matrix R if a frame is to be rotated by a given angleφabout a given unit vectork.
In order to find its inverse solution, first, let us take the trace of (2.8), i.e., tr(R) = 3−2(1−cosφ) = 1 + 2 cosφ
so that
φ= arccos tr(R)−1 2
. (2.9)
Since
RT =I−sinφK+ (1−cosφ)K2, (2.10) subtracting (2.10) from (2.8) yields
R−RT = 2 sinφK, and finally we have
K=R−RT
2 sinφ . (2.11)
Both equations (2.9) and (2.11) offer an inverse formula in a recursive order to determine φ and k in terms of a given rotation matrix R. The formula, at first glance, does not uniquely set the sign. However, since for each cosine
value, there can be ± angle solutions, if you choose a +φ (or −φ) from equation (2.9), then, equation (2.11) will give you a correspondingk(or−k).
Therefore, you may pick any one of the two possible pairsφ andk, and −φ and−k, and either one is correct.
As the first example, if we wish a coordinate frame to be rotated by 600 or π/3 about an axis (1 2 1)T that is referred to the frame itself, then the unit axis and its skew-symmetric matrix become
k= 1
√6
⎛
⎝1 2 1
⎞
⎠, and K= 1
√6
⎛
⎝ 0 −1 2
1 0 −1
−2 1 0
⎞
⎠.
Therefore, the equivalent rotation matrix turns out to be R=I+ sin(600)K+ (1−cos(600))K2=
⎛
⎝ 0.5833 −0.1869 0.7904 0.5202 0.8333 −0.1869
−0.6238 0.5202 0.5833
⎞
⎠.
If you want to reverse the direction, you may apply the inverse formula to determine both the angleφand the axisk in terms of the above rotation matrixR. Based on equation (2.9), since tr(R) = 2, φ= arccos1
2
= 600so that sinφ= √23. Then, based on (2.11), we have
K= R−RT 2 sinφ =
⎛
⎝ 0 −0.4082 0.8165
0.4082 0 −0.4082
−0.8165 0.4082 0
⎞
⎠. (2.12)
Hence, the unit vector is k = (0.4082 0.8165 0.4082)T. Clearly, because cosine is an even function, you can also pick −600 for the angle φ. In this case, your axisk becomes negative accordingly. The two picks are obviously equivalent, because rotating a frame about an axis by an angle will always have the same destination as rotating the original frame about the opposite axis by the negative angle in 3D space.
The second practical example is to find a new frame 1 such that its z1
axis is along a vectorz01= (1 −1 −1)T that is referred to the base frame 0. While it may have a few different approaches to finding the orientation R01 of frame 1, we just try the k−φ procedure to do so. Let the common normal vector to both the base z0-axisz0 = (0 0 1)T and the newz1-axis be determined through the cross-product by
b=z01×z0=
⎛
⎝ 0 1 −1
−1 0 −1
1 1 0
⎞
⎠
⎛
⎝0 0 1
⎞
⎠=
⎛
⎝−1
−1 0
⎞
⎠.
Then, we pick the unit vector ofbto be the rotating axisk:
k= b b = 1
√2
⎛
⎝−1
−1 0
⎞
⎠ such that K= 1
√2
⎛
⎝0 0 −1
0 0 1
1 −1 0
⎞
⎠.
To find the rotating angleφfrom the basez0-axis to the newz1-axis about the abovek, we use the dot-product:
z0Tz10=−1 =z0z01cosφ=√ 3 cosφ, so that
cosφ=− 1
√3 and sinφ=
1−cos2φ= 2
3.
Note that since cosφ <0, it can be predicted thatφis in the second quadrant and thus sinφ >0. Therefore, the new frame orientation can be found by
R10=I+ sinφK+ (1−cosφ)K2=I+ 2
3K+ (1 + 1
√3)K2
=
⎛
⎝0.2113 0.7887 −0.5774 0.7887 0.2113 0.5774 0.5774 −0.5774 −0.5774
⎞
⎠.
Moreover, ifRis symmetric, equation (2.11) will result in a 00 uncertainty.
This means that the frame is to be rotated either by nothing (00) ifR=I, or by 1800 if R = I but RT = R. In the first case R = I, equation (2.9) leads to φ= arccos(1) = 0, and you may just skip over (2.11) to avoid its singularity and do nothing. However, in the second case,φ= arccos(−1) = 1800, and you cannot determineK directly from (2.11). Nevertheless, in this special case, sinφ= 0 and cosφ=−1. Based on the forward equation (2.8), R=I+ sinφK+ (1−cosφ)K2=I+ 2K2, or
K2= 1
2(R−I). (2.13)
Therefore, you have to use this equation to determine K if R = RT but R=I.
For example, if
R=
⎛
⎝ 0 −1 0
−1 0 0
0 0 −1
⎞
⎠,
which is symmetric butR=I. Thus, tr(R) =−1 andφ= 1800. Now, let
K=
⎛
⎝ 0 −k3 k2
k3 0 −k1
−k2 k1 0
⎞
⎠
such that
K2=
⎛
⎝−k32−k22 k1k2 k1k3
k1k2 −k32−k12 k2k3
k1k3 k2k3 −k21−k22
⎞
⎠.
Because
R−I=
⎛
⎝−1 −1 0
−1 −1 0
0 0 −2
⎞
⎠,
by comparing it with K2, based on (2.13), we first have the off-diagonal elements
k1k2=−1
2, k1k3= 0 and k2k3= 0.
This implies thatk3= 0. Then, with the diagonal elements, k32+k22=k22=1
2 and k23+k21=k21= 1 2 such that eitherk1=√
2/2 andk2=−√
2/2, ork1=−√
2/2 andk2=√ 2/2 in corresponding to φ= ±1800, which results in the same rotation. Let us just pick the first pair so that the unit vector
k=
⎛
⎝
√2 2
−√22 0
⎞
⎠ and φ= 1800.
Therefore, only in the case ofRT =R but R =I, we can follow the above procedure to determinekand just setφ= 1800. This example will be revisited again in Chapter 3.
If now postmultiplying the unit vectork to both sides of equation (2.8), we immediately obtain
Rk=k+ sinφKk+ (1−cosφ)K2k=k
becauseKk=k×k≡0. This result shows thatthe unit vector kin the k–φ procedure is actually the eigenvector of the rotation matrixRcorresponding to its eigenvalue +1. Therefore, if you program numerically thek–φprocedure for inverse solution in MATLABT M, you may take advantage of the internal function eig(R) for a given rotation matrixRwithout going through equation (2.11) to determine Kto avoid the possible singularity. In the previous first numerical example,
>> R=[0.5833 -0.1869 0.7904 0.5202 0.8333 -0.1869 -0.6238 0.5202 0.5833];
>> [vec, val] = eig(R) vec =
-0.1291 - 0.6325i -0.1291 + 0.6325i 0.4082 -0.2582 + 0.3162i -0.2582 - 0.3162i 0.8165
0.6455 0.6455 0.4082
val =
0.5000 + 0.8660i 0 0
0 0.5000 - 0.8660i 0
0 0 1.0000
>>
As you can see clearly, once the internal function eig(R) is called, the ele- ment “val” prints the three eigenvalues along the diagonal of the output ma- trix, while “vec” prints the three column eigenvectors for the input matrixR.
Obviously, since the last diagonal element of “val” is 1.0000, its correspond- ing eigenvector on the thrid column of “vec” is (0.4082 0.8165 0.4082)T, which agrees with the result in (2.12). If we tryR =I, the identity matrix, then the working window of MATLABT M will show the following answer:
>> [vec, val]=eig(eye(3)) vec =
1 0 0
0 1 0
0 0 1
val =
1 0 0
0 1 0
0 0 1
>>
This shows that all the three eigenvalues of the 3 by 3 identity I are +1 and their corresponding eigenvectors are given by the unit vectors of the x, y and z axes of the reference frame. Finally, if we try the previous third numerical example in the case of a symmetric but non-identity rotation matrix in MATLABT M, then,
>> [vec, val]=eig([0 -1 0; -1 0 0; 0 0 -1]) vec =
-0.7071 0 -0.7071
-0.7071 0 0.7071
0 1.0000 0
val =
-1 0 0
0 -1 0
0 0 1
>>
This result shows that the unit eigenvector corresponding to the eigenvalue +1 isk= (−0.7071 0.7071 0)T. This is just the negative of the vector from the above example, and it does not affect the final rotation at all because the rotation angleφcan be chosen by ±1800in this special case.
In conclusion, both the forward and inverse compact equations for the rotation matrixR∈SO(3) in (2.8) and thek–φprocedure in (2.9) and (2.11) are developed based on the beautiful exponential mapping in mathematical history between so(3) and SO(3) in (2.7). One will find later that the k–
φ procedure is extremely useful in robotic kinematics and digital human realistic motion-planning, especially for the orientation path-planning.