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

STM32无人机避障实战从零搭建激光雷达超声波融合系统附完整代码去年夏天我在调试一台四轴飞行器时差点让它一头撞上阳台的晾衣杆。那次经历让我深刻意识到对于创客和嵌入式开发者来说一套可靠且成本可控的避障系统有多重要。市面上的高端无人机往往配备昂贵的深度摄像头或固态激光雷达但对于我们这些喜欢自己动手、预算有限的开发者如何用几百元的传感器组合实现稳定避障才是真正的挑战。这篇文章我想和你分享一个我实际验证过的方案基于STM32融合低成本二维激光雷达比如RPLIDAR A1和超声波模块构建一套实时性高、鲁棒性强的无人机避障系统。我们不会空谈理论而是聚焦于工程实现从传感器选型对比、数据同步策略到卡尔曼滤波的实战应用再到如何在FreeRTOS里合理划分任务最后提供可直接编译、下载的完整工程代码。我们的目标很明确用最“接地气”的硬件实现不输于商业方案的避障能力。1. 传感器选型与硬件架构设计在成本与性能间寻找平衡点为无人机选择避障传感器就像为它挑选“眼睛”需要权衡视野、精度、速度、功耗当然还有最重要的——成本。对于创客项目我们通常不会直接选用数万元的工业级激光雷达而是在几百元到一千多元的消费级产品中寻找最优解。激光雷达负责提供二维平面上的高精度距离与角度信息是避障系统的“主力”。市面上常见的低成本型号主要有两种RPLIDAR A1/A1M8和Benewake TFmini/TF02。它们原理不同适用场景也各异。为了让你更直观地对比我把两款雷达的核心参数整理成了表格特性RPLIDAR A1/A1M8Benewake 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 i0; i4; i) { for(int j0; j4-i; j) { if(temp_buf[j] temp_buf[j1]) { float temp temp_buf[j]; temp_buf[j] temp_buf[j1]; temp_buf[j1] temp; } } } return temp_buf[2]; // 返回中值 }经过时间和空间对齐、数据清洗后来自两个传感器的数据才具备了“对话”的基础可以送入融合算法进行下一步处理。3. 卡尔曼滤波实战融合数据与预测轨迹传感器数据即使经过了清洗和同步依然存在噪声。激光雷达在远距离或强光下精度会下降超声波的数据更新慢且易受干扰。更棘手的是无人机和障碍物都在运动我们需要的不只是当前时刻的距离更是对障碍物未来位置的预测。这时卡尔曼滤波就成了我们的核心工具。别被它的数学公式吓到我们可以把它理解为一个“聪明的数据融合与预测器”。卡尔曼滤波主要做两件事预测Predict根据无人机自身的运动状态速度、角速度预测下一时刻各个障碍物可能的位置。更新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像一个小型操作系统一样合理地调度多个任务。我们的系统可以划分为以下几个核心任务并为每个任务分配合适的优先级和堆栈大小激光雷达数据采集任务 (LidarTask)优先级高因为数据流连续不能丢失数据包堆栈建议2KB以上功能通过UART DMA或中断方式持续读取激光雷达数据包解析出距离和角度存入一个线程安全的环形缓冲区Queue。这个任务只负责“搬数据”不做复杂计算。超声波数据采集任务 (UltrasonicTask)优先级中堆栈1KB左右功能定时例如50ms一次触发超声波模块等待回波并计算距离将结果放入另一个队列。如果使用UART型超声波则通过串口接收数据。数据融合与避障决策任务 (FusionDecisionTask)优先级最高决策直接影响安全堆栈需要较大4KB或更多因为包含滤波算法和地图运算。功能这是系统的大脑。它从激光雷达和超声波的队列中取出数据进行时间戳对齐、坐标转换、卡尔曼滤波更新。然后基于处理后的环境地图例如极坐标栅格地图运行避障算法如向量场直方图VFH的简化版或简单的“最近障碍物”法则生成飞行指令如偏航角、速度调整。电机控制任务 (MotorControlTask)优先级非常高仅次于决策需要及时响应堆栈1-2KB功能接收来自决策任务的飞行指令结合姿态控制器如PID的输出计算四个电机的PWM占空比并更新定时器输出。状态监控与调试任务 (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通信时序和中断响应。另外记得给代码加上丰富的调试输出宏方便在运行时通过串口打印关键变量和状态机信息。