■ 背景介绍
对于 一款用于化学实验室样品操作机械臂 的调试过程中，在博文 双轴机械臂调试：步进电机42HS348E,BH32角度传感器，MCU:STC8H1K28 可以看到初步的通过带有 闭环控制的步进电机42HS48EIS 存在以下缺点：
- 由于手臂的转动惯量，运动具有很大的振荡现象；
- 由于存在着摩擦力，运动位置具有一定的误差；
需要结合在肘关节、肩关节安装的 BH38绝对位置14bit角度传感器 来进行闭环角度控制，用于抑制以上两方面的控制缺点。
01控制问题分析
1.转动过冲振荡
下图显示的是肘关节步进电机施加阶跃命令之后出现的震动情况。肩关节的震动情况更加明显。详细可以查看双轴机械臂调试：步进电机42HS348E,BH32角度传感器，MCU:STC8H1K28中的数据，或者 机械臂建模分析 中的相关数据。
▲ 肘关节运动对应角度变化
关于振荡过程，需要引入角度、角度速度，对于给定的步进电机的脉冲进行控制。
2.位置误差
根据在双轴机械臂调试：步进电机42HS348E,BH32角度传感器，MCU:STC8H1K28所得到的步进电机步数与转动角度之间的比值关系，可以测试在给定步进电机运动指令之后，对应的读取的位置数值与理论之间的误差。
(1) 实验基本参数
- 步进电机细分设置：SW2345=0001，细分步数为51200。对应的脉冲数与角度之间的比值关系：
比值的倒数为：Pulse/Angle=3.128。
(2) 实验方案
对于肩关节和肘关节，分别给定运动步长step = 6400，对应移动角度Angle =90°。测量前后有角度传感器读出的数值与实际理论值之间的误差。
对于1/8周
角度传感器变化的理论值对应的角度变化：
测量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
节中的角度移动误差的猜测：
1.设置主频率测量变量
在下位机控制程序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中相同的程序重新测量肘关节往复运动对应的角度变化。
▲ 肘关节往复运动角度变化
通过上面数据可以看到，脉冲频率的变化，对于位置重复精度没有太大的影响。
03肘关节运动控制
肘关节，通过降低输出步进电机的频率来达到抑制震动的目标。
在主程序中，增加输出脉冲分频变量g_ucPulse2Count，来对脉冲2的输出进行分频。分频的倍率有main.h中的PULSE2_COUNT常数决定。
#define PULSE2_COUNT 20
通过对PULSE2_COUNT的试凑，获得最优的常数。
▲ 在机械臂前端增加机械爪（质量大约为560g）
在前臂增加金属机械爪（质量大约为560g），模拟前爪抓取试剂瓶时的质量。设置PULSE2_COUNT 为20左右的时候，可以大体确定前臂运动不在会有震动的产生。
04肩关节运动调节
通过观察可以看到，对于肘关节的运动，只要控制输出频率，可以有效抑制它的震动。实际上，肩关节的振动抑制是控制中最为关节的部分。
■ 背景介绍
对于 一款用于化学实验室样品操作机械臂 的调试过程中，在博文 双轴机械臂调试：步进电机42HS348E,BH32角度传感器，MCU:STC8H1K28 可以看到初步的通过带有 闭环控制的步进电机42HS48EIS 存在以下缺点：
- 由于手臂的转动惯量，运动具有很大的振荡现象；
- 由于存在着摩擦力，运动位置具有一定的误差；
需要结合在肘关节、肩关节安装的 BH38绝对位置14bit角度传感器 来进行闭环角度控制，用于抑制以上两方面的控制缺点。
01控制问题分析
1.转动过冲振荡
下图显示的是肘关节步进电机施加阶跃命令之后出现的震动情况。肩关节的震动情况更加明显。详细可以查看双轴机械臂调试：步进电机42HS348E,BH32角度传感器，MCU:STC8H1K28中的数据，或者 机械臂建模分析 中的相关数据。
▲ 肘关节运动对应角度变化
关于振荡过程，需要引入角度、角度速度，对于给定的步进电机的脉冲进行控制。
2.位置误差
根据在双轴机械臂调试：步进电机42HS348E,BH32角度传感器，MCU:STC8H1K28所得到的步进电机步数与转动角度之间的比值关系，可以测试在给定步进电机运动指令之后，对应的读取的位置数值与理论之间的误差。
(1) 实验基本参数
- 步进电机细分设置：SW2345=0001，细分步数为51200。对应的脉冲数与角度之间的比值关系：
比值的倒数为：Pulse/Angle=3.128。
(2) 实验方案
对于肩关节和肘关节，分别给定运动步长step = 6400，对应移动角度Angle =90°。测量前后有角度传感器读出的数值与实际理论值之间的误差。
对于1/8周角度传感器变化的理论值对应的角度变化：
测量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
节中的角度移动误差的猜测：
1.设置主频率测量变量
在下位机控制程序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中相同的程序重新测量肘关节往复运动对应的角度变化。
▲ 肘关节往复运动角度变化
通过上面数据可以看到，脉冲频率的变化，对于位置重复精度没有太大的影响。
03肘关节运动控制
肘关节，通过降低输出步进电机的频率来达到抑制震动的目标。
在主程序中，增加输出脉冲分频变量g_ucPulse2Count，来对脉冲2的输出进行分频。分频的倍率有main.h中的PULSE2_COUNT常数决定。
#define PULSE2_COUNT 20
通过对PULSE2_COUNT的试凑，获得最优的常数。
▲ 在机械臂前端增加机械爪（质量大约为560g）
在前臂增加金属机械爪（质量大约为560g），模拟前爪抓取试剂瓶时的质量。设置PULSE2_COUNT 为20左右的时候，可以大体确定前臂运动不在会有震动的产生。
04肩关节运动调节
通过观察可以看到，对于肘关节的运动，只要控制输出频率，可以有效抑制它的震动。实际上，肩关节的振动抑制是控制中最为关节的部分。
举报
