实现功能
优势和不足
在目标检测中使用的是基于深度学习的yolov4网络训练出来的模型，可以根据要识别的物体训练相应的模型适用范围更广。不足之处是该机械臂 并没有实现对物体角度的检测和调整，以及 6自由度的控制仍然需要进一步完善。同时在夹取过程中没有对夹取力的反馈是个开环控制仍需要改进。
基本结构与硬件选择
机械臂的结构
机械臂的设计选取参考来了淘宝上卖的成品机械臂，购买了舵机支架和串口舵机进行了机械臂的组装。最终成品采用了6个串口舵机组装出了机械臂，淘宝商家都说这是6自由度的机械臂，其实完全不能达到6自由度机械臂的效果但是对于夹取来说完全足够。
主控的选取
由于我使用的目标检测是使用的yolov4的神经网络模型进行检测物体因此对主控的算力要求较高，开始选择了树莓派作为主控，结果发现在进行识别时识别速度太慢，延迟过高。因此选取了算力更加强大的nvidia nx作为主控。在摄像头上选取了普通的720p无畸变的摄像头。
具体实现过程
夹取部分实现
控制基本运动
控制串口舵机来控制机械臂的行动，选取串口舵机的原因是串口舵机占用主控资源较少同时该机械臂为实验机器并不需要夹取太重物体，综合考虑串口舵机较为适合。结合数字舵机的控制设置了串口舵机的串口参数并实现了对单个舵机的控制，为了实时检测舵机的状态，同时给出了可以读取舵机位置的接口。具体程序如下
import serial
import time
class MOVE:
def __init__(self,name,rate):
self.command = {"MOVE_WRITE":1, "POS_READ":28, "LOAD_UNLOAD_WRITE": 31}
self.ser = serial.Serial(name,rate)
def servoWriteCmd(self,id, cmd, par1 = None, par2 = None):
buf = bytearray(b'\x55\x55')
try:
len = 3
buf1 = bytearray(b'')
if par1 is not None:
len += 2
buf1.extend([(0xff & par1), (0xff & (par1 >> 8))])
if par2 is not None:
len += 2
buf1.extend([(0xff & par2), (0xff & (par2 >> 8))])
buf.extend([(0xff & id), (0xff & len), (0xff & cmd)])
buf.extend(buf1)
sum = 0x00
for b in buf:
sum += b
sum = sum - 0x55 - 0x55
sum = ~sum
buf.append(0xff & sum)
self.ser.write(buf)
except Exception as e:
print(e)
def readPosition(self,id):
self.ser.flushInput()
self.servoWriteCmd(id, self.command["POS_READ"])
time.sleep(0.00034)
time.sleep(0.005)
count = self.ser.inWaiting()
pos = None
if count != 0:
recv_data = self.ser.read(count)
if count == 8:
if recv_data[0] == 0x55 :
if recv_data[1] == 0x55 :
if recv_data[4] == 0x1c:
pos= 0xffff &(recv_data[5]|(0xff00 & recv_data[6] << 8))
return pos
if __name__ =='__main__':
move = MOVE(name="/dev/ttyUSB0",rate=115200)
while(True):
move.servoWriteCmd(2,1,500,0)
pos=move.readPosition(1)
time.sleep(1)
print(pos)在实现对单个舵机的控制之后发现控制舵机转动不能精确到固定角度，因此写了角度转换的的接口。
def runchange_angel (move,ang,times):
for i in range(6):
ang[i] = ang[i]*3.7
pos[i] = start[i]+int(ang[i])
move.servoWriteCmd(5, 1, pos[4], times)
move.servoWriteCmd(3, 1, pos[2], times)
move.servoWriteCmd(4, 1, pos[3], times)
move.servoWriteCmd(2, 1, pos[1], times)
move.servoWriteCmd(1, 1, pos[0], times)
move.servoWriteCmd(6, 1, pos[5], times)在实现对舵机的简单控制之后考虑实现机械臂的基本运动，由于在检测目标时仅仅识别到 二维平面的信息因此只需考虑机械臂在平面上的运动即可，采用极坐标系对夹爪的点进行定位，首先水平转动平台上的舵机来控制角度。该控制直接调用demo即可。对于长度方向上的控制，进行了简单的运动学分析，得出在机械臂伸长时1和2舵机应向相反方向转动相同角度的关系。具体控制代码如下
def extend(move,pos,number,times):
pos[1]+=number
pos[2]+=number
if pos[1]>=1000:
print("伸长超出范围")
pos[1]=1000
if pos[2]>=1000:
print("伸长超出范围")
pos[2]=1000
if pos[1]<=0:
print("伸长超出范围")
pos[1]=0
if pos[2]<=0:
print("伸长超出范围")
pos[2]=0
action.run(move,pos,times)
return pos由于高度固定因此在机械臂的下降过程中直接通过实验直接固定了角度调整的参数。
def decline_catch(move,pos,times):
pos[2]=pos[2]-360
pos[3]=pos[3]-310 #下降
action.run(move,pos,times)
pos[5]=pos[5]+250 #夹取
action.run(move,pos,times)
pos[2]=pos[2]+360
pos[3]=pos[3]+310 #抬升
action.run(move,pos,times)
return pos视觉识别
识别过程共采取两种方案一是通过opencv第三方库进行识别但是对比来看该方法的不足是识别不同物体时需要重新对程序进行大量修改，有时受光线的影响较大。但是该方法对主控的算力要求较低。而采用神经网络的方法适用范围广，对主控算力要求高，综合考虑选取了神经网络的方式进行识别，在识别过程中选取了yolov4模型进行视觉的识别，首先收集到大量的目标物体图片（由于实验夹取的物体特征简单因此只标记了200张图片）进行训练并调用摄像头获得图像进行检测最终将识别到的物体中心坐标以参数的形式返回
if __name__ =="__main__":
move = mv.MOVE(name="/dev/ttyUSB0",rate=115200)
# pos=[500, 511, 484, 800, 390, 276]
# action.run(move,pos,1000)
# decline_catch(move,pos,1000)
# go_home(move,pos,1000)
cap = cv2.VideoCapture(0) #读取摄像头
cap.set(cv2.CAP_PROP_FRAME_WIDTH, 640);
cap.set(cv2.CAP_PROP_FRAME_HEIGHT, 480);
ret, frame = cap.read()
cx=320
cy=240+100
pos,dets=search(move,[200,800],cap)
print(pos,dets)
while ret:
ret, frame = cap.read()
dets,frame = tracking.detector.detect_image(frame,classes=['fy'])
if len(dets) > 0:
name,score,bbox = dets[0]
x = (bbox[0] + bbox[2]) / 2
y = (bbox[1] + bbox[3]) / 2
pos,pan_error,tai_error=tracking.pid_move(move,x,y,cx,cy,pos)
print(pos)
if pan_error==0 and tai_error==0:
pos=decline_catch(move,pos,1000)
go_home(move,pos,1000)
break
cv2.imshow('frame', frame)
cv2.waitKey(1)目标寻找以及PID控制
寻找物体时采用的方式是使机械臂遍历整个区域从而检测目标物，当检测到目标物时在进入PID调节从而快速确定夹爪应在位置。为了快速将夹爪定位到准确位置使用了PID算法进行了控制优化。
import time
from math import pi, isnan
class PID:
_kp = _ki = _kd = _integrator = _imax = 0
_last_error = _last_derivative = _last_t = 0
_RC = 1/(2 * pi * 20)
def __init__(self, p=0, i=0, d=0, imax=0):
self._kp = float(p)
self._ki = float(i)
self._kd = float(d)
self._imax = abs(imax)
self._last_derivative = float('nan')
def get_pid(self, error, scaler):
tnow = time.clock()
dt = tnow - self._last_t
output = 0
if self._last_t == 0 or dt > 1000:
dt = 0
self.reset_I()
self._last_t = tnow
delta_time = float(dt) / float(1000)
output += error * self._kp
if abs(self._kd) > 0 and dt > 0:
if isnan(self._last_derivative):
derivative = 0
self._last_derivative = 0
else:
derivative = (error - self._last_error) / delta_time
derivative = self._last_derivative + \
((delta_time / (self._RC + delta_time)) * \
(derivative - self._last_derivative))
self._last_error = error
self._last_derivative = derivative
output += self._kd * derivative
output *= scaler
if abs(self._ki) > 0 and dt > 0:
self._integrator += (error * self._ki) * scaler * delta_time
if self._integrator < -self._imax: self._integrator = -self._imax
elif self._integrator > self._imax: self._integrator = self._imax
output += self._integrator
return output
def reset_I(self):
self._integrator = 0
self._last_derivative = float('nan')得出PID优化后的偏差后不断进行反复调整最终实现了夹爪的快速定位
def pid_move(move,x,y,cx,cy,pos):
pan_error = cx-x #左边为负
tai_error = cy-y #上边为负
print(pan_error,tai_error)
if abs(pan_error) < 40:
pan_error = 0
if abs(tai_error) < 40:
tai_error = 0
pan_out=pan_pid.get_pid(pan_error,1)
tai_out=tai_pid.get_pid(tai_error,1)
pos=catch.turn(move,pos,int(pan_out),15)
pos=catch.extend(move,pos,int(tai_out),15)
for number in pos:
if number<=0:
number=0
if number>=1000:
number=1000
return(pos,pan_error,tai_error)该机械臂是仍是一个十分不成熟的产品，存在诸多问题。若文中出现错误，恳请大佬批评指正!
