机器人基础知识（1.1）二维空间位姿描述

志匹

1400人浏览 · 2021-05-22 22:04:27

志匹 · 2021-05-22 22:04:27 发布

一、位姿描述

旋转

笛卡尔坐标系，以x轴和y轴为正交轴的坐标系，通常绘制成x轴水平、y轴竖直，两轴的交点为原点。平行于坐标轴的单位向量用 $x^\hat{x}$ 和 $y^\hat{y}$ 表示。一个点用其在x轴和y轴上的坐标（x，y）表示，或写成有界向量形式：
$p=xx^+yy^(1)p=x\hat{x}+y\hat{y}\qquad \tag {1}$
在这里插入图片描述

在图1中，我们希望用参照系{A}来描述坐标系{B}，{B}的原点已经为向量t=（x，y）取代，然后逆时针旋转一个角度θ。在图中有一点p，需要确定 ${^A}p$ 和 ${^B}p$ 之间的关系。回到上图中，我们将问题拆分成两部分：先旋转，后平移。

在这里插入图片描述
先考虑旋转的情况，如图2创建一个新的坐标系{V}，其坐标系平行于坐标系{A}的轴（图1中的坐标系{A}），但原点与坐标系{B}的原点重合。我们可以将点P用{V}中定义坐标轴的单位向量表示为
$Vp=Vxx^V+Vyy^V=(x^Vy^V)(VxVy)(2){^V}p={^V}x\hat{x}_V+{^V}y\hat{y}_V=\begin{pmatrix} {\hat{x}_V} & {\hat{y}_V} \\ \end{pmatrix}\begin{pmatrix} V_x \\ V_y \\ \end{pmatrix}\tag {2}$
上式被写作上一个行向量和一个列向量的点积。
坐标系{B}用两个单位向量表示：
$x^B=cos⁡θx^V+sin⁡θy^Vy^B=−sin⁡θx^V+cos⁡θy^V\hat{x}_B=\cos\theta\hat{x}_V+\sin\theta\hat{y}_V\\ \hat{y}_B=-\sin\theta\hat{x}_V+\cos\theta\hat{y}_V$
用矩阵形式可以分解成：
$(x^By^B)=(x^Vy^V)(cos⁡θ−sin⁡θsin⁡θcos⁡θ)(3)\begin{pmatrix} {\hat{x}_B} & {\hat{y}_B} \\ \end{pmatrix}=\begin{pmatrix} {\hat{x}_V} & {\hat{y}_V} \\ \end{pmatrix}\begin{pmatrix} {\cos\theta} & {-\sin\theta}\\{\sin\theta}&{\cos\theta} \\ \end{pmatrix}\tag{3}$
用方程（1）可以在坐标系{B}中将P点表示为
$Bp=Bxx^B+Byy^B=(x^By^B)(BxBy){^B}p={^B}x\hat{x}_B+{^B}y\hat{y}_B=\begin{pmatrix} {\hat{x}_B} & {\hat{y}_B} \\ \end{pmatrix}\begin{pmatrix} {{^B}x} \\ {{^B}y} \\ \end{pmatrix}$
带入方程（3），得
$Bp=(x^Vy^V)(cos⁡θ−sin⁡θsin⁡θcos⁡θ)(BxBy)(4){^B}p=\begin{pmatrix} {\hat{x}_V} & {\hat{y}_V} \\ \end{pmatrix}\begin{pmatrix} {\cos\theta} & {-\sin\theta}\\{\sin\theta}&{\cos\theta} \\ \end{pmatrix}\begin{pmatrix} {{^B}x} \\ {{^B}y} \\ \end{pmatrix}\tag{4}$
令方程（2）和（4）各自右侧的系数部分相等，得
$(VxVy)=(cos⁡θ−sin⁡θsin⁡θcos⁡θ)(BxBy)\begin{pmatrix} V_x \\ V_y \\ \end{pmatrix}=\begin{pmatrix} {\cos\theta} & {-\sin\theta}\\{\sin\theta}&{\cos\theta} \\ \end{pmatrix}\begin{pmatrix} {{^B}x} \\ {{^B}y} \\ \end{pmatrix}$
（4）式描述了点如何通过坐标系旋转从坐标系{B}变换到坐标系{V}。这种类型的矩阵被称为旋转矩阵，记作 ${^V}R_B$ 。
$(VxVy)=VRB(BxBy)(5)\begin{pmatrix} V_x \\ V_y \\ \end{pmatrix}={^V}R_B\begin{pmatrix} {{^B}x} \\ {{^B}y} \\ \end{pmatrix}\tag{5}$

平移

如图（1）所示，由于坐标系{V}和{A}的轴是平行的，所以
$(AxAy)=(VxVy)+(xy)=(cos⁡θ−sin⁡θsin⁡θcos⁡θ)(BxBy)+(xy)=(cos⁡θ−sin⁡θxsin⁡θcos⁡θy)(BxBy1)\begin{pmatrix} {{^A}x} \\ {{^A}y} \\ \end{pmatrix}=\begin{pmatrix} V_x \\ V_y \\ \end{pmatrix}+\begin{pmatrix} x \\ y \\ \end{pmatrix}=\begin{pmatrix} {\cos\theta} & {-\sin\theta}\\{\sin\theta}&{\cos\theta} \\ \end{pmatrix}\begin{pmatrix} {{^B}x} \\ {{^B}y} \\ \end{pmatrix}+\begin{pmatrix} x \\ y \\ \end{pmatrix}=\begin{pmatrix} {\cos\theta} & {-\sin\theta}&x\\{\sin\theta}&{\cos\theta}&y \\ \end{pmatrix}\begin{pmatrix} {{^B}x} \\ {{^B}y} \\ 1\\ \end{pmatrix}$
或简写成
$(AxAy1)=(VRBt01×21)(BxBy1)\begin{pmatrix} {{^A}x} \\ {{^A}y} \\1\\ \end{pmatrix}=\begin{pmatrix} {{^V}R_B} &t\\0_{1\times2}&1\\ \end{pmatrix}\begin{pmatrix} {{^B}x} \\ {{^B}y} \\1\\ \end{pmatrix}$
其中， $t = （ x ， y ）$ 代表坐标系的平移变换，而坐标系旋转变换用 ${^V}R_B$ 表示。因为{A}和{V}的轴是平行的，所以 ${^A}R_B={^V}R_B$ 。将点P的坐标向量用齐次形式表达为
$Ap^=(VRBt01×21)Bp^=ATBBp^{^A}\hat{p}=\begin{pmatrix} {{^V}R_B} &t\\0_{1\times2}&1\\ \end{pmatrix}{^B}\hat{p}={^A}T_B{^B}\hat{p}$
${^A}T_B$ 称为齐次变换矩阵。
很显然 ${^A}T_B$ 代表相对位姿：
$ξ(xyθ)∽(cos⁡θ−sin⁡θxsin⁡θcos⁡θy001)\xi\begin{pmatrix} x & y &\theta\\ \end{pmatrix}\backsim\begin{pmatrix} {\cos\theta} & {-\sin\theta}&x\\{\sin\theta}&{\cos\theta}&y\\0&0&1 \\ \end{pmatrix}$

MATLAB实现

T1 = SE2(1, 2, 30*pi/180)           #创建一个坐标系，以（1,2）为原点，逆时针旋转180度
axis([0 5 0 5]);                #设置当前坐标轴 x轴 和 y轴的限制范围
trplot2(T1, 'frame', '1', 'color', 'b')           #绘制T1坐标系，名称为{1}，颜色为黑色，下同
T2 = SE2(2, 1, 0)
hold on;
trplot2(T2, 'frame', '2', 'color', 'r')
T3 = T1 * T2
hold on;
trplot2(T3, 'frame', '3', 'color', 'g')
T4 = T2 * T1
hold on;
trplot2(T4, 'frame', '4', 'color', 'c')
p = [ 3 ; 2 ];     #创建一个点（3,2）
hold on;
grid            #保留图像的栅格
plot_point(p, '*');         # 绘制p点，样式为“*”

运行上面程序得到以下仿真结果：

数值结果

T1 = 
    0.8660   -0.5000         1
    0.5000    0.8660         2
         0         0         1
 

T2 = 
         1         0         2
         0         1         1
         0         0         1
 

T3 = 
    0.8660   -0.5000     2.232
    0.5000    0.8660     3.866
         0         0         1
 

T4 = 
    0.8660   -0.5000         3
    0.5000    0.8660         3
         0         0         1

图像结果

P点相对于坐标系{1}的坐标如下
$0p=0ξ1⋅1p{^0}p={^0}\xi_1\cdot{^1}p$
也可写成
$1p=0ξ1⋅0p=(0ξ1)−1⋅0p{^1}p={^0}\xi_1\cdot{^0}p={({^0}\xi_1)}^{-1}\cdot{^0}p$
带入具体数值

>>P1 = inv(T1) * [p ; 1]    #inv为求矩阵的逆
P1=
   1.7321
  -1.0000
   1.0000

辅助函数e2h将欧几里得坐标点转换为齐次形式，而h2e进行逆转换，也可以用homtrans进行转换。

>>h2e( inv(T1) * e2h(p) )
ans = 
     1.7321
    -1.0000
>>homtrans( inv(T1), p)
ans = 
     1.7321
    -1.0000
>>P2 = homtrans( inv(T2), p )
P2=
    1
    1