协作臂学习(9)-雅可比矩阵

最新推荐文章于 2023-04-29 23:22:34 发布

酷比程序

最新推荐文章于 2023-04-29 23:22:34 发布

阅读量1.5k

点赞数

文章标签：算法 python 机器学习人工智能计算机视觉

本文链接：https://blog.csdn.net/truewq/article/details/120432194

版权

雅可比矩阵

在机器人学中，雅可比矩阵最常用于表示关节速度与末端笛卡尔速度之间的关系，有公式：

$\nu = J(\theta)\dot{\theta}\\\$

末端速度

$\nu$ 表示空间速度，刚体的空间速度包括

线（平移）速度

和

角（旋转）速度

部分（比如，地球在太空中绕太阳公转，同时绕南北极轴线自转）。

$\nu=\left[\begin{array}{c} v\\ w \end{array}\right]\\$

线速度向量包括

三个方向的分量

$v=\left[\begin{array}{c} v_x\\ v_y\\ v_z \end{array}\right]\\$

角速度向量包括

三个轴向的分量

$w=\left[\begin{array}{c} w_x\\ w_y\\ w_z \end{array}\right]\\$

将

代入

中，

$\nu=\left[\begin{array}{c} v_x\\ v_y\\ v_z\\ w_x\\ w_y\\ w_z\\ \end{array}\right]=J(\theta)\dot\theta\\$

关节速度

在DH建模法中，关节分为转动型（R型，revolute）、滑动杆 / 平移（P型，prismatic）。因此，

$\theta$ 表示当前关节的位置：R型关节角度，P型关节位移。

$\dot{\theta}$ 表示当前关节的速度：R型关节角速度，P型关节线速度。

速度与微分

首先，回到初中物理公式：速度=距离/时间。在大学物理中，瞬时速度公式：

$v = \frac{dr}{dt}\\$

物体在某一时刻的瞬时速度

定义为位置矢量

随时间

的变化率，于是已知时间的情况下，

速度的问题变成了微分问题。

末端微分

对于机械臂，末端的微分运动

主要包括：沿

轴的微分平移

，绕

轴的微分旋转

$\delta x,\delta y,\delta z$ ，共6个量：

$\left[D\right]=\left[\begin{array}{c} dx\\ dy\\ dz\\ \delta x\\ \delta y\\ \delta z \end{array}\right]\\$

关节微分

对于机械臂，关节的微分运动与关节类型有关，DH建模法6R协作臂为例，关节微分为每个关节角的微分旋转，表示为

$[D_\theta]$ ：

$[D_\theta] = \left[\begin{array}{c} d\theta_1\\ d\theta_2\\ d\theta_3\\ d\theta_4\\ d\theta_5\\ d\theta_6 \end{array}\right]\\$

由于速度变成了微分，下面是雅可比矩阵把微分联系起来的公式：

$\left[D\right]=\left[\begin{array}{c} dx\\ dy\\ dz\\ \delta x\\ \delta y\\ \delta z \end{array}\right]=\left[J\right] \left[\begin{array}{c} d\theta_1\\ d\theta_2\\ d\theta_3\\ d\theta_4\\ d\theta_5\\ d\theta_6 \end{array}\right]=[J][D_\theta]\\$

微分运动

微分运动包括平移、旋转和组合变换（平移+旋转），平移表示为：

$Trans(dx,dy,dz)\\$

坐标系绕

轴旋转

$d\theta$ 度表示为：

$Rot(q,d\theta)\\$

绕一般轴的旋转是由绕

轴的三个旋转构成的：

$(d\theta)q=(\delta x)i+(\delta y)j+(\delta z)k\\$

所以由1.10式和1.11式，有

$Rot(q,d\theta)=Rot(x,\delta x)Rot(y,\delta y)Rot(z,\delta z)=\left[\begin{array}{cccc} 1 & -\delta z & \delta y & 0\\ \delta z & 1 & -\delta x & 0\\ -\delta y & \delta x & 1 & 0\\ 0 & 0 & 0 & 1 \end{array}\right]\\$

坐标系的微分变换

设原坐标系为

，坐标系的微分变化为

，把平移和旋转合并一起来表示坐标系的微分变换，左乘原坐标系就得到微分变化之后的结果。由式1.9和110有：

$[T+dT]=[Trans(dx,dy,dz)Rot(q,d\delta)][T]\\ dT=[Trans(dx,dy,dz)Rot(q,d\delta)-I][T]$

1.13式右边的变换可用

$\Delta$ 表示，由式.9和11.12，可计算出

$\Delta$ 为：

$\Delta=[Trans(dx,dy,dz)Rot(q,d\delta)-I]\\=\left[\begin{array}{cccc} 0 & -\delta z & \delta y & dx\\ \delta z & 0 & -\delta x & dy\\ -\delta y & \delta x & 0 & dz\\ 0 & 0 & 0 & 0 \end{array}\right]$

式1.14表示相对于固定参考坐标系的微分变换。在DH建模法中，都是通过前一个关节的坐标系求下一个关节坐标系，因此，还需要找出相对于当前坐标系的微分变换。

坐标系的相对微分变换

设

$^T\Delta$ 为相对自身变换的算子，相对自身的变换通过右乘

$^T\Delta$ 获得：

$[dT]=[\Delta] [T]=[T]^T\Delta\\$

由式1.15可推出：

$^T\Delta=[T^-1]\Delta[T]\\$

由式1.16可以看出，坐标系相对自身变换的算子，可以通过坐标系自身

、相对固定坐标系的微分变换

$\Delta$ 来表示。设T为n、o、a轴和p表示的坐标系。有：

$T=\left[\begin{array}{cccc} nx & ox & ax & px\\ ny & oy & ay & py\\ nz & oz & az & pz\\ 0 & 0 & 0 & 1 \end{array}\right]\\ T^{-1}=\left[\begin{array}{cccc} nx & ny & nz & -p.n\\ ox & oy & oz & -p.o\\ ax & ay & az & -p.a\\ 0 & 0 & 0 & 1 \end{array}\right]\\$

将式1.17代入1.16中，可以算出

$^T\Delta=\left[\begin{array}{cccc} 0 & -\delta\cdot a & \delta\cdot o & \delta \cdot (p\times n)+d\cdot n\\ \delta\cdot a & 0 & -\delta\cdot n & \delta \cdot (p\times o)+d\cdot o\\ -\delta\cdot o & \delta\cdot n & 0 & \delta \cdot (p\times a)+d\cdot a\\ 0 & 0 & 0 & 0 \end{array}\right]\\$

定义

$^T\Delta$ 为：

$^T\Delta=\left[\begin{array}{cccc} 0 & -^T\delta z & ^T\delta y & ^Tdx\\ ^T\delta z & 0 & -^T\delta x & ^Tdy\\ -^T\delta y & ^T\delta x & 0 & ^Tdz\\ 0 & 0 & 0 & 0 \end{array}\right]\\$

于是由1.18和1.19可推出：

$^T\delta x=\delta \cdot n\\ ^T\delta y=\delta \cdot o\\ ^T\delta z=\delta \cdot a\\ ^Td x=n \cdot [\delta\times p +d]\\ ^Td y=o \cdot [\delta\times p +d]\\ ^Td z=a \cdot [\delta\times p +d]$

旋转关节

在DH建模中，对于旋转关节进一步简化式1.20有：

$^T\delta x=n_z\\ ^T\delta y=o_z\\ ^T\delta z=a_z\\ ^Td x=(-n_xp_y+n_yp_x)\\ ^Td y=(-o_xp_y+o_yp_x)\\ ^Td z=(-a_xp_y+a_yp_x)$

移动关节

在DH建模中，对于移动关节进一步简化式1.20有：

$^T\delta x=0\\ ^T\delta y=0\\ ^T\delta z=0\\ ^Td x=n_z\\ ^Td y=o_z\\ ^Td z=a_z$

雅可比矩阵计算

到这里，雅各比矩阵基本上就已经出来了。对于协作臂从关节0坐标系开始，一直到关节6，相对自身进行微分变换，将所有的微分变换加起来，就是末端最终的微分变换：

$\left[\begin{array}{c} ^{T6}dx\\ ^{T6}dy\\ ^{T6}dz\\ ^{T6}\delta x\\ ^{T6}\delta y\\ ^{T6}\delta z \end{array}\right]=\left[\begin{array}{cccccc} ^{T6}J_{11} & ^{T6}J_{12} & . & . & . & ^{T6}J_{16}\\ ^{T6}J_{21} & ^{T6}J_{22} & . & . & . & ^{T6}J_{26}\\ ^{T6}J_{31} & . & . & . & . & ^{T6}J_{36}\\ ^{T6}J_{41} & . & . & . & . & ^{T6}J_{46}\\ ^{T6}J_{51} & . & . & . & . & ^{T6}J_{56}\\ ^{T6}J_{61} & . & . & . & . & ^{T6}J_{66} \end{array}\right]\left[\begin{array}{c} d\theta_1\\ d\theta_2\\ d\theta_3\\ d\theta_4\\ d\theta_5\\ d\theta_6 \end{array}\right]\\$

对于式1.23中的每一列，可根据关节类型用1.21或1.22来求解。有：

$^Td x=^{T6}J_{1i}\\ ^Td y=^{T6}J_{2i}\\ ^Td z=^{T6}J_{3i}\\^T\delta x=^{T6}J_{4i}\\ ^T\delta y=^{T6}J_{5i}\\ ^T\delta z=^{T6}J_{6i}\\$

而1.24中对应使用的坐标系就是每个关节依次相对自身变换得到的坐标系，第

列用

$^{i-1}T_6$ 进行计算。用下图表示末端与各关节的坐标变换关系，

$^{i-1}T_6$ 可用公式表示。

$\begin{align} ^0T_6 &=A_1A_2A_3A_4A_5A_6 \\ ^1T_6 &=A_2A_3A_4A_5A_6\\ ^2T_6&=A_3A_4A_5A_6\\ ^3T_6&=A_4A_5A_6\\ ^4T_6&=A_5A_6\\ ^5T_6&=A_6\\ \end{align}\\$

总结

雅可比矩阵用于将关节速度与末端速度联系起来，雅可比矩阵中的每个元素是对应运动学方程对其中一个变量的导数。雅可比矩阵的行是协作臂的自由度，列是关节数。

酷比程序

关注

0
点赞
踩
13

收藏

觉得还不错? 一键收藏
0
评论
协作臂学习(9)-雅可比矩阵

雅可比矩阵在机器人学中，雅可比矩阵最常用于表示关节速度与末端笛卡尔速度之间的关系，有公式：末端速度表示空间速度，刚体的空间速度包括线（平移）速度和角（旋转）速度部分（比如，地球在太空中绕太阳公转，同时绕南北极轴线自转）。线速度向量包括三个方向的分量角速度向量包括三个轴向的分量将代入中，关节速度在DH建模法中，关节分为转动型（R型，rev...
复制链接

扫一扫