[X-NUCLEO-53L4A3 TOF评估板] 之手势识别
[复制链接]
[X-NUCLEO-53L4A3 TOF评估板] TouchGFX测距尺 - 传感器 - 电子工程世界-论坛
[X-NUCLEO-53L4A3 TOF评估板] 开箱体验以及常规测距 - 传感器 - 电子工程世界-论坛
【前言】
在通过传感获取与被测物体的距离后，通过获取多个坐标点，与时间组合成一组在二维平面上的一组坐标组合，通过最小二乘法拟合来判断这些数据点是否近似在一条直线上，再通过计算斜率，来判断物体的动运方向。这次就是通过这个原理来实现手挚识别的。
【数据的采集】
数据采集与存储 首先，我定义了两个数组，用于存放对应的距离与系统运行时间
double distanceData[DATA_POINTS]; //用于存放距离
double timeData[DATA_POINTS]; //用于存系统运行时间
再通过一个循环来采集，循环采集我们预设计的值：
status = VL53L4A3_RANGING_SENSOR_GetDistance(i, &Result);
if (status == BSP_ERROR_NONE){
distanceData[j] = Result.ZoneResult[0].Distance[0]; // 采集距离
timeData[j] = HAL_GetTick(); // 采集时间
HAL_Delay(20);
}
// 计算最小二乘法拟合直线的参数并判断是否近似直线
// 计算最小二乘法拟合直线的参数并判断是否近似直线
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[i];
sum_d += d[i];
sum_tt += t[i] * t[i];
sum_td += t[i] * d[i];
}
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[i] - (a * t[i] + b);
mse += diff * diff;
}
mse /= n;
// 设定一个均方误差阈值，根据实际情况调整
const double threshold = 3000;
printf("方向:%.2f\r\n",mse);
return mse;
}
通过计算是符合直线的值，我通过一个试验后，来确定其域值，来组合判断：
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[i];
sum_d += distanceData[i];
sum_tt += timeData[i] * timeData[i];
sum_td += timeData[i] * distanceData[i];
}
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");
}
【实验效果】
下载到开发板后，通过串口查看，我们可以准确识别到了三种状态，即停止，向前，向后。
【总结】
由于53L4A3只是单纯测距，目前还没有找到可以进行区域测距的API所，只能判断这三种状态。当然还可以通过计算斜率来得速度，进一步的做多种姿态的判断。当然还在获取数据以及计算还可以能过往BUFF中持续添加数据的数集采方法来判断更多的算法结果。
附完整算法文件：
/**
******************************************************************************
* @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[DATA_POINTS];
double timeData[DATA_POINTS];
int isApproximatelyLinear(double t[], double d[], int n) ;
/* Private variables ---------------------------------------------------------*/
static int32_t status = 0;
static uint8_t ToF_Present[RANGING_SENSOR_INSTANCES_NBR] = {0};
volatile uint8_t ToF_EventDetected = 0;
static const char *TofDevStr[] =
{
[VL53L4A3_DEV_CENTER] = "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[device]);
status = VL53L4A3_RANGING_SENSOR_Init(device);
if (status != BSP_ERROR_NONE)
{
printf("VL53L4A3_RANGING_SENSOR_Init %d failed\n", device);
ToF_Present[device] = 0; /* device not detected */
}
else
{
ToF_Present[device] = 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[device] == 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[device], 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[j] = Result.ZoneResult[0].Distance[0]; // 这里只是示例赋值，实际需要从传感器获取
timeData[j] = 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[0],
(long)Result->ZoneResult.Distance[0]);
}
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[i];
sum_d += d[i];
sum_tt += t[i] * t[i];
sum_td += t[i] * d[i];
}
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[i] - (a * t[i] + b);
mse += diff * diff;
}
mse /= n;
// 设定一个均方误差阈值，根据实际情况调整
const double threshold = 3000;
printf("方向:%.2f\r\n",mse);
return mse;
}
#ifdef __cplusplus
}
#endif
| 
提升卡
变色卡
千斤顶
1/10 
京公网安备 11010802033920号
Copyright © 2005-2024 EEWORLD.com.cn, Inc. All rights reserved
