[X-NUCLEO-53L4A3 TOF评估板] 之手势识别

lugl4313820 发表于 2024-11-16 08:17

<a href="https://bbs.eeworld.com.cn/thread-1298111-1-1.html"> TouchGFX测距尺 - 传感器 - 电子工程世界-论坛</a>

<a href="https://bbs.eeworld.com.cn/thread-1297238-1-1.html"> 开箱体验以及常规测距 - 传感器 - 电子工程世界-论坛</a>

【前言】

在通过传感获取与被测物体的距离后，通过获取多个坐标点，与时间组合成一组在二维平面上的一组坐标组合，通过最小二乘法拟合来判断这些数据点是否近似在一条直线上，再通过计算斜率，来判断物体的动运方向。这次就是通过这个原理来实现手挚识别的。

【数据的采集】

数据采集与存储首先，我定义了两个数组，用于存放对应的距离与系统运行时间

<pre>
<code>double distanceData; //用于存放距离
double timeData; //用于存系统运行时间</code></pre>

再通过一个循环来采集，循环采集我们预设计的值：

<pre>
<code> status = VL53L4A3_RANGING_SENSOR_GetDistance(i, &Result);
if (status == BSP_ERROR_NONE){
distanceData = Result.ZoneResult.Distance; // 采集距离
timeData = HAL_GetTick();// 采集时间
HAL_Delay(20);
}</code></pre>

// 计算最小二乘法拟合直线的参数并判断是否近似直线

<pre>
<code>// 计算最小二乘法拟合直线的参数并判断是否近似直线
int isApproximatelyLinear(double t[], double d[], int n) {
double sum_t = 0, sum_d = 0, sum_tt = 0, sum_td = 0;
for (int i = 0; i < n; i++) {
 sum_t += t;
 sum_d += d;
 sum_tt += t * t;
 sum_td += t * d;
}
double denominator = n * sum_tt - sum_t * sum_t;
if (denominator == 0) {
 return 0;
}
double a = (n * sum_td - sum_t * sum_d) / denominator;
double b = (sum_d - a * sum_t) / n;
double mse = 0;// 均方误差
for (int i = 0; i < n; i++) {
 double diff = d - (a * t + b);
 mse += diff * diff;
}
mse /= n;
// 设定一个均方误差阈值，根据实际情况调整
const double threshold = 3000;
printf("方向:%.2f\r\n",mse);
return mse;
}</code></pre>

通过计算是符合直线的值，我通过一个试验后，来确定其域值，来组合判断：

<pre>
<code> ret = isApproximatelyLinear(timeData, distanceData, DATA_POINTS);
if (ret <100)
{
printf("物体静止或近似静止。\n");
}
else if( ret >=100 && ret <5000) //如果直线的概率在可接受范围内，则得出是向前还是向后的运动方向
{
double sum_t = 0, sum_d = 0, sum_tt = 0, sum_td = 0;
for (int i = 0; i < DATA_POINTS; i++) {
sum_t += timeData;
sum_d += distanceData;
sum_tt += timeData * timeData;
sum_td += timeData * distanceData;
}
double denominator = DATA_POINTS * sum_tt - sum_t * sum_t;
double a = (DATA_POINTS * sum_td - sum_t * sum_d) / denominator;
if (a > 0) {
printf("物体向前运动。\n");
} else if (a < 0) {
printf("物体向后运动。\n");
} else {
printf("物体静止或近似静止。\n");
}
} else {
printf("数据点不近似在一条直线上，无法简单判断运动方向。\n");
}</code></pre>

【实验效果】

下载到开发板后，通过串口查看，我们可以准确识别到了三种状态，即停止，向前，向后。

  

【总结】

由于53L4A3只是单纯测距，目前还没有找到可以进行区域测距的API所，只能判断这三种状态。当然还可以通过计算斜率来得速度，进一步的做多种姿态的判断。当然还在获取数据以及计算还可以能过往BUFF中持续添加数据的数集采方法来判断更多的算法结果。

附完整算法文件：

<pre>
<code>/**
******************************************************************************
* @File : app_tof.c
* @author : IMG SW Application Team
* @brief : This file provides code for the configuration
* of the STMicroelectronics.X-CUBE-TOF1.3.4.2 instances.
******************************************************************************
*
* @attention *
* Copyright (c) 2023 STMicroelectronics.
* All rights reserved.
*
* This software is licensed under terms that can be found in the LICENSE file
* in the root directory of this software component.
* If no LICENSE file comes with this software, it is provided AS-IS.
*
******************************************************************************
*/

#ifdef __cplusplus
extern "C" {
#endif

/* Includes ------------------------------------------------------------------*/
#include "app_tof.h"
#include "main.h"
#include <stdio.h>

#include "53l4a3_ranging_sensor.h"
#include "app_tof_pin_conf.h"
#include "stm32f4xx_nucleo.h"

/* Private typedef -----------------------------------------------------------*/

/* Private define ------------------------------------------------------------*/
#define TIMING_BUDGET (30U) /* 10 ms < TimingBudget < 200 ms */
#define POLLING_PERIOD (250U) /* refresh rate for polling mode (ms, shall be consistent with TimingBudget value) */

#include <math.h>
#define DATA_POINTS 50

double distanceData;
double timeData;
int isApproximatelyLinear(double t[], double d[], int n) ;

/* Private variables ---------------------------------------------------------*/
static int32_t status = 0;
static uint8_t ToF_Present = {0};
volatile uint8_t ToF_EventDetected = 0;

static const char *TofDevStr[] =
{

= "CENTER",
};

/* Private function prototypes -----------------------------------------------*/
static void MX_53L4A3_MultiSensorRanging_Init(void);
static void MX_53L4A3_MultiSensorRanging_Process(void);
static void print_result(RANGING_SENSOR_Result_t *Result);
static void write_lowpower_pin(uint8_t device, GPIO_PinState pin_state);
static void reset_all_sensors(void);

static void reset_all_sensors(void);

void MX_TOF_Init(void)
{
/* USER CODE BEGIN SV */

/* USER CODE END SV */

/* USER CODE BEGIN TOF_Init_PreTreatment */

/* USER CODE END TOF_Init_PreTreatment */

/* Initialize the peripherals and the TOF components */

MX_53L4A3_MultiSensorRanging_Init();

/* USER CODE BEGIN TOF_Init_PostTreatment */

/* USER CODE END TOF_Init_PostTreatment */
}

/*
* LM background task
*/
void MX_TOF_Process(void)
{
/* USER CODE BEGIN TOF_Process_PreTreatment */

/* USER CODE END TOF_Process_PreTreatment */

MX_53L4A3_MultiSensorRanging_Process();

/* USER CODE BEGIN TOF_Process_PostTreatment */

/* USER CODE END TOF_Process_PostTreatment */
}

static void MX_53L4A3_MultiSensorRanging_Init(void)
{
uint8_t device;
uint16_t i2c_addr;
uint32_t id;

/* Initialize Virtual COM Port */
BSP_COM_Init(COM1);

printf("53L4A3 Multi Sensor Ranging demo application\n");

reset_all_sensors();

/* Turn off all the sensors */
for (device = 0; device < RANGING_SENSOR_INSTANCES_NBR; device++)
{
write_lowpower_pin(device, GPIO_PIN_RESET);
}

/* initializes each device and put it in low power mode */
for (device = 0; device < RANGING_SENSOR_INSTANCES_NBR; device++)
{
/* enable only one sensor */
write_lowpower_pin(device, GPIO_PIN_SET);
HAL_Delay(2);

printf("Initialize sensor %s\n", TofDevStr);
status = VL53L4A3_RANGING_SENSOR_Init(device);

if (status != BSP_ERROR_NONE)
{
 printf("VL53L4A3_RANGING_SENSOR_Init %d failed\n", device);
 ToF_Present = 0; /* device not detected */
}
else
{
 ToF_Present = 1; /* device detected */
}

write_lowpower_pin(device, GPIO_PIN_RESET); /* turn off the device */
}

/* power on the devices one at a time, initialize them and change their address.
* once the address is updated, the communication with the devices is checked
* reading its ID.
*/
for (device = 0; device < RANGING_SENSOR_INSTANCES_NBR; device++)
{
/* skip the sensor if init not successful */
if (ToF_Present == 0) { continue; }

/* turn on the device */
write_lowpower_pin(device, GPIO_PIN_SET);
HAL_Delay(2);

/* left: 0x54, center: 0x56, right: 0x58 */
i2c_addr = (RANGING_SENSOR_VL53L4ED_ADDRESS + (device + 1) * 2);
printf("Set sensor %s I2C address to 0X%x\n", TofDevStr, i2c_addr);
VL53L4A3_RANGING_SENSOR_SetAddress(device, i2c_addr);

/* check the communication with the device reading the ID */
VL53L4A3_RANGING_SENSOR_ReadID(device, &id);
printf("ToF sensor %d - ID: %04lX\n", device, (unsigned long)id);
}
}

static void MX_53L4A3_MultiSensorRanging_Process(void)
{
uint8_t i,j;
int ret;
RANGING_SENSOR_Result_t Result;
RANGING_SENSOR_ProfileConfig_t Profile;

Profile.RangingProfile = VL53L4ED_PROFILE_CONTINUOUS;
Profile.TimingBudget = TIMING_BUDGET;
Profile.Frequency = 0; /* Induces intermeasurement period, NOT USED for normal ranging */
Profile.EnableAmbient = 0; /* Enable: 1, Disable: 0 */
Profile.EnableSignal = 0; /* Enable: 1, Disable: 0 */

for (i = 0; i < RANGING_SENSOR_INSTANCES_NBR; i++)
{
/* skip this device if not detected */
if (ToF_Present != 1) { continue; }

VL53L4A3_RANGING_SENSOR_ConfigProfile(i, &Profile);
status = VL53L4A3_RANGING_SENSOR_Start(i, RS_MODE_BLOCKING_CONTINUOUS);

if (status != BSP_ERROR_NONE)
{
 printf("VL53L4A3_RANGING_SENSOR_Start failed\n");
 while (1);
}
}

while (1)
{
/* polling mode */
for (i = 0; i < RANGING_SENSOR_INSTANCES_NBR; i++)
{
 if (!ToF_Present) { continue; }
for(j = 0;j<DATA_POINTS; j++)
{
status = VL53L4A3_RANGING_SENSOR_GetDistance(i, &Result);
if (status == BSP_ERROR_NONE){
distanceData = Result.ZoneResult.Distance; // 这里只是示例赋值，实际需要从传感器获取
timeData = HAL_GetTick();// 同样是示例赋值
HAL_Delay(20);
}
}
ret = isApproximatelyLinear(timeData, distanceData, DATA_POINTS);
if (ret <100)
{
printf("物体静止或近似静止。\n");
}
else if( ret >=100 && ret <5000)
{
double sum_t = 0, sum_d = 0, sum_tt = 0, sum_td = 0;
for (int i = 0; i < DATA_POINTS; i++) {
sum_t += timeData;
sum_d += distanceData;
sum_tt += timeData * timeData;
sum_td += timeData * distanceData;
}
double denominator = DATA_POINTS * sum_tt - sum_t * sum_t;
double a = (DATA_POINTS * sum_td - sum_t * sum_d) / denominator;
if (a > 0) {
printf("物体向前运动。\n");
} else if (a < 0) {
printf("物体向后运动。\n");
} else {
printf("物体静止或近似静止。\n");
}
} else {
printf("数据点不近似在一条直线上，无法简单判断运动方向。\n");
}
}
printf("\n");
}
}

static void print_result(RANGING_SENSOR_Result_t *Result)
{
uint8_t i;

for (i = 0; i < RANGING_SENSOR_MAX_NB_ZONES; i++)
{
printf("Status = %2ld, Distance = %5ld mm\r\n",
 (long)Result->ZoneResult.Status,
 (long)Result->ZoneResult.Distance);
}
printf("\n");
}

static void write_lowpower_pin(uint8_t device, GPIO_PinState pin_state)
{
switch (device)
{
case VL53L4A3_DEV_CENTER:
 HAL_GPIO_WritePin(VL53L4A3_XSHUT_C_PORT, VL53L4A3_XSHUT_C_PIN, pin_state);
 break;

case VL53L4A3_DEV_LEFT:
 HAL_GPIO_WritePin(VL53L4A3_XSHUT_L_PORT, VL53L4A3_XSHUT_L_PIN, pin_state);
 break;

case VL53L4A3_DEV_RIGHT:
 HAL_GPIO_WritePin(VL53L4A3_XSHUT_R_PORT, VL53L4A3_XSHUT_R_PIN, pin_state);
 break;

default:
 break;
}
}

static void reset_all_sensors(void)
{
HAL_GPIO_WritePin(VL53L4A3_XSHUT_C_PORT, VL53L4A3_XSHUT_C_PIN, GPIO_PIN_RESET);
HAL_GPIO_WritePin(VL53L4A3_XSHUT_L_PORT, VL53L4A3_XSHUT_L_PIN, GPIO_PIN_RESET);
HAL_GPIO_WritePin(VL53L4A3_XSHUT_R_PORT, VL53L4A3_XSHUT_R_PIN, GPIO_PIN_RESET);
HAL_Delay(2);

HAL_GPIO_WritePin(VL53L4A3_XSHUT_C_PORT, VL53L4A3_XSHUT_C_PIN, GPIO_PIN_SET);
HAL_GPIO_WritePin(VL53L4A3_XSHUT_L_PORT, VL53L4A3_XSHUT_L_PIN, GPIO_PIN_SET);
HAL_GPIO_WritePin(VL53L4A3_XSHUT_R_PORT, VL53L4A3_XSHUT_R_PIN, GPIO_PIN_SET);
HAL_Delay(2);

}

// 计算最小二乘法拟合直线的参数并判断是否近似直线
int isApproximatelyLinear(double t[], double d[], int n) {
double sum_t = 0, sum_d = 0, sum_tt = 0, sum_td = 0;
for (int i = 0; i < n; i++) {
 sum_t += t;
 sum_d += d;
 sum_tt += t * t;
 sum_td += t * d;
}
double denominator = n * sum_tt - sum_t * sum_t;
if (denominator == 0) {
 return 0;
}
double a = (n * sum_td - sum_t * sum_d) / denominator;
double b = (sum_d - a * sum_t) / n;
double mse = 0;// 均方误差
for (int i = 0; i < n; i++) {
 double diff = d - (a * t + b);
 mse += diff * diff;
}
mse /= n;
// 设定一个均方误差阈值，根据实际情况调整
const double threshold = 3000;
printf("方向:%.2f\r\n",mse);
return mse;
}

#ifdef __cplusplus
}
#endif
</code></pre>

Jacktang 发表于 2024-11-16 09:24

只能判断这三种状态，也是一种办法，谢谢分享

不爱胡萝卜的仓鼠 发表于 2024-11-16 10:08

厉害啊，单点能实现这样的效果很不错了，如果可以有段展示视频就更好了。后续有机会可以玩玩VL53L5，8X8的点阵，应该可以识别更多的手势动作

HonestQiao 发表于 2024-11-16 14:49

牛牛牛，向大佬学习！！！

lugl4313820 发表于 2024-11-17 08:22

不爱胡萝卜的仓鼠发表于 2024-11-16 10:08
厉害啊，单点能实现这样的效果很不错了，如果可以有段展示视频就更好了。后续有机会可以玩玩VL53L5，8X8的 ...

后续拍个视频，事情有点多，忙不过来呀！感谢提醒！

lugl4313820 发表于 2024-11-17 08:23

HonestQiao 发表于 2024-11-16 14:49
牛牛牛，向大佬学习！！！

感谢帮主大佬的捧场，这里也有您对我帮助的功劳，再次感谢！

秦天qintian0303 发表于 2024-11-18 15:02

最小二乘法最实用的，这个对拟合参数有限制吗？多少个效率最高

lugl4313820 发表于 2024-11-18 16:58

秦天qintian0303 发表于 2024-11-18 15:02
最小二乘法最实用的，这个对拟合参数有限制吗？多少个效率最高

没有做这个列，只测出来可以用就行了，其实要做到项目上，还得需要做数组缓冲来弄，这只是能试出有这个功能而已。

页: [1]

电子工程世界-论坛's Archiver

[X-NUCLEO-53L4A3 TOF评估板] 之手势识别