《機器人學導論》第2章MATLAB習題
MATLAB版本：R2019b
Robotics Toolbox版本：10。4
文中公式取自《機器人學導論》，且標出與書本相應的公式序號。
MATLAB程式碼推薦使用實時指令碼，方便對照程式碼檢視結果。
對於機器人，本人也在學習中。若答案有誤，望指出。
工具箱版本問題
rotz（），roty（），rotx（），rpy2r（）等函式在版本10。4中預設輸入為角度制，而在較前的版本中（如10。3）需要加上‘deg’表示角度制輸入。
習題1
習題1
a）
運用公式（2-71）寫出轉換函式
公式（2-71）
function
R
=
ZYXeul2R
（
alpha，beta，gamma
）
%輸入尤拉角α，β，γ，輸出旋轉矩陣R
R
=［
cos
（
alpha
）
*
cos
（
beta
），
cos
（
alpha
）
*
sin
（
beta
）
*
sin
（
gamma
）
-
sin
（
alpha
）
*
cos
（
gamma
），
cos
（
alpha
）
*
sin
（
beta
）
*
cos
（
gamma
）
+
sin
（
alpha
）
*
sin
（
gamma
）；
。。。
sin
（
alpha
）
*
cos
（
beta
），
sin
（
alpha
）
*
sin
（
beta
）
*
sin
（
gamma
）
+
cos
（
alpha
）
*
cos
（
gamma
），
sin
（
alpha
）
*
sin
（
beta
）
*
cos
（
gamma
）
-
cos
（
alpha
）
*
sin
（
gamma
）；
。。。
-
sin
（
beta
），
cos
（
beta
）
*
sin
（
gamma
），
cos
（
beta
）
*
cos
（
gamma
）］；
end
例子測試
ABR1
=
ZYXeul2R
（
deg2rad
（
10
），
deg2rad
（
20
），
deg2rad
（
30
））
ABR2
=
ZYXeul2R
（
deg2rad
（
30
），
deg2rad
（
90
），
deg2rad
（
-
55
））
結果
a）測試結果
證明部分：根據單位正交旋轉矩陣的性質可證，不細說。
b）
根據公式（2-66）和（2-67）寫出轉換函式
公式（2-66）
如β=90°，解得
公式（2-67）
function
［alpha，beta，gamma］
=
R2ZYXeul
（
R
）
%輸入旋轉矩陣R，輸出尤拉角α，β，γ
cbeta
=
sqrt
（
R
（
1
，
1
）
^
2
+
R
（
2
，
1
）
^
2
）；
if
cbeta
==
0
beta
=
pi
/
2
；
alpha
=
0
；
gamma
=
atan2
（
R
（
1
，
2
），
R
（
2
，
2
））；
else
beta
=
atan2
（
-
R
（
3
，
1
），
cbeta
）；
alpha
=
atan2
（
R
（
2
，
1
）
/
cos
（
beta
），
R
（
1
，
1
）
/
cos
（
beta
））；
gamma
=
atan2
（
R
（
3
，
2
）
/
cos
（
beta
），
R
（
3
，
3
）
/
cos
（
beta
））；
end
例子測試
［alpha1，beta1，gamma1］=R2ZYXeul（ABR1）；
rad2deg（［alpha1，beta1，gamma1］）
［alpha2，beta2，gamma2］=R2ZYXeul（ABR2）；
rad2deg（［alpha2，beta2，gamma2］）
結果
b）測試結果
c）
theta=deg2rad（20）；
Ry=［cos（theta） 0 sin（theta）
0 1 0
-sin（theta） 0 cos（theta）］；
BP=［1 0 1］。‘；
AP=Ry*BP
c）結果
d）工具箱檢查
函式rpy2r（）為輸入
繞 X-Y-Z固定角座標系旋轉的度數γ，β，α
，輸出
旋轉矩陣。
而該旋轉矩陣等同於 繞Z-Y-X尤拉角的旋轉矩陣。即繞Z-Y-X尤拉角10，20，30等於繞X-Y-Z固定角30，20，10。
函式tr2rpy為上述函式的逆運算，
'deg'
為角度制輸出。
注意到angles2並不是［-55，90，30］，是因為在β為90°的情況下，只能求出α和γ的和或差。
函式roty（）為輸入
繞Y軸旋轉的度數
，輸出
旋轉矩陣
。
ABR1=rpy2r（30，20，10）
ABR2=rpy2r（-55，90，30）
angles1=tr2rpy（ABR1，’deg‘）
angles2=tr2rpy（ABR2，’deg‘）
AP=roty（20）*［1 0 1］。’
工具箱檢查結果1
工具箱檢查結果2
習題2
習題2
a）
function T = ZYXeulVector2Tr（alpha，beta，gamma，P）
%輸入尤拉角α，β，γ和位置向量P，輸出齊次變換矩陣T
T=［ZYXeul2R（alpha，beta，gamma） P；
0 0 0 1］；
end
例子測試
ABT1=ZYXeulVector2Tr（deg2rad（10），deg2rad（20），deg2rad（30），［1 2 3］。‘）
ABT2=ZYXeulVector2Tr（0，deg2rad（20），0，［3 0 1］。’）
結果
a）測試結果
b）
AP=ZYXeulVector2Tr（0，deg2rad（20），0，［3 0 1］。‘）*［1 0 1 1］。’
結果
b）結果
c）
根據公式（2-45）可求得其次逆變換。
公式（2-45）
function Tv = invT（T）
%輸出用符號公式計算的齊次變換矩陣T的逆矩陣
Tv（1：3，1：3）=T（1：3，1：3）。‘；
Tv（1：3，4）=-T（1：3，1：3）。’*T（1：3，4）；
Tv（4，：）=［0，0，0，1］；
end
對a）中的i）和i））驗證
ABT1*invT（ABT1）
invT（ABT1）*ABT1
ABT2*invT（ABT2）
invT（ABT2）*ABT2
可得所有結果皆為單位矩陣。
d）
i）
ABT=ABT1；
BCT=ABT2；
ACT=ABT*BCT
CAT=invT（BCT）*invT（ABT）
i）結果
ii）和iii）
ABTii=ACT*invT（BCT）
ABTii-ABT；
BCTiii=invT（ABT）*ACT
BCTiii-BCT；
透過做矩陣減法可得結果與已知答案几乎一致（相差10^-15）
e）工具箱檢查
函式rt2tr（R，t）將
旋轉矩陣R和平移量t
合成
齊次變換矩陣
。
這裡只對a）和b）做了工具箱檢查。
ABT1=rt2tr（rpy2r（30，20，10），［1，2，3］。‘）
ABT2=rt2tr（rpy2r（0，20，0），［3，0，1］。’）
AP=ABT2*［1 0 1 1］。‘