基于卡爾曼濾波和增量式PID控制的智能車載系統(tǒng)

張哲 吳劍 何誠(chéng) 李玉琪

摘? 要：針對(duì)復(fù)雜環(huán)境中智能體的電機(jī)易受外界干擾的問(wèn)題，設(shè)計(jì)了一種基于卡爾曼濾波和增量式PID控制的Wi-Fi智能車載系統(tǒng)。首先，在勻速階段，建立了無(wú)刷直流電機(jī)的數(shù)學(xué)模型，利用增量式PID算法實(shí)現(xiàn)對(duì)系統(tǒng)電機(jī)轉(zhuǎn)速的精準(zhǔn)控制。其次，在加速階段，結(jié)合動(dòng)力學(xué)分析，建立線性系統(tǒng)模型，利用卡爾曼濾波算法對(duì)系統(tǒng)的真實(shí)軌跡進(jìn)行不斷修正。最后，系統(tǒng)以STM32單片機(jī)為核心，同時(shí)具有Wi-Fi視頻傳輸、環(huán)境避障和精準(zhǔn)定位等多種功能。經(jīng)測(cè)試，系統(tǒng)電機(jī)能夠輸出穩(wěn)定的速度，真實(shí)軌跡不斷趨近于理想軌跡，視頻傳輸穩(wěn)定。該方法能夠消除外部干擾所造成的誤差，具有良好的現(xiàn)實(shí)意義。

關(guān)鍵詞：增量式PID;卡爾曼濾波;Wi-Fi視頻傳輸;上位機(jī);抗干擾

中圖分類號(hào)：TP273? ? ? ?文獻(xiàn)標(biāo)識(shí)碼：A 文章編號(hào)：2096-4706（2019）10-0029-05

Abstract：For the problem that the motor of agent in complex environment is susceptible to external interference，a Wi-Fi intelligent vehicle system based on Kalman filter and incremental PID control is designed. Firstly，the mathematical model of BLDCM is established in the uniform speed stage，and the incremental PID algorithm is used to control the speed of the system motor accurately. Secondly，in the acceleration stage，combined with dynamic analysis，the linear system model is established，and the Kalman filter algorithm is used to continuously correct the real trajectory of the system. Finally，the system takes STM32 as its core，and has many functions such as Wi-Fi video transmission，environment obstacle avoidance and accurate positioning. After testing，the system motor can output stable speed，the real trajectory keeps approaching the ideal trajectory，and the video transmission is stable. This method can eliminate the error caused by external interference and has good practical significance.

Keywords：incremental PID;Kalman filtering;Wi-Fi video transmission;upper monitor;anti-interference

0? 引? 言

隨著人們對(duì)智能化產(chǎn)品需求的日益增長(zhǎng)，Wi-Fi視頻監(jiān)控技術(shù)[1]、嵌入式技術(shù)和數(shù)據(jù)傳輸技術(shù)得到了快速發(fā)展。由于天線網(wǎng)絡(luò)與傳統(tǒng)的通信方式相比具有覆蓋面積大、布線比較簡(jiǎn)單和傳輸速度快等優(yōu)勢(shì)，因此能夠應(yīng)用在機(jī)電設(shè)備較為密集、空間范圍小、環(huán)境惡劣和有線電纜布局難度大的場(chǎng)景中。然而在一些復(fù)雜的作業(yè)環(huán)境中，各種各樣的干擾會(huì)對(duì)智能體系統(tǒng)穩(wěn)定運(yùn)行造成影響，尤其是會(huì)造成智能體左右電機(jī)的轉(zhuǎn)速不同，使系統(tǒng)無(wú)法沿著理想軌跡運(yùn)行。

文獻(xiàn)[2]提出了一種初始比例值優(yōu)化的模糊自適應(yīng)PI控制算法，給出了理論上的可行性仿真，但在實(shí)際應(yīng)用中電機(jī)轉(zhuǎn)速精度仍存在少量誤差。文獻(xiàn)[3-6]研究了多種PID控制方法去減小誤差干擾，但僅適用于干擾強(qiáng)度較小的環(huán)境中，一旦出現(xiàn)較大的干擾，仍然無(wú)法保證對(duì)系統(tǒng)的穩(wěn)定控制。文獻(xiàn)[7]提出了應(yīng)用增量型PID控制算法，但并未考慮小車系統(tǒng)在加速階段由于外界強(qiáng)干擾造成的軌跡誤差。

本文設(shè)計(jì)了一種基于增量式PID和卡爾曼濾波的Wi-Fi智能車載系統(tǒng)。分別采用增量式PID算法和卡爾曼濾波技術(shù)對(duì)系統(tǒng)勻速和加速階段的狀態(tài)進(jìn)行控制，以STM32單片機(jī)為核心，同時(shí)具有Wi-Fi視頻傳輸、環(huán)境避障和精準(zhǔn)定位等功能，減小了復(fù)雜環(huán)境中各種干擾產(chǎn)生的誤差，實(shí)現(xiàn)了對(duì)智能體精準(zhǔn)穩(wěn)定的控制。此外，系統(tǒng)還增添了電子羅盤MAG3110[8]和多種傳感器[9]（如溫濕度傳感器DHT11和氣體傳感器MQ-9），實(shí)現(xiàn)了系統(tǒng)實(shí)時(shí)視頻傳輸和智能控制。

1? 系統(tǒng)的總體設(shè)計(jì)

操作人員控制指揮終端（PC端）通過(guò)Wi-Fi網(wǎng)絡(luò)連接到小車系統(tǒng)上的Wi-Fi模塊，此時(shí)系統(tǒng)中的單片機(jī)將接收到無(wú)線模塊上的相應(yīng)指令，并將其轉(zhuǎn)化為實(shí)際控制指令，通過(guò)內(nèi)置控制電路，對(duì)小車執(zhí)行相應(yīng)的操作。與此同時(shí)，系統(tǒng)中的多種傳感器和攝像頭云臺(tái)將對(duì)應(yīng)的數(shù)據(jù)返回到控制終端，從而完成了雙向通信和上下行控制，實(shí)現(xiàn)了將采集到的視頻實(shí)時(shí)播放、保存，在PC端上遠(yuǎn)程控制及感知環(huán)境信息等。總體設(shè)計(jì)如圖1所示。

2? 主要研究?jī)?nèi)容

2.1? 增量式PID控制器設(shè)計(jì)

在勻速階段，系統(tǒng)電機(jī)易受到外界各種干擾并不能輸出穩(wěn)定準(zhǔn)確的速度，因此采用增量式PID控制方法對(duì)系統(tǒng)進(jìn)行精準(zhǔn)控制，該方法在計(jì)算時(shí)不需要累加，僅需最近幾次誤差的采樣值，易于在內(nèi)存要求不高的控制器上實(shí)現(xiàn)?？刂破髟O(shè)計(jì)結(jié)構(gòu)如圖2所示。

增量式PID算法表達(dá)式：

其中Δu（k）為PID生成的控制率，KP為比例增益，KI為積分增益，KD為微分增益，e（k）為速度誤差。

2.1.1? 無(wú)刷直流電機(jī)的數(shù)學(xué)模型

采樣無(wú)刷直流電機(jī)，其數(shù)學(xué)模型可以近似慣性環(huán)節(jié)，對(duì)其進(jìn)行拉普拉斯變換，得到傳遞函數(shù)：

其中A、B為常數(shù)，w為轉(zhuǎn)到角速度，U為電機(jī)控制電壓。

2.1.2? 設(shè)計(jì)方法

設(shè)小車系統(tǒng)的速度真值為ν0，小車左輪實(shí)際速度為ν1，小車由輪實(shí)際速度為ν2，d1和d2分別是相互獨(dú)立的高斯白噪聲。速度關(guān)系如下：

2.2? 卡爾曼濾波算法

2.2.1? 加速階段系統(tǒng)的線性模型

在加速階段，運(yùn)用卡爾曼濾波算法[10]對(duì)系統(tǒng)進(jìn)行動(dòng)力學(xué)分析，首先建立小車在加速階段的線性系統(tǒng)模型為：

其中xk為系統(tǒng)k時(shí)刻的狀態(tài)量，A為狀態(tài)矩陣，B為輸入矩陣，Zk為測(cè)量值，H為觀測(cè)矩陣，uk為k時(shí)刻的輸入量，wk和vk分別表示過(guò)程噪聲和測(cè)量噪聲。

2.2.2? 算法實(shí)現(xiàn)

由卡爾曼濾波算法原理結(jié)合系統(tǒng)在加速階段的運(yùn)動(dòng)狀態(tài)模型可知，預(yù)測(cè)方程? 和預(yù)測(cè)誤差協(xié)方差陣? 分別為：

2.3? Windows上位機(jī)開(kāi)發(fā)

采用C#自主設(shè)計(jì)研發(fā)上位機(jī)軟件界面。該上位機(jī)相當(dāng)于一個(gè)大型的指揮平臺(tái)，所有的傳感器數(shù)據(jù)、視頻信息和控制操作都能在上面顯示，實(shí)現(xiàn)整個(gè)系統(tǒng)的數(shù)據(jù)顯示、視頻顯示和控制指令的融合。

2.4? 通信協(xié)議

Windows上位機(jī)采用數(shù)據(jù)包格式發(fā)送操作指令，數(shù)據(jù)包開(kāi)頭的指令為0XFF，數(shù)據(jù)包結(jié)尾的指令為0XFF，無(wú)校驗(yàn)位。上位機(jī)端利用內(nèi)置的無(wú)線網(wǎng)卡連接網(wǎng)絡(luò)，通過(guò)Socket發(fā)送數(shù)據(jù)包，由路由解碼再經(jīng)過(guò)串口將數(shù)據(jù)傳遞到STM32微型處理器的控制模塊，并執(zhí)行對(duì)應(yīng)的操作指令，從而實(shí)現(xiàn)整個(gè)通信過(guò)程。

無(wú)線Wi-Fi模塊AR9331采用IEEE制訂的國(guó)際標(biāo)準(zhǔn)802.11b/g/n通信協(xié)議[11]，配置內(nèi)存為32M，F(xiàn)lash為4M。采用DSSS、OFDM、BPSK、QPSK、CCK和QAM基帶調(diào)制技術(shù)，其中最大連接速度為150Mbps。

STM32F407單片機(jī)采用USART串口通信協(xié)議，這種方式提供了一種靈活的方法與使用工業(yè)標(biāo)準(zhǔn)NRZ異步串行數(shù)據(jù)格式的外部設(shè)備之間進(jìn)行全雙工數(shù)據(jù)交換。USART支持雙向通信，配置三個(gè)端口分別為接收數(shù)據(jù)輸入端（RX）、發(fā)送數(shù)據(jù)輸出端（TX）和發(fā)送器時(shí)鐘輸出（CK），在IRDA模式下，設(shè)置波特率為115200，無(wú)奇偶校驗(yàn)位。

2.5? 下位機(jī)

驅(qū)動(dòng)板微處理器采用意法半導(dǎo)體集團(tuán)研發(fā)的STM32F4系列芯片，其具有高速、低功耗、抗強(qiáng)干擾和指令代碼兼容性高等特點(diǎn)。下位機(jī)部分程序包括了初始化、串口中斷、定時(shí)器、外部中斷、PWM調(diào)速和舵機(jī)配置等。下位機(jī)部分通過(guò)接受Windows上位機(jī)通過(guò)Wi-Fi發(fā)送的操作指令，實(shí)現(xiàn)視頻實(shí)時(shí)傳輸、攝像頭云臺(tái)角度控制、電機(jī)驅(qū)動(dòng)轉(zhuǎn)向、超聲波測(cè)距、測(cè)速及紅外避障等功能。

2.6? 多種靈敏硬件的綜合開(kāi)發(fā)

本系統(tǒng)涉及多種驅(qū)動(dòng)硬件，如攝像頭、Wi-Fi模塊、舵機(jī)、電機(jī)啟動(dòng)器、超聲波模塊和紅外模塊等。涉及多種傳感器，如速度傳感器、溫度濕度傳感器、氣體傳感器和光學(xué)灰塵傳感器等。在開(kāi)發(fā)過(guò)程中需要查閱相關(guān)硬件的數(shù)據(jù)手冊(cè)，對(duì)各類數(shù)據(jù)做整合處理及數(shù)據(jù)融合，經(jīng)過(guò)AD/DA轉(zhuǎn)換將數(shù)據(jù)信息呈現(xiàn)出來(lái)。整個(gè)系統(tǒng)的模型概念如圖3所示。

3? 系統(tǒng)測(cè)試

3.1? PID控制器對(duì)系統(tǒng)電機(jī)的控制

設(shè)PID參數(shù)正定，KP=0.4，KI=0.6，KD=0.3，取A=1，

B=1，車輪半徑R為1。小車v0=5，v1=v0+sqrt（0.1）×randn

（sz），v2=v0+sqrt（0.15）×randn（sz），sz與仿真時(shí)間有關(guān)，在0到0.2秒時(shí)間內(nèi)都為常值，得到系統(tǒng)在2秒內(nèi)左右輪速度與設(shè)定速度如圖4，誤差值如圖5，雙輪的控制律如圖6。

3.2? 卡爾曼濾波算法對(duì)系統(tǒng)軌跡的預(yù)測(cè)

假設(shè)小車的實(shí)際位移方程為，a為加速度取1，t∈（0，5），測(cè)量噪聲方差R為10，分別取過(guò)程噪聲方差Q為0.5和5，得到在不同噪聲強(qiáng)度干擾下的小車系統(tǒng)的位移真值與估計(jì)值如圖7和圖8。

3.3? 系統(tǒng)的智能控制

將制作所需的硬件按照模型概念圖安裝完成。向STM 32單片機(jī)中燒錄編寫好的程序，給上車系統(tǒng)上電，上電時(shí)系統(tǒng)默認(rèn)已經(jīng)開(kāi)啟內(nèi)置Wi-Fi。打開(kāi)Windows上位機(jī)將PC端連接系統(tǒng)Wi-Fi，實(shí)現(xiàn)對(duì)小車系統(tǒng)的遠(yuǎn)程控制。經(jīng)過(guò)實(shí)際調(diào)試運(yùn)行，小車系統(tǒng)的控制范圍為50m，具有實(shí)時(shí)視頻傳輸、拍照及視頻存儲(chǔ)功能。在小車系統(tǒng)運(yùn)行過(guò)程中，Windows上位機(jī)能夠?qū)崟r(shí)顯示環(huán)境溫濕度、航向、運(yùn)行速度和空氣中氣體含量等信息。軟件界面如圖9所示。

4? 結(jié)? 論

（1）在小車系統(tǒng)勻速運(yùn)行時(shí)，利用增量式PID算法實(shí)現(xiàn)對(duì)系統(tǒng)電機(jī)轉(zhuǎn)速的精準(zhǔn)控制。測(cè)試結(jié)果表明，時(shí)間在0.2到1.6秒范圍內(nèi)，小車左輪速度與右輪速度，明顯趨近初始設(shè)定速度，左輪速度誤差與右輪速度誤差趨近于零，由小車的控制律可以看出，增量式PID控制器能夠穩(wěn)定地減小外界干擾使電機(jī)產(chǎn)生的速度誤差。

（2）利用卡爾曼濾波算法對(duì)系統(tǒng)在外界干擾下的軌跡進(jìn)行預(yù)測(cè)以及不斷修正，由于小車的加速階段狀態(tài)方程具有不確定性，所以增加過(guò)程噪聲。經(jīng)測(cè)試，隨著噪聲的增大（即干擾強(qiáng)度增大），估計(jì)值逐漸接近真值，證明了模型的合理性及有效性。

（3）小車系統(tǒng)通過(guò)Wi-Fi進(jìn)行視頻實(shí)時(shí)傳輸，采用多種傳感器實(shí)時(shí)準(zhǔn)確地顯示周圍環(huán)境信息，通過(guò)Windows上位機(jī)對(duì)系統(tǒng)進(jìn)行精準(zhǔn)控制和自主避障。該系統(tǒng)可用于有線電纜布線難度大的監(jiān)控地點(diǎn)、工業(yè)環(huán)境復(fù)雜危險(xiǎn)作業(yè)面、公安防衛(wèi)敏感地帶及環(huán)化高位污染區(qū)域搜索等復(fù)雜強(qiáng)干擾的環(huán)境中。如何快速地減小外界干擾使電機(jī)轉(zhuǎn)速產(chǎn)生的誤差，這將是未來(lái)研究的另一個(gè)方向。

參考文獻(xiàn)：

[1] 宋璐璐.WIFI技術(shù)的優(yōu)勢(shì)及發(fā)展分析 [J].電子世界，2015（21）：91-92.

[2] 王正家，何博，李濤，等.基于模糊自適應(yīng)PI的無(wú)刷直流電機(jī)調(diào)速系統(tǒng)研究 [J].現(xiàn)代電子技術(shù)，2019，42（1）：139-142.

[3] 冷雪鋒.基于PID的STM32智能小車機(jī)器人的設(shè)計(jì) [J].自動(dòng)化技術(shù)與應(yīng)用，2016，35（11）：122-127.

[4] 胡改玲，桂亮，權(quán)雙璐，等.串聯(lián)模糊PID控制的四旋翼無(wú)人機(jī)控制系統(tǒng)設(shè)計(jì) [J].實(shí)驗(yàn)技術(shù)與管理，2019，36（3）：132-135.

[5] Ali R Y，Bilal E，Ak?n D，et al.Design of gain-scheduling PID controllers for Z-source inverter using iterative reduction-based heuristic algorithms [J].Simulation Modelling Practice and Theory，2019，94：162-176.

[6] 董藝，司文凱.基于步進(jìn)式PID控制的直流電機(jī)啟動(dòng)控制系統(tǒng)研究 [J].通化師范學(xué)院學(xué)報(bào)，2019，40（2）：12-15.

[7] 吳房勝，徐金秀，李如平.增量型PID算法控制的嵌入式視頻智能車設(shè)計(jì) [J].宜賓學(xué)院學(xué)報(bào)，2016，16（12）：48-52.

[8] 龔健穎，劉忠富，金健男，等.基于STM32的電子羅盤設(shè)計(jì) [J].物聯(lián)網(wǎng)技術(shù)，2015，5（12）：8-9+12.

[9] 馬傳彬，王志坤.空氣質(zhì)量檢測(cè)與傳感器的應(yīng)用 [J].電子世界，2017（11）：143.

[10] 王守華，李云柯，紀(jì)元法，等.一種基于位置修正和卡爾曼濾波的姿態(tài)角推算算法 [J].科學(xué)技術(shù)與工程，2019，19（7）：115-119.

[11] 楊軍，羅建，趙春波.IEC61850國(guó)際標(biāo)準(zhǔn)通信協(xié)議 [J].重慶電力高等專科學(xué)校學(xué)報(bào)，2004（4）：1-4.

作者簡(jiǎn)介：張哲（1995-），男，回族，浙江溫州人，碩士生，研究方向：嵌入式技術(shù)及應(yīng)用。