ROS下使用Hokuyo和AMCL进行P3dx平台导航的实现
roswiki
roswiki 3066 0
2016-06-13 18:09

      最近一段时间使用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<ros/ros.h>
#include<tf/transform_broadcaster.h>
#include<nav_msgs/Odometry.h>
#include<geometry_msgs/Twist.h>   
#include<geometry_msgs/PoseWithCovarianceStamped.h>
#include<math.h>
#include<stdio.h>
#include<vector>
#include<iostream>
  
using namespacestd;
ros::Subscriberodom_sub;
ros::Publisherodom_pub;
nav_msgs::Odometryodom;
boost::shared_ptr<tf::TransformBroadcaster>odom_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<nav_msgs::Odometry>("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();
    }
}

#include<ros/ros.h>
#include<tf/transform_broadcaster.h>
#include<nav_msgs/Odometry.h>
#include<geometry_msgs/Twist.h>  
#include<geometry_msgs/PoseWithCovarianceStamped.h>
#include<math.h>
#include<stdio.h>
#include<vector>
#include<iostream>
 
using namespacestd;
ros::Subscriberodom_sub;
ros::Publisherodom_pub;
nav_msgs::Odometryodom;
boost::shared_ptr<tf::TransformBroadcaster>odom_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<nav_msgs::Odometry>("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


 
 
        笔者了解的也不是很深入,在此过程中可能有错误和不足之处,欢迎大家交流。
分享:
游客
要评论请先登录 或者 注册
返回顶部