机器人学中的数学基础
关于符号表示: sθ=sinθ,cθ=cosθ
1. 位置和姿态的表示
【位置矢量】对于直角坐标系A,空间任一点p的位置可用3×1的列矢量 Ap 表示:
Ap=[pxpypz]【旋转矩阵】对于物体的方位(orientation),我们可由某个固接于此物体的坐标系描述。为了规定空间某刚体 B 的方位,设置一直角坐标系 B 与此刚体固接。用坐标系 B 的三个单位主矢量 xB,yB,zB 相对于参考坐标系 A 的方向余弦组成的 3×3 矩阵:
ABR=[AxBAyBAzB]=[r11r12r13r21r22r23r31r32r33]来表示刚体 B 相对于坐标系 A 的方位,其中 ABR 称为旋转矩阵。
上标 A 表示代表参考坐标系 A ,下标 B 表示被描述的坐标系 B 。其中,ABR 的三个列矢量 AxB,AyB,AzB 都是单位矢量且两两垂直。
旋转矩阵 ABR 正交,且满足条件
ABR−1=ABRT;|ABR|=1因为其正交性质 (1.3) ,我们有
BAR=ABR−1=ABRT对于轴 x,y,z 作转角为 θ 的旋转变换,其旋转矩阵分别为:
R(x,θ)=[1000cθ−sθ0sθcθ],R(y,θ)=[cθ0−sθ010sθ0cθ],R(z,θ)=[cθ−sθ0sθcθ0001]【位姿描述】首先选取 B 的坐标原点(一般选择物体 B 的特征点,如质心上)。相对参考系 A ,刚体 B 的位姿由坐标系 B 来表述(其中包括刚体 B 特征点的位置矢量 ApBO 和旋转矩阵 ABR ),即有
{B}={ABRApBO}【手爪坐标系】为了描述手爪的位置和姿态(位姿),规定一坐标系与手抓固结,称手爪坐标系。其 z 轴设在手指接近物体的方向,称接近矢量 a (approach);y 轴设在两手指的联线方向,称为方位矢量 o (orientation);x 轴由右手法则确定:n=o×a,n 称为法向矢量(normal)。如此,手爪的方位由旋转矩阵 R 所规定,即
R=[n,o,a]三个单位正交矢量 n,o,a 描述了手爪的姿态。而手爪的位置由位置矢量 p 规定,它代表手爪坐标系的原点。因此,手爪的位姿由四个矢量 n,o,a,p 来描述,记为
{T}={n,o,a,p}2. 坐标变换
坐标平移方程
Ap=Bp+ApBO坐标旋转方程
Ap=ABRBp一般情况(平移和旋转的复合)
Ap=ABRBp+ApBO3. 齐次坐标变换
【齐次变换形式】式子 (2.3) 对于点 Bp 而言非齐次,若要齐次,做如下变形:
[Ap1]=[ABRApBO01][Bp1]其中,4×1 的列矢量表示三位空间的点,称为点的齐次坐标,仍然记为齐次坐标,仍然记为 Ap 或 Bp。于是上式可写为矩阵形式
Ap=ABTBp,whileABT=[ABRApBO01]=[nxoxaxpxnyoyaypynzozazpz0001]空间某点 p 的直角坐标描述和齐次坐标描述分别为:
p=[xyz],p=[xyz1]=[ωxωyωzω]其中,ω为非零常数,为【坐标比例系数】。
坐标原点的矢量,即零矢量表示为 [0,0,0,1]T。矢量 [0,0,0,0]T 没有定义。具有形如 [a,b,c,0] 的矢量表示无限远矢量(无穷远点),用来表示方向,我们把包括无穷远点的空间称为扩大空间,而把第4个元素非零的点称为非无穷远点。[1,0,0,0],[0,1,0,0],[0,0,1,0] 分别表示 x,y,z 轴的方向。
点积和交积
两矢量的点积(标量):
a⋅b=axbx+ayby+azbz交积(矢量):
a×b=|ijkaxayazbxbybz|平移齐次坐标系
Trans(a,b,c)=[100a010b001c0001]非零常数乘以变换矩阵的每个元素,不改变该变换矩阵的特性。
旋转齐次坐标变换
改造 (1.5),可以得到对应于轴 x,y,z 作转角为 θ 的旋转变换,分别可得
Rot(x,θ)=[10000cθ−sθ00sθcθ00001],Rot(y,θ)=[cθ0−sθ00100sθ0cθ00001],Rot(y,θ)=[cθ−sθ00sθcθ0000100001]变换矩阵左乘和右乘的解释
矩阵的乘法不具有交换性质,即 AB≠BA
- 变换顺序“从右向左”,指明运动是相对固定坐标系而言;
- 变换顺序“从左向右”,指明运动是相对运动坐标系而言。
4. 物体的变换及逆变换
物体位置描述
如果我们想对一个坐标系内的多个点进行变换,我们可以将这多个点的点坐标作为列坐标组合得到一个矩阵,然后可以“快乐搞批发”。
[Ap1,Ap2,...,Apn]=ABT[Bp1,Bp2,...,Bpn]复合变换
ACT=ABTBCT=[ABRApBO01][BCRApCO01]=[ABRBCRABRBpCO+ApBO01]齐次变换的逆变换
方法有二,一种是直接对 4×4 的齐次变换矩阵 ABT 求逆;另一种是利用齐次变换矩阵的特点,简化矩阵求逆运算。
正交法
对于
ABT=[ABRApBO01]我们有
BAT=ABT−1=[ABRT−ABRTApBO01]证明如下:
直接法
若
T=[nxoxaxpxnyoyaypynzozazpz0001]则其逆变换为
T−1=[nxnynz−p⋅noxoyoz−p⋅oaxayaz−p⋅a0001]5. 通用旋转变换
研究某个绕着从原点出发的任一矢量(轴)f 旋转 θ 角时的旋转矩阵。
通用旋转变换公式
通过 f 和 θ 去求旋转变换矩阵。
假设 f 为坐标系 C 的 z 轴上的单位矢量,即
C=[nxoxax0nyoyay0nzozaz00001]f=axi+ayj+azk于是绕矢量 f 旋转相当于绕坐标系 C 的 z 轴旋转,即有
Rot(f,θ)=Rot(cz,θ)而通过推导和化简,可以得到
Rot(f,θ)=[fxfxversθ+cθfyfxversθ−fzsθfzfxversθ+fysθ0fxfyversθ+fzsθfyfyversθ+cθfzfyversθ−fxsθ0fxfzversθ+fzsθfyfzversθ+fxsθfzfzversθ+cθ00001]等效转角与转轴
通过旋转变换矩阵求解其等效的转轴与等效转角,是上一小部分的反向问题。
由任一旋转变换,能够通过式 (5.2) 求得等效旋转角 θ 的转轴。
对于旋转变换:
R=[nxoxax0nyoyay0nzozaz00001]=[fxfxversθ+cθfyfxversθ−fzsθfzfxversθ+fysθ0fxfyversθ+fzsθfyfyversθ+cθfzfyversθ−fxsθ0fxfzversθ+fzsθfyfzversθ+fxsθfzfzversθ+cθ00001]等效转角
tanθ=sθcθ=√(oz−ay)2+(ax−nz)2+(ny−ox)2nx+oy+az−1等效转轴
f=(fx,fy,fz),while{fx=(oz−ay)/2sθfy=(ax−nz)/2sθfz=(ny−ox)/2sθ6. 欧拉角与RPY角
绕固定轴 x−y−z 轴旋转(RPY角)
- Roll:绕z轴的旋转(α角)
- Pitch: 绕y轴的旋转(β角)
- Yaw: 绕x轴的旋转(γ角)
【描述坐标系 B 的法则】B 的初始方位与参考系 A 重合。首先将 B 绕 zA 转 γ 角,再绕 yA 转 β 角,最后绕 zA 转 α 角。
求旋转矩阵
因为三次旋转都是相对于固定坐标系 \(\{A\}\) 而言,所以按照“从右向左”原则,得到相应的旋转矩阵
ABRxyz(γ,β,α)=R(zA,α)R(yA,β)R(xA,γ)最后可得
ABRxyz(γ,β,α)=[cα cβcα sβ sγ−sα cγcα sβ cγ+sα sγsα cβsα sβ sγ+cα cγsα sβ cγ−cα sγ−sβcβ sγcβ cγ]RPY角反解
对于旋转矩阵
ABRxyz(γ,β,α)=[r11r12r13r21r22r23r31r32r33]由式 (6.2) 和 (6.3),我们可以求得
cosβ=√r211+r221如果 cosβ≠0,
{β=Atan2(−r31,√r211+r221)α=Atan2(r21.r11)γ=Atan2(r32,r33)关于 Atan2(y,z) 见附录。
如果 β=90∘,
{β=90∘α=0γ=Atan2(r12,r22)如果 β=−90∘,
{β=−90∘α=0γ=−Atan2(r12,r22)z−y−x 欧拉角
【描述坐标系 B 的法则】B 的初始方位与参考系 A 相同。首先使 B 绕 zB 转 α 角,再绕 yB 转 β 角,最后绕 xB 转 γ 角。
这种描述方法中,各次转动都是相对于运动坐标系的某轴进行的,而不是相对于固定的参考系 A。这样的三次转动称为欧拉角,有因转动的顺序是绕z轴、y轴和x轴,故这种描述法为 z−y−x 欧拉角。
求zyx欧拉角
另外,因为“各次转动都是相对于运动坐标系的某轴进行的”,所以根据“从左向右”的原则来安排各次旋转对应的矩阵:
ABRzyx(α,β,γ)=R(z,α)R(y,β)R(x,γ)矩阵相乘得到
ABRzyx(α,β,γ)=[cα cβcα sβ sγ−sα cγcα sβ cγ+sα sγsα cβsα sβ sγ+cα cγsα sβ cγ−cα sγ−sβcβ sγcβ cγ]反解zyx欧拉角
我们可以发现式 (6.5) 与式 (6.2) 完全相同,故而角度 α,β,γ 的求解也可以使用式 (6.4) 。
为什么相同的原因:绕固定轴旋转的顺序若与绕运动轴旋转的顺序相反,且旋转的角度也对应相等时,所得到的变换矩阵是相同的。因此,z−y−x 欧拉角与固定轴 x−y−z 转角描述坐标系 B 是完全等价的。
z−y−z欧拉角
【描述坐标系 B 的法则】B 的初始方位与参考系 A 重合。首先使 B 绕 zB 转 α 角,再绕 yB 转 β 角,最后绕 zB 转 γ 角。
求zyz欧拉角的旋转矩阵
同样,因为转动相对于运动坐标系而言,所以使用“从左向右”原则,去求旋转矩阵:
ABRzyx(α,β,γ)=R(z,α)R(y,β)R(z,γ)=[cα cβ cγ−sα sγ−cα sβ sγ−sα cγcα sβsα cβ cγ+cα sγ−sα cβ sγ+cα cγsα sβ−sβ cγsβ sγcβ]反解zyz欧拉角
令旋转矩阵如下
ABRzyz(α,β,γ)=[r11r12r13r21r22r23r31r32r33]如果 sinβ≠0,则
{β=Atan2(√r231+r232,r33)α=Atan2(r23,r13)γ=Atan2(r32,−r31)如果 β=0∘,
{β=0α=0γ=Atan2(−r12,r11)如果 β=180∘,
{β=180∘α=0γ=Atan2(r12,−r11)附录

参考
[1] 蔡自兴. (2000). 机器人学 (1st ed.). 清华大学出版社.
[2] 熊有伦. (1996). 机器人技术基础. 华中科技大学出版社.
Related Issues not found
Please contact @AuthurWhywait to initialize the comment