欢迎您访问程序员文章站本站旨在为大家提供分享程序员计算机编程知识！
您现在的位置是： 首页

Ros navigation功能包集源码详解(一) amcl

程序员文章站 2022-05-28 23:25:28
...

Navigation stack总体结构

Ros navigation功能包集源码详解(一) amcl
  本文主要解读amcl包对蒙特卡洛定位算法的具体实现过程,主要关注源码中定义的数据类型,及程序的具体实现流程,对于详细的算法原理不作过多解释(主要是我不太懂,讲不好),想了解涉及到的理论知识推荐阅读<< Probabilistic Robotics >>

1.定位包amcl的 功能

  输入:地图,激光数据,里程计
  输出:位姿pose(x,y,θ),粒子集

2.源码文件结构

Ros navigation功能包集源码详解(一) amcl
最为核心的是include目录和src目录,一共包含三个大类,map,pf,以及sensors,还有一个重要文件amcl_node.cpp
 1). map处理地图，pf是算法的核心粒子滤波(particle filter),sensors就是处理雷达跟里程计传感器的
 2). amcl_node.cpp，定义了一个可运行节点,定位过程主要在此文件中组织实现

3.主要数据类型与算法

3.1 pf

Ros navigation功能包集源码详解(一) amcl”
a).eig3(指头文件和源文件,下同)实现的是一个3x3对称矩阵的特征值与特征向量的计算,首先用Householder矩阵将矩阵变换为三对角矩阵,然后使用ql分解迭代计算
b).pf_kdtree定义了一个kdtree以及维护方法来管理所有粒子
c).pf_pdf主要定义了一个从给定pdf中采样粒子的方法
d).pf_vector定义了三维列向量和三维矩阵和基本的运算方法
e).pf定义了粒子单元pf_sample_t,粒子集pf_sample_set_t,粒子滤波pf_t的数据类型,还有一个 pf_cluster_t表示粒子集的聚类信息,关键函数主要包含如下三个,分别对应粒子滤波中的运动更新,观测更新,重采样三个过程 

// Update the filter with some new action
void pf_update_action(pf_t *pf, pf_action_model_fn_t action_fn, void *action_data);

// Update the filter with some new sensor observation
void pf_update_sensor(pf_t *pf, pf_sensor_model_fn_t sensor_fn, void *sensor_data);

// Resample the distribution
void pf_update_resample(pf_t *pf);

3.2 sensors

Ros navigation功能包集源码详解(一) amcl

a) amcl_sencor定义了基类,其成员函数都为虚函数,主要有如下两个,为两个派生类amcl_laser,amcl_odom提供统一接口 

// Update the filter based on the action model.  Returns true if the filter
// has been updated.
public: virtual bool UpdateAction(pf_t *pf, AMCLSensorData *data);

// Update the filter based on the sensor model.  Returns true if the
// filter has been updated.
public: virtual bool UpdateSensor(pf_t *pf, AMCLSensorData *data);

b) amcl_laser定义了激光数据类型,三种观测更新模型(详细见<<概率机器人>>),具体实现了UpdateSensor,用于计算粒子权值 

//amcl_laser.h   
// Laser sensor data   
class AMCLLaserData : public AMCLSensorData   
{
public:
AMCLLaserData () {ranges=NULL;};
virtual ~AMCLLaserData() {delete [] ranges;};
// Laser range data (range, bearing tuples)
public: int range_count;
public: double range_max;
public: double (*ranges)[2];
};
...
//amcl_laser.cpp
// Apply the laser sensor model
bool AMCLLaser::UpdateSensor(pf_t *pf, AMCLSensorData *data)
{
if (this->max_beams < 2)
return false;

// Apply the laser sensor model
if(this->model_type == LASER_MODEL_BEAM)
pf_update_sensor(pf, (pf_sensor_model_fn_t) BeamModel, data);
else if(this->model_type == LASER_MODEL_LIKELIHOOD_FIELD)
pf_update_sensor(pf, (pf_sensor_model_fn_t) LikelihoodFieldModel, data);  
else if(this->model_type == LASER_MODEL_LIKELIHOOD_FIELD_PROB)
pf_update_sensor(pf, (pf_sensor_model_fn_t) LikelihoodFieldModelProb, data);  
else
pf_update_sensor(pf, (pf_sensor_model_fn_t) BeamModel, data);

return true;
}
...

c) amcl_odom具体实现了基类定义的UpdateAction函数,用于根据运动更新粒子,定义了两种运动模型,差分和全向

public: void SetModelDiff(double alpha1, 
                        double alpha2, 
                        double alpha3, 
                        double alpha4);

public: void SetModelOmni(double alpha1, 
                        double alpha2, 
                        double alpha3, 
                        double alpha4,
                        double alpha5);

public: void SetModel( odom_model_t type,
                     double alpha1,
                     double alpha2,
                     double alpha3,
                     double alpha4,
                     double alpha5 = 0 );

// Update the filter based on the action model.  Returns true if the filter
// has been updated.
public: virtual bool UpdateAction(pf_t *pf, AMCLSensorData *data);

3.3 map

Ros navigation功能包集源码详解(一) amcl
a) map中主要定义了概率栅格地图的数据表示,如下所示

// Description for a single map cell.
typedef struct
{  
int occ_state;// Occupancy state (-1 = free, 0 = unknown, +1 = occ) 
double occ_dist;// Distance to the nearest occupied cell
} map_cell_t;

// Description for a map
typedef struct
{                                    
double origin_x, origin_y; // Map origin; the map is a viewport onto a conceptual larger map.
double scale;      // Map scale (m/cell) 
int size_x, size_y;  // Map dimensions (number of cells)  
map_cell_t *cells;  // The map data, stored as a grid
double max_occ_dist;  // Max distance at which we care about obstacles, 
                                      //for constructing likelihood field 
} map_t;

b) 此外还有四个源文件map_cspace.cpp,map_draw.c,map_range.c,map_store.c 不是主要内容(还没仔细读),暂不深究

4.amcl_node.cpp解读

4.1 类AmclNode的主要内容

 public:除构造函数和析构函数外,只有三个函数 

void runFromBag(const std::string &in_bag_fn);//根据信息记录包来运行amcl,                                                                              //实际此函 数是把信息记录不断的发布出去
int process();  //这个好像只声明了但没定义          
void savePoseToServer();     //把位姿信息保存到参数服务器

 private:定义了很多变量,tf ,激光,里程,粒子滤波参数和变量等等,订阅者和发布者以及一些回调函数

4.2 main函数

int main(int argc, char** argv)
{
 ros::init(argc, argv, "amcl");
 ros::NodeHandle nh;                                    
 //监听到程序是否接收到退出命令,或者中断命令等处理
 // Override default sigint handler
 signal(SIGINT, sigintHandler);                                   
 // Make our node available to sigintHandler
 amcl_node_ptr.reset(new AmclNode());//使boost::shared_ptr指向一个新的对象

 if (argc == 1)
 {
   // run using ROS input
   ros::spin();
 }
 else if ((argc == 3) && (std::string(argv[1]) == "--run-from-bag"))
 {
   amcl_node_ptr->runFromBag(argv[2]);
 }                                    
 // Without this, our boost locks are not shut down nicely
 amcl_node_ptr.reset();//销毁boost::shared_ptr指向的对象                               
 // To quote Morgan, Hooray!
 return(0);
}

  关键一句amcl_node_ptr.reset(new AmclNode());主要实现在于构造函数AmclNode()

4.3 构造函数AmclNode()

 主要包含如下内容:

//设置粒子滤波参数,运动模型参数,观测模型参数等各种
//消息发布:
1.pose_pub_ = nh_.advertise<geometry_msgs::PoseWithCovarianceStamped>("amcl_pose", 2, true);
        (位姿,均值方差表示的)
2.particlecloud_pub_ = nh_.advertise<geometry_msgs::PoseArray>("particlecloud", 2, true);
        (所有的粒子)
//服务: 
1.global_localization,(globalLocalizationCallback)
       这里的globalLocalization就是在地图范围内产生均匀分布的粒子
2.request_nomotion_update,(nomotionUpdateCallback)
        就是设置个标志,使amcl在无运动时也能够更新粒子
3.set_map,(setMapCallback)调用如下两个函数
    handleMapMessage(req.map);//进行地图转换 ，记录free space ，
    以及初始化pf_t 结构体,实例化运动模型(odom)和观测模型(laser)
    handleInitialPoseMessage(req.initial_pose);
    根据接收的初始位姿消息,在该位姿附近高斯采样重新生成粒子集
//消息订阅:
1.laser_scan_filter_,->registerCallback(boost::bind(&AmclNode::laserReceived,this, _1));
       回调函数laserReceived是粒子滤波主要过程,根据激光扫描数据更新粒子 
2.initial_pose_sub_ = nh_.subscribe("initialpose", 2, &AmclNode::initialPoseReceived, this);
        回调函数initialPoseReceived中实际调用handleInitialPoseMessage,在接收到的初始位姿附近采样生成    粒子集
//动态调参
    dsrv_ = new dynamic_reconfigure::Server<amcl::AMCLConfig>(ros::NodeHandle("~"));
    ...CallbackType cb = boost::bind(&AmclNode::reconfigureCB, this, _1, _2);
    dsrv_->setCallback(cb);
    动态配置参数服务,重新设置粒子滤波参数,设置运动模型参数,设置观测模型参数,订阅消息等,
    就是AmclNode构造函数里做的事情

  以上的最关键内容在回调函数laserReceived()

4.4 回调函数laserReceived()

  • a： 获取laser对应于baselink的坐标
  • b：获取baselink对应于odom的坐标
  • c：根据里程计的变化值+高斯噪音 更新 pf_t中samples的内里程计值（运动模型）
    odom->updateAction()
  • d：根据当前雷达数据更新各里程计对应的权值weights
    laser_[laser_index]->updateSensor（）
  • e：得到滤波结果后,分别在话题/amcl_pose和/ particlecloud上发布位姿和粒子集

4.5 amcl_node的重要内容大体如上,总结一下它的几个主要过程

  • a： 构造时初始化,从参数服务器中获取数据初始化各类参数;(接收地图设置,gui显示发布频率,保存位姿到参数服务器频率,laser测距范围及其概率模型参数,odom概率模型参数,粒子滤波及kld重采样参数,从参数服务器获取初始位姿,然后初始化了订阅者,发布者,服务)
  • b：地图加载,两种方式(1.订阅/map话题2.请求服务得到地图),得到地图后也有个初始化过程(将消息类型的地图转换为定义的map类数据,统计free状态的栅格索引,从参数服务器获取位姿信息,并初始化粒子滤波器pf_,初始化odom模型参数,初始化laser模型参数)
  • c：粒子滤波,订阅laser_scan的回调函数中处理,得到结果后发布位姿和粒子集
  • d：initialpose的回调,接收到初始位姿消息后,融入最新的里程改变,然后在该位姿附近重新生成粒子集
    剩下的是提供的服务及动态参数配置,提供给外部控制接口,如第三节所述

参考资料:

* 注:欢迎指正,如内容有侵犯您的权益,请联系作者.

相关标签： ros Navigation

上一篇： Parse error: syntax error, unexpected '[' in C:AppServwwwgenerate_order.ph

下一篇： Dijkstra算法原理

推荐阅读