控制器设计篇

前面介绍了利用matlab实现系统的模型辨识，本篇将在运动学模型的基础上，采用Matlab工具箱进行位置控制器的设计，并在仿真环境中进行了实验验证，所采用的控制器设计流程也适用于其他控制对象，简要设计流程如下

1. 数据采集：本文旨在进行位置控制。在完成无人机速度环和姿态环的基础上，输入激励信号，以尽可能引发被控对象的特性。可以通过三角函数叠加的方式进行激励信号的输入。同时，确定期望速度并采集实际速度。
2. 模型辨识：考虑到内环中动态不确定性的存在，导致期望速度与实际速度之间存在一定的偏差。为了解决这一问题，需要对速度的传递函数进行辨识。通过对速度的辨识，并经过积分环节，可以得到速度到位置的模型。这一模型能够更准确地反映系统的动态特性，从而提高位置控制的精度和鲁棒性。
3. 控制器参数整定：借助Matlab控制系统设计工具箱，可以导入已经辨识的模型信息。在这个过程中，通过考虑控制指标的要求，可以自动完成对控制器参数的整定。通过这一工具箱，系统可以更加智能地调整控制器的参数，以满足特定的性能和稳定性指标。这样的自动化参数整定过程能够有效地提高系统的响应速度、抑制振荡，从而实现更为精确和可靠的位置控制。
4. 控制器设计：经过参数的自动整定，将所得到的控制器参数进行编程实现，以完成对实际控制对象的闭环控制。通过将控制器嵌入系统，能够实现实时的控制和反馈机制，确保系统能够稳定地追踪期望位置。

1.Matlab控制器设计工具箱

model_x=tf(P2(1,1));
model_y=tf(P2(2,2));

从输入u1到输出y1：传递函数为：

\frac{3.384}{0.004499s^2+0.1367s+1}

从输入u2到输出y2：传递函数为：

\frac{3.825}{ 2.335e-06s^2 + 0.1551s + 1}

此时的模型为期望速度到实际速度的模型，控制任务为位置控制，即给定期望位置得到运动体的位置，将此速度模型与积分器相乘可得到期望速度到位置的模型，即V(s)*\frac{1}{s}

打开Matlab控制系统设计工具箱：

选择结构如下：输入是期望位置，输出是实际位置，G为控制对象，即为V(s)*\frac{1}{s}的模型，C是待设计的控制器：

导入被控对象模型：

得到C=1的控制效果：

选择PID控制器，进行参数整定

PID参数整定：

在界面上可以看到两个滑动条，它们分别代表了“响应时间”和“过渡行为”：

1. 响应时间（Response Time）：此滑动条调整控制系统响应到目标值所需要的时间。向“更快（Faster）”调整将尝试缩短系统达到设定点的时间，这通常会增加控制器的比例增益，但也可能增加超调量（即系统响应超过设定点的幅度）和振荡。相反，向“更慢（Slower）”调整可能会减少比例增益，从而使系统响应更加温和，但响应时间会更长。
2. 过渡行为（Transient Behavior）：这个滑动条调节的是系统从一个状态过渡到另一个状态时的动态表现。向“积极（Aggressive）”调整可能会增加系统的积分和/或微分作用，使得系统更快地消除误差，但可能会引起更大的超调和振荡。向“稳健（Robust）”调整通常会减少这些参数，导致更平滑的过渡，但响应可能较慢。

此时的响应曲线为：

可进行反复调试，得到满意的曲线，但调试过程中需要考虑被控对象执行机构的输出饱和，本文例：当前位置为0，期望为1，最大速度为1，则至少需要1s的时间被控对象才能到达目标位置，当小于1s时会发生执行器饱和的现象，所以调节响应时间时不要过小。

也可使用Matlab命令pidTuner进行PID参数整定：

matlab s = tf('s'); tf_modelx=model_x; intger_modes=1/s; tf_with_ingter=tf_modelx*intger_modes; pidTuner(tf_with_ingter)

得到PID参数：

K_p=4.487,K_i=5.445,K_d=0.5458

同理得到y方向的PID控制参数为：

K_p=4.007,K_i=7.379,K_d=0.5439

根据获取的参数进行控制设计。

4.控制器设计

离散PID控制器的形式：

\mathrm{u}(k)=K_p\Bigg(e(k)+\frac{T}{T_i}\sum_{n=0}^ke(n)+\frac{T_d}{T}\Big(e(k)-e(k-1)\Big)\Bigg)

比例系数：K_p，积分系数：K_p \frac{T}{T_i}，可以用K_i表示；微分系数：K_p\frac{T_d}{T}，可以用K_d表示。

代码实现：

import math
import time
import matplotlib.pyplot as plt

import gym
import numpy as np

import gym_drones


class PIDController:
    def __init__(self, kp, ki, kd):
        self.kp = kp
        self.ki = ki
        self.kd = kd
        self.prev_error = 0
        self.integral_sum = 0
        self.T = 0.001

    def calculate(self, error):
        # Proportional term
        P = self.kp * error

        # Integral term
        self.integral_sum += error
        I = self.ki * self.T * self.integral_sum

        # Derivative term
        D = self.kd * (error - self.prev_error) / self.T
        self.prev_error = error

        # PID output
        output = P + I + D
        return output


env = gym.make('tello-v0', gui=True)
env.seed(0)
print(gym_drones)
action_space = env.action_space

# 创建更多的插值点
num_points = 200
t = np.linspace(0, 1, num_points)
obs, initial_info = env.reset()
terminated = False
angle_rad = math.atan2(obs[1], obs[0])

# 将弧度转换为角度
angle_deg = math.degrees(angle_rad)
# 确保角度在 0 到 360 度之间
angle_deg = angle_deg % 360
random_action = np.array([[0.0, 0.0, 0.0, 1.0]])
radius = 1.5
angular_velocity = 0.1
current_count = round(angle_deg / angular_velocity)
# 存储轨迹数据
des_x_trajectory = []
des_y_trajectory = []
real_x_trajectory = []
real_y_trajectory = []
error_x_trajectory = []
error_y_trajectory = []
count_turns = 0
# Example usage
pid_controller_x = PIDController(kp=4.4, ki=5.4, kd=0.545)
pid_controller_y = PIDController(kp=4.0, ki=7.4, kd=0.543)

last_real_x = 0
last_real_y = 0

while True:
    if terminated:
        env.reset()
    angle = current_count * angular_velocity
    des_x = radius * math.cos(math.radians(angle))
    des_y = radius * math.sin(math.radians(angle))
    real_x = obs[0]
    real_y = obs[1]

    # 存储轨迹数据
    des_x_trajectory.append(des_x)
    des_y_trajectory.append(des_y)
    real_x_trajectory.append(real_x)
    real_y_trajectory.append(real_y)
    error_x = des_x - real_x
    error_y = des_y - real_y
    desv_x = pid_controller_x.calculate(error_x)
    desv_y = pid_controller_y.calculate(error_y)
    error_x_trajectory.append(error_x)
    error_y_trajectory.append(error_y)

    desv_x = np.clip(desv_x, -1, 1)
    desv_y = np.clip(desv_y, -1, 1)

    # 控制器部分
    random_action[0][0] = desv_x
    random_action[0][1] = desv_y

    random_action[0][2] = 0
    random_action[0][3] = 1
    random_action = np.clip(random_action, -1, 1)

    obs, reward, terminated, info = env.step(random_action)
    current_count += 1
    if angle > 360:
        current_count = 0
        count_turns += 1
    if count_turns > 1:
        break
    last_real_x = real_x
    last_real_y = real_y

5.控制效果

实际轨迹与期望轨迹：

跟踪误差：

动态效果：

6.总结

本文在上篇文章模型的基础上，利用Matlab工具箱设计了位置控制器，并在仿真环境中对其进行了实验验证。所采用的控制器设计流程具有普遍适用性，可应用于其他类型的控制对象。