在移动机器人建图和导航过程中,提供相对准确的里程计信息非常关键,是后续很多工作的基础,因此需要对其进行测试保证没有严重的错误或偏差。实际中最可能发生错误的地方在于机器人运动学公式有误,或者正负号不对,或者定义的坐标系之间方向不一致等。

  整个移动机器人的控制结构如下图所示,其中base_controller节点将订阅的cmd_vel信息通过串口或其它通信接口发送给下位机(嵌入式控制板)。下位机中根据机器人运动学公式进行解算,将机器人速度转换为每个轮子的速度,然后通过CAN总线(或其它总线接口)将每个轮子的转速发送给电机驱动板控制电机转动。电机驱动板对电机转速进行闭环控制(PID控制),并统计单位时间内接收到的编码器脉冲数,计算出轮子转速。

  base_controller节点将接收到的cmd_vel速度信息转换为自定义的结构体或union类型的数据(自定义的数据类型中可以包含校验码等其它信息),并通过串口发送控制速度信息(speed_buf)或读取机器人传回的速度信息 (speed_buf_rev)。base_controller节点正确读取到底层(比如嵌入式控制板)传回的速度后进行积分,计算出机器人的估计位置和姿态,并将里程计信息和tf变换发布出去。下面的代码只是一个例子,需要完善自定义的数据结构和校验函数:

  1. #include <iostream> // C++标准库头文件
  2. #include <iomanip>
  3. #include <math.h>
  4.  
  5. #include <boost/asio.hpp> // boost库头文件
  6. #include <boost/bind.hpp>
  7.  
  8. #include <ros/ros.h> // ros头文件
  9. #include <std_msgs/String.h>
  10. #include <nav_msgs/Odometry.h>
  11. #include <geometry_msgs/Quaternion.h>
  12. #include <geometry_msgs/PoseStamped.h>
  13. #include <geometry_msgs/PolygonStamped.h>
  14. #include <geometry_msgs/Polygon.h>
  15. #include <geometry_msgs/Point32.h>
  16. #include <tf/tf.h>
  17. #include <tf/transform_broadcaster.h>
  18.  
  19. using namespace std;
  20. using namespace boost::asio;
  21.  
  22. /********************************回调函数***************************************/
  23. void cmd_velCallback(const geometry_msgs::Twist &twist_aux)
  24. {
  25.    // 在回调函数中将接收到的cmd_vel速度消息转换为自定义的结构体(或者union)类型的数据
  26. speed_buf.vx = twist_aux.linear.x;
  27. speed_buf.vy = twist_aux.linear.y;
  28. speed_buf.vth = twist_aux.angular.z;
  29. }
  30.  
  31. double x = 0.0; // 初始位置x的坐标
  32. double y = 0.0; // 初始位置y的坐标
  33. double th = 0.0; // 初始位置的角度
  34. double vx = 0.0; // x方向的初始速度
  35. double vy = 0.0; // y方向的初始速度
  36. double vth = 0.0; // 初始角速度
  37. double dt = 0.0; // 积分时间
  38.  
  39. int main(int argc, char** argv)
  40. {
  41. /****************************** 配置串口 ***********************************/
  42. io_service iosev;
  43. serial_port sp(iosev, "/dev/ttyUSB0"); // 设置串口名称
  44. sp.set_option(serial_port::baud_rate()); // 设置波特率
  45. sp.set_option(serial_port::flow_control(serial_port::flow_control::none)); // 设置控制方式
  46. sp.set_option(serial_port::parity(serial_port::parity::none)); // 设置奇偶校验
  47. sp.set_option(serial_port::stop_bits(serial_port::stop_bits::one)); // 设置停止位
  48. sp.set_option(serial_port::character_size()); // 设置字母位数为8位
  49.  
  50. ros::init(argc, argv, "base_controller"); // 初始化node节点
  51. ros::NodeHandle n;
  52.  
  53. ros::Subscriber cmd_vel_sub = n.subscribe("cmd_vel", , cmd_velCallback); // 订阅cmd_vel topic
  54. ros::Publisher odom_pub = n.advertise<nav_msgs::Odometry>("odom", ); // 发布odom topic
  55. tf::TransformBroadcaster odom_broadcaster; // 发布base_link --> odom的tf变换
  56. ros::Publisher poly_pub = n.advertise<geometry_msgs::PolygonStamped>("polygon",); // 发布PolygonStamped信息,rviz中显示机器人边界
  57.  
  58. ros::Time current_time, last_time;
  59. current_time = ros::Time::now();
  60. last_time = ros::Time::now();
  61.  
  62. while(ros::ok())
  63. {
  64. current_time = ros::Time::now();
  65.  
  66. read(sp, buffer(speed_buf_rev)); // 从串口读取机器人速度数据
  67.  
  68. if(CRC_verify(speed_buf_rev)) // 对接收到的数据进行校验
  69. {
  70. vx = speed_buf_rev.vx;
  71. vy = speed_buf_rev.vy;
  72. vth = speed_buf_rev.vth;
  73.  
  74. // 打印接收到的机器人速度信息
  75. ROS_INFO("vx is %2f", vx);
  76. ROS_INFO("vy is %2f", vy);
  77. ROS_INFO("vth is %2f", vth);
  78.  
  79. /**compute odometry in a typical way given the velocities of the robot**/
  80. double dt = (current_time - last_time).toSec();
  81. double delta_x = (vx * cos(th) - vy * sin(th)) * dt;
  82. double delta_y = (vx * sin(th) + vy * cos(th)) * dt;
  83. double delta_th = vth * dt;
  84.  
  85. x += delta_x;
  86. y += delta_y;
  87. th += delta_th;
  88.  
  89. /***********first, we'll publish the transform over tf*************/
  90. geometry_msgs::TransformStamped odom_trans;
  91. odom_trans.header.stamp = current_time;
  92. odom_trans.header.frame_id = "odom";
  93. odom_trans.child_frame_id = "base_link";
  94.  
  95. odom_trans.transform.translation.x = x;
  96. odom_trans.transform.translation.y = y;
  97. odom_trans.transform.translation.z = 0.0;
  98. odom_trans.transform.rotation = tf::createQuaternionMsgFromYaw(th);
  99.  
  100. // send the transform
  101. odom_broadcaster.sendTransform(odom_trans);
  102.  
  103. /*********next, we'll publish the odometry message over ROS*******/
  104. nav_msgs::Odometry odom;
  105. odom.header.stamp = current_time;
  106. odom.header.frame_id = "odom";
  107. odom.child_frame_id = "base_link";
  108.  
  109. // since all odometry is 6DOF we'll need a quaternion created from yaw
  110. geometry_msgs::Quaternion odom_quat = tf::createQuaternionMsgFromRollPitchYaw(,,th);
  111.  
  112. //set the position
  113. odom.pose.pose.position.x = x;
  114. odom.pose.pose.position.y = y;
  115. odom.pose.pose.position.z = 0.0;
  116. odom.pose.pose.orientation = odom_quat;
  117.  
  118. // set the velocity
  119. odom.twist.twist.linear.x = vx;
  120. odom.twist.twist.linear.y = vy;
  121. odom.twist.twist.angular.z = vth;
  122.  
  123. odom_pub.publish(odom);
  124.  
  125. /*******************publish polygon message***********************/
  126. geometry_msgs::Point32 point[];
  127. // coordinates described in base_link frame
  128. point[].x = -0.39; point[].y = -0.31;
  129. point[].x = 0.39; point[].y = -0.31;
  130. point[].x = 0.39; point[].y = 0.31;
  131. point[].x = -0.39; point[].y = 0.31;
  132.  
  133. geometry_msgs::PolygonStamped poly;
  134. poly.header.stamp = current_time;
  135. poly.header.frame_id = "base_link"; // 多边形相对于base_link来描述
  136. poly.polygon.points.push_back(point[]);
  137. poly.polygon.points.push_back(point[]);
  138. poly.polygon.points.push_back(point[]);
  139. poly.polygon.points.push_back(point[]);
  140.  
  141. poly_pub.publish(poly);
  142. }
  143. else
  144.         ROS_INFO("Serial port communication failed!");
  145.  
  146. write(sp, buffer(speed_buf, buffer_length)); // 速度控制信息写入串口
  147.  
  148. last_time = current_time;
  149.  
  150. ros::spinOnce();
  151. } // end-while
  152.  
  153. iosev.run();
  154.  
  155. }

  teleop_joy节点订阅手柄发布的joy消息,并将该消息转换为机器人速度:

  1. #include <ros/ros.h>
  2. #include <geometry_msgs/Twist.h>
  3. #include <sensor_msgs/Joy.h>
  4.  
  5. class Teleop
  6. {
  7. public:
  8. Teleop();
  9.  
  10. private:
  11. void joyCallback(const sensor_msgs::Joy::ConstPtr& joy);
  12.  
  13. ros::NodeHandle nh_;
  14.  
  15. int linear_, angular_,transverse_; // 手柄上的轴号(分别表示用哪个轴控制前后移动、旋转以及横向运动)
  16. double l_scale_, a_scale_, t_scale_; // 速度比例系数
  17.  
  18. ros::Publisher vel_pub_;
  19. ros::Subscriber joy_sub_;
  20. };
  21.  
  22. Teleop::Teleop():linear_(),angular_(),transverse_()
  23. {
  24. // param()函数从参数服务器取参数值给变量。如果无法获取,则将默认值赋给变量
  25. nh_.param("axis_linear", linear_, linear_);
  26. nh_.param("axis_angular", angular_, angular_);
  27. nh_.param("axis_transverse", transverse_, transverse_);
  28. nh_.param("scale_angular", a_scale_, a_scale_);
  29. nh_.param("scale_linear", l_scale_, l_scale_);
  30. nh_.param("scale_transverse", t_scale_, t_scale_);
  31.  
  32. vel_pub_ = nh_.advertise<geometry_msgs::Twist>("cmd_vel", );
  33. joy_sub_ = nh_.subscribe<sensor_msgs::Joy>("joy", , &Teleop::joyCallback, this);
  34. }
  35.  
  36. void Teleop::joyCallback(const sensor_msgs::Joy::ConstPtr& joy)
  37. {
  38. geometry_msgs::Twist twist;
  39.  
  40. // 发布的机器人速度等于joy数据乘以速度比例系数
  41. // 比如手柄X轴向前推到最大时为1.0,速度比例系数为0.4,则机器人最大前进速度为0.4m/s
  42. twist.linear.x = l_scale_*joy->axes[linear_];
  43. twist.linear.y = t_scale_*joy->axes[transverse_];
  44. twist.angular.z = a_scale_*joy->axes[angular_];
  45.  
  46. vel_pub_.publish(twist); // 发布机器人速度信息
  47. }
  48.  
  49. int main(int argc, char** argv)
  50. {
  51. ros::init(argc, argv, "teleop_joy");
  52. Teleop teleop_turtle;
  53.  
  54. ros::spin();
  55. }

   为了方便测试,将相关节点写到teleop_control.launch文件中,启动后分别打开base_controller、joy、teleop_joy、rviz这四个节点。注意之前teleop_joy程序中用param()函数获取参数并赋值给程序中的变量,这样就可以将配置参数写在launch文件中,想要改变程序中某些变量的值时直接修改launch文件就行,不用再编译一遍源程序,调试和使用时会很方便。

  1. <launch>
  2. <!-- Start the base_controller node -->
  3. <node pkg="slam" type="base_controller" name="base_controller" />
  4.  
  5. <!-- Start IMU message publish node -->
  6. <!-- <node pkg="imu" type="imu" name="imu" /> -->
  7.  
  8. <!--Import robot_pose_ekf file into the current file -->
  9. <!-- <include file="$(find slam)/launch/robot_pose_ekf.launch" /> -->
  10.  
  11. <!-- Start joy node: publish joystick message -->
  12. <node pkg="joy" type="joy_node" name="turtle_joy" respawn="true" output="screen">
  13. <param name="dev" type="string" value="/dev/input/js0" /> <!-- Defult device name -->
  14. <param name="deadzone" value="0.12" />
  15. </node>
  16.  
  17. <!-- Axes configuration-->
  18. <param name="axis_linear" value="1" type="int" /> <!-- Axes for forward and backword movement -->
  19. <param name="axis_angular" value="0" type="int" /> <!-- Axes for counterclockwise and clockwise rotation -->
  20. <param name="axis_transverse" value="2" type="int" /> <!-- Axes for left and right movement-->
  21. <param name="scale_linear" value="0.4" type="double" /> <!-- maximum vx is 0.4m/s -->
  22. <param name="scale_angular" value="-0.3" type="double" /> <!-- maximum angular velocity is 0.3rad/s -->
  23. <param name="scale_transverse" value="0.3" type="double" /> <!-- maximum vy is 0.3m/s -->
  24. <!-- Start teleop_joy node to control the robot by joystick-->
  25. <node pkg="slam" type="teleop_joy" name="teleop_joy" />
  26.  
  27. <!-- visualization -->
  28. <node pkg="rviz" type="rviz" name="rviz" args="-d $(find slam)/rviz/teleop.rviz" />
  29. </launch>

  实际测试时,机器人的移动距离和转动角度都要进行测试(要确保机器人的实际运动方向与发送的速度指令方向一致,并且偏差在正常范围内),如果测试与预期情况不一样则需要查找原因。另外由于轮子打滑、以及各种误差的影响,对速度积分进行航迹推算得到的里程计累积误差会越来越大。实际测试时rviz中的Odometry信息(红色箭头)以及机器人边界(蓝色矩形)如下图所示。机器人从一个固定参考位置开始运动,主要是前进后退以及横向移动,最终回到起始位置。可以发现误差还算比较小:

  下面用手柄控制机器人走了更远的距离,并且运动过程中旋转较多(出现了打滑的情况),最终回到初始位置时Odometry的位置和角度偏差较大:

  单独使用里程计来估计小车位置姿态的效果不是特别好,因为轮子打滑等情况在实际中很难避免。因此可以考虑使用IMU或其它传感器来同时进行测量,使用robot_pose_ekf(扩展卡尔曼滤波)对imu和里程计的数据进行融合,可以得到更准确的机器人位姿信息。注意在使用robot_pose_ekf时base_controller程序中就不应再发送base_link和odom之间的tf变换了,因为robot_pose_ekf会发布相应的变换。

  下图就是实际测试的情况,其中黄色箭头为多传感器信息融合后得到的里程计信息(odom_combined),红色为单独使用编码器计算的里程计信息(odom)。机器人从一个固定参考位置出发,转一圈之后回到起始位置,融合后的位姿信息更准一点。

  在测试时,某些情况下需要给机器人发送一个恒定的速度,用手柄比较难做到,可以在命令行中使用rostopic pub向指定的话题上发布数据:

  1. rostopic pub <topic-name> <topic-type> [data...]

  使用rostopic pub发布消息时有3种模式:

  1. 锁存模式Latching mode:rostopic will publish a message to /topic_name and keep it latched -- any new subscribers that come online after you start rostopic will hear this message. You can stop this at any time by pressing ctrl-C.

  2. 单次模式Once mode:If you don't want to have to stop rostopic with ctrl-C, you can publish in once mode. rostopic will keep the message latched for 3 seconds, then quit.

  3. 循环模式Rate mode:In rate mode, rostopic will publish your message at a specific rate. For example, -r 10 will publish at 10hz. For file and piped input, this defaults to 10hz.

  三种模式在命令行中对应的选项为:

  1. -l, --latch:Enable latch mode. Latching mode is the default when using command-line arguments.

  2. -r RATE:Enable rate mode. Rate mode is the default (10hz) when using piped or file input.

  3. -1--once:Enable once mode.

  比如让机器人以0.5m/s的速度前进,可以输入下面的指令(如果需要循环发送控制指令机器人才能运动,可以将命令中的-1改为-r 10,即每秒发送10次速度指令):

  1. $ rostopic pub - /cmd_vel geometry_msgs/Twist '{linear: {x: 0.5, y: 0.0, z: 0.0}, angular: {x: 0.0,y: 0.0,z: 0.0}}'

参考:

rostopic command-line tool

微软Xbox One无线手柄控制机器人

使用robot_pose_ekf对传感器信息融合

Publishing Odometry Information over ROS

Dashgo底盘入门教程-校准-不带陀螺仪的精度校准

ROS在rviz中实时显示轨迹(nav_msgs/Path消息的使用)

ROS中测试机器人里程计信息的更多相关文章

  1. ROS中3D机器人建模(一)

    一,机器人建模的ROS软件包 *urdf : 机器人建模最重要的ros软件包是urdf软件包.这个软件包包含一个用于统一机器人描述格式(URDF)的C++解析器,它是一个表示机器人模型的XML文件,还 ...

  2. ROS中3D机器人建模(五)

    一.创建一个差速驱动移动机器人模型 前面我们已经创建了一个7-DOF机械臂机器人模型,接下来我们将创建一个差速机器人模型,差速轮式机器人在机器人底盘的两端安装两个轮子, 整个底盘由一个或两个脚轮支撑. ...

  3. ROS中3D机器人建模(四)

    一.创建一个7-DOF机械臂机器人 创建一个名为seven_dof_arm.xacro的文件,写入相应的代码,其关节名称如下: bottom_joint shoulder_pan_joint shou ...

  4. SLAM+语音机器人DIY系列:(三)感知与大脑——3.轮式里程计与运动控制

    摘要 在我的想象中机器人首先应该能自由的走来走去,然后应该能流利的与主人对话.朝着这个理想,我准备设计一个能自由行走,并且可以与人语音对话的机器人.实现的关键是让机器人能通过传感器感知周围环境,并通过 ...

  5. ROS里程计

    gmapping导航建图包里建图需要里程计信息,且导航也需要. 整个移动机器人的控制结构如下图所示,其中base_controller节点将订阅的cmd_vel信息通过串口或其它通信接口发送给下位机( ...

  6. ROS中利用V-rep进行地图构建仿真

    V-rep中显示激光扫描点  在VREP自带的场景中找到practicalPathPlanningDemo.ttt文件,删除场景中多余的物体只保留静态的地图.然后在Model browser→comp ...

  7. SLAM入门之视觉里程计(2):相机模型(内参数,外参数)

    相机成像的过程实际是将真实的三维空间中的三维点映射到成像平面(二维空间)过程,可以简单的使用小孔成像模型来描述该过程,以了解成像过程中三维空间到二位图像空间的变换过程. 本文包含两部分内容,首先介绍小 ...

  8. SLAM——视觉里程计(一)feature

    从现在开始下面两篇文章来介绍SLAM中的视觉里程计(Visual Odometry).这个是我们正式进入SLAM工程的第一步,而之前介绍的更多的是一些基础理论.视觉里程计完成的事情是视觉里程计VO的目 ...

  9. 【cartographer_ros】四: 发布和订阅里程计odom信息

    上一节介绍了激光雷达Scan传感数据的订阅和发布. 本节会介绍里程计Odom数据的发布和订阅.里程计在cartographer中主要用于前端位置预估和后端优化. 官方文档: http://wiki.r ...

随机推荐

  1. poj 2031 给出每个结点的3维坐标 以及结点的半径 (MST)

    3维空间中有N个圆球,给出x y z 以及圆球的半径 ,求最小生成树 边的权值为两个圆球间的距离 如果圆球相互接触 则权值为0 求最小的权值和 Sample Input 3 //n10.000 10. ...

  2. Map集合遍历的四种方式理解和简单使用

    ~Map集合是键值对形式存储值的,所以遍历Map集合无非就是获取键和值,根据实际需求,进行获取键和值 1:无非就是通过map.keySet()获取到值,然后根据键获取到值 for(String s:m ...

  3. Codeforces 385D - Bear and Floodlight

    385D - Bear and Floodlight 题目大意:有一个人从( l , 0 ) 想走到 ( r , 0 ),有 n 盏路灯,位置为( xi , yi ),每盏路灯都有一个照射的角度ai ...

  4. 桌面版centos安装vncserver并在windows下使用VNC Viewer远程连接

    首先关闭防火墙 在Centos中安装vncserver yum install tigervnc-server 拷贝一份  /lib/systemd/system/vncserver@.service ...

  5. 《Android进阶之光》--Dagger2

    No1: Project的build.gradle文件添加 buildscript{ dependencies{ ...classpath 'com.neenbedankt.gradle.plugin ...

  6. 用js来实现那些数据结构02(数组篇02-数组方法)

    上一篇文章简单的介绍了一下js的类型,以及数组的增删方法.这一篇文章,我们一起来看看数组还有哪些用法,以及在实际工作中我们可以用这些方法来做些什么.由于其中有部分内容并不常用,所以我尽量缩小篇幅.在这 ...

  7. 10,EasyNetQ-发布确认

    默认的AMQP发布不是事务性的,并且不能保证您的消息实际上会到达代理. AMQP指定了一个事务性发布,但是对于RabbitMQ来说,它非常慢,我们还没有通过EasyNetQ API支持. 对于高性能保 ...

  8. supervisor 管理 celery

    安装supervisor [root@ipv6-api ~]# pip3 install  supervisor 生成配置文件 [root@ipv6-api ~]#echo_supervisord_c ...

  9. Web大前端面试题-Day10

    1. px和em的区别? px和em都是长度单位; 区别是: px的值是固定的,指定是多少就是多少, 计算比较容易. em得值不是固定的,并且em会继承父级元素的字体大小. 浏览器的默认字体高都是16 ...

  10. python魔法方法-自定义序列

    自定义序列的相关魔法方法允许我们自己创建的类拥有序列的特性,让其使用起来就像 python 的内置序列(dict,tuple,list,string等). 如果要实现这个功能,就要遵循 python ...