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

---

---

前言

在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 PWMHAL_TIM_PWM_Init(&htim4);sConfigOC.Pulse = 0;             // 初始占空比0sConfigOC.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精度）优化参数。

---