base_local_planner

包base_local_planner提供了进行DWA(Dynamic Window Approach)的局部路径规划的工具和基本功能模块。 所实现的功能就是给定需要跟踪的轨迹和代价地图的情况下，根据当前的机器人和环境状态，计算用于控制机器人运动的速度指令。

DWA算法的基本思想是，在机器人的控制空间(线速度、角速度指令)中离散地采样，对于每个样本指令，在机器人当前状态和地图信息的基础上，推演未来很短的一段时间内的运动轨迹。 根据发生碰撞的可能性、目标点的接近程度、全局轨迹的跟踪近似度、速度限制等多方面的指标评价这些轨迹并打分，选取得分最高的轨迹，将其对应的指令下发给底盘，控制机器人运动。

base_local_planner提供了用于生成样本轨迹的SimpleTrajectoryGenerator，评估轨迹优势的各种准则类，比如ObstacleCostFunction等，以及实际进行规划的SimpleScoredSamplingPlanner。 把这些功能模块组合起来，就可以得到一个DWA算法的实现。包dwa_local_planner中的DWAPlanner就是这样一个示例。 本文中我们分析这些基本模块的功能和实现。

1. 轨迹采样

base_local_planner的设计思想与move_base的是一脉相承的，它为必要的基本组件用纯虚类定义了接口形式，并在面向这些接口组合各个组件。比如说用于轨迹采样的组件 SimpleTrajectoryGenerator就继承自纯虚类TrajectorySampleGenerator。我们也可以根据自己的需要实现自己的采样器，只要保证实现了接口定义的函数即可。

下面是纯虚类TrajectorySampleGenerator的类型定义。有两个纯虚函数，其中hasMoreTrajectories用于判定是否还有更多的样本，nextTrajectory则用与获取一个样本， 其参数traj就是一个输出参数，用于记录新采样的样本，布尔类型的返回值用于标志成功生成样本与否。

        class TrajectorySampleGenerator {
        public:
            virtual bool hasMoreTrajectories() = 0;
            virtual bool nextTrajectory(Trajectory &traj) = 0;
            virtual ~TrajectorySampleGenerator() {}
        protected:
            TrajectorySampleGenerator() {}
        };

下面是SimpleTrajectoryGenerator的代码片段，可以看出它继承自TrajectorySampleGenerator。在代码片段中列举了它的一些关键成员变量，以注释的形式说明了它们的意义。

        class SimpleTrajectoryGenerator: public base_local_planner::TrajectorySampleGenerator {
            unsigned int next_sample_index_;                    // 生成样本计数
            std::vector<Eigen::Vector3f> sample_params_;        // 用于采样的速度指令参数,每个样本对应一个
            base_local_planner::LocalPlannerLimits* limits_;    // 机器人的运动约束，比如最大线速度角速度等
            Eigen::Vector3f pos_;                               // 机器人的当前位姿
            Eigen::Vector3f vel_;                               // 机器人的当前速度
            bool use_dwa_;                                      // 是否使用DWA算法
            double sim_period_;                                 // DWA仿真周期，只在use_dwa_为真时有效
            double sim_time_;                                   // 仿真时长
            double sim_granularity_;                            // 位移仿真粒度
            double angular_sim_granularity_;                    // 角度仿真粒度
            // 省略其它成员定义
        };

SimpleTrajectoryGenerator只有一个默认的构造函数，它只是为约束对象limits_赋予了空指针。

        SimpleTrajectoryGenerator() { limits_ = NULL; }

下面是SimpleTrajectoryGenerator的初始化函数，有6个参数，其中pos, vel和goal是三个输入参数分别记录了当前机器人的位姿、速度和目标点。limits记录机器人的各种运动限制， vsamples则记录了在x,y方向上和转角的指令样本数量，discretize_by_time则用于配置采样器是否需要在时间上进行离散化处理。

        void SimpleTrajectoryGenerator::initialise(const Eigen::Vector3f& pos,
            const Eigen::Vector3f& vel,
            const Eigen::Vector3f& goal,
            base_local_planner::LocalPlannerLimits* limits,
            const Eigen::Vector3f& vsamples,
            bool discretize_by_time = false) {

在函数的初始，先用成员变量保存这些输入参数，并定义一些局部变量获取机器人的运动制。

            pos_ = pos;
            vel_ = vel;
            limits_ = limits;
            discretize_by_time_ = discretize_by_time;

            Eigen::Vector3f acc_lim = limits->getAccLimits();   // 加速度限制
            double max_vel_th = limits->max_vel_theta;          // 最大角速度
            double min_vel_th = -1.0 * max_vel_th;              // 最小角速度
            double min_vel_x = limits->min_vel_x;               // x轴最小速度
            double max_vel_x = limits->max_vel_x;               // x轴最大速度
            double min_vel_y = limits->min_vel_y;               // y轴最小速度
            double max_vel_y = limits->max_vel_y;               // y轴最大速度

同时，清空采样配置。

            next_sample_index_ = 0;
            sample_params_.clear();

根据样本数量的限制，只要有一个维度的样本数量为0就不再进行采样。

            if (vsamples[0] * vsamples[1] * vsamples[2] > 0) {

首先根据机器人当前的速度和加速度限制，计算机器人的速度限制，并保存在局部变量max_vel和min_vel中。在下面的代码片段中省略了use_dwa_为false时的分支，它与我们的DWA算法无关。

                Eigen::Vector3f max_vel = Eigen::Vector3f::Zero();
                Eigen::Vector3f min_vel = Eigen::Vector3f::Zero();
                // 省略if (!use_dwa_)分支
                max_vel[0] = std::min(max_vel_x, vel[0] + acc_lim[0] * sim_period_);
                max_vel[1] = std::min(max_vel_y, vel[1] + acc_lim[1] * sim_period_);
                max_vel[2] = std::min(max_vel_th, vel[2] + acc_lim[2] * sim_period_);
        
                min_vel[0] = std::max(min_vel_x, vel[0] - acc_lim[0] * sim_period_);
                min_vel[1] = std::max(min_vel_y, vel[1] - acc_lim[1] * sim_period_);
                min_vel[2] = std::max(min_vel_th, vel[2] - acc_lim[2] * sim_period_);

接下来就是根据指定的速度样本参数，生成采样速度指令并将之保存在sample_params_中。类型VelocityIterator，看名字是一个迭代器，它本质上就是对速度指令空间均分为vsamples-1个区间， 并依次取样本空间的最小值、最大值、以及各个区间分界点作为样本点。

                Eigen::Vector3f vel_samp = Eigen::Vector3f::Zero();
                VelocityIterator x_it(min_vel[0], max_vel[0], vsamples[0]);
                VelocityIterator y_it(min_vel[1], max_vel[1], vsamples[1]);
                VelocityIterator th_it(min_vel[2], max_vel[2], vsamples[2]);
                for(; !x_it.isFinished(); x_it++) {
                    vel_samp[0] = x_it.getVelocity();
                    for(; !y_it.isFinished(); y_it++) {
                        vel_samp[1] = y_it.getVelocity();
                        for(; !th_it.isFinished(); th_it++) {
                            vel_samp[2] = th_it.getVelocity();
                            sample_params_.push_back(vel_samp);
                        }
                        th_it.reset();
                    }
                    y_it.reset();
                }
            }
        }

下面是接口hasMoreTrajectories的实现，它只是对比当前样本技术next_sample_index_与样本参数列表的长度，不超过列表长度就认为是还有样本没有采集。

        bool SimpleTrajectoryGenerator::hasMoreTrajectories() {
            return next_sample_index_ < sample_params_.size();
        }

下面则是接口nextTrajectory的实现。它先判定是否还有剩余样本，若有则通过成员函数generateTrajectory，根据样本速度指令生成对应的仿真轨迹。 最后增加样本计数并返回是否成功生成轨迹。

        bool SimpleTrajectoryGenerator::nextTrajectory(Trajectory &comp_traj) {
            bool result = false;
            if (hasMoreTrajectories()) {
                if (generateTrajectory(pos_,vel_, sample_params_[next_sample_index_],  comp_traj))
                    result = true;
            }
            next_sample_index_++;
            return result;
        }

下面是成员函数generateTrajectory的代码片段，它有四个参数，其中pos和vel分别是机器人目前的位姿和速度，sample_target_vel则是样本速度指令，而traj是一个输出参数，它将记录生成的样本轨迹。

        bool SimpleTrajectoryGenerator::generateTrajectory(Eigen::Vector3f pos, Eigen::Vector3f vel, Eigen::Vector3f sample_target_vel,
                        base_local_planner::Trajectory& traj) {

首先清空轨迹traj。

            traj.cost_   = -1.0;
            traj.resetPoints();

然后通过C语言的数学库函数hypot求取x和y的合速度值。hypot本身用于求取直角三角形的斜边长。局部变量eps是一个有限小量，用于对机器人的速度限幅作出适当的膨胀。

            double vmag = hypot(sample_target_vel[0], sample_target_vel[1]);
            double eps = 1e-4;

接下来，判定指令的速度是否在膨胀之后的指令空间内，若不是则报错退出。

            if ((limits_->min_vel_trans >= 0 && vmag + eps < limits_->min_vel_trans) &&
                (limits_->min_vel_theta >= 0 && fabs(sample_target_vel[2]) + eps < limits_-<min_vel_theta))
                return false;
            if (limits_->max_vel_trans >=0 && vmag - eps > limits_->max_vel_trans)
                return false;

我们根据仿真粒度计算仿真步数，即样本点数。如果速度量过小，在sim_time_的时间内不能产生超过仿真粒度的线位移或者角位移就报错退出。

            double sim_time_distance = vmag * sim_time_;
            double sim_time_angle = fabs(sample_target_vel[2]) * sim_time_;
            int num_steps = ceil(std::max(sim_time_distance / sim_granularity_, sim_time_angle / angular_sim_granularity_));
            if (num_steps == 0)
                return false;

计算仿真步长。如果考虑加速度，则通过成员函数computeNewVelocities计算，依据当前速度、指令速度、加速度限制、以及仿真步长计算新的速度量。

            double dt = sim_time_ / num_steps;
            Eigen::Vector3f loop_vel;
            if (continued_acceleration_)
                loop_vel = computeNewVelocities(sample_target_vel, vel, limits_->getAccLimits(), dt);
            else
                loop_vel = sample_target_vel;
            traj.time_delta_ = dt;
            traj.xv_ = loop_vel[0];
            traj.yv_ = loop_vel[1];
            traj.thetav_ = loop_vel[2];

在一个for循环中，依次计算各个仿真步的位置点，并将之添加到输出参数traj中。

            for (int i = 0; i < num_steps; ++i) {
                traj.addPoint(pos[0], pos[1], pos[2]);
                if (continued_acceleration_)
                    loop_vel = computeNewVelocities(sample_target_vel, loop_vel, limits_->getAccLimits(), dt);
                pos = computeNewPositions(pos, loop_vel, dt);
            }
            return true;
        }

至于成员函数computeNewVelocities和computeNewPositions，所实现的功能就是函数名称的字面意思，计算新的速度和位置。具体实现结合物理和几何关系很容易理解，这里不再详述。

通过轨迹生成器SimpleTrajectoryGenerator，我们可以在一段很短的仿真时间内，采样得到多个局部轨迹样本。DWA算法将在这些生成的样本中间选择最优的路径和速度指令，作为下一控制时刻的局部路径。 base_local_planner包中定义了很多评价轨迹的准则类，下面我们来分析这些准则类是如何评价轨迹和打分的。

2. 轨迹评价

在DWAPlanner中，定义和实现了如下的一些真则对象，这些准则对象虽然具有不同的数据类型， 但是它们都继承自同一个基类TrajectoryCostFunction。

基类TrajectoryCostFunction仍然是一个纯虚类，下面是其类型定义。这里定义了两个接口函数prepare和scoreTrajectory，分别用于更新准则对象的上下文和对样本轨迹打分。 除此之外，它还定义了一个私有成员scale_并为之提供了get和set接口。

        class TrajectoryCostFunction {
        public:
            virtual bool prepare() = 0;
            virtual double scoreTrajectory(Trajectory &traj) = 0;
            double getScale() { return scale_; }
            void setScale(double scale) { scale_ = scale; }
            virtual ~TrajectoryCostFunction() {}
        protected:
            TrajectoryCostFunction(double scale = 1.0): scale_(scale) {}
        private:
            double scale_;
        };

这些准则计算的都是代价，也就是说机器人沿着一条仿真轨迹运动所需要付出的代价。代价越小意味着路径越优，但是代价都应当是正数。如果准则对象为某条路径计算的代价是个负数， 意味着该路径是不合理的。

下面是TwirlingCostFunction的实现，它是这里面最简单的准则类。接口prepare总是返回true，scoreTrajectory总是以轨迹的角速度值作为轨迹的代价。其它准则类的实现，这里不再深究， 具体的意义可以参考上表。

        class TwirlingCostFunction: public base_local_planner::TrajectoryCostFunction {
        public:
            TwirlingCostFunction() {}
            ~TwirlingCostFunction() {}
            double scoreTrajectory(Trajectory &traj) { return fabs(traj.thetav_); }
            bool prepare() { return true; }
        };

3. 轨迹筛选

在DWAPlanner中使用的是SimpleScoredSamplingPlanner对样本轨迹进行筛选。SimpleScoredSamplingPlanner也有一个纯虚类的基类TrajectorySearch，其类定义如下， 只有一个接口函数findBestTrajectory。该函数有两个参数，其中traj是一个输出参数记录了最优的轨迹。all_explored也是一个输出参数，它是一个指针可以传递NULL， 如果传参不是NULL将在容器对象中记录所有的样本轨迹，主要用于Debug。

        class TrajectorySearch {
        public:
            virtual bool findBestTrajectory(Trajectory& traj, std::vector* all_explored) = 0;
            virtual ~TrajectorySearch() {}
        protected:
            TrajectorySearch() {}
        };

下面是SimpleScoredSamplingPlanner的类定义，它有两个私有的成员变量gen_list_和critics_，分别记录了轨迹生成器和准则对象。这里，我们只关注接口findBestTrajectory。

        class SimpleScoredSamplingPlanner : public base_local_planner::TrajectorySearch {
        public:
            ~SimpleScoredSamplingPlanner() {}
            SimpleScoredSamplingPlanner() {}
            SimpleScoredSamplingPlanner(std::vector<TrajectorySampleGenerator*> gen_list, std::vector<TrajectoryCostFunction*>& critics, int max_samples = -1);
            double scoreTrajectory(Trajectory& traj, double best_traj_cost);
            bool findBestTrajectory(Trajectory& traj, std::vector<Trajectory>* all_explored = 0);
        private:
            std::vector<TrajectorySampleGenerator*> gen_list_;
            std::vector<TrajectoryCostFunction*> critics_;
            int max_samples_;
        };

下面是接口函数findBestTrajectory的代码片段。首先，我们遍历所有的准则对象，只要有一个对象没有准备好，就报错退出。

        bool SimpleScoredSamplingPlanner::findBestTrajectory(Trajectory& traj, std::vector<Trajectory>* all_explored) {
            for (std::vector<TrajectoryCostFunction*>::iterator loop_critic = critics_.begin();
                 loop_critic != critics_.end(); ++loop_critic) {
                TrajectoryCostFunction* loop_critic_p = *loop_critic;
                if (loop_critic_p->prepare() == false)
                    return false;
            }

接着遍历所有的轨迹生成器，重置计数器count和count_valid。

            for (std::vector<TrajectorySampleGenerator*>::iterator loop_gen = gen_list_.begin();
                 loop_gen != gen_list_.end(); ++loop_gen) {
                TrajectorySampleGenerator* gen_ = *loop_gen;
                count = 0;
                count_valid = 0;

根据采样器的hasMoreTrajectories接口来判定是否生成了所有的样本。并通过接口nextTrajectory生成样本轨迹。

                while (gen_->hasMoreTrajectories()) {
                    gen_success = gen_->nextTrajectory(loop_traj);
                    if (false == gen_success)
                        continue;

然后计算轨迹代价，如果代价小于当前最优轨迹的代价，则替换之。

                    loop_traj_cost = scoreTrajectory(loop_traj, best_traj_cost);
                    if (loop_traj_cost >= 0) {
                        count_valid++;
                        if (best_traj_cost < 0 || loop_traj_cost < best_traj_cost) {
                            best_traj_cost = loop_traj_cost;
                            best_traj = loop_traj;
                        }
                    }
                    // 省略最大样本数量的限制语句
                }

如果遍历完一个采样器，得到了一条较优的路径，我们就记录下来并退出循环。

                if (best_traj_cost >= 0) {
                    traj.xv_ = best_traj.xv_;
                    traj.yv_ = best_traj.yv_;
                    traj.thetav_ = best_traj.thetav_;
                    traj.cost_ = best_traj_cost;
                    traj.resetPoints();
                    double px, py, pth;
                    for (unsigned int i = 0; i < best_traj.getPointsSize(); i++) {
                        best_traj.getPoint(i, px, py, pth);
                        traj.addPoint(px, py, pth);
                    }
                    break;
                }
            }
            return best_traj_cost >= 0;
        }

4. 完

本文中，我们分析了包base_local_planner的源码。它为DWA算法定义了各个基本组件，包括用于采样的轨迹生成器、评价轨迹的准则类、以及进行优选的规划器。