2021SC@SDUSC
ROS源代码阅读（8）
“SLAM定位： 机器⼈定位的⽅法可以分为⾮⾃主定位与⾃主定位两⼤类。 ⾮⾃主定位是在定位的过程中机器⼈需要借助机器⼈本⾝以外的装置如：全球定位系统(GPS)、全局视觉系统等进⾏定位； ⾃主定位是机器⼈仅依靠机器⼈本⾝携带的传感器进⾏定位。由于在室内环境中，不能使⽤GPS，⽽安装其它的辅助定位系统⽐较⿇烦。 因此机器⼈⼀般采⽤⾃主定位的⽅法。 按照初始位姿是否已知，可把机器⼈⾃主定位分为初始位姿已知的位姿跟踪(Pose tracking)和初始位姿未知的全局定位(Global localization)。 位姿跟踪是在已知机器⼈的初始位姿的条件下，在机器⼈的运动过程中通过将观测到的特征与地图中的特征进⾏匹配，求取它们之间的差别，进⽽更新机器⼈的位姿的机器⼈定位⽅法。位姿跟踪通常采⽤扩展卡尔曼滤波器(Extended Kalman Filter，EKF)来实现。该⽅法采⽤⾼斯分布来近似地表⽰机器⼈位姿的后验概率分布，其计算过程主要包括三步：⾸先是根据机器⼈的运模型预测机器⼈的位姿，然后将观测信息与地图进⾏匹配，最后根据预测后的机器⼈位姿以及匹配的特征计算机器⼈应该观测到的信息，并利⽤应该观测到的信息与实际观测到的信息之间的差距来更新机器⼈的位姿。 全局定位是在机器⼈的初始位姿不确定的条件下，利⽤局部的、不完全的观测信息估计机器⼈的当前位姿。能否解决最典型⽽ ⼜最富挑战性的“恢复”问题在⼀定程度上反应了机器⼈全局定位⽅法的鲁棒性与可靠性。 实际情况是机器⼈在移动，周围物体（⽐如墙，各种路标等）是静⽌的。但是相对机器⼈⽽⾔，是墙在移动。我们通过特征点的匹配，得出墙移动后的位置
（x2,y2,z2,alpha2,beta2,gama2)，相机中两帧间的位置差（x2-x1,y2-y1,z2-z1,…)。于是，我们就可以得出机器⼈的位移，从⽽实现机器⼈的定位。 SLAM建图： 我们所谓的地图，即所有路标点的集合。⼀旦我们确定了路标点的位置，那就可以说我们完成了建图。 稠密建图： 单个图像中的像素，只能提供物体与相机成像平⾯的⾓度以及物体采集到的亮度，⽽⽆法提供物体的距离（Range）。⽽在稠密重建，我们需要知道每⼀个像素点（或⼤部分像素点）的距离，⼤致上有以下⼏种解决⽅案： 1.使⽤单⽬相机，利⽤移动相机之后进⾏三⾓化，测量像素的距离。 2.使⽤双⽬相机，利⽤左右⽬的视差计算像素的距离（多⽬原理相同）。 3.使⽤ RGB-D 相机直接获得像素距离。使⽤ RGB-D 进⾏稠密重建往往是更常见的选择。⽽单⽬双⽬的好处，是在⽬前 RGB-D 还⽆法很好应⽤的室外、⼤场景场合中，仍能通过⽴体视觉估计深度信息。 点云地图（Point_Cloud Map） 1.在⽣成每帧点云时，去掉深度值太⼤或⽆效的点。 2.利⽤统计滤波器⽅法去除孤⽴点。 3.该滤波器统计每个点与它最近 N 个点的距离值的分布，去除距离均值过⼤的点。这样，我们保留了那些“粘在⼀起”的点，去掉了孤⽴的噪声点。 最后，利⽤体素滤波器（Voxel Filter）进⾏降采样。由于多个视⾓存在视野重叠，在重叠区域会存在⼤量的位置⼗分相近的点。这会⽆益地占⽤许多内存空间。体素滤波保证在某个⼀定⼤⼩
的⽴⽅体（或称体素）内仅有⼀个点，相当于对三维空间进⾏了降采样，从⽽节省了很多存储空间。 建图和定位⼀样，过程中会产⽣误差，并且误差会逐渐积累。有许多技术能补偿这些误差，⽐如那些能再现某些特征过去的值的⽅法（也就是说，图像匹配法或者环路闭合检测法），或者对现有的地图进⾏处理——以融合该特征在不同时间的不同值。此外还有⼀些⽤于SLAM统计学的技术可起到作⽤，包括卡尔曼滤波、粒⼦滤波（实际上是⼀种蒙特卡罗⽅法）以及扫描匹配的数据范围。”
amcl就是2D的概率定位系统，输⼊激光雷达数据、⾥程计数据，输出机器⼈在地图中的位姿。⽤的是⾃适应蒙特卡洛定位⽅法，这个⽅法是在已知地图中使⽤粒⼦滤波⽅法得到位姿的。如果⾥程计没有误差，完美的情况下，我们可以直接使⽤⾥程计信息推算出机器⼈
（base_frame）相对⾥程计坐标系的位置。但现实情况，⾥程计存在漂移以及⽆法忽略的累计误差，所以AMCL采⽤先根据⾥程计信息初步定位base_frame，然后通过测量模型得到base_frame相对于map_frame（全局地图坐标系）的偏移，也就知道了机器⼈在地图中的位姿。（注意，这⾥虽然估计的是base到map的转换，但最后发布的是map到odom的转换，可以理解为⾥程计的漂移。）
AMCL定位算法直接影响机器⼈的导航精度，因此对源码的研究和分析⾮常必要。
看CMakeLists。我们可以看到，这个包会⽣成
三个库：
amcl_pf
amcl_map
amcl_sensors
⼀个节点：
amcl
其中amcl的订阅与发布：
发布话题：
amcl_pose： geometry_msgs::PoseWithCovarianceStamped，后验位姿+⼀个6*6的协⽅差矩阵（xyz+三个转⾓）
particlecloud：geometry_msgs::PoseArray，粒⼦位姿的数组
⼀个15秒的定时器：AmclNode::checkLaserReceived，检查上⼀次收到激光雷达数据⾄今是否超过15秒，如超过则报错。
发布服务：
global_localization：&AmclNode::globalLocalizationCallback，这⾥是没有给定初始位姿的情况下在全局范围内初始化粒⼦位姿，该Callback调⽤pf_init_model，然后调⽤AmclNode::uniformPoseGenerator在地图的free点随机⽣成pf->max_samples个粒⼦
request_nomotion_update：&AmclNode::nomotionUpdateCallback没运动模型更新的情况下也暂时更新粒⼦
set_map：&AmclNode::setMapCallback
dynamic_reconfigure::Server动态参数配置器。
订阅话题：
scan_topic_：sensor_msgs::LaserScan，AmclNode::laserReceived，这⾥是tf订阅，转换到odom_frame_id_
initialpose：AmclNode::initialPoseReceived，这个应该就是订阅rviz中给的初始化位姿，调⽤
AmclNode::handleInitialPoseMessage，只接受global_frame_id_（⼀般为map）的坐标，并重新⽣成粒⼦
map：AmclNode::mapReceived这个在use_map_topic_的时候才订阅，否则requestMap()；我这⾥也没有订阅，因为只使⽤了⼀个固定的地图。
主要看amcl.cpp
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_set(new AmclNode());
if (argc == 1)
{
// run using ROS input
ros::spin();
}
}
主要就是定义了amcl节点，初始化了⼀个AmclNode的类对象，最关键的中断函数配置都在该类的构造函数中实现。
AmclNode
AmclNode::AmclNode()
{
蒲公英化妆品boost::recursive_mutex::scoped_lock l(configuration_mutex_);
std::string tmp_model_type;
皮革加工private_nh_.param("laser_model_type", tmp_model_type, std::string("likelihood_field"));
private_nh_.param("odom_model_type", tmp_model_type, std::string("diff"));
//从参数服务器中获取初始位姿及初始分布
updatePoseFromServer();
pose_pub_ = nh_.advertise<geometry_msgs::PoseWithCovarianceStamped>("amcl_pose", 2, true);
}
requestMap
AmclNode::requestMap()
{
while(!ros::service::call("static_map", req, resp))
干式放电线圈
{
ROS_WARN("Request for map failed; ");
ros::Duration d(0.5);
d.sleep();
}
handleMapMessage( resp.map );
}
⼀直请求服务static_map直到成功，该服务在map_server这个包的map_server节点中进⾏定义
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_size(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();
}
这⾥请求服务static_server提供map，然后调⽤handleMapMessage处理地图信息。这⾥的地图类型是nav_msgs::OccupancyGrid。laserReceived
感觉这⾥是⽀持多个激光雷达的，到当前响应的激光雷达之前存储的信息，如相对于base的转换，是否更新等，使⽤map结构直接通过id来到对应序号即可，如果之前没有备案则在map结构中备案，然后存到frame_to_laser_及lasers_中下次备⽤
AmclNode::laserReceived(const sensor_msgs::LaserScanConstPtr& laser_scan)
{
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，
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_))
{
pf_update_resample(pf_);
CO2封存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.ientation);
// 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++)
{
variance[6*i+j] = set->cov.m[i][j];
}
}
variance[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
豫公网安备41160202000603
站长QQ:729038198
关于我们
投诉建议