机器人学——位置与姿态描述

一、位置与姿态描述

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。
在这里插入图片描述

  1. 空间中点P的位置分别在坐标系{A}和{B}中的描述满足:

    A

    P
      

    =
      

    A

    ξ

    B

    B

    P

    {}^AP\;=\;{}^A\xi_B\cdot{}^BP

    AP=AξBBP

    操作符·表示向量转换
  2. 相对位姿的特点是可以被合成或组合

      

    A

    ξ

    C

      

    =
        

    A

    ξ

    B


      

    B

    ξ

    C

    \;{}^A\xi_C\;=\;\;{}^A\xi_B\oplus\;{}^B\xi_C

    AξC=AξBBξC

    操作符

    \oplus

    表示相对位姿合成(A=>B=>C)
  3. 位姿表示具有较强的代数运算能力,位姿的逆操作:

    ξ

    F

    \circleddash\xi_F

    ξF
    代表

    ξ

    F

    \xi_F

    ξ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个独立值

  1. 每一列是单位长度,提供3个约束
  2. 列与列之间相互正交,提供3个约束

2.三角度表示法

欧拉定理的旋转顺序有两种:欧拉氏和卡尔丹式。

  1. 欧拉氏:XYX/XZX/YXY/YZY/ZXZ/ZYZ
  2. 卡尔丹式: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)

版权声明:本文为weixin_46025237原创文章,遵循 CC 4.0 BY-SA 版权协议,转载请附上原文出处链接和本声明。
THE END
< <上一篇
下一篇>>