TEB算法-1

文章目录

  • 0. 背景
  • 1. 参数配置
  • 2. PoseSE2 类
  • 3. Obstacle 类族
  • 4 ViaPointContainer
  • 5. 相关代码

0. 背景

本篇学习记录主要针对于GitHub上的TEB算法的非ROS实现:https://github.com/linyicheng1/teb_local_planner。学习的也是上面的代码。然后,根据作者写的一个测试程序,去学习整个项目。

最后,能做到会用TEB算法就行。

1. 参数配置

这里主要包括算法的参数说明,算法的一些类的数据结构和函数介绍。

主要为TebConfig 类,该类只是用来记录算法的参数,相当于teb原本算法的参数文件。

2. PoseSE2 类

该类用于表示位姿。由x, y 和 theta 组成。其中theta的范围是 [-pi, pi]

每个函数的含义可以直接查头文件pose_se2.h。里面的注释很详细。

3. Obstacle 类族

  1. Obstacle类只是一个抽象类,用来定义障碍物的接口。有如下方法:

    1. getCentroid() 获取障碍物的质心坐标。
    2. getCentroidCplx() 获取障碍物的质心复数坐标。
    3. checkCollision(const Eigen::Vector2d& position, double min_dist) 碰撞检查和距离计算。当给定一个点的时候检查是否会与这个障碍物碰撞,当检查的距离小于参数min_dist时,判定为碰撞true
    4. checkLineIntersection(const Eigen::Vector2d& line_start, const Eigen::Vector2d& line_end, double min_dist=0) 检查给定的2点之间的线是否与障碍物相交,并保持一定的安全距离min_dist
    5. getMinimumDistance(const Eigen::Vector2d& position) 获得点到障碍物的最小欧氏距离。
    6. getMinimumDistance(const Eigen::Vector2d& line_start, const Eigen::Vector2d& line_end) 获得2点之间的线段到障碍物的最短欧氏距离。
    7. getMinimumDistance(const Point2dContainer& polygon) 获得多边形到障碍物的最小欧氏距离。多边形用二维中的点集合来描述。
    8. getClosestPoint(const Eigen::Vector2d& position) 获得距离 position 障碍物边界上最近的点。

    接下来的函数解析不是很用得到,本次项目主要还是对于固定障碍物的避障,对于动态障碍物,并不做要求。而接下来要介绍的函数是针对动态障碍物的。

    1. getMinimumSpatioTemporalDistance(const Eigen::Vector2d& position, double t) 用来估计点到以恒定速度移动的障碍物的最小时空距离。其中t指的是时间,用于估计到障碍物最短距离
    2. getMinimumSpatioTemporalDistance(const Eigen::Vector2d& line_start, const Eigen::Vector2d& line_end, double t) 用来估计线段到以恒定速度移动的障碍物的最短时空距离。其中t指的是时间,用于估计到障碍物最短距离。
    3. getMinimumSpatioTemporalDistance(const Point2dContainer& polygon, double t) 用来估计多边形到以恒定速度移动的障碍物的最短时空距离。其中t指的是时间,用于估计到障碍物最短距离。
    4. predictCentroidConstantVelocity(double t, Eigen::Ref position) 用来预测以恒定速度运动的障碍物的质心在 t 时间后的位置。
    5. isDynamic() 用来判断障碍物是不是以一个非0的速度进行运动。
    6. setCentroidVelocity(const Eigen::Ref& vel) 根据质心,给障碍物设置2维的速度 (vx, vy)
    7. [不建议用]setCentroidVelocity(const TwistWithCovariance& velocity,const Quaternion& orientation) 根据质心,给障碍物设置2维的速度 (vx, vy)。只是用TwistWithCovariance数据表示(vx, vy)而已。这里需要用到tf变换,然后作者把它给删除了,也就是用不到四元数了,这个函数慎用。
    8. getCentroidVelocity() 获取质心的速度(vx, vy)作为障碍物整体的速度。

    下面是一些帮助函数。

    1. toPolygonMsg(Polygon& polygon) 将当前障碍物转换成多边形信息输入,其中polygon参数用来输出。
    2. toTwistWithCovarianceMsg(TwistWithCovariance& twistWithCovariance) 将当前障碍物转换成TwistWithCovariance信息。其中,TwistROS发布的用来控制小车移动的话题。其中,包含着线速度和角速度。这里面, TwistWithCovariance指的是带有协方差矩阵的线速度和角速度信息。其中,协方差应该是用来表示线速度和角速度之间的相关性。
    3. EIGEN_MAKE_ALIGNED_OPERATOR_NEW Eigen的一组函数,用来校正数据,对齐?
  2. PointObstacle点障碍物。

  3. CircularObstacle圆障碍物,其实就是点障碍物加上半径。

  4. LineObstacle线障碍物。

  5. PillObstacle椭圆形障碍物。就是线障碍物和距离或者说是半径的组合。

  6. PolygonObstacle多边形障碍物。一个障碍物有着任意多的顶点。如果一个多边形障碍物只有2个顶点,那么就可以认为是线障碍物,否则,该多边形必须是封闭的。其中的顶点是按照顺序连接起来,其中,第一个顶点和最后一个顶点自动连接。

4 ViaPointContainer

这只是一个类型的其他名称。

typedef std::vector< Eigen::Vector2d, Eigen::aligned_allocator > ViaPointContainer;

不过对于这个类型,我们应该不需要掌握很多,需要研究TEB算法才需要了解。这里面应该是TEB算法存储各种点的容器。

5. 相关代码

TebConfig config;
PoseSE2 start(-2,0,0);
PoseSE2 end(2,0,0);
std::vector<ObstaclePtr> obst_vector;
obst_vector.emplace_back(boost::make_shared<PointObstacle>(0,0));
ViaPointContainer via_points;

你可能感兴趣的:(项目实训,算法,自动驾驶,人工智能)