基于STM32的带云台智能小车图像识别系统
系统总览
1. 系统架构
┌─────────────────────────────────────────────────────┐
│ STM32云台智能小车图像识别系统 │
├─────────────────────────────────────────────────────┤
│ 应用层 │
│ ┌─────────┐ ┌─────────┐ ┌─────────┐ ┌─────────┐│
│ │ 目标追踪│ │ 颜色识别│ │ 人脸识别│ │ 手势控制││
│ └─────────┘ └─────────┘ └─────────┘ └─────────┘│
├─────────────────────────────────────────────────────┤
│ 控制层 │
│ ┌─────────┐ ┌─────────┐ ┌─────────┐ ┌─────────┐│
│ │ PID控制 │ │ 运动控制│ │ 云台控制│ │ 避障算法││
│ └─────────┘ └─────────┘ └─────────┘ └─────────┘│
├─────────────────────────────────────────────────────┤
│ 硬件层 │
│ ┌─────────┐ ┌─────────┐ ┌─────────┐ ┌─────────┐│
│ │ STM32F4│ │ OV2640 │ │ 云台舵机│ │ 电机驱动││
│ └─────────┘ └─────────┘ └─────────┘ └─────────┘│
└─────────────────────────────────────────────────────┘
2. 硬件配置
| 模块 | 型号 | 功能 |
|---|---|---|
| 主控芯片 | STM32F407VET6 | 168MHz, 512KB Flash, 192KB RAM |
| 摄像头 | OV2640 | 200万像素, JPEG输出 |
| 云台舵机 | MG996R ×2 | 2自由度云台控制 |
| 电机驱动 | TB6612FNG | 双H桥直流电机驱动 |
| 直流电机 | N20减速电机×4 | 四驱底盘 |
| 传感器 | MPU6050, HC-SR04 | 姿态和距离检测 |
| 无线模块 | ESP8266/HC-05 | WiFi/蓝牙通信 |
| 显示屏 | 1.44寸TFT LCD | 实时图像显示 |
快速开始
1. 环境搭建
# 安装开发工具
1. STM32CubeMX - 引脚配置和初始化代码生成
2. Keil MDK-ARM/STM32CubeIDE - 编译和调试
3. ST-Link Utility - 固件烧录
# 硬件连接
摄像头引脚:
SIOD -> PB8 (I2C1_SCL)
SIOC -> PB9 (I2C1_SDA)
VSYNC -> PA8
HREF -> PC9
PCLK -> PC8
D0-D7 -> PC0-PC7
云台舵机:
水平舵机 -> PA0 (TIM2_CH1)
垂直舵机 -> PA1 (TIM2_CH2)
电机驱动:
AIN1 -> PB0, AIN2 -> PB1, PWMA -> PB6
BIN1 -> PB2, BIN2 -> PB3, PWMB -> PB7
2. 代码实现
2.1 主控制程序 (main.c)
/**
* 基于STM32的云台智能小车主程序
* 功能: 图像识别、目标追踪、自主导航
*/
#include "main.h"
#include "camera.h"
#include "motor.h"
#include "gimbal.h"
#include "uart.h"
#include "lcd.h"
#include "ov2640.h"
#include "algorithm.h"
#include "FreeRTOS.h"
#include "task.h"
#include "queue.h"
#include "timers.h"
#include <stdio.h>
#include <string.h>
// 硬件外设句柄
UART_HandleTypeDef huart1;
DCMI_HandleTypeDef hdcmi;
DMA_HandleTypeDef hdma_dcmi;
I2C_HandleTypeDef hi2c1;
TIM_HandleTypeDef htim2;
TIM_HandleTypeDef htim3;
TIM_HandleTypeDef htim4;
// 全局变量
camera_image_t g_image_buffer[2];
volatile uint8_t g_image_ready = 0;
volatile uint8_t g_current_buffer = 0;
target_info_t g_target_info = {0};
car_status_t g_car_status = {0};
gimbal_control_t g_gimbal = {0};
uint8_t g_operation_mode = MODE_MANUAL;
uint8_t g_target_color = COLOR_RED;
// FreeRTOS任务句柄
TaskHandle_t xVisionTaskHandle = NULL;
TaskHandle_t xControlTaskHandle = NULL;
TaskHandle_t xMotorTaskHandle = NULL;
TaskHandle_t xGimbalTaskHandle = NULL;
TaskHandle_t xDisplayTaskHandle = NULL;
// 消息队列
QueueHandle_t xImageQueue = NULL;
QueueHandle_t xControlQueue = NULL;
QueueHandle_t xCommandQueue = NULL;
// 互斥锁
SemaphoreHandle_t xCameraMutex = NULL;
SemaphoreHandle_t xMotorMutex = NULL;
// 函数声明
void SystemClock_Config(void);
static void MX_GPIO_Init(void);
static void MX_DCMI_Init(void);
static void MX_I2C1_Init(void);
static void MX_TIM_Init(void);
static void MX_DMA_Init(void);
static void MX_USART1_UART_Init(void);
void Error_Handler(void);
void Vision_Task(void *pvParameters);
void Control_Task(void *pvParameters);
void Motor_Task(void *pvParameters);
void Gimbal_Task(void *pvParameters);
void Display_Task(void *pvParameters);
void DCMI_IRQHandler(void);
void DMA2_Stream1_IRQHandler(void);
void process_image(camera_image_t *img);
void tracking_algorithm(target_info_t *target);
void autonomous_navigation(void);
void manual_control(uint8_t command);
void update_status(void);
void send_status_to_pc(void);
void beep(uint8_t count, uint16_t duration);
int main(void) {
// HAL库初始化
HAL_Init();
// 配置系统时钟
SystemClock_Config();
// 初始化外设
MX_GPIO_Init();
MX_DMA_Init();
MX_DCMI_Init();
MX_I2C1_Init();
MX_TIM_Init();
MX_USART1_UART_Init();
// 初始化各模块
camera_init(&hdcmi, &hi2c1);
motor_init();
gimbal_init(&htim2);
lcd_init();
uart_init(&huart1, 115200);
// 创建FreeRTOS任务
xTaskCreate(Vision_Task, "Vision", 4096, NULL, 4, &xVisionTaskHandle);
xTaskCreate(Control_Task, "Control", 2048, NULL, 3, &xControlTaskHandle);
xTaskCreate(Motor_Task, "Motor", 1024, NULL, 2, &xMotorTaskHandle);
xTaskCreate(Gimbal_Task, "Gimbal", 1024, NULL, 2, &xGimbalTaskHandle);
xTaskCreate(Display_Task, "Display", 2048, NULL, 1, &xDisplayTaskHandle);
// 创建消息队列和互斥锁
xImageQueue = xQueueCreate(2, sizeof(camera_image_t*));
xControlQueue = xQueueCreate(10, sizeof(uint8_t));
xCommandQueue = xQueueCreate(10, sizeof(uint8_t));
xCameraMutex = xSemaphoreCreateMutex();
xMotorMutex = xSemaphoreCreateMutex();
// 启动摄像头捕获
camera_start_capture();
// 启动蜂鸣器提示
beep(2, 100);
printf("云台智能小车系统启动完成\r\n");
printf("固件版本: V1.0.0\r\n");
printf("运行模式: 手动控制\r\n");
// 启动调度器
vTaskStartScheduler();
// 如果调度器启动失败
while(1) {
Error_Handler();
}
}
// 视觉处理任务
void Vision_Task(void *pvParameters) {
camera_image_t *current_image = NULL;
while(1) {
// 等待图像就绪信号
if(xSemaphoreTake(xCameraMutex, portMAX_DELAY) == pdTRUE) {
if(g_image_ready) {
current_image = &g_image_buffer[g_current_buffer];
g_image_ready = 0;
// 处理图像
process_image(current_image);
// 将处理结果发送到控制任务
if(xQueueSend(xImageQueue, ¤t_image, 0) != pdPASS) {
printf("图像队列已满\r\n");
}
}
xSemaphoreGive(xCameraMutex);
}
vTaskDelay(20 / portTICK_PERIOD_MS); // 50Hz
}
}
// 控制决策任务
void Control_Task(void *pvParameters) {
camera_image_t *processed_image = NULL;
uint8_t command = 0;
while(1) {
// 检查是否有处理后的图像
if(xQueueReceive(xImageQueue, &processed_image, 0) == pdTRUE) {
// 根据运行模式执行不同控制策略
switch(g_operation_mode) {
case MODE_MANUAL:
// 手动模式，等待遥控指令
if(xQueueReceive(xCommandQueue, &command, 0) == pdTRUE) {
manual_control(command);
}
break;
case MODE_AUTO_TRACKING:
// 自动追踪模式
tracking_algorithm(&g_target_info);
break;
case MODE_AUTO_NAVIGATION:
// 自主导航模式
autonomous_navigation();
break;
case MODE_LINE_FOLLOWING:
// 巡线模式
line_following_algorithm(processed_image);
break;
}
}
// 更新系统状态
update_status();
vTaskDelay(10 / portTICK_PERIOD_MS); // 100Hz
}
}
// 电机控制任务
void Motor_Task(void *pvParameters) {
TickType_t xLastWakeTime = xTaskGetTickCount();
while(1) {
// 保护电机控制
if(xSemaphoreTake(xMotorMutex, portMAX_DELAY) == pdTRUE) {
// 执行速度控制
motor_speed_control();
// 执行转向控制
motor_steering_control();
xSemaphoreGive(xMotorMutex);
}
vTaskDelayUntil(&xLastWakeTime, 20 / portTICK_PERIOD_MS); // 50Hz
}
}
// 云台控制任务
void Gimbal_Task(void *pvParameters) {
while(1) {
// 平滑移动云台到目标位置
gimbal_smooth_move(g_gimbal.pan_angle, g_gimbal.tilt_angle);
// 自动扫描模式
if(g_operation_mode == MODE_AUTO_SCAN) {
gimbal_auto_scan();
}
vTaskDelay(10 / portTICK_PERIOD_MS); // 100Hz
}
}
// 显示任务
void Display_Task(void *pvParameters) {
static uint32_t last_update = 0;
uint32_t current_time = 0;
while(1) {
current_time = HAL_GetTick();
if(current_time - last_update >= 100) { // 10Hz更新
last_update = current_time;
// 更新LCD显示
lcd_update_display(&g_target_info, &g_car_status, &g_gimbal);
// 发送状态到上位机
send_status_to_pc();
}
vTaskDelay(50 / portTICK_PERIOD_MS);
}
}
// 图像处理函数
void process_image(camera_image_t *img) {
if(img == NULL) return;
// 颜色识别
if(g_target_color != COLOR_NONE) {
color_detection(img, g_target_color, &g_target_info);
}
// 形状识别
shape_detection(img, &g_target_info);
// 人脸检测
if(g_operation_mode == MODE_FACE_TRACKING) {
face_detection(img, &g_target_info);
}
// 目标跟踪
if(g_target_info.detected) {
// 在图像上绘制目标框
draw_target_box(img, &g_target_info);
// 更新云台目标位置
int16_t pan_error = g_target_info.x - (IMAGE_WIDTH / 2);
int16_t tilt_error = g_target_info.y - (IMAGE_HEIGHT / 2);
// PID控制云台
float pan_output = pid_calculate(&g_gimbal.pan_pid, 0, pan_error, 0.1);
float tilt_output = pid_calculate(&g_gimbal.tilt_pid, 0, tilt_error, 0.1);
g_gimbal.pan_angle -= pan_output;
g_gimbal.tilt_angle -= tilt_output;
// 限制角度范围
g_gimbal.pan_angle = constrain(g_gimbal.pan_angle, -90, 90);
g_gimbal.tilt_angle = constrain(g_gimbal.tilt_angle, -45, 45);
}
}
// 目标追踪算法
void tracking_algorithm(target_info_t *target) {
if(target->detected) {
// 计算目标在图像中的位置
int16_t x_error = target->x - (IMAGE_WIDTH / 2);
int16_t y_error = target->y - (IMAGE_HEIGHT / 2);
// 控制小车移动
if(abs(x_error) < 20) {
// 目标在中心，前进
motor_move(FORWARD, 30);
} else if(x_error < 0) {
// 目标在左侧，左转
motor_turn(LEFT, 20);
} else {
// 目标在右侧，右转
motor_turn(RIGHT, 20);
}
// 如果目标太小，靠近
if(target->area < 1000) {
motor_move(FORWARD, 20);
}
// 如果目标太大，后退
else if(target->area > 5000) {
motor_move(BACKWARD, 20);
}
} else {
// 未检测到目标，停止
motor_stop();
// 开启云台扫描寻找目标
g_operation_mode = MODE_AUTO_SCAN;
}
}
// 自主导航
void autonomous_navigation(void) {
static uint8_t state = STATE_FORWARD;
float distance = 0;
// 获取前方距离
distance = ultrasonic_get_distance();
switch(state) {
case STATE_FORWARD:
if(distance > 30.0) { // 30cm内无障碍
motor_move(FORWARD, 40);
// 检查是否检测到目标
if(g_target_info.detected) {
state = STATE_TRACKING;
}
} else {
motor_stop();
state = STATE_AVOID;
}
break;
case STATE_AVOID:
motor_turn(LEFT, 30);
HAL_Delay(500);
// 重新检测距离
distance = ultrasonic_get_distance();
if(distance > 30.0) {
state = STATE_FORWARD;
} else {
motor_turn(RIGHT, 30);
HAL_Delay(1000);
}
break;
case STATE_TRACKING:
tracking_algorithm(&g_target_info);
break;
}
}
// 手动控制
void manual_control(uint8_t command) {
switch(command) {
case CMD_FORWARD:
motor_move(FORWARD, 50);
break;
case CMD_BACKWARD:
motor_move(BACKWARD, 50);
break;
case CMD_LEFT:
motor_turn(LEFT, 30);
break;
case CMD_RIGHT:
motor_turn(RIGHT, 30);
break;
case CMD_STOP:
motor_stop();
break;
case CMD_PAN_LEFT:
g_gimbal.pan_angle += 10;
break;
case CMD_PAN_RIGHT:
g_gimbal.pan_angle -= 10;
break;
case CMD_TILT_UP:
g_gimbal.tilt_angle += 10;
break;
case CMD_TILT_DOWN:
g_gimbal.tilt_angle -= 10;
break;
}
}
// 更新状态
void update_status(void) {
static uint32_t last_update = 0;
uint32_t current_time = HAL_GetTick();
if(current_time - last_update >= 1000) { // 1秒更新一次
last_update = current_time;
g_car_status.running_time++;
g_car_status.battery_level = get_battery_level();
g_car_status.distance_traveled += g_car_status.speed * 0.277; // 转换为米
}
}
// 发送状态到PC
void send_status_to_pc(void) {
static uint8_t buffer[64];
int len = 0;
len = sprintf((char*)buffer,
"STAT:%.1f,%.1f,%d,%d,%d,%d,%d\r\n",
g_car_status.speed,
g_car_status.distance_traveled,
g_car_status.battery_level,
g_target_info.detected,
g_gimbal.pan_angle,
g_gimbal.tilt_angle,
g_operation_mode);
HAL_UART_Transmit(&huart1, buffer, len, 100);
}
// 蜂鸣器提示
void beep(uint8_t count, uint16_t duration) {
for(uint8_t i = 0; i < count; i++) {
HAL_GPIO_WritePin(GPIOB, GPIO_PIN_8, GPIO_PIN_SET);
HAL_Delay(duration);
HAL_GPIO_WritePin(GPIOB, GPIO_PIN_8, GPIO_PIN_RESET);
if(i < count - 1) HAL_Delay(duration);
}
}
2.2 摄像头驱动模块 (camera.c)
/**
* 摄像头驱动模块
* 支持OV2640传感器
*/
#include "camera.h"
#include "ov2640.h"
#include <string.h>
DCMI_HandleTypeDef hdcmi;
I2C_HandleTypeDef hi2c1;
DMA_HandleTypeDef hdma_dcmi;
// 图像缓冲区
camera_image_t image_buffer[2];
volatile uint8_t image_ready = 0;
volatile uint8_t current_buffer = 0;
volatile uint32_t frame_count = 0;
// 摄像头初始化
uint8_t camera_init(DCMI_HandleTypeDef *hdcmi_ptr, I2C_HandleTypeDef *hi2c_ptr) {
// 保存句柄
hdcmi = *hdcmi_ptr;
hi2c1 = *hi2c_ptr;
// 初始化OV2640
if(ov2640_init(&hi2c1) != 0) {
printf("OV2640初始化失败\r\n");
return 0;
}
// 配置摄像头参数
ov2640_set_format(JPEG);
ov2640_set_framesize(QVGA); // 320x240
ov2640_set_quality(12);
ov2640_set_light_mode(AUTO);
ov2640_set_color_saturation(2);
ov2640_set_brightness(3);
ov2640_set_contrast(2);
// 初始化DCMI DMA
__HAL_RCC_DMA2_CLK_ENABLE();
hdma_dcmi.Instance = DMA2_Stream1;
hdma_dcmi.Init.Channel = DMA_CHANNEL_1;
hdma_dcmi.Init.Direction = DMA_PERIPH_TO_MEMORY;
hdma_dcmi.Init.PeriphInc = DMA_PINC_DISABLE;
hdma_dcmi.Init.MemInc = DMA_MINC_ENABLE;
hdma_dcmi.Init.PeriphDataAlignment = DMA_PDATAALIGN_WORD;
hdma_dcmi.Init.MemDataAlignment = DMA_MDATAALIGN_WORD;
hdma_dcmi.Init.Mode = DMA_CIRCULAR;
hdma_dcmi.Init.Priority = DMA_PRIORITY_HIGH;
hdma_dcmi.Init.FIFOMode = DMA_FIFOMODE_ENABLE;
hdma_dcmi.Init.FIFOThreshold = DMA_FIFO_THRESHOLD_FULL;
hdma_dcmi.Init.MemBurst = DMA_MBURST_SINGLE;
hdma_dcmi.Init.PeriphBurst = DMA_PBURST_SINGLE;
HAL_DMA_Init(&hdma_dcmi);
// 关联DCMI和DMA
__HAL_LINKDMA(&hdcmi, DMA_Handle, hdma_dcmi);
// 使能DCMI中断
HAL_NVIC_SetPriority(DCMI_IRQn, 5, 0);
HAL_NVIC_EnableIRQ(DCMI_IRQn);
// 使能DMA中断
HAL_NVIC_SetPriority(DMA2_Stream1_IRQn, 5, 0);
HAL_NVIC_EnableIRQ(DMA2_Stream1_IRQn);
printf("摄像头初始化完成\r\n");
return 1;
}
// 开始捕获
void camera_start_capture(void) {
// 清除缓冲区
memset(image_buffer, 0, sizeof(image_buffer));
// 启动DCMI DMA传输
HAL_DCMI_Start_DMA(&hdcmi, DCMI_MODE_SNAPSHOT,
(uint32_t)image_buffer[0].data,
IMAGE_BUFFER_SIZE / 4);
printf("开始图像捕获\r\n");
}
// 停止捕获
void camera_stop_capture(void) {
HAL_DCMI_Stop(&hdcmi);
printf("停止图像捕获\r\n");
}
// DCMI帧中断回调
void HAL_DCMI_FrameEventCallback(DCMI_HandleTypeDef *hdcmi) {
// 切换缓冲区
current_buffer ^= 1;
frame_count++;
// 标记图像就绪
image_ready = 1;
// 重新启动DMA传输到新缓冲区
HAL_DCMI_Start_DMA(&hdcmi, DCMI_MODE_SNAPSHOT,
(uint32_t)image_buffer[current_buffer].data,
IMAGE_BUFFER_SIZE / 4);
}
// DCMI错误回调
void HAL_DCMI_ErrorCallback(DCMI_HandleTypeDef *hdcmi) {
printf("DCMI错误发生\r\n");
// 重启摄像头
camera_stop_capture();
HAL_Delay(100);
camera_start_capture();
}
// 获取当前图像
camera_image_t* camera_get_image(void) {
if(image_ready) {
image_ready = 0;
return &image_buffer[current_buffer ^ 1]; // 返回已完成的缓冲区
}
return NULL;
}
// 获取帧率
uint32_t camera_get_fps(void) {
static uint32_t last_count = 0;
static uint32_t last_time = 0;
uint32_t current_time = HAL_GetTick();
uint32_t fps = 0;
if(current_time - last_time >= 1000) {
fps = (frame_count - last_count) * 1000 / (current_time - last_time);
last_count = frame_count;
last_time = current_time;
}
return fps;
}
2.3 云台控制模块 (gimbal.c)
/**
* 云台控制模块
* 2自由度云台，水平+垂直控制
*/
#include "gimbal.h"
#include "pid.h"
#include <math.h>
// 全局变量
gimbal_control_t g_gimbal = {0};
static TIM_HandleTypeDef *g_htim = NULL;
// PID控制器
PID_HandleTypeDef pan_pid, tilt_pid;
// 云台初始化
void gimbal_init(TIM_HandleTypeDef *htim) {
g_htim = htim;
// 初始化PID控制器
PID_Init(&pan_pid, 1.0, 0.1, 0.5, 1000);
PID_Init(&tilt_pid, 1.0, 0.1, 0.5, 1000);
// 设置云台初始位置
g_gimbal.pan_angle = 0;
g_gimbal.tilt_angle = 0;
g_gimbal.pan_offset = 0;
g_gimbal.tilt_offset = 0;
g_gimbal.speed = 5; // 度/秒
// 设置舵机初始位置
gimbal_set_position(0, 0);
printf("云台初始化完成\r\n");
}
// 设置云台位置
void gimbal_set_position(int16_t pan_angle, int16_t tilt_angle) {
// 限制角度范围
pan_angle = constrain(pan_angle, PAN_MIN_ANGLE, PAN_MAX_ANGLE);
tilt_angle = constrain(tilt_angle, TILT_MIN_ANGLE, TILT_MAX_ANGLE);
// 计算PWM脉宽
uint16_t pan_pulse = angle_to_pulse(pan_angle + g_gimbal.pan_offset);
uint16_t tilt_pulse = angle_to_pulse(tilt_angle + g_gimbal.tilt_offset);
// 设置PWM
__HAL_TIM_SET_COMPARE(g_htim, TIM_CHANNEL_1, pan_pulse);
__HAL_TIM_SET_COMPARE(g_htim, TIM_CHANNEL_2, tilt_pulse);
// 更新当前角度
g_gimbal.pan_angle = pan_angle;
g_gimbal.tilt_angle = tilt_angle;
printf("云台位置: Pan=%d, Tilt=%d\r\n", pan_angle, tilt_angle);
}
// 平滑移动云台
void gimbal_smooth_move(int16_t target_pan, int16_t target_tilt) {
static int16_t current_pan = 0, current_tilt = 0;
static uint8_t is_moving = 0;
// 限制目标角度
target_pan = constrain(target_pan, PAN_MIN_ANGLE, PAN_MAX_ANGLE);
target_tilt = constrain(target_tilt, TILT_MIN_ANGLE, TILT_MAX_ANGLE);
// 计算当前位置
int16_t actual_pan = g_gimbal.pan_angle;
int16_t actual_tilt = g_gimbal.tilt_angle;
// 检查是否到达目标位置
if(abs(actual_pan - target_pan) <= 1 && abs(actual_tilt - target_tilt) <= 1) {
if(is_moving) {
is_moving = 0;
}
return;
}
is_moving = 1;
// 计算移动步长
int16_t pan_step = (target_pan > actual_pan) ? g_gimbal.speed : -g_gimbal.speed;
int16_t tilt_step = (target_tilt > actual_tilt) ? g_gimbal.speed : -g_gimbal_speed;
// 如果距离小于步长，直接到达目标
if(abs(target_pan - actual_pan) <= g_gimbal.speed) {
current_pan = target_pan;
} else {
current_pan = actual_pan + pan_step;
}
if(abs(target_tilt - actual_tilt) <= g_gimbal.speed) {
current_tilt = target_tilt;
} else {
current_tilt = actual_tilt + tilt_step;
}
// 设置新位置
gimbal_set_position(current_pan, current_tilt);
}
// 云台自动扫描
void gimbal_auto_scan(void) {
static uint8_t scan_direction = 0; // 0:右转, 1:左转
static uint8_t scan_row = 0;
if(scan_direction == 0) {
// 向右扫描
g_gimbal.pan_angle += 5;
if(g_gimbal.pan_angle >= PAN_MAX_ANGLE) {
scan_direction = 1;
g_gimbal.tilt_angle += 10;
}
} else {
// 向左扫描
g_gimbal.pan_angle -= 5;
if(g_gimbal.pan_angle <= PAN_MIN_ANGLE) {
scan_direction = 0;
g_gimbal.tilt_angle += 10;
}
}
// 检查垂直范围
if(g_gimbal.tilt_angle > TILT_MAX_ANGLE) {
g_gimbal.tilt_angle = TILT_MIN_ANGLE;
}
gimbal_set_position(g_gimbal.pan_angle, g_gimbal.tilt_angle);
}
// 角度转PWM脉宽
uint16_t angle_to_pulse(int16_t angle) {
// 角度范围: -90° ~ +90°
// 对应脉宽: 500us ~ 2500us
uint16_t pulse = SERVO_MID_PULSE + (angle * 2000 / 180);
// 限制脉宽范围
if(pulse < SERVO_MIN_PULSE) pulse = SERVO_MIN_PULSE;
if(pulse > SERVO_MAX_PULSE) pulse = SERVO_MAX_PULSE;
return pulse;
}
// 目标追踪控制
void gimbal_target_tracking(int16_t target_x, int16_t target_y) {
// 计算目标在图像中的偏差
float pan_error = target_x - (IMAGE_WIDTH / 2);
float tilt_error = target_y - (IMAGE_HEIGHT / 2);
// PID控制
float pan_output = PID_Calculate(&pan_pid, 0, pan_error, 0.1);
float tilt_output = PID_Calculate(&tilt_pid, 0, tilt_error, 0.1);
// 转换为角度
int16_t pan_angle = g_gimbal.pan_angle - pan_output;
int16_t tilt_angle = g_gimbal.tilt_angle - tilt_output;
// 平滑移动到目标位置
gimbal_smooth_move(pan_angle, tilt_angle);
}
// 云台复位
void gimbal_reset(void) {
gimbal_smooth_move(0, 0);
printf("云台复位完成\r\n");
}
2.4 图像识别算法 (algorithm.c)
/**
* 图像识别算法模块
* 包含颜色识别、形状识别、人脸检测等算法
*/
#include "algorithm.h"
#include "camera.h"
#include <math.h>
#include <string.h>
// 颜色阈值表
const color_threshold_t color_thresholds[] = {
// 红色 (HSV)
{COLOR_RED, 0, 10, 100, 255, 50, 255, "Red"},
{COLOR_RED, 160, 180, 100, 255, 50, 255, "Red"},
// 绿色
{COLOR_GREEN, 35, 85, 50, 255, 50, 255, "Green"},
// 蓝色
{COLOR_BLUE, 100, 130, 50, 255, 50, 255, "Blue"},
// 黄色
{COLOR_YELLOW, 20, 35, 100, 255, 50, 255, "Yellow"},
// 结束标记
{0, 0, 0, 0, 0, 0, 0, ""}
};
// 颜色识别
uint8_t color_detection(camera_image_t *img, uint8_t target_color, target_info_t *result) {
uint32_t x_sum = 0, y_sum = 0;
uint32_t pixel_count = 0;
uint16_t min_x = IMAGE_WIDTH, max_x = 0;
uint16_t min_y = IMAGE_HEIGHT, max_y = 0;
// 转换为RGB565处理
uint16_t *rgb_data = (uint16_t*)img->data;
for(uint16_t y = 0; y < IMAGE_HEIGHT; y += 2) { // 跳采样提高速度
for(uint16_t x = 0; x < IMAGE_WIDTH; x += 2) {
uint16_t pixel = rgb_data[y * IMAGE_WIDTH + x];
// 提取RGB分量
uint8_t r = ((pixel >> 11) & 0x1F) << 3;
uint8_t g = ((pixel >> 5) & 0x3F) << 2;
uint8_t b = (pixel & 0x1F) << 3;
// 转换为HSV
float h, s, v;
rgb_to_hsv(r, g, b, &h, &s, &v);
// 检查是否符合目标颜色阈值
if(is_color_in_range(h, s, v, target_color)) {
x_sum += x;
y_sum += y;
pixel_count++;
if(x < min_x) min_x = x;
if(x > max_x) max_x = x;
if(y < min_y) min_y = y;
if(y > max_y) max_y = y;
}
}
}
if(pixel_count > MIN_PIXEL_COUNT) {
result->detected = 1;
result->x = x_sum / pixel_count;
result->y = y_sum / pixel_count;
result->width = max_x - min_x;
result->height = max_y - min_y;
result->area = pixel_count;
result->color = target_color;
return 1;
}
result->detected = 0;
return 0;
}
// 形状识别
uint8_t shape_detection(camera_image_t *img, target_info_t *result) {
// 边缘检测
uint8_t edges[IMAGE_HEIGHT][IMAGE_WIDTH] = {0};
edge_detection_sobel(img, edges);
// 查找轮廓
contour_t contours[20];
uint8_t contour_count = find_contours(edges, contours, 20);
if(contour_count == 0) {
return 0;
}
// 分析轮廓形状
for(uint8_t i = 0; i < contour_count; i++) {
if(contours[i].point_count < 20) continue; // 忽略太小轮廓
// 计算轮廓特征
float circularity = calculate_circularity(&contours[i]);
float rectangularity = calculate_rectangularity(&contours[i]);
// 判断形状
if(circularity > 0.85) {
result->shape = SHAPE_CIRCLE;
} else if(rectangularity > 0.85) {
result->shape = SHAPE_SQUARE;
} else if(contours[i].point_count == 3) {
result->shape = SHAPE_TRIANGLE;
} else {
result->shape = SHAPE_UNKNOWN;
}
result->detected = 1;
result->x = contours[i].center_x;
result->y = contours[i].center_y;
result->width = contours[i].width;
result->height = contours[i].height;
result->area = contours[i].area;
return 1;
}
return 0;
}
// 边缘检测 (Sobel算子)
void edge_detection_sobel(camera_image_t *img, uint8_t edges[][IMAGE_WIDTH]) {
int16_t gx, gy;
int16_t magnitude;
for(uint16_t y = 1; y < IMAGE_HEIGHT - 1; y++) {
for(uint16_t x = 1; x < IMAGE_WIDTH - 1; x++) {
// Sobel算子
gx = -1 * get_pixel_brightness(img, x-1, y-1) +
0 * get_pixel_brightness(img, x, y-1) +
1 * get_pixel_brightness(img, x+1, y-1) +
-2 * get_pixel_brightness(img, x-1, y) +
0 * get_pixel_brightness(img, x, y) +
2 * get_pixel_brightness(img, x+1, y) +
-1 * get_pixel_brightness(img, x-1, y+1) +
0 * get_pixel_brightness(img, x, y+1) +
1 * get_pixel_brightness(img, x+1, y+1);
gy = -1 * get_pixel_brightness(img, x-1, y-1) +
-2 * get_pixel_brightness(img, x, y-1) +
-1 * get_pixel_brightness(img, x+1, y-1) +
0 * get_pixel_brightness(img, x-1, y) +
0 * get_pixel_brightness(img, x, y) +
0 * get_pixel_brightness(img, x+1, y) +
1 * get_pixel_brightness(img, x-1, y+1) +
2 * get_pixel_brightness(img, x, y+1) +
1 * get_pixel_brightness(img, x+1, y+1);
magnitude = (int16_t)sqrt(gx*gx + gy*gy);
// 二值化
edges[y][x] = (magnitude > EDGE_THRESHOLD) ? 255 : 0;
}
}
}
// RGB转HSV
void rgb_to_hsv(uint8_t r, uint8_t g, uint8_t b, float *h, float *s, float *v) {
float rd = r / 255.0f;
float gd = g / 255.0f;
float bd = b / 255.0f;
float max = fmaxf(rd, fmaxf(gd, bd));
float min = fminf(rd, fminf(gd, bd));
float delta = max - min;
*v = max;
if(max > 0.0f) {
*s = delta / max;
} else {
*s = 0.0f;
*h = 0.0f;
return;
}
if(delta == 0.0f) {
*h = 0.0f;
} else if(max == rd) {
*h = 60.0f * fmodf((gd - bd) / delta, 6.0f);
} else if(max == gd) {
*h = 60.0f * (((bd - rd) / delta) + 2.0f);
} else {
*h = 60.0f * (((rd - gd) / delta) + 4.0f);
}
if(*h < 0.0f) {
*h += 360.0f;
}
}
// 检查颜色是否在范围内
uint8_t is_color_in_range(float h, float s, float v, uint8_t target_color) {
const color_threshold_t *threshold = color_thresholds;
while(threshold->color != 0) {
if(threshold->color == target_color) {
if(h >= threshold->h_min && h <= threshold->h_max &&
s >= threshold->s_min && s <= threshold->s_max &&
v >= threshold->v_min && v <= threshold->v_max) {
return 1;
}
}
threshold++;
}
return 0;
}
// 绘制目标框
void draw_target_box(camera_image_t *img, target_info_t *target) {
uint16_t color = 0xF800; // 红色
// 绘制矩形框
for(uint16_t x = target->x - target->width/2; x <= target->x + target->width/2; x++) {
if(x >= 0 && x < IMAGE_WIDTH) {
draw_pixel(img, x, target->y - target->height/2, color);
draw_pixel(img, x, target->y + target->height/2, color);
}
}
for(uint16_t y = target->y - target->height/2; y <= target->y + target->height/2; y++) {
if(y >= 0 && y < IMAGE_HEIGHT) {
draw_pixel(img, target->x - target->width/2, y, color);
draw_pixel(img, target->x + target->width/2, y, color);
}
}
// 绘制中心点
for(int16_t dx = -2; dx <= 2; dx++) {
for(int16_t dy = -2; dy <= 2; dy++) {
uint16_t x = target->x + dx;
uint16_t y = target->y + dy;
if(x >= 0 && x < IMAGE_WIDTH && y >= 0 && y < IMAGE_HEIGHT) {
draw_pixel(img, x, y, 0xFFE0); // 黄色
}
}
}
}
2.5 电机控制模块 (motor.c)
/**
* 电机控制模块
* 四驱小车，TB6612驱动
*/
#include "motor.h"
#include "pid.h"
#include <math.h>
// 电机控制结构
motor_control_t motors[4];
PID_HandleTypeDef speed_pid[4];
// 电机初始化
void motor_init(void) {
// 初始化4个电机
for(uint8_t i = 0; i < 4; i++) {
motors[i].current_speed = 0;
motors[i].target_speed = 0;
motors[i].direction = STOP;
motors[i].acceleration = 5; // 5%每周期
motors[i].enabled = 1;
// 初始化速度PID
PID_Init(&speed_pid[i], 1.0, 0.05, 0.1, 1000);
}
// 设置电机引脚
motors[0].pwm_channel = TIM_CHANNEL_1;
motors[0].in1_port = GPIOA;
motors[0].in1_pin = GPIO_PIN_0;
motors[0].in2_port = GPIOA;
motors[0].in2_pin = GPIO_PIN_1;
motors[1].pwm_channel = TIM_CHANNEL_2;
motors[1].in1_port = GPIOA;
motors[1].in1_pin = GPIO_PIN_2;
motors[1].in2_port = GPIOA;
motors[1].in2_pin = GPIO_PIN_3;
motors[2].pwm_channel = TIM_CHANNEL_3;
motors[2].in1_port = GPIOA;
motors[2].in1_pin = GPIO_PIN_4;
motors[2].in2_port = GPIOA;
motors[2].in2_pin = GPIO_PIN_5;
motors[3].pwm_channel = TIM_CHANNEL_4;
motors[3].in1_port = GPIOA;
motors[3].in1_pin = GPIO_PIN_6;
motors[3].in2_port = GPIOA;
motors[3].in2_pin = GPIO_PIN_7;
printf("电机初始化完成\r\n");
}
// 设置电机速度
void motor_set_speed(uint8_t motor_id, uint8_t speed, uint8_t direction) {
if(motor_id >= 4 || !motors[motor_id].enabled) return;
// 限制速度范围
if(speed > 100) speed = 100;
motors[motor_id].target_speed = speed;
motors[motor_id].direction = direction;
// 设置方向引脚
switch(direction) {
case FORWARD:
HAL_GPIO_WritePin(motors[motor_id].in1_port, motors[motor_id].in1_pin, GPIO_PIN_SET);
HAL_GPIO_WritePin(motors[motor_id].in2_port, motors[motor_id].in2_pin, GPIO_PIN_RESET);
break;
case BACKWARD:
HAL_GPIO_WritePin(motors[motor_id].in1_port, motors[motor_id].in1_pin, GPIO_PIN_RESET);
HAL_GPIO_WritePin(motors[motor_id].in2_port, motors[motor_id].in2_pin, GPIO_PIN_SET);
break;
case STOP:
HAL_GPIO_WritePin(motors[motor_id].in1_port, motors[motor_id].in1_pin, GPIO_PIN_RESET);
HAL_GPIO_WritePin(motors[motor_id].in2_port, motors[motor_id].in2_pin, GPIO_PIN_RESET);
break;
case BRAKE:
HAL_GPIO_WritePin(motors[motor_id].in1_port, motors[motor_id].in1_pin, GPIO_PIN_SET);
HAL_GPIO_WritePin(motors[motor_id].in2_port, motors[motor_id].in2_pin, GPIO_PIN_SET);
break;
}
}
// 控制小车移动
void motor_move(uint8_t direction, uint8_t speed) {
switch(direction) {
case FORWARD:
for(uint8_t i = 0; i < 4; i++) {
motor_set_speed(i, speed, FORWARD);
}
break;
case BACKWARD:
for(uint8_t i = 0; i < 4; i++) {
motor_set_speed(i, speed, BACKWARD);
}
break;
case LEFT:
// 左轮慢，右轮快
motor_set_speed(0, speed * 0.3, FORWARD); // 左前
motor_set_speed(2, speed * 0.3, FORWARD); // 左后
motor_set_speed(1, speed, FORWARD); // 右前
motor_set_speed(3, speed, FORWARD); // 右后
break;
case RIGHT:
// 左轮快，右轮慢
motor_set_speed(0, speed, FORWARD); // 左前
motor_set_speed(2, speed, FORWARD); // 左后
motor_set_speed(1, speed * 0.3, FORWARD); // 右前
motor_set_speed(3, speed * 0.3, FORWARD); // 右后
break;
}
}
// 急转弯
void motor_sharp_turn(uint8_t direction, uint8_t speed) {
switch(direction) {
case LEFT:
// 左轮反转，右轮正转
motor_set_speed(0, speed, BACKWARD); // 左前
motor_set_speed(2, speed, BACKWARD); // 左后
motor_set_speed(1, speed, FORWARD); // 右前
motor_set_speed(3, speed, FORWARD); // 右后
break;
case RIGHT:
// 左轮正转，右轮反转
motor_set_speed(0, speed, FORWARD); // 左前
motor_set_speed(2, speed, FORWARD); // 左后
motor_set_speed(1, speed, BACKWARD); // 右前
motor_set_speed(3, speed, BACKWARD); // 右后
break;
}
}
// 停止
void motor_stop(void) {
for(uint8_t i = 0; i < 4; i++) {
motor_set_speed(i, 0, STOP);
}
}
// 刹车
void motor_brake(void) {
for(uint8_t i = 0; i < 4; i++) {
motor_set_speed(i, 0, BRAKE);
}
}
// 速度控制
void motor_speed_control(void) {
static uint32_t last_time = 0;
uint32_t current_time = HAL_GetTick();
float dt = (current_time - last_time) / 1000.0f;
if(dt < 0.01f) return; // 10ms更新一次
last_time = current_time;
for(uint8_t i = 0; i < 4; i++) {
if(!motors[i].enabled) continue;
// PID控制
float pid_output = PID_Calculate(&speed_pid[i],
motors[i].target_speed,
motors[i].current_speed,
dt);
// 更新当前速度
int16_t new_speed = motors[i].current_speed + pid_output;
// 限制速度范围
if(new_speed < 0) new_speed = 0;
if(new_speed > 100) new_speed = 100;
motors[i].current_speed = new_speed;
// 更新PWM
uint16_t pwm_value = (new_speed * 2000) / 100; // 映射到0-2000
switch(motors[i].pwm_channel) {
case TIM_CHANNEL_1:
__HAL_TIM_SET_COMPARE(&htim3, TIM_CHANNEL_1, pwm_value);
break;
case TIM_CHANNEL_2:
__HAL_TIM_SET_COMPARE(&htim3, TIM_CHANNEL_2, pwm_value);
break;
case TIM_CHANNEL_3:
__HAL_TIM_SET_COMPARE(&htim3, TIM_CHANNEL_3, pwm_value);
break;
case TIM_CHANNEL_4:
__HAL_TIM_SET_COMPARE(&htim3, TIM_CHANNEL_4, pwm_value);
break;
}
}
}
// 差速转向控制
void motor_steering_control(void) {
static float left_speed = 0, right_speed = 0;
// 获取转向指令
float steering = get_steering_input();
// 计算左右轮速度差
float speed_diff = steering * 50; // 最大50%速度差
left_speed = g_car_status.speed - speed_diff;
right_speed = g_car_status.speed + speed_diff;
// 限制速度范围
left_speed = constrain(left_speed, 0, 100);
right_speed = constrain(right_speed, 0, 100);
// 设置左右轮速度
motor_set_speed(0, left_speed, g_car_status.direction); // 左前
motor_set_speed(2, left_speed, g_car_status.direction); // 左后
motor_set_speed(1, right_speed, g_car_status.direction); // 右前
motor_set_speed(3, right_speed, g_car_status.direction); // 右后
}
参考代码 stm32的单片机带云台智能小车 有图像识别 www.youwenfan.com/contentcst/183154.html
项目结构
STM32_Gimbal_SmartCar/
├── Core/
│ ├── Inc/
│ │ ├── main.h
│ │ ├── camera.h
│ │ ├── motor.h
│ │ ├── gimbal.h
│ │ ├── algorithm.h
│ │ └── config.h
│ └── Src/
│ ├── main.c
│ ├── camera.c
│ ├── motor.c
│ ├── gimbal.c
│ ├── algorithm.c
│ └── pid.c
├── Drivers/
│ ├── CMSIS/
│ ├── STM32F4xx_HAL_Driver/
│ └── BSP/
├── Middlewares/
│ ├── FreeRTOS/
│ └── FatFS/
├── USB_DEVICE/
├── USB_HOST/
└── README.md
功能模式
1. 手动遥控模式
- 通过串口/蓝牙/WiFi接收控制指令
- 控制小车移动和云台转动
- 实时显示摄像头画面
2. 颜色追踪模式
- 识别特定颜色目标
- 自动追踪目标并保持距离
- 云台自动对准目标
3. 自主导航模式
- 超声波避障
- 视觉定位
- 自动路径规划
4. 巡线模式
- 识别地面轨迹
- 自动沿轨迹行驶
- 识别交通标志
调试与优化
1. 参数调优
// 在config.h中调整参数
#define PID_KP 1.0f // 比例系数
#define PID_KI 0.05f // 积分系数
#define PID_KD 0.1f // 微分系数
#define COLOR_THRESHOLD 50 // 颜色识别阈值
#define MIN_TARGET_AREA 100 // 最小目标面积
#define TRACKING_SPEED 30 // 追踪速度
2. 性能优化技巧
-
图像处理优化：
- 使用跳采样减少计算量
- 固定点运算代替浮点运算
- 使用查找表提高速度
-
内存优化：
- 使用DMA传输图像数据
- 双缓冲区避免图像撕裂
- 合理分配堆栈空间
-
功耗优化：
- 动态调整CPU频率
- 空闲时进入低功耗模式
- 关闭不必要的外设时钟
测试方案
1. 单元测试
void system_test(void) {
printf("开始系统测试...\r\n");
// 1. 摄像头测试
camera_test();
// 2. 云台测试
gimbal_test();
// 3. 电机测试
motor_test();
// 4. 传感器测试
sensor_test();
// 5. 图像识别测试
vision_test();
printf("系统测试完成\r\n");
}
2. 综合测试
void integrated_test(void) {
// 测试目标追踪
printf("测试目标追踪...\r\n");
g_operation_mode = MODE_AUTO_TRACKING;
g_target_color = COLOR_RED;
// 运行5分钟
for(int i = 0; i < 300; i++) {
HAL_Delay(1000);
printf("运行时间: %d秒\r\n", i);
}
printf("目标追踪测试完成\r\n");
}
这个完整的STM32云台智能小车系统具备了图像识别、目标追踪、自主导航等功能，代码结构清晰，易于扩展和修改。可以根据实际需求调整算法参数和硬件配置。
