如何用PID算法，操控?zé)o人機(jī)懸停？

做控制時(shí)，大家經(jīng)常會(huì)有這樣的感受“代碼很豐滿，現(xiàn)實(shí)很骨感”，這是因?yàn)閷?a class="article-link" target="_blank" href="/tag/%E8%AE%A1%E7%AE%97%E6%9C%BA/">計(jì)算機(jī)指令轉(zhuǎn)移到實(shí)際硬件時(shí)，由于物體的慣性以及各種非理想化的因素影響，往往會(huì)出現(xiàn)實(shí)際與預(yù)期不符合的情況。

這篇文章將以“操控遙控飛機(jī)從地面飛到10米高度并懸?！睘槔?，用最通俗易懂的方式，讓你理解PID。在這個(gè)問題中，我們假設(shè)加速度是可以直接調(diào)控的（實(shí)際生活中往往也是這樣），因此，我們輸入的量為加速度的大小和方向（正負(fù)），而我們最終想要得到的結(jié)果就是高度穩(wěn)定在10米。

首先我們來(lái)講控制方法：

控制方法主要分為“開環(huán)控制”和“閉環(huán)控制”，這兩種控制方法的簡(jiǎn)單理解為：

開環(huán)控制：計(jì)算出飛機(jī)從地面到10米高度所需要的加速度以及作用時(shí)間，然后將其編寫為一條固定的指令，“一次執(zhí)行，全過程受益”。

閉環(huán)控制：在飛機(jī)飛行的過程中，系統(tǒng)時(shí)刻關(guān)注飛機(jī)的狀態(tài)，并做出相應(yīng)的調(diào)整。而PID控制就是最常用的閉環(huán)控制。

PID原理

一講到原理，很多人都會(huì)搬出PID公式，數(shù)學(xué)較好或者學(xué)過自控的人還好，要是遇見一個(gè)半路轉(zhuǎn)行做控制的，看見“微分”和“積分”，頭都大了。其實(shí)，由于生活中信號(hào)采樣具有一定的間隔，因此我們經(jīng)常遇見的都是離散信號(hào)的控制，只需要讀懂下圖即可：

實(shí)踐出真知(python實(shí)現(xiàn)PID)

3.1 導(dǎo)入包

`import?time`

`import?matplotlib.pyplot?as?plt`

3.2 PID實(shí)現(xiàn)

#?實(shí)現(xiàn)一個(gè)PID控制器
class?PIDController:

????def?__init__(self,?kp,?ki,?kd):

????????"""

????????初始化PID控制器

????????參數(shù):

????????kp?(float):?比例系數(shù)

????????ki?(float):?積分系數(shù)

????????kd?(float):?微分系數(shù)

????????"""

????????self.kp?=?kp??#?比例系數(shù)

????????self.ki?=?ki??#?積分系數(shù)

????????self.kd?=?kd??#?微分系數(shù)

????????self.prev_error?=?0??#?上一次的誤差

????????self.integral?=?0??#?誤差積分值

????def?calculate(self,?setpoint,?current_value):

????????"""

????????計(jì)算PID控制器的輸出

????????參數(shù):

????????setpoint?(float):?設(shè)定值（目標(biāo)值）

????????current_value?(float):?當(dāng)前值（被控制的系統(tǒng)當(dāng)前狀態(tài)）

????????返回:

????????output?(float):?控制器的輸出

????????"""

????????error?=?setpoint?-?current_value??#?計(jì)算誤差

????????self.integral?+=?error??#?更新誤差積分

????????derivative?=?error?-?self.prev_error??#?計(jì)算誤差導(dǎo)數(shù)

????????output?=?self.kp?*?error?+?self.ki?*?self.integral?+?self.kd?*?derivative

????????#?計(jì)算控制輸出，包括比例、積分和微分部分

????????self.prev_error?=?error??#?保存當(dāng)前誤差作為下一步的上一次誤差

????????return?output??#?返回控制器的輸出

3.3 飛行器模擬

#飛行器模擬
class?AircraftSimulator:

????def?__init__(self):

????????self.height?=?0??#?飛行器初始高度為0

????????self.velocity?=?0??#?飛行器初始速度為0

????def?update(self,?throttle,?time_step):

????????"""

????????更新飛行器狀態(tài)：高度和速度

????????參數(shù):

????????throttle?(float):?油門輸入，控制引擎輸出的力量

????????time_step?(float):?時(shí)間步長(zhǎng)，模擬器更新的時(shí)間間隔

????????"""

????????acceleration?=?throttle?-?0.1?*?self.velocity??

????????#?根據(jù)簡(jiǎn)化的動(dòng)力模型計(jì)算飛行器的加速度

????????#?加速度等于油門輸入減去速度的一部分，這是簡(jiǎn)化的模型

????????self.velocity?+=?acceleration?*?time_step??

????????#?根據(jù)加速度更新速度

????????#?新速度等于當(dāng)前速度加上加速度乘以時(shí)間步長(zhǎng)

????????self.height?+=?self.velocity?*?time_step??

????????#?根據(jù)速度更新飛行器的高度

????????#?新高度等于當(dāng)前高度加上速度乘以時(shí)間步長(zhǎng)

3.4 主函數(shù)與繪圖

#?主函數(shù)

def?main():

????#?PID參數(shù)

????kp?=?5.0

????ki?=?0.1

????kd?=?10

????#?初始化PID控制器和飛行器模擬

????pid_controller?=?PIDController(kp,?ki,?kd)

????aircraft_simulator?=?AircraftSimulator()

????target_height?=?10.0

????time_step?=?0.1

????total_time?=?20??#?總模擬時(shí)間增加到20秒

????current_time?=?0.0

????#?存儲(chǔ)時(shí)間和高度數(shù)據(jù)的列表

????time_data?=?[]

????height_data?=?[]

????#?模擬循環(huán)

????while?current_time?<?total_time:

????????#?使用PID控制器計(jì)算控制信號(hào)

????????control_signal?=?pid_controller.calculate(target_height,?aircraft_simulator.height)

????????#?添加擾動(dòng)

????????disturbance?=?-1.5

????????control_signal?+=?disturbance

????????#?使用控制信號(hào)和時(shí)間步長(zhǎng)更新飛行器模擬

????????aircraft_simulator.update(control_signal,?time_step)

????????#?存儲(chǔ)時(shí)間和高度數(shù)據(jù)

????????time_data.append(current_time)

????????height_data.append(aircraft_simulator.height)

????????current_time?+=?time_step

????????time.sleep(time_step)??#?添加時(shí)間延遲以模擬實(shí)時(shí)行為

????print("Simulation?completed.")

????#?繪圖

????plt.plot(time_data,?height_data,?label='Height')

????plt.axhline(y=target_height,?color='r',?linestyle='--',?label='Target?Height')

????plt.xlabel('Time?(s)')

????plt.ylabel('Height?(m)')

????plt.title('Aircraft?Height?Control?with?Random?Disturbance')

????plt.legend()

????plt.grid(True)

????plt.show()

if?__name__?==?"__main__":

????main()

實(shí)驗(yàn)與參數(shù)理解

PID的控制經(jīng)常會(huì)涉及到KP、KI、KD三個(gè)參數(shù)的調(diào)節(jié)，如果盲目調(diào)節(jié)則會(huì)花較長(zhǎng)時(shí)間，接下來(lái)我們將用直觀實(shí)驗(yàn)來(lái)理解以下幾個(gè)參數(shù)的具體含義。

4.1 比例環(huán)節(jié)

計(jì)算公式為KP × 誤差，具體的含義即為誤差越大，值越大。這一點(diǎn)是非常直觀的，誤差越大則說明偏離預(yù)期值越遠(yuǎn)，我們要加大“油門”，快速調(diào)整！以下是當(dāng)KI、KD為0，只有KP=5的測(cè)試結(jié)果：

從圖中我們可以看到雖然慢慢的想10收斂，但由于誤差越大，其“油門”越大，就像是一個(gè)“莽夫”，盡管每次都在調(diào)整，但總是用力過猛！

4.2 微分環(huán)節(jié)

計(jì)算公式為KD × (本次誤差 - 上次誤差)，對(duì)于這個(gè)公式，我們可以理解為用來(lái)中和“用力過猛”。以下是當(dāng)KP=5、KD=10、KI=0的測(cè)試結(jié)果：

顯然，這個(gè)結(jié)果要比上次好很多，但始終低于10，這是因?yàn)槲覀冊(cè)谀M中加入了一個(gè)干擾條件：

#?添加擾動(dòng)

????????disturbance?=?-1.5

????????control_signal?+=?disturbance

因此，要想消除這個(gè)干擾，就需要積分環(huán)節(jié)的加入。

4.3 積分環(huán)節(jié)

積分環(huán)節(jié)的公式為KI × 誤差累計(jì)和，用官方的語(yǔ)言來(lái)說，用來(lái)調(diào)整“穩(wěn)態(tài)誤差”，其實(shí)，所謂的穩(wěn)態(tài)誤差就可以理解為“一直存在的誤差”，也就是在本次實(shí)驗(yàn)中加入的持續(xù)干擾！以下是當(dāng)KP=5、KD=10、KI=0.1的測(cè)試結(jié)果：

從這次的測(cè)試中，我們看出，得到了幾乎完美的結(jié)果！

總結(jié)

對(duì)于PID參數(shù)調(diào)節(jié)，認(rèn)準(zhǔn)3個(gè)點(diǎn)：

P:大力出奇跡

I：消除持續(xù)存在的誤差

D：“中和”用力過猛，減少波動(dòng)

原創(chuàng)不易，感謝點(diǎn)個(gè)在看，點(diǎn)贊支持！

關(guān)注公眾號(hào)，掃碼加入嵌入式交流群：