(3-2)速度控制算法：基于PID的速度控制算法

3.2.1  常用的速度控制算法

在现实应用中，常用的速度控制算法主要包括一些经典的和先进的控制策略。在下面列出了一些常见的速度控制算法。

1. PID控制算法

1. 原理：PID控制算法基于比例（P）、积分（I）和微分（D）三个控制项，通过比较实际速度与期望速度的差异，调整控制输出。
2. 应用：在许多工业、机械和自动化系统中广泛应用。

2. 模型预测控制（MPC）

1. 原理：MPC使用系统的动态模型进行预测，并通过优化来调整控制输入，以实现期望的速度。
2. 应用：适用于对系统动态性能要求高的场景，如化工过程、无人机等。

3. 模糊逻辑控制（FLC）

1. 原理：模糊逻辑控制使用模糊集合和模糊规则进行推理，处理非线性和模糊性，适用于复杂系统。
2. 应用：在一些需要处理不确定性和模糊信息的控制系统中应用广泛。

4. 滑模控制（SMC）

1. 原理：滑模控制引入一个滑动面，通过使系统在滑动面上运动来实现鲁棒控制。
2. 应用：对于对外部扰动和不确定性具有鲁棒性要求的系统。

5. 最优控制算法（Optimal Control）

1. 原理：最优控制使用最优化理论，通过优化控制输入来实现系统的最佳性能。
2. 应用：在需要最小化某个性能指标的系统中应用广泛，如飞行器控制。

6. 自适应控制（Adaptive Control）

1. 原理：自适应控制能够根据系统参数的变化自动调整控制策略，适应系统的变化。
2. 应用：在系统参数可能发生变化的情况下应用，提高系统的适应性。

注意：在实际应用中，在不同的应用场景中具体选择使用哪一种算法，这取决于系统的动态性能要求、对鲁棒性的需求以及系统的非线性特性等因素。在实际使用时，也可能会组合使用不同的算法以充分发挥各自的优势。

3.2.2  基于PID的速度控制算法

PID（比例-积分-微分）控制算法在速度控制方面具有广泛的应用，它通过实时反馈来调整控制输出，使得系统的实际速度尽量接近期望速度。以下是PID控制算法在速度控制中的一些应用场景和特点。

1. 电机控制：在电机驱动系统中，PID控制用于调整电机的输出，以实现所需的速度。这可以应用于工业生产线、自动化系统以及电动车辆等领域。
2. 飞行器控制：在飞行器中，如四轴飞行器、直升机等，PID控制用于稳定和调整飞行器的速度，确保平稳的飞行和悬停。
3. 汽车巡航控制：在智能驾驶中，PID控制可用于调整汽车的速度，以维持与前方车辆的安全距离或实现自适应巡航控制。
4. 传送带系统：工业生产线上的传送带系统可以使用PID控制来确保物品在传送带上以稳定的速度移动，以适应生产过程的需要。
5. 机器人运动控制：在机器人运动中，PID控制可以应用于关节的速度控制，以确保机器人的运动平稳且符合预期轨迹。
6. 航空器速度控制：在航空领域，如飞机，PID控制可用于调整飞机的速度，确保飞行器按照预定的速度进行飞行。

例如下面是一个使用PID实现速度控制的简单例子，假设有一个移动机器人在迷宫中导航，机器人的任务是在避开障碍物的同时保持期望的速度。我们将使用PID控制器进行基本的模拟，并可视化机器人的路径。

实例3-1：使用PID控制机器人的速度（源码路径：codes\3\speed\pi.py）

实例文件pi.py的具体实现代码如下所示。

import matplotlib.pyplot as plt
import numpy as np

class PIDController:
    def __init__(self, kp, ki, kd, setpoint):
        self.kp = kp
        self.ki = ki
        self.kd = kd
        self.setpoint = setpoint
        self.prev_error = 0
        self.integral = 0

    def calculate(self, current_speed):
        error = self.setpoint - current_speed
        self.integral += error
        derivative = error - self.prev_error
        output = self.kp * error + self.ki * self.integral + self.kd * derivative
        self.prev_error = error
        return output

class Robot:
    def __init__(self, initial_speed=0, position=(0, 0)):
        self.speed = initial_speed
        self.position = position
        self.path = [position]

    def update_speed(self, control_input):
        # 模拟机器人的速度更新过程，这里简单地将控制输入直接加到速度上
        self.speed += control_input
        print(f"Current Speed: {self.speed:.2f}")

    def move(self, time_step):
        # 模拟机器人的移动过程，根据当前速度更新位置
        x, y = self.position
        new_x = x + self.speed * np.cos(np.radians(self.speed)) * time_step
        new_y = y + self.speed * np.sin(np.radians(self.speed)) * time_step
        self.position = (new_x, new_y)
        self.path.append(self.position)

# 模拟机器人速度控制和移动过程
def simulate_robot_motion(controller, robot, time_steps, time_step_duration):
    for _ in range(time_steps):
        control_input = controller.calculate(robot.speed)
        robot.update_speed(control_input)
        robot.move(time_step_duration)

if __name__ == "__main__":
    # 设置PID参数和初始速度
    kp, ki, kd = 0.1, 0.01, 0.05
    initial_speed = 5.0
    setpoint_speed = 10.0

    # 创建PID控制器和机器人对象
    pid_controller = PIDController(kp, ki, kd, setpoint_speed)
    robot = Robot(initial_speed)

    # 模拟机器人速度控制和移动过程
    time_steps = 100
    time_step_duration = 0.1
    simulate_robot_motion(pid_controller, robot, time_steps, time_step_duration)

    # 绘制机器人路径图
    path = np.array(robot.path)
    plt.plot(path[:, 0], path[:, 1], label='Robot Path', marker='o')
    plt.xlabel('X-axis')
    plt.ylabel('Y-axis')
    plt.legend()
    plt.title('Robot Motion with PID Control')
    plt.show()

在这个例子中，机器人通过PID控制器调整速度，同时模拟了机器人在迷宫中的移动过程。最后，我们使用matplotlib库绘制了机器人的路径图。上述代码的实现流程如下所示：

1. 首先，定义了类PIDController，它包含比例（kp）、积分（ki）、微分（kd）和设定速度（setpoint）等参数。该类实现了一个calculate方法，用于根据当前速度计算PID控制器的输出。
2. 接着，定义了类Robot，表示机器人。这个类包含了机器人的当前速度、位置和路径。在update_speed方法中，我们模拟了机器人的速度更新过程，将PID控制器的输出直接加到机器人的速度上。在move方法中，我们根据当前速度和时间步长更新机器人的位置，并将新位置添加到机器人的路径中。
3. 然后，定义了函数simulate_robot_motion，用于模拟机器人的速度控制和移动过程。在每个时间步骤中，我们通过PID控制器计算控制输入，更新机器人的速度，然后根据当前速度更新机器人的位置。
4. 接着，在主程序中设置了PID控制器的参数、初始速度，并创建了PID控制器和机器人对象。通过调用simulate_robot_motion函数，模拟了机器人在一定时间内的运动过程。在模拟过程中，机器人的速度更新和位置移动会被打印输出。
5. 最后，使用库matplotlib绘制了机器人的运动路径图，可视化了机器人的运动轨迹。如图3-1所示。

图3-1  机器人的运动路径图

注意：尽管PID控制在许多应用中表现良好，但对于一些复杂的、非线性的系统，可能需要更高级的控制算法来满足更高的性能和鲁棒性需求。

PID控制器是控制系统的抽象部分，确保所需的过渡过程类型。简而言之，它会根据我们的需要改变一些值——快速、缓慢、准确或不太明显。首先，我们从列表中获取变量 - 这些是使用 python 的功能。然后我们计算控制误差，很快就需要我们了。常见的PID公式为：

U = (Kp * Error) + (Ki * Sum_Error) + (Kd * (Error - Old_Error)/dt)

再看下面的例子，实现了一个四轴飞行器的角速度控制系统。在本实例中，将使用 PID 控制器来计算必要的电机 RPM 命令，然后通过此命令以确保逼真的建模 - 四轴飞行器不能旋转太快，它可能会损坏。该系统将使我们能够准确、平稳地提供所需的角速度。

实例3-3：四轴飞行器的角速度控制系统（源码路径：codes\3\speed\jiao.py）

实例文件jiao.py的具体实现代码如下所示。

import numpy as np
import pylab

g = 9.81  # 重力加速度
Timelapse = 1  # 仿真时间
dt = 0.01  # 时间步长

mass = 1  # 质量
inert = 0.000001  # 惯性矩
length = 0.15  # 质心到电机的距离
b = 0.001  # 推力和转矩的比例系数
d = 0.0000000001  # 陀螺仪偏差

Wx_start = 0  # X轴初始角速度
Wy_start = 0  # Y轴初始角速度
Wz_start = 0  # Z轴初始角速度

P_des = 50  # 期望推力
Wx_des = 0.5  # 期望X轴角速度
Wy_des = 0.4  # 期望Y轴角速度
Wz_des = 0.1  # 期望Z轴角速度

def Saturation(var, max, min):
    if var > max:
        return max
    elif var < min:
        return min
    else:
        return var

def PID(pid_input_parameters):
    # 变量定义
    kp = pid_input_parameters[0]  # PID 比例参数
    ki = pid_input_parameters[1]  # PID 积分参数
    kd = pid_input_parameters[2]  # PID 微分参数
    set_point = pid_input_parameters[3]  # 设定值
    current_point = pid_input_parameters[4]  # 当前值
    old_error = pid_input_parameters[5]  # 之前的误差
    sum_error = pid_input_parameters[6]  # 误差积分
    dt = pid_input_parameters[7]  # 时间步长
    # 计算PID影响
    error = set_point - current_point
    impact = kp * error + kd * ((error - old_error) / dt) + ki * sum_error
    return impact

def Mixer(P_cmd, Roll_cmd, Pitch_cmd, Yaw_cmd, w):
    # 为四轴飞行器混合命令
    w[0] = Saturation(P_cmd - Yaw_cmd + Roll_cmd + Pitch_cmd, 20000, 0)
    w[1] = Saturation(P_cmd + Yaw_cmd - Roll_cmd + Pitch_cmd, 20000, 0)
    w[2] = Saturation(P_cmd - Yaw_cmd - Roll_cmd - Pitch_cmd, 20000, 0)
    w[3] = Saturation(P_cmd + Yaw_cmd + Roll_cmd - Pitch_cmd, 20000, 0)

def PhysModel(P_des, Wx_des, Wy_des, Wz_des, Timelapse, dt):
    w = [0, 0, 0, 0]
    motors_data = [[], [], [], []]
    time = 0.0
    Wx = []
    Wx_cur = Wx_start
    Wx_old_error = 0
    Wx_sum_error = 0
    Wy = []
    Wy_cur = Wy_start
    Wy_old_error = 0
    Wy_sum_error = 0
    Wz = []
    Wz_cur = Wz_start
    Wz_old_error = 0
    Wz_sum_error = 0
    while time < Timelapse:
        Wx_input_parameters = [100, 0.001, 0.1, Wx_des, Wx_cur, Wx_old_error, Wx_sum_error, dt]
        Wx_impact = PID(Wx_input_parameters)  # 计算X轴影响
        Pitch_cmd = Saturation(Wx_impact, 500, 0)

        Wy_input_parameters = [100, 0.001, 0.1, Wy_des, Wy_cur, Wy_old_error, Wy_sum_error, dt]
        Wy_impact = PID(Wy_input_parameters)  # 计算Y轴影响
        Roll_cmd = Saturation(Wy_impact, 500, 0)

        Wz_input_parameters = [500, 1, 1, Wz_des, Wz_cur, Wz_old_error, Wz_sum_error, dt]
        Wz_impact = PID(Wz_input_parameters)  # 计算Z轴影响
        Yaw_cmd = Saturation(Wz_impact, 10, -10)

        Mixer(P_des, Roll_cmd, Pitch_cmd, Yaw_cmd, w)  # 进行命令混合

        M0 = ((w[0] * pow(b, 2)) * length / 2)  # 计算每个引擎的力矩
        M1 = ((w[1] * pow(b, 2)) * length / 2)
        M2 = ((w[2] * pow(b, 2)) * length / 2)
        M3 = ((w[3] * pow(b, 2)) * length / 2)
        Mz = d * (pow(w[3], 2) + pow(w[1], 2) - pow(w[0], 2) - pow(w[2], 2))  # 计算陀螺仪力矩

        Ax = (M0 + M1 - M2 - M3) / inert  # 计算X轴加速度
        Wx_cur += Ax * dt  # 计算X轴角速度
        Wx_sum_error += (Wx_des - Wx_cur)  # 计算误差并记住旧误差（需要用于PID）
        Wx_old_error = Wx_des - Wx_cur

        Ay = (M0 + M3 - M1 - M2) / inert  # 计算Y轴加速度
        Wy_cur += Ay * dt  # 计算Y轴角速度
        Wy_sum_error += (Wy_des - Wy_cur)  # 计算误差并记住旧误差（需要用于PID）
        Wy_old_error = Wy_des - Wy_cur

        Az = Mz / inert  # 计算Z轴加速度
        Wz_cur += Az * dt  # 计算Z轴角速度
        Wz_sum_error += (Wz_des - Wz_cur)  # 计算误差并记住旧误差（需要用于PID）
        Wz_old_error = Wz_des - Wz_cur

        time += dt  # 下一个时间步

        motors_data[0].append(w[0] * 100)  # 记录电机数据
        motors_data[1].append(w[1] * 100)
        motors_data[2].append(w[2] * 100)
        motors_data[3].append(w[3] * 100)

        Wx.append(Wx_cur * 57.3)  # 记录角速度数据 * 57.3，类似于角度
        Wy.append(Wy_cur * 57.3)
        Wz.append(Wz_cur * 57.3)

    return [Wx, Wy, Wz, motors_data]  # 这是过程输出，我们将绘制或分析它

if __name__ == '__main__':
    arr = PhysModel(P_des, Wx_des, Wy_des, Wz_des, Timelapse, dt)

    Wx = np.array(arr[0])
    Wy = np.array(arr[1])
    Wz = np.array(arr[2])

    Motor1 = np.array(arr[3][0])
    Motor2 = np.array(arr[3][1])
    Motor3 = np.array(arr[3][2])
    Motor4 = np.array(arr[3][3])

    hrz = np.arange(0, Timelapse, dt)

    pylab.figure(figsize=(7, 7), num='Angle speed controller')

    pylab.subplot(211)
    pylab.title("Angle speed", fontsize=12)
    pylab.xlabel("Time, sec", color="black")
    pylab.ylabel("Angle speed, deg/sec ", color="black")
    pylab.plot(hrz, Wx, hrz, Wy, hrz, Wz)
    pylab.legend(["Pitch", "Roll", "Yaw"], loc=1)

    pylab.subplot(212)
    pylab.title("Motors RPM", fontsize=12)
    pylab.xlabel("Time, sec")
    pylab.ylabel("RPM")
    pylab.plot(hrz, Motor1, hrz, Motor2, hrz, Motor3, hrz, Motor4)
    pylab.legend(["Motor 1", "Motor 2", "Motor 3", "Motor 4"], loc=1)

    pylab.tight_layout()
    pylab.show()

整体而言，本实例现了一个基于PID控制器的四轴飞行器模型，并通过图形展示了飞行器在一定时间内的运动情况。上述代码的实现流程如下所示：

1. 首先，定义了一系列常数和参数，包括物理系统的质量、惯性、长度，以及控制器的期望值和初始条件。
2. 接着，实现了几个功能函数，其中Saturation函数用于限制变量在指定的上下限之间，确保其数值在可控范围内；PID函数实现了一个简单的PID控制器，根据给定的PID参数和当前状态，计算出控制输入的影响；Mixer函数将控制命令混合为四个电机的输入，这是四轴飞行器的控制策略。
3. 然后，定义了物理模型的主函数PhysModel，该函数基于PID控制器和飞行器动力学方程模拟了系统在一段时间内的运动。在循环中，分别计算了X、Y、Z轴上的控制输入，然后根据控制输入计算了电机的输出和飞行器的角速度。在每个时间步长内，记录了电机输出和角速度的数据。
4. 最后，通过调用PhysModel函数模拟了飞行器的运动，并使用库Matplotlib绘制了两个子图。第一个子图显示了飞行器的角速度随时间的变化，包括俯仰（Pitch）、翻滚（Roll）和偏航（Yaw）角速度。第二个子图显示了四个电机的转速随时间的变化。如图3-2所示。

图3-2  绘制的两个子图