SLAM从入门到精通（amcl定位使用）

【 声明：版权所有，欢迎转载，请勿用于商业用途。 联系信箱：feixiaoxing @163.com】

        学习slam，一般就是所谓的边定位、边制图的知识。然而在实际生产过程中，比如扫地机器人、agv、巡检机器人、农业机器人，很多情况下都是先绘制好地图。等地图ok了，再规划机器人特定的路径。路径这些都没问题了之后，机器人就会按照我们的设想，去指定的地方执行指定的任务就可以了。所以，制定完地图之后，后续的任务就不再需要制图了，主要就是定位。也就是机器人在行进的过程当中不要丢位置就可以了。

        在这过程中其实就衍生出一些问题，也就是学术和生产的问题。对于学术来说，slam的制图和定位常常是一起的。而工业生产中，对定位的要求可能更多一点。这中间，因为场景变化很大，我们绘制出来的地图够不够鲁棒、满足要求，这很重要。除此之外，定位标准也是有差别的。道路上精度可以差一点，但是对于具体的库位，则要求必须精准。这也是学术和工业生产不一样的地方。

        在ros上面，自带了一些定位算法，这中间就有amcl。虽然amcl比较经典，但是由于计算量比较大、反馈比较慢（常常是秒级别的），工业上用反光柱可能更多一点。但是这并不妨碍我们去学习amcl这个经典算法。

1、打开仿真场景

        在开始使用amcl之前，第一步，我们需要的还是打开仿真场景，即输入完source ./devel/setup.sh之后，立刻输入roslaunch wpr_simulation wpb_stage_slam.launch。

2、启动map_server

        amcl除了需要接收robot的odom、laser这些数据之外，另外一个很重要的输入就是map文件。所以这个时候需要输入rosrun map_server map_server ./map.yaml。

feixiaoxing@feixiaoxing-VirtualBox:~/Desktop/catkin_ws$ rosrun map_server map_server ./map.yaml
[ INFO] [1695982052.911529758]: Loading map from image "./map.pgm"
[ INFO] [1695982053.109551903, 334.643000000]: Read a 1024 X 1024 map @ 0.025 m/cell
[ INFO] [1695982275.007835915, 449.119000000]: Sending map

3、启动amcl

        amcl的启动就比较简单了，直接输入rosrun amcl amcl即可。关于amcl的参数设置，我们将来可以慢慢理解，这里全部使用默认的参数。

feixiaoxing@feixiaoxing-VirtualBox:~/Desktop/catkin_ws$ rosrun amcl amcl
[ INFO] [1695982274.997554194]: Requesting the map...
[ INFO] [1695982275.107380439]: Received a 1024 X 1024 map @ 0.025 m/pix
 
[ INFO] [1695982275.271526958, 449.297000000]: Initializing likelihood field model; this can take some time on large maps...
[ INFO] [1695982275.419726758, 449.388000000]: Done initializing likelihood field model.
 

4、启动rviz

        仿真环境、map_server和amcl都启动完毕之后，下面就是启动rviz了。直接输入rosrun rviz rviz即可。等rviz启动后，依次添加RobotModel、LaserScan、Map，最重要的就是还要添加PoseArray，也就是amcl生成的粒子数据。这些粒子决定了最终robot的位姿信息。

5、添加小车控制设备

        为了实现对小车的控制，我们输入rosrun rqt_robot_steering rqt_robot_steering，就可以直接操纵小车移动了。随着小车的前进，大家会看到小车周围的粒子越来越集中，这也是amcl算法的特点，本身也说明了小车的位姿也是越来越准确了。

6、发布pose

        前面我们通过amcl算法发现，车辆走的越远，位姿越来越稳定。这个时候，可以通过rostopic echo amcl_pose，查看小车的位姿信息，

feixiaoxing@feixiaoxing-VirtualBox:~/Desktop/catkin_ws$ rostopic echo amcl_pose
header: 
  seq: 5
  stamp: 
    secs: 367
    nsecs: 355000000
  frame_id: "map"
pose: 
  pose: 
    position: 
      x: 1.829333671192329
      y: -0.023028699572001186
      z: 0.0
    orientation: 
      x: 0.0
      y: 0.0
      z: -0.005907350114325032
      w: 0.9999825514550875
  covariance: [0.00653005930345163, 0.0013395990524377593, 0.0, 0.0, 0.0, 0.0, 0.0013395990524377593, 0.007374991974678432, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.002811733697012357]
---

        上面position中的打印x、y、z是坐标信息，orientation中的x、y、z、w是角度信息，表示方法是四元数，如果需要看角度的话，需要进一步通过矩阵转成roll、pitch和yaw。因为机器人是运行在平面上的，所以这里的yaw就是我们希望获得的数值。至此，我们通过ros、hector slam和amcl就可以实现最最基本的slam工作了。