基于Keil5的激光雷达避障小车程序
一、系统整体设计
1.1 硬件架构
// 硬件配置
// 主控: STM32F103C8T6
// 激光雷达: TFmini-S (IIC/UART)
// 电机驱动: TB6612FNG
// 电源: 12V锂电池
// 传感器: 编码器、超声波备用
1.2 软件架构
├── 应用层
│ ├── 避障算法
│ ├── 路径规划
│ └── 运动控制
├── 驱动层
│ ├── 激光雷达驱动
│ ├── 电机驱动
│ ├── PWM驱动
│ └── 编码器驱动
└── 硬件抽象层
├── GPIO
├── UART
├── TIM
└── I2C
二、主程序框架
// main.c
#include "stm32f1xx_hal.h"
#include "lidar.h"
#include "motor.h"
#include "pwm.h"
#include "encoder.h"
#include "uart.h"
#include "pid.h"
#include "oled.h"
// 系统状态定义
typedef enum {
SYS_IDLE, // 空闲
SYS_MANUAL, // 手动模式
SYS_AUTO_AVOID, // 自动避障
SYS_AUTO_TRACK, // 自动跟踪
SYS_EMERGENCY // 紧急停止
} SystemState;
// 障碍物信息
typedef struct {
float distance; // 距离(cm)
float angle; // 角度(度)
uint8_t type; // 类型
} ObstacleInfo;
// 激光雷达数据
typedef struct {
uint16_t distance[360]; // 360度距离数据
uint16_t intensity[360]; // 强度数据
uint8_t valid; // 数据有效标志
} LidarData;
// 全局变量
SystemState g_sysState = SYS_IDLE;
LidarData g_lidarData;
ObstacleInfo g_obstacles[20];
uint8_t g_obstacleCount = 0;
// 避障参数
#define SAFE_DISTANCE 30.0f // 安全距离(cm)
#define SLOW_DISTANCE 50.0f // 减速距离(cm)
#define TURN_ANGLE 45.0f // 转向角度(度)
// PID参数
PID_TypeDef g_pidLeft, g_pidRight;
int main(void) {
// 1. 硬件初始化
HAL_Init();
SystemClock_Config();
// 2. 外设初始化
UART1_Init(115200); // 调试串口
UART2_Init(115200); // 激光雷达串口
TIM3_PWM_Init(); // PWM初始化
TIM2_Encoder_Init(); // 编码器初始化(TIM2)
TIM4_Encoder_Init(); // 编码器初始化(TIM4)
I2C1_Init(); // OLED I2C
Motor_GPIO_Init(); // 电机控制GPIO
// 3. 模块初始化
OLED_Init();
Lidar_Init();
Motor_Init();
PID_Init(&g_pidLeft, 0.8, 0.1, 0.05, 100, -100);
PID_Init(&g_pidRight, 0.8, 0.1, 0.05, 100, -100);
// 4. 系统启动
OLED_ShowString(0, 0, "Lidar Car v1.0");
HAL_Delay(1000);
OLED_Clear();
// 5. 主循环
while (1) {
// 系统状态机
switch (g_sysState) {
case SYS_IDLE:
IdleMode_Process();
break;
case SYS_AUTO_AVOID:
AutoAvoidMode_Process();
break;
case SYS_EMERGENCY:
EmergencyStop();
break;
default:
break;
}
// 实时监测
Monitor_Process();
// 低功耗处理
if (CheckIdle()) {
EnterSleepMode();
}
}
}
// 空闲模式处理
void IdleMode_Process(void) {
static uint32_t lastTime = 0;
if (HAL_GetTick() - lastTime > 1000) {
OLED_ShowString(0, 0, "Ready");
lastTime = HAL_GetTick();
}
// 等待启动指令
if (CheckStartCommand()) {
g_sysState = SYS_AUTO_AVOID;
OLED_ShowString(0, 0, "Auto Avoid");
}
}
// 自动避障模式
void AutoAvoidMode_Process(void) {
// 1. 获取雷达数据
if (Lidar_GetData(&g_lidarData)) {
// 2. 障碍物检测
g_obstacleCount = DetectObstacles(&g_lidarData, g_obstacles);
// 3. 避障决策
AvoidanceDecision(g_obstacles, g_obstacleCount);
// 4. 显示信息
DisplayInfo();
}
// 5. 电机控制
Motor_Control();
}
// 紧急停止
void EmergencyStop(void) {
Motor_SetSpeed(0, 0);
OLED_ShowString(0, 0, "EMERGENCY STOP");
// 等待手动复位
if (CheckResetButton()) {
g_sysState = SYS_IDLE;
}
}
// 系统监控
void Monitor_Process(void) {
// 电压监测
float voltage = GetBatteryVoltage();
if (voltage < 10.5f) { // 电压过低
g_sysState = SYS_EMERGENCY;
}
// 温度监测
float temp = GetTemperature();
if (temp > 60.0f) { // 温度过高
g_sysState = SYS_EMERGENCY;
}
}
三、激光雷达驱动
// lidar.h
#ifndef __LIDAR_H
#define __LIDAR_H
#include "stm32f1xx_hal.h"
// 激光雷达型号定义
#define LIDAR_TFMINI 0
#define LIDAR_RPLIDAR 1
#define LIDAR_YDLIDAR 2
// 当前使用的雷达型号
#define LIDAR_TYPE LIDAR_TFMINI
#if LIDAR_TYPE == LIDAR_TFMINI
#define LIDAR_HEADER 0x59
#define LIDAR_FRAME_LEN 9
#define LIDAR_MAX_DISTANCE 1200 // 12m
#define LIDAR_MIN_DISTANCE 10 // 0.1m
#elif LIDAR_TYPE == LIDAR_RPLIDAR
// RPLIDAR配置
#endif
// 雷达数据结构
typedef struct {
uint16_t distance; // 距离 (cm)
uint16_t strength; // 信号强度
uint8_t temperature; // 温度
uint8_t error; // 错误代码
} LidarFrame;
// 函数声明
void Lidar_Init(void);
uint8_t Lidar_GetData(LidarData* data);
uint8_t Lidar_ParseFrame(uint8_t* buffer, LidarFrame* frame);
void Lidar_StartScan(void);
void Lidar_StopScan(void);
float Lidar_GetAngleResolution(void);
void Lidar_Calibration(void);
#endif
// lidar.c
#include "lidar.h"
#include "uart.h"
// 全局变量
UART_HandleTypeDef huart2; // 激光雷达串口
DMA_HandleTypeDef hdma_usart2_rx;
uint8_t lidar_rx_buffer[256];
uint8_t lidar_frame_buffer[LIDAR_FRAME_LEN];
uint8_t lidar_frame_index = 0;
volatile uint8_t lidar_data_ready = 0;
LidarFrame current_frame;
// 雷达初始化
void Lidar_Init(void) {
// 初始化串口2
huart2.Instance = USART2;
huart2.Init.BaudRate = 115200;
huart2.Init.WordLength = UART_WORDLENGTH_8B;
huart2.Init.StopBits = UART_STOPBITS_1;
huart2.Init.Parity = UART_PARITY_NONE;
huart2.Init.Mode = UART_MODE_TX_RX;
huart2.Init.HwFlowCtl = UART_HWCONTROL_NONE;
huart2.Init.OverSampling = UART_OVERSAMPLING_16;
if (HAL_UART_Init(&huart2) != HAL_OK) {
Error_Handler();
}
// 启动DMA接收
HAL_UART_Receive_DMA(&huart2, lidar_rx_buffer, sizeof(lidar_rx_buffer));
// 发送启动指令
Lidar_StartScan();
// 校准
Lidar_Calibration();
}
// 启动雷达扫描
void Lidar_StartScan(void) {
#if LIDAR_TYPE == LIDAR_TFMINI
// TFmini默认上电自动扫描
#elif LIDAR_TYPE == LIDAR_RPLIDAR
uint8_t start_cmd[] = {0xA5, 0x60};
HAL_UART_Transmit(&huart2, start_cmd, 2, 1000);
#endif
}
// 解析雷达数据帧
uint8_t Lidar_ParseFrame(uint8_t* buffer, LidarFrame* frame) {
#if LIDAR_TYPE == LIDAR_TFMINI
// TFmini数据格式: 0x59 0x59 Dist_L Dist_H Strength_L Strength_H Temp_L Temp_H Checksum
if (buffer[0] == LIDAR_HEADER && buffer[1] == LIDAR_HEADER) {
// 计算校验和
uint8_t checksum = 0;
for (int i = 0; i < 8; i++) {
checksum += buffer[i];
}
if (checksum == buffer[8]) {
// 提取数据
frame->distance = buffer[2] | (buffer[3] << 8);
frame->strength = buffer[4] | (buffer[5] << 8);
frame->temperature = buffer[6];
frame->error = buffer[7];
// 数据有效性检查
if (frame->distance >= LIDAR_MIN_DISTANCE &&
frame->distance <= LIDAR_MAX_DISTANCE &&
frame->strength > 100) {
return 1;
}
}
}
#endif
return 0;
}
// 获取完整雷达数据
uint8_t Lidar_GetData(LidarData* data) {
static uint16_t angle_index = 0;
static uint32_t last_update = 0;
if (!lidar_data_ready) {
return 0;
}
// 处理接收缓冲区
for (int i = 0; i < sizeof(lidar_rx_buffer); i++) {
lidar_frame_buffer[lidar_frame_index++] = lidar_rx_buffer[i];
if (lidar_frame_index >= LIDAR_FRAME_LEN) {
lidar_frame_index = 0;
if (Lidar_ParseFrame(lidar_frame_buffer, ¤t_frame)) {
// 存储到数据数组
if (angle_index < 360) {
data->distance[angle_index] = current_frame.distance;
data->intensity[angle_index] = current_frame.strength;
angle_index++;
if (angle_index >= 360) {
angle_index = 0;
data->valid = 1;
lidar_data_ready = 0;
return 1;
}
}
}
}
}
// 超时处理
if (HAL_GetTick() - last_update > 1000) {
angle_index = 0;
last_update = HAL_GetTick();
}
return 0;
}
// 串口接收完成回调
void HAL_UART_RxCpltCallback(UART_HandleTypeDef *huart) {
if (huart->Instance == USART2) {
lidar_data_ready = 1;
// 重新启动DMA接收
HAL_UART_Receive_DMA(&huart2, lidar_rx_buffer, sizeof(lidar_rx_buffer));
}
}
// 雷达校准
void Lidar_Calibration(void) {
// 校准参数
float distance_offset = 0.0f;
float angle_offset = 0.0f;
uint16_t samples = 100;
OLED_ShowString(0, 0, "Calibrating...");
// 采集校准数据
for (int i = 0; i < samples; i++) {
// 测量已知距离
// ...
HAL_Delay(10);
}
// 计算校准值
// ...
OLED_ShowString(0, 0, "Calibration OK");
HAL_Delay(500);
}
四、避障算法实现
// avoidance.c
#include "avoidance.h"
#include "math.h"
// 避障参数
#define OBSTACLE_THRESHOLD 30.0f // 障碍物阈值(cm)
#define CLUSTER_DISTANCE 20.0f // 聚类距离(cm)
#define MIN_OBSTACLE_SIZE 3 // 最小障碍物点数
#define SAFE_ANGLE 30.0f // 安全角度(度)
// 避障决策结果
typedef struct {
float target_speed; // 目标速度
float target_angle; // 目标角度
uint8_t action; // 动作类型
} AvoidanceResult;
// 障碍物聚类
typedef struct {
float center_angle; // 中心角度
float min_distance; // 最小距离
float width; // 宽度(度)
uint8_t point_count; // 点数
} ObstacleCluster;
// 检测障碍物
uint8_t DetectObstacles(LidarData* lidar_data, ObstacleInfo* obstacles) {
uint8_t obstacle_count = 0;
ObstacleCluster clusters[20];
uint8_t cluster_count = 0;
// 1. 数据预处理
for (int i = 0; i < 360; i++) {
if (lidar_data->distance[i] < OBSTACLE_THRESHOLD &&
lidar_data->distance[i] > 0) {
// 找到障碍物点
}
}
// 2. 聚类分析
cluster_count = ClusterObstacles(lidar_data, clusters);
// 3. 提取障碍物信息
for (int i = 0; i < cluster_count; i++) {
if (clusters[i].point_count >= MIN_OBSTACLE_SIZE) {
obstacles[obstacle_count].distance = clusters[i].min_distance;
obstacles[obstacle_count].angle = clusters[i].center_angle;
obstacles[obstacle_count].type = 0; // 普通障碍物
obstacle_count++;
if (obstacle_count >= 20) break;
}
}
return obstacle_count;
}
// 障碍物聚类
uint8_t ClusterObstacles(LidarData* lidar_data, ObstacleCluster* clusters) {
uint8_t cluster_idx = 0;
uint8_t in_cluster = 0;
float last_angle = 0;
float last_distance = 0;
clusters[0].point_count = 0;
clusters[0].min_distance = 9999.0f;
clusters[0].center_angle = 0;
for (int i = 0; i < 360; i++) {
float distance = lidar_data->distance[i];
float angle = i * 1.0f; // 1度分辨率
if (distance < OBSTACLE_THRESHOLD && distance > 0) {
if (!in_cluster) {
// 开始新聚类
in_cluster = 1;
clusters[cluster_idx].point_count = 1;
clusters[cluster_idx].min_distance = distance;
clusters[cluster_idx].center_angle = angle;
clusters[cluster_idx].width = 1.0f;
} else {
// 检查是否属于当前聚类
float angle_diff = angle - last_angle;
float dist_diff = fabs(distance - last_distance);
if (angle_diff < 5.0f && dist_diff < CLUSTER_DISTANCE) {
// 属于当前聚类
clusters[cluster_idx].point_count++;
clusters[cluster_idx].center_angle =
(clusters[cluster_idx].center_angle *
(clusters[cluster_idx].point_count - 1) + angle) /
clusters[cluster_idx].point_count;
if (distance < clusters[cluster_idx].min_distance) {
clusters[cluster_idx].min_distance = distance;
}
clusters[cluster_idx].width = angle -
(clusters[cluster_idx].center_angle -
clusters[cluster_idx].width/2);
} else {
// 开始新聚类
cluster_idx++;
if (cluster_idx >= 20) break;
clusters[cluster_idx].point_count = 1;
clusters[cluster_idx].min_distance = distance;
clusters[cluster_idx].center_angle = angle;
clusters[cluster_idx].width = 1.0f;
}
}
} else {
in_cluster = 0;
}
last_angle = angle;
last_distance = distance;
}
return cluster_idx + 1;
}
// 避障决策
void AvoidanceDecision(ObstacleInfo* obstacles, uint8_t count) {
static AvoidanceResult result;
static uint8_t last_action = ACTION_FORWARD;
// 1. 寻找最佳路径
float best_angle = 0;
float best_score = -9999.0f;
// 评估各个方向
for (float angle = -90.0f; angle <= 90.0f; angle += 5.0f) {
float score = EvaluateDirection(angle, obstacles, count);
if (score > best_score) {
best_score = score;
best_angle = angle;
}
}
// 2. 决策
if (best_score < -50.0f) {
// 无路可走，后退
result.action = ACTION_BACKWARD;
result.target_speed = 0.5f; // 半速后退
result.target_angle = 0;
} else if (fabs(best_angle) < 10.0f) {
// 直行
result.action = ACTION_FORWARD;
result.target_speed = 1.0f; // 全速前进
// 根据前方障碍物距离调整速度
float front_dist = GetFrontDistance(obstacles, count);
if (front_dist < SLOW_DISTANCE) {
result.target_speed = 0.5f + 0.5f * (front_dist - SAFE_DISTANCE) /
(SLOW_DISTANCE - SAFE_DISTANCE);
}
result.target_angle = 0;
} else {
// 转向
result.action = (best_angle > 0) ? ACTION_TURN_LEFT : ACTION_TURN_RIGHT;
result.target_speed = 0.7f; // 转向时减速
result.target_angle = best_angle;
}
// 3. 平滑处理
result = SmoothDecision(result, last_action);
last_action = result.action;
// 4. 执行决策
ExecuteDecision(&result);
}
// 评估方向得分
float EvaluateDirection(float angle, ObstacleInfo* obstacles, uint8_t count) {
float score = 0.0f;
// 1. 基础得分：越靠近0度得分越高
score = 100.0f - fabs(angle) * 0.5f;
// 2. 障碍物惩罚
for (int i = 0; i < count; i++) {
float angle_diff = fabs(angle - obstacles[i].angle);
float distance = obstacles[i].distance;
if (angle_diff < 30.0f) { // 30度内考虑影响
float penalty = 0;
if (distance < SAFE_DISTANCE) {
penalty = 100.0f * (SAFE_DISTANCE - distance) / SAFE_DISTANCE;
} else if (distance < SLOW_DISTANCE) {
penalty = 20.0f * (SLOW_DISTANCE - distance) /
(SLOW_DISTANCE - SAFE_DISTANCE);
}
// 角度权重
float angle_weight = 1.0f - angle_diff / 30.0f;
score -= penalty * angle_weight;
}
}
// 3. 历史方向偏好（保持稳定性）
static float last_angle = 0;
float angle_change = fabs(angle - last_angle);
if (angle_change < 45.0f) {
score += 20.0f * (1.0f - angle_change / 45.0f);
}
last_angle = angle;
return score;
}
// 获取前方距离
float GetFrontDistance(ObstacleInfo* obstacles, uint8_t count) {
float min_distance = 9999.0f;
for (int i = 0; i < count; i++) {
if (fabs(obstacles[i].angle) < 30.0f) { // 前方30度
if (obstacles[i].distance < min_distance) {
min_distance = obstacles[i].distance;
}
}
}
return (min_distance < 9999.0f) ? min_distance : 9999.0f;
}
// 平滑决策
AvoidanceResult SmoothDecision(AvoidanceResult new_decision, uint8_t last_action) {
static AvoidanceResult smoothed;
static float smooth_factor = 0.3f; // 平滑系数
// 角度平滑
smoothed.target_angle = smoothed.target_angle * (1 - smooth_factor) +
new_decision.target_angle * smooth_factor;
// 速度平滑
smoothed.target_speed = smoothed.target_speed * (1 - smooth_factor) +
new_decision.target_speed * smooth_factor;
// 动作切换平滑
if (new_decision.action != last_action) {
// 添加过渡动作
if ((last_action == ACTION_FORWARD &&
new_decision.action == ACTION_TURN_LEFT) ||
(last_action == ACTION_TURN_LEFT &&
new_decision.action == ACTION_FORWARD)) {
// 平滑过渡
}
}
smoothed.action = new_decision.action;
return smoothed;
}
// 执行决策
void ExecuteDecision(AvoidanceResult* decision) {
float left_speed, right_speed;
switch (decision->action) {
case ACTION_FORWARD:
left_speed = decision->target_speed;
right_speed = decision->target_speed;
break;
case ACTION_BACKWARD:
left_speed = -decision->target_speed;
right_speed = -decision->target_speed;
break;
case ACTION_TURN_LEFT:
left_speed = decision->target_speed * 0.5f;
right_speed = decision->target_speed;
break;
case ACTION_TURN_RIGHT:
left_speed = decision->target_speed;
right_speed = decision->target_speed * 0.5f;
break;
default:
left_speed = 0;
right_speed = 0;
break;
}
// 设置电机速度
Motor_SetSpeed(left_speed, right_speed);
}
五、电机驱动控制
// motor.c
#include "motor.h"
#include "pwm.h"
#include "encoder.h"
#include "pid.h"
// 电机参数
#define MOTOR_MAX_SPEED 100 // 最大速度百分比
#define MOTOR_MIN_SPEED 10 // 最小速度百分比
#define WHEEL_DIAMETER 6.5f // 轮子直径(cm)
#define WHEEL_DISTANCE 20.0f // 轮距(cm)
// 电机控制结构
typedef struct {
float target_speed; // 目标速度(cm/s)
float current_speed; // 当前速度(cm/s)
int16_t encoder_count; // 编码器计数
uint32_t last_count; // 上次计数
uint32_t last_time; // 上次时间
} MotorControl;
MotorControl motor_left, motor_right;
// 电机初始化
void Motor_Init(void) {
// GPIO初始化
__HAL_RCC_GPIOA_CLK_ENABLE();
__HAL_RCC_GPIOB_CLK_ENABLE();
GPIO_InitTypeDef GPIO_InitStruct = {0};
// 电机方向控制引脚
// AIN1, AIN2: PA0, PA1 (左电机)
// BIN1, BIN2: PA2, PA3 (右电机)
// STBY: PA4
GPIO_InitStruct.Pin = GPIO_PIN_0 | GPIO_PIN_1 | GPIO_PIN_2 | GPIO_PIN_3 | GPIO_PIN_4;
GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
GPIO_InitStruct.Pull = GPIO_NOPULL;
GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
HAL_GPIO_Init(GPIOA, &GPIO_InitStruct);
// 使能电机驱动器
HAL_GPIO_WritePin(GPIOA, GPIO_PIN_4, GPIO_PIN_SET);
// 初始化编码器
Encoder_Init();
// 初始化PID
PID_Reset(&g_pidLeft);
PID_Reset(&g_pidRight);
// 初始化电机控制结构
motor_left.target_speed = 0;
motor_left.current_speed = 0;
motor_left.encoder_count = 0;
motor_left.last_count = 0;
motor_left.last_time = HAL_GetTick();
motor_right.target_speed = 0;
motor_right.current_speed = 0;
motor_right.encoder_count = 0;
motor_right.last_count = 0;
motor_right.last_time = HAL_GetTick();
}
// 设置电机速度
void Motor_SetSpeed(float left_speed, float right_speed) {
// 限制速度范围
left_speed = constrain(left_speed, -1.0f, 1.0f);
right_speed = constrain(right_speed, -1.0f, 1.0f);
// 转换为cm/s
motor_left.target_speed = left_speed * 30.0f; // 最大30cm/s
motor_right.target_speed = right_speed * 30.0f;
}
// 电机控制任务（定期调用）
void Motor_Control(void) {
static uint32_t last_control_time = 0;
uint32_t current_time = HAL_GetTick();
if (current_time - last_control_time < 10) { // 10ms控制周期
return;
}
last_control_time = current_time;
// 1. 更新当前速度
UpdateMotorSpeed(&motor_left, TIM2);
UpdateMotorSpeed(&motor_right, TIM4);
// 2. PID控制
float left_pwm = PID_Calculate(&g_pidLeft,
motor_left.target_speed,
motor_left.current_speed);
float right_pwm = PID_Calculate(&g_pidRight,
motor_right.target_speed,
motor_right.current_speed);
// 3. 设置PWM
SetMotorPWM(&motor_left, left_pwm, TIM3_CH1);
SetMotorPWM(&motor_right, right_pwm, TIM3_CH2);
}
// 更新电机速度
void UpdateMotorSpeed(MotorControl* motor, TIM_HandleTypeDef* encoder_tim) {
uint32_t current_time = HAL_GetTick();
float dt = (current_time - motor->last_time) / 1000.0f; // 转换为秒
if (dt > 0) {
// 获取编码器计数
int16_t count = (int16_t)__HAL_TIM_GET_COUNTER(encoder_tim);
__HAL_TIM_SET_COUNTER(encoder_tim, 0);
// 计算速度 (cm/s)
// 编码器分辨率: 11线 * 减速比 * 4倍频
// 假设减速比30:1，则每转计数 = 11 * 30 * 4 = 1320
float counts_per_rev = 1320.0f;
float circumference = WHEEL_DIAMETER * 3.14159f;
motor->current_speed = (count / counts_per_rev) * circumference / dt;
motor->last_time = current_time;
}
}
// 设置电机PWM
void SetMotorPWM(MotorControl* motor, float pwm, uint32_t channel) {
// 限制PWM范围
pwm = constrain(pwm, -100.0f, 100.0f);
// 设置方向
if (pwm > 0) {
// 正转
if (motor == &motor_left) {
HAL_GPIO_WritePin(GPIOA, GPIO_PIN_0, GPIO_PIN_SET);
HAL_GPIO_WritePin(GPIOA, GPIO_PIN_1, GPIO_PIN_RESET);
} else {
HAL_GPIO_WritePin(GPIOA, GPIO_PIN_2, GPIO_PIN_SET);
HAL_GPIO_WritePin(GPIOA, GPIO_PIN_3, GPIO_PIN_RESET);
}
} else if (pwm < 0) {
// 反转
pwm = -pwm;
if (motor == &motor_left) {
HAL_GPIO_WritePin(GPIOA, GPIO_PIN_0, GPIO_PIN_RESET);
HAL_GPIO_WritePin(GPIOA, GPIO_PIN_1, GPIO_PIN_SET);
} else {
HAL_GPIO_WritePin(GPIOA, GPIO_PIN_2, GPIO_PIN_RESET);
HAL_GPIO_WritePin(GPIOA, GPIO_PIN_3, GPIO_PIN_SET);
}
} else {
// 停止
if (motor == &motor_left) {
HAL_GPIO_WritePin(GPIOA, GPIO_PIN_0, GPIO_PIN_RESET);
HAL_GPIO_WritePin(GPIOA, GPIO_PIN_1, GPIO_PIN_RESET);
} else {
HAL_GPIO_WritePin(GPIOA, GPIO_PIN_2, GPIO_PIN_RESET);
HAL_GPIO_WritePin(GPIOA, GPIO_PIN_3, GPIO_PIN_RESET);
}
}
// 设置PWM占空比
uint16_t pwm_value = (uint16_t)(pwm * 10.0f); // PWM范围0-1000
__HAL_TIM_SET_COMPARE(&htim3, channel, pwm_value);
}
// 约束函数
float constrain(float value, float min, float max) {
if (value < min) return min;
if (value > max) return max;
return value;
}
六、PID控制器
// pid.c
#include "pid.h"
// PID初始化
void PID_Init(PID_TypeDef* pid, float kp, float ki, float kd,
float max_output, float min_output) {
pid->kp = kp;
pid->ki = ki;
pid->kd = kd;
pid->max_output = max_output;
pid->min_output = min_output;
pid->integral = 0;
pid->prev_error = 0;
pid->prev_time = 0;
}
// PID计算
float PID_Calculate(PID_TypeDef* pid, float setpoint, float measured) {
uint32_t current_time = HAL_GetTick();
float dt = (current_time - pid->prev_time) / 1000.0f;
if (dt <= 0 || dt > 1.0f) {
dt = 0.01f; // 默认10ms
}
float error = setpoint - measured;
// 比例项
float P = pid->kp * error;
// 积分项
pid->integral += error * dt;
// 积分限幅
if (pid->integral > pid->max_output / pid->ki) {
pid->integral = pid->max_output / pid->ki;
} else if (pid->integral < pid->min_output / pid->ki) {
pid->integral = pid->min_output / pid->ki;
}
float I = pid->ki * pid->integral;
// 微分项
float derivative = (error - pid->prev_error) / dt;
float D = pid->kd * derivative;
// 计算输出
float output = P + I + D;
// 输出限幅
if (output > pid->max_output) {
output = pid->max_output;
} else if (output < pid->min_output) {
output = pid->min_output;
}
// 更新状态
pid->prev_error = error;
pid->prev_time = current_time;
return output;
}
// PID重置
void PID_Reset(PID_TypeDef* pid) {
pid->integral = 0;
pid->prev_error = 0;
pid->prev_time = HAL_GetTick();
}
七、OLED显示
// oled.c
#include "oled.h"
#include "i2c.h"
// OLED初始化
void OLED_Init(void) {
HAL_Delay(100);
// 初始化序列
OLED_WriteCmd(0xAE); // 关闭显示
OLED_WriteCmd(0xD5); // 设置时钟分频
OLED_WriteCmd(0x80);
OLED_WriteCmd(0xA8); // 多路复用率
OLED_WriteCmd(0x3F);
OLED_WriteCmd(0xD3); // 显示偏移
OLED_WriteCmd(0x00);
OLED_WriteCmd(0x40); // 起始行
OLED_WriteCmd(0x8D); // 电荷泵
OLED_WriteCmd(0x14);
OLED_WriteCmd(0x20); // 内存模式
OLED_WriteCmd(0x00);
OLED_WriteCmd(0xA1); // 段重映射
OLED_WriteCmd(0xC8); // COM扫描方向
OLED_WriteCmd(0xDA); // COM引脚配置
OLED_WriteCmd(0x12);
OLED_WriteCmd(0x81); // 对比度
OLED_WriteCmd(0xCF);
OLED_WriteCmd(0xD9); // 预充电周期
OLED_WriteCmd(0xF1);
OLED_WriteCmd(0xDB); // VCOMH电平
OLED_WriteCmd(0x40);
OLED_WriteCmd(0xA4); // 全亮显示
OLED_WriteCmd(0xA6); // 正常显示
OLED_WriteCmd(0xAF); // 开启显示
OLED_Clear();
}
// 显示避障信息
void DisplayInfo(void) {
char buffer[20];
// 第1行：状态
OLED_ShowString(0, 0, "Auto Avoid");
// 第2行：速度
sprintf(buffer, "L:%.1f R:%.1f",
motor_left.current_speed,
motor_right.current_speed);
OLED_ShowString(0, 2, buffer);
// 第3行：障碍物数量
sprintf(buffer, "Obs:%d", g_obstacleCount);
OLED_ShowString(0, 4, buffer);
// 第4行：最近距离
float min_dist = 9999.0f;
for (int i = 0; i < g_obstacleCount; i++) {
if (g_obstacles[i].distance < min_dist) {
min_dist = g_obstacles[i].distance;
}
}
if (min_dist < 9999.0f) {
sprintf(buffer, "Dist:%.1fcm", min_dist);
} else {
sprintf(buffer, "Dist:---");
}
OLED_ShowString(0, 6, buffer);
}
八、系统配置文件
// system_config.c
#include "stm32f1xx_hal.h"
// 系统时钟配置
void SystemClock_Config(void) {
RCC_OscInitTypeDef RCC_OscInitStruct = {0};
RCC_ClkInitTypeDef RCC_ClkInitStruct = {0};
// 配置HSE
RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSE;
RCC_OscInitStruct.HSEState = RCC_HSE_ON;
RCC_OscInitStruct.HSEPredivValue = RCC_HSE_PREDIV_DIV1;
RCC_OscInitStruct.PLL.PLLState = RCC_PLL_ON;
RCC_OscInitStruct.PLL.PLLSource = RCC_PLLSOURCE_HSE;
RCC_OscInitStruct.PLL.PLLMUL = RCC_PLL_MUL9;
if (HAL_RCC_OscConfig(&RCC_OscInitStruct) != HAL_OK) {
Error_Handler();
}
// 配置系统时钟
RCC_ClkInitStruct.ClockType = RCC_CLOCKTYPE_HCLK|RCC_CLOCKTYPE_SYSCLK
|RCC_CLOCKTYPE_PCLK1|RCC_CLOCKTYPE_PCLK2;
RCC_ClkInitStruct.SYSCLKSource = RCC_SYSCLKSOURCE_PLLCLK;
RCC_ClkInitStruct.AHBCLKDivider = RCC_SYSCLK_DIV1;
RCC_ClkInitStruct.APB1CLKDivider = RCC_HCLK_DIV2;
RCC_ClkInitStruct.APB2CLKDivider = RCC_HCLK_DIV1;
if (HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_2) != HAL_OK) {
Error_Handler();
}
}
// 错误处理
void Error_Handler(void) {
// 错误指示灯
__HAL_RCC_GPIOC_CLK_ENABLE();
GPIO_InitTypeDef GPIO_InitStruct = {0};
GPIO_InitStruct.Pin = GPIO_PIN_13;
GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
GPIO_InitStruct.Pull = GPIO_NOPULL;
GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
HAL_GPIO_Init(GPIOC, &GPIO_InitStruct);
while (1) {
HAL_GPIO_TogglePin(GPIOC, GPIO_PIN_13);
HAL_Delay(100);
}
}
九、Makefile配置
# Makefile for STM32 Lidar Car
# 工具链
CC = arm-none-eabi-gcc
OBJCOPY = arm-none-eabi-objcopy
OBJDUMP = arm-none-eabi-objdump
SIZE = arm-none-eabi-size
# 目标
TARGET = lidar_car
MCU = cortex-m3
# 目录
SRC_DIR = src
INC_DIR = inc
OBJ_DIR = obj
BIN_DIR = bin
# 源文件
SRCS = $(wildcard $(SRC_DIR)/*.c)
OBJS = $(patsubst $(SRC_DIR)/%.c,$(OBJ_DIR)/%.o,$(SRCS))
# 头文件路径
INC = -I$(INC_DIR) -IDrivers/CMSIS/Include -IDrivers/STM32F1xx_HAL_Driver/Inc
# 编译选项
CFLAGS = -mcpu=$(MCU) -mthumb -Wall -O2 -fdata-sections -ffunction-sections
CFLAGS += -DSTM32F103xB -DUSE_HAL_DRIVER
CFLAGS += $(INC)
# 链接选项
LDFLAGS = -TSTM32F103C8Tx_FLASH.ld
LDFLAGS += -Wl,--gc-sections
LDFLAGS += -specs=nano.specs
LDFLAGS += -u _printf_float
# 库文件
LIBS = -lm -lc
# 规则
all: $(BIN_DIR)/$(TARGET).bin
$(BIN_DIR)/$(TARGET).bin: $(BIN_DIR)/$(TARGET).elf
$(OBJCOPY) -O binary $< $@
$(SIZE) $<
$(BIN_DIR)/$(TARGET).elf: $(OBJS)
@mkdir -p $(BIN_DIR)
$(CC) $(CFLAGS) $(LDFLAGS) -o $@ $^ $(LIBS)
$(OBJ_DIR)/%.o: $(SRC_DIR)/%.c
@mkdir -p $(OBJ_DIR)
$(CC) $(CFLAGS) -c $< -o $@
clean:
rm -rf $(OBJ_DIR) $(BIN_DIR)
flash: $(BIN_DIR)/$(TARGET).bin
st-flash write $(BIN_DIR)/$(TARGET).bin 0x8000000
.PHONY: all clean flash
参考代码 激光雷达程序 www.youwenfan.com/contentcsu/70376.html
十、Keil5工程配置
10.1 工程结构
Lidar_Car_Project/
├── Core/
│ ├── Inc/
│ │ ├── main.h
│ │ ├── lidar.h
│ │ ├── motor.h
│ │ ├── avoidance.h
│ │ └── ...
│ ├── Src/
│ │ ├── main.c
│ │ ├── lidar.c
│ │ ├── motor.c
│ │ ├── avoidance.c
│ │ └── ...
│ └── Startup/
│ └── startup_stm32f103xb.s
├── Drivers/
│ ├── CMSIS/
│ └── STM32F1xx_HAL_Driver/
└── MDK-ARM/
├── lidar_car.uvprojx
└── STM32F103C8Tx_FLASH.ld
10.2 编译配置
-
Target Options:
- Device: STM32F103C8
- Use MicroLIB: √
- Optimization: Level 2 (-O2)
-
C/C++ Options:
- Define: STM32F103xB, USE_HAL_DRIVER
- Include Paths: Add all include directories
-
Debug Options:
- Use: ST-Link Debugger
- Port: SW
- Max Clock: 4MHz
十一、调试与测试
// debug.c
#include "debug.h"
// 调试模式
#define DEBUG_MODE 1
#if DEBUG_MODE
// 串口调试输出
void Debug_Printf(const char* format, ...) {
char buffer[128];
va_list args;
va_start(args, format);
vsprintf(buffer, format, args);
va_end(args);
HAL_UART_Transmit(&huart1, (uint8_t*)buffer, strlen(buffer), 1000);
}
// 数据监控
void Debug_Monitor(void) {
static uint32_t last_time = 0;
if (HAL_GetTick() - last_time > 100) { // 100ms输出一次
Debug_Printf("=== System Monitor ===\r\n");
Debug_Printf("State: %d\r\n", g_sysState);
Debug_Printf("Left Speed: %.1f cm/s\r\n", motor_left.current_speed);
Debug_Printf("Right Speed: %.1f cm/s\r\n", motor_right.current_speed);
Debug_Printf("Obstacles: %d\r\n", g_obstacleCount);
Debug_Printf("Battery: %.1f V\r\n", GetBatteryVoltage());
Debug_Printf("\r\n");
last_time = HAL_GetTick();
}
}
#endif
// 性能测试
void Performance_Test(void) {
uint32_t start_time, end_time;
float cpu_load;
// 测试避障算法性能
start_time = HAL_GetTick();
for (int i = 0; i < 1000; i++) {
AvoidanceDecision(g_obstacles, g_obstacleCount);
}
end_time = HAL_GetTick();
cpu_load = (end_time - start_time) * 100.0f / 1000.0f; // 百分比
Debug_Printf("Avoidance CPU Load: %.1f%%\r\n", cpu_load);
}
十二、避障算法优化建议
12.1 高级避障算法
// 动态窗口法(Dynamic Window Approach)
typedef struct {
float v_min, v_max; // 速度范围
float w_min, w_max; // 角速度范围
float dt; // 时间步长
} DWA_Config;
// 人工势场法(Artificial Potential Field)
typedef struct {
float k_att; // 引力增益
float k_rep; // 斥力增益
float d0; // 影响距离
} APF_Config;
12.2 机器学习避障
// 简单神经网络避障
typedef struct {
float weights[3][4]; // 输入层到隐藏层权重
float bias[4]; // 隐藏层偏置
float output_weights[4]; // 隐藏层到输出层权重
} NeuralNetwork;
// 训练数据收集
void CollectTrainingData(void) {
// 收集雷达数据+动作标签
// 用于离线训练
}
十三、扩展功能
13.1 添加超声波传感器
// 超声波避障
void Ultrasonic_Avoidance(void) {
float front_dist = Ultrasonic_GetDistance(US_FRONT);
float left_dist = Ultrasonic_GetDistance(US_LEFT);
float right_dist = Ultrasonic_GetDistance(US_RIGHT);
// 融合激光雷达和超声波数据
// 提高近距离障碍物检测精度
}
13.2 添加IMU
// 姿态辅助
void IMU_Assist(void) {
float roll, pitch, yaw;
IMU_GetEuler(&roll, &pitch, &yaw);
// 在斜坡上调整速度
if (fabs(pitch) > 10.0f) {
Motor_SetSpeed(0.5, 0.5); // 减速
}
}
十四、注意事项
- 电源管理：确保电机电源和MCU电源隔离
- 接地处理：数字地和模拟地分开
- PWM频率：建议使用10-20kHz
- 编码器滤波：添加软件滤波去除噪声
- 安全机制：急停开关必须硬件实现
- 温度保护：添加温度传感器防止过热
