01 方案綜述

1.1 項(xiàng)目設(shè)計(jì)意義

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

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

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

1.2 項(xiàng)目方案總述

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

1.3 項(xiàng)目任務(wù)要求

1.3.1 競賽要求

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

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

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

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

2.1 總述

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

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

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

2.2 車模切割與支架安裝

2.2.1 車模切割與電池安裝

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

裁切完成之后，我們?cè)谲嚹５撞课恢谜迟N了魔術(shù)貼，以此方便地進(jìn)行電池的安裝與更換。

2.2.2 動(dòng)量輪與支架的安裝

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

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

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

1. 在車模底部安裝動(dòng)量輪，可降低重心。

2. 在車模頂部安裝動(dòng)量輪，可提升力矩。

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

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

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

2.3 電路板的安裝

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

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

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

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

2.3.1 主控板PCB安裝

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

03 硬件設(shè)計(jì)

3.1 外設(shè)選型

本方案中，我們使用了大量的外設(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化，這里對(duì)這些模塊進(jìn)行一些簡單的介紹，讓我們?cè)谠O(shè)計(jì)硬件的過程中對(duì)各種模塊與MCU的連接不至于太陌生。

各個(gè)模塊的具體介紹與接口定義如下表，各種接口目前的封裝都是標(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ū)動(dòng)MCU和其他絕大部分外設(shè)，模塊。事實(shí)上，考慮到一些外設(shè)需要5V供電加之5V也是常用的邏輯電平，我們常用的方案是將電源電壓先降到5V，再從5V降到3.3V。

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

LDO最大的優(yōu)點(diǎn)是非常好用，1VIN，1VOUT和1GND三個(gè)引腳即可使用，幾乎不會(huì)壞。但是LDO本質(zhì)上還是線性穩(wěn)壓器，舉個(gè)例子用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的降壓方式類似于一個(gè)滑動(dòng)變阻器的話，DCDC的降壓方式類似與快速開關(guān)電源輸入來達(dá)到預(yù)定的電壓。這決定了DC-DC的效率一般可以超過90%。

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

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

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

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

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

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

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

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

TC264D是英飛凌AURIX系列嵌入式芯片中典型的一款，片上集成兩個(gè)核心。AURIX系列芯片主要是面向汽車電子的各種控制類應(yīng)用，集成了各種各樣的復(fù)雜功能，因此對(duì)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)亮芯片，但實(shí)際上為了穩(wěn)定，我們通常使用3.3V供電+1.3V開關(guān)電源輸入的方式為芯片提供電力。

TC264D內(nèi)部還支持多種方式產(chǎn)生1.3V的核心電壓，這里主要分為LDO和SMSP模式，正好對(duì)應(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的輸入引腳）這四類。

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

3.4 驅(qū)動(dòng)電路設(shè)計(jì)與實(shí)現(xiàn)

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

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

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

3.4.2 橋臂mos管選型

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

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

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

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

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

04 軟件設(shè)計(jì)

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

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

4.1.1 4-Layers架構(gòu)

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

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

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

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

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

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

4.2.1 移植RT-Thread到Aurix MCU

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

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

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

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

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

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

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

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

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

05 控制原理

5.1 總述

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

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

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

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

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

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

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

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

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

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

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

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

其中 為拉格朗日算子， 為系統(tǒng)的廣義坐標(biāo)， 為系統(tǒng)的動(dòng)能， 為系統(tǒng)的勢能。同時(shí)對(duì) 作線性化，用 代替?？梢缘玫饺缦碌臓顟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)動(dòng)慣量和飛輪繞其圓心的轉(zhuǎn)動(dòng)慣量。c_1 ,c_2$ 分別是擺桿繞原點(diǎn)的摩擦阻力矩系數(shù)和飛輪繞其圓心的摩擦阻力矩系數(shù)。

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

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

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

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

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

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

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

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

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

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

5.4.1 卡爾曼濾波理論分析

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

• 先驗(yàn)估計(jì)：

• 計(jì)算先驗(yàn)誤差協(xié)方差：

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

• 更新卡爾曼增益：

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

• 后驗(yàn)估計(jì)：

  $$\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)用卡爾曼濾波

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

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

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

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

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

5.5 PID平衡控制算法

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

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

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

5.5.2 串級(jí)PID控制算法

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

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

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

06 圖像處理

6.1 總體思路

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

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

6.2 灰度圖像處理

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

6.2.1 固定閾值法

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

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

6.2.2 OTSU二值化

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

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

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

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

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

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

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

6.3 八鄰域跟蹤算法

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

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

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

6.4 拐點(diǎn)搜索

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

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

6.5.1 十字路口元素

①十字前

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

②十字中

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

③斜入十字

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

6.5.2 環(huán)島元素

①入環(huán)前

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

②入環(huán)中

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

6.5.3 小彎道

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

07 程序調(diào)試

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

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

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

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

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

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

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

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

08 功能測試

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

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

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

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

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

09 結(jié) 論

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

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

9.2 不足與改進(jìn)

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

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

9.3 致謝

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

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