ROS中测试机器人里程计信息

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

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

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

#include <iostream>                   // C++标准库头文件
#include <iomanip>
#include <math.h>

#include <boost/asio.hpp>             // boost库头文件
#include <boost/bind.hpp>

#include <ros/ros.h>                  // ros头文件
#include <std_msgs/String.h>
#include <nav_msgs/Odometry.h>
#include <geometry_msgs/Quaternion.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/PolygonStamped.h>
#include <geometry_msgs/Polygon.h>
#include <geometry_msgs/Point32.h>
#include <tf/tf.h>
#include <tf/transform_broadcaster.h>

using namespace std;
using namespace boost::asio;


/********************************回调函数***************************************/
void cmd_velCallback(const geometry_msgs::Twist &twist_aux)
{
   // 在回调函数中将接收到的cmd_vel速度消息转换为自定义的结构体(或者union)类型的数据
    speed_buf.vx = twist_aux.linear.x;
    speed_buf.vy = twist_aux.linear.y;
    speed_buf.vth = twist_aux.angular.z;
}


double x = 0.0;                   // 初始位置x的坐标
double y = 0.0;                   // 初始位置y的坐标
double th = 0.0;                  // 初始位置的角度
double vx = 0.0;                  // x方向的初始速度
double vy = 0.0;                  // y方向的初始速度
double vth = 0.0;                 // 初始角速度
double dt = 0.0;                  // 积分时间


int main(int argc, char** argv)
{
    /******************************  配置串口 ***********************************/
    io_service iosev;
    serial_port sp(iosev, "/dev/ttyUSB0");                                      // 设置串口名称
    sp.set_option(serial_port::baud_rate(115200));                              // 设置波特率
    sp.set_option(serial_port::flow_control(serial_port::flow_control::none));  // 设置控制方式
    sp.set_option(serial_port::parity(serial_port::parity::none));              // 设置奇偶校验
    sp.set_option(serial_port::stop_bits(serial_port::stop_bits::one));         // 设置停止位
    sp.set_option(serial_port::character_size(8));                              // 设置字母位数为8位

    ros::init(argc, argv, "base_controller");                                   // 初始化node节点
    ros::NodeHandle n;

    ros::Subscriber cmd_vel_sub = n.subscribe("cmd_vel", 100, cmd_velCallback); // 订阅cmd_vel topic
    ros::Publisher odom_pub = n.advertise<nav_msgs::Odometry>("odom", 10);      // 发布odom topic
    tf::TransformBroadcaster odom_broadcaster;                                  // 发布base_link --> odom的tf变换
    ros::Publisher poly_pub = n.advertise<geometry_msgs::PolygonStamped>("polygon",10); // 发布PolygonStamped信息,rviz中显示机器人边界

    ros::Time current_time, last_time;
    current_time = ros::Time::now();
    last_time = ros::Time::now();

    while(ros::ok())
    {
        current_time = ros::Time::now();

        read(sp, buffer(speed_buf_rev));   // 从串口读取机器人速度数据

        if(CRC_verify(speed_buf_rev))      // 对接收到的数据进行校验
        {
             vx  = speed_buf_rev.vx;
             vy  = speed_buf_rev.vy;
             vth = speed_buf_rev.vth;

             // 打印接收到的机器人速度信息
             ROS_INFO("vx  is %2f", vx);
             ROS_INFO("vy  is %2f", vy);
             ROS_INFO("vth is %2f", vth);

             /**compute odometry in a typical way given the velocities of the robot**/
             double dt = (current_time - last_time).toSec();
             double delta_x = (vx * cos(th) - vy * sin(th)) * dt;
             double delta_y = (vx * sin(th) + vy * cos(th)) * dt;
             double delta_th = vth * dt;

             x += delta_x;
             y += delta_y;
             th += delta_th;

             /***********first, we'll publish the transform over tf*************/
             geometry_msgs::TransformStamped odom_trans;
             odom_trans.header.stamp = current_time;
             odom_trans.header.frame_id = "odom";
             odom_trans.child_frame_id = "base_link";

             odom_trans.transform.translation.x = x;
             odom_trans.transform.translation.y = y;
             odom_trans.transform.translation.z = 0.0;
             odom_trans.transform.rotation = tf::createQuaternionMsgFromYaw(th);

             // send the transform
             odom_broadcaster.sendTransform(odom_trans);


             /*********next, we'll publish the odometry message over ROS*******/
             nav_msgs::Odometry odom;
             odom.header.stamp = current_time;
             odom.header.frame_id = "odom";
             odom.child_frame_id = "base_link";

             // since all odometry is 6DOF we'll need a quaternion created from yaw
             geometry_msgs::Quaternion odom_quat = tf::createQuaternionMsgFromRollPitchYaw(0,0,th);

             //set the position
             odom.pose.pose.position.x = x;
             odom.pose.pose.position.y = y;
             odom.pose.pose.position.z = 0.0;
             odom.pose.pose.orientation = odom_quat;

             // set the velocity
             odom.twist.twist.linear.x = vx;
             odom.twist.twist.linear.y = vy;
             odom.twist.twist.angular.z = vth;

             odom_pub.publish(odom);


             /*******************publish polygon message***********************/
             geometry_msgs::Point32 point[4];
             // coordinates described in base_link frame
             point[0].x = -0.39;  point[0].y = -0.31;
             point[1].x = 0.39;   point[1].y = -0.31;
             point[2].x = 0.39;   point[2].y = 0.31;
             point[3].x = -0.39;  point[3].y = 0.31;

             geometry_msgs::PolygonStamped poly;
             poly.header.stamp = current_time;
             poly.header.frame_id = "base_link";  // 多边形相对于base_link来描述
             poly.polygon.points.push_back(point[0]);
             poly.polygon.points.push_back(point[1]);
             poly.polygon.points.push_back(point[2]);
             poly.polygon.points.push_back(point[3]);

             poly_pub.publish(poly);
        }
        else
        ROS_INFO("Serial port communication failed!");
  

        write(sp, buffer(speed_buf, buffer_length));     // 速度控制信息写入串口

        last_time = current_time;

        ros::spinOnce();
    }   // end-while

    iosev.run();

}
View Code

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

#include <ros/ros.h>
#include <geometry_msgs/Twist.h>
#include <sensor_msgs/Joy.h>


class Teleop
{
  public:
    Teleop();

  private:
    void joyCallback(const sensor_msgs::Joy::ConstPtr& joy);

    ros::NodeHandle nh_;

    int linear_, angular_,transverse_;    // 手柄上的轴号(分别表示用哪个轴控制前后移动、旋转以及横向运动)
    double l_scale_, a_scale_, t_scale_;  // 速度比例系数
    
    ros::Publisher vel_pub_;
    ros::Subscriber joy_sub_;
};


Teleop::Teleop():linear_(1),angular_(0),transverse_(2)
{
  // param()函数从参数服务器取参数值给变量。如果无法获取,则将默认值赋给变量
  nh_.param("axis_linear", linear_, linear_);
  nh_.param("axis_angular", angular_, angular_);
    nh_.param("axis_transverse", transverse_, transverse_);
  nh_.param("scale_angular", a_scale_, a_scale_);
  nh_.param("scale_linear", l_scale_, l_scale_);
    nh_.param("scale_transverse", t_scale_, t_scale_);

  vel_pub_ = nh_.advertise<geometry_msgs::Twist>("cmd_vel", 1);
  joy_sub_ = nh_.subscribe<sensor_msgs::Joy>("joy", 10, &Teleop::joyCallback, this);
}


void Teleop::joyCallback(const sensor_msgs::Joy::ConstPtr& joy)
{
  geometry_msgs::Twist twist;
  
  // 发布的机器人速度等于joy数据乘以速度比例系数
  // 比如手柄X轴向前推到最大时为1.0,速度比例系数为0.4,则机器人最大前进速度为0.4m/s
  twist.linear.x = l_scale_*joy->axes[linear_];
    twist.linear.y = t_scale_*joy->axes[transverse_];
  twist.angular.z = a_scale_*joy->axes[angular_];

  vel_pub_.publish(twist);  // 发布机器人速度信息
}


int main(int argc, char** argv)
{
  ros::init(argc, argv, "teleop_joy");
  Teleop teleop_turtle;

  ros::spin();
}
View Code

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

<launch>
    <!-- Start the base_controller node -->
    <node pkg="slam" type="base_controller" name="base_controller" />

    <!-- Start IMU message publish node -->
    <!-- <node pkg="imu" type="imu" name="imu" /> -->

    <!--Import robot_pose_ekf file into the current file -->
    <!-- <include file="$(find slam)/launch/robot_pose_ekf.launch" /> -->

    <!-- Start joy node: publish joystick message -->
    <node pkg="joy" type="joy_node" name="turtle_joy" respawn="true" output="screen">
        <param name="dev" type="string" value="/dev/input/js0" />   <!-- Defult device name -->
        <param name="deadzone" value="0.12" />
    </node>


    <!-- Axes configuration-->
    <param name="axis_linear" value="1" type="int" />               <!-- Axes for forward and backword movement -->
    <param name="axis_angular" value="0" type="int" />              <!-- Axes for counterclockwise and clockwise rotation -->
    <param name="axis_transverse" value="2" type="int" />           <!-- Axes for left and right movement-->
    <param name="scale_linear" value="0.4" type="double" />         <!-- maximum vx is 0.4m/s -->
    <param name="scale_angular" value="-0.3" type="double" />       <!-- maximum angular velocity is 0.3rad/s  -->
    <param name="scale_transverse" value="0.3" type="double" />     <!-- maximum vy is 0.3m/s -->
    <!-- Start teleop_joy node to control the robot by joystick-->
    <node pkg="slam" type="teleop_joy" name="teleop_joy" />


    <!-- visualization -->
    <node pkg="rviz" type="rviz" name="rviz" args="-d $(find slam)/rviz/teleop.rviz" />
</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向指定的话题上发布数据:

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次速度指令):

$ rostopic pub -1 /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消息的使用)

posted @ 2017-12-28 18:07  XXX已失联  阅读(16657)  评论(2编辑  收藏  举报