控制（Controls）

1.PID控制簡介

在工程實(shí)際中，應(yīng)用最為廣泛的調(diào)節(jié)器控制規(guī)律為比例、積分、微分控制，簡稱PID控制，又稱PID調(diào)節(jié)。PID控制器問世至今已有近70年歷史，它 以其結(jié)構(gòu)簡單、穩(wěn)定性好、工作可靠、調(diào)整方便而成為工業(yè)控制的主要技術(shù)之一。當(dāng)被控對(duì)象的結(jié)構(gòu)和參數(shù)不能完全掌握，或得不到精確的數(shù)學(xué)模型時(shí)，控制理論的 其它技術(shù)難以采用時(shí)，系統(tǒng)控制器的結(jié)構(gòu)和參數(shù)必須依靠經(jīng)驗(yàn)和現(xiàn)場(chǎng)調(diào)試來確定，這時(shí)應(yīng)用PID控制技術(shù)最為方便。即當(dāng)我們不完全了解一個(gè)系統(tǒng)和被控對(duì)象，或 不能通過有效的測(cè)量手段來獲得系統(tǒng)參數(shù)時(shí)，最適合用PID控制技術(shù)。PID控制，實(shí)際中也有PI和PD控制。PID控制器就是根據(jù)系統(tǒng)的誤差，利用比例、 積分、微分計(jì)算出控制量進(jìn)行控制的。

比例（P）控制

比例控制是一種最簡單的控制方式。其控制器的輸出與輸入誤差信號(hào)成比例關(guān)系。當(dāng)僅有比例控制時(shí)系統(tǒng)輸出存在穩(wěn)態(tài)誤差（Steady-state error）。

積分（I）控制

在積分控制中，控制器的輸出與輸入誤差信號(hào)的積分成正比關(guān)系。對(duì)一個(gè)自動(dòng)控制系統(tǒng)，如果在進(jìn)入穩(wěn)態(tài)后存在穩(wěn)態(tài)誤差，則稱這個(gè)控制系統(tǒng)是有穩(wěn)態(tài)誤差的 或簡稱有差系統(tǒng)（System with Steady-state Error）。為了消除穩(wěn)態(tài)誤差，在控制器中必須引入“積分項(xiàng)”。積分項(xiàng)對(duì)誤差取決于時(shí)間的積分，隨著時(shí)間的增加，積分項(xiàng)會(huì)增大。這樣，即便誤差很小，積 分項(xiàng)也會(huì)隨著時(shí)間的增加而加大，它推動(dòng)控制器的輸出增大使穩(wěn)態(tài)誤差進(jìn)一步減小，直到接近于零。因此，比例+積分（PI）控制器，可以使系統(tǒng)在進(jìn)入穩(wěn)態(tài)后幾乎無穩(wěn) 態(tài)誤差。

微分（D）控制

在微分控制中，控制器的輸出與輸入誤差信號(hào)的微分（即誤差的變化率）成正比關(guān)系。自動(dòng)控制系統(tǒng)在克服誤差的調(diào)節(jié)過程中可能會(huì)出現(xiàn)振蕩甚至失穩(wěn)。其原因是由于存在有較大慣性組件（環(huán)節(jié)）或有滯后（delay）組件，具有抑制誤差的作用， 其變化總是落后于誤差的變化。解決的辦法是使抑制誤差的作用的變化“超前”，即在誤差接近零時(shí)，抑制誤差的作用就應(yīng)該是零。這就是說，在控制器中僅引入 “比例”項(xiàng)往往是不夠的，比例項(xiàng)的作用僅是放大誤差的幅值，而需要增加的是“微分項(xiàng)”，它能預(yù)測(cè)誤差變化的趨勢(shì)，這樣，具有比例+微分的控制器，就能 夠提前使抑制誤差的控制作用等于零，甚至為負(fù)值，從而避免了被控量的嚴(yán)重超調(diào)。所以對(duì)有較大慣性或滯后的被控對(duì)象，比例+微分（PD）控制器能改善系統(tǒng)在 調(diào)節(jié)過程中的動(dòng)態(tài)特性。

2.PID控制模擬器

這里給出了兩個(gè)圖表。

第一個(gè)圖是四旋翼的高度與時(shí)間的關(guān)系圖。可以看到我們也畫出了10米的設(shè)定點(diǎn)。最終的目標(biāo)是盡可能快地達(dá)到這個(gè)設(shè)定值，并保持最小的振蕩和超調(diào)。

第二張圖是四軸飛行器對(duì)時(shí)間的速度。理想的情況是，在到達(dá)設(shè)定值之前，速度增加，到達(dá)設(shè)定值之后，速度迅速下降，保證四軸飛行器穩(wěn)定在設(shè)定高度。

在這里，我們看到對(duì)四軸飛行器施加了1.7的連續(xù)控制力。我們可以看到這不是一個(gè)好的解決方案，因?yàn)樗妮S飛行器將繼續(xù)上升超過設(shè)定點(diǎn)。我們將構(gòu)建一個(gè)更好的控制器，使之控制工作時(shí)的波動(dòng)，以使我們保持在設(shè)定點(diǎn)。

我們將使用的下一個(gè)圖表是控制工作量與時(shí)間的關(guān)系。控制力是執(zhí)行器為達(dá)到設(shè)定點(diǎn)而需要施加的力。理想情況下，對(duì)于一個(gè)良好的控制器，它不包括初始致動(dòng)以達(dá)到設(shè)定點(diǎn)，我們希望通過配備一個(gè)調(diào)整良好的控制器來最大程度地減少控制工作量。

在這里，我們可以看到一直處于1.7的控制之下，它從未被最小化，這是我們需要解決的問題。

首先來看下面三個(gè)代碼庫，熟悉代碼庫將會(huì)有助于接下來的任務(wù)。由于我沒有使用虛擬機(jī)，而是使用了pycharm。所以需要將hover_plot.py等文件所在的路徑添加進(jìn)系統(tǒng)的環(huán)境變量中，然后程序才可以運(yùn)行。

hover_plot.py：

import numpy as np
import matplotlib.pyplot as plt
from open_controller import Open_Controller
from quad1d_eom import ydot##################################################################################
# 應(yīng)用一個(gè)值為1.7的恒定的作用力。
control_effort = 1.7
################################################################################### 仿真參數(shù)
N = 500  # 仿真的點(diǎn)數(shù)
t0 = 0  # 起始時(shí)間(sec)
tf = 30  # 結(jié)束時(shí)間(sec)
time = np.linspace(t0, tf, N)
dt = time[1] - time[0]  # delta t, (sec)##################################################################################
# 核心仿真代碼
# 初始條件 (i.e., 初始狀態(tài)向量)
y = [0, 0]#y[0] = initial altitude, (m)#y[1] = initial speed, (m/s)# 初始化數(shù)組以存儲(chǔ)值
soln = np.zeros((len(time), len(y)))# 創(chuàng)建Open_Controller類的實(shí)例
controller = Open_Controller()# 設(shè)置作用力
controller.setControlEffort(control_effort)# 設(shè)定高度目標(biāo)
r = 10  # meters
controller.setTarget(r)# 模擬四軸運(yùn)動(dòng)
j = 0  # 計(jì)數(shù)器
for t in time:# 評(píng)估下一個(gè)時(shí)間點(diǎn)的狀態(tài)y = ydot(y, t, controller)# 儲(chǔ)存結(jié)果soln[j, :] = yj += 1##################################################################################
# 繪制結(jié)果
# 圖一:四軸飛行器高度隨時(shí)間的變化的曲線
SP = np.ones_like(time)*r  # 高度設(shè)置點(diǎn)
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)')# 圖二:這是四軸直升機(jī)的速度隨時(shí)間的變化曲線
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()# 圖三:這是四軸直升機(jī)的作用力隨時(shí)間變化的曲線
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：

##################################################################################
# 這是控制類，我們將在接下來對(duì)其進(jìn)行更改。
################################################################################### 創(chuàng)建一個(gè)Open_Controller類
class Open_Controller:# 定義類的初始化序列def __init__(self, start_time=0):# 創(chuàng)建一個(gè)類變量來存儲(chǔ)啟動(dòng)時(shí)間self.start_time_ = start_time# 創(chuàng)建一個(gè)類變量來存儲(chǔ)作用力self.u = 0# 創(chuàng)建一個(gè)類變量來存儲(chǔ)最后的時(shí)間戳self.last_timestamp_ = 0# 創(chuàng)建一個(gè)類變量來存儲(chǔ)設(shè)置點(diǎn)self.set_point_ = 0# 為所有應(yīng)用的控制工作創(chuàng)建一個(gè)類變量self.effort_applied = []# 設(shè)置高度設(shè)置點(diǎn)def setTarget(self, target):self.set_point_ = float(target)# 設(shè)置所需的作用力def setControlEffort(self, control_effort):self.u = float(control_effort)# 檢索當(dāng)前控制工作效果def getControlEffort(self, time):# 存儲(chǔ)最后的時(shí)間戳self.last_timestamp_ = time# 儲(chǔ)存作用力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
# 這個(gè)文件代表了一維四旋翼的動(dòng)力學(xué)方程
##################################################################################def ydot(y, t, controller):'''返回下一個(gè)時(shí)間步長的狀態(tài)向量參數(shù):----------y(k) = 狀態(tài)向量, a length 2 list= [高度, 速度]t = time, (sec)pid = PID Controller類的實(shí)例return-------y(k+1) = [y[0], y[1]] = y(k) + ydot*dt'''# 模型狀態(tài)y0 = y[0]  # 高度, (m)y1 = y[1]  # 速度, (m/s)# 模型參數(shù)g = -9.81  # 重力, m/s/sm =  1.54  # 四軸飛行器重量, kgc =  10.0  # 機(jī)電系統(tǒng)的傳輸常數(shù)# 時(shí)間步長, (sec)dt = t - controller.last_timestamp_# 作用力u = controller.getControlEffort(t)# State derivativesif (y0 <= 0.):# 如果控制輸入，u <=重力，飛行器在地面上保持靜止，這樣可以防止四旋翼在推力太小時(shí)“墜落”地面。if u <= np.absolute(g*m/c):y0dot = 0.y1dot = 0.else:  # 否則，如果u>重力，則四軸加速向上y0dot = y1y1dot = g + c/m*u - 0.75*y1else:  # 或者四軸飛行器已經(jīng)升空# y0dot為速度大小y0dot = y1# y1dot為加速度大小，其中0.75*y1為阻力大小y1dot = g + c/m*u - 0.75*y1y0 += y0dot*dty1 += y1dot*dtreturn [y0, y1]

運(yùn)行hover_plot.py輸出如下：

其實(shí)原本輸出兩個(gè)圖的，我稍微改了一下程序，讓三個(gè)圖輸出在一個(gè)圖中，這三張圖和前面給出的圖是一樣的。

3.比例控制

（1）比例控制介紹

假設(shè)四旋翼飛機(jī)的傳感器可以完美地測(cè)量其高度，也許可以使用氣壓計(jì)，GPS或超聲波傳感器。最初，四旋翼飛行器已通電，但靜止不動(dòng)在啟動(dòng)板上。激活后，預(yù)編程的飛行路徑將使四旋翼飛行器最大上升至10米，將其懸停在原處5秒鐘，然后在保持恒定高度的情況下向東北方向行駛。

在四旋翼飛機(jī)開始執(zhí)行飛行計(jì)劃的第一瞬間，高度誤差為：
$e(t)=r?y(t)$
$=10m?0$
$=10m$

控制器感應(yīng)到較大的誤差，并命令電動(dòng)機(jī)產(chǎn)生與誤差成正比的垂直推力，即

$u(t)=K_p?e(t)$

當(dāng)四旋翼飛機(jī)接近所需高度時(shí)，誤差減小，因此控制器輸入也減小。

比例增益如何放大“當(dāng)前”（當(dāng)前時(shí)間步長）誤差，而不考慮過去或?qū)淼恼`差。

許多現(xiàn)實(shí)世界的系統(tǒng)都由二階微分方程控制，并且在設(shè)置為某個(gè)穩(wěn)態(tài)值之前，會(huì)對(duì)階躍輸入表現(xiàn)出振蕩響應(yīng)。此振蕩響應(yīng)的主要特征如下所示：

• Rise time （上升時(shí)間）$T_r$ ,是響應(yīng)從指定的低值移動(dòng)到指定的高值所需的時(shí)間，通常表示為最終值的百分比。例如，其最終(穩(wěn)態(tài))值的0%到100%。

• Peak time（峰值時(shí)間）$T_p$，達(dá)到第一個(gè)超調(diào)的峰值需要時(shí)間

• Maximum percent overshoot （最大超調(diào)量）：$M_{OS}=\frac{y(T_p)?y(T_{ss})}{y(T_{ss})}\times 100\%$

• Settling time（穩(wěn)定時(shí)間）$T_s$,響應(yīng)達(dá)到并保持在最終穩(wěn)定狀態(tài)值(通常定義為$y_{ss}$的2 - 5%)的范圍內(nèi)所需要的時(shí)間。

振蕩與系統(tǒng)內(nèi)阻尼的大小密切相關(guān)。阻尼是任何消耗能量的機(jī)制的術(shù)語，例如汽車或自行車上的減震器或摩擦。

• 欠阻尼系統(tǒng)比重阻尼系統(tǒng)具有更大的振幅和頻率。

• 臨界阻尼系統(tǒng)沒有振蕩。

• 過阻尼系統(tǒng)（即，比臨界阻尼更大的阻尼）也不會(huì)振蕩，但是達(dá)到穩(wěn)態(tài)值的時(shí)間也會(huì)增加。
  

（2）比例控制器編程

需要對(duì)三個(gè)文件進(jìn)行修改，主要修改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##################################################################################
# 應(yīng)用一個(gè)值為1.7的恒定的作用力。
control_effort = 1.7
################################################################################### 仿真參數(shù)
N = 500  # 仿真的點(diǎn)數(shù)
t0 = 0  # 起始時(shí)間(sec)
tf = 30  # 結(jié)束時(shí)間(sec)
time = np.linspace(t0, tf, N)
dt = time[1] - time[0]  # delta t, (sec)##################################################################################
# 核心仿真代碼
# 初始條件 (i.e., 初始狀態(tài)向量)
y = [0, 0]#y[0] = initial altitude, (m)#y[1] = initial speed, (m/s)# 初始化數(shù)組以存儲(chǔ)值
soln = np.zeros((len(time), len(y)))# 創(chuàng)建Open_Controller類的實(shí)例
controller = P_Controller()# 設(shè)置作用力
# controller.setControlEffort(control_effort)# 設(shè)定高度目標(biāo)
r = 10.0  # meters
controller.setTarget(r)
kp = 0.2
controller.setKP(kp)
# 模擬四軸運(yùn)動(dòng)
j = 0  # 計(jì)數(shù)器
for t in time:# 評(píng)估下一個(gè)時(shí)間點(diǎn)的狀態(tài)y = ydot(y, t, controller)# 儲(chǔ)存結(jié)果soln[j, :] = yj += 1##################################################################################
# 繪制結(jié)果
# 圖一:四軸飛行器高度隨時(shí)間的變化的曲線
SP = np.ones_like(time)*r  # 高度設(shè)置點(diǎn)
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)')# 圖二:這是四軸直升機(jī)的速度隨時(shí)間的變化曲線
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控制器可以用一個(gè)特定的kp值來激活self.kp_ = float(kp)# TODO:為set_point_創(chuàng)建內(nèi)部類變量并將其設(shè)置為0.0，并將start_time_設(shè)置為start_time變量。########################################self.set_point_ = 0.0self.start_time_ = start_time######################################### 存儲(chǔ)最后時(shí)間戳self.last_timestamp_ = 0.0# 作用力記錄self.u_p = [0]# 設(shè)置高度設(shè)置點(diǎn)def setTarget(self, target):self.set_point_ = float(target)def setKP(self, kp):# 使用提供的變量設(shè)置內(nèi)部kp_值self.kp_ = kpreturndef update(self, measured_value, timestamp):# 使用last_timestamp_計(jì)算delta_time以及提供的時(shí)間戳參數(shù)########################################delta_time = timestamp - self.last_timestamp_########################################if delta_time == 0:# 時(shí)間為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):'''返回下一個(gè)時(shí)間步長的狀態(tài)向量參數(shù):----------y(k) = 狀態(tài)向量, a length 2 list= [高度, 速度]t = time, (sec)pid = PID Controller類的實(shí)例return-------y(k+1) = [y[0], y[1]] = y(k) + ydot*dt'''# 模型狀態(tài)y0 = y[0]  # 高度, (m)y1 = y[1]  # 速度, (m/s)# 模型參數(shù)g = -9.81  # 重力, m/s/sm =  1.54  # 四軸飛行器重量, kgc =  10.0  # 機(jī)電系統(tǒng)的傳輸常數(shù)# 時(shí)間步長, (sec)dt = t - controller.last_timestamp_# 作用力u = controller.update(measured_value=y0, timestamp=t)# State derivativesif (y0 <= 0.):# 如果控制輸入，u <=重力，飛行器在地面上保持靜止，這樣可以防止四旋翼在推力太小時(shí)“墜落”地面。if u <= np.absolute(g*m/c):y0dot = 0.y1dot = 0.else:  # 否則，如果u>重力，則四軸加速向上y0dot = y1y1dot = g + c/m*u - 0.75*y1else:  # 或者四軸飛行器已經(jīng)升空# 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控制介紹

通常，對(duì)于可容忍的穩(wěn)態(tài)誤差（SSE）的數(shù)量有非常嚴(yán)格的設(shè)計(jì)要求，并且僅比例控制是不夠的。那么如何消除或至少減少SSE？答案是，添加積分控制。

同樣，這里是對(duì)其起作用的直觀描述。基本思想是相對(duì)于總累積誤差增加來控制輸入。因此，積分控制器會(huì)考慮所有過去的系統(tǒng)誤差值。最終結(jié)果是，即使很小的錯(cuò)誤也會(huì)（最終）被放大，并導(dǎo)致控制器增加對(duì)被控對(duì)象（plant）的輸入。這樣，積分控制器可以消除比例控制器容易產(chǎn)生的較小的穩(wěn)態(tài)誤差。

PV =過程變量（即，測(cè)量的輸出）
SP =設(shè)定點(diǎn)（參考信號(hào)）

此時(shí)需要考慮的一件事是如何在計(jì)算機(jī)上實(shí)際實(shí)現(xiàn)連續(xù)時(shí)間控制器方程式。畢竟，計(jì)算機(jī)是離散時(shí)間設(shè)備。例如，雖然您可能會(huì)看到汽車在沿著高速公路加速時(shí)的速度平穩(wěn)連續(xù)變化，如下圖上半部分所示，

如下部分圖像所示，計(jì)算機(jī)僅以周期性的間隔“看到”樣本。顯然，需要積分和導(dǎo)數(shù)的離散時(shí)間近似值。

通過微積分，積分項(xiàng)代表曲線下的面積。在離散時(shí)間情況下，這可以近似為簡單地將矩形相加，

${\int }_0^{t}e(\tau )d\tau \approx {\sum }_{k=1}^n{e}_k\Delta t$

矩形的“高度”是每個(gè)時(shí)間點(diǎn)的誤差($e_k$)寬度是時(shí)間間隔 $\Delta t$。

在每個(gè)新的時(shí)間步長上，我們要做的就是計(jì)算新的誤差并將其添加到運(yùn)行總和中，即

$E_k=E_{k?1}+e_k$

添加積分控制可以消除SSE，但PI控制器還有另一個(gè)好處。增加積分增益可以平滑某些類型的噪聲（即，在零均值附近波動(dòng)）。但是，像比例增益一樣，不能毫無后果地任意增大積分增益。如果$K_i$太大，則過度補(bǔ)償會(huì)通過幅度增大的振蕩而導(dǎo)致不穩(wěn)定。

（2）PI控制編程

對(duì)hover_plot.py經(jīng)行以下更改：

import numpy as np
import matplotlib.pyplot as plt
from PI_controller import PI_Controller
from quad1d_eom import ydot# 仿真參數(shù)
N = 500  # 仿真的點(diǎn)數(shù)
t0 = 0  # 起始時(shí)間(sec)
tf = 30  # 結(jié)束時(shí)間(sec)
time = np.linspace(t0, tf, N)
dt = time[1] - time[0]  # delta t, (sec)##################################################################################
# 核心仿真代碼
# 初始條件 (i.e., 初始狀態(tài)向量)
y = [0, 0]
# y[0] = initial altitude, (m)
# y[1] = initial speed, (m/s)# 初始化數(shù)組以存儲(chǔ)值
soln = np.zeros((len(time), len(y)))# 創(chuàng)建Open_Controller類的實(shí)例
controller = PI_Controller()# 設(shè)置kp,ki的值
kp = 0.2
ki = 0.004
# 設(shè)置控制器的Kp值
controller.setKP(kp)# 設(shè)置控制器的Ki值
controller.setKI(ki)
# 設(shè)置作用力
# controller.setControlEffort(control_effort)# 設(shè)定高度目標(biāo)
r = 10.0  # meters
controller.setTarget(r)
kp = 0.2
controller.setKP(kp)
# 模擬四軸運(yùn)動(dòng)
j = 0  # 計(jì)數(shù)器
for t in time:# 評(píng)估下一個(gè)時(shí)間點(diǎn)的狀態(tài)y = ydot(y, t, controller)# 儲(chǔ)存結(jié)果soln[j, :] = yj += 1##################################################################################
# 繪制結(jié)果
# 圖一:四軸飛行器高度隨時(shí)間的變化的曲線
SP = np.ones_like(time)*r  # 高度設(shè)置點(diǎn)
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)')# 圖二:這是四軸直升機(jī)的速度隨時(shí)間的變化曲線
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):# 設(shè)定kp和ki的值self.kp_ = float(kp)self.ki_ = float(ki)# 定義error_sum_并設(shè)置為0.0error_sum = 0.0self.error_sum_ = float(error_sum)# 存儲(chǔ)相關(guān)數(shù)據(jù)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):# 使用提供的變量設(shè)置內(nèi)部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# 計(jì)算誤差error = self.set_point_ - measured_value# 設(shè)置last_timestamp_self.last_timestamp_ = timestamp# 計(jì)算 error_sum_self.error_sum_ += error# 比例誤差p = self.kp_ * error# 計(jì)算積分誤差i = self.ki_ * self.error_sum_# 設(shè)置作用力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不需要進(jìn)行更改。

輸出如下：

注意要控制好$k_i$和$k_p$的比例關(guān)系。

5.PD控制器

（1）PD控制器介紹

加上積分增益，就可以最小化SSE。但是，這是以設(shè)置時(shí)間和百分比超調(diào)為代價(jià)的。

導(dǎo)數(shù)項(xiàng)試圖通過對(duì)誤差值的變化進(jìn)行線性外推來“預(yù)測(cè)”誤差將是什么，即它是對(duì)未來值的預(yù)測(cè)(回想一下導(dǎo)數(shù)的有限差分近似是切線的斜率)。通過考慮誤差的變化率，系統(tǒng)可以更優(yōu)雅、更快速地接近設(shè)定值。

但是，需要考慮如何在離散時(shí)間系統(tǒng)中近似連續(xù)時(shí)間導(dǎo)數(shù)。函數(shù)的導(dǎo)數(shù)表示在特定點(diǎn)評(píng)估的切線的斜率。然后可以使用單步向后差分公式來近似斜率，

（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##################################################################################
# 應(yīng)用一個(gè)值為1.7的恒定的作用力。
control_effort = 1.7
################################################################################### 仿真參數(shù)
N = 500  # 仿真的點(diǎn)數(shù)
t0 = 0  # 起始時(shí)間(sec)
tf = 30  # 結(jié)束時(shí)間(sec)
time = np.linspace(t0, tf, N)
dt = time[1] - time[0]  # delta t, (sec)##################################################################################
# 核心仿真代碼
# 初始條件 (i.e., 初始狀態(tài)向量)
y = [0, 0]
# y[0] = initial altitude, (m)
# y[1] = initial speed, (m/s)# 初始化數(shù)組以存儲(chǔ)值
soln = np.zeros((len(time), len(y)))# 創(chuàng)建Open_Controller類的實(shí)例
controller = PD_Controller()# 設(shè)置kp,ki的值
kp = 1.7
kd = 0.45
# 設(shè)置控制器的Kp值
controller.setKP(kp)# 設(shè)置控制器的Ki值
controller.setKD(kd)
# 設(shè)置作用力
# controller.setControlEffort(control_effort)# 設(shè)定高度目標(biāo)
r = 10.0  # meters
controller.setTarget(r)# 模擬四軸運(yùn)動(dòng)
j = 0  # 計(jì)數(shù)器
for t in time:# 評(píng)估下一個(gè)時(shí)間點(diǎn)的狀態(tài)y = ydot(y, t, controller)# 儲(chǔ)存結(jié)果soln[j, :] = yj += 1##################################################################################
# 繪制結(jié)果
# 圖一:四軸飛行器高度隨時(shí)間的變化的曲線
SP = np.ones_like(time)*r  # 高度設(shè)置點(diǎn)
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)')# 圖二:這是四軸直升機(jī)的速度隨時(shí)間的變化曲線
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_并設(shè)置為0.0error_sum = 0.0self.error_sum_ = float(error_sum)# 定義e_k-1 error_pastself.error_past_ = 0.0# 存儲(chǔ)相關(guān)數(shù)據(jù)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):# 使用提供的變量設(shè)置內(nèi)部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# 計(jì)算誤差error = self.set_point_ - measured_value# 設(shè)置 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_并設(shè)置為0.0error_start = 0.0self.error_sum_ = float(error_start)# 定義e_k-1 error_pastself.error_past_ = 0.0# 存儲(chǔ)相關(guān)數(shù)據(jù)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):# 設(shè)置飛行高度self.set_point_ = float(target)def setKP(self, kp):# 使用提供的變量設(shè)置內(nèi)部kp_值self.kp_ = float(kp)def setKD(self, kd):# 使用提供的變量設(shè)置內(nèi)部kd_值self.kd_ = float(kd)def setKI(self, ki):# 使用提供的變量設(shè)置內(nèi)部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# 計(jì)算誤差error = self.set_point_ - measured_value# 設(shè)置 last_timestamp_self.last_timestamp_ = timestamp# 找到 error_sum_self.error_sum_ += error# 計(jì)算相對(duì)誤差delta_error = error - self.error_past_# 存儲(chǔ)本次error，作為下次error_past_使用self.error_past_ = error# 計(jì)算比例誤差p = self.kp_ * error# 計(jì)算積分誤差i = self.ki_ * self.error_sum_# 計(jì)算微分誤差d = self.kd_ * (delta_error / delta_time)# 計(jì)算總作用力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# 仿真參數(shù)
N = 500  # 仿真的點(diǎn)數(shù)
t0 = 0  # 起始時(shí)間(sec)
tf = 30  # 結(jié)束時(shí)間(sec)
time = np.linspace(t0, tf, N)
dt = time[1] - time[0]  # delta t, (sec)# 核心仿真代碼
# 初始條件 (i.e., 初始狀態(tài)向量)
y = [0, 0]
# y[0] = initial altitude, (m)
# y[1] = initial speed, (m/s)# 初始化數(shù)組以存儲(chǔ)值
soln = np.zeros((len(time), len(y)))# 創(chuàng)建Open_Controller類的實(shí)例
controller = PID_Controller()# 設(shè)置kp,ki的值
kp = 1.5
kd = 0.75
ki = 0.12
# 設(shè)置控制器的Kp值
controller.setKP(kp)# 設(shè)置控制器的Ki值
controller.setKI(ki)# 設(shè)置控制器的Kd值
controller.setKD(kd)# 設(shè)定高度目標(biāo)
r = 10.0  # meters
controller.setTarget(r)# 模擬四軸運(yùn)動(dòng)
j = 0  # 計(jì)數(shù)器
for t in time:# 評(píng)估下一個(gè)時(shí)間點(diǎn)的狀態(tài)y = ydot(y, t, controller)# 儲(chǔ)存結(jié)果soln[j, :] = yj += 1##################################################################################
# 繪制結(jié)果
# 圖一:四軸飛行器高度隨時(shí)間的變化的曲線
SP = np.ones_like(time)*r  # 高度設(shè)置點(diǎn)
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)')# 圖二:這是四軸直升機(jī)的速度隨時(shí)間的變化曲線
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中加入了對(duì)速度的限制，使輸出結(jié)果變得更好些。
quad1d_eom.py

import numpy as npdef ydot(y, t, controller):'''返回下一個(gè)時(shí)間步長的狀態(tài)向量參數(shù):----------y(k) = 狀態(tài)向量, a length 2 list= [高度, 速度]t = time, (sec)pid = PID Controller類的實(shí)例return-------y(k+1) = [y[0], y[1]] = y(k) + ydot*dt'''# 模型狀態(tài)y0 = y[0]  # 高度, (m)y1 = y[1]  # 速度, (m/s)# 模型參數(shù)g = -9.81  # 重力, m/s/sm =  1.54  # 四軸飛行器重量, kgc =  10.0  # 機(jī)電系統(tǒng)的傳輸常數(shù)# 時(shí)間步長, (sec)dt = t - controller.last_timestamp_# 作用力u = controller.update(measured_value=y0, timestamp=t)# State derivativesif (y0 <= 0.):# 如果控制輸入，u <=重力，飛行器在地面上保持靜止，這樣可以防止四旋翼在推力太小時(shí)“墜落”地面。if u <= np.absolute(g*m/c):y0dot = 0.y1dot = 0.else:  # 否則，如果u>重力，則四軸加速向上y0dot = y1y1dot = g + c/m*u - 0.75*y1else:  # 或者四軸飛行器已經(jīng)升空# y0dot為速度大小y0dot = y1# y1dot為加速度大小，其中0.75*y1為阻力大小y1dot = g + c/m*u - 0.75*y1y0 += y0dot*dty1 += y1dot*dt# 進(jìn)行速度限制if y1 > 20:y1 = 20return [y0, y1]

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

（2）PID總結(jié)

各個(gè)參數(shù)對(duì)PID的影響

參數(shù)	上升時(shí)間	超調(diào)	穩(wěn)定時(shí)間	穩(wěn)態(tài)誤差	穩(wěn)定性
$K_p$	減少	增加	影響較小	降低	降低
$K_i$	減少	增加	增加	消除	降低
$K_d$	影響較小	減少	減少	理論無影響	如果$K_d$小則改善

PID局限性

• 總是對(duì)干擾進(jìn)行相應(yīng)
• PlD參數(shù)整定不易
• PID魯棒性不理想
• 對(duì)死區(qū)時(shí)間較長系統(tǒng)響應(yīng)不理想

總結(jié)

以上是生活随笔為你收集整理的Udacity机器人软件工程师课程笔记（二十三) - 控制（其一）- PID控制及其python实现的全部內(nèi)容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網(wǎng)站內(nèi)容還不錯(cuò)，歡迎將生活随笔推薦給好友。