實驗教學(xué)機(jī)器人的無線傳感網(wǎng)絡(luò)軟件設(shè)計

陳一銘，吳耀光，陳科全，郭小龍，張鐵異

（廣西大學(xué) 機(jī)械工程學(xué)院，廣西 南寧 530000）

0 引 言

很多高校都開設(shè)了有關(guān)機(jī)器人的課程，但在使用實驗教學(xué)機(jī)器人過程中，還缺少對機(jī)器人狀態(tài)的實時感知[1]。本文在六自由度教學(xué)機(jī)器人實驗平臺上，搭建ZigBee無線傳感網(wǎng)絡(luò)，實時監(jiān)測機(jī)器人運動狀態(tài)[2]，對提高高校實驗教學(xué)機(jī)器人教學(xué)效果以及教學(xué)管理效率具有一定意義。

1 總體方案設(shè)計

本文的實驗平臺采用具有6個轉(zhuǎn)動關(guān)節(jié)的教學(xué)機(jī)器人來進(jìn)行實驗。通過多種傳感器的應(yīng)用，實時監(jiān)測機(jī)器人在運動中的角速度、加速度、關(guān)節(jié)姿態(tài)、電機(jī)溫度等重要參數(shù)。在實驗教學(xué)過程中，能夠讓學(xué)生直觀了解機(jī)器人運動狀態(tài)的變化，通過ZigBee無線傳感網(wǎng)絡(luò)系統(tǒng)在實驗教室之間形成局域網(wǎng)[3]，并對范圍內(nèi)的多臺實驗教學(xué)機(jī)器人進(jìn)行實時狀態(tài)監(jiān)測。

2 無線傳感網(wǎng)絡(luò)軟件設(shè)計

無線傳感網(wǎng)絡(luò)軟件設(shè)計主要包括協(xié)調(diào)器節(jié)點程序設(shè)計、路由器節(jié)點程序設(shè)計、終端節(jié)點程序設(shè)計、機(jī)器人運動狀態(tài)采集節(jié)點設(shè)計和驅(qū)動電機(jī)溫度節(jié)點程序設(shè)計。

2.1 Z-Stack協(xié)議棧

本文在IAR Embedded Workbench for 8051開發(fā)環(huán)境下進(jìn)行，開發(fā)基礎(chǔ)是德州儀器公司推出的符合ZigBee 2007/Pro協(xié)議規(guī)范的Z-Stack協(xié)議棧，并根據(jù)課題需求對其進(jìn)行程序設(shè)計、編譯，通過開發(fā)底板的JTAG接口下載至無線核心模塊中，最終搭建起能夠感知網(wǎng)絡(luò)內(nèi)機(jī)器人運動狀態(tài)的ZigBee無線傳感網(wǎng)絡(luò)。

Z-Stack是采用ZigBee 2007/Pro協(xié)議規(guī)范的協(xié)議棧，它是一種半開源的協(xié)議棧[4]。Z-Stack協(xié)議棧與ZigBee協(xié)議模型之間有相對應(yīng)代碼，大大降低了協(xié)議棧難度，為開發(fā)工作帶來極大的方便[5]。

協(xié)議棧工作機(jī)制是基于OSAL操作系統(tǒng)。該操作系統(tǒng)通過把每個協(xié)議層都定義成不同任務(wù)，每個任務(wù)下面由許多事件組成[6]。任務(wù)的執(zhí)行采用輪詢機(jī)制，即不同任務(wù)有不同優(yōu)先級別。

2.2 協(xié)調(diào)器節(jié)點程序設(shè)計

協(xié)調(diào)器接收路由器轉(zhuǎn)發(fā)的電機(jī)溫度以及機(jī)器人運動狀態(tài)參數(shù)，進(jìn)行處理后經(jīng)過串口上傳至上位機(jī)。協(xié)調(diào)器處于整個無線傳感網(wǎng)絡(luò)的中心位置，是無線傳感網(wǎng)絡(luò)中最關(guān)鍵的節(jié)點。整個傳感網(wǎng)絡(luò)只允許一個協(xié)調(diào)器存在，負(fù)責(zé)創(chuàng)建網(wǎng)絡(luò)ID、各個節(jié)點地址分配、維護(hù)以及管理工作，并能夠通過串口將數(shù)據(jù)發(fā)送到網(wǎng)關(guān)或者上位機(jī)。

設(shè)備上電后，首先進(jìn)行硬件初始化，接著進(jìn)行網(wǎng)絡(luò)初始化，步驟如下：

（1）建立協(xié)調(diào)器，通過射頻前端發(fā)送信標(biāo)請求幀（Beacon Request Command），接著設(shè)置掃描期限（T_scan_duration），在掃描期限內(nèi)無信標(biāo)回應(yīng)時，則確定無沖突網(wǎng)絡(luò)存在，并創(chuàng)建ZigBee網(wǎng)絡(luò)，之后協(xié)調(diào)器節(jié)點不斷產(chǎn)生信標(biāo)幀在網(wǎng)絡(luò)半徑內(nèi)廣播，告知網(wǎng)絡(luò)存在。

（2）掃描信道，對設(shè)定的信道進(jìn)行能量檢測，對可用信道進(jìn)行標(biāo)注，接著網(wǎng)絡(luò)層發(fā)送MLME-SCAN.Request進(jìn)行主動掃描，搜索網(wǎng)絡(luò)半徑內(nèi)的信標(biāo)幀，通過搜索到的信息確定其他網(wǎng)絡(luò)ZigBee設(shè)備使用信道情況，最后根據(jù)收到的信息來確定最合適而且無沖突信道，在主動掃描期間，MAC層只處理物理層數(shù)據(jù)服務(wù)接收到的信標(biāo)幀。

（3）設(shè)置個域網(wǎng)地址（PAN ID），其是一個16位的無線網(wǎng)絡(luò)標(biāo)志符。通過協(xié)議棧配置文件f8wConfg.cfg中的ZDO_CONFIG_PAN_ID函數(shù)，可以設(shè)置PANID為0～0x3FFF之間的一個值。一個無線傳感網(wǎng)絡(luò)只允許一個PAN ID存在，而且它在所處的信道中地址ID是唯一的。此外還確定協(xié)調(diào)器短地址，短地址是網(wǎng)絡(luò)中設(shè)備的標(biāo)識號。默認(rèn)協(xié)調(diào)器短地址為0x0000。

以上3個步驟完成后，網(wǎng)絡(luò)初始化完成，進(jìn)入監(jiān)控模式，等待其他節(jié)點加入。

2.3 路由器節(jié)點程序設(shè)計

路由器在無線傳感網(wǎng)絡(luò)的功能是拓展網(wǎng)絡(luò)半徑，幫助網(wǎng)外設(shè)備加入網(wǎng)絡(luò)，并轉(zhuǎn)發(fā)其他節(jié)點數(shù)據(jù)。在本文中路由節(jié)點負(fù)責(zé)轉(zhuǎn)發(fā)終端節(jié)點監(jiān)測到的電機(jī)溫度還有機(jī)器人運動狀態(tài)參數(shù)。在定義為路由器的設(shè)備上電后，設(shè)備會運行應(yīng)用層函數(shù)GenericApp_Init（）進(jìn)行軟、硬件的初始化，接著進(jìn)行節(jié)點加入網(wǎng)絡(luò)初始化，一共有4個步驟。

（1）掃描指定信道，在該信道里發(fā)送請求命令幀（Beacon Request）。如果射頻范圍內(nèi)存在協(xié)調(diào)器，在收到請求命令幀之后，回復(fù)給路由器信標(biāo)幀，信標(biāo)幀里包含了協(xié)調(diào)器當(dāng)前網(wǎng)絡(luò)的狀態(tài)信息。

（2）路由器比較收到的各個協(xié)調(diào)器信標(biāo)幀信息，根據(jù)比較結(jié)果，選定最合適的網(wǎng)絡(luò)協(xié)調(diào)器作為父節(jié)點，發(fā)出關(guān)聯(lián)請求命令幀（Associate Request Command），協(xié)調(diào)器收到關(guān)聯(lián)請求幀后MAC層根據(jù)當(dāng)前擁有的資源（存儲及能量）判斷是否允許路由器節(jié)點加入，如果允許加入，則回復(fù)節(jié)點ACK。

（3）節(jié)點收到協(xié)調(diào)器的ACK后，等待協(xié)調(diào)器連接響應(yīng)，之后協(xié)調(diào)器會給節(jié)點分配一個16位的網(wǎng)絡(luò)短地址（Short Addr），用于網(wǎng)絡(luò)內(nèi)節(jié)點的識別。

（4）節(jié)點收到網(wǎng)絡(luò)短地址后回復(fù)給協(xié)調(diào)器ACK，節(jié)點加入網(wǎng)絡(luò)成功。建立成功后該節(jié)點以及協(xié)調(diào)器會同時在全網(wǎng)范圍內(nèi)廣播該節(jié)點入網(wǎng)成功信息，并在廣播數(shù)據(jù)幀（Broadcast）里攜帶有該入網(wǎng)節(jié)點的網(wǎng)絡(luò)短地址。

2.4 終端節(jié)點程序設(shè)計

終端節(jié)點也就是無線傳感網(wǎng)絡(luò)的感知節(jié)點，主要任務(wù)是負(fù)責(zé)采集各個傳感器的信息和終端數(shù)據(jù)的發(fā)送。

在搭建的整個無線傳感網(wǎng)絡(luò)系統(tǒng)中要實現(xiàn)的功能：一是采集傳感器的各項數(shù)據(jù)；二是加入無線傳感網(wǎng)絡(luò)并定時向父節(jié)點發(fā)送數(shù)據(jù)。

終端節(jié)點入網(wǎng)流程與路由器入網(wǎng)流程一致，而傳感器數(shù)據(jù)采集功能則根據(jù)不同的傳感器軟件設(shè)計。這里進(jìn)行采集節(jié)點的發(fā)送數(shù)據(jù)功能設(shè)計，終端節(jié)點工作流程如圖1所示。

終端節(jié)點加入網(wǎng)絡(luò)后，協(xié)議棧進(jìn)入OSAL的輪詢工作機(jī)制，這里定義SAMPLEAPP_SEND_PERI-ODIC_MSG_EVT事件函數(shù)來執(zhí)行定期發(fā)送數(shù)據(jù)返回父節(jié)點，如果deviceID=1，即判斷這個是MPU6050節(jié)點，執(zhí)行SampleApp_Send_MPU6050_Message（）函數(shù)，并定義osal_start_timerEx來進(jìn)行定時，如果是DS18B20溫度節(jié)點，則執(zhí)行SampleApp_Send_Temp_Message（）函數(shù)來實現(xiàn)往父節(jié)點發(fā)送數(shù)據(jù)功能，每1 000 ms往父節(jié)點發(fā)送一次傳感器數(shù)據(jù)。數(shù)據(jù)經(jīng)應(yīng)用層事件處理后，往下一層執(zhí)行，最后經(jīng)物理層處理成比特流后添加載波送至射頻前端，發(fā)送至父節(jié)點。

圖1 終端節(jié)點工作流程

2.5 機(jī)器人運動狀態(tài)采集節(jié)點設(shè)計

該節(jié)點程序設(shè)計主要有4個工作流程，首先是通過加速度計以及陀螺儀獲取節(jié)點的加速度、角速度；其次是傳感器自帶運動處理器DMP進(jìn)行原始數(shù)據(jù)融合處理，輸出表示姿態(tài)的四元數(shù)；接著在CC2530無線核心模塊進(jìn)行反三角運算，得出歐拉角；最后節(jié)點對原始數(shù)據(jù)以及歐拉角一共9個數(shù)據(jù)進(jìn)行打包，發(fā)送至父節(jié)點。

2.5.1 加速度、角速度獲取

針對于原始加速度、角速度的數(shù)據(jù)獲取，主要是在CC2530上電后，定義MPU_Init（void）函數(shù)對傳感器進(jìn)行初始化設(shè)置，包括初始化I2C總線、復(fù)位傳感器、選擇相應(yīng)量程等基本操作[8]。這里選擇了加速度計±2g量程，由對應(yīng)數(shù)據(jù)手冊知靈敏度為16 384 LSB/g，陀螺儀角速度量程選擇為 ±2 000(°)/s，其靈敏度是 16.4 LSB ·(°)-1·s-1。ax，ay，az分別對應(yīng) x，y，z三軸加速度，gx，gy，gz分別對應(yīng)繞 x，y，z三軸角速度。

2.5.2 運動姿態(tài)解算

在3D表示姿態(tài)的圖形學(xué)中，處理坐標(biāo)系旋轉(zhuǎn)方法有兩種：方向余弦法和四元數(shù)法[9]。用方向余弦法計算姿態(tài)矩陣時，雖然可以實現(xiàn)全姿態(tài)工作，但是運算量巨大，需要求解多個微分方程，不適合在能力有限的系統(tǒng)運行；而四元數(shù)姿態(tài)運算獲取旋轉(zhuǎn)軸及旋轉(zhuǎn)角度，計算量比前者小很多，故后者適合在運算能力有限的處理器上運行。

MPU6050芯片內(nèi)置有獨立的數(shù)字運動處理器（DMP），陀螺儀的三軸角速度、加速度計的三軸加速度數(shù)據(jù)經(jīng)DMP自身運動處理算法處理后生成表示姿態(tài)的角度值，輸出格式為q30的四元數(shù)，但四元數(shù)只在運算上有優(yōu)勢，無法直觀表示物體的運動姿態(tài)[10]。

為了直觀表達(dá)姿態(tài)角度值，需要在CC2530無核心模塊上將四元數(shù)換算成歐拉角。

四元數(shù)q表示為：

旋轉(zhuǎn)的四元數(shù)為單位四元數(shù)且模為1，可表示為：

“四元數(shù)表示”可轉(zhuǎn)為“歐拉角表示”，如下：

式中：α為z軸的偏航角；ψ為y軸的俯仰角；θ為x軸的滾轉(zhuǎn)角。

2.6 驅(qū)動電機(jī)溫度節(jié)點程序設(shè)計

在本設(shè)計中，電機(jī)溫度采集節(jié)點有兩個，分別檢測大臂步進(jìn)電機(jī)以及中臂步進(jìn)電機(jī)的溫度。這里只就一個節(jié)點設(shè)計進(jìn)行論述。

終端設(shè)備上電后，通過函數(shù)初始化DS18B20溫度傳感器，完成初始化輸出，之后通過函數(shù)unsigned char ReadDS18B20（void）讀取傳感器采集到的溫度數(shù)據(jù)，先讀低8位溫度數(shù)據(jù)，后讀高8位溫度數(shù)據(jù)，經(jīng)過轉(zhuǎn)換成溫度數(shù)值后，由協(xié)議棧SampleApp Send_Temp_Message（）函數(shù)進(jìn)行數(shù)據(jù)打包處理，最后將溫度信息送至射頻前端，發(fā)送至路由器。打包時，除了包含該節(jié)點溫度值數(shù)據(jù)外，還有加入的節(jié)點device ID，方便區(qū)分不同節(jié)點的溫度。傳感器軟件工作流程如圖2所示。

圖2 溫度傳感器軟件工作流程

3 結(jié) 語

本文以六自由度機(jī)器人為載體，通過運用多種傳感器，設(shè)計實驗機(jī)器人無線傳感系統(tǒng)的軟件部分。通過采集機(jī)器人的實時狀態(tài)，感知機(jī)器人運動狀態(tài)，對信息進(jìn)行無線傳輸，為以后研究打下了良好的基礎(chǔ)。