News Message

机器人运动控制算法1

by wittx 2020-06-07

一、机器人运动控制算法

机器人运动学（位姿）正解：

预先约定一下，在机器人领域内，一些字母的含义（后面会用得到）

大地坐标系（或基坐标系）：B

末端坐标系：E

工具坐标系：T

测量坐标系：M

坐标系原点：ORG

第1小节物体位姿的表示方法

点在三维坐标系中可以表示为：

为了解姿态在三维坐标系中的表达方式，我们先了解一下空间中一条直线l的（单位）方向向量在坐标系的表达方式，

一个坐标系是由三条相互垂直的单位直线来描述的，因此，一个坐标系在另一个坐标系中的姿态描述可以表示为，

R中的第一列为坐标系X轴的（单位）方向向量在另一个坐标系中的表示，其他列同理。

（R这个矩阵还是挺有意思的，单位、对称且正交，还有其他的方面，后面会说）

在W系中表示的P点如何在U系中表示呢？

再简洁一些：

令：

这就是所谓的变换矩阵。

那么问题又来了，难道要在显示的屏幕把这个变换矩阵显示出来了么？太繁琐了吧，而且对实物的描述太抽象了，所以要寻找一种简洁又不太抽象的一种表示方法来代替变换矩阵。

根据正交矩阵的凯莱公式可知，任意一个正交矩阵R,存在一个反对称矩阵S,满足

其中I3是一个3X3的单位阵，一个3维反对称矩阵可由3个参数表示，

由上述公式可以直接得出结论：任何一个姿态矩阵可用3个参数确定。

这样就可以把原先变换矩阵中的12个变量转换成了6个变量（参数表示的个数瞬间减小了一半）。

这里简单介绍用3个变量表示姿态的方法：角坐标系表示法。

角坐标系表示法共有24种，其中12种为固定角坐标系法，另12种为欧拉角坐标系法。

这里只描述一种X-Y-Z固定角坐标系。

首先将坐标系{B}和一个已知参考坐标系{A}重合。先将{B}绕A系的X轴旋转γ角度，再绕A系得Y轴旋转β角度，最后绕A的Z轴旋转α角度。每个旋转轴都是绕着固定参考系{A}的轴，规定这种姿态的表示法为X-Y-Z固定角坐标系。有的地方也把它们定义为回转角、俯仰角和偏航角。

由于所有的旋转都是绕着参考坐标系得各轴进行得，可直接推导等价姿态矩阵：

其中cα是cosα的简写,sα是sinα的简写。最重要的是搞清楚上面公式的旋转顺序。

现令，

则，

这样再显示器种就可以显示6个参数代表末端坐标系的位姿了，当然，用欧拉角表示的方式还是有缺陷的，就是万向锁问题（解决的方法是用四元数表示位姿），有兴趣的同学可以上网搜一下，这里就不详述了。

第2小节机器人正向运动学模型

第1小节主要讲解末端位姿的表达方式，那这一部分主要讲解正向（位姿）运动学模型。

这里我们以传统地6R机器人为例，为更简洁地描述机器人的位姿，我们在机器人各连杆上建立坐标系（当然是虚拟的），这样就在机器人各个连杆上建立起坐标系，这样就可以计算后一个连杆坐标系相对于前一个连杆坐标系的位姿矩阵了（方便计算机进行计算），直到末端的位置。

建立连杆坐标系的规则也是各有各的不同，这里拿M-DH来举例（还有其他建模方法，这里并不会一一举例，有想了解的小伙伴可以私信我）。

在M-DH中，相邻两连杆坐标系之间的关系为：

则机器人末端坐标系（E）在基坐标系（B）中的表示为：

相邻两连杆坐标系中的θ表示关节角度（唯一可以变化的参数），其他参数可以看图了解它的含义。一般情况下，我们把末端坐标系在基坐标系中的表示写为：

这样机器人末端坐标系得位置和姿态就可以在B系中表示了（人类的思想是强大且神奇的）。

第3小节机器人运动学模型逆解

常用的机器人运动学模型的逆解方法有三种，一种是以解析的方式求各轴的角度，另一种以数值迭代（其本质为最优化的过程）的方式，最后一种是几何法，只针对一些结构比较简单的机器人。所以，我这里只讲解前两种。

3.1 pieper法运动学逆解

满足pieper法求解运动学逆解的条件是后三轴相交才行，该方法对关节类型没有要求。由于最后三轴相交，因此，

由式1可知，

令

则

令上式中

则

现在讨论公式3，求解关节角度 $\theta_{3}$ ，分三种情况：

（1）若 $a_{1}=0$ ,则 $r=k_{3}$ ,又因为r是已知的， $k_{3}$ 是仅关于 $\theta_{3}$ 的函数，因此可以求出 $\theta_{3}$ ;

（2）若 $s\alpha_{1}=0$ ,则 $z=k_{4}$ ,这里z为已知，也可求出 $\theta_{3}$ ;

（3）否则，联立公式3中的两个式子可以消去 $s_{2}$ 和 $c_{2}$ ,得到 $\frac{(r-k_{3})^{2}}{4a_{1}^{2}}+\frac{(z-k_{4})^{2}}{s^{2}\alpha_{1}}=k_{1}^{2}+k_{2}^{2}$ ,可以解出 $\theta_{3}$ 。

解出 $\theta_{3}$ 后，根据公式3解出 $\theta_{2}$ ，再根据公式2解出 $\theta_{1}$ 。

剩下的还有 $\theta_{4}、\theta_{5}、\theta_{6}$ ，由于这些轴相交，因此这些关节角只影响最后末端坐标系的姿态 $_{6}^{B}R$ ，在求出前三个关节角后，当 $\theta_{4}=0$ 时，可以由连杆坐标系{4}相对于基坐标系计算出 $_{4}^{B}R|_{\theta_{4}=0}$ 。坐标系{6}的期望姿态与连杆坐标系{4}的姿态的差别仅是最后三个关节角的作用，可以通过下面的公式解决，

$_{6}^{4}R|_{\theta_{4}=0}=_{6}^{4}R^{-1}|_{\theta_{4}=0}\cdot_{6}^{B}R$

对于大多数的操作臂来讲，可以运动欧拉角解法应用于 $_{6}^{4}R|_{\theta_{4}=0}$ ，解出最后三个关节角。

3.2 数值法运动学逆解

使用数值法运动学逆解的本质可以看作是最优化的过程，给定一个期望位姿，利用雅可比矩阵 $J$ 不断迭代，直到与期望位姿的误差很小为止。（关于雅可比矩阵的内容我会另起一篇来讲）

这里使用牛顿-拉弗森迭代求解运动学逆解，具体步骤为：

（1）已知机器人当前位姿 $T_{Cur}$ 与目标位姿 $T_{End}$ ,计算微分运动矩阵：

（2）由微分运动矩阵得到运动矢量 $D=[d_{x}，d_{y}，d_{z}，\delta_{x}，\delta_{y}，\delta_{z}]^{T}$ ，根据下面公式求出 $d\theta$ ,

（3）若 $\left|\left| d\theta \right| \right|\leq\zeta$ ,结束迭代并输出结果，其中 $\zeta$ 表示满足精度要求的充分小的数，若迭代次数超过设定值，则迭代停止并退出；若 $\left|\left| d\theta \right| \right|\geq\zeta$ 且在设定的迭代次数内，令 $\theta_{Cur}=\theta_{Cur}+d\theta$ ,重复步骤（1-2）直到迭代退出条件。