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

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

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

0 引 言

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

1 總體方案設(shè)計(jì)

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

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

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

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è)計(jì)、編譯，通過開發(fā)底板的JTAG接口下載至無線核心模塊中，最終搭建起能夠感知網(wǎng)絡(luò)內(nèi)機(jī)器人運(yùn)動狀態(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)通過把每個(gè)協(xié)議層都定義成不同任務(wù)，每個(gè)任務(wù)下面由許多事件組成[6]。任務(wù)的執(zhí)行采用輪詢機(jī)制，即不同任務(wù)有不同優(yōu)先級別。

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

協(xié)調(diào)器接收路由器轉(zhuǎn)發(fā)的電機(jī)溫度以及機(jī)器人運(yùn)動狀態(tài)參數(shù)，進(jìn)行處理后經(jīng)過串口上傳至上位機(jī)。協(xié)調(diào)器處于整個(gè)無線傳感網(wǎng)絡(luò)的中心位置，是無線傳感網(wǎng)絡(luò)中最關(guān)鍵的節(jié)點(diǎn)。整個(gè)傳感網(wǎng)絡(luò)只允許一個(gè)協(xié)調(diào)器存在，負(fù)責(zé)創(chuàng)建網(wǎng)絡(luò)ID、各個(gè)節(jié)點(diǎn)地址分配、維護(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)時(shí)，則確定無沖突網(wǎng)絡(luò)存在，并創(chuàng)建ZigBee網(wǎng)絡(luò)，之后協(xié)調(diào)器節(jié)點(diǎn)不斷產(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è)置個(gè)域網(wǎng)地址（PAN ID），其是一個(gè)16位的無線網(wǎng)絡(luò)標(biāo)志符。通過協(xié)議棧配置文件f8wConfg.cfg中的ZDO_CONFIG_PAN_ID函數(shù)，可以設(shè)置PANID為0～0x3FFF之間的一個(gè)值。一個(gè)無線傳感網(wǎng)絡(luò)只允許一個(gè)PAN ID存在，而且它在所處的信道中地址ID是唯一的。此外還確定協(xié)調(diào)器短地址，短地址是網(wǎng)絡(luò)中設(shè)備的標(biāo)識號。默認(rèn)協(xié)調(diào)器短地址為0x0000。

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

2.3 路由器節(jié)點(diǎn)程序設(shè)計(jì)

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

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

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

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

2.4 終端節(jié)點(diǎn)程序設(shè)計(jì)

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

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

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

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

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

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

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

2.5.1 加速度、角速度獲取

針對于原始加速度、角速度的數(shù)據(jù)獲取，主要是在CC2530上電后，定義MPU_Init（void）函數(shù)對傳感器進(jìn)行初始化設(shè)置，包括初始化I2C總線、復(fù)位傳感器、選擇相應(yīng)量程等基本操作[8]。這里選擇了加速度計(jì)±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 運(yùn)動姿態(tài)解算

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

MPU6050芯片內(nèi)置有獨(dú)立的數(shù)字運(yùn)動處理器（DMP），陀螺儀的三軸角速度、加速度計(jì)的三軸加速度數(shù)據(jù)經(jīng)DMP自身運(yùn)動處理算法處理后生成表示姿態(tài)的角度值，輸出格式為q30的四元數(shù)，但四元數(shù)只在運(yùn)算上有優(yōu)勢，無法直觀表示物體的運(yùn)動姿態(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é)點(diǎn)程序設(shè)計(jì)

在本設(shè)計(jì)中，電機(jī)溫度采集節(jié)點(diǎn)有兩個(gè)，分別檢測大臂步進(jìn)電機(jī)以及中臂步進(jìn)電機(jī)的溫度。這里只就一個(gè)節(jié)點(diǎn)設(shè)計(jì)進(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ā)送至路由器。打包時(shí)，除了包含該節(jié)點(diǎn)溫度值數(shù)據(jù)外，還有加入的節(jié)點(diǎn)device ID，方便區(qū)分不同節(jié)點(diǎn)的溫度。傳感器軟件工作流程如圖2所示。

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

3 結(jié) 語

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