利用雅可比方法求线性方程组C语言_二、机器人运动控制算法——速度与雅可比矩阵...

——Q点的位置向量

——Q点的线速度在B系中的表示

——在A系中看Q点线速度在B中的表示

——坐标系B对着坐标系A以

的方式做旋转

——在C系中来看B系绕A系旋转的角速度

第1小节 速度描述方法

线速度：

如何描述Q点位置

在一个坐标系(B系)中的线速度呢？通常情况下的线速度是位移对时间的一阶导，因此定义线速度为：

如果我想要站在A系中看待Q点在B系下的线速度表达呢，这又该怎么办？

角速度：

图1

如图1，其中黑色坐标系为A系，蓝色坐标系为B系，橘色轴为B系绕A系旋转的瞬时轴。

为方便理解，现在分为两种情况讨论，一种是两坐标系原点重合的情况(只存在坐标系的线速度)，另一种两坐标系不重合，既有坐标系旋转也有坐标系平移的情况。

（1）两坐标系原点重合

由于坐标系是由三个相互垂直的轴线表示的，因此进一步简化，看一下一条带方向的轴线绕另一条轴线旋转的情况，

图2

如图2所示，空间中有一旋转轴

，

表示t时刻下绕

轴旋转的另一条单位轴线，经过

时间后到了

,其变化量为

,根据图中信息可以得到如下关系：

左右两侧同时乘以

得

然后再看一下图2，

的方向与

是垂直的，并且也是与

是垂直的。因此，

现在在图1中的B系中存在一个点

，然后B系绕A系以

旋转，则该点在A系中的速度为：

一般情况下，Q点在B系中也是存在运动的，因此需要在上面的式子中加入这一项的速度，即

式（1）

（2）两坐标系原点不重合

两坐标系原点不重合的情况只比两坐标系原点重合情况多了一项坐标系原点平移的一项，因此在式（1）的基础上添加这一项即可，即

第2小节 相邻两坐标系之间的速度描述

对于旋转关节机器人中的坐标系而言，电机的转轴通常为连杆坐标系的Z轴，并且只能绕Z轴旋转。

由图可知，

为方便迭代计算，上式转化到{i+1}坐标系中，即等式左右两侧同时乘

如果机器人中有移动关节怎么办？

那就是关节的旋转速度为0，即

转换到{i+1}系中后，

第3小节 雅可比矩阵

一般情况下，假设我们有6个自变量，有6个因变量：

接下来对自变量进行偏微：

写成矩阵的形式为：

简写为：

其中的

即为一般意义下的雅可比矩阵。上式左右同时除以时间的微元，得：

对于机器人的雅可比矩阵来说，

到

可以用机器人末端位姿的6个量来表示，自变量就是机器人关节角，也就得到了机器人末端位姿速度与角速度之间得关系了。

如何通过计算得到雅可比矩阵呢？

常用得方法有两种，一种是根据雅可比矩阵得定义直接对末端位姿求导，也是最不用动脑子的方法，另一种就是利用迭代的方式，根据几何关系求相邻两连杆坐标系之间的速度关系，然后从第0轴到第1轴，第1轴到第2轴，...最终到第6轴，求出第6轴到第0轴的速度关系。

接下来，以平面二连杆机构为例，推导一下各轴角速度与末端速度之间的关系。

这里的第0轴角速度就是固定在绝对坐标系中，因此为0

这个例子是我在台大的林沛群的课中搜到的，B站有他的视频，有看不懂的小伙伴去看一下他的视频，讲解非常清楚。