4D雷达跟踪算法代码实现——预处理

        算法设计请参考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;					// 传动比