我使用python进行一个六轴机械臂的运动学逆解，在进行J2和J3的分析时，为了消除未知数，将具有pz的公式进行平方，虽然消除了未知数，但导致pz将强制变成正值，如果pz為負，机械臂无法判断，如pz = -10会被改为10。
代码是依照台大林沛群教授的机器人学线上课程撰写的:P5連結(例題) P8連結(詳細算法過程)
計算theta3的代碼:
eq0 = ma.sqrt(px**2+py**2) - a1
eq2 = pz
eqa = 2*(-a3)*a2
eqb = 2*d4 * a2
#eq0 = 435.76 - a1 (這裡是例題的數字
#eq2 = 188.6
#化簡常數
eqc = - (((eq0**2)+(eq2**2)) -(a3 **2 + d4**2 + a2**2))
#這裡eq2 = pz ，在eq2被平方後pz負號就被強制消除了
print(eqc)
equ = 2 * ma.atan2((eqb-ma.sqrt(eqb**2+eqa**2-eqc**2)),(eqa+eqc)) *57.3
theta3 = equ
計算theta2的代碼:
f1 = a3*ma.cos(theta3 * dtr) + d4*ma.sin(alpha3*dtr) * ma.sin(theta3*dtr) +a2
f2 = a3*ma.cos(alpha2*dtr) * ma.sin(theta3 * dtr) - d4*ma.sin(alpha3 * dtr) * ma.cos(alpha2 *dtr) * ma.cos(theta3 * dtr) -d4*sin(alpha2 * dtr)*ma.cos(alpha3*dtr)-d3*ma.sin(alpha2*dtr)
g1 = eq0
eqc = eq0
eqa = f1
eqb = -f2
equ = 2 * ma.atan2((eqb-ma.sqrt(abs(eqb**2+eqa**2-eqc**2))),(eqa+eqc)) *57.3
theta2 = equ
現在的結果是:J1、J4~6正確，J2、J3無法判斷Z軸正負，因此px、py以及最終夾爪姿態都是正確的，僅有當pz為負時會被強制判斷為正
目前嘗試的解決方法:
1.使用solve直接解多項式，但效果不好，跑了1分鐘才出來結果，而且不一定正確
2.直接將常數項補上-pz，但會導致等式不成立
# 定義變數
theta2, theta3 = symbols('theta2 theta3')
# 定義方程
eq1 = -40*sp.cos(theta2*theta3) - 338*sp.sin(theta2*theta3*dtr) + 340*sp.cos(theta2) - 465.79
eq2 = 40*sp.sin(theta2*theta3) - 338*sp.cos(theta2*theta3) - 340*sp.sin(theta2) - 188.6
# 求解方程組
solutions = solve((eq1, eq2), (theta2, theta3))
print(solutions)
我希望能完成的是修改J2 J3的計算公式或是化簡方式使pz的負號不會被消除
以下是PPT中的過程:
主要是這張圖中的Eq2平方 如果pz是-188.6會被變為正值:
完整代碼:
'''
快速導覽:
21~73行:基礎變量定義,包含機械臂基礎DH參數以及,旋轉矩陣計算
74~141行:計算旋轉矩陣以及位移矩陣,於137行分離旋轉矩陣,140行分離位移矩陣,141行輸出目標點px,py,pz
170行:計算theta1角度
193~205行:計算theta3角度 (目前問題)
234~241行:計算theta2角度 (目前問題)
259~299行:計算theta4~6角度
301~302行:輸出結果
'''
'''筆記區
1.三個矩陣聯乘不要使用(a,b,c) 要用 (dot(a,b),c) 詳見48~51行
2.使用sympy計算方程式時,不能使用numpy函數如三角函數的np.cos要改成sp.cos
3.Python TypeError: unsupported operand type(s) for -: 'int' and 'function' 代表某函數使用時他還沒創建 順序改一下就好
4.顯示小數:1: print(round(x,3)) , 2:print('%.3f'%x)
5.Numpy矩陣相乘可以直接用 @ 取代 np.dot
6.160行中的theta3計算公式中,後方sqrt中的數字如果Z過小(目前測試),會變成負數無法開根號,所以加了一個abs()改善,目前未發現負面影響
'''
import numpy as np
import math as ma
import sympy as sp
from sympy import *
from scipy.optimize import fsolve
Pi = 3.14159265358
dtr = Pi/180
theta1 = theta2 = theta3 = theta4 = theta5 = theta6 = 0.0
def map(value, in_min, in_max, out_min, out_max):
return (value - in_min) * (out_max - out_min) // (in_max - in_min) + out_min
# 定义旋转矩阵绕 X 轴旋转的函数 -252.8 -158.5
def rotate_x(theta):
return np.array([
[1, 0, 0],
[0, cos(theta), -sin(theta)],
[0, sin(theta), cos(theta)]
])
# 定义旋转矩阵绕 Z 轴旋转的函数
def rotate_z(theta):
return np.array([
[cos(theta), -sin(theta), 0],
[sin(theta), cos(theta), 0],
[0, 0, 1]
])
# 定义旋转矩阵绕 Y 轴旋转的函数
def rotate_y(theta):
return np.array([
[cos(theta), 0, sin(theta),1],
[0, 1, 0, 1],
[-sin(theta), 0, cos(theta),1],
[0, 0, 0, 1]
])
DH_table = np.array([[ -90, 0, -90, 90, -90,0],#alpha-1
[ -30, 340, -40, 0, 0,0],#a-1 35 146 52 0 0
[0, 0, 0, 338, 0, 0],#d 0 0 0 260.522 0 0
[theta1, theta2, theta3, theta4, theta5, theta6]])#theta
'''
DH_table = np.array([[ -90, 0, -90, 90, -90,0],#alpha-1
[ 35, 200, 52, 0, 0,0],#a-1 35 146->200 52 0 0
[0, 0, 0, 260.5, 0, 0],#d 0 0 0 260.522 0 0 ##Lbase高 25.5
[theta1, theta2, theta3, theta4, theta5, theta6]])#theta
'''
alpha0 = 0
alpha1, alpha2, alpha3, alpha4, alpha5, alpha6 = DH_table[0]
a1, a2, a3, a4, a5, a6 = DH_table[1]
d1, d2, d3, d4, d5, d6 = DH_table[2]
#---各個目標變數---/*
#桌手XYZ 預設:830 20 330
W_D_X = 830
W_D_Y = 20
W_D_Z = 330
#杯桌XYZ 預設:-280 250 62.5 原點:0 0 右上角 292 292 , 185 368
D_C_X = -280
D_C_Y = 250
D_C_Z = 62.5
#夾爪底座離地高度 預設 373
W_0_Z = 373
#夾爪長度 預設 206 (夾爪最後一根 這不會放在DH:151.5)
six_C_Z = 206
#夾爪最終姿態 預設 35 0 (朝下為0 90) (Matlab中z軸為頭)
#lastangleX = map(D_C_Y,0,620,-45,45)
#print(lastangleX)
lastangleX = 35
#lastangleY = map(D_C_X,-400,0,90+45,90-45)
lastangleY = 0
#print(lastangleY)
#---各個目標變數---*/
#桌子對於手臂0的T
W_D = np.array([[1, 0, 0, W_D_X],
[0, 1, 0, W_D_Y],
[0, 0, 1, W_D_Z],
[0, 0, 0, 1]])
#杯子對於桌子0的T
D_C = np.array([[ma.cos(lastangleX * Pi/180),-ma.sin(lastangleX * Pi/180),0,D_C_X],
[ma.sin(lastangleX * Pi/180),ma.cos(lastangleX * Pi/180),0,D_C_Y],
[0, 0, 1, D_C_Z],
[0, 0, 0, 1]])
if lastangleY != 0:
D_C = D_C @ rotate_y(lastangleY * dtr)
#杯子對於手臂0的T (手臂to桌子to杯子) (前面兩個相乘)
W_C = np.dot(W_D, D_C)
#0對於手臂的T(基本上只有單純的底座高度增加而已)
W_0 = np.array([[1, 0, 0, 0],
[0, 1, 0, 0],
[0, 0, 1, W_0_Z],
[0, 0, 0, 1]])
#杯子對於夾爪(夾爪自己的長度補償)
six_C = np.array([[0 ,0, 1, 0],
[0, -1, 0, 0],
[1, 0, 0, six_C_Z],
[0, 0, 0 ,1]])
#原:0~杯 = 0~手0 * 手0~手6 * 手6~杯 求手0~手6
#轉:手0~手6 = 0~手0(逆) * 0~杯(不逆) * 手6~杯(逆)
#(手臂的0~6全角度)(非常重要!!)
'''錯誤方式
zero_six_T = np.dot(np.linalg.inv(W_0), W_C , np.linalg.inv(six_C))
print("A:",zero_six_T)
'''
zero_six_T = np.dot(np.dot(np.linalg.inv(W_0), W_C) , np.linalg.inv(six_C))
#print(zero_six_T)
#分離旋轉矩陣R
zero_six_R = zero_six_T[:3, :3]
#print("R:",zero_six_R)
#分離位移矩陣P
zero_six_P= np.array([[zero_six_T[0,3]],[zero_six_T[1,3]],[zero_six_T[2,3]]])
print("目標點:",zero_six_P)
#STEP3:找1~6角度!!!!!
#1~3分離
#4對3的T
three_4_T = np.array([[alpha3],
[-(d4)*ma.sin(alpha3 * Pi/180)],
[d4*ma.cos(alpha3 * Pi/180)],
[d4*ma.cos(alpha3 * Pi/180)],
[1]])
r = (zero_six_P[0]*zero_six_P[1]*zero_six_P[2])
ax = zero_six_R[0,2]
ay = zero_six_R[1,2]
az = zero_six_R[2,2]
px = zero_six_P[0,0]
py = zero_six_P[1,0]
pz = zero_six_P[2,0]
#print(px,py,pz)
#print ((ma.atan2(py,px))
#-ma.atan2(-d2 ,-ma.sqrt( px**2+py**2-d2**2)))
#print(ma.atan2(zero_six_P[1,0],zero_six_P[0,0])-ma.atan2(d3/ma.sqrt(px**2+py**2),ma.sqrt(1-(d3**2/(ma.sqrt(px**2+py**2))**2))))
#print(ma.atan(py/px))
#print("py:",py ,"px:",px)
#print(-ma.atan2(-d2 , ma.sqrt(zero_six_P[0,0]**2+zero_six_P[1,0]**2-d2**2)))
#print(((ma.atan2(py,px))-ma.atan2(d2,ma.sqrt((ma.sqrt(px**2+py**2-d2**2))))))
'''theta1 = (((ma.atan2(py,px))-ma.atan2(d2,ma.sqrt((ma.sqrt(px**2+py**2-d2**2))))) * 57.3)'''
theta1 = ma.atan2(py,px)*57.3
#theta1_2 = (((ma.atan2(py,px))-ma.atan2(d2,-ma.sqrt((ma.sqrt(px**2+py**2-d2**2))))) * 57.3) 0.018 0.274
#k = ((px**2+py**2+ pz**2 - a2**2-a3**2-d3**2-d4**2)/(2*a2))
#print((a3**2+d4**2-k**2))
#theta3 = ((((ma.atan2(a3,d4))- (ma.atan2(k,np.sqrt(abs(a3**2+d4**2-k**2))))))) *57.3
#print(theta3)
'''print("k:","%.3f"%k)
print("theta3前半","%.3f"%((ma.atan2(a3,d4))))
print("theta3後半","%.3f"%(ma.atan2(k,ma.sqrt(abs(a3**2+d4**2-k**2)))))'''
# 定義變數
#theta2, theta3 = symbols('theta2 theta3')
# 定義方程
eq1 = -40*sp.cos(theta2*theta3) - 338*sp.sin(theta2*theta3*dtr) + 340*sp.cos(theta2) - 465.79
eq2 = 40*sp.sin(theta2*theta3) - 338*sp.cos(theta2*theta3) - 340*sp.sin(theta2) - 188.6
# 求解方程組
#solutions = solve((eq1, eq2), (theta2, theta3))
#print(solutions)
eq0 = ma.sqrt(px**2+py**2) - a1
eq2 = pz
eqa = 2*(-a3)*a2
eqb = 2*d4 * a2
#eq0 = 435.76 - a1
#print(eq0)
#eq2 = 188.6
#化簡常數
eqc = - (((eq0**2)+(eq2**2)) -(a3 **2 + d4**2 + a2**2)) #這裡eq2 = pz ，在eq2被平方後pz負號就被強制消除了
print(eqc)
equ = 2 * ma.atan2((eqb-ma.sqrt(eqb**2+eqa**2-eqc**2)),(eqa+eqc)) *57.3
theta3 = equ
#print("eqa:",eqa, "eqb",eqb, "eqc",eqc)
#print(eq0[0]**2+eq0[2]**2)
#print("-a3:",-a3, "d3",d4, "a2",a2)
'''def equations(variables):
theta2, theta3 = variables
Eq1 = -40*np.cos(theta2+theta3) - 338*np.sin(theta2+theta3) + 340*np.cos(theta2) - eq0
Eq2 = 40*np.sin(theta2+theta3) - 338*np.cos(theta2+theta3) - 340*np.sin(theta2) - pz
return [Eq1, Eq2]
# 初始估计值
initial_guess = [0.0324, 1.4]
# 解方程组
theta2, theta3 = fsolve(equations, initial_guess)
print("theta2 =", theta2)
print("theta3 =", theta3)'''
#theta3 = ((((ma.atan2(a3,d4))- (ma.atan2(k,np.sqrt(abs(a3**2+d4**2-k**2))))))) *57.3 *0.277
#print(theta3)
'''theta2 = (ma.atan2((-a3-a2*ma.cos(theta3*dtr))*pz+(ma.cos(theta1*dtr)*px+ma.sin(theta1*dtr)*py)*(a2*ma.sin(theta3*dtr)-d4)
,(-d4+a2*ma.cos(theta3*dtr))*pz-(ma.cos(theta1*dtr)*px+ma.sin(theta1*dtr)*py)*(-a2*ma.cos(theta3*dtr)-a3))*57.3 - (theta3)) #*1.02554'''
'''theta22 = (ma.atan2((-a3-a2*ma.cos(theta32*dtr))*pz+(ma.cos(theta1*dtr)*px+ma.sin(theta1*dtr)*py)*(a2*ma.sin(theta32*dtr)-d4)
,(-d4+a2*ma.cos(theta32*dtr))*pz-(ma.cos(theta1*dtr)*px+ma.sin(theta1*dtr)*py)*(-a2*ma.cos(theta32*dtr)-a3))*57.3 -theta32)'''
f1 = a3*ma.cos(theta3 * dtr) + d4*ma.sin(alpha3*dtr) * ma.sin(theta3*dtr) +a2
f2 = a3*ma.cos(alpha2*dtr) * ma.sin(theta3 * dtr) - d4*ma.sin(alpha3 * dtr) * ma.cos(alpha2 *dtr) * ma.cos(theta3 * dtr) -d4*sin(alpha2 * dtr)*ma.cos(alpha3*dtr)-d3*ma.sin(alpha2*dtr)
g1 = eq0
eqc = eq0
eqa = f1
eqb = -f2
equ = 2 * ma.atan2((eqb-ma.sqrt(abs(eqb**2+eqa**2-eqc**2))),(eqa+eqc)) *57.3
theta2 = equ
#print("theta22","%.1f"%theta22)
#theta2 = ma.acos((l1**2+l2**2-l3**2)/(2*l1*l2))
#theta2 = ma.atan(pz/ma.sqrt(px**2+py**2)+ma.acos((l1**2+l3**2-l2**2)/(2*l1*l3))) *57.3
#print("theta2:",'%.3f'%theta2)
#print("theta1:",'%.3f'%theta1)
'''if(theta3>181):
theta3 = theta3 -360
if(theta4>181):
theta4 = theta4 -360
if(theta5>181):
theta5 = theta5 -360
if(theta6>181):
theta6 = theta6 -360'''
# 读入用户输入的旋转角度
theta1_x = alpha0 * dtr
theta1_z = theta1 * dtr
theta2_x = alpha1 * dtr
theta2_z = theta2 * dtr
theta3_z = alpha2 * dtr
theta4_z = theta3 * dtr
# 计算旋转矩阵 R
zero_three_R = rotate_x(theta1_x) @ rotate_z(theta1_z) @ rotate_x(theta2_x) @ rotate_z(theta2_z) @ rotate_z(theta3_z) @ rotate_z(theta4_z)
# 输出旋转矩阵 R
'''print("0-6 RotationMatrix = ")
for row in zero_three_R:
for element in row:
print("%.3f" % element, end=' ')
print()'''
#print(zero_six_R)
#print("轉換前:\n",zero_three_R)
#----------
zero_three_R = zero_three_R @ rotate_x(-90*dtr) #--4章題目的是轉90前 所以會有點不一樣
zero_three_R = np.float64(zero_three_R)
#print("轉換90後\n:",zero_three_R)
three_six_R = np.linalg.inv(zero_three_R) @ zero_six_R
#print("0-6的R:\n",zero_six_R)
'''print("3-6 RoatationMatrix = ")
for row in three_six_R:
for element in row:
print("%.3f" % element, end=' ')
print()'''
theta5 = ma.atan2(ma.sqrt(three_six_R[2,0]**2 + three_six_R[2,1]**2),three_six_R[2,2]) * 57.3
#print(ma.sqrt(three_six_R[2,0]**2+three_six_R[2,1]**2))
theta4 = ma.atan2((three_six_R[1,2]/ma.cos(theta5*dtr)),(three_six_R[0,2]/ma.cos(theta5*dtr))) *57.3
theta6 = ma.atan2((three_six_R[2,1]/ma.cos(theta5*dtr)),(-three_six_R[2,0]/ma.cos(theta5*dtr))) *57.3
print("theta1:","%.1f"%theta1,"theta2:","%.1f"%theta2,"theta3:","%.1f"%theta3)
print("theta4:","%.1f"%theta4,"theta5:","%.1f"%-theta5,"theta6:","%.1f"%theta6)
#print("theta4A:","%.3f"%(theta4+180),"theta5A:","%.3f"%theta5,"theta6A:","%.3f"%(theta6-180))
