嵌入式硬件篇---TOF｜PID

---

文章目录

• 前言
• [1. 硬件准备](#1. 硬件准备)
• • 主控芯片
  • ToF模块
  • • 1.VL53L0X
    • 2.TFmini
  • 执行机构：
  • • 电机
    • 舵机
    • 其他
• [2. 硬件连接](#2. 硬件连接)
• • [(1) VL53L0X（I²C接口）](#(1) VL53L0X（I²C接口）)
  • [(2) TFmini（串口通信）](#(2) TFmini（串口通信）)
• [3. ToF模块初始化与数据读取](#3. ToF模块初始化与数据读取)
• • [(1) VL53L0X（基于HAL库）](#(1) VL53L0X（基于HAL库）)
  • [(2) TFmini（串口接收）](#(2) TFmini（串口接收）)
• [4. PID算法实现](#4. PID算法实现)
• • [(1) PID结构体定义](#(1) PID结构体定义)
  • [(2) PID计算函数（带抗积分饱和）](#(2) PID计算函数（带抗积分饱和）)
• [5. 控制执行机构](#5. 控制执行机构)
• • [(1) 电机控制（PWM调速）](#(1) 电机控制（PWM调速）)
  • [(2) 舵机控制（角度调整）](#(2) 舵机控制（角度调整）)
• [6. 主循环逻辑](#6. 主循环逻辑)
• [7. 关键优化与问题处理](#7. 关键优化与问题处理)
• • [(1) ToF数据滤波](#(1) ToF数据滤波)
  • [(2) PID参数整定](#(2) PID参数整定)
  • • 阶跃响应法
    • 典型参数范围：
  • [(3) 动态目标适应](#(3) 动态目标适应)
• [8. 实际应用注意事项](#8. 实际应用注意事项)
• • ToF模块限制
  • 实时性
  • 机械延迟

---

前言

在STM32F103RCT6 上使用ToF（Time-of-Flight）模块（如VL53L0X、VL53L1X或TFmini ）结合PID算法 实现稳定距离控制，适用于高精度场景（如自动跟随、避障或工业定位）。以下是简单实现步骤：

---

1. 硬件准备

主控芯片

主控芯片：STM32F103RCT6（Cortex-M3，72MHz，足够处理ToF数据与PID运算）。

ToF模块

1.VL53L0X

VL53L0X：测距范围30cm~2m，精度±3mm，I²C接口。

2.TFmini

TFmini：串口通信，测距0.3m~12m，精度1%。

执行机构：

电机

电机（直流电机+编码器/PWM调速）。

舵机

舵机（用于方向调整，可选）。

其他

其他：电源、电机驱动（如TB6612）、OLED（显示距离，可选）。

2. 硬件连接

(1) VL53L0X（I²C接口）

VL53L0X引脚 STM32引脚 说明

VCC 3.3V 模块供电

GND GND 共地

SDA PB7 I²C数据线

SCL PB6 I²C时钟线

XSHUT PA8 复位引脚（可选）

(2) TFmini（串口通信）

TFmini引脚 STM32引脚 说明

VCC 5V 模块供电

GND GND 共地

TX PA10 接STM32的RX

RX PA9 接STM32的TX

3. ToF模块初始化与数据读取

(1) VL53L0X（基于HAL库）

#include "vl53l0x.h"
VL53L0X_Dev_t dev = {.i2c_handle = &hi2c1}; // I²C初始化略

void ToF_Init() {
  VL53L0X_Error status;
  status = VL53L0X_Init(&dev);
  if (status != VL53L0X_ERROR_NONE) {
    printf("ToF init failed!\n");
  }
  VL53L0X_StartMeasurement(&dev);
}

float Get_Distance() {
  VL53L0X_RangingMeasurementData_t data;
  VL53L0X_GetRangingMeasurementData(&dev, &data);
  return data.RangeMilliMeter / 10.0f; // 转换为cm
}

(2) TFmini（串口接收）

uint8_t tfmini_buffer[9];
float Get_Distance() {
  HAL_UART_Receive(&huart1, tfmini_buffer, 9, 100); // 接收9字节数据帧
  if (tfmini_buffer[0] == 0x59 && tfmini_buffer[1] == 0x59) { // 帧头校验
    uint16_t distance = tfmini_buffer[2] + (tfmini_buffer[3] << 8);
    return distance / 100.0f; // 转换为米
  }
  return -1; // 无效数据
}

4. PID算法实现

PID控制器通过调节输出使当前距离（反馈值）趋近目标距离（设定值）。

(1) PID结构体定义

typedef struct {
  float Kp, Ki, Kd;    // PID参数
  float target;        // 目标距离（单位与ToF一致）
  float error, last_error, integral;
  float output_max, output_min; // 输出限幅
} PID_Controller;

PID_Controller pid = {
  .Kp = 0.8, .Ki = 0.05, .Kd = 0.2,
  .target = 50.0,         // 目标距离50cm
  .output_max = 100, .output_min = -100
};

(2) PID计算函数（带抗积分饱和）

float PID_Update(PID_Controller *pid, float current) {
  pid->error = pid->target - current;

  // 积分项（抗饱和）
  pid->integral += pid->error;
  if (pid->integral > pid->output_max) pid->integral = pid->output_max;
  else if (pid->integral < pid->output_min) pid->integral = pid->output_min;

  // 微分项（抑制突变）
  float derivative = pid->error - pid->last_error;
  pid->last_error = pid->error;

  // PID输出（限幅）
  float output = pid->Kp * pid->error + 
                 pid->Ki * pid->integral + 
                 pid->Kd * derivative;
  if (output > pid->output_max) output = pid->output_max;
  else if (output < pid->output_min) output = pid->output_min;

  return output;
}

5. 控制执行机构

(1) 电机控制（PWM调速）

// 初始化PWM（TIM4通道1，PB6）
void PWM_Init() {
  TIM_OC_InitTypeDef sConfigOC = {0};
  htim4.Instance = TIM4;
  htim4.Init.Prescaler = 71;       // 1MHz频率
  htim4.Init.Period = 999;         // 1kHz PWM
  HAL_TIM_PWM_Init(&htim4);
  sConfigOC.Pulse = 0;             // 初始占空比0
  sConfigOC.OCMode = TIM_OCMODE_PWM1;
  HAL_TIM_PWM_ConfigChannel(&htim4, &sConfigOC, TIM_CHANNEL_1);
  HAL_TIM_PWM_Start(&htim4, TIM_CHANNEL_1);
}

// 设置电机速度和方向
void Set_Motor(float pid_output) {
  uint16_t pwm = (uint16_t)fabs(pid_output);
  __HAL_TIM_SET_COMPARE(&htim4, TIM_CHANNEL_1, pwm);

  // 方向控制（假设PB0为方向引脚）
  HAL_GPIO_WritePin(GPIOB, GPIO_PIN_0, pid_output > 0 ? GPIO_PIN_SET : GPIO_PIN_RESET);
}

(2) 舵机控制（角度调整）

// 设置舵机角度（PID输出映射到0~180°）
void Set_Servo(float pid_output) {
  uint16_t angle = 90 + (int16_t)pid_output; // 示例：PID输出±30对应60°~120°
  uint16_t pwm = 500 + angle * 2000 / 180;   // 0.5ms~2.5ms
  __HAL_TIM_SET_COMPARE(&htim2, TIM_CHANNEL_1, pwm);
}

6. 主循环逻辑

int main(void) {
  HAL_Init();
  SystemClock_Config();
  MX_GPIO_Init();
  MX_I2C1_Init();    // VL53L0X使用I²C
  // MX_USART1_UART_Init(); // TFmini使用串口
  PWM_Init();
  ToF_Init();

  float current_dist, pid_output;

  while (1) {
    current_dist = Get_Distance();          // 获取当前距离
    pid_output = PID_Update(&pid, current_dist);
    Set_Motor(pid_output);                // 控制电机
    // Set_Servo(pid_output);             // 或控制舵机

    HAL_Delay(20); // 控制周期20ms（50Hz）
  }
}

7. 关键优化与问题处理

(1) ToF数据滤波

移动平均滤波：
#define FILTER_SIZE 5
float filter_buffer[FILTER_SIZE];
float Filter_Distance(float new_value) {
  static uint8_t index = 0;
  filter_buffer[index++] = new_value;
  if (index >= FILTER_SIZE) index = 0;
  
  float sum = 0;
  for (uint8_t i = 0; i < FILTER_SIZE; i++) sum += filter_buffer[i];
  return sum / FILTER_SIZE;
}

(2) PID参数整定

阶跃响应法

设Ki=0, Kd=0，逐渐增大Kp直到系统振荡 ，然后取50%的值 。

加入Kd抑制超调，最后加Ki消除稳态误差。

典型参数范围：

Kp: 0.5~2.0（比例增益）

Ki: 0.01~0.1（积分时间）

Kd: 0.1~0.5（微分时间）

(3) 动态目标适应

若目标距离变化频繁，可加入动态参数调整：
if (fabs(pid.error) > 20) pid.Kp = 1.5; // 大误差时提高响应
else pid.Kp = 0.8;

8. 实际应用注意事项

ToF模块限制

ToF模块限制：VL53L0X在强光下性能下降 ，需避免直射阳光。

实时性

实时性：控制周期建议20~50ms ，过短可能导致PID震荡。

机械延迟

机械延迟：电机响应滞后时，需增加Kd或降低Ki 。

通过上述步骤，STM32F103RCT6可精确控制物体与ToF模块间的距离。实际调试时需结合硬件特性（如电机惯性、ToF精度）优化参数。

---