机器人控制算法——TEB算法障碍物检测分析

本文主要是介绍机器人控制算法——TEB算法障碍物检测分析，希望对大家解决编程问题提供一定的参考价值，需要的开发者们随着小编来一起学习吧！

1.Background

在规划路线的时，需要机器人路线附近的障碍物距离，机器人控制系统需要知道当前机器人与障碍物最短的距离。本文主要是分析如何计算机器人与障碍物的距离，如果将机器人和障碍物分别考虑成质点，机器人与障碍物的距离就很容易求解了，但是事实上，障碍物与机器人在实际工程中不可能是质点。因此，本文需要解决的是：
机器人形状分别圆形、线性、多边形，障碍物也分别是圆形、线性、多边形时，二者的最小距离求解。

2.Algorithm

TEB算法的障碍物程序的入口在此处：

void TebOptimalPlanner::AddEdgesObstacles(double weight_multiplier){if (cfg_->optim.weight_obstacle==0 || weight_multiplier==0 || obstacles_==nullptr )return; // if weight equals zero skip adding edges!bool inflated = cfg_->obstacles.inflation_dist > cfg_->obstacles.min_obstacle_dist;Eigen::Matrix<double,1,1> information;information.fill(cfg_->optim.weight_obstacle * weight_multiplier);//mat.fill(n)　将 mat 的所有元素均赋值为 nEigen::Matrix<double,2,2> information_inflated;information_inflated(0,0) = cfg_->optim.weight_obstacle * weight_multiplier;information_inflated(1,1) = cfg_->optim.weight_inflation;information_inflated(0,1) = information_inflated(1,0) = 0;std::vector<Obstacle*> relevant_obstacles;relevant_obstacles.reserve(obstacles_->size());// iterate all teb points (skip first and last)for (int i=1; i < teb_.sizePoses()-1; ++i){double left_min_dist = std::numeric_limits<double>::max();double right_min_dist = std::numeric_limits<double>::max();Obstacle* left_obstacle = nullptr;Obstacle* right_obstacle = nullptr;relevant_obstacles.clear();const Eigen::Vector2d pose_orient = teb_.Pose(i).orientationUnitVec();// iterate obstaclesfor (const ObstaclePtr& obst : *obstacles_){// we handle dynamic obstacles differently below　　//我们以不同的方式处理下面的动态障碍if(cfg_->obstacles.include_dynamic_obstacles && obst->isDynamic())continue;// calculate distance to robot model// //! 根据不同的机器人模型（点，圆，多边形等），不同的障碍物模型（点，线，多边形），有不同的距离计算方法double dist = robot_model_->calculateDistance(teb_.Pose(i), obst.get());

1. 机器人是圆形时

（1）障碍物是圆形时：

        /*** @brief Calculate the distance between the robot and an obstacle* @param current_pose Current robot pose* @param obstacle Pointer to the obstacle* @return Euclidean distance to the robot*/virtual double calculateDistance(const PoseSE2& current_pose, const Obstacle* obstacle) const{return obstacle->getMinimumDistance(current_pose.position()) - radius_;}

        // implements getMinimumDistance() of the base classvirtual double getMinimumDistance(const Eigen::Vector2d& position) const{return (position-pos_).norm() - radius_;}

使用机器人坐标计算出机器人坐标与障碍物坐标的距离，然后再分别减去机器人的半径radius_和障碍物的半径radius_即可

（2）障碍物是多边形

 virtual double getMinimumDistance(const Eigen::Vector2d& position) const{return distance_point_to_polygon_2d(position, vertices_);}

/*** @brief Helper function to calculate the smallest distance between a point and a closed polygon* @param point 2D point* @param vertices Vertices describing the closed polygon (the first vertex is not repeated at the end)* @return smallest distance between point and polygon
*/inline double distance_point_to_polygon_2d(const Eigen::Vector2d& point, const Point2dContainer& vertices){double dist = HUGE_VAL;// the polygon is a pointif (vertices.size() == 1){return (point - vertices.front()).norm();}// check each polygon edgefor (int i=0; i<(int)vertices.size()-1; ++i){double new_dist = distance_point_to_segment_2d(point, vertices.at(i), vertices.at(i+1));
//       double new_dist = calc_distance_point_to_segment( position,  vertices.at(i), vertices.at(i+1));if (new_dist < dist)dist = new_dist;}if (vertices.size()>2) // if not a line close polygon 数组头和尾顶点的边也要算上{double new_dist = distance_point_to_segment_2d(point, vertices.back(), vertices.front()); // check last edgeif (new_dist < dist)return new_dist;}return dist;}

position是机器人圆心位置坐标，vertices_为多边形的顶点，我们会使用循环计算出来，机器人位置与多边形的每条边的距离（本质就是点到直线距离的求解），然后取最小，最小的距离再减去机器人圆形的半径就是机器人距离障碍物的最小距离。

2. 机器人是多变形

障碍物是多边形时，这种是最复杂的情况求解。

          * @brief Calculate the distance between the robot and an obstacle* @param current_pose Current robot pose* @param obstacle Pointer to the obstacle* @return Euclidean distance to the robot*/virtual double calculateDistance(const PoseSE2& current_pose, const Obstacle* obstacle) const{Point2dContainer polygon_world(vertices_.size());transformToWorld(current_pose, polygon_world);return obstacle->getMinimumDistance(polygon_world);}

       // implements getMinimumDistance() of the base classvirtual double getMinimumDistance(const Point2dContainer& polygon) const{return distance_polygon_to_polygon_2d(polygon, vertices_);}

polygon代表机器人的多边形形状的顶点， vertices_代表障碍物的多边形的顶点。

/*** @brief Helper function to calculate the smallest distance between two closed polygons* @param vertices1 Vertices describing the first closed polygon (the first vertex is not repeated at the end)* @param vertices2 Vertices describing the second closed polygon (the first vertex is not repeated at the end)* @return smallest distance between point and polygon
*/inline double distance_polygon_to_polygon_2d(const Point2dContainer& vertices1, const Point2dContainer& vertices2){double dist = HUGE_VAL;// the polygon1 is a pointif (vertices1.size() == 1){return distance_point_to_polygon_2d(vertices1.front(), vertices2);}// check each edge of polygon1for (int i=0; i<(int)vertices1.size()-1; ++i){double new_dist = distance_segment_to_polygon_2d(vertices1[i], vertices1[i+1], vertices2);if (new_dist < dist)dist = new_dist;}if (vertices1.size()>2) // if not a line close polygon1{double new_dist = distance_segment_to_polygon_2d(vertices1.back(), vertices1.front(), vertices2); // check last edgeif (new_dist < dist)return new_dist;}return dist;}