ROS turtlebot3 gazebo角速度跳变

Occupy

我在Ubuntu 20.04的noetic ros1，Gazebo 11.14上运行Gazebo Turtlebot3的仿真，但是机器人的角速度经常发生跳变。
比如用下面这段代码：

#include "ros/ros.h"
#include <Eigen/Core>
#include <Eigen/Dense>
#include <iostream>
#include "nav_msgs/Path.h"
#include "nav_msgs/Odometry.h"
#include <tf2_ros/buffer.h>
#include <tf2/LinearMath/Matrix3x3.h>
#include "tf2_ros/transform_listener.h"
#include "tf2_geometry_msgs/tf2_geometry_msgs.h"
#include "narrow_space_navigation/point_style.h"
#include "narrow_space_navigation/utils.h"
#include <random>

Controller::car_state cur_pose;

void odom_callback(const nav_msgs::Odometry::ConstPtr &msg)
{
    try
    {
        geometry_msgs::Quaternion odom_quat = msg->pose.pose.orientation;
        tf2::Quaternion quat;
        tf2::fromMsg(odom_quat, quat);

        double roll, pitch, yaw;
        tf2::Matrix3x3(quat).getRPY(roll, pitch, yaw);

        cur_pose.x = msg->pose.pose.position.x;
        cur_pose.y = msg->pose.pose.position.y;
        cur_pose.yaw = yaw;
        cur_pose.v = msg->twist.twist.linear.x;
        cur_pose.w = msg->twist.twist.angular.z;

    }
    catch(const std::exception& e)
    {
        ROS_INFO("Some errors in the odom_callback.");
    }
}

int main(int argc, char *argv[])
{
    ros::init(argc,argv,"ramdom_test");
    FILE* new_log;
    new_log = fopen("new_test.txt", "w");
    ROS_INFO("ros init");
    ros::NodeHandle nh;
    ros::Publisher twist_pub = nh.advertise<geometry_msgs::Twist>("/cmd_vel", 10);
    ros::Subscriber odom_sub = nh.subscribe("/odom", 10, odom_callback);

    int multiplier = atoi(argv[1]);
    geometry_msgs::Twist control_output;
    double dt = 0.02;
    double a = 0.0;
    ros::Rate r(50);
    while (ros::ok())
    {
        a = a + dt;
        control_output.linear.x = 0.25 * (1 - cos(a)) * multiplier;
        double w_acc = 0.25 * sin(a) * multiplier;
        control_output.angular.z = control_output.angular.z + w_acc * dt;
        r.sleep();
        ros::spinOnce();
        twist_pub.publish(control_output);
        fprintf(new_log, "%.6f "
                             "%.6f %.6f %.6f %.6f %.6f %.6f %.6f\n",
                cur_pose.x, cur_pose.y, cur_pose.yaw, control_output.linear.x, control_output.angular.z,
                cur_pose.v, cur_pose.w, w_acc);
        fflush(new_log);
    }
    return 0;
}

用默认的waffle_pi进行测试，得到的图像是这样，这是因为摩擦力太小了：
8606a0b9-b905-4b65-aac3-68e1e3d9caf5-{31E206AE-4C76-4A21-9FFB-561FF0B2C6EF}.png
于是我修改了机器人的URDF文件：

<?xml version="1.0"?>
<robot name="turtlebot3_waffle_pi_sim" xmlns:xacro="http://ros.org/wiki/xacro">
  <xacro:arg name="laser_visual"  default="false"/>
  <xacro:arg name="camera_visual" default="false"/>
  <xacro:arg name="imu_visual"    default="false"/>

  <gazebo reference="base_link">
    <material>Gazebo/DarkGrey</material>
  </gazebo>

  <gazebo reference="wheel_left_link">
    <mu1>1.0</mu1>
    <mu2>1.0</mu2>
    <soft_cfm>0.0</soft_cfm>
    <soft_erp>0.2</soft_erp>
    <kp>1e15</kp>
    <kd>4e12</kd>
    <minDepth>0.001</minDepth>
    <maxVel>10</maxVel>
    <fdir1>1 0 0</fdir1>
    <material>Gazebo/FlatBlack</material>
  </gazebo>

  <gazebo reference="wheel_right_link">
    <mu1>1.0</mu1>
    <mu2>1.0</mu2>
    <soft_cfm>0.0</soft_cfm>
    <soft_erp>0.2</soft_erp>
    <kp>1e15</kp>
    <kd>4e12</kd>
    <minDepth>0.001</minDepth>
    <maxVel>10</maxVel>
    <fdir1>1 0 0</fdir1>
    <material>Gazebo/FlatBlack</material>
  </gazebo>

  <gazebo reference="caster_back_right_link">
    <mu1>0.01</mu1>
    <mu2>0.01</mu2>
    <soft_cfm>0.0</soft_cfm>
    <soft_erp>0.2</soft_erp>
    <kp>1e15</kp>
    <kd>4e12</kd>
    <minDepth>0.001</minDepth>
    <maxVel>10</maxVel>
    <material>Gazebo/FlatBlack</material>
  </gazebo>

  <gazebo reference="caster_back_left_link">
    <mu1>0.01</mu1>
    <mu2>0.01</mu2>
    <soft_cfm>0.0</soft_cfm>
    <soft_erp>0.2</soft_erp>
    <kp>1e15</kp>
    <kd>4e12</kd>
    <minDepth>0.001</minDepth>
    <maxVel>10</maxVel>
    <material>Gazebo/FlatBlack</material>
  </gazebo>

  <gazebo reference="imu_link">
    <sensor type="imu" name="imu">
      <always_on>true</always_on>
      <visualize>$(arg imu_visual)</visualize>
    </sensor>
    <material>Gazebo/Grey</material>
  </gazebo>

  <gazebo>
    <plugin name="turtlebot3_waffle_pi_controller" filename="libgazebo_ros_diff_drive.so">
      <commandTopic>cmd_vel</commandTopic>
      <odometryTopic>odom</odometryTopic>
      <odometryFrame>odom</odometryFrame>
      <odometrySource>world</odometrySource>
      <publishOdomTF>true</publishOdomTF>
      <robotBaseFrame>base_footprint</robotBaseFrame>
      <publishWheelTF>false</publishWheelTF>
      <publishTf>true</publishTf>
      <publishWheelJointState>true</publishWheelJointState>
      <legacyMode>false</legacyMode>
      <updateRate>100</updateRate>
      <leftJoint>wheel_left_joint</leftJoint>
      <rightJoint>wheel_right_joint</rightJoint>
      <wheelSeparation>0.287</wheelSeparation>
      <wheelDiameter>0.066</wheelDiameter>
      <wheelAcceleration>4</wheelAcceleration>
      <wheelTorque>10</wheelTorque>
      <rosDebugLevel>na</rosDebugLevel>
    </plugin>
  </gazebo>

  <gazebo>
    <plugin name="imu_plugin" filename="libgazebo_ros_imu.so">
      <alwaysOn>true</alwaysOn>
      <bodyName>imu_link</bodyName>
      <frameName>imu_link</frameName>
      <topicName>imu</topicName>
      <serviceName>imu_service</serviceName>
      <gaussianNoise>0.0</gaussianNoise>
      <updateRate>0</updateRate>
      <imu>
        <noise>
          <type>gaussian</type>
          <rate>
            <mean>0.0</mean>
            <stddev>2e-4</stddev>
            <bias_mean>0.0000075</bias_mean>
            <bias_stddev>0.0000008</bias_stddev>
          </rate>
          <accel>
            <mean>0.0</mean>
            <stddev>1.7e-2</stddev>
            <bias_mean>0.1</bias_mean>
            <bias_stddev>0.001</bias_stddev>
          </accel>
        </noise>
      </imu>
    </plugin>
  </gazebo>

  <gazebo reference="base_scan">
    <material>Gazebo/FlatBlack</material>
    <sensor type="ray" name="lds_lfcd_sensor">
      <pose>0 0 0 0 0 0</pose>
      <visualize>$(arg laser_visual)</visualize>
      <update_rate>5</update_rate>
      <ray>
        <scan>
          <horizontal>
            <samples>360</samples>
            <resolution>1</resolution>
            <min_angle>0.0</min_angle>
            <max_angle>6.28319</max_angle>
          </horizontal>
        </scan>
        <range>
          <min>0.120</min>
          <max>3.5</max>
          <resolution>0.015</resolution>
        </range>
        <noise>
          <type>gaussian</type>
          <mean>0.0</mean>
          <stddev>0.01</stddev>
        </noise>
      </ray>
      <plugin name="gazebo_ros_lds_lfcd_controller" filename="libgazebo_ros_laser.so">
        <topicName>scan</topicName>
        <frameName>base_scan</frameName>
      </plugin>
    </sensor>
  </gazebo>

<!--link : https://www.raspberrypi.org/documentation/hardware/camera/-->
  <gazebo reference="camera_rgb_frame">
    <sensor type="camera" name="Pi Camera">
      <always_on>true</always_on>
      <visualize>$(arg camera_visual)</visualize>
      <camera>
          <horizontal_fov>1.085595</horizontal_fov>
          <image>
              <width>640</width>
              <height>480</height>
              <format>R8G8B8</format>
          </image>
          <clip>
              <near>0.03</near>
              <far>100</far>
          </clip>
      </camera>
      <plugin name="camera_controller" filename="libgazebo_ros_camera.so">
        <alwaysOn>true</alwaysOn>
        <updateRate>30.0</updateRate>
        <cameraName>camera</cameraName>
        <frameName>camera_rgb_optical_frame</frameName>
        <imageTopicName>rgb/image_raw</imageTopicName>
        <cameraInfoTopicName>rgb/camera_info</cameraInfoTopicName>
        <hackBaseline>0.07</hackBaseline>
        <distortionK1>0.0</distortionK1>
        <distortionK2>0.0</distortionK2>
        <distortionK3>0.0</distortionK3>
        <distortionT1>0.0</distortionT1>
        <distortionT2>0.0</distortionT2>
      </plugin>
    </sensor>
  </gazebo>

</robot>

此时图像为：
e14b42ff-0e07-4a28-825e-9822e1f8404d-{1AA9C12E-4555-4BD5-B756-39466DACD39E}.png
似乎好了不少，但是在图像开始区域出现了大的跳变。我以为是角速度和角加速度太大了，因此平滑一下（此处不是上面代码执行的，而是另一个MPC程序）：
c23aca60-fc09-49fc-99a8-a1d0e02d4b6d-{6DD45853-3F72-46F5-8F07-AADF30E727DF}.png
我的控制输入似乎足够平滑了，但机器人能会出现非常离谱的跳变。后面我进一步减少角速度控制的角加速度，但让然会有非常大的跳变出现，我想知道这是为什么呢？
如果这是模型问题，那我该怎么调整参数？如果是控制问题，是发生在哪里？
最后，如果是Turtlebot3这个模型不支持这么大的角加速度（即便我已经调的很小了，0.5m/s^2)，那我应该自己写一个吗，还是在找一个开源的模型。

机器人小伊

针对您在使用Turtlebot3在Gazebo中进行仿真时遇到的角速度跳变问题，我们可以从几个方面来分析和解决：

1. 仿真模型参数调整

首先，您已经对轮子的摩擦系数和物理属性进行了调整，这是一个很好的开始。然而，可能还需要进一步检查以下参数：

wheelTorque 和 wheelAcceleration：这些参数直接影响轮子的响应速度和力量。如果设置得过高，可能会导致模型在物理仿真中表现不稳定。尝试逐步降低这些值，看看是否有助于减少跳变。
仿真步长（Simulation Step Size）：Gazebo中的仿真步长（或称为时间步长）也可能影响仿真的稳定性。虽然您已经在代码中设置了较高的控制更新率（50Hz），但Gazebo的物理引擎可能以不同的步长运行。可以在Gazebo的设置中调整这个参数。

2. 控制算法

从您提供的代码来看，您似乎正在使用一个基于时间积分的控制算法来调整角速度。这种方法在理想条件下可能工作得很好，但在物理仿真中，由于存在各种不确定性和噪声，可能会导致不稳定的行为。

平滑控制输入：除了调整角加速度的增量外，还可以考虑使用更平滑的控制算法，如PID控制器或更高级的控制策略，如模型预测控制（MPC）。
限制控制输入：确保控制输入（即线性速度和角速度）在合理的物理范围内。例如，可以设置一个最大角速度限制。

3. 仿真环境

地面条件：地面的摩擦系数和不平整度也会影响仿真结果。确保地面属性（如摩擦系数）与实际情况相符。
其他物体：检查仿真环境中是否有其他可能影响Turtlebot3运动的物体，如墙壁、障碍物等。

4. 代码实现

异常处理：在odom_callback中，您已经尝试捕获并处理异常，但确保这些异常处理逻辑是完整且有效的。
数据同步：确保在发布控制命令和读取里程计数据之间没有显著的时间延迟。

5. 替换模型

如果调整以上参数和算法后问题仍未解决，可能需要考虑使用其他仿真模型。Turtlebot3的模型在大多数情况下应该足够稳定，但如果您的应用场景非常特殊或需要更高的精度，可能需要自定义模型。

结论

建议先从调整仿真参数和控制算法开始，因为这些通常是解决此类问题的最直接和有效的方法。如果这些方法都不能解决问题，再考虑替换模型或进一步深入分析物理仿真引擎的行为。在开发过程中，保持对控制输入和仿真输出的仔细监控，以便及时发现问题并进行调整。

以上为生成回答，仅供参考～

Occupy

@Occupy 我在 https://robotics.stackexchange.com/questions/113285/gazebo-simulation-with-a-fluctuating-angular-velocity 也发了提问，但没得到满意的回答。

Occupy

没有朋友知道发生的原因吗？