（7-2）反馈控制与系统稳定性：使用PID控制机器人的物理运动_pid控制、运动控制

在本书前面的内容中已经讲解了上述算法的基本知识和用法，大家可以根据具体的控制系统需求选择使用。请看下面的实例，演示了使用PID控制算法实现反馈控制的过程。该实例通过建模机器人的物理运动，综合考虑了摩擦和物理参数，并运用数值模拟方法实现了对机器人运动的仿真。通过引入PID控制器，项目展示了对机器人角度、角速度和位置的精确控制，使机器人能够在运动中保持竖直姿势，并以特定速度移动。反馈控制在此过程中发挥了关键作用，通过实时调整控制输入，使机器人能够快速而准确地响应外部变化，提高了系统的稳定性和鲁棒性，为机器人在复杂环境中的运动控制提供了可行性和灵活性。

实例7-1：使用PID控制机器人的物理运动（源码路径：codes\7\feed\pid.py）

实例文件pid.py的具体实现流程如下所示。

1. 基本运动

（1）函数solve通过数值方法（Runge-Kutta积分法）解决了一组一阶常微分方程的初值问题，它接受初始状态、时间序列以及计算下一个状态和导数的函数作为参数，然后通过迭代计算在给定时间点上系统的状态演化。函数返回一个包含各个时间点状态的NumPy数组。这一过程模拟了物理系统在时间上的演变，对于探索动力学系统的行为和性质非常有用。

import numpy as np
from numpy import sin, cos, pi
import matplotlib.pyplot as plt
from matplotlib import animation
 
def solve(initial_state, times, integrate_func, derivative_func):
    """
    解决了一阶常微分方程的初值问题
    :param initial_state: 初始状态
    :param times: 要解决的时间点序列
    :param integrate_func: 计算下一个状态的函数
    :param derivative_func: 计算每个状态分量的导数的函数
    :return:
    """
    dt = times[1] - times[0]
    states = [initial_state]
    for step, t in enumerate(times):
        states.append(integrate_func(states[-1], step, t, dt, derivative_func))
    return np.array(states)
'
运行
运行

（2）函数integrate_rk4实现了四阶Runge-Kutta（RK4）方法，用于解决常微分方程的数值积分。该函数通过计算各阶导数（k1到k4）和加权平均，更新当前状态以计算下一个时间步的状态。这一过程模拟了系统状态在时间上的演变，RK4方法通过提高数值解的精度而在数学和物理建模中广泛应用。

def integrate_rk4(state, step, t, dt, dydx_func):
    """
    四阶Runge-Kutta方法。
    """
    k1 = dydx_func(state, step, t, dt)
    k2 = dydx_func([v + d * dt / 2 for v, d in zip(state, k1)], step, t, dt)
    k3 = dydx_func([v + d * dt / 2 for v, d in zip(state, k2)], step, t, dt)
    k4 = dydx_func([v + d * dt for v, d in zip(state, k3)], step, t, dt)
    return [v + (k1_ + 2 * k2_ + 2 * k3_ + k4_) * dt / 6 for v, k1_, k2_, k3_, k4_ in zip(state, k1, k2, k3, k4)]
'
运行
运行

（3）下面代码定义了一个动力学系统的参数，具体说明如下所示。

1. r：轮子的半径。
2. l：杆的长度。
3. M：杆的质量。
4. m：轮子的质量。
5. g：重力加速度。
6. I：系统的转动惯量，通过轮子和杆的质量分布计算得到。

# 系统参数
r = 0.25
l = 1.0
M = 0.25
m = 0.3
g = 9.8
I = 0.5 * M * r
'
运行
运行

（4）下面的代码定义了一个名为derivate的函数，该函数计算了一个物理系统的状态在给定时间点上的导数。具体而言，函数接受当前状态 state、时间步 step、当前时间 t 和时间步长 dt 作为参数，然后使用系统的动力学方程计算系统状态的导数。

在这个例子中，系统状态由四个变量表示：杆的角度 th、杆的角速度 dth、轮子的角度 phi 和轮子的角速度 dphi。函数derivate返回这四个变量的导数，用于在数值积分中更新系统的状态。随后，通过使用 solve 函数，在时间范围内（0到10秒）对系统进行数值积分，并得到系统在每个时间点上的状态，结果存储在 solution 变量中。

def derivate(state, step, t, dt):
    dth, th, dphi, phi = state
    _dphi = (m * l * r * dth ** 2 * sin(th) - m * g * r * sin(th) * cos(th)) / (m * r ** 2 * sin(th) ** 2 + I)
    _dth = (g * sin(th) - r * _dphi * cos(th)) / l
    return [_dth, dth, _dphi, dphi]
 
 
times = np.linspace(0, 10, 500)
solution = solve([0.0, pi / 12, .0, .0], times, integrate_rk4, derivate)

（5）下面的这段代码定义了一个名为 render 的函数，用于可视化模拟系统的运动过程。具体而言，函数render接受一个包含系统在不同时间点上状态的 solution 数组，并通过 matplotlib 库创建一个动画。在每个时间点上，函数计算轮子和杆的位置，然后绘制这些位置的动画。在动画中，绘制了轮子、轮子上的标记、杆和质点的运动轨迹。这样的动画有助于直观地理解系统的运动行为。

def render(solution):
    theta = solution[:, 1]
    phi = solution[:, 3]
    wheel_x = phi * r
    spot_r = 0.7 * r
    wheel_spot_x = wheel_x + spot_r * cos(phi - pi / 2)
    wheel_spot_y = r - spot_r * sin(phi - pi / 2)
    mass_x = wheel_x + l * cos(theta - pi / 2)
    mass_y = r - l * sin(theta - pi / 2)
    fig = plt.figure()
    ax = fig.add_subplot(111, autoscale_on=False, xlim=(-1.5, 1.5), ylim=(-1.5, 1.5))
    ax.set_aspect('equal')
    ax.grid(True)
    line, = ax.plot([], [], 'k-', lw=2)
    wheel = plt.Circle((0.0, r), r, color='black', fill=False, lw=2)
    wheel_spot = plt.Circle((0.0, spot_r), 0.02, color='red')
    mass = plt.Circle((0.0, 0.0), 0.1, color='black')
 
    def init():
        return []
 
    def animate(i):
        wheel.set_center((wheel_x[i], r))
        wheel_spot.set_center((wheel_spot_x[i], wheel_spot_y[i]))
        mass.set_center((mass_x[i], mass_y[i]))
        line.set_data([wheel_x[i], mass_x[i]], [r, mass_y[i]])
        patches = [line, ax.add_patch(wheel), ax.add_patch(wheel_spot), ax.add_patch(mass)]
        return patches
 
    ani = animation.FuncAnimation(fig, animate, np.arange(1, len(solution)),interval=25, blit=True, init_func=init)
    plt.close(fig)
    return ani
'
运行
运行

在上述代码中，函数render中的各个变量的具体说明如下所示。

1. theta：杆的角度随时间变化的数组。
2. phi：轮子的角度随时间变化的数组。
3. wheel_x：轮子中心在 x 轴上的位置。
4. spot_r：用于表示轮子上的标记的半径。
5. wheel_spot_x：轮子上标记的 x 轴坐标。
6. wheel_spot_y：轮子上标记的 y 轴坐标。
7. mass_x：杆上的质点在 x 轴上的位置。
8. mass_y：杆上的质点在 y 轴上的位置。

（6）使用前面定义的函数render创建一个动画对象ani，表示了系统在不同时间点上的运动过程。随后，通过调用 ani.save 方法，将动画保存为 GIF 文件（'free_robot.gif'），使用 ImageMagick 作为写入器，设置帧率为 24 帧每秒。

ani = render(solution)
ani.save('free_robot.gif', writer='imagemagick', fps=24)

执行后会生成了一个展示自由摆动机器人运动的 GIF 动画文件，如图7-1所示。

图7-1  自由摆动的机器人

2. 反馈控制

接下来需要为本项目添加反馈功能：角度和速度控制，以调节系统的竖直位置和速度。在控制系统中，反馈回路涉及持续监测系统的输出，并根据期望状态与实际状态之间的差异调整输入。在本项目中使用PID控制算法实现反馈控制功能，以影响系统的行为并保持稳定性。

（1）实现PID（比例-积分-微分）控制类PIDController，用于调节系统的输出，使其接近或保持在期望值。类PIDController的主要成员属性和方法如下所示。

1. __init__：类的构造函数，用于初始化PID控制器的参数和初始状态。k_p、k_d、k_i分别表示比例、微分和积分部分的增益，target表示期望的目标值，init_value表示初始状态。
2. get_control：获取PID控制器的输出，根据当前的测量值（value）和时间步长（dt）计算PID控制器的输出值。其中包括比例、微分和积分三个部分。
3. set_target：设置PID控制器的目标值，即调节系统输出的期望值。

通过这个PID控制器，可以通过比例、微分和积分部分的组合来调整系统，使其更好地满足期望值。

class PIDController:
    def __init__(self, k_p, k_d, k_i, target, init_value=0.0):
        self.Kp = k_p
        self.Kd = k_d
        self.Ki = k_i
        self.target = target
        self.lastValue = init_value
        self.integral_error = 0.0
    
    def get_control(self, value, dt):
        """
            返回PID控制。
            通过 dError/dt = -dInput/dt 缓解了导数的剧烈变化。
        """
        error = self.target - value
        derivative = -(value - self.lastValue) / dt
        self.lastValue = value
        self.integral_error += error * dt
        return self.Kp * error + self.Kd * derivative + self.Ki * self.integral_error
    
    def set_target(self, target):
        self.target = target
'
运行
运行

（2）下面这段代码定义了两个摩擦系数（b1和b2），分别描述了机器人身体和轮子旋转时的摩擦效应。接着创建了两个PID控制器对象，th_pid用于调节机器人的角度，velocity_pid用于调节机器人的角速度。这些控制器通过设定不同的比例、微分和积分增益，以及目标值，实现对机器人运动中角度和角速度的精准控制。PID控制器通过反馈调节系统状态，使其稳定地达到目标值，从而增强机器人的运动性能。

# 轮子摩擦系数
b1 = 0.01
b2 = 0.01
 
th_pid = PIDController(k_p=10.0, k_d=2.5, k_i=0.0, target=0.0)
velocity_pid = PIDController(k_p=0.002, k_d=0.0, k_i=0.001, target=0.0)

（3）函数limit的功能是限制输入值 v 在指定的上下限范围内。如果 v 超过上限 lim，则返回上限值；如果 v 低于下限 -lim，则返回下限值；否则返回原始值 v。这样的限制函数通常用于确保某个变量在特定范围内，避免超出系统可接受的范围，保证控制系统的稳定性。在控制系统中，这种函数可用于限制控制信号的幅度。

def limit(v, lim):
    if v > lim:
        return lim
    elif v < -lim:
        return -lim
    else:
        return v

（4）函数 derivate_velocity_pid 实现了一个控制机器人角速度的微分方程，通过调用之前定义的PID控制器 velocity_pid，计算出目标角速度 th_target，并将其作为输入用于另一个PID控制器 th_pid，从而得到控制输入 u。接着，使用限制函数 limit 将控制输入 u 限制在 ±10的范围内，以确保控制信号不会过大。最终，根据系统动力学方程，计算出角度变化速度 _dth 和角速度变化率 _dphi，用于更新系统状态。这一过程通过PID控制来调整机器人的运动，使其按照预期的角速度进行控制。

def derivate_velocity_pid(state, step, t, dt):
    dth, th, dphi, phi = state
    th_target = velocity_pid.get_control(dphi, dt)
    th_pid.set_target(th_target)
    u = -th_pid.get_control(th, dt)
    u = limit(u, 10)
    s = sin(th)
    c = cos(th)
    _dphi = (m * r * (l * dth ** 2 * s + b1 * dth * c - g * s * c) - b2 * dphi + u) / (I + m * r ** 2 * s ** 2)
    _dth = (g * s - r * _dphi * c - b1 * dth) / l
    return [_dth, dth, _dphi, dphi]
'
运行
运行

（5）下面的代码首先通过 solve 函数求解了机器人在给定初始条件下的角度和角速度的动态演化过程。接着，利用 render 函数生成了对应的运动动画 ani。最后，通过 ani.save 方法将该动画保存为 GIF 文件（'velocity_pid.gif'），以便进一步分析和展示机器人在角速度控制下的运动行为。

solution = solve([0.0, pi / 12, .0, .0], times, integrate_rk4, derivate_velocity_pid)
ani = render(solution)
ani.save('velocity_pid.gif', writer='imagemagick', fps=24)

GIF 文件（'velocity_pid.gif'）的效果如图7-2所示。

图7-2  机器人在角速度控制下的运动行为

（6）定义一个新的 PID 控制器 position_pid 用于控制机器人的位置。PID 控制器的参数被设置为 k_p=0.07（比例增益）、k_d=0.07（微分增益）、k_i=0.0（积分增益），目标值为 0.0。

th_pid = PIDController(k_p=40.0, k_d=20.0, k_i=0.0, target=0.0)
position_pid = PIDController(k_p=0.07, k_d=0.07, k_i=0.0, target=0.0)

3. 位置控制

这里的位置控制也是反馈控制的一种，具体实现流程如下所示。

（1）定义了一个新的微分方程函数 derivate_position_pid，该函数利用 position_pid 控制器来计算目标位置的角度，并结合 th_pid 控制器来生成最终的控制输入。此控制输入用于调整机器人的角度和角速度，从而实现对位置的控制。最后，通过数值积分方法（Runge-Kutta）对该微分方程进行求解，得到机器人在一段时间内的运动状态。

def derivate_position_pid(state, step, t, dt):
    dth, th, dphi, phi = state
    th_target = position_pid.get_control(phi, dt)
    th_pid.set_target(th_target)
    u = -th_pid.get_control(th, dt)
    u = limit(u, 10)
    s = sin(th)
    c = cos(th)
    _dphi = (m * r * (l * dth ** 2 * s + b1 * dth * c - g * s * c) - b2 * dphi + u) / (I + m * r ** 2 * s ** 2)
    _dth = (g * s - r * _dphi * c - b1 * dth) / l
    return [_dth, dth, _dphi, dphi]
'
运行
运行

（2）使用前面定义的 函数solve，通过数值积分方法（Runge-Kutta）求解了机器人在给定初始条件下，通过位置和角度的 PID 控制器（position_pid 和 th_pid）控制的运动过程。接着，通过 render 函数生成了对应的运动动画 ani。最后，通过 ani.save 方法将该动画保存为 GIF 文件（'position_pid.gif'），以便进一步分析和展示机器人在位置控制下的运动行为。

solution = solve([0.0, pi / 12, .0, .0], times, integrate_rk4, derivate_position_pid)
ani = render(solution)
ani.save('position_pid.gif', writer='imagemagick', fps=24)

GIF 文件（'position_pid.gif'）的效果如图7-3所示。

图7-3  机器人在位置控制下的运动行为