ROS Navigation之amcl源码解析(完全详解)

分类: ROS Navigation源码完全详解

Posted by 白夜行的狼 on May 6, 2018

0. 写在最前面

本文持续更新地址:https://haoqchen.site/2018/05/06/amcl-code/

这篇文章记录下自己在阅读amcl源码过程中的一些理解,如有不妥,欢迎评论或私信。

本文中所有代码因为篇幅等问题,都只给出主要部分,详细的自己下载下来对照着看。

如果觉得写得还不错,就请收藏一下啦~~~也可以找一下我写的其他包的源码解读来看一下。关注一下我的专栏什么的。

你的Star是作者坚持下去的最大动力哦~~~

1. amcl是干什么的

  Amcl is a probabilistic localization system for a robot moving in 2D. It implements the adaptive (or KLD-sampling) Monte Carlo localization approach (as described by Dieter Fox), which uses a particle filter to track the pose of a robot against a known map.

以上是官网的介绍,说白了就是2D的概率定位系统,输入激光雷达数据、里程计数据,输出机器人在地图中的位姿。用的是自适应蒙特卡洛定位方法,这个方法是在已知地图中使用粒子滤波方法得到位姿的。

  如下图所示,如果里程计没有误差,完美的情况下,我们可以直接使用里程计信息(上半图)推算出机器人(base_frame)相对里程计坐标系的位置。但现实情况,里程计存在漂移以及无法忽略的累计误差,所以AMCL采用下半图的方法,即先根据里程计信息初步定位base_frame,然后通过测量模型得到base_frame相对于map_frame(全局地图坐标系),也就知道了机器人在地图中的位姿。(注意,这里虽然估计的是base到map的转换,但最后发布的是map到odom的转换,可以理解为里程计的漂移。) frame_relation

2. 总体情况

2.1 CMakeLists

研究一个ROS包,肯定要先看CMakeLists。我们可以看到,这个包会生成

三个库:

  • amcl_pf
  • amcl_map
  • amcl_sensors 一个节点:
  • amcl

2.2 其中amcl的订阅与发布:

发布话题:

  1. amcl_pose: geometry_msgs::PoseWithCovarianceStamped,后验位姿+一个6*6的协方差矩阵(xyz+三个转角)
  2. particlecloud:geometry_msgs::PoseArray,粒子位姿的数组
  3. 一个15秒的定时器:AmclNode::checkLaserReceived,检查上一次收到激光雷达数据至今是否超过15秒,如超过则报错。

发布服务:

  1. global_localization:&AmclNode::globalLocalizationCallback,这里是没有给定初始位姿的情况下在全局范围内初始化粒子位姿,该Callback调用pf_init_model,然后调用AmclNode::uniformPoseGenerator在地图的free点随机生成pf->max_samples个粒子
  2. request_nomotion_update:&AmclNode::nomotionUpdateCallback没运动模型更新的情况下也暂时更新粒子群
  3. set_map:&AmclNode::setMapCallback
  4. dynamic_reconfigure::Server动态参数配置器。

订阅话题:

  1. scan_topic_:sensor_msgs::LaserScan,AmclNode::laserReceived,这里是tf订阅,转换到odom_frame_id_
  2. initialpose:AmclNode::initialPoseReceived,这个应该就是订阅rviz中给的初始化位姿,调用AmclNode::handleInitialPoseMessage,只接受global_frame_id_(一般为map)的坐标,并重新生成粒子
  3. map:AmclNode::mapReceived这个在use_map_topic_的时候才订阅,否则requestMap();我这里也没有订阅,因为只使用了一个固定的地图。

3. amcl_node.cpp

这个文件实现了上述的amcl节点功能。

3.1 main

1
2
3
4
5
6
7
8
9
10
11
12
13
14
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());
  if (argc == 1)
  {
    // run using ROS input
    ros::spin();
  }
 }

没啥好说的,主要就是定义了amcl节点,初始化了一个AmclNode的类对象,最关键的中断函数配置都在该类的构造函数中实现。

3.2 AmclNode

1
2
3
4
5
6
7
8
9
10
11
12
13
14
AmclNode::AmclNode()
{
  boost::recursive_mutex::scoped_lock l(configuration_mutex_);
//测量模型选择,具体参考《概率机器人》第6章内容,这里默认的likelihood_field是6.4节的似然域模型
  std::string tmp_model_type;
  private_nh_.param("laser_model_type", tmp_model_type, std::string("likelihood_field"));
  
//机器人模型选择,具体参考《概率机器人》5.4节里程计运动模型内容,这里默认的diff应该是差分型(两轮驱动)的机器人?,另外一个是全向机器人?
  private_nh_.param("odom_model_type", tmp_model_type, std::string("diff"));
//从参数服务器中获取初始位姿及初始分布
  updatePoseFromServer();
//定义话题及订阅等,具体在本文第2章有讲
  pose_pub_ = nh_.advertise<geometry_msgs::PoseWithCovarianceStamped>("amcl_pose", 2, true);
}

3.3 requestMap

这里请求服务static_server提供map,然后调用handleMapMessage处理地图信息。这里的地图类型是nav_msgs::OccupancyGrid,这里不介绍,有兴趣自己去看数据类型。然后获取初始位姿、初始化粒子、里程计信息等。

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
AmclNode::requestMap()
{
//一直请求服务static_map直到成功,该服务在map_server这个包的map_server节点中进行定义
  while(!ros::service::call("static_map", req, resp))
  {
    ROS_WARN("Request for map failed; trying again...");
    ros::Duration d(0.5);
    d.sleep();
  }
  handleMapMessage( resp.map );
}

AmclNode::handleMapMessage(const nav_msgs::OccupancyGrid& msg)
{
//free相应的指针
  freeMapDependentMemory();
//转换成标准地图,0->-1(不是障碍);100->+1(障碍);else->0(不明)
  map_ = convertMap(msg);

//将不是障碍的点的坐标保存下来
#if NEW_UNIFORM_SAMPLING
  // Index of free space
  free_space_indices.resize(0);
  for(int i = 0; i < map_->size_x; i++)
    for(int j = 0; j < map_->size_y; j++)
      if(map_->cells[MAP_INDEX(map_,i,j)].occ_state == -1)
        free_space_indices.push_back(std::make_pair(i,j));
#endif
  // Create the particle filter,定义了一个回调,尚未清除干啥
  pf_ = pf_alloc(min_particles_, max_particles_,
                 alpha_slow_, alpha_fast_,
                 (pf_init_model_fn_t)AmclNode::uniformPoseGenerator,
                 (void *)map_);
  // 从参数服务器获取初始位姿及方差放到pf中
  updatePoseFromServer();

  // 定义里程计与激光雷达并初始化数据
  // Odometry
  delete odom_;
  odom_ = new AMCLOdom();
  ROS_ASSERT(odom_);
  odom_->SetModel( odom_model_type_, alpha1_, alpha2_, alpha3_, alpha4_, alpha5_ );
  // Laser
  delete laser_;
  laser_ = new AMCLLaser(max_beams_, map_);

  // In case the initial pose message arrived before the first map,
  // try to apply the initial pose now that the map has arrived.
  applyInitialPose();
}

3.4 laserReceived

其中变量pose是base相对于odom的位姿;pf_odom_pose_则是上一时刻base相对于odom的位姿,用于后续得到机器人的相对运动程度。

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
AmclNode::laserReceived(const sensor_msgs::LaserScanConstPtr& laser_scan)
{ 
  // Do we have the base->base_laser Tx yet?感觉这里是支持多个激光雷达的,找到当前响应的激光雷达之前存储的信息,
//如相对于base的转换,是否更新等,使用map结构直接通过id来找到对应序号即可
//如果之前没有备案则在map结构中备案,然后存到frame_to_laser_及lasers_中下次备用
  if(frame_to_laser_.find(laser_scan->header.frame_id) == frame_to_laser_.end())
  {
    frame_to_laser_[laser_scan->header.frame_id] = laser_index;
  } else {
    // we have the laser pose, retrieve laser index
    laser_index = frame_to_laser_[laser_scan->header.frame_id];
  }

  // Where was the robot when this scan was taken?获得base在激光雷达扫描时候相对于odom的相对位姿
  pf_vector_t pose;
  if(!getOdomPose(latest_odom_pose_, pose.v[0], pose.v[1], pose.v[2],
                  laser_scan->header.stamp, base_frame_id_))
  {
    ROS_ERROR("Couldn't determine robot's pose associated with laser scan");
    return;
  }

  pf_vector_t delta = pf_vector_zero();
//如果不是第一帧,看运动幅度是否超过设定值需要更新(第一帧是指更新了地图或者更新初始位姿)
  if(pf_init_)
  {
    // Compute change in pose
    //delta = pf_vector_coord_sub(pose, pf_odom_pose_);
    delta.v[0] = pose.v[0] - pf_odom_pose_.v[0];
    delta.v[1] = pose.v[1] - pf_odom_pose_.v[1];
    delta.v[2] = angle_diff(pose.v[2], pf_odom_pose_.v[2]);

    // See if we should update the filter
    bool update = fabs(delta.v[0]) > d_thresh_ ||
                  fabs(delta.v[1]) > d_thresh_ ||
                  fabs(delta.v[2]) > a_thresh_;
    update = update || m_force_update;
    m_force_update=false;

    // Set the laser update flags
    if(update)
      for(unsigned int i=0; i < lasers_update_.size(); i++)
        lasers_update_[i] = true;
  }
//第一帧则初始化一些值
  bool force_publication = false;
  if(!pf_init_)
  {
    // Pose at last filter update
    pf_odom_pose_ = pose;
    // Filter is now initialized
    pf_init_ = true;
  }
  // If the robot has moved, update the filter
  else if(pf_init_ && lasers_update_[laser_index])//如果已经初始化并需要更新则更新运动模型
  {
//这是amcl_odom.cpp中最重要的一个函数,实现了用运动模型来更新现有的每一个粒子的位姿(这里得到的只是当前时刻的先验位姿)
    // Use the action data to update the filter
    odom_->UpdateAction(pf_, (AMCLSensorData*)&odata);
  }

  bool resampled = false;
  // If the robot has moved, update the filter
  if(lasers_update_[laser_index])
  {//接下来一大片都是对原始激光雷达数据进行处理,转换到AMCLLaserData。包括角度最小值、增量到base_frame的转换、测距距离最大值、最小值。
    //具体看代码,篇幅限制不详细列了
    try
    {
      tf_->transformQuaternion(base_frame_id_, min_q, min_q);
      tf_->transformQuaternion(base_frame_id_, inc_q, inc_q);
    }
  
    ldata.range_max = std::min(laser_scan->range_max, (float)laser_max_range_);
    range_min = std::max(laser_scan->range_min, (float)laser_min_range_);
        
    ldata.ranges = new double[ldata.range_count][2];
    for(int i=0;i<ldata.range_count;i++)
    {
      // amcl doesn't (yet) have a concept of min range.  So we'll map short
      // readings to max range.//激光雷达传上来的数据只标记了最大值最小值,但是没做处理,直接将原始数据传上来,
      if(laser_scan->ranges[i] <= range_min)//这里将最小值当最大值处理,因为在类似likelihood_field模型中,会直接将最大值丢弃
        ldata.ranges[i][0] = ldata.range_max;
    }
//注意这里是amcl_laser.cpp的UpdateSensor,不是amcl_sensor.cpp的。通过判断前面设置的测量模型调用pf_update_sensor,
//该函数需要传入相应模型的处理函数,这里所有的测量模型在《概率机器人》的第六章有详细讲,具体自己看,后面只针对自己使用的likelihood_field模型讲一下
//pf_update_sensor实现对所有粒子更新权重,并归一化、计算出《概率机器人》8.3.5的失效恢复的长期似然和短期似然
    lasers_[laser_index]->UpdateSensor(pf_, (AMCLSensorData*)&ldata);

    lasers_update_[laser_index] = false;
    pf_odom_pose_ = pose;
//多少次激光雷达回调之后进行重采样呗,我这里resample_interval_=0.5,只有一个激光雷达,每次都更新。
    // Resample the particles
    if(!(++resample_count_ % resample_interval_))
    {
//按照一定的规则重采样粒子,包括前面说的失效恢复、粒子权重等,然后放入到kdtree,暂时先理解成关于位姿的二叉树,
//然后进行聚类,得到均值和方差等信息,个人理解就是将相近的一堆粒子融合成一个粒子了,没必要维持太多相近的@TODO
      pf_update_resample(pf_);
      resampled = true;
    }

    // Publish the resulting cloud
    // TODO: set maximum rate for publishing
    if (!m_force_update) {
      //将新粒子发布到全局坐标系下,一般是map
      particlecloud_pub_.publish(cloud_msg);
    }
  }

  if(resampled || force_publication)
  {
    //遍历所有粒子簇,找出权重均值最大的簇,其平均位姿就是我们要求的机器人后验位姿,到此一次循环已经所有完成
    for(int hyp_count = 0;
        hyp_count < pf_->sets[pf_->current_set].cluster_count; hyp_count++)
    {
      if (!pf_get_cluster_stats(pf_, hyp_count, &weight, &pose_mean, &pose_cov))
      {
        break;
      }
      hyps[hyp_count].weight = weight;
      hyps[hyp_count].pf_pose_mean = pose_mean;
      hyps[hyp_count].pf_pose_cov = pose_cov;

      if(hyps[hyp_count].weight > max_weight)
      {
        max_weight = hyps[hyp_count].weight;
        max_weight_hyp = hyp_count;
      }
    }

    //将位姿、粒子集、协方差矩阵等进行更新、发布
    if(max_weight > 0.0)
    {
      geometry_msgs::PoseWithCovarianceStamped p;
      // Fill in the header
      p.header.frame_id = global_frame_id_;
      p.header.stamp = laser_scan->header.stamp;
      // Copy in the pose
      p.pose.pose.position.x = hyps[max_weight_hyp].pf_pose_mean.v[0];
      p.pose.pose.position.y = hyps[max_weight_hyp].pf_pose_mean.v[1];
      tf::quaternionTFToMsg(tf::createQuaternionFromYaw(hyps[max_weight_hyp].pf_pose_mean.v[2]),
                            p.pose.pose.orientation);
      // Copy in the covariance, converting from 3-D to 6-D
      pf_sample_set_t* set = pf_->sets + pf_->current_set;
      for(int i=0; i<2; i++)
      {
        for(int j=0; j<2; j++)
        {
          p.pose.covariance[6*i+j] = set->cov.m[i][j];
        }
      }
      p.pose.covariance[6*5+5] = set->cov.m[2][2];

      pose_pub_.publish(p);
      last_published_pose = p;
//这里就是所说的,map~base减去odom~base得到map~odom,最后发布的是map~odom。
  // subtracting base to odom from map to base and send map to odom instead
      tf::Stamped<tf::Pose> odom_to_map;
      try
      {
        tf::Transform tmp_tf(tf::createQuaternionFromYaw(hyps[max_weight_hyp].pf_pose_mean.v[2]),
                             tf::Vector3(hyps[max_weight_hyp].pf_pose_mean.v[0],
                                         hyps[max_weight_hyp].pf_pose_mean.v[1],
                                         0.0));
        tf::Stamped<tf::Pose> tmp_tf_stamped (tmp_tf.inverse(),
                                              laser_scan->header.stamp,
                                              base_frame_id_);
        this->tf_->transformPose(odom_frame_id_,
                                 tmp_tf_stamped,
                                 odom_to_map);
      }
      catch(tf::TransformException)
      {
        ROS_DEBUG("Failed to subtract base to odom transform");
        return;
      }

      latest_tf_ = tf::Transform(tf::Quaternion(odom_to_map.getRotation()),
                                 tf::Point(odom_to_map.getOrigin()));
      latest_tf_valid_ = true;

      if (tf_broadcast_ == true)
      {
        // We want to send a transform that is good up until a
        // tolerance time so that odom can be used
        ros::Time transform_expiration = (laser_scan->header.stamp +
                                          transform_tolerance_);
        tf::StampedTransform tmp_tf_stamped(latest_tf_.inverse(),
                                            transform_expiration,
                                            global_frame_id_, odom_frame_id_);
        this->tfb_->sendTransform(tmp_tf_stamped);
        sent_first_transform_ = true;
      }
    }
    else
    {
      ROS_ERROR("No pose!");
    }
  }
  else if(latest_tf_valid_)//if(resampled || force_publication)
  {

  }
}

3.5 AMCLOdom::UpdateAction

  首先获取上一帧的位姿,然后根据自己前面选择的机器人模型进行运动模型更新。这里由于我自己的模型是ODOM_MODEL_DIFF,所以只针对这个模型进行讲解,剩下的原理应该一样,细节上有区别,自己看。代码中有讲到,利用到的算法是《Probabilistic Robotics 2》136页的内容,如果是Probabilistic Robotics 1或者中文版其实就是5.4节中的采样算法,这里不再细述。

  我直白一点的理解就是,已知上一时刻所有粒子的后验位姿、上一时刻到现在两个轮子的编码器值,给所有粒子随机采样当前时刻的先验位姿。为什么是随机,不是确定的直接从编码器计算得到呢?因为编码器等存在误差,所以采样到的每个粒子的先验位姿都是不确定的,但理论上是服从一个分布的(这里是正态,书本上给出三种分布),跟实际运动有关,如果运动越大、则采样到的跟直接里程计计算得到的值就越有可能相差的比较大(算法上表示为方差越大)。

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
// Apply the action model
bool AMCLOdom::UpdateAction(pf_t *pf, AMCLSensorData *data)
{
  set = pf->sets + pf->current_set;
  pf_vector_t old_pose = pf_vector_sub(ndata->pose, ndata->delta);

  switch( this->model_type )
  {
   case ODOM_MODEL_DIFF:
  {
    // Implement sample_motion_odometry (Prob Rob p 136)
    double delta_rot1, delta_trans, delta_rot2;
    double delta_rot1_hat, delta_trans_hat, delta_rot2_hat;
    double delta_rot1_noise, delta_rot2_noise;
//计算里程计得到的ut,即用旋转、直线运动和另一个旋转来表示相对运动
    // Avoid computing a bearing from two poses that are extremely near each
    // other (happens on in-place rotation).
    if(sqrt(ndata->delta.v[1]*ndata->delta.v[1] + //如果原地旋转的情况,定义第一个旋转为0,认为所有旋转都是第二个旋转做的
            ndata->delta.v[0]*ndata->delta.v[0]) < 0.01)
      delta_rot1 = 0.0;
    else
      delta_rot1 = angle_diff(atan2(ndata->delta.v[1], ndata->delta.v[0]),
                              old_pose.v[2]);
    delta_trans = sqrt(ndata->delta.v[0]*ndata->delta.v[0] +
                       ndata->delta.v[1]*ndata->delta.v[1]);
    delta_rot2 = angle_diff(ndata->delta.v[2], delta_rot1);
//这里作者比书本多考虑了一种情况,就是机器人旋转小角度,然后往后走的情况,比如旋转-20°,然后往后走,计算出来可能是160°,
//但实际只转了20°。按书本可能是比较大的方差(160°),不太准确,但这里的话还是按照比较小的方差来采样。
//但我表示怀疑,万一真的转了160°再前向运动的话怎么办?还是用比较小的方差来采样,可能会使得采样的准确率下降。
//实际中,编码器采样率很高,且设定了一个很小的变化角度就进行运动模型更新,所以我说的这种情况几乎不会发生
    // We want to treat backward and forward motion symmetrically for the
    // noise model to be applied below.  The standard model seems to assume
    // forward motion.
    delta_rot1_noise = std::min(fabs(angle_diff(delta_rot1,0.0)),
                                fabs(angle_diff(delta_rot1,M_PI)));
    delta_rot2_noise = std::min(fabs(angle_diff(delta_rot2,0.0)),
                                fabs(angle_diff(delta_rot2,M_PI)));
//对每个粒子用书本的方法进行更新
    for (int i = 0; i < set->sample_count; i++)
    {
      pf_sample_t* sample = set->samples + i;

      // Sample pose differences
      delta_rot1_hat = angle_diff(delta_rot1,
                                  pf_ran_gaussian(this->alpha1*delta_rot1_noise*delta_rot1_noise +
                                                  this->alpha2*delta_trans*delta_trans));
      delta_trans_hat = delta_trans - 
              pf_ran_gaussian(this->alpha3*delta_trans*delta_trans +
                              this->alpha4*delta_rot1_noise*delta_rot1_noise +
                              this->alpha4*delta_rot2_noise*delta_rot2_noise);
      delta_rot2_hat = angle_diff(delta_rot2,
                                  pf_ran_gaussian(this->alpha1*delta_rot2_noise*delta_rot2_noise +
                                                  this->alpha2*delta_trans*delta_trans));

      // Apply sampled update to particle pose
      sample->pose.v[0] += delta_trans_hat * 
              cos(sample->pose.v[2] + delta_rot1_hat);
      sample->pose.v[1] += delta_trans_hat * 
              sin(sample->pose.v[2] + delta_rot1_hat);
      sample->pose.v[2] += delta_rot1_hat + delta_rot2_hat;
    }
  }
  }
  return true;
}

3.6 LikelihoodFieldModel

算法原理请参看《概率机器人》6.4节,这里会给每个粒子(代表一个位姿)计算概率、更新概率,然后返回所有粒子概率的总和。

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
double AMCLLaser::LikelihoodFieldModel(AMCLLaserData *data, pf_sample_set_t* set)
{
  // Compute the sample weights遍历所有激光雷达数据点,
  for (j = 0; j < set->sample_count; j++)
  {
    sample = set->samples + j;
    pose = sample->pose;//遍历每个粒子,这是粒子对应的位姿,是经运动模型更新后先验位姿

    // Take account of the laser pose relative to the robot
//这个应该是通过机器人与全局坐标系的位姿(每个粒子的位姿),计算激光雷达相对于全局坐标系的位姿。方便后续将激光雷达扫描的终点转换到全局坐标系
//点进去pf_vector_coord_add这个函数,b对应的就是《概率机器人》P128,6.4第一个公式中的机器人在t时刻的位姿,a代表“与机器人固连的传感器局部坐标系位置”
    pose = pf_vector_coord_add(self->laser_pose, pose);//这里的laser_pose其实是laserReceived函数一开始初始化每个激光雷达时得到的激光雷达在base中的位姿

    p = 1.0;

    // Pre-compute a couple of things
    double z_hit_denom = 2 * self->sigma_hit * self->sigma_hit;//测量噪声的方差
    double z_rand_mult = 1.0/data->range_max;//无法解释的随机测量的分母
//因为时间问题,并不是全部点都进行似然计算的,这里只是间隔地选点,我这里设置的是一共选择60个点~~~
    step = (data->range_count - 1) / (self->max_beams - 1);
    for (i = 0; i < data->range_count; i += step)
    {
      obs_range = data->ranges[i][0];
      obs_bearing = data->ranges[i][1];

//继续上面《概率机器人》P128的后半段公式,得到激光雷达点最远端的世界坐标
      // Compute the endpoint of the beam
      hit.v[0] = pose.v[0] + obs_range * cos(pose.v[2] + obs_bearing);
      hit.v[1] = pose.v[1] + obs_range * sin(pose.v[2] + obs_bearing);

//转换到栅格坐标
      // Convert to map grid coords.
      int mi, mj;
      mi = MAP_GXWX(self->map, hit.v[0]);
      mj = MAP_GYWY(self->map, hit.v[1]);

//这是提前计算好的最近距离,计算函数在map_cspace.cpp的map_update_cspace中实现遍历计算,该函数是被AMCLLaser::SetModelLikelihoodField调用
//而这个函数则只在AmclNode::handleMapMessage中调用???而这个handle在多个地方调用,暂时未清楚@TODO      
      // Part 1: Get distance from the hit to closest obstacle.
      // Off-map penalized as max distance
      if(!MAP_VALID(self->map, mi, mj))
        z = self->map->max_occ_dist;
      else
        z = self->map->cells[MAP_INDEX(self->map,mi,mj)].occ_dist;
//书本算法流程的公式
      // Gaussian model
      // NOTE: this should have a normalization of 1/(sqrt(2pi)*sigma)
      pz += self->z_hit * exp(-(z * z) / z_hit_denom);
      // Part 2: random measurements
      pz += self->z_rand * z_rand_mult;

      // TODO: outlier rejection for short readings

      assert(pz <= 1.0);
      assert(pz >= 0.0);
      //      p *= pz;
      // here we have an ad-hoc weighting scheme for combining beam probs
      // works well, though...
      p += pz*pz*pz;//这里有点不太懂。。。理论上应该是上面注释掉的公式。这里应该是为了防止噪声,比如某个出现错误的0,其余都不管用了
    }//每个粒子的所有激光雷达点循环

    sample->weight *= p;
    total_weight += sample->weight;
  }//粒子循环

  return(total_weight);
}

参考

https://wenku.baidu.com/view/92a45164a9956bec0975f46527d3240c8447a1d5.html
https://www.cnblogs.com/lysuns/articles/4710712.html
ROS官网参数配置


喜欢我的文章的话Star一下呗Star

版权声明:本文为白夜行的狼原创文章,未经允许不得以任何形式转载