在用机器人和激光雷达跑SLAM算法构建地图时,总感觉激光雷达数据在RVIZ中显示的方向有问题(前提是激光雷达的安装方向正确),知道是tf坐标转换存在问题,但一直没有调整到好的状态,所以查了些相关资料,终于搞明白了ROS中的坐标转换是怎么回事,于是记录下来。  

一、首先要搞明白URDF、TF和odom的关系

  ROS 中对于多坐标系的处理是使用树型表示,在机器人自主导航中,ROS会构建这几个很重要的坐标系。一般在urdf文件中都要定义base_link,它代表了机器人的主干,其它所有的frame都是相对于base_link定义并粘在一起的。它们一起相对于大地图map移动,让机器人移动就是向tf发布 geometry_msgs::TransformStamped 消息通知ros  base_link相对于map的tf转换关系。先看一下这几个概念在ros中的定义:  
  • base_link: 一般位于tf tree的最根部,物理语义原点一般为表示机器人中心,为相对机器人的本体的坐标系。
  • odom:一般直接与base_link 相链接,语义为一个对于机器人全局位姿的粗略估计。取名来源于odometry(里程计),一般这个坐标系的数据也是来源于里程计。对于全局位姿的估计方法很多,比如在hector SLAM与导航体系中,就采用了imu数据估计全局位姿,还有很多视觉里程计的算法(visual odometry)也能提供位姿估计。原点为开始计算位姿那个时刻的机器人的位置。
  • map: 一般与odom(或者odom_combined)相连,语义为一个经过先验(或者SLAM)地图数据矫正过的,在地图中的位姿信息。与odom同为全局坐标系。原点为地图原点(地图原点在地图相应的yaml文件中有规定)。
 

各个Frames的关系

  frame之间是按树状结构组织的。所以每个frame只有一个父节点和任意多个子节点。 上述几个frame的关系:  
map --> odom --> base_link
odom到base_link的坐标转换是从运动源计算出来广播的。map到base_link的坐标转换是被定位模块计算出来的. 但定位模块不发布map到base_link的转换. 相反它先接受从odom到base_link的转换, 再计算并广播map到odom的位置转换关系。  
  • fixed_frame:RViz中认定的大世界就是fixed_frame
  • target_frame:Rviz中视觉跟踪的frame是 target_frame
 

二、坐标转换的两种方法

 

方法一:修改启动文件

  在lidar的launch启动文件中增加:  
<node pkg="tf" type="static_transform_publisher" name="base_link_to_laser4" 
    args="0.0 0.0 0.2 0.0 3.1415926 0.0 /base_link /laser_frame 40" />
</launch>
  其中args="0.0 0.0 0.2 0.0 3.1415926 0.0 /base_link /laser_frame 40"参数依次对应的含义是:static_transform_publisher x y z yaw pitch roll frame_id child_frame_id period_in_ms,其中 x y z 是 x y z 的坐标, yaw pitch roll 是 绕 z旋转弧度角,绕y旋转弧度角,绕x旋转弧度角,frame_id child_frame_id period_in_ms 父坐标系,子坐标系,发布间隔(ms),上述是指的 z方向增加0.2m,绕y旋转180°。(其中注意 yaw pitch roll 不要弄错,可以通过rviz查看, 红色是x , 绿色是y, 蓝色是z)  

方法二:修改urdf文件的坐标

  找到urdf的描述文件,如turtlebot_description/urdf/turtlebot_library.urdf.xacro,在文件中修改坐标  
<joint name="laser" type="fixed">
    <origin xyz="0.00 0.00 0.20" rpy="0 3.1415926 0" />
    <parent link="base_link" />
    <child link="base_laser_link" />
  </joint>
 
  <link name="base_laser_link">
    <visual>
      <geometry>
        <box size="0.00 0.05 0.06" />
      </geometry>
      <material name="Green" />
    </visual>
    <inertial>
      <mass value="0.000001" />
      <origin xyz="0 0 0" />
      <inertia ixx="0.0001" ixy="0.0" ixz="0.0"
        iyy="0.0001" iyz="0.0"
        izz="0.0001" />
    </inertial>
  </link>
 

三、坐标的校正方法

  本节以校正F4激光雷达与D1机器人的坐标为例,其它机器人校正方法类似,F4 与 D1 的坐标校对,是通过对launch文件中的参数进行修改来使两者坐标一致的,要运行哪个launch文件就要对哪个launch文件中的参数进行修改,使两者坐标一致的。在本教程中,便是要修改 gmapping_demo.launch 文件,找到并打开该文件  
<launch>
<node name="arduino" pkg="dashgo_bringup" type="dashgo_driver.py" output="screen">
<rosparam file="$(find dashgo_bringup)/config/my_dashgo_params.yaml" command="load" />
</node>
 
<node name="flashgo" pkg="flashgo" type="flashgo_node" output="screen">
<param name="serial_port" type="string" value="/dev/flashlidar"/>
<param name="serial_baudrate" type="int" value="230400"/>
<param name="frame_id" type="string" value="laser_frame"/>
<param name="inverted" type="bool" value="false"/>
<param name="angle_compensate" type="bool" value="true"/>
</node>
 
<include file="$(find dashgo_description)/launch/dashgo_description.launch"/>
<node pkg="tf" type="static_transform_publisher" name="base_link_to_laser4"
args="0.0 0.0 0.2 0.0 3.1415926 0.0 /base_link /laser_frame 40" /> //修改args的参数值
 
<include file="$(find dashgo_nav)/launch/gmapping.launch"/>
<include file="$(find dashgo_nav)/launch/move_base.launch"/>
</launch>
  args="0.0 0.0 0.2 0.0 3.1415926 0.0 /base_link /laser_frame 40",在 /base_link 前面的 6 个参数,只需要调整前面5个参数,最后一个默认为 0.0 即可,这 5 个参数都是相对于 D1 的坐标系来调整的。前面 3 个,分别表示激光雷达 F4 在X、Y、Z轴(右手定则)上距离(0 , 0 , 0)点的坐标位置,(0 , 0 , 0)点是 D1 的坐标系原点,该点是 D1 的重心点。后面 2 个,分别表示沿着 D1 中心线(正前方与正后方连成的直线)左右方向、上下方向移动偏离的角度,大小范围为 -3.1415926 ~ 3.1415926 ,-3.1415926 为-180度, 3.1415926 为180度。   前后方向校正,只需修改第 4 个参数,D1 的前方是有三个超声波模块的,中间的超声波模块就是D1的正前方,D1 的后方是单独一个超声波模块的,也是D1的正后方,D1 的前后方向了解之后,便调整 F4 的坐标方向与 D1 的一致即可。   一般来说,F4 已经固定在 D1 已设定的位置上,而且 F4 在不停旋转,便要借用较大块、较平整的挡板,如:大纸箱,来确定 F4 当前的方位。先将挡板横摆在 D1 正前方,挡板中心与 D1 正前方那个超声波模块成一条直线,挡板的面要与直线成90度角。然后,对照rviz中 F4 的扫描显示,D1 正前方是否横画着一条红线障碍物。若有,则说明前方坐标正确,再将挡板横摆在 D1 正后方,若 rviz 中同样在 D1 正后方横画着一条红线障碍物,则 D1 前后方向已经正确。若实际中挡板的位置与rviz显示的位置不一致,则根据对前面 参数调整 的了解,相对应地对参数进行调整。具体操作如下:挡板横摆在 D1 正前方,rviz 显示应该如下图:   但rviz实际显示,如下图所示:     Ctrl + C ,关闭 rviz , 也关闭正在运行的launch文件,打开该launch文件。先将第 4 个参数改大(注意数值范围),保存launch文件,接着启动launch文件并打开rviz,查看效果。若显示的红线向正确的红线位置靠近,则再将参数慢慢加大以修正;若偏离正确位置,则将参数改小来修正。正前方的位置校正后,将挡板横摆在 D1 正后方,rviz显示与实际情况是相符的。 左右方向校正,只需要修改第 5 个参数。先将挡板横摆在 D1 正左方,挡板中心与 D1 中心成一条直线,挡板的面要平行于 D1 正前方与正后方所成的直线。然后,对照 rviz 中 F4 的扫描显示,D1 正左方是否横画着一条红线障碍物。若有,则说明左方坐标正确,再将挡板横摆在 D1 正右方,若 rviz 中同样在 D1 正右方横画着一条红线障碍物,则 D1 左右方向已经正确,不用修改参数。若实际中挡板的位置与 rviz 显示的位置不一致,则修改第 5 个参数,若当前数值为 0.0 ,则修改成 3.1415926 ;若当前数值为 3.1415926 ,则修改成 0.0 即可。再运行 launch 文件与 rviz 核对效果。