STM32无人机避障实战：从零搭建激光雷达+超声波融合系统（附完整代码）

STM32无人机避障实战：从零搭建激光雷达+超声波融合系统（附完整代码）

去年夏天，我在调试一台四轴飞行器时，差点让它一头撞上阳台的晾衣杆。那次经历让我深刻意识到，对于创客和嵌入式开发者来说，一套可靠且成本可控的避障系统有多重要。市面上的高端无人机往往配备昂贵的深度摄像头或固态激光雷达，但对于我们这些喜欢自己动手、预算有限的开发者，如何用几百元的传感器组合实现稳定避障，才是真正的挑战。

这篇文章，我想和你分享一个我实际验证过的方案：基于STM32，融合低成本二维激光雷达（比如RPLIDAR A1）和超声波模块，构建一套实时性高、鲁棒性强的无人机避障系统。我们不会空谈理论，而是聚焦于工程实现，从传感器选型对比、数据同步策略，到卡尔曼滤波的实战应用，再到如何在FreeRTOS里合理划分任务，最后提供可直接编译、下载的完整工程代码。我们的目标很明确：用最“接地气”的硬件，实现不输于商业方案的避障能力。

1. 传感器选型与硬件架构设计：在成本与性能间寻找平衡点

为无人机选择避障传感器，就像为它挑选“眼睛”，需要权衡视野、精度、速度、功耗，当然还有最重要的——成本。对于创客项目，我们通常不会直接选用数万元的工业级激光雷达，而是在几百元到一千多元的消费级产品中寻找最优解。

激光雷达负责提供二维平面上的高精度距离与角度信息，是避障系统的“主力”。市面上常见的低成本型号主要有两种：RPLIDAR A1/A1M8和Benewake TFmini/TF02。它们原理不同，适用场景也各异。

为了让你更直观地对比，我把两款雷达的核心参数整理成了表格：

特性	RPLIDAR A1/A1M8	Benewake TFmini/TF02
测距原理	三角测距法	飞行时间法（ToF）
扫描方式	360° 机械旋转扫描	单点/小范围固态扫描
最大测距	约 12 米	约 12 米
测量频率	最高 8000 点/秒	最高 1000 点/秒（单点）
角度分辨率	约 0.9°	无（单点）或固定小视场角
输出数据	距离、角度、信号质量	距离、信号强度
接口	UART（串口）	UART（串口）
核心优势	获取周围360°点云，环境感知全面	数据简单，响应快，无运动部件更可靠
主要局限	有旋转电机，抗振动性稍弱，功耗较高	只能感知正前方狭窄区域，存在“隧道视野”问题
参考价格	约 600 - 800 元	约 200 - 300 元

我的选择思路是这样的：如果你的无人机主要用于室内复杂环境探索，需要全面感知周围障碍物，那么RPLIDAR A1提供的360°点云数据无可替代。但它的旋转机构在无人机高频振动下可能是个隐患，需要做好减震。如果应用场景是室外空旷环境下的定高巡航或简单前向避障，TFmini这类固态雷达则更合适，它没有活动部件，更皮实，数据处理也更简单。

超声波传感器则是激光雷达的完美补充。它成本极低（几十元），对玻璃、纯黑物体等激光雷达不友好的表面有很好的检测效果。但其波束角宽，易受环境噪声干扰，测距精度和频率也较低。因此，我们的策略是：用激光雷达做主力探测和地图构建，用超声波做近距离（如1米内）的冗余校验和特定材质障碍物兜底。

硬件连接上，STM32F4或H7系列是理想的主控。它们拥有多个UART接口，可以轻松同时连接激光雷达和超声波。一个典型的接线示例如下：

// 假设使用STM32CubeMX配置 // Laser UART - USART2 #define LASER_UART_HANDLE huart2 // Ultrasonic UART (或GPIO触发) - USART3 #define ULTRASONIC_UART_HANDLE huart3 // 超声波模块若使用GPIO触发，则还需定义Trig和Echo引脚 #define US_TRIG_GPIO_Port GPIOA #define US_TRIG_Pin GPIO_PIN_0 #define US_ECHO_GPIO_Port GPIOA #define US_ECHO_Pin GPIO_PIN_1

电源方面，务必注意RPLIDAR A1的工作电压是5V，而STM32的IO口是3.3V电平。你需要一个电平转换模块，或者选择支持3.3V UART的版本。TFmini通常是3.3V/5V兼容的，直接连接即可。整个系统的供电要稳定，建议为雷达单独供电或使用大电流的稳压模块，避免电机启动时电压跌落导致雷达重启。

2. 多源数据同步与预处理：让传感器“对齐”说话

当激光雷达在高速旋转扫描，超声波在间歇性发出声波时，它们产生的数据在时间和空间上都是不同步的。直接使用这些原始数据，就像试图用一张模糊的旧地图和一句过时的路况广播来开车，非常危险。因此，数据同步是融合系统的基石。

时间同步是第一步。最直接的方法是利用STM32的硬件定时器，为每一个收到的数据包打上精确的时间戳（微秒级）。对于激光雷达的每个扫描点，对于超声波的每次测距结果，都记录下它们被主控读取的时刻。这样，我们在后续处理时，就能知道哪些数据是“同一时刻”或“最近时刻”的环境快照。

// 为传感器数据定义带时间戳的结构体 typedef struct { float distance_mm; float angle_deg; uint32_t timestamp_us; // 来自定时器 TIM2->CNT 的微秒时间戳 } LidarPoint_t; typedef struct { float distance_mm; uint32_t timestamp_us; } UltrasonicData_t;

空间同步则更关键。激光雷达通常安装在无人机中心，超声波则可能装在机头或机腹。它们的位置不同，看到的“世界”坐标也就不同。我们需要建立一个统一的机体坐标系（Body Frame），通常以无人机重心为原点，机头方向为X轴正方向，右侧为Y轴正方向，垂直向下为Z轴正方向。

假设超声波传感器安装在机头正前方X轴+0.1米，下方Z轴-0.05米处。那么，超声波测得的距离d_us，需要转换到机体坐标系下的坐标(x_us, y_us, z_us)：

• x_us = d_us + 0.1（障碍物在超声波前方的距离加上安装偏移）
• y_us = 0（假设正前方安装，Y方向无偏移）
• z_us = -0.05（安装位置低于重心）

激光雷达的数据本身带有角度信息，转换更直接。对于一个扫描点，其极坐标(d_lidar, θ)转换到机体坐标系：

• x_lidar = d_lidar * cos(θ)
• y_lidar = d_lidar * sin(θ)
• z_lidar = 0（二维雷达默认在同一水平面）

注意：这里的角度θ需要根据雷达的安装方向进行零点校准。例如，如果雷达的0度方向指向机头，则直接使用；如果指向机尾，则θ需要加上180度。

预处理环节，我们还需要对原始数据进行“清洗”。激光雷达的点云中常包含因镜面反射、阳光直射或超出量程产生的无效点（距离值为0或极大值）。超声波数据则容易因多次反射或噪声干扰产生跳变。一个简单的滤波策略是设置合理的有效范围，并采用滑动窗口中值滤波。

#define LIDAR_DISTANCE_MIN 50 // 最小有效距离 50mm #define LIDAR_DISTANCE_MAX 8000 // 最大有效距离 8000mm #define US_DISTANCE_MIN 20 #define US_DISTANCE_MAX 4000 // 滑动窗口中值滤波示例（用于超声波） float median_filter_ultrasonic(float new_sample) { static float buffer[5] = {0}; static uint8_t index = 0; float temp_buf[5]; buffer[index] = new_sample; index = (index + 1) % 5; // 拷贝到临时数组排序 memcpy(temp_buf, buffer, sizeof(buffer)); // 简单的冒泡排序找中值（实际可用更高效算法） for(int i=0; i<4; i++) { for(int j=0; j<4-i; j++) { if(temp_buf[j] > temp_buf[j+1]) { float temp = temp_buf[j]; temp_buf[j] = temp_buf[j+1]; temp_buf[j+1] = temp; } } } return temp_buf[2]; // 返回中值 }

经过时间和空间对齐、数据清洗后，来自两个传感器的数据才具备了“对话”的基础，可以送入融合算法进行下一步处理。

3. 卡尔曼滤波实战：融合数据与预测轨迹

传感器数据即使经过了清洗和同步，依然存在噪声。激光雷达在远距离或强光下精度会下降，超声波的数据更新慢且易受干扰。更棘手的是，无人机和障碍物都在运动，我们需要的不只是当前时刻的距离，更是对障碍物未来位置的预测。这时，卡尔曼滤波就成了我们的核心工具。别被它的数学公式吓到，我们可以把它理解为一个“聪明的数据融合与预测器”。

卡尔曼滤波主要做两件事：

1. 预测（Predict）：根据无人机自身的运动状态（速度、角速度），预测下一时刻各个障碍物可能的位置。
2. 更新（Update）：当新的传感器数据到来时，将预测值与测量值进行加权融合，得到更优的估计值，并更新对自身预测不确定性的评估。

在我们的系统中，可以为每一个被跟踪的障碍物（或空间中的一个关注点）维护一个卡尔曼滤波器。状态向量通常包括位置和速度。这里，我们以实现一个简单的一维距离跟踪为例（比如用超声波跟踪正前方的障碍物）：

// 一维卡尔曼滤波器结构体 typedef struct { float x; // 状态估计值：距离 float v; // 状态估计值：距离变化率（速度） float P[2][2]; // 估计误差协方差矩阵 float Q[2][2]; // 过程噪声协方差（系统不确定性） float R; // 测量噪声协方差（传感器不确定性） } KalmanFilter1D_t; // 卡尔曼滤波初始化 void KalmanFilter_Init(KalmanFilter1D_t *kf, float init_distance, float init_velocity) { kf->x = init_distance; kf->v = init_velocity; // 初始化误差协方差，表示初始估计的不确定性 kf->P[0][0] = 1000.0f; kf->P[0][1] = 0.0f; kf->P[1][0] = 0.0f; kf->P[1][1] = 1000.0f; // 过程噪声，根据系统模型调整 kf->Q[0][0] = 0.1f; kf->Q[0][1] = 0.0f; kf->Q[1][0] = 0.0f; kf->Q[1][1] = 0.1f; // 测量噪声，根据传感器精度调整 kf->R = 50.0f; // 假设超声波测量噪声方差为50mm^2 } // 预测步骤 void KalmanFilter_Predict(KalmanFilter1D_t *kf, float dt) { // 状态预测: x = x + v*dt kf->x = kf->x + kf->v * dt; // 误差协方差预测: P = F*P*F^T + Q // 对于匀速模型，状态转移矩阵 F = [[1, dt], [0, 1]] float F[2][2] = {{1, dt}, {0, 1}}; // 这里简化计算，实际需进行矩阵运算。可以使用CMSIS-DSP库的矩阵函数。 // 为简化示例，我们仅做概念性更新： kf->P[0][0] += (kf->P[0][1] + kf->P[1][0]) * dt + kf->P[1][1] * dt * dt + kf->Q[0][0]; kf->P[0][1] += kf->P[1][1] * dt + kf->Q[0][1]; kf->P[1][0] += kf->P[1][1] * dt + kf->Q[1][0]; kf->P[1][1] += kf->Q[1][1]; } // 更新步骤 void KalmanFilter_Update(KalmanFilter1D_t *kf, float measurement) { // 计算卡尔曼增益 K = P * H^T / (H * P * H^T + R) // 测量矩阵 H = [1, 0]，因为我们只测量距离 float S = kf->P[0][0] + kf->R; // 创新协方差 float K[2] = {kf->P[0][0] / S, kf->P[1][0] / S}; // 卡尔曼增益 // 计算测量残差 float y = measurement - kf->x; // 状态更新: x = x + K * y kf->x = kf->x + K[0] * y; kf->v = kf->v + K[1] * y; // 误差协方差更新: P = (I - K*H) * P float P00_new = (1 - K[0]) * kf->P[0][0]; float P01_new = (1 - K[0]) * kf->P[0][1]; float P10_new = -K[1] * kf->P[0][0] + kf->P[1][0]; float P11_new = -K[1] * kf->P[0][1] + kf->P[1][1]; kf->P[0][0] = P00_new; kf->P[0][1] = P01_new; kf->P[1][0] = P10_new; kf->P[1][1] = P11_new; }

在实际应用中，我们会在主循环或定时器中断中，以固定周期（如20ms）调用KalmanFilter_Predict。每当超声波或激光雷达的有效数据到来，就调用KalmanFilter_Update。经过滤波后的距离值kf->x会平滑许多，并且包含了速度信息kf->v，我们可以利用这个速度来预测未来几十毫秒内障碍物的位置，为避障决策留出宝贵的反应时间。

对于激光雷达的二维点云，处理思路类似但更复杂。我们可以将扫描平面划分为多个扇形区域（例如，将前方180度划分为12个15度的扇区），对每个扇区内的点进行聚类，并为其中心点维护一个二维的卡尔曼滤波器（状态向量包含x, y, vx, vy）。这样，我们就能同时跟踪多个障碍物的位置和运动趋势。

4. FreeRTOS任务划分与系统集成：让代码优雅地并行跑起来

一个实时性要求高的避障系统，传感器数据读取、滤波算法、避障决策、电机控制等任务必须并行不悖。用裸机的超级循环（super loop）很难保证时效性，而FreeRTOS正是解决这一问题的利器。它能让我们的STM32像一个小型操作系统一样，合理地调度多个任务。

我们的系统可以划分为以下几个核心任务，并为每个任务分配合适的优先级和堆栈大小：

1. 激光雷达数据采集任务 (LidarTask)：

   • 优先级：高（因为数据流连续，不能丢失数据包）
   • 堆栈：建议2KB以上
   • 功能：通过UART DMA或中断方式持续读取激光雷达数据包，解析出距离和角度，存入一个线程安全的环形缓冲区（Queue）。这个任务只负责“搬数据”，不做复杂计算。

2. 超声波数据采集任务 (UltrasonicTask)：

   • 优先级：中
   • 堆栈：1KB左右
   • 功能：定时（例如50ms一次）触发超声波模块，等待回波并计算距离，将结果放入另一个队列。如果使用UART型超声波，则通过串口接收数据。

3. 数据融合与避障决策任务 (FusionDecisionTask)：

   • 优先级：最高（决策直接影响安全）
   • 堆栈：需要较大，4KB或更多，因为包含滤波算法和地图运算。
   • 功能：这是系统的大脑。它从激光雷达和超声波的队列中取出数据，进行时间戳对齐、坐标转换、卡尔曼滤波更新。然后，基于处理后的环境地图（例如极坐标栅格地图），运行避障算法（如向量场直方图VFH的简化版，或简单的“最近障碍物”法则），生成飞行指令（如偏航角、速度调整）。

4. 电机控制任务 (MotorControlTask)：

   • 优先级：非常高（仅次于决策，需要及时响应）
   • 堆栈：1-2KB
   • 功能：接收来自决策任务的飞行指令，结合姿态控制器（如PID）的输出，计算四个电机的PWM占空比，并更新定时器输出。

5. 状态监控与调试任务 (MonitorTask)：

   • 优先级：最低
   • 堆栈：2KB
   • 功能：通过串口将传感器数据、滤波结果、系统状态等信息发送到上位机（如PC端的PlotJuggler或自己写的Qt工具），方便调试。也可以驱动一个OLED小屏幕显示关键信息。

在CubeMX中配置FreeRTOS并创建这些任务非常直观。关键在于任务间通信必须使用FreeRTOS提供的原语，如队列（Queue）、信号量（Semaphore）、任务通知（Task Notification）等，避免使用全局变量直接共享数据，否则会引发竞态条件。

// 在CubeMX中启用FreeRTOS，并创建任务 void StartDefaultTask(void const * argument) { // 通常用作初始化或低优先级任务 for(;;) { osDelay(1000); } } // 激光雷达任务函数示例 void LidarTask(void const * argument) { LidarPoint_t point; for(;;) { if(LIDAR_ParseOnePoint(&point)) { // 解析到一个有效点 // 将数据点发送到融合任务的队列 if(xQueueSend(fusionQueue, &point, 10) != pdPASS) { // 队列满，处理错误（如丢弃最旧数据或增加队列长度） } } osDelay(1); // 短暂释放CPU } } // 融合决策任务函数示例 void FusionDecisionTask(void const * argument) { LidarPoint_t lidar_point; UltrasonicData_t us_data; BaseType_t xStatus; for(;;) { // 尝试从激光雷达队列获取数据（非阻塞） xStatus = xQueueReceive(lidarQueue, &lidar_point, 0); if(xStatus == pdPASS) { // 处理激光雷达点云... updateOccupancyGrid(&lidar_point); } // 尝试从超声波队列获取数据（非阻塞） xStatus = xQueueReceive(ultrasonicQueue, &us_data, 0); if(xStatus == pdPASS) { // 处理超声波数据... kalman_update_front(&us_data); } // 每50ms执行一次决策 static uint32_t last_decision_tick = 0; if((osKernelSysTick() - last_decision_tick) >= 50) { last_decision_tick = osKernelSysTick(); run_obstacle_avoidance_algorithm(); send_command_to_motor_task(); } osDelay(5); // 任务主循环延时 } }

提示：务必使用osDelay()而非HAL_Delay()，这样在延时期间其他任务可以运行。合理设置任务优先级，确保高实时性任务（如电机控制）能及时抢占低优先级任务（如调试输出）。

5. 避障策略与代码实战：从地图到动作

当所有数据准备就绪，最后一个问题就是：无人机该如何行动？一个简单粗暴的“遇到障碍就停”策略显然不够智能。我们需要一个能根据环境信息生成平滑、安全运动指令的策略。

一个在创客项目中非常有效的策略是人工势场法的简化版。其核心思想是：目标点对无人机产生“引力”，障碍物产生“斥力”，无人机的运动方向由合力决定。

• 引力：方向指向目标点，大小与距离成正比。
• 斥力：方向远离障碍物，大小与距离成反比（距离越近，斥力越大）。

我们可以用激光雷达构建一个极坐标直方图，将前方180度空间分成N个扇区。对于每个扇区，根据其内障碍物的最近距离计算一个“危险度”。决策时，无人机优先选择危险度低的扇区飞行，并适当调整速度。

下面是一个极度简化的决策函数示例，它只考虑正前方、左前、右前三个扇区：

typedef enum { DIRECTION_FRONT = 0, DIRECTION_LEFT, DIRECTION_RIGHT, DIRECTION_COUNT } Direction_t; float danger_level[DIRECTION_COUNT] = {0}; // 三个方向的危险度 void simple_avoidance_decision(float *desired_yaw_rate, float *desired_speed) { // 假设我们已经根据雷达数据计算好了 danger_level[] // danger_level 值越大代表越危险（0-1之间） const float SAFE_DISTANCE = 1000.0f; // 1米为安全距离 const float MAX_YAW_RATE = 30.0f; // 最大偏航角速度 度/秒 const float BASE_SPEED = 0.5f; // 基础前进速度 m/s // 1. 速度决策：如果正前方危险度很高，减速甚至停止 if (danger_level[DIRECTION_FRONT] > 0.8f) { *desired_speed = 0.0f; // 紧急停止 } else if (danger_level[DIRECTION_FRONT] > 0.3f) { *desired_speed = BASE_SPEED * (1.0f - danger_level[DIRECTION_FRONT]); // 线性减速 } else { *desired_speed = BASE_SPEED; } // 2. 方向决策：比较左右危险度，决定转向 float yaw_bias = 0.0f; if (danger_level[DIRECTION_LEFT] > danger_level[DIRECTION_RIGHT]) { // 左边更危险，向右转 yaw_bias = danger_level[DIRECTION_LEFT] * MAX_YAW_RATE; } else if (danger_level[DIRECTION_RIGHT] > danger_level[DIRECTION_LEFT]) { // 右边更危险，向左转 yaw_bias = -danger_level[DIRECTION_RIGHT] * MAX_YAW_RATE; } // 如果正前方危险度最高，且左右差不多，则加强转向幅度 if (danger_level[DIRECTION_FRONT] > 0.5f && fabs(danger_level[DIRECTION_LEFT] - danger_level[DIRECTION_RIGHT]) < 0.2f) { // 默认向右转（或根据其他策略） yaw_bias = MAX_YAW_RATE * 0.5f; } *desired_yaw_rate = yaw_bias; }

这个函数输出的desired_yaw_rate和desired_speed就是给底层飞行控制器的指令。底层控制器（通常是姿态PID控制器）会努力让无人机达到这个偏航角速度和前进速度。

最后，将所有这些模块集成起来。我为你准备了一个基于STM32CubeIDE和HAL库的完整工程框架，你可以通过以下链接获取（请根据你的具体硬件修改引脚定义和参数）：

// 工程主要文件结构 Project/ ├── Core/ │ ├── Inc/ │ │ ├── lidar.h // 激光雷达驱动 │ │ ├── ultrasonic.h // 超声波驱动 │ │ ├── kalman_filter.h // 卡尔曼滤波器 │ │ ├── obstacle_map.h // 障碍物地图 │ │ └── avoidance.h // 避障决策 │ ├── Src/ │ │ ├── lidar.c │ │ ├── ultrasonic.c │ │ ├── kalman_filter.c │ │ ├── obstacle_map.c │ │ ├── avoidance.c │ │ └── freertos.c (由CubeMX生成) │ └── Src/main.c // 主函数，任务创建和初始化 ├── Drivers/ ├── Middlewares/FreeRTOS/ └── .ioc // CubeMX配置文件

在main.c中，初始化所有硬件和外设后，创建FreeRTOS任务并启动调度器：

int main(void) { HAL_Init(); SystemClock_Config(); MX_GPIO_Init(); MX_USART2_UART_Init(); // 激光雷达串口 MX_USART3_UART_Init(); // 超声波串口 MX_TIM2_Init(); // 用于高精度时间戳的定时器 MX_FREERTOS_Init(); // 初始化FreeRTOS内核 // 创建队列 lidarQueue = xQueueCreate(200, sizeof(LidarPoint_t)); ultrasonicQueue = xQueueCreate(10, sizeof(UltrasonicData_t)); commandQueue = xQueueCreate(5, sizeof(FlightCommand_t)); // 创建任务（任务函数在freertos.c中定义） osThreadDef(LidarTask_Handle, LidarTask, osPriorityHigh, 0, 2048); lidarTaskHandle = osThreadCreate(osThread(LidarTask_Handle), NULL); // ... 创建其他任务 osKernelStart(); // 启动调度器，永不返回 while (1) {} }

在实际飞行测试前，务必先进行地面静态测试。用串口工具将处理后的数据实时发送到电脑上可视化，确保激光雷达能正确扫描出房间轮廓，超声波能稳定测距，卡尔曼滤波的输出平滑合理。然后进行系留测试（用绳子拴住无人机），验证避障决策逻辑是否按预期触发转向或刹车。最后才是谨慎的自主飞行测试。

调试这种多传感器系统，逻辑分析仪和一台好的示波器能帮你省下大量时间，尤其是检查UART通信时序和中断响应。另外，记得给代码加上丰富的调试输出宏，方便在运行时通过串口打印关键变量和状态机信息。