控制的一大类对象为以机器人为代表的运动体控制,相应地,我们会比较关心三维空间中被控对象的运动状态,即位置、速度和姿态,对位置和速度的描述需要首先确定坐标系,即是在什么坐标系下的位置和速度,而姿态则描述坐标系与坐标系之间的关系。本部分的主要参考文献为[1]和[2]。
我们通常采用右手坐标系描述运动量,设三维空间中有两个原点重合的坐标系:s系{x^s,y^s,z^s}和b系{x^b,y^b,z^b},x^s,y^s,z^s和x^b,y^b,z^b分别为s系和b系下相互正交的三个单位矢量(默认情形下这里的矢量取列矢量形式),且分别与s系和b系下的三个坐标轴正方向平行,对于空间中的任意一个矢量r,可以投影到任意坐标系下进行表示,设其在s系下的三个分量为r1s,r2s,r3s,在b系下的三个分量为r1b,r2b,r3b,则有
r=r1sx^s+r2sy^s+r3sz^s=r1bx^b+r2by^b+r3bz^b(1)
首先以s系为基准坐标系,对式(1)后半部分的左右两端依次和x^s,y^s,z^s点积,可得
⎩⎨⎧r1s=r1bx^b⋅x^s+r2by^b⋅x^s+r3bz^b⋅x^sr2s=r1bx^b⋅y^s+r2by^b⋅y^s+r3bz^b⋅y^sr3s=r1bx^b⋅z^s+r2by^b⋅z^s+r3bz^b⋅z^s(2)
令rs=[r1s,r2s,r3s]T和rb=[r1b,r2b,r3b]T分别表示矢量r在与s系和b系下的投影,式(2)可进一步表示为
rs=⎣⎡r1sr2sr3s⎦⎤=⎣⎡x^b⋅x^sx^b⋅y^sx^b⋅z^sy^b⋅x^sy^b⋅y^sy^b⋅z^sz^b⋅x^sz^b⋅y^sz^b⋅z^s⎦⎤⎣⎡r1br2br3b⎦⎤=Cbsrb(3)
式中,Cbn为从b系到s系的旋转矩阵。
然后以b系为基准坐标系,对式(1)后半部分的左右两端依次和x^b,y^b,z^b点积,可得
⎩⎨⎧r1b=r1sx^s⋅x^b+r2sy^s⋅x^b+r3sz^s⋅x^br2b=r1sx^s⋅y^b+r2sy^s⋅y^b+r3sz^s⋅y^br3b=r1sx^s⋅z^b+r2sy^s⋅z^b+r3sz^s⋅z^b(4)
式(4)可进一步表示为
rb=⎣⎡r1br2br3b⎦⎤=⎣⎡x^s⋅x^bx^s⋅y^bx^s⋅z^by^s⋅x^by^s⋅y^by^s⋅z^bz^s⋅x^bz^s⋅y^bz^s⋅z^b⎦⎤⎣⎡r1sr2sr3s⎦⎤=(Cbs)Trs=Csbrs(5)
式中,Csb=(Cbs)T为从s系到b系的旋转矩阵。
考虑到x^b,y^b,z^b实际上也是三个矢量,因此它们也可以利用s系{x^s,y^s,z^s}进行表示,即有
⎩⎨⎧x^b=l1x^s+m1y^s+n1z^sy^b=l2x^s+m2y^s+n2z^sz^b=l3x^s+m3y^s+n3z^s(6)
则式(3)和(5)可进一步表示为
rs=⎣⎡r1sr2sr3s⎦⎤=⎣⎡l1m1n1l2m2n2l3m3n3⎦⎤⎣⎡r1br2br3b⎦⎤=[x^b y^b z^b]rb=Cbsrb(7)
rb=[x^b y^b z^b]Trs=⎣⎡l1l2l3m1m2m3n1n2n3⎦⎤rs(8)
另一方面,考虑到x^s,y^s,z^s实际上也是三个矢量,因此它们也可以利用b系{x^b,y^b,z^b}进行表示,且由式(8)可知
⎩⎨⎧x^s=l1x^b+l2y^b+l3z^by^s=m1x^b+m2y^b+m3z^bz^s=n1x^b+n2y^b+n3z^b(9)
由于x^s,y^s,z^s和x^b,y^b,z^b均为相互正交的三个单位矢量,因此有
l12+m12+n12l22+m22+n22l32+m32+n32l12+l22+l32m12+m22+m32n12+n22+n32=1,=1,=1,=1,=1,=1,l1l2+m1m2+n1n2l1l3+m1m3+n1n3l2l3+m2m3+n2n3l1m1+l2m2+l3m3l1n1+l2n2+l3n3m1n1+m2n2+m3n3=0=0=0=0=0=0(10)
由式(10)可知
===Cbs(Cbs)T⎣⎡l1m1n1l2m2n2l3m3n3⎦⎤⎣⎡l1l2l3m1m2m3n1n2n3⎦⎤⎣⎡l12+l22+l32l1m1+l2m2+l3m3l1n1+l2n2+l3n3l1m1+l2m2+l3m3m12+m22+m32m1n1+m2n2+m3n3l1n1+l2n2+l3n3m1n1+m2n2+m3n3n12+n22+n32⎦⎤I(11)
===(Cbs)TCbs⎣⎡l1l2l3m1m2m3n1n2n3⎦⎤⎣⎡l1m1n1l2m2n2l3m3n3⎦⎤⎣⎡l12+m12+n12l1l2+m1m2+n1n2l1l3+m1m3+n1n3l1l2+m1m2+n1n2l22+m22+n22l2l3+m2m3+n2n3l1l3+m1m3+n1n3l2l3+m2m3+n2n3l32+m32+n32⎦⎤I(12)
因此有
(Cbs)T=(Cbs)−1(13)
即Cbs为正交矩阵。进一步考虑到矩阵行列式性质(矩阵乘积的行列式等于矩阵行列式的乘积;矩阵转置后的行列式不变),可知det(Cbs)=±1,因此有
l1(m2n3−m3n2)−l2(m1n3−m3n1)+l3(m1n2−m2n1)=±1(14)
进一步考虑到x^b,y^b,z^b均为相互正交的三个单位矢量,相互之间的矢量叉积满足
x^b×y^b=z^b,y^b×z^b=x^b,z^b×x^b=y^b(15)
结合式(9)和(15)可得
∣∣∣∣∣∣x^bl1m1y^bl2m2z^bl3m3∣∣∣∣∣∣∣∣∣∣∣∣x^bm1n1y^bm2n2z^bm3n3∣∣∣∣∣∣∣∣∣∣∣∣x^bn1l1y^bn2l2z^bn3l3∣∣∣∣∣∣=n1x^b+n2y^b+n3z^b=l1x^b+l2y^b+l3z^b=m1x^b+m2y^b+m3z^b(16)
对式(16)进一步整理可得
l1m1n1=m2n3−m3n2,=n2l3−n3l2,=l2m3−l3m2,l2m2n2=m3n1−m1n3,=n3l1−n1l3,=l3m1−l1m3,l3m3n3=m1n2−m2n1=n1l2−n2l1=l1m2−l2m1(17)
结合式(10)与式(17)可得
l1(m2n3−m3n2)−l2(m1n3−m3n1)+l3(m1n2−m2n1)=1(18)
即det(Cbs)=1。
我们以s系为基准系,进一步考虑旋转矩阵Csb,根据式(7)和(8)有Csb=[x^b y^b z^b]T。设b1系为s系绕z^s轴正向旋转θ角,如图1所示。
图1 s系绕z^s轴正向旋转θ角
则有
⎩⎨⎧x^b1=cos(θ)x^s+sin(θ)y^sy^b1=−sin(θ)x^s+cos(θ)y^sz^b1=z^s(19)
因此
Csb1=[x^b1 y^b1 z^b1]T=⎣⎡cos(θ)−sin(θ)0sin(θ)cos(θ)0001⎦⎤(20)
我们将式(20)中的旋转矩阵记为R(z^,θ)(为简便起见,后续我们将x^s,y^s,z^s简写为x^,y^,z^,在不至于引起混淆的前提下,根据需要也会将x^,y^,z^视为不同坐标系的轴向的单位矢量),类似可得
R(x^,θ)=⎣⎡1000cos(θ)−sin(θ)0sin(θ)cos(θ)⎦⎤, R(y^,θ)=⎣⎡cos(θ)0sin(θ)010−sin(θ)0cos(θ)⎦⎤(21)
注意从y^轴正向看x^z^平面时,根据右手坐标系的关系可知x^轴水平朝左,z^轴水平朝上,逆时针旋转θ角后x^轴跑到水平面下方了,因此R(y^,θ)中sin(θ)与−sin(θ)的位置正好与R(x^,θ)和R(z^,θ)对称。对于R(z^,θ),有
R(z^,θ)x^R(z^,θ)y^R(z^,θ)z^=cos(θ)x^−sin(θ)y^=cos(θ)x^−sin(θ)z^×x^,=cos(θ)y^+sin(θ)x^=cos(θ)y^−sin(θ)z^×y^=z^(22)
对R(x^,θ)和R(y^,θ)也有类似性质。一般地,设n^为任意单位矢量,v⊥为任意矢量v在与n^垂直平面上的投影,v∥为矢量v在n^方向上的投影,R(n^,θ)表示绕n^正向旋转θ角对应的旋转矩阵,则有
R(n^,θ)v=v∥+cos(θ)v⊥−sin(θ)n^×v⊥=v∥+cos(θ)v⊥+sin(θ)[[n^]]v⊥(23)
其中[[n^]]为与n^对应的反对称阵,即设n^=[n1,n2,n3]T,则有
[[n^]]=⎣⎡0−n3n2n30−n1−n2n10⎦⎤(24)
对任意两个矢量a和b,由式(24)中的定义可知a×b=−[[a]]b,有的文献也定义−[[a]]为与a对应的反对称阵,这个看个人习惯。
考虑矢量三重叉积公式
a×(b×c)=(a⋅c)b−(a⋅b)c(25)
在式(25)中取a=b=n^,c=v,则有
v=(n^⋅v)n^−n^×(n^×v)=n^n^Tv−[[n^]]2v=v∥+v⊥(26)
结合式(23)和(26)可知
R(n^,θ)v=n^n^Tv−cos(θ)[[n^]]2v−sin(θ)[[n^]]3v=n^n^Tv−cos(θ)[[n^]]2v+sin(θ)[[n^]]v(27)
式(27)中用到了反对称阵的性质[[n^]]3=−∥n^∥2[[n^]]=−[[n^]]。由于v为任意矢量,因此有
R(n^,θ)=n^n^T−cos(θ)[[n^]]2+sin(θ)[[n^]]=cos(θ)I+(1−cos(θ))n^n^T+sin(θ)[[n^]](28)
式(28)中用到了反对称阵的性质[[a]][[b]]=−(a⋅b)I+baT,取a=b=n^可得[[n^]]2=−I+n^n^T,式(28)也可进一步写为
R(n^,θ)=I+sin(θ)[[n^]]+(1−cos(θ))[[n^]]2(29)
将式(29)写成分量形式,可得
=R(n^,θ)⎣⎡cos(θ)+n12(1−cos(θ))n2n1(1−cos(θ))−n3sin(θ)n3n1(1−cos(θ))+n2sin(θ)n1n2(1−cos(θ))+n3sin(θ)cos(θ)+n22(1−cos(θ))n3n2(1−cos(θ))−n1sin(θ)n1n3(1−cos(θ))−n2sin(θ)n2n3(1−cos(θ))+n1sin(θ)cos(θ)+n32(1−cos(θ))⎦⎤(30)
若已知R(n^,θ),则根据式(30)可知
cosθ=21(trR−1)(31)
当sin(θ)=0时,有
n^=2sin(θ)1⎣⎡R23−R32R31−R13R12−R21⎦⎤(32)
当θ=0时,n^没有物理意义;当θ=π时,R(n^,π)=−I+2n^n^T,此时R(n^,π)+I的三个列均与n^平行,而n^的方向则不影响结果。
至此,我们获得了旋转矩阵的一些初步性质。
参考文献
[1]Battin RH. An introduction to the mathematics and methods of astrodynamics, revised edition. American Institute of Aeronautics and Astronautics; 1999.
[2]Shuster MD. A survey of attitude representations. Navigation. 1993;8(9):439-517.