微分几何在机器人领域的应用

Sunday_ding

已于 2022-12-07 22:34:15 修改

阅读量1.4k

点赞数

分类专栏：机器人文章标签：线性代数矩阵

于 2022-12-07 22:30:55 首次发布

本文链接：https://blog.csdn.net/a1058926697/article/details/128228157

版权

机器人专栏收录该内容

1 篇文章 1 订阅

订阅专栏

微分几何基础

微分几何是现代数学领域中的重要分支，在理论探索和实际应用中都是重要学科。大名鼎鼎的高斯、欧拉是微分几何学派的创建者（是否记得多少公式和定理以这两人的名字命名）。20世纪是微分几何发展迅猛的100年，中国的数学家也做出过重要贡献，如陈省身、邱成桐（菲尔兹奖得主）。在计算机领域，微分几何是计算机图形学的基础，逼真酷炫的电脑游戏、电影特效等，都是在微分几何基础上的产业化。在机器人领域，核心控制系统需要合适的传感器（如相机）获取信息，并理解环境信息，属于计算机视觉的范畴；如需完成复杂动作，如抓取、放置等操作，则需要理解物体的几何信息，需要用几何特征描述来决策机器人要执行的动作。

完成机器人抓取需要如下两个过程：

识别过程，属于视觉和深度学习的范畴，在此不再赘述。
获取物体的三维空间描述，微分几何。

三维空间中的物体有哪些特征呢？

曲率

为理解曲率，首先回到二维平面。什么是曲率？简答说来，是几何体的不平坦程度。平面曲线的曲率定义为其密切圆的倒数。采用微分的定义，密切圆在很小的范围内同曲线重合。故平面中的圆所有点曲率一直，为半径的倒数，密切圆为其本身。直线曲率处处为0，因其密切圆半径无穷大。

曲线的密切圆和密切圆半径。曲率为半径的倒数。

三维空间中可用曲率描述曲面。包括两个主曲率、高斯曲率、平局曲率等。点的主曲率是通过此点曲线最大和最小曲率。高斯曲率为两个曲率之积，平均曲率则是两个主曲率之平均。

一些特殊情况，如负曲率，如马鞍型，常见使用：冷却塔，广州塔。

二次曲线（Conics）和二次曲面（Quadrics）

二次曲线也称圆锥曲线，其在数学上的定位为一个正圆锥面和一个平面的相切形成的曲线。其公式可表述为：

其中A，B，C不得皆等于0。故常见的圆、椭圆、抛物线等皆属于二次曲线。

二次曲面则是三维空间中最常见的曲面，其一般公式为：

常见的二次曲面包括：

椭球（Ellipsoid），形如
的曲面。故球体是椭球的一种特例。

双曲面（Hyperbolic），形如

或

圆锥体（elliptic cone），形如：

一些特殊二次曲面示例：

曲面拟合

在机器人抓取领域，一般采用深度相机作为传感器。深度相机可直接获取空间点云信息。对于特定物体的抓取，一般在检测定位的基础上，采用点云拟合的方式定位，从而获取物体在深度相机坐标系下的位置和姿态。常用的拟合有如下几种：

平面拟合。空间中的平面可由空间中一点和法向量唯一确定。常用拟合方案有，主成分分析；最小二乘法；随机采样法（RANSAC）。
圆柱拟合。实际抓取场景中经常碰到圆柱面物体的情况。实际点云拟合中，如果已知主轴方向，则可投影到平面中，做圆的拟合。如方向未知，可首先用PCA的方法确定主轴方向。
球体拟合，看似复杂，实际只需确定圆心（一个点）和半径。总共4个自由度（未知变量），可使用最小二乘法或数值最优化方法来确定。

不规则形状

机器人抓取的实际场景中，一般曲面较为复杂，很难用简单公式表述。对于复杂曲面（曲线），一般采用ICP（IterativeClosestPoint）的方案完成自由形状的对齐。

总结

曲率是描述空间中的曲线或曲面最重要的特征。一般来说，进行机器人抓取，需要首先利用图像信息确定物体的图像位置，然后通过深度相机获取的点云信息技术其几何特性，完成抓取过程。曲面拟合和ICP的方案仍然有许多细节，在机器人抓取中需要特别注意。

空间几何变换

空间中的几何变换分为多类，从最简单，到逐渐复杂的变换，分别有如下几种。

等距变换（Isometries）。等距变换下点到点的欧式距离保持不变。刚体变换是典型的等距变换。
相似变换（Similarity）。在等距变换的基础上加上一个各向同性的缩放。矩阵表示上需要在旋转矩阵部分乘以一个系数s。
仿射变换（Affine）。是一个非奇异的线性变换加上一个平移向量组成的变换。
投影变换（Projective）。任意非奇异的4×4矩阵所构成的变换。

变换的分类和特征如下图所示:

三维刚体的空间变换属于第一种情况。如果问题不变形，那么刚体变换涵盖物理世界中的所有情况。刚体变换包含三个平移自由度和三个旋转自由度，总共6个自由度。应用刚体变换，点到点的距离保持不变，同时矢量的点积和叉积保持不变。平移自由度易于理解，故本文重点讨论旋转分量，即旋转矩阵R。

旋转矩阵：

在理解高维理论时，我们一般采用降维的方式理解，由易到难。首先回到二维空间的变换。二维平面中，刚体变换有三个自由度，x, y 和旋转角θ。用矩阵的形式表示：

其中

　　分别为旋转矩阵和平移向量。可以看到旋转矩阵只有一个自由度，因其只有一个变量θ。

旋转矩阵R的性质：

旋转矩阵的逆矩阵是它的转置矩阵，故旋转矩阵是正交矩阵。（如果不理解逆矩阵和转置矩阵，请首先恶补线性代数）。
一个矩阵是旋转矩阵，当且仅当它是正交矩阵，且它的行列式是1。正交矩阵的行列式是±1。读者可思考行列式为-1的情况对应什么变换。

二维旋转矩阵可用旋转角唯一表示。正角表示逆时针旋转。

　　如下图表示的是当θ=20°的情况。

　　二位旋转矩阵的许多性质在三维空间中同样满足。

让我们回到三维空间。旋转可以有三个旋转组合而成。在右手（笛卡尔）坐标系下分别绕x, y, z轴旋转。其旋转矩阵分别对应为

任意旋转矩阵可写作一定角度下的三个矩阵的乘积。

注意：矩阵乘法不符合交换律！故顺序不同，得到的旋转矩阵并不相同。

欧拉角

航空领域，一般定义飞机前后轴为x轴，沿x轴旋转的角度一般称为Roll，中文称作翻滚角；两翼方向称作Pitch，中文称作俯仰角；垂直地面的方向是航向角（Yaw），如下图所示。个人觉得中文翻译很符合愿意，更易于理解。可以记住在驾驶飞机时，如何操纵翻滚角，俯仰角，航向角。Roll，Pitch，Yaw，又称作欧拉角。习惯上，三个欧拉角的方向是z-y-x，使用时需要特别重要，欧拉角顺序错了，旋转矩阵也会发生变换。

程序实现：

程序使用基于C++的Eigen库[3]。注意，Eigen库是一个仅包含头文件的基础矩阵库，没有静态或动态库。使用时仅需要把相关的目录include就可以了。

再次注意：三个欧拉角的顺序！

 1 #define _USE_MATH_DEFINES
 2 #include <math.h>
 3 
 4 #include <Eigen/Core>
 5 #include <Eigen/Dense>
 6 typedef Eigen::Vector3f            Geo3d;
 7 typedef Eigen::Matrix3f            GeoMat3;
 8 typedef Eigen::Matrix4f            GeoMat4;
 9 typedef GeoMat3                    RotMat;
10 
11 GeoMat3 RotationMatrixFromEulers(float rx, float ry, float rz)
12 {
13 　　Eigen::AngleAxisf quat = Eigen::AngleAxisf(rz, Geo3d::UnitZ()) * Eigen::AngleAxisf(ry, Geo3d::UnitY()) * Eigen::AngleAxisf(rx, Geo3d::UnitX());
14　　 return quat.matrix();
15 }
16 
17 int main(int argc, char *argv[])
18 {
19     auto rot = RotationMatrixFromEulers(M_PI/12, -M_PI/3, M_PI/2);
20     std::cout << “Rotation Matrix: “ << rot << std::endl;
21 
22     // to euler angles. (2, 1, 0) means, rz, ry, rx
23     Geo3d euler_angles = rot. eulerAngles(2, 1, 0);
24     std::cout <<”Euler angles: “ << euler_angles.transpose() << std::endl;
25     return 1;
26 }

李群和李代数

三维旋转矩阵是最直观的表示方法，但旋转矩阵有9个变量，只有3个自由度，故信息是冗余的。旋转矩阵在工程使用更好的表达方法。根据定义，所有的刚体变换属于一个群（李群，Lie Group）。刚体变换又称作特殊欧式变换（special Euclidean transformation），通常写作SE（3）。李群中的变换满足如下特性。详细性质可参见李群和李代数的资料。如果只限于3D视觉或机器人学，只需记住其主要特性：