PID算法及在机械臂上的应用(附简单Python实现)

PID算法及在机械臂上的应用(附简单Python实现)

最近在整理自己所做过的项目，顺便写一些总结，方便以后查看

1. PID控制原理

PID 控制器由三个部分组成：比例（Proportional, P）、积分（Integral, I）和微分（Derivative, D）。

1. 比例（P）部分：比例控制根据当前的误差大小进行调整。误差越大，控制量的调整幅度就越大。​比例控制有助于快速响应误差，但可能导致系统超调和振荡。比例控制的输出与误差成正比，公式为：$K_p\times e(t)$，其中 $K_p$ 是比例增益，$e(t)$ 是当前误差。
2. 积分（I）部分：积分控制关注误差的累积效果。积分控制有助于消除系统的静差，即使得被控制量最终达到设定值。积分控制的输出与误差的积分成正比，公式为：$K_i\times \int e(t)dt$，其中 Ki 是积分增益。
3. 微分（D）部分：微分控制关注误差的变化速率。 微分控制有助于减小系统的超调幅度和振荡次数，从而使系统更加稳定。微分控制的输出与误差的变化速率成正比，公式为：$K_d\times \left(\frac{de(t)}{dt}\right)$，其中 $K_d$ 是微分增益。

在PID类的control方法中，我们根据PID公式计算控制信号output。PID公式如下：
$$output=K_p\times e(t)+K_i\times \int e(t)dt+K_d\times \frac{\Delta e(t)}{\Delta t}$$
其中：

1. $e(t)$：当前误差（目标距离 - 当前距离）
2. $K_p$：比例系数
3. $Ki$：积分系数
4. $K_d$：微分系数
5. $\int e(t)dt$：积分项，用于消除静差
6. $\frac{\Delta e(t)}{\Delta t}$：微分项，用于减小超调幅度和振荡次数

2. 机械臂应用场景

假设有一个机械臂和一个超声波距离传感器，现在需要用机械臂去抓取物品，用超声波距离传感器检测机械臂与物体的距离。

超声波距离传感器直接返回机械臂与目标物体的距离，那么 $e(t)$ 就是传感器返回的值，即代码中的 error 。

这里给出一个Python代码示例：

import time

class PID:
    def __init__(self, kp, ki, kd):
        self.kp = kp
        self.ki = ki
        self.kd = kd
        self.previous_error = 0
        self.integral = 0
        self.last_time = time.time()

    def control(self, error):
        current_time = time.time()
        delta_time = current_time - self.last_time
        derivative = (error - self.previous_error) / delta_time
        self.integral += error * delta_time

        output = self.kp * error + self.ki * self.integral + self.kd * derivative

        self.previous_error = error
        self.last_time = current_time

        return output

def read_distance_sensor():
    # 在这里实现你的距离传感器代码，返回距离误差值
    pass

def move_arm(distance):
    # 在这里实现你的机械臂移动代码，接受一个距离参数
    pass

pid_controller = PID(kp=1, ki=0.1, kd=0.01)

while True:
    error = read_distance_sensor()  # 获取距离传感器返回的误差值
    arm_adjustment = pid_controller.control(error)  # 通过PID控制器计算机械臂的调整量
    move_arm(arm_adjustment)  # 控制机械臂移动
    time.sleep(0.01)  # 控制循环速度