使用一個(gè)桌面型的六軸機(jī)械臂，在機(jī)械臂的末端安裝一個(gè)攝像頭，來(lái)進(jìn)行人臉識(shí)別和跟蹤的一個(gè)功能。該功能分為兩個(gè)模塊，一個(gè)是人臉識(shí)別模塊，另一個(gè)是機(jī)械臂的運(yùn)動(dòng)控制模塊。

在前文有介紹到怎么控制機(jī)械臂的基本運(yùn)動(dòng)和人臉識(shí)別是如何實(shí)現(xiàn)的，在這里就不再?gòu)?fù)述了，本篇的內(nèi)容主要是介紹是如何完成運(yùn)動(dòng)控制模塊的。

使用到的設(shè)備

mechArm 270 -Pi ，適配的攝像頭

設(shè)備的詳情可以了解前文

機(jī)械臂的運(yùn)動(dòng)控制模塊

接下來(lái)介紹運(yùn)動(dòng)控制的模塊。

控制模塊，常見(jiàn)的運(yùn)動(dòng)控制輸入的是笛卡爾空間的絕對(duì)位置，想要獲得絕對(duì)位置需要做相機(jī)和手臂的手眼標(biāo)定算法，這個(gè)涉及的未知參數(shù)就有十幾個(gè)了，我們略過(guò)了這個(gè)步驟，選擇使用相對(duì)位移做運(yùn)動(dòng)控制，這就需要設(shè)計(jì)一套采樣運(yùn)動(dòng)機(jī)制，確保一次控制周期能完整地獲得人臉的偏移并實(shí)施跟蹤。

所以我想要整個(gè)功能能夠快速呈現(xiàn)出來(lái)，就沒(méi)有選擇用手眼標(biāo)定的算法來(lái)處理相機(jī)和手臂的關(guān)系。因?yàn)槭盅蹣?biāo)定的工作量是相當(dāng)龐大的。

下面的代碼是：從人臉識(shí)別算法中得到的信息來(lái)獲取參數(shù)。

code

_, img = cap.read()
# Converted to grey scale
gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
# Detecting faces
faces = face_cascade.detectMultiScale(gray, 1.1, 4)
# Drawing the outline
for (x, y, w, h) in faces:
if w > 200 or w < 80:?
#Limit the recognition width to between 80 and 200 pixels
continue
cv2.rectangle(img, (x, y), (x+w, y+h), (255, 0, 0), 3)
center_x = (x+w-x)//2+x
center_y = (y+h-y)//2+y
size_face = w

將獲取到的 center_x,center_y ,size_face這幾個(gè)變量用來(lái)計(jì)算位置。

下面是處理數(shù)據(jù)來(lái)控制運(yùn)動(dòng)的算法的代碼

run_num = 20 #Control cycle of 20 frames
if save_state == False:
# Save a start point (save_x, save_y)
save_x = center_x
save_y = center_y
save_z = size_face
origin_angles = mc.get_angles()
print("origin point = ", save_x, save_y, origin_angles)
time.sleep(2);
current_coords = mc.get_coords()
save_state = TRUE
else:
if run_count > run_num: # Limit the control period to 20 frames
run_count = 0
# Recording relative offsets
error_x = center_x - save_x
error_y = center_y - save_y
error_z = size_face - save_z

# Pixel differences are converted into actual offsets, which can be scaled and oriented
trace_1 = -error_x * 0.15
trace_z = -error_y * 0.5
trace_x = -error_z * 2.0

# x/z axis offset, note that this is open loop control
current_coords[2] += trace_z
current_coords[0] += trace_x

#Restricting the Cartesian space xz range
if current_coords[0] < 70:
current_coords[0] = 70

if current_coords[0] > 150:
current_coords[0] = 150

if current_coords[2] < 220:
current_coords[2] = 220

if current_coords[2] > 280:
current_coords[2] = 280

# Inverse kinematic solutions
x = current_coords[0]
z = current_coords[2]
# print(x, z)

L1 = 100;
L3 = 96.5194;
x = x - 56.5;
z = z - 114;

cos_af = (L1*L1 + L3*L3 - (x*x + z*z))/(2*L1*L3);
cos_beta = (L1*L1 - L3*L3 + (x*x + z*z))/(2*L1*math.sqrt((x*x + z*z)));
reset = False
# The solution is only applicable to some poses, so there may be no solution
if abs(cos_af) > 1:
reset = True
if reset == True:
current_coords[2] -= trace_z
current_coords[0] -= trace_x
print("err = ",cos_af)
continue

af = math.acos(cos_af);
beta = math.acos(cos_beta);

theta2 = -(beta + math.atan(z/x) - math.pi/2);
theta3 = math.pi/2 - (af - math.atan(10/96));
theta5 = -theta3 - theta2;
cof = 57.295 #Curvature to angle

move_juge = False
# Limits the distance travelled, where trace_1 joint is in ° and trace_x/z is in mm
if abs(trace_1) > 1 and abs(trace_1) < 15:
move_juge = True
if abs(trace_z) > 10 and abs(trace_z) < 50:
move_juge = True
if abs(trace_x) > 25 and abs(trace_x) < 80:
move_juge = True

if (move_juge == True):
print("trace = ", trace_1, trace_z, trace_x)
origin_angles[0] += trace_1
origin_angles[1] = theta2*cof
origin_angles[2] = theta3*cof
origin_angles[4] = theta5*cof
mc.send_angles(origin_angles, 70)
else:
#Due to the open-loop control, if no displacement occurs the current coordinate value needs to be restored
current_coords[2] -= trace_z
current_coords[0] -= trace_x
else:
# 10 frames set aside for updating the camera coordinates at the end of the motion
if run_count < 10:
save_x = center_x
save_y = center_y
save_z = size_face
run_count += 1

在算法模塊中，獲得相對(duì)位移后如何進(jìn)行手臂移動(dòng)，為了確保運(yùn)動(dòng)效果我們并沒(méi)有直接采用mecharm提供的坐標(biāo)運(yùn)動(dòng)接口，而是在python中添加了逆運(yùn)動(dòng)學(xué)部分，針對(duì)應(yīng)用場(chǎng)景計(jì)算了特定姿態(tài)下的機(jī)械臂逆解，將坐標(biāo)運(yùn)動(dòng)轉(zhuǎn)化成了角度運(yùn)動(dòng)，避免了奇異點(diǎn)等影響笛卡爾空間運(yùn)動(dòng)的因素。結(jié)合上人臉識(shí)別部分的代碼，整個(gè)項(xiàng)目就算是完成了。

正常來(lái)說(shuō)人臉識(shí)別會(huì)對(duì)算力有比較高的要求，它的算法機(jī)制是針對(duì)相鄰像素做重復(fù)計(jì)算從而增加識(shí)別精度，我們使用的mechArm 270-Pi它的主控是以樹(shù)莓派4B 為處理器，來(lái)進(jìn)行人臉識(shí)別的算力處理。

樹(shù)莓派的算力是400MHZ。我們用的樹(shù)莓派算力不足所以簡(jiǎn)化了這個(gè)過(guò)程，把識(shí)別機(jī)制改成了只算幾次的模糊識(shí)別，在我們應(yīng)用的時(shí)候就需要背景簡(jiǎn)單一些。

總結(jié)

這個(gè)人臉識(shí)別和機(jī)械臂跟蹤項(xiàng)目到目前就算是做完了。

總結(jié)項(xiàng)目一些關(guān)鍵信息：

1 在對(duì)于低算力的情況下，設(shè)定簡(jiǎn)單的使用場(chǎng)景，實(shí)現(xiàn)流暢的效果

2 將復(fù)雜的手眼標(biāo)定算法換成選擇相對(duì)位置移動(dòng)，使用采樣運(yùn)動(dòng)機(jī)制，確保每一控制周期能完整的獲得人臉的偏移并跟蹤

3 在python中添加了逆運(yùn)動(dòng)學(xué)部分，針對(duì)應(yīng)用場(chǎng)景計(jì)算了特定姿態(tài)下的機(jī)械臂逆解，將坐標(biāo)運(yùn)動(dòng)轉(zhuǎn)化成了角度運(yùn)動(dòng)，避免了奇異點(diǎn)等影響笛卡爾空間運(yùn)動(dòng)。

項(xiàng)目一些不足的地方：

在使用的場(chǎng)景有一定的要求，需要干凈的背景才能運(yùn)行成功。（通過(guò)固定的場(chǎng)景，簡(jiǎn)化了很多參數(shù)）

前面也有提到，樹(shù)莓派的算力是不足的，更換其他控制主板的，運(yùn)行起來(lái)會(huì)更加流暢，例如用jetsonnano （600MHZ），高性能圖像處理的電腦。

另外就是運(yùn)動(dòng)控制模塊，因?yàn)闆](méi)有做手眼標(biāo)定所以只能用相對(duì)位移，控制分為“采樣階段”“移動(dòng)階段”，目前盡量要求采樣的時(shí)候鏡頭是靜止的，但實(shí)際上比較難保證鏡頭靜止，就會(huì)出現(xiàn)在采樣的時(shí)候鏡頭同時(shí)還在運(yùn)動(dòng)，導(dǎo)致讀到的坐標(biāo)會(huì)有偏差。

最后，在這里特別感謝大象機(jī)器人在項(xiàng)目的開(kāi)發(fā)時(shí)提供的幫助，能夠讓項(xiàng)目完成。這次使用的mechArm是一款中心對(duì)稱結(jié)構(gòu)的機(jī)械臂，在關(guān)節(jié)運(yùn)動(dòng)上有所限制，如果將程序運(yùn)用在活動(dòng)范圍更加靈活的mycobot上可能是不一樣情況。

如果你對(duì)項(xiàng)目有啥想要了解更多的地方請(qǐng)?jiān)谙路浇o我留言。

審核編輯黃宇