一文教你如何用C++写pid，且使用cmake工具部署到ROS2控制PX4中

2025年4月16日|作者：小迈的学习笔记|分类：pid算法部署到c++工程

---

引言：在工业控制，机器人，电源控制，自动驾驶，等众多领域，PID(比例-积分微分)算法作为经典的控制策略，因其结构简单，适应性强等特点，广泛应用于各种控制领域。然而，理论层面的PID公式仅仅是起点，如何将其高效，稳定的部署到实际的工程中，是初学者不可避免的一关

C++凭借其高性能，跨平台特性以及对底层硬件的直接操控能力，成为工业级控制系统的首选语言。但复杂的项目结构，多平台兼容性以及算法模块与工程框架的集成问题，常常让初学者陷入“算法能用，工程难用”的困境。此时,Cmake作为现代C++工程的构建工具，能够有效管理代码依赖，有效管理流程，显著提升项目的可维护性和可移植性

本文以实战为导向（LINUX+UBUNTU22.04+ROS2环境下），从PID的原理出发，到C++实现，结合CMake工具，逐步将控制算法模块化，并最终移植到实际工程框架中。无论你是控制领域的新手，还是希望优化工程实践的开发者，都能学会一些可落地的工程方案

一.算法原理

• 比例项（P） ：与当前误差成正比
• 积分项（ I ） ：与当前误差积累量成正比
• 积分项（D） ：与误差变化率成正比
• 输出公式：

• 离散化形式（离散化的原因是在代码中实现连续型运算比较难以实现，我们一般用离散化运算来逼近实际值

二.实现代码

• 计算error值（目标值与实测值相减）：

double error = target - current;

• 计算误差积分（加上积分限幅，防止积分爆炸）：

integral += error;//积分项运算
integral = std::clamp(integral,-integral_limit,integral_limit);//添加积分限幅

• 计算误差微分（时间戳暂时省略，先用一个dt代替）：

double derivative = (error - last_error)/dt

• 定义PID控制器的值

    double Kp,Ki,Kd;//比例，积分，微分系数
    double intergral;//积分积累量
    double laster_error;//上一次误差
    double output_limit//输出限幅

• 计算输出值(加上一个输出限幅，增加系统稳定性和安全性)

double output = Kp * error + Ki * integral + kd * derivative;
if (output > out_put_limit) return output_limit;
if (output < -out_put_limit) return -output_limit;
return output;

• 创建PID重置函数（适配一些控制系统的需要）

void reset()
{
        integral = 0;
        last_error = 0;
}

上述就是创建一个pid控制器的思路，我们把这些放在一个C++类里面实现

#include <iostream>

class PIDController
{
private:
    double Kp,Ki,Kd;//比例，积分，微分系数
    double intergral;//积分积累量
    double laster_error;//上一次误差
    double output_limit//输出限幅
        
public:
    //构造函数
    PIDController(double p,double i,double d,double limit = 1000.0)
        :Kp(p),Ki(i),Kd(d),integral(0),last_error(0),outpit_limit(limit){}
    
    //计算控制量
    double compute(double current,double target,double dt)
    {
        id(dt<=0) return 0;//避免被除以0
        
        double error = target - current;//误差项运算
        integral += error;//积分项运算
        integral = std::clamp(integral,-integral_limit,integral_limit);//添加积分限幅
        double derivative = (error - last_error)/dt
        last_error = error;//保存当前误差
        double output = Kp * error + Ki * integral + kd * derivative;
        
        //输出限幅
        if (output > out_put_limit) return output_limit;
        if (output < -out_put_limit) return -output_limit;
        return output;
    }
    void reset()
    {
        integral = 0;
        last_error = 0;
    }
};

创建好类了，我们该如何使用它呢？

int main()
{
    //初始化PID参数：Kp=2.0,Ki=0.5,Kd=0.1,输出限幅100
    PIDControler pid(2.0,0.5,0.1,100);
    
    double current_value = 25.0;//当前系统状态
    double target_value = 50.0; //目标值
    double dt = 0.1;
    
    for (int i = 0; i < 10 ; ++i)
    {    
        double output pid.compute(current_value,target_value,dt);
        
        //模拟系统响应（简单一阶系统）
        current_value += output * dt * 0.1;
        
        std::cout << "Step" << i+1
                  << ":Output=" << output
                  << ", Current = " << current_value << std::endl;        
    }
    return 0;
}

这样我们就完成了一个C++实现PID的简单实例。恭喜你通过了新手村！🪅🪅🪅

三.工程实例

由于Linux是开源的操作系统，有非常多方便的开发工具可以使用，开发者非常喜欢的操作系统，所以再次使用Linux操作系统作为例子

接下来我们将在UBUNTU22,04环境中，以ROS2(humble)控制PX4实现无人机运动闭环控制为例子，带你进一步了解如何使用CMake将针对性写的PID文件部署到工程中

• 相关环境配置教程
  • https://blog.csdn.net/qq_41035541/article/details/135220472?ops_request_misc=%257B%2522request%255Fid%2522%253A%2522a5633f41b420740b0910ef99ec6b9e2e%2522%252C%2522scm%2522%253A%252220140713.130102334…%2522%257D&request_id=a5633f41b420740b0910ef99ec6b9e2e&biz_id=0&utm_medium=distribute.pc_search_result.none-task-blog-2_alltop_click~default-1-135220472-null-null.142^v102pc_search_result_base3&utm_term=ubuntu22.04%E5%8F%8C%E7%B3%BB%E7%BB%9F&spm=1018.2226.3001.4187(配置linux双系统)
  • https://blog.csdn.net/weixin_55944949/article/details/130848009?spm=1001.2014.3001.5502（ROS2加px4环境配置）

环境配好之后，我们来下载官方代码

git clone https://github.com/PX4/px4_ros_com.git

下载好了之后我们创建一个工作空间

mkdir -p ~/ros2_ws/src
cd ~/ros2_ws

使用图形化界面将代码文件移植到工作空间中

可以看到

然后我们下载vscode(最受欢迎的代码编辑器),推荐鱼香ros一键安装

wget http://fishros.com/install -O fishros && . fishros

选择7即可发现电脑已经安装vscode

然后下载好python,c++,ros2，cmake插件，再在vscode中打开工程

就可以看到这些

未完待续。。。。。。