(3-3)速度控制算法：基于MPC的速度控制算法和基于FLC的速度控制算法

3.3.1  基于MPC的速度控制算法

模型预测控制（Model Predictive Control，MPC）是一种先进的控制策略，其主要思想是在每个时刻通过对未来一定时间内系统行为进行预测，选择当前时刻的最优控制输入，以满足特定的性能指标。

1. 基本概念

1. 动态模型：MPC基于系统的动态模型，该模型描述了系统在一段时间内如何演变。这可以是基于物理方程、系统辨识或数据驱动的模型。
2. 预测优化：MPC通过对未来一定时间内系统行为的预测进行优化，选择当前时刻的最优控制输入。通过最小化或最大化性能指标，MPC寻找使系统行为最符合期望的控制策略。
3. 时变性能：由于在每个时刻都进行重新优化，MPC具有时变性能，能够适应系统动态变化和外部扰动。
4. 约束处理：MPC考虑系统的约束条件，包括控制输入的范围、状态变量的限制等。优化问题中的约束确保系统操作在可行范围内。

2. 实现步骤

（1）系统建模

1. 动态模型：首先，对被控制的系统建立一个动态数学模型。这个模型描述了系统的演变方式，通常基于物理方程、系统辨识或其他数学方法得到。
2. 状态方程：模型包括一个状态方程，描述系统当前状态如何从过去状态和控制输入演变而来。

（2）控制目标设定

1. 性能指标：定义系统的性能指标，即要优化的目标。这可以包括最小化误差、最小化能量消耗等。
2. 约束条件：确定系统操作的约束条件，如控制输入的范围、状态变量的限制等。

（3）建立优化问题

1. 预测模型：利用系统动态模型，建立一个包含多个时刻的预测模型，预测系统在未来一定时间内的演变情况。
2. 控制输入序列：引入一个控制输入序列，通过优化算法在预测时段内寻找最优的控制输入序列，使得性能指标最优。
3. 约束条件：将系统的约束条件纳入优化问题，确保得到的解是可行的。

（4）优化问题求解

使用优化求解器（例如二次规划求解器）来求解建立的优化问题，找到最优的控制输入序列。

（5）实施控制

1. 应用控制输入：从优化问题中得到的最优控制输入序列中提取当前时刻的控制输入，应用于实际系统。
2. 系统演变：系统根据动态模型演变到下一个时刻。

（6）重复迭代：

1. 更新状态：根据系统的反馈信息，更新当前状态。
2. 重复：重复上述步骤，不断迭代，每个时刻都重新解决优化问题，生成新的控制输入。

请看下面的例子，使用基于MPC（模型预测控制）的速度控制算法模拟机器人在目标速度轨迹上运动，通过随机生成控制输入来调整机器人速度，最终绘制了实际速度和目标速度的对比图。

实例3-3：使用基于MPC控制机器人的移动和速度（源码路径：codes\3\speed\mpc.py）

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

import numpy as np
import matplotlib.pyplot as plt

# 机器人动力学模型
A = 1.0
B = 0.1

# MPC参数
N = 10
Q = np.eye(N)
R = 0.1 * np.eye(N)

# 目标速度
target_velocity = 5.0

# 目标轨迹生成
desired_trajectory = target_velocity * np.ones(N)

# 模拟机器人速度演变
current_velocity = 0.0
velocity_history = [current_velocity]

for _ in range(50):
    # 优化控制输入
    u_optimal = np.random.rand(N)  # 这里简单地使用随机生成的控制输入，实际应用需要使用MPC求解器

    # 模拟机器人速度演变
    for i in range(N):
        current_velocity += A * u_optimal[i] - B * current_velocity
        velocity_history.append(current_velocity)

# 绘制结果
plt.plot(velocity_history, label='Robot Velocity')
plt.plot(desired_trajectory, label='Desired Velocity', linestyle='--')
plt.xlabel('Time Steps')
plt.ylabel('Velocity')
plt.legend()
plt.title('MPC-based Velocity Control for Robot')
plt.show()

上述代码的实现流程如下所示：

1. 首先，定义机器人的动力学模型参数A和B，以及MPC的相关参数N、Q、R和目标速度。
2. 接着，生成目标速度轨迹，并通过随机生成的控制输入模拟机器人速度演变。
3. 最后，利用matplotlib绘制实际速度和目标速度的对比图，展示MPC算法在速度控制中的应用。如图3-3所示。

图3-3  实际速度和目标速度的对比图

3.3.2  基于FLC的速度控制算法

模糊逻辑控制（Fuzzy Logic Control，FLC）是一种基于模糊集合理论的控制方法，适用于复杂、不确定或模糊的系统。

1. 基本概念

1. 模糊化：将输入（速度误差、变化率等）和输出（控制输入）通过模糊集合映射到隶属度函数上，将实际问题的模糊特性进行建模。
2. 规则库：设计一组模糊规则，其中每个规则定义了在不同输入条件下应该采取的控制动作。
3. 推理：根据输入的模糊集合和规则库，通过推理得到模糊输出。
4. 去模糊化：将模糊输出映射回实际的控制输入，以便应用到系统。

2. 实现步骤

（1）定义输入和输出：确定速度控制系统的输入变量（如速度误差、变化率）和输出变量（控制输入）。

（2）模糊集合和隶属度函数：为每个输入和输出变量定义模糊集合，并指定它们的隶属度函数，描述了输入和输出的模糊关系。

（3）规则库设计：创建一组规则，规定在不同输入条件下应该采取的控制策略。规则的设计依赖于系统的特性和控制目标。

（4）推理过程：根据当前输入值，通过模糊推理得到模糊输出。

（5）去模糊化：将模糊输出映射为具体的控制输入，采用去模糊化方法，如加权平均。

在实际应用中，既可以使用模糊逻辑控制工具库，也可以手动编写模糊逻辑控制的算法。FLC广泛应用于各种领域，特别是在控制系统中，以处理模糊性和不确定性。

例如下面的实例演示了基于模糊逻辑控制（FLC）的速度控制算法的实现过程，通过定义速度误差和变化率的输入变量以及控制输入的输出变量，并设计对应的隶属度函数和规则库，通过模糊推理来确定控制输入。

实例3-4：控制系统重的速度控制算法（源码路径：codes\3\speed\fl.py）

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

import numpy as np
import skfuzzy as fuzz
from skfuzzy import control as ctrl
import matplotlib.pyplot as plt

# 输入变量
error = ctrl.Antecedent(np.arange(-10, 11, 1), 'error')
change_rate = ctrl.Antecedent(np.arange(-5, 6, 1), 'change_rate')

# 输出变量
control_input = ctrl.Consequent(np.arange(-1, 2, 1), 'control_input')

# 定义隶属度函数
error['negative'] = fuzz.trimf(error.universe, [-10, -5, 0])
error['zero'] = fuzz.trimf(error.universe, [-5, 0, 5])
error['positive'] = fuzz.trimf(error.universe, [0, 5, 10])

change_rate['negative'] = fuzz.trimf(change_rate.universe, [-5, -3, 0])
change_rate['zero'] = fuzz.trimf(change_rate.universe, [-3, 0, 3])
change_rate['positive'] = fuzz.trimf(change_rate.universe, [0, 3, 5])

control_input['decelerate'] = fuzz.trimf(control_input.universe, [-1, -1, 0])
control_input['maintain'] = fuzz.trimf(control_input.universe, [-1, 0, 1])
control_input['accelerate'] = fuzz.trimf(control_input.universe, [0, 1, 1])

# 规则库
rule1 = ctrl.Rule(error['negative'] & change_rate['negative'], control_input['decelerate'])
rule2 = ctrl.Rule(error['zero'] & change_rate['zero'], control_input['maintain'])
rule3 = ctrl.Rule(error['positive'] & change_rate['positive'], control_input['accelerate'])

# 控制系统
speed_control = ctrl.ControlSystem([rule1, rule2, rule3])
speed_control_simulation = ctrl.ControlSystemSimulation(speed_control)

# 设置输入值
speed_control_simulation.input['error'] = -3
speed_control_simulation.input['change_rate'] = 2

# 进行模糊推理
speed_control_simulation.compute()

# 输出结果
print("Control Input:", speed_control_simulation.output['control_input'])

# 可视化结果
error.view(sim=speed_control_simulation)
change_rate.view(sim=speed_control_simulation)
control_input.view(sim=speed_control_simulation)
plt.show()

上述代码的实现流程如下所示：

1. 首先，定义速度误差和变化率的输入变量，并确定它们的隶属度函数，以反映模糊性。
2. 然后，设计输出变量（控制输入）的隶属度函数和规则库，规定不同输入条件下的控制策略。
3. 接着，通过模糊推理，根据输入变量的模糊值和规则库，计算出输出变量（控制输入）的模糊值。
4. 最后，通过去模糊化操作，将模糊输出转换为具体的控制输入值，并输出结果，展示模糊逻辑控制在速度调节中的应用。

执行后会绘制模糊逻辑控制算法中隶属度函数和规则库的可视化图，如图3-4所示。这个图展示了输入变量（速度误差和变化率）以及输出变量（控制输入）的隶属度函数，同时显示了规则库中定义的各个规则的权重。通过这样的可视化，可以直观地了解模糊逻辑控制中输入输出关系的模糊表示。

图3-4  模糊逻辑控制算法的可视化图