使用Ceres最小化姿势和方向估计中的误差

问题描述

我将UWB锚放置为彼此相邻（总共2个锚），并且我还将2个标签放置为彼此相邻但与UWB锚相对（因此2个标签面向2个锚）。

由于我知道2个标签之间以及2个锚点之间的距离（它们是已知的并且相等），因此我想估计自己在全局地图坐标系中的位置（2个标签的位置）。请注意，UWB锚点的全局坐标是已知的。

唯一的不确定性是锚点和标签之间的测量值，因为UWB的精度为+/- 10 cm。

因此，我决定进行几次测量（标签和锚之间的距离），并使用Ceres优化器为我的标签提供非常准确的姿势和方向估计（位置+/- 5cm和+/- 1度）方向）。

这是我使用谷神仙的方法：

ceres::Problem problem;
ceres::LossFunction* loss_func = new ceres::HuberLoss(0.9);
for (uint var = 0; var < anchor_pos.size(); ++var) {
    ceres::CostFunction* cost_f = new ceres::autodiffCostFunction<CostFunctor,1,3>(new CostFunctor(anchor_pos[var],tags[var],distances[var],length_));
     problem.AddResidualBlock(cost_f,loss_func,robot_pose);
}
ceres::Solver::Options options;
options.minimizer_type = ceres::TRUST_REGION;
options.linear_solver_type = ceres::DENSE_QR;
options.function_tolerance = 1e-12;
options.gradient_tolerance = 1e-12;
options.parameter_tolerance = 1e-12;
options.max_num_iterations = 1000;

因此，在for循环中，我正在使用上述变量创建成本函数（全局地图中的锚位置，全局标签中我的标签的估算位置，标签与锚之间的距离和+/- 10 cm的精度和距离（标签之间的长度）。

我的位置精度大约为+/- 20 cm，方向精度为5度。有什么可以改进的？

解决方法

暂无找到可以解决该程序问题的有效方法，小编努力寻找整理中！

如果你已经找到好的解决方法，欢迎将解决方案带上本链接一起发送给小编。

小编邮箱:dio#foxmail.com (将#修改为@）