机器人控制算法——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 的所有元素均赋值为 n

        Eigen::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 obstacles
            for (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 class
        virtual 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 point
        if (vertices.size() == 1)
        {
            return (point - vertices.front()).norm();
        }

        // check each polygon edge
        for (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 edge
            if (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 class
        virtual 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 point
        if (vertices1.size() == 1)
        {
            return distance_point_to_polygon_2d(vertices1.front(), vertices2);
        }

        // check each edge of polygon1
        for (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 edge
            if (new_dist < dist)
                return new_dist;
        }

        return dist;
    }

这个代码的思路就是，以障碍物的顶点为准，然后循环计算该顶点和机器人的多边形每个边的距离（本质也是点到直线距离，很容易求解）并记录最短距离，然后障碍物的顶点遍历完即可，这样找对了最小距离。

3.Summary

上述没有介绍全，机器人有好多形状，障碍物也有好多形状，二者随机组合，就会出现不同的距离求解方法，具体可去看TEB开源代码。