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

已于 2023-09-02 23:51:24 修改
阅读量465 收藏 4
点赞数 2
分类专栏： 机器人 文章标签： 机器人
于 2023-09-02 17:45:32 首次发布
版权声明：本文为博主原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。
本文链接：https://blog.csdn.net/weixin_46034116/article/details/132557391
版权
  • 待完善： 第5-7【暂时不清楚如何确定】

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

  • 第6-7： 连猜带蒙🤣

####################################################
整个流程 走下来，只剩一个解了
在这里插入图片描述
不理解 第5-7题 为啥 还有双解。

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

coursera链接

文章目录

第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​

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​ 转向 与四指弯曲方向相反， 为负

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 代码

最终版本：

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]

## 求解 θ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]


## 求解  θ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]

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

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]

## 求解 θ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]


## 求解  θ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 代码
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-3177❌
5064-6131-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
-5064-8227-65✔
-50-64-82-2765❌

根据 正运动学 方法 求解 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
## 通过 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链接

Gaogaogaoshu
关注
  • 2
    点赞
  • 踩
  • 4
    收藏
    觉得还不错? 一键收藏
  • 1
    评论
专栏目录
【机器人技术基础】MATLAB Robotics Toolbox PUMA560 运动学正逆解
【人工智障 | 计算鸡视觉 | 机器孩】
01-17 1万+
机器人技术基础实验2报告 1. 实验目的和要求 2. 实验手段 2.1 实验原理 2.2 函数详解 3. 实验主要内容 4. 实验结果和分析 4.1 正运动学求解 例1 例2 4.2 逆运动学求解 例1 例2 1. 实验目的和要求 掌握常见形式的机器人的D-H坐标系建立方法、运动学建模； 正逆运动学分析与求解； 使用Robotics Toolbox对直角坐标机器人、圆柱坐标机器人、球坐标机器人、PUMA560其中之一建立运动学模型，正逆求解和仿真。 2. 实验手段 2.1
《机器人学一(Robotics(1))》_台大林沛群 第 7 周 【轨迹规划_综合】Quiz 7
weixin_46034116的博客
09-08 162
θ3(弧度值): [(-3.00276174766702,), (-2.70746877163218,), (-0.669714525698784,), (-0.374421549663944,)]θ3(以度为单位): [-172.045575024647, -155.126533777993, -38.3718158011460, -21.4527745544920]θ2(以度为单位): [-31.9721523316306, -28.0813579787176]
《机器人学一(Robotics(1))》_台大林沛群 第4周 Quiz4
screaming_zz的博客
01-12 1152
前两题主要是细心观察即可，第三题主要是使用勾股定理以及简单的反三角函数求解即可，长度与角度答案分别为80和30，不作赘述，主要阐述从第四题开始的解题过程。
机器人逆运动学
ben392797097的专栏
04-14 5232
我们已经看到了前向运动学问题。逆运动学问题更有趣，其解决方案更有用。在位置层面，问题表述为：“考虑到机器人手部的理想位置，所有机器人关节的角度必须是什么？”人类一直在解决这个问题，甚至没有想到它。当你早上吃麦片时，你只需伸手拿起勺子。你不会想，“我的肩膀需要这样做，我的肘部需要这样做，等等。” 下面我们将看看大多数机器人如何解决这个问题。我们将从一个非常简单的例子开始。上图是位于XY平面上的简单机...
《机器人学一(Robotics(1))》_台大林沛群 第3周 Quiz3
screaming_zz的博客
12-30 379
【代码】《机器人学一(Robotics(1))》_台大林沛群 第3周 Quiz3。
中正平和的机器人学笔记——2. 机械臂逆运动学（附MATAB代码）
BonhomieStriker_Robotics/Control/RL
12-08 6543
0. 预备知识（别跳过呀） 上一篇我们讲了正运动学，也就是通过齐次变换矩阵，知晓关节角后求得机械臂末端的位姿，并表示在基座标系下。这一片我们就来讲一讲逆运动学问题——已知工具坐标系{T}相对于固定坐标系{S}的期望位姿，如何计算一系列满足期望要求的关节角？ 0.1 求解的难度 这是一个难度更大的问题，因为求解操作臂运动学方程是一个非线性问题。譬如六自由度的机械臂Puma560，回顾最后求得的运动学...
工业机器人（4）-- Matlab Robot Toolbox运动学正、逆解
热门推荐
Techblog of HaoWANG
02-04 1万+
【Matlab Robotics Toolbox】robotics toolbox学习及使用记录，方便自己后面复习、改进。 基于Matlab R2019b 9.5; Peter Corke的Robotics Toolbox 10.3.1 目录 0. 前言 Robot toolbox- rtbdemo 1. 依据D-H参数建立机器人模型 2. 代码解释 单个Link的解释 建立机器人整体的解释 运动学正解 绘图 运动学逆解 微分运动学（求雅克比矩阵） 0. 前言 在初学机器人学的时候
Robotics与机器人基础（一）：正逆运动学与机械臂搭建
qq_40600539的博客
11-23 8313
摘要：本文基于Robotics工具箱，结合机器人运动基础，用matlab搭建一个机械臂，并讲述其所需要的运动学知识。 所用环境：Robotics_10.3  matlab_2018a   一、坐标系建立与位姿态计算 每一个连杆具有两个坐标系（注意：坐标系的搭建不是唯一的，不同的坐标系可能导致不同的计算结果，但是都对），我们用一个4x4的齐次变换矩阵来描述不同连杆间的位姿（这里命名为矩阵A）...
工业机器人运动学与Matlab正逆解算法学习笔记（用心总结一文全会）（一）——DH模型与正运动学
ooorczgc的博客
06-13 9400
以ZK-500工业机器人为例整理机器人运动学、DH建模、正逆解的相关内容以及matlab代码
MATLAB - 机器人逆运动学设计器（Inverse Kinematics Designer APP）
weixin_46300916的博客
12-22 1751
通过逆运动学设计器，您可以为 URDF 机器人模型设计逆运动学求解器。您可以调整逆运动学求解器并添加约束条件，以实现所需的行为。使用该程序，您可以。
matlab机械臂逆运动学代码-Robotics-HW-RoboArm:ZJU机器人学ROS机械臂作业
05-25
matlab机械臂逆运动学代码 ReadMe File Folder Presentation /code 含probot_gazebo ROS Package; 其中/src 下包含实验1-5&大作业代码 /img 文档内所需图片 /video 验证视频 /doc md文档 /experiment 机械臂敲铃实物...
MATLAB实现六轴机器人正逆运动学求解源码
07-31
以UR5为例，机器人的正逆运动学求解源码，使用PeterCorke的robotics toolbox做计算结果验证，并附有robotics toolbox的GitHub源码和本地、在线安装包。
Robotics__Vision_and_Control_Fundamental_Algorithms_in_MATLAB英文版07-机械臂运动学
12-23
机器人大牛Peter Corke的经典之作，内容涵盖机器人学（工业机器人，移动机器人，飞行器等等），机器视觉，机器人控制和算法。同时还有matlab机器人工具箱和机器视觉工具箱的实例代码，是一本全面而详细研究机器人...
机械臂正运动学程序，两种实现方式，robotics工具箱和转换矩阵
07-09
包含两种机械臂建模实现方式，可以求出机械臂正运动学末端坐标系到基坐标系的转换矩阵，有转换矩阵形式实现和robotics toolbox实现
IKBT:一个以符号形式解决机器人手臂逆运动学的python包
05-08
IKBT 一个基于python的系统，用于使用行为树进行动作选择来生成机械手逆运动学问题的闭式解。最新消息2021年3月升级到Python3（主要是在打印语句和新的python3-sympy中添加括号）。 新的“主要”分支提供了更多受人...
FANUC机器人SRVO-454和SRVO-455故障报警处理总结
最新发布
Robot_PLC_自动化学院
06-15 144
FANUC机器人SRVO-454和SRVO-455故障报警处理总结
[ROS 系列学习教程] 建模与仿真 - 使用 Arbotix 控制机器人
淋曦的进击手记
06-08 1043
Arbotix 是一款控制电机、舵机的硬件控制板，并提供相应的 ros 功能包，是一款基于 ROS的机器人控制系统，它旨在为小型机器人提供硬件控制和传感器接口，以便于机器人的运动和感知。它不仅可以驱动真实的 arbotix 控制板，还提供一个差速控制器，通过接收速度控制指令更新机器人的 joint 状态，从而帮助我们实现机器人在仿真环境中的运动。源码包含如下几部分：其中，：控制器，现已合并到中。：处理与 ArbotiX-ROS 绑定通信的固件源代码。：ArbotiX 的消息和服务定义。
机器人建模、运动学与动力学仿真分析（importrobot，loadrobot，smimport）
FL1717的博客
06-11 697
通过URDF，可以定义机器人的连杆（links）、关节（joints）、惯性（inertia）、视觉和碰撞属性等。在MATLAB中，可以使用Simulink和MATLAB/Simulink Robotics System Toolbox进行动力学仿真分析。这些工具提供了丰富的库函数和工具箱，可用于机器人运动学和动力学建模、仿真和控制。加载模型后，可以使用MATLAB的机器人工具箱（Robotics Toolbox）进行进一步的分析和仿真。机器人运动学是研究机器人各部分之间的相对运动、速度和加速度的学科。
CMU最新论文：机器人智慧流畅的躲避障碍物论文详细讲解
jiayoushijie的博客
06-14 1257
CMU华人博士生Tairan He最新论文：Agile But Safe: Learning Collision-Free High-Speed Legged Locomotion 代码开源：Code: https://github.com/LeCAR-Lab/ABS我会详细解读论文的内容,让我们开始吧。腿式机器人在杂乱环境中导航必须兼顾敏捷性以提高执行任务的效率,同时要确保安全性以避免与障碍物或人发生碰撞。
matlab计算机械臂逆运动学
10-14
计算机械臂逆运动学是指已知机械臂末端的位置和姿态，求解机械臂各关节的角度。在MATLAB中，可以使用 Robotics System Toolbox 来实现计算机械臂逆运动学的功能。具体步骤如下： 1. 定义机械臂模型：使用 Robotics System Toolbox 中的 robotics.RigidBodyTree 类定义机械臂模型，包括各个关节的类型、长度、质量等参数。 2. 定义末端位姿：使用 robotics.Pose 类定义机械臂末端的位姿，包括位置和姿态。 3. 求解逆运动学：使用 robotics.InverseKinematics 类求解机械臂的逆运动学，即求解各个关节的角度。 下面是一个简单的示例代码： ```matlab % 定义机械臂模型 robot = robotics.RigidBodyTree; L1 = robotics.RigidBody('L1'); J1 = robotics.Joint('J1','revolute'); setFixedTransform(J1,trvec2tform([0 0 0])); setJoint(J1,'homeposition',0); J1.JointAxis = [0 0 1]; L1.Joint = J1; addBody(robot,L1,'base'); % 定义末端位姿 endEffectorPose = robotics.Pose([0.3 0.4 0.5],rotm2quat(rotx(pi/2)*roty(pi/4)*rotz(pi/3))); % 求解逆运动学 ik = robotics.InverseKinematics('RigidBodyTree',robot); ik.SolverParameters.MaxIterations = 1000; ik.SolverParameters.SolutionTolerance = 1e-6; [q,~,exitflag] = ik('end_effector',endEffectorPose); % 输出关节角度 if exitflag >= 0 disp(q) else disp('无法求解逆运动学') end ```

“相关推荐”对你有帮助么？

  • 非常没帮助
  • 没帮助
  • 一般
  • 有帮助
  • 非常有帮助
提交
评论 1
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

当前余额3.43元 前往充值 >
需支付：10.00元
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付元
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明：

1.余额是钱包充值的虚拟货币，按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载，可以购买VIP、付费专栏及课程。

余额充值