TEB teb_local_planner 构建超图过程error cost分析

2024-04-02 19:12
文章标签 分析 构建 error 过程 planner local cost 超图 teb

本文主要是介绍TEB teb_local_planner 构建超图过程error cost分析，希望对大家解决编程问题提供一定的参考价值，需要的开发者们随着小编来一起学习吧！

G2O 背景

G20 边和顶点的定义
https://blog.csdn.net/weixin_43013761/category_11647404.html
使用G20优化器 优化 路径
https://blog.csdn.net/qq_29598161/article/details/115414732
https://zhuanlan.zhihu.com/p/121628349

buildGraph

TebOptimalPlanner::buildGraph 位于　optimal_planner.cpp 文件内，是整个teb planner 的核心

1. 添加顶点 AddTEBVertices

主要是添加位姿顶点和时间间隔顶点
顶点分姿态顶点和时间顶点
1.1.
姿态顶点 VertexPose 内部需要优化的变量主要有三个 x,y,theta
1.2
时间顶点 TimeDiffVertex 内部需要优化的变量主要有1个 dt

void TebOptimalPlanner::AddTEBVertices()
{// add vertices to graphROS_DEBUG_COND(cfg_->optim.optimization_verbose, "Adding TEB vertices ...");unsigned int id_counter = 0; // used for vertices idsfor (int i=0; i<teb_.sizePoses(); ++i){//基本状态　x,y,thetateb_.PoseVertex(i)->setId(id_counter++);//先赋值 再自行+1optimizer_->addVertex(teb_.PoseVertex(i));if (teb_.sizeTimeDiffs()!=0 && i<teb_.sizeTimeDiffs()){//基本状态　dtteb_.TimeDiffVertex(i)->setId(id_counter++);//先赋值 再自行+1 id 累积增加optimizer_->addVertex(teb_.TimeDiffVertex(i));}} 
}

2 Edge 的边的定义与Cost误差函数定义

使用到的惩罚函数
double penaltyBoundFromBelow(const double& var, const double& a,const double& epsilon)
函数定义如下
f ( x ) = { 0.0 , if  v a r > = a + e p s i l o n − v a r + ( a + e p s i l o n ) , if  v a r < ( a + e p s i l o n ) f(x) = \begin{cases} 0.0, & \text{if } var >= a+epsilon \\ -var + (a + epsilon), & \text{if } var < (a+epsilon) \\ \end{cases} f(x)={0.0,−var+(a+epsilon),​if var>=a+epsilonif var<(a+epsilon)​

2.1 EdgeObstacle

误差函数计算代码

  void computeError(){获取顶点姿态const VertexPose* bandpt = static_cast<const VertexPose*>(_vertices[0]);double dist = robot_model_->calculateDistance(bandpt->pose(), _measurement);// Original obstacle cost._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){// Optional non-linear cost. _error[0] = cfg_->obstacles.min_obstacle_dist * std::pow(_error[0] / cfg_->obstacles.min_obstacle_dist, cfg_->optim.obstacle_cost_exponent);}ROS_ASSERT_MSG(std::isfinite(_error[0]), "EdgeObstacle::computeError() _error[0]=%f\n",_error[0]);}

如果配置文件 obstacle_cost_exponent == 1.0 那么error为线性函数公式整理如下
e r r o r [ 0 ] = { 0.0 , if  d i s t > = o b s . m i n _ d i s t + o p t i m . p e n a l t y _ e p s i l o n − d i s t + ( o b s . m i n _ d i s t + o p t i m . p e n a l t y _ e p s i l o n ) , else  error[0] = \begin{cases} 0.0, & \text{if } dist >= obs.min\_dist+optim.penalty\_epsilon \\ -dist + (obs.min\_dist+optim.penalty\_epsilon), & \text{else }\\ \end{cases} error[0]={0.0,−dist+(obs.min_dist+optim.penalty_epsilon),​if dist>=obs.min_dist+optim.penalty_epsilonelse ​
如果配置文件 obstacle_cost_exponent != 1.0 那么error[0]继续更新为非线性函数公式整理如下
e r r o r [ 0 ] = o b s . m i n _ d i s t ∗ e r r o r [ 0 ] o b s . m i n _ d i s t c o s t _ e x p o e n t error[0] = obs.min\_dist * \frac{error[0] }{obs.min\_dist}^{cost\_expoent} error[0]=obs.min_dist∗obs.min_disterror[0]​cost_expoent

2.2 EdgeInflatedObstacle

  • EdgeInflatedObstacle 是一个一元边 继承于 public BaseTebUnaryEdge<2, const Obstacle*, VertexPose>
  • 观测类型是const Obstacle*
  • 连接的顶点是VertexPose
  void computeError(){const VertexPose* bandpt = static_cast<const VertexPose*>(_vertices[0]);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){// Optional non-linear cost. _error[0] = cfg_->obstacles.min_obstacle_dist * std::pow(_error[0] / cfg_->obstacles.min_obstacle_dist, cfg_->optim.obstacle_cost_exponent);}// Additional linear inflation cost_error[1] = penaltyBoundFromBelow(dist, cfg_->obstacles.inflation_dist, 0.0);}

error[0]的计算和EdgeObstacle 类型一样，额外增加一个error[1]
e r r o r [ 1 ] = { 0.0 , if  d i s t > = o b s . i n f l a t i o n _ d i s t − d i s t + ( o b s . i n f l a t i o n _ d i s t ) , else  error[1] = \begin{cases} 0.0, & \text{if } dist >= obs.inflation\_dist \\ -dist + (obs.inflation\_dist), & \text{else }\\ \end{cases} error[1]={0.0,−dist+(obs.inflation_dist),​if dist>=obs.inflation_distelse ​

2.3 EdgeDynamicObstacle

  • EdgeDynamicObstacle 继承于 BaseTebUnaryEdge<2, const Obstacle*, VertexPose> 是一个一元边
  void computeError(){const VertexPose* bandpt = static_cast<const VertexPose*>(_vertices[0]);double dist = robot_model_->estimateSpatioTemporalDistance(bandpt->pose(), _measurement, t_);_error[0] = penaltyBoundFromBelow(dist, cfg_->obstacles.min_obstacle_dist, cfg_->optim.penalty_epsilon);_error[1] = penaltyBoundFromBelow(dist, cfg_->obstacles.dynamic_obstacle_inflation_dist, 0.0);}
  • 该函数dist主要是预测障碍物在t秒后的位置和姿态顶点的距离
    robot_model_->estimateSpatioTemporalDistance
  • error[0] 计算和EdgeInflatedObstacle EdgeObstacle相同
  • error[1] 主要使用dynamic_obstacle_inflation_dist来判断

2.4 EdgeViaPoint

  • class EdgeViaPoint 继承 BaseTebUnaryEdge<1, const Eigen::Vector2d*, VertexPose> 一元边
  • 误差向量维度1
  • 观测为Eigen::Vector2d*
  void computeError(){const VertexPose* bandpt = static_cast<const VertexPose*>(_vertices[0]);_error[0] = (bandpt->position() - *_measurement).norm();}

总结

  • EdgeViaPoint 主要是评判机器人贴合原始路径的error cost 离原始点位越远 则 error cost 越大

2.5 EdgeVelocity

  • EdgeVelocity 继承 public BaseTebMultiEdge<2, double> 三元边
  • 连接了3个顶点 2个姿态顶点 1个时间顶点
  void computeError(){const VertexPose* conf1 = static_cast<const VertexPose*>(_vertices[0]);const VertexPose* conf2 = static_cast<const VertexPose*>(_vertices[1]);const VertexTimeDiff* deltaT = static_cast<const VertexTimeDiff*>(_vertices[2]);const Eigen::Vector2d deltaS = conf2->estimate().position() - conf1->estimate().position();double dist = deltaS.norm();const double angle_diff = g2o::normalize_theta(conf2->theta() - conf1->theta());if (cfg_->trajectory.exact_arc_length && angle_diff != 0){double radius =  dist/(2*sin(angle_diff/2));dist = fabs( angle_diff * radius ); // actual arg length!}double vel = dist / deltaT->estimate();//     vel *= g2o::sign(deltaS[0]*cos(conf1->theta()) + deltaS[1]*sin(conf1->theta())); // consider directionvel *= fast_sigmoid( 100 * (deltaS.x()*cos(conf1->theta()) + deltaS.y()*sin(conf1->theta())) ); // consider directionconst double omega = angle_diff / deltaT->estimate();_error[0] = penaltyBoundToInterval(vel, -cfg_->robot.max_vel_x_backwards, cfg_->robot.max_vel_x,cfg_->optim.penalty_epsilon);_error[1] = penaltyBoundToInterval(omega, cfg_->robot.max_vel_theta,cfg_->optim.penalty_epsilon);}

penaltyBoundToInterval 函数如下

inline double penaltyBoundToInterval(const double& var,const double& a, const double& b, const double& epsilon)
{if (var < a+epsilon){return (-var + (a + epsilon));}if (var <= b-epsilon){return 0.;}else{return (var - (b - epsilon));}
}

总结

  • EdgeVelocity 主要是控制了路径的最大最小速度进行的error cost评判 使得机器人速度 以及角速度在合理的范围之内

2.6 EdgeAcceleration

  • class EdgeAcceleration 继承 public BaseTebMultiEdge<2, double> 多元边
  void computeError(){ROS_ASSERT_MSG(cfg_, "You must call setTebConfig on EdgeAcceleration()");const VertexPose* pose1 = static_cast<const VertexPose*>(_vertices[0]);const VertexPose* pose2 = static_cast<const VertexPose*>(_vertices[1]);const VertexPose* pose3 = static_cast<const VertexPose*>(_vertices[2]);const VertexTimeDiff* dt1 = static_cast<const VertexTimeDiff*>(_vertices[3]);const VertexTimeDiff* dt2 = static_cast<const VertexTimeDiff*>(_vertices[4]);// VELOCITY & ACCELERATIONconst Eigen::Vector2d diff1 = pose2->position() - pose1->position();const Eigen::Vector2d diff2 = pose3->position() - pose2->position();double dist1 = diff1.norm();double dist2 = diff2.norm();const double angle_diff1 = g2o::normalize_theta(pose2->theta() - pose1->theta());const double angle_diff2 = g2o::normalize_theta(pose3->theta() - pose2->theta());if (cfg_->trajectory.exact_arc_length) // use exact arc length instead of Euclidean approximation{if (angle_diff1 != 0){const double radius =  dist1/(2*sin(angle_diff1/2));dist1 = fabs( angle_diff1 * radius ); // actual arg length!}if (angle_diff2 != 0){const double radius =  dist2/(2*sin(angle_diff2/2));dist2 = fabs( angle_diff2 * radius ); // actual arg length!}}double vel1 = dist1 / dt1->dt();double vel2 = dist2 / dt2->dt();// consider directions
//     vel1 *= g2o::sign(diff1[0]*cos(pose1->theta()) + diff1[1]*sin(pose1->theta())); 
//     vel2 *= g2o::sign(diff2[0]*cos(pose2->theta()) + diff2[1]*sin(pose2->theta())); vel1 *= fast_sigmoid( 100*(diff1.x()*cos(pose1->theta()) + diff1.y()*sin(pose1->theta())) ); vel2 *= fast_sigmoid( 100*(diff2.x()*cos(pose2->theta()) + diff2.y()*sin(pose2->theta())) ); const double acc_lin  = (vel2 - vel1)*2 / ( dt1->dt() + dt2->dt() );_error[0] = penaltyBoundToInterval(acc_lin,cfg_->robot.acc_lim_x,cfg_->optim.penalty_epsilon);// ANGULAR ACCELERATIONconst double omega1 = angle_diff1 / dt1->dt();const double omega2 = angle_diff2 / dt2->dt();const double acc_rot  = (omega2 - omega1)*2 / ( dt1->dt() + dt2->dt() );_error[1] = penaltyBoundToInterval(acc_rot,cfg_->robot.acc_lim_theta,cfg_->optim.penalty_epsilon);ROS_ASSERT_MSG(std::isfinite(_error[0]), "EdgeAcceleration::computeError() translational: _error[0]=%f\n",_error[0]);ROS_ASSERT_MSG(std::isfinite(_error[1]), "EdgeAcceleration::computeError() rotational: _error[1]=%f\n",_error[1]);}

总结

  • 主要限制了最大角速度和角加速度

2.7 EdgeTimeOptimal

  • EdgeTimeOptimal 继承 public BaseTebUnaryEdge<1, double, VertexTimeDiff> 一元边
  • 误差向量维度为1
  void computeError(){const VertexTimeDiff* timediff = static_cast<const VertexTimeDiff*>(_vertices[0]);_error[0] = timediff->dt();}

总结

  • 时间越小越好， 这样子机器人会向最大速度去调整

2.8 EdgeShortestPath

  • EdgeShortestPath : public BaseTebBinaryEdge<1, double, VertexPose, VertexPose> 二元边
  • 误差向量维度为1
  void computeError() {const VertexPose *pose1 = static_cast<const VertexPose*>(_vertices[0]);const VertexPose *pose2 = static_cast<const VertexPose*>(_vertices[1]);_error[0] = (pose2->position() - pose1->position()).norm();}

总结

  • 两个姿态顶点之间的距离越小cost 越低

2.9 EdgeKinematicsDiffDrive

  • EdgeKinematicsDiffDrive : public BaseTebBinaryEdge<2, double, VertexPose, VertexPose> 二元边
  • 误差维度为2
    nage
  void computeError(){const VertexPose* conf1 = static_cast<const VertexPose*>(_vertices[0]);const VertexPose* conf2 = static_cast<const VertexPose*>(_vertices[1]);Eigen::Vector2d deltaS = conf2->position() - conf1->position();// non holonomic constraint_error[0] = fabs( ( cos(conf1->theta())+cos(conf2->theta()) ) * deltaS[1] - ( sin(conf1->theta())+sin(conf2->theta()) ) * deltaS[0] );// positive-drive-direction constraintEigen::Vector2d angle_vec ( cos(conf1->theta()), sin(conf1->theta()) );	   _error[1] = penaltyBoundFromBelow(deltaS.dot(angle_vec), 0,0);// epsilon=0, otherwise it pushes the first bandpoints away from start}

总结

  • 不允许反方向
  • 不允许 超越动力学范围

2.10 EdgePreferRotDir

  • EdgePreferRotDir : public BaseTebBinaryEdge<1, double, VertexPose, VertexPose> 二元边
  • 误差维度为1
  void computeError(){const VertexPose* conf1 = static_cast<const VertexPose*>(_vertices[0]);const VertexPose* conf2 = static_cast<const VertexPose*>(_vertices[1]);    _error[0] = penaltyBoundFromBelow( _measurement*g2o::normalize_theta(conf2->theta()-conf1->theta()) , 0, 0);}

总结

  • 主要是前3个姿态点 更偏向偏向朝哪个个方向旋转

这篇关于TEB teb_local_planner 构建超图过程error cost分析的文章就介绍到这儿，希望我们推荐的文章对编程师们有所帮助！


http://www.chinasem.cn/article/870821。

相关文章

canal实现mysql数据同步的详细过程

canal实现mysql数据同步的详细过程

《canal实现mysql数据同步的详细过程》：本文主要介绍canal实现mysql数据同步的详细过程,本文通过实例图文相结合给大家介绍的非常详细,对大家的学习或工作具有一定的参考借鉴价值,需要的... 目录1、canal下载2、mysql同步用户创建和授权3、canal admin安装和启动4、canal
阅读更多...
MySQL存储过程之循环遍历查询的结果集详解

MySQL存储过程之循环遍历查询的结果集详解

《MySQL存储过程之循环遍历查询的结果集详解》：本文主要介绍MySQL存储过程之循环遍历查询的结果集,具有很好的参考价值,希望对大家有所帮助,如有错误或未考虑完全的地方,望不吝赐教... 目录前言1. 表结构2. 存储过程3. 关于存储过程的SQL补充总结前言近来碰到这样一个问题：在生产上导入的数据发现
阅读更多...
SpringBoot集成LiteFlow实现轻量级工作流引擎的详细过程

SpringBoot集成LiteFlow实现轻量级工作流引擎的详细过程

《SpringBoot集成LiteFlow实现轻量级工作流引擎的详细过程》LiteFlow是一款专注于逻辑驱动流程编排的轻量级框架,它以组件化方式快速构建和执行业务流程,有效解耦复杂业务逻辑,下面给大... 目录一、基础概念1.1 组件（Component）1.2 规则（Rule）1.3 上下文（Conte
阅读更多...
MyBatis Plus 中 update_time 字段自动填充失效的原因分析及解决方案(最新整理)

MyBatis Plus 中 update_time 字段自动填充失效的原因分析及解决方案(最新整理)

《MyBatisPlus中update_time字段自动填充失效的原因分析及解决方案(最新整理)》在使用MyBatisPlus时,通常我们会在数据库表中设置create_time和update... 目录前言一、问题现象二、原因分析三、总结：常见原因与解决方法对照表四、推荐写法前言在使用 MyBATis
阅读更多...
Python主动抛出异常的各种用法和场景分析

Python主动抛出异常的各种用法和场景分析

《Python主动抛出异常的各种用法和场景分析》在Python中,我们不仅可以捕获和处理异常,还可以主动抛出异常,也就是以类的方式自定义错误的类型和提示信息,这在编程中非常有用,下面我将详细解释主动抛... 目录一、为什么要主动抛出异常？二、基本语法：raise关键字基本示例三、raise的多种用法1. 抛
阅读更多...
基于Python构建一个高效词汇表

基于Python构建一个高效词汇表

《基于Python构建一个高效词汇表》在自然语言处理（NLP）领域,构建高效的词汇表是文本预处理的关键步骤,本文将解析一个使用Python实现的n-gram词频统计工具,感兴趣的可以了解下... 目录一、项目背景与目标1.1 技术需求1.2 核心技术栈二、核心代码解析2.1 数据处理函数2.2 数据处理流程
阅读更多...
github打不开的问题分析及解决

github打不开的问题分析及解决

《github打不开的问题分析及解决》：本文主要介绍github打不开的问题分析及解决,具有很好的参考价值,希望对大家有所帮助,如有错误或未考虑完全的地方,望不吝赐教... 目录一、找到github.com域名解析的ip地址二、找到github.global.ssl.fastly.net网址解析的ip地址三
阅读更多...
Python FastMCP构建MCP服务端与客户端的详细步骤

Python FastMCP构建MCP服务端与客户端的详细步骤

《PythonFastMCP构建MCP服务端与客户端的详细步骤》MCP（Multi-ClientProtocol）是一种用于构建可扩展服务的通信协议框架,本文将使用FastMCP搭建一个支持St... 目录简介环境准备服务端实现（server.py）客户端实现（client.py）运行效果扩展方向常见问题结
阅读更多...
Spring Boot 整合 Apache Flink 的详细过程

Spring Boot 整合 Apache Flink 的详细过程

《SpringBoot整合ApacheFlink的详细过程》ApacheFlink是一个高性能的分布式流处理框架,而SpringBoot提供了快速构建企业级应用的能力,下面给大家介绍Spri... 目录Spring Boot 整合 Apache Flink 教程一、背景与目标二、环境准备三、创建项目 & 添
阅读更多...
Mysql的主从同步/复制的原理分析

Mysql的主从同步/复制的原理分析

《Mysql的主从同步/复制的原理分析》：本文主要介绍Mysql的主从同步/复制的原理分析,具有很好的参考价值,希望对大家有所帮助,如有错误或未考虑完全的地方,望不吝赐教... 目录为什么要主从同步？mysql主从同步架构有哪些？Mysql主从复制的原理/整体流程级联复制架构为什么好？Mysql主从复制注意
阅读更多...

Copyright China编程 23002807@qq.com

沪ICP备2023033072号-2