童 金，李恒宇，羅 均，謝少榮

TONG Jin, LI Heng-yu, LUO Jun, XIE Shao-rong

（上海大學(xué) 機(jī)電工程與自動(dòng)化學(xué)院，上海 200072）

0 引言

無(wú)人機(jī)(UAV， unmanned air vehicles)具備能夠在危險(xiǎn)，復(fù)雜的未知區(qū)域環(huán)境執(zhí)行相關(guān)任務(wù)，在軍用及民用上廣泛的應(yīng)用前景[1,2]。同時(shí)，多無(wú)人機(jī)協(xié)同執(zhí)行任務(wù)及相關(guān)研究逐漸成為無(wú)人機(jī)研究的一種趨勢(shì)[3～5]。

無(wú)人機(jī)的空中優(yōu)勢(shì)，能夠適用于快速對(duì)地面目標(biāo)的搜尋定位。而測(cè)向交叉定位系統(tǒng)具有全方位、能夠快速較遠(yuǎn)、在受到干擾的情況下仍能正常工作等優(yōu)點(diǎn)。在目標(biāo)測(cè)向交叉定位方面，目前國(guó)內(nèi)外主要的研究集中在實(shí)現(xiàn)多個(gè)傳感器的合理布局以提高定位精度問(wèn)題[1,4]。多無(wú)人機(jī)協(xié)同控制方面，目前國(guó)內(nèi)外研究主要集中在編隊(duì)控制，多機(jī)協(xié)同任務(wù)和路徑規(guī)劃[2,6]。在上述研究基礎(chǔ)上，我們提出采用基于提高定位精度的導(dǎo)航算法來(lái)實(shí)現(xiàn)無(wú)人機(jī)的搜尋算法的研究，具體研究集中在導(dǎo)航算法采用地上無(wú)人機(jī)器人上實(shí)現(xiàn)。為了驗(yàn)證所提出搜尋方法的合理性，基于兩個(gè)自主行進(jìn)機(jī)器人，設(shè)計(jì)開發(fā)了一套模擬搜尋控制系統(tǒng)，并進(jìn)行了相關(guān)實(shí)驗(yàn)。

1 多機(jī)搜尋有源目標(biāo)的原理

多無(wú)人機(jī)搜尋主要基于無(wú)人飛行機(jī)器人自主飛行基礎(chǔ)上，通過(guò)導(dǎo)航控制飛行機(jī)器人，尋找目標(biāo)并予以定位[3,5]。為了能夠模擬空中飛行搜尋目實(shí)驗(yàn)，本文將無(wú)人機(jī)搜尋系統(tǒng)簡(jiǎn)化。假設(shè)無(wú)人機(jī)執(zhí)行搜尋任務(wù)時(shí)飛行高度可以忽略，將三維的搜尋定位系統(tǒng)轉(zhuǎn)化為二維平面搜尋定位系統(tǒng)。假設(shè)測(cè)量角度誤差是零均值的高斯白噪聲，具體如圖1所示，無(wú)人機(jī)A與無(wú)人機(jī)B在不同位置對(duì)目標(biāo)T進(jìn)行定位。

圖1 多無(wú)人機(jī)搜尋目標(biāo)示意簡(jiǎn)圖

其中目標(biāo)定位誤差區(qū)域?yàn)閇8]：

本文所提出的搜尋定位過(guò)程，主要分為全局搜索、逼近目標(biāo)和定位目標(biāo)三個(gè)階段：

全局目標(biāo)搜尋階段，首先無(wú)人機(jī)在通訊范圍的允許下，與搜尋邊界及友機(jī)之間拉開最大距離，最大化搜尋區(qū)域；其次，無(wú)人機(jī)盡可能直線飛行來(lái)減小轉(zhuǎn)向所帶來(lái)的能量額外損耗，增加執(zhí)行任務(wù)時(shí)間；同時(shí)，無(wú)人機(jī)盡可能小的搜尋整個(gè)系統(tǒng)已經(jīng)搜尋過(guò)的區(qū)域。各個(gè)無(wú)人機(jī)在搜尋同時(shí)不斷計(jì)算下一個(gè)搜尋點(diǎn)，對(duì)飛機(jī)進(jìn)行導(dǎo)航。一旦有目標(biāo)信號(hào)出現(xiàn)，立刻進(jìn)入第二階段。

逼近目標(biāo)階段，當(dāng)接收到目標(biāo)產(chǎn)生的信號(hào)，測(cè)向設(shè)備向機(jī)器人系統(tǒng)反饋測(cè)向信號(hào)。同時(shí)機(jī)器人等待接收友機(jī)發(fā)出的測(cè)向數(shù)據(jù)，結(jié)合收集到的數(shù)據(jù)采用交叉定位原理對(duì)目標(biāo)進(jìn)行定位[3]；基于目標(biāo)定位誤差區(qū)域最小化來(lái)導(dǎo)航機(jī)器人，同時(shí)以最小的轉(zhuǎn)向半徑飛向最后目標(biāo)估計(jì)位置。

無(wú)人機(jī)逼近的導(dǎo)航代價(jià)函數(shù)如下：

其中d1為飛機(jī)到最近的友機(jī)的距離，d2為飛機(jī)到估算目標(biāo)位置，Φ為飛機(jī)的轉(zhuǎn)彎半徑，P為飛機(jī)的轉(zhuǎn)彎需要的離散點(diǎn)數(shù)量。

定位目標(biāo)：當(dāng)無(wú)人機(jī)收到友機(jī)發(fā)出目標(biāo)測(cè)向相關(guān)數(shù)據(jù)之后，這時(shí)候定位系統(tǒng)進(jìn)入?yún)f(xié)調(diào)定位階段。通過(guò)前面提到的交叉定位及卡爾曼濾波理論來(lái)對(duì)多路測(cè)向數(shù)據(jù)進(jìn)行融合，對(duì)目標(biāo)信號(hào)位置進(jìn)行最優(yōu)估計(jì)，并計(jì)算定位精度[2,7]。同時(shí)，通過(guò)機(jī)器人的無(wú)線通訊設(shè)備將目標(biāo)定位相關(guān)數(shù)據(jù)發(fā)送回地面控制系統(tǒng)。

圖2 搜尋系統(tǒng)的模擬實(shí)驗(yàn)平臺(tái)

2 實(shí)驗(yàn)平臺(tái)設(shè)計(jì)

2.1 試驗(yàn)系統(tǒng)的硬件組成部分

地面實(shí)驗(yàn)平臺(tái)如圖2所示，主要由具備自主行進(jìn)能力的機(jī)器人（兩輛），信號(hào)發(fā)生器，測(cè)向設(shè)備，通訊設(shè)備，地面控制系統(tǒng)組成。

該實(shí)驗(yàn)系統(tǒng)的硬件結(jié)構(gòu)如圖3所示。

圖3 試驗(yàn)系統(tǒng)硬件結(jié)構(gòu)圖

2.1.1 MCU

機(jī)器人系統(tǒng)，要求其易擴(kuò)展其他硬件設(shè)備,同時(shí)具備較強(qiáng)的處理能力來(lái)實(shí)現(xiàn)復(fù)雜的算法。因此采用基于工業(yè)PC架構(gòu)的開放式機(jī)器人，該機(jī)器人處理器是AMD3000+，集成Windows XP操作系統(tǒng)和VC開發(fā)環(huán)境，最大化的使用硬件軟件資源，同時(shí)易于獲取廣泛的驅(qū)動(dòng)支持。

2.1.2 運(yùn)動(dòng)控制模塊

系統(tǒng)的運(yùn)動(dòng)控制器是完全基于PC架構(gòu)的開放式控制器。用戶可以通過(guò)運(yùn)動(dòng)控制卡（基于TMS320 F2812 DSP的控制卡）實(shí)時(shí)控制機(jī)器人運(yùn)動(dòng)和實(shí)時(shí)獲取編碼器的數(shù)據(jù)。利用實(shí)時(shí)獲取的編碼器數(shù)據(jù)可以進(jìn)行精確的PID運(yùn)動(dòng)控制算法對(duì)運(yùn)動(dòng)進(jìn)行反饋控制。由于采用了USB接口，用戶可以方便的擴(kuò)展出多軸運(yùn)動(dòng)控制。通過(guò)MCU給DSP運(yùn)動(dòng)板卡發(fā)出指令，能精確的控制機(jī)器人行走。

2.1.3 避障模塊

由于機(jī)器人搜尋區(qū)域環(huán)境的未知不確定性，同時(shí)為了機(jī)器人系統(tǒng)相互能夠更好的執(zhí)行任務(wù)，我們給機(jī)器人加裝了避障模塊，在本文實(shí)驗(yàn)是通過(guò)加裝聲納模塊與PSD(Position Sensitive Detector)模塊。聲納測(cè)距模塊，來(lái)測(cè)定機(jī)器人周圍障礙物距離（前，左，右），聲納的探測(cè)距離是41cm到7m，測(cè)量精度是+/-5%，測(cè)量頻率是20Hz。配備的PSD模塊來(lái)實(shí)現(xiàn)80cm內(nèi)障礙測(cè)量，PSD測(cè)量范圍是10-80cm，測(cè)量精度是+/-8%，測(cè)量頻率是100HZ。

2.1.4 通訊模塊

多無(wú)人機(jī)之間相互協(xié)調(diào)工作的前提就是通過(guò)建立一個(gè)通訊組網(wǎng)，能夠?qū)崿F(xiàn)機(jī)器人之間，機(jī)器人與基地之間，實(shí)時(shí)，大數(shù)據(jù)量的數(shù)據(jù)交互。通過(guò)給系統(tǒng)機(jī)器人及基地各加裝XStream-PKG系列的無(wú)線數(shù)據(jù)傳輸模塊，900MHz,具備最遠(yuǎn)32km的傳輸距離，通過(guò)配置可以給多機(jī)器人組建立一個(gè)通訊組網(wǎng)數(shù)據(jù)鏈，實(shí)現(xiàn)多機(jī)器人之間的相互通訊。

2.1.5 導(dǎo)航模塊

搜尋系統(tǒng)通過(guò)算法修改機(jī)器人航向，機(jī)器人實(shí)時(shí)狀態(tài)數(shù)據(jù)消息則通過(guò)GPS及電子羅盤，編碼器獲取。GPS模塊，采用GARMIN的12通道GPS,差分模式下具備小于5m的定位精度。通過(guò)串口向機(jī)器人系統(tǒng)發(fā)出GPS數(shù)據(jù)。電子羅盤，使用的是Honeywell的HMR3000，通過(guò)RS232串口通信，向機(jī)器人輸出航向，航向精度0.5°，分辨率為0.1°。機(jī)器人系統(tǒng)運(yùn)動(dòng)狀態(tài)可以通過(guò)運(yùn)動(dòng)控制模塊從編碼器獲得，多路狀態(tài)數(shù)據(jù)，在本實(shí)驗(yàn)中，采用卡爾曼濾波方法對(duì)機(jī)器人運(yùn)動(dòng)數(shù)據(jù)進(jìn)行融合。

2.1.6 測(cè)向模塊

在具備自主導(dǎo)航行進(jìn)能力機(jī)器人上，為了能夠使機(jī)器人系統(tǒng)進(jìn)行搜尋定位目標(biāo)，我們給機(jī)器人加裝了測(cè)向裝置。機(jī)器人能夠?qū)τ性茨繕?biāo)發(fā)出的信號(hào)進(jìn)行方向測(cè)量，來(lái)用于機(jī)器人對(duì)目標(biāo)進(jìn)行逐步定位，從而也不斷給予機(jī)器人搜尋導(dǎo)航。測(cè)向裝置的測(cè)向誤差是+/-1°，由于實(shí)驗(yàn)區(qū)域范圍我們假定在范圍之內(nèi)進(jìn)行，因此低精度的測(cè)向設(shè)備完全滿足我們實(shí)驗(yàn)定位精度要求。

2.1.7 地面控制模塊

地面控制模塊（基地系統(tǒng)）通過(guò)安裝無(wú)線數(shù)據(jù)鏈，與機(jī)器建立通訊組網(wǎng)，實(shí)時(shí)監(jiān)控并接收機(jī)器人發(fā)回來(lái)的數(shù)據(jù)，供系統(tǒng)進(jìn)行決策。同時(shí)系統(tǒng)通過(guò)通訊組網(wǎng)向機(jī)器人發(fā)出任務(wù)執(zhí)行及其它命令，供機(jī)器人執(zhí)行。

2.2 實(shí)驗(yàn)系統(tǒng)的軟件系統(tǒng)

由于機(jī)器人系統(tǒng)是基于PC機(jī)平臺(tái)來(lái)實(shí)現(xiàn)運(yùn)動(dòng)控制及與外設(shè)的數(shù)據(jù)處理，我們通過(guò)VC++來(lái)實(shí)現(xiàn)相關(guān)控制算法代碼的編寫。系統(tǒng)軟件流程圖如圖4所示。

圖4 搜尋系統(tǒng)的軟件圖

通過(guò)上面的流程圖，我們采用VC6.0進(jìn)行相關(guān)控制軟件編寫。通過(guò)串口讀取測(cè)向設(shè)備目標(biāo)方向數(shù)據(jù)及測(cè)向誤差。在搜尋系統(tǒng)初始化后，機(jī)器人等待基地系統(tǒng)發(fā)出指令，開始搜尋目標(biāo)。在搜尋目標(biāo)同時(shí)，機(jī)器人等待基地系統(tǒng)發(fā)出結(jié)束任務(wù)指令，實(shí)現(xiàn)機(jī)器人回收任務(wù)。在搜尋到目標(biāo)之后，通過(guò)與友機(jī)器人進(jìn)行通訊得到友機(jī)的測(cè)向數(shù)據(jù)，通過(guò)UKF最優(yōu)估計(jì)理論進(jìn)行數(shù)據(jù)融合計(jì)算出目標(biāo)[3]，然后通過(guò)無(wú)線傳輸數(shù)據(jù)鏈傳輸模塊將數(shù)據(jù)發(fā)送給基地系統(tǒng)。

3 實(shí)驗(yàn)結(jié)果及其分析

考慮到無(wú)人機(jī)器人的實(shí)驗(yàn)過(guò)程的航程較短（一般小于50m），為了簡(jiǎn)便，此時(shí)可以把小范圍的地球看作平面，故本項(xiàng)目選擇當(dāng)?shù)厮阶鴺?biāo)系XOY（也稱東北天坐標(biāo)系）作為導(dǎo)航坐標(biāo)系，并取其原點(diǎn)為無(wú)人機(jī)器人起始點(diǎn)，X軸為正東方向，Y軸為正北方向，Z軸向天，坐標(biāo)軸的單位均為米。已知地球赤道圈長(zhǎng)為40075.36千米和地球子午線長(zhǎng)為39940.67千米，設(shè)起始點(diǎn)在WGS-84坐標(biāo)系中的值為（λ0，?0），則WGS-84坐標(biāo)系到XOY坐標(biāo)系的轉(zhuǎn)換公式為：

在實(shí)驗(yàn)中為了保持實(shí)驗(yàn)穩(wěn)定性，把自主行進(jìn)機(jī)器人速度控制在0.1m/s，通過(guò)在坐標(biāo)系的建立運(yùn)動(dòng)函數(shù)，數(shù)據(jù)處理周期間隔是0.01秒，通過(guò)采集數(shù)據(jù)，不斷記錄自主行進(jìn)機(jī)器人行進(jìn)的軌跡。

首先將基地系統(tǒng)位置經(jīng)坐標(biāo)系轉(zhuǎn)化為坐標(biāo)起始位置。同時(shí)初始化給定目標(biāo)（運(yùn)動(dòng)或靜止）起始位置，自主行進(jìn)機(jī)器人處于待命令階段，打開目標(biāo)信號(hào)源發(fā)送目標(biāo)信號(hào)；同時(shí)，將兩機(jī)器人放出，在我們定義的系統(tǒng)坐標(biāo)系里面，UV1在（0，35）位置，航向135，速度0.5m/s，UV2起始（30，0）航向315，速度0.5m/s，打開信號(hào)源靜止目標(biāo)在位置（25，25）發(fā)出信號(hào)。

當(dāng)機(jī)器人搜尋到目標(biāo)信號(hào)后，通過(guò)無(wú)線數(shù)據(jù)鏈傳傳輸?shù)降孛婵刂葡到y(tǒng)。根據(jù)采集得到數(shù)據(jù)，我們利用Matlab 來(lái)描繪搜尋軌跡，得到機(jī)器人搜尋軌跡圖如圖5所示。

圖5 機(jī)器人搜尋軌跡圖

在地面模擬試驗(yàn)中，當(dāng)機(jī)器人接收到目標(biāo)信號(hào)后，通過(guò)與友機(jī)進(jìn)行相互通訊，對(duì)目標(biāo)位置進(jìn)行定位，同時(shí)依據(jù)產(chǎn)生最小誤差，機(jī)器人開始逐漸改變自己航向，不斷逼近目標(biāo)。這一過(guò)程都完成的比較好，從圖5可以看出，在不同位置，一旦接收到目標(biāo)信號(hào)，機(jī)器人便開始更正航向，按照我們?cè)O(shè)定的搜尋算法對(duì)目標(biāo)的定位與逼近，同時(shí)不斷的對(duì)目標(biāo)進(jìn)行了定位。實(shí)驗(yàn)結(jié)果表明該控制系統(tǒng)的可適用性以及實(shí)驗(yàn)平臺(tái)和控制算法的有效性。

4 結(jié)論

隨著無(wú)人機(jī)研究的日趨深入，多無(wú)人機(jī)協(xié)調(diào)控制是目前研究的一個(gè)熱點(diǎn)問(wèn)題。本文提出基于提高定位精度的導(dǎo)航算法來(lái)實(shí)現(xiàn)無(wú)人機(jī)對(duì)有源目標(biāo)的搜尋，并基于兩個(gè)自主行進(jìn)機(jī)器人的實(shí)驗(yàn)平臺(tái)，設(shè)計(jì)開發(fā)了一套模擬搜尋控制系統(tǒng)，并進(jìn)行實(shí)驗(yàn)驗(yàn)證以及誤差分析。實(shí)驗(yàn)結(jié)果表明該控制系統(tǒng)的可適用性以及實(shí)驗(yàn)平臺(tái)和控制算法的有效性。

[1]修建娟,何友,王國(guó)宏,等.測(cè)向交叉定位系統(tǒng)中交匯角研究[J].宇航學(xué)報(bào),2005,26(3):282-286.

[2]廖沫,陳宗基,周銳.基于MAS的多UAV協(xié)同任務(wù)分配設(shè)計(jì)與仿真[J].系統(tǒng)仿真學(xué)報(bào),2008.529(增).

[3]王中華,覃征,韓毅.基于UKF的雙平臺(tái)無(wú)源融合跟蹤方法[J].系統(tǒng)仿真學(xué)報(bào),2007,10,19(19):4477-4486.

[4]K.Pine,M.E.Evans,K.Sammut and F.He , Radio Direction Finding for Maritime Search and Rescue[J],5th Asian Control Conference,2004,723-730.

[5]Randal W.Beard,Timothy W.McLain,Multiple UAV Cooperative Search under Collision Avoidance and Limited Range Communication Constraints [J],Proceedings of the 42nd IEEE Conference on Decision and Control.Maui,Hawaii USA,December,2003,25-30.

[6]Gregory J.Toussaint,Pedro De Lima, and Daniel J.Pack,Localizing RF Targets with Cooperative Unmanned Aerial Vehicles [J],Proceedings of the 2007 American Control Conference Marriott Marquis Hotel at Times Square New York City,USA, July 11-13,2007,5928-5933.

[7]胡來(lái)招.無(wú)源定位[M].北京:國(guó)防工業(yè)出版社,2004.

[8]王紅星,曹建平.通信偵查與干擾技術(shù)[M].北京:國(guó)防工業(yè)出版社,2005.