双轴机械臂位置闭环控制: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。对应的脉冲数与角度之间的比值关系: A n g l e P u l s e = 2 14 51200 = 0.32     {{Angle} \over {Pulse}} = {{2^{14} } \over {51200}} = 0.32\,\,\, PulseAngle=51200214=0.32

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

(2) 实验方案

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

对于1/8周( π / 4 \pi /4 π/4)角度传感器变化的理论值对应的角度变化: 2 14 − 3 = 2 11 = 2048 2^{14 - 3} = 2^{11} = 2048 2143=211=2048

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

双轴机械臂位置闭环控制:STC8H1K28,42HS48EIS,BH32_第1张图片

▲ 肘关节运动重复性

(3) 实验数据

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

双轴机械臂位置闭环控制:STC8H1K28,42HS48EIS,BH32_第2张图片

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

  • 均值:2041.62; 方差:0.5056
  • 肩部往返运动
  • 均值:2058.25, 方差:1.24

双轴机械臂位置闭环控制:STC8H1K28,42HS48EIS,BH32_第3张图片

▲ 肩部往返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.设置主频率测量变量

在下位机控制程序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

双轴机械臂位置闭环控制:STC8H1K28,42HS48EIS,BH32_第4张图片

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

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

 

03肘关节运动控制


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

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

#define PULSE2_COUNT 20

通过对PULSE2_COUNT的试凑,获得最优的常数。
双轴机械臂位置闭环控制:STC8H1K28,42HS48EIS,BH32_第5张图片

▲ 在机械臂前端增加机械爪(质量大约为560g)

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

▲ 带有机械爪的前臂运动

 

04肩关节运动调节


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

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

大扭力的电机


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

你可能感兴趣的:(合作项目)