前言
从目前的一些网站资料上看，机械臂运动学相关的讲解是非常散乱的，就算是有人用大量篇幅去讲解、归纳也还是有很多问题存在。我觉得这中间最重要的点在于过程中公式推导和几何理解没办法直接通过文本形式讲的很透彻，原理理解方面推荐台大林沛群老师的机器人运动学课程，里面老师会结合机器人模型进行讲解，更容易理解。无论是网课还是网上主流的教程，都只给出了满足佩珀准则（机械臂三轴交于一点）的方法，我的项目所应用的类 UR 结构机械臂并没有明显的 piper 特征，但鉴于另有说法是三轴平行的机械臂默认在无限远处交于一点，同样满足准则。我还是根据准则进行了计算以及程序编写。
在此章中我具体的展示了过程中的矩阵公式以及程序代码，但具体的公式推导则需要考虑几何结构进行理解，这一部分可以点击链接从视频中学习,涉及的代码内容量较多，因此大部分公式以截图的形式进行说明。
处理逆运动学问题有两种主要的方法，前面提到的应用 piper 准则的解析法以及通过牛顿迭代等方法进行处理的数值法，在此处我优先介绍解析法，一方面是由于目前市面上的大多数机器人都满足 piper 准则，另一方面数值迭代的方法教程资源不是很多，我还没太搞懂。另外正运动学的部分之前写的不太详细，仅仅是贴出了代码，具体如何在 python 中进行应用，如何将旋转矩阵和欧拉角进行相互转换，还需要再做讲解。
求解逆解
首先，整个求解过程需要围绕之前正运动中应用标准 DH 参数法得到的变换矩阵 T 展开，矩阵基本形式如下所示：
左上角的 3*3 矩阵即为旋转矩阵，可以通过以下公式变换转化为欧拉角，欧拉角是姿态角，即 Rx、Ry、Rz（不同的坐标系可能不一样，还要再做转换）。具体公式如下：
逆运动学的整体目标是实现末端执行器的位姿和姿态到关节角度的转换，此处定义末端执行器姿态为：[ 
后续推导将围绕这个矩阵进行展开。
关节1求解
关节5求解
关节6求解
关节3求解
关节2求解
关节4求解
小结
推导流程大致就是根据矩阵等式，将六个关节参数陆续进行求解，当然随着参数的增加以及公式的计算，会逐渐增加解的个数，最终会有八组逆解。在这八组逆解中如何取舍，同样是我们需要研究的问题，之后会进行记录。在写码的过程中，我基本上是围绕以上公式进行编写的，需要注意的有以下几点：
- 在 python 中一定要使用 numpy 库进行处理，math 库虽然有对应的公式，但是无法对矩阵进行处理，后续会产生报错。
- 多解的情况要分类处理，储存在对应的矩阵中，结果为正的解放在一行，为负的解要放在另一行，并且需要实现规定好矩阵维度。
- 编写前如果对 numpy 没有过一定的了解，建议先去学习一下概念，只需要了解基本公式指令以及如何定义空矩阵即可。
- 虽然可以直接调用转换矩阵参数，但由于这些参数在计算过程中要频繁调用，建议一一定义进行简化。
如果没有一步步跟着公式进行代码编写，我绝对不会发现以上的问题，所以希望还是能够自己跟着计算过程进行编写，这对于写代码的基本功来说帮助是很大的。
逆解 python 代码实现
上面的公式其实在 MATLAB 中要更好编写（参考代码便是 MATLAB 形式），但基于我们项目整体的背景需要，将逆解代码放在 python 中进行处理。整体流程并无太多需要着重讲述的地方，只需要注意上面提到的几个注意点即可，代码如下(引用库为 math 和 numpy)：
1 | def inverse_kinematics(pos, rot, d, a): |
其余部分为我自己定义的初始化部分，以及调用之前的正运动学进行了一定的测试，可以进行参考。
1 | import math |
最优解选择
逆解程序给出我们八组逆解，但我们所需要的只有唯一的一组逆解，并且还有两种情况要考虑：
- 并不是所有情况都有八组解，无解的坐标会输出为 nan
- 没有规定明确的关节角范围，需要通过范围进行筛选
这相当于是机器人的一个初步的运动规划，具体深入的话可能还要考虑运动过程中的中间点，再到后续还有工作空间的整体规划，这一部分要更加复杂，暂且按下不表。我们现在要实现的就是选择一个最合适的末端位姿即可，这一部分资料要更少，仅有一些理论层次的说法，具体实操的公式代码以及过程实现则根本没有找到。因此这块的代码是我针对功能要求自己编写的，主体是以循环、矩阵操作的方式进行处理。代码水平不是很高，也可能有很多需要改进的地方，如果在调试运行的过程中有新的想法，请反馈给我加以修正。
我把整体代码划成了三大块，分别是有效解选取、标准解选取以及最优解选取。从后续来看其实有些部分是完全可以合并在一起的，但是过程中逻辑比较混乱，掺在一起的话出现错误太难修改了，因此还是选择分成三步进行处理。首先是有效解选取部分，代码如下：
1 | def get_useful_solution(solutions): |
万事开头难，这一部分我想实现的流程是将矩阵中数据进行一行行的检查（不同行是不同解，不同列是不同关节），如果发现无效数据，就跳转到下一行从头检查。要实现这个过程当然是通过循环解决方便，但最大的难题在于如何在循环检查的同时将有效数据保存为一个新的矩阵。我是通过设立了一个旗帜位进行处理的，这主要是受启发于之前的上位机程序，在不同的线程间通过旗帜位进行处理使其产生联系，同样适用于目前这种不方便将循环和有效数据保存融合的情况。因此整体流程代码便实现了，八个解对应八个循环，每次循环内对每个关节角度进行检查，一旦出现无效数据则跳转至下一行并改变旗帜位，旗帜位的改变使得数据保存环节不会进行；而下次循环开始后先重置了旗帜位，如果所有关节数据都没有问题，则可以进入数据保存环节，将此行数据记录到新矩阵中。
还有一个问题是我刚开始不知道如何定义一个未知维度的矩阵，后面通过查找相关指令后选择通过 numpy 的 resize 指令进行处理，先随便定义维度，在每次保存前根据当前的行数进行重构，这样就可以将数据记录在对应的合适矩阵中。另外，需要注意无效数据 nan 必须通过对应的判别指令 pd.isnull 进行处理，这个指令来自于 pandas 库，numpy 也有对应的指令，但据说可能有报错等情况，所以此处还是选择了 pandas。
接下来是标准解选取：
1 | def select_standard_solution(array): |
整体语法上就直接套用了上一部分功能的语法结构，只是六个关节角度的范围各不相同，因此要多增加几个判断语句，其余差别不大。
最优解选取：
1 | def best_solution(finalArray): |
这一部分的核心就是使用了 numpy 的 linalg 模块，即线性代数函数库，norm 则是表示范数，通过对两个矩阵的差求范数来得到两者之间的距离，通过调试我选择的是二范数。然后将距离和上一距离进行对比（初始值是无穷大），更小的那一个成为新的最小值，这样结果就会是唯一的最优解。而求距离的对象则是预先设好默认值 config，6个关节角度和它进行对比，选择出最接近的一个。在实际的应用中应该要和上一次运动的关节角进行对比，此处为了方便测试选择默认，还待后续改进。
总结
至此，机器人运动学的基本流程就实现了，剩下的内容还有对正运动学 python 代码的补充以及更多算法的扩展，但是基本的框架已经打下了，根据这个框架可以对机器人有个初步的理解。通过这段时间对这些相关理论的学习，最大的感受有几个，在此进行分享：
- 首先是国内的代码资源环境，可以说是非常差，很多内容深度不够的同时给出的代码也根本跑不通，没有详细的对代码进行说明。如果只是想了解一些东西，很浅的进行尝试，那这些教程还是能够解决一些问题的。但如果真的想要深入学习，我的建议还是从理论出发，从课程视频以及书籍出发，自己去尝试功能实现远比代码拼接来的快，对自己的提高作用要更加明显。
- 过程中有很多地方用到了 chatGPT，接触 GPT 已经有一段时间了，过程中也经历了一系列的心理变化，但我永远会建议任何人去学习好好使用它。最开始的接触对我产生了很大的震撼，它可以根 据你的要求很好的完成任务，它为我在 mqtt 协议的部分提供了很大的帮助，能够让我在一个一窍不通的领域迅速上手。但随之而来的就是些许的迷茫，既然它可以自己写出我不会的一些代码，那还需要我做什么？我的工作是不是可以被它所取代？事实证明这只是由于科技迅速革新而带来的一点错觉，在后续深入的应用中，我逐渐了解了 GPT 运作的底层逻辑，也对它的局限性有了更全面的认识。GPT 并没有办法实现一些有深度或者说有创新性的功能，它所给出的代码也仅仅是网上现有代码的拼凑，可以这样说，它只是一个大大省略了我们时间成本和一定的学习成本的搜索引擎。让它给出一套完整的机器人运动学流程是不可能的，它不知道如何根据你的机械臂选取对应的方法，不知道如何梳理整体代码的逻辑，它能做的更类似于给出一个参考，让你对相关的指令更加熟悉。
因此，仅对于编程而言，GPT 是一个非常高效的工具，有什么想实现的功能，或者说想快速了解一个程序的各种功能指令，它绝对是最佳的选择。但是，至少从目前它的运作逻辑来看，是绝不可能代替编程的。
后续还需要将机器人通讯、逆运动学、坐标转换以及视觉识别各部分的代码结合起来，已经有了初步的雏形，具体如何处理还要进行调试，之后成功实现后再做整理。这周先把在博客之前学的一些内容做个整理放上来，主要是 TCP/IP 通讯协议这一块，有空的话再把博客建立整体流程梳理梳理。
