前言在上一篇《让空间感知更完美自带陀螺仪的毫米波雷达》中我们完成了模组硬件验证与SDK烧录打通。本文将进入实战阶段——点云采集、雷达的一些基础算法简单版本、动态帧率控制、以及有目标/无目标两种场景下的功耗实测。一、核心思路动态帧率降功耗毫米波雷达的功耗与帧率成正比。对于电池供电的场景我们需要一种策略场景帧率说明有人活动50ms20fps需要高帧率跟踪人体微动无人静止500ms2fps仅做有无检测省电目标通过动态帧率切换可以有效的看到电流变小其中最低点符合RS6130待机10uA的描述最终目标是实现锂电池长期供电。二、代码实现动态帧率控制系统2.1 核心机制代码中实现了一套完整的帧率自适应系统mmw_frame_cb (每帧回调) ↓ 检测是否有目标通过点云聚类 多目标跟踪 ↓ 有目标 → 请求恢复 50ms 帧率 无目标 → 累加计数器连续 N 帧无目标 → 请求降速到 500ms ↓ mmw_entry_cb (下一帧入口回调) ↓ 执行实际的帧率切换stop → cfg → start2.2 关键代码解读帧率切换请求在 mmw_frame_cb 中if (active_track_count 0) { no_target_frame_cnt 0; if (curr_period_ms 0) { mmw_frame_get(curr_period_ms, frame_num); } if (curr_period_ms FRAME_PERIOD_SLOW_MS) { target_confirm_cnt; if (target_confirm_cnt TARGET_RESTORE_CNT) { g_pending_frame_period_ms FRAME_PERIOD_NORMAL_MS; curr_period_ms FRAME_PERIOD_NORMAL_MS; target_confirm_cnt 0; printk(mmw_frame_cb: target confirmed %d frames, restore to %dms\n, TARGET_RESTORE_CNT, FRAME_PERIOD_NORMAL_MS); } } return 0; }实际执行切换在 mmw_entry_cb 中int mmw_entry_cb(uint32_t frame_idx) { (void)frame_idx; if (g_pending_frame_period_ms 0) { return 0; /* 无待处理切换 */ } uint32_t target_period g_pending_frame_period_ms; /* * mmw_frame_cfg 在控制器 RUNNING 状态下会返回错误码。 * 必须先 stop配置完后再 start。 * 在 entry 回调中执行比 post 回调安全 * entry 回调执行时当前帧的 HIF 数据尚未生成不会打断流水线。 */ int ret mmw_ctrl_stop(); if (ret ! 0) { printk(mmw_entry_cb: mmw_ctrl_stop FAILED, err%d, retry next frame\n, ret); return 0; } ret mmw_frame_cfg(target_period, 0); if (ret ! 0) { printk(mmw_entry_cb: frame_cfg(%dms) FAILED, err%d, restarting...\n, target_period, ret); mmw_ctrl_start(); /* 即使 cfg 失败也要重启避免雷达完全停止 */ return 0; } ret mmw_ctrl_start(); if (ret ! 0) { printk(mmw_entry_cb: mmw_ctrl_start FAILED, err%d\n, ret); return 0; } printk(mmw_entry_cb: frame rate switched to %dms\n, target_period); g_pending_frame_period_ms 0; return 0; }为什么不在 mmw_frame_cb 中直接切换因为此时 HIF 传输管道正在发送本帧数据重配置会打断流水线同步。改为在 mmw_entry_cb下一帧入口处理前调用此时协处理器处于可接受配置的状态。2.3 防抖设计为了避免单帧误判导致频繁切换代码加入了防抖计数器#define NO_TARGET_SLOWDOWN_CNT 10 /* 连续10帧无目标才降速到500ms防抖动 */ #define TARGET_RESTORE_CNT 3 /* 连续3帧有目标才恢复50ms防单帧误判 */这意味着从50ms降到500ms需要10帧 × 50ms 0.5秒 持续无目标从500ms恢复到50ms需要3帧 × 500ms 1.5秒 持续有目标三、功耗实测数据3.1 测试条件项目参数模组60GHz RS6130 9轴IMU供电3.7V锂电池通过LDO稳压至3.3V测试仪器万用表串联测电流 示波器抓取瞬态测试场景室内办公室距离雷达2~4m3.2 有目标场景50ms帧率实测数据表明在有目标的高帧率模式下电流曲线呈现明显的周期性脉冲特征平均电流约为45mA级别。3.3 无目标场景500ms帧率在无目标的低帧率模式下平均电流4mA以内相比高帧率模式降低了一个数量级以上。3.4 功耗对比总结场景相对功耗固定50ms始终高帧率基准100%固定500ms始终低帧率大幅降低动态切换本文方案介于两者之间取决于有目标占比四、功耗优化方向产品化建议目前的动态帧率方案已显著降低了无目标场景的功耗。但要达到锂电池长期供电 的目标还需要以下优化4.1 深度休眠模式无目标时长时间待机预期可将空闲电流降至极低水平4.2 点云数量裁剪无目标时仅开启1T1R关闭两个接收通道进行有无判断减少FFT点数预期可降低发射处理电流4.3 IMU辅助休眠利用陀螺仪检测环境振动过滤不必要的唤醒4.4 预期优化路线优化步骤效果当前方案动态帧率基础省电深度休眠无目标时电流大幅下降点云裁剪有目标时电流进一步降低IMU辅助更智能的休眠唤醒策略终极目标实现锂电池长期供电五、完整源码本文涉及的核心代码已在文章中分段讲解。完整工程文件包括SDK配置、点云处理、多目标跟踪、帧率控制可联系作者获取。以下是关键性代码更换SDK中内容实测可运行/** ****************************************************************************** * file applications.c * brief 由 radar_framework 调用的开发者回调函数 * note 包含动态帧率控制、点云聚类、多目标跟踪、多径分析 ****************************************************************************** */ #include mmw_ctrl.h #include mmw_type.h #include log.h #include math.h #include prj_config.h #include mmw_app_pointcloud_config.h #include mrs6130_ddmer_config.h #include hal_os.h #include osi_common.h #include osi_thread.h /* 点云聚类参数 */ #define TARGET_CLUSTER_SIZE_CM 50.0f /* 聚类半径50cm。之前20cm太严苛目标短暂消失时难恢复高帧率 */ #define TARGET_MIN_POINTS 5 /* 最少5个点才视为有效聚类过滤零星噪声 */ #define HIGH_ACTIVITY_POINTS 30 /* 超过30个点大幅运动立即恢复50ms帧率无需防抖 */ /* 动态帧率参数 */ #define NO_TARGET_SLOWDOWN_CNT 10 /* 连续10帧无目标才降速到500ms防抖动 */ #define TARGET_RESTORE_CNT 3 /* 连续3帧有目标才恢复50ms防单帧误判 */ #define FRAME_PERIOD_NORMAL_MS 50 /* 有人20fps高帧率跟踪 */ #define FRAME_PERIOD_SLOW_MS 500 /* 无人2fps低帧率省电 */ /* 多目标跟踪参数 */ #define MAX_TRACKED_TARGETS 5 /* 最多同时跟踪5个目标家用场景足够 */ #define MAX_CLUSTERS 5 /* 单帧最多5个聚类 */ #define TRACK_COAST_LIMIT 10 /* 目标丢失后滑行10帧约0.5s50ms/5s500ms再删除 */ #define TRACK_CONFIRM_FRAMES 3 /* 连续匹配3帧才确认为真目标 */ #define TRACK_ASSOC_GATE_CM 80.0f /* 聚类与轨迹关联门限80cm。比聚类半径大允许目标小幅移动 */ #define ALPHA_TRACK 0.7f /* Alpha-Beta滤波器位置平滑系数越大越相信新测量 */ #define BETA_TRACK 0.3f /* 速度估计系数越小速度变化越平滑 */ /* 目标状态机 */ #define TRACK_STATE_EMERGING 0 /* 新兴刚创建未确认 */ #define TRACK_STATE_CONFIRMED 1 /* 已确认连续匹配 CONFIRM_FRAMES */ #define TRACK_STATE_COASTING 2 /* 滑行已确认但本帧无匹配可能进入静止 */ /* 多径标记 */ #define MP_TAG_NONE 0 #define MP_TAG_WIDE_ANGLE 1 /* 大方位角 |azi_phase|0.7可能墙壁反射 */ #define MP_TAG_RANGE_MIRROR 2 /* 距离镜像角度相近但距离为聚类1.5x~2.5x */ #define MP_TAG_LOW_SNR 3 /* 低SNR比聚类平均SNR低10dB以上 */ #define MP_TAG_ISOLATED 4 /* 孤立点周围30cm内无其他点 */ /* * pending_frame_period_ms跨回调通信的标志位 * * 设计原因mmw_frame_cfg 不能在 mmw_frame_cb 中直接调用 * 因为此时 HIF 传输管道正在发送本帧数据重配置会打断流水线同步。 * 改为在 mmw_entry_cb下一帧入口处理前调用此时协处理器可接受配置。 * * 写入者mmw_frame_cb * 读取并清零者mmw_entry_cb */ static uint32_t g_pending_frame_period_ms 0; /* 追踪目标结构体 */ /* 用 Alpha-Beta 滤波器平滑质心位置和速度估计 */ typedef struct { uint8_t active; /* 0空槽位, 1使用中 */ uint8_t id; /* 目标ID从1开始递增永不回收 */ uint8_t state; /* EMERGING / CONFIRMED / COASTING */ float pos_x, pos_y, pos_z; /* 滤波质心位置 (cm) */ float vel_x, vel_y, vel_z; /* 估计速度 (cm/帧) */ uint16_t point_count; /* 最近匹配点数 */ uint16_t coast_frames; /* 连续未匹配帧数 */ uint16_t confirm_frames; /* 连续匹配帧数 */ uint16_t total_frames; /* 累计跟踪帧数 */ float extent_x, extent_y, extent_z; /* 最近聚类包围盒 (cm) */ float centroid_range; /* 最近聚类平均距离 (cm) */ float avg_snr; /* 最近聚类平均 SNR (dB) */ } TrackedTarget; /* 聚类信息结构体 */ typedef struct { float cx, cy, cz; /* 质心 (cm) */ float ext_x, ext_y, ext_z; /* 包围盒 (cm) */ uint16_t point_count; /* 点数 */ uint16_t member_indices[20]; /* 成员在点云中的原始索引 */ uint16_t member_count; float avg_snr; /* 平均 SNR (dB) */ float avg_range; /* 平均距离 (cm) */ } Cluster; /* 全局追踪目标列表 */ static TrackedTarget g_tracks[MAX_TRACKED_TARGETS]; static uint8_t g_next_track_id 1; /* 辅助两点三维欧氏距离 (cm) */ static inline float track_dist3d(float x1, float y1, float z1, float x2, float y2, float z2) { float dx x1 - x2, dy y1 - y2, dz z1 - z2; return sqrtf(dx * dx dy * dy dz * dz); } /* * mmw_entry_cb每帧入口回调 * 在帧处理开始前调用负责执行待处理的帧率切换 * */ int mmw_entry_cb(uint32_t frame_idx) { (void)frame_idx; if (g_pending_frame_period_ms 0) { return 0; /* 无待处理切换 */ } uint32_t target_period g_pending_frame_period_ms; /* * mmw_frame_cfg 在控制器 RUNNING 状态下会返回错误码。 * 必须先 stop配置完后再 start。 * 在 entry 回调中执行比 post 回调安全 * entry 回调执行时当前帧的 HIF 数据尚未生成不会打断流水线。 */ int ret mmw_ctrl_stop(); if (ret ! 0) { printk(mmw_entry_cb: mmw_ctrl_stop FAILED, err%d, retry next frame\n, ret); return 0; } ret mmw_frame_cfg(target_period, 0); if (ret ! 0) { printk(mmw_entry_cb: frame_cfg(%dms) FAILED, err%d, restarting...\n, target_period, ret); mmw_ctrl_start(); /* 即使 cfg 失败也要重启避免雷达完全停止 */ return 0; } ret mmw_ctrl_start(); if (ret ! 0) { printk(mmw_entry_cb: mmw_ctrl_start FAILED, err%d\n, ret); return 0; } printk(mmw_entry_cb: frame rate switched to %dms\n, target_period); g_pending_frame_period_ms 0; return 0; } /* * mmw_frame_cb每帧数据处理回调 * 包含点云预处理、聚类、多径分析、多目标跟踪、动态帧率决策 * */ __sram_text int mmw_frame_cb(uint8_t argc, void* arg[]) { /* 静态计数器跨帧保持 */ static uint32_t no_target_frame_cnt 0; /* 连续无目标帧计数 */ static uint32_t target_confirm_cnt 0; /* 连续有目标帧计数防抖 */ static uint32_t curr_period_ms 0; /* 当前帧周期缓存避免重复查询 */ uint32_t frame_num; PointCloudBuffer_t *pc (PointCloudBuffer_t*)arg[1]; uint16_t point_num pc-point_cloud_num; /* 跨 goto 使用的变量提前声明 */ uint16_t xyz_count 0; uint16_t total_clusters 0; float min_range 1e6f, max_range -1e6f; float best_band_center 0.0f; uint16_t mp_counts[5] {0}; /* 多径标签计数 */ Cluster clusters[MAX_CLUSTERS]; uint16_t active_track_count 0; /* ---------------------------------------------------------------- * 第1步点数为0 → 跳过点云处理直接执行跟踪更新 * ---------------------------------------------------------------- */ if (point_num 0) { goto track_update; } /* ---------------------------------------------------------------- * 第2步高活跃度检测 * 点云数量多 有人在雷达前大幅运动 → 立即恢复高帧率 * 不需要空间聚类直接判定为活跃场景 * ---------------------------------------------------------------- */ if (point_num HIGH_ACTIVITY_POINTS) { no_target_frame_cnt 0; target_confirm_cnt 0; if (curr_period_ms 0) { mmw_frame_get(curr_period_ms, frame_num); } if (curr_period_ms FRAME_PERIOD_SLOW_MS) { g_pending_frame_period_ms FRAME_PERIOD_NORMAL_MS; curr_period_ms FRAME_PERIOD_NORMAL_MS; printk(mmw_frame_cb: high activity (pts%d), restore to %dms\n, point_num, FRAME_PERIOD_NORMAL_MS); } return 0; } /* ---------------------------------------------------------------- * 第3步获取距离参数计算 range bin 对应的物理距离 * ---------------------------------------------------------------- */ uint32_t range_mm, resol_mm; uint16_t range_fft_num, doppler_fft_num; mmw_range_get(range_mm, resol_mm); mmw_fft_num_get(range_fft_num, doppler_fft_num); float range_bin_size_cm (float)range_mm / (float)range_fft_num * 0.1f; /* ---------------------------------------------------------------- * 第3.5步主导距离带投票滤除杂波点 * * 问题远距杂波256cm或大角度镜面反射的点混入目标点云48~64cm * 导致包围盒被撑大到100cm触发无目标误判。 * 解决找出点数最密集的距离带±12cm窗口仅该带内点参与后续计算。 * ---------------------------------------------------------------- */ #define RANGE_BAND_HALF_CM 12.0f uint16_t best_band_count 0; { float r_cm_cache[20]; uint16_t n_cache (point_num 20) ? 20 : point_num; for (uint16_t i 0; i n_cache; i) { r_cm_cache[i] (float)pc-ptr_motion_point_cloud_data[i].range_idx * range_bin_size_cm; } for (uint16_t i 0; i n_cache; i) { float ri r_cm_cache[i]; uint16_t cnt 0; for (uint16_t j 0; j n_cache; j) { float diff r_cm_cache[j] - ri; if (diff -RANGE_BAND_HALF_CM diff RANGE_BAND_HALF_CM) cnt; } if (cnt best_band_count) { best_band_count cnt; best_band_center ri; } } } /* ---------------------------------------------------------------- * 第4步主导距离带内的点 → 极坐标转直角坐标 * * 公式 * sin_x azi_phase → x/r 方向余弦 * sin_z ele_phase → z/r 方向余弦 * sin_y sqrt(1 - sin_x² - sin_z²) → y/r 方向余弦 * x range_cm * sin_x 横向水平 * y range_cm * sin_y 径向 * z range_cm * sin_z 高度 * ---------------------------------------------------------------- */ float x_arr[20], y_arr[20], z_arr[20], range_arr[20]; float azi_arr[20], ele_arr[20]; uint16_t orig_idx_arr[20]; for (uint16_t i 0; i point_num xyz_count 20; i) { float range_cm (float)pc-ptr_motion_point_cloud_data[i].range_idx * range_bin_size_cm; float diff range_cm - best_band_center; if (diff -RANGE_BAND_HALF_CM || diff RANGE_BAND_HALF_CM) { continue; } float sin_x pc-ptr_motion_point_cloud_data[i].azi_phase; float sin_z pc-ptr_motion_point_cloud_data[i].ele_phase; float sin_y2 1.0f - sin_x * sin_x - sin_z * sin_z; float sin_y (sin_y2 0.0f) ? sqrtf(sin_y2) : 0.0f; x_arr[xyz_count] range_cm * sin_x; y_arr[xyz_count] range_cm * sin_y; z_arr[xyz_count] range_cm * sin_z; range_arr[xyz_count] range_cm; azi_arr[xyz_count] sin_x; ele_arr[xyz_count] sin_z; if (range_cm min_range) min_range range_cm; if (range_cm max_range) max_range range_cm; orig_idx_arr[xyz_count] i; xyz_count; } /* ---------------------------------------------------------------- * 第5步贪心聚类 * 将空间邻近的点归为一类输出聚类信息供多目标跟踪使用 * ---------------------------------------------------------------- */ uint8_t visited[20] {0}; uint8_t visited_for_mp[20] {0}; for (uint16_t i 0; i xyz_count total_clusters MAX_CLUSTERS; i) { if (visited[i]) continue; float c_min_x x_arr[i], c_max_x x_arr[i]; float c_min_y y_arr[i], c_max_y y_arr[i]; float c_min_z z_arr[i], c_max_z z_arr[i]; uint16_t cluster_members[20]; uint16_t cluster_cnt 0; for (uint16_t j 0; j xyz_count; j) { if (visited[j]) continue; float dx x_arr[j] - x_arr[i]; float dy y_arr[j] - y_arr[i]; float dz z_arr[j] - z_arr[i]; if (fabsf(dx) TARGET_CLUSTER_SIZE_CM fabsf(dy) TARGET_CLUSTER_SIZE_CM fabsf(dz) TARGET_CLUSTER_SIZE_CM) { cluster_members[cluster_cnt] j; if (x_arr[j] c_min_x) c_min_x x_arr[j]; if (x_arr[j] c_max_x) c_max_x x_arr[j]; if (y_arr[j] c_min_y) c_min_y y_arr[j]; if (y_arr[j] c_max_y) c_max_y y_arr[j]; if (z_arr[j] c_min_z) c_min_z z_arr[j]; if (z_arr[j] c_max_z) c_max_z z_arr[j]; } } float ext_x c_max_x - c_min_x; float ext_y c_max_y - c_min_y; float ext_z c_max_z - c_min_z; if (cluster_cnt TARGET_MIN_POINTS ext_x TARGET_CLUSTER_SIZE_CM ext_y TARGET_CLUSTER_SIZE_CM ext_z TARGET_CLUSTER_SIZE_CM) { Cluster *cl clusters[total_clusters]; cl-cx (c_min_x c_max_x) * 0.5f; cl-cy (c_min_y c_max_y) * 0.5f; cl-cz (c_min_z c_max_z) * 0.5f; cl-ext_x ext_x; cl-ext_y ext_y; cl-ext_z ext_z; cl-point_count cluster_cnt; float sum_snr 0.0f, sum_range 0.0f; for (uint16_t k 0; k cluster_cnt; k) { uint16_t idx cluster_members[k]; uint16_t orig_i orig_idx_arr[idx]; cl-member_indices[k] orig_i; sum_snr pc-ptr_motion_point_cloud_data[orig_i].sig_snr; sum_range range_arr[idx]; visited_for_mp[idx] 1; } cl-member_count cluster_cnt; cl-avg_snr sum_snr / (float)cluster_cnt; cl-avg_range sum_range / (float)cluster_cnt; total_clusters; for (uint16_t k 0; k cluster_cnt; k) { visited[cluster_members[k]] 1; } } else { visited[i] 1; } } /* 诊断输出有点但未找到有效聚类时打印详细信息 */ if (total_clusters 0 xyz_count 0) { printk(mmw_frame_cb: no cluster (band%.0fcm, inband%d/%d, range%.0f~%.0f)\n, best_band_center, xyz_count, point_num, min_range, max_range); for (uint16_t i 0; i xyz_count; i) { printk( [%d] r%.0f x%.1f y%.1f z%.1f (azi%.3f ele%.3f snr%.1f)\n, i, range_arr[i], x_arr[i], y_arr[i], z_arr[i], azi_arr[i], ele_arr[i], pc-ptr_motion_point_cloud_data[orig_idx_arr[i]].sig_snr); } } /* ---------------------------------------------------------------- * 第6步多径分析 * 对未被聚类的带内点进行标记识别可能的虚假回波 * ---------------------------------------------------------------- */ if (xyz_count 0) { for (uint16_t i 0; i xyz_count; i) { if (visited_for_mp[i]) continue; uint8_t tag MP_TAG_NONE; uint16_t orig_i orig_idx_arr[i]; float snr pc-ptr_motion_point_cloud_data[orig_i].sig_snr; /* 检测1大方位角 → 可能墙壁反射 */ if (fabsf(azi_arr[i]) 0.7f) { tag MP_TAG_WIDE_ANGLE; } /* 检测2距离镜像 → 可能地板/天花板反射 */ if (tag MP_TAG_NONE) { for (uint16_t c 0; c total_clusters; c) { float cl_azi clusters[c].cx / (clusters[c].avg_range 0.1f ? clusters[c].avg_range : 0.1f); float azi_diff fabsf(fabsf(azi_arr[i]) - fabsf(cl_azi)); float range_ratio range_arr[i] / (clusters[c].avg_range 0.1f ? clusters[c].avg_range : 0.1f); if (azi_diff 0.25f range_ratio 1.4f range_ratio 2.8f) { tag MP_TAG_RANGE_MIRROR; break; } } } /* 检测3低SNR → 比聚类平均低10dB以上 */ if (tag MP_TAG_NONE total_clusters 0) { float weighted_snr 0.0f; uint16_t total_pts 0; for (uint16_t c 0; c total_clusters; c) { weighted_snr clusters[c].avg_snr * (float)clusters[c].point_count; total_pts clusters[c].point_count; } if (total_pts 0 snr (weighted_snr / (float)total_pts) - 10.0f) { tag MP_TAG_LOW_SNR; } } /* 检测4孤立点 → 30cm内无其他点 */ if (tag MP_TAG_NONE) { uint8_t has_neighbor 0; for (uint16_t j 0; j xyz_count; j) { if (i j) continue; if (track_dist3d(x_arr[i], y_arr[i], z_arr[i], x_arr[j], y_arr[j], z_arr[j]) 30.0f) { has_neighbor 1; break; } } if (!has_neighbor) { tag MP_TAG_ISOLATED; } } if (tag 1 tag 4) { mp_counts[tag - 1]; } } } /* ---------------------------------------------------------------- * 第7步聚类→轨迹关联贪婪最近邻匹配 * ---------------------------------------------------------------- */ track_update: { uint8_t cluster_matched[MAX_CLUSTERS] {0}; uint8_t track_assigned[MAX_TRACKED_TARGETS] {0}; /* 7a匹配已有轨迹 */ for (uint16_t c 0; c total_clusters; c) { float best_dist TRACK_ASSOC_GATE_CM; int16_t best_t -1; for (uint16_t t 0; t MAX_TRACKED_TARGETS; t) { if (!g_tracks[t].active) continue; if (track_assigned[t]) continue; float d track_dist3d( clusters[c].cx, clusters[c].cy, clusters[c].cz, g_tracks[t].pos_x, g_tracks[t].pos_y, g_tracks[t].pos_z); if (d best_dist) { best_dist d; best_t (int16_t)t; } } if (best_t 0) { cluster_matched[c] 1; track_assigned[best_t] 1; /* Alpha-Beta 滤波器更新 */ TrackedTarget *trk g_tracks[best_t]; float rx clusters[c].cx - trk-pos_x; float ry clusters[c].cy - trk-pos_y; float rz clusters[c].cz - trk-pos_z; trk-pos_x ALPHA_TRACK * rx; trk-pos_y ALPHA_TRACK * ry; trk-pos_z ALPHA_TRACK * rz; trk-vel_x BETA_TRACK * rx; trk-vel_y BETA_TRACK * ry; trk-vel_z BETA_TRACK * rz; trk-point_count clusters[c].point_count; trk-extent_x clusters[c].ext_x; trk-extent_y clusters[c].ext_y; trk-extent_z clusters[c].ext_z; trk-centroid_range clusters[c].avg_range; trk-avg_snr clusters[c].avg_snr; trk-coast_frames 0; trk-total_frames; /* 状态转移 */ if (trk-state TRACK_STATE_EMERGING) { trk-confirm_frames; if (trk-confirm_frames TRACK_CONFIRM_FRAMES) { trk-state TRACK_STATE_CONFIRMED; } } else if (trk-state TRACK_STATE_COASTING) { trk-state TRACK_STATE_CONFIRMED; trk-confirm_frames TRACK_CONFIRM_FRAMES; } else { trk-confirm_frames; } } } /* ---------------------------------------------------------------- * 第8步未匹配聚类 → 创建新轨迹 * 未匹配轨迹 → 滑行或删除 * ---------------------------------------------------------------- */ /* 8a未匹配聚类 → 新轨迹 */ for (uint16_t c 0; c total_clusters; c) { if (cluster_matched[c]) continue; int16_t slot -1; for (uint16_t t 0; t MAX_TRACKED_TARGETS; t) { if (!g_tracks[t].active) { slot (int16_t)t; break; } } if (slot 0) continue; TrackedTarget *trk g_tracks[slot]; trk-active 1; trk-id g_next_track_id; trk-state TRACK_STATE_EMERGING; trk-pos_x clusters[c].cx; trk-pos_y clusters[c].cy; trk-pos_z clusters[c].cz; trk-vel_x 0.0f; trk-vel_y 0.0f; trk-vel_z 0.0f; trk-point_count clusters[c].point_count; trk-coast_frames 0; trk-confirm_frames 1; trk-total_frames 1; trk-extent_x clusters[c].ext_x; trk-extent_y clusters[c].ext_y; trk-extent_z clusters[c].ext_z; trk-centroid_range clusters[c].avg_range; trk-avg_snr clusters[c].avg_snr; } /* 8b未匹配轨迹 → 滑行/预测/删除 */ for (uint16_t t 0; t MAX_TRACKED_TARGETS; t) { if (!g_tracks[t].active) continue; if (track_assigned[t]) continue; g_tracks[t].coast_frames; /* 新兴目标3帧无匹配即删除 */ if (g_tracks[t].state TRACK_STATE_EMERGING g_tracks[t].coast_frames 3) { g_tracks[t].active 0; continue; } /* 已确认目标 → 滑行状态 */ if (g_tracks[t].state TRACK_STATE_CONFIRMED) { g_tracks[t].state TRACK_STATE_COASTING; } /* 滑行中用速度外推位置 */ if (g_tracks[t].state TRACK_STATE_COASTING) { g_tracks[t].pos_x g_tracks[t].vel_x; g_tracks[t].pos_y g_tracks[t].vel_y; g_tracks[t].pos_z g_tracks[t].vel_z; g_tracks[t].point_count 0; } /* 超时删除 */ if (g_tracks[t].coast_frames TRACK_COAST_LIMIT) { g_tracks[t].active 0; } } } /* ---------------------------------------------------------------- * 第9步统计活跃轨迹 * 活跃 CONFIRMED 或 COASTINGEMERGING 不算避免噪声假目标 * ---------------------------------------------------------------- */ active_track_count 0; uint16_t emerging_count 0; for (uint16_t t 0; t MAX_TRACKED_TARGETS; t) { if (!g_tracks[t].active) continue; if (g_tracks[t].state TRACK_STATE_CONFIRMED || g_tracks[t].state TRACK_STATE_COASTING) { active_track_count; } else { emerging_count; } } /* 汇总日志 */ { uint16_t total_mp mp_counts[0] mp_counts[1] mp_counts[2] mp_counts[3]; printk(mmw_frame_cb: T:%d(%d) C:%d MP:%d(w%d,r%d,s%d,i%d) |, active_track_count, emerging_count, total_clusters, total_mp, mp_counts[0], mp_counts[1], mp_counts[2], mp_counts[3]); if (active_track_count 0) { uint16_t printed 0; for (uint16_t t 0; t MAX_TRACKED_TARGETS printed 2; t) { if (!g_tracks[t].active) continue; if (g_tracks[t].state ! TRACK_STATE_CONFIRMED g_tracks[t].state ! TRACK_STATE_COASTING) continue; const char *st (g_tracks[t].state TRACK_STATE_COASTING) ? CST : OK; printk( T%d%s(%dp,x%.0f,y%.0f,z%.0f), g_tracks[t].id, st, g_tracks[t].point_count, g_tracks[t].pos_x, g_tracks[t].pos_y, g_tracks[t].pos_z); printed; } printk( target\n); } else { printk( no_target\n); } } /* ---------------------------------------------------------------- * 第10步决策与帧率管理 * 有活跃轨迹 → 保持/恢复高帧率 * 无活跃轨迹 → 累加计数器达到阈值后降速 * ---------------------------------------------------------------- */ if (active_track_count 0) { no_target_frame_cnt 0; if (curr_period_ms 0) { mmw_frame_get(curr_period_ms, frame_num); } if (curr_period_ms FRAME_PERIOD_SLOW_MS) { target_confirm_cnt; if (target_confirm_cnt TARGET_RESTORE_CNT) { g_pending_frame_period_ms FRAME_PERIOD_NORMAL_MS; curr_period_ms FRAME_PERIOD_NORMAL_MS; target_confirm_cnt 0; printk(mmw_frame_cb: target confirmed %d frames, restore to %dms\n, TARGET_RESTORE_CNT, FRAME_PERIOD_NORMAL_MS); } } return 0; } no_target: target_confirm_cnt 0; no_target_frame_cnt; if (no_target_frame_cnt NO_TARGET_SLOWDOWN_CNT) { if (curr_period_ms 0) { mmw_frame_get(curr_period_ms, frame_num); } if (curr_period_ms ! FRAME_PERIOD_SLOW_MS) { g_pending_frame_period_ms FRAME_PERIOD_SLOW_MS; curr_period_ms FRAME_PERIOD_SLOW_MS; printk(mmw_frame_cb: no target %d frames, slow down to %dms\n, no_target_frame_cnt, FRAME_PERIOD_SLOW_MS); } } return 0; } /* 应用生命周期回调 */ int app_init_cb(void) { return 0; } int hw_init_cb(void) { return 0; } int app_deinit_cb(void) { return 0; } int hw_deinit_cb(void) { return 0; }运行日志mmw_frame_cb: T:0(0) C:0 MP:0(w0,r0,s0,i0) | no_targetmmw_frame_cb: T:0(0) C:0 MP:0(w0,r0,s0,i0) | no_targetmmw_frame_cb: no target for 10 frames, request slow down frame rate to 500msmmw_entry_cb: frame rate switched to 500msmmw_frame_cb: --- no cluster found (band544cm, inband5/28, range536~552) ---[0] range536 x38.9 y222.6 z-486.0 (azi0.073 ele-0.907 snr15.2)[1] range536 x7.3 y149.5 z-514.7 (azi0.014 ele-0.960 snr15.1)[2] range544 x-199.0 y240.4 z-445.6 (azi-0.366 ele-0.819 snr15.1)[3] range544 x-213.5 y213.5 z-452.5 (azi-0.393 ele-0.832 snr15.0)[4] range552 x-23.2 y227.6 z-502.4 (azi-0.042 ele-0.910 snr15.0)mmw_frame_cb: T:0(0) C:0 MP:5(w0,r0,s0,i5) | no_targetmmw_frame_cb: T:0(0) C:0 MP:0(w0,r0,s0,i0) | no_targetmmw_frame_cb: T:0(0) C:0 MP:0(w0,r0,s0,i0) | no_target六、下篇预告《RS6130 9轴IMU数据读取》 — 陀螺仪磁力计的驱动与融合算法《9轴IMU修正雷达点云坐标让安装不再需要调水平》 — 陀螺仪磁力计如何补偿安装倾角实现免标定安装 RS6130 点云采集与超低功耗实测（附完整源码）
前言在上一篇《让空间感知更完美自带陀螺仪的毫米波雷达》中我们完成了模组硬件验证与SDK烧录打通。本文将进入实战阶段——点云采集、雷达的一些基础算法简单版本、动态帧率控制、以及有目标/无目标两种场景下的功耗实测。一、核心思路动态帧率降功耗毫米波雷达的功耗与帧率成正比。对于电池供电的场景我们需要一种策略场景帧率说明有人活动50ms20fps需要高帧率跟踪人体微动无人静止500ms2fps仅做有无检测省电目标通过动态帧率切换可以有效的看到电流变小其中最低点符合RS6130待机10uA的描述最终目标是实现锂电池长期供电。二、代码实现动态帧率控制系统2.1 核心机制代码中实现了一套完整的帧率自适应系统mmw_frame_cb (每帧回调) ↓ 检测是否有目标通过点云聚类 多目标跟踪 ↓ 有目标 → 请求恢复 50ms 帧率 无目标 → 累加计数器连续 N 帧无目标 → 请求降速到 500ms ↓ mmw_entry_cb (下一帧入口回调) ↓ 执行实际的帧率切换stop → cfg → start2.2 关键代码解读帧率切换请求在 mmw_frame_cb 中if (active_track_count 0) { no_target_frame_cnt 0; if (curr_period_ms 0) { mmw_frame_get(curr_period_ms, frame_num); } if (curr_period_ms FRAME_PERIOD_SLOW_MS) { target_confirm_cnt; if (target_confirm_cnt TARGET_RESTORE_CNT) { g_pending_frame_period_ms FRAME_PERIOD_NORMAL_MS; curr_period_ms FRAME_PERIOD_NORMAL_MS; target_confirm_cnt 0; printk(mmw_frame_cb: target confirmed %d frames, restore to %dms\n, TARGET_RESTORE_CNT, FRAME_PERIOD_NORMAL_MS); } } return 0; }实际执行切换在 mmw_entry_cb 中int mmw_entry_cb(uint32_t frame_idx) { (void)frame_idx; if (g_pending_frame_period_ms 0) { return 0; /* 无待处理切换 */ } uint32_t target_period g_pending_frame_period_ms; /* * mmw_frame_cfg 在控制器 RUNNING 状态下会返回错误码。 * 必须先 stop配置完后再 start。 * 在 entry 回调中执行比 post 回调安全 * entry 回调执行时当前帧的 HIF 数据尚未生成不会打断流水线。 */ int ret mmw_ctrl_stop(); if (ret ! 0) { printk(mmw_entry_cb: mmw_ctrl_stop FAILED, err%d, retry next frame\n, ret); return 0; } ret mmw_frame_cfg(target_period, 0); if (ret ! 0) { printk(mmw_entry_cb: frame_cfg(%dms) FAILED, err%d, restarting...\n, target_period, ret); mmw_ctrl_start(); /* 即使 cfg 失败也要重启避免雷达完全停止 */ return 0; } ret mmw_ctrl_start(); if (ret ! 0) { printk(mmw_entry_cb: mmw_ctrl_start FAILED, err%d\n, ret); return 0; } printk(mmw_entry_cb: frame rate switched to %dms\n, target_period); g_pending_frame_period_ms 0; return 0; }为什么不在 mmw_frame_cb 中直接切换因为此时 HIF 传输管道正在发送本帧数据重配置会打断流水线同步。改为在 mmw_entry_cb下一帧入口处理前调用此时协处理器处于可接受配置的状态。2.3 防抖设计为了避免单帧误判导致频繁切换代码加入了防抖计数器#define NO_TARGET_SLOWDOWN_CNT 10 /* 连续10帧无目标才降速到500ms防抖动 */ #define TARGET_RESTORE_CNT 3 /* 连续3帧有目标才恢复50ms防单帧误判 */这意味着从50ms降到500ms需要10帧 × 50ms 0.5秒 持续无目标从500ms恢复到50ms需要3帧 × 500ms 1.5秒 持续有目标三、功耗实测数据3.1 测试条件项目参数模组60GHz RS6130 9轴IMU供电3.7V锂电池通过LDO稳压至3.3V测试仪器万用表串联测电流 示波器抓取瞬态测试场景室内办公室距离雷达2~4m3.2 有目标场景50ms帧率实测数据表明在有目标的高帧率模式下电流曲线呈现明显的周期性脉冲特征平均电流约为45mA级别。3.3 无目标场景500ms帧率在无目标的低帧率模式下平均电流4mA以内相比高帧率模式降低了一个数量级以上。3.4 功耗对比总结场景相对功耗固定50ms始终高帧率基准100%固定500ms始终低帧率大幅降低动态切换本文方案介于两者之间取决于有目标占比四、功耗优化方向产品化建议目前的动态帧率方案已显著降低了无目标场景的功耗。但要达到锂电池长期供电 的目标还需要以下优化4.1 深度休眠模式无目标时长时间待机预期可将空闲电流降至极低水平4.2 点云数量裁剪无目标时仅开启1T1R关闭两个接收通道进行有无判断减少FFT点数预期可降低发射处理电流4.3 IMU辅助休眠利用陀螺仪检测环境振动过滤不必要的唤醒4.4 预期优化路线优化步骤效果当前方案动态帧率基础省电深度休眠无目标时电流大幅下降点云裁剪有目标时电流进一步降低IMU辅助更智能的休眠唤醒策略终极目标实现锂电池长期供电五、完整源码本文涉及的核心代码已在文章中分段讲解。完整工程文件包括SDK配置、点云处理、多目标跟踪、帧率控制可联系作者获取。以下是关键性代码更换SDK中内容实测可运行/** ****************************************************************************** * file applications.c * brief 由 radar_framework 调用的开发者回调函数 * note 包含动态帧率控制、点云聚类、多目标跟踪、多径分析 ****************************************************************************** */ #include mmw_ctrl.h #include mmw_type.h #include log.h #include math.h #include prj_config.h #include mmw_app_pointcloud_config.h #include mrs6130_ddmer_config.h #include hal_os.h #include osi_common.h #include osi_thread.h /* 点云聚类参数 */ #define TARGET_CLUSTER_SIZE_CM 50.0f /* 聚类半径50cm。之前20cm太严苛目标短暂消失时难恢复高帧率 */ #define TARGET_MIN_POINTS 5 /* 最少5个点才视为有效聚类过滤零星噪声 */ #define HIGH_ACTIVITY_POINTS 30 /* 超过30个点大幅运动立即恢复50ms帧率无需防抖 */ /* 动态帧率参数 */ #define NO_TARGET_SLOWDOWN_CNT 10 /* 连续10帧无目标才降速到500ms防抖动 */ #define TARGET_RESTORE_CNT 3 /* 连续3帧有目标才恢复50ms防单帧误判 */ #define FRAME_PERIOD_NORMAL_MS 50 /* 有人20fps高帧率跟踪 */ #define FRAME_PERIOD_SLOW_MS 500 /* 无人2fps低帧率省电 */ /* 多目标跟踪参数 */ #define MAX_TRACKED_TARGETS 5 /* 最多同时跟踪5个目标家用场景足够 */ #define MAX_CLUSTERS 5 /* 单帧最多5个聚类 */ #define TRACK_COAST_LIMIT 10 /* 目标丢失后滑行10帧约0.5s50ms/5s500ms再删除 */ #define TRACK_CONFIRM_FRAMES 3 /* 连续匹配3帧才确认为真目标 */ #define TRACK_ASSOC_GATE_CM 80.0f /* 聚类与轨迹关联门限80cm。比聚类半径大允许目标小幅移动 */ #define ALPHA_TRACK 0.7f /* Alpha-Beta滤波器位置平滑系数越大越相信新测量 */ #define BETA_TRACK 0.3f /* 速度估计系数越小速度变化越平滑 */ /* 目标状态机 */ #define TRACK_STATE_EMERGING 0 /* 新兴刚创建未确认 */ #define TRACK_STATE_CONFIRMED 1 /* 已确认连续匹配 CONFIRM_FRAMES */ #define TRACK_STATE_COASTING 2 /* 滑行已确认但本帧无匹配可能进入静止 */ /* 多径标记 */ #define MP_TAG_NONE 0 #define MP_TAG_WIDE_ANGLE 1 /* 大方位角 |azi_phase|0.7可能墙壁反射 */ #define MP_TAG_RANGE_MIRROR 2 /* 距离镜像角度相近但距离为聚类1.5x~2.5x */ #define MP_TAG_LOW_SNR 3 /* 低SNR比聚类平均SNR低10dB以上 */ #define MP_TAG_ISOLATED 4 /* 孤立点周围30cm内无其他点 */ /* * pending_frame_period_ms跨回调通信的标志位 * * 设计原因mmw_frame_cfg 不能在 mmw_frame_cb 中直接调用 * 因为此时 HIF 传输管道正在发送本帧数据重配置会打断流水线同步。 * 改为在 mmw_entry_cb下一帧入口处理前调用此时协处理器可接受配置。 * * 写入者mmw_frame_cb * 读取并清零者mmw_entry_cb */ static uint32_t g_pending_frame_period_ms 0; /* 追踪目标结构体 */ /* 用 Alpha-Beta 滤波器平滑质心位置和速度估计 */ typedef struct { uint8_t active; /* 0空槽位, 1使用中 */ uint8_t id; /* 目标ID从1开始递增永不回收 */ uint8_t state; /* EMERGING / CONFIRMED / COASTING */ float pos_x, pos_y, pos_z; /* 滤波质心位置 (cm) */ float vel_x, vel_y, vel_z; /* 估计速度 (cm/帧) */ uint16_t point_count; /* 最近匹配点数 */ uint16_t coast_frames; /* 连续未匹配帧数 */ uint16_t confirm_frames; /* 连续匹配帧数 */ uint16_t total_frames; /* 累计跟踪帧数 */ float extent_x, extent_y, extent_z; /* 最近聚类包围盒 (cm) */ float centroid_range; /* 最近聚类平均距离 (cm) */ float avg_snr; /* 最近聚类平均 SNR (dB) */ } TrackedTarget; /* 聚类信息结构体 */ typedef struct { float cx, cy, cz; /* 质心 (cm) */ float ext_x, ext_y, ext_z; /* 包围盒 (cm) */ uint16_t point_count; /* 点数 */ uint16_t member_indices[20]; /* 成员在点云中的原始索引 */ uint16_t member_count; float avg_snr; /* 平均 SNR (dB) */ float avg_range; /* 平均距离 (cm) */ } Cluster; /* 全局追踪目标列表 */ static TrackedTarget g_tracks[MAX_TRACKED_TARGETS]; static uint8_t g_next_track_id 1; /* 辅助两点三维欧氏距离 (cm) */ static inline float track_dist3d(float x1, float y1, float z1, float x2, float y2, float z2) { float dx x1 - x2, dy y1 - y2, dz z1 - z2; return sqrtf(dx * dx dy * dy dz * dz); } /* * mmw_entry_cb每帧入口回调 * 在帧处理开始前调用负责执行待处理的帧率切换 * */ int mmw_entry_cb(uint32_t frame_idx) { (void)frame_idx; if (g_pending_frame_period_ms 0) { return 0; /* 无待处理切换 */ } uint32_t target_period g_pending_frame_period_ms; /* * mmw_frame_cfg 在控制器 RUNNING 状态下会返回错误码。 * 必须先 stop配置完后再 start。 * 在 entry 回调中执行比 post 回调安全 * entry 回调执行时当前帧的 HIF 数据尚未生成不会打断流水线。 */ int ret mmw_ctrl_stop(); if (ret ! 0) { printk(mmw_entry_cb: mmw_ctrl_stop FAILED, err%d, retry next frame\n, ret); return 0; } ret mmw_frame_cfg(target_period, 0); if (ret ! 0) { printk(mmw_entry_cb: frame_cfg(%dms) FAILED, err%d, restarting...\n, target_period, ret); mmw_ctrl_start(); /* 即使 cfg 失败也要重启避免雷达完全停止 */ return 0; } ret mmw_ctrl_start(); if (ret ! 0) { printk(mmw_entry_cb: mmw_ctrl_start FAILED, err%d\n, ret); return 0; } printk(mmw_entry_cb: frame rate switched to %dms\n, target_period); g_pending_frame_period_ms 0; return 0; } /* * mmw_frame_cb每帧数据处理回调 * 包含点云预处理、聚类、多径分析、多目标跟踪、动态帧率决策 * */ __sram_text int mmw_frame_cb(uint8_t argc, void* arg[]) { /* 静态计数器跨帧保持 */ static uint32_t no_target_frame_cnt 0; /* 连续无目标帧计数 */ static uint32_t target_confirm_cnt 0; /* 连续有目标帧计数防抖 */ static uint32_t curr_period_ms 0; /* 当前帧周期缓存避免重复查询 */ uint32_t frame_num; PointCloudBuffer_t *pc (PointCloudBuffer_t*)arg[1]; uint16_t point_num pc-point_cloud_num; /* 跨 goto 使用的变量提前声明 */ uint16_t xyz_count 0; uint16_t total_clusters 0; float min_range 1e6f, max_range -1e6f; float best_band_center 0.0f; uint16_t mp_counts[5] {0}; /* 多径标签计数 */ Cluster clusters[MAX_CLUSTERS]; uint16_t active_track_count 0; /* ---------------------------------------------------------------- * 第1步点数为0 → 跳过点云处理直接执行跟踪更新 * ---------------------------------------------------------------- */ if (point_num 0) { goto track_update; } /* ---------------------------------------------------------------- * 第2步高活跃度检测 * 点云数量多 有人在雷达前大幅运动 → 立即恢复高帧率 * 不需要空间聚类直接判定为活跃场景 * ---------------------------------------------------------------- */ if (point_num HIGH_ACTIVITY_POINTS) { no_target_frame_cnt 0; target_confirm_cnt 0; if (curr_period_ms 0) { mmw_frame_get(curr_period_ms, frame_num); } if (curr_period_ms FRAME_PERIOD_SLOW_MS) { g_pending_frame_period_ms FRAME_PERIOD_NORMAL_MS; curr_period_ms FRAME_PERIOD_NORMAL_MS; printk(mmw_frame_cb: high activity (pts%d), restore to %dms\n, point_num, FRAME_PERIOD_NORMAL_MS); } return 0; } /* ---------------------------------------------------------------- * 第3步获取距离参数计算 range bin 对应的物理距离 * ---------------------------------------------------------------- */ uint32_t range_mm, resol_mm; uint16_t range_fft_num, doppler_fft_num; mmw_range_get(range_mm, resol_mm); mmw_fft_num_get(range_fft_num, doppler_fft_num); float range_bin_size_cm (float)range_mm / (float)range_fft_num * 0.1f; /* ---------------------------------------------------------------- * 第3.5步主导距离带投票滤除杂波点 * * 问题远距杂波256cm或大角度镜面反射的点混入目标点云48~64cm * 导致包围盒被撑大到100cm触发无目标误判。 * 解决找出点数最密集的距离带±12cm窗口仅该带内点参与后续计算。 * ---------------------------------------------------------------- */ #define RANGE_BAND_HALF_CM 12.0f uint16_t best_band_count 0; { float r_cm_cache[20]; uint16_t n_cache (point_num 20) ? 20 : point_num; for (uint16_t i 0; i n_cache; i) { r_cm_cache[i] (float)pc-ptr_motion_point_cloud_data[i].range_idx * range_bin_size_cm; } for (uint16_t i 0; i n_cache; i) { float ri r_cm_cache[i]; uint16_t cnt 0; for (uint16_t j 0; j n_cache; j) { float diff r_cm_cache[j] - ri; if (diff -RANGE_BAND_HALF_CM diff RANGE_BAND_HALF_CM) cnt; } if (cnt best_band_count) { best_band_count cnt; best_band_center ri; } } } /* ---------------------------------------------------------------- * 第4步主导距离带内的点 → 极坐标转直角坐标 * * 公式 * sin_x azi_phase → x/r 方向余弦 * sin_z ele_phase → z/r 方向余弦 * sin_y sqrt(1 - sin_x² - sin_z²) → y/r 方向余弦 * x range_cm * sin_x 横向水平 * y range_cm * sin_y 径向 * z range_cm * sin_z 高度 * ---------------------------------------------------------------- */ float x_arr[20], y_arr[20], z_arr[20], range_arr[20]; float azi_arr[20], ele_arr[20]; uint16_t orig_idx_arr[20]; for (uint16_t i 0; i point_num xyz_count 20; i) { float range_cm (float)pc-ptr_motion_point_cloud_data[i].range_idx * range_bin_size_cm; float diff range_cm - best_band_center; if (diff -RANGE_BAND_HALF_CM || diff RANGE_BAND_HALF_CM) { continue; } float sin_x pc-ptr_motion_point_cloud_data[i].azi_phase; float sin_z pc-ptr_motion_point_cloud_data[i].ele_phase; float sin_y2 1.0f - sin_x * sin_x - sin_z * sin_z; float sin_y (sin_y2 0.0f) ? sqrtf(sin_y2) : 0.0f; x_arr[xyz_count] range_cm * sin_x; y_arr[xyz_count] range_cm * sin_y; z_arr[xyz_count] range_cm * sin_z; range_arr[xyz_count] range_cm; azi_arr[xyz_count] sin_x; ele_arr[xyz_count] sin_z; if (range_cm min_range) min_range range_cm; if (range_cm max_range) max_range range_cm; orig_idx_arr[xyz_count] i; xyz_count; } /* ---------------------------------------------------------------- * 第5步贪心聚类 * 将空间邻近的点归为一类输出聚类信息供多目标跟踪使用 * ---------------------------------------------------------------- */ uint8_t visited[20] {0}; uint8_t visited_for_mp[20] {0}; for (uint16_t i 0; i xyz_count total_clusters MAX_CLUSTERS; i) { if (visited[i]) continue; float c_min_x x_arr[i], c_max_x x_arr[i]; float c_min_y y_arr[i], c_max_y y_arr[i]; float c_min_z z_arr[i], c_max_z z_arr[i]; uint16_t cluster_members[20]; uint16_t cluster_cnt 0; for (uint16_t j 0; j xyz_count; j) { if (visited[j]) continue; float dx x_arr[j] - x_arr[i]; float dy y_arr[j] - y_arr[i]; float dz z_arr[j] - z_arr[i]; if (fabsf(dx) TARGET_CLUSTER_SIZE_CM fabsf(dy) TARGET_CLUSTER_SIZE_CM fabsf(dz) TARGET_CLUSTER_SIZE_CM) { cluster_members[cluster_cnt] j; if (x_arr[j] c_min_x) c_min_x x_arr[j]; if (x_arr[j] c_max_x) c_max_x x_arr[j]; if (y_arr[j] c_min_y) c_min_y y_arr[j]; if (y_arr[j] c_max_y) c_max_y y_arr[j]; if (z_arr[j] c_min_z) c_min_z z_arr[j]; if (z_arr[j] c_max_z) c_max_z z_arr[j]; } } float ext_x c_max_x - c_min_x; float ext_y c_max_y - c_min_y; float ext_z c_max_z - c_min_z; if (cluster_cnt TARGET_MIN_POINTS ext_x TARGET_CLUSTER_SIZE_CM ext_y TARGET_CLUSTER_SIZE_CM ext_z TARGET_CLUSTER_SIZE_CM) { Cluster *cl clusters[total_clusters]; cl-cx (c_min_x c_max_x) * 0.5f; cl-cy (c_min_y c_max_y) * 0.5f; cl-cz (c_min_z c_max_z) * 0.5f; cl-ext_x ext_x; cl-ext_y ext_y; cl-ext_z ext_z; cl-point_count cluster_cnt; float sum_snr 0.0f, sum_range 0.0f; for (uint16_t k 0; k cluster_cnt; k) { uint16_t idx cluster_members[k]; uint16_t orig_i orig_idx_arr[idx]; cl-member_indices[k] orig_i; sum_snr pc-ptr_motion_point_cloud_data[orig_i].sig_snr; sum_range range_arr[idx]; visited_for_mp[idx] 1; } cl-member_count cluster_cnt; cl-avg_snr sum_snr / (float)cluster_cnt; cl-avg_range sum_range / (float)cluster_cnt; total_clusters; for (uint16_t k 0; k cluster_cnt; k) { visited[cluster_members[k]] 1; } } else { visited[i] 1; } } /* 诊断输出有点但未找到有效聚类时打印详细信息 */ if (total_clusters 0 xyz_count 0) { printk(mmw_frame_cb: no cluster (band%.0fcm, inband%d/%d, range%.0f~%.0f)\n, best_band_center, xyz_count, point_num, min_range, max_range); for (uint16_t i 0; i xyz_count; i) { printk( [%d] r%.0f x%.1f y%.1f z%.1f (azi%.3f ele%.3f snr%.1f)\n, i, range_arr[i], x_arr[i], y_arr[i], z_arr[i], azi_arr[i], ele_arr[i], pc-ptr_motion_point_cloud_data[orig_idx_arr[i]].sig_snr); } } /* ---------------------------------------------------------------- * 第6步多径分析 * 对未被聚类的带内点进行标记识别可能的虚假回波 * ---------------------------------------------------------------- */ if (xyz_count 0) { for (uint16_t i 0; i xyz_count; i) { if (visited_for_mp[i]) continue; uint8_t tag MP_TAG_NONE; uint16_t orig_i orig_idx_arr[i]; float snr pc-ptr_motion_point_cloud_data[orig_i].sig_snr; /* 检测1大方位角 → 可能墙壁反射 */ if (fabsf(azi_arr[i]) 0.7f) { tag MP_TAG_WIDE_ANGLE; } /* 检测2距离镜像 → 可能地板/天花板反射 */ if (tag MP_TAG_NONE) { for (uint16_t c 0; c total_clusters; c) { float cl_azi clusters[c].cx / (clusters[c].avg_range 0.1f ? clusters[c].avg_range : 0.1f); float azi_diff fabsf(fabsf(azi_arr[i]) - fabsf(cl_azi)); float range_ratio range_arr[i] / (clusters[c].avg_range 0.1f ? clusters[c].avg_range : 0.1f); if (azi_diff 0.25f range_ratio 1.4f range_ratio 2.8f) { tag MP_TAG_RANGE_MIRROR; break; } } } /* 检测3低SNR → 比聚类平均低10dB以上 */ if (tag MP_TAG_NONE total_clusters 0) { float weighted_snr 0.0f; uint16_t total_pts 0; for (uint16_t c 0; c total_clusters; c) { weighted_snr clusters[c].avg_snr * (float)clusters[c].point_count; total_pts clusters[c].point_count; } if (total_pts 0 snr (weighted_snr / (float)total_pts) - 10.0f) { tag MP_TAG_LOW_SNR; } } /* 检测4孤立点 → 30cm内无其他点 */ if (tag MP_TAG_NONE) { uint8_t has_neighbor 0; for (uint16_t j 0; j xyz_count; j) { if (i j) continue; if (track_dist3d(x_arr[i], y_arr[i], z_arr[i], x_arr[j], y_arr[j], z_arr[j]) 30.0f) { has_neighbor 1; break; } } if (!has_neighbor) { tag MP_TAG_ISOLATED; } } if (tag 1 tag 4) { mp_counts[tag - 1]; } } } /* ---------------------------------------------------------------- * 第7步聚类→轨迹关联贪婪最近邻匹配 * ---------------------------------------------------------------- */ track_update: { uint8_t cluster_matched[MAX_CLUSTERS] {0}; uint8_t track_assigned[MAX_TRACKED_TARGETS] {0}; /* 7a匹配已有轨迹 */ for (uint16_t c 0; c total_clusters; c) { float best_dist TRACK_ASSOC_GATE_CM; int16_t best_t -1; for (uint16_t t 0; t MAX_TRACKED_TARGETS; t) { if (!g_tracks[t].active) continue; if (track_assigned[t]) continue; float d track_dist3d( clusters[c].cx, clusters[c].cy, clusters[c].cz, g_tracks[t].pos_x, g_tracks[t].pos_y, g_tracks[t].pos_z); if (d best_dist) { best_dist d; best_t (int16_t)t; } } if (best_t 0) { cluster_matched[c] 1; track_assigned[best_t] 1; /* Alpha-Beta 滤波器更新 */ TrackedTarget *trk g_tracks[best_t]; float rx clusters[c].cx - trk-pos_x; float ry clusters[c].cy - trk-pos_y; float rz clusters[c].cz - trk-pos_z; trk-pos_x ALPHA_TRACK * rx; trk-pos_y ALPHA_TRACK * ry; trk-pos_z ALPHA_TRACK * rz; trk-vel_x BETA_TRACK * rx; trk-vel_y BETA_TRACK * ry; trk-vel_z BETA_TRACK * rz; trk-point_count clusters[c].point_count; trk-extent_x clusters[c].ext_x; trk-extent_y clusters[c].ext_y; trk-extent_z clusters[c].ext_z; trk-centroid_range clusters[c].avg_range; trk-avg_snr clusters[c].avg_snr; trk-coast_frames 0; trk-total_frames; /* 状态转移 */ if (trk-state TRACK_STATE_EMERGING) { trk-confirm_frames; if (trk-confirm_frames TRACK_CONFIRM_FRAMES) { trk-state TRACK_STATE_CONFIRMED; } } else if (trk-state TRACK_STATE_COASTING) { trk-state TRACK_STATE_CONFIRMED; trk-confirm_frames TRACK_CONFIRM_FRAMES; } else { trk-confirm_frames; } } } /* ---------------------------------------------------------------- * 第8步未匹配聚类 → 创建新轨迹 * 未匹配轨迹 → 滑行或删除 * ---------------------------------------------------------------- */ /* 8a未匹配聚类 → 新轨迹 */ for (uint16_t c 0; c total_clusters; c) { if (cluster_matched[c]) continue; int16_t slot -1; for (uint16_t t 0; t MAX_TRACKED_TARGETS; t) { if (!g_tracks[t].active) { slot (int16_t)t; break; } } if (slot 0) continue; TrackedTarget *trk g_tracks[slot]; trk-active 1; trk-id g_next_track_id; trk-state TRACK_STATE_EMERGING; trk-pos_x clusters[c].cx; trk-pos_y clusters[c].cy; trk-pos_z clusters[c].cz; trk-vel_x 0.0f; trk-vel_y 0.0f; trk-vel_z 0.0f; trk-point_count clusters[c].point_count; trk-coast_frames 0; trk-confirm_frames 1; trk-total_frames 1; trk-extent_x clusters[c].ext_x; trk-extent_y clusters[c].ext_y; trk-extent_z clusters[c].ext_z; trk-centroid_range clusters[c].avg_range; trk-avg_snr clusters[c].avg_snr; } /* 8b未匹配轨迹 → 滑行/预测/删除 */ for (uint16_t t 0; t MAX_TRACKED_TARGETS; t) { if (!g_tracks[t].active) continue; if (track_assigned[t]) continue; g_tracks[t].coast_frames; /* 新兴目标3帧无匹配即删除 */ if (g_tracks[t].state TRACK_STATE_EMERGING g_tracks[t].coast_frames 3) { g_tracks[t].active 0; continue; } /* 已确认目标 → 滑行状态 */ if (g_tracks[t].state TRACK_STATE_CONFIRMED) { g_tracks[t].state TRACK_STATE_COASTING; } /* 滑行中用速度外推位置 */ if (g_tracks[t].state TRACK_STATE_COASTING) { g_tracks[t].pos_x g_tracks[t].vel_x; g_tracks[t].pos_y g_tracks[t].vel_y; g_tracks[t].pos_z g_tracks[t].vel_z; g_tracks[t].point_count 0; } /* 超时删除 */ if (g_tracks[t].coast_frames TRACK_COAST_LIMIT) { g_tracks[t].active 0; } } } /* ---------------------------------------------------------------- * 第9步统计活跃轨迹 * 活跃 CONFIRMED 或 COASTINGEMERGING 不算避免噪声假目标 * ---------------------------------------------------------------- */ active_track_count 0; uint16_t emerging_count 0; for (uint16_t t 0; t MAX_TRACKED_TARGETS; t) { if (!g_tracks[t].active) continue; if (g_tracks[t].state TRACK_STATE_CONFIRMED || g_tracks[t].state TRACK_STATE_COASTING) { active_track_count; } else { emerging_count; } } /* 汇总日志 */ { uint16_t total_mp mp_counts[0] mp_counts[1] mp_counts[2] mp_counts[3]; printk(mmw_frame_cb: T:%d(%d) C:%d MP:%d(w%d,r%d,s%d,i%d) |, active_track_count, emerging_count, total_clusters, total_mp, mp_counts[0], mp_counts[1], mp_counts[2], mp_counts[3]); if (active_track_count 0) { uint16_t printed 0; for (uint16_t t 0; t MAX_TRACKED_TARGETS printed 2; t) { if (!g_tracks[t].active) continue; if (g_tracks[t].state ! TRACK_STATE_CONFIRMED g_tracks[t].state ! TRACK_STATE_COASTING) continue; const char *st (g_tracks[t].state TRACK_STATE_COASTING) ? CST : OK; printk( T%d%s(%dp,x%.0f,y%.0f,z%.0f), g_tracks[t].id, st, g_tracks[t].point_count, g_tracks[t].pos_x, g_tracks[t].pos_y, g_tracks[t].pos_z); printed; } printk( target\n); } else { printk( no_target\n); } } /* ---------------------------------------------------------------- * 第10步决策与帧率管理 * 有活跃轨迹 → 保持/恢复高帧率 * 无活跃轨迹 → 累加计数器达到阈值后降速 * ---------------------------------------------------------------- */ if (active_track_count 0) { no_target_frame_cnt 0; if (curr_period_ms 0) { mmw_frame_get(curr_period_ms, frame_num); } if (curr_period_ms FRAME_PERIOD_SLOW_MS) { target_confirm_cnt; if (target_confirm_cnt TARGET_RESTORE_CNT) { g_pending_frame_period_ms FRAME_PERIOD_NORMAL_MS; curr_period_ms FRAME_PERIOD_NORMAL_MS; target_confirm_cnt 0; printk(mmw_frame_cb: target confirmed %d frames, restore to %dms\n, TARGET_RESTORE_CNT, FRAME_PERIOD_NORMAL_MS); } } return 0; } no_target: target_confirm_cnt 0; no_target_frame_cnt; if (no_target_frame_cnt NO_TARGET_SLOWDOWN_CNT) { if (curr_period_ms 0) { mmw_frame_get(curr_period_ms, frame_num); } if (curr_period_ms ! FRAME_PERIOD_SLOW_MS) { g_pending_frame_period_ms FRAME_PERIOD_SLOW_MS; curr_period_ms FRAME_PERIOD_SLOW_MS; printk(mmw_frame_cb: no target %d frames, slow down to %dms\n, no_target_frame_cnt, FRAME_PERIOD_SLOW_MS); } } return 0; } /* 应用生命周期回调 */ int app_init_cb(void) { return 0; } int hw_init_cb(void) { return 0; } int app_deinit_cb(void) { return 0; } int hw_deinit_cb(void) { return 0; }运行日志mmw_frame_cb: T:0(0) C:0 MP:0(w0,r0,s0,i0) | no_targetmmw_frame_cb: T:0(0) C:0 MP:0(w0,r0,s0,i0) | no_targetmmw_frame_cb: no target for 10 frames, request slow down frame rate to 500msmmw_entry_cb: frame rate switched to 500msmmw_frame_cb: --- no cluster found (band544cm, inband5/28, range536~552) ---[0] range536 x38.9 y222.6 z-486.0 (azi0.073 ele-0.907 snr15.2)[1] range536 x7.3 y149.5 z-514.7 (azi0.014 ele-0.960 snr15.1)[2] range544 x-199.0 y240.4 z-445.6 (azi-0.366 ele-0.819 snr15.1)[3] range544 x-213.5 y213.5 z-452.5 (azi-0.393 ele-0.832 snr15.0)[4] range552 x-23.2 y227.6 z-502.4 (azi-0.042 ele-0.910 snr15.0)mmw_frame_cb: T:0(0) C:0 MP:5(w0,r0,s0,i5) | no_targetmmw_frame_cb: T:0(0) C:0 MP:0(w0,r0,s0,i0) | no_targetmmw_frame_cb: T:0(0) C:0 MP:0(w0,r0,s0,i0) | no_target六、下篇预告《RS6130 9轴IMU数据读取》 — 陀螺仪磁力计的驱动与融合算法《9轴IMU修正雷达点云坐标让安装不再需要调水平》 — 陀螺仪磁力计如何补偿安装倾角实现免标定安装