基于紅外定位的六自由度板球控制系統(tǒng)設(shè)計(jì)①

王正剛， 代廣珍

(安徽工程大學(xué)電氣工程學(xué)院，安徽 蕪湖 241000)

0 引 言

板球控制系統(tǒng)是一種多變量的非線性控制系統(tǒng)，是桿球控制系統(tǒng)的擴(kuò)展和延伸[1]。板球控制系統(tǒng)可以很直觀地驗(yàn)證控制算法的有效性，為實(shí)際工程上的控制問題提供解決方案。同時(shí)，研究板球控制算法可以提高實(shí)際系統(tǒng)的穩(wěn)定性。因此，設(shè)計(jì)一種合理有效的板球系統(tǒng)控制方法是非常有意義的[2]。

系統(tǒng)以2017年全國電子設(shè)計(jì)大賽B題為背景，設(shè)計(jì)并搭建了一個(gè)六自由度運(yùn)動(dòng)控制平臺(tái)，采用STM32F407單片機(jī)作為主控模塊，包含角度檢測模塊、直線電機(jī)模塊、紅外線定位裝置、電機(jī)驅(qū)動(dòng)和直流穩(wěn)壓電源模塊，通過采集獲取平臺(tái)的姿態(tài)參數(shù)并進(jìn)行計(jì)算調(diào)節(jié)，實(shí)現(xiàn)對(duì)小球軌跡的控制，通過控制平臺(tái)的傾斜，使直徑不大于25mm的小球能夠按要求在限定時(shí)間內(nèi)完成各種指定的運(yùn)動(dòng)軌跡。

1 方案的選擇

1.1 滾球控制方案

小球運(yùn)動(dòng)平臺(tái)的姿態(tài)調(diào)整可采用多自由度并聯(lián)控制，利用PID控制算法進(jìn)行運(yùn)算，并將運(yùn)算結(jié)果用于平臺(tái)控制。最簡單的控制方式是在x和y軸上對(duì)平臺(tái)進(jìn)行上下調(diào)整的二自由度控制。六自由度控制設(shè)計(jì)雖然復(fù)雜，但位置控制精度高，圖1為六自由度并聯(lián)運(yùn)動(dòng)平臺(tái)設(shè)計(jì)效果圖。六個(gè)動(dòng)力系統(tǒng)平均分布在平臺(tái)下方，與其他方案相比，六自由度并聯(lián)運(yùn)動(dòng)平臺(tái)具有剛度大、位置精度高和承載能力強(qiáng)等優(yōu)點(diǎn)，可實(shí)現(xiàn)對(duì)平臺(tái)的精確控制。

圖1 六自由度并聯(lián)控制運(yùn)動(dòng)平臺(tái)設(shè)計(jì)效果圖

圖2 鉸接點(diǎn)矢量圖

1.2 硬件設(shè)計(jì)方案

使用直線電機(jī)，結(jié)構(gòu)設(shè)計(jì)緊密，重量輕，用直流電機(jī)齒輪減速，內(nèi)置微動(dòng)開關(guān)，可實(shí)現(xiàn)行程走完自動(dòng)停止，安裝簡易，同時(shí)易于調(diào)節(jié)和控制。采用MPU6050傳感器。MPU6050模塊內(nèi)部自帶電壓穩(wěn)定電路，直接連接STM32F407總控制器系統(tǒng)。通過讀取MPU6050內(nèi)部運(yùn)動(dòng)處理器所計(jì)算出的結(jié)果，可解算得到姿態(tài)角，讀取簡單、響應(yīng)迅速，可有效減輕CPU負(fù)擔(dān)。通過設(shè)置紅外定位裝置，檢測小球的位置，反饋到控制器，計(jì)算控制量，通過控制直線電機(jī)以控制平臺(tái)的傾斜角度，進(jìn)而控制小球的運(yùn)動(dòng)軌跡。

圖3 系統(tǒng)總體框圖

圖4 MPU6050原理圖

2 運(yùn)動(dòng)平臺(tái)位置計(jì)算分析

已知運(yùn)動(dòng)平臺(tái)的姿態(tài)參數(shù)為q=(x,y,z,α,β,γ)，利用位置反解方法求解伸縮桿的桿長li。圖2為六自由度并聯(lián)運(yùn)動(dòng)平臺(tái)的鉸接點(diǎn)的矢量圖，如圖2所示A為固定平臺(tái)，B為運(yùn)動(dòng)平臺(tái)，A上對(duì)應(yīng)鉸接點(diǎn)Ai與B上對(duì)應(yīng)鉸接點(diǎn)Bi與之間的距離|AiBi|即為各個(gè)伸縮桿的桿長li。

T為旋轉(zhuǎn)變換矩陣，反映了固定坐標(biāo)系OXYZ和運(yùn)動(dòng)坐標(biāo)系ouvw之間的關(guān)系。p=(xpypzp)T是運(yùn)動(dòng)坐標(biāo)系原點(diǎn)o在固定坐標(biāo)系中的位置矢量ai=(xAi,yAizAi)T是Ai在固定坐標(biāo)系中的位置矢量；bi=(xBiyBizBi)T是Bi在運(yùn)動(dòng)坐標(biāo)系中的位置矢量。由圖2可知，伸縮桿的桿長矢量Li為：

ai-(T×bi+p)

(1)

因此，計(jì)算伸縮桿長度的公式為：

li=|Li|=ai-(p+T×bi)=

(2)

已知ai與bi，給定運(yùn)動(dòng)平臺(tái)B的位姿參數(shù)q=(x,y,z,α,β,γ)，通過位置反解，就可以確定向量p與矩陣T，再利用公式(2)就能算出伸縮桿長度li[3]。

3 系統(tǒng)設(shè)計(jì)

3.1 電路設(shè)計(jì)

3.1.1 系統(tǒng)總體框圖

圖3所示為系統(tǒng)總體框圖，主要由STM32F407單片機(jī)、MPU6050六軸傳感器模塊、直線電機(jī)控制模塊、紅外定位裝置和直流穩(wěn)壓電源模塊等部分組成[4]。

圖5 直線電機(jī)驅(qū)動(dòng)原理圖

圖6 紅外定位裝置示意圖

3.1.2 角度檢測模塊設(shè)計(jì)

角度檢測模塊采用MPU6050六軸傳感器設(shè)計(jì)，圖4所示為MPU6050設(shè)計(jì)原理圖。MPU6050內(nèi)部自帶三軸陀螺儀和三軸加速度傳感器，含有兩個(gè)I2C接口，主I2C接口結(jié)合自帶的數(shù)字運(yùn)動(dòng)處理器硬件加速引擎，可以向應(yīng)用端輸出完整的九軸姿態(tài)融合演算數(shù)據(jù)；從I2C接口可以和外部磁力傳感器連接。使用運(yùn)動(dòng)處理資料庫，可非常方便地實(shí)現(xiàn)姿態(tài)解算，降低主控模塊的運(yùn)算負(fù)荷，同時(shí)大大降低了開發(fā)難度。

3.1.3 直線電機(jī)驅(qū)動(dòng)設(shè)計(jì)

IR2104型半橋驅(qū)動(dòng)芯片可以驅(qū)動(dòng)高端和低端兩個(gè)N溝道MOSFET，能產(chǎn)生較大的柵極驅(qū)動(dòng)電流，并具有硬件死區(qū)、硬件防同臂導(dǎo)通等功能。使用兩片IR2104可以組成完整的直流電機(jī)H橋式驅(qū)動(dòng)電路。電機(jī)驅(qū)動(dòng)原理圖如圖5所示[5]。

圖7 程序流程圖

3.1.4 紅外定位裝置設(shè)計(jì)

圖6所示為紅外定位裝置示意圖。在平臺(tái)四周放置紅外線發(fā)射和接收陣列，系統(tǒng)逐個(gè)掃描光電管接收的信號(hào)。因?yàn)樾∏虻淖钄?，通過檢測未接收到信號(hào)的光電管便可以快速獲取橫向和縱向坐標(biāo)，從而確定小球在平臺(tái)上的精確位置，定位精度達(dá)到5.4mm[6]。

3.2 程序設(shè)計(jì)

3.2.1 程序流程圖

圖7所示為系統(tǒng)控制程序流程圖，包含主程序流程和中斷程序流程。主程序主要完成功能是對(duì)系統(tǒng)進(jìn)行初始化和延時(shí)UI(用戶界面)處理，包括OLED實(shí)時(shí)顯示小球位置區(qū)域及停留時(shí)間、按鍵控制等；中斷處理程序主要是對(duì)平臺(tái)姿態(tài)(加速度、角速度)、小球位置和速度、直線電機(jī)長度和速度等數(shù)據(jù)進(jìn)行獲取和處理，控制輸出PWM信號(hào)，以及對(duì)小球進(jìn)行定位時(shí)間檢測等。

3.2.2 程序功能分析

系統(tǒng)通過STM32F407單片機(jī)來控制驅(qū)動(dòng)電路，根據(jù)PID的調(diào)節(jié)來得到需要設(shè)定的角度，同時(shí)得到對(duì)應(yīng)控制脈沖，輸出PWM控制直線電機(jī)的推桿伸縮，使平臺(tái)按照指定角度傾斜，從而使小球按照指定動(dòng)作運(yùn)動(dòng)。其中按鍵開關(guān)進(jìn)行模式和區(qū)域設(shè)置。

4 測試方案及結(jié)果

4.1 測試設(shè)備及方式

所用測試設(shè)備有：小球、量角器、直尺、秒表等。

測試方法描述如下：

(1)定位測試。將小球放置在平臺(tái)上指定區(qū)域內(nèi)，保持一定時(shí)間靜止。

(2)定點(diǎn)測試。將小球放置在平臺(tái)上某區(qū)域內(nèi)，調(diào)節(jié)滾球控制系統(tǒng)使小球移動(dòng)到另一指定區(qū)域內(nèi)，并停留一段時(shí)間。

(3)定時(shí)測試。使小球在某一規(guī)定時(shí)間內(nèi)完成指定動(dòng)作。

4.2 測試數(shù)據(jù)

(1)驅(qū)動(dòng)滾球控制系統(tǒng)工作，使小球在規(guī)定區(qū)域內(nèi)停留，并記錄停留時(shí)間和偏差位置。測試結(jié)果如表1所示：

表1 區(qū)域定點(diǎn)停留測試數(shù)據(jù)

測試結(jié)果分析：定點(diǎn)停留測試能夠準(zhǔn)確完成，誤差極小。

(2)驅(qū)動(dòng)滾球裝置系統(tǒng)工作，使小球從一個(gè)區(qū)域滾動(dòng)到另一個(gè)指定區(qū)域，并停留一段時(shí)間。記錄運(yùn)動(dòng)時(shí)間和停留時(shí)間。測試數(shù)據(jù)如表2所示：

表2 運(yùn)動(dòng)和停留測試數(shù)據(jù)

測試結(jié)果分析：運(yùn)動(dòng)停留測試基本能夠準(zhǔn)確完成，但是在路線復(fù)雜情況下，運(yùn)動(dòng)時(shí)間較長。

(3) 規(guī)定時(shí)間內(nèi)，讓小球先后進(jìn)入?yún)^(qū)域 2、區(qū)域 6，停止于區(qū)域9，在區(qū)域9中停留時(shí)間不少于2s。記錄運(yùn)動(dòng)時(shí)間和停留時(shí)間，測試數(shù)據(jù)如表3：

表3 規(guī)定路線測試數(shù)據(jù)

測試結(jié)果分析：要通過的區(qū)域多，還要在指定區(qū)域停留，因此實(shí)現(xiàn)起來有一定難度，但是基本可以在指定時(shí)間內(nèi)完成要求。時(shí)間上還有待縮短。

(4)用按鍵設(shè)置A、B、C、D區(qū)域，并控制小球完成動(dòng)作。

表4 按鍵指定區(qū)域測試數(shù)據(jù)

測試結(jié)果分析：設(shè)置區(qū)域路線簡單，能夠較好完成，但是若為折線路線，則實(shí)現(xiàn)起來比較困難，在運(yùn)動(dòng)過程中，難以控制，耗時(shí)較長。

5 結(jié) 論

系統(tǒng)搭建了基于紅外定位的六自由度板球控制平臺(tái)，使用STM32F407單片機(jī)作為主控模塊。經(jīng)測試，控制精準(zhǔn)，操作簡單，性能可靠，技術(shù)指標(biāo)達(dá)到了設(shè)計(jì)要求。

在系統(tǒng)設(shè)計(jì)和搭建過程中，分工明確，充分發(fā)揮各自所長，放棄使用傳統(tǒng)的舵機(jī)作為執(zhí)行機(jī)構(gòu)，采用六自由度并聯(lián)控制運(yùn)動(dòng)平臺(tái)，雖然設(shè)計(jì)難度有所增加，但提高了控制精度，可實(shí)現(xiàn)對(duì)平臺(tái)的精確控制。

[1] 蘇信, 孫政順, 趙世敏.板球系統(tǒng)的模糊控制方法研究[J].計(jì)算機(jī)仿真. 2006(09).165-167.

[2] 翟晨汐,李洪興. 板球系統(tǒng)的直接自適應(yīng)模糊滑?？刂芠J].計(jì)算機(jī)仿真. 2016(02).383-388.

[3] 李沛,楊小強(qiáng),李煥良,等.基于Matlab/SimMechanics的六自由度并聯(lián)運(yùn)動(dòng)平臺(tái)建模與分析[J].機(jī)械與電子.2016(03).75-80.

[4] 王紅睿,田彥濤,隋振,等.板球系統(tǒng)的非線性自適應(yīng)控制[J].系統(tǒng)仿真學(xué)報(bào).2010(05).1251-1256.

[5] 王金晨,紀(jì)成,何印洲.基于PWM的可調(diào)速電動(dòng)汽車模型控制系統(tǒng)的設(shè)計(jì)[J].伺服控制.2011(08).42-45.

[6] 何成林,鐘鳴,姚玉峰. 基于紅外定位的微小型自重構(gòu)移動(dòng)機(jī)器人[J]. 機(jī)械與電子.2016(09).69-72.