第五组机械臂项目文档

小组信息

曹邵恒 652022320001

李淳 652022320002

韩茁 502022320004

---

本文档由4个部分组成，在第一部分，我们会描述如何运行我们的代码，后面三个部分会讲不同模块的设计实现思路，分别是目标识别设计与实现，机械臂操作设计与实现和GUI探索与机械臂调度的设计与实现。

代码运行

代码位置

我们的代码位于树莓派根目录下的 automated_testing 文件夹中，我们将所有的代码都写在了.ipynb文件里方便直接通过8888端口访问执行。

代码结构

我们的代码主要分为三个部分，目标检测、机械臂移动与调度，我们还准备了一个简单的可以直接在notebook中看见的界面来辅助使用。

在实际运行的时候，主循环会先根据目标检测的结果获取所有的控件，接着通过调度函数选择一个控件，然后通过机械臂移动的来进行点击。

主循环如下所示

# main函数
def start_button_handler(obj):
    global sentinel
    while sentinel:
        # 等待初始化
        arm_init()
        time.sleep(1)
        # 获取所有的对象以及图片信息
        origin, positions = get_all_widgets_position()
        # 获取所有反解出来的关节旋转角度
        arm_move_params = [[get_arm_move_param(positions[i].clicked), i] for i in range(len(positions))]
        if len(arm_move_params) == 0:
            continue
        # 获取目标
        p = schedule(arm_move_params)
        index = p[1]
        # 在图片上标记将要点击的控件
        origin = highlight_img(origin, positions[index].corners)
        image_box.value = bgr8_to_jpeg(origin)
        # 旋转、点击
        p = p[0]
        p_pre = p[:]
        p_pre[1] = 90
        time.sleep(1)
        # 为了稳定点击，这里先将第2号舵机调整到90度
        arm_move(p_pre)
        arm_move(p)
        arm_move(p_pre)

代码运行

在代码运行前，需要先执行pre_script.sh文件来关闭机械臂本身运行的Yahboom程序，解放机械臂上的摄像头。接着启动ROS节点，这个节点是机械臂根据运动学原理求舵机旋转角度的服务端。

由于我们的代码是基于notebook编写，运行只需要重启kernel，然后全部执行我们的单元格即可，在执行完所有的单元格后，最后一个单元格会出现一个简单的GUI界面，可以在这个界面操作机械臂。

这个界面提供了启动和停止的功能，在启动后，机械臂会不断的读取摄像头接收到的图片，我们的代码会对图片进行识别，将要点击的轮廓标注出来，再写入notebook中。

注：当且仅当识别出轮廓时才会有图片现实

目标检测模块的设计与实现

难点

目标检测的难点主要是摄像头都手机屏幕拍摄出的图片质量较低。我们注意到，在手机屏幕对比度较低，或者屏幕亮度太高，或者周围环境的光线很强时，摄像头很难拍到高质量的图片，导致目标检测模块很容易失效。所以，我们在实验时尽可能降低周围环境的亮度，将手机屏幕的亮度进行调整，同时让背景的控件的颜色差别尽可能大。

摄像头拍摄GUI界面

在使用pre_script脚本关闭机械臂应用后，就可以正常使用opencv读取图片，主要流程如下所示

image = cv2.VideoCapture(0) # 打开摄像头
image_widget = widgets.Image(format='jpeg', width=600, height=500)  #设置摄像头显示组件
display(image_widget)                                               #显示摄像头组件

启动摄像头后，我们可以调用opencv的read接口读取图片，然后将图片信息转换为字节串传输给摄像头现实组件

_, frame = image.read()
frame = cv2.resize(frame,(640, 480))
image_widget.value = bgr8_to_jpeg(frame)

在项目中，我们启动摄像头、读取图片、将图片展示的代码并不是在同一个地方，此处只是举例我们所用的API。

轮廓检测

为了标注出界面上的对象，我们需要使用计算机视觉技术来处理摄像头读到的图片，并且将其上的轮廓找到，这里我们尝试了两种不同的算法，一种是形态学开操作，另一种是边缘检测。

形态学开操作

形态学是基于形状处理图像的一组广泛的图像处理运算。形态学运算将结构元素应用于输入图像，从而创建相同大小的输出图像。在形态学运算中，输出图像中每个像素的值基于输入图像中对应像素与其相邻像素的比较。

形态学有两种计算方式，膨胀操作，使对象更加明显可见并填充对象中的小孔，线条看起来更粗。腐蚀操作去除了孤立像素和细线，从而只留下实质对象。

基于这两种计算方式，产生了形态学开和形态学闭两种处理方式，形态学开会先腐蚀再膨胀，而闭操作会先膨胀再腐蚀。经过实际操作，我们发现在我们的设备和环境下，形态学开操作的效果会更好。原始论文中使用的是形态学闭操作。

具体的代码如下所示

# 形态学开操作检测轮廓
def morphological_processing(img):
    # 转换成灰度图
    gray = cv2.cvtColor(img,cv2.COLOR_BGR2GRAY)
    # 高斯滤镜模糊图像
    gray = cv2.GaussianBlur(gray, (5, 5), 1)
    # 根据手机和光线的亮度要设置阈值
    ret,threshold = cv2.threshold(gray,150,255,cv2.THRESH_BINARY)
    # 设置核函数检测形状
    kernel = np.ones((3, 3), np.uint8)
    # 形态学开操作
    blur = cv2.morphologyEx(threshold, cv2.MORPH_OPEN, kernel, iterations=4)
    # 找到图片中的所有轮廓
    contours,hierarchy = cv2.findContours(blur,cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_NONE)
    return contours

每一行的代码都进行了标注，转成灰度图是为了避免颜色的影响，高斯模糊减少图像噪声以及降低细节层次，接着我们设置阈值，进一步减少图像的噪声。然后我们设置了一个3*3的卷积核，使用形态学开操作进行4次迭代，接着我们再获取图片中的所有轮廓。下图是我们的检测结果

边缘检测

我们还参照论文尝试复现他们的检测算法，原论文中，他们先通过边缘检测提取出所有的边缘，再进行了一次形态学闭操作，在复现的过程中，我们发现，在我们的设备和环境下，形态学开闭操作的结果都不如直接使用边缘检测的结果。因此我们最后舍弃了形态学操作，直接使用边缘检测的结果进行轮廓提取。代码如下所示

# canny边缘检测检测轮廓
def canny(img):
    gray = cv2.cvtColor(img,cv2.COLOR_BGR2GRAY)
    gray = cv2.GaussianBlur(gray, (5, 5), 1)
    # 获取边缘
    imgray = cv2.Canny(gray,600,100,3)
    # 根据手机和光线的亮度要设置阈值
    ret, threshold = cv2.threshold(imgray,150,255,cv2.THRESH_BINARY)
    # kernel = np.ones((3, 3), np.uint8)
    ir = threshold
    # 原论文中使用了canny+形态学，但是由于环境的问题，canny+形态学会导致检测不到目标
    # ir = cv2.morphologyEx(ir, cv2.MORPH_OPEN, kernel, iterations=4)
    # 获取图片中的所有轮廓
    contours, _ = cv2.findContours(ir, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_NONE)
    return contours

方法整体和形态学的方法类似，主要是使用了边缘检测，下图是canny边缘检测的结果

控件提取

通过以上两种方法检测到轮廓后，我们会根据轮廓围出来的面积进行过滤，根据实践，我们排除了面积小于900或者大于7000的轮廓。

接着，我们提取轮廓的x、y坐标与宽度和高度，结合四个变量算出来轮廓的中心点，这样，我们就提取出了界面上的控件。具体代码如下所示

def detect(img):
    # 这里提供两种轮廓检测
#     contours = morphological_processing(img)
    contours = canny(img)
    indexs = []
    for i, c in enumerate(contours):
        # 获取边缘的四角
        x, y, w, h = cv2.boundingRect(c)
        # 获取边缘围成图形的面积
        area = cv2.contourArea(c)
        # 过滤掉过大或者过小的面积，超参数在实践中微调得来
        if area < 900 or area > 7000:
            continue
        point_x = float(x + w / 2)
        point_y = float(y + h / 2)
        # 计算机械臂点击的位置
        (a, b) = (round(((point_x - 320) / 4000)-0.02, 5), round(((480 - point_y) / 3000) * 1.8+0.19+0.01, 5))
        # 存储数据
        o = Object([x,y,w,h], [a,b])
        indexs.append(o)
    return img, indexs

在获取界面上控件的位置后，我们需要注意，这个位置只是他在图片上的位置，而我们的机械臂不是以垂直的方式往下看，而是以45度角倾斜着看。因此，我们需要将中心点进行调整，调整为最后点击时实际的落点。

对于调整的方式，我们参考了Dofbot官方网站的方法，并对超参数进行了一定的调整。x和y分别对应了机械臂落点的水平方向和竖直方向。最后，我们能够获取的所有控件如下图所示。

图像标记

图像标记主要是使用了opencv的库函数，我们根据控件提取中获得的x y w h来绘制包裹轮廓的矩形和中心点

# 在img图像画出矩形，(x, y), (x + w, y + h)是矩形坐标，(0, 255, 0)设置通道颜色，2是设置线条粗度
cv2.rectangle(img, (x, y), (x + w, y + h), (0, 255, 0), 2)
# 绘制矩形中心
cv2.circle(img, (int(point_x), int(point_y)), 5, (0, 0, 255), -1)

透视变换（弃用）

由于机械臂的摄像头并且和手机屏幕垂直，我们考虑过使用透视变换将图片转化为与摄像头垂直，但是经过实验发现透视完的图片没有太大的变化，因此我们没有将其纳入代码之中。

机械臂操作的设计与实现

难点

我们注意到，最好的情况下，应该是机械臂垂直点击手机屏幕，但是由于每个舵机转动的角度有限，在摄像头和手机屏幕垂直的时候，一方面要考虑到摄像头拍摄的范围，让手机距离摄像头一个合适的距离，另一方面要让机械臂能够点击到屏幕，这需要保证手机位于机械臂的转动范围内。

同时满足上面两个要求是有点困难的，主要难点是机械臂的点击范围。因此我们选择让摄像头保持斜视手机的状态。

旋转角度获取

在获取了控件的位置，计算出机械臂最后实际的落点位置之后，我们需要计算出机械臂的旋转角度，让机械臂能够成功移动到目标位置。

此时，我们的问题变成了，已知机械臂的当前舵机的角度和机械臂的末端位置，如何求舵机需要的旋转角度？这个问题是机械臂的逆运动学问题。Dofbot的树莓派中已经提供了相应的代码供我们进行反解。

def server_joint(posxy):
        '''
        发布位置请求,获取关节旋转角度
        :param posxy: 位置点x,y坐标
        :return: 每个关节旋转角度
        '''
        # 等待server端启动
        client.wait_for_service()
        # 创建消息包
        request = kinemaricsRequest()
        request.tar_x = posxy[0]
        request.tar_y = posxy[1]
        request.kin_name = "ik"
        try:
            response = client.call(request)
            if isinstance(response, kinemaricsResponse):
                # 获得反解响应结果
                joints = [0.0, 0.0, 0.0, 0.0, 0.0]
                joints[0] = response.joint1
                joints[1] = response.joint2
                joints[2] = response.joint3
                joints[3] = response.joint4
                joints[4] = response.joint5
                # 当逆解越界,出现负值时,适当调节.
                if joints[2] < 0:
                    joints[1] += joints[2] * 3 / 5
                    joints[3] += joints[2] * 3 / 5
                    joints[2] = 0
                # print joints
                return joints
        except Exception:
            rospy.loginfo("arg error")

在运行代码前，我们需要先启动服务端，然后创建相应的消息包，发给服务端后服务端会进行反解，最后把角度返回给我们。

这里需要注意的是，计算机器人运动学逆解首先要考虑可解性，即考虑无解、多解等情况。在这里，是有可能计算出负值的角度的，在遇到负值时就需要进行调整。

点击模拟

在获得转动角度后，我们就可以进行转动了。具体将角度传输给机械臂的方式，我们使用了Dofbot提供的API接口来实现。

# move arm basic on list
def arm_move(p, s_time = 500):
    for i in range(len(p)):
        id = i + 1
        if id == 5:
            time.sleep(.1)
            Arm.Arm_serial_servo_write(id, p[i], int(s_time*1.2))
        else:
            Arm.Arm_serial_servo_write(id, p[i], s_time)
        time.sleep(.01)
    time.sleep(s_time/1000)

我们采用了比较简单的实现，即每次都传输所有舵机的角度。

在转动的过程中，我们需要模拟点击。因为机械臂是弯曲的点击放在桌面上的手机。我们需要尽可能的让机械臂的末端以一个垂直的状态落在手机上。

p = server_joint((x,y))
p_pre = p[:]
p_pre[1] = 90
# 为了稳定点击，这里先将第2号舵机调整到90度
arm_move(p_pre)
arm_move(p)
arm_move(p_pre)

经过实验，我们发现，先将2号舵机调整为90度，再进行点击是比较合理的方案，此时机械臂的末点几乎和手机是垂直的状态，这个时候点击的效果是最好的。

GUI探索与机械臂调度的设计与实现

因为这两个模块的耦合度较高，所以我们将其合在一起解释。

在GUI探索上，我们采用了Monkey的策略，即随机在界面上进行点击。这样做是出于两种考虑，一方面，Monkey实现简单，效率高，另一方面，我们也考虑过设计一个基于状态的策略，但是在设备也是黑盒的情况下，我们只能根据目标检测的结果来判断状态的转移，考虑到摄像头拍摄图片的质量，我们认为基于状态的GUI探索可能效果较差，因此我们使用了Monkey的策略。

在机械臂的调度上，我们为了总移动路径较低，我们会优先点击距离我们最近的控件，我们没有计算机械臂末端距离控件的距离，而是计算了每个检测到的控件所对应的旋转角度，对旋转角度进行排序，选择了一个旋转角度最小的控件。

# 调度，对于所有要点击的对象，选择一个旋转角度最小的
def schedule(arm_move_params):
    def sort_by_angle(six_angle):
        s = 0
        for i in range(len(six_angle)):
            s += abs(init_state[i] - six_angle[i])
        return s
    params = sorted(arm_move_params, key = lambda x : sort_by_angle(x[0]))
    return params[0]

# 获取所有的对象以及图片信息
origin, positions = get_all_widgets_position()
# 获取所有反解出来的关节旋转角度
arm_move_params = [[get_arm_move_param(positions[i].clicked), i] for i in range(len(positions))]
if len(arm_move_params) == 0:
    continue
# 获取目标
p = schedule(arm_move_params)

GUI界面

为了更好的展示我们的工具，我们设计了一个极简的GUI界面，包含启动，结束和一个展示当前被点击控件的图片框，在使用的时候可以通过这个界面观测到当前被点击的控件是什么，可以进行手机位置的调整等。