基于Free RTOS的移動機器人控制系統(tǒng)設(shè)計

曹 俊,陳勁杰

(上海理工大學機械工程學院,上海 200082)

0 引言(Introduction)

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

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

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

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

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

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

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

2.1 MPU6050模塊

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

2.2 電源模塊

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

2.3 電池管理模塊

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

2.4 電機驅(qū)動模塊

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

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

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

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

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

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

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

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

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

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

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

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

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

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

3.2 電機PI控制

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

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

PI算法的核心代碼如下:

3.3 鋰電池的SOC算法

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

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

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

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

安時積分法的核心算法如下:

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

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

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

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

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

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

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

5 結(jié)論(Conclusion)

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