python串级pid角度环和速度环

### Python 实现串级 PID 控制

#### 角度环和速度环设计原理

在多变量控制系统中，串级控制结构被广泛应用于提高系统的响应性能和鲁棒性。对于角度环和速度环的设计而言，通常采用两个独立的PID控制器分别负责不同层次的任务：

- **外环（角度环）**：主要关注于设定的角度目标值与实际测量到的角度之间的差异，其输出作为内环的速度指令。

- **内环（速度环）**：专注于执行来自外环的速度命令，并尽可能快速而精确地达到该速度。

这种架构能够有效分离低频的位置调节任务和高频的速度动态特性管理[^1]。

#### Python 示例代码实现

下面是一个简化版的Python程序片段，用于展示如何构建一个基本的串级PID控制系统，其中包含了角度环和速度环的具体实现方式。

import numpy as np
from scipy.integrate import odeint


class PidController:
    def __init__(self, kp=0.0, ki=0.0, kd=0.0):
        self.kp = kp
        self.ki = ki
        self.kd = kd
        self.set_point = 0.0
        self.previous_error = 0.0
        self.integral = 0.0
    
    def update(self, current_value, dt):
        error = self.set_point - current_value
        
        proportional_term = self.kp * error
        integral_term = self.integral + (error * dt) * self.ki
        derivative_term = ((error - self.previous_error)/dt)*self.kd if dt>0 else 0
        
        output = proportional_term + integral_term + derivative_term
        
        # 更新状态
        self.previous_error = error
        self.integral += error*dt
        
        return output
    

def cascaded_pid_control(t, y, u_angle_cmd, pid_speed, pid_angle):
    angle, angular_velocity = y
    
    # 外环(角度环): 计算所需角速度
    desired_angular_velocity = pid_angle.update(angle, t)

    # 内环(速度环): 使用期望角速度来调整实际控制力矩
    control_torque = pid_speed.update(desired_angular_velocity - angular_velocity, t)
    
    dydt = [angular_velocity,
            control_torque / I]  # 假设I为转动惯量
            
    return dydt


if __name__ == "__main__":
    time_span = np.linspace(0., 10., num=500)
    initial_conditions = [np.pi/6, 0.]  # 初始条件: 小角度偏移 & 静止
    setpoints = lambda t : np.sin(t)+np.pi/4  # 设定随时间变化的目标角度轨迹
    
    # 初始化PID控制器参数
    pid_controller_angle = PidController(kp=.8, ki=.05, kd=.2)
    pid_controller_speed = PidController(kp=1.5, ki=0.07, kd=0.)
    
    sol = odeint(lambda y,t: cascaded_pid_control(
                    t,y,setpoints(t),pid_controller_speed,pid_controller_angle),
                 initial_conditions,time_span)
                 
    angles, velocities = zip(*sol.tolist())

上述代码展示了利用`odeint`函数求解常微分方程组的方法，以此模拟连续时间内系统的行为；并通过定义类`PidController`封装了标准的比例-积分-微分运算逻辑。