本發(fā)明涉及一種自主探索無(wú)人機(jī)系統(tǒng)及方法，具體涉及一種基于多機(jī)協(xié)作的受限空間自主探索無(wú)人機(jī)系統(tǒng)及方法，屬于無(wú)人機(jī)。

背景技術(shù)：

1、無(wú)人機(jī)自主探索系統(tǒng)是一種集成了多種先進(jìn)技術(shù)的復(fù)雜系統(tǒng)，旨在使無(wú)人機(jī)能夠在無(wú)人為控制的情況下，自主地執(zhí)行任務(wù)、探索未知環(huán)境并做出決策。其在許多領(lǐng)域都有較好的應(yīng)用前景，包括但不限于軍事偵察、環(huán)境監(jiān)測(cè)、交通監(jiān)控、農(nóng)業(yè)、野生動(dòng)物保護(hù)、災(zāi)難響應(yīng)和搜救行動(dòng)。

2、申請(qǐng)?zhí)枮?020100510052的發(fā)明專利公布了一種無(wú)人機(jī)的探測(cè)定位系統(tǒng)及方法，包括：中心處理站、多個(gè)偵收站；中心處理站包括：第一衛(wèi)星模塊、第一處理器、第一衛(wèi)星天線、第一數(shù)據(jù)鏈路模塊；其中，第一衛(wèi)星模塊和第一數(shù)據(jù)鏈路模塊分別連接所述第一處理器；每個(gè)偵收站包括：第二衛(wèi)星模塊、第二處理器、第二衛(wèi)星天線、空地鏈路信號(hào)偵收模塊、偵收天線、時(shí)間戳標(biāo)定模塊、第二數(shù)據(jù)鏈路模塊；其中，空地鏈路信號(hào)偵收模塊分別與偵收天線以及時(shí)間戳標(biāo)定模塊連接，第二衛(wèi)星模塊、空地鏈路信號(hào)偵收模塊、時(shí)間戳標(biāo)定模塊以及第二數(shù)據(jù)鏈路模塊分別連接第二處理器。該專利技術(shù)可以提高對(duì)無(wú)人機(jī)的探測(cè)定位精度。但是，如上述專利的無(wú)人機(jī)系統(tǒng)大多依賴于gnss系統(tǒng)獲得定位，僅能在室外開(kāi)放空間工作，無(wú)法在室內(nèi)、隧道、管道、罐體、地下等受限空間內(nèi)工作。

3、隨著技術(shù)的進(jìn)步，有部分廠家設(shè)計(jì)的無(wú)人機(jī)產(chǎn)品安裝有一個(gè)激光雷達(dá)并運(yùn)行slam算法，從而可以在受限空間內(nèi)獲得定位并飛行?，F(xiàn)有的多機(jī)協(xié)同slam算法往往都依賴于多機(jī)之間的互相觀測(cè)和對(duì)周圍環(huán)境感知數(shù)據(jù)的互通訊，但在實(shí)際的應(yīng)用場(chǎng)景中，這兩種條件往往都得不到滿足，因此導(dǎo)致現(xiàn)有的多機(jī)協(xié)同slam算法僅停留在學(xué)術(shù)階段，難以得到工程應(yīng)用。比如，具體到受限空間的自主探索而言，各架無(wú)人機(jī)節(jié)點(diǎn)往往各自飛往不同的樓層、不同的隧道岔路方向，互相不可見(jiàn)，根本無(wú)法滿足多機(jī)中間能夠持續(xù)實(shí)時(shí)互相觀測(cè)和持續(xù)進(jìn)行大帶寬環(huán)境感知數(shù)據(jù)的需求。而大多數(shù)現(xiàn)有的多機(jī)協(xié)同slam算法都依賴實(shí)時(shí)的互觀測(cè)和互通訊數(shù)據(jù)進(jìn)行位姿優(yōu)化，一旦節(jié)點(diǎn)進(jìn)入互相不可見(jiàn)狀態(tài)，slam算法很容易發(fā)散，進(jìn)而導(dǎo)致無(wú)人機(jī)自主探索失敗。

4、可見(jiàn)，現(xiàn)有的無(wú)人機(jī)系統(tǒng)探測(cè)效率低，無(wú)法滿足應(yīng)急救災(zāi)、軍事偵察等需要在短時(shí)間內(nèi)遍歷某一受限空間并建立地圖模型的需求，鑒于上述情況，能夠多機(jī)協(xié)同進(jìn)行受限空間自主飛行探索的無(wú)人機(jī)系統(tǒng)需求迫切。

技術(shù)實(shí)現(xiàn)思路

1、為解決現(xiàn)有技術(shù)的不足，本發(fā)明的目的在于提供一種基于多機(jī)協(xié)作受限空間自主探索無(wú)人機(jī)系統(tǒng)和方法，該系統(tǒng)和方法能夠在復(fù)雜、狹窄或危險(xiǎn)環(huán)境中進(jìn)行自主導(dǎo)航和執(zhí)行任務(wù)，具有實(shí)際工程應(yīng)用的可行性。

2、為了實(shí)現(xiàn)上述目標(biāo)，本發(fā)明采用如下的技術(shù)方案：

3、本發(fā)明首先公布了一種基于多機(jī)協(xié)作受限空間自主探索無(wú)人機(jī)系統(tǒng)，其包括：相互通信的若干無(wú)人機(jī)節(jié)點(diǎn)和中心指揮站，所述中心指揮站上運(yùn)行有多節(jié)點(diǎn)實(shí)時(shí)在線協(xié)作slam算法和離線高精度slam建模算法；所述無(wú)人機(jī)節(jié)點(diǎn)包括：飛行控制器、執(zhí)行機(jī)構(gòu)、若干視覺(jué)傳感器和激光雷達(dá)傳感器、高性能處理器、存儲(chǔ)器和多點(diǎn)通訊裝置，所述視覺(jué)傳感器用于采集周圍環(huán)境的圖像信息，所述激光雷達(dá)傳感器用于采集周圍環(huán)境的點(diǎn)云數(shù)據(jù)，所述高性能處理器上運(yùn)行有各無(wú)人機(jī)節(jié)點(diǎn)的本地slam建圖和遍歷探索算法，并與所述視覺(jué)傳感器、激光雷達(dá)傳感器、存儲(chǔ)器及多點(diǎn)通訊裝置相連接，且所述高性能處理器控制飛行控制器，進(jìn)而發(fā)送指令至執(zhí)行機(jī)構(gòu)。所述視覺(jué)傳感器和激光雷達(dá)傳感器通過(guò)以太網(wǎng)、mipi或usb高速數(shù)據(jù)接口與高性能處理器連接。

4、優(yōu)選地，前述存儲(chǔ)器用于存儲(chǔ)無(wú)人機(jī)所搭載的各種傳感器的原始數(shù)據(jù)信息，所述多點(diǎn)通訊裝置用于建立各個(gè)無(wú)人機(jī)和中心指揮站的數(shù)據(jù)通訊。

5、本發(fā)明還公布了基于前述的基于多機(jī)協(xié)作的受限空間自主探索無(wú)人機(jī)系統(tǒng)的探索方法，包括三個(gè)階段：

6、(1)初始階段，用于檢測(cè)各無(wú)人機(jī)的初始位姿，初始化各個(gè)slam節(jié)點(diǎn)初值；

7、(2)自主探索粗建模階段，各個(gè)無(wú)人機(jī)節(jié)點(diǎn)進(jìn)行自主探索，在中心指揮站上建立實(shí)時(shí)粗略模型，并且該步驟對(duì)于節(jié)點(diǎn)間的通訊丟失是魯棒的；

8、(3)離線精確建模階段，各個(gè)無(wú)人機(jī)節(jié)點(diǎn)返航后將數(shù)據(jù)傳送或拷貝至中心指揮站，離線建立精確的模型。

9、優(yōu)選地，在前述初始階段，無(wú)人機(jī)集中于中心節(jié)點(diǎn)附近待起飛，環(huán)境中還可部署一個(gè)或多個(gè)激光合作目標(biāo)，從而增強(qiáng)初始階段各無(wú)人機(jī)對(duì)周圍環(huán)境的可觀性。

10、更優(yōu)選地，前述初始階段的具體工作流程為：

11、s1.1、使各無(wú)人機(jī)節(jié)點(diǎn)保持靜置狀態(tài)數(shù)秒，得到無(wú)人機(jī)機(jī)載的imu傳感器數(shù)據(jù)的零偏和重力觀測(cè)，同步無(wú)人機(jī)節(jié)點(diǎn)將積累數(shù)秒鐘的視覺(jué)和激光雷達(dá)觀測(cè)數(shù)據(jù)，積分得到對(duì)周圍環(huán)境點(diǎn)云數(shù)據(jù)的初始觀測(cè)p0i，其中下角標(biāo)0表示初始時(shí)刻，上角標(biāo)i表示無(wú)人機(jī)節(jié)點(diǎn)編號(hào)，按照無(wú)人機(jī)通電順序排列，第一臺(tái)通電的無(wú)人機(jī)記為0，后續(xù)通電的無(wú)人機(jī)編號(hào)記為1到n，以編號(hào)為0的無(wú)人機(jī)通電時(shí)的機(jī)體坐標(biāo)系為全局坐標(biāo)系；

12、s1.2、各無(wú)人機(jī)節(jié)點(diǎn)將自身對(duì)周圍環(huán)境的點(diǎn)云觀測(cè)降采樣后的數(shù)據(jù)傳輸?shù)街行闹笓]站；此時(shí)，各無(wú)人機(jī)節(jié)點(diǎn)尚未出發(fā)，仍處于中心節(jié)點(diǎn)附近，故此傳輸是具備可行性的。

13、s1.3、中心指揮站將各無(wú)人機(jī)采樣的點(diǎn)云觀測(cè)數(shù)據(jù)與第一臺(tái)無(wú)人機(jī)采樣的點(diǎn)云觀測(cè)數(shù)據(jù)互相匹配，得到各無(wú)人機(jī)出發(fā)前的相對(duì)位姿和變換矩陣其中指初始時(shí)刻第i架無(wú)人機(jī)在全局坐標(biāo)下的位置向量，指初始時(shí)刻第i架無(wú)人機(jī)在全局坐標(biāo)系下的姿態(tài)角四元數(shù)，指初始時(shí)刻從第i架無(wú)人機(jī)機(jī)體坐標(biāo)轉(zhuǎn)移到全局坐標(biāo)系的轉(zhuǎn)移矩陣；

14、s1.4、中心指揮站將各無(wú)人機(jī)出發(fā)前的相對(duì)位姿發(fā)回給各無(wú)人機(jī)節(jié)點(diǎn)，用于初始化各slam節(jié)點(diǎn)狀態(tài)變量的初始位姿。

15、再優(yōu)選地，前述自主探索階段的具體工作流程為：

16、s2.0、各無(wú)人機(jī)和中心指揮站將要探索的空間柵格化，劃分成三維體素網(wǎng)格，并將所有網(wǎng)格均初始化為未抵達(dá)的；

17、s2.1、各無(wú)人機(jī)節(jié)點(diǎn)運(yùn)行自身的slam算法，通過(guò)imu傳感器數(shù)據(jù)進(jìn)行狀態(tài)更新，將狀態(tài)傳導(dǎo)至其中k指當(dāng)前時(shí)刻，k-1指上一時(shí)刻；

18、s2.2、各無(wú)人機(jī)節(jié)點(diǎn)將點(diǎn)云觀測(cè)數(shù)據(jù)匹配到各自的局部地圖mlocali上進(jìn)行觀測(cè)更新，優(yōu)化自身的位姿信息和變換矩陣并使用該位姿信息進(jìn)行無(wú)人機(jī)的位姿控制，指當(dāng)前時(shí)刻k從第i架無(wú)人機(jī)機(jī)體坐標(biāo)轉(zhuǎn)移到全局坐標(biāo)系的轉(zhuǎn)移矩陣；

19、s2.3、各無(wú)人機(jī)節(jié)點(diǎn)維護(hù)拼接自己的局部地圖：

20、s2.4、各無(wú)人機(jī)節(jié)點(diǎn)將自身的位姿信息變換矩陣和降采樣后的觀測(cè)點(diǎn)云數(shù)據(jù)傳輸?shù)街行闹笓]站；

21、s2.5、中心指揮站根據(jù)s2.2中接收到的變換矩陣將各無(wú)人機(jī)觀測(cè)到的新點(diǎn)云數(shù)據(jù)變換到全局坐標(biāo)系，并拼接成粗略的實(shí)時(shí)全局地圖：

22、s2.6、中心指揮站將粗略實(shí)時(shí)全局地圖映射到三維體素柵格地圖，并將有映射的柵格標(biāo)記為已抵達(dá)的，其余柵格仍保持原狀態(tài)；

23、s2.7、中心指揮站將已抵達(dá)柵格的更新信息廣播傳輸給各無(wú)人機(jī)節(jié)點(diǎn)；

24、s2.8、各無(wú)人機(jī)節(jié)點(diǎn)按照距離遠(yuǎn)近搜索自身附近的柵格，直到找到最近的未抵達(dá)柵格，然后將無(wú)人機(jī)指引飛向該方向，并從步驟s2.1開(kāi)始重復(fù)工作；若周圍所有柵格均已抵達(dá)，則控制無(wú)人機(jī)返航。

25、進(jìn)一步優(yōu)選地，前述每個(gè)三維體素網(wǎng)格可存儲(chǔ)一種探索狀態(tài)，所述探索狀態(tài)分為：已抵達(dá)、未抵達(dá)。

26、再進(jìn)一步優(yōu)選地，前述離線精確建模階段的具體工作流程為：

27、s3.1、中心指揮站控制各無(wú)人機(jī)節(jié)點(diǎn)返航到初始位置，各無(wú)人機(jī)的原始傳感器數(shù)據(jù)傳送到中心指揮站上；

28、s3.2、按照s1.3中觀測(cè)出的初始位姿，為每個(gè)子slam算法賦初值；

29、s3.3、拼接各個(gè)節(jié)點(diǎn)的初始觀測(cè)點(diǎn)云，作為初始精確全局地圖：

30、s3.4、按照每個(gè)節(jié)點(diǎn)采樣時(shí)間順序輪流的方式，根據(jù)每個(gè)子slam節(jié)點(diǎn)的imu數(shù)據(jù)為每個(gè)子slam算法執(zhí)行更新步驟，將狀態(tài)傳導(dǎo)至

31、s3.5、按照每個(gè)節(jié)點(diǎn)采樣時(shí)間順序輪流的方式，為每個(gè)子slam算法執(zhí)行觀測(cè)步驟，將觀測(cè)點(diǎn)云匹配到精細(xì)全局地圖，優(yōu)化狀態(tài)變量中的位姿精度。該步驟中，與s2.2中的區(qū)別是：點(diǎn)云觀測(cè)將在整個(gè)精細(xì)全局地圖mglobalp上進(jìn)行，因而能夠更好地將多個(gè)無(wú)人機(jī)感知的點(diǎn)云數(shù)據(jù)匹配起來(lái)；

32、s3.6、拼接每一步的精細(xì)全局地圖：

33、s3.7、執(zhí)行回環(huán)檢測(cè)和圖優(yōu)化，進(jìn)一步提高位姿和建模精度；

34、s3.8、循環(huán)至s3.3重新開(kāi)始工作，直至所有傳感器數(shù)據(jù)均被處理完為止；

35、s3.9、輸出最終的全局地圖mglobalp，作為精細(xì)化全局模型。

36、優(yōu)選地，步驟s3.1中，通過(guò)數(shù)據(jù)鏈路或可移動(dòng)的存儲(chǔ)介質(zhì)將各無(wú)人機(jī)的原始傳感器數(shù)據(jù)傳輸或拷貝到中心指揮站上。

37、本發(fā)明的有益之處在于：

38、本發(fā)明的具有創(chuàng)新性的多機(jī)協(xié)同受限空間自主探索無(wú)人機(jī)系統(tǒng)，其能夠在中心指揮站的控制下，由多臺(tái)無(wú)人機(jī)通過(guò)機(jī)載的視覺(jué)傳感器和激光雷達(dá)傳感器自主在受限空間內(nèi)進(jìn)行定位和建圖，自主規(guī)劃探索軌跡，遍歷該受限空間，并在中心指揮站上建立粗略的實(shí)時(shí)地圖。無(wú)人機(jī)返回后，將多機(jī)存儲(chǔ)的傳感器原始數(shù)據(jù)合并導(dǎo)入到中心指揮站，并計(jì)算建立精確的地圖模型，能夠?qū)崿F(xiàn)在復(fù)雜、狹窄或危險(xiǎn)環(huán)境中進(jìn)行自主導(dǎo)航和執(zhí)行任務(wù)，供應(yīng)急搜救、軍事行動(dòng)指揮之用。

39、與現(xiàn)有技術(shù)中其他類似系統(tǒng)相比，本發(fā)明將協(xié)同探索過(guò)程分為了僅依賴各無(wú)人機(jī)節(jié)點(diǎn)局部地圖信息的實(shí)時(shí)自主探索階段和需要全局信息的離線精確建模階段。其中前一個(gè)過(guò)程無(wú)需依賴無(wú)人機(jī)之間的實(shí)時(shí)大帶寬數(shù)據(jù)通訊，而后一個(gè)過(guò)程在無(wú)人機(jī)返航后進(jìn)行，故而本發(fā)明具備了實(shí)際工程應(yīng)用的可行性。其他類似系統(tǒng)在探索階段均需要無(wú)人機(jī)節(jié)點(diǎn)之間觀測(cè)信息的實(shí)時(shí)數(shù)據(jù)交互來(lái)進(jìn)行位置解算和建模，而在本發(fā)明針對(duì)的受限空間探索場(chǎng)景，因地形、墻壁、障礙的遮擋，這種傳輸是不具備可行性的，故其他類似系統(tǒng)在不做針對(duì)性優(yōu)化的前提下很難在本發(fā)明場(chǎng)景下得到應(yīng)用。