amcl初探
剛開始學習定位，大概過了一遍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里首先進行了各類參數的設置。
其中，重要的設定模型的語句
//雷達模型（似然域模型）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 indexlaser_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 flagsif(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 updatepf_odom_pose_ = pose;// Filter is now initializedpf_init_ = true;// Should update sensor datafor(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 correctlyodata.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 themif(laser_max_range_ > 0.0)ldata.range_max = std::min(laser_scan->range_max, (float)laser_max_range_);elseldata.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_);elserange_min = laser_scan->range_min;// The AMCLLaserData destructor will free this memoryldata.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;elseldata.ranges[i][0] = laser_scan->ranges[i];// Compute bearingldata.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 publishingif (!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);}//將新粒子發布到全局坐標系下，一般是mapparticlecloud_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 headerp.header.frame_id = global_frame_id_;p.header.stamp = laser_scan->header.stamp;// Copy in the posep.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-Dpf_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_poselast_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～odomtf::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 usedros::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 serverros::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
參考文獻
http://wiki.ros.org/amcl?distro=melodic
https://blog.csdn.net/u013834525/article/details/80166552 ??
總結
- 上一篇： LED驱动电源不足，都有哪些原因
- 下一篇： 2015.2.2号工作日志-------