大家好，我是（月更侠）小明，很高兴我们又见面啦。相信大家从我之前的文章中能看出来，我所讲的内容基本都面向ROS新人，因为大家刚入门时，往往很难一窥程序的深层运行逻辑，使得许多问题迟迟无法解决，甚是耽误时间。例如在和小伙伴们日常交流时，就反复遇到过一些关于Gazebo仿真控制的“经典”问题：

 

A 我的模型一直抖动，怎么办？

B PID参数该怎么设置啊？我怎么调都没用！

C 我必须要设置PID参数吗？如果不设置会怎么样

D gazebo_ros_control插件可以完成计算力矩控制等更加复杂的控制策略吗？

E 我的并联机器人模型只能用sdf创建，可以使用gazebo_ros_control进行控制吗？

......

 

通过本篇文章的分析，我们将揭晓这些问题的答案。

 

在Gazebo仿真中，我们最常使用的控制插件就是是gazebo_ros_control，它是ros_control针对Gazebo仿真开发的插件。

图1

图1 ros_control组成概览，其中左上角方框内就是gazebo_ros_control插件所实现的功能 图片来源

 

该插件的详细使用方法，可参考古月居ROS探索总结系列的相关文章。简单来说，就是进行了如下三个操作：

 

1 在机器人模型的URDF文件中，添加gazebo_ros_control插件，并添加transmission指明了各关节控制器的类型信息

2 建立一个yaml文件，用来定义各控制器的pid参数

3 建立一个roslaunch，用来加载模型、控制参数、控制器，并发布tf

 

1 文件结构

 

接下来我们来看看gazebo_ros_control功能包究竟对这些文件做了什么。需要注意，gazebo_ros_control源码已经集成到了github帐号ros-simulation下的gazebo_ros_pkgs项目中，而ros_controls账号下的gazebo_ros_pkgs是旧版本，现在已经不再使用了。源码地址：gazebo_ros_control 

 

该功能包的核心是硬件接口文件src/default_robot_hw_sim.cpp，该文件定义了以下函数，用以实现相应功能：

clamp()：区间限定函数，用以规范某些变量的上下限

DefaultRobotHWSim::initSim()：初始化插件的各项属性

DefaultRobotHWSim::readSim()：从Gazebo仿真器中读取模型的信息和状态

DefaultRobotHWSim::writeSim()：将目标值写入Gazebo仿真器中

 

下面我们分析几个重点函数。

 

DefaultRobotHWSim::initSim()函数初始化了整个插件，首先处理了URDF文件中transmissions模块信息，主要是以下方面：

 

1 定义了机器人的自由度数

2 从transmissions中获取各个关节接口类型，名称

3 判断是否使用PID控制，如果没有设置PID参数，则视为不使用PID

 

并对模型进行了以下处理：

 

1 从gazebo模型中获取关节名称，并实例化各关节

2 为各个关节gazebo模型的行程设置上下限

 

读取函数readSim()和写入函数writeSim()，分别实现以下功能：

 

1 从gazebo仿真器中读取个关节的当前状态

2 根据控制器类型将控制量写入仿真器

 

2 控制代码

 

我们看看各类型的控制方法具体是如何实现的

 

1 力控制

 

力控制十分简单，直接将目标量写入了仿真器，其中SetForce()函数是Gazebo仿真器自带的关节力设置函数。

case EFFORT:
{
    const double effort = e_stop_active_ ? 0 : joint_effort_command_[j];
    sim_joints_[j]->SetForce(0, effort);
}
break;

 

2 PID位置控制

 

PID位置控制稍显复杂，首先计算当前状态下的位置误差error，其中对于REVOLUTE（转动副）类型的关节，还要考虑关节限位。再利用pid_controllers下的computeCommand()方法计算当前误差下应当施加到关节上的力，并使用区间限定函数clamp()来规范力的上下限。从此处可以看出，gazebo_ros_control插件实际上使用了输入力的PID位置控制，该方法的原理可见我在古月居发表的第一篇文章。最后，利用SetForce()函数将目标量写入了仿真器。

case POSITION_PID:
{
    double error;
    switch (joint_types_[j])
    {
        case urdf::Joint::REVOLUTE:
            angles::shortest_angular_distance_with_limits(joint_position_[j],
                                                          joint_position_command_[j],
                                                          joint_lower_limits_[j],
                                                          joint_upper_limits_[j],
                                                          error);
            break;

        case urdf::Joint::CONTINUOUS:
            error = angles::shortest_angular_distance(joint_position_[j],
                                                      joint_position_command_[j]);
            break;
 
        default:
            error = joint_position_command_[j] - joint_position_[j];
    }
 
    const double effort_limit = joint_effort_limits_[j];
    const double effort = clamp(pid_controllers_[j].computeCommand(error, period),
                                -effort_limit, effort_limit);
    sim_joints_[j]->SetForce(0, effort);
}
 
break;

 

3 PID速度控制

 

PID速度控制函数和PID位置控制函数的逻辑完全一致，唯一的区别是误差error取的是速度误差。

case VELOCITY_PID:
    double error;
    if (e_stop_active_)
        error = -joint_velocity_[j];
    else
        error = joint_velocity_command_[j] - joint_velocity_[j];
    const double effort_limit = joint_effort_limits_[j];
    const double effort = clamp(pid_controllers_[j].computeCommand(error, period),
                                -effort_limit, effort_limit);
    sim_joints_[j]->SetForce(0, effort);
    break;

 

4 非PID的位置控制和速度控制

 

如果不使用PID控制，那么硬件会直接调用Gazebo的SetPosition()和SetVelocity()接口，强行设置关节的位置和速度，而非通过输入力的方式进行控制。代码分别位于320行到330行，以及359行到372行。

 

3 答案揭晓

 

根据源码的分析，我们在Gazebo仿真中的很多“万年难题”就迎刃而解啦！例如：

 

A 我的模型在仿真时一直抖动，怎么办？

 

答：在保证模型惯量属性、关节摩擦、阻尼合理的前提下，调整PID参数。从源码看到，控制器的PID控制实际上是输入力的PID控制，模型物理属性不符合真实情况、PID参数不合理必然导致关节振荡。这也提醒我们，良好的Gazebo仿真，其前提是真实的物理模型和合理的理论推导。务必不要将其当做纯动画制作软件！

 

图2

 

图3

图2 图3 由于质量属性\PID参数不合理，机械臂发生了细微的或鬼畜般的抖动。 图片来源

 

B PID参数该怎么设置啊？我怎么调都没用！

 

答：自己努力吧孩子！还是如上一个问题所述，在仿真之前，先进行完整的控制理论推导，而非随便尝试。提供两个小技巧：动态配置插件Dynamic Reconfigure可以方便地动态调整PID参数，如图。另外尤其要注意一个问题：gazebo_ros_control对于PID中的积分增益i是默认锁定为0的，要想使用积分控制，需要在yaml文件中打开积分增益的上下限，否则无论怎么调积分增益都没有用的（坑...）。

  # pid增益的正确配置姿势(如要使用积分控制，必须打开积分增益的上下限，否则默认均为0)
  joint1_position_controller:
    type: effort_controllers/JointPositionController
    joint: joint_1
    pid: {p: 100.0, i: 1.0, d: 10.0, ,i_clamp_max: 0.0, i_clamp_max: 1.0}

 

图4

图4 PID参数动态配置插件Dynamic Reconfigure

 

C 我必须要设置PID参数吗？如果不设置会怎么样

 

答：不是必要的，但推荐在yaml中设置PID参数。从源码中可以看到，gazebo_ros_control插件会自动判断用户是否在yaml文件中设置了PID参数。如果没有，当用户使用位置控制和速度控制时，就会调用Gazebo默认的位置和速度设置接口。而根据注释信息可以看到，如果你的Gazebo版本小于9，这样做就是有坏处的：仿真器不会模拟模型的重力，会导致动力学仿真异常。因此，我们最好还是在yaml文件中设置合理的PID参数。

ROS_WARN_ONCE("The default_robot_hw_sim plugin is using the Joint::SetPosition method without preserving the link velocity.");
 
ROS_WARN_ONCE("As a result, gravity will not be simulated correctly for your model.");
 
ROS_WARN_ONCE("Please set gazebo_pid parameters, switch to the VelocityJointInterface or EffortJointInterface, or upgrade to Gazebo 9.");
 
ROS_WARN_ONCE("For details, see https://github.com/ros-simulation/gazebo_ros_pkgs/issues/612");

 

图5

图5 在gazebo9以前的版本中，一只手由于没有设置PID参数导致重力异常，悬浮在空中。图片来源

 

D gazebo_ros_control插件可以完成计算力矩控制等更加复杂的控制策略吗？

 

答：需要基于插件的自定义接口自己编写程序。从源码中看到，插件所提供的控制策略只有基本的PID控制，其输入量的计算只考虑了误差，并没有考虑任何动力学模型，详见control_toolbox项目下的src/pid.cpp文件。

 

E 我的并联机器人模型只能用sdf创建，可以使用gazebo_ros_control进行控制吗？

 

答：不可以。从源码看出，控制器类型等相关信息必须从urdf下的transmissions获取，甚至在注册关节限位时也使用了urdf类，而并没有支持sdf格式。不过，由于sdf是Gazebo默认的支持格式，有完整的官方文档提供，因此根据gazebo_ros_control的逻辑重新创建一个适用于sdf的控制插件其实并不困难。

 

图6

图6 笔者利用sdf创建的六自由度并联机器人，使用自定义Gazebo控制插件

 

以上就是我们从源码中找到的答案，有没有解决你的一些疑惑呢？

 

如果你还想进一步交流，欢迎在这篇文章下给我留言。本次分享就到这里啦，期待我们下一次见面~