使用到的设备：
机械臂、总线舵机、树莓派、舵机控制板、USB摄像头、锂电池
使用的设备并不唯一如树莓派可以换成jeston nano、USB摄像头可以换成SCI的、总线舵机可以换成PWM舵机，执行功能一致即可
流程:
摄像头捕捉物体坐标反馈给树莓派，然后树莓派控制机械臂进行夹取
机械臂夹取
我们这里为简易装置并不会设计复杂的空间变换，非常容易上手
我们首先使机械臂与物体处于同一条水平线上，然后在分析物体的x,y坐标，这样可以将三位简化为二维，大大降低了难度。如下图所示：
那么如何让机械臂与物体处于同一个水平线上？我个人非常建议使用PID，PID的精度非常高会让机械臂与物体对着的非常准，PID计算输出是个控制量，与当前误差有关，我们通过这个输出量控制机械臂运动。那么误差由什么决定？对于接触过PID的人而言很容易就想到使用当前图像的中心坐标-物体的中心坐标。我们识别物体有多种方法如：颜色、轮廓、yolov等。我这里使用颜色识别。我这里使用的为总线舵机，用0~1000来控制舵机旋转角度，我夹取的物体大致方位固定因此我在400+PID输出量来控制机械臂在400方向左右来回寻找物体。同时加入测距判断与物体间的距离，x,y方法一致我们以x为例。
假设我们有一个宽度为 W 的目标或者物体。然后我们将这个目标放在距离我们的相机为 D 的位置。我们用相机对物体进行拍照并且测量物体的像素宽度 P 。这样我们就得出了相机焦距的公式：
F = (P x D) / W
我们将目标物体放在固定位置将相机焦距计算出来，然后通过此公式可以将D计算出来。近距离内误差并不大，爪子又比物体大很多因此可以通过矫正让他更加精准。
import numpy as np
import cv2
import time
from duoji import servoWriteCmd
from getpost import getpost
from getpost import main
from getpost import getdata
import os
# w = 640 h = 480
a = 0
b = 0
x_w = 1
last_btm_degree = 430 # 最近一次底部舵机的角度值记录
btm_kp = 3 # 底部舵机的Kp系数
offset_dead_block = 0.1
#cv2.namedWindow('FaceDetect',flags=cv2.WINDOW_NORMAL | cv2.WINDOW_KEEPRATIO | cv2.WINDOW_GUI_EXPANDED)
def update_btm_kp(value):
# 更新底部舵机的比例系数
global btm_kp
btm_kp = valueet_dead_block = 0.1 # 设置偏移量的死区
cv2.createTrackbar('BtmServoKp','FaceDetect',0, 20,update_btm_kp)
# 设置btm_kp的默认值
cv2.setTrackbarPos('BtmServoKp', 'FaceDetect', btm_kp)
class ColorMeter(object):
color_hsv = {
# HSV，H表示色调（度数表示0-180），S表示饱和度（取值0-255），V表示亮度（取值0-255）
# "orange": [np.array([11, 115, 70]), np.array([25, 255, 245])],
#"green": [np.array([35, 115, 70]), np.array([77, 255, 245])],
#"blue": [np.array([100, 115, 70]), np.array([124, 255, 245])],
"red": [np.array([156, 115, 70]), np.array([179, 255, 245])],
}
def __init__(self, is_show=False):
self.is_show = is_show
self.img_shape = None
def detect_color(self, frame):
global a,x_w
self.img_shape = frame.shape
res = {}
# 将图像转化为HSV格式
hsv = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV)
for text, range_ in self.color_hsv.items():
# 去除颜色范围外的其余颜色
mask = cv2.inRange(hsv, range_[0], range_[1])
erosion = cv2.erode(mask, np.ones((1, 1), np.uint8), iterations=2)
dilation = cv2.dilate(erosion, np.ones((1, 1), np.uint8), iterations=2)
target = cv2.bitwise_and(frame, frame, mask=dilation)
# 将滤波后的图像变成二值图像放在binary中
ret, binary = cv2.threshold(dilation, 127, 255, cv2.THRESH_BINARY)
# 在binary中发现轮廓，轮廓按照面积从小到大排列
contours, hierarchy = cv2.findContours(
binary, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE
)
if len(contours) > 0:
# cv2.boundingRect()返回轮廓矩阵的坐标值，四个值为x, y, w, h， 其中x, y为左上角坐标，w,h为矩阵的宽和高
boxes = [
box
for box in [cv2.boundingRect(c) for c in contours]
if min(frame.shape[0], frame.shape[1]) / 10
< min(box[2], box[3])
< min(frame.shape[0], frame.shape[1]) / 1
]
if boxes:
res[text] = boxes
if self.is_show:
for box in boxes:
x, y, w, h = box
# 绘制矩形框对轮廓进行定位
cv2.rectangle(
frame, (x, y), (x + w, y + h), (153, 153, 0), 2
)
a = x+w/2
x_w = w
# 将绘制的图像保存并展示
# cv2.imwrite(save_image, img)
cv2.putText(
frame, # image
text, # text
(x, y), # literal direction
cv2.FONT_HERSHEY_SIMPLEX, # dot font
0.9, # scale
(255, 255, 0), # color
2, # border
)
if self.is_show:
cv2.imshow("image", frame)
cv2.waitKey(1)
# cv2.destroyAllWindows()
return res
def btm_servo_control(offset_x):
'''
底部舵机的比例控制
这里舵机使用开环控制
'''
global offset_dead_block # 偏移量死区大小
global btm_kp # 控制舵机旋转的比例系数
global last_btm_degree # 上一次底部舵机的角度
# 设置最小阈值
if abs(offset_x) < offset_dead_block:
offset_x = 0
# offset范围在-50到50左右
delta_degree = offset_x * btm_kp
# 计算得到新的底部舵机角度
next_btm_degree = last_btm_degree + delta_degree
# 添加边界检测
if next_btm_degree < -180:
next_btm_degree = -180
elif next_btm_degree > 180:
next_btm_degree = 180
return int(next_btm_degree)
class MyPID:
def __init__(self, Kp=1.2, Ki=1., Kd=0.002):
self.Kp = Kp # 比例系数Proportional
self.Ki = Ki # 积分系数Integral
self.Kd = Kd # 微分系数Derivative
self.Ek = 0 # 当前误差 e(k)
self.Ek1 = 0 # 前一次误差 e(k - 1)
self.Ek2 = 0 # 再前一次误差 e(k - 2)
self.LocSum = 0 # 累计积分位置
def PIDLoc(self, SetValue, ActualValue):
''' PID位置(Location)计算 :
param SetValue:设置值(期望值) :
param ActualValue:实际值(反馈值) :
return:PID位置 '''
self.Ek = SetValue - ActualValue
self.LocSum += self.Ek # 累计误差
PIDLoc = self.Kp * self.Ek + (self.Ki * self.LocSum) + self.Kd * (self.Ek1 - self.Ek)
self.Ek2 = self.Ek1
self.Ek1 = self.Ek
return PIDLoc
def PIDInc(self, SetValue, ActualValue):
''' PID增量(Increment)计算
:param SetValue: 设置值(期望值)
:param ActualValue:实际值(反馈值)
:return: 本次PID增量(+/-) '''
self.Ek = SetValue - ActualValue
PIDInc = (self.Kp * self.Ek) - (self.Ki * self.Ek1) + (self.Kd * self.Ek2)
self.Ek2 = self.Ek1
self.Ek1 = self.Ek
return PIDInc
def X(aa):
servoWriteCmd(1,1,400+aa,100)
print(aa)
time.sleep(0.1)
myPID = MyPID(Kp=0.1, Ki=0.008, Kd=0.05)
SetValue = 320
def state_Init():
servoWriteCmd(1,1,408,200)
servoWriteCmd(2,1,470,200)
servoWriteCmd(3,1,634,200)
servoWriteCmd(4,1,339,200)
servoWriteCmd(6,1,249,200)
def state_Init_ment():
servoWriteCmd(1,1,408,200)
servoWriteCmd(2,1,470,200)
servoWriteCmd(3,1,634,200)
servoWriteCmd(4,1,339,200)
def put():
servoWriteCmd(1,1,777,200)
servoWriteCmd(2,1,450,300)
time.sleep(1)
servoWriteCmd(3,1,510,400)
time.sleep(1)
servoWriteCmd(6,1,249,200)
def back():
servoWriteCmd(3,1,634,300)
time.sleep(0.5)
servoWriteCmd(1,1,408,300)
time.sleep(0.5)
if __name__ == "__main__":
w = 3
f = 880
flage = 0
d=0
state_Init()
cap = cv2.VideoCapture(0)
m = ColorMeter(is_show=True)
while True:
#print(a)
ActualValue = a
success, frame = cap.read()
res = m.detect_color(frame)
PIDLoc = myPID.PIDLoc(SetValue, ActualValue)
ActualValue = PIDLoc
d=(w*f)/x_w
print(d)
#if a!=0:
#a=a-320
#angle = btm_servo_control(a)
PIDLoc=int(PIDLoc)
X(PIDLoc)
#last_btm_degree = angle
if ord(' ') == cv2.waitKey(10):
break
if d!=0 and flage>=100:
d=d-4
a,b,c=getpost(d,-3)
getdata(a,b,c)
servoWriteCmd(6,1,420,1000)
time.sleep(2)
state_Init_ment()
break
flage=flage+1
print(f"flage{flage}")
time.sleep(1)
state_Init_ment()
time.sleep(1)
put()
time.sleep(1)
back()
cv2.destoryAlllwindows()
cap.release()
然后我们学习一下机械臂的控制，虽然说是六轴机械臂，但是我们在使用过程中也就通过4轴控制就够了。
我们将舵机从下至上一次编号，在实际使用中1号舵机为PID舵机、6号舵机负责夹取，我们夹取状态为爪子水平抓取物体，那么4号舵机用来控制爪子部分为水平状态，5号舵机是用来控制爪子旋转的因此我们用不到。实际用来让爪子到达某个x,y坐标的舵机只有2号与3号。
我们根据（x，y）坐标使用余弦定理便可以计算出∠B的大小我们画一个辅助线
∠A拆分为两个角，一个在三角形内部使用余弦定理cos A= (b²+c²-a²)/2bc便可以计算，两个黑边为机械臂，长度我们已知，三角形外面那个角直接使用坐标x,y边可以计算出。在在直角三角形中余弦值为临边比斜边。之后经过反三角函数变成关节之间需要的弧度，我舵机安装的时候是不规范的，我将她经过矫正之后将度数经过转换变成0-1000之间的命令。注意的是总线舵机宏观上是同时运行，因此我们需要加入延时让舵机一个一个按顺序运行。
import math
import numpy as np
from sympy import *
from duoji import servoWriteCmd
import time
def getpost(a,b):
l1 = 12.5#机械臂两个边
l2 = 12.0
l3 = 12.0#我们计算的位置是让第三舵机到达x,y与爪子在x轴方向的误差
r = math.sqrt(a*a+b*b)
x1 = a - l3
#y/x
print((r*r-l1*l1-l2*l2)/(2*l1*l2))
q2 = math.acos((r*r-l1*l1-l2*l2)/(2*l1*l2))#弧度一
dd=(r*r - l1*l1-l2*l2)/(2*l1*l2)
A2 = q2 * 180.0 / math.pi#弧度转角度
q1=math.atan2(b,a)+math.atan2(l2*math.sin(q2),l1+l2*math.cos(q2))
A1 = q1 * 180.0 / math.pi
A11 = A1
A2 = 180 - A2
A22 = A2
print(A1,A2)
#1c = 4.09
if A1>90:
A1 = A1-90
A1 = 594-A1*4.09
A1=int(A1)
elif A1==90:
A1 = 594
elif A1<90:
A1 = 983-A1*4.09
A1 = int(A1)
if A2 > 90:
A2 = A2-90
A2 = 568+A2*4.04
A2 = int(A2)
elif A2 == 90:
A2 = 568
elif A2<90:
A2 = 568-(90-A2)*4.04
A2 = int(A2)
if A2<307:
A2 = 307
A3 = 360-A11-A22
print(A3)
if A3>180:#爪子连接那个机械臂 让他保持平行夹取状态
A3 = A3-180
A3 = A3*4.13+664
A3 = int(A3)
elif A3==180:
A3 = 664
elif A3<180:
A3 = 180-A3
A3 = 665-A3*4.13
A3 = int(A3)
if A3 < 176:
A3 = 176
return (A1,A2,A3)
def main(a,b,c):
servoWriteCmd(4,1,350,400)
time.sleep(1)
servoWriteCmd(3,1,b,400)
time.sleep(1)
servoWriteCmd(2,1,a,1000)
time.sleep(1)
def getdata(a,b,c):
servoWriteCmd(3,1,b,400)
time.sleep(1)
servoWriteCmd(4,1,c,400)
time.sleep(1)
servoWriteCmd(2,1,a,1000)
time.sleep(1)
if __name__ == '__main__':
a,b,c=getpost(21,-5)
getdata(a,b,c)
time.sleep(1)
# a,b,c=getpost(5,15)
# getdata(a,b,c)
print(a,b,c)
扫一扫
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
