机械臂正逆运动学-----数值解

建立DH坐标系
求正运动学
- 单关节齐次传递矩阵
- 正运动学：返回齐次矩阵
- 正运动学：返回欧拉角向量
求雅可比矩阵
求机械臂逆运动学
合成通用运动学类

机械臂的运动学包括正运动学和逆运动学，其雅可比矩阵代表速度级的正逆运动学，机械臂的逆运动学数值解采用雅可比矩阵在求取速度级的逆运动学，然后采用迭代法求取位置级逆运动学。

建立DH坐标系

源代码：

#DH参数
DH0_armc = np.array([[0, -pi/2, 0, 0.248],[0,  pi/2, 0, 0    ],[0, -pi/2, 0, 0.305],[0,  pi/2, 0, 0    ],[0, -pi/2, 0, 0.306],[0,  pi/2, 0, 0    ],[0,  0,    0, 0.213]])
#关节极限
q_min_armc = np.array([-180, -95, -180, -95, -180, -95, -180])*pi/180
q_max_armc = np.array([180, 95, 180, 95, 180, 95, 180])*pi/180

求正运动学

单关节齐次传递矩阵

源代码：

#单关节传递矩阵
def  trans(theta,alpha,a,d):'''本函数用于求取n自由度机械臂正运动学输入参数为DH参数，角度单位为rad，长度单位为mm参数分别为theta,alpha,a,d，为0维常数值返回齐次传递函数矩阵'''T = np.array([[math.cos(theta), -math.sin(theta)*math.cos(alpha), math.sin(theta)*math.sin(alpha),  a*math.cos(theta)],[math.sin(theta), math.cos(theta)*math.cos(alpha),  -math.cos(theta)*math.sin(alpha), a*math.sin(theta)],[0,               math.sin(alpha),                  math.cos(alpha),                  d                ],[0,               0,                                0,                                  1              ]])return T

正运动学：返回齐次矩阵

源代码：

#返回齐次矩阵的正运动学
def fkine(theta,alpha,a,d):'''本函数用于求取n自由度机械臂正运动学输入参数为DH参数，角度单位为rad，长度单位为mm参数分别为theta,alpha,a,d，为1维常数值返回齐次传递函数矩阵 '''#关节自由度n = len(theta)#建立4×4的齐次传递矩阵,定义为numpy类型An = np.eye(4)for i  in range(n):T =  trans(theta[i],alpha[i],a[i],d[i])An = np.dot(An,T)  #末端到惯性坐标系传递矩阵return An

正运动学：返回欧拉角向量

源代码：

#输入初始时刻DH_0和相对转角,输出六维末端位姿
def fkine_euler(DH_0,qr):'''本函数用于求取n自由度机械臂正运动学输入参数为DH参数，角度单位为rad，长度单位为mm参数分别为theta,alpha,a,d，为1维常数值返回齐次传递函数矩阵'''#DH参数theta = DH_0[:, 0] + qralpha = DH_0[:, 1]a = DH_0[:, 2]d = DH_0[:, 3]#关节自由度n = len(theta)xe = np.zeros(6)#建立4×4的齐次传递矩阵,定义为numpy类型An = np.eye(4)for i  in range(n):T =  trans(theta[i],alpha[i],a[i],d[i]) #需要加入该函数：单关节齐次矩阵An = np.dot(An,T)  #末端到惯性坐标系传递矩阵xe[0:3] = An[0:3,3]xe[3:6] = rot2euler_zyx(An[0:3,0:3]) #需要加入该函数：欧拉角转换函数return xe

旋转矩阵与欧拉角转换：

#旋转矩阵转变为ZYX欧拉角
def rot2euler_zyx(Re):'''ZYX欧拉角速度变为姿态角速度转化矩阵input:旋转矩阵output:欧拉角[alpha,beta,gamma]'''euler_zyx = np.zeros(3)if(abs(abs(Re[2, 0]) - 1) < math.pow(10, -6)):if(Re[2,0] < 0):beta = pi/2alpha = np.arctan2(-Re[1,2],Re[1,1])gamma = 0else:beta = -pi/2alpha = -np.arctan2(-Re[1, 2], Re[1, 1])gamma = 0else:p_beta = math.asin(-Re[2,0])cb = np.cos(p_beta)alpha = math.atan2(Re[1,0]*cb,Re[0,0]*cb)gamma = math.atan2(Re[2,1]*cb,Re[2,2]*cb)if((math.sin(gamma)*Re[2,1]) < 0):beta = pi - p_betaelse:beta = p_betaeuler_zyx[0] = alphaeuler_zyx[1] = betaeuler_zyx[2] = gammafor i in range(3):if(euler_zyx[i]>=3.14 or euler_zyx[i]<=-3.14):euler_zyx[i] = 0.0return euler_zyx

求雅可比矩阵

源代码：
（1）便于理解版

#构造法求雅克比矩阵,时间0.3ms
def jacobian(DH_0,qr):'''本函数用于求取机械臂的雅克比矩阵input:DH_0参数，长度单位mm,角度单位redqr,相对初始位置的转角output:J,该位置点的雅克比矩阵'''n = len(qr)theta = DH_0[:,0] + qralpha = DH_0[:,1]a = DH_0[:,2]d = DH_0[:,3]#求取末端位置An = fkine(theta,alpha,a,d)  #正运动学函数p_n = An[0:3,3]J = np.zeros([6,n])J[3:6,n-1] = An[0:3,2]#求取其余转轴方向及位置点Ai = np.eye(4)for i in range(n-1):z_i = Ai[0:3,2]p_i = Ai[0:3,3]p_in = p_n - p_iJ[0:3,i] = np.cross(z_i,p_in)J[3:6,i] = z_iAi = np.dot(Ai, trans(theta[i], alpha[i], a[i], d[i]))return J

（2）高速计算版

#运行时间更快0.1ms
def jeco_0(DH_0, qr):'''本函数基于雅克比迭代求解n自由度机械臂逆运动学方程input:DH_0 = [q_init,alpha,a,d];q_ready是上一时刻的位置,单位:弧度;T0e为DH坐标系确定的DH{0}坐标系与DH{6}之间的关系(目标矩阵);efs求解误差阀值，默认值10^(-10)i_limit迭代最大次数,默认值1000            output:qq为相对与DH_q0的转动角度,单位:弧度;已处理到[-pi, pi] 之间'''#建立初时刻迭代初值q = DH_0[:,0] + qralpha = DH_0[:,1]a = DH_0[:,2]d = DH_0[:,3]#计数及标签n = len(q)#计算雅克比矩阵U = np.eye(4)Jn = np.zeros([6,n])T = np.zeros([4,4,n])for i in range(n):i = n - i - 1T[:,:,i] = trans(q[i],alpha[i],a[i],d[i])#单关节传递函数U = np.dot(T[:,:,i],U)dd = np.array([-U[0,0]*U[1,3] + U[1,0]*U[0,3],-U[0,1]*U[1,3] + U[1,1]*U[0,3],-U[0,2]*U[1,3] + U[1,2]*U[0,3]])Jn[0:3,i] = ddJn[3:6,i] = U[2,0:3] An = fkine(q,alpha,a,d)     #正运动学函数R = An[0:3,0:3]J_R = np.zeros([6,6])J_R[0:3,0:3] = RJ_R[3:6,3:6] = RJ0 = np.dot(J_R,Jn)return J0

求机械臂逆运动学

源代码：

#***基于雅克比矩阵迭代求解逆运动学***#
def iterate_ikine(DH_0, q_ready, T0e, efs = pow(10,-12), i_max = 1000):'''本函数基于雅克比迭代求解n自由度机械臂逆运动学方程input:DH_0 = [q_init,alpha,a,d];q_ready是上一时刻的位置,单位:弧度;T0e为DH坐标系确定的DH{0}坐标系与DH{6}之间的关系(目标矩阵);efs求解误差阀值，默认值10^(-10)i_limit迭代最大次数,默认值1000            output:qq为相对与DH_q0的转动角度,单位:弧度;已处理到[-pi, pi] 之间'''#建立初时刻迭代初值q_r = DH_0[:,0] + q_readyalpha = DH_0[:,1]a = DH_0[:,2]d = DH_0[:,3]#计数及标签n = len(q_r)deltaQ = 1  temp_count = 0#迭代循环求解while (deltaQ > efs):#求解正运动学An = np.eye(4)T = np.zeros([4,4,n])for i in range(n):T[:,:,i] = trans(q_r[i],alpha[i],a[i],d[i])An = np.dot(An,T[:,:,i])#计算末端误差dA = np.zeros(6)dA[0:3] = T0e[0:3,3] - An[0:3,3]dA[3:6] = 0.5*(np.cross(An[0:3,0],T0e[0:3,0]) + np.cross(An[0:3,1],T0e[0:3,1]) + np.cross(An[0:3,2],T0e[0:3,2]))#print dA                       #计算雅克比矩阵U = np.eye(4)Jn = np.zeros([6,n])for i in range(n):i = n - i - 1U = np.dot(T[:,:,i],U)dd = np.array([ -U[0,0]*U[1,3] + U[1,0]*U[0,3],-U[0,1]*U[1,3] + U[1,1]*U[0,3],-U[0,2]*U[1,3] + U[1,2]*U[0,3]])Jn[0:3,i] = ddJn[3:6,i] = U[2,0:3]R = An[0:3,0:3]J_R = np.zeros([6,6])J_R[0:3,0:3] = RJ_R[3:6,3:6] = RJ0 = np.dot(J_R,Jn)#求取关节角关节角度偏差值dq = np.dot(np.linalg.pinv(J0),dA)q_r = q_r + dqdeltaQ = np.linalg.norm(dq)temp_count =temp_count + 1if (temp_count > i_max):print("Solution wouldn't converge")return q_readyq_tmp = q_r - DH_0[:,0]       q = bf.qq_choose(q_tmp) #选择函数return q

关节角选择函数：

#将关节角计算到正负pi
def qq_choose(qq):'''本函数用于选着关节角范围input:qq为计算出的关节角output:q关节角范围[-pi,pi]'''n = len(qq)q = np.copy(qq)for i in range(n):while (q[i] > pi):q[i] = q[i] - 2*piwhile (q[i] < - pi):q[i] = q[i] + 2*pireturn q

合成通用运动学类

源代码：

#==========================通用运动学类======================#
class GeneralKinematic(object):'''函数依赖math和numpy'''def __init__(self, DH_0,q_min=rp.q_min, q_max=rp.q_max):self.DH_0 = DH_0self.theta = DH_0[:, 0]self.alpha = DH_0[:, 1]self.a = DH_0[:, 2]self.d = DH_0[:, 3]self.q_min = q_minself.q_max = q_maxself.n = len(self.theta)#相邻关节传递矩阵def trans(self, theta, alpha, a, d):T = np.array([[math.cos(theta), -math.sin(theta) * math.cos(alpha),math.sin(theta) * math.sin(alpha), a * math.cos(theta)],[math.sin(theta), math.cos(theta) * math.cos(alpha),-math.cos(theta) * math.sin(alpha), a * math.sin(theta)],[0, math.sin(alpha), math.cos(alpha), d],[0, 0, 0, 1]])return T# ZYX欧拉角转变为旋转矩阵def euler_zyx2rot(self,phi):'''ZYX欧拉角转变为旋转矩阵input:欧拉角output:旋转矩阵'''R = np.array([[np.cos(phi[0]) * np.cos(phi[1]),np.cos(phi[0]) * np.sin(phi[1]) * np.sin(phi[2]) - np.sin(phi[0]) * np.cos(phi[2]),np.cos(phi[0]) * np.sin(phi[1]) * np.cos(phi[2]) + np.sin(phi[0]) * np.sin(phi[2])],[np.sin(phi[0]) * np.cos(phi[1]),np.sin(phi[0]) * np.sin(phi[1]) * np.sin(phi[2]) + np.cos(phi[0]) * np.cos(phi[2]),np.sin(phi[0]) * np.sin(phi[1]) * np.cos(phi[2]) - np.cos(phi[0]) * np.sin(phi[2])],[-np.sin(phi[0]), np.cos(phi[1]) * np.sin(phi[2]), np.cos(phi[1]) * np.cos(phi[2])]])return R# 旋转矩阵转变为ZYX欧拉角def rot2euler_zyx(self, Re):'''ZYX欧拉角速度变为姿态角速度转化矩阵input:旋转矩阵output:欧拉角[alpha,beta,gamma]'''euler_zyx = np.zeros(3)if(abs(abs(Re[2, 0]) - 1) < math.pow(10, -6)):if(Re[2,0] < 0):beta = pi/2alpha = np.arctan2(-Re[1,2],Re[1,1])gamma = 0else:beta = -pi/2alpha = -np.arctan2(-Re[1, 2], Re[1, 1])gamma = 0else:p_beta = math.asin(-Re[2,0])cb = np.cos(p_beta)alpha = math.atan2(Re[1,0]*cb,Re[0,0]*cb)gamma = math.atan2(Re[2,1]*cb,Re[2,2]*cb)if((math.sin(gamma)*Re[2,1]) < 0):beta = pi - p_betaelse:beta = p_betaeuler_zyx[0] = alphaeuler_zyx[1] = betaeuler_zyx[2] = gammafor i in range(3):if(euler_zyx[i]>=3.14 or euler_zyx[i]<=-3.14):euler_zyx[i] = 0.0return euler_zyx# 将关节角计算到正负pidef qq_choose(self, qq):'''本函数用于选着关节角范围input:qq为计算出的关节角output:q关节角范围[-pi,pi]'''q = np.copy(qq)for i in range(self.n):while (q[i] > math.pi):q[i] = q[i] - 2 * math.piwhile (q[i] < - math.pi):q[i] = q[i] + 2 * math.pireturn q#正运动学,返回齐次矩阵def fkine(self, qr):An = np.eye(4)for i in range(self.n):T = self.trans(self.theta[i] + qr[i], self.alpha[i], self.a[i], self.d[i])An = np.dot(An, T)  # 末端到惯性坐标系传递矩阵return An#正运动学,输出六维末端位姿,姿态用zyx欧拉角表示def fkine_euler(self, qr):xe = np.zeros(6)An = np.eye(4)for i in range(self.n):T = self.trans(self.theta[i] + qr[i], self.alpha[i], self.a[i], self.d[i])An = np.dot(An, T)  # 末端到惯性坐标系传递矩阵xe[0:3] = An[0:3, 3]xe[3:6] = self.rot2euler_zyx(An[0:3, 0:3])return xe#求取雅克比矩阵def jeco(self, qr):# 计算雅克比矩阵U = np.eye(4)Jn = np.zeros([6, self.n])T = np.zeros([4, 4, self.n])for i in range(self.n):i = self.n - i - 1T[:, :, i] = self.trans(self.theta[i] + qr[i], self.alpha[i], self.a[i], self.d[i])U = np.dot(T[:, :, i], U)dd = np.array([-U[0, 0] * U[1, 3] + U[1, 0] * U[0, 3],-U[0, 1] * U[1, 3] + U[1, 1] * U[0, 3],-U[0, 2] * U[1, 3] + U[1, 2] * U[0, 3]])Jn[0:3, i] = ddJn[3:6, i] = U[2, 0:3]An = self.fkine(qr)R = An[0:3, 0:3]J_R = np.zeros([6, 6])J_R[0:3, 0:3] = RJ_R[3:6, 3:6] = RJ0 = np.dot(J_R, Jn)return J0# ***基于雅克比矩阵迭代求解逆运动学***#def iterate_ikine(self, q_guess, Te, efs=pow(10, -12), i_max=1000):'''本函数基于雅克比迭代求解n自由度机械臂逆运动学方程q_ready是上一时刻的位置,单位:弧度;T0e为DH坐标系确定的DH{0}坐标系与DH{6}之间的关系(目标矩阵);efs求解误差阀值，默认值10^(-10)i_limit迭代最大次数,默认值1000output:qq为相对与DH_q0的转动角度,单位:弧度;已处理到[-pi, pi] 之间'''# 建立初时刻迭代初值q_r =self.theta + q_guess# 计数及标签deltaQ = 1temp_count = 0# 迭代循环求解while (deltaQ > efs):# 求解正运动学An = np.eye(4)T = np.zeros([4, 4, self.n])for i in range(self.n):T[:, :, i] = self.trans(q_r[i], self.alpha[i], self.a[i], self.d[i])An = np.dot(An, T[:, :, i])# 计算末端误差dA = np.zeros(6)dA[0:3] = Te[0:3, 3] - An[0:3, 3]dA[3:6] = 0.5 * (np.cross(An[0:3, 0], Te[0:3, 0]) + np.cross(An[0:3, 1], Te[0:3, 1])+ np.cross(An[0:3, 2], Te[0:3, 2]))# 计算雅克比矩阵U = np.eye(4)Jn = np.zeros([6, self.n])for i in range(self.n):i = self.n - i - 1U = np.dot(T[:, :, i], U)dd = np.array([-U[0, 0] * U[1, 3] + U[1, 0] * U[0, 3],-U[0, 1] * U[1, 3] + U[1, 1] * U[0, 3],-U[0, 2] * U[1, 3] + U[1, 2] * U[0, 3]])Jn[0:3, i] = ddJn[3:6, i] = U[2, 0:3]R = An[0:3, 0:3]J_R = np.zeros([6, 6])J_R[0:3, 0:3] = RJ_R[3:6, 3:6] = RJ0 = np.dot(J_R, Jn)# 求取关节角关节角度偏差值dq = np.dot(np.linalg.pinv(J0), dA)q_r = q_r + dqdeltaQ = np.linalg.norm(dq)temp_count = temp_count + 1if (temp_count > i_max):print("Solution wouldn't converge")return q_guessq_tmp = q_r - self.thetaq = self.qq_choose(q_tmp)return q#机械臂关节极限判断，返回值为0或1def exceed_joint_limit(self, qq, q_min, q_max):'''判断关节角是否超出限制input:关节角，关节角范围outpu：0,未超出，1超出'''n = len(qq)limit = Falsefor i in range(n):if((qq[i] < q_min[i]) or (qq[i] > q_max[i])):print "第", i+1, "关节超出极限:", qq[i]*180/np.pilimit = Truebreakreturn limit#带关节限制def iterate_ikine_limit_xyz(self, q_guess, Xe):Te = np.eye(4)Te[0:3, 0:3] = self.euler_zyx2rot(Xe[3:])Te[0:3, 3] = Xe[:3]print "Te:", Teqr = self.iterate_ikine(q_guess, Te)flag = self.exceed_joint_limit(qr ,self.q_min, self.q_max)if(flag):#print "flag:", flagqr = np.copy(q_guess)return qr# 带关节限制def iterate_ikine_limit(self, q_guess, Te):qr = self.iterate_ikine(q_guess, Te)flag = self.exceed_joint_limit(qr, self.q_min, self.q_max)if (flag):# print "flag:", flagqr = np.copy(q_guess)return qr

机械臂正逆运动学-----数值解相关推荐

修正逆解文章——六轴UR机械臂正逆运动学求解_MATLAB代码（标准DH参数表）
如下参考链接1的作者大大实现了UR5机械臂的正运动学和逆运动学的Matlab代码.但逆解部分在不同版本的Matlab中运行有错误. 本篇文章是MatlabR2016a下完成的,并说明一下原代码错误的原 ...
实验一机械臂正逆运动学
实验一机械臂正逆运动学一.实验目的 1.巩固正逆运动学基础概念. 2.了解正逆运动学在机械臂控制中的实际用途. 二.实验内容 1.机械臂模型DH参数的计算. 2.机械臂正运动学的计算. 3.机械臂 ...
UR5构型机械臂正逆运动学
前言整理之前的一个项目,当时看着一个博客硬生生计算了差不多一个星期.尝试用MatLab符号推导工具箱化简一部分工作.我使用的大象机器人一款开源入门级协作机器人产品myCobot,开发文档十分完善,但 ...
UR机械臂正逆运动学求解
最近有个任务:求解UR机械臂正逆运动学,在网上参考了一下大家的求解办法,众说纷纭,其中有些朋友求解过程非常常规,但是最后求解的8组解,只有4组可用.在这里我介绍一个可以求解8组解析解的方法,供大家参考 ...
五自由度机械臂正逆运动学算法（Ｃ语言+Matlab）
五自由度机械臂建模学习代码都记录在个人github上,欢迎关注~ Matlab机器人工具箱版本9.10 机械臂还是原来的机械臂,之前用ROS做底层驱动,不需要写正逆运动学和相关算法就能得到一些简单的 ...
ROS机械臂正逆运动学
这里做一个六轴机械臂用于正逆运动学实验. 这里其实一共只有3轴,只有3轴位置没有姿态.所以urdf文件里我在末端做了3个虚拟关节,以便将kdl的frame能够填满,使得齐次坐标变换是规则的. 1.ur ...
用matlab实现机械臂正逆运动学控制
设计要求: 1.建立一个三自由度的机器人 2.建立坐标系,给出 D-H 参数表: 3.推导正运动学,并写出机器人的齐次变换矩阵: 4.推导逆运动学,并让机器人完成按要求绘制给定图形. 5.MATLAB ...
matlab欧拉迭代,matlab机械臂正逆运动学求解问题，使用牛顿-欧拉迭代算法
代码复制的有问题,详细见本楼,谢谢. clc;clear; DR=pi/180; %time j = 1; for i = 0 : 0.1 : 2 %input theta= 45 * DR *(1+ ...
6轴机械臂正逆解运算实现
6轴机械臂正逆解运算实现利用Gluon-6L3机械臂模型的参数,对机械臂进行运动学分析. 这里采用标准DH坐标系,并将d6设置为0,方便后续计算. 首先,SDH的变换矩阵为: ii−1T=Ai=^{ ...

机械臂正逆运动学-----数值解

机械臂正逆运动学-----数值解

建立DH坐标系

求正运动学

单关节齐次传递矩阵

正运动学：返回齐次矩阵

正运动学：返回欧拉角向量

求雅可比矩阵

求机械臂逆运动学

合成通用运动学类

机械臂正逆运动学-----数值解相关推荐

最新文章

热门文章