基于慣性傳感器的集群移動機器人定位系統設計

，，，，

(1.寧夏大學 物理與電子電氣工程學院，銀川 750021；2.北京交通大學)

基于慣性傳感器的集群移動機器人定位系統設計

孟一飛1,2，謝堂健1，楊文慧1，劉麗萍1，陳燕1

(1.寧夏大學 物理與電子電氣工程學院，銀川 750021；2.北京交通大學)

為了對集群機器人的定位技術做進一步研究，本文對群體移動機器人的整體框架進行了設計，主要包括控制單元、通信模塊和傳感器單元等，重點對基于MPU9250慣性傳感器的定位技術進行了研究，定位系統以ZigBee模塊組成無線傳感網絡，用Arduino對MPU9250進行姿態數據獲取并進行處理，利用ZigBee網絡將位置信息傳輸到上位機。本文提出的機器人慣導定位技術對于解決機器人的定位問題具有一定的指導意義。

集群；Arduino；ZigBee；定位；MPU9250

引 言

集群是大自然中群體生物常見的生活方式，自然界中的生物多以群體生活，如大雁的遷徙、魚群的覓食、蟻群的群居生活等。通過模擬這些行為，揭示在群體行為中蘊含的科學知識，將其中的技術應用在人類的生活實踐中，能夠推動仿生學的發展。群體里的每個機器人可以根據需要自主控制自己的行動，但還是要通過合作彌補個體智能的不同，從而使群體更加智能，這些都為一些大型且復雜任務的選擇提供了更好的幫助。現在也有很多的例子可以說明群體機器人的廣泛使用，例如美國DARPA公司開展的Smart Dust項目，運用MEMS技術做成了移動機器人，然后建成感知網絡的方式，就是運用群體合作來進行的。還有美國MIT AI實驗室微型多移動機器人系統Ants也是運用了集群這種方式。

本文重點論述了集群移動機器人中的定位問題，旨在解決機器人之間的相互定位難題，主要采用ZigBee技術與MPU9250傳感器相結合的解決思路，利用Arduino對MPU9250進行了數據讀取，以及通過無線傳感器網絡實現信息的交互傳輸。

1 無線網絡技術概述

1.1 ZigBee無線傳感網絡

無線網伴隨微電子技術、無線通信等技術而發展，而物聯網技術的發展推動了無線傳感網絡(Wireless Sensor Networks, WSN)的快速發展。WSN主要是用來監測、感知和采集信息，能夠實現信息的采集、處理和傳輸功能[1]，在智慧農業[2]、智能家居[3]、軍事等領域有著重要的應用價值。本文是利用無線傳感網絡來監測移動機器人的加速度、角速度等物理量。

本文所用到的ZigBee節點芯片選用的是CC2530，內含增強型微控制器內核，該芯片具有良好的抗干擾性以及靈敏的接收度，8 KB RAM和多種閃存版本，具備RF收發器，硬件支持CSMA/CA，CC2530的工作電壓在2～3.6 V，串口可運行于UART和SPI模式。

1.2 室內定位技術概述

圖1 集群移動機器人整體框架

定位技術是機器人的重要研究方向之一，本文的研究背景是室內環境下機器人定位。參考文獻[4]提出了傳感器和視覺里程計結合的定位算法；參考文獻[5]采用的是超聲波定位技術，該技術基于超聲波測距原理，GPS定位技術雖然在室外環境具有廣泛的應用，但是一般的民用精度也不是很高(10 m左右)，面對復雜的室內環境，基本無使用；參考文獻[6]給出了基于WiFi信號的室內機器人定位實現，建立了指紋圖并引入了粒子群算法去噪；參考文獻[7]設計了一種基于超寬帶(UWB)的高精度室內定位系統，具有精度高、穩定性高的特點，能夠滿足機器人的定位需求；參考文獻[8]采用智能手機定位思路，對指紋數據庫進行了優化改進，其實質還是利用了WiFi差分信號強度SSD，此外，可以利用智能手機內嵌的加速度計、陀螺儀、磁力計等慣性傳感器推算移動節點的位移。

雖然作為LBS(Location Based Service)最后一米的室內定位飽受關注，但其依然不夠成熟。不同于GPS、AGPS等室外定位系統，室內定位系統依然沒有形成統一的標準，多種室內定位技術相映生輝。目前，室內定位技術主要有藍牙定位、射頻識別、超寬帶、WiFi定位等，定位算法根據是否需要測距離分為基于測距和非測距兩類。前者主要有TOA (Time of Arrive) 、AOA (Angle of Arrive) 、TDOA (Time Difference of Arrive)等方法 ，后者主要有基于接收信號強度RSSI(Received Signal Strength Indicator)的定位算法等[10]。

基于無線網的的定位技術研究是一個熱門方向，WiFi、ZigBee、藍牙都處于2.4 GHz ISM(Industrial Scientific Medical)頻段，它們的定位基理是RSSI，物理模型就是信號衰減模型。無線網定位系統主要由錨節點和待定節點構成，利用信號的傳播特性來計算距離，進而轉換成位置坐標。室內環境復雜多樣，這就決定了信號傳播模型是制約定位精度的重要因素。

機器人超聲波定位技術容易實現，技術成熟，但是在群體機器人中，機器人之間相互定位的時候存在著阻擋問題。UWB屬高精度室內定位技術，它利用窄帶脈沖傳輸數據，無須載波，該技術常采用TDOA算法。UWB定位系統主要包括接收器、標簽等，該技術存在頻譜利用率低、信息傳輸速率不高等特點，并且成本較高。除了以上提及的定位技術外，還有基于計算機視覺的方法。

2 集群移動機器人總體框架設計

整個集群移動機器人系統主要包括幾大技術：控制技術、定位技術、通信技術和協同算法。集群移動功能的實現采用Arduino作控制、以ZigBee為通信方式、以魚群算法作為協同算法。整體框架如圖 1所示?？刂茊卧捎肁Tmega328P作為機器人的主控芯片，該芯片是一款增強型的AVR單片機，用它控制機器人的前進、后退等動作；定位部分采用慣性傳感器與ZigBee技術結合的方式，核心器件是MPU9250九軸傳感器模塊與CC2530，由Arduino讀取MPU9250的姿態數據并換算處理，再經ZigBee節點以無線的方式將信息發送出去，各節點的信息可匯總至協調器經上位機顯示。其中，Arduino與MPU9250之間的通信方式采用I2C通信，Arduino與CC2530之間采用UART通信；協同算法采用人工魚群算法，以此來實現機器人的聚群、追尾等功能。本文主要對集群移動機器人的定位系統進行研究。

2.1 控制單元設計

Arduino是一種開源單片機控制器，而Arduino Uno又是典型的控制器，功能齊全，使用廣泛，本文使用的ATmega328P工作電壓為5 V，具有32 KB 片上可編程Flash, 支持UART、I2C和SPI通信。由于其具有先進的指令集以及單時鐘周期指令執行時間，該芯片的數據吞吐率非常高，從而降低了系統功耗，提高了運行速度。對于一個機器人而言，僅僅有主控芯片是不夠的，在本系統中，用舵機、電源、輪胎等器件組成了機器人的外部結構。對于Arduino的軟件開發，要在Arduino IDE開發環境中進行。

2.2 通信模塊設計

作為無線通信的主流方式之一，ZigBee技術以其眾多優勢在物聯網等領域取得了廣泛的應用，而且在機器人領域已經取得了一席之地。CC2530芯片是ZigBee芯片中應用最廣的芯片之一，它比CC243x系列性能要更穩定。根據內置閃存容量區分，可將CC2530分為以下4個類型的產品：CC2530F32、CC2530F64、CC2530F128和CC2530F256。本文采用的是CC2530F256芯片，該產品支持自組網和透明傳輸功能。本文主要將CC2530用于終端移動節點和協調器，從而為構建ZigBee網絡提供支撐。

2.3 集群機器人算法研究

集群機器人算法主要是在人工魚群算法的基礎上來模擬生物群體聚群并統一步調整體運動的過程，主要修改了人工魚的各項參數，并更換行為評價函數和食物密度函數，實現了個體間能夠保持一定距離，朝向一個方向整體運動的功能。機器人在運動過程中有4種基本行為：聚群行為、隨機行為、覓食行為和追尾行為。這4種基本行為的選擇由評價函數給出執行選擇，其中隨機行為是其他三種行為的補充行為。通過對人工魚群算法的改進，使得個體能夠較好地實現聚群，即個體間保持一定的距離，又不至于過度遠離。首先完成群體的聚群，選擇最優的聚群參數。在完成群體的聚群條件下，修改食物密度函數、行為評價函數，使得群體能夠按照制定的路線運動。

3 基于MPU9250的定位技術研究

定位系統的硬件部分主要包括Arduino、ZigBee節點和MPU9250慣性傳感器，它們分別實現執行、通信和采集數據的功能。

3.1 MPU9250慣性傳感器

MPU9250為InvenSense公司的第二代9軸運動跟蹤設備，該傳感器體積小，功耗低。MPU9250是一個集成模塊，包括兩部分：MPU6500三軸傳感器和AK8963三軸磁力計。其中MPU6500內含3軸加速度計和3軸陀螺儀，故稱其為9軸運動傳感器[13]。MPU9250支持I2C和SPI通信，本文采用的是I2C方案，在該方案中，Arduino是主機，MPU9250是從機，數據的傳輸主要是通過SDA和SCL引腳實現的，可輸出9軸數據，I2C通信方案如圖2所示。MPU9250具有3個16位加速度A/D輸出、3個16位陀螺儀A/D輸出、3個6位磁力計A/D輸出，此外該傳感器提供了全量程的參數編程設置，具體的量程分配如表1所列，其中g為重力加速度。MPU9250含有一個512字節的FIFO寄存器，可以將加速度值等數據寫入FIFO緩沖區。

圖2 Arduino與MPU9250 I2C通信連接

名 稱可編量程單位加速度計±2，±4，±8，±16g陀螺儀±250，±500，±1000°/s磁力計±4800μT

在I2C模式下，通信速率最快可達400 kHz，SCL用來傳輸時鐘信號，SDA用來傳輸數據。當SDA處在下降沿，SCL置高的時候，通信開始，之后主機發出地址位和讀寫位。然后，主機釋放SDA線，并等待來自從機的確認信號(ACK)。每次數據傳輸后必須跟一位讀寫位。從機應答即是拉低SDA到SCL高電平周期結束。當SDA處于上升沿且SCL是高電平的時候，代表通信停止。具體的I2C通信時序如圖3所示。

圖3 I2C數據傳輸時序圖

3.2 基于ZigBee和MPU9250的定位原理

本文提出了將ZigBee無線傳感網絡與MPU9250慣導數據相結合的思路。基于MPU9250的定位系統通過陀螺儀、加速度計以及磁力計的結合實現節點定位。首先提供移動節點初始位置及速度，然后在節點運動過程中對MPU9250的信息進行獲取和處理，不斷更新當前位置及速度。通過檢測加速度和角速度，定位系統可以檢測位置變化、速度變化和姿態變化(繞各個軸的旋轉)。陀螺儀用于測量移動機器人的角速率，加速度計用于測量線加速度，通過跟蹤系統當前角速率及相對于運動系統測量到的當前線加速度，即確定當前線加速度。以起始速度作為初始條件，用運動學方程對慣性加速度進行積分，就可得到系統慣性速率，然后以起始位置作初始條件再次積分就可得到位置，即對加速度二次積分得到位置。慣性傳感器的誤差會隨時間累積，故需要不斷進行修正?，F代慣性導航系統使用各種信號(例如全球定位系統及磁羅盤等)對其進行修正，采取控制論原理對不同信號進行權級過濾，保證慣性導航系統的精度及可靠性。

加速度是一個物體速度的變化速率。即

積分和導數相反。如果一個物體的加速度已知，那么能夠利用二重積分獲得物體的位置。假設初始條件為0，那么有：

上式給出了加速度定位的數學理論知識，圖4給出了加速度定位的算法流程。

4 集群移動機器人定位軟件設計

4.1 控制單元與ZigBee網絡的通信接口設計

4.1.1 Arduino串口程序

圖4 利用加速度求解 位置算法流程

Arduino與ZigBee節點的通信是通過UART方式完成的。Arduino與ZigBee節點的連接方案是：Arduino的TX引腳接CC2530的RX引腳，RX引腳接CC2530的TX引腳。Arduino控制板通過數字引腳0(RX)和1(TX)將信息傳輸至ZigBee節點，對于Arduino串口程序的設計，本文在Arduino 1.6.11 (Windows)環境下開發，下面僅介紹Arduino發送信息程序中用到的主要函數。

//串口通信初始化

Serial.begin(115200);

//輸出數據

Serial.println();

4.1.2 ZigBee串口程序

ZigBee串口程序主要實現CC2530接收Arduino傳回的數據。整個過程包括串口初始化、串口中斷接收等流程，其中，串口的初始化流程如圖5所示。

圖5 CC2530串口初始化

選擇外部32 MHz晶振作為主時鐘源，代碼如下：

//設置系統時鐘源為32 MHz晶振

CLKCONCMD &=～0x40;

//等待晶振穩定為32 MHz

while(CLKCONSTA & 0x40);

CC2530的串口是USART0和USART1，兩個串口都可以運行在UART或SPI模式，本文中使用的是UART模式，故應先配置模式。串口配置以及波特率的設置代碼如下：

//外設控制寄存器 USART 0的I/O位置:0為P0口位置1

PERCFG = 0x00;

//P0_2,P0_3用作串口(外設功能)

P0SEL = 0x0C;

//P0優先作為UART0

P2DIR &= ～0xC0;

//設置為UART方式

U0CSR |= 0x80;

//波特率設為115200

U0GCR |= 11;

U0BAUD |= 216;

4.2 基于MPU9250的定位程序設計

MPU9250驅動程序的設計主要包括圖6所示的流程。

圖6 MPU9250驅動軟件流程

對于MPU9250的初始化，主要是依據MPU9250的使用說明和寄存器手冊對一些寄存器的定義和配置。比如加速計的配置用到的ACCEL_CONFIG寄存器可以對加速度計是否自檢和量程進行設置，代碼如下：

//將ACCEL_CONFIG設置為0x00，即把加速度計設為不自檢模式且量程為±2g

I2Cdev::writeByte(0x68,ACCEL_CONFIG, 0x00);

I2C的軟件編寫主要根據上文介紹的MPU9250 I2C時序圖編寫，主要程序包括I2C開始、I2C結束、I2C應答、I2C讀取字節等。數據獲取即對9軸數據進行獲取，從而得到9軸輸出的真實數據。下面以加速度和陀螺儀X軸的數據獲取為例，給出數據獲取程序。MPU9250加速度和陀螺儀X軸數據的獲取程序如下：

//讀取計算加速度X軸數據

*ax = (((int16_t)buffer[0]) << 8) | buffer[1];

*ax=2.0*g*(*ax)/(32768);

//讀取計算陀螺儀X軸數據

圖7 磁力計 數據獲取流程

*gx = (((int16_t)buffer[8]) << 8) | buffer[9];

*gx=250.0*(*gx)/32768;

計算公式為：真實值=ADC值×選擇的滿量程/32768。

加速度計與陀螺儀分別有三個16位的ADC，每個軸使用一個，最高位為符號位，剩下15位為數值位，因此輸出值為32 768～-32 768，為了得到真正的加速度值，需要在原始數據的基礎上除以32 768。所以設計中，選取加速度量程為±2g時，精度為16 384 LSB/g。

磁力計數據獲取流程如圖7所示。

4.3 ZigBee組網程序設計

ZigBee網絡的建立主要包括兩大部分，一是協調器建立網絡，二是節點的入網。具體的軟件流程如圖8和圖9所示。軟件開發是在IAR Embedded Workbench環境下進行，并且使用了Z-stack協議棧編程。

圖8 協調器的軟件流程

圖9 節點的軟件流程

5 實驗及測試

以ax，ay，az分別代表加速度計的X軸、Y軸、Z軸數據，以gx，gy，gz分別代表陀螺儀的X軸、Y軸、Z軸數據。靜止狀態下的加速度值和陀螺儀數據如表2所列。由于MPU9250處于靜止狀態，加速度水平方向的X軸和Y軸的數據都是0，Z軸受重力影響，存在1g大小的重力加速度，而表中得到的數據是-9，屬誤差。X軸的角速度不為0亦是誤差原因。其中，加速度值的單位是m/s2，陀螺儀數據即角速度的單位是°/s。

表2 靜止狀態下MPU9250的加速度值和角速度值

沿X軸負方向時的加速度值和陀螺儀數據如表3所列。沿X軸負方向做變加速直線運動時，ax顯然是負值。

繞Z軸逆時針旋轉時的加速度值和陀螺儀數據如表4所列。繞Z軸逆時針旋轉時，gz是變化的。

表3 沿X軸負方向運動時MPU9250的加速度值和角速度值

表4 繞Z軸逆時針旋轉時MPU9250的加速度值和角速度值

靜止狀態下，從AK8963中讀出的數據如表5所列，其中磁力計數據的單位為μT。

表5 靜止時MPU9250的磁力計數據

結 語

[1] 謝金龍,鄧人銘.物聯網無線傳感網絡技術與應用(ZigBee版)[M].北京:人民郵電出版社,2016.

[2] 饒強,于舒娟,張昀,等.基于無線傳感網絡精細農業系統盲檢測[J].電視技術,2015(21):87-90,103.

[3] 肖天柱.基于無線傳感網絡的智能家居管理系統

DesignofSwarmMobileRobotsLocationSystemBasedonInertialSensor

MengYifei1,2,XieTangjian1,YangWenhui1,LiuLiping1,ChenYan1

(1.School of Physics and Electronic-electrical Engineering,Ningxia University,Yinchuan 750021,China;2.Beijing Jiaotong University)

In order to study the positioning technology of cluster robots,the overall framework of the swarm-robot is designed in the paper,which mainly includes the control unit,the communication module and the sensor unit.The positioning technology of the MPU9250 inertial sensor is mainly studied.The positioning system uses the ZigBee module to form the wireless sensor network and uses Arduino to acquire and process the attitude data from MPU9250.The location information will be transmitted to the host computer via ZigBee network.The robot inertial navigation technology proposed in the paper has certain guiding significance to solve the problem of robot positioning.

swarm;Arduino;ZigBee;positioning;MPU9250

TP368

A