基于STC8H1K28的双轴机械臂驱动模块该如何去实现呢

■ 背景介绍

    对于 一款用于化学实验室样品操作机械臂 的调试过程中，在博文 双轴机械臂调试：步进电机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节中的角度移动误差的猜测：
  

• 上述均值误差和方差是否与 基于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
  

  
  

  

   ▲ 肘关节往复运动角度变化  
  通过上面数据可以看到，脉冲频率的变化，对于位置重复精度没有太大的影响。
   
  03肘关节运动控制

    肘关节，通过降低输出步进电机的频率来达到抑制震动的目标。
  在主程序中，增加输出脉冲分频变量g_ucPulse2Count，来对脉冲2的输出进行分频。分频的倍率有main.h中的PULSE2_COUNT常数决定。
  

   #define PULSE2_COUNT 20
  

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

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

  04肩关节运动调节

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

• 肩部力矩太小，需要改动大力据电机。