参考链接： 1、原理解读； 2、源码解读；
负责组内的定位导航也已经有一段时间了，HDL定位也用了一段时间，大部分情况下效果也还不错。但对其原理还是不是很清晰，只是大概知道用ukf融合了imu数据和匹配用的是ndt。最近要求优化定位效果，做一些自己的东西，因此需要了解原理进行增删改查。特此学习并记录ＨＤＬ定位的原理及源码。
一，ＨＤＬ原理
ｈｄｌ的后端数据融合使用的UKF，与EKF相比较，UKF更好地拟合非线性的概率分布，而不是强行进行线性化，EKF由于要计算雅可比矩阵，更烧脑一些，UKF要对协方差矩阵做开方，也需要用到矩阵分解，两者计算量差不多。当然，如果一定要用EKF应该也能解决如今的低速定位问题。 在hdl中使用的ndt就像我们熟悉的pcl接口一样，只不过调用了多线程omp模式，它提供的是测量值。imu提供的是先验值，我们如果选择不用imu则先验值为原有的位姿。 参与融合的状态向量依然是p、v、q、ba、bg，一共16维，观测量则是7维，即p、q。和其它KF的接口一样，这里提供了predict和correct两个接口： 1.更新预测 control是传入的一帧imu数据，我们姑且看作是控制量，预测时首先判断协方差矩阵是否是方阵并且是否正定，因为我们在求sigma点的过程中要求协方差矩阵的逆，因此提取出它们的特征值和特征向量并再次相乘。接下来就是求解sigma点了：
void computeSigmaPoints(const VectorXt& mean, const MatrixXt& cov, MatrixXt& sigma_points) {
const int n = mean.size();
assert(cov.rows() == n && cov.cols() == n);
//用llt分解求协方差的逆矩阵
Eigen::LLT
llt.compute((n + lambda) * cov);
MatrixXt l = llt.matrixL();
sigma_points.row(0) = mean;
for (int i = 0; i < n; i++) {
sigma_points.row(1 + i * 2) = mean + l.col(i);
sigma_points.row(1 + i * 2 + 1) = mean - l.col(i);
}
}
在计算到所有的sigma点之后，用这些点重新拟合一个正态分布，在这个过程中引入了权重weights，它的初始赋值在构造函数中：
weights[0] = lambda / (N + lambda);
for (int i = 1; i < 2 * N + 1; i++) {
weights[i] = 1 / (2 * (N + lambda));
}
void predict(const VectorXt& control)
{
// calculate sigma points
ensurePositiveFinite(cov);
computeSigmaPoints(mean, cov, sigma_points);
for (int i = 0; i < S; i++) {
//状态转移方程
sigma_points.row(i) = system.f(sigma_points.row(i), control);
}
const auto& R = process_noise;
// unscented transform
VectorXt mean_pred(mean.size());
MatrixXt cov_pred(cov.rows(), cov.cols());
mean_pred.setZero();
cov_pred.setZero();
for (int i = 0; i < S; i++) {
mean_pred += weights[i] * sigma_points.row(i);
}
for (int i = 0; i < S; i++) {
VectorXt diff = sigma_points.row(i).transpose() - mean_pred;
cov_pred += weights[i] * diff * diff.transpose();
}
cov_pred += R;
mean = mean_pred;
cov = cov_pred;
}
在每帧imu传入后都进行一次预测更新，在观测矫正之前我们得到了sigma点拟合的状态分布，而非状态转换矩阵，很像粒子滤波。
2.观测值估计真值
correct函数是观测矫正的过程，我们先拟合观测的sigma点，再用观测方程求解观测的均值，并加权计算扩增后的预测状态以及观测状态的协方差矩阵。
void correct(const VectorXt& measurement)
{
// create extended state space which includes error variances
//状态扩增，即先更新预测值的协方差矩阵，将上一部分用sigma点拟合的协方差加上测量噪声
VectorXt ext_mean_pred = VectorXt::Zero(N + K, 1);
MatrixXt ext_cov_pred = MatrixXt::Zero(N + K, N + K);
ext_mean_pred.topLeftCorner(N, 1) = VectorXt(mean);
ext_cov_pred.topLeftCorner(N, N) = MatrixXt(cov);
ext_cov_pred.bottomRightCorner(K, K) = measurement_noise;
//拟合状态扩增后的均值与协方差，也就是再算一遍sigma点
ensurePositiveFinite(ext_cov_pred);
computeSigmaPoints(ext_mean_pred, ext_cov_pred, ext_sigma_points);
// unscented transform
//UT变换，即拟合测量的均值和协方差
expected_measurements.setZero();
for (int i = 0; i < ext_sigma_points.rows(); i++) {
expected_measurements.row(i) = system.h(ext_sigma_points.row(i).transpose().topLeftCorner(N, 1));
expected_measurements.row(i) += VectorXt(ext_sigma_points.row(i).transpose().bottomRightCorner(K, 1));
}
//虽然叫expected，但这是7维的测量值，也就是用sigma点拟合的测量分布！
VectorXt expected_measurement_mean = VectorXt::Zero(K);
for (int i = 0; i < ext_sigma_points.rows(); i++) {
expected_measurement_mean += ext_weights[i] * expected_measurements.row(i);
}
//测量的协方差矩阵，即Pk+1
MatrixXt expected_measurement_cov = MatrixXt::Zero(K, K);
for (int i = 0; i < ext_sigma_points.rows(); i++) {
VectorXt diff = expected_measurements.row(i).transpose() - expected_measurement_mean;
expected_measurement_cov += ext_weights[i] * diff * diff.transpose();
}
// calculated transformed covariance
//那么，在这里的23*7的sigma矩阵就是状态扩增后的Pk|k+1
MatrixXt sigma = MatrixXt::Zero(N + K, K);
for (int i = 0; i < ext_sigma_points.rows(); i++) {
auto diffA = (ext_sigma_points.row(i).transpose() - ext_mean_pred);
auto diffB = (expected_measurements.row(i).transpose() - expected_measurement_mean);
sigma += ext_weights[i] * (diffA * diffB.transpose());
}
//计算卡尔曼增益K=Pk|k+1/Pk
kalman_gain = sigma * expected_measurement_cov.inverse();
const auto& K = kalman_gain;
//更新观测后的真值估计
VectorXt ext_mean = ext_mean_pred + K * (measurement - expected_measurement_mean);
MatrixXt ext_cov = ext_cov_pred - K * expected_measurement_cov * K.transpose();
mean = ext_mean.topLeftCorner(N, 1);
cov = ext_cov.topLeftCorner(N, N);
}
最后简单总结一下UKF的过程：
1.产生2n+1个sigma点，在状态向量的附近，有点像粒子滤波。
2.利用状态转移矩阵，预测观测量的sigma点，并根据权重计算状态向量的均值和协方差矩阵。
3.利用测量矩阵得到sigma点的预测测量值。
4.根据sigma点和权重得到状态向量的预测值。
5.根据KF公式，将状态向量的预测值和观测值进行真值估计。
因此hdl的逻辑就很清晰地显示出来了，在获取到估计值后，便将状态向量的前10维，也就是pvq取出得到位姿的估计值，实现了定位的迭代。
Eigen::Vector3f pos() const {
return Eigen::Vector3f(ukf->mean[0], ukf->mean[1], ukf->mean[2]);
}
Eigen::Vector3f vel() const {
return Eigen::Vector3f(ukf->mean[3], ukf->mean[4], ukf->mean[5]);
}
Eigen::Quaternionf quat() const {
return Eigen::Quaternionf(ukf->mean[6], ukf->mean[7], ukf->mean[8], ukf->mean[9]).normalized();
}
Eigen::Matrix4f matrix() const {
Eigen::Matrix4f m = Eigen::Matrix4f::Identity();
m.block<3, 3>(0, 0) = quat().toRotationMatrix();
m.block<3, 1>(0, 3) = pos();
return m;
}
二、源代码解析
主要参考：https://blog.csdn.net/wyang9x/article/details/112464644； 点云数据的预处理： 一般下面这几种情况需要进行点云滤波处理：
（1）点云数据密度不规则需要平滑
（2）因为遮挡等问题造成离群点需要去除
（3）大量数据需要下采样
（4）噪声数据需要去除 前文提到ＨＤＬ采用的是ｎｄｔ进行匹配的，单其实代码里是有GICP_OMP（广义迭代最近点）可选的
void initialize_params() {
// intialize scan matching method
double downsample_resolution = private_nh.param
std::string ndt_neighbor_search_method = private_nh.param
double ndt_resolution = private_nh.param
boost::shared_ptr
voxelgrid->setLeafSize(downsample_resolution, downsample_resolution, downsample_resolution);
downsample_filter = voxelgrid;
//定义了ndt和glcp
pclomp::NormalDistributionsTransform
pclomp::GeneralizedIterativeClosestPoint
//ndt参数与搜索方法。默认DIRECT7，作者说效果不好可以尝试改为DIRECT1.
ndt->setTransformationEpsilon(0.01);
ndt->setResolution(ndt_resolution);
if(ndt_neighbor_search_method == "DIRECT1") {
NODELET_INFO("search_method DIRECT1 is selected");
ndt->setNeighborhoodSearchMethod(pclomp::DIRECT1);
registration = ndt;
} else if(ndt_neighbor_search_method == "DIRECT7") {
NODELET_INFO("search_method DIRECT7 is selected");
ndt->setNeighborhoodSearchMethod(pclomp::DIRECT7);
registration = ndt;
} else if(ndt_neighbor_search_method == "GICP_OMP"){
//GICP_OMP 需要在启动文件中的配置参数中配置；
NODELET_INFO("search_method GICP_OMP is selected");
registration = gicp;
}
else {
if(ndt_neighbor_search_method == "KDTREE") {
NODELET_INFO("search_method KDTREE is selected");
} else {
NODELET_WARN("invalid search method was given");
NODELET_WARN("default method is selected (KDTREE)");
}
ndt->setNeighborhoodSearchMethod(pclomp::KDTREE);
registration = ndt;
}
// initialize pose estimator
//设置起点用。
if(private_nh.param
NODELET_INFO("initialize pose estimator with specified parameters!!");
pose_estimator.reset(new hdl_localization::PoseEstimator(registration,
ros::Time::now(),
Eigen::Vector3f(private_nh.param
Eigen::Quaternionf(private_nh.param
private_nh.param
));
}
}
launch文件的更改：
好文推荐
发表评论