ST意法半导体中文论坛机械臂手势跟踪以及声控系统设计2.1

机械臂手势跟踪以及声控系统设计2.1

[复制链接]

作者：Yude 2021-02-03 17:18:41 1 390

项目名称

项目主题

传感器

项目简介

项目图片

软硬件框图

演示视频

支持哔哩哔哩、优酷、腾讯视频。支持mp4、m4v格式

项目代码仓库

创客秀
项目名称:
主题:	传感器
项目简介:
项目图片:	-
项目图片描述:
软硬件框图:	-
软硬件框图描述:
演示视频:	-
项目代码仓库:

机械臂手势跟踪以及声控系统设计

by传媒学子

一、项目背景

现代传感技术的进步，催生机器人等行业进入新的发展时期，加之以ST为代表的MEMS传感器的技术革新，机器人行业将会迎来新的发展。

本项目结合机器人舵机控制，采用ST高性能MEMS传感器LSM6DSOX和LIS25BA骨传导传感器，实现对四轴机械臂的手势和声音(震动)控制。从而，为近端机械控制提供参考。本项目结合5G通信可以实现远程机械移动的精确控制；在本地端，借助wifi/蓝牙转串口技术可实现化学实验室密闭有毒环境下远程试验。

二、作品简介

本项目采用LSM6DSOX高性能传感器和LIS25BA高精度骨传导传感器，实现对四轴机械臂的控制。通过LSM6DSOX结合FMS和MLC可实现对机械臂的物品抓取和放下。而通过LIS25BA可实现对震动的识别从而控制舵机完成相应动作。由于LIS25BA需要结合麦克风才能实现真正意义的声音控制，这里仅将声音控制缩小到敲击产生的震动，以此在不影响项目意图的同时，简化整个项目的实施难度。

三、系统框图

3.1 通过LSM6DSOX传感器实现手势对机械臂的控制

项目分为两个部分，第一部分为LSM6DSOX实现手势控制，第二部分为LIS25BA实现声音震动控制。

图3.1 LSM6DSOX实现手势控制系统原理图

该部分主要利用FSM和MLC识别特定手势，来控制机械臂。数据采集利用MK190V3母版，而Unico软件实现FSM配置和MLC配置。FSM配置了3个状态机：FSM1/FMS2分别识别向右、向左手势，控制机械臂向左或者向右；MLC配合FSM3识别翻转手势，控制舵机完成既定动作（向下抓取物品或者丢掉物品）。母板配合uncio发送中断，中断传送给STM32F429，STM32F429通过I2C控制驱动pca8965实现对舵机的控制。

3.2 通过LIS25BA实现声控(震动控制)

图3.2 LIS25BA实现声音震动控制系统原理图

该部分主要利用利用MK190V3控制LIS25BA高精度骨传导传感器实现数据采集，上位机通过python实现对传感器的寄存器配置以及数据采集、算法实施和运动控制。

Python通过调用pyserial将配置数据写入LIS25BA，然后，通过特定算法，识别桌面震动，从而通过串口告知stm32f429,驱动I2C舵机驱动，从而实现特定动作。

四、各部分功能实现

本章节将详细介绍各模块的实现原理，按照系统原理图，本章节依然分为4.1和4.2两个大节来分别介绍手势控制和声音震动控制。

舵机控制模块，两种控制方式采用的是相同的控制方式。均采用STM32F429接收PC串口数据或者母板中断1和中断2来驱动舵机驱动模块，控制舵机。舵机驱动采用I2C驱动器PCA8965，采用HAL库进行开发，具体代码见附件，由于这部分不是项目重点，这里就不着重叙述。下面重点介绍如何利用ST MEMS传感器实现手势识别和声音震动识别。

4.1 手势控制系统 (LSM6DSOX)

LSM6DSOX内部的FSM和MLC能够大大解放外部处理器的压力，借助FSM和MLC可以实现复杂运动的检测。本项目手势控制系统采用3个状态机和MLC实现对3种不同手势运动的识别。以下测试传感器ODR=208Hz,FSM ODR=104Hz，仅开启加速度传感器。且测试时，手握USB端（USB端朝内），晃动母板。

4.1.1 FSM1实现向左手势识别

实现向左手势识别，则需要首先分析向左移动时传感器的数据变化,结合数据变化，对FSM进行配置，配置如图4.1.1.1所示，运动特征和中断输出如图4.1.1.2所示。当识别到向左手势，则发出中断1.

图4.1.1.1 向左划动状态机配置

图4.1.1.2 向左划动特征和对应中断输出

向左运动特征：X+轴数据先到达一个负极，然后接着有一个正极值；配置X+轴数据输出，当X+小于-1.5，则进入S1，S1检测数据是否大于1.5，然后设置T3=16个采样点；如果T3计时器时间中，X+轴数据没有大于1.5，则复位。通过调节T3定时器的数值，来调节识别间隔。

4.1.2 FSM2实现向右手势识别

向右手势和向左手势正好相反，因此，状态机比较类似，这里不再详述。当识别到向右手势，则发出中断2.

图4.1.2.1 向右划动状态机配置

图4.1.2.2 向右划动特征和对应中断输出

4.1.3 FSM3结合MLC实现翻转手势识别

FSM3检查MLC机器学习内核的数据；MLC配置为1个决策树，up->0, down->4. 机器学习内核，通过python生成对应的决策树，训练数据均由Unico软件获取。

图4.1.3.1 机器学习内核up输出

图4.1.3.2 机器学习内核down输出

FSM3结合Z轴数据变动以及定时器等指令，实现左向右翻转手势识别。FSM3对应INT1和INT2均输出，STM32F429检测到两个信号同时输出时，驱动舵机完成翻转手势对应的动作。

图4.1.3.3 左向右翻转手势对应的FSM配置

图4.1.3.4 左向右翻转手势对应的特征和中断输出

手势控制系统的中断与手势对应关系如下：

向左划动 - INT1

向右划动 - INT2

翻转手势 - INT1、INT2同时输出

后续，由STM32F429检测中断1和中断2的输出，驱动舵机，执行对应的手势动作。

4.2 声音震动识别 (LIS25BA）

骨传导传感器没有FSM和MLC，这两个功能是lsm6dox的功能。因此，要实现对LIS25BA的数据采集，必须结合UM2116中叙述的相关指令进行配置和数据获取。

4.2.1 程序流程图

图4.2.1.1 整体上位机流程图

母板一个串口，用于配置LIS25BA传感器和获取传感器数据；另一个串口连接STM32F429，然后根据震动检测的算法的最终输出来控制舵机执行相应的动作；算法实施由python实施。

4.2.2 寄存器配置和算法实施

LIS25BA传感器内部寄存器配置命令如下：

import serial
import time
import re
def print_hex(bytes):
l = [hex(int(i)) for i in bytes]
print(" ".join(l))
ser = serial.Serial('COM3',115200,timeout=1)
ser1 = serial.Serial('COM9',115200,timeout=1)
print(ser.name)
print(ser1.name)
ser.is_open
s=ser.write(b'*setdb211v1\r\n')
ser.write(b'*Zoff\r\n')
time.sleep(1)
#配置寄存器
ser.write(b'*w253F\r\n')
ser.write(b'*w2600\r\n')
ser.write(b'*w2E60\r\n')
ser.write(b'*w2FE0\r\n')
#ser.write(b'*single\r\n')
#enter debug mode to get acc sensor data
ser.write(b'*debug\r\n')
j=0
flag=0

每次读取1000个byte数据，进行分析，如果检测到大于2000的Z轴震动，则认为检测到震动。同时，由于震动附近波动数据较大，会让舵机反复执行不利于系统稳定，因此检测到一次震动后，退出当前1000个数据分析循环。主要算法代码如下：

while 1:
s=ser.read(1000)
s = s.decode('utf-8')
#print(re.search('\r\n', s, flags=0))
print(s)
a=s.split("\r\n");
print(a);
#去掉第一行，去掉最后一行，解析出Z轴数据
i=0
pre_value=0
for x in a:
i +=1;
if (i>1)and(i< (len(a)-1)):
#print(x)
x=x.split("AZ =",2)
#print(x)
#print(abs(int(x[1])))
num=abs(int(x[1]))
print(num)
if i==2:
pre_value=num
if i>2:
if abs(num-pre_value) >2000:
print("有敲击声")
if flag:
s=ser1.write(b'\x15')
flag=0
break #跳出for循环，防止触法多次
#time.sleep(1)
if flag==0:
s=ser1.write(b'\x25')
flag=1
break
#time.sleep(1)
pre_value=num
ser.close

得到的原始数据：

处理后的数据（仅保留Z轴绝对值）：

最终，通过串口2（对应代码中ser1）来输出舵机偏移值，实现对震动的检测。

五、作品源码和案例中处理的传感器数据

作品源码详见本帖附件。

5.1手势识别部分

FSM代码和传感器数据参照第四章节，下位机程序部分代码如下：

/* USER CODE BEGIN WHILE */
//uint8_t value_temp=1;
value=30;
flag=0;//默认机械臂处于down state
pca9685_set_channel_pwm_times(&handle, 2, 0, 4096*0.5*32/200);
pca9685_set_channel_pwm_times(&handle, 0, 0, 4096*0.5*value/200);
while (1)
{
/* USER CODE END WHILE */
if((HAL_GPIO_ReadPin(GPIOA, GPIO_PIN_3) == 1)&&(HAL_GPIO_ReadPin(GPIOA, GPIO_PIN_4) == 1))
{ //右舵机-1-20H；左边舵机20H down, up-35H
//down
if(flag==1){
pca9685_set_channel_pwm_times(&handle, 2, 0, 4096*0.5*32/200);//32
pca9685_set_channel_pwm_times(&handle, 3, 0, 4096*0.5*40/200);
delay_1ms(300);
pca9685_set_channel_pwm_times(&handle, 3, 0, 4096*0.5*32/200);
flag=0;
delay_1ms(200);
}
}
else{
//up
if(flag==0){
flag=1;
pca9685_set_channel_pwm_times(&handle, 2, 0, 4096*0.5*50/200);
pca9685_set_channel_pwm_times(&handle, 3, 0, 4096*0.5*32/200);
//delay_1ms(200);
}
else{
if(HAL_GPIO_ReadPin(GPIOA, GPIO_PIN_3) == 1)
{
pca9685_set_channel_pwm_times(&handle, 0, 0, 4096*0.5*value/200);
value-=1;
printf(" value:%d ",value);
delay_1ms(60);
}
if(HAL_GPIO_ReadPin(GPIOA, GPIO_PIN_4) == 1)
{
pca9685_set_channel_pwm_times(&handle, 0, 0, 4096*0.5*value/200);
value+=1;
printf(" value:%d ",value);
delay_1ms(60);
}
if(value<8)
{
value=53;
}
if(value>53)
{
value=8;
}
}
}
}

MLC原始训练数据和python生成决策树代码见结项贴附件，部分数据见下图：

图5.1.1 MLC机器内核训练原始数据

5.2 声音震动识别部分

上位机中python代码参照4.2.2. 下位机中主要代码与手势识别章节复用，如下：

/*
* claw，十六进制 20H=32闭合，28H=40开启
底盘舵机，0x08(8)->0° 0x1e(30)->90° 0x35(53)->180°
右边舵机，20H为竖直状，35H为水平状态；
左边舵机，当左边舵机为20H时，右边舵机必须大于等于20H，
*/
void HAL_UART_RxCpltCallback(UART_HandleTypeDef *UartHandle)
{
HAL_UART_Transmit(&huart1, (uint8_t *)&"\r\ninto HAL_UART_RxCpltCallback\r\n",32,0xffff); //验证进入这个函数了
HAL_UART_Transmit(&huart1,(uint8_t *)&value,1,0xffff);////把接收到的数据通过串口发送出去
HAL_UART_Receive_IT(&huart1,(uint8_t *)&value,1);////重新打开串口中断
//value 0x08~0x35 0.5ms -- 0; 1.0ms---45°；---- 2.5ms--180° 0x08->0° 0x1e(30)->90° 0x35(53)->180°
assert(value >= 0x08);
assert(value <= 0x35);
printf("\r\n%d %d+serv0 \r\n",value, pca9685_set_channel_pwm_times(&handle, 0, 0, 4096*0.5*value/200));
}