路径规划算法之刚体变换

欢迎大家关注我的B站：

偷吃薯片的Zheng同学的个人空间-偷吃薯片的Zheng同学个人主页-哔哩哔哩视频 (bilibili.com)

目录

1 一般概念

1.1 基元的变换

1.2 一个参数化的变换族

2 2D变换

2.1 translation

2.2 rotation

2.3 Combining translation and rotation

3 3D变换

3.1 Yaw, pitch, and roll rotations

3.2 Determining yaw, pitch, and roll from a rotation matrix

3.3 The homogeneous transformation matrix for 3D bodies

---

本文对机器人运动规划的经典书籍《planning algorithm》进行解析，原文请参考3.2 Rigid-Body Transformations (lavalle.pl)

1 一般概念

假设刚体机器人a被定义为R2或R3的一个子集。刚体变换是一个函数 h: A→W，它将A的每一点映射到W，有两个要求:1)A的任意两点之间的距离必须保持，2)A的方向必须保持(没有“镜像”)。对于某a∈A, h(a)表示W中被a“占据”的点。

这是h的像，表示变换后的机器人占据W中的所有点。形象地说就是机器人坐标系转化到世界坐标系的一个映射。

1.1 基元的变换

上述是一整个刚体进行变换，但是对于一个复杂的机器人，我们通常用多个基元的组合来定义，如

因此变换的函数如下

 

如果利用逆映射，则变换的函数如下

 

1.2 一个参数化的变换族

将q定义为参数变量，由q来决定A到W的变换。同时将h(q, a)简化为a(q)，将h(q, A)简化为A(q)。这可以形象地认为将body frame以一定位置和方向放置到world frame。同时我们也区分一下body frame 和 world frame，在一般机器人的运动规划问题中，world frame基本不会变，一般都是机器人运动，也就是body frame不断地在做刚体变换。

2 2D变换

2.1 translation

 上面介绍的内容还是相对抽象，本节开始就比较具体了，下面就是刚体变换中平移的映射，

 现在考虑用基元定义的A的实心表示形式

 当然translation有两种形式，第二种相当于机器人“倒退”（根据相对运动）

2.2 rotation

旋转的映射关系如下

 通过2*2的矩阵可以表示为

线性代数中提出左乘矩阵相当于对其进行线性变换，这里就是旋转变换，同时回想一下列向量其实表示新坐标系的基向量。R(θ)的列向量是单位向量，它们的内积为零，也表明它们是正交的，理解了这一步，有助于对高维旋转矩阵的理解

2.3 Combining translation and rotation

平移和旋转我们都已经探究了一遍，现在对平移和旋转结合，也就能达到平面的移动机器人真正在做的刚体变换。因为平移和旋转相互独立，所以能直接进行叠加，将旋转矩阵吸收平移的部分，公式如下

 

上式左乘的矩阵T被称为齐次变换矩阵。T表示旋转和平移。每个机器人的基元都可以使用T的逆变换，从而得到一个变换后的机器人实体模型。齐次变换矩阵是组合变换的一种简便的表示方法。因此，它经常用于机器人、力学、计算机图形学和其他领域。它被称为齐次变换是因为在R3上它只是一个没有任何平移的线性变换。在射影几何中，增加一维来吸收平移部分的技巧是常见的。

3 3D变换

对于平面移动机器人，如自动驾驶车辆，AGV等，2D变换是非常重要的内容，但对于三维移动机器人，如无人机，机械臂等，3D变换就变得尤为重要。

3.1 Yaw, pitch, and roll rotations

二维刚体变换有两个平移的自由度和一个旋转的自由度，而三维刚体变换有三个平移的自由度和三个旋转的自由度。平移的部分与二维类似，这里就不再赘述。

横摆是绕Z轴旋转

 

 俯仰是绕y轴旋转

 

翻滚是绕x轴旋转

 

上述三个专业名词根据不同的载体而定，这里主要是六自由度车辆的专业术语。

每个旋转矩阵都是二维旋转矩阵的简单扩展。例如，横摆矩阵Rz(α)本质上执行了一个关于x和y坐标的2D旋转，而保持z坐标不变。三种旋转可将3D体置于任何方向。通过将三个旋转矩阵相乘，可以得到单个旋转矩阵

 但要注意左乘矩阵的顺序不同。3D刚体变换的结果不同

3.2 Determining yaw, pitch, and roll from a rotation matrix

直接从给定的旋转矩阵确定α、β和γ参数通常是很方便的。如上式假设一个任意旋转矩阵得

 

 

3.3 The homogeneous transformation matrix for 3D bodies

上面对3D刚体的旋转矩阵做了推导，现在来把平移与旋转结合，也就是探究一个3D刚体的齐次变换矩阵，矩阵如下