TEB算法-1

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信息。其中，Twist是ROS发布的用来控制小车移动的话题。其中，包含着线速度和角速度。这里面， 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;