源码解读 | Gazebo仿真控制中,有哪些你不知道的秘密?

1150
4
2020年4月12日 15时48分

大家好,我是(月更侠)小明,很高兴我们又见面啦。相信大家从我之前的文章中能看出来,我所讲的内容基本都面向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控制插件

 

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

 

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

发表评论

后才能评论

评论列表(4条)

  • Sean 2020年5月11日 下午5:22

    小明你好,我最初下载的gazebo_ros_pkgs在默认安装目录,但是没有找到default_robot_hw_sim.cpp文件。然后我又在工作目录下载了gazebo_ros_pkgs,可以找到default_robot_hw_sim.cpp文件。现在我创建了PID动态调整插件,不知道后续怎么调用,看到请回复一下,谢谢啦~

  • 9p1rw_6206 2020年4月26日 下午4:36

    你好,我想问一下gazebo里面有没有什么实时控制机械臂运动的方法,比如实时发送关节力矩或者关节速度,我试了gazebo_ros_control的jointvelocitycontrollers感觉不太行,控制效果很差

  • 9p1rw_6206 2020年4月14日 下午5:58

    你好,文章里面提到的位置PID、速度PID和不用PID可以怎样设置呢,默认都是位置PID吗?我想直接对gazebo中的机械臂进行速度控制,gazebo有现成的topic接口吗,还是只能在编程的时候使用SetVelocity()接口

    • 小明工坊 回复 9p1rw_6206 2020年4月14日 下午9:02

      在配置gazebo_ros_control插件时,选择相应的控制器即可。具体来说,就是将transmission和yaml中的控制器type改为速度控制即可,详见http://gazebosim.org/tutorials/?tut=ros_control或古月居的早期相关文章。如果使用gazebo_ros_control,就不需要调用gazebo的接口。