制作一个简易的自主夹取机械臂

使用到的设备：
机械臂、总线舵机、树莓派、舵机控制板、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_kpbtm_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_showself.img_shape = Nonedef detect_color(self, frame):global a,x_wself.img_shape = frame.shaperes = {}# 将图像转化为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 = [boxfor 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] = boxesif 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/2x_w = w# 将绘制的图像保存并展示# cv2.imwrite(save_image, img)cv2.putText(frame,  # imagetext,  # text(x, y),  # literal directioncv2.FONT_HERSHEY_SIMPLEX,  # dot font0.9,  # scale(255, 255, 0),  # color2,  # 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 = -180elif next_btm_degree > 180:next_btm_degree = 180return int(next_btm_degree)
class MyPID:def __init__(self, Kp=1.2, Ki=1., Kd=0.002):self.Kp = Kp # 比例系数Proportionalself.Ki = Ki # 积分系数Integralself.Kd = Kd # 微分系数Derivativeself.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 - ActualValueself.LocSum += self.Ek # 累计误差PIDLoc = self.Kp * self.Ek + (self.Ki * self.LocSum) + self.Kd * (self.Ek1 - self.Ek)self.Ek2 = self.Ek1self.Ek1 = self.Ekreturn PIDLocdef PIDInc(self, SetValue, ActualValue):''' PID增量(Increment)计算:param SetValue: 设置值(期望值):param ActualValue:实际值(反馈值):return: 本次PID增量(+/-) '''self.Ek = SetValue - ActualValuePIDInc = (self.Kp * self.Ek) - (self.Ki * self.Ek1) + (self.Kd * self.Ek2)self.Ek2 = self.Ek1self.Ek1 = self.Ekreturn PIDIncdef 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 = 3f = 880flage = 0d=0state_Init()cap = cv2.VideoCapture(0)m = ColorMeter(is_show=True)while True:#print(a)ActualValue = asuccess, frame = cap.read()res = m.detect_color(frame)PIDLoc = myPID.PIDLoc(SetValue, ActualValue)ActualValue = PIDLocd=(w*f)/x_wprint(d)#if a!=0:#a=a-320#angle = btm_servo_control(a)PIDLoc=int(PIDLoc)X(PIDLoc)#last_btm_degree = angleif ord(' ') == cv2.waitKey(10):breakif d!=0 and flage>=100:d=d-4a,b,c=getpost(d,-3)getdata(a,b,c)servoWriteCmd(6,1,420,1000)time.sleep(2)state_Init_ment()breakflage=flage+1print(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 timedef getpost(a,b):l1 = 12.5#机械臂两个边l2 = 12.0l3 = 12.0#我们计算的位置是让第三舵机到达x,y与爪子在x轴方向的误差r = math.sqrt(a*a+b*b)x1 = a - l3#y/xprint((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.piA11 = A1A2 = 180 - A2A22 = A2print(A1,A2)#1c = 4.09if A1>90:A1 = A1-90A1 = 594-A1*4.09A1=int(A1)elif A1==90:A1 = 594elif A1<90:A1 = 983-A1*4.09A1 = int(A1)if A2 > 90:A2 = A2-90A2 = 568+A2*4.04A2 = int(A2)elif A2 == 90:A2 = 568elif A2<90:A2 = 568-(90-A2)*4.04A2 = int(A2)if A2<307:A2 = 307A3 = 360-A11-A22print(A3)if A3>180:#爪子连接那个机械臂 让他保持平行夹取状态A3 = A3-180A3 = A3*4.13+664A3 = int(A3)elif A3==180:A3 = 664elif A3<180:A3 = 180-A3A3 = 665-A3*4.13A3 = int(A3)if A3 < 176:A3 = 176return (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)

制作一个简易的自主夹取机械臂相关推荐

使用Java制作一个简易的远控终端
使用Java制作一个简易的远控终端远控终端的本质 1.服务端(攻击者)传输消息 ----> socket连接 ----> 客户端(被攻击者)接收消息 2.客户端执行消息内容(即执行服务端 ...
利用CSS浮动制作一个简易导航栏
初学CSS,利用CSS浮动和无序列表制作一个简易导航栏: <!DOCTYPE html> <html lang="en"> <head>< ...
使用 history 对象和 location 对象中的属性和方法制作一个简易的网页浏览工具
查看本章节查看作业目录需求说明: 使用 history 对象和 location 对象中的属性和方法制作一个简易的网页浏览工具实现思路: 使用history对象中的 forward() 方法和 ...
用Python制作一个简易的计时器
前言今天又带来个小玩意 - 用Python制作一个简易的计时器这个其实也能自定义一些东西的就比如名字颜色啥的自己看着改就行有想法的朋友也能自己再写写改改出其他的小功能效果展示实现代码 ...
制作一个简易的计算器
这里写自定义目录标题欢迎使用Markdown编辑器新的改变功能快捷键合理的创建标题,有助于目录的生成如何改变文本的样式插入链接与图片如何插入一段漂亮的代码片生成一个适合你的列表创建一 ...
四节1.5V的5号电池、一个电容、一个12V的报警蜂鸣器、铜线和螺母，在螺母所栓的铜线触发接通电源后，缓慢放电10秒，制作一个简易震动报警器，需要用什么样的电容合适？...
根据题目描述,需要制作一个简易震动报警器,使用四节1.5V的5号电池作为电源,一个电容,一个12V的报警蜂鸣器,铜线和螺母.在螺母所栓的铜线触发接通电源后,需要缓慢放电10秒. 在这种情况下,需要一个 ...
使用python制作一个简易的远控终端
使用python制作一个简易的远控终端远控终端的本质 1.服务端(攻击者)传输消息 ----> socket连接 ----> 客户端(被攻击者)接收消息 2.客户端执行消息内容(即执行服 ...
用HTML5制作一个简易计算器
用H5制作一个简易计算器最近刚学JavaScript,之后紧接着做了一个简易的计算器,能够实现数字的加减乘除运算. 首先,先用HTML5搭建好计算器大体框架.我这里用了两个表格,一个充当显示器,另一 ...
电赛练习1《基础版》— 利用Multisim设计并制作一个简易的方波-三角波-正弦波信号发生器，要求输出频率可调，矩形波占空比可调等
首先呢, 感慨一下,我记得去年这个时候,也是放寒假,我在CSDN上写了第一篇原创,使用python画了一个蜡笔小新的头像,并且我在文末说明了自己会陆续更新很多文章,结果- 到了今天放寒假,我的文章还是 ...

制作一个简易的自主夹取机械臂

制作一个简易的自主夹取机械臂相关推荐

最新文章

热门文章