控制（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, :] = yj += 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/sm =  1.54  # 四轴飞行器重量, kgc =  10.0  # 机电系统的传输常数# 时间步长, (sec)dt = t - controller.last_timestamp_# 作用力u = controller.getControlEffort(t)# State derivativesif (y0 <= 0.):# 如果控制输入，u <=重力，飞行器在地面上保持静止，这样可以防止四旋翼在推力太小时“坠落”地面。if u <= np.absolute(g*m/c):y0dot = 0.y1dot = 0.else:  # 否则，如果u>重力，则四轴加速向上y0dot = y1y1dot = g + c/m*u - 0.75*y1else:  # 或者四轴飞行器已经升空# y0dot为速度大小y0dot = y1# y1dot为加速度大小，其中0.75*y1为阻力大小y1dot = g + c/m*u - 0.75*y1y0 += y0dot*dty1 += y1dot*dtreturn [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)
=10m−0\qquad =10m-0=10m−0
=10m\qquad=10m=10m

控制器感应到较大的误差，并命令电动机产生与误差成正比的垂直推力，即

u(t)=Kp∗e(t)u(t)=K_p*e(t)u(t)=Kp∗e(t)

当四旋翼飞机接近所需高度时，误差减小，因此控制器输入也减小。

比例增益如何放大“当前”（当前时间步长）误差，而不考虑过去或将来的误差。

许多现实世界的系统都由二阶微分方程控制，并且在设置为某个稳态值之前，会对阶跃输入表现出振荡响应。此振荡响应的主要特征如下所示：

Rise time （上升时间）TrT_rTr ,是响应从指定的低值移动到指定的高值所需的时间，通常表示为最终值的百分比。例如，其最终(稳态)值的0%到100%。
Peak time（峰值时间）TpT_pTp，达到第一个超调的峰值需要时间
Maximum percent overshoot （最大超调量）：MOS=y(Tp)−y(Tss)y(Tss)×100%M_{OS}=\frac{y(T_p)-y(T_{ss})}{y(T_{ss})}\times100\%MOS=y(Tss)y(Tp)−y(Tss)×100%
Settling time（稳定时间）TsT_sTs,响应达到并保持在最终稳定状态值(通常定义为yssy_{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, :] = yj += 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.0self.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_ = kpreturndef update(self, measured_value, timestamp):# 使用last_timestamp_计算delta_time以及提供的时间戳参数########################################delta_time = timestamp - self.last_timestamp_########################################if delta_time == 0:# 时间为0return 0# Calculate the error as the differnce between# the set_point_ and the measured_value########################################e_dot = 0.0e_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 npdef 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/sm =  1.54  # 四轴飞行器重量, kgc =  10.0  # 机电系统的传输常数# 时间步长, (sec)dt = t - controller.last_timestamp_# 作用力u = controller.update(measured_value=y0, timestamp=t)# State derivativesif (y0 <= 0.):# 如果控制输入，u <=重力，飞行器在地面上保持静止，这样可以防止四旋翼在推力太小时“坠落”地面。if u <= np.absolute(g*m/c):y0dot = 0.y1dot = 0.else:  # 否则，如果u>重力，则四轴加速向上y0dot = y1y1dot = g + c/m*u - 0.75*y1else:  # 或者四轴飞行器已经升空# y0dot为速度大小y0dot = y1# y1dot为加速度大小，其中0.75*y1为阻力大小y1dot = g + c/m*u - 0.75*y1y0 += y0dot*dty1 += y1dot*dtreturn [y0, y1]

输出如下：

4.PI控制

（1）PI控制介绍

通常，对于可容忍的稳态误差（SSE）的数量有非常严格的设计要求，并且仅比例控制是不够的。那么如何消除或至少减少SSE？答案是，添加积分控制。

同样，这里是对其起作用的直观描述。基本思想是相对于总累积误差增加来控制输入。因此，积分控制器会考虑所有过去的系统误差值。最终结果是，即使很小的错误也会（最终）被放大，并导致控制器增加对被控对象（plant）的输入。这样，积分控制器可以消除比例控制器容易产生的较小的稳态误差。

PV =过程变量（即，测量的输出）
SP =设定点（参考信号）

此时需要考虑的一件事是如何在计算机上实际实现连续时间控制器方程式。毕竟，计算机是离散时间设备。例如，虽然您可能会看到汽车在沿着高速公路加速时的速度平稳连续变化，如下图上半部分所示，

如下部分图像所示，计算机仅以周期性的间隔“看到”样本。显然，需要积分和导数的离散时间近似值。

通过微积分，积分项代表曲线下的面积。在离散时间情况下，这可以近似为简单地将矩形相加，

∫0te(τ)dτ≈∑k=1nekΔt\int_0^t e(\tau) d\tau \approx \sum_{k=1}^n e_kΔt∫0te(τ)dτ≈∑k=1nekΔt

矩形的“高度”是每个时间点的误差(eke_kek)宽度是时间间隔 ΔtΔtΔt。

在每个新的时间步长上，我们要做的就是计算新的误差并将其添加到运行总和中，即

Ek=Ek−1+ekE_k = E_{k-1}+e_kEk=Ek−1+ek

添加积分控制可以消除SSE，但PI控制器还有另一个好处。增加积分增益可以平滑某些类型的噪声（即，在零均值附近波动）。但是，像比例增益一样，不能毫无后果地任意增大积分增益。如果KiK_iKi太大，则过度补偿会通过幅度增大的振荡而导致不稳定。

（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, :] = yj += 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.0error_sum = 0.0self.error_sum_ = float(error_sum)# 存储相关数据self.last_timestamp_ = 0.0self.set_point_ = 0.0self.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 zeroreturn 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不需要进行更改。

输出如下：

注意要控制好kik_iki和kpk_pkp的比例关系。

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, :] = yj += 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.0error_sum = 0.0self.error_sum_ = float(error_sum)# 定义e_k-1 error_pastself.error_past_ = 0.0# 存储相关数据self.last_timestamp_ = 0.0self.set_point_ = 0.0self.start_time_ = start_timeself.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 zeroreturn 0# 计算误差error = self.set_point_ - measured_value# 设置 last_timestamp_self.last_timestamp_ = timestamp# 找到 error_sum_self.error_sum_ += error * delta_timedelta_error = error - self.error_past_self.error_past_ = errorp = self.kp_ * errord = self.kd_ * (delta_error / delta_time)u = p + dself.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.0error_start = 0.0self.error_sum_ = float(error_start)# 定义e_k-1 error_pastself.error_past_ = 0.0# 存储相关数据self.last_timestamp_ = 0.0self.set_point_ = 0.0self.start_time_ = start_timeself.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 zeroreturn 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, :] = yj += 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 npdef 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/sm =  1.54  # 四轴飞行器重量, kgc =  10.0  # 机电系统的传输常数# 时间步长, (sec)dt = t - controller.last_timestamp_# 作用力u = controller.update(measured_value=y0, timestamp=t)# State derivativesif (y0 <= 0.):# 如果控制输入，u <=重力，飞行器在地面上保持静止，这样可以防止四旋翼在推力太小时“坠落”地面。if u <= np.absolute(g*m/c):y0dot = 0.y1dot = 0.else:  # 否则，如果u>重力，则四轴加速向上y0dot = y1y1dot = g + c/m*u - 0.75*y1else:  # 或者四轴飞行器已经升空# y0dot为速度大小y0dot = y1# y1dot为加速度大小，其中0.75*y1为阻力大小y1dot = g + c/m*u - 0.75*y1y0 += y0dot*dty1 += y1dot*dt# 进行速度限制if y1 > 20:y1 = 20return [y0, y1]

最后，PID控制器的输出如下

（2）PID总结

各个参数对PID的影响

参数	上升时间	超调	稳定时间	稳态误差	稳定性
KpK_pKp	减少	增加	影响较小	降低	降低
KiK_iKi	减少	增加	增加	消除	降低
KdK_dKd	影响较小	减少	减少	理论无影响	如果KdK_dKd小则改善

PID局限性

总是对干扰进行相应
PlD参数整定不易
PID鲁棒性不理想
对死区时间较长系统响应不理想

Udacity机器人软件工程师课程笔记（二十三) - 控制（其一）- PID控制及其python实现相关推荐

Udacity机器人软件工程师课程笔记（十三）-运动学-机械手介绍及分类
运动学在研究控制机械臂之前,有必要学习一些运动学知识,比如学习反向运动学内容,解决命令任意串行操纵器沿预先计划的轨迹朝向目标的问题等等. 下面是一个大纲,介绍了"运动学-机械手介绍及分类& ...
Udacity机器人软件工程师课程笔记（五）-样本搜索和找回-基于漫游者号模拟器-自主驾驶
9.自主驾驶在接下来的环节中,我们要实现漫游者号的自动驾驶功能. 完成这个功能我们需要四个程序,第一个为感知程序,其对摄像头输入的图片进行变换处理和坐标变换使用.第二个程序为决策程序,功能是帮助漫游 ...
Udacity机器人软件工程师课程笔记（二十四) - 控制（其二） - PID优化，梯度下降算法，带噪声的PID控制
7.非理想情况 (1)积分饱和到目前为止,我们一直使用的"理想"形式的PID控制器很少用于工业中."时间常数"形式更为常见. 当前说明了理想形式的一些重大缺陷 ...
Udacity机器人软件工程师课程笔记（七）-ROS介绍和Turtlesim包的使用
Robotics Software engineer笔记 1.ROS简介与虚拟机配置 (1)ROS简介 ROS是一款机器人软件框架,即机器人操作系统(Robot Operating System). ...
Udacity机器人软件工程师课程笔记（三十三) - 蒙特卡洛定位算法（MCL）
一.概述之前的文章介绍过卡尔曼滤波算法进行定位,我们知道kalman算法适合用于线性的高斯分布的状态环境中,我们也介绍了EKF,来解决在非高斯和非线性环境下的机器人定位算法.但是他们在现实应用中存在 ...
Udacity机器人软件工程师课程笔记（三十二) - 卡尔曼滤波器 - 一维卡尔曼滤波器 - 多维卡尔曼滤波器 - 拓展卡尔曼滤波器（EKF）
卡尔曼滤波器一.概述二.一维高斯分布均值和方差三.一维卡尔曼滤波器变量命名约定卡尔曼滤波循环 1.测量值更新 (1)平均值计算 (2)程序实现 2.位置预测位置预测公式 3.一维卡尔曼滤 ...
Udacity机器人软件工程师课程笔记（二十七) - 卷积神经网络（CNN）
1.卷积神经网络介绍 **卷积神经网络(Convolutional Neural Network,CNN)**是一种前馈神经网络,它的人工神经元可以响应一部分覆盖范围内的周围单元,对于大型图像处理有出 ...
Udacity机器人软件工程师课程笔记（二十一) - 对点云进行集群可视化 - 聚类的分割 - K-means|K均值聚类, DBSCAN算法
聚类的分割 1.K-均值聚类 (1)K-均值聚类介绍 k均值聚类算法(k-means clustering algorithm)是一种迭代求解的聚类分析算法,其步骤是随机选取K个对象作为初始的聚类中心 ...
Udacity机器人软件工程师课程笔记（二十) - 感知 - 校准，过滤，分段， RANSAC
校准,过滤, 分段, RANSAC 首先,我们将讨论传感器校准,也就是说,从几何形状,失真和噪声方面校准相机如何看待周围的世界.了解相机的校准对于了解测量数据如何代表实际物理环境至关重要. 之后,我们 ...

Udacity机器人软件工程师课程笔记（二十三) - 控制（其一）- PID控制及其python实现

控制（Controls）

1.PID控制简介

比例（P）控制

积分（I）控制

微分（D）控制

2.PID控制模拟器

3.比例控制

（1）比例控制介绍

（2）比例控制器编程

4.PI控制

（1）PI控制介绍

（2）PI控制编程

5.PD控制器

（1）PD控制器介绍

（2）PD控制器编程

6.PID控制器

（1）PID程序

（2）PID总结

各个参数对PID的影响

PID局限性

Udacity机器人软件工程师课程笔记（二十三) - 控制（其一）- PID控制及其python实现相关推荐

最新文章

热门文章