ROS下使用Hokuyo和AMCL进行P3dx平台导航的实现

ROS下使用Hokuyo和AMCL进行P3dx平台导航的实现

       最近一段时间使用AMCL package进行了小车导航和避障，主要参考ROS官网上的相关教程。这里主要讲述如何使用navigation stack 进行导航的，使用的硬件是Hokuyo激光测距仪，硬件平台是p3dx，使用的package有Gmapping,AMCL,move_base,map_server等.完成的功能是使用Gmapping来构建2D地图,使用AMCL实现定位，使用Navigation来完成避障和导航。

       实现的过程分为四个部分来叙述。1.硬件部分2.软件安装3.地图构建4.导航和避障。这里会涉及许多ROS的基本知识，由于篇幅有限，就将所用到的一些关键点在下一篇进行讲述，主要包括三个接口，tf ,AMCL,mobe_base的参数解析和配置。这里主要讲述如何实现，给出具体的步骤，供大家参考。难免有错误和不足之处，望各位大神指导。

参考（谢谢各位前辈）：

http://wiki.ros.org/navigation/

http://wiki.ros.org/navigation/Tutorials

http://blog.exbot.net/archives/1129

http://blog.csdn.net/crazyquhezheng/article/details/43346907

http://m.blog.csdn.net/blog/dxuehui/39347041

http://blog.csdn.net/heyijia0327/article/details/41823809

1.硬件部分

硬件平台：p3dx，Dell Vostro_5460 1台

传感器：Hokuyo激光测距仪

其他：RS232串口线1根，joystick1个

如下图所示：

 

       首先是在电脑上安装激光测距仪的驱动，一般Hokuyo插上就能直接使用，无需安装驱动。如果发现无法识别Hokuyo，请确认安装驱动；如果使用的是其他的rplidar，第一步也是先在电脑上装上驱动。

       对Hokuyo的端口配置，命令是：$sudochmod a+wr /dev/ttyACM0

可以用命令$ls -l /dev/ttyACM0 来察看端口是否已经配置好。（个人比较喜欢这种方式）

       joystick 是用来在构建地图的时候控制小车运动的，可以人为控制，构建地图是比较方便。这样牵着机器人走一圈下来，地图基本就建好了。

       对joystick的端口配置，命令是：$sudo chmod a+wr /dev/input/js0

2.软件安装

       主要是安装Navigation stack，在catkin_ws/src/中，装入Navigation stack，编译通过一切OK。

       Navigation stack 装入后的文件夹，如图所示：

        我们可以看到里面有AMCL,base_local_planner,map_server,move_base,costmap_2d,dwa_local_planner等，这些都是导航需要用到的。

        Navigation package可以用命令行的方式装入，但作者在用相关节点的时候，总是提示有错误说找不到该节点，所以重新下载原文件，解压，编译，最终没有问题。

如：

        有时有一些依赖项或者文件没有装入，编译是出现错误通不过，建议根据错误提示，把需要的依赖项或者库文件补全。如我所遇到的情况:错误提示cmake_modules 未安装，编译时总是出现错误，后来用命令$sudo apt-get install ros-hydro-cmake-modules，问题解决。

       另外，装入package时如果直接放进工作空间中，编译，通过，这样便于修改原程序，以实现你想要的功能，而用命令行装入的方式则不便于修改。

3.构建地图

       构建地图的方式在另一篇博文中讲到了，这里不再细讲，但大致的流程还是会说明下。

       思路是：首先启动小车与电脑的连接，启动激光测距仪，将激光测距仪的位置变换到/base_link坐标系，加载小车外形文件；然后将里程计的信息提取出来，发布并广播/odom，建立/base_link 和/odom之间的变换关系，运行gmapping，用joystick 来控制小车移动，用rviz来显示构建过程，最终完成整个地图的构建，将所建的地图保存。

       我们首先看下我写的两个launch文件，笔者根据这两个文件来说明建图的步骤，最后会讲到所建地图的保存。

（1）两个launch文件

（a）基本配置文件

包括启动p2os_driver, hokuyo_node, static tf,pioneer3dx_urdf.

（b）运行gmapping并用rviz 显示

包括启动odom_tf, amcl, gmapping, static tf, rviz.

（2）步骤

step1:启动p2os_driver，建立机器人与电脑之间的连接。port是/dev/ttyUSB0，pulse是0.5。这样上层的速度指令就可以通过p2os来和小车通信，从而控制小车的电机转动。

step2:启动hokuyo_node，使用激光数据。激光测距仪在扫描时，我们可以察看它的/scan topic，会发现有数据。 

step3:laser 和base_link frame之间的静态tf 变换。激光测距仪安装位置相对于base_link坐标系的位置变换是（0.28，0，0.16），单位是米。这个可以根据实际的激光测距仪的安装位置而定。

step4:加载小车的urdf文件。这个在p2os 里都有的，可以直接调用。此步不是必须的，如果没有，也无所谓。有兴趣的也可以根据网上的教程建立自己小车的urdf文件。

step5:发布并广播变换odom。这里我根据官网上的发布tf变换的教程写了自己用的odom_tf变换。这部分主要是将里程计的信息发布出去并广播，主要涉及/odom和/base_link frame之间的变换。在使用pioneer平台的时候，它会发布/pose topic，这个就是根据里程计得到的位姿，可以直接用。

step6:启动joystick.用joystick来控制小车移动。如果没有，手推小车移动也是可以的。

step7:启动gmapping. 主要是用这个package来建立地图的，原理可以参考http://openslam.org，ROS的 gmapping就是调用的这个上面的代码。

step8:/map 和/odom frame之间的static tf 变换。

step9:使用Rviz来显示。在打开的界面中，添加要现实的内容，将Laserscan 的topic 设置为/scan ,将Map 的topic 设置为/map ,这样就可以看到建立过程中的地图了。当小车移动时，地图不断扩大。

step10:地图的保存。$rosrun map_server map_saver -f mymapname

将会得到.pgm格式的图片和.yaml格式的地图配置文件。另外可以在rviz里保存.rviz配置文件，便于下一次使用。

（3）效果图

a）实验室外的部分走廊（约20m*2m）

 

b）整个楼道（约50m*8m）

 

       我们在使用gmapping建图的时候还是有误差的，这个图中明显可看到有倾斜，主要原因是里程计的误差。所以建图的时候尽量沿着直线走，速度要慢一些。

4.导航和避障

        地图建立好之后，我们就可以用AMCL 定位来进行导航了。这部分的关键是move_base 里的配置文件，参数会影响到实际小车跑的效果，需要不断尝试。

        思路： 基本配置文件p3dx_configuration.launch文件不变，首先和小车建立连接，启动激光测距仪，发布激光测距仪坐标系和以小车为中心的坐标系间的静态变换，加载小车外形文件用于显示。然后是发布并广播/odom，建立/base_link 和/odom frame 之间的变换（这个是动态变化的），加载建立好的地图，使用AMCL 定位模块，加载move_base类配置规划的参数，并给出/odom和/map 之间的静态变换关系，用rviz 来显示。最后设置目标点，用action client 和server来响应。

        AMCL是机器人在二维移动过程中概率定位系统。它应用自适应的蒙特卡洛定位方式（或者KLD采样），采用微粒过滤器来跟踪已知地图中机器人的位姿。由于小车平台自带编码器，因此决定采用AMCL定位模块来定位。

(0)准备工作

        我们首先看一下下面这张经典的图，有人说，对于navigation来说，这张图绝对是最好的。从软件架构角度来讲move_base类作为navigation的逻辑核心存在。从移动机器人体系结构来说，move_base规定了整个规划层的行为流程。对于Navigation，我们要清楚它的输入和输出是什么，给出对应的输入，则会有对应的输出。中间的过程路径规划过程我们可以不必了解那么清楚。等把整个系统都做完时，如果有兴趣，可以去研究下里面的算法。

 

move_basepackage 主要功能：在已建立好的地图中,根据机器人的传感器信息,作出路径规划，输出cmd_vel，控制机器人到达我们想要的目标位置。

move_basepackage 的输入和输出：

输入：

·        amcl:定位模块，基于概率的定位系统。

·        odometry：根据机器人左右轮速度推算出的航向信息（即/odom 坐标系中机器人x,y坐标以及航向角yaw），是小车编码器可以提供的信息。

·        tf ： 各个坐标系之间的转换关系，如/map frame --> /odom frame ，/odom frame --> /base_link frame，需要弄清楚各个变换之间的关系，确保位置信息的传递。

·        LaserScan：激光传感器的信息，可通过Laser建立环境地图，也可用于定位和避障。

·        Map:用于地位和避障，表示了周围的环境信息。

·        goal ： 期望机器人在地图中的目标位置，或者自己设定的在全局坐标系中的目标点。

        也可以用其他的传感器，如RGB_D。如果没有其中的一些传感器，也是可以的，各个部分不是每一个都需要的，可根据自己的实际情况来选择。

输出：

·        cmd_vel：在cmd_vel这个主题上发布Twist消息，这个消息包含的就是机器人的期望前进速度和转向速度。可以通过速度接口把上层的速度指令转换为轮子的转速，从而控制小车运动。（这里对于Pioneer是用p2os实现的。）

注：从这里我们要去了解整个系统的控制系统的架构。可参考这边博文中的介绍。http://blog.csdn.net/heyijia0327/article/details/41823809

(1)看一下笔者的launch 文件

a)move_base.launch文件

 

其中包含了odom_tf, map_server,move_base,amcl, statictf, rviz以及action_client 和action_server.

b)action_client.launch文件

 

可以设定目标点在waypoints.yaml中，然后启动action_client 节点。

c)action_server.launch文件

 

启动action_server节点。

d)joystick.launch文件

 

       joystick 的使用，首先是安装驱动joystick_drivers，然后写程序来设置按键所代表的动作。

(2)步骤

step1:启动p3dx_configuration.launch，如前面的介绍。命令 $roslaunch p3dx_configuration.launch

step2:发布并广播/odom变换，启动节点tf_odom，将/base_link变换到/odom。这一步非常关键，如果没有考虑到这个动态变换，在信息的流动上是不通的，所以下面再怎么做都是没用的。可以用$rosrun tf view_frames察看tf变换之间的关系，确保tf变换关系是正确的。很多人会遇到这个问题。

step3:加载地图。将建立好的地图的配置文件.yaml加载进去。里面有分辨率，要注意在下面的路径规划中全局和局部的costmap的地图分辨率在数值上不能低于这个分辨率的值，即路径规划地图的分辨率不要高于所建立地图的分辨率。

step4:启动move_base节点，加载路径规划的参数。参数文件有base_local_planner_params.yaml,costmap_commom_params.yaml,global_costmap_params.yaml,local_costmap_params.yamml。理解每个参数的含义，并设置合理的参数值。每个参数的描述在ROS官网上navigation 中的每个package 的页面中都有介绍。

step5:使用AMCL定位，启动amcl节点。分为差分和全向两种，根据你所使用的平台选择合适的launch文件。

step6:发布/map 和/odom之间的静态tf变换。

step7:加载.rviz文件，用rviz来显示。可以在上面设定目标点(2D navigation goal),可以看到小车按照规划的路径走。到这步为止，导航的基本功能已经实现。可以在rviz界面中人为设置障碍，看小车是否会避障，也可以让小车在实际环境中跑，看是否达到了你想要的结果。（一般情况下，很难一次就把小车调的很顺，所以要在理解基本内容的基础上，不断尝试，调节参数，去达到你想要的结果。有人对我说：小车走的歪歪扭扭的，让小车走直线的概率很小。意在说明程序嘛就是要根据你的实际情况反复调试的。）

step8:这步是我自己写的，为了可以人为的设置目标点。官网上也有action_client 和action_server 的例程，可作为参考。主要功能是设置一系列的目标点，小车根据设定的目标点规划路径，然后到达第一个目标点，到达一个目标点之后再执行到到下一个目标点，依次往下推。

odom_tf.cpp代码：

#include
#include
#include
#include   
#include
#include
#include
#include
#include
 
using namespacestd;
ros::Subscriberodom_sub;
ros::Publisherodom_pub;
nav_msgs::Odometryodom;
boost::shared_ptrodom_broadcaster;
geometry_msgs::TransformStampedodom_trans
geometry_msgs::Twistcmdvel;
ros::Timeodom_stamp;
doubleodom_poseYaw, odom_poseX, odom_poseY;
ros::Timecurrent_time, last_time;
 
void odom_cb(constnav_msgs::Odometry& odom_data)
{  
    odom_stamp = odom_data.header.stamp;
    //convert from quaternion into radium
    odom_poseX = odom_data.pose.pose.position.x;     //poseX;
    odom_poseY =odom_data.pose.pose.position.y;    //poseY;
    odom_poseYaw =tf::getYaw(odom_data.pose.pose.orientation);  //poseYaw;
    //cap to (-2PI, 2PI)
    while (odom_poseYaw >= 2*M_PI)    odom_poseYaw -= 2*M_PI;
    while (odom_poseYaw <   0)       odom_poseYaw += 2*M_PI;
    ROS_INFO("odom %f %f %f",odom_poseX, odom_poseY, odom_poseYaw);
    current_time = ros::Time::now();
 
    odom_trans.header.stamp = current_time;
    odom_trans.header.frame_id ="odom";
    odom_trans.child_frame_id ="base_link";
 
    odom_trans.transform.translation.x =odom_poseX;
    odom_trans.transform.translation.y =odom_poseY;
    odom_trans.transform.translation.z = 0.0;
    //since all odometry is 6DOF we'll need aquaternion created from yaw
    geometry_msgs::Quaternion odom_quat =tf::createQuaternionMsgFromYaw(odom_poseYaw);
    odom_trans.transform.rotation = odom_quat;
 
    //send the transform
   odom_broadcaster->sendTransform(odom_trans);
 
    //next, we'll publish the odometry messageover ROS
 
    odom.header.stamp = current_time;
    odom.header.frame_id = "odom";
    odom_trans.child_frame_id ="base_link";
 
    //set the position
    odom.pose.pose.position.x = odom_poseX;
    odom.pose.pose.position.y = odom_poseY;
    odom.pose.pose.position.z = 0.0;
    odom.pose.pose.orientation = odom_quat;
  
    //set the velocity
    odom.child_frame_id ="base_link";
    odom.twist.twist.linear.x =odom_data.twist.twist.linear.x;
    odom.twist.twist.linear.y = odom_data.twist.twist.linear.y;
    odom.twist.twist.angular.z =odom_data.twist.twist.angular.z;
 
    //publish the message
    odom_pub.publish(odom);
    last_time = ros::Time::now();
}
int main(int argc,char **argv) 
{
  ros::init(argc, argv, "odom_tf");
  
  ros::NodeHandle prv_nh("~");
  odom_broadcaster.reset(newtf::TransformBroadcaster);
  odom_sub = prv_nh.subscribe("/pose", 50, &odom_cb);//p2os provides the car's pose throughodometry.
  odom_pub =prv_nh.advertise("odom", 50);
  ros::Rate r(10.0);
  ROS_INFO("odom %f %f %f",odom_poseX, odom_poseY, odom_poseYaw); //odom_pose
  while(ros::ok())
    {
       ros::spinOnce();          
       r.sleep();
    }
}

分析：

(1)tf

a）未加载小车urdf外形文件时

 

 

b）加载小车urdf外形文件时

 

 

（2）各个变换之间的情况

命令：$rostopic echo /tf

（3）rviz效果图

a)实验室里

 

用2D Nav Goal设定目标点时：提示

如果无法找到有效路径时：提示

规划的路径（全局）：绿色线条

 

 

b)空白地图

3）使用action_client 和action_server

 

 

        笔者了解的也不是很深入，在此过程中可能有错误和不足之处，欢迎大家交流。