在自动驾驶和机器人技术中,雷达(RADAR)是关键的传感器之一,广泛用于检测周围物体的位置和速度。本文将深入探讨如何通过 C++ 代码实现雷达自我速度估计。代码结合了 RANSAC(随机采样一致性算法)和最小二乘法,展示了在含噪声的数据中如何进行准确的速度估计。
背景
雷达自我速度估计涉及从雷达点云数据中提取有用的信息来计算物体的速度。为了提高估计的鲁棒性和准确性,文献[1]利用RANSAC 和最小二乘法估计雷达自身速度。以下是对代码中主要功能的详细讲解。(完成代码在后面)
代码结构
本文的代码实现包含两个主要部分:
estimate
函数:处理雷达扫描数据并进行速度估计。solve3DFullRansac
和solve3DFull
函数:使用 RANSAC 和最小二乘法进行速度估计。
1. estimate
函数
这个函数的主要任务是从雷达数据中提取有效的目标,并进行速度估计。以下是函数的代码及其中文注释:
bool RadarEgoVelocityEstimator::estimate(const sensor_msgs::PointCloud2& radar_scan_msg,
Vector3& v_r,
Vector3& sigma_v_r,
sensor_msgs::PointCloud2& inlier_radar_msg,
sensor_msgs::PointCloud2& outlier_radar_msg)
{
// 创建点云指针以存储雷达点云数据
auto radar_scan(new pcl::PointCloud<RadarPointCloudType>);
auto radar_scan_inlier(new pcl::PointCloud<RadarPointCloudType>);
auto radar_scan_outlier(new pcl::PointCloud<RadarPointCloudType>);
std::cout << "雷达扫描消息大小: " << radar_scan_msg.width << std::endl;
bool success = false;
// 从 ROS 消息中提取点云数据
pcl::fromROSMsg(radar_scan_msg, *radar_scan);
std::vector<Vector11> valid_targets;
// 遍历点云数据,筛选出有效的目标
for (uint i = 0; i < radar_scan->size(); ++i)
{
const auto target = radar_scan->at(i);
const double r = Vector3(target.x, target.y, target.z).norm();
Real azimuth = std::atan2(target.y, target.x);
Real elevation = std::atan2(std::sqrt(target.x * target.x + target.y * target.y), target.z) - M_PI_2;
// 根据距离、强度、方位角和俯仰角筛选有效目标
if (r > config_.min_dist && r < config_.max_dist && target.intensity > config_.min_db &&
std::fabs(azimuth) < angles::from_degrees(config_.azimuth_thresh_deg) &&
std::fabs(elevation) < angles::from_degrees(config_.elevation_thresh_deg))
{
Vector11 v_pt;
v_pt << target.x, target.y, target.z, target.intensity, -target.doppler * config_.doppler_velocity_correction_factor,
r, azimuth, elevation, target.x / r, target.y / r, target.z / r;
valid_targets.emplace_back(v_pt);
}
}
std::cout << "有效目标数量: " << valid_targets.size() << std::endl;
// 如果有效目标的数量超过两个,进行速度估计
if (valid_targets.size() > 2)
{
std::vector<double> v_dopplers;
// 提取每个目标的多普勒速度
for (const auto& v_pt : valid_targets) v_dopplers.emplace_back(std::fabs(v_pt[idx_.doppler]));
const size_t n = v_dopplers.size() * (1.0 - config_.allowed_outlier_percentage);
std::nth_element(v_dopplers.begin(), v_dopplers.begin() + n, v_dopplers.end());
const auto median = v_dopplers[n];
// 如果中位数速度小于阈值,则认为速度接近零
if (median < config_.thresh_zero_velocity)
{
v_r = Vector3(0, 0, 0);
sigma_v_r = Vector3(config_.sigma_zero_velocity_x, config_.sigma_zero_velocity_y, config_.sigma_zero_velocity_z);
for (const auto& item : valid_targets)
if (std::fabs(item[idx_.doppler]) < config_.thresh_zero_velocity)
radar_scan_inlier->push_back(toRadarPointCloudType(item, idx_));
success = true;
}
else
{
Matrix radar_data(valid_targets.size(), 4);
uint idx = 0;
// 将有效目标数据填充到矩阵中
for (const auto& v_pt : valid_targets)
radar_data.row(idx++) = Vector4(v_pt[idx_.normalized_x], v_pt[idx_.normalized_y], v_pt[idx_.normalized_z], v_pt[idx_.doppler]);
// 使用 RANSAC 或最小二乘法进行速度估计
if (config_.use_ransac)
{
std::vector<uint> inlier_idx_best;
std::vector<uint> outlier_idx_best;
success = solve3DFullRansac(radar_data, v_r, sigma_v_r, inlier_idx_best, outlier_idx_best);
// 根据内点和外点更新点云数据
for (const auto& idx : inlier_idx_best)
radar_scan_inlier->push_back(toRadarPointCloudType(valid_targets.at(idx), idx_));
for (const auto& idx : outlier_idx_best)
radar_scan_outlier->push_back(toRadarPointCloudType(valid_targets.at(idx), idx_));
}
else
{
for (const auto& item : valid_targets) radar_scan_inlier->push_back(toRadarPointCloudType(item, idx_));
success = solve3DFull(radar_data, v_r, sigma_v_r);
}
}
}
else ROS_INFO("有效目标数量太小(< 2)");
// 将内点和外点的点云数据转换为 ROS 消息
radar_scan_inlier->height = 1;
radar_scan_inlier->width = radar_scan_inlier->size();
pcl::PCLPointCloud2 tmp;
pcl::toPCLPointCloud2<RadarPointCloudType>(*radar_scan_inlier, tmp);
pcl_conversions::fromPCL(tmp, inlier_radar_msg);
inlier_radar_msg.header = radar_scan_msg.header;
radar_scan_outlier->height = 1;
radar_scan_outlier->width = radar_scan_outlier->size();
pcl::PCLPointCloud2 tmp_o;
pcl::toPCLPointCloud2<RadarPointCloudType>(*radar_scan_outlier, tmp_o);
pcl_conversions::fromPCL(tmp_o, outlier_radar_msg);
outlier_radar_msg.header = radar_scan_msg.header;
return success;
}
2. solve3DFullRansac
函数
solve3DFullRansac
函数在处理雷达数据时,使用了 RANSAC(随机样本一致性算法)来提高估计的鲁棒性,尤其是在数据中可能存在异常值的情况下。RANSAC 是一种迭代方法,用于从包含噪声的数据中估计模型参数。该函数的目的是通过 RANSAC 算法找到最佳的速度估计,并将数据分为内点(inliers)和外点(outliers)。
函数代码
bool RadarEgoVelocityEstimator::solve3DFullRansac(const Matrix& radar_data,
Vector3& v_r,
Vector3& sigma_v_r,
std::vector<uint>& inlier_idx_best,
std::vector<uint>& outlier_idx_best)
{
// 构建设计矩阵 H
Matrix H_all(radar_data.rows(), 3);
H_all.col(0) = radar_data.col(0); // x 分量
H_all.col(1) = radar_data.col(1); // y 分量
H_all.col(2) = radar_data.col(2); // z 分量
const Vector y_all = radar_data.col(3); // 多普勒速度数据
// 创建索引列表用于 RANSAC 迭代
std::vector<uint> idx(radar_data.rows());
for (uint k = 0; k < radar_data.rows(); ++k) idx[k] = k;
std::random_device rd;
std::mt19937 g(rd());
if (radar_data.rows() >= config_.N_ransac_points)
{
for (uint k = 0; k < ransac_iter_; ++k)
{
std::shuffle(idx.begin(), idx.end(), g); // 随机打乱数据索引
Matrix radar_data_iter;
radar_data_iter.resize(config_.N_ransac_points, 4);
for (uint i = 0; i < config_.N_ransac_points; ++i) radar_data_iter.row(i) = radar_data.row(idx.at(i));
bool rtn = solve3DFull(radar_data_iter, v_r, sigma_v_r, false);
if (rtn == false) ROS_INFO("Failure at solve3DFullRansac() 1");
if (rtn)
{
const Vector err = (y_all - H_all * v_r).array().abs();
std::vector<uint> inlier_idx;
std::vector<uint> outlier_idx;
for (uint j = 0; j < err.rows(); ++j)
{
if (err(j) < config_.inlier_thresh)
inlier_idx.emplace_back(j); // 识别内点
else
outlier_idx.emplace_back(j); // 识别外点
}
// 如果外点比例过高,将外点视为内点
if ( float(outlier_idx.size())/(inlier_idx.size()+outlier_idx.size()) > 0.05 )
{
inlier_idx.insert(inlier_idx.end(), outlier_idx.begin(), outlier_idx.end());
outlier_idx.clear();
}
// 更新最佳内点和外点索引
if (inlier_idx.size() > inlier_idx_best.size())
{
inlier_idx_best = inlier_idx;
}
if (outlier_idx.size() > outlier_idx_best.size())
{
outlier_idx_best = outlier_idx;
}
}
}
}
else
{
ROS_INFO("Warning: radar_data.rows() < config_.N_ransac_points");
}
// 如果找到了内点
if (!inlier_idx_best.empty())
{
Matrix radar_data_inlier(inlier_idx_best.size(), 4);
for (uint i = 0; i < inlier_idx_best.size(); ++i) radar_data_inlier.row(i) = radar_data.row(inlier_idx_best.at(i));
bool rtn = solve3DFull(radar_data_inlier, v_r, sigma_v_r, true);
if (rtn == false) ROS_INFO("Failure at solve3DFullRansac() 2");
return rtn;
}
ROS_INFO("Failure at solve3DFullRansac() 3");
return false;
}
函数解析
1. 构建设计矩阵:
Matrix H_all(radar_data.rows(), 3);
H_all.col(0) = radar_data.col(0);
H_all.col(1) = radar_data.col(1);
H_all.col(2) = radar_data.col(2);
const Vector y_all = radar_data.col(3);
从 radar_data
中提取 x、y、z 分量,并构建完整的设计矩阵 H_all
。
2. 初始化 RANSAC 参数:
std::vector<uint> idx(radar_data.rows());
for (uint k = 0; k < radar_data.rows(); ++k) idx[k] = k;
std::random_device rd;
std::mt19937 g(rd());
创建索引列表用于 RANSAC 迭代,并初始化随机数生成器。
3. RANSAC 迭代:
for (uint k = 0; k < ransac_iter_; ++k)
{
std::shuffle(idx.begin(), idx.end(), g);
Matrix radar_data_iter;
radar_data_iter.resize(config_.N_ransac_points, 4);
for (uint i = 0; i < config_.N_ransac_points; ++i) radar_data_iter.row(i) = radar_data.row(idx.at(i));
bool rtn = solve3DFull(radar_data_iter, v_r, sigma_v_r, false);
// 计算残差并分离内点和外点
}
随机选择 N_ransac_points
个点进行最小二乘估计,并计算每个点的残差,将其分为内点和外点。根据内点和外点的数量更新最佳的内点和外点索引。
4. 最终估计:
if (!inlier_idx_best.empty())
{
Matrix radar_data_inlier(inlier_idx_best.size(), 4);
for (uint i = 0; i < inlier_idx_best.size(); ++i) radar_data_inlier.row(i) = radar_data.row(inlier_idx_best.at(i));
bool rtn = solve3DFull(radar_data_inlier, v_r, sigma_v_r, true);
return rtn;
}
如果找到了内点,使用这些内点数据进行最终的最小二乘估计。
3.solve3DFull
函数详解
solve3DFull
函数是最小二乘法的核心实现,负责从雷达数据中解算速度。它通过构建设计矩阵并解线性方程来估计目标速度,并可以选择性地计算标准差。
函数代码
bool RadarEgoVelocityEstimator::solve3DFull(const Matrix& radar_data,
Vector3& v_r,
Vector3& sigma_v_r,
bool estimate_sigma)
{
// 构建设计矩阵 H
Matrix H(radar_data.rows(), 3);
H.col(0) = radar_data.col(0); // x 分量
H.col(1) = radar_data.col(1); // y 分量
H.col(2) = radar_data.col(2); // z 分量
const Matrix HTH = H.transpose() * H; // 计算 H^T * H
const Vector y = radar_data.col(3); // 多普勒速度数据
// 使用 SVD (奇异值分解) 来解决最小二乘问题
Eigen::JacobiSVD<Matrix> svd(HTH);
Real cond = svd.singularValues()(0) / svd.singularValues()(svd.singularValues().size() - 1);
// 检查条件数是否合理,避免数值不稳定
if (1)//std::fabs(cond) < 1.0e3
{
// 选择 Cholesky 分解或 SVD 计算速度
if (config_.use_cholesky_instead_of_bdcsvd)
{
v_r = (HTH).ldlt().solve(H.transpose() * y);
}
else
{
v_r = H.bdcSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(y);
}
// 如果需要估计误差的标准差
if (estimate_sigma)
{
// 计算估计误差
const Vector e = H * v_r - y;
const Matrix C = (e.transpose() * e).x() * (HTH).inverse() / (H.rows() - 3); // 计算协方差矩阵
sigma_v_r = Vector3(C(0, 0), C(1, 1), C(2, 2)); // 提取对角线元素
sigma_v_r = sigma_v_r.array(); // 转换为数组形式
// 确保标准差非负,并应用偏移量
if (sigma_v_r.x() >= 0.0 && sigma_v_r.y() >= 0.0 && sigma_v_r.z() >= 0.)
{
sigma_v_r = sigma_v_r.array().sqrt(); // 对标准差取平方根
sigma_v_r += Vector3(config_.sigma_offset_radar_x, config_.sigma_offset_radar_y, config_.sigma_offset_radar_z); // 添加偏移量
// 检查标准差是否在允许范围内
if (sigma_v_r.x() < config_.max_sigma_x && sigma_v_r.y() < config_.max_sigma_y &&
sigma_v_r.z() < config_.max_sigma_z)
{
return true; // 如果标准差合理,返回成功
}
}
}
else
{
return true; // 如果不需要估计标准差,直接返回成功
}
}
// 如果条件数过大,可能说明计算不稳定
//ROS_INFO("cond too large, cond = %f", cond);
return true; // 返回成功标志
}
函数解析
1. 构建设计矩阵:
Matrix H(radar_data.rows(), 3);
H.col(0) = radar_data.col(0);
H.col(1) = radar_data.col(1);
H.col(2) = radar_data.col(2);
const Matrix HTH = H.transpose() * H;
从雷达数据中提取 x、y 和 z 分量,并构建设计矩阵 H
。计算 H^T * H
为后续的求解做准备。
2. 奇异值分解:
Eigen::JacobiSVD<Matrix> svd(HTH);
Real cond = svd.singularValues()(0) / svd.singularValues()(svd.singularValues().size() - 1);
对 H^T * H
进行奇异值分解,并计算条件数 cond
。条件数用于判断矩阵的数值稳定性。
3. 选择分解方法:
if (config_.use_cholesky_instead_of_bdcsvd)
{
v_r = (HTH).ldlt().solve(H.transpose() * y);
}
else
{
v_r = H.bdcSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(y);
}
根据配置选择 Cholesky 分解或 BDCSVD 分解来计算速度向量 v_r
。
4. 估计标准差:
if (estimate_sigma)
{
const Vector e = H * v_r - y;
const Matrix C = (e.transpose() * e).x() * (HTH).inverse() / (H.rows() - 3);
sigma_v_r = Vector3(C(0, 0), C(1, 1), C(2, 2));
sigma_v_r = sigma_v_r.array();
if (sigma_v_r.x() >= 0.0 && sigma_v_r.y() >= 0.0 && sigma_v_r.z() >= 0.)
{
sigma_v_r = sigma_v_r.array().sqrt();
sigma_v_r += Vector3(config_.sigma_offset_radar_x, config_.sigma_offset_radar_y, config_.sigma_offset_radar_z);
if (sigma_v_r.x() < config_.max_sigma_x && sigma_v_r.y() < config_.max_sigma_y &&
sigma_v_r.z() < config_.max_sigma_z)
{
return true;
}
}
}
如果需要估计标准差,计算误差的协方差矩阵,并将其转换为标准差。如果标准差在允许范围内,则返回成功。
总结
solve3DFullRansac
函数利用 RANSAC 算法来处理含有噪声和异常值的数据,选择最佳的内点集合进行速度估计,从而提高估计的鲁棒性。solve3DFull
函数使用最小二乘法直接从雷达数据中计算速度,并根据需要估计误差的标准差。
通过结合这两个函数,可以有效地从雷达数据中估计目标的速度,并处理数据中的异常值和噪声问题。
[1] Doer C, Trommer G F. An ekf based approach to radar inertial odometry[C]//2020 IEEE International Conference on Multisensor Fusion and Integration for Intelligent Systems (MFI). IEEE, 2020: 152-159.
代码连接:https://github.com/christopherdoer/reve
CSDN下载https://download.csdn.net/download/qq_40301351/89649333