识别颜色物块
一、摄像头调整
首先，需要使用python运行mycobot_ai包下的openVideo.py。若开启的摄像头为电脑摄像头这需要修改cap_num，具体可参考：注意事项。确保摄像头完全包含整个识别区域，且识别区域在视频中是正正方方的，如下图所示。若识别区域在视频中不符合要求，则需要调整摄像头的位置。
二、案例重现
如上视频操作即可实现颜色识别物块并抓取的demo。接下来我们将用文字描述视频中的操作流程：
M5版本：
- 通过文件管理器去到mycobot-ros的工作空间中的mycobot_ai包下。
- 鼠标右键，打开终端。
- 赋予操作机械臂权限，输入
sudo chmod 777 /dev/ttyU并使用Tab键补出机械臂设备名称。 - 若设备名称不为
/dev/ttyUSB0则需要更改vision.launch文件中的port值。 - 输入
roslaunch launch/vision.launch开启vision.launch文件，该文件包含ros的一些核心库以及依赖等。 - 在rviz图形界面中创建一个marker，并为其命名为cube。
- 在命令终端中键入
ctrl+shift+t开启同目录下的另一个命令窗口。 - 输入
python scripts/detect_obj_color.py打开识别颜色程序，即可实现颜色识别并抓取了。
若不清楚如何修改port值以及创建marker可参考：ROS建立方块模型
树莓派版本：
一 在文件终端使用方法：
- 开机点击桌面Home文档，之后点击catkin_ws文档——src文档——mycobot_ros未定。
- 点击mycobot_ai，进入界面鼠标右键，打开终端。
- 输入roslaunch launch/vision.launch开启vision.launch文件，该文件包含ros的一些核心库以及依赖等。
- 在rviz图形界面中创建一个marker，并为其命名为cube。
- 在命令终端中键入ctrl+shift+t开启同目录下的另一个命令窗口。
- 输入python scripts/detect_obj_color.py打开识别颜色程序，即可实现颜色识别并抓取了。
二 桌面终端使用方法：
- 鼠标右键点击ai_windows，打开终端，输入python3 ai_windows.py，按回车开启gui。
- 打开gui界面，先点击“open ros按钮”打开ros。
- 等待ros打开完毕，选择要运行的颜色识别程序，点击运行即可。
注意事项
当摄像头没有正确自动框出识别区域，需要关闭程序，调整摄像头的位置，可将摄像头向左向右移动等操作；光线会影响摄像头不识别，可适当补光。
若命令终端没有出现ok，且无法识别颜色时，需要将摄像头稍微向后或向前移动一下，当命令终端出现ok时程序即可正常运行。
OpenCV图像识别会受环境的影响，若处在较为昏暗的环境下识别效果将大大降低。
三、代码讲解
本案例是基于OpenCV以及ROS通信控制机械臂实现的。首先，对摄像头进行校准，确保摄像头的准确性。通过识别抓取范围内的两个aruco码智能定位识别范围，并确定实际识别范围的中心点与视频像素点位之间的对应关系。使用OpenCV自带的颜色识别函数对物块进行识别并确定物块在视频中的像素点位，根据物块在视频中的像素点与实际识别范围中心的视频像素点计算出物块相对于实际识别范围中心的坐标情况，之后根据实际识别范围中心与机械臂的相对坐标可计算出物块相对于机械臂的相对坐标。最后通过设计一系列动作抓取物块并放置到相应的桶中。
看完之后是否还是不太明白，不用担心，接下来将分步讲解整个实现流程。
1、识别aruco模块
使用OpenCV自带的aruco识别函数识别图片的aruco，进行一些简要的信息筛选，获得两个aruco的像素点位信息。
def get_calculate_params(self,img):
# 将图片转换为灰色度图片
gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
# 检测图片中是否有aruco
corners, ids, rejectImaPoint = cv2.aruco.detectMarkers(
gray, self.aruco_dict, parameters=self.aruco_params
)
"""
要求图片中存在两个aruco，并且顺序一致。
corners中存在两个aruco，每个aruco包含着其四个角的像素点位。
根据aruco的四个角确定aruco的中心位置。
"""
if len(corners) > 0:
if ids is not None:
if len(corners) <= 1 or ids[0]==1:
return None
x1=x2=y1=y2 = 0
point_11,point_21,point_31,point_41 = corners[0][0]
x1, y1 = int((point_11[0] + point_21[0] + point_31[0] + point_41[0]) / 4.0), int((point_11[1] + point_21[1] + point_31[1] + point_41[1]) / 4.0)
point_1,point_2,point_3,point_4 = corners[1][0]
x2, y2 = int((point_1[0] + point_2[0] + point_3[0] + point_4[0]) / 4.0), int((point_1[1] + point_2[1] + point_3[1] + point_4[1]) / 4.0)
return x1,x2,y1,y2
return None
2、裁剪视频模块
根据两个aruco的像素点位，确定识别范围在视频中的像素范围然后进行裁剪。
"""
将视频像素扩大1.5倍，也就是将视频大小放大1.5倍。
若已经计算出两个aruco值，则进行视频裁剪。
"""
def transform_frame(self, frame):
# 将图片放大1.5倍
fx = 1.5
fy = 1.5
frame = cv2.resize(frame, (0, 0), fx=fx, fy=fy, interpolation=cv2.INTER_CUBIC)
if self.x1 != self.x2:
# 这里的裁剪比例是根据实际情况调整而来的
frame = frame[int(self.y2*0.4):int(self.y1*1.15), int(self.x1*0.7):int(self.x2*1.15)]
return frame
3、颜色识别模块
对接收到的图片进行色度转换处理，将图片转换为灰色度图片，根据自定义类初始化的HSV设置颜色识别范围。将转换后的灰色度图片进行腐蚀、膨胀等操作，加深图片颜色对比程度。通过过滤以及检查轮廓等操作，识别物块颜色并对其进行定位。最后，通过一些必要的数据筛选，在图片中框出颜色物块。
def color_detect(self, img):
x = y = 0
for mycolor, item in self.HSV.items():
redLower = np.array(item[0])
redUpper = np.array(item[1])
# 将图片转换为灰色度图片
hsv = cv2.cvtColor(img, cv2.COLOR_BGR2HSV)
# 设置颜色识别范围
mask = cv2.inRange(hsv, item[0], item[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(img, img, mask=dilation)
# 将滤波后的图像变成二值图像放在binary中
ret, binary = cv2.threshold(dilation, 127, 255, cv2.THRESH_BINARY)
# 获取图像轮廓坐标，其中contours为坐标值，此处只检测外形轮廓
contours, hierarchy = cv2.findContours(
dilation, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
if len(contours) > 0:
# 对误识别的情况进行一些处理
boxes = [
box
for box in [cv2.boundingRect(c) for c in contours]
if min(img.shape[0], img.shape[1]) / 10
< min(box[2], box[3])
< min(img.shape[0], img.shape[1]) / 1
]
if boxes:
for box in boxes:
x, y, w, h = box
# 找到符合要求最大的物体
c = max(contours, key=cv2.contourArea)
# 获得定位物体的左下点和右上点
x, y, w, h = cv2.boundingRect(c)
# 在图片中框出物块
cv2.rectangle(img, (x, y), (x+w, y+h), (153, 153, 0), 2)
# 计算物块中心
x, y = (x*2+w)/2, (y*2+h)/2
# 判断物体是什么颜色
if mycolor == "yellow":
self.color = 1
elif mycolor == "red":
self.color = 0
# 判断是否正常识别
if abs(x) + abs(y) > 0:
return x, y
else:
return None
4、抓取实现模块
为机械臂的运动设计一系列的点，如机械臂的初始化点、待抓取点、蓝桶上方点、绿桶上方点等。为了能在rviz中模拟物块的移动，还为物块的运动设置了一系列点。由于rviz里面的模型坐标是以m为单位的，而机械臂坐标是以mm为单位的，所以在传递数据时需要除以1000。
def move(self, x,y,color):
angles = [
[-7.11, -6.94, -55.01, -24.16, 0, -38.84], # 初始化点
[-1.14, -10.63, -87.8, 9.05, -3.07, -37.7], # 待抓取点
[17.4, -10.1, -87.27, 5.8, -2.02, -37.7], # 待抓取点
]
coords = [
[106.1, -141.6, 240.9, -173.34, -8.15, -83.11], # 蓝桶上方点
[208.2, -127.8, 246.9, -157.51, -17.5, -71.18], # 绿桶上方点
[209.7, -18.6, 230.4, -168.48, -9.86, -39.38], # cube待抓取点
[196.9, -64.7, 232.6, -166.66, -9.44, -52.47], # cube待抓取点
[126.6, -118.1, 305.0, -157.57, -13.72, -75.3], # cube待抓取点
]
# 发送角度移动机械臂
self.pub_angles(angles[0], 20)
time.sleep(1.5)
self.pub_angles(angles[1], 20)
time.sleep(1.5)
self.pub_angles(angles[2], 20)
time.sleep(1.5)
# 发送坐标移动机械臂
self.pub_coords([x, y, 165, -178.9, -1.57, -25.95], 20, 1)
time.sleep(1.5)
self.pub_coords([x, y, 110, -178.9, -1.57, -25.95], 20, 1)
time.sleep(1.5)
# 开吸泵
self.pub_pump(True)
time.sleep(0.5)
self.pub_angles(angles[2], 20)
time.sleep(3)
self.pub_marker(coords[2][0]/1000.0, coords[2][1]/1000.0, coords[2][2]/1000.0)
self.pub_angles(angles[1], 20)
time.sleep(1.5)
self.pub_marker(coords[3][0]/1000.0, coords[3][1]/1000.0, coords[3][2]/1000.0)
self.pub_angles(angles[0], 20)
time.sleep(1.5)
self.pub_marker(coords[4][0]/1000.0, coords[4][1]/1000.0, coords[4][2]/1000.0)
self.pub_coords(coords[color], 20, 1)
self.pub_marker(coords[color][0]/1000.0, coords[color][1]/1000.0, coords[color][2]/1000.0)
time.sleep(2)
# 关吸泵
self.pub_pump(False)
if color==1:
self.pub_marker(coords[color][0]/1000.0+0.04, coords[color][1]/1000.0-0.02)
elif color==0:
self.pub_marker(coords[color][0]/1000.0+0.03, coords[color][1]/1000.0)
self.pub_angles(angles[0], 20)
time.sleep(3)
5、位置计算
通过测量抓取区域中的两个aruco的像素点位可计算出两个aruco的之间的像素距离M1，测量两个aruco的实际距离M2，这样我们就能获得像素与实际距离的比值ratio = M2/M1。我们可以从图片中计算出颜色物块距离抓取区域中心的像素差，这样我们可以计算出物块实际距离抓取区域中心的相对坐标(x1, y1)。将相对坐标(x1, y1)加上抓取区域中心到机械臂的相对坐标(x2,y2)即可求得物块对于机械臂的相对坐标(x3, y3)。具体的代码实现可查看程序源码。
如果想要对整个程序的实现进行透彻的了解，可以直接查看程序源码，源码中提供了详细的注解参考。