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

最新推荐文章于 2022-06-27 18:47:10 发布

志匹

最新推荐文章于 2022-06-27 18:47:10 发布

阅读量925

点赞数 2

分类专栏：机器人（MATLAB）文章标签： matlab

本文链接：https://blog.csdn.net/qq_46107700/article/details/117107792

版权

机器人（MATLAB）专栏收录该内容

4 篇文章 5 订阅

订阅专栏

一、位姿描述

旋转

笛卡尔坐标系，以x轴和y轴为正交轴的坐标系，通常绘制成x轴水平、y轴竖直，两轴的交点为原点。平行于坐标轴的单位向量用 $\hat{x}$ 和 $\hat{y}$ 表示。一个点用其在x轴和y轴上的坐标（x，y）表示，或写成有界向量形式：
$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}中定义坐标轴的单位向量表示为
${^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}用两个单位向量表示：
$\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$
用矩阵形式可以分解成：
$\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点表示为
${^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），得
${^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）各自右侧的系数部分相等，得
$\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$ 。
$\begin{pmatrix} V_x \\ V_y \\ \end{pmatrix}={^V}R_B\begin{pmatrix} {{^B}x} \\ {{^B}y} \\ \end{pmatrix}\tag{5}$

平移

如图（1）所示，由于坐标系{V}和{A}的轴是平行的，所以
$\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}$
或简写成
$\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的坐标向量用齐次形式表达为
${^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$ 代表相对位姿：
$\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}的坐标如下
${^0}p={^0}\xi_1\cdot{^1}p$
也可写成
${^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