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 类族
-
Obstacle
类只是一个抽象类,用来定义障碍物的接口。有如下方法:getCentroid()
获取障碍物的质心坐标。getCentroidCplx()
获取障碍物的质心复数坐标。checkCollision(const Eigen::Vector2d& position, double min_dist)
碰撞检查和距离计算。当给定一个点的时候检查是否会与这个障碍物碰撞,当检查的距离小于参数min_dist
时,判定为碰撞true
。checkLineIntersection(const Eigen::Vector2d& line_start, const Eigen::Vector2d& line_end, double min_dist=0)
检查给定的2点之间的线是否与障碍物相交,并保持一定的安全距离min_dist
。getMinimumDistance(const Eigen::Vector2d& position)
获得点到障碍物的最小欧氏距离。getMinimumDistance(const Eigen::Vector2d& line_start, const Eigen::Vector2d& line_end)
获得2点之间的线段到障碍物的最短欧氏距离。getMinimumDistance(const Point2dContainer& polygon)
获得多边形到障碍物的最小欧氏距离。多边形用二维中的点集合来描述。getClosestPoint(const Eigen::Vector2d& position)
获得距离position
障碍物边界上最近的点。
接下来的函数解析不是很用得到,本次项目主要还是对于固定障碍物的避障,对于动态障碍物,并不做要求。而接下来要介绍的函数是针对动态障碍物的。
getMinimumSpatioTemporalDistance(const Eigen::Vector2d& position, double t)
用来估计点到以恒定速度移动的障碍物的最小时空距离。其中t
指的是时间,用于估计到障碍物最短距离getMinimumSpatioTemporalDistance(const Eigen::Vector2d& line_start, const Eigen::Vector2d& line_end, double t)
用来估计线段到以恒定速度移动的障碍物的最短时空距离。其中t
指的是时间,用于估计到障碍物最短距离。getMinimumSpatioTemporalDistance(const Point2dContainer& polygon, double t)
用来估计多边形到以恒定速度移动的障碍物的最短时空距离。其中t
指的是时间,用于估计到障碍物最短距离。predictCentroidConstantVelocity(double t, Eigen::Ref<Eigen::Vector2d> position)
用来预测以恒定速度运动的障碍物的质心在t
时间后的位置。isDynamic()
用来判断障碍物是不是以一个非0的速度进行运动。setCentroidVelocity(const Eigen::Ref<const Eigen::Vector2d>& vel)
根据质心,给障碍物设置2维的速度(vx, vy)
。- [不建议用]
setCentroidVelocity(const TwistWithCovariance& velocity,const Quaternion& orientation)
根据质心,给障碍物设置2维的速度(vx, vy)
。只是用TwistWithCovariance
数据表示(vx, vy)
而已。这里需要用到tf变换,然后作者把它给删除了,也就是用不到四元数了,这个函数慎用。 getCentroidVelocity()
获取质心的速度(vx, vy)
作为障碍物整体的速度。
下面是一些帮助函数。
toPolygonMsg(Polygon& polygon)
将当前障碍物转换成多边形信息输入,其中polygon
参数用来输出。toTwistWithCovarianceMsg(TwistWithCovariance& twistWithCovariance)
将当前障碍物转换成TwistWithCovariance
信息。其中,Twist
是ROS
发布的用来控制小车移动的话题。其中,包含着线速度和角速度。这里面,TwistWithCovariance
指的是带有协方差矩阵的线速度和角速度信息。其中,协方差应该是用来表示线速度和角速度之间的相关性。EIGEN_MAKE_ALIGNED_OPERATOR_NEW
Eigen的一组函数,用来校正数据,对齐?
-
PointObstacle
点障碍物。 -
CircularObstacle
圆障碍物,其实就是点障碍物加上半径。 -
LineObstacle
线障碍物。 -
PillObstacle
椭圆形障碍物。就是线障碍物和距离或者说是半径的组合。 -
PolygonObstacle
多边形障碍物。一个障碍物有着任意多的顶点。如果一个多边形障碍物只有2个顶点,那么就可以认为是线障碍物,否则,该多边形必须是封闭的。其中的顶点是按照顺序连接起来,其中,第一个顶点和最后一个顶点自动连接。
4 ViaPointContainer
这只是一个类型的其他名称。
typedef std::vector< Eigen::Vector2d, Eigen::aligned_allocator<Eigen::Vector2d> > 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;