经过一周的研究，对六自由度机械臂运动进行了研究，利用高中几何知识进行了运动控制策略的设计，无偿贡献出源码，可以为入门的小伙伴提供一定的借鉴。
1、机械臂物理参数的介绍
买了一个六轴机械臂，作为研究对象，如果是其它机械臂，可以根据机械臂参数对代码进行修改。机械臂参数如下图所示：
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
抓手长=60
J=[0,0,0,0,0,0] #表示关节角度，J1,J2,J3,J4,J5,J6
抓手坐标=(0,0)
工作半径范围=(50,骨长2+骨长3-5)
R=0
mycobot=None
当前坐标=[0,0,0,0,0,0]
当前角度=[0,0,0,0,0]
winevent={
"doubleclick":0,
"keymod":0
}
winH=900
winW=1600
pz2=(int(winW*3/4),int(winH/3))
p0=(None,None)
thi=0
pJ5=(None,None)
def __init__(self, **kwargs):
"""
初始化类:
串口:com="COM21"
波特率:baud=115200
web端口: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=30
res,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,R
print("计算结果J1，J5，R：",[self.J[0],self.J[4],self.R])
print("目标点坐标",self.p0)
#计算其它坐标
return 1
def 计算J1和R(self):
"""
#已知抓手坐标，求J1单位/°,R距离/mm
"""
try:
(x,y)=self.抓手坐标
J1=np.arctan(y/x)/np.pi*180
self.R=x/np.cos(J1)
self.J[0]=J1
return 1
except 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 1
elif x<0 and y>0:
return 2
elif x<0 and y<0:
return 3
elif x>0 and y<0:
return 4
elif x>0 and y==0:
return 10
elif x<0 and y==0:
return -10
elif x==0 and y>0:
return 20
elif x==0 and y<0:
return -20
else:
return 0
def 计算J5坐标(self,x0,y0,thi):
"""
已知物体的坐标，主方向，计算J1、J6、R
x0，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 False
except 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-x0
y=y1-y0
alpha1=math.degrees(cmath.polar(complex(x,y))[1])
#向量P0-->p2的极坐标转角
x=x2-x0
y=y2-y0
alpha2=math.degrees(cmath.polar(complex(x,y))[1])
alpha=alpha2-alpha1
return round(alpha, 2)
except Exception as e:
logging.error("计算计算J5坐标异常：",e)
def 计算J1_5_R(self,x0,y0,thi):
"""
已知物体的坐标，主方向，计算J1、J5、R
x0，y0为物体中心点坐标
thi为物体主方向与机械臂x轴的逆时针夹角°
"""
g0=self.小骨长
(minr,maxr)=self.工作半径范围
try:
pJ5=self.计算J5坐标(x0,y0,thi)
if pJ5!=0:
(x5,y5)=pJ5
self.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+i2
p0=(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.R
else:
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.R
except Exception as e:
logging.error("计算计算J1_5_R异常：",e)
def 计算J2_4(self):
"""
已知
g1=self.骨长1
g2=self.骨长2
g3=self.骨长3
g4=self.骨长4
g5=g4+self.抓手长
r=self.R
求关节，J2、J3、J4
"""
try:
g1=self.骨长1
g2=self.骨长2
g3=self.骨长3
g4=self.骨长4
g5=g4+self.抓手长
r=self.R
lr=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*180
i3=np.arccos((g3**2+g2**2-lr**2)/(2*g3*g2))/np.pi*180
i4=np.arccos((lr**2+g3**2-g2**2)/(2*lr*g3))/np.pi*180
J2=Ji-i2
J3=180-i3
J4=180-i4-Ji
self.J[1]=J2
self.J[2]=J3
self.J[3]=J4
#print("计算结果",lr,Ji,i2,i3,i4,J2,J3,J4)
return 1
except Exception as e:
logging.error("计算J2_4异常：",e)
def 计算各关节点坐标(self):
"""
已知关节旋转角度，求在R平面的关节坐标
"""
g1=self.骨长1
g2=self.骨长2
g3=self.骨长3
g4=self.骨长4
g5=g4+self.抓手长
r=self.R
J2=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]=-r
y[3]=g5
x[4]=-r
y[4]=0
return (x,y)
except Exception as e:
logging.error("计算各点坐标异常：",e)
def ui(self,H,W):
#text format
org = (40, 80)
fontFace = cv2.FONT_HERSHEY_COMPLEX
fontScale = 0.5
fontcolor = (0, 255, 0) # BGR
thickness = 1
lineType = 4
bottomLeftOrigin = 1
while 1:
#背景颜色 高，宽
self.winH=H
self.winW=W
background = np.zeros((H, W, 3), np.uint8) #生成一个空灰度图像
key = cv2.waitKey(1)
if int(key) == ord('q'):
cv2.destroyWindow('mini_arm')
break
if self.winevent["keymod"]==0:
if int(key) == ord('['):
self.J[0]+=2
if self.J[0]>=160:
self.J[0]=160
if int(key) == ord(']'):
self.J[0]-=2
if self.J[0]<=-160:
self.J[0]=-160
if int(key) == ord('w'):
self.R+=2
if self.R>=self.工作半径范围[1]:
self.R=self.工作半径范围[1]
if int(key) == ord('s'):
self.R-=2
if self.R<=self.工作半径范围[0]:
self.R=self.工作半径范围[0]
if int(key)==ord('z'):
self.R=200
self.J[0]=0
else:
if int(key) == ord('['):
self.thi+=2
if self.thi>=180:
self.thi=180
res,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(']'):
self.thi-=2
if self.thi<=-180:
self.thi=-180
res,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"]=1
else:
self.winevent["keymod"]=0
#update(a,axes)
if self.winevent["keymod"]==1:
text = "window selected mod press o to esc"
(px,py)=self.pz2
if 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=0
bones=[]
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=200
a.J[0]=30
print("正解抓手坐标为：",a.计算抓手坐标(a.J[0],a.R))
x=173.20
y=100
print("正解抓手J1为：",a.计算J1(x,y))
a.ui(900,1600)以上是几何实现，那么如果采用更通用的数值逆解，应该如何实现呢，新开辟一个新专栏——机器人学基础——python算法_JAMES费的博客-CSDN博客，打算继续用python这个工具，把机械臂这件事情研究得更透彻一些。
可以继续移步相关文章：N轴机械臂的MDH正向建模，及python算法_JAMES费的博客-CSDN博客,
本文章为转载内容，我们尊重原作者对文章享有的著作权。如有内容错误或侵权问题，欢迎原作者联系我们进行内容更正或删除文章。
