ROS实验 | PID控制做了什么？

各位ROS与机器人技术爱好者们，大家好！我是小明，很荣幸受古月君的邀请，成为古月居的签约作者，相信以后我们会经常见面的。

联想到刚上手ROS时，心中始终充斥着一个困惑：初步了解ROS系统后，该如何实践书本上的理论知识呢？于是我决定将“ROS实验”作为本文的主题。顾名思义，通过“做实验”，我们既能熟悉ROS的基本功能，又能加深对机器人学理论知识的理解。在文末我分享了实验项目的源代码，便于大家亲自实践，实验使用ubuntu 18.04，ROS melodic。

在与小伙伴们的日常沟通中，我发现“仿真”是大家学习ROS的一个重要需求，而Gazebo仿真软件和Gazebo_ros_control等插件无疑是很好上手的选择。想必大家记得，在使用ros_control插件时，我们在yaml文件中设置了控制器类型、关节名称和PID等参数：

 joint1_position_controller:      type: effort_controllers/JointPositionController      joint: joint_1      pid: {p: 100, i: 0.01, d: 1.0, i_clamp_max: 0.01}

那么，该控制器究竟是如何作用于机器人的呢？如果能明白这一点，将有利于我们更加合理地进行仿真。

1、 机器人小S

假设有一个单关节的机器人小S，它的结构和连杆1的参数如图：

根据上述参数，可以建立小S的仿真模型。将s_arm功能包复制到ros工作空间的src目录下，编译工作空间，启动Gazebo查看该模型：

 $ cd ~/catkin_ws  $ catkin_make  $ roslaunch s_arm s_arm_gazebo.launch

2 、PID控制策略

根据经典控制理论，系统要有明确的输入和输出。例如，对小S进行位置（点位）控制，那么系统的输入量就是目标关节位置 θ_d​ ，输出量就是当前关节位置​ θ 。由于小S是由电机驱动的，而电机本身并不直接识别位置信号，而是通过电流来控制力矩【注1】。于是我们需要一个控制策略，将输入量和输出量转化为所需的关节力矩信号 ​τ ，如图所示。

  2.1 比例控制 P  

最直观的策略是将输入量和输出量作差，得到误差 θ_e​ = θ_d – θ  。再令关节力矩与该误差成正比：

这里面的逻辑挺好理解：假如当前的位置还有误差，那么电机持续将输出力矩使关节转动，误差的大小决定了力矩的大小，直到这个误差被消除。这就是P控制，又叫比例控制，系数​ K_p 称为比例增益。

为了测试该策略是否有效，打开s_arm/config/s_arm_control.yaml文件，将 PID 控制器的 p 增益参数设置为100.0【注2】，其余参数为0.0：

 pid: {p: 100.0, i: 0.0, d: 0.0}

打开终端，运行启动文件：

 $ roslaunch s_arm s_arm_gazebo.launch

在Gazebo仿真界面Ctrl+P打开绘图工具，绘制MODELS/arm/joint_1/Position/Axis 0的图像，以监测小S关节的位置。接着新开一个终端来发布控制消息，目标位置-1.5707：

 $ rostopic pub /s_arm/joint1_position_controller/command std_msgs/Float64 "data: -1.5707"

从监测图像看到，关节位置很快便到达了期望值：

那么，机器人是不是只用P控制就可以了呢？我们再发布一条控制消息，目标位置0.0：

 $ rostopic pub /s_arm/joint1_position_controller/command std_msgs/Float64 "data: 0.0"

问题出现了，机器人并没有停到我们期望的位置上，使用rostopic echo查看话题，发现存在约0.098rad的误差。这是为什么呢？

  2.2 积分控制 I  

假设小S的运动需要克服惯性力、阻尼力和重力，对应的动力学方程为：

代入P控制的表达式，且机器人处于稳定状态，即 ​：

绘制 θ_e​​ 关于 θ​ 的图像，可看到除非 cosθ = 0​，否则无论如何 ​θ_e​ 都不为0，该误差称为稳态误差，是由重力引起的。代入上面的实验数据（目标位置为-1.5707时，稳态误差约0；目标位置为0.0时，稳态误差约0.098），也可以验证该结论。

消除该稳态误差的一种方法【注3】是，为控制策略添加一项如下：

该项对系统的误差进行了累加（积分）。即，只要误差持续地存在，力矩就会不断增大，直至误差完全消除。这就是PI控制，又叫比例-积分控制，系数 K_i​ 称为积分增益。

将PID参数修改如下（积分增益为500.0，并设置i_clamp_max，以解除对积分增益的限制）

 pid: {p: 100.0, i: 500.0, d: 0.0, i_clamp_max: 500.0}

再次重复上面的操作，发布控制消息，目标位置0.0

 $ rostopic pub /s_arm/joint1_position_controller/command std_msgs/Float64 "data: 0.0"

看到，先前存在的稳态误差已经被消除了，系统稳定在目标位置附近。但同时，图像出现了明显的振荡，这又是为什么呢？

  2.3 微分控制 D  

通过加入积分项，我们消除了稳态误差，但也引入了另一个问题：在误差消除之前，积分项所累积的力矩可能已经超过了实际需求。这样，系统只能运动到超过目标位置后，通过“负误差”的累积来减掉多余的力矩。更糟糕的是，这种调整可能反复出现，体现在图像上，就是系统在目标位置附近反复振荡。实际上，前面的比例控制也引发了类似的效应，这种振荡削弱了系统的稳定性。

为了解决这一问题，我们继续为控制策略添加一项如下：

可以看到，新添项与稳态误差的导数成正比。这意味着，当机器人接近目标位置的过程中，该项将从总力矩中“扣除”一定的力矩（因为稳态误差在减小，导数为负）。接近得越快，稳态误差的导数的绝对值越大，“扣除”的力矩也就越大。这种“扣除作用”提前减小了力矩，从而缓解了到达目标时的冲击。这就是PID控制，又叫比例-积分-微分控制，系数 ​K_d 称为微分增益。

但万万要小心，这种作用是有风险的：高频噪声也会使导数增大，即微分作用可能会放大噪声，反而使系统的不稳定。

将PID参数修改如下（添加微分增益为10.0）

 pid: {p: 100.0, i: 500.0, d: 10.0, i_clamp_max: 500.0}

再次重复上面的操作，发布控制消息，目标位置0.0

 $ rostopic pub /s_arm/joint1_position_controller/command std_msgs/Float64 "data: 0.0"

可以看到，运动的振荡被明显削弱了。

总结

本次实验，我们利用ROS-Gazebo仿真工具探索了PID在机器人控制中的作用，最后用一张表格总结如下。小明水平有限，不足之处还望多多包涵！

【注1】 实际上，采用步进电机驱动时，也可以使用速度或位置作为作用信号。而查看gazebo_ros_control插件目录下的src/default_robot_hw_sim.cpp，POSITION_PID情况下调用了Gazebo的SetForce()接口，因此该插件的确是以力/力矩作为作用信号的

【注2】 本实验的参数选择只为凸显结果，不代表具有合理性

【注3】 后文提到，积分控制可能会削弱稳定性，所以另有方法也可消除稳态误差，例如重力补偿的PD控制等

项目地址

https://github.com/XM522706601/XM_Studio-guyuehome.git