基于STM32的新型遙控巡跡小車設(shè)計(jì)?

徐琬婷, 柯國(guó)琴, 侯健偉

(1. 蕪湖職業(yè)技術(shù)學(xué)院 汽車與航空學(xué)院, 安徽 蕪湖,241006;2. 南京艾格慧元農(nóng)業(yè)科技有限公司, 江蘇 南京,451100)

隨著移動(dòng)小車的應(yīng)用越來越廣泛, 要求移動(dòng)小車環(huán)境適應(yīng)性也越來越強(qiáng), 這就對(duì)移動(dòng)小車的靈活性、機(jī)動(dòng)性有了更高的要求, 以滿足在不同的作業(yè)環(huán)境中靈活移動(dòng)。目前工業(yè)上所用的移動(dòng)小車類似于普通家用轎車的移動(dòng)方式, 該種方式自由度有限, 對(duì)空間要求高, 不適合狹小空間。最新出現(xiàn)的麥克納姆輪, 它是一種新型的, 能夠適用于狹小空間的高度靈活具有高機(jī)動(dòng)性的萬向輪。本設(shè)計(jì)利用麥克納姆輪構(gòu)建新型巡跡移動(dòng)小車, 并通過手機(jī)藍(lán)牙進(jìn)行無線控制, 可實(shí)現(xiàn)小車10 個(gè)自由度的移動(dòng), 且控制簡(jiǎn)單, 穩(wěn)定性強(qiáng), 是一種結(jié)構(gòu)簡(jiǎn)單、控制方便的新型移動(dòng)小車[1]。

1 系統(tǒng)總方案

系統(tǒng)需要實(shí)現(xiàn)多自由度精準(zhǔn)巡點(diǎn)移動(dòng)、無線遙控功能、循跡等功能, 充分考慮系統(tǒng)的成本及可靠性,制定總方案如圖1 所示。

圖1 系統(tǒng)總框圖

系統(tǒng)采用麥克納姆輪, 雖然非常適合于小空間、多轉(zhuǎn)向的場(chǎng)合, 但車輪的摩擦力較大, 控制也較為復(fù)雜, 需要在不同的方向受力完成定向移動(dòng)。為了能夠克服輪體的摩擦力, 并實(shí)現(xiàn)精準(zhǔn)控制, 系統(tǒng)采用了帶載能力強(qiáng)的驅(qū)動(dòng)模塊驅(qū)動(dòng)直流減速電機(jī), 再利用4 個(gè)直流減速電機(jī)帶動(dòng)麥克納姆輪。在這個(gè)過程中,直流電機(jī)上的編碼器將實(shí)時(shí)讀取解算電機(jī)轉(zhuǎn)速, 并構(gòu)成控制閉環(huán), 精確控制速度。另外, 小車的瞬時(shí)移動(dòng)的姿態(tài)也通過姿態(tài)傳感器實(shí)時(shí)采集不斷調(diào)整姿態(tài), 保證小車的運(yùn)行方向。

系統(tǒng)為了實(shí)現(xiàn)無線遙控功能。利用手機(jī)藍(lán)牙遙控APP, 再通過藍(lán)牙數(shù)據(jù)傳輸協(xié)議, 將控制信號(hào)發(fā)送至系統(tǒng)藍(lán)牙模塊, 并將其轉(zhuǎn)化為串口信號(hào), 與主控芯片相連, 建立數(shù)據(jù)通道, 即可實(shí)現(xiàn)無線控制。

綜上, 通過元器件比較, 巡跡移動(dòng)小車主要由下列模塊構(gòu)成: 它以STM32F103RCT6 為主控制器,采用BT04-A 藍(lán)牙模塊完成手機(jī)與系統(tǒng)通信, 紅外循跡模塊可以完成循跡,BTN7971 驅(qū)動(dòng)模塊完成電機(jī)驅(qū)動(dòng)帶動(dòng)麥克納姆輪, 并通過顯示屏進(jìn)行人機(jī)交互, 再通過MPU6050 姿態(tài)傳感器和編碼器進(jìn)行實(shí)時(shí)姿態(tài)及速度采集, 形成速度及姿態(tài)的閉環(huán)控制[2]。

2 系統(tǒng)硬件模塊設(shè)計(jì)

2.1 主控芯片最小系統(tǒng)設(shè)計(jì)

STM32F103RCT6 單片機(jī)是整個(gè)系統(tǒng)的核心, 在系統(tǒng)中, 需要用到的片內(nèi)資源有: 四路PWM 輸出,四路外部中斷輸入, 一路串口通信, 并且要有足夠快的速度進(jìn)行PID 運(yùn)算[3]。STM32F103RCT6 單片機(jī)有64 個(gè)引腳, 內(nèi)部資源豐富, 滿足系統(tǒng)需求, 且STM32F103R CT6 單片機(jī)價(jià)格較低,可以有效的降低小車成本。

STM32F103RCT 6 單片機(jī)供電電源為3.3 V, 還需要外圍電路保證其能正常運(yùn)行如復(fù)位電路、時(shí)鐘源以及濾波電容, 最小系統(tǒng)圖如圖2 所示。

圖2 STM32F103RCT6 的最小系統(tǒng)電路圖

2.2 麥克納姆輪的運(yùn)動(dòng)原理

麥克納姆輪分為麥克納姆A 輪和麥克納姆B 輪, 為了更清晰的說明A 輪和B 輪的區(qū)別, 圖3 分別展示了麥克納姆A 輪和麥克納姆B 輪的外形結(jié)構(gòu)。

如圖3 所示,A、B 輪的區(qū)別在于輪子外圍輥?zhàn)优c輪子中軸線夾角的方向不同。麥克納姆輪的外形結(jié)構(gòu)主要是由輪子和輪子上的與輪子中軸線夾角為45°的輥?zhàn)咏M成, 所有輥?zhàn)拥耐廨喞€連接正好是一個(gè)圓, 以此來保證麥克納姆輪時(shí)刻著地[4],因?yàn)锳、B 輪的機(jī)械結(jié)構(gòu)的不同, 朝同一方向轉(zhuǎn)動(dòng)時(shí), 會(huì)產(chǎn)生力的方向也不同。

圖3 麥克納姆輪外形結(jié)構(gòu)

接下來分別對(duì)麥克納姆A、B 輪進(jìn)行力學(xué)分析。首先是麥克納姆A 輪的正轉(zhuǎn)和反轉(zhuǎn)的力學(xué)分析圖如圖4 所示。由圖4 可知, 當(dāng)麥克納姆A輪正轉(zhuǎn)時(shí), 輪子外周與地面輥?zhàn)訒?huì)向前旋轉(zhuǎn), 因?yàn)檩佔(zhàn)邮桥c輪子中軸線呈45°夾角, 所以產(chǎn)生的是一個(gè)垂直于輥?zhàn)酉蚯暗牧? 與正前方向夾角45°, 因?yàn)榱κ且粋€(gè)矢量, 利用力的正交分解法, 將合力分解為X方向和Y方向兩個(gè)分力, 便于麥克納姆輪的受力情況分析, 受力分析如圖4 所示。同理可以得到麥克納姆麥克納姆B 輪正、反轉(zhuǎn)時(shí)的受力分析如圖5 所示。

圖4 麥克納姆A 輪正轉(zhuǎn)和反轉(zhuǎn)的力學(xué)分析

圖5 麥克納姆B 輪正轉(zhuǎn)和反轉(zhuǎn)的力學(xué)分析

麥克納姆輪構(gòu)成有多種組合, 下面分析能實(shí)現(xiàn)全向移動(dòng)的麥克納姆輪輪組合ABBA, 此種排列方式,4 個(gè)輪子的輥?zhàn)臃较虺尸F(xiàn)“X”型, 所以又稱“X”型排列[5]。

首先是移動(dòng)平臺(tái)的前向直行, 要實(shí)現(xiàn)移動(dòng)平臺(tái)前向直行, 必須使移動(dòng)平臺(tái)左右方向即X方向合外力相互抵消, 僅剩下前向的力[6]。4 個(gè)麥克納姆輪均向前轉(zhuǎn)動(dòng)時(shí)的受力示意圖如圖6 所示, 由圖6 可知, 此時(shí)左右方向, 即X方向的力均已經(jīng)兩兩抵消,4 個(gè)車輪都只剩下往前的分力, 這樣一來, 整個(gè)移動(dòng)平臺(tái)所受合外力向前。由此可得: 若要移動(dòng)平臺(tái)朝前直線移動(dòng), 只需要4 個(gè)輪子均往前轉(zhuǎn)動(dòng)即可實(shí)現(xiàn)[7]。另外, 由于麥克納姆輪正轉(zhuǎn)和反轉(zhuǎn)時(shí)的力是完全相反的, 所以可以推測(cè)出, 若要實(shí)現(xiàn)移動(dòng)平臺(tái)的后向直線移動(dòng), 只需要4 個(gè)輪子與前向移動(dòng)時(shí)的轉(zhuǎn)向完全相反, 即4 個(gè)輪子均往后轉(zhuǎn)動(dòng)即可[8]。

圖6 ABAB 組合下的移動(dòng)平臺(tái)前向直行受力分析

再次, 實(shí)現(xiàn)全向移動(dòng)還需要朝左前、右前、左后、右后45 度4個(gè)方向的移動(dòng), 在此選用一個(gè)右前方向?yàn)榇磉M(jìn)行分析, 受力分析如圖7 所示, 當(dāng)只有左前輪和右后輪正轉(zhuǎn)其他兩輪不轉(zhuǎn)動(dòng)時(shí), 移動(dòng)平臺(tái)所受合力朝右前方。移動(dòng)平臺(tái)往右前方45 度平移, 此時(shí)其他兩個(gè)不轉(zhuǎn)動(dòng)的輪子是不會(huì)跟地面產(chǎn)生巨大摩擦力從而導(dǎo)致不能按照正常的軌跡進(jìn)行移動(dòng)的, 因?yàn)辂溈思{姆輪的機(jī)械結(jié)構(gòu)是每個(gè)麥克納姆輪外周都有一圈輥?zhàn)? 輥?zhàn)邮强梢宰杂赊D(zhuǎn)動(dòng)的, 這樣兩個(gè)不轉(zhuǎn)動(dòng)的輪子跟地面之間的摩擦就是輥?zhàn)雍偷孛嬷g的滾動(dòng)摩擦, 相較于滑動(dòng)摩擦是小很多的, 所以不會(huì)出現(xiàn)不能按照正常理論預(yù)定軌跡運(yùn)動(dòng)的情況[9]。

圖7 ABAB 組合下的移動(dòng)平臺(tái)向右前方45 度平移受力分析

除此之外, 就是移動(dòng)平臺(tái)的原地旋轉(zhuǎn)。首先, 要實(shí)現(xiàn)移動(dòng)平臺(tái)的原地左向旋轉(zhuǎn), 只需要左邊前后輪向后轉(zhuǎn)動(dòng), 右邊前后輪都向前轉(zhuǎn)動(dòng)就可以實(shí)現(xiàn)[10]。同理, 要實(shí)現(xiàn)移動(dòng)平臺(tái)的原地右向旋轉(zhuǎn), 只需要右邊前后輪向后轉(zhuǎn)動(dòng), 左邊前后輪都向前轉(zhuǎn)動(dòng)就可以實(shí)現(xiàn)。

麥克納姆輪全向移動(dòng)平臺(tái)有10 個(gè)自由度, 移動(dòng)的受力情況和實(shí)現(xiàn)方法, 如表1 所示。由以上分析可知, 要想實(shí)現(xiàn)遙控循跡小車的10 個(gè)自由度的全向移動(dòng), 可以通過單片機(jī)控制車輪電機(jī)的旋轉(zhuǎn)方向來實(shí)現(xiàn)。

表1 麥克納姆輪移動(dòng)平臺(tái)不同移動(dòng)狀態(tài)對(duì)應(yīng)車輪轉(zhuǎn)向總結(jié)

2.3 BT04-A 藍(lán)牙模塊與主控的連接

小車與手機(jī)之間采用了BT04-A 藍(lán)牙通信模塊進(jìn)行通訊, 由于該模塊采用了藍(lán)牙3.0 的傳輸協(xié)議, 所以在傳輸距離和穩(wěn)定性上有很大的提升。該模塊有6 個(gè)引腳, 其中TXD 和RXD 是串口通信的引腳,TXD 用于發(fā)送數(shù)據(jù), 與單片機(jī)的串口二的接收引腳(RXD)相連接, 同樣,RXD 是控制數(shù)據(jù)接收的引腳與主控芯片的串口二的發(fā)送引腳(TXD)相連, 構(gòu)成完整的數(shù)據(jù)傳輸通路。電路連接原理圖如圖8所示。由圖8 可知BT04-A 的兩個(gè)信號(hào)引腳與主控芯片的PA3、PA2 相連,其中PA2 是主控芯片串口2 的TXD,PA3 是主控芯片串口2 的RXD。

圖8 藍(lán)牙模塊原理圖

2.4 BTN7971 驅(qū)動(dòng)電路的設(shè)計(jì)

BTN7971 驅(qū)動(dòng)電路由三部分組成, 一部分是以BTN7971 芯片為主體的驅(qū)動(dòng)電路主體, 第二部分是電源電路, 主要包括一些濾波電路和電壓轉(zhuǎn)換電路, 第三部分是光耦隔離電路, 它的主要作用是把來自主控芯片的控制信號(hào)與電源的供電線路隔離開, 因?yàn)殡娫垂╇婋娐肥?2 V 左右的較高電壓, 而控制信號(hào)是3.3 V, 所以需要隔離以防止主控芯片被燒壞。BTN7971 電機(jī)驅(qū)動(dòng)能力強(qiáng)、內(nèi)阻小、響應(yīng)快, 可以保證直流電機(jī)的驅(qū)動(dòng)力。在驅(qū)動(dòng)電路中, 選用HCPL-2630 芯片完成了光耦的隔離, 具體電路如圖9 所示。

圖9 BTN7971 驅(qū)動(dòng)電路主體原理圖

利用BTN7971 驅(qū)動(dòng)控制電機(jī), 可利用單片機(jī)控制一個(gè)腳輸出PWM信號(hào), 另一個(gè)腳輸出固定的高低電平, 通過控制PWM信號(hào)的高低電平時(shí)間來改變電機(jī)管腳的電平, 實(shí)現(xiàn)同速反轉(zhuǎn)。

2.5 紅外循跡模塊的電路圖設(shè)計(jì)

紅外循跡模塊的功能是能夠識(shí)別地面上預(yù)先設(shè)定好的黑色路線, 并將信號(hào)傳送至單片機(jī), 控制小車按照預(yù)設(shè)路徑, 自動(dòng)完成循跡動(dòng)作。采樣數(shù)字量輸出的模塊, 可以與單片機(jī)進(jìn)行通信。當(dāng)紅外對(duì)管檢測(cè)到黑色時(shí), 會(huì)通過OUT 輸出一個(gè)電平給單片機(jī), 實(shí)現(xiàn)循跡。紅外循跡模塊電路圖如10 所示。

2.6 編碼器電機(jī)電路原理圖

在系統(tǒng)中, 編碼器電機(jī)是構(gòu)成速度閉環(huán)的核心器件。它不僅要有足夠的轉(zhuǎn)動(dòng)扭矩, 以提供驅(qū)動(dòng)力,還要有速度反饋裝置, 以構(gòu)成速度閉環(huán), 為PID 算法提供反饋量。因此有2 種方案: 一是集成的編碼器直流減速電機(jī), 二是用普通的直流減速電機(jī),配合測(cè)速傳感器進(jìn)行使用, 但精確度和體積略比集成的編碼器電機(jī)減速差, 所以選用集成的編碼器電機(jī)。電機(jī)編碼器檢測(cè)車輪的轉(zhuǎn)速傳送給單片機(jī), 單片機(jī)對(duì)其進(jìn)行反饋控制, 精確控制車速, 并提高系統(tǒng)運(yùn)行穩(wěn)定性和小車行進(jìn)軌跡的精確度, 盡可能減小軌跡偏移。設(shè)計(jì)中所用到的編碼器是與動(dòng)力電機(jī)集成在一起的, 編碼器的原理圖如圖11 所示, 編碼器電機(jī)共有6 根管腳, 其中兩根用于驅(qū)動(dòng)電機(jī)正反轉(zhuǎn), 剩余4 個(gè)端口, 有兩個(gè)是編碼器供電端口, 剩余的兩個(gè)端口是編碼器的脈沖輸出端口, 輸出兩相相位相差90°的脈沖信號(hào), 用于解算電機(jī)轉(zhuǎn)速和轉(zhuǎn)向。

圖10 紅外循跡模塊電路圖

圖11 編碼器電機(jī)原理圖

2.7 姿態(tài)傳感器原理

姿態(tài)傳感器MPU6050 集成了3 軸陀螺儀,3 軸加速度計(jì), 以及一個(gè)可擴(kuò)展的數(shù)字運(yùn)動(dòng)處理器。為了實(shí)現(xiàn)更加精確地循跡, 以及定點(diǎn)啟停, 就必須采用MPU6050 作為小車的實(shí)時(shí)姿態(tài)反饋傳感器, 并在軟件中每10 ms 對(duì)該傳感器的數(shù)據(jù)進(jìn)行讀取和解算, 獲取小車姿態(tài)數(shù)據(jù)。

3 主程序流程

手機(jī)藍(lán)牙遙控APP, 通過藍(lán)牙數(shù)據(jù)傳輸協(xié)議, 將控制信號(hào)發(fā)送至BT04-A 藍(lán)牙模塊, 通過藍(lán)牙模塊轉(zhuǎn)化為串口信號(hào), 藍(lán)牙模塊的TXD、RXD 分別與主控芯片的USART2 的RXD、TXD 相連, 建立數(shù)據(jù)通道。主控芯片接收到來自手機(jī)藍(lán)牙遙控APP 的控制信號(hào), 控制移動(dòng)平臺(tái)的4 個(gè)電機(jī), 進(jìn)行相應(yīng)方向的轉(zhuǎn)動(dòng), 實(shí)現(xiàn)對(duì)應(yīng)方向移動(dòng)的效果。為消除因地面摩擦或其他一切未知因素的影響, 采用了電機(jī)尾部自帶的編碼器, 當(dāng)電機(jī)轉(zhuǎn)動(dòng)時(shí), 輸出兩相相位相差90°的脈沖信號(hào), 并發(fā)送至主控芯片的外部中斷引腳,讀取脈沖數(shù)量, 系統(tǒng)可以通過程序算法解算出電機(jī)的轉(zhuǎn)速和轉(zhuǎn)向, 作為反饋值構(gòu)成電機(jī)轉(zhuǎn)速閉環(huán), 再由PID 控制算法, 計(jì)算輸出PWM 信號(hào)的占空比, 輸出相應(yīng)占空比的PWM 信號(hào), 控制電機(jī)轉(zhuǎn)速, 達(dá)到精準(zhǔn)巡點(diǎn)控制的目的。

本設(shè)計(jì)主程序有兩種模式可以選擇, 普通遙控模式和自動(dòng)循跡模式, 兩種模式用一個(gè)轉(zhuǎn)換開關(guān)實(shí)現(xiàn)選擇, 在程序最開始會(huì)判斷工作模式, 模式的選擇由一個(gè)開關(guān)進(jìn)行控制, 在普通遙控模式狀態(tài)下, 系統(tǒng)一直等待來自串口2 的信號(hào), 系統(tǒng)對(duì)接收到的信號(hào)做出判斷, 做出相應(yīng)動(dòng)作。在自動(dòng)循跡模式, 系統(tǒng)會(huì)讀取前后左右4 個(gè)紅外尋跡模塊的信號(hào), 對(duì)軌跡做出判斷, 按照地面黑色軌跡進(jìn)行自動(dòng)循跡。主程序流程圖如圖12 所示。

圖12 主程序控制流程圖

3.1 藍(lán)牙接收程序

藍(lán)牙接收程序是本控制系統(tǒng)遙控模式下的重要組成部分, 用于接收遙控信號(hào), 實(shí)現(xiàn)過程為: 手機(jī)藍(lán)牙APP 進(jìn)行不同的操作時(shí), 將發(fā)送不同的內(nèi)容到藍(lán)牙模塊, 并通過藍(lán)牙協(xié)議傳輸至主控的串口2, 主控芯片開啟串口2 接收中斷, 在中斷內(nèi)接收解算內(nèi)容, 從而進(jìn)行不同的控制。

在本設(shè)計(jì)中, 藍(lán)牙APP 的內(nèi)部設(shè)定是, 進(jìn)行直行操作時(shí),APP 會(huì)發(fā)送字母A給藍(lán)牙模塊, 同理順時(shí)針旋轉(zhuǎn)8 個(gè)方向分別是A、B、C、D、E、F、G、H, 程序內(nèi)部和藍(lán)牙手機(jī)APP 統(tǒng)一協(xié)議即可實(shí)現(xiàn)控制,具體流程如圖13 所示。

圖13 藍(lán)牙控制流程圖

3.2 循跡姿態(tài)糾偏算法

大多數(shù)情況下對(duì)小車的循跡精度都有很高的要求, 此時(shí)僅僅依賴于一個(gè)紅外循跡這一數(shù)字量反饋模塊是遠(yuǎn)遠(yuǎn)不夠的, 要實(shí)現(xiàn)更加精確地循跡, 以及定點(diǎn)啟停, 就必須采用與其他精密的傳感器配合并采用相應(yīng)的軟件算法進(jìn)行處理, 本設(shè)計(jì)采用MPU6050 作為小車的實(shí)時(shí)姿態(tài)反饋傳感器, 該傳感器集成了高精度的陀螺儀、加速度計(jì)、地磁場(chǎng)傳感器。軟件中每10 ms 對(duì)該傳感器的數(shù)據(jù)進(jìn)行讀取和解算, 獲取小車姿態(tài)數(shù)據(jù), 這將產(chǎn)生大量的數(shù)據(jù)需要處理, 難免會(huì)有偏差較大的數(shù)據(jù)無法使用, 因此需要軟件算法相配合, 以最大限度的保證控制系統(tǒng)不產(chǎn)生較大誤差, 本設(shè)計(jì)采用中位值濾波和動(dòng)態(tài)卡爾曼濾波雙重保障, 使得控制精度大大提高。

中位值濾波算法, 是指對(duì)一變量進(jìn)行連續(xù)N次采樣, 并利用C 語言冒泡排序法從大到小排序, 取中位值作為有效數(shù)據(jù)。此方法能夠?yàn)V除一些過大過小無法使用的值, 是一種基本的濾波算法, 能夠大幅提高控制精度的是本設(shè)計(jì)的第二重保障, 即動(dòng)態(tài)卡爾曼濾波算法。

卡爾曼濾波實(shí)際上是基于對(duì)控制系統(tǒng)的輸入輸出狀態(tài)的觀測(cè), 結(jié)合不同噪聲源的數(shù)據(jù)進(jìn)行的一種最優(yōu)估計(jì)。

在本系統(tǒng)中卡爾曼濾波的主要作用是通過對(duì)加速度的一次積分和二次積分得到速度和位置, 通過卡爾曼濾波進(jìn)行互補(bǔ), 在不同干擾下做出最有估計(jì), 具體運(yùn)算過程如下:

設(shè)X為一四維數(shù)組, 為系統(tǒng)輸入狀態(tài)向量, 其中包含的4 個(gè)狀態(tài)變量分別為4 個(gè)輪子對(duì)應(yīng)的PWM信號(hào)占空比,Y為該系統(tǒng)的輸出狀態(tài)向量, 內(nèi)包含六軸傳感器觀測(cè)到的數(shù)據(jù)、加速度等, 和加速度的一次二次積分得到的速度和位置信息, 構(gòu)建本控制系統(tǒng)的狀態(tài)空間表達(dá)式為

根據(jù)卡爾曼濾波算法, 構(gòu)建控制系統(tǒng)結(jié)構(gòu)框如圖14 所示。由圖14可知, 卡爾曼濾波器實(shí)質(zhì)上在控制領(lǐng)域是一個(gè)狀態(tài)觀測(cè)器,u為系統(tǒng)的輸入信號(hào),e為觀測(cè)器的輸出和原系統(tǒng)輸出之間的差。根據(jù)圖14 可以推導(dǎo)出卡爾曼濾波器的數(shù)學(xué)表達(dá)式為

圖14 卡爾曼濾波狀態(tài)空間框圖

其中黑色圖線是實(shí)際傳感器測(cè)量值, 紅色圖線為真實(shí)值,綠色圖線是卡爾曼濾波后的最優(yōu)估計(jì)值, 可以看出, 在添加了卡爾曼濾波算法后, 整個(gè)圖線更加趨于平滑, 使得控制效果更好。

圖16是加卡爾曼濾波器前后系統(tǒng)的誤差波形, 由圖16可知卡爾曼濾波器明顯的降低了控制系統(tǒng)誤差, 系統(tǒng)運(yùn)行更加穩(wěn)定。

圖15 卡爾曼濾波先后控制效果圖

圖16 卡爾曼濾波前后系統(tǒng)誤差波形

3.3 PID 算法實(shí)現(xiàn)轉(zhuǎn)速閉環(huán)的電機(jī)恒速控制程序

在本系統(tǒng)中要控制電機(jī)的精準(zhǔn)轉(zhuǎn)速, 只需使用增量式比例積分控制器(PI 控制器)就可以達(dá)到比較完美的效果, 所以本系統(tǒng)僅通過PI 控制器來控制電機(jī)轉(zhuǎn)速,PID 算法的具體公式為

可通過MATLAB仿真直觀看出PID算法在控制中的作用, MATLAB 程序見附錄, 仿真波形如圖17 所示。

圖17 PID 控制器輸出波形

4 系統(tǒng)調(diào)試

插上電源接口的“T”形插頭, 整個(gè)系統(tǒng)通電,BTN7971上電源指示燈亮, 主控板電源指示燈亮,OLED 開始顯示,藍(lán)牙模塊指示燈閃爍, 等待連接, 電路連接正常, 如圖18所示。

圖18 上電后的小車

打開手機(jī)藍(lán)牙遙控APP 連接系統(tǒng)藍(lán)牙模塊BT04-A, 如圖19 所示為藍(lán)牙連接成功界面, 連接成功后即可進(jìn)行遙控操作, 可以使用搖桿操作。

圖19 藍(lán)牙連接成功界面圖及遙控界面圖

循跡模式的調(diào)試: 轉(zhuǎn)換開關(guān)撥至循跡模式, 將小車放置至軌道上, 軌道如圖20, 然后開啟電機(jī)使能, 小車即可按照軌跡運(yùn)動(dòng), 并停在固定點(diǎn)位置。如圖20 是本設(shè)計(jì)的循跡過程, 圖片左方為小車的前方, 安裝于小車前方的黑色突出部分為三路紅外循跡模組, 能夠識(shí)別軌道顏色, 前進(jìn)的同時(shí),不斷根據(jù)紅外循跡模塊輸出的反饋信號(hào)調(diào)整姿態(tài), 精準(zhǔn)循跡巡點(diǎn)。

圖20 移動(dòng)機(jī)器人循跡過程

5 結(jié)論

新型巡跡小車控制簡(jiǎn)單, 穩(wěn)定性強(qiáng)。針對(duì)目前移動(dòng)小車移動(dòng)轉(zhuǎn)向空間需求大的問題, 系統(tǒng)采用了麥克納姆輪, 并利用編碼器及姿態(tài)傳感器實(shí)時(shí)采集車輪的速度及小車姿態(tài), 實(shí)現(xiàn)了對(duì)小車多方位的閉環(huán)控制。為了滿足巡點(diǎn)要求,采用了紅外模組進(jìn)行識(shí)別, 通過對(duì)黑度的辨別輸出數(shù)字量, 完成了小車的巡點(diǎn)功能。對(duì)于在調(diào)試過程中, 出現(xiàn)的巡點(diǎn)稍有誤差的問題, 利用了多種循跡算法, 包含中位值濾波算法通過大量的處理數(shù)據(jù), 濾除一些過大過小和無法使用的值, 從而大幅增加控制精度。而卡爾曼濾波, 則是通過4 個(gè)輪子對(duì)應(yīng)4 個(gè)狀態(tài)變量的PWM 信號(hào)占空比、六軸傳感器觀測(cè)到的數(shù)據(jù)及加速度來通過算法進(jìn)行不斷調(diào)整控制速度及姿態(tài), 最終使得巡跡小車精準(zhǔn)到達(dá)指定位置。系統(tǒng)速度及姿態(tài)都采用了閉環(huán)的PID 控制, 保證了小車運(yùn)行的穩(wěn)定型。新型巡跡小車具有很強(qiáng)的靈活性和機(jī)動(dòng)性、操作方便, 是一種非常值得推廣的全向移動(dòng)小車。