刚开始学习定位，大概过了一遍amcl，开始记录下自己看的。
个人感觉amcl的各类框架比较清晰，但是理论基础不够，也没有时间沉下心慢慢研读代码，学习不够，很惭愧。
现有的amcl应该是存了激光雷达的位置和里程计的位置，二者融合位姿作为最后的位姿，或者说利用激光雷达去修正里程计。
- 190403
看了信息融合的东西，现在回来补一下
1、基本架构
先看ros.wiki官网来的tf图1，其实里程计是可以直接单独用来做定位、导航的，这就是所谓的单传感器定位/导航。这就是下图上半部分说的，但是一般来说，我们不推荐单传感器。原因如下
- 任何单一传感器都具有弊端，如里程计具有累计误差，且里程计相对来说精度较低，需要提前进行校准标定，精度与码盘线数也有关系。再比如激光雷达定位精度很高，但是价格昂贵，最主要的是可测范围较小，随着测量范围的增大，价格上涨很快。
- 单一传感器在发生错误时无法自动修正。如轮子出现打滑、搬运、空转等情况，这种时候无法补正，会导致极大的风险。
- 没有反馈，无法提前感知危险信息。比如用里程计根本不可能知道我要撞墙了。
下面再说下用AMCL的好处吧，一方面是和上面相对应，都有所提高；另一方面就是可以深入研究，进行改进，这是后话，再议。
下面说一下代码，amcl包共有三个文件夹（map、pf、sensors）、一个amcl_node.cpp，也可以说三个库和一个节点。
其中，三个库（我捋了一下这三个库，每个文件的大概功能在下面程序包里有提到）
- map库主要是定义了地图的有关功能。
- pf库是粒子滤波的实现（包括取值，kdtree的实现等）
- sensors库主要是两个模型，预测模型和观测模型，这里分别是里程计运动模型和似然域模型。最近我在怼这两个模型，认真怼怼弄懂他们。
一个节点（amcl_node）
amcl_node则是我们整个程序的主要部分。我们的基本实现都在这一cpp文件内实现。具体内容下文解释。
2、程序包
2.1 amcl_node.cpp
amcl_node.cpp大概程序流程如上图（callback即回调函数是个很有用的东西，ROS的收发机制真实妙极，现在终于搞明白一点，ROS已经提供了各类需要的信息，string，pose，bool等等各种各样的类，直接用就可以了。）
用到ROS的收发之后，发现回调函数是每次接收到所订阅的话题时，在回调函数中进行处理。
进入main函数之后，经过初始化就进入了类amcl_node，开始正片。
amcl_node里首先进行了各类参数的设置。
// Grab params off the param server
private_nh_.param("use_map_topic", use_map_topic_, false);
private_nh_.param("first_map_only", first_map_only_, false);
double tmp;
private_nh_.param("gui_publish_rate", tmp, -1.0);
gui_publish_period = ros::Duration(1.0/tmp);
private_nh_.param("save_pose_rate", tmp, 0.5);
save_pose_period = ros::Duration(1.0/tmp);
//设置雷达、里程计相关参数
private_nh_.param("laser_min_range", laser_min_range_, -1.0);
private_nh_.param("laser_max_range", laser_max_range_, -1.0);
private_nh_.param("laser_max_beams", max_beams_, 30);
private_nh_.param("min_particles", min_particles_, 100);
private_nh_.param("max_particles", max_particles_, 5000);
private_nh_.param("kld_err", pf_err_, 0.01);
private_nh_.param("kld_z", pf_z_, 0.99);
private_nh_.param("odom_alpha1", alpha1_, 0.2);
private_nh_.param("odom_alpha2", alpha2_, 0.2);
private_nh_.param("odom_alpha3", alpha3_, 0.2);
private_nh_.param("odom_alpha4", alpha4_, 0.2);
private_nh_.param("odom_alpha5", alpha5_, 0.2);
private_nh_.param("do_beamskip", do_beamskip_, false);
private_nh_.param("beam_skip_distance", beam_skip_distance_, 0.5);
private_nh_.param("beam_skip_threshold", beam_skip_threshold_, 0.3);
if (private_nh_.hasParam("beam_skip_error_threshold_"))
{
private_nh_.param("beam_skip_error_threshold_", beam_skip_error_threshold_);
}
else
{
private_nh_.param("beam_skip_error_threshold", beam_skip_error_threshold_, 0.9);
}
//雷达参数的设定，hit、short、max、rand四个误差等
private_nh_.param("laser_z_hit", z_hit_, 0.95);
private_nh_.param("laser_z_short", z_short_, 0.1);
private_nh_.param("laser_z_max", z_max_, 0.05);
private_nh_.param("laser_z_rand", z_rand_, 0.05);
private_nh_.param("laser_sigma_hit", sigma_hit_, 0.2);
private_nh_.param("laser_lambda_short", lambda_short_, 0.1);
private_nh_.param("laser_likelihood_max_dist", laser_likelihood_max_dist_, 2.0);
其中，重要的设定模型的语句
//雷达模型（似然域模型）
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"));
然后是定义各类发布、订阅服务（实现了函数调用）
//发布
pose_pub_ = nh_.advertise<geometry_msgs::PoseWithCovarianceStamped>("amcl_pose", 2, true);
particlecloud_pub_ = nh_.advertise<geometry_msgs::PoseArray>("particlecloud", 2, true);
//---省略中间的一大部分，主要是这些我也没怎么看鸭----
initial_pose_sub_ = nh_.subscribe("initialpose", 2, &AmclNode::initialPoseReceived, this);
在这些服务中，最重要的就是laserReceived。下面进入laserReceived。
void
AmclNode::laserReceived(const sensor_msgs::LaserScanConstPtr& laser_scan)
{
std::string laser_scan_frame_id = stripSlash(laser_scan->header.frame_id);
last_laser_received_ts_ = ros::Time::now();
if( map_ == NULL ) {
return;
}
boost::recursive_mutex::scoped_lock lr(configuration_mutex_);
int laser_index = -1;
// 判断我们是否有该雷达编号，如果没有，则加入。
if(frame_to_laser_.find(laser_scan_frame_id) == frame_to_laser_.end())
{
ROS_DEBUG("Setting up laser %d (frame_id=%s)\n", (int)frame_to_laser_.size(), laser_scan_frame_id.c_str());
lasers_.push_back(new AMCLLaser(*laser_));
lasers_update_.push_back(true);
laser_index = frame_to_laser_.size();
geometry_msgs::PoseStamped ident;
ident.header.frame_id = laser_scan_frame_id;
ident.header.stamp = ros::Time();
tf2::toMsg(tf2::Transform::getIdentity(), ident.pose);
geometry_msgs::PoseStamped laser_pose;
try
{
this->tf_->transform(ident, laser_pose, base_frame_id_);
}
catch(tf2::TransformException& e)
{
ROS_ERROR("Couldn't transform from %s to %s, "
"even though the message notifier is in use",
laser_scan_frame_id.c_str(),
base_frame_id_.c_str());
return;
}
//激光雷达位姿
pf_vector_t laser_pose_v;
laser_pose_v.v[0] = laser_pose.pose.position.x;
laser_pose_v.v[1] = laser_pose.pose.position.y;
// 默认激光雷达中心是对着正前的。所以角度为0。
laser_pose_v.v[2] = 0;
lasers_[laser_index]->SetLaserPose(laser_pose_v);
ROS_DEBUG("Received laser's pose wrt robot: %.3f %.3f %.3f",
laser_pose_v.v[0],
laser_pose_v.v[1],
laser_pose_v.v[2]);
frame_to_laser_[laser_scan_frame_id] = laser_index;
} else {
// we have the laser pose, retrieve laser index
laser_index = frame_to_laser_[laser_scan_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]);
// 更新标志update----这个强制更新在某些时候很有用，但是某些时候可能会导致定位崩溃。
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;
// Should update sensor data
for(unsigned int i=0; i < lasers_update_.size(); i++)
lasers_update_[i] = true;
force_publication = true;
resample_count_ = 0;
}
//如果已经初始化并需要更新
else if(pf_init_ && lasers_update_[laser_index])
{
//printf("pose\n");
//pf_vector_fprintf(pose, stdout, "%.3f");
AMCLOdomData odata;
odata.pose = pose;
// HACK
// Modify the delta in the action data so the filter gets
// updated correctly
odata.delta = delta;
// Use the action data to update the filter
//实现了用运动模型来更新现有的每一个粒子的位姿
odom_->UpdateAction(pf_, (AMCLSensorData*)&odata);
// Pose at last filter update
//this->pf_odom_pose = pose;
}
bool resampled = false;
// If the robot has moved, update the filter
// 更新激光雷达，下面打是对激光雷达数据的处理。
if(lasers_update_[laser_index])
{
// 创建一个类，赋值之后对这个ldate操作。
AMCLLaserData ldata;
ldata.sensor = lasers_[laser_index];
ldata.range_count = laser_scan->ranges.size();
// To account for lasers that are mounted upside-down, we determine the
// min, max, and increment angles of the laser in the base frame.
//
// Construct min and max angles of laser, in the base_link frame.
tf2::Quaternion q;
q.setRPY(0.0, 0.0, laser_scan->angle_min);
geometry_msgs::QuaternionStamped min_q, inc_q;
min_q.header.stamp = laser_scan->header.stamp;
min_q.header.frame_id = stripSlash(laser_scan->header.frame_id);
tf2::convert(q, min_q.quaternion);
q.setRPY(0.0, 0.0, laser_scan->angle_min + laser_scan->angle_increment);
inc_q.header = min_q.header;
tf2::convert(q, inc_q.quaternion);
try
{
tf_->transform(min_q, min_q, base_frame_id_);
tf_->transform(inc_q, inc_q, base_frame_id_);
}
catch(tf2::TransformException& e)
{
ROS_WARN("Unable to transform min/max laser angles into base frame: %s",
e.what());
return;
}
double angle_min = tf2::getYaw(min_q.quaternion);
double angle_increment = tf2::getYaw(inc_q.quaternion) - angle_min;
// wrapping angle to [-pi .. pi]
angle_increment = fmod(angle_increment + 5*M_PI, 2*M_PI) - M_PI;
ROS_DEBUG("Laser %d angles in base frame: min: %.3f inc: %.3f", laser_index, angle_min, angle_increment);
// Apply range min/max thresholds, if the user supplied them
if(laser_max_range_ > 0.0)
ldata.range_max = std::min(laser_scan->range_max, (float)laser_max_range_);
else
ldata.range_max = laser_scan->range_max;
double range_min;
if(laser_min_range_ > 0.0)
range_min = std::max(laser_scan->range_min, (float)laser_min_range_);
else
range_min = laser_scan->range_min;
// The AMCLLaserData destructor will free this memory
ldata.ranges = new double[ldata.range_count][2];
ROS_ASSERT(ldata.ranges);
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)
ldata.ranges[i][0] = ldata.range_max;
else
ldata.ranges[i][0] = laser_scan->ranges[i];
// Compute bearing
ldata.ranges[i][1] = angle_min +
(i * angle_increment);
}
//注意这里是amcl_laser.cpp的UpdateSensor。
lasers_[laser_index]->UpdateSensor(pf_, (AMCLSensorData*)&ldata);
lasers_update_[laser_index] = false;
pf_odom_pose_ = pose;
// Resample the particles
// 重采样粒子
if(!(++resample_count_ % resample_interval_))
{
pf_update_resample(pf_);
resampled = true;
}
pf_sample_set_t* set = pf_->sets + pf_->current_set;
ROS_DEBUG("Num samples: %d\n", set->sample_count);
// Publish the resulting cloud
// TODO: set maximum rate for publishing
if (!m_force_update)
{
geometry_msgs::PoseArray cloud_msg;
cloud_msg.header.stamp = ros::Time::now();
cloud_msg.header.frame_id = global_frame_id_;
cloud_msg.poses.resize(set->sample_count);
for(int i=0;i<set->sample_count;i++)
{
cloud_msg.poses[i].position.x = set->samples[i].pose.v[0];
cloud_msg.poses[i].position.y = set->samples[i].pose.v[1];
cloud_msg.poses[i].position.z = 0;
tf2::Quaternion q;
q.setRPY(0, 0, set->samples[i].pose.v[2]);
tf2::convert(q, cloud_msg.poses[i].orientation);
}
//将新粒子发布到全局坐标系下，一般是map
particlecloud_pub_.publish(cloud_msg);
}
}
if(resampled || force_publication)
{
// Read out the current hypotheses
//遍历所有粒子簇，找出权重均值最大的簇。
double max_weight = 0.0;
int max_weight_hyp = -1;
std::vector<amcl_hyp_t> hyps;
hyps.resize(pf_->sets[pf_->current_set].cluster_count);
for(int hyp_count = 0;
hyp_count < pf_->sets[pf_->current_set].cluster_count; hyp_count++)
{
double weight;
pf_vector_t pose_mean;
pf_matrix_t pose_cov;
if (!pf_get_cluster_stats(pf_, hyp_count, &weight, &pose_mean, &pose_cov))
{
//获取权值最高的坐标点进行聚类，对高权值得cluster内的点求均值即为当前机器人所在位置坐标
ROS_ERROR("Couldn't get stats on cluster %d", hyp_count);
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) //将位姿、粒子集、协方差矩阵等进行更新、发布
{
ROS_DEBUG("Max weight pose: %.3f %.3f %.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]);
/*
puts("");
pf_matrix_fprintf(hyps[max_weight_hyp].pf_pose_cov, stdout, "%6.3f");
puts("");
*/
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];
tf2::Quaternion q;
q.setRPY(0, 0, hyps[max_weight_hyp].pf_pose_mean.v[2]);
tf2::convert(q, 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++)
{
// Report the overall filter covariance, rather than the
// covariance for the highest-weight cluster
//p.covariance[6*i+j] = hyps[max_weight_hyp].pf_pose_cov.m[i][j];
p.pose.covariance[6*i+j] = set->cov.m[i][j];
}
}
// Report the overall filter covariance, rather than the
// covariance for the highest-weight cluster
//p.covariance[6*5+5] = hyps[max_weight_hyp].pf_pose_cov.m[2][2];
p.pose.covariance[6*5+5] = set->cov.m[2][2];
pose_pub_.publish(p); //发布amcl_pose
last_published_pose = p;
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]);
//map～base减去odom～base得到map～odom，最后发布的是map～odom
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));
//机器人在map坐标系的位姿
tf::Stamped<tf::Pose> tmp_tf_stamped (tmp_tf.inverse(),
laser_scan->header.stamp,
base_frame_id_);
map坐标系原点在 base_link坐标系下的表示
this->tf_->transformPose(odom_frame_id_,
tmp_tf_stamped,
odom_to_map);
map坐标系原点在 odom坐标系下的表示
}
catch(tf2::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_);
geometry_msgs::TransformStamped tmp_tf_stamped;
tmp_tf_stamped.header.frame_id = global_frame_id_;
tmp_tf_stamped.header.stamp = transform_expiration;
tmp_tf_stamped.child_frame_id = odom_frame_id_;
tf2::convert(latest_tf_.inverse(), tmp_tf_stamped.transform);
this->tfb_->sendTransform(tmp_tf_stamped);
sent_first_transform_ = true;
}
}
else
{
ROS_ERROR("No pose!");
}
}
else if(latest_tf_valid_)
{
if (tf_broadcast_ == true)
{
// Nothing changed, so we'll just republish the last transform, to keep
// everybody happy.
ros::Time transform_expiration = (laser_scan->header.stamp +
transform_tolerance_);
geometry_msgs::TransformStamped tmp_tf_stamped;
tmp_tf_stamped.header.frame_id = global_frame_id_;
tmp_tf_stamped.header.stamp = transform_expiration;
tmp_tf_stamped.child_frame_id = odom_frame_id_;
tf2::convert(latest_tf_.inverse(), tmp_tf_stamped.transform);
this->tfb_->sendTransform(tmp_tf_stamped);
}
// Is it time to save our last pose to the param server
ros::Time now = ros::Time::now();
if((save_pose_period.toSec() > 0.0) &&
(now - save_pose_last_time) >= save_pose_period)
{
this->savePoseToServer();
save_pose_last_time = now;
}
}
}
2.2 sensors
2.3 pf
2.4 map
6475
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
