■ 实验背景

在 两轴机械臂+机械爪整体控制板设计与机械爪控制调试 的基础上对于机械臂各关节进行了 双关节机械臂+机械爪运动控制 ，其中存在的主要问题还是肩关节运动不平稳的情况。

^{▲ 机械臂肩部运动}

01建立肩部模型

1.肩部受阻步数与扭矩之间的关系

使用一个压力传感器定住机械臂旋转，测量机械臂在受阻情况下所产生的旋转扭矩与步数之间的关系。

由于测量过程中机械臂保持静止，所以受阻压力FobsF_{obs}Fobs与旋转扭矩TshoulderT_{shoulder}Tshoulder之间成正比。

Fobs=Rfs×TshoulderF_{obs} = R_{fs} \times T_{shoulder}Fobs=Rfs×Tshoulder

下图实测示意图。

^{▲ 使用压力传感器测量机械臂的力矩}

实验参数：

步进数值： Nstep=10N_{step} = 10Nstep=10；
步进数量：Nm=100N_m = 100Nm=100；

测试的压力曲线为：

(1) 实验一:

^{▲ 步进电机移动步数(单位脉冲：10)与压力传感器之间的关系}

(2) 实验二：

^{▲ 步进电机移动步数(单位脉冲：10)与压力传感器之间的关系}

2.测量结果

#!/usr/local/bin/python
# -*- coding: gbk -*-
#============================================================
# TEST4.PY                     -- by Dr. ZhuoQing 2020-09-08
#
# Note:
#============================================================
from headm import *
from tsmodule.tsstm32       import *
pressdim = []
for i in range(100):stm32cmd('step1 -10')time.sleep(1)meter = meterval()pressdim.append(meter[0])printff(i, meter)
tspsavenew('press', press=pressdim)
plt.plot(pressdim)
plt.xlabel("移动步数")
plt.ylabel("压力传感器读数(V)")
plt.grid(True)
plt.tight_layout()
plt.show()
#------------------------------------------------------------
#        END OF FILE : TEST4.PY
#============================================================

将两次实验结果绘制在一起：

受阻步数与力矩之间关系呈现两个阶段：

第一个阶段：基本线性阶段，长度为288。对应角度传感器读数：92。16（2.025°）
第二个阶段：饱和阶段，长度458。对应角度传感器读数：146.56（3.22°）

测试实验数据：

press=[0.0110,0.0120,0.0120,0.0120,0.0120,0.0120,0.0120,0.0120,0.0210,0.0380,0.0550,0.0700,0.0860,0.1030,0.1190,0.1330,0.1470,0.1630,0.1760,0.1910,0.2100,0.2290,0.2470,0.2650,0.2840,0.3020,0.3150,0.3240,0.3420,0.3570,0.3700,0.3830,0.3950,0.4060,0.4150,0.4220,0.4290,0.4340,0.4380,0.4410,0.4430,0.4450,0.4470,0.4520,0.4570,0.4650,0.4730,0.4630,0.4670,0.4780,0.4900,0.5060,0.5210,0.5280,0.5290,0.5290,0.5290,0.5280,0.5290,0.5290,0.5280,0.5290,0.5290,0.5290,0.5290,0.5290,0.5290,0.5290,0.5300,0.5290,0.5300,0.5290,0.5290,0.5290,0.5290,0.5300,0.5290,0.5290,0.5290,0.5300,0.5290,0.5290,0.5290,0.5290,0.5290,0.5300,0.5300,0.5290,0.5300,0.5290,0.5290,0.5290,0.5300,0.5300,0.5290,0.5290,0.5290,0.5290,0.5290,0.5290]

press=[0.0120,0.0120,0.0120,0.0130,0.0130,0.0130,0.0130,0.0130,0.0200,0.0360,0.0540,0.0700,0.0850,0.1020,0.1160,0.1320,0.1480,0.1600,0.1740,0.1910,0.2080,0.2280,0.2470,0.2660,0.2850,0.2970,0.3100,0.3270,0.3420,0.3580,0.3720,0.3840,0.3970,0.4070,0.4170,0.4240,0.4310,0.4360,0.4400,0.4430,0.4450,0.4470,0.4500,0.4540,0.4590,0.4520,0.4530,0.4590,0.4680,0.4800,0.4900,0.5060,0.5210,0.5290,0.5290,0.5300,0.5310,0.5300,0.5300,0.5310,0.5300,0.5300,0.5300,0.5310,0.5300,0.5310,0.5300,0.5310,0.5300,0.5310,0.5300,0.5300,0.5300,0.5300,0.5300,0.5300,0.5300,0.5310,0.5300,0.5310,0.5310,0.5310,0.5310,0.5310,0.5310,0.5310,0.5310,0.5310,0.5310,0.5300,0.5310,0.5310,0.5310,0.5310,0.5310,0.5310,0.5300,0.5310,0.5300,0.5310]

3.建立步数与扭矩之间的函数关系

去8~37之间的数据，建立移动步数与扭矩之间的线性关系。

F=a⋅x+bF = a \cdot x + bF=a⋅x+b

a = 0.001545
b = 0.02596
^{▲ 使用线性函数对扭矩与步骤之间建立模型}

#!/usr/local/bin/python
# -*- coding: gbk -*-
#============================================================
# TEST11.PY                    -- by Dr. ZhuoQing 2020-09-08
#
# Note:
#============================================================
from headm import *
from scipy.optimize import curve_fit
def func(x, a, b):return x*a+b
press = tspload('press11', 'press')
pressl = press[8:36]
param = [0.1, 0]
xlist = array(range(len(pressl)))*10
printf(xlist)
param, conv = curve_fit(func, xlist, pressl, p0=param)
printf(param)
fitdata = func(xlist, *param)
plt.plot(xlist, pressl)
plt.plot(xlist, fitdata)
plt.xlabel("Step")
plt.ylabel("Output Voltage(mV)")
plt.grid(True)
plt.tight_layout()
plt.show()
#------------------------------------------------------------
#        END OF FILE : TEST11.PY
#============================================================

02设置关节运动脉冲计数

在主程序中，建立关节输出摸脉冲计数变量，用于跟踪（观测）步进电机内部的脉冲数量。为后面根据（01）节中间建立的扭矩模型给出关节运动扭矩的观测模型。

1.关节步进脉冲计数变量初始化

关节脉冲计数变量与管脚角度之间使用如下函数进行初始化。该函数在程序启动时，机械臂保持静止状态下进行初始化。在两个关节步进电机的旋转一种细分步数都设置为51200时，关节脉冲计数NarthrosisN_{arthrosis}Narthrosis与关节角度14bit绝对数值旋转编码器数值θBH38\theta _{BH38}θBH38之间的关系理论值为：

Narthorsis=θBH32×3.125N_{arthorsis} = \theta _{BH32} \times 3.125Narthorsis=θBH32×3.125

void Angle2AccumulateAngleNumber(void) {GetAngleGlobal();g_nAccum1Number = (unsigned int)(g_nAngle1 * 3.125);g_nAccum2Number = (unsigned int)(g_nAngle2 * 3.125);
}

2.测试角度脉冲跟踪的误差

通过指令angle1,angle2获得机械臂运行不同的角度值，对比脉冲跟踪计数变量的数值与理论值之间的误差关系。

(1) 一次运动误差曲线

^{▲ 两个关节运动后脉冲变量与理论计算值之间的误差}

肩部误差： mean = 17.0, std = 14.31
肘部误差：mean = -30.2, std=18.2

根据在 双关节机械臂+机械爪运动控制 中，对于指令运动后存在着两个关节之间的方差：

Var(Shoulder) = 51.25, Var(elbow) = 64.66
通过计算可以得到:

Std(Shoulder) = sqrt(Var) = 7.16; Std(Elbow) =sqrt(var) = 8.04

考虑到 双关节机械臂+机械爪运动控制 中给出的步进角度比Rpa=Pulse(51200)Angle(14bit)=3.125R_{pa} = {{Pulse\left( {51200} \right)} \over {Angle\left( {14bit} \right)}} = 3.125Rpa=Angle(14bit)Pulse(51200)=3.125

因此：Rpa×Std(shoulder)=7.16×3.125=22.375R_{pa} \times Std\left( {shoulder} \right) = 7.16 \times 3.125 = 22.375Rpa×Std(shoulder)=7.16×3.125=22.375

Rpa×Std(elbow)=8.04×3.125=25.125R_{pa} \times Std\left( {elbow} \right) = 8.04 \times 3.125 = 25.125Rpa×Std(elbow)=8.04×3.125=25.125

可以看到上面的误差与前面测量得到的一次移动所由于摩擦阻尼的存在所存在的随机误差有关系。

(2) 两次运动误差曲线

使用移动的方式来重新测量关节脉冲计数变量误差。
如下是测量的结果：

^{▲ 关节脉冲观察变量与计时理论计算之间ade误差}

Shoulder Error: Mean = 24.0, Std=15.01
Elbow Error: Mean = 13.75, Std=20.1

对比两次移动和一次移动可以看到，关节脉冲变量与角度之间的误差方差并没有得到减少，这与 双关节机械臂+机械爪运动控制 中两次移动对于角度误差的减小是不相同的。具体原因有待进一步分析。

(3) 数据分析

通过前面两次实验可以测到如下结论：

一次移动和两次移动（移动修正）对于关节脉冲计数误差没有改善；
脉冲关键脉冲计数误差并不会随着移动次数增加出现累积性的误差。

03肩关节恒力矩运动

1.实验方法和问题

(1) 控制方法：在函数中增加恒力矩跟踪控制。也就是根据当前的角度与关节脉冲计数来控制关节运动的力矩。初步测试是可以防止一顿死过冲的。但是出现了新的问题：实际步进电机丢失脉冲！.

(2) 出现的问题：在（02）节中的的是，肩关节运动使用了分频的方式发送，脉冲数据没有丢失，而在后面测试中，发现丢失脉冲的现象比较严重。

(3) 解决思路：通过降低肩关节的分频系数，来减少所需要脉冲输出的频率，进而降低丢失脉冲的可能性。

根据 57HSXXXXEIS一体化步进伺服驱动电机 中不仅电机伺服器的细分设置，将原来设定的51200修改成25600。

^{▲ 肩部步进电机细分设置}

2.好像猜想是错误的

通过降低细分的步数，问题还是没有完全解决。

● 进一步的猜想: 还是有可能是机械部分，上下转动不同轴，造成力矩的多多少少的不均匀？

使用step1(50) 前进100步，使用angle 获得Angle1, g_nAccum1Number。进行线性拟合：

^{▲ 进行线性拟合后的结果}

线性拟合后对应的Angle误差，如下图所示。

^{▲ 线性拟合误差曲线}

下面是更大范围运动线性拟合曲线误差。使用step1(100)移动100步。

^{▲ 线性拟合误差曲线}

因此，猜想悬臂运动与角度传感器不同轴的猜想是错误的。

3.还是角度测量时间太长惹的祸

由于读取角度值需要时间，大约 BH38旋转编码器初步测试 读取数值大约需要10ms。此时读取的角度与输出脉冲之间差别。因此，需要根据当时的速度对于角度差别进行修正。

^{▲ 肩部运动过程}

使用nDeltaAngle对于实际测量的Angle进行修正之后，机械臂进行恒力矩运行效果非常好了。

nDeltaAngle = g_nAngle1;
nDeltaAngle -= g_nLastAngle1;lnNumber = g_nAccum1Number;
lnNumber -= (unsigned int)((g_nAngle1 - nDeltaAngle / 4) * STEP_ANGLE_RATIO1);

※ 结论

通过施加肩关节的脉冲变量对于步进电机的力矩进行检测。在调节过程中对于力矩进行控制，实现恒力矩运行并进行终端缓冲。

需要考虑到角度传感器读取角度过程中的延迟，使得实际关节脉冲与实际角度之间不是同事测量得到的。使用两次角度测量差（反映了旋转角度速度）来修正实际角度值，可以消除运动中的不稳定性。

下面给出了肩关节运行控制命令代码。

//------------------------------------------------------------------
#define MAX_PULSE1          150
#define MIN_PULSE1          40
#define RATIO_PULSE1        50
#define MIN_DELTA_ANGLE     15if(g_nAngle1Set != g_nAngle1 && g_ucMoveControlFlag) {if(g_nAngle1Set > g_nAngle1) {if(g_ucMoveControlFlag == 0xff) {g_ucMoveControlFlag = 0;
//                        printf("U\r\n");continue;}nNumber = (unsigned int)((g_nAngle1Set - g_nAngle1) * STEP_ANGLE_RATIO1);                                        nMaxNumber = MAX_PULSE1;if(nMaxNumber > nNumber / RATIO_PULSE1) nMaxNumber = nNumber / RATIO_PULSE1;if(nMaxNumber < MIN_PULSE1) nMaxNumber = MIN_PULSE1;nDeltaAngle = g_nAngle1;nDeltaAngle -= g_nLastAngle1;lnNumber = g_nAccum1Number;lnNumber -= (unsigned int)((g_nAngle1 - nDeltaAngle / 4) * STEP_ANGLE_RATIO1);//                    printf("(%ld, %d)", lnNumber, nDeltaAngle);if(lnNumber < nMaxNumber) {nIncNumber = nNumber;if(nIncNumber + lnNumber > nMaxNumber) {nIncNumber = nMaxNumber - lnNumber;                            g_nPulse1Number += nIncNumber;ON(DIR1_PIN);}} else {if(nDeltaAngle < MIN_DELTA_ANGLE)g_nAccum1Number = (unsigned int)(g_nAngle1 * STEP_ANGLE_RATIO1);}} else if(g_nAngle1Set < g_nAngle1) {if(g_ucMoveControlFlag == 0x1) {g_ucMoveControlFlag = 0;
//                        printf("V\r\n");continue;}nNumber = (unsigned int)((g_nAngle1 - g_nAngle1Set) * STEP_ANGLE_RATIO1);nMaxNumber = MAX_PULSE1;if(nMaxNumber > nNumber / RATIO_PULSE1) nMaxNumber = nNumber / RATIO_PULSE1;if(nMaxNumber < MIN_PULSE1) nMaxNumber = MIN_PULSE1;nDeltaAngle = g_nLastAngle1;nDeltaAngle -= g_nAngle1;lnNumber = (unsigned int)((g_nAngle1 + nDeltaAngle / 4) * STEP_ANGLE_RATIO1);lnNumber -= g_nAccum1Number;//                    printf("%ld ", lnNumber);if(lnNumber < nMaxNumber) {nIncNumber = nNumber;if(nIncNumber + lnNumber > nMaxNumber) {nIncNumber = nMaxNumber - lnNumber;g_nPulse1Number += nIncNumber;OFF(DIR1_PIN);}} else {if(nDeltaAngle < MIN_DELTA_ANGLE)g_nAccum1Number = (unsigned int)(g_nAngle1 * STEP_ANGLE_RATIO1);}}}

■ 相关文献链接:

两轴机械臂+机械爪整体控制板设计与机械爪控制调试
双关节机械臂+机械爪运动控制
57HSXXXXEIS一体化步进伺服驱动电机
BH38旋转编码器初步测试

双轴机械臂中的闭环步进电机平顺控制算法： 42HS48EIS，57HS相关推荐

双轴机械臂调试：步进电机42HS348E,BH32角度传感器，MCU:STC8H1K28
■ 前言这款双轴机械臂是应用于实验室中进行自动样品转换的执行机构.为了能够在一定范围内将试剂瓶(试管) 完成自动搬运,需要完成对样品的抓取,提升和放下.水平移动(二维) 等功能.使用带有肘关节.肩关 ...
基于STC8H1K28的双轴机械臂驱动模块:步进电机42HS348E,BH32角度传感器
作者:卓晴博士,清华大学自动化系更新时间:2020-08-08 Saturday ■ 设计背景这是一款用于化学实验室样品上样器的机械臂.设置有肘关节.肩关节以及上下移动关节. ▲ 双轴机械臂结构 ...
双轴机械臂位置闭环控制：STC8H1K28，42HS48EIS,BH32
■ 背景介绍对于一款用于化学实验室样品操作机械臂的调试过程中,在博文双轴机械臂调试:步进电机42HS348E,BH32角度传感器,MCU:STC8H1K28 可以看到初步的通过带有闭环控制的 ...
组装肩部带有减速器双轴机械臂组装与调试
➤ 00背景在增加了机械爪的双轴机械臂安装调试之后, 发现进行平顺控制效果不好 ,因此在原来的基础上进行了如下的改动: 肩部和肘部的角度传感器采用了: 角度编码器 ST-3806-15-RS 读 ...
双轴机械臂串口控制命令开发与测试：STM32F103控制板,简易调试命令集合
▌01 底层串口控制命令 1.调试说明本文是继调试机械臂一体化控制电路博文中对于两轴机械臂+机械爪整体控制板设计与机械爪控制调试在基于STM32F103双轴机械臂完整电路板控制下进行串口 ...
双轴机械臂建模分析数据
作者:卓晴博士,清华大学自动化系更新时间:2020-08-19 Wednesday ■ 背景在对于实验室双轴机械臂进行运动调试过程中,设计最后的控制器需要对运动对象进行力学建模分析.利用肩关节. ...
基于STM32F103双轴机械臂完整电路板设计
➤01 机械臂设计在基于STM32对于三轴机械臂控制器设计设计了机械臂的控制电路板.采用了双板分开设计方式.并分别进行了如下的测试: 1. 对于STM32F103三轴机械臂控制器进行基本 ...
对于STM32F103控制的三轴机械臂基本功能测试-关节转动控制
➤01 机械臂调试 1.简介对于基于STM32对于三轴机械臂控制器设计的设计已经进行了如下的调试: 对于STM32F103三轴机械臂控制器进行基本功能测试-关节角度读取对于STM32F103三 ...
基于STM32对于三轴机械臂控制器设计
简介: 本文使用了STM32对于一款三轴机械臂进行初步驱动,调试他的各个关节的运动情况. 关键词: 机械臂,三轴机械臂,STM32 ➤ 01背景在组装肩部带有减速器双轴机械臂组装与调试的调试基 ...

双轴机械臂中的闭环步进电机平顺控制算法： 42HS48EIS，57HS