aspcms企业网站模板,索引网站有哪些,建设专题网站,网站内部建设和程序算法设计请参考4D雷达目标检测跟踪算法设计-CSDN博客#xff0c;这里仅讨论代码实现。
1.坐标转换 坐标转换是将雷达点云从雷达中心点极坐标系转换到车辆后轴中心的直角坐标系#xff0c;直接使用公式计算 x r*cos(eleφ)*cos(aziθ)OFFSET_X y r*cos(eleφ)*sin(aziθ)OF… 算法设计请参考4D雷达目标检测跟踪算法设计-CSDN博客这里仅讨论代码实现。
1.坐标转换 坐标转换是将雷达点云从雷达中心点极坐标系转换到车辆后轴中心的直角坐标系直接使用公式计算 x r*cos(eleφ)*cos(aziθ)OFFSET_X y r*cos(eleφ)*sin(aziθ)OFFSET_Y z r*sin(eleφ)OFFSET_Z 这里ALIGN_PHI是俯仰角标定值ALIGN_THETA是方位角标定值为宏定义
tmp_radar.x tmp_radar.distance * cosf(tmp_radar.elevation ALIGN_PHI) * cosf(tmp_radar.azimuth ALIGN_THETA) OFFSET_X;
tmp_radar.y tmp_radar.distance * cosf(tmp_radar.elevation ALIGN_PHI) * sinf(tmp_radar.azimuth ALIGN_THETA) OFFSET_Y;
tmp_radar.z tmp_radar.distance * sinf(tmp_radar.elevation ALIGN_PHI) OFFSET_Z; 2.噪点剔除 这里先根据ROI粗筛再根据rcs做进一步处理。 raw_radar_target和noise_target都是雷达点云结构体前者是所有原始点云后者代表噪声点云。被判定为噪声点的原始点云会打上噪声标签噪声点云记录用作后续分析。
void GetRoiTarget(RadarTarget* raw_radar_target, RadarTarget* noise_target)
{noise_target-timestamp raw_radar_target-timestamp;noise_target-amb_speed raw_radar_target-amb_speed;for (size_t i 0; i raw_radar_target-points.size(); i) {auto radar_pt raw_radar_target-points[i];if (radar_pt.x noise_x std::abs(radar_pt.y) noise_y radar_pt.z noise_min_z radar_pt.z noise_max_z std::abs(radar_pt.azimuth) noise_azi std::abs(radar_pt.elevation) noise_ele radar_pt.snr - radar_pt.rcs noise_snr_rcs) { // 设置ROI,粗筛radar_pt.noise_flag false;// 设定噪声点判断条件可能需要精细调整目标RCS随距离增加而增加if (radar_pt.distance noise_near_dis) { // 近距if (radar_pt.rcs noise_near_rcs) {radar_pt.noise_flag true;noise_target-points.push_back(radar_pt);raw_radar_target-points[i].noise_flag true;}}else if (radar_pt.distance noise_far_dis) { // 中距if (radar_pt.rcs radar_pt.distance * rcs_coef noise_near_rcs - noise_near_dis * rcs_coef) {radar_pt.noise_flag true;noise_target-points.push_back(radar_pt);raw_radar_target-points[i].noise_flag true;}}else { // 远距if (radar_pt.rcs noise_far_rcs) {radar_pt.noise_flag true;noise_target-points.push_back(radar_pt);raw_radar_target-points[i].noise_flag true;}}}else {radar_pt.noise_flag true;noise_target-points.push_back(radar_pt);raw_radar_target-points[i].noise_flag true;}}raw_radar_target-target_num raw_radar_target-points.size();noise_target-target_num noise_target-points.size();
} 常量数值设置可根据实际使用和数据统计分析得到这里给出设置的参考
const float noise_near_dis 10.0f;
const float noise_far_dis 50.0f;
const float noise_near_rcs -20.0f; // 商泰给的地面点门限
const float noise_far_rcs -10.0f;
// rcs随距离增加的系数
const float rcs_coef 0.25f;
// 距离、方位角度门限值
const float noise_x 100.0f;
const float noise_y 25.0f;
const float noise_min_z -0.5f;
const float noise_max_z 2.0f;
const float noise_azi 55.0f * DEG2RAD; // 方位角一般120度筛选110度
const float noise_ele 10.0f * DEG2RAD; // 俯仰角一般30度筛选20度
const float noise_snr_rcs 0.0f; // rcs和snr差值,正常snr-rcs 30
3.动静分离 动静分离理论上可以选出大部分运动点云但切向运动的目标由于径向速度接近0也会被认为是静止点云需要后续关联跟踪处理。
void SplitMoveStaticPoint(RadarTarget* raw_radar_target, const RadarVehicleInfo vehicle_info, RadarTarget* static_target, RadarTarget* move_target)
{float threshold_vr 0.0f;threshold_vr 0.6f vehicle_info.speed * 0.08f;static_target-timestamp raw_radar_target-timestamp;static_target-amb_speed raw_radar_target-amb_speed;static_target-mea_start_time raw_radar_target-mea_start_time;static_target-mea_end_time raw_radar_target-mea_end_time;move_target-timestamp raw_radar_target-timestamp;move_target-amb_speed raw_radar_target-amb_speed;move_target-mea_start_time raw_radar_target-mea_start_time;move_target-mea_end_time raw_radar_target-mea_end_time;float fix_angle vehicle_info.steering_angle / trans_ratio * DEG2RAD; // 雷达相对于车辆的转角for (size_t i 0; i raw_radar_target-points.size(); i) {auto radar_pt raw_radar_target-points[i];float coef cosf(radar_pt.elevation) * cosf(radar_pt.azimuth - fix_angle);radar_pt.cal_vr radar_pt.origin_vr - raw_radar_target-amb_speed;if (fabsf(radar_pt.origin_vr) fabsf(radar_pt.cal_vr)) {radar_pt.comp_vr radar_pt.origin_vr vehicle_info.speed * coef;}else {radar_pt.comp_vr radar_pt.cal_vr vehicle_info.speed * coef;}// 非噪声点if (!radar_pt.noise_flag) {if (fabsf(radar_pt.origin_vr vehicle_info.speed * coef) threshold_vr ||fabsf(radar_pt.cal_vr vehicle_info.speed * coef) threshold_vr) { // 静止点后续精细计算概率radar_pt.static_status 1;radar_pt.static_prob 1.0f;static_target-points.push_back(radar_pt);}else { // 运动点增加能量筛选 if (radar_pt.rcs -20.0f) {radar_pt.static_status 0;radar_pt.static_prob 0.0f;move_target-points.push_back(radar_pt);}}}}static_target-target_num static_target-points.size();move_target-target_num move_target-points.size();
} 这里涉及参数传动比不同车辆的数值不同从实现方法来说动静分离的准确度和车辆的速度、转向角等有关车辆传感器精度高结果会准确。
const float trans_ratio 15.2f; // 传动比
