曹 俊,陳勁杰

(上海理工大學(xué)機(jī)械工程學(xué)院,上海 200082)

0 引言(Introduction)

隨著控制技術(shù)和智能制造的快速發(fā)展,越來(lái)越多的智能移動(dòng)裝備應(yīng)用于工業(yè)生產(chǎn)和人們的日常生活中。四輪移動(dòng)機(jī)器人作為智能化裝備的一部分,對(duì)工業(yè)和農(nóng)業(yè)的改革與發(fā)展有著不可代替的作用。許多行業(yè)都需要移動(dòng)機(jī)器人協(xié)助或者代替人類完成工作,這樣可降低工作人員的勞動(dòng)強(qiáng)度,提高勞動(dòng)效率[1]。例如,工業(yè)園區(qū)的貨物運(yùn)送、快遞和外賣的配送以及一些危險(xiǎn)的勘探任務(wù)等。因此,對(duì)于移動(dòng)機(jī)器人控制系統(tǒng)的研究符合未來(lái)發(fā)展方向并能夠加快智能移動(dòng)裝備的發(fā)展進(jìn)程[2]。當(dāng)前的一些移動(dòng)機(jī)器人環(huán)境適應(yīng)能力差、實(shí)時(shí)性差且功能擴(kuò)展性較差,難以普及。針對(duì)上述背景,本文采用FreeRTOS實(shí)時(shí)操作系統(tǒng)和STM32硬件平臺(tái)設(shè)計(jì)了一種低成本和高實(shí)時(shí)性、可靠性的小型四輪移動(dòng)機(jī)器人控制系統(tǒng)。

1 系統(tǒng)設(shè)計(jì)方案(Systemdesign scheme)

本文基于嵌入式實(shí)時(shí)操作系統(tǒng),設(shè)計(jì)了四輪移動(dòng)機(jī)器人小車(以下簡(jiǎn)稱四輪移動(dòng)小車)的下位機(jī)控制系統(tǒng)。四輪移動(dòng)小車底層控制系統(tǒng)以STM32F4作為核心處理器,使用MPU6050傳感器獲取四輪移動(dòng)小車的位姿,上傳至上位機(jī)主控制器進(jìn)行數(shù)據(jù)融合,以獲得更加精準(zhǔn)的位姿數(shù)據(jù),使用BQ7692003PWR芯片模塊獲取磷酸鐵鋰電池的電壓數(shù)據(jù),并使用開(kāi)路電壓法和安時(shí)積分法計(jì)算電池的SOC(剩余電量),使用PI算法控制四輪移動(dòng)小車的速度。通過(guò)整套設(shè)計(jì)可以實(shí)現(xiàn)四輪移動(dòng)機(jī)器人控制的實(shí)時(shí)性和穩(wěn)定性。系統(tǒng)總體框圖如圖1所示。

圖1 系統(tǒng)總體架構(gòu)圖Fig.1 Overall system architecture diagram

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

四輪移動(dòng)小車下位機(jī)控制系統(tǒng)采用STM32F405RGT6主控芯片,通過(guò)串口USART3完成與上位機(jī)主控制器的通信,并傳輸下位機(jī)的相關(guān)數(shù)據(jù)[3];通過(guò)I2C雙向總線獲取MPU6050傳感器的數(shù)據(jù),將位姿傳感器獲得的數(shù)據(jù)傳給主控制器;通過(guò)I2C接口獲取BQ7692003PWR芯片模塊傳輸?shù)碾娦倦妷簲?shù)據(jù)對(duì)電池系統(tǒng)進(jìn)行管理;使用定時(shí)器產(chǎn)生PWM(脈沖寬度調(diào)制),通過(guò)電機(jī)驅(qū)動(dòng)模塊對(duì)4個(gè)直流電機(jī)進(jìn)行控制,電機(jī)通過(guò)光電編碼器反饋電機(jī)的實(shí)時(shí)速度[4]。這4個(gè)子系統(tǒng)共同完成對(duì)四輪移動(dòng)小車系統(tǒng)的控制,系統(tǒng)硬件設(shè)計(jì)架構(gòu)如圖2所示。

圖2 系統(tǒng)硬件設(shè)計(jì)架構(gòu)Fig.2 System hardware design architecture

2.1 MPU6050模塊

MPU6050陀螺儀模塊是四輪移動(dòng)小車的位姿檢測(cè)模塊,是一種六軸傳感器模塊,可以同時(shí)檢測(cè)三軸加速度、三軸陀螺儀的運(yùn)動(dòng)數(shù)據(jù)及溫度數(shù)據(jù)。六軸傳感器模塊內(nèi)置數(shù)字運(yùn)動(dòng)處理器,可對(duì)數(shù)據(jù)進(jìn)行濾波、融合處理。該模塊通過(guò)模擬I2C接口向下位機(jī)主控制器輸出運(yùn)動(dòng)數(shù)據(jù)[5]。

2.2 電源模塊

四輪移動(dòng)小車的電源模塊使用12 V 的鋰電池,鋰電池具有重量輕、容量大和壽命較長(zhǎng)等優(yōu)點(diǎn)。鋰電池使用18650鋰電池,通過(guò)串、并聯(lián)方式連接后容量可達(dá)13 600 mAh,可以滿足小型移動(dòng)機(jī)器人的電壓和電流需求,并且此類電池的質(zhì)量較輕,不影響機(jī)器人的移動(dòng)性能,價(jià)格較其他電池實(shí)惠,適用于小型的移動(dòng)平臺(tái)。

2.3 電池管理模塊

使用BQ7692003PWR芯片模塊對(duì)四輪移動(dòng)小車進(jìn)行電池管理,鋰電池采用串、并聯(lián)方式連接。電池分為4組,每組由3節(jié)18650鋰電池串聯(lián)而成。使用BQ7692003PWR芯片模塊對(duì)一組3節(jié)電池進(jìn)行電芯電壓數(shù)據(jù)的采集,再通過(guò)開(kāi)路電壓法和安時(shí)積分法對(duì)鋰電池的剩余電量進(jìn)行實(shí)時(shí)的估算。通過(guò)I2C接口向STM32主控芯片傳輸電壓數(shù)據(jù)。

2.4 電機(jī)驅(qū)動(dòng)模塊

四輪移動(dòng)小車采用的電機(jī)是12 V直流減速電機(jī)。電機(jī)尾部帶光電編碼器,可實(shí)時(shí)反饋四輪移動(dòng)小車電機(jī)的轉(zhuǎn)速。電機(jī)驅(qū)動(dòng)模塊采用WSDC2412D,是直流電機(jī)驅(qū)動(dòng)集成電路,采用標(biāo)準(zhǔn)邏輯電平信號(hào)控制;使用高性能MOSFET(金氧半場(chǎng)效晶體管)組成“H”橋驅(qū)動(dòng),可驅(qū)動(dòng)最高300 W 電功率的直流電機(jī);允許PWM 輸入的最小脈寬低至3μs,可保證PWM 的動(dòng)態(tài)調(diào)節(jié)范圍。

3 系統(tǒng)軟件設(shè)計(jì)(Systemsoft ware design)

3.1 基于FreeRTOS的軟件系統(tǒng)設(shè)計(jì)

FreeRTOS操作系統(tǒng)是一款輕量級(jí)的實(shí)時(shí)操作系統(tǒng),可移植在嵌入式設(shè)備上,它可免費(fèi)用于商用。FreeRTOS操作系統(tǒng)提供任務(wù)管理、時(shí)間管理、信號(hào)量和消息隊(duì)列等功能,完全可滿足移動(dòng)機(jī)器人的系統(tǒng)功能需求[6]。

四輪移動(dòng)機(jī)器人軟件控制系統(tǒng)基于FreeRTOS實(shí)時(shí)操作系統(tǒng)開(kāi)發(fā),軟件系統(tǒng)架構(gòu)如圖3所示。需要建立6個(gè)主應(yīng)用任務(wù):串口通信任務(wù)、位姿檢測(cè)任務(wù)、電機(jī)控制任務(wù)、電池狀態(tài)監(jiān)測(cè)任務(wù)、電池狀態(tài)分析任務(wù)及信息打印任務(wù),使用FreeRTOS操作系統(tǒng)對(duì)多個(gè)任務(wù)進(jìn)行協(xié)調(diào)管理,各任務(wù)的功能如下。

圖3 軟件系統(tǒng)架構(gòu)Fig.3 Soft ware system architecture

(1)串口通信任務(wù)。串口通信任務(wù)使用USART3和上位機(jī)主控芯片進(jìn)行通信。當(dāng)串口通信任務(wù)運(yùn)行時(shí),首先會(huì)對(duì)四輪移動(dòng)小車的x、y、z軸的目標(biāo)速度等信息進(jìn)行賦值,其次使用串口將數(shù)據(jù)發(fā)送到下位機(jī)主控制器。接收信息時(shí),會(huì)等待串口中斷回調(diào)函數(shù)中的消息通知,當(dāng)上位機(jī)發(fā)送命令時(shí)會(huì)觸發(fā)中斷接收函數(shù),下位機(jī)對(duì)數(shù)據(jù)進(jìn)行接收和解析。

(2)位姿檢測(cè)任務(wù)。在四輪移動(dòng)小車開(kāi)機(jī)前,位姿檢測(cè)模塊首先使用MPU6050模塊檢測(cè)讀取陀螺儀的零點(diǎn),行駛中得到加速度傳感器數(shù)據(jù)和陀螺儀數(shù)據(jù)。其次使用模擬I2C接口將數(shù)據(jù)傳輸?shù)较挛粰C(jī)主控制器。最后下層主控制器將數(shù)據(jù)發(fā)送到上位機(jī)主控制器進(jìn)行數(shù)據(jù)擬合。

(3)電機(jī)控制任務(wù)。電機(jī)控制任務(wù)首先讀取上位機(jī)傳來(lái)的x、y、z軸的目標(biāo)速度,根據(jù)編碼器讀取四輪移動(dòng)小車的實(shí)際速度。其次使用PI算法進(jìn)行調(diào)速,將得到的值賦值給PWM 寄存器對(duì)電機(jī)的轉(zhuǎn)速進(jìn)行調(diào)整,進(jìn)而提高四輪移動(dòng)小車運(yùn)動(dòng)控制的準(zhǔn)確性[7]。

(4)電池狀態(tài)監(jiān)測(cè)任務(wù)。電池狀態(tài)監(jiān)測(cè)任務(wù)是通過(guò)BQ7692003PWR芯片模塊對(duì)鋰電池的單體電芯電壓和電池組電壓進(jìn)行監(jiān)測(cè)。測(cè)得數(shù)據(jù)可用于電池狀態(tài)分析任務(wù)。

(5)電池狀態(tài)分析任務(wù)。電池狀態(tài)分析任務(wù)對(duì)BQ7692003PWR芯片得到的電壓數(shù)據(jù)進(jìn)行分析。首先進(jìn)行簡(jiǎn)單的數(shù)據(jù)分析,并通過(guò)簡(jiǎn)單的計(jì)算獲得最大電壓差、平均電壓和最大最小電壓。其次使用開(kāi)路電壓算法和安時(shí)積分算法估算鋰電池的剩余電量。

(6)信息打印任務(wù)。信息打印任務(wù)負(fù)責(zé)打印4個(gè)電機(jī)轉(zhuǎn)速、鋰電池的單體電壓和電池的SOC等信息。

在FreeRTOS實(shí)時(shí)操作系統(tǒng)中,任務(wù)和中斷都具有不同的優(yōu)先級(jí)。調(diào)度器總是在所有處于就緒態(tài)的任務(wù)中選擇具有最高優(yōu)先級(jí)的任務(wù)執(zhí)行。優(yōu)先級(jí)相同的任務(wù)在時(shí)鐘節(jié)拍中斷中進(jìn)行切換并執(zhí)行相同的時(shí)間片大小。當(dāng)任務(wù)優(yōu)先級(jí)足夠高,該任務(wù)就會(huì)立即得到響應(yīng),然后根據(jù)優(yōu)先級(jí)依次響應(yīng)。本設(shè)計(jì)中的電機(jī)控制任務(wù)和通信任務(wù)相對(duì)于電池管理任務(wù),優(yōu)先級(jí)更高。任務(wù)優(yōu)先級(jí)分配如表1所示。

表1 任務(wù)優(yōu)先級(jí)Tab.1 Task priority

3.2 電機(jī)PI控制

四輪移動(dòng)小車的電機(jī)控制采用PI(比例積分)控制器。P為比例環(huán)節(jié),可以反映偏差信號(hào),以減少偏差;I是積分環(huán)節(jié),可以消除系統(tǒng)靜差。比例積分控制器是根據(jù)給定值與實(shí)際輸出值構(gòu)成控制偏差,將偏差的比例(P)和積分(I)通過(guò)線性組合構(gòu)成控制量,對(duì)車速進(jìn)行控制[8]。PI位置式算法離散公式如公式(1)所示:

其中,U(t)是控制器輸出值;e k是本次控制器輸入與設(shè)定值之間的誤差;K P是比例系數(shù);K i是積分系數(shù)。

PI算法的核心代碼如下:

3.3 鋰電池的SOC算法

四輪移動(dòng)小車的SOC(State Of Charge),即電池剩余電量,采用的是開(kāi)路電壓法和安時(shí)積分法結(jié)合的方法對(duì)電量進(jìn)行估算。開(kāi)路電壓法是在四輪移動(dòng)小車長(zhǎng)時(shí)間待機(jī)靜止后以最低電芯電壓為標(biāo)準(zhǔn)校準(zhǔn)一次電容[7];安時(shí)積分法是電池充放電狀態(tài)下實(shí)時(shí)對(duì)電流進(jìn)行積分校準(zhǔn)電池的實(shí)際容量,然后計(jì)算SOC值。前者用于準(zhǔn)確地估計(jì)初始SOC值,后者用于估計(jì)實(shí)時(shí)SOC值。兩種方法結(jié)合可以更加精準(zhǔn)地估計(jì)電池的剩余電量。

開(kāi)路電壓法是測(cè)量鋰電池后得到一個(gè)具有100個(gè)元素的數(shù)組,然后根據(jù)數(shù)組查找最小電芯電壓在數(shù)組中位置對(duì)應(yīng)的SOC,從而得出當(dāng)前電池電量,經(jīng)過(guò)實(shí)驗(yàn)可得開(kāi)路電壓與SOC關(guān)系曲線和開(kāi)路電壓法流程圖,如圖4所示。

圖4 開(kāi)路電壓與SOC 的關(guān)系曲線圖和開(kāi)路電壓法流程圖Fig.4 Relation curve bet ween open circuit voltage and SOC and flow chart of open circuit voltage method

安時(shí)積分法,測(cè)量電池主回路的實(shí)時(shí)電流,將電流對(duì)時(shí)間積分,數(shù)值在放電時(shí)為正,充電時(shí)為負(fù)。放電過(guò)程中用初始電量減去積分結(jié)果,得到當(dāng)前電量;充電過(guò)程中用初始電量加上積分結(jié)果,得到當(dāng)前電量。如公式(2)所示,SOC0為鋰電池荷電狀態(tài)的初始值,SOCt為t時(shí)刻電池的剩余電量,Q N為電池的額定容量,I為t時(shí)刻的充放電負(fù)載電流[8]。

安時(shí)積分法的核心算法如下:

安時(shí)積分法算出的SOC為SOC1,開(kāi)路電壓法得出的SOC為SOC2。如果SOC2＞SOC1,則SOC=SOC1,取最小值可以使SOC更為準(zhǔn)確;如果SOC2＜SOC1,則SOC=SOC2,這樣可以修正放置一段時(shí)間后電池的SOC值;安時(shí)積分法和開(kāi)路電壓法相結(jié)合,能夠估算較為準(zhǔn)確的SOC。

4 系統(tǒng)實(shí)現(xiàn)(System implementation)

四輪移動(dòng)小車的硬件平臺(tái)如圖5所示,主要包括四輪移動(dòng)小車底盤、下位機(jī)主控制器STM32、電池管理模塊、位姿傳感器、電機(jī)驅(qū)動(dòng)器、電機(jī)、電源。

通過(guò)實(shí)驗(yàn)可以證明四輪移動(dòng)小車在行駛過(guò)程中有很好的實(shí)時(shí)性和動(dòng)態(tài)反應(yīng)能力,并且對(duì)地形有較好的適應(yīng)能力。信息打印任務(wù)監(jiān)控的功能如表2所示。

表2 監(jiān)控信息列表Tab.2 Monitoring information list

一組電池的測(cè)試數(shù)據(jù)如表3所示。電池的最大誤差為4 mV?？梢?jiàn),電池管理模塊可以很好地管理四輪移動(dòng)小車的電源,實(shí)現(xiàn)對(duì)電源剩余電量的監(jiān)測(cè)。

表3 測(cè)試數(shù)據(jù)Tab.3 test data

5 結(jié)論(Conclusion)

本文設(shè)計(jì)的四輪移動(dòng)機(jī)器人底層控制系統(tǒng),采用電機(jī)控制模塊、光電編碼器、位姿傳感器、電源管理模塊實(shí)現(xiàn)了四輪移動(dòng)機(jī)器人的快速轉(zhuǎn)向和加速能力,并且可實(shí)時(shí)監(jiān)控電源的剩余電量。四輪移動(dòng)機(jī)器人基于STM32平臺(tái)和FreeRTOS實(shí)時(shí)操作系統(tǒng),使用PI算法進(jìn)行電機(jī)調(diào)速保證電機(jī)的動(dòng)態(tài)響應(yīng),并使用開(kāi)路電壓法和安時(shí)積分法對(duì)電池的剩余電量進(jìn)行監(jiān)測(cè),提高了四輪移動(dòng)機(jī)器人的運(yùn)動(dòng)能力和環(huán)境適應(yīng)能力,應(yīng)用場(chǎng)景較為廣泛,具有較好的實(shí)際應(yīng)用價(jià)值。