机器人学——位置与姿态描述
一、位置与姿态描述
1.位姿(Pose)
物体(刚体)自身坐标系的位置和方向,图形上表示为一组坐标轴,如坐标系{B}及其坐标轴
x
B
x_B
xB和
y
B
y_B
yB
2.相对位姿(relative pose)
相对于一个参考坐标系A的某个坐标系B的相对位姿用
A
ξ
B
{}^A\xi_B
AξB(ksi)表示,描述了坐标系{A}经过平移和旋转转化为{B}的动作。若没有上标A,表示相对于世界坐标系O。
- 空间中点P的位置分别在坐标系{A}和{B}中的描述满足:
A
P
=
A
ξ
B
⋅
B
P
{}^AP\;=\;{}^A\xi_B\cdot{}^BP
操作符·表示向量转换 - 相对位姿的特点是可以被合成或组合
A
ξ
C
=
A
ξ
B
⊕
B
ξ
C
\;{}^A\xi_C\;=\;\;{}^A\xi_B\oplus\;{}^B\xi_C
操作符⊕
\oplus
- 位姿表示具有较强的代数运算能力,位姿的逆操作:
⊝
ξ
F
\circleddash\xi_F
ξ
F
\xi_F
注意复合运算不具有可交换性
二、二维空间位姿描述
坐标系{A}和{B}的相对位姿可等价表示为两个坐标系间的平移和旋转角度。
先考虑旋转,定义中间坐标系{V}
考虑平移
为了使上述推导更加形象,下面使用MATLAB工具箱展示一些具体数值化的例子:
·函数se2创建一个齐次变换
% T1是世界坐标系(1,2)的平移和旋转30°后得到的
T1 = SE2(1,2,30*pi/180);
axis([0 10 0 10]);
trplot2(T1,'frame','1','color','b')
运行结果:
T1 =
0.8660 -0.5000 1
0.5000 0.8660 2
0 0 1
·复合运算
T2 = SE2(2,1,0);
hold on
trplot2(T2,'frame','2','color','r')
%最终坐标系{3}相对于世界坐标系的平移量不是(3,3),因为它是相对于旋转后的
%坐标系{1}再平移的
T3 = T1*T2;
hold on
trplot2(T3,'frame','3','color','g')
·复合运算不可逆
%复合运算不可交换
%观察{3}和{4}的绘制结果即可知
T4 = T2*T1;
hold on
trplot2(T4,'frame','4','color','c')
·点坐标表示
%相对于世界坐标系定义一个点(3,2)
P = [3;2];
plot_point(P,'*')
%确定其相对于{1}的坐标
P1 = double(inv(T1))*[P;1];
%使用工具箱还可以进行反变换(下面两条指令效果相同)
h2e(double(inv(T1))*e2h(P));
%homtrans(double(inv(T1)),P)
运行结果:
ans =
1.7321
-1.0000
三、三维空间姿态描述
欧拉旋转定理:任何两个独立的正交坐标系都可以通过一系列(不超过三次)相对于坐标轴的旋转联系起来,但其中连续的两次旋转不能绕同一轴。
旋转轴的顺序很重要,具有不可交换性
1.正交旋转矩阵
正交矩阵由分别绕x,y,z轴旋转一定角度获得
工具箱也提供了一些函数用来计算基本旋转矩阵:
R = rotx(90)%绕x轴旋转90°
trplot(R)%绘制相应坐标系
运行结果:
R =
1 0 0
0 0 -1
0 1 0
·旋转复合操作
%为了说明旋转的复合,继续令坐标旋转
R = rotx(90) * roty(90)
trplot(R)
运行结果:
R =
0 0 1
1 0 0
0 1 0
正交旋转矩阵9个元素,有6个约束,因此只有3个独立值
- 每一列是单位长度,提供3个约束
- 列与列之间相互正交,提供3个约束
2.三角度表示法
欧拉定理的旋转顺序有两种:欧拉氏和卡尔丹式。
- 欧拉氏:XYX/XZX/YXY/YZY/ZXZ/ZYZ
- 卡尔丹式:XYZ/XZY/YXZ/YZX/ZYX/ZXY
ZYZ序列欧拉角:
R
=
R
Z
(
ϕ
)
R
Y
(
θ
)
R
Z
(
Ψ
)
R\;=\;R_Z(\phi)R_Y(\theta)R_Z(\Psi)
R=RZ(ϕ)RY(θ)RZ(Ψ)
%ZYZ序列欧拉角
R = rotz(0.1/pi*180)*roty(0.2/pi*180)*rotz(0.3/pi*180);
%或者更简单的表达形式
R = eul2r(0.1/pi*180,0.2/pi*180,0.3/pi*180);
运行结果:
R =
0.9021 -0.3836 0.1977
0.3875 0.9216 0.0198
-0.1898 0.0587 0.9801
%逆命题求解:已知正交旋转矩阵求欧拉角
gamma = tr2eul(R);
%如果绕y轴旋转所对应的欧拉角是负的
R = eul2r(0.1/pi*180,-0.2/pi*180,0.3/pi*180)
%其反函数结果
gamma = tr2eul(R)
%它返回了正的y轴欧拉旋转角,其余两个欧拉旋转角是负的
%但这组欧拉角对应的旋转矩阵与前一组相同
%说明从旋转矩阵到欧拉角的映射不唯一
运行结果:
R =
0.9021 -0.3836 -0.1977
0.3875 0.9216 -0.0198
0.1898 -0.0587 0.9801
gamma =
-3.0416 0.2000 -2.8416
另一种广泛使用的旋转角是:横滚(roll)-俯仰(pitch)-偏航角(yaw)(适用于飞机、船舶、车辆姿态)
R
=
R
X
(
θ
r
)
R
Y
(
θ
p
)
R
Z
(
θ
y
)
R\;=\;R_X(\theta_r)R_Y(\theta_p)R_Z(\theta_y)
R=RX(θr)RY(θp)RZ(θy)
X:前进方向
Y:右手方向
Z:垂直向下
该旋转角允许每个角度有正负号,不会产生奇异解。
奇异点:pitch = 90°
%横滚-俯仰-偏航角
%R = Rx*Ry*Rz
R = rpy2r(0.1/pi*180,0.2/pi*180,0.3/pi*180)
gamma = tr2rpy(R)
运行结果:
R =
0.9363 -0.2751 0.2184
0.2896 0.9564 -0.0370
-0.1987 0.0978 0.9752
gamma =
0.1000 0.2000 0.3000
3.双向量表示法
对于关节臂式机器人,一般会在它的末端执行器上固联一个坐标系{E},工具的轴线为坐标系的Z轴,并被定义为接近向量
α
∧
=
(
α
X
,
α
Y
,
α
Z
)
\overset\wedge\alpha\;=\;(\alpha_X,\alpha_Y,\alpha_Z)
α∧=(αX,αY,αZ)
定义一个与接近向量正交的向量,它位于机器人爪的两个手指之间,称为姿态向量
o
∧
=
(
o
X
,
o
Y
,
o
Z
)
\overset\wedge o\;=\;(o_X,o_Y,o_Z)
o∧=(oX,oY,oZ)
n
∧
=
o
∧
×
α
∧
\begin{array}{l}\overset\wedge n\;=\;\overset\wedge o\times\overset\wedge\alpha\\\end{array}
n∧=o∧×α∧
有了接近向量和姿态向量,就足以完全确定旋转矩阵:
a = [1 0 0]';%接近向量
o = [0 1 0]';%姿态向量
oa2r(o,a)%确定旋转矩阵
4.绕任意向量旋转
空间中任意两个坐标系,总可以找到一个轴,使得其中一个坐标系经过绕该轴旋转一个角度,与另一个坐标系重合。
正交旋转矩阵的特征值由一个实特征值 1,一对共轭复特征值组成:
其中
θ
\begin{array}{l}\theta\\\end{array}
θ表示旋转的角度。
当特征值是1的时候:
该特征向量没有旋转,因此它是R的旋转轴。
四个参数表示
(
θ
,
v
)
\begin{array}{l}(\theta,v)\\\end{array}
(θ,v),但也可以简化为三个独立参数,由于\begin{array}{l}v_3;=;\sqrt{1-v_12-v_22}\\end{array}
使用罗德里格旋转方程,可以用角度和向量计算出旋转矩阵:
注:S(v)是反对称矩阵
%根据正交旋转矩阵求旋转角度和向量
R = rpy2r(0.1/pi*180,0.2/pi*180,0.3/pi*180);
[theta,v] = tr2angvec(R);
%求特征值与特征向量
[v,lambda] = eig(R);
%已知旋转角度和旋转轴求解正交旋转矩阵
R = angvec2r(90,[1 0 0]);
四、三维空间位态描述
齐次变换矩阵
向量t:坐标系原点相对于参考坐标系的笛卡尔位置
正交旋转矩阵R:坐标系相对于参考坐标系的姿态
%transl创建一个有平移但无旋转的相对位姿
%trotx则返回一个绕x轴旋转90°的齐次变换矩阵
T = transl(1,0,0)*trotx(90)*transl(0,1,0);
trplot(T);
%提取矩阵T中的旋转矩阵部分
t2r(T)
%提取平移部分
transl(T)'
t2r(T)