机器人学中的数学基础

Published: by Creative Commons Licence

关于符号表示: 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 正交,且满足条件

ABR1=ABRT;|ABR|=1

因为其正交性质 (1.3) ,我们有

BAR=ABR1=ABRT

对于轴 x,y,z 作转角为 θ 的旋转变换,其旋转矩阵分别为:

R(x,θ)=[1000cθsθ0sθcθ],R(y,θ)=[cθ0sθ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×an 称为法向矢量(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+ApBO

3. 齐次坐标变换

【齐次变换形式】式子 (2.3) 对于点 Bp 而言非齐次,若要齐次,做如下变形:

[Ap1]=[ABRApBO01][Bp1]

其中,4×1 的列矢量表示三位空间的点,称为点的齐次坐标,仍然记为齐次坐标,仍然记为 ApBp。于是上式可写为矩阵形式

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 轴的方向。

点积和交积

两矢量的点积(标量):

ab=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θ0sθ00100sθ0cθ00001],Rot(y,θ)=[cθsθ00sθcθ0000100001]

变换矩阵左乘和右乘的解释

矩阵的乘法不具有交换性质,即 ABBA

  • 变换顺序“从右向左”,指明运动是相对固定坐标系而言;
  • 变换顺序“从左向右”,指明运动是相对运动坐标系而言。

4. 物体的变换及逆变换

物体位置描述

如果我们想对一个坐标系内的多个点进行变换,我们可以将这多个点的点坐标作为列坐标组合得到一个矩阵,然后可以“快乐搞批发”。

[Ap1,Ap2,...,Apn]=ABT[Bp1,Bp2,...,Bpn]

复合变换

ACT=ABTBCT=[ABRApBO01][BCRApCO01]=[ABRBCRABRBpCO+ApBO01]

齐次变换的逆变换

方法有二,一种是直接对 4×4 的齐次变换矩阵 ABT 求逆;另一种是利用齐次变换矩阵的特点,简化矩阵求逆运算。

正交法

对于

ABT=[ABRApBO01]

我们有

BAT=ABT1=[ABRTABRTApBO01]

证明如下:

直接法

T=[nxoxaxpxnyoyaypynzozazpz0001]

则其逆变换为

T1=[nxnynzpnoxoyozpoaxayazpa0001]

5. 通用旋转变换

研究某个绕着从原点出发的任一矢量(轴)f 旋转 θ 角时的旋转矩阵。

通用旋转变换公式

通过 fθ 去求旋转变换矩阵。

假设 f 为坐标系 Cz 轴上的单位矢量,即

C=[nxoxax0nyoyay0nzozaz00001]f=axi+ayj+azk

于是绕矢量 f 旋转相当于绕坐标系 Cz 轴旋转,即有

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θ=(ozay)2+(axnz)2+(nyox)2nx+oy+az1

等效转轴

f=(fx,fy,fz),while{fx=(ozay)/2sθfy=(axnz)/2sθfz=(nyox)/2sθ

6. 欧拉角与RPY角

绕固定轴 xyz 轴旋转(RPY角)

  • Roll:绕z轴的旋转(α角)
  • Pitch: 绕y轴的旋转(β角)
  • Yaw: 绕x轴的旋转(γ角)

描述坐标系 B 的法则B 的初始方位与参考系 A 重合。首先将 BzAγ 角,再绕 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)

zyx 欧拉角

描述坐标系 B 的法则B 的初始方位与参考系 A 相同。首先使 BzBα 角,再绕 yBβ 角,最后绕 xBγ 角。

这种描述方法中,各次转动都是相对于运动坐标系的某轴进行的,而不是相对于固定的参考系 A。这样的三次转动称为欧拉角,有因转动的顺序是绕z轴、y轴和x轴,故这种描述法为 zyx 欧拉角。

求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)

为什么相同的原因:绕固定轴旋转的顺序若与绕运动轴旋转的顺序相反,且旋转的角度也对应相等时,所得到的变换矩阵是相同的。因此,zyx 欧拉角与固定轴 xyz 转角描述坐标系 B 是完全等价的。

zyz欧拉角

描述坐标系 B 的法则B 的初始方位与参考系 A 重合。首先使 BzBα 角,再绕 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). 机器人技术基础. 华中科技大学出版社.