AMCL的初始位姿估計(jì)可以來自于兩個(gè)地方：

1.用粒子濾波算法估計(jì)出的機(jī)器人位姿作為最新的位姿

在AmclNode::AmclNode() 中參數(shù)設(shè)置完后首先調(diào)用了這么一個(gè)函數(shù)：

updatePoseFromServer();

跳到該函數(shù)看一下：

void AmclNode::updatePoseFromServer() {init_pose_[0] = 0.0;init_pose_[1] = 0.0;init_pose_[2] = 0.0;init_cov_[0] = 0.5 * 0.5;init_cov_[1] = 0.5 * 0.5;init_cov_[2] = (M_PI/12.0) * (M_PI/12.0);// Check for NAN on input from param server, #5239double tmp_pos;//設(shè)置初始位姿，該位姿來自于上一次退出時(shí)存儲(chǔ)的位姿，如果沒有參數(shù)則默認(rèn)為0.//將initial_pose_x賦值給tmp_pos，沒有時(shí)使用默認(rèn)值init_pose_[0]。private_nh_.param("initial_pose_x", tmp_pos, init_pose_[0]);if(!std::isnan(tmp_pos))init_pose_[0] = tmp_pos;else ROS_WARN("ignoring NAN in initial pose X position");private_nh_.param("initial_pose_y", tmp_pos, init_pose_[1]);if(!std::isnan(tmp_pos))init_pose_[1] = tmp_pos;elseROS_WARN("ignoring NAN in initial pose Y position");private_nh_.param("initial_pose_a", tmp_pos, init_pose_[2]);if(!std::isnan(tmp_pos))init_pose_[2] = tmp_pos;elseROS_WARN("ignoring NAN in initial pose Yaw");private_nh_.param("initial_cov_xx", tmp_pos, init_cov_[0]);if(!std::isnan(tmp_pos))init_cov_[0] =tmp_pos;elseROS_WARN("ignoring NAN in initial covariance XX");private_nh_.param("initial_cov_yy", tmp_pos, init_cov_[1]);if(!std::isnan(tmp_pos))init_cov_[1] = tmp_pos;elseROS_WARN("ignoring NAN in initial covariance YY");private_nh_.param("initial_cov_aa", tmp_pos, init_cov_[2]);if(!std::isnan(tmp_pos))init_cov_[2] = tmp_pos;elseROS_WARN("ignoring NAN in initial covariance AA"); }

這個(gè)函數(shù)設(shè)置了一個(gè)位姿以及一個(gè)方差，位姿參數(shù)來自于："initial_pose_x"以及"initial_pose_y"等，這個(gè)參數(shù)在另外一個(gè)函數(shù)：

//這里只是把最新的odom位姿轉(zhuǎn)換成最新的地圖位姿去存儲(chǔ)，即初始位姿map_pose,我們也對(duì)last_published_pose取其協(xié)方差, //作為初始位姿map_pose的協(xié)方差。記住last_published_pose很重要的哦！它一般來源于上一次粒子濾波算法估計(jì)出的機(jī)器人位置。 void AmclNode::savePoseToServer() {// We need to apply the last transform to the latest odom pose to get// the latest map pose to store. We'll take the covariance from// last_published_pose.//這里的tf2有點(diǎn)不太好理解，似乎最后存儲(chǔ)了一個(gè)世界坐標(biāo)系下的坐標(biāo)，但是它初始應(yīng)該是從odom變換過去的還是怎么得到的？？？tf2::Transform odom_pose_tf2;tf2::convert(latest_odom_pose_.pose, odom_pose_tf2);tf2::Transform map_pose = latest_tf_.inverse() * odom_pose_tf2;//獲取坐標(biāo)的角度double yaw = tf2::getYaw(map_pose.getRotation());ROS_DEBUG("Saving pose to server. x: %.3f, y: %.3f", map_pose.getOrigin().x(), map_pose.getOrigin().y() );//獲取坐標(biāo)的xy值private_nh_.setParam("initial_pose_x", map_pose.getOrigin().x());private_nh_.setParam("initial_pose_y", map_pose.getOrigin().y());private_nh_.setParam("initial_pose_a", yaw);//獲取協(xié)方差值？private_nh_.setParam("initial_cov_xx", last_published_pose.pose.covariance[6*0+0]);private_nh_.setParam("initial_cov_yy", last_published_pose.pose.covariance[6*1+1]);private_nh_.setParam("initial_cov_aa", last_published_pose.pose.covariance[6*5+5]); }

這里主要用到了tf2的一些知識(shí)，將map_pose的位姿賦值給了上述幾個(gè)參數(shù)。具體的tf2變換可以參考ROS官網(wǎng)對(duì)于tf2的描述。

這里其實(shí)賦值完后就沒事了，但是在后面：

if(use_map_topic_) {map_sub_ = nh_.subscribe("map", 1, &AmclNode::mapReceived, this);ROS_INFO("Subscribed to map topic.");} else {//所以這里調(diào)用了map_server節(jié)點(diǎn)requestMap();//請(qǐng)求靜態(tài)地圖，調(diào)用map_server節(jié)點(diǎn)}

默認(rèn)use_map_topic_=false所以調(diào)用了requestMap();函數(shù)。

AmclNode::requestMap() {boost::recursive_mutex::scoped_lock ml(configuration_mutex_);// get map via RPC GetMap服務(wù)nav_msgs::GetMap::Request req;nav_msgs::GetMap::Response resp;ROS_INFO("Requesting the map...");while(!ros::service::call("static_map", req, resp)){ROS_WARN("Request for map failed; trying again...");ros::Duration d(0.5);d.sleep();}handleMapMessage( resp.map ); }

這里轉(zhuǎn)到了：

AmclNode::handleMapMessage(const nav_msgs::OccupancyGrid& msg)

函數(shù)處理，注意到在這個(gè)函數(shù)中有一段代碼：

updatePoseFromServer();pf_vector_t pf_init_pose_mean = pf_vector_zero();pf_init_pose_mean.v[0] = init_pose_[0];pf_init_pose_mean.v[1] = init_pose_[1];pf_init_pose_mean.v[2] = init_pose_[2];pf_matrix_t pf_init_pose_cov = pf_matrix_zero();pf_init_pose_cov.m[0][0] = init_cov_[0];pf_init_pose_cov.m[1][1] = init_cov_[1];pf_init_pose_cov.m[2][2] = init_cov_[2];pf_init(pf_, pf_init_pose_mean, pf_init_pose_cov);pf_init_ = false;

這里跟前面的位姿初始化關(guān)聯(lián)起來了。這個(gè)位姿最后是賦值給了pf_init_pose_mean，這是init_pose_唯一被使用到的地方。pf_init_pose_mean的值再次被傳參到：

pf_init(pf_, pf_init_pose_mean, pf_init_pose_cov);

函數(shù)對(duì)粒子進(jìn)行初始化。也就是說算法通過該方法初始化的粒子的位姿最初是來自于AmclNode::savePoseToServer函數(shù)的。

2.訂閱話題獲得初始位姿

除了從上述方法獲得位姿外，算法還可以通過訂閱話題的方式獲得位姿。函數(shù)中訂閱了一個(gè)位姿話題：

initial_pose_sub_ = nh_.subscribe("initialpose", 2, &AmclNode::initialPoseReceived, this);

回調(diào)函數(shù)initialPoseReceived調(diào)用了另外一個(gè)函數(shù)handleInitialPoseMessage去處理了這個(gè)消息：

AmclNode::initialPoseReceived(const geometry_msgs::PoseWithCovarianceStampedConstPtr& msg) {handleInitialPoseMessage(*msg); }

進(jìn)入到handleInitialPoseMessage函數(shù)，可以看到其主要做了一件事情：

將位姿賦值給變量initial_pose_hyp_并調(diào)用applyInitialPose函數(shù)進(jìn)行濾波器的初始化。具體內(nèi)容包括了以下幾個(gè)部分：

2.1 frame_id確認(rèn)

對(duì)于傳入的消息，首先確認(rèn)其消息是否是對(duì)應(yīng)frame_id。frame_id需要對(duì)應(yīng)為“map“，否則跳出。

if(msg.header.frame_id == ""){// This should be removed at some pointROS_WARN("Received initial pose with empty frame_id. You should always supply a frame_id.");}// We only accept initial pose estimates in the global frame, #5148.else if(stripSlash(msg.header.frame_id) != global_frame_id_)//global_frame_id_是指map{ROS_WARN("Ignoring initial pose in frame \"%s\"; initial poses must be in the global frame, \"%s\"",stripSlash(msg.header.frame_id).c_str(),global_frame_id_.c_str());return;}

感覺這里if語句缺少一個(gè)return。

2.2 tf2變換

確認(rèn)消息輸入沒有問題的情況下，調(diào)用tf2變換將位姿變換到指定坐標(biāo)系下。

geometry_msgs::TransformStamped tx_odom;try{ros::Time now = ros::Time::now();// wait a little for the latest tf to become available//數(shù)據(jù)的坐標(biāo)變換。獲得兩個(gè)坐標(biāo)系之間轉(zhuǎn)換的關(guān)系，包括旋轉(zhuǎn)和平移。tx_odom = tf_->lookupTransform(base_frame_id_, msg.header.stamp,base_frame_id_, ros::Time::now(),//base_linkodom_frame_id_, ros::Duration(0.5));//odom}catch(tf2::TransformException e){// If we've never sent a transform, then this is normal, because the// global_frame_id_ frame doesn't exist. We only care about in-time// transformation for on-the-move pose-setting, so ignoring this// startup condition doesn't really cost us anything.if(sent_first_transform_)ROS_WARN("Failed to transform initial pose in time (%s)", e.what());tf2::convert(tf2::Transform::getIdentity(), tx_odom.transform);}tf2::Transform tx_odom_tf2;tf2::convert(tx_odom.transform, tx_odom_tf2);tf2::Transform pose_old, pose_new;tf2::convert(msg.pose.pose, pose_old);pose_new = pose_old * tx_odom_tf2;// Transform into the global frame//變換一個(gè)世界坐標(biāo)ROS_INFO("Setting pose (%.6f): %.3f %.3f %.3f",ros::Time::now().toSec(),pose_new.getOrigin().x(),pose_new.getOrigin().y(),tf2::getYaw(pose_new.getRotation()));// Re-initialize the filterpf_vector_t pf_init_pose_mean = pf_vector_zero();pf_init_pose_mean.v[0] = pose_new.getOrigin().x();pf_init_pose_mean.v[1] = pose_new.getOrigin().y();pf_init_pose_mean.v[2] = tf2::getYaw(pose_new.getRotation());pf_matrix_t pf_init_pose_cov = pf_matrix_zero();// Copy in the covariance, converting from 6-D to 3-Dfor(int i=0; i<2; i++){for(int j=0; j<2; j++){pf_init_pose_cov.m[i][j] = msg.pose.covariance[6*i+j];}}pf_init_pose_cov.m[2][2] = msg.pose.covariance[6*5+5];delete initial_pose_hyp_;initial_pose_hyp_ = new amcl_hyp_t();initial_pose_hyp_->pf_pose_mean = pf_init_pose_mean;initial_pose_hyp_->pf_pose_cov = pf_init_pose_cov;

這一部分感覺是位姿初始化中最難理解的地方，tf2的內(nèi)容網(wǎng)上能查到的不太多，目前也是一知半解，后面再找個(gè)時(shí)間專門學(xué)習(xí)一下。

2.3 初始化粒子分布

調(diào)用applyInitialPose函數(shù)對(duì)粒子進(jìn)行初始化，這里除了需要一個(gè)初始位姿還需要一個(gè)條件：地圖存在。

AmclNode::applyInitialPose() {boost::recursive_mutex::scoped_lock cfl(configuration_mutex_);if( initial_pose_hyp_ != NULL && map_ != NULL ) {// 用高斯分布來初始化濾波器pf_init(pf_, initial_pose_hyp_->pf_pose_mean, initial_pose_hyp_->pf_pose_cov);pf_init_ = false;delete initial_pose_hyp_;initial_pose_hyp_ = NULL;} }//經(jīng)歷如此種種之后，粒子濾波器得到初始化，也就是粒子得到了新的位姿。

上述的兩個(gè)辦法中粒子的初始化使用的都是高斯分布，其實(shí)粒子初始化模型有兩種，另外一種只在特定條件下使用，后面再詳細(xì)介紹。

兩種方式的對(duì)比

這兩種方式都用于初始化位姿，但是用處不同。很多時(shí)候我們算法中沒有對(duì)應(yīng)的話題時(shí)我們都會(huì)使用第一種方式進(jìn)行初始化。

注意到第一種方式中的savePoseToServer，其實(shí)是用于位姿保存的，該函數(shù)會(huì)在每次程序結(jié)束時(shí)執(zhí)行一次，以及每隔一定頻率也會(huì)執(zhí)行一次：

如果程序關(guān)閉而ROS master并沒有關(guān)閉的話，該位姿會(huì)被記錄下來，下次啟動(dòng)時(shí)還會(huì)調(diào)用該位姿。

例如下圖中，機(jī)器人處于場(chǎng)景中某一個(gè)非原點(diǎn)的位置時(shí)，我們關(guān)閉amcl節(jié)點(diǎn)。如果我們保留ROS master然后重新打開amcl節(jié)點(diǎn)，會(huì)發(fā)現(xiàn)機(jī)器人的初始化位姿就是我們上一次節(jié)點(diǎn)關(guān)閉時(shí)的位姿：

而當(dāng)我們關(guān)閉amcl節(jié)點(diǎn)時(shí)，同時(shí)關(guān)閉ROS master節(jié)點(diǎn)。然后重新打開ROS master以及amcl。會(huì)發(fā)現(xiàn)機(jī)器人的初始化位姿變成了原點(diǎn)而不是機(jī)器人真實(shí)存在的位姿：

所以第一種方式生效的條件一般是需要機(jī)器人啟動(dòng)時(shí)在地圖原點(diǎn)或者機(jī)器人的ROS master節(jié)點(diǎn)不能關(guān)閉的情況。

而對(duì)于第二種情況，我們可以通過一個(gè)自己寫的節(jié)點(diǎn)測(cè)試一下。這里簡(jiǎn)單寫了一個(gè)讀取仿真中odom數(shù)據(jù)然后以initialpose話題名稱重新發(fā)布的節(jié)點(diǎn)，注意數(shù)據(jù)格式要跟amcl中的數(shù)據(jù)格式對(duì)應(yīng)起來，要不然會(huì)報(bào)錯(cuò)。這里使用的數(shù)據(jù)格式是<geometry_msgs::PoseWithCovarianceStamped>格式。
這個(gè)是在上面一張圖的基礎(chǔ)上新增了一個(gè)initialpose后得到的結(jié)果，可以看到initialpose話題對(duì)于amcl節(jié)點(diǎn)來說其優(yōu)先級(jí)高于第一種初始化方式。它修正了第一種位姿初始化后的錯(cuò)誤的位姿估計(jì)。

另外，這里由于我的initialpose節(jié)點(diǎn)是一直發(fā)布的，所以最后得到的位姿始終只有一個(gè)，就是只顯示一個(gè)箭頭：
包括我連續(xù)運(yùn)行：

而當(dāng)我將該節(jié)點(diǎn)關(guān)閉之后，繼續(xù)運(yùn)行之后會(huì)回到最初的狀態(tài)，也就是一堆離散的坐標(biāo)點(diǎn)：

根據(jù)這張消息打印結(jié)果大致也能夠看出，在程序開始時(shí)，算法調(diào)用第一種初始化位姿方式進(jìn)行了一次位姿初始化，然后在訂閱到initialpose后采用第二種方式再次進(jìn)行了一次初始化。程序的主要執(zhí)行順序即根據(jù)打印消息的順序執(zhí)行。由于這里initialpose是一直在接收的，所以打印消息中出現(xiàn)了很多次。如果只執(zhí)行一次的話后面的順序跟前面第一張終端圖中的消息順序是一樣的。

參考：

https://zhuanlan.zhihu.com/p/434271496

總結(jié)

以上是生活随笔為你收集整理的AMCL代码详解（二）位姿初始化的全部?jī)?nèi)容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網(wǎng)站內(nèi)容還不錯(cuò)，歡迎將生活随笔推薦給好友。