双轴机械臂位置闭环控制：STC8H1K28，42HS48EIS,BH32

 

■ 背景介绍

---

对于 一款用于化学实验室样品操作机械臂 的调试过程中，在博文 双轴机械臂调试：步进电机42HS348E,BH32角度传感器，MCU:STC8H1K28 可以看到初步的通过带有 闭环控制的步进电机42HS48EIS 存在以下缺点：

• 由于手臂的转动惯量，运动具有很大的振荡现象；
• 由于存在着摩擦力，运动位置具有一定的误差；

需要结合在肘关节、肩关节安装的 BH38绝对位置14bit角度传感器 来进行闭环角度控制，用于抑制以上两方面的控制缺点。

 

01控制问题分析

---

1.转动过冲振荡

下图显示的是肘关节步进电机施加阶跃命令之后出现的震动情况。肩关节的震动情况更加明显。详细可以查看双轴机械臂调试：步进电机42HS348E,BH32角度传感器，MCU:STC8H1K28中的数据，或者 机械臂建模分析 中的相关数据。

^{▲ 肘关节运动
▲ 肘关节运动对应角度变化}

关于振荡过程，需要引入角度、角度速度，对于给定的步进电机的脉冲进行控制。

2.位置误差

根据在双轴机械臂调试：步进电机42HS348E,BH32角度传感器，MCU:STC8H1K28所得到的步进电机步数与转动角度之间的比值关系，可以测试在给定步进电机运动指令之后，对应的读取的位置数值与理论之间的误差。

(1) 实验基本参数

• 步进电机细分设置：SW2345=0001，细分步数为51200。对应的脉冲数与角度之间的比值关系：$$\frac{Angle}{Pulse}=\frac{2^{14}}{51200}=0.32$$

比值的倒数为：Pulse/Angle=3.128。

(2) 实验方案

对于肩关节和肘关节，分别给定运动步长$step=6400$，对应移动角度Angle =90°。测量前后有角度传感器读出的数值与实际理论值之间的误差。

对于1/8周（$\pi /4$）角度传感器变化的理论值对应的角度变化：$2^{14-3}=2^{11}=2048$。

测量100次，获得测量角度变化值的均值、方差。

^{▲ 肘关节运动重复性}

(3) 实验数据

• 肘部测量数据
  肘部往返45°运动角度变化数据。每次运动等待3秒钟。测量100次数据如下。

^{▲ 肘部往返45°角度数值变化}

• 均值：2041.62； 方差：0.5056
• 肩部往返运动
• 均值：2058.25， 方差：1.24

^{▲ 肩部往返45°角度数值变化}

(3) 结果分析

• 在肘关节的均值为2041.6，比起理论值2048少了6.4（0.141°）
• 肩关节的均值为2058.25， 比起理论值2048多了10.25（0.2252°）

上述关节角度与运动之间的误差究竟来源是什么？现在还不知道。

对比肘关节和肩关节的位置关节标准差，肩关节更大一些。

#!/usr/local/bin/python
# -*- coding: gbk -*-
#============================================================
# TEST3.PY                     -- by Dr. ZhuoQing 2020-08-22
#
# Note:
#============================================================
from headm import *
from tsmodule.tsstm32       import *
anglediff = []
for i in range(20):
    stm32cmd('CLEAR')
    stm32cmd('angle')
    time.sleep(.2)
    val = stm32memo(1)
    startangle = val[0]
    SLEEP_TIME = 15
    stm32cmd('step1 6400')
    time.sleep(SLEEP_TIME)
    stm32cmd('CLEAR')
    stm32cmd('angle')
    time.sleep(.2)
    val = stm32memo(1)
    endangle = val[0]
    dif = abs(startangle - endangle)
    printff(i, dif)
    anglediff.append(dif)
    stm32cmd('step1 -6400')
    time.sleep(SLEEP_TIME)
    stm32cmd('CLEAR')
    stm32cmd('angle')
    time.sleep(.2)
    val = stm32memo(1)
    dif = abs(val[0] - endangle)
    anglediff.append(dif)
    printff(i, dif)
tspsave('angle1', angle=anglediff)
printff(mean(anglediff), std(anglediff))
plt.plot(anglediff)
plt.xlabel("Sample")
plt.ylabel("Angle Difference")
plt.grid(True)
plt.tight_layout()
plt.show()
#------------------------------------------------------------
#        END OF FILE : TEST3.PY
#============================================================

 

02测试输出脉冲频率的影响

---

对于01节中的角度移动误差的猜测：

• 上述均值误差和方差是否与 基于STC8H1K28的双轴机械臂驱动模块 给定步进电机脉冲频率有关系呢？
• 验证：将驱动模块的脉冲降低10倍。修改其中的Delay(n)中的参数，验证上述肘关节的误差是否还存在。

1.设置主频率测量变量

在下位机控制程序¹的主程序，增加关于主循环频率测量变量。

• 在Delay()函数中，对于全局变量g_nDelayCount++, 并在100ms中断中保存g_nDelayCount值并将其清零。
• 设置全局100ms定时变量。

使用串口命令“100ms”，可以获得回复： DelayCount：14272。由于这个数字是主循环在100ms内调用Delay的次数，也预示着主循环的频率为142.72kHz。

如果将Delay(10)修改成Delay(100)，则返回的100ms命令的返回为：DelayCount:2699，对应主循环的频率为27kHz。这个数字比在一款用于化学实验室样品操作机械臂所测量的13.85kHz减少了很多。

2.测量肘关节往复运动角度变化

利用和 01.2中相同的程序重新测量肘关节往复运动对应的角度变化。

• 均值：:2042.025，方差：0.156

^{▲ 肘关节往复运动角度变化}

通过上面数据可以看到，脉冲频率的变化，对于位置重复精度没有太大的影响。

 

03肘关节运动控制

---

肘关节，通过降低输出步进电机的频率来达到抑制震动的目标。

在主程序中，增加输出脉冲分频变量g_ucPulse2Count，来对脉冲2的输出进行分频。分频的倍率有main.h中的PULSE2_COUNT常数决定。

#define PULSE2_COUNT 20

通过对PULSE2_COUNT的试凑，获得最优的常数。

^{▲ 在机械臂前端增加机械爪（质量大约为560g）}

在前臂增加金属机械爪（质量大约为560g），模拟前爪抓取试剂瓶时的质量。设置PULSE2_COUNT 为20左右的时候，可以大体确定前臂运动不在会有震动的产生。

^{▲ 带有机械爪的前臂运动}

 

04肩关节运动调节

---

通过观察可以看到，对于肘关节的运动，只要控制输出频率，可以有效抑制它的震动。实际上，肩关节的振动抑制是控制中最为关节的部分。

1. 肩部力矩太小，需要改动大力据电机：

大扭力的电机

---

1. 下位机控制程序:C51\STC\Project\XQWF\2020\STC8H2AXES\STC8H2AXES.uvproj ↩︎