里程计模型 (两轮差速运动模型)

里程计模型 (两轮差速运动模型)

1. 由左右轮角度变化量更新里程计

gazebo 两轮差速驱动仿真插件

void GazeboRosDiffDrive::UpdateOdometryEncoder()
{
    double vl = joints_[LEFT]->GetVelocity ( 0 );
    double vr = joints_[RIGHT]->GetVelocity ( 0 );
#if GAZEBO_MAJOR_VERSION >= 8
    common::Time current_time = parent->GetWorld()->SimTime();
#else
    common::Time current_time = parent->GetWorld()->GetSimTime();
#endif
    double seconds_since_last_update = ( current_time - last_odom_update_ ).Double();
    last_odom_update_ = current_time;

    double b = wheel_separation_;

    // Book: Sigwart 2011 Autonompus Mobile Robots page:337
   
    double sl = vl * ( wheel_diameter_ / 2.0 ) * seconds_since_last_update;
    double sr = vr * ( wheel_diameter_ / 2.0 ) * seconds_since_last_update;
    double ssum = sl + sr;

    double sdiff = sr - sl;
    // 在非常短的时间内,可以用多边形近似圆弧
    // 曲线长度代替始末点连线长度，始末点连线角度增量: delta_angle/2 (由圆弧构建等腰三角形推出）
    double dx = ( ssum ) /2.0 * cos ( pose_encoder_.theta + ( sdiff ) / ( 2.0*b ) );
    double dy = ( ssum ) /2.0 * sin ( pose_encoder_.theta + ( sdiff ) / ( 2.0*b ) );
    // 由 sr/R_r = sl/R_l = dtheta, R_r-R_r = b 联合求解推出 dtheta
    double dtheta = ( sdiff ) /b;

    pose_encoder_.x += dx;
    pose_encoder_.y += dy;
    pose_encoder_.theta += dtheta;

    double w = dtheta/seconds_since_last_update;
    // sqrt ( dx*dx+dy*dy ) 实际上等于 ssum/2 ( 因为 cos(a)^2+sin(a)^2=1 )
    double v = sqrt ( dx*dx+dy*dy ) /seconds_since_last_update;

    tf::Quaternion qt;
    tf::Vector3 vt;
    qt.setRPY ( 0,0,pose_encoder_.theta );
    vt = tf::Vector3 ( pose_encoder_.x, pose_encoder_.y, 0 );

    odom_.pose.pose.position.x = vt.x();
    odom_.pose.pose.position.y = vt.y();
    odom_.pose.pose.position.z = vt.z();

    odom_.pose.pose.orientation.x = qt.x();
    odom_.pose.pose.orientation.y = qt.y();
    odom_.pose.pose.orientation.z = qt.z();
    odom_.pose.pose.orientation.w = qt.w();

    odom_.twist.twist.angular.z = w;
    odom_.twist.twist.linear.x = v;
    odom_.twist.twist.linear.y = 0;
}

2. 由 cmd_vel 更新里程计

ROS 导航教程的提供示例

#include <ros/ros.h>
#include <tf/transform_broadcaster.h>
#include <nav_msgs/Odometry.h>

int main(int argc, char** argv){
  ros::init(argc, argv, "odometry_publisher");

  ros::NodeHandle n;
  ros::Publisher odom_pub = n.advertise<nav_msgs::Odometry>("odom", 50);
  tf::TransformBroadcaster odom_broadcaster;

  double x = 0.0;
  double y = 0.0;
  double th = 0.0;

  double vx = 0.1;
  double vy = -0.1;
  double vth = 0.1;

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

  ros::Rate r(1.0);
  while(n.ok()){

    ros::spinOnce();               // check for incoming messages
    current_time = ros::Time::now();

    //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;

    //since all odometry is 6DOF we'll need a quaternion created from yaw
    geometry_msgs::Quaternion odom_quat = tf::createQuaternionMsgFromYaw(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 = odom_quat;

    //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";

    //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.child_frame_id = "base_link";
    odom.twist.twist.linear.x = vx;
    odom.twist.twist.linear.y = vy;
    odom.twist.twist.angular.z = vth;

    //publish the message
    odom_pub.publish(odom);

    last_time = current_time;
    r.sleep();
  }
}

turtlebot3 发布里程计示例: 获取 cmd_vel ,发布俩轮关节状态,发布里程计信息,发布tf

turtlebot3 提供示例

/*******************************************************************************
 * Copyright 2016 ROBOTIS CO., LTD.
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *******************************************************************************/

/* Authors: Yoonseok Pyo */

#include <turtlebot3_fake/turtlebot3_fake.h>

Turtlebot3Fake::Turtlebot3Fake() : nh_priv_("~")
{
    // Init fake turtlebot node
    bool init_result = init();
    ROS_ASSERT(init_result);
}

Turtlebot3Fake::~Turtlebot3Fake()
{
}

/*******************************************************************************
 * Init function
 *******************************************************************************/
bool Turtlebot3Fake::init()
{
    // initialize ROS parameter

    std::string robot_model = nh_.param<std::string>("tb3_model", "");

    if (!robot_model.compare("burger")) {
        wheel_seperation_ = 0.16;
        turning_radius_ = 0.080;
        robot_radius_ = 0.10;
    } else if (!robot_model.compare("waffle") ||
               !robot_model.compare("waffle_pi")) {
        wheel_seperation_ = 0.287;
        turning_radius_ = 0.1435;
        robot_radius_ = 0.220;
    }

    nh_.param("wheel_left_joint_name", joint_states_name_[LEFT],
              std::string("wheel_left_joint"));
    nh_.param("wheel_right_joint_name", joint_states_name_[RIGHT],
              std::string("wheel_right_joint"));
    nh_.param("joint_states_frame", joint_states_.header.frame_id,
              std::string("base_footprint"));
    nh_.param("odom_frame", odom_.header.frame_id, std::string("odom"));
    nh_.param("base_frame", odom_.child_frame_id,
              std::string("base_footprint"));

    // initialize variables
    wheel_speed_cmd_[LEFT] = 0.0;
    wheel_speed_cmd_[RIGHT] = 0.0;
    goal_linear_velocity_ = 0.0;
    goal_angular_velocity_ = 0.0;
    cmd_vel_timeout_ = 1.0;
    last_position_[LEFT] = 0.0;
    last_position_[RIGHT] = 0.0;
    last_velocity_[LEFT] = 0.0;
    last_velocity_[RIGHT] = 0.0;

    double pcov[36] = { 0.1, 0, 0,   0, 0,   0, 0, 0.1, 0, 0,   0, 0,
                        0,   0, 1e6, 0, 0,   0, 0, 0,   0, 1e6, 0, 0,
                        0,   0, 0,   0, 1e6, 0, 0, 0,   0, 0,   0, 0.2 };
    memcpy(&(odom_.pose.covariance), pcov, sizeof(double) * 36);
    memcpy(&(odom_.twist.covariance), pcov, sizeof(double) * 36);

    odom_pose_[0] = 0.0;
    odom_pose_[1] = 0.0;
    odom_pose_[2] = 0.0;

    odom_vel_[0] = 0.0;
    odom_vel_[1] = 0.0;
    odom_vel_[2] = 0.0;

    joint_states_.name.push_back(joint_states_name_[LEFT]);
    joint_states_.name.push_back(joint_states_name_[RIGHT]);
    joint_states_.position.resize(2, 0.0);
    joint_states_.velocity.resize(2, 0.0);
    joint_states_.effort.resize(2, 0.0);

    // initialize publishers
    joint_states_pub_ =
        nh_.advertise<sensor_msgs::JointState>("joint_states", 100);
    odom_pub_ = nh_.advertise<nav_msgs::Odometry>("odom", 100);

    // initialize subscribers
    cmd_vel_sub_ = nh_.subscribe(
        "cmd_vel", 100, &Turtlebot3Fake::commandVelocityCallback, this);

    prev_update_time_ = ros::Time::now();

    return true;
}

/*******************************************************************************
 * Callback function for cmd_vel msg
 *******************************************************************************/
void Turtlebot3Fake::commandVelocityCallback(
    const geometry_msgs::TwistConstPtr cmd_vel_msg)
{
    last_cmd_vel_time_ = ros::Time::now();

    goal_linear_velocity_ = cmd_vel_msg->linear.x;
    goal_angular_velocity_ = cmd_vel_msg->angular.z;

    wheel_speed_cmd_[LEFT] = goal_linear_velocity_ -
                             (goal_angular_velocity_ * wheel_seperation_ / 2);
    wheel_speed_cmd_[RIGHT] = goal_linear_velocity_ +
                              (goal_angular_velocity_ * wheel_seperation_ / 2);
}

/*******************************************************************************
 * Calculate the odometry
 *******************************************************************************/
bool Turtlebot3Fake::updateOdometry(ros::Duration diff_time)
{
    double wheel_l, wheel_r; // rotation value of wheel [rad]
    double delta_s, delta_theta;
    double v[2], w[2];

    wheel_l = wheel_r = 0.0;  //左右轮转角初始化为0
    delta_s = delta_theta = 0.0; //base_link 前进的距离和旋转的角度初始化为0

    v[LEFT] = wheel_speed_cmd_[LEFT]; // 由 cmd_vel 推出的左轮线速度
    w[LEFT] = v[LEFT] / WHEEL_RADIUS; // w = v / r
    v[RIGHT] = wheel_speed_cmd_[RIGHT];//由 cmd_vel 推出的右轮线速度
    w[RIGHT] = v[RIGHT] / WHEEL_RADIUS;
    // 更新左右轮的角速度
    last_velocity_[LEFT] = w[LEFT];
    last_velocity_[RIGHT] = w[RIGHT];
    // diff_time 时间内的转角
    wheel_l = w[LEFT] * diff_time.toSec();
    wheel_r = w[RIGHT] * diff_time.toSec();

    if (isnan(wheel_l)) {
        wheel_l = 0.0;
    }

    if (isnan(wheel_r)) {
        wheel_r = 0.0;
    }
    // 更新左右轮的绝对角度值
    last_position_[LEFT] += wheel_l;
    last_position_[RIGHT] += wheel_r;
    // base_link 的前进距离
    delta_s = WHEEL_RADIUS * (wheel_r + wheel_l) / 2.0;
    // base_link 的转角
    delta_theta = WHEEL_RADIUS * (wheel_r - wheel_l) / wheel_seperation_;

    // compute odometric pose (计算方式与1相似，用多边形近似圆弧进行位移计算)
    // x 轴向上， y 轴向左， z 轴垂直纸面向外
   
    odom_pose_[0] += delta_s * cos(odom_pose_[2] + (delta_theta / 2.0));
    odom_pose_[1] += delta_s * sin(odom_pose_[2] + (delta_theta / 2.0));
    odom_pose_[2] += delta_theta;

    // compute odometric instantaneouse velocity (计算方式与1相似)
    odom_vel_[0] = delta_s / diff_time.toSec(); // v
    odom_vel_[1] = 0.0;
    odom_vel_[2] = delta_theta / diff_time.toSec(); // w

    odom_.pose.pose.position.x = odom_pose_[0];
    odom_.pose.pose.position.y = odom_pose_[1];
    odom_.pose.pose.position.z = 0;
    odom_.pose.pose.orientation = tf::createQuaternionMsgFromYaw(odom_pose_[2]);

    // We should update the twist of the odometry
    odom_.twist.twist.linear.x = odom_vel_[0];
    odom_.twist.twist.angular.z = odom_vel_[2];

    return true;
}

/*******************************************************************************
 * Calculate the joint states
 *******************************************************************************/
void Turtlebot3Fake::updateJoint(void)
{
    joint_states_.position[LEFT] = last_position_[LEFT];
    joint_states_.position[RIGHT] = last_position_[RIGHT];
    joint_states_.velocity[LEFT] = last_velocity_[LEFT];
    joint_states_.velocity[RIGHT] = last_velocity_[RIGHT];
}

/*******************************************************************************
 * Calculate the TF
 *******************************************************************************/
void Turtlebot3Fake::updateTF(geometry_msgs::TransformStamped &odom_tf)
{
    odom_tf.header = odom_.header;
    odom_tf.child_frame_id = odom_.child_frame_id;
    odom_tf.transform.translation.x = odom_.pose.pose.position.x;
    odom_tf.transform.translation.y = odom_.pose.pose.position.y;
    odom_tf.transform.translation.z = odom_.pose.pose.position.z;
    odom_tf.transform.rotation = odom_.pose.pose.orientation;
}

/*******************************************************************************
 * Update function
 *******************************************************************************/
bool Turtlebot3Fake::update()
{
    ros::Time time_now = ros::Time::now();
    ros::Duration step_time = time_now - prev_update_time_;
    prev_update_time_ = time_now;

    // zero-ing after timeout
    if ((time_now - last_cmd_vel_time_).toSec() > cmd_vel_timeout_) {
        wheel_speed_cmd_[LEFT] = 0.0;
        wheel_speed_cmd_[RIGHT] = 0.0;
    }

    // odom
    updateOdometry(step_time);
    odom_.header.stamp = time_now;
    odom_pub_.publish(odom_);

    // joint_states
    updateJoint();
    joint_states_.header.stamp = time_now;
    joint_states_pub_.publish(joint_states_);

    // tf
    geometry_msgs::TransformStamped odom_tf;
    updateTF(odom_tf);
    tf_broadcaster_.sendTransform(odom_tf);

    return true;
}

/*******************************************************************************
 * Main function
 *******************************************************************************/
int main(int argc, char *argv[])
{
    ros::init(argc, argv, "turtlebot3_fake_node");
    Turtlebot3Fake tb3fake;

    ros::Rate loop_rate(30);

    while (ros::ok()) {
        tb3fake.update();
        ros::spinOnce();
        loop_rate.sleep();
    }

    return 0;
}


*****************
 * Main function
 *******************************************************************************/
int main(int argc, char *argv[])
{
    ros::init(argc, argv, "turtlebot3_fake_node");
    Turtlebot3Fake tb3fake;

    ros::Rate loop_rate(30);

    while (ros::ok()) {
        tb3fake.update();
        ros::spinOnce();
        loop_rate.sleep();
    }

    return 0;
}