（原创）机器人学：从空间变换到正逆向运动学

前言

一直有计划开个坑，写一个比较系统完整的博客专栏，一方面是有利于自己巩固所学的知识，对学习历程做一个记录；另一方面，也希望其他有志于机器人领域的像博主一样的萌新，能够在入门的时候获得一些帮助，得到一些比较系统的知识。这个专栏初定普通机器人学（其实是想到《普通心理学》这个书名。。。捂脸 ），所以它就真的很“普通”，只是一些入门向的知识，重在知识的系统和实践部分，每一个理论都附带着代码实现。那就废话不多说，让我们开始吧！

众所周知，机器人学是一个十分浩瀚的领域，交叉着众多的学科知识。根据知乎@YY硕的分类，机器人学有四个核心的领域：

1.感知（Perception）。视觉传感器、图像传感器、触觉和力传感器、惯导等。
2.认知（Cognition）。人工智能、知识表达、规划、任务调度、机器学习等。
3.行为（Action）。运动学、动力学、控制、manipulation和locomotion等。
4.数学基础（Math Foundations）。最优估计、微分几何、计算几何、运筹学等。

实际上并没有一个严格的划分标准，也有人把机器人分为感知（Perception）、决策（Planning）、控制（Control）三大块。每个细分的领域都充满着探索和挑战，并且困难不小。下到底层的硬件如控制器、各种传感器、电路板、减速器、伺服电机等，然后需要依据实际问题构建出数学模型——其中不乏复杂的数学知识（见仁见智吧。。。），最后通过编程进行实现，需要学习者孜孜以求。例如博主，研一的时候粗浅地学习过SLAM（Simultaneous Localization and Mapping），因为博主本科一直都在摸鱼，所以研一恶补了诸如Gilbert Strange的Linear Algebra，上完Coursera的“Estimation and Learning”，看完了《Essential C++》,看了一部分的《视觉SLAM十四讲》， 粗浅地接触了ROS、OpenCV、Eigen、Sophus、g2o等开源库，学习使用Ubuntu、cmake、matlab等，用turtlebot跑过一些demo，但仍感觉自己一直在门外徘徊，未曾踏入slam的门。研二需要去企业联合培养，企业要我做运动控制和轨迹规划的方向，于是又捡起了JJCraig的《Introduction to Robotics Mechanics and Control》，开始恶补运动学、动力学、轨迹规划一类的知识，这就是写作这个专栏的初衷，督促自己不停总结自己所学内容，加深理解，然后表达出来，不要再像研一一样只是蜻蜓点水般浅浅一过。

所以，这个专栏还是会偏向运动学、动力学一类的，偏向工业机器人的知识，涉及运动控制、轨迹规划等，可能以后还会涉及一些参数辨识、机器视觉一类的内容。语言我会用自己的话来进行表述，可能不如教科书般严谨，博主尽量不出现知识性的错误，如果出现了，还请大佬不吝赐教，我会进行修改。

空间变换的数学描述

1.空间变换的基本概念

想象这么一个场景：现在摆在你面前的是一台三个自由度的SCARA机器臂，这台机器臂有且仅有三个转动关节，并且机械结构是已知的，假设在基座建立一个基座坐标系（base frame），以这个坐标系为参考系，我们可以得知一个小球的空间位置[${}^{B}X$ ${}^{B}Y$ ${}^{B}Z$]${}^T$，但是我们可爱的手手（腕部坐标系，wrist frame）也想得知小球的位置，然后过去抓住小球，这时候我们如何得知小球相对于腕部坐标系的位置[${}^{W}X$ ${}^{W}Y$ ${}^{W}Z$]${}^T$，这就需要空间变换的概念了。空间变换描述了一个坐标系相对于另一个坐标系的变换关系，这个变换关系只有两种：旋转和平移，我们需要用数学语言进行阐述。那如何求得这种变换关系？因为我们已知机器臂的机械结构，即可知每个连杆和关节的物理参数，通过这些物理参数——一种叫D-H参数表的东西，我们就可以建立起机器臂的运动学方程，得到两个坐标系之间的变换关系。最后，当我们知道小球相对于基坐标系的空间位置，又如何知道每个关节应该如何运动才能到达这个位置，这就是第三个问题——逆运动学的问题了。

其实只要翻开任何一本和机器人有关的教材，头一两章总会给你介绍空间变换的知识，空间变换就是机器人运动的数学描述。假设存在两个坐标系{ A}和{ B}，{ A}经过一次旋转R和一次平移P到达{ B}，这时{ B}可以描述为：$$\{\mathbf{B}\}={\{}_B^A\mathbf{R}{,}^A{\mathbf{P}}_{BORG}\}\text{\hspace{1em}(1)}$$ 其中，${}_B^A\mathbf{R}$是坐标系{ B}相对于坐标系{ A}的旋转矩阵（Rotation matrix），读作“from frame B to frame A”；${}^A{\mathbf{P}}_{BORG}$是坐标系{B}的原点在坐标系{A}中的平移向量（Translation vector）。有了旋转矩阵和平移向量，我们可以很轻松的就得到两个坐标系之间的变换关系。那么现在有一个点${}^B\mathbf{P}$，由变换关系可得其在坐标系{ A}中的位置${}^A\mathbf{P}$ $${}^A\mathbf{P}={}_B^A\mathbf{R}{}^B\mathbf{P}+{}^A{\mathbf{P}}_{BORG}\text{\hspace{1em}(2)}$$ 我们把旋转矩阵和平移向量放到一个矩阵里，写成齐次转换矩阵（Homogeneous Transformation matrix）的形式，记做${}_B^A\mathbf{T}$，则有$${}_B^A\mathbf{T}=\left[\begin{array}{cc}{}_B^A\mathbf{R}& {}^A{\mathbf{P}}_{BORG}\\ 0^T& 1\end{array}\right]\text{\hspace{1em}(3)}$$ 这样，我们就可以把式（2）简化成$${}^A\mathbf{P}={}_B^A\mathbf{T}{}^B\mathbf{P}\text{\hspace{1em}(4)}$$ 既然我们已经知道了怎么将一个点从一个坐标系映射（mapping）到另一个坐标系，那么我们就只剩一件事情：“旋转矩阵 ${}_B^A\mathbf{R}$ 怎么求？”旋转矩阵当然好求，就上面的例子来说，旋转矩阵就是坐标系{ B}的三根轴分别在坐标系{ A}上的投影（或者说点积） (5) B A R = [ X ^ B ⋅ X ^ A Y ^ B ⋅ X ^ A Z ^ B ⋅ X ^ A X ^ B ⋅ Y ^ A Y ^ B