机械臂运动学整理

原创
2023/10/11 14:26
阅读数 1.4K

刚体运动状态描述

空间中的刚体

空间中的刚体,要描述其状态一般需要6个参数,3个平动参数,3个转动参数,分别对应着世界直角坐标系的三个轴X,Y,Z。

整合表达刚体的状态:在刚体上建立坐标系刚体系(body frame),坐标系的原点建立在刚体的质心上。这里需要注意的是,该坐标系的坐标轴不一定与世界直角坐标系的坐标轴平行

刚体平动时,由刚体系的原点位置来判定;刚体转动时,借由刚体系相比于世界坐标轴的姿态来判定。

当然这样的表述只能表述某一个时刻刚体的状态,而我们将刚体整个运动轨迹中各个时刻的这些状态参数全部记录下来

利用各个时刻平动参数对时间的微分,由位移就可以转换到速度和加速度等运动状态;利用各个时刻转动参数对时间的微分,由姿态就可以转换到角速度和角加速度等运动状态。

  • 向量表达空间关系的两个方式
  1. 表达空间中的一个位置
    1. 上式表达的是某一个刚体系的原点在世界坐标系中的位置。
  2. 表达空间上的一个方向
    1. 上图表达的是刚体系的各个轴\(X_B,Y_B,Z_B\)也可以在世界坐标系中用向量来表示,它们只表示方向,在表示方向时,一般取模为1的单位向量。
  • 方向余弦(direct cosines)

方向余弦是指在解析几何里,一个向量的三个方向余弦,分别是该向量与空间直角坐标系的三个轴方向的余弦。

方向余弦矩阵:由两组不同的标准正交基的单位向量之间的方向余弦所形成的矩阵。可以用来表达一组标准正交基与另一组标准正交基的关系。也可以表达一个向量对另一组标准正交基的方向余弦。

  • 量化表达

平动:以向量来描述刚体系{B}的原点相对于世界坐标系的状态。

我们以来说明

我们对{B}的原点分别向世界坐标系中做投影,它们在X、Y、Z轴上的值分别为10、3、3。

转动:描述刚体系{B}对于世界坐标系的姿态——旋转矩阵(Rotation Matrix)

上图中,刚体系{B}的各个轴所指向的方向,可以由下式来表明

上式是一个矩阵,代表由世界坐标系{A}来表征{B},该矩阵的每一列都是3维的列向量,代表{B}各个轴在{A}中的方向,整个矩阵就是一个3*3的样式,它就是一个旋转矩阵。

R的三个列向量即为刚体系{B}的在世界坐标系{A}的,而且是一组正交基。(有关基的概念可以参考线性代数整理 中的空间的基以及线性代数整理(二) 中的正交基与标准正交基)。我们可以使用方向余弦来表述刚体的姿态。

我们假设世界坐标系{A}的三个坐标轴的单位向量分别为\(A_1,A_2,A_3\),刚体系{B}三个坐标轴的单位向量分别为\(B_1,B_2,B_3\)。定义刚体系{B}与世界坐标系{A}坐标轴之间的方向余弦为

\(a_{ij}=cosθ_{ij}=A_i⋅B_j\)

因为\(A_i,B_j\)都是单位向量,模为1,所以上式成立。它可以表述为两个坐标系任意轴的全排列。故而

可以表述为

我们以下例来说明

上图中,蓝色坐标系为世界坐标系{A},红色坐标系为刚体坐标系{B},现在我们要求{B}相对于{A}的姿态

首先,{B}的X‘’轴与{A}的Z轴反向,故而

它代表{B}的X‘’轴在{A}的方向。

{B}的Y''轴与{A}的Y轴同向,故而

{B}的Z''轴与{A}的X轴同向,故而

因此{B}相对于{A}的姿态为:

因为{A}本身的值为\((1,1,1)^T\)

再来看一个更一般的情况。

上图中,世界坐标系{A}的Z轴与刚体系{B}的Z'轴重合。那么我们来看一下它XY平面的俯视图。

我们将刚体系{B}的\(X_B\)\(Y_B\)分别向{A}的X轴,Y轴进行投影。那么得到的相对值就为

又因为Z轴重合,那么就有

那么最终可得{B}相对于{A}的姿态为

旋转矩阵

之前我们知道了该式子的含义,更进一步的,对于的点乘,我们可以理解成刚体系的X轴在世界坐标系的X轴上的投影,其他相同。

因为向量的点乘满足交换律,故而上式又可以写成

从该矩阵的行向量来看,可以看成世界坐标系的一个轴在刚体系的各个坐标轴的投影。如就可以看成是世界坐标系的X轴在刚体系的X轴的投影,其他相同。因为我们平时说的向量都是指的列向量,故而这里会写成,指的是使用刚体系{B}来表征世界坐标系{A}的X轴的转置。

上式又可以写成

故而可以看出两个坐标系互相表征,只不过相差的就是一个转置而已。

旋转矩阵的转置与它自身相乘会得到一个单位矩阵\(I_3\),这又说明它们互为可逆。(关于该部分的内容可以参考线性代数整理 中的矩阵的逆以及线性代数整理(二) 中的标准正交矩阵),这在我们对不同的坐标系进行转换的时候非常方便,只需要进行转置即可,而不需要去求矩阵的逆。

  • 旋转矩阵的三种用法
  1. 描述一个坐标系相对于另一个坐标系的姿态。
  2. 将一个点由某一个坐标系的表达换到另一个跟该坐标系仅有相对转动的坐标系的表达。
    1. 上式的表述为在P点在B坐标系中的位置向量左乘旋转矩阵即可得到它在A坐标系中的位置向量。
  3. 将一个点在同一个坐标系中进行转动。
    1. 上式表述为P点的位置向量左乘旋转矩阵R(θ)得到它旋转到的另一个位置的位置向量,这是在同一个坐标系下进行的。
  • 旋转矩阵与转角

空间中的旋转是3参数的,我们需要将旋转矩阵所表达的姿态,拆解成3次旋转角度以应对3个参数。

拆解成3次旋转需要注意的事项:

  1. 旋转不同于平动,平动的先后顺序可以不予计较,因为一个质点先向X方向移动,再向Y方向移动或者先向Y方向移动,再向X方向移动,效果是一样的,这种特性称为可变换的(commutable)。但是多次转动的先后顺序是需要明确化的,否则,转动后的姿态是不同的。
  2. 旋转的转轴也需要明确定义。是对“固定不动”的转轴旋转,还是对"转动过后的坐标系"的转轴旋转?

两种拆解方式:

  1. 对方向"固定不动"的转轴旋转,称为固定角度(Fixed angles)
  2. 对不断转动的刚体系的转轴方向旋转(刚体系的坐标轴本身是会随时变化的),称为欧拉角度(Euler angles)

旋转矩阵的第三种用法,可以进一步描述物体转动的状态。以旋转矩阵为基础来进行说明。

1、围绕Z轴转动

上图中蓝色的空间直角坐标系为初始状态,它围绕着Z轴逆时针旋转了一个角度θ,得到了末状态的红色的空间直角坐标系,Z轴与初始状态重合。此时的旋转矩阵为

这里简化为

2、围绕X轴转动

旋转矩阵为

3、围绕Y轴转动

旋转矩阵为

例:轴旋转\(30^∘\),求

首先获取旋转矩阵

最后求

  • 固定角度

这里主要是针对连续旋转

上图中定义好了空间直角坐标系{A}(蓝色虚线部分)之后,它的各个轴的方向就不再发生改变,因此被称为固定角度。

而我们的刚体坐标系{B}(红色线部分)一开始与{A}重合,然后沿着X轴逆时针旋转一个角度γ,再沿着Y轴逆时针转动一个角度β,再沿着Z轴逆时针旋转一个角度α。它最终得到的旋转矩阵,我们记为

该式的含义为围绕着X轴、Y轴、Z轴分别转动γ角、β角、α角为Z轴、Y轴、X轴三个旋转矩阵的连乘(先转的放后面,后转的放前面)。这里需要注意的是矩阵乘法是不满足乘法交换律的,所以必须要注意乘法因子的顺序。该式的结果为

关于矩阵乘法的内容可以参考线性代数整理 矩阵和矩阵的乘法。

例:以固定角度的方式,先对X轴旋转60度,后对Y轴旋转30度;先对Y轴旋转30度,后对X轴旋转60度,求这两次转动各自的

先对X轴转60度,再对Y轴转30度

Python代码

import numpy as np
import math

if __name__ == '__main__':

    gamma = math.radians(60)
    beta = math.radians(30)
    alpha = 0

    R_z = np.array([[math.cos(alpha), -math.sin(alpha), 0], [math.sin(alpha), math.cos(alpha), 0],
                    [0, 0, 1]])
    R_y = np.array([[math.cos(beta), 0, math.sin(beta)], [0, 1, 0],
                    [-math.sin(beta), 0, math.cos(beta)]])
    R_x = np.array([[1, 0, 0], [0, math.cos(gamma), -math.sin(gamma)],
                    [0, math.sin(gamma), math.cos(gamma)]])
    R_t = np.dot(R_z, R_y)
    print(np.dot(R_t, R_x))

运行结果

[[ 0.8660254  0.4330127  0.25     ]
 [ 0.         0.5       -0.8660254]
 [-0.5        0.75       0.4330127]]

先对Y轴转30度,再对X轴转60度

Python代码

import numpy as np
import math

if __name__ == '__main__':

    gamma = math.radians(60)
    beta = math.radians(30)
    alpha = 0

    R_z = np.array([[math.cos(alpha), -math.sin(alpha), 0], [math.sin(alpha), math.cos(alpha), 0],
                    [0, 0, 1]])
    R_y = np.array([[math.cos(beta), 0, math.sin(beta)], [0, 1, 0],
                    [-math.sin(beta), 0, math.cos(beta)]])
    R_x = np.array([[1, 0, 0], [0, math.cos(gamma), -math.sin(gamma)],
                    [0, math.sin(gamma), math.cos(gamma)]])
    R_t = np.dot(R_z, R_x)
    print(np.dot(R_t, R_y))

运行结果

[[ 0.8660254  0.         0.5      ]
 [ 0.4330127  0.5       -0.75     ]
 [-0.25       0.8660254  0.4330127]]

这里需要注意的是,先对Y轴转,再对X轴转不能使用之前的结果,而是需要重新去推新的计算公式。我这里推得的为

上面都是已知角度来推算旋转矩阵,现在如果已知旋转矩阵,则可以反过来推算角度。现在我们将旋转矩阵定义形式如下

可以得到,若\(β≠90^∘\)

  1. \(β=Atan2(-r_{31},\sqrt{r_{11}^2+r_{21}^2})\)
  2. \(α=Atan2({r_{21}\over cβ},{r_{11}\over cβ})\)
  3. \(γ=Atan2({r_{32}\over cβ},{r_{33}\over cβ})\)

如果\(β=90^∘\)

  1. \(α=0^∘\)
  2. \(γ=Atan2(r_{12},r_{22})\)

如果\(β=-90^∘\)

  1. \(α=0^∘\)
  2. \(γ=-Atan2(r_{12},r_{22})\)

这里atan2(y,x)是反正切函数atan(\(y\over x\))的变种,它不仅取决于 atan(\(y\over x\))的值,还取决于点(x,y)落入何象限。

  1. 当点(x,y)落入第一象限时,atan2(y,x)的范围是0~\(π\over 2\)
  2. 当点(x,y)落入第二象限时,atan2(y,x)的范围是\(π\over 2\)~π
  3. 当点(x,y)落入第三象限时,atan2(y,x)的范围是-\(π\over 2\)~0
  4. 当点(x,y)落入第四象限时,atan2(y,x)的范围是-π~-\(π\over 2\)

例:以固定角度的方式,反算的旋转角。

用Python来实现

import numpy as np
import math

if __name__ == '__main__':

    m = np.array([[0.866, 0.433, 0.25], [0, 0.5, -0.866], [-0.5, 0.75, 0.433]])
    beta = math.atan2(-m[2][0], math.sqrt(m[0][0]**2 + m[1][0]**2))
    alpha = math.atan2(m[1][0] / math.cos(beta), m[0][0] / math.cos(beta))
    gamma = math.atan2(m[2][1] / math.cos(beta), m[2][2] / math.cos(beta))
    print(math.degrees(beta))
    print(math.degrees(alpha))
    print(math.degrees(gamma))

运行结果

30.000727780827372
0.0
60.00072777015302
  • 欧拉角度

欧拉角度与固定角度不同,上图是先围绕着Z轴旋转α角,此时X、Y轴都发生了转动,由\(X_A\)转动到了\(X_{B'}\)\(Y_A\)转动到了\(Y_{B'}\);此时再围绕Y轴转动时,将不再是围绕着原始的\(Y_A\)进行转动而是\(Y_{B'}\)进行转动β角,此时Z、X轴都发生了转动,由\(Z_{B'}\)转动到了\(Z_{B''}\)\(X_{B'}\)转动到了\(X_{B''}\);同理,在对X轴转动,也不是原始的\(X_A\)或者是\(X_{B'}\)转动,而是围绕着\(X_{B''}\)转动了γ角,此时\(Z_{B''}\)转动到了\(Z_{B'''}\)\(Y_{B''}\)转动到了\(Y_{B'''}\)。它最终得到的旋转矩阵,我们记为

该式的含义,左边的Z'Y'X'只是有别于固定角度的表达而已,没有其他的含义。中间的代表第一次转动后的B'坐标系用A坐标系来表达,第二次转动后的B''坐标系用B'坐标系来表达,第三次转动后的B坐标系用B''来表达(B表达最终坐标系,为上图中的B''')。最右边的\(R_{Z'}(α)R_{Y'}(β)R_{X'}(γ)\)表示对于一个向量,经过了三次转动,得到了;如果我们要将反转回,那么我们肯定需要依次左乘最后一次的旋转矩阵,第二次的旋转矩阵,第一次的旋转矩阵,才能得到最初的,于是就有了

所以这里跟固定角度不同的是,欧拉角度的连乘需要将先转的放前面,后转的放后面。该式的结果为

虽然它跟固定角度的表达式一模一样,但是它们的转轴旋转的顺序是不同的。基于此,我们也可以得出欧拉角度的正转跟固定角度的反转会得到的相同的姿态

例:以欧拉角度的方式,先对X轴旋转60度,后对Y轴旋转30度;先对Y轴旋转30度,后对X轴旋转60度,求这两次转动各自的

先对 X 轴转 60 度,再对 Y 轴转 30 度

Python代码

import numpy as np
import math

if __name__ == '__main__':

    gamma = math.radians(60)
    beta = math.radians(30)
    alpha = 0

    R_z = np.array([[math.cos(alpha), -math.sin(alpha), 0], [math.sin(alpha), math.cos(alpha), 0],
                    [0, 0, 1]])
    R_y = np.array([[math.cos(beta), 0, math.sin(beta)], [0, 1, 0],
                    [-math.sin(beta), 0, math.cos(beta)]])
    R_x = np.array([[1, 0, 0], [0, math.cos(gamma), -math.sin(gamma)],
                    [0, math.sin(gamma), math.cos(gamma)]])
    R_t = np.dot(R_z, R_x)
    print(np.dot(R_t, R_y))

运行结果

[[ 0.8660254  0.         0.5      ]
 [ 0.4330127  0.5       -0.75     ]
 [-0.25       0.8660254  0.4330127]]

先对 Y 轴转 30 度,再对 X 轴转 60 度

Python代码

import numpy as np
import math

if __name__ == '__main__':

    gamma = math.radians(60)
    beta = math.radians(30)
    alpha = 0

    R_z = np.array([[math.cos(alpha), -math.sin(alpha), 0], [math.sin(alpha), math.cos(alpha), 0],
                    [0, 0, 1]])
    R_y = np.array([[math.cos(beta), 0, math.sin(beta)], [0, 1, 0],
                    [-math.sin(beta), 0, math.cos(beta)]])
    R_x = np.array([[1, 0, 0], [0, math.cos(gamma), -math.sin(gamma)],
                    [0, math.sin(gamma), math.cos(gamma)]])
    R_t = np.dot(R_z, R_y)
    print(np.dot(R_t, R_x))

运行结果

[[ 0.8660254  0.4330127  0.25     ]
 [ 0.         0.5       -0.8660254]
 [-0.5        0.75       0.4330127]]

Z-Y-Z的欧拉角度

上图中先转的是\(Z_A\)轴,转动角度为α,\(Y_A\)会转到\(Y_{B'}\)\(X_A\)会转到\(X_{B'}\);然后会围绕\(Y_{B'}\)转动β角度,\(X_{B'}\)会转到\(X_{B''}\)\(Z_{B'}\)会转到\(Z_{B''}\);最后会围绕\(Z_{B''}\)转动γ角度,\(X_{B''}\)会转动到\(X_{B'''}\)\(Y_{B''}\)会转动到\(Y_{B'''}\)。它最终得到的旋转矩阵,我们记为

该式的结果为

对于欧拉角度来讲,同样可以由旋转矩阵来反推旋转角度,我们定义Z-Y-Z最后的旋转矩阵为

可以得到,若\(β≠0^∘\)

  1. \(β=Atan2(\sqrt{r_{31}^2+r_{32}^2},r_{33})\)
  2. \(α=Atan2({r_{23}\over sβ},{r_{13}\over sβ})\)
  3. \(γ=Atan2({r_{32}\over sβ},-{r_{31}\over sβ})\)

如果\(β=0^∘\)

  1. \(α=0^∘\)
  2. \(γ=Atan2(-r_{12},r_{11})\)

如果\(β=180^∘\)

  1. \(α=0^∘\)
  2. \(γ=Atan2(r_{12},-r_{11})\)

例:以欧拉角度的方式,反算X-Y-Z的旋转矩阵以Z-Y-Z的公式得到的旋转角度。

  • 旋转矩阵与转角小结

三轴转动的种类,比方说X-Y-Z,X-Z-Y,X-Y-X...

我们将这些种类进行排列组合,一共有\(C_3^1C_2^1C_2^1\)=3*2*2=12种,无论是固定角度还是欧拉角度都是如此。

对刚体转动的其他表示法:

  1. 角度-转轴(Angle-axis)表达法
    1. 这里指的是任一方向的单位向量,对其旋转一个角度θ。它同样也是三自由度的,该向量2个,角度一个。
  2. 四元素表达法(Quatemion)
    1. 四元素法是使用复数来处理的(一个实部加三个虚部)

变换矩阵

之前我们说了对于平动来说,我们是以一个向量来描述刚体系 {B} 的原点相对于世界坐标系的状态。

对于转动来说是由刚体系的姿态来判定,具体则由旋转矩阵来代表姿态,可以理解为刚体系三个坐标轴的三个方向

现在我们要将平动和转动进行整合,并可以进行量化处理。

具体如下

该矩阵是一个4*4的矩阵,左上的部分为3*3的旋转矩阵,左下为一个全部为0的行向量,右上为代表平动的3维列向量,右下为1。该矩阵称为齐次变换矩阵(homogeneous transformation matrix)表达为刚体系{B}对世界坐标系{A}的变换。现在我们来看看运算的正确性。

  • 仅有平动

已知在刚体系{B}中的一点P,记为,现在我们要将其转化为世界坐标系{A}中的坐标,则为

该式代表刚体系中的向量加上该刚体系的原点在世界坐标系中的向量就为该点在世界坐标系中的坐标向量。

现在我们用变换矩阵来表达

上式中间的左矩阵为变换矩阵,因为没有转动,所以它的旋转矩阵部分为单位矩阵。因为我们知道矩阵与向量相乘,向量的维度需要等于矩阵的列数,又因为变换矩阵是4*4的方阵,列数为4,所以需要补充到4维,这里我们补充了一个1。展开后可得的向量。

  • 仅有转动

已知在刚体系{B}中的一点P,记为,刚体系的三轴与世界坐标系的三轴并不平行。现在我们要将其转化为世界坐标系{A}中的坐标,则为

该式代表刚体系中的向量经过旋转矩阵变换,得到该点在世界坐标系中的坐标向量。

现在我们用变换矩阵来表达

上式中间的左矩阵为变换矩阵,因为没有平动,所以代表平动的3维列向量的分量全部为0。展开后可得的向量。

  • 平动和转动复合

已知在刚体系{B}中的一点P,记为,刚体系的三轴与世界坐标系的三轴并不平行。我们需要先通过转动,来得到P点在世界坐标系{A}的坐标向量,然后才能以{A}为基准进行平动。

该式代表刚体系中的向量先转动,再平动。

现在我们用变换矩阵来表达

展开后可得的向量。

可连续操作

该式的含义为{B}系对{D}系变换,{D}系再对{C}系变换,最后{C}系再对{A}系变换。它表示一系列的复合的平动和转动的变换。

例:已知,求

直接使用变换矩阵,得

对于这种方式,我们都是先转动再平动;现在我们来看看先平动再转动的方式。

  • 仅有平动

这里只有一个世界坐标系{A},上面的一个点\(P_1\)通过一段位移到达位置\(P_2\)。该过程可以表达为

用变换矩阵来表达

我们知道在线性代数中,任何向量的变换都可以用一个矩阵与向量的乘法来表示。这里的这个矩阵就是变换矩阵D(Q),因为只有平动没有转动,故而旋转矩阵的部分为单位矩阵I;由于这里没有刚体系{B},所以这里平动的3维列向量使用位移来代替,表示想要移动的大小和方向。展开后可得向量。

  • 仅有转动

 这里只有一个世界坐标系{A},上面的一个向量\(P_1\) 转过一个θ角到达\(P_2\)。该过程可以表达为

用变换矩阵来表达

因为只有转动,没有平动,所以代表平动的3维列向量的分量全部为0。展开后可得的向量。

  • 平动和转动复合

  这里只有一个世界坐标系{A},上面的一个向量\(P_1\) 转过一个θ角到达\(P_{1-2}\) ,再从\(P_{1-2}\)通过一段位移 到达位置\(P_2\) 。该过程可以表达为

用变换矩阵来表达

这是先转动再平动的过程,再看一下先平动再转动的过程。

首先从图中可以看出先转动再平动和先平动再转动最终所到达的位置是不一样的

这里只有一个世界坐标系{A},上面的一个向量\(P_1\) 通过一段位移 到达位置\(P_{1-2}\) 再从\(P_{1-2}\) 转过一个θ角到达\(P_2\)。该过程可以表达为

该式与先转动再平动的式子比较,会发现位移也发生了转动。那么变换矩阵可以定义为

例:点先对Z轴转\(30^∘\),然后移动\(P_2\),求\(P_2\)

直接使用变换矩阵,得

这里需要注意的是,Z轴的旋转矩阵为

,代入\(30^∘\),即可得到上面的旋转矩阵的数值。

  • 运动相对性

运动的相对性,当我们对点(向量)进行平动和转动的操作,也可以看成是对坐标轴进行"反向"的平动和转动的操作。

点向前移=坐标轴向后移

点逆时针转=坐标轴顺时针转

  • 变换矩阵的三种用法
  1. 描述一个坐标系相对于另一个坐标系的空间状态。
  2. 将点由某一个坐标系的表达换到另一个坐标系来表达。
  3. 将点(向量)在同一个坐标系中进行平动和转动。
  • 连续运算

上图中有一个点P,现在只知道该点在坐标系{C}中的表达,我们想知道的是P在坐标系{A}中的表达,但是{C}跟{A}没有直接的关联,中间有一个坐标系{B},{B}可以跟{A}关联,{C}可以跟{B}关联。它们的关系式为

这里我们可以看到当两个变换矩阵相乘时,在旋转矩阵的部分可以直接相乘,但是在平动向量的部分却不是直接相加,而是需要将{C}的原点在{B}上的表达左乘{A}对{B}的旋转矩阵转成在{A}上的表达才能进行相加,因为不同坐标系下的向量表达是不能进行相加操作的,只有同一个坐标系下的向量表达才可以

如果是四个坐标系的变换,可以写成

  • 变换矩阵的逆

我们都知道变换矩阵是指的将坐标系{B}中的值转换到坐标系{A}中的值,那如果反过来,已知{A}对{B}的变换矩阵,那么{B}对{A}的变换矩阵的逆就是该矩阵。根据矩阵的逆的性质有

展开,有

该式旋转矩阵的部分有

由此,{A}对{B}的旋转矩阵只需要将{B}对{A}的旋转矩阵转置就可以得到。

该式平动的向量部分有

由此,{A}对{B}的坐标原点等于{B}对{A}的坐标原点左乘负的{B}对{A}的旋转矩阵转置

最终可得变换矩阵的逆的表达式为

  • 连续运算求未知的相关关系

上图中有5个坐标系,如果我们想知道{U}与{D}的关系,其实它有两条路径,一个是{D}、{A}、{U};另一个是{D}、{C}、{B}、{U}。

这两条路径的结果都一致,可以表达为

以上的表达为这五个不同的坐标系的变换矩阵都是已知,如果其中有一个是未知的,可以利用上面的关系求出这个未知的坐标系变换矩阵。

例如,如果未知,则有

由于这里有变换矩阵的逆,所以我们需要知道这些变换矩阵的逆是怎么求的。

又例如未知,则有

  • 连续运算法则

如果两个坐标系{A}和{B}重合(coincide),则有:(这里可以理解同一点(向量)在这两个坐标轴中的表达是相同的,故而变换矩阵是单位矩阵I)

1、{B}对{A}的转轴旋转,使用预乘(premultiply)

{B}依序经过\(T_1、T_2、T_3\)三次变换,则

那么对于一个向量v,它要变换到其他的位置v',则它要左乘则有

这里跟之前的固定角度的方式一样,先转的放后面,后转的放前面。因为这里{A}是不变的,相当于世界坐标系。注意这里是在同一个坐标系中的变换

2、{B}对{B}自身的转轴旋转,使用后置乘法(postmultiply)

{B}依序经过\(T_1、T_2、T_3\)三次变换,则

那么对于一个向量,它要变换到其他的位置,则它要左乘则有

这里跟之前的欧拉角度的方式一样,先转的放前面,后转的放后面。注意这里是在不同的坐标系中的变换

机械臂顺向运动学

运动学:讨论运动状态本身,不考虑"力"的作用;动力学:讨论力/力矩如何产生运动。这里基本只讨论运动学的内容,动力学的内容暂时不考虑。

机械臂

上图是一个平面机械臂的简单结构,多个刚体连杆相串联,具有复杂的几何外形;杆件间可相对移动或转动,由制动器驱动来达成。

  • 对应关系
  1. 需求:一般我们的人手,是伸到空间中的某个位置去做一个动作。机械臂也一样,也是机械臂的末端点状态(位置\(w_P\)、速度......)。
  2. 达成方式:驱动各制动器,驱动方程为
    1. \(w_P=f(θ_1,θ_2,...,θ_n)\)
    2. 该式的含义就是说当我们知道了上图中的\(θ_1、θ_2、θ_3...\)之后,就可以知道机械臂末端点在世界坐标系中的位置。
  • 描述机械臂状态方法
  1. 找出杆件间的相对几何状态。
  2. 在各杆件上建立坐标系,以坐标系状态来代表杆件状态。

机械臂几何描述方法

  • 关节(Joint)
  1. 每个转动或移动的关节具有1个自由度。(只能朝一个方向转或者朝一个方向移动)
  2. 每个关节对某特定轴(Axis)进行旋转或平移。
    1. 上图中绿色的部分就是关节。
  • 连杆(Link)
  1. 连接关节的连杆,为刚体。
  2. 编号方式
    1. Link 0:地杆,不动的连杆
    2. Link 1:和Link 0相连,第一个可动的连杆
    3. Link 2:第二个可动的连杆
    4. 依序下去...
    5. 一般我们会对不动的连杆画上斜线表示不动
  • 精准定义

上图是两个方向不同的转轴(不在同一个平面上),一个是第i轴,一个是第i-1轴。在这个定义之下,第i-1连杆是在i-1轴的前侧;同样第i连杆也是在第i轴的前侧。在这两个转轴之间具有一线段和此两个转轴都相互垂直,那么这段垂线的长度称为杆长(Link length)a。两个转轴放入同一个平面中所形成的夹角称为杆扭(Link twist)α。这样在每2杆之间,用杆长a和杆扭α就可以清晰定义

但若要多杆串联,则需要另外两个参数来描述相邻线段\(a_{i-1}\)\(a_i\)间的相对几何关系。

第一个是\(a_{i-1}\)线段和\(a_i\)线段在第i轴上相隔的距离称为杆偏移(Link offset)d,另一个是\(a_{i-1}\)线段和\(a_i\)线段在一个平面内所形成的夹角称为关节角(Joint angle)θ。

故而需要精确描述一个机械臂,需要四个参数(\(α_i,a_i,d_i,θ_i\))

对于移动和转动不同,只有一个参数是变动的,另外三个是固定的。我们假设第i轴是转动的,也就是说连杆i-1和连杆i是会相互转动的,那么θ是会随着时间变化的,剩下的α、a、d就是固定的参数;如果第i轴是移动的,也就是说连杆i-1和连杆i是会相互移动的,那么d就是会变化的,α、a、θ就是固定的参数。

运动方式 固定参数 变化参数
转动 α、a、d θ
移动 α、a、θ d

在机械臂上建立坐标系

我们在机械臂上建立坐标系,一般会把每一个轴(转轴或移动轴)作为Z轴,轴与轴之间的距离a作为X轴。\(X_i\)的方向跟\(Z_i\)\(Z_{i+1}\)都垂直。

上图就是两个不同的轴所构成的Z轴,X轴与它们俩都垂直。

Y轴则是与X和Z都垂直,满足右手螺旋法则,四指从X到Y的方向旋转,大拇指就是Z的方向。

  • 地杆Link 0

在上图中,第0轴跟第1轴之间的第0杆为地杆,它是不动的。为了简化,地杆的坐标系跟第1杆的坐标系的初始时态,我们会使用同一个坐标系。当我们对第一个关节刚开始进行旋转的时候(这里假设第1轴为旋转轴),此时的\(θ_1\)为0,\(d_1=0\);又或者第1轴为移动轴的时候,刚刚开始移动的时候,此时\(d_1\)为0,\(θ_1=0\)。当然当第一个关节运动了一段时间之后,地杆的坐标系跟第1杆的坐标系将不再重合。

  • 最后一杆(Last link(n))

由于最后一个连杆没有下一个连杆相连,因此没有\(a_n\)这条线段。一般我们会将最后一个连杆的\(X_n\)轴选择与\(X_{n-1}\)轴同向。当最后一杆的第n轴为转动轴时,它的\(d_n=0\)\(θ_n\)随意;当最后一杆的第n轴为移动轴时,它的\(θ_n=0\)\(d_n\)随意。

连杆变换(Link Transformations)

如果在各个连杆都建立了坐标系,那么在第一篇中的内容就可以使用了。

假设有一点P,我们知道了它在第i轴坐标系上的位置,又知道了第i-1坐标系对第i坐标系的变换矩阵,那么就可以知道该点在第i-1坐标系中的位置坐标

为了使得i-1的坐标系能够表达i坐标系,我们需要建立几个中间的坐标系。首先我们转动i-1轴\(α_{i-1}\)角,使得\(Z_{i-1}\)轴与第i坐标系的\(Z_i\)轴平行,此时将转动后的坐标轴记为\(Z_R\)\(X_{i-1}\)轴保持不变,另记为\(X_R\)轴。此时我们将\(Z_R\)轴跟\(X_R\)轴平移到第i轴上,变成\(Z_Q\)\(X_Q\)轴。此时我们会将\(X_Q\)绕着\(Z_Q\)轴做一个转动\(θ_i\)角到达\(X_P\)轴,\(Z_Q\)轴保持不变,记为\(Z_P\)轴。我们将\(Z_P\)\(X_P\)轴向上拉,就可以跟\(Z_i\)\(X_i\)轴重合。这里我们知道从i-1坐标系到i坐标系,中间经历了3个坐标系的变换(RQP),那么就可以写为

它满足欧拉角度的变换规则(从先到后)。那么这四个变换矩阵刚好是对机械臂的四个参数进行操作。

第一个是i-1坐标系沿着X轴转动\(α_{i-1}\)角变成R坐标系,第二个是R坐标系沿着X轴平动\(a_{i-1}\)距离变成Q坐标系,第三个是Q坐标系沿Z轴转动\(θ_i\)角变成P坐标系,第四个是P坐标系沿着Z轴平动\(d_i\)距离与第i坐标系重合。最后展开,可得

具体推导过程如下

  • 连续连杆变换

上面我们得到了两个连接连杆的变换矩阵,那么多个连杆的变换矩阵也就可以得到了

例一:表述下图机械臂中的几何参数。

上图是一个平面的结构,有3个关节,3个连杆。

首先,上图中的红色实心圆代表三个关节的轴,它们都是转轴。以这三个轴建立坐标系,垂直于纸面向外纸面的方向定义为Z轴,逆时针旋转的方向定义为正方向,依次就定义好了\(Z_1,Z_2,Z_3\)三个轴。Z轴之间的连线,定义为X轴,X轴的定义是从\(Z_{i-1}\)\(Z_i\)\(X_{i-1}\)轴,故而从\(Z_1\)\(Z_2\)\(X_1\)轴,\(Z_2\)\(Z_3\)\(X_2\)轴。当然还要考虑到首尾,地杆也就是首杆,它的坐标系跟第1轴初始状态是重合的,那么Z轴是重合的,\(X_0\)轴是\(θ_1\)=0时的方向。最后一杆在上图中为第3轴的下侧,\(X_3\)轴一般定义成当\(θ_3\)=0时与\(X_2\)轴同向。那么该机械臂的参数表如下

i \(α_{i-1}\) \(a_{i-1}\) \(d_i\) \(θ_i\)
1 0 0 0 \(θ_1\)
2 0 \(L_1\) 0 \(θ_2\)
3 0 \(L_2\) 0 \(θ_3\)

这里因为三个Z轴都是平行的,所以所有的α都是0;地杆与第一杆的Z轴是重合的,所以\(a_0=0\),其他都是Z轴之间的距离;因为三个Z轴都是平行的,所以这里不考虑它们因为两两角度不同所产生的杆长在空间上的距离偏差,所以d都是0;地杆的X轴是第一杆初始状态时的方向,但是现在第一杆已经发生了转动,故而第一杆的关节角为\(θ_1\),第一杆与第二杆的X轴所形成的关节角为\(θ_2\),第三杆(也就是最后一杆)的初始状态的X轴与第二杆的X轴方向相同,但是现在第三杆已经发生了转动,故而现在第二杆与第三杆的X轴所形成的关节角为\(θ_3\)

现在我们看到上图中的P点,假设该点就是机械臂末端的爪要抓东西的位置,由图中可知,P点在第3轴坐标系的坐标为。我们如果想要知道P点对地杆坐标系的关系,那么我们就可以使用连续连杆变换公式结合两杆之间的变换矩阵公式进行求解。

例二:表述下图机械臂中的几何参数。

这里也同样有3个关节,3个连杆。

这三个关节的轴如上图所示,第1轴为转轴,第2轴为移动轴,第3轴为转轴。对于第1轴来说,它是逆时针旋转的,故\(Z_1\)轴方向向上。第2轴为移动轴,它是左右移动的,我们定义向右为正方向,故\(Z_2\)轴方向向右;第3轴为转轴,它也是逆时针旋转的,故\(Z_3\)轴方向也向右。这样依次就定义好了\(Z_1,Z_2,Z_3\)三个轴。这里由于\(Z_1、Z_2\)处于同一个平面上且相互垂直,它们没有空间距离,所以X轴的选取失去了依据,这里是任意选取一个方向作为\(X_1\)轴,\(X_2\)轴与\(X_1\)平行。考虑首尾,地杆轴为第1轴初始状态相同,我们定义\(X_1\)轴的方向就为第1轴的初始状态,故而地杆坐标系与第1杆坐标系重合。最后一杆就是第3杆, \(Z_3\)轴跟\(Z_2\)是同方向,\(X_3\)轴初始状态与\(X_2\)轴同向。那么该机械臂的参数表如下

i \(α_{i-1}\) \(a_{i-1}\) \(d_i\) \(θ_i\)
1 0 0 0 \(θ_1\)
2 \(90^∘\) 0 \(d_2\) 0
3 0 0 \(L_2\) \(θ_3\)

这里因为地轴与第1轴重合,故\(α_0=0\)\(Z_1\)\(Z_2\)垂直,故\(α_1=90^∘\)\(Z_2\)\(Z_3\)同向,故\(α_2=0\);又因为\(Z_1,Z_2,Z_3\)在同一个平面上,它们没有空间距离,故a都为0;地轴与第1轴重合,\(d_1=0\)\(X_1\)轴到\(X_2\)轴的空间距离为\(d_2\)\(X_2\)轴到\(X_3\)轴的空间距离为\(L_2\),故\(d_3=L_2\);地杆与第1杆如上图中是已经转动了\(θ_1\)角,第2轴为移动轴,无法转动,所以\(θ_2=0\),而第3杆转动了\(θ_3\)角。

上图中的P点位于第3轴的,同样我们也可以使用连续连杆变换公式结合两杆之间的变换矩阵公式算得它对地的位置。

  • 选择轴的任意性

\(a_1=0\)的时候,意味着Z轴相交。

\(Z_2\)有两个选择

在上图中\(Z_2\)的方向可以向右,也可以向左。

\(X_1\)有两个选择

比较上下两个图,\(X_1\)可以向下,也可以向上。一般我们会根据转动的方向和平移的方向来定它们的方向。

  • 现实中的机械臂

这是一种芯片晶元机械臂,是从晶元柜中取出晶元放入设备中的自动化设备。它有四个自由度。

参数表

i \(α_{i-1}\) \(a_{i-1}\) \(d_i\) \(θ_i\)
1 0 0 \(d_1\) 0
2 0 0 \(d_2\) \(θ_2\)
3 0 \(a_2\) \(d_3\) \(θ_3\)
4 0 \(a_3\) \(d_4\) \(θ_4\)

从上图中,我们可以看出,它所有的Z轴都是竖直向上平行的,故而所有的α都是0;\(Z_0、Z_1、Z_2\)在同一条直线上,故而\(a_0、a_1\)都为0,这里的第2杆其实是水平的,它并不是向上伸展的(会有视觉误差),故而\(a_2\)是垂直于\(Z_2、Z_3\)的,同理第3杆也是水平的,\(a_3\)也是垂直于\(Z_3、Z_4\);这里除了\(d_1\)以外,\(d_2、d_3、d_4\)都是连杆的厚度,\(d_1\)是伸缩可变的;这里需要说明的是第1轴是移动轴,它可以上下移动,故而\(θ_1=0\)\(θ_2\)\(X_1\)轴与\(X_2\)轴所形成的夹角,\(θ_3\)\(X_2\)轴与\(X_3\)轴所形成的夹角,\(θ_4\)\(X_3\)轴与\(X_4\)轴所形成的夹角,第2轴、第3轴、第4轴都是转轴。像这样的机械臂我们称为PRRR(平转转转)机械臂。

RRRP(转转转平)机械臂

这是一个先转动,再平动拿取物品的机械臂。它也是四个自由度。

参数表

i \(α_{i-1}\) \(a_{i-1}\) \(d_i\) \(θ_i\)
1 0 0 \(d_1\) \(θ_1\)
2 0 \(a_1\) \(d_2\) \(θ_2\)
3 0 \(a_2\) 0 \(θ_3\)
4 0 0 \(d_4\) 0

该机械臂的所有Z轴也都是竖直向上平行的,故α都是0;\(Z_0、Z_1\)成一条直线,故\(a_0=0\)\(Z_1\)轴到\(Z_2\)轴的距离为\(a_1\)\(Z_2\)轴到\(Z_3\)轴的距离为\(a_2\)\(Z_3\)轴跟\(Z_4\)轴在一条直线上,故\(a_3=0\)\(d_1\)为地杆的长度加上第一杆的厚度,\(d_2\)为第二杆的厚度,\(X_2\)轴跟\(X_3\)轴在一个平面上,故而\(d_3=0\)\(X_3\)\(X_4\)轴的距离为\(d_4\);第1轴、第2轴跟第3轴都是转轴,\(X_0\)\(X_1\)的转角为\(θ_1\)\(X_1\)\(X_2\)的转角为\(θ_2\)\(X_2\)\(X_3\)的转角为\(θ_3\),第4轴为移动轴,故而\(θ_4=0\)

致动器、关节和笛卡尔空间

在上图中,如果我们知道了\(θ_1、θ_2、θ_3\)就可以知道P点在世界坐标系中的表达法。方法就是找到各个关节坐标系的变换矩阵T以及P点在第3轴坐标系中的坐标,再通过各种计算就可以得到结果,这个过程,我们称为顺向运动学(Forward Kinematics)

但我们的需求其实是我们知道的是P点在世界坐标系中的坐标,我们要知道我们的机械臂要如何通过连杆、关节的运动抵达该点P,其实就是求关节坐标系的最终位置。该过程,我们称为逆向运动学(Inverse Kinematics)

在逆向运动学中,由连结致动器和关节的机构决定。

在上图中,我们的致动器就是马达,马达一般不会直接去驱动关节,而是经过减速器来驱动关节。马达一般属于高转速,低扭力的,而连杆的转动一般是低转速的,但是需要高扭力,所以需要经过一个减速器。上图的减速就是\(1\over 2\)

机械臂逆向运动学

给予{H}或,求得\(θ_i\)

假设机械臂有6个自由度

上面是一个示意图,只标示出了地杆坐标系跟第6杆坐标系。那么它有6个未知的关节角或者杆偏移(\(θ_i\)\(d_i\),i=1,...,6)。之前我们在向量的坐标系变换中知道

这里我们假设{A}是世界坐标系,{B}是最后一杆的坐标系,这里是已知,这里A=0。那么在中,W=0,H=6,即

这里我们需要求解12个非线性方程式,其中包括6个未知数,6个限制条件

  • 几个概念
  1. 可到达空间(Reachable workspace):机械臂可以用一种以上的姿态到达的位置;
  2. 灵活空间(Dexterous workspace):机械臂可以用任何的姿态到达的位置;
  3. 灵活空间是可到达空间的子集;
    1. 上图是一个包含两个转轴的二轴机械臂,杆长\(l_1>l_2\)。当\(l_2\)完全伸展,与\(l_1\)在同一条直线且同向时,可以画出圆环的最外径;当\(l_2\)完全收缩,与\(l_1\)在同一条直线且反向时,可以画出圆环的最内径;在圆环的最外经和最内径之间的整个圆环区域内,该机械臂都可以抵达,那么这个圆环区域就是可到达空间
    2. 上图的杆长\(l_1=l_2\),它的可到达区域是整个圆,但可以被称为灵活空间的只有圆心,因为只要当\(l_2\)完全收缩,与\(l_1\)在同一条直线且反向时,无论\(l_1\)呈何角度,它都可以到达圆心。
  4. 子空间(Subspace):机械臂在定义头尾的T(变换矩阵)所能到达的变动范围。
    1. 在空间中可以包含的移动和转动很多,但是机械臂不一定所有的选择都能到达,所能到达的区域就是可到达空间,可以转换为T的一部分,即为子空间。
    2. 上图是一个两个自由度的机械臂,第1轴为旋转轴,第2轴为移动轴。从俯视的角度看如下图
    3. 所以它有一个\(θ_1\)(\(X_0\)\(X_1\)的夹角)和\(d_2\)(\(X_1\)\(X_2\)的距离)是可变化的。机械臂的末端点想要到达某个(x,y)。当我们决定了(x,y)也就决定了机械臂各轴的姿态。
    4. 上式为使用世界坐标系来表达第2轴坐标系的变换矩阵。首先,我们要知道第2轴在第0轴中的姿态,即它的旋转矩阵,在未发生移动之前,我们只需要看第2轴的坐标系原点与第0轴重合即可,重合后,\(X_2\)位置向量为\([cosθ_1,sinθ_1,0]^T\),这里\(cosθ_1={y\over \sqrt{x^2+y^2}}\)\(sinθ_1={-x\over \sqrt{x^2+y^2}}\)(这里x本身为负);\(Y_2\) (方向竖直向下)位置向量为\([0,0,-1]^T\)\(Z_2\)位置向量为\([-sinθ_1,cosθ_1,0]^T\),则构成了上式中的旋转矩阵部分,平动部分的位置向量即为\([x,y,0]^T\),则构成了上式中的变换矩阵。
    5. 从上式可以看出,(x,y)已知不仅是变换矩阵平动部分已知,而且旋转矩阵部分也已知了,则\(Z_2\)方向(平动的方向)已知,那么没有多的自由度可以让机械臂从各个方向到达(x,y),这也符合了可到达空间的概念,而不是灵活空间。又由变换矩阵可以看出机械臂能够到达的空间是很有限的,随着(x,y)的确定,旋转矩阵原本可以变化的9个参量只剩下了4个参量可以变化((1,1),(2,1),(1,3),(2,3)),而其他5个参量都是固定的,这就说明机械臂有很多的姿态是摆不出来的。也就是说机械臂的自由度有限,被T限制了活动范围,也就是子空间。

多重解

由于是非线性方程式,6个未知数6个方程式不代表有唯一解。而是由关节数和连杆参数决定的。    

例如一个RRRRRR(6转轴)机械臂

\(a_i\) 解的数目
\(a_1=a_3=a_5=0\) ≤4
\(a_3=a_5=0\) ≤8
\(a_3=0\) ≤16
All \(a_i≠0\) ≤16

上表说明的是该机械臂解的数目跟杆长有很大的关系,随着杆长的数目越多,解的数目也越多。当连杆的设计比较简单的时候,解的数目就越少;当连杆设计的比较复杂,解的数目就越多。

上图是一个6转轴机械臂前3轴针对同一个空间位置的4种姿态。在这4种姿态中,手腕(第4轴)转动都会有2种姿态,故而6转轴机械臂一共会有4*2=8组解。

但是这8组解有时候会因为几何的限制,并非每一种解都可以运作。例如在上图中的第4副图,当大手臂需要向后转动,下面有一个装置使得小手臂会发生碰撞,无法运动过去的时候,该解就无效。虽然在理论上有8组解,但是在真实场景中,可能有些解是无法使用的,故而对于任何的姿态,其实解越多越好,如果有几何限制的话,解越多越容易避免这些情况。

  • 解的选择方式
  1. 离目前状态最近的解
    1. 最快
    2. 最省能
    3. 对于空间中的一个点,机械臂到达该点可以呈现两种姿态,如上图
    4. 空间中可以呈现两种姿态的点为上图中的B点,而机械臂初始状态位于A点,此时从A点运动到B点,必然是上姿态为最快最省能的。
  2. 避开障碍物
    1. 但如果上姿态存在障碍物无法到达,那么下姿态就成了唯一解。

求解方法

  • 解析法

使用代数或几何的方式求解。目前大多数机械臂都会设计成具有解析解。

相邻三轴相交一点必有解析解

对于RRR(三转轴)机械臂,给定(x,y,φ),求(\(θ_1,θ_2,θ_3\))。

首先,我们来看一下机械臂顺向运动学的变换矩阵

这里的\(c_{123}\)表示\(cos(θ_1+θ_2+θ_3)\)\(s_{123}\)表示\(sin(θ_1+θ_2+θ_3)\)\(c_{12}\)表示\(cos(θ_1+θ_2)\)\(s_{12}\)表示\(sin(θ_1+θ_2)\)

推导过程如下

这里只需要将\(c_{123}\)\(s_{123}\)\(c_{12}\)\(s_{12}\)按照两角和的正余弦公式

  1. sin(α+β)=sinαcosβ+cosαsinβ
  2. cos(α+β)=cosαcosβ-sinαsinβ

展开即可得到上图中的结果。

然后,我们再来看一下机械臂逆向运动学的变换矩阵

它同样是先将第3轴与地杆坐标轴重合

则第3轴的X轴在第0轴的位置向量为\([cosφ,sinφ,0]^T\),Y轴在第0轴的位置向量为\([cos(φ+90^∘),sin(φ+90^∘),0]^T=[-sinφ,cosφ,0]^T\),Z轴在第0轴中的位置向量为\([0,0,1]^T\),平动部分的位置向量为\([x,y,0]^T\),故而可得上面的变换矩阵。

将两个变换矩阵联立,它们是相等的,可以得到方程式。

  • 几何法:将空间几何切割成平面几何。

我们将两个连杆连成一个三角形(如上图中的绿线部分),由三角形的几何关系,有

\(x^2+y^2=l_1^2+l_2^2-2l_1l_2cos(180^∘-θ_2)\)

这里上式的左边是勾股定理,右边是余弦定理,它都等于上图中非连杆绿线的长度平方,则

\(cosθ_2={x^2+y^2-l_1^2-l_2^2\over 2l_1l_2}\)

这里\(θ_2\)会有两个解,一个大于\(0^∘\),一个小于\(0^∘\),它对应着上图中绿的两种不同姿态,这两种姿态都可以到达(x,y)点,当绿色姿态时,\(θ_2>0^∘\)(逆时针);当青色姿态时,\(θ_2<0^∘\)(顺时针)。

同样由余弦定理,有

\(cosψ={x^2+y^2+l_1^2-l_2^2\over 2l_1\sqrt{x^2+y^2}}\)

ψ是三角形内角,则\(0^∘<ψ<180^∘\),由于\(θ_2\)有两个解,那么\(θ_1\)也有两个解。

  1. \(θ_2<0^∘\)\(θ_1=atan2(y,x)+ψ\)
  2. \(θ_2>0^∘\)\(θ_1=atan2(y,x)-ψ\)

故而\(θ_3=φ-θ_1-θ_2\)

例:量化计算

上图是一个机械臂的位置形态,第1杆长\(l_1\)为5,第2杆长\(l_2\)为2,我们希望能够到达(3,5)的位置。

首先我们要算出第2关节角\(θ_2\)

\(cosθ_2={x^2+y^2-l_1^2-l_2^2\over 2l_1l_2}={3^2+5^2-5^2-2^2\over 2*5*2}={1\over 4}\)

\(θ_2=acos0.25=75.5^∘\)

再求ψ值,有

\(cosψ={x^2+y^2+l_1^2-l_2^2\over 2l_1\sqrt{x^2+y^2}}={3^2+5^2+5^2-2^2\over 2*5*\sqrt{3^2+5^2}}={55\over 58.3}=0.94\)

\(ψ=acos0.94=19.4^∘\)

由于我们取的\(θ_2>0^∘\)\(θ_1=atan2(y,x)-ψ=atan2(5,3)-19.4=59-19.4=39.6^∘\)

最后可得\(θ_3=φ-θ_1-θ_2=45-39.6-75.5=-70.1^∘\)

Python代码

import math

def theta2(x, y, l1, l2):
    c2 = (x**2 + y**2 - l1**2 - l2**2) / (2 * l1 * l2)
    return math.degrees(math.acos(c2))

def theta1(x, y, l1, l2):
    cs = (x**2 + y**2 + l1**2 - l2**2) / (2 * l1 * math.sqrt(x**2 + y**2))
    s = math.degrees(math.acos(cs))
    return math.degrees(math.atan2(y, x)) - s

def theta3(f, theta1, theta2):
    return f - theta1 - theta2

if __name__ == '__main__':

    t1 = theta2(3, 5, 5, 2)
    print(t1)
    t2 = theta1(3, 5, 5, 2)
    print(t2)
    t3 = theta3(45, t1, t2)
    print(t3)

运行结果

75.52248781407008
39.63961778937328
-70.16210560344336

 

展开阅读全文
cos
加载中
点击引领话题📣 发布并加入讨论🔥
打赏
0 评论
0 收藏
0
分享
返回顶部
顶部