PID调参与自定义消息
在上文中，我们通过在Gazebo的插件中订阅一个ROS主题来控制雷达的扫描速度。 而实际控制雷达扫描速度的是一个PID控制器，很自然的我们可以认为这个控制的效果与PID控制器的参数有着直接的关系。那么，如下面的代码片段所示，当时我们在代码中直接硬编码的参数是否真的有效呢？
this->mPid = common::PID(0.1, 0, 0);
其实关于PID控制器我们并不陌生，曾今在PID控制器一文中以直流有刷电机为例介绍了它的基本原理， 以及离散化之后的数字实现方式。本文中，我们将通过系统的阶跃响应曲线来分析控制器的优劣，并简单的调参来查看比例、积分、微分三个控制项的作用。
为了方便调整控制器参数，我们提供了ROS服务和Gazebo订阅模式两种接口。也是为了研究如何在Gazebo插件中提供ROS服务，如何通过protobuf自定义Gazebo的消息类型。
1. 绘制阶跃响应曲线
所谓的阶跃响应曲线是指系统输入从0到1发生跃变后系统输出的变化曲线。一般阶跃响应曲线都长右图这样，图中横坐标表示时间， 纵坐标表示系统实际输出\(h_t\)与稳态时的期望输出\(h_{\infty}\)的比值。系统的初始状态为零，经过一段调整之后趋于稳态，达到或者接近\(h_{\infty}\)。
关于阶跃响应我们一般关注六个指标，用来评价系统的响应速度、响应过程的平稳性、以及精度：
- 延迟时间\(t_d\):表示系统的阶跃响应输出\(h_t\)达到期望输出\(h_{\infty}\)的50%所需要的时间；
- 上升时间\(t_r\):表示系统的阶跃响应输出\(h_t\)从10%上升到90%所需要的时间，有时也描述为从0上升到90%的时间；
- 峰值时间\(t_p\):表示系统的阶跃响应输出\(h_t\)超出期望输出\(h_{\infty}\)后达到第一个峰值的时间。
- 超调量\(\sigma \%\):表示系统输出的峰值\(h_{t_p}\)与期望输出\(h_{\infty}\)的偏差量与\(h_{\infty}\)的比值； $$ \sigma \% = \cfrac{h_{t_p} - h_{\infty}}{h_{\infty}} \times 100\% $$
- 调节时间\(t_s\):指系统进入稳态附近落入误差带中再也不出来的最早时间。误差带宽根据系统的精度需要自行定义，一般取期望输出的\(±5\%\)或者\(±2\%\)。
- 稳态误差\(e_{ss}\):指系统进入稳态之后与期望输出之间的偏差量。
因为Gazebo的插件都是用C++写的，我们还想在插件运行过程中绘制阶跃响应曲线，意味着我们需要有一个C++的库，能够跟着Gazebo的仿真节奏实时的绘制曲线。 由于Gazebo的资料有点少，我找了半天也没找到Gazebo是否提供了原生的接口。最后在github上看到了matplotlib-cpp， 这是一个C++对pyhon的绘图库matplotlib的一个封装，看样子挺符合我们的需求的。
确定了绘图方式之后，我们需要做的就是研究Gazebo的仿真节奏，根据例程改写我们需要的功能。
绘制雷达转速的阶跃响应曲线，本质上就是定期的对雷达转速进行采样。一开始我是想Gazebo是不是提供什么采样器或者定时器之类的东西，让我能够注册一个回调函数，在里面完成定时采样呢。 但是经过一通找没有发现这样的class，然后我就想是不是可以写一个ROS的节点，以一个固定的频率请求Gazebo插件发布关于转速的消息。总觉得这样做不是很美观，正在犹豫的时候， 看到Gazebo提供了一堆事件，其中就有一个WorldUpdateBegin，我们可以通过接口ConnectWorldUpdateBegin注册一个回调函数。每当Gazebo开始一个仿真周期的更新就会触发该事件， 那么我们就可以在这个回调函数里面完成采样绘图工作。想想都是很完美的。
如下面的代码片段所示，我们先给插件VelodynePlugin添加一个成员变量mUpdateConnection，然后在Load函数中通过ConnectWorldUpdateBegin注册回调。 我们将在回调函数ConnectWorldUpdateBegin中获取转速信息，然后使用matplotlib-cpp画图。
private: event::ConnectionPtr mUpdateConnection;
public: virtual void Load(physics::ModelPtr model, sdf::ElementPtr sdf) {
// ...
mUpdateConnection = event::Events::ConnectWorldUpdateBegin(boost::bind(&VelodynePlugin::OnWorldUpdateBegin, this));
// ...
}
由于matplotlib-cpp只需要一个头文件就可以了，它所依赖的库就是python的so文件， 所以我们直接把matplotlibcpp.h拷贝到源码目录下，并引用之。 为了防止名字太长码起来麻烦，根据例程调整一下命名空间。
#include "matplotlibcpp.h"
namespace plt = matplotlibcpp;
下面是回调函数OnWorldUpdateBegin的代码片段，在函数的一开始先获取当前的仿真时间。其中mSimTime是一个新增的成员变量，记录当前的仿真时间。
private: void OnWorldUpdateBegin() {
common::Time curTime = mWorld->SimTime();
mSimTime = curTime;
然后通过标识变量mPlotFlag来控制是否更新绘图。每当有新的速度指令进来，都会将在函数SetVelocity中将mPlotFlag赋为true。 我们还分别用了三个容器mPlotTimes, mPlotVels, mPlotCmds记录下当前的仿真时间、雷达的扫描速度、以及速度指令，用于后面调用plt::plot进行绘图，同样的它们也将在函数SetVelocity中清空。 我们希望系统能够快速的达到指定的速度，记录过多的数据是没有意义的，所以这里只记录了100个采样点。由于Gazebo的默认仿真周期是1毫秒，所以这100个采样点也就相当于是0.1秒的时间。
if (mPlotFlag) {
if (mPlotTimes.size() > 100)
mPlotFlag = false;
mPlotTimes.push_back(curTime.Double());
mPlotVels.push_back(this->mJoint->GetVelocity(0));
mPlotCmds.push_back(mVelCmd);
plt::clf();
plt::plot(mPlotTimes, mPlotVels);
plt::plot(mPlotTimes, mPlotCmds);
plt::xlim(mPlotTimes.front(), mPlotTimes.back());
plt::pause(0.001);
}
}
一开始写本系列文章的时候，我是想尽量脱离ROS的环境来介绍Gazebo的仿真，所以自己构建了一套编译过程。但很多时候还是借助ROS的工具要方便一些，所以从本文开始又切换到ROS的catkin编译环境下了， gazebo_demos将作为一个包发布。我们假设读者已经有一个称为"catkin_gazebo_ws"的ROS工作空间，并已经将gazebo_demos克隆到其中的src目录下编译成功了。 接下来我们通过运行如下的一些指令就可以得到上面右图所示的响应曲线。
$ roscd gazebo_demos
$ source ./setup.sh
$ roscore &
$ gazebo worlds/velodyne.world &
$ rostopic pub /my_velodyne/vel_cmd std_msgs/Float32 "data: 1.0"
从上面右图中的曲线中，我们可以看到控制器的精度还是可以的，基本上没有静差，而且响应速度也很快，但是有一个很大的超调量。 此时的PID参数是我们在代码中硬编码的配置，比例系数是0.1没有积分和微分环节。下面我们尝试通过ROS的服务提供调参的功能，来研究一下不同的PID参数对于系统阶跃响应的作用。
2. 通过ROS服务调参
为了能够通过ROS服务调参，我们在gazebo_demos的根目录下创建了一个srv的子目录， 并在其中编写了SetPIDParams.srv文件。至于如何将该文件导入ROS的系统中并在C++程序中应用， 这里不再赘述，读者可以参考简单的Service服务器和客户端一文。
如下面的代码片段所示，先添加一个成员变量mRosSetPIDParamSrv，然后在Load函数中构建了对象aso描述服务的名称、回调函数以及处理回调函数的队列，并最终通过该对象发布服务。
private: ros::ServiceServer mRosSetPIDParamSrv;
public: virtual void Load(physics::ModelPtr model, sdf::ElementPtr sdf) {
// ...
ros::AdvertiseServiceOptions aso = ros::AdvertiseServiceOptions::create<gazebo_demos::SetPIDParams>(
"/" + this->mModel->GetName() + "/SetPIDParams",
boost::bind(&VelodynePlugin::OnSetPIDParamSrvFromROS, this, _1, _2),
ros::VoidPtr(), &this->mRosQueue);
this->mRosSetPIDParamSrv = this->mRosNode->advertiseService(aso);
// ...
}
在下面的回调函数OnSetPIDParamSrvFromROS中，我们从服务请求中获取PID的控制参数更新对象mPid，并通过接口SetVelocityPID作用到实际的控制器上。
private: bool OnSetPIDParamSrvFromROS(gazebo_demos::SetPIDParams::Request & req, gazebo_demos::SetPIDParams::Response & res) {
mPid.SetPGain(req.P);
mPid.SetIGain(req.I);
mPid.SetDGain(req.D);
this->mModel->GetJointController()->SetVelocityPID(this->mJoint->GetScopedName(), this->mPid);
res.result = true;
return true;
}
如果一切编译正常，我们就可以通过rosservice工具直接请求服务修改控制参数。
$ rosservice call /my_velodyne/SetPIDParams "P: 0.01
I: 0.0
D: 0.0"
result: True
$ rostopic pub /my_velodyne/vel_cmd std_msgs/Float32 "data: 1.0"
右图是根据上述指令，将比例系数改为0.01后的阶跃响应曲线。从图中可以看到比例系数调小之后，系统的响应时间就明显增长了，但带来的好处就是整个控制过程更平稳，没有超调。
3. 自定义Gazebo消息调参
我们曾今介绍过Gazebo的Topics，了解到Gazebo也有和ROS类似的订阅者和发布者模式进行通信， 它们的使用套路基本都一样。都是构建节点、通过节点创建发布器或者接收器、通过回调函数来处理消息。这里我们来研究一下，如何自定义一个Gazebo的消息类型。
在Gazebo中消息类型都是通过谷歌的protobuf工具定义和实现的，参考官方的自定义消息教程， 我们首先在gazebo_demos的根目录下创建一个proto的子目录用于保存我们新建的消息文件SetPIDParamRequest.proto。接下来的工作就是在proto子目录下编写一个CMakeLists.txt文件，用于指引生成头文件和连接库。 下面是该CMakeLists.txt的代码片段，我们先查找到protobuf和gazebo相关的包。
cmake_minimum_required(VERSION 2.8.3)
find_package(Protobuf REQUIRED)
find_package(gazebo REQUIRED)
include_directories(${GAZEBO_INCLUDE_DIRS})
link_directories(${GAZEBO_LIBRARY_DIRS})
然后在Gazebo的include路径下搜索Gazebo原生的proto文件的路径。
set(PROTOBUF_IMPORT_DIRS)
foreach(ITR ${GAZEBO_INCLUDE_DIRS})
if(ITR MATCHES ".*gazebo-[0-9.]+$")
set(PROTOBUF_IMPORT_DIRS "${ITR}/gazebo/msgs/proto")
endif()
endforeach()
最后把要解析编译的proto文件添加到变量msgs中，通过PROTOBUF_GENERATE_CPP和add_library生成头文件和动态连接库libgazebo_demos_proto.so。
set (msgs SetPIDParamRequest.proto)
PROTOBUF_GENERATE_CPP(PROTO_SRCS PROTO_HDRS ${msgs})
add_library(gazebo_demos_proto SHARED ${PROTO_SRCS})
target_link_libraries(gazebo_demos_proto ${PROTOBUF_LIBRARY})
此外，我们还需要在gazebo_demos根目录下的CMakeLists.txt中添加如下的语句，用于指引编译proto子目录。 其中变量PROTO_GEN_DIR用于指引生成临时文件的目录，在该目录中我们将找到头文件SetPIDParamRequest.pb.h和源文件SetPIDParamRequest.pb.cc，以及编译之后生成的so文件。
set(PROTO_GEN_DIR ${CMAKE_CURRENT_BINARY_DIR}/proto)
include_directories(${PROTO_GEN_DIR})
link_directories(${PROTO_GEN_DIR})
add_subdirectory(proto)
最后修改插件目录下的CMakeLists.txt文件， 给对象velodyne_plugin和vel添加对库文件libgazebo_demos_proto.so文件的依赖。这样我们就可以在程序中正常使用自定义的消息了。
target_link_libraries(velodyne_plugin gazebo_demos_proto ${GAZEBO_libraries} ${catkin_LIBRARIES} ${PYTHON_LIBRARIES})
add_dependencies(velodyne_plugin gazebo_demos_proto)
target_link_libraries(vel gazebo_demos_proto ${GAZEBO_LIBRARIES})
add_dependencies(vel gazebo_demos_proto)
在程序中，我们需要先给插件VelodynePlugin添加一个Gazebo的消息订阅器，同时在Load函数中完成订阅和回调函数的注册，如下面代码片段所示。
#include <SetPIDParamRequest.pb.h>
class VelodynePlugin : public ModelPlugin {
// ...
private: gazebo::transport::SubscriberPtr mSetPIDParamSub;
public: virtual void Load(physics::ModelPtr model, sdf::ElementPtr sdf) {
// ...
mSetPIDParamSub = this->mGazeboNode->Subscribe("~/my_velodyne/set_pid_param", &VelodynePlugin::OnSetPIDParamMsg, this);
// ...
}
// ...
}
下面是回调函数OnSetPIDParamMsg的实现，其函数内容没什么好讲的，重点在它的参数的类型上。类型SetPIDParamRequestPtr实际上并没有在生成的头文件SetPIDParamRequest.pb.h中有定义， 而是我们通过typedef定义的一个智能指针。
typedef const boost::shared_ptr<const gazebo_demos_proto::msgs::SetPIDParamRequest> SetPIDParamRequestPtr;
private: void OnSetPIDParamMsg(SetPIDParamRequestPtr & msg) {
gazebo_demos_proto::msgs::SetPIDParamRequest request;
mPid.SetPGain(msg->pgain());
mPid.SetIGain(msg->igain());
mPid.SetDGain(msg->dgain());
this->mModel->GetJointController()->SetVelocityPID(this->mJoint->GetScopedName(), this->mPid);
}
我们还提供了一个简单的消息发布器vel.cc，它就只有一个main函数内容也很简单，这里不再细述了。 编译通过之后，我们就可以通过运行生成的可执行文件vel来调整控制器的参数。
4. 完
本文中，我们介绍了阶跃响应的意义和经常关注的一些指标。选用matplotlib-cpp进行实时的画图，demo中有一点缺陷就是这个画图过程会影响仿真的速度， 而且弹出的GUI界面除了显示个曲线之外所有的交互功能都是不起作用的。一种解决方法就是在创建一个线程，在这个线程中完成绘图和GUI界面的更新。但毕竟这个工具还不是很熟， 有可能它已经提供了解决方案，这个以后再研究。
然后，我们介绍了如何在插件中注册ROS的服务，并简单的通过该接口调整了一下PID参数，可以从曲线上明显的看到不同的参数对系统的影响。 最后，我们还研究了一下如何自定义Gazebo的消息，主要分析了与之相关的CMake文件，指导如何根据proto文件生成头文件和动态连接库，并在实际的程序中使用。