对六自由度机械臂的运动控制及python实现（附源码）

经过一周的研究，对六自由度机械臂运动进行了研究，利用高中几何知识进行了运动控制策略的设计，无偿贡献出源码，可以为入门的小伙伴提供一定的借鉴。

1、机械臂物理参数的介绍

买了一个大象机械臂mycobot，作为研究对象，如果是其它机械臂，可以根据机械臂参数对代码进行修改。机械臂参数如下图所示：

1.1 部件与尺寸

机械臂如下部件组成，描述如下表：

序号	名称	描述	在python中的定义
1	骨长1	机械臂原点至第1弯头中心，距离170.48mm	类名：Mini_Arm，属性名：骨长1
2	骨长2	第1弯头至第4弯头中心，距离138.35mm	类名：Mini_Arm，属性名：骨长2
3	骨长3	第4弯头至第5弯头中心，距离100mm	类名：Mini_Arm，属性名：骨长3
4	骨长4	第5弯头至第7弯头中心，距离85mm	类名：Mini_Arm，属性名：骨长4
5	小骨	第（1至2）、（3至4）、（5至6）弯头距离的统称，距离80mm	类名：Mini_Arm，属性名：小骨
6	末骨	J5转轴中心至机械臂末端的距离，62.40mm	类名：Mini_Arm，属性名：末骨
7	J1	第1个旋转电机角度，范围[-165°,165°]	类名：Mini_Arm，属性名：J[0]
8	J2	第2个旋转电机角度，范围[-90°,90°]	类名：Mini_Arm，属性名：J[1]
9	J3	第3个旋转电机角度，范围[-120°,120°]	类名：Mini_Arm，属性名：J[2]
10	J4	第4个旋转电机角度，范围[-120°,120°]	类名：Mini_Arm，属性名：J[3]
11	J5	第5个旋转电机角度，范围[-180°,180°]	类名：Mini_Arm，属性名：J[4]
12	J6	第6个旋转电机角度，范围[-180°,180°]	类名：Mini_Arm，属性名：J[5]

1.2坐标系与工作空间

机械臂坐标系为数学直角正交坐标系，每个电机绕着旋转轴顺时针、逆时针旋转（正负已经标明），方向如图1所示，机械臂通过6个电机的旋转运动，实现了一系列复杂的动作。

注意，由于机械结构和设计的原因，有的电机不能进行-180°至180°的全向转动，会有一个转动的范围（见上表），这部分没有在python中实现。

2、机械臂运动控制原理

机械臂运动控制要解决的核心问题是，知道物体的位置（中心点坐标）和主方向（物体的朝向），程序自动计算出机械臂的各关节角度位置，将夹具准确送到抓取物体的部位，并对准抓取位置。

2.1机械臂运动的几何简化

总体思路是将机械臂的三维复杂运动简化为两个平面的运动，即“俯视平面”，和“垂直平面（本文叫R平面）”的组合运动。如下两个图：

2.2机械臂抓手即后面末关节的处理

普通的机械臂的控制以上就可以，由于我手头这个大象机械臂比较特殊，有很多90度弯头，而且抓手安装后，他的落点与上面简化模型就出现了偏差，因此，还需要根据这个特点，根据实际的物体坐标和主方向，计算出上节的理想抓手坐标点（x,y），然后就能求出全部关节的角度了。

3、python源码

以下是源代码，可以直接运行，

按键盘"O"键，进入自动鼠标点击右侧图机械臂工作半径范围的地方，自动计算各关节的旋转坐标。

再按键盘"O"键，退出鼠标点击计算模式，改用监盘手动操作模式，按“[”或 “]”，是J1关节旋转，按“w"是扩大作业半径，按"s"是缩小作业半径。

运行效果如下：

# -*- coding: utf-8 -*-
"""
Created on Sat Jul 31 11:29:37 2021@author: JAMES费
"""import matplotlib.animation as animation
import matplotlib.pyplot as plt
import sys
import logging
import time
import numpy as np
import math
import cmath
import cv2
logging.basicConfig(level=logging.DEBUG)class Mini_Arm:"""定义机械臂的长度，单位mm工作空间坐标计算"""exval={"com":"COM21","baud":115200,"port":6666,"host":"localhost"}    #以下定义机械臂的物理参数，对照手册配图骨长1=170.46骨长2=136.35骨长3=100骨长4=75小骨长=80末骨长=62.4抓手长=60J=[0,0,0,0,0,0] #表示关节角度，J1,J2,J3,J4,J5,J6抓手坐标=(0,0)工作半径范围=(50,骨长2+骨长3-5)R=0mycobot=None当前坐标=[0,0,0,0,0,0]当前角度=[0,0,0,0,0]winevent={"doubleclick":0,"keymod":0}winH=900winW=1600pz2=(int(winW*3/4),int(winH/3))p0=(None,None)thi=0pJ5=(None,None)def __init__(self, **kwargs):"""初始化类:串口:com="COM21"波特率:baud=115200web端口:port=6666主机地址：host="localhost""""self.mathods=dir(self) for k,v in kwargs.items():if k in self.exval.keys():                self.exval[k]=v#self.mycobot = MyCobot(self.exval["com"])cv2.namedWindow('mini_arm')cv2.setMouseCallback('mini_arm',self.mousecallback)#print(self.exval["com"])#监听点击像素坐标def mousecallback(self,event,x,y,flags,param):if event==cv2.EVENT_LBUTTONDBLCLK:             if self.winevent["keymod"]==1:self.根据窗口点击计算目标点(x,y)print("点击坐标点",(x,y))def 根据窗口点击计算目标点(self,x,y):#转化为机械臂坐标(px,py)=self.pz2#r=np.sqrt((x-px)**2+(py-y)**2) (x0,y0)=(x-px,py-y)self.thi=30res,J1,J5,R=self.计算J1_5_R(x0,y0,self.thi) if res!=True:print("out of workspace")self.p0=(None,None)else:   self.p0=(x0,y0)            self.J[0],self.J[4],self.R=J1,J5,Rprint("计算结果J1，J5，R：",[self.J[0],self.J[4],self.R])print("目标点坐标",self.p0)#计算其它坐标return 1def 计算J1和R(self):"""#已知抓手坐标，求J1单位/°,R距离/mm"""        try:(x,y)=self.抓手坐标J1=np.arctan(y/x)/np.pi*180self.R=x/np.cos(J1)self.J[0]=J1return 1except Exception as e:logging.error("计算J1异常：",e)    def 计算抓手坐标(self,J1,R): """#已知J1单位/°,R距离/mm，求抓手坐标"""try:(x,y)=(0,0)x=R*np.cos(J1/180*np.pi)y=R*np.sin(J1/180*np.pi)return (round(x, 2),round(y, 2))except Exception as e:logging.error("计算J1异常：",e)   def 计算J1(self,x,y):  """#已知抓手坐标，求J1单位/°,R距离/mm"""try:R=np.sqrt(x**2+y**2)             J1=math.degrees(cmath.polar(complex(x,y))[1])return round(R, 2),round(J1, 2)except Exception as e:logging.error("计算J1异常：",e)  def 判断象限(self,x,y):"""根据直角坐标（x,y）,判断在第几象限返回值1：第一象限2：第二象限3：第三象限4：第四象限10：在x正半轴-10：在x负半轴20：在y正半轴-20：在y负半轴0:在坐标原点"""if x>0 and y>0:return 1elif x<0 and y>0:return 2elif x<0 and y<0:return 3elif x>0 and y<0:return 4elif x>0 and y==0:return 10elif x<0 and y==0:return -10elif x==0 and y>0:return 20elif x==0 and y<0:return -20else:return 0def 计算J5坐标(self,x0,y0,thi):"""已知物体的坐标，主方向，计算J1、J6、Rx0，y0为物体中心点坐标thi为物体主方向与机械臂x轴的逆时针夹角°"""g6=self.末骨长g0=self.小骨长try:        x5=x0+g6*np.cos(math.radians(thi))x5i=x0+g6*np.cos(math.radians(180+thi))y5=y0+g6*np.sin(math.radians(thi))y5i=y0+g6*np.sin(math.radians(180+thi))r=np.sqrt(x5**2+y5**2)ri=np.sqrt(x5i**2+y5i**2)  #print("x5,y5,x5i,y5i,r,ri",x5,y5,x5i,y5i,r,ri)if r<=ri and g0<r:return (x5,y5)elif r<=ri and g0<ri and g0>r:return (x5i,y5i)elif r>ri and g0<ri:return (x5i,y5i)elif r>ri and g0<r and g0>ri:return (x5,y5)else:self.p0=(None,None)logging.error("cannot 计算 J5，woring 目标值")return Falseexcept Exception as e:logging.error("计算计算J5坐标异常：",e)        def 计算三点逆时针角度(self,p1,p0,p2):"""计算向量P0-->p1至 P0-->p2的逆时针转角math.degrees(x)弧度转换为角度math.radians(x)角度转弧度cn = complex(3,4)cmath.polar(cn)  #返回长度和弧度cn1 = cmath.rect(2, cmath.pi)极坐标转直直角坐标cn1.real，cn1.imag#返回x,y"""try:     (x0,y0)=p0(x1,y1)=p1(x2,y2)=p2 #向量P0-->p1的极坐标转角x=x1-x0y=y1-y0alpha1=math.degrees(cmath.polar(complex(x,y))[1])#向量P0-->p2的极坐标转角x=x2-x0y=y2-y0alpha2=math.degrees(cmath.polar(complex(x,y))[1])    alpha=alpha2-alpha1return round(alpha, 2)  except Exception as e:logging.error("计算计算J5坐标异常：",e)   def 计算J1_5_R(self,x0,y0,thi): """已知物体的坐标，主方向，计算J1、J5、Rx0，y0为物体中心点坐标thi为物体主方向与机械臂x轴的逆时针夹角°"""g0=self.小骨长(minr,maxr)=self.工作半径范围try:pJ5=self.计算J5坐标(x0,y0,thi)if pJ5!=0:(x5,y5)=pJ5self.pJ5=pJ5#print("Pj5:",pJ5)cr=np.sqrt(x5**2+y5**2)i1=math.degrees(cmath.polar(complex(x5,y5))[1])                R=np.sqrt(cr**2-g0**2)i2=math.degrees(np.arcsin(g0/cr))J1=i1+i2p0=(x0,y0)#目标点中心坐标pm=self.计算抓手坐标(J1,R)#假想目标点中心坐标J5=self.计算三点逆时针角度(p0,pJ5,pm)if R<minr or R>maxr:logging.error("超出工作范围")return False,self.J[0],self.J[4],self.Relse:return True,round(J1, 2),round(J5, 2),round(R, 2)else:logging.error("计算J1_5_R异常")return False,self.J[0],self.J[4],self.Rexcept Exception as e:logging.error("计算计算J1_5_R异常：",e)          def 计算J2_4(self):"""已知g1=self.骨长1g2=self.骨长2g3=self.骨长3g4=self.骨长4g5=g4+self.抓手长  r=self.R求关节，J2、J3、J4"""try:g1=self.骨长1g2=self.骨长2g3=self.骨长3g4=self.骨长4g5=g4+self.抓手长  r=self.Rlr=np.sqrt(r**2+(g5-g1)**2)Ji=np.arcsin(r/lr)/np.pi*180;i2=np.arccos((lr**2+g2**2-g3**2)/(2*lr*g2))/np.pi*180i3=np.arccos((g3**2+g2**2-lr**2)/(2*g3*g2))/np.pi*180i4=np.arccos((lr**2+g3**2-g2**2)/(2*lr*g3))/np.pi*180J2=Ji-i2J3=180-i3J4=180-i4-Jiself.J[1]=J2self.J[2]=J3self.J[3]=J4    #print("计算结果",lr,Ji,i2,i3,i4,J2,J3,J4)return 1except Exception as e:logging.error("计算J2_4异常：",e)    def 计算各关节点坐标(self):"""已知关节旋转角度，求在R平面的关节坐标"""g1=self.骨长1g2=self.骨长2g3=self.骨长3g4=self.骨长4g5=g4+self.抓手长  r=self.RJ2=self.J[1]try:x=[0,0,0,0,0] y=[0,0,0,0,0]x[1]=0#关节J2的x坐标y[1]=g1#关节J2的y坐标x[2]=g2*np.cos((90+J2)/180*np.pi)y[2]=g1+g2*np.sin((90+J2)/180*np.pi)x[3]=-ry[3]=g5 x[4]=-ry[4]=0return (x,y)except Exception as e:logging.error("计算各点坐标异常：",e) def ui(self,H,W):#text formatorg = (40, 80)fontFace = cv2.FONT_HERSHEY_COMPLEXfontScale = 0.5fontcolor = (0, 255, 0) # BGRthickness = 1 lineType = 4bottomLeftOrigin = 1while 1: #背景颜色               高，宽self.winH=Hself.winW=Wbackground = np.zeros((H, W, 3), np.uint8) #生成一个空灰度图像        key = cv2.waitKey(1)if int(key) == ord('q'):cv2.destroyWindow('mini_arm')breakif self.winevent["keymod"]==0:if int(key) == ord('['):self.J[0]+=2if self.J[0]>=160:self.J[0]=160if int(key) == ord(']'):self.J[0]-=2if self.J[0]<=-160:self.J[0]=-160if int(key) == ord('w'):self.R+=2if self.R>=self.工作半径范围[1]:self.R=self.工作半径范围[1]if int(key) == ord('s'):self.R-=2if self.R<=self.工作半径范围[0]:self.R=self.工作半径范围[0]if int(key)==ord('z'):self.R=200self.J[0]=0else:if int(key) == ord('['):self.thi+=2if self.thi>=180:self.thi=180res,J1,J5,R=self.计算J1_5_R(self.p0[0],self.p0[1],self.thi) if res:self.J[0],self.J[4],self.R=J1,J5,Rif int(key) == ord(']'):self.thi-=2if self.thi<=-180:self.thi=-180res,J1,J5,R=self.计算J1_5_R(self.p0[0],self.p0[1],self.thi) if res:self.J[0],self.J[4],self.R=J1,J5,R if int(key)==ord('o'):if self.winevent["keymod"]==0:self.winevent["keymod"]=1else:self.winevent["keymod"]=0            #update(a,axes) if self.winevent["keymod"]==1:text = "window selected mod press o to esc"(px,py)=self.pz2if self.p0 !=(None,None) and self.pJ5!=(None,None):p=self.计算抓手坐标(self.J[0],self.R)px0=px+int(p[0])py0=py-int(p[1])  cv2.circle(background,(int(self.p0[0]+px),int(py-self.p0[1])),5,(255,255,255),-1) cv2.circle(background,(int(self.pJ5[0]+px),int(py-self.pJ5[1])),5,(255,255,255),-1) cv2.line(background,(px0,py0),(int(self.pJ5[0]+px),int(py-self.pJ5[1])),(255,0,255),4,cv2.LINE_AA)else:text = "keyboard manhand control mod " tips="press [/] turn J1,w/s to front / back"cv2.putText(background, tips, (W-400, 30), fontFace, fontScale, fontcolor, thickness, lineType)       cv2.putText(background, text, (W-400, 10), fontFace, fontScale, fontcolor, thickness, lineType)        #######绘制左边边的坐标图pz1=(int(W*1.5/4),int(H/2))#左侧坐标的原点if self.计算J2_4():point=self.计算各关节点坐标()x=point[0]y=point[1]##画关节点num=0bones=[]           for dx,dy,j in zip(x,y,self.J):tx=pz1[0]+int(dx)ty=pz1[1]-int(dy) bones.append((tx,ty))cv2.circle(background,(tx,ty),5,(255,255,0),-1) #画标注text = str((round(dx, 2),round(dy, 2)))+"J:"+str(round(j, 2))+"'"#text = 'angles:little[ddd'+str(888)cv2.putText(background, text, (tx+8, ty-8), fontFace, fontScale, fontcolor, thickness, lineType)##画骨架for i in range(4):                cv2.line(background,bones[i],bones[i+1],(255,0,255),4,cv2.LINE_AA)#######绘制右边的坐标图pz2=(int(W*3/4),int(H/3))#旋转坐标的原点self.pz2=pz2#计算抓手坐标及转换成图像坐标        p=self.计算抓手坐标(self.J[0],self.R)px=pz2[0]+int(p[0])py=pz2[1]-int(p[1])       ###画半径cv2.circle(background,pz2,int(self.R),(0,0,255),2)  #画圆#画抓手坐标点cv2.circle(background,(px,py),5,(0,255,255),-1)  #画圆#画标注text = str(p)+"J1:"+str(self.J[0])#text = 'angles:little[ddd'+str(888)cv2.putText(background, text, (px+8, py-8), fontFace, fontScale, fontcolor, thickness, lineType)cv2.line(background,pz2,(px,py),(0,255,255),2,cv2.LINE_AA)#计算边界线p=self.计算抓手坐标(160,self.R)px=pz2[0]+int(p[0])py=pz2[1]-int(p[1]) pyy=pz2[1]+int(p[1]) cv2.line(background,pz2,(px,py),(0,0,255),4,cv2.LINE_AA)cv2.line(background,pz2,(px,pyy),(0,0,255),4,cv2.LINE_AA)cv2.imshow('mini_arm',background)if __name__ == '__main__':a=Mini_Arm()    a.R=200a.J[0]=30print("正解抓手坐标为：",a.计算抓手坐标(a.J[0],a.R))x=173.20y=100print("正解抓手J1为：",a.计算J1(x,y))a.ui(900,1600)