代码收藏家 嵌入式硬件专题：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 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精度）优化参数。
作者：Ronin-Lotus
