RobotFootprintModel
为机器人的模型基类,用来定义机器人的形状。但机器人模型类目前只用于优化,因为考虑到导航栈的足迹可能是低效的。因此,设置机器人的形状只是用来检测可行性。
此类族在 robot_footprint_model.h
头文件中。
1.1 BaseRobotFootprintModel
函数如下:
1.2 其他RobotFootprintModel
下面介绍更具体的模型。
teb_local_planner
是TEB算法的核心代码。里面包含了对最优路径的选择和计算,并且,可以求出当前应该要行驶的线速度和角速度。
2.1 基类PlannerInterface
PlannerInterface
是TEB算法继承的基类。里面定义了一个规划器应该具有的功能和能提供的服务。
里面的函数有:
2.2 TebOptimalPlanner
该类采用g2o框架优化了时间弹性带轨迹。
TebOptimalPlanner(const TebConfig& cfg, ObstContainer* obstacles = NULL, RobotFootprintModelPtr robot_model = boost::make_shared<PointRobotFootprint>(), TebVisualizationPtr visual = TebVisualizationPtr(), const ViaPointContainer* via_points = NULL);
必选参数介绍:
可选参数介绍:
初始化函数,用于构造函数:
void initialize(const TebConfig& cfg, ObstContainer* obstacles = NULL, RobotFootprintModelPtr robot_model = boost::make_shared<PointRobotFootprint>(), TebVisualizationPtr visual = TebVisualizationPtr(), const ViaPointContainer* via_points = NULL)
这里作用为初始化算法,其中的参数的含义和上面构造函数类似。
- 重置TEB规划器。清除内部的数据结构图和路径。
virtual void clearPlanner()
{
clearGraph();
teb_.clearTimedElasticBand();
}
- 设定倾向的最初的转弯角度。通过惩罚另外一个完成。如果计划的轨迹在两个成本相近的解决方案(在同一等价类中!)之间摇摆,可以指定一个期望的(初始)转弯方向。检查参数,以便调整惩罚的权重。初始意味着惩罚只适用于轨迹的前几个姿势。
virtual void setPreferredTurningDir(RotType dir) {prefer_rotdir_=dir;}
- 将为TEB定义的顶点和边注册到
g2o::Factory
。
static void registerG2OTypes();
- 访问内部的TimedElasticBand轨迹。
TimedElasticBand& teb() {return teb_;};
const TimedElasticBand& teb() const {return teb_;};
- 访问内部的
g2o
优化器。
boost::shared_ptr<g2o::SparseOptimizer> optimizer() {return optimizer_;};
boost::shared_ptr<const g2o::SparseOptimizer> optimizer() const {return optimizer_;};
- 检测最后一次优化是否成功
bool isOptimized() const {return optimized_;};
- 计算一个给定的优化问题的cost vector(hyper-graph(超图) 必须有)。使用这种方法可以获得关于当前边缘误差/成本(局部成本函数)的信息。成本值的向量是根据不同的边的类型(时间_最佳,障碍物,…)组成的。详细的构成请参考方法声明。最小化时间差的边(EdgeTimeOptimal)的成本对应于所有单次时间差的平方之和。$ \sum_i \Delta T_i^2 $。有时,用户可能希望得到一个与实际轨迹转换时间成比例或相同的值 $\sum_i Δ T_i $。将
alternative_time_cost
设置为true
,以便得到用后一个方程计算的成本,但要检查实现的定义,如果该值被缩放以匹配其他成本值的大小。
void computeCurrentCost(double obst_cost_scale=1.0, double viapoint_cost_scale=1.0, bool alternative_time_cost=false);
- 计算并返回当前优化图的代价(支持多个轨迹)。
virtual void computeCurrentCost(std::vector<double>& cost, double obst_cost_scale=1.0, double viapoint_cost_scale=1.0, bool alternative_time_cost=false)
{
computeCurrentCost(obst_cost_scale, viapoint_cost_scale, alternative_time_cost);
cost.push_back( getCurrentCost() );
}
- 获取成本向量(cost vector)。通过使用
computeCurrentCost
或调用了cost
为true
的optimationTEB
函数计算出的累计成本值。
double getCurrentCost() const {return cost_;}
- 从连续位姿和时间差中提取速度(包括完整性机器人的strafing(纵向?)速度)。速度是用有限差分法提取的。平移速度的方向也被确定。
inline void extractVelocity(const PoseSE2& pose1, const PoseSE2& pose2, double dt, float& vx, float& vy, float& omega) const;
- 计算轨迹的速度分布。这种方法计算出完整计划轨迹的平移和旋转速度。第一个速度是作为初始速度提供的速度(固定)。索引k=2…end-1的速度与从姿势_{k-1}到姿势_k的转换有关。最后一个速度是最终速度(固定的)。因此Twist对象的数量是sizePoses()+1。总结如下:
v[0] = v_start,
v[1,...end-1] = +-(pose_{k+1}-pose{k})/dt,
v(end) = v_goal
void getVelocityProfile(std::vector<Twist>& velocity_profile) const;
- 获取完整的路径信息,包括位姿序列,速度分布和时间信息。它对于评估和调试目的或开环控制很有用。由于用差分商数得到的速度是连续姿势之间的平均速度,因此在时间戳k处的每个姿势的速度是通过取两个速度之间的平均值得到的。第一个姿势的速度是
v_start
(提供初始值),最后一个姿势的速度是v_goal
(通常为零,如果free_goal_vel
为false
)。参见getVelocityProfile()
获取连续点之间的速度列表。速度分布暂时还没有加上。
void getFullTrajectory(std::vector<Eigen::Vector3f>& trajectory) const;
- 检查规划的路径是否可行。这个方法目前只检查轨迹或轨迹的一部分是否无碰撞。障碍物在这里被表示为
costmap
而不是内部的ObstacleContainer
。
virtual bool isTrajectoryFeasible(void);
protected
的函数这里就不展开介绍了。
Original: https://blog.csdn.net/loyer_kong/article/details/123619749
Author: loyer_kong
Title: TEB算法-2
原创文章受到原创版权保护。转载请注明出处:https://www.johngo689.com/597552/
转载文章受原作者版权保护。转载请注明原作者出处!