01控制调试基础

在博文两轴机械臂+机械爪整体控制板设计与机械爪控制调试 中给出了双轴机械臂整体控制电路板的设计以及初步调试的过程。本文则完成对于双臂运动的控制调试。

控制调试包括两部分的内容：

机械臂的运动动态性能：速度、稳定性、低过冲；
机械位置的运动精度：重复到达位置的位置精度；

02一体化控制电路安装

1.设置WiFi STC调试器

通过手机选择USR-WIFI232-195，选择10.10.100.254进行登录。然后选择STA的登录的WiFI的热点。

▲ 登录10.10.100.254进行设置WiFI接入热点

^{▲ 登录10.10.100.254进行设置WiFI接入热点}

2.控制板与机械臂其它部件连接关系

下图显示了控制板与其它的机械臂的部件连接的关系。

▲ 控制电路板与机械臂其它部件连接关系

^{▲ 控制电路板与机械臂其它部件连接关系}

03初步测试

1.肘关节运动范围

min = 3000
max = 14000
step range = 35000

▲ 肘关节线性增加的角度（逆时针旋转）

^{▲ 肘关节线性增加的角度（逆时针旋转）}

2.肩关节运动范围

对于肩关节的角度处理：

▲ 肩关节线性增加角度

^{▲ 肩关节线性增加角度}

3.测量双轴移动步数与角度比值

(1) 测试1

记录起始位置，然后使用m;l; move12正向移动20000步。

▲ 机械臂移动前后的位置

^{▲ 机械臂移动前后的位置}

起始位置	关节1:1961	关节2:2672
结束位置	10089	9072
变化数值	δ1：8128	δ2：6400

(2) 测试2

记录起始位置，然后使用m;l; move12正向移动20000步。

起始位置	关节1: 1972	关节2: 2659
结束位置	10019	9062
变化数值	δ1：8047	δ2：6403

$\Delta_{1} = {{\delta _{11} + \delta _{12} } \over 2} = {{8128 + 8047} \over 2} = 8087.5$
$\Delta _2 = {{\delta _{21} + \delta _{22} } \over 2} = {{6400 + 6403} \over 2} = 6401.5$

由于 BH38旋转编码器初步测试 一周的数值为： $N_{BH38} = 2^{14} = 16384$ 。由此可以得到肩部（关节1），肘部（关节2）旋转一周的步数N1, N2:

$N_1 = {{2^{14} \times 20000} \over {\Delta _1 }} = {{2^{14} \times 20000} \over {8087.5}} = 40516.85$
$N_2 = {{2^{14} \times 20000} \over {\Delta _2 }} = {{2^{14} \times 20000} \over {6401.5}} = 51188.0$

N1,N2的比值符合 57HSXXXXEIS ， 42HS48EIS 的步进细分表表格的设置。

为了统一两个电极的控制，将57HSXXXXEIS（肩部步进电机控制器）的细分设置也设置为512000。重新进行测定。

修改步进电机设置之后，重新上电启动系统。

起始位置	关节1：1984	关节2:2698
结束位置	8327	9097
位置变化	6343	6399

经过测试可以看到，两者之间的步进角度比（ $R_{sa}$ ）都接近于：
$R_{sa} = {{{\rm{Step}}\,{\rm{Numer}}} \over {Angle}} = {{51200} \over {2^{14} }} = 3.125$

04位置闭环控制

1.一次运动误差

对应的一次运动误差是指，仅仅运动一次，然后测量静止时刻的误差。生成两个角度随机设定值，然后机械臂运行到相应的位置。读取角度取值，获取与设定值之间的差值。作为误差值进行统计。

计算的方式：根据当前的角度误差，使用步进角度比(Rsa) 来计算每个关节的移动步距（即移动步骤脉冲个数）和具体的方向。

具体测试的结果见下面的曲线。统计值为：

meanx = -0.15， Var(x) = 51.25
mean(y) = 0.02; Var(y) = 64.66

delx=[7.0, 10.0, 10.0, 7.0, 3.0, 11.0, 17.0, 8.0, 3.0, 5.0, 6.0, 12.0, 12.0, 4.0, 6.0, 1.0, 1.0, 1.0, 8.0, 8.0, 2.0, 6.0, 0.0, 11.0, 8.0, 4.0, 9.0, 2.0, 5.0, 12.0, 5.0, 6.0, 9.0, 7.0, 1.0, 1.0, 2.0, 0.0, 2.0, 1.0, 7.0, 16.0, 2.0, 0.0, 3.0, 5.0, 7.0, 3.0, 12.0, 0.0]
dely=[12.0, 6.0, 8.0, 15.0, 1.0, 5.0, 1.0, 0.0, 7.0, 14.0, 10.0, 10.0, 6.0, 6.0, 2.0, 8.0, 0.0, 2.0, 6.0, 5.0, 5.0, 9.0, 1.0, 10.0, 6.0, 4.0, 1.0, 7.0, 10.0, 11.0, 6.0, 0.0, 6.0, 13.0, 12.0, 1.0, 2.0, 5.0, 16.0, 13.0, 8.0, 4.0, 5.0, 5.0, 6.0, 5.0, 0.0, 3.0, 1.0, 4.0]

2.二次运动误差

mean(x) = -0.094; Var(x) = 4.08
mean(y) = 0.031; Var(y) = 4.28

delx=[1.0, 1.0, 1.0, 2.0, 1.0, 1.0, 1.0, 1.0, 4.0, 3.0, 1.0, 0.0, 4.0, 3.0, 3.0, 0.0, 2.0, 2.0, 1.0, 1.0, 2.0, 2.0, 0.0, 0.0, 2.0, 3.0, 4.0, 0.0, 2.0, 2.0, 2.0, 1.0]
dely=[0.0, 1.0, 3.0, 2.0, 2.0, 3.0, 2.0, 1.0, 1.0, 3.0, 2.0, 0.0, 0.0, 2.0, 4.0, 2.0, 1.0, 4.0, 2.0, 2.0, 1.0, 3.0, 1.0, 3.0, 1.0, 2.0, 2.0, 1.0, 1.0, 1.0, 3.0, 1.0]

#!/usr/local/bin/python
# -*- coding: gbk -*-
#============================================================
# TEST1.PY                     -- by Dr. ZhuoQing 2020-09-07
#
# Note:
#============================================================

from headm import *
from random                 import *
from tsmodule.tsstm32       import *

randpos = [(randint(3000,8000), randint(4000, 8000)) for s in range(50)]
printf(randpos)

delxdim = []
delydim = []

for p in randpos:
    stm32cmd('angle12 %d %d'%(p[0], p[1]))
    time.sleep(.5)

    while True:
        stm32cmd('CLEAR')
        stm32cmd('angle')
        time.sleep(.5)
        val = stm32memo(1)

        if len(val) != 4: continue

        if val[2] + val[3] == 0: break

    time.sleep(2)
    stm32cmd('angle12 %d %d'%(p[0], p[1]))
    time.sleep(.5)
    time.sleep(1)

    stm32cmd('CLEAR')
    stm32cmd('angle')
    time.sleep(.5)
    val = stm32memo(1)
    delxdim.append(p[0] - val[0])
    delydim.append(p[1] - val[1])

    tspsave('delta1', delx=delxdim, dely=delydim)
    printf(val)

tspsavenew('delta1', delx=delxdim, dely=delydim)
plt.plot(delxdim)
plt.plot(delydim)
plt.xlabel("Sample")
plt.ylabel("DeltaX Y")
plt.grid(True)
plt.tight_layout()
plt.show()

#------------------------------------------------------------
#        END OF FILE : TEST1.PY
#============================================================

通过上面实验可以验证，使用二次位置校正，可以有效的的角度空间位置统计误差可以降低到±4左右。假设肩部的角度误差 $\Delta \theta _{shoulder} = {4 \over {2^{14} }} \times 2\pi = 0.001534$

肩部的臂长 $L_{should} = 1$ 米，那么中断对应的位值误差为：

$\Delta L = \Delta \theta _{shoulder} \times L = 1.53mm$

对应的最坏的位置误差小于2mm。达到最初的设计精度要求。

05结论

根据对双关节+机械爪运动初步控制，运动中的主要矛盾还是出现在对肩部的动态控制方面。如果还是采用放缓输出角度，则无法达到运动速度和过冲之间的矛盾。

下面需要进一步同安肩部角度反馈来获得对肩部运动的高速稳定扫之。

另文给出：

肩部高速平滑控制方式；
机械爪带有力矩反馈的控制；

■ 相关文献链接:

双关节机械臂+机械爪运动控制