■ 实验背景

---

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

^{▲ 机械臂肩部运动}

 

01建立肩部模型

---

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

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

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

$$F_{obs}=R_{fs}\times T_{shoulder}$$

下图实测示意图。

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

实验参数：

• 步进数值： $N_{step}=10$；
• 步进数量：$N_m=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\cdot 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时，关节脉冲计数$N_{arthrosis}$与关节角度14bit绝对数值旋转编码器数值${\theta }_{BH38}$之间的关系理论值为：

$$N_{arthorsis}={\theta }_{BH32}\times 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

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

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

$$R_{pa}\times Std\left(elbow\right)=8.04\times 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     15

            if(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旋转编码器初步测试