PID算法–机器人算法相关

学习PID算法可以直接看这个课程：从不懂到会用！PID从理论到实践~_哔哩哔哩_bilibili，本文简短介绍pid算法以及使用python在ros中使用pid算法。

PID算法介绍

PID 算法，全称为比例 – 积分 – 微分控制算法（Proportional-Integral-Derivative Controller）主要在控制机器人运动时使用。

PID算法在连续时间域情况下的数学公式是：

但是众所周知，我们的系统不是连续时间，上述数学公式变成：

解释一下，U(n)我们认为是机器人的速度，Kp、Ki、Kd是我们可以调整的三个参数，机器人运动地好不好就看我们这三个参数调整地好不好。

e(n)是”偏差“，比如我的目标地点距离我100m，我就说”偏差“100m，即e(n)=100。

Ki后边部分是对此前所有误差的积分，T 是采样周期（两次控制计算的时间间隔）。

Kd​ 后面的部分是微分环节，e(n) – e(n-1)是当前时刻偏差与上一时刻偏差的差值，反映偏差的 “变化量”，除以T后反应上个时间段的速度变化情况。

Kp是“快速响应当前偏差”，简单来说：

我的速度只有Kp部分，Ki=0，Kd=0，我现在距离目标为100m（上面e(n)=100），如果Kp=0.1，则当前速度机器会自己调整为100*0.1=10m/s，当按照10m/s运动一段时间后我们会重新获得当前和目标的距离，比如说变成了90m，则当前速度为90*0.1=9m/s。

但是这样理论上永远无法到达目标点，所以我们引入Ki ”消除累积偏差“。

Ki的作用是通过添加之前误差的积分来使得机器人能最终运动到目标地点。我们上面只用了Kp，现在我们Ki不设为0后运动图像为：

可以看到是由于对之前的所有内容做了积分，所以Ki部分到达目标地点后仍然不会停止，这个时候Kp部分的速度变成了负的，将其移动回目标点，Ki部分积分也反向增加，使Ki部分也变小。最终又会”刹不住车“，最终变成震荡的形状。

为了让运动尽快刹住车，我们加入Kd部分，这个部分的作用就是减小到达目标地点时的震荡。最终加入Kd后的运动图像和上面的图像基本一样，但是震荡幅度减小了很多。

Python使用ros实现PID算法

读者如果没有安装ros可以看这个教程《ROS 2机器人开发从入门到实践》1.2.4 在Ubuntu中安装ROS2_哔哩哔哩_bilibili

读者可以直接下载我的GitHub中做好的测试项目my-robocon/topic_ws at main · gupengzu/my-robocon

其中的节点文件为turtle_pid_node.py：

import rclpy
from rclpy.node import Node
from geometry_msgs.msg import Twist
from turtlesim.msg import Pose
import math

class PIDController:
    def __init__(self, kp, ki, kd):
        self.kp = kp
        self.ki = ki
        self.kd = kd
        self.last_error = 0.0
        self.integral = 0.0

    def compute(self, error):
        self.integral += error
        derivative = error - self.last_error
        output = self.kp * error + self.ki * self.integral + self.kd * derivative
        self.last_error = error
        return output

class TurtlePIDNode(Node):
    def __init__(self):
        super().__init__('turtle_pid_node')
        self.publisher_ = self.create_publisher(Twist, '/turtle1/cmd_vel', 10)
        self.subscription = self.create_subscription(Pose, '/turtle1/pose', self.pose_callback, 10)

        # 用参数方程生成高密度爱心轨迹点
        self.target_points = []
        for t in [i * 0.05 for i in range(int(2 * math.pi / 0.05) + 1)]:
            x = 5.5 + 4 * math.sin(t) ** 3
            y = 5.5 + 3.5 * math.cos(t) - 1.5 * math.cos(2 * t) - 0.5 * math.cos(3 * t) - 0.2 * math.cos(4 * t)
            self.target_points.append((x, y))
        # 回到起点
        self.target_points.append(self.target_points[0])

        self.current_target_idx = 0
        self.pose = None
        self.pid_linear = PIDController(1.2, 0.0, 0.1)
        self.pid_angular = PIDController(10.0, 0.0, 2.0)

    def pose_callback(self, msg):
        self.pose = msg
        self.move_to_target()

    def move_to_target(self):
        if self.pose is None or self.current_target_idx >= len(self.target_points):
            return
        target_x, target_y = self.target_points[self.current_target_idx]
        dx = target_x - self.pose.x
        dy = target_y - self.pose.y
        distance = math.sqrt(dx**2 + dy**2)
        angle_to_target = math.atan2(dy, dx)
        angle_error = self.normalize_angle(angle_to_target - self.pose.theta)

        # PID控制
        linear_speed = self.pid_linear.compute(distance)
        angular_speed = self.pid_angular.compute(angle_error)

        # 到达目标点则切换下一个
        if distance < 0.05:
            self.current_target_idx += 1
            self.pid_linear.integral = 0
            self.pid_angular.integral = 0

        twist = Twist()
        twist.linear.x = max(min(linear_speed, 1.0), -1.0)
        twist.angular.z = max(min(angular_speed, 6.0), -6.0)
        self.publisher_.publish(twist)

    @staticmethod
    def normalize_angle(angle):
        while angle > math.pi:
            angle -= 2 * math.pi
        while angle < -math.pi:
            angle += 2 * math.pi
        return angle

def main(args=None):
    rclpy.init(args=args)
    node = TurtlePIDNode()
    rclpy.spin(node)
    node.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()

这一段是用来生成高密度的爱心曲线（笛卡尔曲线）上的点的方法：（读者可以替换为别的形状）

# 用参数方程生成高密度爱心轨迹点
        self.target_points = []
        for t in [i * 0.05 for i in range(int(2 * math.pi / 0.05) + 1)]:
            x = 5.5 + 4 * math.sin(t) ** 3
            y = 5.5 + 3.5 * math.cos(t) - 1.5 * math.cos(2 * t) - 0.5 * math.cos(3 * t) - 0.2 * math.cos(4 * t)
            self.target_points.append((x, y))
        # 回到起点
        self.target_points.append(self.target_points[0])

克隆文件后：
一：启动小海龟： ros2 run turtlesim turtlesim_node
二：启动项目，发送消息：
1.colcon build
2.source install/setup.bash
3.ros2 run demo_python_topic turtle_pid_node

运行后的结果是：（这个结果很好看，其实主要是因为我选取了一组高密度的爱心曲线的点，不是因为我的PID三个参数调得多好，读者换成其他的图形后应该会发现这个问题，读者可以自行调整成更好的PID参数，让小乌龟移动地更稳定）