如何完成机械臂的运动控制模块

使用一个桌面型的六轴机械臂，在机械臂的末端安装一个摄像头，来进行人脸识别和跟踪的一个功能。该功能分为两个模块，一个是人脸识别模块，另一个是机械臂的运动控制模块。

在前文有介绍到怎么控制机械臂的基本运动和人脸识别是如何实现的，在这里就不再复述了，本篇的内容主要是介绍是如何完成运动控制模块的。

使用到的设备

mechArm 270 -Pi ，适配的摄像头

设备的详情可以了解前文

机械臂的运动控制模块

接下来介绍运动控制的模块。

控制模块，常见的运动控制输入的是笛卡尔空间的绝对位置，想要获得绝对位置需要做相机和手臂的手眼标定算法，这个涉及的未知参数就有十几个了，我们略过了这个步骤，选择使用相对位移做运动控制，这就需要设计一套采样运动机制，确保一次控制周期能完整地获得人脸的偏移并实施跟踪。

所以我想要整个功能能够快速呈现出来，就没有选择用手眼标定的算法来处理相机和手臂的关系。因为手眼标定的工作量是相当庞大的。

下面的代码是：从人脸识别算法中得到的信息来获取参数。

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这几个变量用来计算位置。

下面是处理数据来控制运动的算法的代码

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

在算法模块中，获得相对位移后如何进行手臂移动，为了确保运动效果我们并没有直接采用mecharm提供的坐标运动接口，而是在python中添加了逆运动学部分，针对应用场景计算了特定姿态下的机械臂逆解，将坐标运动转化成了角度运动，避免了奇异点等影响笛卡尔空间运动的因素。结合上人脸识别部分的代码，整个项目就算是完成了。

正常来说人脸识别会对算力有比较高的要求，它的算法机制是针对相邻像素做重复计算从而增加识别精度，我们使用的mechArm 270-Pi它的主控是以树莓派4B 为处理器，来进行人脸识别的算力处理。

树莓派的算力是400MHZ。我们用的树莓派算力不足所以简化了这个过程，把识别机制改成了只算几次的模糊识别，在我们应用的时候就需要背景简单一些。

总结

这个人脸识别和机械臂跟踪项目到目前就算是做完了。

总结项目一些关键信息：

1 在对于低算力的情况下，设定简单的使用场景，实现流畅的效果

2 将复杂的手眼标定算法换成选择相对位置移动，使用采样运动机制，确保每一控制周期能完整的获得人脸的偏移并跟踪

3 在python中添加了逆运动学部分，针对应用场景计算了特定姿态下的机械臂逆解，将坐标运动转化成了角度运动，避免了奇异点等影响笛卡尔空间运动。

项目一些不足的地方：

在使用的场景有一定的要求，需要干净的背景才能运行成功。（通过固定的场景，简化了很多参数）

前面也有提到，树莓派的算力是不足的，更换其他控制主板的，运行起来会更加流畅，例如用jetsonnano （600MHZ），高性能图像处理的电脑。

另外就是运动控制模块，因为没有做手眼标定所以只能用相对位移，控制分为“采样阶段”“移动阶段”，目前尽量要求采样的时候镜头是静止的，但实际上比较难保证镜头静止，就会出现在采样的时候镜头同时还在运动，导致读到的坐标会有偏差。

最后，在这里特别感谢大象机器人在项目的开发时提供的帮助，能够让项目完成。这次使用的mechArm是一款中心对称结构的机械臂，在关节运动上有所限制，如果将程序运用在活动范围更加灵活的mycobot上可能是不一样情况。

如果你对项目有啥想要了解更多的地方请在下方给我留言。

审核编辑黄宇