# 10.控制速度-PID控制器实现

你好，我是小鱼。上一节我们通过编码器完成了对机器人单个轮子的速度测量，完成了电机速度闭环控制的重要一步-反馈。

![image-20230305020838403](10.%E6%8E%A7%E5%88%B6%E9%80%9F%E5%BA%A6-PID%E6%8E%A7%E5%88%B6%E5%99%A8%E5%AE%9E%E7%8E%B0/imgs/image-20230305020838403.png)

有了反馈，接着我们需要设计一个控制器来帮助我们实现这个需求，这个控制器的输入是当前的速度和目标速度，输出是应该给到电机的PWM占空比。

## 一、PID控制器介绍

PID控制器是一种广泛应用于工业控制、自动化控制等领域的控制算法，其名称来源于“比例-积分-微分”三个控制器参数，即Proportional（比例）、Integral（积分）、Derivative（微分）。

PID控制器的基本原理是通过测量目标系统的反馈信号和期望输出信号之间的误差，根据一定的数学模型计算出控制信号，使目标系统能够稳定地达到期望输出。具体来说，PID控制器的计算公式为：
$$
\text{Output} = K_p \cdot \text{Error} + K_i \cdot \int\text{Error dt} + K_d \cdot \frac{\text{d(Error)}}{\text{dt}}
$$


其中，Kp、Ki和Kd分别表示比例系数、积分系数和微分系数，Error表示目标系统的误差，Integral(Error)表示误差的积分，Derivative(Error)表示误差的微分。

在PID控制器中，比例系数、积分系数和微分系数的选取是关键，需要根据具体的控制需求进行调整。比例系数主要影响系统的响应速度和稳定性，积分系数主要影响系统的稳态误差，而微分系数主要影响系统的抗干扰性能。

说了理论你可能不是很理解，没关系，写完代码和调参后你感受就会变的深刻起来。



## 二、新建工程搭建框架

### 2.1 新建工程

`example27_pid_controller`

![image-20230305023258123](10.%E6%8E%A7%E5%88%B6%E9%80%9F%E5%BA%A6-PID%E6%8E%A7%E5%88%B6%E5%99%A8%E5%AE%9E%E7%8E%B0/imgs/image-20230305023258123.png)


修改`platformio.ini`配置，添加开源库和microros配置

```ini
[env:featheresp32]
platform = espressif32
board = featheresp32
framework = arduino
board_microros_transport = wifi
board_microros_distro = humble
lib_deps = 
	https://gitee.com/ohhuo/micro_ros_platformio.git
	https://github.com/fishros/Esp32McpwmMotor.git
	https://github.com/fishros/Esp32PcntEncoder.git
```

### 2.2 添加PidController

在`lib`下新建`PidController`文件夹，并在`PidController`下新建`PidController.h`和`PidController.cpp`

最终目录结构

```
.
├── include
│   └── README
├── lib
│   ├── PidController
│   │   ├── PidController.cpp
│   │   └── PidController.h
│   └── README
├── platformio.ini
├── src
│   └── main.cpp
└── test
    └── README

5 directories, 7 files
```

### 2.3 复制并修改代码

将之前遥控车的代码复制粘贴到当前的main函数中，同时

- 添加`PidController.h`的头文件
- 删除原有的控制逻辑
- 添加电机速度测量函数
- 修改为双核通信
- 添加了一些注释

最终代码如下

```cpp
#include <Arduino.h>
#include <micro_ros_platformio.h>    // 包含用于 ESP32 的 micro-ROS PlatformIO 库
#include <WiFi.h>                    // 包含 ESP32 的 WiFi 库
#include <rcl/rcl.h>                 // 包含 ROS 客户端库 (RCL)
#include <rclc/rclc.h>               // 包含用于 C 的 ROS 客户端库 (RCLC)
#include <rclc/executor.h>           // 包含 RCLC 执行程序库，用于执行订阅和发布
#include <geometry_msgs/msg/twist.h> // 包含 ROS2 geometry_msgs/Twist 消息类型
#include <Esp32PcntEncoder.h>        // 包含用于计数电机编码器脉冲的 ESP32 PCNT 编码器库
#include <Esp32McpwmMotor.h>         // 包含使用 ESP32 的 MCPWM 硬件模块控制 DC 电机的 ESP32 MCPWM 电机库
#include <PidController.h>           // 包含 PID 控制器库，用于实现 PID 控制

Esp32PcntEncoder encoders[2];      // 创建一个长度为 2 的 ESP32 PCNT 编码器数组
rclc_executor_t executor;          // 创建一个 RCLC 执行程序对象，用于处理订阅和发布
rclc_support_t support;            // 创建一个 RCLC 支持对象，用于管理 ROS2 上下文和节点
rcl_allocator_t allocator;         // 创建一个 RCL 分配器对象，用于分配内存
rcl_node_t node;                   // 创建一个 RCL 节点对象，用于此基于 ESP32 的机器人小车
rcl_subscription_t subscriber;     // 创建一个 RCL 订阅对象，用于订阅 ROS2 消息
geometry_msgs__msg__Twist sub_msg; // 创建一个 ROS2 geometry_msgs/Twist 消息对象
Esp32McpwmMotor motor;             // 创建一个 ESP32 MCPWM 电机对象，用于控制 DC 电机
float out_motor_speed[2];          // 创建一个长度为 2 的浮点数数组，用于保存输出电机速度
float current_speeds[2];           // 创建一个长度为 2 的浮点数数组，用于保存当前电机速度

void twist_callback(const void *msg_in)
{
    const geometry_msgs__msg__Twist *twist_msg = (const geometry_msgs__msg__Twist *)msg_in;
    float linear_x = twist_msg->linear.x;   // 获取 Twist 消息的线性 x 分量
    float angular_z = twist_msg->angular.z; // 获取 Twist 消息的角度 z 分量
    if (linear_x == 0 && angular_z == 0)    // 如果 Twist 消息没有速度命令
    {
        motor.updateMotorSpeed(0, 0); // 停止第一个电机
        motor.updateMotorSpeed(1, 0); // 停止第二个电机
        return;                       // 退出函数
    }
}

// 这个函数是一个后台任务，负责设置和处理与 micro-ROS 代理的通信。
void microros_task(void *param)
{
  // 设置 micro-ROS 代理的 IP 地址。
  IPAddress agent_ip;
  agent_ip.fromString("192.168.2.105");
  
  // 使用 WiFi 网络和代理 IP 设置 micro-ROS 传输层。
  set_microros_wifi_transports("fishbot", "12345678", agent_ip, 8888);
  
  // 等待 2 秒，以便网络连接得到建立。
  delay(2000);
  
  // 设置 micro-ROS 支持结构、节点和订阅。
  allocator = rcl_get_default_allocator();
  rclc_support_init(&support, 0, NULL, &allocator);
  rclc_node_init_default(&node, "esp32_car", "", &support);
  rclc_subscription_init_default(
      &subscriber,
      &node,
      ROSIDL_GET_MSG_TYPE_SUPPORT(geometry_msgs, msg, Twist),
      "/cmd_vel");
  
  // 设置 micro-ROS 执行器，并将订阅添加到其中。
  rclc_executor_init(&executor, &support.context, 1, &allocator);
  rclc_executor_add_subscription(&executor, &subscriber, &sub_msg, &twist_callback, ON_NEW_DATA);
  
  // 循环运行 micro-ROS 执行器以处理传入的消息。
  while (true)
  {
    delay(100);
    rclc_executor_spin_some(&executor, RCL_MS_TO_NS(100));
  }
}

// 这个函数根据编码器读数更新两个轮子速度。
void update_speed()
{
  // 初始化静态变量以存储上一次更新时间和编码器读数。
  static uint64_t last_update_time = millis();
  static int64_t last_ticks[2];
  
  // 获取自上次更新以来的经过时间。
  uint64_t dt = millis() - last_update_time;
  if (dt == 0)
    return;
  
  // 获取当前的编码器读数并计算当前的速度。
  int32_t pt[2];
  pt[0] = encoders[0].getTicks() - last_ticks[0];
  pt[1] = encoders[1].getTicks() - last_ticks[1];
  current_speeds[0] = float(pt[0] * 0.1051566) / dt * 1000;
  current_speeds[1] = float(pt[1] * 0.1051566) / dt * 1000;
  
  // 更新上一次更新时间和编码器读数。
  last_update_time = millis();
  last_ticks[0] = encoders[0].getTicks();
  last_ticks[1] = encoders[1].getTicks();
}

void setup()
{
  // 初始化串口通信，波特率为115200
  Serial.begin(115200);
  // 将两个电机分别连接到引脚22、23和12、13上
  motor.attachMotor(0, 22, 23);
  motor.attachMotor(1, 12, 13);
  // 在引脚32、33和26、25上初始化两个编码器
  encoders[0].init(0, 32, 33);
  encoders[1].init(1, 26, 25);
  // 在核心0上创建一个名为"microros_task"的任务，栈大小为10240
  xTaskCreatePinnedToCore(microros_task, "microros_task", 10240, NULL, 1, NULL, 0);
}

void loop()
{
  // 更新电机速度
  update_speed();
  // 更新电机0和电机1的速度值
  motor.updateMotorSpeed(0, out_motor_speed[0]);
  motor.updateMotorSpeed(1, out_motor_speed[1]);
  // 延迟10毫秒
  delay(10);
}
```

## 三、PID控制器代码实现

接着我们来编写`PidController.h`和`PidController.cpp`。

### 3.1 `PidController.h`

```cpp
#ifndef __PIDCONTROLLER_H__ // 如果没有定义__PIDCONTROLLER_H__
#define __PIDCONTROLLER_H__ // 定义__PIDCONTROLLER_H__

class PidController
{ // 定义一个PID控制器类
public:
    PidController() = default;                   // 默认构造函数
    PidController(float kp, float ki, float kd); // 构造函数，传入kp、ki、kd

public:
    float target_;      // 目标值
    float out_mix_;     // 输出下限
    float out_max_;     // 输出上限
    float kp_;          // 比例系数
    float ki_;          // 积分系数
    float kd_;          // 微分系数
    float last_output_; // 上一次输出值
    // pid
    float error_sum_;           // 误差累积和
    float derror_;              // 误差变化率
    float error_pre_;           // 上上次误差
    float error_last_;          // 上一次误差
    float intergral_up_ = 2500; // 积分上限

public:
    float update(float control);                   // 更新输出值
    void reset();                                  // 重置PID控制器
    void update_pid(float kp, float ki, float kd); // 更新PID系数
    void update_target(float target);              // 更新目标值
    void out_limit(float out_mix, float out_max);  // 输出限制
};

#endif // __PIDCONTROLLER_H__ // 结束条件
```

定义`PidController`，提供五个函数。

- update(control): 传入当前控制量control并返回PID控制器的输出值
- reset(): 将PID控制器的状态重置为初始状态
- update_pid(kp, ki, kd): 更新PID控制系数
- update_target(target): 更新目标值
- out_limit(out_mix, out_max): 输出限制

### 3.2 `PidController.cpp`

```cpp
#include "PidController.h"
#include "Arduino.h"

PidController::PidController(float kp, float ki, float kd)
{
    reset(); // 初始化控制器
    update_pid(kp, ki, kd); // 更新PID参数
}

float PidController::update(float control)
{
    // 计算误差及其变化率
    float error = target_ - control; // 计算误差
    derror_ = error_last_ - error; // 计算误差变化率
    error_last_ = error;

    // 计算积分项并进行积分限制
    error_sum_ += error;
    if (error_sum_ > intergral_up_)
        error_sum_ = intergral_up_;
    if (error_sum_ < -1 * intergral_up_)
        error_sum_ = -1 * intergral_up_;

    // 计算控制输出值
    float output = kp_ * error + ki_ * error_sum_ + kd_ * derror_;

    // 控制输出限幅
    if (output > out_max_)
        output = out_max_;
    if (output < out_mix_)
        output = out_mix_;

    // 保存上一次的控制输出值
    last_output_ = output;

    return output;
}

void PidController::update_target(float target)
{
    target_ = target; // 更新控制目标值
}

void PidController::update_pid(float kp, float ki, float kd)
{
    reset(); // 重置控制器状态
    kp_ = kp; // 更新比例项系数
    ki_ = ki; // 更新积分项系数
    kd_ = kd; // 更新微分项系数
}

void PidController::reset()
{
    // 重置控制器状态
    last_output_ = 0.0f; // 上一次的控制输出值
    target_ = 0.0f; // 控制目标值
    out_mix_ = 0.0f; // 控制输出最小值
    out_max_ = 0.0f; // 控制输出最大值
    kp_ = 0.0f; // 比例项系数
    ki_ = 0.0f; // 积分项系数
    kd_ = 0.0f; // 微分项系数
    error_sum_ = 0.0f; // 误差累计值
    derror_ = 0.0f; // 误差变化率
    error_last_ = 0.0f; // 上一次的误差值
}

void PidController::out_limit(float out_mix, float out_max)
{
    out_mix_ = out_mix; // 控制输出最小值
    out_max_ = out_max; // 控制输出最大值
}
```

上面这段代码是用于实现一个PID控制器的C++代码。PID控制器是一种常用的控制器，它的输入是控制系统的误差信号，输出是控制器的控制量。PID控制器由比例项、积分项和微分项三个部分组成，这三个部分的系数可以通过调节来实现控制器的性能优化。

以下是代码中各部分的注释：

- `PidController::PidController(float kp, float ki, float kd)`：PID控制器的构造函数，用于初始化控制器状态并更新PID参数。
- `void PidController::update_target(float target)`：用于更新控制器的目标值。
- `void PidController::update_pid(float kp, float ki, float kd)`：用于更新控制器的PID参数。
- `void PidController::out_limit(float out_mix, float out_max)`：用于限制控制器的控制输出范围。
- `float PidController::update(float control)`：控制器的核心函数，用于根据当前控制量计算出下一时刻的控制量。具体实现包括以下步骤：
  - 计算误差及其变化率；
  - 计算积分项并进行积分限制；
  - 计算控制输出值，并进行输出限幅；
  - 保存上一次的控制输出值。
- `void PidController::reset()`：用于重置控制器的状态。包括重置PID参数、目标值、控制输出范围等状态变量。

在代码实现中，`float`代表浮点数类型，在C++中用于表示实数。`kp_`、`ki_`、`kd_`分别代表PID控制器中的比例项系数、积分项系数、微分项系数。`target_`代表控制器的目标值，`out_mix_`和`out_max_`用于限制控制器的控制输出范围。`error_sum_`代表误差累计值，`derror_`代表误差变化率，`error_last_`代表上一次的误差值。`last_output_`保存上一次的控制输出值。

## 四、修改主程序

```cpp
#include <Arduino.h>
#include <micro_ros_platformio.h>    // 包含用于 ESP32 的 micro-ROS PlatformIO 库
#include <WiFi.h>                    // 包含 ESP32 的 WiFi 库
#include <rcl/rcl.h>                 // 包含 ROS 客户端库 (RCL)
#include <rclc/rclc.h>               // 包含用于 C 的 ROS 客户端库 (RCLC)
#include <rclc/executor.h>           // 包含 RCLC 执行程序库，用于执行订阅和发布
#include <geometry_msgs/msg/twist.h> // 包含 ROS2 geometry_msgs/Twist 消息类型
#include <Esp32PcntEncoder.h>        // 包含用于计数电机编码器脉冲的 ESP32 PCNT 编码器库
#include <Esp32McpwmMotor.h>         // 包含使用 ESP32 的 MCPWM 硬件模块控制 DC 电机的 ESP32 MCPWM 电机库
#include <PidController.h>           // 包含 PID 控制器库，用于实现 PID 控制

Esp32PcntEncoder encoders[2];      // 创建一个长度为 2 的 ESP32 PCNT 编码器数组
rclc_executor_t executor;          // 创建一个 RCLC 执行程序对象，用于处理订阅和发布
rclc_support_t support;            // 创建一个 RCLC 支持对象，用于管理 ROS2 上下文和节点
rcl_allocator_t allocator;         // 创建一个 RCL 分配器对象，用于分配内存
rcl_node_t node;                   // 创建一个 RCL 节点对象，用于此基于 ESP32 的机器人小车
rcl_subscription_t subscriber;     // 创建一个 RCL 订阅对象，用于订阅 ROS2 消息
geometry_msgs__msg__Twist sub_msg; // 创建一个 ROS2 geometry_msgs/Twist 消息对象
Esp32McpwmMotor motor;             // 创建一个 ESP32 MCPWM 电机对象，用于控制 DC 电机
float out_motor_speed[2];          // 创建一个长度为 2 的浮点数数组，用于保存输出电机速度
float current_speeds[2];           // 创建一个长度为 2 的浮点数数组，用于保存当前电机速度
PidController pid_controller[2];   // 创建PidController的两个对象

void twist_callback(const void *msg_in)
{
  const geometry_msgs__msg__Twist *twist_msg = (const geometry_msgs__msg__Twist *)msg_in;
  float linear_x = twist_msg->linear.x;   // 获取 Twist 消息的线性 x 分量
  float angular_z = twist_msg->angular.z; // 获取 Twist 消息的角度 z 分量
  if (linear_x == 0 && angular_z == 0)    // 如果 Twist 消息没有速度命令
  {
    pid_controller[0].update_target(0); // 更新控制器的目标值
    pid_controller[1].update_target(0);
    motor.updateMotorSpeed(0, 0); // 停止第一个电机
    motor.updateMotorSpeed(1, 0); // 停止第二个电机
    return;                       // 退出函数
  }

  // 根据线速度和角速度控制两个电机的转速
  if (linear_x != 0)
  {
    pid_controller[0].update_target(linear_x * 1000);  // 使用mm/s作为target
    pid_controller[1].update_target(linear_x * 1000);
  }
}

// 这个函数是一个后台任务，负责设置和处理与 micro-ROS 代理的通信。
void microros_task(void *param)
{
  // 设置 micro-ROS 代理的 IP 地址。
  IPAddress agent_ip;
  agent_ip.fromString("192.168.2.105");

  // 使用 WiFi 网络和代理 IP 设置 micro-ROS 传输层。
  set_microros_wifi_transports("fishbot", "12345678", agent_ip, 8888);

  // 等待 2 秒，以便网络连接得到建立。
  delay(2000);

  // 设置 micro-ROS 支持结构、节点和订阅。
  allocator = rcl_get_default_allocator();
  rclc_support_init(&support, 0, NULL, &allocator);
  rclc_node_init_default(&node, "esp32_car", "", &support);
  rclc_subscription_init_default(
      &subscriber,
      &node,
      ROSIDL_GET_MSG_TYPE_SUPPORT(geometry_msgs, msg, Twist),
      "/cmd_vel");

  // 设置 micro-ROS 执行器，并将订阅添加到其中。
  rclc_executor_init(&executor, &support.context, 1, &allocator);
  rclc_executor_add_subscription(&executor, &subscriber, &sub_msg, &twist_callback, ON_NEW_DATA);

  // 循环运行 micro-ROS 执行器以处理传入的消息。
  while (true)
  {
    delay(100);
    rclc_executor_spin_some(&executor, RCL_MS_TO_NS(100));
  }
}

// 这个函数根据编码器读数更新两个轮子速度。
void update_speed()
{
  // 初始化静态变量以存储上一次更新时间和编码器读数。
  static uint64_t last_update_time = millis();
  static int64_t last_ticks[2];

  // 获取自上次更新以来的经过时间。
  uint64_t dt = millis() - last_update_time;
  if (dt == 0)
    return;

  // 获取当前的编码器读数并计算当前的速度。
  int32_t pt[2];
  pt[0] = encoders[0].getTicks() - last_ticks[0];
  pt[1] = encoders[1].getTicks() - last_ticks[1];
  current_speeds[0] = float(pt[0] * 0.1051566) / dt * 1000;
  current_speeds[1] = float(pt[1] * 0.1051566) / dt * 1000;

  // 更新上一次更新时间和编码器读数。
  last_update_time = millis();
  last_ticks[0] = encoders[0].getTicks();
  last_ticks[1] = encoders[1].getTicks();
}

void setup()
{
  // 初始化串口通信，波特率为115200
  Serial.begin(115200);
  // 将两个电机分别连接到引脚22、23和12、13上
  motor.attachMotor(0, 22, 23);
  motor.attachMotor(1, 12, 13);
  // 在引脚32、33和26、25上初始化两个编码器
  encoders[0].init(0, 32, 33);
  encoders[1].init(1, 26, 25);
  // 初始化PID控制器的kp、ki和kd
  pid_controller[0].update_pid(0.625, 0.125, 0.0);
  pid_controller[1].update_pid(0.625, 0.125, 0.0);
  // 初始化PID控制器的最大输入输出，MPCNT大小范围在正负100之间
  pid_controller[0].out_limit(-100, 100);
  pid_controller[1].out_limit(-100, 100);

  // 在核心0上创建一个名为"microros_task"的任务，栈大小为10240
  xTaskCreatePinnedToCore(microros_task, "microros_task", 10240, NULL, 1, NULL, 0);
}

void loop()
{
  // 更新电机速度
  update_speed();
  // 计算最新的电机输出值
  out_motor_speed[0] = pid_controller[0].update(current_speeds[0]);
  out_motor_speed[1] = pid_controller[1].update(current_speeds[1]);
  // 更新电机0和电机1的速度值
  motor.updateMotorSpeed(0, out_motor_speed[0]);
  motor.updateMotorSpeed(1, out_motor_speed[1]);
  // 延迟10毫秒
  delay(10);
}
```

添加PidController控制器到main函数中，关于Pid控制器的kp、ki和kd的设置，这里小鱼直接使用了比较合适的0.625和0.125，对于KD并没有设置，接下来我们下载代码进去并修改下PID进行测试。

需要注意：你要修改网络参数为你的当前环境的网络参数。

## 五、下载测试

下载代码，运行agent，点击RST按键。

```shell
sudo docker run -it --rm -v /dev:/dev -v /dev/shm:/dev/shm --privileged --net=host microros/micro-ros-agent:$ROS_DISTRO udp4 --port 8888 -v6
```

![image-20230306023859873](10.%E6%8E%A7%E5%88%B6%E9%80%9F%E5%BA%A6-PID%E6%8E%A7%E5%88%B6%E5%99%A8%E5%AE%9E%E7%8E%B0/imgs/image-20230306023859873.png)

看到连接建立表示通信成功，接着用`ros2 topic list`

```shell
ros2 topic list
```

![image-20230306024034226](10.%E6%8E%A7%E5%88%B6%E9%80%9F%E5%BA%A6-PID%E6%8E%A7%E5%88%B6%E5%99%A8%E5%AE%9E%E7%8E%B0/imgs/image-20230306024034226.png)

看到`/cmd_vel`表示正常，接着我们使用`teleop_twist_keyboard`进行键盘控制

```shell
ros2 run teleop_twist_keyboard teleop_twist_keyboard
```

把速度修改为0.10左右，接着把小车放到地上，点击键盘上的i，记时10s之后点击k或者控制让机器人停下来，接着看看机器人行走距离是不是1m。

![image-20230306024308205](10.%E6%8E%A7%E5%88%B6%E9%80%9F%E5%BA%A6-PID%E6%8E%A7%E5%88%B6%E5%99%A8%E5%AE%9E%E7%8E%B0/imgs/image-20230306024308205.png)

测试结果

![pid_test](10.%E6%8E%A7%E5%88%B6%E9%80%9F%E5%BA%A6-PID%E6%8E%A7%E5%88%B6%E5%99%A8%E5%AE%9E%E7%8E%B0/imgs/pid_test.gif)

## 六、PID调节实验

请自行修改PID参数进行测试，注意结合理论进行。

参数整定找最佳，从小到大顺序查；
先是比例后积分，最后再把微分加；
曲线振荡很频繁，比例度盘要放大；
曲线漂浮绕大湾，比例度盘往小扳；
曲线偏离回复慢，积分时间往下降；
曲线波动周期长，积分时间再加长；
曲线振荡频率快，先把微分降下来；
动差大来波动慢。微分时间应加长；
理想曲线两个波，前高后低四比一；
一看二调多分析，调节质量不会低；

## 七、总结

本节我们完成了PID控制器对两个电机速度的控制，但是仅限于前进和后退，如果想实现角速度的控制，我们还要结合两轮差速运动学模型才行。