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 重映射一下<remap from="scan" to="filter_scan">
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树
bool AmclNode::getOdomPose(tf::Stamped<tf::Pose>& odom_pose, double& x, double& y, double& yaw, const ros::Time& t, const std::string& f) { // Get the robot's pose tf::Stamped<tf::Pose> 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<tf::Pose> 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<tf::Pose> 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