福建理工大学卢志明机械臂逆运算与碰撞空间电控方案

1.技术背景

其实我第一次接触工程的时候，是大一的五一假期，那时候距离比赛已经没有几天时间了，而工程车辆原本的电控学长已然消失的无影无踪，当时看着工程车我心的第一想法是：我真的能做到吗？我相信很多电控看着学长的车问自己的第一句话都是这个，我也不例外。最终我也做到了，车辆在赛场上没什么问题，得益于机械的极致发挥，在一个低成本和没有自定义控制器的情况下最高兑了三个四级矿，虽然不是什么很好的成绩，但我觉得综合当时的各种情况，已经够了。

学习一件事的过程很漫长，做出来一件事情更漫长，大一时学长教的少，懂得少，我只能自己去群里看，看开源代码结构，猜测别人是如何把事情做好的，在无人的时候偷偷去看学长的车，看看车上到底有什么，那些东西到底是如何使用的，问问比我懂的多的人他们到底懂了什么，在求知的路上一步步探索，慢慢将自己的猜测变为现实，再次特地感谢我的电控组长和学长邓威，在那段时间他们帮了我很多。打完比赛后脑子只有一个念头：不服气。不断地问自己为什么，打比赛的意义是什么？自己真的做好了吗？在无尽的盘问下我只告诉自己两个字：进步。是的，一个沉寂了很久的队伍需要进步，我帮不了步兵，我帮不了英雄，我帮不了任何人，因为我看到他们眼里的决绝，我知道大家都有一个目标，那就是进步，于是我疯狂的查找资料，我不想再用羡慕的眼神看着别的学校的机械臂，看着他人的高效率发出自己的感叹：哇！机械臂哎！然后回头看看自己，连自定义控制器也没有。

其实从思路的建立再到想法的执行是我迈出困难最大的一步，因为我一无所知，网上似乎也没那么多的例子，更多的只是成品方案的开源，找了那么多资料，成品中无思路，思路中无知识的情况让我很恼火，甚至有点生气，多数用ros控制的机械臂需要依托高算力的mini-pc，然后在赛季规划中，我们虽然有视觉兑换的研发思路，却没有pc的预算，也就意味着工程身上最终没有mini-pc，那样似乎再多的开源也无所用，但是他的原理还在，原理的存在就意味着你可以根据自身情况写出属于自己的代码。其实一开始当学完坐标系正逆解时，脑中对于机械臂的实现也比较清晰了，知识巨大的学习成本与时间还是让我有点缓不过来，在没人给予我传承经验的时候，我更注重如何将知识传承下去，我的学弟可以不聪明，但不能懂得没我多！给予我们队伍的祖传电控框架是库函数，我萌生了机械臂库函数的想法，它可以像配置io一样配置机械臂，根据不同的臂做不同的坐标平移单独解算，最后整和输出结果。

如图所示为.h文件部分代码，做到一半后发现，尽管对于机械臂分类，分类解算的部分做的比较好了，但是还是在解算连接部分具有较大的问题，出于这个赛季对于自己程序的保底规划方案，我不得不暂时放弃编写库函数的想法，转而去完成现有的刚需的程序。

2.逆运算技术方案

对于数学模型来讲，分为一般模型和抽象模型，抽象模型多为特点思路为辅，集合化简后运算为主的模型，更多的运用到数形结合抽象映射等方式完成。

如图，为普通dht坐标系下的普通六轴机械臂的图形描述：

Dht坐标系认为基于每个关节构建属于自己的姿态描述坐标系，将自身坐标系与自身对称轴的运动结合起来，将其与世界坐标系对比构建trans-port，将自身坐标系的xyz轴分别用向量表示并放置于向量矩阵，此时trans-port就相当于各个关节向量矩阵的连接链条，通过运算可以得知各个矩阵间数值的关系，但是此种方法对于程序来说逻辑复杂，算力要求较高，精度方面有待考究（其实我也没能力考究），所以我选择自己总结模型的方法进行解算。

其实对于视觉兑换来讲，更多的是要求定点定位，那是什么意思呢：

定点--即确定坐标位置。

定位--即确定位姿状态。

于此同时我们发现，对于取矿过程中，正真的逆运算是发生在大臂的部分，因为取矿需要知道矿石的姿态，矿石坐标，而我们通常的机械臂都是前端差速器加后端大小pitch的结构，于是我们可以发现，基于矿石的姿态描述下，大小臂视为一个整体，他们负责让差速器到达指定位置，而差速器负责迎接矿石对应姿态实现取矿。如图所示：

所以对于取矿兑矿时候，我们的需求是先定点A1，再定向向量j即可。

这时我们发现，当我们定点时，机械臂理应是少了三个自由度的，事实上也确实如此，这三个自由度应当在解算方面也反映出来。这时我们让机械臂底座的大yaw消失，便发现机械臂能动的部分只存在于一个二维平面了，如果我把这些部分都分开计算，那不就全是二维的计算了吗？

此时对于一般性构型机械臂，我们要做的是分解再计算，分解时将大小臂和yaw轴整合到一起：

此时大小臂便在一个平面上，对于固定目标点，我们先选择转动yaw轴，让yz面与空间目标点重合，重合后，此时目标点与大小臂便成为了二维解算的部分：

此时臂的可活动关节为A1,A2，目标点A3，以A1,A3为圆心做半径为臂长l1，l2的圆，圆会有两个交点，此时对应的两个交点均为姿态合适的解算点，一般去上面的那个点为解算点，完成大小臂的解算部分。

那么到此定点的目标我们完成了，那么定位呢？

一个空间上的点p，可以用一个新的向量坐标系描述他的空间位姿。

我们用XB、YB和2B来表示坐标系主轴方向的单位矢量。当用新坐标系a作参考坐标系时,它们被写成4XB、4YB和^2B。我们很容易将这三个单位矢量按照4XB、4YB、^2B的顺序排列组成一个3×3的矩阵。我们称这个矩阵为旋转矩阵,并且由于这个特殊旋转矩阵是(B相对于(A)的表达,所以我们用符号R来表示(下一节将进一步说明旋转矩阵中左上标和左下标的选择方法):

总之,一组三个矢量可以用来确定一个姿态。为简单起见,我们用三个矢量作为矩阵的列来构造一个3×3的矩阵。于是,点的位置可用一个矢量来表示,物体的姿态可用一个矩阵来表示。在2.8节,我们将考虑另外一些只需要三个参数的姿态描述方法。

标量r可用每个矢量在其参考坐标系中轴线方向上投影的分量来表示。于是,R的各个分量可用一对单位矢量的点积来表示:

进一步观察,可以看出矩阵的行是A的单位矢量在B中的表达,即：

所以有：

如图，基于世界坐标系下（正坐标系或参考系），每个坐标系用向量连接，这与dht解算过程无异。其中黑色部分为参考系，黄色部分为连接向量，红色部分为新参考系，绿色部分为实际姿态描述坐标系，这样的正逆解算就完成，我们就可以用正解算将定点后的坐标系映射到头部，头部单独定位即可。

由此正解的trans-port我们可以知道

因为正交矩阵的逆等于他的转置，所以不论是基于a描述p还是基于p描述a，我们都只需要求出它们的trans-port即可。即用一个矩阵形式的算子表示从一个坐标系到另一个坐标系的映射。

其中r是主坐标系，而p/borg是待转换坐标系描述。矩阵被称为齐次变换矩阵。它完全可以被看作用一个简单的矩阵形式表示了一般变换的旋转和平移。

而大小臂各个节点之前的trans-port就相对较为简单，因为此时大小臂的位姿描述都已了然。

此时逆变换的trans-port表示为

此时头部的位姿描述我们依然使用dht坐标描述，由上我们解除各个点位之间的trans-port时，不论是正逆解还是坐标系映射，我们只需要用各个节点之间的trans-port来算出关系即可

所以最终的过程就是将目标点先定点，先逆计算定点，再由正解算映射坐标系至末端，在计算定位，逆运算与正运算过程完全二维化，我称为二维映射逆运算过程。

3.碰撞空间技术方案

基于机械臂易碰撞和脆弱的特点，基于软件的碰撞空间也显得极为重要。碰撞空间实现后的最小时间需求为不大于o（2）^8，而空间需求为o（3）^8，勉强满足单片机的性能需求。

首先定义空间分为两大类，碰撞空间和被碰撞空间。

其中臂体称为碰撞空间，外部静态物体为被碰撞空间，首先，我们将臂视作定长空间向量，给予他长度参数，让他经过函数转化后可以成为空间正长方体（为何是规则正长方体，因为这样极大的节省计算时间复杂度，关于碰撞空间我未精细研究，也从未正规学习，我图方便自己想了这种逃课方法，如有错误或意见可以指出），此时碰撞空间的描述我们已经完成，现在来看空间的二维描述：

如图所示，任意空间都可以用三个二维的封闭图形表示其空间特性，此时判断线段是否和矩形相交，如果三个二维平面都相交，此时线段与矩形相交：

此时我们需要将三维直线转化成各个平面的二维直线描述，之后一一对比即可，即对被碰撞空间进行线条分割，依次检查线段与矩形是否相交，即矩形与矩形是否相交，即可达到空间是否重叠，即是否碰撞的功能。

此时被碰撞空间我们用非正长方体来描述，设置空间时他的描述为为对角两点的三维坐标，此时多个被碰撞空间通过线段边界检测的方式融合在一起，节省系统计算资源：

当然，上述提到线段相交的判断方法较为重复，后面为了减少时间复杂度和空间复杂度，我们增加了判断的先决条件，以空间投影做圆，他将分为四个区域如图：

此时直线是否与矩形相交，通过先决条件的判断，将大大节省计算资源。例如先判断直线与圆有无交点，再判断在哪个区域，若在任意区域，判断直线与圆的交点，计算交点与区域直线的距离远近即可，从而做到线段与矩形判断的内核，而非线段与线段判断的内核，大大节省了计算资源。

四.代码

由于特殊原因，仅展示所用数学库部分包含.c与.h文件供大家参考。

martix.c martix.h

五.参考资料与推荐资料

1.机器人学导论-INTRODUCTION TO ROBOTICS【约翰克雷格】北京机械工业出版社2018

2. （推荐资料）

六.通讯信息：

wechat：llzzmm0303 qq：2178285205