2D激光SLAM：：AMCL发布的odom----map坐标TF变换解读

摘自：https://blog.csdn.net/dieju8330/article/details/96770964

2D激光SLAM：：AMCL发布的odom----map坐标TF变换解读

dieju8330 2019-07-24 21:56:08 2896 收藏 41

分类专栏： 2D激光SLAM ROS

版权

AMCL发布的odom----map坐标TF变换解读

---

一、背景

1、AMCL的作用是什么？

答：AMCL是基于蒙特卡洛定位方法的一种定位方法实现，集成于ROS操作系统的navigation导航包里面，简单来说，AMCL就是得到一个相对准确的机器人的世界坐标。

2、为什么需要AMCL？机器人地盘不是都有里程计和陀螺仪吗？

答：

（1）里程计会有累计误差，即跑远了之后，这个误差会一直叠加，另外，还会出现车轮打滑的现象，这时候的里程计信息就不准确了。

（2）陀螺仪是基于积分型的，也会出现误差。

3、AMCL获取到机器人的世界坐标之后，如何把这个信息传达给navigation导航包的move_base路径规划模块？ 

答：这个就涉及到本文要说的，amcl获取到机器人的世界坐标之后，经过一系列变换，最后发布一个tf变换，关于odom-----map两个坐标系的TF变换

---

 二、关于odom和map坐标系

1、odom

odom指里程计的坐标系，这个坐标系在小车启动底盘的时候建立（即以小车的起点为原点），以小车前进方向为X轴（在ROS中，坐标系是右手，拇指、食指和中指），张开这三个手指，相互垂直，然后食指指向正前方，此时，中指的方向就是Y轴，拇指则是Z轴。

 2、map

map我个人理解为全局地图也就是全局坐标系---global map

global map同样也是在机器人启动的时候建立，坐标系的方向跟odom一致。

到这里就会有疑惑，odom和map两个坐标系不应该就重合了吗？

答：在机器人刚启动的时候，这两个坐标系的确是重合的，但是跑远了之后，由于里程计的误差，会使得odom坐标系与global map坐标系的原点不重合，（个人理解是，按照里程计给出的机器人当前位姿，然后根据这个数据返回，最后机器人返回的地方并非一开始启动时的原点）

刚启动时，

跑远之后，或者车轮打滑了【可以看到，map没有动，这是机器人真正的起点，而odom坐标系的原点已经偏移了，就是说，依照里程计给出的机器人位姿信息，逆着反推回去，只能回到下图红色的odom‘，而不是最初的起点】

---

 三、如何修正？

发布一个odom----map的TF坐标变换即可

 1、如何发布？

首先需要明确一些必须的信息，要发布这个TF坐标变换，需要得到一个重要的参数---【机器人真实的位姿（世界坐标系下）】但是如何获取，这是AMCL主要解决的事情，这里不过多的关注

1）假定我们得到了base_link在世界坐标系global_map的坐标变换tmp_tf （即base_link在global_map下的坐标）

2）那么tmp_tf.inverse()则表示世界坐标系global_map到base_link的坐标变换（即global_map在base_link下的坐标）

3）由于base_link到odom坐标系的坐标变换是可以直接知道的（即base_link在odom下的坐标）

4）因此，使用tf.transformPose可以获得global_map到odom的变换tmp1_tf，（即global_map原点在odom坐标系下的坐标)

5）最后，对tmp1_tf求逆，得到odom-->map的变换，（即odom在global_map坐标系下的坐标）

6）发布odom-->map即可实现修正

四、AMCL模块关于此TF变换的源码（带注释，可结合上述过程来看）

1. //发布最大权重的集群的pose统计值

2. pose_pub_.publish(p);

3. last_published_pose = p;

4.  

5. ROS_DEBUG("New pose: %6.3f %6.3f %6.3f",

6. hyps[max_weight_hyp].pf_pose_mean.v[0],

7. hyps[max_weight_hyp].pf_pose_mean.v[1],

8. hyps[max_weight_hyp].pf_pose_mean.v[2]);

9.  

10. // subtracting base to odom from map to base and send map to odom instead

11. // map->odom = map->base_link - base_link->odom

12. geometry_msgs::PoseStamped odom_to_map;

13. try

14. {

15. tf2::Quaternion q;

16. q.setRPY(0, 0, hyps[max_weight_hyp].pf_pose_mean.v[2]);

17. //tmp_tf是base_link在global map下的坐标，即base-->map

18. tf2::Transform tmp_tf(q, tf2::Vector3(hyps[max_weight_hyp].pf_pose_mean.v[0],

19. hyps[max_weight_hyp].pf_pose_mean.v[1],

20. 0.0));

21.  

22. geometry_msgs::PoseStamped tmp_tf_stamped;

23. tmp_tf_stamped.header.frame_id = base_frame_id_;

24. tmp_tf_stamped.header.stamp = laser_scan->header.stamp;

25. //tmp_tf.inverse()是输入，tmp_tf_stamped.pose是输出

26. //tmp_tf_stamped是global map原点在base_link下的坐标，即map-->base

27. tf2::toMsg(tmp_tf.inverse(), tmp_tf_stamped.pose);

28.  

29. //odom_frame_id_ default value is "odom"

30. //将global map原点在base_link下的坐标变换成global map原点在odom下的坐标

31. //即map-->odom，相当于在odom原点看map原点的位置

32. //这里的odom_to_map并非真的odom-->map，而是反过来map-->odom

33. this->tf_->transform(tmp_tf_stamped, odom_to_map, odom_frame_id_);

34. }

35. catch(tf2::TransformException)

36. {

37. ROS_DEBUG("Failed to subtract base to odom transform");

38. return;

39. }

40.  

41. //转换odom_to_map.pose为latest_tf_

42. tf2::convert(odom_to_map.pose, latest_tf_);

43. latest_tf_valid_ = true;

44.  

45. if (tf_broadcast_ == true)

46. {

47. // We want to send a transform that is good up until a

48. // tolerance time so that odom can be used

49. ros::Time transform_expiration = (laser_scan->header.stamp +

50. transform_tolerance_);

51. //设置tmp_tf_stamped头部信息

52. geometry_msgs::TransformStamped tmp_tf_stamped;

53. tmp_tf_stamped.header.frame_id = global_frame_id_;

54. tmp_tf_stamped.header.stamp = transform_expiration;

55. //这个变换就是child_frame_id在parent_frame_id下的坐标

56. tmp_tf_stamped.child_frame_id = odom_frame_id_;

57. //tmp_tf_stamped这个变换是odom原点在map坐标系的坐标，即odom-->map

58. tf2::convert(latest_tf_.inverse(), tmp_tf_stamped.transform);

59.  

60. //发布

61. this->tfb_->sendTransform(tmp_tf_stamped);

62. sent_first_transform_ = true;

63. }

64. }

---

五、个人仿照AMCL发布的TF变换源码（使用tf包替代了AMCL使用的tf2，感觉tf2比较小众？）

为了方便测试，代码中的真实位姿使用了odom的信息来替代了

1. #include

2. #include

3. #include

4. #include "ros/ros.h"

5. #include "sensor_msgs/LaserScan.h"

6. #include "message_filters/subscriber.h"

7. #include "tf/message_filter.h"

8. #include "tf/transform_datatypes.h"

9. #include "tf/transform_listener.h"

10. #include "tf/transform_broadcaster.h"

11. #include "nav_msgs/OccupancyGrid.h"

12. //#include "tf2/utils.h"

13. #include "math.h"

14. //#include "tf2/convert.h"

15. //#include "tf2_ros/message_filter.h"

16. using namespace std;

17.  

18. tf::TransformListener *tf_;

19. tf::TransformBroadcaster *tfb_;

20. nav_msgs::Odometry::ConstPtr oppp;

21.  

22. // The basic vector

23. typedef struct

24. {

25. double v[3]={0};

26. } pose_vector_t;

27.  

28. static double normalize(double z)

29. {

30. return atan2(sin(z),cos(z));

31. }

32. static double angle_diff(double a, double b)

33. {

34. double d1, d2;

35. a = normalize(a);

36. b = normalize(b);

37. d1 = a-b;

38. d2 = 2*M_PI - fabs(d1);

39. if(d1 > 0)

40. d2 *= -1.0;

41. if(fabs(d1) < fabs(d2))

42. return(d1);

43. else

44. return(d2);

45. }

46.  

47. // Return a zero vector

48. pose_vector_t pose_vector_zero()

49. {

50. pose_vector_t c;

51.  

52. c.v[0] = 0.0;

53. c.v[1] = 0.0;

54. c.v[2] = 0.0;

55.  

56. return c;

57. }

58.  

59. void pose_vector_setValue(pose_vector_t * c,double x,double y,double yaw)

60. {

61. double *v;

62. v=c->v;

63. *v=x;

64. v++;

65. *v=y;

66. v++;

67. *v=yaw;

68. //c->(v+1)=y;

69. //*c->(v=yaw;

70. }

71.  

72. pose_vector_t lastPose_v;

73. tf::Transform lastTransfrom_map_in_odom;

74.  

75. void odomMsgCallback(const nav_msgs::Odometry::ConstPtr &odomMsg){

76.  

77. static bool init=false;

78. static double delta_x=0;

79. static bool forward=true;

80. ros::Duration transform_tolerance_;

81. transform_tolerance_.fromSec(0.1);

82. //位姿偏移量初始化

83. pose_vector_t delta = pose_vector_zero();

84. lastTransfrom_map_in_odom=tf::Transform(tf::createQuaternionFromRPY(0, 0, 0),

85. tf::Vector3(0, 0, 0));

86.  

87. if(!init){

88. init=true;

89. pose_vector_setValue(&lastPose_v,

90. odomMsg->pose.pose.position.x,

91. odomMsg->pose.pose.position.y,

92. tf::getYaw(odomMsg->pose.pose.orientation));

93.  

94. }else{

95. delta.v[0] = odomMsg->pose.pose.position.x - lastPose_v.v[0];

96. delta.v[1] = odomMsg->pose.pose.position.y - lastPose_v.v[1];

97. delta.v[2] = angle_diff(tf::getYaw(odomMsg->pose.pose.orientation), lastPose_v.v[2]);

98.  

99. //判断位移偏移量是否大于阈值

100. if(true/*sqrt(pow(delta.v[0],2)+pow(delta.v[1],2))>=0*/){

101.  

102. /******************发布坐标变换**********************************/

103. //取odom获取的位姿作为真实位姿

104. pose_vector_t truePose_v;

105. truePose_v.v[0]=odomMsg->pose.pose.position.x+delta_x;

106. truePose_v.v[1]=odomMsg->pose.pose.position.y;

107. truePose_v.v[2]=tf::getYaw(odomMsg->pose.pose.orientation);

108.  

109. tf::Stamped map_in_odom;

110. tf::Stamped odom_in_map;

111. //tf::Transform map_in_odom;

112. try{

113. //创建一个基于global map的坐标

114. tf::Stamped ident(tf::Transform(tf::createQuaternionFromRPY(0, 0, truePose_v.v[2]),

115. tf::Vector3(truePose_v.v[0], truePose_v.v[1], 0)),odomMsg->header.stamp, "map");

116. //创建上面坐标的逆，即global map原点在Base_link坐标系下的坐标

117. tf::Stamped tmp(ident.inverse(),odomMsg->header.stamp, "base_link");

118. //然后将该坐标转换到odom坐标系下

119. //得到的map_in_odom是 global map原点在odom坐标系下的坐标

120. tf_->transformPose("odom", tmp, map_in_odom);

121.  

122. //求逆变换

123. //得到的odom_in_map是 odom坐标原点在global map坐标系下的坐标

124. //即得到从odom坐标系到global map坐标系的变换矩阵

125. odom_in_map.setData(map_in_odom.inverse());

126. odom_in_map.frame_id_="map";

127. odom_in_map.stamp_=odomMsg->header.stamp+transform_tolerance_;

128.  

129. ROS_INFO("calculate odom in map success");

130. ROS_INFO("odom in map x:[%f] y:[%f] yaw:[%f]",

131. odom_in_map.getOrigin().x(),

132. odom_in_map.getOrigin().y(),

133. odom_in_map.getRotation().getAngle());

134.  

135. ROS_INFO("now send the TF Broadcast odom_in_map");

136.  

137. tfb_->sendTransform(tf::StampedTransform(odom_in_map,odom_in_map.stamp_,"map","odom"));

138.  

139. lastTransfrom_map_in_odom=odom_in_map;

140.  

141. }catch(tf::TransformException e){

142. ROS_WARN("Failed to compute odom pose, skipping scan (%s)", e.what());

143. return ;

144. }

145. // ROS_INFO("now send the TF Broadcast odom_in_map");

146. // odom_in_map.setData(tf::Transform(tf::createQuaternionFromRPY(0, 0, 0),

147. // tf::Vector3(0, 0, 0)));

148. // odom_in_map.frame_id_="map";

149. // odom_in_map.stamp_=odomMsg->header.stamp+transform_tolerance_;

150. // tfb_->sendTransform(tf::StampedTransform(odom_in_map,odom_in_map.stamp_,"map","odom"));

151. if(delta_x<=0){

152. forward=true;

153. }

154. if(delta_x>=10){

155. forward=false;

156. }

157. if(forward)

158. delta_x+=0.02;

159. else

160. delta_x-=0.02;

161.  

162. // pose_vector_setValue(&lastPose_v,

163. // odomMsg->pose.pose.position.x,

164. // odomMsg->pose.pose.position.y,

165. // tf::getYaw(odomMsg->pose.pose.orientation));

166. }else{

167. //位移偏移量没有达到阈值

168. //发布之前的变换

169. ROS_INFO("now send the TF Broadcast odom_in_map");

170. tfb_->sendTransform(tf::StampedTransform(lastTransfrom_map_in_odom,odomMsg->header.stamp+transform_tolerance_,"map","odom"));

171. }

172. }

173.  

174. // tf::Stamped odom_in_map;

175. // tfb_->sendTransform(tf::StampedTransform(odom_in_map,odom_in_map.stamp_,"map","odom"));

176.  

177. }

178.  

179. int main(int argc, char **argv)

180. {

181. ros::init(argc, argv, "publish_tf_node");

182. ros::NodeHandle nh;

183.  

184. tf_=new tf::TransformListener;

185. tfb_=new tf::TransformBroadcaster;

186. message_filters::Subscriber* odom_sub_;

187. //message_filters::Subscriber* scan_sub_;

188. tf::MessageFilter* odom_filter_;

189.  

190. // Subscribers

191. //订阅"odom"

192. ROS_INFO("subscribed the topic \"odom\" ");

193. odom_sub_ = new message_filters::Subscriber(nh, "odom", 100);

194. //odom_sub_->registerCallback(odomMsgCallback/*boost::bind(odomMsgCallback, this, _1)*/);

195. odom_filter_ =new tf::MessageFilter(*odom_sub_, *tf_, "base_link", 100);

196. odom_filter_->registerCallback(odomMsgCallback);

197. ros::spin();

198.  

199. }