2

障碍约束和雅格比矩阵的推导

 1 year ago
source link: https://charon-cheung.github.io/2023/03/04/%E8%B7%AF%E5%BE%84%E8%A7%84%E5%88%92/TEB%E7%AE%97%E6%B3%95/%E9%9A%9C%E7%A2%8D%E7%BA%A6%E6%9D%9F%E5%92%8C%E9%9B%85%E6%A0%BC%E6%AF%94%E7%9F%A9%E9%98%B5%E7%9A%84%E6%8E%A8%E5%AF%BC/
Go to the source link to view the article. You can view the picture content, updated content and better typesetting reading experience. If the link is broken, please click the button below to view the snapshot at that time.
neoserver,ios ssh client

障碍约束和雅格比矩阵的推导 | 沉默杀手

障碍约束和雅格比矩阵的推导
2023-03-04|路径规划TEB算法各类约束及雅格比矩阵|
Word count: 184|Reading time: 1 min

障碍约束定义的误差函数表示机器人到障碍的最小距离。 minpenaltyBelow(dist2point)⋅weightminpenaltyBelow(dist2point)⋅weight. 

const VertexPose* bandpt = static_cast<const VertexPose*>(_vertices[0]);
// calculateDistance(const PoseSE2& current_pose, const Obstacle* obstacle) 计算机器人到障碍物的距离
// current_pose为当前机器人的位姿。
double dist = robot_model_->calculateDistance(bandpt->pose(), _measurement);
_error[0] = penaltyBoundFromBelow(dist, cfg_->obstacles.min_obstacle_dist, cfg_->optim.penalty_epsilon);

if (cfg_->optim.obstacle_cost_exponent != 1.0 && cfg_->obstacles.min_obstacle_dist > 0.0)
{
  _error[0] = cfg_->obstacles.min_obstacle_dist * std::pow(_error[0] / cfg_->obstacles.min_obstacle_dist, 
  										cfg_->optim.obstacle_cost_exponent);
}

obstacle_cost_exponent和min_obstacle_dist的设置，一般都会进入if情况。 注意 max cost (before weighting) is the same as the straight line version and that all other costs are below the straight line (for positive exponent), so it may be necessary to increase weight_obstacle and/or the inflation_weight when using larger exponents.


report

Recommend

About Joyk


Aggregate valuable and interesting links.
Joyk means Joy of geeK