控制（Controls）
1.PID控制简介
在工程实际中，应用最为广泛的调节器控制规律为比例、积分、微分控制，简称PID控制，又称PID调节。PID控制器问世至今已有近70年历史，它 以其结构简单、稳定性好、工作可靠、调整方便而成为工业控制的主要技术之一。当被控对象的结构和参数不能完全掌握，或得不到精确的数学模型时，控制理论的 其它技术难以采用时，系统控制器的结构和参数必须依靠经验和现场调试来确定，这时应用PID控制技术最为方便。即当我们不完全了解一个系统和被控对象，或 不能通过有效的测量手段来获得系统参数时，最适合用PID控制技术。PID控制，实际中也有PI和PD控制。PID控制器就是根据系统的误差，利用比例、 积分、微分计算出控制量进行控制的。
比例（P）控制
比例控制是一种最简单的控制方式。其控制器的输出与输入误差信号成比例关系。当仅有比例控制时系统输出存在稳态误差（Steady-state error）。
积分（I）控制
在积分控制中，控制器的输出与输入误差信号的积分成正比关系。对一个自动控制系统，如果在进入稳态后存在稳态误差，则称这个控制系统是有稳态误差的 或简称有差系统（System with Steady-state Error）。为了消除稳态误差，在控制器中必须引入“积分项”。积分项对误差取决于时间的积分，随着时间的增加，积分项会增大。这样，即便误差很小，积 分项也会随着时间的增加而加大，它推动控制器的输出增大使稳态误差进一步减小，直到接近于零。因此，比例+积分（PI）控制器，可以使系统在进入稳态后几乎无稳 态误差。
微分（D）控制
在微分控制中，控制器的输出与输入误差信号的微分（即误差的变化率）成正比关系。自动控制系统在克服误差的调节过程中可能会出现振荡甚至失稳。其原因是由于存在有较大惯性组件（环节）或有滞后（delay）组件，具有抑制误差的作用， 其变化总是落后于误差的变化。解决的办法是使抑制误差的作用的变化“超前”，即在误差接近零时，抑制误差的作用就应该是零。这就是说，在控制器中仅引入 “比例”项往往是不够的，比例项的作用仅是放大误差的幅值，而需要增加的是“微分项”，它能预测误差变化的趋势，这样，具有比例+微分的控制器，就能 够提前使抑制误差的控制作用等于零，甚至为负值，从而避免了被控量的严重超调。所以对有较大惯性或滞后的被控对象，比例+微分（PD）控制器能改善系统在 调节过程中的动态特性。
2.PID控制模拟器
这里给出了两个图表。
第一个图是四旋翼的高度与时间的关系图。可以看到我们也画出了10米的设定点。最终的目标是尽可能快地达到这个设定值，并保持最小的振荡和超调。
第二张图是四轴飞行器对时间的速度。理想的情况是，在到达设定值之前，速度增加，到达设定值之后，速度迅速下降，保证四轴飞行器稳定在设定高度。
在这里，我们看到对四轴飞行器施加了1.7的连续控制力。我们可以看到这不是一个好的解决方案，因为四轴飞行器将继续上升超过设定点。我们将构建一个更好的控制器，使之控制工作时的波动，以使我们保持在设定点。
我们将使用的下一个图表是控制工作量与时间的关系。控制力是执行器为达到设定点而需要施加的力。理想情况下，对于一个良好的控制器，它不包括初始致动以达到设定点，我们希望通过配备一个调整良好的控制器来最大程度地减少控制工作量。
在这里，我们可以看到一直处于1.7的控制之下，它从未被最小化，这是我们需要解决的问题。
首先来看下面三个代码库，熟悉代码库将会有助于接下来的任务。由于我没有使用虚拟机，而是使用了pycharm。所以需要将hover_plot.py等文件所在的路径添加进系统的环境变量中，然后程序才可以运行。
hover_plot.py：
import numpy as np
import matplotlib.pyplot as plt
from open_controller import Open_Controller
from quad1d_eom import ydot
##################################################################################
# 应用一个值为1.7的恒定的作用力。
control_effort = 1.7
##################################################################################
# 仿真参数
N = 500 # 仿真的点数
t0 = 0 # 起始时间(sec)
tf = 30 # 结束时间(sec)
time = np.linspace(t0, tf, N)
dt = time[1] - time[0] # delta t, (sec)
##################################################################################
# 核心仿真代码
# 初始条件 (i.e., 初始状态向量)
y = [0, 0]
#y[0] = initial altitude, (m)
#y[1] = initial speed, (m/s)
# 初始化数组以存储值
soln = np.zeros((len(time), len(y)))
# 创建Open_Controller类的实例
controller = Open_Controller()
# 设置作用力
controller.setControlEffort(control_effort)
# 设定高度目标
r = 10 # meters
controller.setTarget(r)
# 模拟四轴运动
j = 0 # 计数器
for t in time:
# 评估下一个时间点的状态
y = ydot(y, t, controller)
# 储存结果
soln[j, :] = y
j += 1
##################################################################################
# 绘制结果
# 图一:四轴飞行器高度随时间的变化的曲线
SP = np.ones_like(time)*r # 高度设置点
fig = plt.figure()
ax1 = fig.add_subplot(211)
ax1.plot(time, soln[:, 0], time, SP, '--')
ax1.set_xlabel('Time, (sec)')
ax1.set_ylabel('Altitude, (m)')
# 图二:这是四轴直升机的速度随时间的变化曲线
ax2 = fig.add_subplot(212)
ax2.plot(time, soln[:, 1])
ax2.set_xlabel('Time, (sec)')
ax2.set_ylabel('Speed, (m/s)')
plt.tight_layout()
plt.show()
# 图三:这是四轴直升机的作用力随时间变化的曲线
fig2 = plt.figure()
ax3 = fig2.add_subplot(111)
ax3.plot(time, controller.effort_applied, label='effort', linewidth=3, color = 'red')
ax3.set_xlabel('Time, (sec)')
ax3.set_ylabel('Control Effort')
h, l = ax3.get_legend_handles_labels()
ax3.legend(h, l)
plt.tight_layout()
plt.show()
##################
y0 = soln[:, 0] # 高度
rise_time_index = np.argmax(y0>r)
RT = time[rise_time_index]
print("The rise time is {0:.3f} seconds".format(RT))
OS = (np.max(y0) - r)/r*100
if OS < 0:
OS = 0
print("The percent overshoot is {0:.1f}%".format(OS))
print("The offset from the target at 30 seconds is {0:.3f} meters".format(abs(soln[-1, 0]-r)))
open_controller.py：
##################################################################################
# 这是控制类，我们将在接下来对其进行更改。
##################################################################################
# 创建一个Open_Controller类
class Open_Controller:
# 定义类的初始化序列
def __init__(self, start_time=0):
# 创建一个类变量来存储启动时间
self.start_time_ = start_time
# 创建一个类变量来存储作用力
self.u = 0
# 创建一个类变量来存储最后的时间戳
self.last_timestamp_ = 0
# 创建一个类变量来存储设置点
self.set_point_ = 0
# 为所有应用的控制工作创建一个类变量
self.effort_applied = []
# 设置高度设置点
def setTarget(self, target):
self.set_point_ = float(target)
# 设置所需的作用力
def setControlEffort(self, control_effort):
self.u = float(control_effort)
# 检索当前控制工作效果
def getControlEffort(self, time):
# 存储最后的时间戳
self.last_timestamp_ = time
# 储存作用力
self.effort_applied.append(self.u)
return self.u
quad1d_eom.py
import numpy as np
import matplotlib.pyplot as plt
from open_controller import Open_Controller
##################################################################################
# DO NOT MODIFY ANY PORTION OF THIS FILE
# 这个文件代表了一维四旋翼的动力学方程
##################################################################################
def ydot(y, t, controller):
'''
返回下一个时间步长的状态向量
参数:
----------
y(k) = 状态向量, a length 2 list
= [高度, 速度]
t = time, (sec)
pid = PID Controller类的实例
return
-------
y(k+1) = [y[0], y[1]] = y(k) + ydot*dt
'''
# 模型状态
y0 = y[0] # 高度, (m)
y1 = y[1] # 速度, (m/s)
# 模型参数
g = -9.81 # 重力, m/s/s
m = 1.54 # 四轴飞行器重量, kg
c = 10.0 # 机电系统的传输常数
# 时间步长, (sec)
dt = t - controller.last_timestamp_
# 作用力
u = controller.getControlEffort(t)
# State derivatives
if (y0 <= 0.):
# 如果控制输入，u <=重力，飞行器在地面上保持静止，这样可以防止四旋翼在推力太小时“坠落”地面。
if u <= np.absolute(g*m/c):
y0dot = 0.
y1dot = 0.
else: # 否则，如果u>重力，则四轴加速向上
y0dot = y1
y1dot = g + c/m*u - 0.75*y1
else: # 或者四轴飞行器已经升空
# y0dot为速度大小
y0dot = y1
# y1dot为加速度大小，其中0.75*y1为阻力大小
y1dot = g + c/m*u - 0.75*y1
y0 += y0dot*dt
y1 += y1dot*dt
return [y0, y1]
运行hover_plot.py输出如下：
其实原本输出两个图的，我稍微改了一下程序，让三个图输出在一个图中，这三张图和前面给出的图是一样的。
3.比例控制
（1）比例控制介绍
假设四旋翼飞机的传感器可以完美地测量其高度，也许可以使用气压计，GPS或超声波传感器。最初，四旋翼飞行器已通电，但静止不动在启动板上。激活后，预编程的飞行路径将使四旋翼飞行器最大上升至10米，将其悬停在原处5秒钟，然后在保持恒定高度的情况下向东北方向行驶。
在四旋翼飞机开始执行飞行计划的第一瞬间，高度误差为：
e
(
t
)
=
r
−
y
(
t
)
e(t) \,= r-y(t)
e(t)=r−y(t)
=
10
m
−
0
\qquad =10m-0
=10m−0
=
10
m
\qquad=10m
=10m
控制器感应到较大的误差，并命令电动机产生与误差成正比的垂直推力，即
u ( t ) = K p ∗ e ( t ) u(t)=K_p*e(t) u(t)=Kp∗e(t)
当四旋翼飞机接近所需高度时，误差减小，因此控制器输入也减小。
比例增益如何放大“当前”（当前时间步长）误差，而不考虑过去或将来的误差。
许多现实世界的系统都由二阶微分方程控制，并且在设置为某个稳态值之前，会对阶跃输入表现出振荡响应。此振荡响应的主要特征如下所示：
-
Rise time （上升时间） T r T_r Tr ,是响应从指定的低值移动到指定的高值所需的时间，通常表示为最终值的百分比。例如，其最终(稳态)值的0%到100%。
-
Peak time（峰值时间） T p T_p Tp，达到第一个超调的峰值需要时间
-
Maximum percent overshoot （最大超调量）： M O S = y ( T p ) − y ( T s s ) y ( T s s ) × 100 % M_{OS}=\frac{y(T_p)-y(T_{ss})}{y(T_{ss})}\times100\% MOS=y(Tss)y(Tp)−y(Tss)×100%
-
Settling time（稳定时间） T s T_s Ts,响应达到并保持在最终稳定状态值(通常定义为 y s s y_{ss} yss的2 - 5%)的范围内所需要的时间。
振荡与系统内阻尼的大小密切相关。阻尼是任何消耗能量的机制的术语，例如汽车或自行车上的减震器或摩擦。
-
欠阻尼系统比重阻尼系统具有更大的振幅和频率。
-
临界阻尼系统没有振荡。
-
过阻尼系统（即，比临界阻尼更大的阻尼）也不会振荡，但是达到稳态值的时间也会增加。
（2）比例控制器编程
需要对三个文件进行修改，主要修改controller class定义的部分。
hover_plot.py:
import numpy as np
import matplotlib.pyplot as plt
from p_controller import P_Controller
from quad1d_eom import ydot
##################################################################################
# 应用一个值为1.7的恒定的作用力。
control_effort = 1.7
##################################################################################
# 仿真参数
N = 500 # 仿真的点数
t0 = 0 # 起始时间(sec)
tf = 30 # 结束时间(sec)
time = np.linspace(t0, tf, N)
dt = time[1] - time[0] # delta t, (sec)
##################################################################################
# 核心仿真代码
# 初始条件 (i.e., 初始状态向量)
y = [0, 0]
#y[0] = initial altitude, (m)
#y[1] = initial speed, (m/s)
# 初始化数组以存储值
soln = np.zeros((len(time), len(y)))
# 创建Open_Controller类的实例
controller = P_Controller()
# 设置作用力
# controller.setControlEffort(control_effort)
# 设定高度目标
r = 10.0 # meters
controller.setTarget(r)
kp = 0.2
controller.setKP(kp)
# 模拟四轴运动
j = 0 # 计数器
for t in time:
# 评估下一个时间点的状态
y = ydot(y, t, controller)
# 储存结果
soln[j, :] = y
j += 1
##################################################################################
# 绘制结果
# 图一:四轴飞行器高度随时间的变化的曲线
SP = np.ones_like(time)*r # 高度设置点
fig = plt.figure()
ax1 = fig.add_subplot(211)
ax1.plot(time, soln[:, 0], time, SP, '--')
ax1.set_xlabel('Time, (sec)')
ax1.set_ylabel('Altitude, (m)')
# 图二:这是四轴直升机的速度随时间的变化曲线
ax2 = fig.add_subplot(212)
ax2.plot(time, soln[:, 1])
ax2.set_xlabel('Time, (sec)')
ax2.set_ylabel('Speed, (m/s)')
plt.tight_layout()
plt.show()
fig2 = plt.figure()
ax3 = fig2.add_subplot(111)
ax3.plot(time, controller.u_p, label='u_p', linewidth=3, color = 'red')
ax3.set_xlabel('Time, (sec)')
ax3.set_ylabel('Control Effort')
h, l = ax3.get_legend_handles_labels()
ax3.legend(h, l)
plt.tight_layout()
plt.show()
##################
y0 = soln[:, 0] # altitude
rise_time_index = np.argmax(y0 > r)
RT = time[rise_time_index]
print("The rise time is {0:.3f} seconds".format(RT))
OS = (np.max(y0) - r)/r*100
if OS < 0:
OS = 0
print("The percent overshoot is {0:.1f}%".format(OS))
print("The steady state offset at 30 seconds is {0:.3f} meters".format(abs(soln[-1, 0]-r)))
p_controller.py:
class P_Controller:
def __init__(self, kp=0.0, start_time=0):
# P控制器可以用一个特定的kp值来激活
self.kp_ = float(kp)
# TODO:为set_point_创建内部类变量并将其设置为0.0，并将start_time_设置为start_time变量。
########################################
self.set_point_ = 0.0
self.start_time_ = start_time
########################################
# 存储最后时间戳
self.last_timestamp_ = 0.0
# 作用力记录
self.u_p = [0]
# 设置高度设置点
def setTarget(self, target):
self.set_point_ = float(target)
def setKP(self, kp):
# 使用提供的变量设置内部kp_值
self.kp_ = kp
return
def update(self, measured_value, timestamp):
# 使用last_timestamp_计算delta_time以及提供的时间戳参数
########################################
delta_time = timestamp - self.last_timestamp_
########################################
if delta_time == 0:
# 时间为0
return 0
# Calculate the error as the differnce between
# the set_point_ and the measured_value
########################################
e_dot = 0.0
e_dot = self.set_point_ - measured_value
########################################
# Set the last_timestamp_ to current timestamp
########################################
self.last_timestamp_ = timestamp
########################################
# Calculate the proportional error here. Be sure to access the
# the internal Kp class variable
########################################
p = self.kp_ * e_dot
########################################
# Set the control effort
# u is the sum of all your errors. In this case it is just
# the proportional error.
########################################
u = p
########################################
# Here we are storing the control effort history for post control
# observations.
self.u_p.append(p)
return u
quad1d_eom.py
import numpy as np
def ydot(y, t, controller):
'''
返回下一个时间步长的状态向量
参数:
----------
y(k) = 状态向量, a length 2 list
= [高度, 速度]
t = time, (sec)
pid = PID Controller类的实例
return
-------
y(k+1) = [y[0], y[1]] = y(k) + ydot*dt
'''
# 模型状态
y0 = y[0] # 高度, (m)
y1 = y[1] # 速度, (m/s)
# 模型参数
g = -9.81 # 重力, m/s/s
m = 1.54 # 四轴飞行器重量, kg
c = 10.0 # 机电系统的传输常数
# 时间步长, (sec)
dt = t - controller.last_timestamp_
# 作用力
u = controller.update(measured_value=y0, timestamp=t)
# State derivatives
if (y0 <= 0.):
# 如果控制输入，u <=重力，飞行器在地面上保持静止，这样可以防止四旋翼在推力太小时“坠落”地面。
if u <= np.absolute(g*m/c):
y0dot = 0.
y1dot = 0.
else: # 否则，如果u>重力，则四轴加速向上
y0dot = y1
y1dot = g + c/m*u - 0.75*y1
else: # 或者四轴飞行器已经升空
# y0dot为速度大小
y0dot = y1
# y1dot为加速度大小，其中0.75*y1为阻力大小
y1dot = g + c/m*u - 0.75*y1
y0 += y0dot*dt
y1 += y1dot*dt
return [y0, y1]
输出如下：
4.PI控制
（1）PI控制介绍
通常，对于可容忍的稳态误差（SSE）的数量有非常严格的设计要求，并且仅比例控制是不够的。那么如何消除或至少减少SSE？答案是，添加积分控制。
同样，这里是对其起作用的直观描述。基本思想是相对于总累积误差增加来控制输入。因此，积分控制器会考虑所有过去的系统误差值。最终结果是，即使很小的错误也会（最终）被放大，并导致控制器增加对被控对象（plant）的输入。这样，积分控制器可以消除比例控制器容易产生的较小的稳态误差。
PV =过程变量（即，测量的输出）
SP =设定点（参考信号）
此时需要考虑的一件事是如何在计算机上实际实现连续时间控制器方程式。毕竟，计算机是离散时间设备。例如，虽然您可能会看到汽车在沿着高速公路加速时的速度平稳连续变化，如下图上半部分所示，
如下部分图像所示，计算机仅以周期性的间隔“看到”样本。显然，需要积分和导数的离散时间近似值。
通过微积分，积分项代表曲线下的面积。在离散时间情况下，这可以近似为简单地将矩形相加，
∫ 0 t e ( τ ) d τ ≈ ∑ k = 1 n e k Δ t \int_0^t e(\tau) d\tau \approx \sum_{k=1}^n e_kΔt ∫0te(τ)dτ≈∑k=1nekΔt
矩形的“高度”是每个时间点的误差( e k e_k ek)宽度是时间间隔 Δ t Δt Δt。
在每个新的时间步长上，我们要做的就是计算新的误差并将其添加到运行总和中，即
E k = E k − 1 + e k E_k = E_{k-1}+e_k Ek=Ek−1+ek
添加积分控制可以消除SSE，但PI控制器还有另一个好处。增加积分增益可以平滑某些类型的噪声（即，在零均值附近波动）。但是，像比例增益一样，不能毫无后果地任意增大积分增益。如果 K i K_i Ki太大，则过度补偿会通过幅度增大的振荡而导致不稳定。
（2）PI控制编程
对hover_plot.py经行以下更改：
import numpy as np
import matplotlib.pyplot as plt
from PI_controller import PI_Controller
from quad1d_eom import ydot
# 仿真参数
N = 500 # 仿真的点数
t0 = 0 # 起始时间(sec)
tf = 30 # 结束时间(sec)
time = np.linspace(t0, tf, N)
dt = time[1] - time[0] # delta t, (sec)
##################################################################################
# 核心仿真代码
# 初始条件 (i.e., 初始状态向量)
y = [0, 0]
# y[0] = initial altitude, (m)
# y[1] = initial speed, (m/s)
# 初始化数组以存储值
soln = np.zeros((len(time), len(y)))
# 创建Open_Controller类的实例
controller = PI_Controller()
# 设置kp,ki的值
kp = 0.2
ki = 0.004
# 设置控制器的Kp值
controller.setKP(kp)
# 设置控制器的Ki值
controller.setKI(ki)
# 设置作用力
# controller.setControlEffort(control_effort)
# 设定高度目标
r = 10.0 # meters
controller.setTarget(r)
kp = 0.2
controller.setKP(kp)
# 模拟四轴运动
j = 0 # 计数器
for t in time:
# 评估下一个时间点的状态
y = ydot(y, t, controller)
# 储存结果
soln[j, :] = y
j += 1
##################################################################################
# 绘制结果
# 图一:四轴飞行器高度随时间的变化的曲线
SP = np.ones_like(time)*r # 高度设置点
fig = plt.figure()
ax1 = fig.add_subplot(211)
ax1.plot(time, soln[:, 0], time, SP, '--')
ax1.set_xlabel('Time, (sec)')
ax1.set_ylabel('Altitude, (m)')
# 图二:这是四轴直升机的速度随时间的变化曲线
ax2 = fig.add_subplot(212)
ax2.plot(time, soln[:, 1])
ax2.set_xlabel('Time, (sec)')
ax2.set_ylabel('Speed, (m/s)')
plt.tight_layout()
plt.show()
fig2 = plt.figure()
ax3 = fig2.add_subplot(111)
ax3.plot(time, controller.u_p, label='u_p', linewidth=3, color = 'red')
ax3.plot(time, controller.u_i, label='u_i', linewidth=3, color = 'blue')
ax3.set_xlabel('Time, (sec)')
ax3.set_ylabel('Control Effort')
h, l = ax3.get_legend_handles_labels()
ax3.legend(h, l)
plt.tight_layout()
plt.show()
##################
y0 = soln[:, 0] # altitude
rise_time_index = np.argmax(y0 > r)
RT = time[rise_time_index]
print("The rise time is {0:.3f} seconds".format(RT))
OS = (np.max(y0) - r)/r*100
if OS < 0:
OS = 0
print("The percent overshoot is {0:.1f}%".format(OS))
print("The steady state offset at 30 seconds is {0:.3f} meters".format(abs(soln[-1, 0]-r)))
PI_controller.py
class PI_Controller:
def __init__(self, kp=0.0, ki=0.0, start_time=0):
# 设定kp和ki的值
self.kp_ = float(kp)
self.ki_ = float(ki)
# 定义error_sum_并设置为0.0
error_sum = 0.0
self.error_sum_ = float(error_sum)
# 存储相关数据
self.last_timestamp_ = 0.0
self.set_point_ = 0.0
self.start_time_ = start_time
# 作用力记录
self.u_p = [0]
self.u_i = [0]
def setTarget(self, target):
self.set_point_ = float(target)
def setKP(self, kp):
self.kp_ = float(kp)
def setKI(self, ki):
# 使用提供的变量设置内部ki_值
self.ki_ = float(ki)
def update(self, measured_value, timestamp):
delta_time = timestamp - self.last_timestamp_
if delta_time == 0:
# Delta time is zero
return 0
# 计算误差
error = self.set_point_ - measured_value
# 设置last_timestamp_
self.last_timestamp_ = timestamp
# 计算 error_sum_
self.error_sum_ += error
# 比例误差
p = self.kp_ * error
# 计算积分误差
i = self.ki_ * self.error_sum_
# 设置作用力
u = p + i
# Here we are storing the control effort history for post control
# observations.
self.u_p.append(p)
self.u_i.append(i)
return u
quad_1db不需要进行更改。
输出如下：
注意要控制好 k i k_i ki和 k p k_p kp的比例关系。
5.PD控制器
（1）PD控制器介绍
加上积分增益，就可以最小化SSE。但是，这是以设置时间和百分比超调为代价的。
导数项试图通过对误差值的变化进行线性外推来“预测”误差将是什么，即它是对未来值的预测(回想一下导数的有限差分近似是切线的斜率)。通过考虑误差的变化率，系统可以更优雅、更快速地接近设定值。
但是，需要考虑如何在离散时间系统中近似连续时间导数。函数的导数表示在特定点评估的切线的斜率。然后可以使用单步向后差分公式来近似斜率，
（2）PD控制器编程
hover_plot.py
import numpy as np
import matplotlib.pyplot as plt
from PD_controller import PD_Controller
from quad1d_eom import ydot
##################################################################################
# 应用一个值为1.7的恒定的作用力。
control_effort = 1.7
##################################################################################
# 仿真参数
N = 500 # 仿真的点数
t0 = 0 # 起始时间(sec)
tf = 30 # 结束时间(sec)
time = np.linspace(t0, tf, N)
dt = time[1] - time[0] # delta t, (sec)
##################################################################################
# 核心仿真代码
# 初始条件 (i.e., 初始状态向量)
y = [0, 0]
# y[0] = initial altitude, (m)
# y[1] = initial speed, (m/s)
# 初始化数组以存储值
soln = np.zeros((len(time), len(y)))
# 创建Open_Controller类的实例
controller = PD_Controller()
# 设置kp,ki的值
kp = 1.7
kd = 0.45
# 设置控制器的Kp值
controller.setKP(kp)
# 设置控制器的Ki值
controller.setKD(kd)
# 设置作用力
# controller.setControlEffort(control_effort)
# 设定高度目标
r = 10.0 # meters
controller.setTarget(r)
# 模拟四轴运动
j = 0 # 计数器
for t in time:
# 评估下一个时间点的状态
y = ydot(y, t, controller)
# 储存结果
soln[j, :] = y
j += 1
##################################################################################
# 绘制结果
# 图一:四轴飞行器高度随时间的变化的曲线
SP = np.ones_like(time)*r # 高度设置点
fig = plt.figure()
ax1 = fig.add_subplot(211)
ax1.plot(time, soln[:, 0], time, SP, '--')
ax1.set_xlabel('Time, (sec)')
ax1.set_ylabel('Altitude, (m)')
# 图二:这是四轴直升机的速度随时间的变化曲线
ax2 = fig.add_subplot(212)
ax2.plot(time, soln[:, 1])
ax2.set_xlabel('Time, (sec)')
ax2.set_ylabel('Speed, (m/s)')
plt.tight_layout()
plt.show()
fig2 = plt.figure()
ax3 = fig2.add_subplot(111)
ax3.plot(time, controller.u_p, label='u_p', linewidth=3, color='red')
ax3.plot(time, controller.u_d, label='u_d', linewidth=3, color='blue')
ax3.set_xlabel('Time, (sec)')
ax3.set_ylabel('Control Effort')
h, l = ax3.get_legend_handles_labels()
ax3.legend(h, l)
plt.tight_layout()
plt.show()
##################
y0 = soln[:, 0] # altitude
rise_time_index = np.argmax(y0 > r)
RT = time[rise_time_index]
print("The rise time is {0:.3f} seconds".format(RT))
OS = (np.max(y0) - r)/r*100
if OS < 0:
OS = 0
print("The percent overshoot is {0:.1f}%".format(OS))
print("The steady state offset at 30 seconds is {0:.3f} meters".format(abs(soln[-1, 0]-r)))
PD_controller.py
class PD_Controller:
def __init__(self, kp=0.0, kd=0.0, start_time=0):
self.kp_ = float(kp)
self.kd_ = float(kd)
# 定义error_sum_并设置为0.0
error_sum = 0.0
self.error_sum_ = float(error_sum)
# 定义e_k-1 error_past
self.error_past_ = 0.0
# 存储相关数据
self.last_timestamp_ = 0.0
self.set_point_ = 0.0
self.start_time_ = start_time
self.error_sum_ = 0.0
# 作用力记录
self.u_p = [0]
self.u_d = [0]
def setTarget(self, target):
self.set_point_ = float(target)
def setKP(self, kp):
self.kp_ = float(kp)
def setKD(self, kd):
# 使用提供的变量设置内部ki_值
self.kd_ = float(kd)
def update(self, measured_value, timestamp):
delta_time = timestamp - self.last_timestamp_
if delta_time == 0:
# Delta time is zero
return 0
# 计算误差
error = self.set_point_ - measured_value
# 设置 last_timestamp_
self.last_timestamp_ = timestamp
# 找到 error_sum_
self.error_sum_ += error * delta_time
delta_error = error - self.error_past_
self.error_past_ = error
p = self.kp_ * error
d = self.kd_ * (delta_error / delta_time)
u = p + d
self.u_p.append(p)
self.u_d.append(d)
return u
输出如下：
6.PID控制器
（1）PID程序
综合上述，PID程序如下
PID_controller.py
class PID_Controller:
def __init__(self, kp=0.0, kd=0.0, ki=0.0, start_time=0):
self.kp_ = float(kp)
self.kd_ = float(kd)
self.ki_ = float(ki)
# 定义error_sum_并设置为0.0
error_start = 0.0
self.error_sum_ = float(error_start)
# 定义e_k-1 error_past
self.error_past_ = 0.0
# 存储相关数据
self.last_timestamp_ = 0.0
self.set_point_ = 0.0
self.start_time_ = start_time
self.error_sum_ = 0.0
# 作用力记录
self.u_p = [0]
self.u_d = [0]
self.u_i = [0]
def setTarget(self, target):
# 设置飞行高度
self.set_point_ = float(target)
def setKP(self, kp):
# 使用提供的变量设置内部kp_值
self.kp_ = float(kp)
def setKD(self, kd):
# 使用提供的变量设置内部kd_值
self.kd_ = float(kd)
def setKI(self, ki):
# 使用提供的变量设置内部ki_值
self.ki_ = float(ki)
def update(self, measured_value, timestamp):
delta_time = timestamp - self.last_timestamp_
if delta_time == 0:
# Delta time is zero
return 0
# 计算误差
error = self.set_point_ - measured_value
# 设置 last_timestamp_
self.last_timestamp_ = timestamp
# 找到 error_sum_
self.error_sum_ += error
# 计算相对误差
delta_error = error - self.error_past_
# 存储本次error，作为下次error_past_使用
self.error_past_ = error
# 计算比例误差
p = self.kp_ * error
# 计算积分误差
i = self.ki_ * self.error_sum_
# 计算微分误差
d = self.kd_ * (delta_error / delta_time)
# 计算总作用力
u = p + d + i
# 记录作用力
self.u_p.append(p)
self.u_d.append(d)
self.u_i.append(i)
return u
hover_plot.py
import numpy as np
import matplotlib.pyplot as plt
from PID_controller import PID_Controller
from quad1d_eom import ydot
# 仿真参数
N = 500 # 仿真的点数
t0 = 0 # 起始时间(sec)
tf = 30 # 结束时间(sec)
time = np.linspace(t0, tf, N)
dt = time[1] - time[0] # delta t, (sec)
# 核心仿真代码
# 初始条件 (i.e., 初始状态向量)
y = [0, 0]
# y[0] = initial altitude, (m)
# y[1] = initial speed, (m/s)
# 初始化数组以存储值
soln = np.zeros((len(time), len(y)))
# 创建Open_Controller类的实例
controller = PID_Controller()
# 设置kp,ki的值
kp = 1.5
kd = 0.75
ki = 0.12
# 设置控制器的Kp值
controller.setKP(kp)
# 设置控制器的Ki值
controller.setKI(ki)
# 设置控制器的Kd值
controller.setKD(kd)
# 设定高度目标
r = 10.0 # meters
controller.setTarget(r)
# 模拟四轴运动
j = 0 # 计数器
for t in time:
# 评估下一个时间点的状态
y = ydot(y, t, controller)
# 储存结果
soln[j, :] = y
j += 1
##################################################################################
# 绘制结果
# 图一:四轴飞行器高度随时间的变化的曲线
SP = np.ones_like(time)*r # 高度设置点
fig = plt.figure()
ax1 = fig.add_subplot(211)
ax1.plot(time, soln[:, 0], time, SP, '--')
ax1.set_xlabel('Time, (sec)')
ax1.set_ylabel('Altitude, (m)')
# 图二:这是四轴直升机的速度随时间的变化曲线
ax2 = fig.add_subplot(212)
ax2.plot(time, soln[:, 1])
ax2.set_xlabel('Time, (sec)')
ax2.set_ylabel('Speed, (m/s)')
plt.tight_layout()
plt.show()
fig2 = plt.figure()
ax3 = fig2.add_subplot(111)
ax3.plot(time, controller.u_p, label='u_p', linewidth=3, color='red')
ax3.plot(time, controller.u_d, label='u_d', linewidth=3, color='blue')
ax3.plot(time, controller.u_i, label='u_i', linewidth=3, color='green')
ax3.set_xlabel('Time, (sec)')
ax3.set_ylabel('Control Effort')
h, l = ax3.get_legend_handles_labels()
ax3.legend(h, l)
plt.tight_layout()
plt.show()
##################
y0 = soln[:, 0]
rise_time_index = np.argmax(y0 > r)
RT = time[rise_time_index]
print("The rise time is {0:.3f} seconds".format(RT))
OS = (np.max(y0) - r)/r*100
if OS < 0:
OS = 0
print("The percent overshoot is {0:.1f}%".format(OS))
print("The steady state offset at 30 seconds is {0:.3f} meters".format(abs(soln[-1, 0]-r)))
我在quad1d_eom.py中加入了对速度的限制，使输出结果变得更好些。
quad1d_eom.py
import numpy as np
def ydot(y, t, controller):
'''
返回下一个时间步长的状态向量
参数:
----------
y(k) = 状态向量, a length 2 list
= [高度, 速度]
t = time, (sec)
pid = PID Controller类的实例
return
-------
y(k+1) = [y[0], y[1]] = y(k) + ydot*dt
'''
# 模型状态
y0 = y[0] # 高度, (m)
y1 = y[1] # 速度, (m/s)
# 模型参数
g = -9.81 # 重力, m/s/s
m = 1.54 # 四轴飞行器重量, kg
c = 10.0 # 机电系统的传输常数
# 时间步长, (sec)
dt = t - controller.last_timestamp_
# 作用力
u = controller.update(measured_value=y0, timestamp=t)
# State derivatives
if (y0 <= 0.):
# 如果控制输入，u <=重力，飞行器在地面上保持静止，这样可以防止四旋翼在推力太小时“坠落”地面。
if u <= np.absolute(g*m/c):
y0dot = 0.
y1dot = 0.
else: # 否则，如果u>重力，则四轴加速向上
y0dot = y1
y1dot = g + c/m*u - 0.75*y1
else: # 或者四轴飞行器已经升空
# y0dot为速度大小
y0dot = y1
# y1dot为加速度大小，其中0.75*y1为阻力大小
y1dot = g + c/m*u - 0.75*y1
y0 += y0dot*dt
y1 += y1dot*dt
# 进行速度限制
if y1 > 20:
y1 = 20
return [y0, y1]
最后，PID控制器的输出如下
（2）PID总结
各个参数对PID的影响
| 参数 | 上升时间 | 超调 | 稳定时间 | 稳态误差 | 稳定性 |
|---|---|---|---|---|---|
| K p K_p Kp | 减少 | 增加 | 影响较小 | 降低 | 降低 |
| K i K_i Ki | 减少 | 增加 | 增加 | 消除 | 降低 |
| K d K_d Kd | 影响较小 | 减少 | 减少 | 理论无影响 | 如果 K d K_d Kd小则改善 |
PID局限性
- 总是对干扰进行相应
- PlD参数整定不易
- PID鲁棒性不理想
- 对死区时间较长系统响应不理想
扫一扫
1047
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
