機器人學（一）

剛體運動狀態描述

林沛群
國立台灣大學
機械工程學系
 導讀 -1

 一個剛體(Rigid body)的狀態該如何描述？
 平面：

移動 2 DOFs、轉動 1 DOF
Degree of freedom

{𝑊} world frame

Y

X
 導讀 -2

 一個剛體(Rigid body)的狀態該如何描述？
 平面：

移動 2 DOFs、轉動 1 DOF
Degree of freedom
 空間：

移動 3 DOFs、轉動 3 DOFs

{𝑊} world frame

Y

Z
 導讀 -3

 該如何整合表達剛體的狀態？

在剛體(Rigid body)上建立frame，常建立在質心上
 移動：由body frame 的原點位置判定

 轉動：由body frame的姿態判定

{𝑊} world frame 𝑍𝐵 {𝐵} body frame

Y 𝑌𝐵

Z 𝑋𝐵
 導讀 -4

 一個剛體(Rigid body)的「運動」狀態該如何描述？
 利用各個DOF的微分，將位移和姿態（displacement / orientation）
轉換到速度（velocity）和加速度（acceleration）等運動狀態

{𝑊} world frame 𝑍𝐵 {𝐵} body frame

Y 𝑌𝐵

Z 𝑋𝐵
 移動 -1

 移動：以向量（vector）𝑃 來描述{𝐵}的原點相對於{𝐴}的狀
態 𝑍𝐵
{𝑊} world frame {𝐵} body frame
Y 𝑃𝑥 𝑌𝐵
𝑃 = 𝑃𝑦
𝑃𝑧

X
𝑃𝑦 = 3
Z 𝑋𝐵 𝑃𝑧 = 3
𝑃𝑥 = 10

𝑃𝑥 10
 Ex:
𝑃 = 𝑃𝑦 = 3
𝑃𝑧 3
 移動 -2

 向量可表達空間關係的兩個方式
 A position in space (i.e., position vector)

如同前一頁內容，以此方式描述body frame原點
𝑃𝑥
𝑃 = 𝑃𝑦 = 𝐴𝑃𝐵 𝑜𝑟𝑔 = origin of 𝐵 represented in 𝐴
𝑃𝑧
 A vector (i.e., displacement, frame basis)

以此方式表達body frame上principal axes的方向

𝐵 : 𝑋෠𝐵 , 𝑌෠𝐵 , 𝑍መ𝐵 represented in 𝐴 : 𝑋෠𝐴 , 𝑌෠𝐴 , 𝑍መ𝐴 𝑍𝐵 𝑌𝐵

Y
𝑃 = 𝐴𝑃𝐵 𝑜𝑟𝑔 {𝐵}
{𝐴}
X 𝑋𝐵
Z
 轉動 -1

 轉動：描述{𝐵}相對於{𝐴}之姿態---Rotation Matrix

| | |
𝐴 𝐴෠ 𝐴෠ 𝐴መ
𝐵𝑅 = 𝑋𝐵 𝑌𝐵 𝑍𝐵 R的三個columns即為frame 𝐵
| | | 的𝑏𝑎𝑠𝑖𝑠: 𝑋෠𝐵 , 𝑌෠𝐵 , 𝑍መ𝐵 （由 𝐴 看）
B relative
“column vector” 𝑍መ𝐵
to A
{𝐵} 𝑌෠𝐵
𝑋෠𝐵 ∙ 𝑋෠𝐴 𝑌෠𝐵 ∙ 𝑋෠𝐴 𝑍መ𝐵 ∙ 𝑋෠𝐴
= 𝑋෠𝐵 ∙ 𝑌෠𝐴 𝑌෠𝐵 ∙ 𝑌෠𝐴 𝑍መ𝐵 ∙ 𝑌෠𝐴
𝑋෠𝐵 ∙ 𝑍መ𝐴 𝑌෠𝐵 ∙ 𝑍መ𝐴 𝑍መ𝐵 ∙ 𝑍መ𝐴
“direct cosines” {𝐴}

𝑋෠𝐵
 轉動 -2

 Ex: {𝐵}相對於{𝐴}之姿態 𝐵𝐴𝑅 =?

藍虛線: World Frame {𝐴}
紅實線: Body Frame {𝐵}

0
{𝐵}的x”軸為{𝐴}的z軸反向 ⇒ 𝐴𝑋෠𝐵 = 0
−1
0
{𝐵}的y”軸與{𝐴}的y軸重疊 ⇒ 𝐴𝑌෠𝐵 = 1
0
1
𝐴መ
{𝐵}的z”軸與{𝐴}的x軸重疊 ⇒ 𝑍𝐵 = 0
0

| | | 0 0 1
因此，{𝐵}相對於{𝐴}之姿態： 𝐵𝐴𝑅 = 𝐴𝑋෠𝐵 𝐴෠
𝑌𝐵 𝐴መ
𝑍𝐵 = 0 1 0
| | | −1 0 0
 轉動 -3

 Ex: {𝐵}相對於{𝐴}之姿態 𝐵𝐴𝑅 =?

以{𝐴}為參考基準

上視圖

𝑋෠𝐵 ∙ 𝑋෠𝐴 0.866 {𝐵}相對於{𝐴}之姿態：

𝐴෠
𝑋𝐵 = 𝑋෠𝐵 ∙ 𝑌෠𝐴 = 0.5 0.866 −0.5 0
𝐴
𝑋෠𝐵 ∙ 𝑍መ𝐴 0 𝐵𝑅 = 0.5 0.866 0
0 0 1
𝑌෠𝐵 ∙ 𝑋෠𝐴 −0.5
𝐴෠
𝑌𝐵 = 𝑌෠𝐵 ∙ 𝑌෠𝐴 = 0.866
𝑌෠𝐵 ∙ 𝑍መ𝐴 0
藍虛線: World Frame {𝐴}
𝑍መ𝐵 ∙ 𝑋෠𝐴 0
紅實線: Body Frame {𝐵} 𝐴መ
𝑍𝐵 = 𝑍መ𝐵 ∙ 𝑌෠𝐴 = 0
𝑍መ𝐵 ∙ 𝑍መ𝐴 1
 轉動 -4

 In-video Quiz: {𝐵}相對於{𝐴}之姿態 𝐵𝐴𝑅 =?

藍虛線: World Frame {𝐴}
紅實線: Body Frame {𝐵}

0 −0.866 0.5
{A} A. 0 0.5 0.866
−1 0 0
投影到 0 0.866 −0.5
{𝐵} B. 0 −0.5 −0.866
XY/YZ平面 −1 0 0
0 0 −1
C. 0.866 −0.5 0
−0.5 −0.866 0
0 0 −1
D. −0.866 0.5 0
0.5 0.866 0
 轉動 -4

 In-video Quiz: {𝐵}相對於{𝐴}之姿態 𝐵𝐴𝑅 =?

藍虛線: World Frame {𝐴}
紅實線: Body Frame {𝐵}

0 −0.866 0.5
{A} A. 0 0.5 0.866
−1 0 0
投影到 0 0.866 −0.5
{𝐵} B. 0 −0.5 −0.866
XY/YZ平面 −1 0 0
0 0 −1
C. 0.866 −0.5 0
−0.5 −0.866 0
0 0 −1
D. −0.866 0.5 0
0.5 0.866 0
 Rotation Matrix -1
𝑍መ𝐴 𝑍መ𝐵 {𝐵} 𝑌෠𝐵
 特性
𝑋෠𝐵 ∙ 𝑋෠𝐴 𝑌෠𝐵 ∙ 𝑋෠𝐴 𝑍መ𝐵 ∙ 𝑋෠𝐴 | | |
𝐴 ෠ ෠ 𝐴෠ 𝐴෠ 𝐴መ
𝐵𝑅 = 𝑋𝐵 ∙ 𝑌𝐴 𝑌෠𝐵 ∙ 𝑌෠𝐴 𝑍መ𝐵 ∙ 𝑌෠𝐴 = 𝑋𝐵 𝑌𝐵 𝑍𝐵
{𝐴}
𝑋෠𝐵 ∙ 𝑍መ𝐴 𝑌෠𝐵 ∙ 𝑍መ𝐴 𝑍መ𝐵 ∙ 𝑍መ𝐴 | | |
𝑋෠𝐵
𝑌෠𝐴
𝑋෠𝐴
前後向量互換 𝑇
𝐵
𝑋෠𝐴 ∙ 𝑋෠𝐵 𝑋෠𝐴 ∙ 𝑌෠𝐵 𝑋෠𝐴 ∙ 𝑍መ𝐵 − 𝑋෠𝐴 −
= 𝑌෠𝐴 ∙ 𝑋෠𝐵 𝑌෠𝐴 ∙ 𝑌෠𝐵 𝑌෠𝐴 ∙ 𝑍መ𝐵 = − 𝐵෠ 𝑇
𝑌𝐴 −
𝑍መ𝐴 ∙ 𝑋෠𝐵 𝑍መ𝐴 ∙ 𝑌෠𝐵 𝑍መ𝐴 ∙ 𝑍መ𝐵 − 𝐵መ 𝑇
𝑍𝐴 −
𝑇
| | |
= 𝐵𝑋෠𝐴 𝐵෠
𝑌𝐴 𝐵መ
𝑍𝐴 = 𝐵𝐴𝑅𝑇
| | |

𝐴
𝐵𝑅 = 𝐵𝐴𝑅𝑇
 Rotation Matrix -2

 Ex: {𝐴}相對於{𝐵}之姿態 𝐵𝐴𝑅 =?

以{𝐵}為參考基準

上視圖

𝑋෠𝐴 ∙ 𝑋෠𝐵 {𝐴}相對於{𝐵}之姿態：

0.866
𝐵෠
𝑋𝐴 = 𝑌෠𝐴 ∙ 𝑋෠𝐵 = −0.5 0.866 0.5 0
𝐵
𝑍መ𝐴 ∙ 𝑋෠𝐵 0 𝐴𝑅 = −0.5 0.866 0 = 𝐵𝐴𝑅 𝑇
0 0 1
𝑋෠𝐴 ∙ 𝑌෠𝐵 0.5
𝐵෠
𝑌𝐴 = 𝑌෠𝐴 ∙ 𝑌෠𝐵 = 0.866
𝐴
𝑅，「轉動 -3」頁面結
藍虛線: World Frame {𝐴} መ
𝑍𝐴 ∙ 𝑌𝐵෠ 0 𝐵
果
紅實線: Body Frame {𝐵} ෠
𝑋𝐴 ∙ 𝑍𝐵መ 0
𝐵መ ෠ መ
𝑍𝐴 = 𝑌𝐴 ∙ 𝑍𝐵 = 0
𝑍መ𝐴 ∙ 𝑍መ𝐵 1
 Rotation Matrix -3
𝑍መ𝐴 𝑍መ𝐵 𝑌෠𝐵
 特性 {𝐵}
𝑇
| | | | | |
𝐴 𝑇𝐴 𝐴෠ 𝐴෠ 𝐴መ 𝐴෠ 𝐴෠ 𝐴መ
𝐵 𝑅 𝐵 𝑅 = 𝑋𝐵 𝑌𝐵 𝑍𝐵 𝑋𝐵 𝑌𝐵 𝑍𝐵 {𝐴}
| | | | | | 𝑋෠𝐵
𝑌෠𝐴
𝐴෠ 𝑇
− 𝑋𝐵 − | | | 𝑋෠𝐴
= − 𝐴෠ 𝑇 𝐴෠
𝑋𝐵 𝐴෠
𝑌𝐵 𝐴መ
𝑍𝐵
𝑌𝐵 −
𝐴መ 𝑇 | | |
− 𝑍𝐵 −

= 𝐼3
3x3 identity matrix

= 𝐵𝐴𝑅−1 𝐵𝐴𝑅

𝐵 𝑅 = 𝐵 𝑅 = 𝐴𝑅
𝐴 𝑇 𝐴 −1 𝐵
 Rotation Matrix -4

 A 3x3 orthogonal matrix 𝑄 𝑄𝑄𝑇 = 𝑄𝑇 𝑄 = 𝐼

 Always invertible 𝑄 −1 = 𝑄𝑇
 Columns: orthonormal basis
o Length = 1

o Mutually perpendicular

 Rotation matrix (R)有9個數字，但上列兩個條件置入了6個

constraints，所以R只有3個DOFs，與空間中轉動具有3 DOFs相符

 Determinant =1 (rotation); =-1 (reflection)

 Rotation Matrix -5

 Rotation matrix 除描述{𝐵}相對於{𝐴}之姿態，也可用於轉

換向量之座標 𝑍መ𝐵 𝑌෠𝐵
{𝐵}
original coordinate 𝐵𝑃 = 𝐵𝑃𝑥 𝑋෠𝐵 + 𝐵𝑃𝑦 𝑌෠𝐵 + 𝐵𝑃𝑧 𝑍መ𝐵 {𝐴}
new coordinate 𝐴𝑃 = 𝐴𝑃𝑥 𝑋෠𝐴 + 𝐴𝑃𝑦 𝑌෠𝐴 + 𝐴𝑃𝑧 𝑍መ𝐴 𝑃

where 𝐴𝑃𝑥 = 𝐵𝑃 ∙ 𝑋෠𝐴 = 𝑋෠𝐵 ∙ 𝑋෠𝐴 𝐵𝑃𝑥 + 𝑌෠𝐵 ∙ 𝑋෠𝐴 𝐵𝑃𝑦 + 𝑍መ𝐵 ∙ 𝑋෠𝐴 𝐵𝑃𝑧
𝐴𝑃
𝑦 = 𝐵𝑃 ∙ 𝑌෠𝐴 = 𝑋෠𝐵 ∙ 𝑌෠𝐴 𝐵𝑃𝑥 + 𝑌෠𝐵 ∙ 𝑌෠𝐴 𝐵𝑃𝑦 + 𝑍መ𝐵 ∙ 𝑌෠𝐴 𝐵𝑃𝑧 𝑋෠𝐵
𝐴
𝑃𝑧 = 𝐵𝑃 ∙ 𝑍መ𝐴 = 𝑋෠𝐵 ∙ 𝑍መ𝐴 𝐵𝑃𝑥 + 𝑌෠𝐵 ∙ 𝑍መ𝐴 𝐵𝑃𝑦 + 𝑍መ𝐵 ∙ 𝑍መ𝐴 𝐵𝑃𝑧
𝐴 𝐵
𝑃𝑥 𝑋෠𝐵 ∙ 𝑋෠𝐴 𝑌෠𝐵 ∙ 𝑋෠𝐴 𝑍መ𝐵 ∙ 𝑋෠𝐴 𝑃𝑥
𝐴 𝑃𝑦 = 𝑋෠𝐵 ∙ 𝑌෠𝐴 𝑃𝑦 = 𝐵𝐴𝑅 𝐵𝑃
𝑃= 𝑌෠𝐵 ∙ 𝑌෠𝐴 𝑍መ𝐵 ∙ 𝑌෠𝐴
𝑃𝑧 𝑋෠𝐵 ∙ 𝑍መ𝐴 𝑌෠𝐵 ∙ 𝑍መ𝐴 𝑍መ𝐵 ∙ 𝑍መ𝐴 𝑃𝑧

和「轉動-1」頁matrix相同，為rotation matrix
 Rotation Matrix -6

 Ex: 若{𝐵}和{𝐴} 的相對狀態同「轉動 -3」頁面所示，假設

𝐵
𝑃 = [1.732 1 0]𝑇 ， 𝐴𝑃 =?
𝐴
𝑃 = 𝐵𝐴𝑅 𝐵𝑃
0.866 −0.5 0 1.732
𝐴
𝑃 = 0.5 0.866 0 1
0 0 1 0
1
𝐴
𝑃= 1.732 上視圖
驗證答案
0
𝐵
𝑃

𝐴
藍虛線: World Frame {𝐴} 𝑃
紅實線: Body Frame {𝐵}
 Rotation Matrix -7

 Rotation matrix的第三個功能，可進一步來描述物體「轉動
」的狀態

 以對三個principal axes旋轉的matrix為基礎

 About 𝑍መ𝐴 with 𝜃 旋轉角度

𝑐𝑜𝑠𝜃 −𝑠𝑖𝑛𝜃 0
𝑍መ𝐴 𝑍መ𝐵
𝑅𝑍෠𝐴 𝜃 = 𝑠𝑖𝑛𝜃 𝑐𝑜𝑠𝜃 0
0 0 1
旋轉軸
𝑌෠𝐵 𝑐𝜃 −𝑠𝜃 0
= 𝑠𝜃 𝑐𝜃 0
𝑌෠𝐴 0 0 1
𝜃
𝑋෠𝐴 | | |
𝑋෠𝐵 Note: 𝐵𝐴𝑅 = 𝐴𝑋෠𝐵 𝐴෠
𝑌𝐵 𝐴መ
𝑍𝐵
| | |
 Rotation Matrix -8

 About 𝑋෠𝐴 with 𝜃 𝑍መ𝐵 𝑍መ𝐴

1 0 0 1 0 0
𝑅𝑋෠ 𝐴 𝜃 = 0 𝑐𝑜𝑠𝜃 −𝑠𝑖𝑛𝜃 = 0 𝑐𝜃 −𝑠𝜃 𝑌෠𝐵
0 𝑠𝑖𝑛𝜃 𝑐𝑜𝑠𝜃 0 𝑠𝜃 𝑐𝜃 𝜃 𝑌෠𝐴

𝑋෠𝐴 ෠
𝑋𝐵
 About 𝑌෠𝐴 with 𝜃
𝑍መ𝐴
𝑍መ𝐵
𝑐𝑜𝑠𝜃 0 𝑠𝑖𝑛𝜃 𝑐𝜃 0 𝑠𝜃
𝑅𝑌෠ 𝐴 𝜃 = 0 1 0 = 0 1 0 𝜃
−𝑠𝑖𝑛𝜃 0 𝑐𝑜𝑠𝜃 −𝑠𝜃 0 𝑐𝜃
𝑌෠𝐴
𝑌෠𝐵
𝑋෠𝐴
𝑋෠𝐵
 Rotation Matrix -9

 Ex: 𝐴𝑃 = [0 1 1.732]𝑇 對𝑋෠𝐴 軸旋轉30°， 𝐴𝑃′ =?

𝑅𝑋෠ 𝐴 𝜃

1 0 0
= 0 cos 30° − sin 30°
0 sin 30° cos 30°
𝐴 1 0 0
𝑃′ 30° 𝐴
𝑃 = 0 0.866 −0.5
0 0.5 0.866
30° 𝐴 𝐴
𝑃′ = 𝑅𝑋෠ 𝐴 𝜃 𝑃
1 0 0 0
= 0 0.866 −0.5 1
0 0.5 0.866 1.732
0
= 0
2
 Rotation Matrix -10

 Rotation matrix 的三種用法

 描述一個frame(相對於另一個frame)的姿態
𝑍መ𝐴 𝑍መ𝐵 {𝐵} 𝑌෠𝐵

| | |
𝐴 𝐴෠ 𝐴෠ 𝐴መ
𝐵𝑅 = 𝑋𝐵 𝑌𝐵 𝑍𝐵 {𝐴}
| | | 𝑋෠𝐵
𝑌෠𝐴
𝑋෠𝐴
 將point由某一個frame的表達換到另一個和此frame僅有相對轉動的
𝑍መ𝐴
frame來表達 𝑍መ𝐵 𝑌෠𝐵
{𝐵}
𝐴 𝐴 𝐵 𝑌෠𝐴
𝑃= 𝐵𝑅 𝑃 {𝐴}
𝑃
 將point(vector)在同一個frame中進行轉動 𝑋෠𝐴
𝑍መ𝐴
𝑋෠𝐵
𝐴 𝐴
𝑃′ = 𝑅 𝜃 𝑃 𝑃’
𝑌෠𝐴
{𝐴}
𝑋෠𝐴 𝑃