上一讲梳理了ExplorationPlanner文件夹下的kinematics文件夹下的所有头文件,这一讲我们梳理planner文件夹下的所有头文件。

ExplorationPlanner

ExplorationPlanner的头文件总览

从上面的截图可以看出,ExplorationPlanner分为这几部分:grid, kinematics, planner, sensor, trajectory, utils。

3.planner部分

planner文件夹的头文件总览

1>ICoolingSchedule.h

看头文件,include<memory>可知接下来会使用智能指针std::unique_ptr;

virtual std::unique_ptr<ICoolingSchedule> clone() const = 0;

看函数功能,这里getNumberOfReplicates获取需要复制的数量,输入是迭代次数;

 virtual unsigned int getNumberOfReplicates(unsigned int iteration) = 0;

全体代码看下来,不难看出这个ICoolingSchedule类作为一个抽象基类使用。

2>ExplorationTaskState.h

先看头文件,一个来自kinematics目录下的PlanarPose.h,该头文件的作用是表示x,y,theta;

#include "ExplorationPlanner/kinematics/PlanarPose.h

第二个是来自grid目录下的PlanarGridOccupancyMap.h,该头文件的作用是表示对占据栅格地图的单个栅格单元,所有栅格单元的内容的管理,包括单个栅格单元的更新,整个栅格地图的更新;

#include "ExplorationPlanner/grid/PlanarGridOccupancyMap.h"

第三个头文件是来自sensor目录下的PlanarObservation.h,这个文件夹主要是用传感器模拟出observations,用于更新栅格地图;

#include "ExplorationPlanner/sensor/PlanarObservation.h

看ExplorationTaskState这个类类内的成员:

private
    PlanarPose robot_pose_;//机器人的位姿
    PlanarGridOccupancyMap map_;//占据栅格地图

以及其它的函数功能,更新地图updateMap:

//更新地图,输入引起栅格地图变化的,由传感器模拟产生的observations
void updateMap(const PlanarObservation& obs)
{
   map_.update(obs);
}

函数设置位姿setPose,获取机器人位姿getRobotPose,获取地图getMap因为机器人的位姿robot_pose和地图map_在私有成员里,需要通过这些类内函数去获取:

void setPose(const PlanarPose& pose) {
        robot_pose_ = pose;
    }
PlanarPose getRobotPose() const  {
        return robot_pose_;
    }
const PlanarGridOccupancyMap& getMap() const {
        return map_;
    }

3>IPlanningAlgorithm.h

先看头文件,一个是planner文件夹本地的ExplorationTaskState.h;

#include "ExplorationTaskState.h"

另一个是kinematics目录下的PlanarPose.h,用于表示x,y,theta,通常用来表示位姿

#include "ExplorationPlanner/kinematics/PlanarPose.h"

再看IPlanningAlgorithm类内所有函数都加上了virtual关键字,我们可以判断是要将此类作为一个抽象基类;

virtual ~IPlanningAlgorithm() {} //虚析构函数
//传入更新量state,输出:是否去规划新的路径
virtual bool plan(const ExplorationTaskState& state) = 0;
//传入已传入的路径,获取已规划的路径
virtual void getPlan(std::vector<PlanarPose> &plan) const = 0;

4>LinearCoolingSchedule.h

先看包含的头文件,是上面我们说的抽象基类ICoolingSchedule所在的头文件,主要用途是获取复制的数量,它的输入是迭代次数;

#include "ICoolingSchedule.h"

下面我们可以看到LinearCoolingSchedule类继承了抽象基类ICoolingSchedule;

class LinearCoolingSchedule : virtual public ICoolingSchedule

接下来我们看到这个类的构造函数使用初始化列表,初始化a_,b_;

 LinearCoolingSchedule(unsigned int a, unsigned int b)
        : a_(a), b_(b)
{
}

而与a_和b_有关,是将其用于计算复制数量C:

[公式]

//获取复制数量
unsigned int getNumberOfReplicates(unsigned int iteration)
{
    return a_*iteration + b_;
}

5>Particle.h

先看头文件,首先是kinematics目录下的两个关于机器人速度和速度采样的头文件;

#include "ExplorationPlanner/kinematics/PlanarRobotVelCmd.h"
#include "ExplorationPlanner/kinematics/IPlanarRobotVelCmdSampler.h"

使用共享指针shared_ptr;

#include <boost/shared_ptr.hpp>

共享指针补充:

看到Particle类,particle是粒子的意思,我们可以看到它的类内私有成员里是粒子的权重,轨迹,机器人的速度及轨迹末端的最后旋转角度;

private:
    // Private attributes
    //
    double weight_;//粒子权重
    std::vector<PlanarPose> trajectory_;//PlanrPose(包含x,t,theta)类型集合的轨迹
    std::vector<PlanarRobotVelCmd> cmds_;//PlanarRobotVelCmd(包含速度,轨迹末端的最后旋转角度)类型集合的速度

正因为这些是私有成员,因此采用类内函数去获取:

//获取轨迹
std::vector<PlanarPose>& getTrajectory() {
        return trajectory_;
    }
//获取速度命令
std::vector<PlanarRobotVelCmd>& getCommands() {
        return cmds_;
    }
//设置权重
 void setWeight(double w)  {
        weight_ = w;
    }
//获取权重
double getWeight() const {
        return weight_;
    }

//下面是函数名前加上const修饰符
const std::vector<PlanarPose>& getTrajectory() const {
        return trajectory_;
}

const std::vector<PlanarRobotVelCmd>& getCommands() const {
        return cmds_;
}

构造函数使用了函数重载:

 Particle()
        : weight_(0.0),
          trajectory_(),
          cmds_()
 {
 }
Particle(double w,
             const std::vector<PlanarPose>& trajectory,
             const std::vector<PlanarRobotVelCmd>& cmds)
        : weight_(w),
          trajectory_(trajectory),
          cmds_(cmds)
 {
}

小结:从上面的分析可以看出,这个Particle类实例化的对象粒子particle,具有权重,携带轨迹以及机器人的速度和轨迹末端的最后旋转角度。

6>ParticleSet.h

从头文件名可以看出,这是一个表示粒子集的头文件。从它引用的头文件可以看出,引用了表示粒子的头文件Particle.h;

#include "Particle.h"

以及utils目录下的头文件RNG.h,用于生成随机数;

#include "ExplorationPlanner/utils/RNG.h

以及trajectory目录下的头文件TrajectoryValueHandler.h,其类内成员包括折扣系数discount,轨迹的值,其实现的功能首先是返回粒子的权重因子,折扣系数discount有关的一些操作(获取最小值,最大值,平均值);

#include "ExplorationPlanner/trajectory/TrajectoryValueHandler.h

关于粒子集合的表示,是由ParticleSet类的私有成员particles_表示,这是一个Particle类型的粒子组成的集合,用vector容器表示整个粒子集合;

std::vector<Particle> particles_;//粒子集合

关于粒子集要进行的功能,首先我们需要用到一个下标运算符[]来遍历粒子集合里的粒子元素;

Particle& operator[] ( unsigned int i )
{
  if ( i >= particles_.size() )
         throw std::out_of_range("Requested particle index out of range");
   return particles_[i];
}

然后我们需要用到获取粒子集的粒子数目getNumberOfParticles,这里用到一个size(),因为粒子集合使用std::vector表示,通用使用size()即可返回vector里数量的大小;

unsigned int getNumberOfParticles() const {
        return particles_.size();    }

关于std::vector的size()功能使用,参考:

接下来是粒子权重weightParticles,输入的是TrajectoryValueHandler类实例化对象类型的vector,包括折扣系数discount,轨迹的值,跟粒子的权重因子,折扣系数discount有关的一些操作(获取最小值,最大值,平均值)有关;

void weightParticles(const std::vector<TrajectoryValueHandler>& h);

紧接着是获取最大权重的粒子getMaxWeightParticle();

Particle getMaxWeightParticle() const;

然后是获取粒子集的EffectiveNumber;

double getEffectiveNumberOfParticles() const;

以及获取粒子集的权重总和getSumOfWeights;

 double getSumOfWeights() const;

最后几步是权重归一化normalizeWeights;

void normalizeWeights();

重采样resample:

void resample(RNG& rng);

小结:从上面的分析可以看出粒子集ParticleSet类里实现的主要功能有:遍历粒子集对每个粒子操作,获取粒子数目的功能,计算粒子集的权重,获取粒子权重最大的粒子,获取粒子的EffectiveNumber,计算粒子集的权重总和,对粒子权重进行归一化,重采样。

关于粒子滤波的补课链接参见:

7>SDMPlanner.h

先看包含的头文件,是planner文件夹下的IPlanningAlgorithm.h,根据上面的讲述,我们知道IPlanningAlgorithm类是一个抽象基类,主要的功能是决定是否去规划路径plan以及获取规划的路径getPlan;而本头文件引用IPlanningAlgorithm.h是为了继承这个抽象基类;

class SDMPlanner : virtual public IPlanningAlgorithm

从SDMPlanner类的构造函数可以看出,其输入是horizon,这个参数与轨迹有关,而另一个输入discount,是折扣系数;

  SDMPlanner(unsigned int horizon, double discount)
        : IPlanningAlgorithm(),//继承自IPlanningAlgorithm类
          horizon_(horizon),//horizon与轨迹有关
          discount_(discount)//discount是折扣系数
 {
 }

在类的私有成员里可以看到:

private:
    unsigned int horizon_;//与轨迹有关
    double discount_;//折扣系数

自然地,要对这两个私有成员进行操作,采用以下函数:

//获取horizon
unsigned int getHorizon() const  {
        return horizon_;
    }
//获取discount折扣系数
double getDiscount() const  {
        return discount_;
    }

8>SMCPlannerParameters.h

看引用的头文件,第一个是ICoolingSchedule.h,其函数实现的功能是输入是迭代次数,获取需要复制的数量。

#include "ICoolingSchedule.h

第二个是来自kinematics文件夹下的IPlanarRobotVelCmdSampler.h,主要的功能是采样,具体是:从{上一次的速度,轨迹末端的旋转角度集合}中,再加上{验证是否在边界约束内,获取Next位姿、轨迹、线速度、角速度、轨迹末端的旋转角度}以及随机数输出{新的速度和轨迹末端的旋转角度集合}。

#include "ExplorationPlanner/kinematics/IPlanarRobotVelCmdSampler.h"

第三个是include <memory>表示要使用智能指针std::unique_ptr;

#include <memory>

再回到我们要讨论的本头文件本身,看头文件名SMCPlannerParameters,表示是SMC规划器的参数。

看到这个SMCPlannerParameters类的构造函数:

SMCPlannerParameters(const ICoolingSchedule& schedule,//粒子需要复制的数量
                         unsigned int num_particles,//粒子数量
                         double resampling_threshold_fraction);//重采样阈值

类内的私有成员:

其中比较重要的类型声明:

//声明随机采样生成的新的速度和轨迹末端的旋转角度集合,用智能指针类型的vector表示
typedef std::vector<std::unique_ptr<IPlanarRobotVelCmdSampler> > SamplerPtrVec;

其它:

//采样生成的速度和轨迹末端的旋转集合的智能指针vector集合实例化,kernels_;
SamplerPtrVec kernels_;
std::unique_ptr<ICoolingSchedule> cooling_schedule_;
//粒子数目
unsigned int num_particles_;
//重采样阈值
double resampling_threshold_fraction_;

再看类内函数的实现,下面这个函数是一个克隆函数clone(),专注于深拷贝;

std::unique_ptr<SMCPlannerParameters> clone() const
    {
        std::unique_ptr<SMCPlannerParameters> s( new SMCPlannerParameters(*cooling_schedule_,
                                                                          num_particles_,
                                                                          resampling_threshold_fraction_) );
        for ( SamplerPtrVec::const_iterator it = kernels_.begin(), iend = kernels_.end(); it != iend; ++it)
            s->addKernel( **it );
        return s;
    }

关于clone函数,参考链接:

再看其它函数,获取粒子集的数量getNumberOfParticles(),获取迭代次数getNumberOfIterations(),获取粒子复制的数量getNumberOfReplicates(输入是迭代次数),获取采样生成的速度和轨迹末端的最后旋转角度的集合kernels中单一的kernel: getKernel(),获取重采样阈值getResamplingThreshold(),增加kernel: addKernel()。

小结:从上述的讲解来看,这个SMCPlannerParameter类的主要对象元素是kernel以及kernel集合kernels和粒子集ParticleSet以及它的复制数量,重采样阈值。

8>SMCPlanner.h

这个头文件名字意思是SMCPlanner,直译过来是SMC规划器,是论文提到的表现得与人远程遥控建图一样优秀的规划器,这个头文件也是planner文件夹下其它头文件的集成者。

//SMCPlanner参数相关,主要是kernel和粒子集ParticleSet
#include "SMCPlannerParameters.h"
//粒子集ParticleSet
#include "ParticleSet.h"
//SDMPlanner,主要是horizon和折扣系数discount
#include "SDMPlanner.h"

另外,该头文件还引用了trajectory目录下的其它头文件,主要是轨迹生成和轨迹评分;

//轨迹评分
#include "ExplorationPlanner/trajectory/TrajectoryEvaluator.h"
//轨迹生成
#include "ExplorationPlanner/trajectory/TrajectoryGenerator.h"

至于引用utils目录下的RNG.h,主要是用来生成随机数;

#include "ExplorationPlanner/utils/RNG.h"

最后一个比较重要的是<memory>,是为了使用智能指针std::unique_ptr;

#include <memory>

好了,说完引用的头文件,我们回到这个SMCPlanner类本身。它完全继承了SDMPlanner类,具体看这一段代码:

class SMCPlanner : virtual public SDMPlanner

我们再来看看SMCPlanner的构造函数,传入了非常多的参数,horizon,折扣系数discount,轨迹生成器generator,轨迹评判器evaluator,SMCPlanner参数params,还有粒子集particleset,随机数,迭代次数iteration;

SMCPlanner(unsigned int horizon,
               double discount,
               const boost::shared_ptr<TrajectoryGenerator>& generator,
               const boost::shared_ptr<TrajectoryEvaluator>& evaluator,
               const SMCPlannerParameters& params)
        : IPlanningAlgorithm(),
          SDMPlanner(horizon, discount),
          generator_(generator),
          evaluator_(evaluator),
          parameters_(params.clone()),
          particleset_(),
          rng_(),
          iteration_(0)
    {

    }

注意到SMCPlanner类的私有成员中,有两个是用共享指针std::shared_ptr和智能指针std::unique_ptr表示的;

private:
    boost::shared_ptr<TrajectoryGenerator> generator_;//轨迹生成器
    boost::shared_ptr<TrajectoryEvaluator> evaluator_;//轨迹评分器
    std::unique_ptr<SMCPlannerParameters> parameters_;//SMCPlanner参数

另外在私有成员里还将ParticleSet类和RNG类,分别实例化成particleset_,rng_:

private:
 ParticleSet particleset_;//粒子集
 RNG rng_;//随机数

跟强化学习的奖励reward有关的,用一个TrajectoryValueHandler类型的vector表示最新的reward集合latest_rewards_,这个TrajectoryValueHandler类主要与折扣系数discount,权重因子weight factor有关,并且这个TrajectortValueHandler类里使用了TrajectoryValue类。

关于TrajectoryValue类,其主要元素是reward,observations,其功能主要是插入reward,obervations,以及获得乘过折扣系数后的reward总和;

std::vector<TrajectoryValueHandler> latest_rewards_;

再回到SMCPlanner类本身,现在我们来关注它实现的功能。

首先是判断是否迭代,输入是由来自planner文件夹本地的ExplorationTaskState.h头文件实现的ExplorationTaskState类实例化的对象类型决定。而ExplorationTaskState类主要包含机器人的位姿robot_pose,占据栅格地图map,主要功能是设置机器人位姿,获取机器人位姿,获取占据栅格地图,根据传入的PlanarObservation类型的observations更新地图。

bool iterate(const ExplorationTaskState& state);//判断是否迭代

接下来是判断是否去规划,输入依旧是ExplorationTaskState类实例化的对象。

bool plan(const ExplorationTaskState& state);//判断是否规划

然后如果可以规划的话,我们获取规划的路径,它是来自于一个PlanarPose类型的vector表示的位姿集合,注意这里的PlanarPose类主要包含x,y,theta。

 void getPlan(std::vector<PlanarPose> &plan) const //获取规划路径

接着是设置SMCPlanner的相关参数,传入是SMCPlannerParameters类实例化的对象,并使用克隆函数clone()来进行赋值。而SMCPlannerParameters类包含的主要参数是是新的速度及轨迹末端的最后旋转角度集合kernel以及kernel集合kernels,粒子集ParticleSet以及它的复制数量,重采样阈值。

SMCPlannerParameters类主要的功能是: 获取粒子集的数量,获取迭代次数,获取粒子复制的数量(输入是迭代次数),获取采样生成的速度和轨迹末端的最后旋转角度的集合kernels中单一的kernel,获取重采样阈值,增加kernel。

void setParameters(const SMCPlannerParameters& p)
{
    parameters_ = p.clone();//使用克隆函数clone()进行参数的拷贝
}

接下来是获取粒子集ParticleSet,注意这里的ParticleSet类包含的成员单个粒子携带权重,轨迹,速度;而粒子集ParticleSet类具备的功能有:遍历粒子集对每个粒子操作,获取粒子数目的功能,计算粒子集的权重,获取粒子权重最大的粒子,获取粒子的EffectiveNumber,计算粒子集的权重总和,对粒子权重进行归一化,重采样。

const ParticleSet& getParticleSet() const
 {
     return particleset_;
 }

紧接着是获取最新的reward,返回类型是TrajectoryValueHandler类类型集合的vector,而TrajectoryValueHandler类主要包括折扣系数discount和TrajectoryValue类型的value,主要功能是对折扣系数的相关处理,获取折扣系数的最小值,最大值以及平均值。

至于TrajectoryValue类包含的主要元素value是奖励reward和观察observation,主要功能是获取乘过折扣系数后的奖励reward的总和,在奖励集合里插入新的reward或者同时插入新的reward和PlanarObservation类型的observation。

std::vector<TrajectoryValueHandler> getLatestRewardHandlers() const {
        return latest_rewards_;//返回最新的奖励
  }

最后再回到SMCPlanner的私有成员里:

private:

// Private attributes

boost::shared_ptr<TrajectoryGenerator> generator_;//轨迹生成器
boost::shared_ptr<TrajectoryEvaluator> evaluator_;//轨迹评价器
std::unique_ptr<SMCPlannerParameters> parameters_;//SMCPlanner的参数设置

ParticleSet particleset_;//粒子集
RNG rng_;//随机数

unsigned int iteration_;//迭代次数

void initializeParticles();//初始化粒子集
bool updateParticles();//更新粒子集

/*
ExplorationTaskState类实例化对象state 主要包含机器人的位姿robot_pose,
占据栅格地图map,主要功能是设置机器人位姿,获取机器人位姿,获取占据栅格地图,
根据传入的PlanarObservation类型的observations更新地图。
*/
void evaluateParticles(const ExplorationTaskState& state);//评价粒子集

/*
TrajectoryValueHandler类类型集合的vector,而TrajectoryValueHandler类
主要包括折扣系数discount和TrajectoryValue类型的value,
主要功能是对折扣系数的相关处理,获取折扣系数的最小值,最大值以及平均值。
至于TrajectoryValue类包含的主要元素value是奖励reward和观察observation,
主要功能是获取乘过折扣系数后的奖励reward的总和在奖励集合里插入新的reward或者
同时插入新的reward和PlanarObservation类型的observation。
*/
std::vector<TrajectoryValueHandler> latest_rewards_;//最新的reward集合

总结:

SMCPlanner类主要实现的功能是判断是否迭代,判断是否规划,获取规划的路径,设置参数,获取粒子集,获取最新的奖励;主要的私有成员有轨迹生成器,轨迹评分器,SMCPlanner参数,粒子集,随机数,最新的奖励;私有成员函数有初始化粒子集,更新粒子集,评价粒子集。

简洁地描述,这里的每一个粒子代表着一条轨迹,一个粒子集表示若干轨迹的集合,我们可以通过轨迹生成器产生可以携带轨迹的粒子,然后用轨迹评分器对生成的轨迹进行打分,打分结果就是奖励reward表示。

关于粒子,这涉及到粒子的初始化,粒子迭代,每一次迭代遍历粒子集,粒子重采样以及评价粒子集。这部分就是粒子滤波(Particle Filter)部分的哲学了。

而作为一个规划器,SMCPlanner的作用是产生运动,这就需要随机采样生成的速度以及轨迹末端最后的旋转角度集合,需要传感器模拟产生的观察量observations,然后是打分机制,用reward表示。这就是强化学习(Reinforcement Learning)部分的哲学了。

Okay,以上就是planner文件夹下的所有头文件了。

¡Hasta mañana !

下一讲见:

Churlaaaaaaa:9.gmapping建图拓展篇:ase_exploration包| 源码梳理(五)