《机器人学一(Robotics(1))》_台大林沛群 第 4 周【机械臂 逆运动学】 Quiz 4

Gaoshu1012023-09-04 19:46
  • 待完善： 第5-7【暂时不清楚如何确定】

谁做出来了，麻烦指下路，谢谢！

  • 第6-7： 连猜带蒙🤣

####################################################

整个流程 走下来，只剩一个解了

不理解 第5-7题 为啥 还有双解。

##########################################################

coursera链接

文章目录

      • 第1题
      • 第2题
      • 第3题
      • 第4题
      • 第5题-8
        • [求解 θ3-θ1的 Python 代码](#求解 θ3-θ1的 Python 代码)
      • 第8题
        • [求解 θ4 - θ6的 Python 代码](#求解 θ4 - θ6的 Python 代码)
        • [θ 确定过程](#θ 确定过程)
        • [正运动学 求解 T_06 验证代码 _Python](#正运动学 求解 T_06 验证代码 _Python)
        • Matlab代码_参考

第1题

i = 2

1)、根据右手定则 ： 右手拇指(Z), 四指(X)，掌心朝向(Y)

  • Z 为 垂直 纸面 朝外

2)、右手拇指指向 X ^ 1 \hat{X}{1} X^1，将 Z ^ 1 \hat{Z}{1} Z^1旋转到 Z ^ 2 \hat{Z}_{2} Z^2的方向，旋转方向与四指弯曲方向相反，为负， α为 -90。

第2题

2、

第3题

3、



t a n ( θ 1 ) = Y X tan(θ_1)=\frac{Y}{X} tan(θ1)=XY

python 复制代码 
import math
θ1 = 180 * math.atan(40/69.28)/ math.pi  ## 弧度转角度
print(θ1)  ## 30

答案： 30

套了半天公式的我🤣

第4题


参考点： D(桌角) Desk

杯子------> 桌角------> 机械手

易于找到P， 用这个公式：

求R：

这里的转轴为 Y

注意角度的正负判断： 右手拇指指向Y， 四指弯曲方向为正

题中 右手拇指指向 Y ^ C \hat{Y}_C Y^C ， X ^ C \hat{X}_C X^C 相对于 X ^ D \hat{X}_D X^D 转向 与四指弯曲方向相反， 为负

python 复制代码 
import numpy as np 

## 无转动
R_WD = [[1, 0, 0],
       [0, 1, 0],
       [0, 0, 1]]
a = np.row_stack((R_WD,[[0, 0, 0]]))  ## 扩展 行
P = np.array([[830, 20, 330, 1]])
T_WD = np.column_stack((a,P.T))  ## 扩展 列， 注意 转置
# print(T_WD)


## 绕  Y轴 转
θ = np.pi * (-60)/180   ## 注意 正负 判断
R_DC = [[np.cos(θ), 0, np.sin(θ)],
       [0, 1, 0],
       [-np.sin(θ), 0, np.cos(θ)]]
# print(R_DC)

a = np.row_stack((R_DC,[[0, 0, 0]]))
P = np.array([[-500, 452, 410, 1]])
T_DC = np.column_stack((a,P.T))

T_WC = np.dot(T_WD, T_DC) 

T_WC = [[float(format(x, '.3g')) for x in T_WC[i]] for i in range(len(T_WC))]  ## 保留 3位有效数字
print(T_WC)

答案： 0.5//-0.866//0.866//330//472//740

############################

补充： 课件里 是沿着 Z轴转

############################

第5题-8

参考 PPT Pieper's Solution 部分， 题5-8一起做，因为由于 θ1的范围限制，可以排除一些 θ3 值。 但 θ2， θ1的选项仍有很多。

求解 θ3-θ1的 Python 代码

最终版本：

python 复制代码 
import numpy as np 

########################## 求 T_WC
## 无转动
R_WD = [[1, 0, 0],
       [0, 1, 0],
       [0, 0, 1]]
a = np.row_stack((R_WD,[[0, 0, 0]]))  ## 扩展 行
P = np.array([[830, 20, 330, 1]])
T_WD = np.column_stack((a,P.T))  ## 扩展 列， 注意 zhuan
# print(T_WD)


## 绕  Y轴 转
θ = np.pi * (-60)/180   ## 注意 正负 判断
R_DC = [[np.cos(θ), 0, np.sin(θ)],
       [0, 1, 0],
       [-np.sin(θ), 0, np.cos(θ)]]
# print(R_DC)

a = np.row_stack((R_DC,[[0, 0, 0]]))
P = np.array([[-500, 452, 410, 1]])
T_DC = np.column_stack((a,P.T))

T_WC = np.dot(T_WD, T_DC) 

# T_WC = [[float(format(x, '.3g')) for x in T_WC[i]] for i in range(len(T_WC))]  
# print(T_WC)   ## 第 4 题答案
#############################################

### 求 T_06

# 求 T_W0
# α, a, d, θ = 0, 0, 373, 0
## 无转动 
T_W0 = [[1, 0, 0, 0],
      [0, 1, 0, 0],
      [0, 0, 1, 373],
      [0, 0, 0, 1]]
# print(T_W0)

# 求 T_6C  Xc 和 Z6 方向相同，  Yc和 Y6 反向， Zc 和 X6 同向
T_6C = [[0, 0, 1, 0],
      [0, -1, 0, 0],
      [1, 0, 0, 206],
      [0, 0, 0, 1]]

T = np.dot(np.linalg.inv(T_W0), T_WC)
T_06 = np.dot(T, np.linalg.inv(T_6C))
# print(T_06)

P_04 = P_06 = np.array([[227, 472, 188.59876682, 1]])
x, y, z  = 227, 472, 188.59876682
################## 


α2, a2, d3 = 0, 340, 0   ## θ3
α3, a3, d4 = np.pi*(-90)/180, -40, 338   ## θ4
'''
## 仅与 θ3 有关
f1 = a3 * np.cos(θ3) + d4 * np.sin(α3)*np.sin(θ3) + a2 
#f2 = a3 * np.cos(α2)*np.sin(θ3) - d4 * np.sin(α3)*np.cos(α2)*np.cos(θ3) -\
#        d4 * np.sin(α2)*np.cos(α3) - d3 * np.sin(α2)
f2 = a3 * np.sin(θ3) - d4 * np.sin(α3) * np.cos(θ3)      
        
#f3 = a3 * np.sin(α2)* np.sin(θ3)- d4 * np.sin(α3)* np.sin(α2) * np.cos(θ3) + \
#       d4 * np.cos(α2) * np.cos(α3) + d3 * np.cos(α2)
f3 = d4 * np.cos(α3)
# print(f3)
'''
# 对 i= 1 i= 2
α0, a0, d1 = 0, 0, 0   ## θ1
α1, a1, d2 = np.pi*(-90)/180, -30, 0  ## θ2

'''
## 和 θ2，θ3有关
g1 = np.cos(θ2)* f1 - np.sin(θ2) * f2 + a1
# g2 = np.sin(θ2)* np.cos(α1) * f1 + np.cos(θ2) * np.cos(α1)* f2 -\
#      np.sin(α1) * f3 - d2 * np.sin(α1)
## 化简
g2 = np.sin(θ2)* np.cos(α1) * f1 + np.cos(θ2) * np.cos(α1)* f2 -\
     np.sin(α1) * f3
# g3 = np.sin(θ2)* np.sin(α1) * f1 + np.cos(θ2) * np.sin(α1)* f2 +\
#      np.cos(α1) * f3 + d2 * np.cos(α1)
## 化简
g3 = np.sin(θ2)* np.sin(α1) * f1 + np.cos(θ2) * np.sin(α1)* f2 +\
     np.cos(α1) * f3 

'''

'''
## a1 不等于 0 
k1 = f1 
k2 = -f2 
#k3 = f1**2 + f2**2 + f3**2 + a1**2 + d2**2 + 2*d2*f3
k3 = f1**2 + f2**2 + f3**2 + a1**2
#k4 = f3 * np.cos(α1) + d2 * np.cos(α1)
k4 = 0
'''

## 
r = x**2 + y**2 + z**2   ## 可解

'''
f1 = a3 * np.cos(θ3) + d4 * np.sin(α3)*np.sin(θ3) + a2
f2 = a3 * np.sin(θ3) - d4 * np.sin(α3) * np.cos(θ3) 
f3 = d4 * np.cos(α3)
k1 = f1 
k2 = -f2
k3 = f1**2 + f2**2 + f3**2 + a1**2
k4 = 0
'''
### 解 超越方程
from sympy import *
θ3 = symbols('θ3')

f = (r-a1**2- (a3 * cos(θ3) + d4 * sin(α3)*sin(θ3) + a2 )**2 \
     - (a3 * sin(θ3) - d4 * sin(α3) * cos(θ3))**2 \
     - (d4 * cos(α3))**2 )**2/(4 * a1**2) \
  + z**2/(sin(α1))**2 \
  - (a3 * cos(θ3) + d4 * sin(α3)*sin(θ3) + a2)**2 \
  - (a3 * sin(θ3) - d4 * sin(α3) * cos(θ3))**2

# f = ((r - k3)**2)/(4*a1**2) + ((z-k4)**2)/(np.sin(α1))**2 - k1**2 - k2**2
root3 = solve([f],[θ3])
print('θ3(弧度值): ', root3)

# lis = [(-3.05085978803173,), (-2.76035600105476,), (-0.616827296276209,), (-0.326323509299240,)]
θ3_du = [180 * root3[i][0] / np.pi for i in range(len(root3))]  ## θ3 弧度换角度
print('θ3(以度为单位): ', θ3_du )  # [-174.801389740395, -158.156748814047, -35.3416007650924, -18.6969598387445]
python 复制代码 
## 求解 θ2
# 结果汇总
# θ3(以度为单位):  [-174.801389740395, -158.156748814047, -35.3416007650924, -18.6969598387445]
# θ3 = -3.05085978803173(无满足要求的θ1), -2.76035600105476(符合), -0.616827296276209(符合), -0.326323509299240(无满足要求的θ1)

# θ2 =[-2.82490122970046, -2.09516142685496],\
    # [0.207932140057394, 0.864458274997230]
    #  [-0.864458274997158, -0.207932140057458]

# θ3的有效解   -2.76035600105476 ,  -0.616827296276209 
θ3 =  -0.616827296276209 

f1 = a3 * np.cos(θ3) + d4 * np.sin(α3)*np.sin(θ3) + a2 
f2 = a3 * np.cos(α2)*np.sin(θ3) - d4 * np.sin(α3)*np.cos(α2)*np.cos(θ3) - d4 * np.sin(α2)*np.cos(α3) - d3 * np.sin(α2)
f3 = a3 * np.sin(α2)* np.sin(θ3)- d4 * np.sin(α3)* np.sin(α2) * np.cos(θ3) + d4 * np.cos(α2) * np.cos(α3) + d3 * np.cos(α2)

k1 = f1 
k2 = -f2 
k3 = f1**2 + f2**2 + f3**2 + a1**2 + d2**2 + 2*d2*f3

θ2 = symbols('θ2')

f = (k1 * cos(θ2) + k2 * sin(θ2)) * 2 * a1 + k3 - r
root2 = solve([f],[θ2])
θ2 = [root2[i][0] for i in range(len(root2))]
print(θ2)

# lis = [(-3.05085978803173,), (-2.76035600105476,), (-0.616827296276209,), (-0.326323509299240,)]
θ2_du = [180 * root2[i][0] / np.pi for i in range(len(root2))]  ## θ3 弧度换角度
print('θ2(以度为单位): ', θ2_du )  # [-174.801389740395, -158.156748814047, -35.3416007650924, -18.6969598387445]
python 复制代码 
## 求解  θ1 [-90, 90]
## 结果汇总
# θ2 = -2.82490122970046(不满足要求)
# θ1 = θ1(以度为单位):  [-115.684399755325, 115.684399755325] 
###
# θ2 = -2.09516142685496(不满足要求)
# θ1 = θ1(以度为单位):  [-115.684399755325, 115.684399755325]

################## θ31  满足
# θ2(以度为单位):  [11.9136340504118, 49.5298107225008]

# θ2 = 0.207932140057394
# θ1(以度为单位):  [-64.3156002446740, 64.3156002446740]

# θ2 = 0.864458274997230
# θ1(以度为单位):  [-64.3156002446740, 64.3156002446740]

################## θ32  
# [-0.864458274997158, -0.207932140057458]
# θ2(以度为单位):  [-49.5298107224966, -11.9136340504155]

# θ2 = -0.864458274997158 
# θ1(以度为单位):  [-64.3156002446747, 64.3156002446747]
# θ2 = -0.207932140057458
# θ1(以度为单位):  [-64.3156002446747, 64.3156002446747]

# θ33

θ2 = -0.207932140057458
g1 = np.cos(θ2)* f1 - np.sin(θ2) * f2 + a1
g2 = np.sin(θ2)* np.cos(α1) * f1 + np.cos(θ2) * np.cos(α1)* f2 -\
     np.sin(α1) * f3 - d2 * np.sin(α1)
g3 = np.sin(θ2)* np.sin(α1) * f1 + np.cos(θ2) * np.sin(α1)* f2 +\
     np.cos(α1) * f3 + d2 * np.cos(α1)

θ1 = symbols('θ1')

f = g1 * cos(θ1) - g2 * sin(θ1) - x
root1 = solve([f],[θ1])
θ1 = [root1[i][0] for i in range(len(root1))]
print(θ1)

# lis = [(-3.05085978803173,), (-2.76035600105476,), (-0.616827296276209,), (-0.326323509299240,)]
θ1_du = [180 * root1[i][0] / np.pi for i in range(len(root1))]  ## θ3 弧度换角度
print('θ1(以度为单位): ', θ1_du )  # [-174.801389740395, -158.156748814047, -35.3416007650924, -18.6969598387445]

中间版本代码（可能有误）：

python 复制代码 
import numpy as np 

########################## 求 T_WC
## 无转动
R_WD = [[1, 0, 0],
       [0, 1, 0],
       [0, 0, 1]]
a = np.row_stack((R_WD,[[0, 0, 0]]))  ## 扩展 行
P = np.array([[830, 20, 330, 1]])
T_WD = np.column_stack((a,P.T))  ## 扩展 列， 注意 zhuan
# print(T_WD)


## 绕  Y轴 转
θ = np.pi * (-60)/180   ## 注意 正负 判断
R_DC = [[np.cos(θ), 0, np.sin(θ)],
       [0, 1, 0],
       [-np.sin(θ), 0, np.cos(θ)]]
# print(R_DC)

a = np.row_stack((R_DC,[[0, 0, 0]]))
P = np.array([[-500, 452, 410, 1]])
T_DC = np.column_stack((a,P.T))

T_WC = np.dot(T_WD, T_DC) 

# T_WC = [[float(format(x, '.3g')) for x in T_WC[i]] for i in range(len(T_WC))]  
# print(T_WC)   ## 第 4 题答案
#############################################

### 求 T_06

# 求 T_W0
# α, a, d, θ = 0, 0, 373, 0
## 无转动 
T_W0 = [[1, 0, 0, 0],
      [0, 1, 0, 0],
      [0, 0, 1, 373],
      [0, 0, 0, 1]]
# print(T_W0)

# 求 T_6C  Xc 和 Z6 方向相同，  Yc和 Y6 反向， Zc 和 X6 同向
T_6C = [[0, 0, 1, 0],
      [0, -1, 0, 0],
      [1, 0, 0, 206],
      [0, 0, 0, 1]]

T = np.dot(np.linalg.inv(T_W0), T_WC)
T_06 = np.dot(T, np.linalg.inv(T_6C))
# print(T_06)

P_04 = P_06 = np.array([[227, 472, 188.59876682, 1]])
x, y, z  = 227, 472, 188.59876682,
################## 
### 针对 i = 3, i = 4
α2, a2, d3 = 0, 340, 0   ## θ3
α3, a3, d4 = np.pi*(-90)/180, -40, 338   ## θ4

"""
## 仅与 θ3 有关
f1  = a3 * np.cos(θ3) + d4 * np.sin(α3)*np.sin(θ3) + a2 
# f2_θ3 = a3 * np.cos(α2)*np.sin(θ3) - d4 * np.sin(α3)*np.cos(α2)*np.cos(θ3) -\
#         d4 * np.sin(α2)*np.cos(α3) - d3 * np.sin(α2)
## 化简：
f2  = a3 * np.sin(θ3) - d4 * np.sin(α3) * np.cos(θ3)
# f3  = a3 * np.sin(α2)* np.sin(θ3)- d4 * np.sin(α3)* np.sin(α2) * np.cos(θ3) + \
#         d4 * np.cos(α2) * np.cos(α3) + d3 * np.cos(α2)
## 化简
f3 = d4 * np.cos(α3)  ## 2.069653090559027e-14   ## 可求
# print(f3)

"""
# 对 i= 1 i= 2
α0, a0, d1 = 0, 0, 0   ## θ1
α1, a1, d2 = np.pi*(-90)/180, -30, 0  ## θ2

"""
## 和 θ2，θ3有关
g1 = np.cos(θ2)* f1 - np.sin(θ2) * f2 + a1
# g2 = np.sin(θ2)* np.cos(α1) * f1 + np.cos(θ2) * np.cos(α1)* f2 -\
#      np.sin(α1) * f3 - d2 * np.sin(α1)
## 化简
g2 = np.sin(θ2)* np.cos(α1) * f1 + np.cos(θ2) * np.cos(α1)* f2 -\
     np.sin(α1) * f3
# g3 = np.sin(θ2)* np.sin(α1) * f1 + np.cos(θ2) * np.sin(α1)* f2 +\
#      np.cos(α1) * f3 + d2 * np.cos(α1)
## 化简
g3 = np.sin(θ2)* np.sin(α1) * f1 + np.cos(θ2) * np.sin(α1)* f2 +\
     np.cos(α1) * f3 


## a1 不等于 0 
k1 = f1 
k2 = -f2 
# k3 = f1**2 + f2**2 + f3**2 + a1**2 + d2**2 + 2*d2*f3
## 化简
k3 = f1**2 + f2**2 + f3**2 + a1**2
# k4 = f3 * np.cos(α1) + d2 * np.cos(α1)
## 化简
k4 = 0
"""

## 
r = x**2 + y**2 + z**2   ## 可解

# from scipy.optimize import fsolve
# def func(θ3):
#     return 


# root = solve([func], [θ3] )
# print(root)

from sympy import *
θ3 = symbols('θ3')

f = (r-a1**2- (a3 * cos(θ3) + d4 * sin(α3)*sin(θ3) + a2 )**2 - (a3 * sin(θ3) - d4 * sin(α3) * cos(θ3))**2 - (d4 * cos(α3))**2 )**2/(4 * a1**2) + z**2/(sin(α1))**2 - (a3 * cos(θ3) + d4 * sin(α3)*sin(θ3) + a2)**2 - (a3 * sin(θ3) - d4 * sin(α3) * cos(θ3))**2

root3 = solve([f],[θ3])
print('θ3(弧度值): ', root3)

# lis = [(-3.05085978803173,), (-2.76035600105476,), (-0.616827296276209,), (-0.326323509299240,)]
θ3_du = [180 * root3[i][0] / np.pi for i in range(len(root3))]  ## θ3 弧度换角度
print('θ3(以度为单位): ', θ3_du )  # [-174.801389740395, -158.156748814047, -35.3416007650924, -18.6969598387445]
python 复制代码 
## 求解 θ2
# 结果汇总
# θ3(以度为单位):  [-174.801389740395, -158.156748814047, -35.3416007650924, -18.6969598387445]
# θ3 = -3.05085978803173(无满足要求的θ1), -2.76035600105476(符合), -0.616827296276209(符合), -0.326323509299240(无满足要求的θ1)

# θ2 =[-2.82490122970046, -2.09516142685496],\
    # [0.207932140057394, 0.864458274997230]
    #  [-0.864458274997158, -0.207932140057458]

θ3 =  -0.326323509299240
f1  = a3 * np.cos(θ3) + d4 * np.sin(α3) * np.sin(θ3) + a2 
f2  = a3 * np.sin(θ3) - d4 * np.sin(α3) * np.cos(θ3)
f3 = d4 * np.cos(α3) 
k1 = f1 
k2 = -f2 
k3 = f1**2 + f2**2 + f3**2 + a1**2

θ2 = symbols('θ2')

f = (k1 * cos(θ2) + k2 * sin(θ2))*2 * a1 + k3 - r
root2 = solve([f],[θ2])
θ2 = [root2[i][0] for i in range(len(root2))]
print(θ2)

# lis = [(-3.05085978803173,), (-2.76035600105476,), (-0.616827296276209,), (-0.326323509299240,)]
θ2_du = [180 * root2[i][0] / np.pi for i in range(len(root2))]  ## θ3 弧度换角度
print('θ2(以度为单位): ', θ2_du )  # [-174.801389740395, -158.156748814047, -35.3416007650924, -18.6969598387445]
python 复制代码 
## 求解  θ1 [-90, 90]
## 结果汇总
# θ2 = -2.82490122970046(不满足要求)
# θ1 = θ1(以度为单位):  [-115.684399755325, 115.684399755325] 
###
# θ2 = -2.09516142685496(不满足要求)
# θ1 = θ1(以度为单位):  [-115.684399755325, 115.684399755325]

## θ31  满足
# θ2(以度为单位):  [11.9136340504118, 49.5298107225008]
# θ2 = 0.207932140057394
# θ1(以度为单位):  [-64.3156002446740, 64.3156002446740]

# θ2 = 0.864458274997230
# θ1(以度为单位):  [-64.3156002446740, 64.3156002446740]

# θ32  
# θ2 = -0.864458274997158 
# θ1(以度为单位):  [-64.3156002446747, 64.3156002446747]
# θ2 = -0.207932140057458
# θ1(以度为单位):  [-64.3156002446747, 64.3156002446747]

# θ33

θ2 = 2.82490122970057
g1 = np.cos(θ2)* f1 - np.sin(θ2) * f2 + a1
g2 = np.sin(θ2)* np.cos(α1) * f1 + np.cos(θ2) * np.cos(α1)* f2 -\
     np.sin(α1) * f3
g3 = np.sin(θ2)* np.sin(α1) * f1 + np.cos(θ2) * np.sin(α1)* f2 +\
     np.cos(α1) * f3 

θ1 = symbols('θ1')

f = g1 * cos(θ1) - g2 * sin(θ1) - x
root1 = solve([f],[θ1])
θ1 = [root1[i][0] for i in range(len(root1))]
print(θ1)

# lis = [(-3.05085978803173,), (-2.76035600105476,), (-0.616827296276209,), (-0.326323509299240,)]
θ1_du = [180 * root1[i][0] / np.pi for i in range(len(root1))]  ## θ3 弧度换角度
print('θ1(以度为单位): ', θ1_du )  # [-174.801389740395, -158.156748814047, -35.3416007650924, -18.6969598387445]

第5题答案：-158//-35

第6题和第7题不理解正负怎么定的


第6题答案 ： 12//-50

第7题答案： 64

第8题





求解 θ4 - θ6的 Python 代码

python 复制代码 
import numpy as np

#  求解  θ4, θ5, θ6   [-90, 90]

## 沿着 Z  旋转 x, 先旋转θ1， 再旋转 θ2, 再旋转 θ3, 
# θ1   θ1(以度为单位):  [-64.3156002446747, 64.3156002446747]
θ = 1.12251898466604  ##   可选 [-1.12251898466604, 1.12251898466604]
α = 0
R_01 = [[np.cos(θ), -np.sin(θ), 0],
       [np.sin(θ)*np.cos(α), np.cos(θ)*np.cos(α), -np.sin(α)],
       [np.sin(θ)*np.sin(α), np.cos(θ)*np.sin(α), np.cos(α)]]

# θ2 = np.pi * (-52.2)/180    ## 12//-50  -12  50
# 可选  4个
# [-0.864458274997158, -0.207932140057458]
# θ2(以度为单位):  [-49.5298107224966, -11.9136340504155]

# [0.864458274997158, 0.207932140057458]
# θ2(以度为单位):   [49.5298107224966, 11.9136340504155]

θ = -0.864458274997158  ## 
α = np.pi * (-90)/180
R_12 = [[np.cos(θ), -np.sin(θ), 0],
       [np.sin(θ)*np.cos(α), np.cos(θ)*np.cos(α), -np.sin(α)],
       [np.sin(θ)*np.sin(α), np.cos(θ)*np.sin(α), np.cos(α)]]

# θ3 = np.pi * (2.5)/180  ## -158//-35
## 可选 2个 

θ = -0.616827296276209 ## -2.76035600105476(符合), -0.616827296276209(符合)
α = 0
R_23 = [[np.cos(θ), -np.sin(θ), 0],
       [np.sin(θ)*np.cos(α), np.cos(θ)*np.cos(α), -np.sin(α)],
       [np.sin(θ)*np.sin(α), np.cos(θ)*np.sin(α), np.cos(α)]]

R = np.dot(R_01, R_12)
R_03 = np.dot(R, R_23)
# print(R_03)

## 由之前 计算的 T_06 
R_06 = [[ -0.8660254,  0. ,0.5],
        [  0., -1., 0.],
        [  0.5, 0., 0.8660254]]

### 注意这一步处理，这里 和 PPT 里不一样
θ = np.pi * (-90)/180
R_34X = [[1, 0, 0],
        [0, np.cos(θ), -np.sin(θ)],
        [0, np.sin(θ), np.cos(θ)]]

R_36 = np.dot(np.linalg.inv(np.dot(R_03, R_34X)), R_06)  ## 需要 先将 Z3 转到 Z4 ， 才能 继续 使用 ZYZ 欧拉角  计算
# print(R_36)
r31 = R_36[2][0]
r32 = R_36[2][1]
r33 = R_36[2][2]
r23 = R_36[1][2]
r13 = R_36[0][2]
import math
β = math.atan2(math.sqrt(r31**2 + r32**2), r33)  ## 此外， 当 β 选负时，还有 一种 姿态选项， 而后续的θ4和 θ6 仅与 β的选值有关

print("解1：")
# print(β)  ## 1.1033617668479667  63
## 由PPT P25 DH定义  与 ZYZ 欧拉角度  转换关系
print('θ5：',180*β/np.pi)

# β = 1.1033617668479667
α = math.atan2(r23/np.sin(β), r13/np.sin(β))
print('θ4：',180*α/np.pi + 180)

γ = math.atan2(r32/np.sin(β), -r31/np.sin(β))
print('θ6：', 180*γ/np.pi + 180)

###
print("解2：")
β = -β  ## 另一组姿态
print('θ5：',180*β/np.pi)
α = math.atan2(r23/np.sin(β), r13/np.sin(β))
print('θ4：',180*α/np.pi + 180)
γ = math.atan2(r32/np.sin(β), -r31/np.sin(β))
print('θ6：', 180*γ/np.pi + 180)

θ 确定过程

1、

θ3(弧度值): [(-3.05085978803173,), (-2.76035600105476,), (-0.616827296276209,), (-0.326323509299240,)]

θ3(以度为单位): [-174.801389740395, -158.156748814047, -35.3416007650924, -18.6969598387445]

11、θ3 = -3.05085978803173

θ2(弧度值): [-2.82490122970046, -2.09516142685496]

θ2(以度为单位): [-161.854918003153, -120.043907157397]

θ1(弧度值): [-2.01907366892374, 2.01907366892374]

θ1(以度为单位): [-115.684399755325, 115.684399755325]

θ1(以度为单位): []
没有符合条件[-90, 90]的θ1

12、θ3 = -2.76035600105476(-158)

θ2(弧度值): [0.207932140057394, 0.864458274997230]

θ2(以度为单位): [11.9136340504118, 49.5298107225008]

两个 θ2 求得的 θ1 一样：

θ1(弧度值): [-1.12251898466603, 1.12251898466603]

θ1(以度为单位): [-64.3156002446740, 64.3156002446740]

求解 θ5， θ4， θ6：

θ2 θ1 θ5 θ4 θ6 与T_06一致
50 -64 -61 -31 77 ❌
50 64 -61 31 -77 ❌

根据 正运动学 方法 求解 T_06， 对比验证：

13、θ3 = -0.616827296276209(-35)

θ2(弧度值): [-0.864458274997158, -0.207932140057458]

θ2(以度为单位): [-49.5298107224966, -11.9136340504155]

两个 θ2 求得的 θ1 一样：

θ1(弧度值): [-1.12251898466603, 1.12251898466603]

θ1(以度为单位): [-64.3156002446740, 64.3156002446740]

求解 θ5， θ4， θ6：

θ2 θ1 θ5 θ4 θ6
-50 64 -82 27 -65 ✔
-50 -64 -82 -27 65 ❌

根据 正运动学 方法 求解 T_06， 对比验证：

14、θ3 = -0.326323509299240

θ2(弧度值): [2.09516142685485, 2.82490122970057]

θ2(以度为单位): [120.043907157391, 161.854918003159]

θ1(弧度值): [-2.01907366892376, 2.01907366892376]

θ1(以度为单位): [-115.684399755326, 115.684399755326]

θ1(弧度值): [-2.01907366892376, 2.01907366892376]

θ1(以度为单位): [-115.684399755326, 115.684399755326]
没有符合条件[-90, 90]的θ1

第8题答案： 27//-82//-65

正运动学 求解 T_06 验证代码 _Python

python 复制代码 
## 通过 T_06  再次验证 
import numpy as np
def getT(α, a, d, θ):
    α = np.pi * α / 180
    θ = np.pi * θ / 180
    T = [[np.cos(θ), -np.sin(θ), 0, a],
       [np.sin(θ)*np.cos(α), np.cos(θ)*np.cos(θ), -np.sin(α), -np.sin(α) * d],
       [np.sin(θ)*np.sin(α), np.cos(θ)*np.sin(α), np.cos(α), np.cos(α) * d],
        [0, 0, 0, 1]]
    return T

T_01 = getT(0, 0, 0,    64)
T_12 = getT(-90, -30, 0, 50)
T = np.dot(T_01, T_12)
T_23 = getT(0, 340, 0,  -158)
T = np.dot(T, T_23)
T_34 = getT(-90, -40,338, 31)
T = np.dot(T, T_34)
T_45 = getT(90, 0, 0,     -61)
T = np.dot(T, T_45) 
T_56 = getT(-90, 0, 0,   -77)
T_06 = np.dot(T, T_56)
print(T_06)

# R_06 = [[ -0.8660254,  0. ,0.5],
#         [  0., -1., 0.],
#         [  0.5, 0., 0.8660254]]

Matlab代码_参考

github链接

相关推荐
星马梦缘9 小时前
Matlab机器人工具箱使用2 DH建模与加载模型
人工智能·matlab·机器人·仿真·dh参数法·改进dh参数法
星马梦缘16 小时前
Matlab机器人工具箱使用1 简单的描述类函数
matlab·矩阵·机器人·位姿·欧拉角·rpy角
神仙别闹1 天前
基于单片机的六足机器人控制系统设计
单片机·嵌入式硬件·机器人
南山二毛2 天前
机器人控制器开发(传感器层——奥比大白相机适配)
数码相机·机器人
房开民2 天前
使用海康机器人相机SDK实现基本参数配置（C语言示例）
c语言·数码相机·机器人
南山二毛2 天前
机器人控制器开发(导航算法——导航栈关联坐标系)
人工智能·架构·机器人
猫头虎2 天前
2025最新超详细FreeRTOS入门教程：第一章 FreeRTOS移植到STM32
stm32·单片机·嵌入式硬件·机器人·硬件架构·freertos·嵌入式实时数据库
xwz小王子3 天前
Nature Machine Intelligence 基于强化学习的磁性微型机器人自主三维位置控制
机器人·微型机器人
IoT砖家涂拉拉3 天前
从“找新家”到“走向全球”，布尔云携手涂鸦智能开启机器人新冒险
人工智能·机器人·ai助手·ai智能体·ai机器人
纪元A梦3 天前
贪心算法应用：机器人路径平滑问题详解
贪心算法·机器人
热门推荐
01UV安装并设置国内源022025 年高教社杯全国大学生数学建模竞赛C 题 NIPT 的时点选择与胎儿的异常判定 完整成品思路模型代码分享，全网首发高质量！！！032025年数学建模国赛C题超详细解题思路04A股预测还能更准？开源大模型Kronos带你跑通预测+回测全流程05不再让Windows更新！&Edge游戏助手卸载及关闭自动更新06KGG转MP3工具|非KGM文件|解密音频07UV 工具安装与国内镜像源配置指南08突破百度网盘的下载限速，两种方法教会你【超详细】09Linux下V2Ray安装配置指南10教你如何认证 Gemini 教育优惠的二次验证，薅个 1年的 Gemini Pro 会员