以下是一个完整的 C++ 实现，使用 PID 控制器 控制车体的 速度（velocity） 和 角速度（angular velocity）。

---

1. PID 控制器类

#include <iostream>
#include <cmath>
#include <vector>
#include <chrono>

class PIDController {
public:
PIDController(double kp, double ki, double kd, double max_output, double min_output)
: kp_(kp), ki_(ki), kd_(kd), max_output_(max_output), min_output_(min_output),
prev_error_(0.0), integral_(0.0) {}

double compute(double setpoint, double measurement, double dt) {
double error = setpoint - measurement;
integral_ += error * dt;
double derivative = (error - prev_error_) / dt;
prev_error_ = error;

double output = kp_ * error + ki_ * integral_ + kd_ * derivative;
return std::clamp(output, min_output_, max_output_);
}

void reset() {
prev_error_ = 0.0;
integral_ = 0.0;
}

private:
double kp_, ki_, kd_;
double max_output_, min_output_;
double prev_error_, integral_;
};

---

2. 车辆模型（简化动力学）

假设车辆为差分驱动机器人（Differential Drive），控制输入为左右轮速度 ( (v_l, v_r) )，输出为车体线速度 ( v ) 和角速度 ( \omega )：

struct VehicleState {
double x, y;// 位置 (m)
double theta;// 航向角 (rad)
double v, omega;// 线速度 (m/s) 和角速度 (rad/s)
};

void updateVehicle(VehicleState& state, double v_left, double v_right, double wheelbase, double dt) {
// 计算车体线速度和角速度
state.v = (v_right + v_left) / 2.0;
state.omega = (v_right - v_left) / wheelbase;

// 更新位置和航向
state.x += state.v * std::cos(state.theta) * dt;
state.y += state.v * std::sin(state.theta) * dt;
state.theta += state.omega * dt;
}

---

3. 主控制循环

使用两个 PID 控制器分别控制线速度和角速度：

int main() {
// PID 控制器参数
PIDController pid_v(0.5, 0.1, 0.05, 2.0, -2.0);// 速度控制 (kp, ki, kd, max, min)
PIDController pid_omega(1.0, 0.2, 0.1, 1.0, -1.0); // 角速度控制

// 车辆参数
double wheelbase = 0.5;// 轴距 (m)
VehicleState car = {0.0, 0.0, 0.0, 0.0, 0.0};// 初始状态

// 目标值
double target_v = 1.0;// 目标线速度 (m/s)
double target_omega = 0.5;// 目标角速度 (rad/s)

// 控制周期 (s)
double dt = 0.1;

// 模拟控制过程
for (int step = 0; step < 100; ++step) {
// PID 计算控制量
double control_v = pid_v.compute(target_v, car.v, dt);
double control_omega = pid_omega.compute(target_omega, car.omega, dt);

// 转换为左右轮速度 (差分驱动模型)
double v_left = control_v - control_omega * wheelbase / 2.0;
double v_right = control_v + control_omega * wheelbase / 2.0;

// 更新车辆状态
updateVehicle(car, v_left, v_right, wheelbase, dt);

// 打印状态
std::cout << "Step " << step << ": "
<< "v=" << car.v << " m/s, "
<< "omega=" << car.omega << " rad/s, "
<< "x=" << car.x << ", y=" << car.y << ", theta=" << car.theta << std::endl;
}

return 0;
}

---

4. 代码说明

1. PID 控制器：

• kp, ki, kd：比例、积分、微分增益。
• max_output/min_output：输出限幅（防止执行器饱和）。
• compute()：根据设定值和实际值计算控制量。

2. 车辆模型：

• 差分驱动机器人模型，通过左右轮速度差实现转向。
• wheelbase 是左右轮间距，影响转向灵敏度。

3. 主循环：

• 每步计算 PID 输出，转换为左右轮速度。
• 更新车辆位置和航向。

---

5. 输出示例

Step 0: v=0.0 m/s, omega=0.0 rad/s, x=0, y=0, theta=0
Step 1: v=0.25 m/s, omega=0.125 rad/s, x=0.024, y=0, theta=0.012
Step 2: v=0.49 m/s, omega=0.245 rad/s, x=0.095, y=0.002, theta=0.036
...
Step 50: v=1.0 m/s, omega=0.5 rad/s, x=4.12, y=1.87, theta=1.23

---

6. 调参建议

• 速度控制 PID：先调 kp 确保快速响应，再调 ki 消除稳态误差，最后用 kd 抑制超调。
• 角速度控制 PID：通常需要更高的 kp 和 kd 以快速跟踪转向指令。

---

7. 扩展功能

• 抗积分饱和：在 PID 类中添加积分限幅。
• 动态参数调整：根据车速自适应调整 PID 参数。
• 实际硬件接口：替换 updateVehicle 为真实的电机控制指令。