机械爪角度与距离之间的关系

 

■ 背景

---

在博文舵机控制的机械爪 测试了舵机机械爪的运动。由于多级的角度与给定的指令之间存在着伺服（角度负反馈闭环）控制关系，因此可以近似为一个线性控制关系。

舵机的角度与两个机械爪之间的距离可以通过几何分析得到理论之间的数值。由于该款机械爪本身使用塑料件注塑而成，配合相对粗糙，所以两个机械爪之间的距离与控制命令之间存在一定的误差噪声。

^{▲ 机械爪在命令控制下张开与闭合}

下面通过图像处理的方式来获得实验拍摄到两个机械爪之间的距离，并给出指令与机械爪距离之间的数值关系。从而为后面该机械爪的使用和控制提供理论模型支撑。

 

01图像处理

---

1.基本方案

在两个机械爪上安放两块绿色塑料片，一方面提供图片测量两个机械爪之间距离的基准，另一方面也便于图像处理。在机械爪的下面铺设红色背景布，提高图像处理的对比度。

使用一个顶视的 HikVision网络摄像头 拍摄机械爪运动图片。由于该款监视摄像头视野具有一定的畸变，通过它拍摄的图片需要进行几何校正。在这里简便起见，就省去了空间校正的环节。

#!/usr/local/bin/python
# -*- coding: gbk -*-
#============================================================
# TEST1.PY                     -- by Dr. ZhuoQing 2020-08-13
#
# Note:
#============================================================
from headm import *
from tsmodule.tsstm32       import *
setnum = linspace(80, 480, 200, endpoint=False)
printf(setnum)
gifid = 2
tspgiffirst(gifid)
for sn in setnum:
    stm32cmd('pwm %d'%sn)
    time.sleep(1)
    tspgifappend(gifid)
    printf(sn)
#------------------------------------------------------------
#        END OF FILE : TEST1.PY
#============================================================

2.测量距离

(1) 获得边缘图

利用 Python Canny 函数获得图片中的边缘。通过调整阈值的大小，使得定位绿色塑料片的边缘呈现完整清晰。

^{▲ 使用Canny算法获得图片的边缘}

(2) 所有采集到图形的边缘图

for f in range(filenumber):
    fn = os.path.join(filepath, '%04d.BMP'%f)
    img = cv2.imread(fn)[:,:,1]
    edges = cv2.Canny(img, sv1, sv2)

(3) 获得机械爪间距

#------------------------------------------------------------
def edgewidth(edge):
    edges = edge[320:350,:]
    edgesadd = sum(edges, 0)

    edgesadd = [int(s) for s in list(edgesadd > 0)]

    firstid = edgesadd.index(1) + 50
    endid = len(edgesadd) - edgesadd[::-1].index(1) - 50

    inneredge = edgesadd[firstid:endid]
    leftid = inneredge.index(1)
    rightid = len(inneredge) - inneredge[::-1].index(1)

    width = rightid - leftid
    return width

^{▲ 机械爪之间的距离的变化}

 

02理论分析

---

以舵机输出轴作为原点，建立直角坐标系。假设舵机输出杆的长度为$L$，那么多级的输出角度$\Theta$与两个机械爪之间的位移$W$之间的关系为：

下图是角度与机械爪间距之间的关系。

对照前面实际测量的角度与机械爪距离之间的关系，可以看到他们之间的相似性。

 

03结果分析

---

机械爪之间的间距与角度之间通过理论分析和实际测量，他们在趋势上是相符的。但是实际测量的结果存在着误差：

• 来自于机械装配之间的随机抖动；
• 来自于舵机角度的起始位置与终止位置的误差；

 

※ 代码

---

#!/usr/local/bin/python
# -*- coding: gbk -*-
#============================================================
# MEASURE1.PY                  -- by Dr. ZhuoQing 2020-08-13
#
# Note:
#============================================================

from headm import *
from PIL                    import Image
import cv2
from tsmodule.tsdraw        import *

gifid = 2

filename = tspgetdopfile(gifid)
filenumber = tspgetgifpage(gifid)
filepath = os.path.dirname(filename)

#------------------------------------------------------------
def edgewidth(edge):
    edges = edge[320:350,:]
    edgesadd = sum(edges, 0)

    edgesadd = [int(s) for s in list(edgesadd > 0)]

    firstid = edgesadd.index(1) + 50
    endid = len(edgesadd) - edgesadd[::-1].index(1) - 50

    inneredge = edgesadd[firstid:endid]
    leftid = inneredge.index(1)
    rightid = len(inneredge) - inneredge[::-1].index(1)

    width = rightid - leftid
    return width

#------------------------------------------------------------
'''

img = cv2.imread(filename)[:,:,::-1]

edges = cv2.Canny(img, 300, 400)[320:350,:]

edgesadd = sum(edges, 0)

edgesadd = [int(s) for s in list(edgesadd > 0)]

firstid = edgesadd.index(1) + 50
endid = len(edgesadd) - edgesadd[::-1].index(1) - 50

inneredge = edgesadd[firstid:endid]
leftid = inneredge.index(1)
rightid = len(inneredge) - inneredge[::-1].index(1)

width = rightid - leftid

printff(leftid, rightid, width)

#printf(edgesadd)

#printf(shape(edges))

plt.subplot(211), plt.imshow(img)
plt.subplot(212), plt.imshow(edges, cmap='gray')
plt.xticks([]), plt.yticks([])
plt.show()

exit()

'''
#------------------------------------------------------------
pltgif = PlotGIF()

plt.draw()
plt.pause(.001)

sv1 = 300
sv2 = 400

widthdim = []

for f in range(filenumber):
    fn = os.path.join(filepath, '%04d.BMP'%f)
    img = cv2.imread(fn)[:,:,1]
    edges = cv2.Canny(img, sv1, sv2)
    widthdim.append(edgewidth(edges))

    printff(fn, widthdim[-1])

#    plt.clf()
#    plt.imshow(edges, cmap='gray')
#    plt.title('Canny(img, %d, %d), Pages:%04d'%(sv1,sv2,f))

#    plt.xticks([]), plt.yticks([])
#    plt.draw()
#    plt.pause(.001)
#    pltgif.append(plt)
#    printf(fn)

#pltgif.save(r'd:\temp\1.gif')

printf('\a')
#plt.show()

plt.plot(list(range(filenumber)), widthdim)
plt.xlabel("Sample")
plt.ylabel("Width")
plt.grid(True)
plt.tight_layout()
plt.show()

#------------------------------------------------------------
#        END OF FILE : MEASURE1.PY
#============================================================