海康对接android

 

■ 前言

---

这款双轴机械臂是应用于实验室中进行自动样品转换的执行机构。为了能够在一定范围内将试剂瓶（试管） 完成自动搬运，需要完成对样品的抓取，提升和放下、水平移动（二维） 等功能。使用带有肘关节、肩关节以及上下移动关节的机械平台可以完成这项设计。

在基于STC8H1K28双轴机械臂驱动模块 给出了对肩关节、肘关节上的步进电机、角度传感器读取和控制模块的设计。本文下面就是利用该模块对机械臂进行运动控制。

^{▲ 双轴机械臂的外观}

 

01联线配线

---

1.步进电机和角度传感器接口

(1) 步进电机引线

引出4PIN的接口，分别定义为：

PIN	符号	功能	颜色
PIN1（MARK）	PU+,DR+,MF+	三个控制端口的公共电源线	红，蓝，棕
PIN2	PU-	脉冲输入	黄
PIN3	DIR-	方向输入	黑
PIN4	MF-	势能输入	绿

上述定义的四线插头可以接入基于STC8H1K28双轴机械臂驱动模块中的PANGx(角度传感器端口)。

(2) BH38角度传感器接口

引出4PIN接口，定义为：

PIN	符号	功能	颜色
PIN1(MARK)	+5V	工作电源	棕：BROWN
PIN2	0V	电源地	蓝：BLUE
PIN3	A+	485总线A+	黑：BLACK
PIN4	B-	485总线B-	白

另外两个引线置零（RED），编程（黄）不再接入端口。

(3) 通道分配

在基于STC8H1K28双轴机械臂驱动模块¹具有两个通道的步进电机控制和角度传感器。将通道1（步进电机控制、角度传感器读取）分配给肩关节；将通道2（步进电机控制，角度传感器读取）分配给肘关节。

2.连接控制模块

按照上述的通道方案，将肘关节、肩关节的部件电机驱动端口、角度传感器端口分别进入实验电路板的相应的端口。具体参见上面的“通道分配”中的示意图。

^{▲ 控制模块与关节步进电机和角度传感器相连}

3.WiFi调试

^{▲ 连接有WiFI调试下载器的系统}

4.步进电机工作电源

在 STC8K28步进电机ULC2003触发器 测试两个步进电机需要供电电压大于15V，这里使用电源24V开关电源进行供电。

^{▲ 供电电源：24V，+5V}

电源同时提供+5V，+24V 电压。供电电流最大4.6A。

使用ADJ电位器可以同时提高和降低输出的+5V, +24V电压。

 

02初步运动控制²

---

直接利用在基于STC8H1K28双轴机械臂驱动模块中的串口命令，通过WiFi调试界面发送指令控制双轴机械臂运动。

1.控制肘关节运动

通过串口命令step2实现肘关节的运动:

step2 2000
step2 -2000

step2的命令参数： $n > 0$:肘臂顺时针旋转； $n < 0$肘臂逆时针旋转。（从顶视图来看。）

^{▲ 肘关节运动}

2.控制肩关节运动

step1 1000
step1 -1000

step1的参数：$n > 0$肩部顺时针旋转。$n < 0$逆时针旋转；（从顶视图来看）。

^{▲ 肩关节运动}

通过上面肩关节和肘关节运动情况来看，证明了以下两方面的事实：

1. 42HS48EIS的闭环步进电机。这款步进电机在带动负载运动过程中，不会丢步，并对位置（步数）进行闭环跟踪控制。那么42HS48EIS的闭环控制特性究竟是什么？这需要进一步通过它的数据手册来了解。
2. 在肩关节和肘关节中，存在着较大的振荡情况。这主要是运动手臂本身转动惯量所引起的。那么在后面的控制指令中如何进一步来消除这种过冲，需要在算法上来实现运动阻尼。

^{▲ 肩关节运动对应的角度变化数值}

^{▲ 肘关节运动对应角度变化}

3.肩关节与肘关节角度极限

(1) 旋转方向与角度增减

使用angle命令读取肘关节与肩关节的角度信息，使用step1（肩关节运动命令），step2（肘关节运动命令）驱动两个运动，可以证明：

• 两个轴的角度：在顺时针旋转的时候减少，在逆时针旋转的时候增加。

(2) 两个轴的极限位置

通过将肩关节和肘关节的电机使能取消，手动将肩关节和肘关节旋转的极限位置。使用串口命令angle，可以获得在各个极限位置下角度的数值。

• 肘关节顺时针旋转，角度变化为：14526 --> 1122

^{▲ 肘关节的角度变化范围}

• 肩关节顺时针旋转，角度变化为：3810 -->10800

^{▲ 肩关节角度范围}

4.旋转与角度之比

• 42HS48EIS步进电机细分设置： SW2345=0001，对应40000步一个周角。

脉冲与角度之间的比值。使用move1(move2)命令，然后测量angle的变化。

■ 表格:move 10000前后的角度变化

肩关节角度	肘关节角度
10781	3697
14851	7773
肩关节变化	肘关节变化
4070	4076

■ 表格:move 10000前后的角度变化

肩关节角度	肘关节角度
14851	7773
2556	11889
肩关节变化	肘关节变化
4033	4116

对于肩关节角度：$A_{start} = 10781$，$A_{stop} = 2556$。考虑到其中的角度可能经过了16328（$2^{14}$），所以，实际角度变化范围：$\Delta A = 2^{14} - A_{start} + A_{stop} = 8159$

对于肘关节：$A_{start} = 3697$，$A_{stop} = 11889$。实际角度变化：$\Delta A = A_{stop} - A_{start} = 11889 - 3697 = 8192$

通过上述分析，可以看到两个角度基本上都是旋转了$2^{13} = 8192$，也就是半圆的角度。对应的移动角度为20000。
查看两个步进电机的设置，SW2,3,4,5=0000（ 42HS48EIS ），电机旋转一周为40000步。所以上述分析与电机的设置是相符合的。

• 42HS48EIS步进电机细化设置： SW2345=0001，对应周角旋转步数51200

使用命令move12移动20000步。

肩关节角度	肘关节角度
2625	11971
12617	5562
角度差	角度差
6392	6409

运动20000步，对应的角度为：

${{20000} \over {51200}} \times 360 = 140.625$

对应的14bit的角度为：
${{20000} \over {51200}} \times 2^{14} = 6400$

从上面的数据可以看到实际测量的角度与理论值之间相差$\pm 10$之内。

• 结论：

将42HS48EIS步进细分设置为：51200，转动与角度只比：
$\Delta Angle:\Delta Step = {{2^{14} } \over {51200}}$

5.试验故障

★ 在调试过程中，肘关节电机输出与机械部分滑脱了，现在已经无法带动大臂旋转。

上述的试验故障，最终通过白超的实验，验证应该数据步进电机的角度偏差范围超出范围决定的。这个故障可以通过步进电机上的指示灯闪烁的模式来确定。

闪烁5次停3秒：表示控制偏差超出范围。

 

03关节运动与角度

---

1.肘关节运动180°对应角度变化

肘关节逆时针运动180°对应的角度变化：

^{▲ 肘关节运动180°对应的角度变化}

^{▲ 肘关节运动}

2.肩关节运动180对应的角度变化

^{▲ 肩关节运动180°对应角度传感器变化值}

^{▲ 机械臂肩部 运动}

显示了对于肩关节角度传感器在运动范围之内有一个角度突变的过程。这是由于角度传感器量程具有有限的bit。

为了便于在程序里判断，对于肩关节的角度做如下的修正：

假设原来的肩关节的角度值：$S_{orig}$，那么修正之后的数值为：$S_{new} = \bmod \left( {S_{orig} + 6000,\,\,2^{14} } \right)$

对应的变化为：

^{▲ 肩关节角度修正之后的变化规律}

修正后的肩关节的变化就没有突变了。

#!/usr/local/bin/python
# -*- coding: gbk -*-
#============================================================
# TEST1.PY                     -- by Dr. ZhuoQing 2020-08-22
#
# Note:
#============================================================
from headm import *
from tsmodule.tsstm32       import *
angledim = []
for i in range(100):
    stm32cmd('CLEAR')
    stm32cmd('step1 -256')
    time.sleep(1)
    stm32cmd('angle')
    time.sleep(0.2)
    angle = stm32memo(1)
    angledim.append(angle[0])
    printff(i, angle[0])
tspsave('Angle1', angle=angledim)
printf('\a')
plt.plot(angledim)
plt.xlabel("Step")
plt.ylabel("Angle")
plt.grid(True)
plt.tight_layout()
plt.show()
#------------------------------------------------------------
#        END OF FILE : TEST1.PY
#============================================================

 

※ 结论

---

将基于STC8H1K28双轴机械臂驱动模块与双轴机械臂组装，进行了初步的调试。需要进行如下的改动：

1. 对于控制序号结合速度和位置来消除运动中的振荡；
2. 在肩关节在调试过程中输出与大臂之间滑脱。需要在机械结构上能够进一步避免这种情况的产生。

---

1. 控制模块AD工程文件:AD\XQWF\2020\机械臂\CNT2AXESSTC8H.PcbDoc ↩︎
2. 单片机C51控制程序:C51\STC\Project\XQWF\2020\STC8H2AXES\STC8H2AXES.uvproj ↩︎