《机器人与数字人：基于MATLAB的建模与控制》——2.3节指数映射和k过程

本节书摘来自华章社区《机器人与数字人：基于MATLAB的建模与控制》一书中的第2章，第2.3节指数映射和k过程，作者［美］顾友谅（Edward Y.L.Gu），更多章节内容可以访问云栖社区“华章社区”公众号查看

2.3指数映射和k过程
如前面所述，李群的SO(3)旋转矩阵常用来表示三维旋转。这已经被公认为最为通用、稳定和独特的方法。然而，在许多应用实例中，希望执行给定坐标系绕自身单位向量k旋转角度，而不是使用一个连续的旋转，即从基坐标系的三个基本旋转矩阵的乘积公式(22)计算出来。由于这种连续旋转变换的矩阵乘法一般是不可交换的，因此此旋转方法在机器人路径规划应用中更加直观、自然、简单有效。
将一个单位向量k=k1
k2k3投影到给定坐标系中进行旋转，显然，‖k‖2=k21+k22+k23=1。给出其相应的斜对称矩阵为

S(k)=K=0－k3k2
k30－k1
－k2k10

显然，对这个3×3斜对称矩阵来说：
1 K是斜对称矩阵，它的迹tr(K)=0；
23
2 K2是对称矩阵，它的迹tr(K2)=-2；

3 K3=-K。

基于以上介绍的单位斜对称矩阵K特有的性质，提出了一个实用的旋转表达方法，称为k过程。首先，令k∈瘙綆3为3×1向量。将它的斜对称矩阵K∈so(3)代入指数映射公式(26)中得到：
exp(K)=R∈SO(3)(27)
指数映射结果是旋转矩阵，它应该等同于绕k轴旋转角度。
将式(27)左边按照泰勒级数展开，结合上面提到的K的性质，得到：

exp(K)=I+K+22!K2+33!K3+44!K4+…

=I+-33!+…K+22!-44!+…K2

=I+sinK+(1-cos)K2=R
(28)
应用著名的泰勒展开式：
sin=－33!+55!－…, cos=1－22!+44!－…
假如给定坐标系绕单位向量k旋转角度，式(28)通常称为罗德里格斯公式，它提供了一种正向求解等效旋转矩阵R的方法。
为了求逆解，首先，计算式(28)的迹，即
tr(R)=3－2(1－cos)=1+2cos
那么
=arccostr(R)-12(29)
因为
RT=I－sinK+(1－cos)K2(210)
将式(28)减去式(210)得
R－RT=2sinK
最后有
K=R-RT2sin(211)

方程(29)和方程(211)提供了递归求逆算法，可以求解给定旋转矩阵R的向量k和旋转角度。乍一看，此公式没有唯一的符号。然而，对于每个余弦值都存在正负角度，假如公式(29)选择+(或-)，那么方程(211)将相对应地得到k(或-k)。因此，可以选择一对可能的和k或者-和-k，且任何一对都是正确的。
例如，如果要使一个坐标系绕轴(121)T自身旋转60°(π/3),则单位坐标轴和其斜对称矩阵为

k=161
2
1,K=160－12
10－1
－210
因此，等效旋转矩阵为
R=I+sin60°K+(1－cos60°)K2=05833-0186907904

0520208333-01869

-062380520205833

通过逆运算，可以应用上面的旋转矩阵R的逆解公式来确定角度和轴线k。根据方程(29)，由于tr(R)=2，=arccos(1/2)=60°，则有sin=3/2。那么，根据方程(211)，有
K＝R-RT2sin=0-0408208165
040820-04082

-08165040820
(212)

因此，单位向量为k=(040820816504082)T。显然，由于余弦函数是偶函数，可以取旋转角度为-60°。在这种情况下，轴线k相应变为负值。这两种选择显然是等价的，因为初始坐标系绕轴线旋转角度与绕相反的轴旋转负角度在三维空间中有同样的终点。
第二个实例是建立一个新的坐标系1，使得它的z1轴在基坐标系0下沿向量z10=(1-1-1)T。虽然可能有不同方法来建立坐标系1的方向矩阵R10，但我们只是尝试用k过程。将基坐标系z0=(001)T轴和新坐标系z1轴的公共法向量通过叉积确定为

b=z10×z0=01－1
－10－1
1100
0
1=－1
－1
0
然后，选择单位向量b作为旋转轴k:

k=b‖b‖=12－1
－1
0， K=1200－1
001
1－10
为了求出基坐标z0轴相对于新坐标系z1轴绕k旋转的角度，可使用点积：
zT0·z10=-1=‖z0‖‖z10‖cos=3cos
那么
cos=-13, sin=1－cos2=23

注意到cos＜0，可见在第二象限，从而sin＞0。因此，可求得新坐标系的方向矩阵

R10=I+sinK+(1－cos)K2=I+23K+1+13K2

=0211307887－05774
078870211305774
05774－05774－05774

此外，如果R是对称的，方程(211)将导致一个00不确定的极限。这意味着坐标系旋转角度如下：要么R=I，旋转0°；要么R≠I且RT=R，旋转180°。在第一种情况下R=I，由方程(29)计算=arccos(1)=0，可以略过公式(211)以避免出现奇异点和零点。然而，在第二种情况下，=arccos(-1)=180°，不能直接由方程(211)确定K。在这个特例中，sin=0，cos=-1。根据前面的方程(28)，R=I+sinK+(1-cos)K2=I+2K2，或者
K2=12(R-I)（213）

因此，当R=RT，R ≠I时，必须使用这个公式来确定K。
例如，如果

R=0－10

－100

00－1
26
R是对称矩阵，但R≠I，则tr(R)=-1，=180°。现在，令
K=0－k3k2
k30－k1
－k2k10
那么
K2=-k23－k22k1k2k1k3
k1k2－k23－k21k2k3
k1k3k2k3－k21－k22
因为
R－I=－1－10
－1－10
00－2
将上式与K2相比，根据式(213)我们首先得到非对角元素值
k1k2=－12,k1k3=0，k2k3=0
这意味着k3=0。然后，得到对角元素值
k23+k22=k22=12,k23+k21=k21=12

这样，无论是k1=2/2，k2=－2/2还是k1=－2/2，k2=2/2，都与=±180°相对应，其旋转结果相同。可以任取一组这样的单位向量

k=2/2
－2/2
0，=180°

因此，仅在RT=R但R≠I的情况下，可以使用上面的方法来确定k，以及设置=180°。这样的情况在第3章中将会再次提到。
如果方程(28)两边都右乘单位向量k，很容易得到
Rk=k+sinKk+(1－cos)K2k=k
因为Kk=k×k≡0。这个结果显示单位向量k是k过程中特征值为1的旋转矩阵R的特征向量。因此，如果在MATLAB中进行k过程反解数值编程，对于给定的旋转矩阵R，可以利用内部求特征值

函数特征向量eig(R)而不需要通过方程(211)确定K，以避免可能产生的奇异点。在先前的数值实例中，

可以清楚地看到，一旦内部函数eig(R)被调用，元素“val”沿着输出矩阵的对角线打印出三个特征值，而“vec”打印输入矩阵R的三列特征向量。显然，因为最后一个对角元素“val”是10000，其对应的特征向量是“vec”的第三列，即(040820816504082)T，得到的结果和公式(212)是一致的。如果令R=I，则单位矩阵在MATLAB工作窗口中输出如下结果：

这一结果表明特征值+1对应的单位特征向量为k=(-07071070710)T。从上面的例子中得到这个向量为负值，但它并不影响最终旋转结果，因为在这种特殊情况下，旋转角度可以选择±180°。
总之，基于数学上的完美指数映射在公式(27)中的so(3)和SO(3)，研究了式(28)中简洁的旋转矩阵R∈SO(3)的正向和逆向方程，以及式(29)和式(211)中的k过程。后面将会发现，k过程在机器人运动学、数字人运动规划尤其是路径定向规划方面是非常有用的。