01 方案綜述

1.1 項目設(shè)計意義

自行車機(jī)器人是雙輪靜態(tài)不穩(wěn)定、動態(tài)可穩(wěn)定機(jī)器人的典型代表。研制能自動駕駛的兩輪智能交通工具，是控制領(lǐng)域經(jīng)典而富有學(xué)術(shù)研究價值的問題。兩輪不共軸系統(tǒng)與共軸系統(tǒng)相比，具有靜止?fàn)顟B(tài)平衡、占地面積小、通過性強(qiáng)和靈活性高的特點(diǎn)。

但是自行車研究目前存在以下問題，首先是自行車平衡系統(tǒng)成本高昂。在互聯(lián)網(wǎng)上流傳甚廣的稚暉君的平衡自行車系統(tǒng)，其動量輪，驅(qū)動器與傳感器的成本過高，不利于自行車機(jī)器人技術(shù)的普及。其次，目前自行車的平衡需要依靠高速旋轉(zhuǎn)的動量輪，極易造成安全事故。

因此，我組以《競賽》規(guī)定的K型車模為基礎(chǔ)，設(shè)計開發(fā)了基于Infineon Aurix平臺的自平衡自行車系統(tǒng)，以較低的成本和較高的安全性，驗證實現(xiàn)了自行車的自平衡與攝像頭賽道循跡。

1.2 項目方案總述

根據(jù)競賽規(guī)則相關(guān)規(guī)定，平衡單車組智能車系統(tǒng)允許使用各類電磁、紅外光電、攝像頭、激光、超聲傳感器器件進(jìn)行賽道和環(huán)境檢測，車模微控制器使用Infineon Aurix系列多核單片機(jī)。賽車的位置信號可由攝像頭或者電磁傳感器采集，動控制決策。通過編碼器測速模塊來檢測車速，進(jìn)行脈沖計數(shù)計算速度和路程；電機(jī)轉(zhuǎn)速控制采用 PID 串級控制。

1.3 項目任務(wù)要求

1.3.1 競賽要求

1. 車模單車使用K型車模。在車模上允許使用動量輪，動量桿維持車模平衡。如果采用無刷電機(jī)驅(qū)動動量輪，則需要使用基于Infineon的驅(qū)動方案。車模作品制作完畢后，對于車模外形尺寸沒有限制。

2. 微控制器與傳感器車模微控制器采用Infineon單片機(jī)。傳感器允許使用攝像頭、CCD、光電管、電磁等。

3. 賽道與比賽任務(wù)比賽賽道采用室內(nèi)循環(huán)賽道，賽道上鋪設(shè)有電磁引導(dǎo)線。車模從車庫出發(fā)，在賽道上運(yùn)行一周后，最終駛?cè)胲噹?。計時標(biāo)準(zhǔn)：車模從車庫駛出到重新駛?cè)胲噹熘g的時間差。

02 機(jī)械設(shè)計

2.1 總述

對平衡自行車這類結(jié)構(gòu)，機(jī)械設(shè)計是決定車輛性能的關(guān)鍵。為了降低控制復(fù)雜度，增強(qiáng)平衡控制的魯棒性，尤其需要對車模的重心、轉(zhuǎn)彎半徑等參數(shù)進(jìn)行嚴(yán)格的測試與控制。本項目依托《全國大學(xué)生智能汽車競賽》的K車模進(jìn)行搭建。在多輪測試下，我們選擇了一種比較優(yōu)秀的結(jié)構(gòu)設(shè)計。有效提高了車身的穩(wěn)定性。

而為了適應(yīng)自平衡等功能的需要，我們對車模進(jìn)行了3D建模，并在此基礎(chǔ)上，對車模進(jìn)行了多層次的改造。

我組對車模的機(jī)械改造主要分以下幾個部分：動量輪以及支架的安裝，電池安裝與車模切割，攝像頭支架安裝等。

2.2 車模切割與支架安裝

2.2.1 車模切割與電池安裝

單車車模體積小巧，如果想在車模已安裝動量輪的前提下，原本放置電池的位置被動量輪替代，因此車模的必須要對車模進(jìn)行裁剪，來安裝電池降低重心，在保證車模原始結(jié)構(gòu)不受影響的前提下，我們對車模進(jìn)行了最低限度的裁剪。

裁切完成之后，我們在車模底部位置粘貼了魔術(shù)貼，以此方便地進(jìn)行電池的安裝與更換。

2.2.2 動量輪與支架的安裝

為了使剪裁后的車模更加穩(wěn)定，我們使用了龍邱的動量輪支架和的直流電機(jī)，實現(xiàn)車體、電機(jī)、飛輪、編碼器和齒輪的配合。

動量輪支架分為上下兩部分，上部用于連接車模，下部用于固定電機(jī)和編碼器，上下兩部分之間使用三角體連接塊連接。上半部分支架有多個螺絲安裝孔，可以直接安裝在車模原有孔位上，無需再對車模打孔。

關(guān)于動量輪支架的安裝，我組嘗試了兩種方案：

1. 在車模底部安裝動量輪，可降低重心。

2. 在車模頂部安裝動量輪，可提升力矩。

在根據(jù)理論推導(dǎo)與實際安裝測試比較后，我組選擇了方案一作為實裝方案。

2.2.3 舵機(jī)選擇與安裝

電單車依靠一顆舵機(jī)實現(xiàn)車頭轉(zhuǎn)向與平衡保持，是單車中最重要的執(zhí)行機(jī)構(gòu)。K 車模原配舵機(jī)為 SD12 舵機(jī)，其體積小，且為金屬齒輪舵機(jī)，響應(yīng)速度、扭矩滿足使用需求。但實際使用后發(fā)現(xiàn)，調(diào)試時不可避免的車模失控，車頭撞擊墻等，容易導(dǎo)致小舵機(jī)齒輪損壞，損壞率高?？紤]到整個舵機(jī)價格較高，為降低成本，選取了 SD5 舵機(jī)。相比 SD12 舵機(jī)，SD5 扭矩更大，不易損壞，響應(yīng)速度滿足要求。但由于不是原配舵機(jī)，其安裝成了需要解決的問題。為此我們使用了簡易的安裝支架，如圖：

2.3 電路板的安裝

考慮到 K 車模上并沒有較多的空間來安裝電路板，因此采用極簡的設(shè)計風(fēng)格，放棄供應(yīng)商提供的主控核心板，而采取全4層PCB+SMT的工藝。我組將電路板上所需的部分分為了2個模塊，分別是：

1. 主控板，包含主控芯片以及各種電源模塊，同時也有部分傳感器接口。

2. 電機(jī)驅(qū)動板：包含電機(jī)驅(qū)動模塊。

電路板的安裝需要考慮各種因素，所以我組對此也嘗試了好幾種方案，并且在最終選出了最適合我們車模的安裝方式。

2.3.1 主控板PCB安裝

主控板PCB上搭載MCU TC264D芯片、三組DC-DC供電與各類外設(shè)接口。為了方便主控板的安裝，我組選擇直接在車模側(cè)面使用魔術(shù)貼固定主控板。

03 硬件設(shè)計

3.1 外設(shè)選型

本方案中，我們使用了大量的外設(shè)模塊來實現(xiàn)各類功能。這些外設(shè)依托各種各樣的總線、接口與MCU交換數(shù)據(jù)。例如，測量加速度和角速度的IMU使用I2C總線，TF卡使用了SPI接口，藍(lán)牙串口使用了UART。其他大部分使用了GPIO口來模擬其他的通信協(xié)議或者連接了片上外設(shè)比如PWM輸出等等。

3.1.1 使用模塊簡介

在本方案中，我們使用MT9V032灰度攝像頭采集賽道信息，ICM-20602感知車身的姿態(tài)，用舵機(jī)控制車身前進(jìn)的方向，編碼器采集轉(zhuǎn)速，TF卡座和藍(lán)牙串口作調(diào)試用，TFT屏幕顯示各種狀態(tài)。

各種模塊的供電電壓不同，通信方式也千變?nèi)f化，這里對這些模塊進(jìn)行一些簡單的介紹，讓我們在設(shè)計硬件的過程中對各種模塊與MCU的連接不至于太陌生。

各個模塊的具體介紹與接口定義如下表，各種接口目前的封裝都是標(biāo)準(zhǔn)的HDR2.54接口或者排線接口。

3.2 系統(tǒng)供電單元

3.2.1 供電方案分析

智能車競賽一般使用2芯或3芯串聯(lián)的鋰電池作為供電電池，這種電池額定電壓為7.4V，在滿電狀態(tài)下最高不超過8.4V。因此必須使用降壓電路，將電源電壓降到3.3V才能正常驅(qū)動MCU和其他絕大部分外設(shè)，模塊。事實上，考慮到一些外設(shè)需要5V供電加之5V也是常用的邏輯電平，我們常用的方案是將電源電壓先降到5V，再從5V降到3.3V。

我組在兩種常見的降壓芯片類型，LDO和DC-DC之中選擇DC-DC作為供電方案。

LDO最大的優(yōu)點(diǎn)是非常好用，1VIN，1VOUT和1GND三個引腳即可使用，幾乎不會壞。但是LDO本質(zhì)上還是線性穩(wěn)壓器，舉個例子用5V轉(zhuǎn)3.3V的LDO AMS1117-3.3，輸出1A電流需要輸入1A電流，也就是有1.7W的功率熱耗散了。這就意味這LDO的輸入輸出壓差不能太大，負(fù)載也不能很多。

DC-DC，簡而言之，就是開關(guān)電源，我們?nèi)粘Ｊ褂玫某潆娖?5%都用到了DC-DC電路。如果說LDO的降壓方式類似于一個滑動變阻器的話，DCDC的降壓方式類似與快速開關(guān)電源輸入來達(dá)到預(yù)定的電壓。這決定了DC-DC的效率一般可以超過90%。

3.2.2 DC-DC芯片選擇（本方案選用）

DC-DC的降壓芯片選用在各種高端ARM的Reference Design中出現(xiàn)的矽力杰同步降壓芯片SY8113和SY8089，這兩款芯片目前在市面上使用廣泛，且能找到各種廉價的PIN to PIN替代品。

3.2.3 供電電路的設(shè)計

為了實現(xiàn)穩(wěn)定供電并且最大限度濾除DC-DC帶來的紋波，我組在參考了廠商的Reference Design的基礎(chǔ)上進(jìn)一步優(yōu)化供電電路，如使用固態(tài)電容濾除雜波。最小化電流環(huán)與負(fù)反饋通路，減小對其他電路的干擾。SY8113與SY8089降壓電路圖如下：

3.3 主控板電路設(shè)計與實現(xiàn)

智能車的主板是承載了MCU的最小系統(tǒng)，各種常用的板載外設(shè)，外設(shè)接口以及設(shè)備供電電路。作為一個復(fù)雜系統(tǒng)工程的硬件核心，主板可以說是智能車一切的基礎(chǔ)。主板的設(shè)計，我們采用分而治之的策略，即將各種功能電路高度解耦，保證每一部分的正常工作，便可以得到一個完整主板。

以下是本方案的大致框架圖，其中TC264D右側(cè)的為板載外設(shè)，下方的均為通過2.54mm HDR接口與其他各種設(shè)備連接。電源方面，方案采用了串級DC-DC降壓方案，提供穩(wěn)定，高效率的5V/3.3V供電。

3.3.1 TC264D的供電輸入與核心電壓配置

TC264D是英飛凌AURIX系列嵌入式芯片中典型的一款，片上集成兩個核心。AURIX系列芯片主要是面向汽車電子的各種控制類應(yīng)用，集成了各種各樣的復(fù)雜功能，因此對TC264D的供電，也不能想一般芯片那樣，一正一負(fù)就可以正常工作。

TC264D的供電主要分為3類，第一類是支持5V-3.3V寬電壓輸入的，比如說IO的供電和ADC的電源，第二類是只支持3.3V電壓輸入，在芯片中主要負(fù)責(zé)給Flash供電，最后是核心的1.3V電壓，這與電腦CPU的供電類似，需要精確且高效的供電支持。

TC264D內(nèi)部集成了5V降壓到3.3V的LDO和3.3V到1.3V的電路，理論上來說，只需要單獨(dú)5V供電即可點(diǎn)亮芯片，但實際上為了穩(wěn)定，我們通常使用3.3V供電+1.3V開關(guān)電源輸入的方式為芯片提供電力。

TC264D內(nèi)部還支持多種方式產(chǎn)生1.3V的核心電壓，這里主要分為LDO和SMSP模式，正好對應(yīng)了上文提到的兩種降壓方式。在本方案中，我們使用了較為穩(wěn)定的SMSP模式產(chǎn)生1.3V電壓，并使用了英飛凌的BSL215C P+N MOS管芯片作為上下管。

3.3.2 TC264D與主控板外設(shè)的連接

TC264D的引腳大致可以分為，電源引腳（VCC,VSS等），功能引腳（如晶振，復(fù)位，JTAG等），IO（可以直接指定高低電平，也可以做片上外設(shè)輸出），模擬輸入引腳（默認(rèn)作VADC的輸入引腳）這四類。

因為TC264D的應(yīng)用場景是汽車電子，芯片內(nèi)部大部分片上外設(shè)可以認(rèn)為是面向功能的，這就導(dǎo)致了其片上外設(shè)經(jīng)常將不同功能器件比如定時器，ADC等封裝成一個外設(shè)。所以我們在后續(xù)編程的過程中，很重要的一步是將這些外設(shè)配置成我們希望的功能。

3.4 驅(qū)動電路設(shè)計與實現(xiàn)

為了使我們的系統(tǒng)擁有澎湃的動力，正確驅(qū)動電機(jī)必須要放在首位。就以競賽中常見的380有刷電機(jī)舉例，經(jīng)我們實測，其穩(wěn)定運(yùn)行的電流在0.6A左右，而堵轉(zhuǎn)時電流可以達(dá)到驚人的16A。如果直接用MCU的GPIO直接控制電機(jī)，無異于蚍蜉撼大樹。因此，在智能車競賽中，我們通常將功率輸出與邏輯控制分離，使用一塊和主板分立的電機(jī)驅(qū)動板來單獨(dú)驅(qū)動電機(jī)。

3.4.1 智能有刷電機(jī)驅(qū)動芯片

DRV8701芯片是TI公司在2015年發(fā)布的智能全橋驅(qū)動芯片，可以驅(qū)動4片外置的N-MOS組成的H橋，還支持多種邏輯控制方案，柵極驅(qū)動電流配置，限制堵轉(zhuǎn)電流，3.3V/4.8V的LDO輸出。

3.4.2 橋臂mos管選型

下表列舉了三種容易買到的貼片mos管，在驅(qū)動能力方面，都可以滿足競賽需要，可以根據(jù)具體需求，如廠家限制，封裝選擇，驅(qū)動板面積等可自行選擇，最終我組選擇TPH1R403NL作為實裝的驅(qū)動mos管。

3.4.3 DRV8701外圍電路設(shè)計

使用DRV8701驅(qū)動有刷電機(jī)需要對參考電壓與電機(jī)電流進(jìn)行限制，如下式：

其中A_V是芯片內(nèi)部放大器的放大系數(shù)，等于20V/V，V_off=0.05V，所以一般取0.02Ω，即可實現(xiàn)10A左右的電流控制。

如此，DRV8701便可以被我們完美的使用起來了。在本方案中，實際上未使用電流限制，以便達(dá)到最大驅(qū)動電流。參考設(shè)計如下：

04 軟件設(shè)計

4.1 系統(tǒng)軟件框架

對于一個多任務(wù)的，涉及運(yùn)動平衡、圖像處理、存儲等功能的復(fù)雜系統(tǒng)，良好健康的軟件框架尤其重要。

4.1.1 4-Layers架構(gòu)

因為本項目的相關(guān)功能代碼從嵌入式底層到實時操作系統(tǒng)均有涉及，跨度較大。因此，我組將工程代碼抽象為4層：

• Hardware Layer與Infineon的iLLD庫耦合，負(fù)責(zé)對MCU的各類片上外設(shè)進(jìn)行配置并抽象出通信接口。

• Peripheral Layer通過Hardware Layer訪問系統(tǒng)的外設(shè)模塊如SD卡、IMU等，根據(jù)各個模塊的具體功能抽象出接口給算法使用。

• Algorithm Layer依靠Peripheral Layer提供的數(shù)據(jù)進(jìn)行相關(guān)算法處理比如Kalman Filter、Image Process等

• Thread Layer則是按照RTOS的邏輯將各類算法，外設(shè)操作按照線程分類、組織，并最終構(gòu)建系統(tǒng)。

4.2 實時操作系統(tǒng)RT-Thread

4.2.1 移植RT-Thread到Aurix MCU

RT-Thread是當(dāng)前熱門的國產(chǎn)RTOS，版權(quán)屬于上海睿賽德電子科技有限公司，于2006 年1月首次發(fā)布，初始版本號為0.1.0，經(jīng)過10來年的發(fā)展，如今主版本號已經(jīng)升級到3.0，累計開發(fā)者達(dá)到數(shù)百萬。

為了在TC264D上使用RT-Thread，就必須進(jìn)行移植，尤其是線程切換等設(shè)計內(nèi)核操作的函數(shù)。而TriCore 提供了一種硬件的上下文機(jī)制，這種機(jī)制是專為嵌入式實時操作系統(tǒng)設(shè)計的，目的就是為了能提高線程切換的效率。

因此，我們借助廠商的支持，在TC264D的CPU0內(nèi)使用了RT-Thread進(jìn)行線程管理，而CPU1作為協(xié)處理器進(jìn)行異步的圖像處理操作。

4.2.2 RT-Thread在項目中的調(diào)度

在本項目中，我組主要使用了RT-Thread的動態(tài)線程與信號量功能。在所有片上，片外外設(shè)初始化完畢后，系統(tǒng)創(chuàng)建若干線程，交由RT-Thread進(jìn)行調(diào)度。

Thread Balance負(fù)責(zé)車輛的姿態(tài)解算、平衡控制與其他各類運(yùn)動學(xué)控制。系統(tǒng)接收到由IMU ICM20602發(fā)出的125Hz中斷信號即發(fā)出信號量將Thread Balance添加到就緒隊列進(jìn)行運(yùn)動學(xué)處理與控制。

Thread Camera主要接收來自協(xié)處理器CPU1處理、提取完成的圖像信息，進(jìn)而進(jìn)行方向控制。同時在也查詢接收按鍵信號量來進(jìn)行拍攝任務(wù)。

Thread Key 負(fù)責(zé)對系統(tǒng)的8個按鍵開關(guān)進(jìn)行掃描，只要檢測到按鍵從release跳轉(zhuǎn)到press或者long press狀態(tài)就發(fā)送對應(yīng)的信號量供其他線程接收。

Thread Life 負(fù)責(zé)對系統(tǒng)狀態(tài)進(jìn)行監(jiān)測，若系統(tǒng)長時間無法切入Life線程，則發(fā)出提示信號，是調(diào)試代碼時，檢驗系統(tǒng)穩(wěn)定性的保障。

05 控制原理

5.1 總述

本項目在競賽要求的基礎(chǔ)上，搭建一個可保持自平衡，具有環(huán)境感知能力并且能夠在保持穩(wěn)定的系統(tǒng)。其中系統(tǒng)的自穩(wěn)定最為關(guān)鍵。我組使用動量輪與控制前輪方向的舵機(jī)作為協(xié)同平衡控制器，icm20602為陀螺儀和加速度計的硬件架構(gòu)。軟件控制算法為PID控制，使用了串級PID、增量式PID、積分分離等手段。對于傳感器測量的源數(shù)據(jù)則采用了Kalman Filter和滑動濾波來得到控制的反饋量。系統(tǒng)的平衡算法，先后經(jīng)過MATLAB系統(tǒng)仿真與實機(jī)調(diào)試驗證，具有較好的性能。

5.2 硬件傳感器設(shè)計

要保持平衡就需要獲取車身的狀態(tài)，包括車身角度、車身角速度。在此我組使用ICM20602作為實裝IMU。ICM20602是新一代MEMS IMU具有極小的零漂與封裝尺寸。僅需使用一個模塊就可以完成對兩個量的測量。同時，ICM20602支持的通信端口包括I2C和10MHz的高速SPI，完全適用于單車的平衡控制。

對于動量輪的控制，除了使用合適的驅(qū)動來控制電機(jī)還需要安裝編碼器來實時測量動量輪的轉(zhuǎn)速，使其向我們的目標(biāo)轉(zhuǎn)速(0rad/min)趨近。在此我們使用的是1024線正交編碼器。實際使用只要4根線電源、地、A相、B相。由于動量輪的轉(zhuǎn)速變換范圍較大，如果使用96線或更小的編碼器會導(dǎo)致在低轉(zhuǎn)時量化誤差較大。1024線時在經(jīng)濟(jì)和性能考慮下較為合適的選擇。

5.3 自行車運(yùn)動學(xué)建模與仿真

對自行車平衡控制，需要對系統(tǒng)進(jìn)行建模來分析系統(tǒng)的各類性能。為了建模的方便，我們將自行車的車身近似為剛體處理，動量輪為偏移車身重心的提供動量的剛體。于是，我們化用倒立擺系統(tǒng)對自行車的運(yùn)動學(xué)進(jìn)行分析。

5.3.1 簡化系統(tǒng)的建模分析

我們常見的倒立擺或單擺系統(tǒng)如圖一所示。若要對其進(jìn)行控制，需要在轉(zhuǎn)軸出基于擺桿一個力矩，如下圖：

對于此系統(tǒng)利用牛頓運(yùn)動定理和簡單的線性化即可得到系統(tǒng)對于的狀態(tài)空間方程：

其中 在支點(diǎn)給輕桿一個力矩T。

但對于我們的單車模型來說，雖然也可近似為一個倒立擺模型，但其控制輸入量來源于動量輪，而非轉(zhuǎn)軸支點(diǎn)處的力矩簡化如圖二。這極大的增加了系統(tǒng)的復(fù)雜程度。

利用拉格朗日方程推導(dǎo)運(yùn)動學(xué)方程：

其中 為拉格朗日算子， 為系統(tǒng)的廣義坐標(biāo)， 為系統(tǒng)的動能， 為系統(tǒng)的勢能。同時對 作線性化，用 代替?？梢缘玫饺缦碌臓顟B(tài)空間方程：

其中  $a = m_1 L_1^{2  + 4m_2 L_1}2  + I_1，b = \left( {m_1  + m_2 } \right)gL_1。m_1 ,m_2分別是擺桿和飛輪的質(zhì)量。L_1 ,L_2分別是擺桿質(zhì)心到原點(diǎn)距離和到飛輪質(zhì)心的距離。I_1 ,I_2分別是擺桿繞原點(diǎn)的轉(zhuǎn)動慣量和飛輪繞其圓心的轉(zhuǎn)動慣量。c_1 ,c_2$ 分別是擺桿繞原點(diǎn)的摩擦阻力矩系數(shù)和飛輪繞其圓心的摩擦阻力矩系數(shù)。

5.3.2 倒立擺系統(tǒng)的SIMULINK仿真

由上面的系統(tǒng)狀態(tài)方程我們可以在SIMULINK中建立如下系統(tǒng)：

或者也可以建立物理模型進(jìn)行仿真：

為了與實際狀態(tài)更加相似，在物理模型中添加了一個配重塊，使擺桿的平衡角度不在豎直狀態(tài)。此時模擬的飛輪倒立擺系統(tǒng)仿真如下圖：

如此，可以建立其作為被控對象的子系統(tǒng)，在創(chuàng)建一個控制部分的子系統(tǒng)即可完成控制：

但實際上，此時電機(jī)的轉(zhuǎn)速是在一直增加的，由此利用電機(jī)轉(zhuǎn)速可以增加并連一條PID控制，保持電機(jī)轉(zhuǎn)速維持在較低的值。但此時，就無法保證達(dá)到任意的設(shè)定值了，倒立擺傾向于在臨界穩(wěn)定角度附近擺動。擺角響應(yīng)曲線如下圖：

5.4 姿態(tài)傳感器數(shù)據(jù)獲取與卡爾曼濾波

雖然，上文提到的ICM20602 IMU和1024線編碼器可以得到我們需要的數(shù)據(jù)。但直接由傳感器獲得的數(shù)據(jù)往往具有很多噪音，需要進(jìn)行濾波處理。

對于編碼器數(shù)據(jù)可以采用滑動濾波，這相當(dāng)于一個低通濾波。由于微分對突變量十分敏感故滑動濾波可以在一定程度上消減突變的干擾。

對于車輛傾角測量，icm20602可以得到實時的角速度與傾角。由角速度積分可以得到傾斜角度，但其具有誤差累積的特性。而加速度計可以直接解算出傾角，但其受多方影響，十分敏感，數(shù)值跳動劇烈。所以需要利用卡爾曼濾波得到更逼近真實值的傾角估計。

5.4.1 卡爾曼濾波理論分析

卡爾曼濾波簡單來說只有五個公式：

• 先驗估計：

• 計算先驗誤差協(xié)方差：

  $$P_k ^{-   = AP_{k - 1} A}T  + Q $$

• 更新卡爾曼增益：

  $$K_k  = {{P_k ^{-  H}T } \over {HP_k ^{-  H}T  + R}} $$

• 后驗估計：

  $$\hat x_k  = \hat x_k ^{-   + K_k \left( {z_k  - H\hat x_k} -  } \right) $$

• 更新協(xié)方差：

其中Q是過程噪聲的協(xié)方差，R是測量噪聲的協(xié)方差。

5.4.2 IMU的建模與應(yīng)用卡爾曼濾波

對于車模在YOZ平面上的姿態(tài)角θ，和陀螺儀的零漂 ，建立狀態(tài)向量

可得系統(tǒng)帶陀螺儀角速度輸入的狀態(tài)方程為：

而通過加速度計對重力分解，也可以求得角度的量測值，量測方程如下：

如此，便可以進(jìn)行卡爾曼濾波了。

融合后的波形大致如圖八所示，其中紅色的就是加速度解算出的角度，可以看到其在角度變換時有較大峰值，而紫線是濾波后的波形，跟隨靈敏且無超調(diào)。

5.5 PID平衡控制算法

PID的控制對象是動量輪，其接收icm20602和編碼器的反饋將單車角度穩(wěn)定在目標(biāo)傾角。在此，采用串級PID控制，角度環(huán)作內(nèi)環(huán)，而速度環(huán)作為外環(huán)。也就是說速度環(huán)的輸出是角度環(huán)的輸入，直觀理解速度環(huán)江改變設(shè)定的目標(biāo)角度，從而達(dá)到自適應(yīng)平衡的目標(biāo)。在此速度環(huán)采用了增量式PID，其輸出疊加在我們預(yù)先設(shè)定的目標(biāo)角度上。如此在保證平衡的前提下，使動量輪的速度趨近于0，如此更有利于單車的動態(tài)控制。

5.5.1 傳統(tǒng)分立式PID控制

直立車傳統(tǒng)控制系統(tǒng)下所示。為使其具有速度控制與姿態(tài)平衡能力，利用2個單獨(dú)的閉環(huán)分別進(jìn)行控制，輸出電機(jī)控制信號為各環(huán)輸出的疊加。該控制方案雖簡單但存在一定問題，如：僅單獨(dú)施加直立控制，其車身平衡效果雖能滿足要求，但此時如果在該基礎(chǔ)上額外施加速度控制，速度控制勢必會打破直立車的平衡姿態(tài)，使得速度與直立控制互相耦合，此時如果再施加轉(zhuǎn)向控制，就會導(dǎo)致高速時轉(zhuǎn)向控制實時性和姿態(tài)的平衡性欠佳。

5.5.2 串級PID控制算法

而串級的PID控制系統(tǒng)如圖5.12所示，速度控制車模的行駛姿態(tài)和動量輪速度。在速度閉環(huán)中，速度PID 計算輸出的值為單車在機(jī)械零點(diǎn)角需要左傾或者右仰的角度，該輸出值需與機(jī)械零點(diǎn)角相加才能將速度控制轉(zhuǎn)化為角度控制，而角度的變化是要由角速度產(chǎn)生的俯仰角度PID 計算輸出值為單車需要保持的角速度，傾斜角角速度PID 計算結(jié)果即為串級的輸出值

單車平衡控制使用串級PID，外環(huán)角度，內(nèi)環(huán)角速度，只需要通過姿態(tài)傳感器采集一個軸的數(shù)據(jù)即可。根據(jù)串級PID控制理論，外環(huán)(速度環(huán))的控制周期需要略小于內(nèi)環(huán)，在此內(nèi)環(huán)角度環(huán)的控制頻率設(shè)為與傳感器采樣頻率一致（125Hz）。另外，外環(huán)輸出傳遞給內(nèi)環(huán)之前，先加一個限幅防止異常突變數(shù)據(jù)的干擾，實測效果非常穩(wěn)定，不會震蕩，也不會出現(xiàn)明顯的反應(yīng)滯后。速度控制和平衡控制，轉(zhuǎn)化為角度的控制，整個控制系統(tǒng)中僅僅有角度環(huán)的輸出給電機(jī)控制，原理上可以有效地消除傳統(tǒng)速度PID 與直立平衡PID 并聯(lián)所產(chǎn)生的耦合。

當(dāng)系統(tǒng)受到多級噪聲時，內(nèi)環(huán)會先進(jìn)行調(diào)整，再由外環(huán)進(jìn)行調(diào)整，所以控制的精度和穩(wěn)定性都優(yōu)于單極PID控制［2］。通過實際小車運(yùn)行狀態(tài)來看，如圖十一所示。傳統(tǒng)算法圍繞中線產(chǎn)生了較大過調(diào)；串級PID算法過調(diào)明顯較小，且跟隨能力遠(yuǎn)強(qiáng)于傳統(tǒng)算法，可以看出常規(guī)PID 的穩(wěn)定時間比串級控制短。串級PID控制器比常規(guī)PID 控制系統(tǒng)具響應(yīng)迅速，穩(wěn)定性高等優(yōu)點(diǎn)［3］。

06 圖像處理

6.1 總體思路

在本項目中我們通過MT9V032 攝像頭進(jìn)行圖像采集，在軟件上通過圖像處理識別黑線，并進(jìn)行相應(yīng)的速度控制以及速度反饋。總體目的是為了控制車模在既定規(guī)則前提下，以最快的速度，跑完整個賽道。不論上哪種方案，軟件的總體框架總是相似的，我們追求的就是穩(wěn)定至上，兼顧速度。

以下是我組關(guān)于圖像處理方案的框圖。整體步驟可以概況為①灰度圖像二值化②八鄰域法搜索邊線③提取邊線拐點(diǎn)④賽道元素分析這幾步。

6.2 灰度圖像處理

通過MT9V032 攝像頭我們得到了120*80大小的灰度圖像，而處理圖像的第一步就是處理灰度圖像，主要有以下幾種思路實現(xiàn)：

6.2.1 固定閾值法

二值化圖像算法中最為基礎(chǔ)的算法，即通過給定一個閾值，將每個像素點(diǎn)與之進(jìn)行比較，從而確定出來這個像素點(diǎn)是黑或白，之后的圖像處理就使用這個二值化后的數(shù)組進(jìn)行處理。

這種方法在圖像處理的過程中僅需遍歷一次圖像，處理速度極快。但其對于光線的魯棒性較低，在實際的運(yùn)行過程中，會由于光線不均勻使得設(shè)定的單一閾值在每段賽道的效果層次不齊。

6.2.2 OTSU二值化

OTSU算法是由日本學(xué)者OTSU于1979年提出的一種對圖像進(jìn)行二值化的高效算法。利用最大類間方差將原圖像分成前景，背景兩個圖像。

OTSU算法實現(xiàn)的偽代碼如下：

OTSU算法實現(xiàn)簡單，當(dāng)目標(biāo)與背景的面積相差不大時，能夠有效地對圖像進(jìn)行分割。但是當(dāng)圖像中的目標(biāo)與背景的面積相差很大時，表現(xiàn)為直方圖沒有明顯的雙峰，或者兩個峰的大小相差很大，分割效果不佳，或者目標(biāo)與背景的灰度有較大的重疊時也不能準(zhǔn)確的將目標(biāo)與背景分開。同時該方法忽略了圖像的空間信息，將圖像的灰度分布作為分割圖像的依據(jù)，對噪聲也相當(dāng)敏感。

6.2.3 Sobel動態(tài)閾值二值化

Sobel算子為邊緣檢測的手段之一，通過對圖片進(jìn)行卷積，得到當(dāng)前像素在四個方向上的梯度信息，在梯度變化明顯的區(qū)間圖像會出現(xiàn)很明顯的高值，因此能夠檢測出完整的邊沿。

下圖展示了如何通過四個方向的Sobel算子來對輸入圖像進(jìn)行卷積：

因為每個像素位置處的二值化閾值是由其周圍鄰域像素的分布來決定的，同亮度、對比度、紋理的局部圖像區(qū)域?qū)碛邢鄬?yīng)的局部二值化閾值，所以Sobel算子適合處理光照不均的圖像。唯一的缺點(diǎn)是，其需要進(jìn)行大量的運(yùn)算，平均一幀需要處理20~30ms。

6.3 八鄰域跟蹤算法

在本項目中，我們主要采用八鄰域跟蹤算法實現(xiàn)巡線。

八鄰域跟蹤算法基本思想是，遍歷圖像找到第一個非零像素點(diǎn)，那么這個點(diǎn)一定是邊界點(diǎn)。設(shè)這個點(diǎn)為起始點(diǎn)x，按照0->1->2->3->4順時針查找該點(diǎn)八鄰域內(nèi)遇到的第一個非零像素點(diǎn)x'（點(diǎn)x'也為邊界點(diǎn)）。令x=x'，再進(jìn)行鄰域內(nèi)的順時針查找，查找的起點(diǎn)為剛剛x到x'過程中x'前一個零點(diǎn)。

可以看到通過八鄰域搜線能夠完整保留邊界信息，得到每一段邊線的所有像素，有了這個可靠基礎(chǔ)，我們下一步進(jìn)行拐點(diǎn)的搜尋。

6.4 拐點(diǎn)搜索

在拐點(diǎn)搜索上，我們采用比較基礎(chǔ)的平面幾何思想來進(jìn)行求解，在上一階段，我們獲取了賽道的各段邊線以及起始終止點(diǎn)信息，通過連接每段邊線的起始終止點(diǎn)，我們計算該段邊線中距離這條直線距離最大的點(diǎn)，即為該段邊線的拐點(diǎn)。

6.5 路徑擬合與賽道元素判別

6.5.1 十字路口元素

①十字前

正入十字情況下，連接十字的左右前后拐點(diǎn)進(jìn)行補(bǔ)線。

②十字中

十字中段，找到兩個后拐點(diǎn)，并利用最小二乘法向前拉線。

③斜入十字

在進(jìn)入十字中，存在不是以正常角度進(jìn)入的情況，即斜入十字，在這種情況下，找到一個后拐點(diǎn)以及另一邊的前后拐帶進(jìn)行補(bǔ)線。

6.5.2 環(huán)島元素

①入環(huán)前

當(dāng)識別到一邊為直線，另一邊出現(xiàn)了前后兩個拐點(diǎn)時，進(jìn)行環(huán)島前補(bǔ)線。

②入環(huán)中

在環(huán)內(nèi)直接按照基本巡線思路進(jìn)行巡線。

6.5.3 小彎道

當(dāng)存在邊線值方差相差不大時，判定為小彎道，進(jìn)行最小二乘擬合。

07 程序調(diào)試

7.1 上位機(jī)調(diào)試軟件簡介

在擁有較完善的硬件配置和軟件系統(tǒng)后，我們需要對現(xiàn)場調(diào)試中涉及的大量參數(shù)進(jìn)行軟硬件聯(lián)合調(diào)試，而這個過程需要一整套的開發(fā)調(diào)試工具以確定這些參數(shù)的最優(yōu)組合。

為此，我們團(tuán)隊自主開發(fā)了一套功能較完備的上位機(jī)軟件用于圖像二值化、賽道邊線的調(diào)試，并且能夠?qū)崟r監(jiān)測智能車在賽道上的運(yùn)行狀態(tài)。

7.2 離線SD卡數(shù)據(jù)分析

因為攝像頭車模所需處理的信息量非常大，在調(diào)試過程中往往需要存儲這些信息以供研究分析，而TC264的存儲空間遠(yuǎn)不能滿足我們的要求。所以我們設(shè)計了外部 SD 卡來存儲海量信息，這讓我們在圖像處理上有很多的發(fā)揮空間。

通過 SD卡，我們存下了我們需要的圖像信息和處理之后的信息，然后通過串口發(fā)送給上位機(jī)，這樣我們就可以方便地分析出圖像上的各種問題，以便在程序上做出修改。

7.3 上位機(jī)運(yùn)行模式

• 仿真模式：由于圖像處理越來越復(fù)雜，對于圖像仿真越來越重要，在PC 端我們可以直接將單片機(jī)代碼導(dǎo)入，查看圖像處理結(jié)果。
• 離線模式：小車的實際圖像反饋對于調(diào)車非常重要，利用 SD 卡將小車行進(jìn)中的圖像以及處理信息保存，上傳到上位機(jī)，以便利用視頻隨時隨地調(diào)試。
• 在線模式：為了提高調(diào)試效率，特別為上位機(jī)做了在線調(diào)試系統(tǒng)，即串口實時傳輸圖像到上位機(jī)，能夠在線直接通過上位機(jī)對其中的參數(shù)進(jìn)行調(diào)節(jié)，極大的節(jié)約時間消耗。
• 參數(shù)保存：上位機(jī)可以直接通過Ctrl+s保存當(dāng)前調(diào)節(jié)的參數(shù)至txt中。

08 功能測試

8.1 靜態(tài)平衡性能分析

將車模放置于靜止無風(fēng)的桌面上。開啟電源，自檢完成后系統(tǒng)便開始工作。由于自穩(wěn)定開始工作之前，車模實際傾角與機(jī)械零位存在較大偏差，舵機(jī)與動量輪同時工作維持平衡測量，記錄車模從開始工作到平衡的一系列數(shù)據(jù)，分析如下：

可見，在最初開始工作的1秒，為了將車身角度控制到機(jī)械零位，舵機(jī)與動量輪同時工作，輸出了較大的控制量。同時可以發(fā)現(xiàn)，舵機(jī)控制量與動量輪控制量在輸出形式上是基本互補(bǔ)的。

當(dāng)車身姿態(tài)穩(wěn)定之后，動量輪速度緩慢下降，最終以靜止的姿態(tài)達(dá)到平衡。

同時，也可以發(fā)現(xiàn)，車身在靜止后的一段時間出現(xiàn)了姿態(tài)的波動，這是由于串級PID中速度環(huán)的K_i較小導(dǎo)致的。實際使用中，較大的積分值能夠在高動態(tài)的環(huán)境下較好地工作，因此對靜態(tài)的系統(tǒng)性能有一定程度上的影響。

09 結(jié) 論

9.1 智能車主要技術(shù)參數(shù)

本次設(shè)計開發(fā)的Infineon Aurix平臺的自平衡自行車系統(tǒng)，以Infineon Tricore架構(gòu)的TC264D雙核MCU為主控，搭載RT-thread實時操作系統(tǒng)。成功地驗證并實現(xiàn)Kalman Filter-串級PID算法。在動量輪-舵機(jī)協(xié)同控制下的實現(xiàn)了車身自平衡功能。

9.2 不足與改進(jìn)

我們自身還是存在一些不足。例如，準(zhǔn)備初始階段，由于自身知識積累不夠，我們遇到很多困難，走了不少彎路，也犯了不少錯誤。我們在系統(tǒng)建模與仿真方面的能力還是略顯不足。但我們在不斷學(xué)習(xí)中、在實踐中熟練操作 MATLAB，逐漸對建模、仿真等有了進(jìn)一步的了解。

由于上海市和同濟(jì)大學(xué)疫情防控要求，在本學(xué)期后階段嘉定校區(qū)實施足不出宿舍等形式的封閉管理，項目開發(fā)不幸陷入停滯狀態(tài)。原本對車模的3D建模，異型PCB，無刷電機(jī)驅(qū)動控制等更進(jìn)一步的功能開發(fā)，甚至完整賽道運(yùn)行調(diào)試均無法正常進(jìn)行。希望能在未來，使用機(jī)理建模的方法，采用主流的先進(jìn)控制方法如LQR、MPC等等。

9.3 致謝

由于上海市疫情，在本學(xué)期后階段項目開發(fā)不幸陷入一連數(shù)月的停滯狀態(tài)。原本對車模的3D建模，異型PCB，無刷電機(jī)驅(qū)動控制等更進(jìn)一步的功能開發(fā)，甚至完整賽道運(yùn)行調(diào)試均無法正常進(jìn)行。所幸，在學(xué)院和指導(dǎo)老師的大力支持下，我們積極應(yīng)對現(xiàn)狀，通過線上渠道進(jìn)行溝通，在家附近租用場地進(jìn)行調(diào)試，在臨近比賽前幾天，我們更是在賓館中夜以繼日的趕進(jìn)度。在這個過程中，是隊員的齊心協(xié)力，攻堅克難，才讓小車從開始的顫顫巍巍到如今的堅強(qiáng)屹立。

在本文結(jié)束之際，特向同濟(jì)大學(xué)智能車及機(jī)器人競賽實驗室的全體參賽隊員、助管學(xué)長、指導(dǎo)老師以及所有給予我們幫助、建議和意見的朋友表示衷心的感謝！而這個過程中，離不開老師，學(xué)長和同學(xué)的幫助。也向籌備華東賽區(qū)和全國總決賽的南京信息工程大學(xué)表示感謝。感謝張志明、徐和根老師的教導(dǎo)和支持，感謝助管學(xué)長方少騰、韓子奇、曾憲坤、趙師兵、馮翊的無私幫助，感謝實驗室同學(xué)的相互支持和幫助。最后要感謝實驗室的隊友，道路是坎坷的，感謝大家的彼此陪伴和支持，一路走到了最后。我們一直相信，是團(tuán)隊協(xié)作的一個個夜晚，是遇到bug的一次次思考，是賽場調(diào)試的一場場分析，才最終造就了智行·YYYDS！正如我們隊伍的logo，只有三人彼此支撐，才能屹立不倒，做彼此的YYDS！