ROS amcl定位参数解析 配置

ROS 下再建图gmapping 、导航navigation过程中需要用到amcl定位包

amcl是navigation包集中的一个定位包 github 地址 https://github.com/ros-planning/navigation

安装步骤：

# mkdir -p ~/catkin_ws/src

# cd catkin_ws/src

# git clone https://github.com/ros-planning/navigation

# cd navigation

查看版本

# git branch

选择 indigo版本的navigation

# git checkout indigo-devel

关于amcl定位模块wiki上有详细说明 http://wiki.ros.org/amcl

着重关注的信息

1.1订阅的主题

        scan (sensor_msgs/LaserScan)

                amcl 需要默认订阅的topics  可以在diff.launch 重映射一下 

1.2发布的主题

      amcl_pose (geometry_msgs/PoseWithCovarianceStamped)

                amcl通过粒子滤波后 估计出机器人最佳的位姿 （x,y,theta） 以及各分量的方差   amcl 最终定位结果输出

      particlecloud (geometry_msgs/PoseArray)

                粒子云 用于rviz可视化显示

     tf (tf/tfMessage)

                整个amcl 定位结果体现在这里，这个tf发布的是  odom坐标系(机器人开机那个坐标原点) 在map坐标系下的转换

                amcl 定位结果与其他模块是通过发布 doom 与map 的tf转换关系发生联系

主要参数

~kld_err (double, default: 0.01)      真实概率分布与估计概率分布间的误差     默认取值就好

~kld_z (double, default: 0.99)    标准正态分位数为（1 - P），其中P是在估计分布的误，要小于kld_err  默认就好

~update_min_d (double, default: 0.2 meters)   向前 运动0.2米 就更新粒子   阈值

~update_min_a (double, default: π/6.0 radians) 与update_min_d 类似，只是对旋转而言

~resample_interval (int, default: 2)   对粒子样本重采样间隔 2 ~ 5就好

~initial_pose_x (double, default: 0.0 meters)

~initial_pose_y (double, default: 0.0 meters)

~initial_pose_a (double, default: 0.0 meters)    初始化amcl模块的机器人位姿  可以修改自己指定的位姿 rivz开启第一个粒子云聚集的坐标点

initial_cov_xx (double, default: 0.5*0.5 meters)

initial_cov_yy (double, default: 0.5*0.5 meters)

initial_cov_aa (double, default: 0.5*0.5 meters)    撒粒子的方差，使粒子均匀的围绕初始化点       （nitial_pose_x initial_pose_x initial_pose_x )服从方差 撒粒子云

里程计相关参数

~odom_model_type (string, default: "diff") 里程计模型   可选 "diff", "omni", "diff-corrected" or "omni-corrected".

        diff：2轮差分    omni：全向轮   y方向有速度

~odom_alpha1 (double, default: 0.2)  机器人在转角分量的运动噪声  增大该值，机器人发生有旋转运动时，就会出现扇形噪声粒子云

~odom_alpha2 (double, default: 0.2)    机器人在横向分量运动噪声，噪声在机器人左右两边分布 0.5如下图

~odom_alpha3 (double, default: 0.2) 改参数是纵向分量运动噪声，沿着机器人前进方向分布 0.5参数如下图

~odom_alpha4 (double, default: 0.2)  斜角方向上的运动噪声 0.5参数如下图

~odom_alpha5 (double, default: 0.2)  第五个参数 对于 2轮差分diff 里程计模型无用，可忽略  该参数只对全向运动模型有用， 

因此参数 odom_alpha1~odom_alpha4里程计运动模型噪声 可适当的参数可提高定位精度跟鲁棒性

TF树 间的转换 Transforms   

# rosrun rqt_tf_tree rqt_tf_tree             执行该命令可以看到 tf树

参考wiki关于坐标系间转换的说明  (http://wiki.ros.org/amcl)

/odom_frame: 里程计坐标系 即机器人上电的那个坐标点，

/base_frame 到 odom_frame 之间的关系在系统中是一直维护着的 ，所以已知

bool AmclNode::getOdomPose(tf::Stamped& odom_pose,
                      double& x, double& y, double& yaw,
                      const ros::Time& t, const std::string& f)
{
  // Get the robot's pose
  tf::Stamped ident (tf::Transform(tf::createIdentityQuaternion(),
                                           tf::Vector3(0,0,0)), t, f);
  try
  {
    this->tf_->transformPose(odom_frame_id_, ident, odom_pose);   //tf_ 中维护/base_frame 到 /domo_frame
  }
  catch(tf::TransformException e)
  {
    ROS_WARN("Failed to compute odom pose, skipping scan (%s)", e.what());
    return false;
  }
  x = odom_pose.getOrigin().x();
  y = odom_pose.getOrigin().y();
  double pitch,roll;
  odom_pose.getBasis().getEulerYPR(yaw, pitch, roll);

  return true;
}

base_frame 到 /map_frame间的坐标系 一开始是未知的，需要 amcl 定位估计出来 粒子滤波中权重最大的那个粒子

 ROS_DEBUG("New pose: %6.3f %6.3f %6.3f",
		               hyps[max_weight_hyp].pf_pose_mean.v[0],
		               hyps[max_weight_hyp].pf_pose_mean.v[1],
		               hyps[max_weight_hyp].pf_pose_mean.v[2]);

再将该点装换成 base_line坐标系到 map坐标系的转换矩阵 Transform 

   // subtracting base to odom from map to base and send map to odom instead
		      tf::Stamped odom_to_map; 
		      try
		      {
		      	// tmp_tf base_line坐标系到 map坐标系的转换矩阵
			        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));
				// tmp_tf_stamped map的坐标系原点在 base_link 坐标系下的 描述
			        tf::Stamped tmp_tf_stamped (tmp_tf.inverse(),
			                                              laser_scan->header.stamp,   //
			                                              base_frame_id_);
			//将base_link坐标系下的点转换到odom坐标系下， 最终得到map原点在odom坐标系下所表示的点	
			        this->tf_->transformPose(odom_frame_id_,// odom坐标系
			                                 tmp_tf_stamped,  // map 坐标系原点在base_link坐标系下的 描述
			                                 odom_to_map);
		      }
		      catch(tf::TransformException)
		      {
			        ROS_DEBUG("Failed to subtract base to odom transform");
			        return;
		      }

map_frame到odom_frame转换   latest_tf_ ，latest_tf_.inverse()就是odom_frame到map_frame 的转换

//得到 map到odom的变换矩阵
		      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_);
			     //  构造一个StampedTransform 需要tf::Transform ,stamp_,father_frame,child_frame 参数
			        tf::StampedTransform tmp_tf_stamped(latest_tf_.inverse(),// odom 到map的转换矩阵
			                                            transform_expiration,
			                                            global_frame_id_, odom_frame_id_);
				 
			        this->tfb_->sendTransform(tmp_tf_stamped);//发布 odom 到map的转换矩阵转换
			        sent_first_transform_ = true;
	      		}

具体参考：https://blog.csdn.net/crazyquhezheng/article/details/49154805

已知tf转换：

/base_frame->/map_frame（amcl定位模块估算出）

/map_frame->/base_frame （/base_frame->/map_frame逆转换）

/base_frame->/odom_frame

下面的公式就可以推算:

/map_frame->/odom_frame = /map_frame->/base_frame - /base_frame->/odom_frame