技術標籤：協作機器人控制演算法SRS機械臂臂型角解析解

機械臂正逆運動學-----SRS型機械臂解析解

• 臂形角引數化原理講解
• 原始碼

臂形角引數化原理講解

原始碼

#!/usr/bin/env python
#-*-coding:utf-8-*-
#本文件用於求7自由度SRS型機械臂運動學相關函式
#程式設計師：陳永*
#版權：哈爾濱工業大學
#日期：2019.9.20
import numpy as np
import numpy.linalg as
 
 nla
import math
from math import pi

#尤拉軸角到旋轉矩陣rotate
def euler_axis2rot(n,theta):
	'''
		本函式將尤拉角轉化為旋轉矩陣
		input:n尤拉軸、theta轉角
		output:R旋轉矩陣
	'''
	n_m = cross_matrix(n)
	R = np.eye(3) + n_m*np.sin(theta) + np.dot(n_m,n_m)*(1 - np.cos(theta))
	return R
	
#採用z0與sw做參考面，求取第一臂形角8組解,求取執行時間0.3ms
def arm_angle_ikine_first_limit
 
(DH,Te):
	'''
	:param DH:
	:param Te:
	:return:
	'''
	#DH引數
	d1 = DH[0, 3]
	d3 = DH[2, 3]
	d5 = DH[4, 3]
	d7 = DH[6, 3]
	#求取SW
	s = np.array([0,0,d1])
	z0 = np.array([0,0,1])
	w = Te[0:3,3] - d7*Te[0:3,2]
	sw = w - s
	l_sw = nla.norm(sw)
	u_sw = sw/l_sw
	#***求取關節角4,第二個引數分別代表q4兩組解***#
	#計算beta值
	beta = np.arccos(
 
(d3**2 + d5**2 - l_sw**2)/(2*d3*d5))
	#關節角
	qq4_1 = pi - beta #上
	qq4_2 = beta - pi #下
	qq4 = np.array([qq4_1,qq4_2])

	#求解關節中心點旋轉矩陣,由於關節極限對稱,所以為0
	R_03_mid = np.array([[1, 0, 0],
						 [0, 0, 1],
						 [0, -1, 0]])

	R_74_mid = np.array([[1, 0, 0],
						 [0, 1, 0],
						 [0, 0, 1]])

	# ***求取關節角1,2,3,第三個引數分別代表q2兩組解***#
	#求取夾角alpha
	alpha_ = np.arccos((d3**2 + l_sw**2 - d5**2)/(2*d3*l_sw))
	#定義轉軸,用於求y_3_0
	V = z0 #參考軸,第一參考軸
	l = np.cross(sw,V)  #參考平面法向量
	u_l = l/nla.norm(l)

	#對應上下臂形
	alpha = np.array([alpha_, - alpha_])

	#定義8組關節角
	Q = np.zeros([8,7])

	#i=0代表上臂形,i=1代表下臂形
	for i in range(2):
		# 求變換矩陣,第二個下標是參考座標系
		R4_3 = np.array([[math.cos(qq4[i]), 0, math.sin(qq4[i])],
						 [math.sin(qq4[i]), 0, -math.cos(qq4[i])],
						 [0			, 1, 0]])

		#**求取初始R03**#
		#求取y3_0
		y3_0 = -np.dot(bf.euler_axis2rot(u_l, alpha[i]), u_sw)
		#求取x3_0
		x3_0 = (sw + (d3 + d5 * np.cos(qq4[i])) * y3_0) / (d5 * np.sin(qq4[i]))
		#求取z3_0
		z3_0 = np.cross(x3_0,y3_0)
		z3_0 = z3_0/nla.norm(z3_0)

		#求取座標系3在初始位置下在基座標系中的表示
		R_03_0 = np.zeros([3,3])
		R_03_0[:, 0] = x3_0
		R_03_0[:, 1] = y3_0
		R_03_0[:, 2] = z3_0

		#**求臂型角,基於關節角最小化**#
		#求取去q1,q2,q3對應重點R_psi_mid1
		R_psi_mid1 = np.dot(R_03_mid, R_03_0.T)
		Q_psi_mid1 = bf.rot_to_quaternion(R_psi_mid1)

		#求取去q5,q6,q7對應重點R_psi_mid2
		R_psi_mid2 = np.dot(np.dot(Te[0:3, 0:3], R_74_mid),np.dot(R_03_0, R4_3).T)
		Q_psi_mid2 = bf.rot_to_quaternion(R_psi_mid2)

		#求取四元數中點值
		Q_mid = (Q_psi_mid1 + Q_psi_mid2)/2.0

		#獲得優化臂型角
		psi = 0.0
		if (abs(Q_mid[0]) < math.pow(10, -6)):
			psi = -pi
		else:
			psi = math.atan(np.dot(Q_mid[1:4], u_sw)/Q_mid[0])

		# 求取臂型角到到旋轉矩陣
		R_psi = euler_axis2rot(u_sw, psi) 

		#求取3號座標系在任意位置在基座標系中的表示
		R_03 = np.dot(R_psi,R_03_0)

		#求取關節角2,有兩組解
		qq2_1 = np.arctan2(np.sqrt(1 - R_03[2, 1] ** 2), -R_03[2,1])
		qq2_2 = np.arctan2(-np.sqrt(1 - R_03[2, 1] ** 2), -R_03[2, 1])

		#求取關節角1
		qq1_1 = np.arctan2(-R_03[1, 1] * np.sin(qq2_1), -R_03[0, 1] * np.sin(qq2_1))
		qq1_2 = np.arctan2(-R_03[1, 1] * np.sin(qq2_2), -R_03[0, 1] * np.sin(qq2_2))

		#求取關節角3
		qq3_1 = np.arctan2(R_03[2, 2] * np.sin(qq2_1), -R_03[2, 0] * np.sin(qq2_1))
		qq3_2 = np.arctan2(R_03[2, 2] * np.sin(qq2_2), -R_03[2, 0] * np.sin(qq2_2))

		#****求取關節角5,6,7****#

		R4 = np.dot(R_psi,np.dot(R_03_0,R4_3))
		R4_7 = np.dot(Te[0:3,0:3].T,R4)
		R_47 = R4_7.T

		#求取關節角6,存在兩組解
		qq6_1 = np.arctan2(np.sqrt(1 - R_47[2, 2] ** 2), R_47[2, 2])
		qq6_2 = np.arctan2(-np.sqrt(1 - R_47[2, 2] ** 2), R_47[2, 2])

		# 求取關節角1
		qq5_1 = np.arctan2(R_47[1, 2] * np.sin(qq6_1), R_47[0, 2] * np.sin(qq6_1))
		qq5_2 = np.arctan2(R_47[1, 2] * np.sin(qq6_2), R_47[0, 2] * np.sin(qq6_2))

		# 求取關節角3
		qq7_1 = np.arctan2(R_47[2, 1] * np.sin(qq6_1), -R_47[2, 0] * np.sin(qq6_1))
		qq7_2 = np.arctan2(R_47[2, 1] * np.sin(qq6_2), -R_47[2, 0] * np.sin(qq6_2))

		#求解8組解
		q = np.array([[qq1_1, qq2_1, qq3_1, qq4[i], qq5_1, qq6_1, qq7_1],
					  [qq1_1, qq2_1, qq3_1, qq4[i], qq5_2, qq6_2, qq7_2],
					  [qq1_2, qq2_2, qq3_2, qq4[i], qq5_1, qq6_1, qq7_1],
					  [qq1_2, qq2_2, qq3_2, qq4[i], qq5_2, qq6_2, qq7_2]])
		if(i==0):
			Q[0:4,:] = q
		else:
			Q[4:8,:] = q
	return Q