一種微型無人機(jī)室內(nèi)自主導(dǎo)航方法
【專利摘要】本發(fā)明涉及一種微型無人機(jī)室內(nèi)自主導(dǎo)航方法，屬于微型無人機(jī)自主導(dǎo)航【技術(shù)領(lǐng)域】，該方法包括：基于RGB-D相機(jī)和MEMS慣性傳感器的微型無人機(jī)運(yùn)動(dòng)狀態(tài)估計(jì)；基于RGB-D相機(jī)和MEMS慣性傳感器融合的三維環(huán)境實(shí)時(shí)建模；實(shí)時(shí)可信路徑規(guī)劃與路徑跟蹤控制，通過所述三個(gè)步驟實(shí)施位置控制和姿態(tài)控制，引導(dǎo)無人機(jī)跟蹤預(yù)定路徑飛行。本發(fā)明提高了無人機(jī)定位以及運(yùn)動(dòng)狀態(tài)估計(jì)的精度；提高了微型無人機(jī)在室內(nèi)的自主環(huán)境感知能力；生成的路徑可在滿足路徑可行性的基礎(chǔ)上有效保證定位精度；有效提高微型無人機(jī)的自主能力。
【專利說明】一種微型無人機(jī)室內(nèi)自主導(dǎo)航方法

【技術(shù)領(lǐng)域】
[0001] 本發(fā)明屬于微型無人機(jī)自主導(dǎo)航【技術(shù)領(lǐng)域】，特別涉及一種基于RGB-D相機(jī)和MEMS 傳感器的微型無人機(jī)室內(nèi)導(dǎo)航方法及其系統(tǒng)。

【背景技術(shù)】
[0002] 微型無人機(jī)具有體積小、機(jī)動(dòng)性強(qiáng)、操作靈活、成本低廉等特點(diǎn)，可在危險(xiǎn)及復(fù)雜 環(huán)境下執(zhí)行任務(wù)，因此近年來在偵察、救災(zāi)、環(huán)境探測等軍用和民用領(lǐng)域得到了非常廣泛的 應(yīng)用。如何在復(fù)雜室內(nèi)環(huán)境（如高層建筑火災(zāi)現(xiàn)場、震后建筑物、塌方礦井等）下實(shí)現(xiàn)微型 無人機(jī)的自主飛行是該領(lǐng)域的重要研究問題，與之相對應(yīng)的自主導(dǎo)航、規(guī)劃與控制是實(shí)現(xiàn) 微型無人機(jī)自主飛行的關(guān)鍵技術(shù)所在。
[0003] 室內(nèi)環(huán)境是一類典型的復(fù)雜任務(wù)場景，與大尺度室外空間相比，結(jié)構(gòu)復(fù)雜且非結(jié) 構(gòu)化，具有封閉三維空間、環(huán)境不確定性、無GPS定位信號輔助等特點(diǎn)。因此，在該類環(huán)境下 實(shí)現(xiàn)自主飛行，要求微小無人機(jī)在無外部輔助導(dǎo)航設(shè)施、自身機(jī)載載荷和計(jì)算資源有限的 情況下，僅采用機(jī)載傳感器自主確定自身的位置、速度、姿態(tài)等運(yùn)動(dòng)狀態(tài)，并利用傳感器信 息建立環(huán)境的三維模型，在此基礎(chǔ)上自主規(guī)劃最優(yōu)飛行軌跡并控制無人機(jī)按照既定軌跡飛 行，完成飛行任務(wù)。
[0004] 這些關(guān)鍵技術(shù)在當(dāng)前均有一定的局限性，難以完全滿足自主室內(nèi)飛行的要求。
[0005] 由于室內(nèi)環(huán)境下無法接收到GPS信號，或者信號的可靠性較低，因此微型無人機(jī) 的狀態(tài)估計(jì)與環(huán)境建模所依賴的機(jī)載傳感器主要包括慣性測量器件和視覺傳感器。其中 典型的慣性測量器件主要包括陀螺和加速度計(jì)，可感測無人機(jī)運(yùn)動(dòng)過程中的角速度和加速 度，基于慣性測量器件的測量數(shù)據(jù)可以確定無人機(jī)的姿態(tài)與速度。微機(jī)電器件（MEMS)具有 重量輕、成本低、功耗低、集成度高等優(yōu)點(diǎn)，可滿足微型無人機(jī)的重量、體積和功耗限制，因 此微型無人機(jī)系統(tǒng)一般采用MEMS慣性測量器件。但MEMS慣性測量器件的測量值往往存在 較大的誤差，且隨時(shí)間漂移，因此必須采用視覺傳感器的測量值與慣性測量值通過濾波技 術(shù)進(jìn)行信息融合，以修正誤差，獲得更高精度的狀態(tài)估計(jì)值。目前的微型無人機(jī)采用的視覺 輔助運(yùn)動(dòng)估計(jì)技術(shù)主要基于激光測距、單目視覺和雙目視覺。
[0006] 激光測距技術(shù)依賴激光測距儀所提供的無人機(jī)與環(huán)境的相對距離測量值，通過對 前后兩幀激光掃描值進(jìn)行對比和匹配，解算出前后兩幀數(shù)據(jù)之間的旋轉(zhuǎn)、平移轉(zhuǎn)換關(guān)系，從 而得到無人機(jī)的運(yùn)動(dòng)狀態(tài)參數(shù)。但激光測距技術(shù)存在很大的局限性。激光測距儀的測量范 圍有限，且本質(zhì)上只能提供在測量平面內(nèi)的二維相對距離信息，因此激光測距技術(shù)僅適用 于具有特殊垂直結(jié)構(gòu)的環(huán)境，當(dāng)環(huán)境僅存在簡單結(jié)構(gòu)時(shí)（長廊、環(huán)形建筑等），激光測距算 法將會失效。
[0007] 視覺運(yùn)動(dòng)估計(jì)技術(shù)基于攝像頭器件獲取的環(huán)境圖像序列，提取連續(xù)圖像序列中的 特征點(diǎn)，對相同的特征點(diǎn)進(jìn)行匹配，進(jìn)而計(jì)算特征點(diǎn)之間的平移和旋轉(zhuǎn)關(guān)系，獲得運(yùn)動(dòng)體的 運(yùn)動(dòng)參數(shù)。根據(jù)所采用的攝像頭器件數(shù)量，視覺運(yùn)動(dòng)估計(jì)技術(shù)主要包括單目視覺和雙目視 覺技術(shù)。與激光測距技術(shù)相比，視覺運(yùn)動(dòng)估計(jì)技術(shù)可以充分利用環(huán)境的三維信息。單目視 覺技術(shù)利用單個(gè)攝像頭器件提供的環(huán)境圖像信息計(jì)算運(yùn)動(dòng)體的方向參數(shù)，但無法提供運(yùn)動(dòng) 體與環(huán)境的相對距離信息（即環(huán)境深度信息）。與之相比，雙目視覺技術(shù)可對兩個(gè)攝像頭器 件所獲取的圖像中的相同環(huán)境特征點(diǎn)進(jìn)行立體匹配，從而計(jì)算深度參數(shù)，并可通過不同幀 圖像的特征提取、特征匹配和運(yùn)動(dòng)估計(jì)過程獲得運(yùn)動(dòng)體的六自由度運(yùn)動(dòng)參數(shù)。但雙目視覺 算法一般較為復(fù)雜，運(yùn)算量大，且視覺運(yùn)動(dòng)估計(jì)技術(shù)通常要求環(huán)境中包括足夠豐富的紋理 特征，因此該類技術(shù)在微型無人機(jī)運(yùn)動(dòng)狀態(tài)估計(jì)中的應(yīng)用也具有較大的局限性。
[0008] RGB-D相機(jī)是近年來在機(jī)器人和計(jì)算機(jī)視覺領(lǐng)域逐漸得到應(yīng)用的新型視覺設(shè)備， 該類型的設(shè)備可同時(shí)提供外部環(huán)境的彩色圖像和三維深度數(shù)據(jù)（即環(huán)境中的點(diǎn)相對于 RGB-D相機(jī)的距離）。其中三維深度的測量基于結(jié)構(gòu)光技術(shù)實(shí)現(xiàn)：RGB-D相機(jī)主動(dòng)向環(huán)境空 間發(fā)射紅外光線，并對環(huán)境空間的被測方向進(jìn)行編碼并產(chǎn)生光斑，進(jìn)而通過檢測環(huán)境中的 光斑測量空間的三維深度。與傳統(tǒng)的攝像頭器件相比，RGB-D相機(jī)成本較低，體積較小，重 量較輕；可直接提供三維深度數(shù)據(jù)，并能夠在光照不良或紋理較少的環(huán)境中有效檢測三維 深度；因此更適用于室內(nèi)環(huán)境中的微型無人機(jī)的運(yùn)動(dòng)估計(jì)和環(huán)境建模。
[0009] 另一方面，自主環(huán)境感知和三維建模也是微型無人機(jī)實(shí)現(xiàn)室內(nèi)自主飛行的關(guān)鍵技 術(shù)，其目的是為了建立微型無人機(jī)所處環(huán)境的三維模型，為實(shí)現(xiàn)路徑規(guī)劃和控制提供基礎(chǔ)。 微型無人機(jī)機(jī)載傳感器（激光測距儀、雙目視覺系統(tǒng)）通過測量其與環(huán)境中各點(diǎn)的三維相 對距離獲得環(huán)境的深度信息，通常以三維點(diǎn)云（一系列的三維點(diǎn)）形式表達(dá)。通過對連續(xù)不 同幀的三維點(diǎn)云數(shù)據(jù)進(jìn)行配準(zhǔn)和融合，即可得到完整的三維環(huán)境模型。當(dāng)前的三維環(huán)境建 模技術(shù)一般單純利用視覺傳感器獲取的三維點(diǎn)云數(shù)據(jù)進(jìn)行匹配，以計(jì)算不同三維點(diǎn)云之間 的旋轉(zhuǎn)和平移關(guān)系，本質(zhì)上與運(yùn)動(dòng)估計(jì)為同一過程。在計(jì)算過程中，每一步中兩幀數(shù)據(jù)之間 匹配的誤差會隨著計(jì)算過程累積，易導(dǎo)致最終的建模誤差。目前的系統(tǒng)一般利用SLAM(即 時(shí)定位與建模）的閉環(huán)檢測進(jìn)行優(yōu)化，但該類方法計(jì)算量較大，難以滿足實(shí)時(shí)性要求。
[0010] 路徑規(guī)劃指的是依據(jù)環(huán)境感知信息，在無人機(jī)狀態(tài)空間內(nèi)規(guī)劃出一條從初始點(diǎn)到 目標(biāo)點(diǎn)的、滿足各方面約束的最優(yōu)連續(xù)路徑的過程。目前常用的路徑規(guī)劃算法包括概率路 徑圖（PRM)、快速隨機(jī)樹（RRT)、A*算法、混合整數(shù)線性規(guī)劃（MILP)等。而在室內(nèi)復(fù)雜環(huán)境 下的路徑規(guī)劃具有其特殊性，微型無人機(jī)的定位和運(yùn)動(dòng)狀態(tài)估計(jì)、以及環(huán)境建模的精度高 度依賴傳感器能夠檢測到的環(huán)境中有效特征信息，因此要求算法能夠結(jié)合機(jī)載傳感器的特 性以及環(huán)境特征，自主規(guī)劃出能夠確保定位和運(yùn)動(dòng)狀態(tài)估計(jì)性能的路徑。在實(shí)際應(yīng)用中，微 型無人機(jī)一般采用濾波（如Kalman濾波等）的方法融合機(jī)載各傳感器的測量信息，從而實(shí) 現(xiàn)定位和運(yùn)動(dòng)狀態(tài)估計(jì)。濾波方法的基本原理是認(rèn)為無人機(jī)位置和各運(yùn)動(dòng)狀態(tài)服從某種類 型的概率分布模型（如高斯分布等），進(jìn)而根據(jù)機(jī)載各傳感器的測量信息估計(jì)無人機(jī)的位 置和各運(yùn)動(dòng)狀態(tài)的后驗(yàn)概率分布（由概率論可知，概率分布以期望和協(xié)方差表示）。因此衡 量定位和運(yùn)動(dòng)狀態(tài)估計(jì)性能的主要指標(biāo)為定位和運(yùn)動(dòng)狀態(tài)估計(jì)的可信度（由概率論可知， 通過濾波方法估計(jì)得到的后驗(yàn)概率分布的協(xié)方差的大小表征了定位和運(yùn)動(dòng)狀態(tài)估計(jì)的可 信度：若協(xié)方差較大，則定位和運(yùn)動(dòng)狀態(tài)估計(jì)的可信度較低；反之則可信度較高）。而目前 常用的方法在規(guī)劃過程中均未考慮定位和狀態(tài)估計(jì)可信度因素，因此難以完全適應(yīng)室內(nèi)自 主飛行的要求。
[0011] 因此，當(dāng)前微型無人機(jī)系統(tǒng)的導(dǎo)航、環(huán)境建模以及路徑規(guī)劃方法均存在一定的局 限性，難以滿足室內(nèi)環(huán)境下自主飛行的特殊需求。


【發(fā)明內(nèi)容】

[0012] 本發(fā)明的目的是為克服已有技術(shù)的不足之處，提出一種微型無人機(jī)室內(nèi)自主導(dǎo)航 方法及其導(dǎo)航系統(tǒng)，以實(shí)現(xiàn)在無外部定位和計(jì)算輔助的條件下，僅依賴機(jī)載傳感器和設(shè)備 的微型無人機(jī)自主定位、環(huán)境感知建模以及路徑規(guī)劃，提高微型無人機(jī)在復(fù)雜室內(nèi)環(huán)境下 的自主飛行能力。
[0013] 本發(fā)明提出的一種微型無人機(jī)室內(nèi)自主導(dǎo)航方法，其特征在于，該方法包括以下 步驟：
[0014] S1:基于RGB-D相機(jī)和MEMS慣性傳感器的微型無人機(jī)運(yùn)動(dòng)狀態(tài)估計(jì)：利用RGB-D 相機(jī)獲取環(huán)境的二維彩色圖像和三維深度數(shù)據(jù)，通過特征點(diǎn)檢測、特征點(diǎn)匹配、運(yùn)動(dòng)狀態(tài)估 計(jì)獲得微型無人機(jī)的姿態(tài)和位置參數(shù)估計(jì)值，利用MEMS傳感器獲取慣性測量值，將所述姿 態(tài)和位置參數(shù)估計(jì)值與慣性測量值通過濾波方法進(jìn)行信息融合，用以抑制慣性測量值誤差 積累，得到更為精確的無人機(jī)包括位置、速度、姿態(tài)在內(nèi)的運(yùn)動(dòng)狀態(tài)估計(jì)值；
[0015] S2:基于RGB-D相機(jī)和MEMS慣性傳感器融合的三維環(huán)境實(shí)時(shí)建模：利用步驟S1 中RGB-D相機(jī)獲取的三維深度數(shù)據(jù)，以及融合后的運(yùn)動(dòng)狀態(tài)估計(jì)值進(jìn)行三維點(diǎn)云的精確配 準(zhǔn)與融合，構(gòu)建三維環(huán)境模型，并觸發(fā)式地對該模型進(jìn)行全局優(yōu)化，用以提高模型精度；
[0016] S3:實(shí)時(shí)可信路徑規(guī)劃與路徑跟蹤控制：設(shè)定規(guī)劃周期，在每個(gè)規(guī)劃周期中，以步 驟S2得到的三維環(huán)境模型為基礎(chǔ)，同時(shí)建立RGB-D相機(jī)的測量誤差模型和定位可信度的評 價(jià)函數(shù)，從而生成無人機(jī)從當(dāng)前點(diǎn)到目標(biāo)點(diǎn)的路徑，在每個(gè)規(guī)劃周期結(jié)束后，選擇當(dāng)前最 優(yōu)路徑執(zhí)行；根據(jù)所述當(dāng)前最優(yōu)路徑，與步驟S1得到運(yùn)動(dòng)狀態(tài)估計(jì)值進(jìn)行比較，得到當(dāng)前 的位置偏差，從而根據(jù)當(dāng)前位置偏差生成無人機(jī)的位置控制指令，根據(jù)位置控制指令解算 為無人機(jī)的姿態(tài)控制指令，實(shí)施位置控制和姿態(tài)控制，引導(dǎo)無人機(jī)跟蹤預(yù)定路徑飛行。
[0017] 本發(fā)明提出的微型無人機(jī)室內(nèi)自主導(dǎo)航方法及其導(dǎo)航系統(tǒng)，其優(yōu)點(diǎn)是：
[0018] 1、本發(fā)明提供的方法中，利用機(jī)載RGB-D相機(jī)和MEMS慣性傳感器進(jìn)行集成，實(shí)現(xiàn) 了基于多傳感器融合的狀態(tài)估計(jì)，有效修正低成本MEMS慣性測量器件的誤差積累，提高了 無人機(jī)定位以及運(yùn)動(dòng)狀態(tài)估計(jì)的精度，可在室內(nèi)無GPS及其它外部定位輔助的特殊情況 下，完全依靠機(jī)載傳感器自主為微型無人機(jī)提供高精度、可靠的運(yùn)動(dòng)狀態(tài)估計(jì)；
[0019] 2、本發(fā)明提供的方法實(shí)現(xiàn)了基于RGB-D相機(jī)和MEMS慣性傳感器融合的實(shí)時(shí)三維 環(huán)境建模，基于融合后的高精度運(yùn)動(dòng)參數(shù)實(shí)現(xiàn)三維點(diǎn)云的精確配準(zhǔn)和融合，有效提高了三 維環(huán)境建模的精度；采用并行的閉環(huán)檢測及優(yōu)化過程對三維環(huán)境模型進(jìn)行全局優(yōu)化，可在 避免影響建I旲實(shí)時(shí)性的如提下有效提商整體建I旲精度，提商了微型無人機(jī)在室內(nèi)的自主環(huán) 境感知能力；
[0020] 3、本發(fā)明基于RGB-D相機(jī)的工作特性，實(shí)現(xiàn)了一種可信路徑實(shí)時(shí)規(guī)劃和路徑跟蹤 控制方法，可有效適應(yīng)不確定和動(dòng)態(tài)環(huán)境，以及無人機(jī)的復(fù)雜動(dòng)力學(xué)和運(yùn)動(dòng)學(xué)約束，生成的 路徑可在滿足路徑可行性的基礎(chǔ)上有效保證定位精度；
[0021] 4、本發(fā)明實(shí)現(xiàn)的微型無人機(jī)自主飛行系統(tǒng)可在復(fù)雜室內(nèi)環(huán)境以及無外部定位和 計(jì)算輔助條件下，完全依賴機(jī)載傳感器和處理單元實(shí)現(xiàn)微型無人機(jī)的自主導(dǎo)航、環(huán)境感知 與建模以及路徑規(guī)劃，有效提高微型無人機(jī)的自主能力。

【專利附圖】

【附圖說明】
[0022] 圖1為本發(fā)明提出的微型無人機(jī)室內(nèi)自主導(dǎo)航方法的總體流程框圖；
[0023] 圖2為根據(jù)本發(fā)明實(shí)施例步驟1基于RGB-D相機(jī)和MEMS慣性傳感器的微型無人 機(jī)運(yùn)動(dòng)狀態(tài)估計(jì)流程圖；
[0024] 圖3為根據(jù)本發(fā)明實(shí)施例步驟2基于RGB-D相機(jī)和MEMS慣性傳感器融合的三維 環(huán)境實(shí)時(shí)建模流程圖；
[0025] 圖4為根據(jù)本發(fā)明實(shí)施例步驟3實(shí)時(shí)可信路徑規(guī)劃與路徑跟蹤控制流程圖；
[0026] 圖5為根據(jù)本發(fā)明實(shí)施例步驟3可信路徑規(guī)劃框架示意圖；
[0027] 圖6為根據(jù)本發(fā)明實(shí)施例的微型無人機(jī)室內(nèi)自主導(dǎo)航系統(tǒng)結(jié)構(gòu)示意圖。

【具體實(shí)施方式】
[0028] 下面結(jié)合附圖和實(shí)施例對本發(fā)明的【具體實(shí)施方式】進(jìn)行詳細(xì)描述。
[0029] 為達(dá)到所述目標(biāo)，本發(fā)明的第一方面的目的在于提供一種微型無人機(jī)室內(nèi)自主導(dǎo) 航方法流程，如圖1所示，該方法包括以下步驟：
[0030] S1:基于RGB-D相機(jī)和MEMS慣性傳感器的微型無人機(jī)運(yùn)動(dòng)狀態(tài)估計(jì)：利用RGB-D 相機(jī)獲取環(huán)境的二維彩色圖像和三維深度數(shù)據(jù)，通過特征點(diǎn)檢測、特征點(diǎn)匹配、運(yùn)動(dòng)狀態(tài)估 計(jì)獲得微型無人機(jī)的姿態(tài)和位置參數(shù)估計(jì)值，利用MEMS傳感器獲取慣性測量值，將所述姿 態(tài)和位置參數(shù)估計(jì)值與慣性測量值通過濾波方法進(jìn)行信息融合，用以抑制慣性測量值誤差 積累，得到更為精確的無人機(jī)包括位置、速度、姿態(tài)在內(nèi)的運(yùn)動(dòng)狀態(tài)估計(jì)值；
[0031] S2:基于RGB-D相機(jī)和MEMS慣性傳感器融合的三維環(huán)境實(shí)時(shí)建模：利用步驟S1中 RGB-D相機(jī)獲取的三維深度數(shù)據(jù)，以及融合后的運(yùn)動(dòng)狀態(tài)估計(jì)值進(jìn)行三維點(diǎn)云的精確配準(zhǔn) 與融合，構(gòu)建三維環(huán)境模型，并觸發(fā)式地對該模型進(jìn)行全局優(yōu)化，用以提高環(huán)境模型精度；
[0032]S3:實(shí)時(shí)可信路徑規(guī)劃與路徑跟蹤控制：設(shè)定規(guī)劃周期，在每個(gè)規(guī)劃周期中，以步 驟S2得到的三維環(huán)境模型為基礎(chǔ)，同時(shí)建立RGB-D相機(jī)的測量誤差模型和定位可信度的評 價(jià)函數(shù)，從而生成無人機(jī)從當(dāng)前點(diǎn)到目標(biāo)點(diǎn)的路徑，在每個(gè)規(guī)劃周期結(jié)束后，選擇當(dāng)前最 優(yōu)路徑執(zhí)行；根據(jù)所述當(dāng)前最優(yōu)路徑，與步驟S1得到運(yùn)動(dòng)狀態(tài)估計(jì)值進(jìn)行比較，得到當(dāng)前 的位置偏差，從而根據(jù)當(dāng)前位置偏差生成無人機(jī)的位置控制指令，根據(jù)位置控制指令解算 為無人機(jī)的姿態(tài)控制指令，實(shí)施位置控制和姿態(tài)控制，引導(dǎo)無人機(jī)跟蹤預(yù)定路徑飛行。
[0033] 進(jìn)一步地，步驟S1中所述的基于RGB-D相機(jī)和MEMS慣性傳感器的微型無人機(jī)運(yùn) 動(dòng)狀態(tài)估計(jì)流程，如圖2所示，具體包括以下步驟：
[0034] S11 :通過RGB-D相機(jī)獲取當(dāng)前環(huán)境的二維彩色圖像與三維深度數(shù)據(jù)，并將該二維 彩色圖像進(jìn)行預(yù)處理，轉(zhuǎn)換為以灰度值表示的二維灰度圖像；
[0035] S12 :從步驟S11預(yù)處理后的每幀二維灰度圖像中提取出一組特征點(diǎn)，并計(jì)算每一 特征點(diǎn)的特征描述向量，并通過步驟S11中獲取的三維深度數(shù)據(jù)對所述特征點(diǎn)進(jìn)行篩選， 剔除與三維深度數(shù)據(jù)無對應(yīng)的特征點(diǎn)；
[0036] S13 :對相鄰兩幀圖像中的特征點(diǎn)進(jìn)行特征匹配，通過對比特征點(diǎn)的對應(yīng)描述向量 獲得兩組匹配的特征點(diǎn)，并再剔除匹配有誤的特征點(diǎn)；
[0037] S14 :根據(jù)特征點(diǎn)對應(yīng)的三維深度數(shù)據(jù)，將步驟S13匹配后的特征點(diǎn)映射到三維空 間，從相鄰兩幀三維深度數(shù)據(jù)中獲得具有匹配關(guān)系的相鄰兩組三維點(diǎn)云；
[0038]S15:求解步驟S14中獲得的該兩組匹配的三維點(diǎn)云之間的旋轉(zhuǎn)和平移參數(shù)，利用 所述旋轉(zhuǎn)和平移參數(shù)反算出無人機(jī)的姿態(tài)和位置參數(shù)估計(jì)值；
[0039]S16:根據(jù)無人機(jī)的動(dòng)力學(xué)特性建立狀態(tài)空間模型（其中選擇無人機(jī)的位置、速 度、姿態(tài)等作為狀態(tài)空間模型的狀態(tài)量）以及RGB-D相機(jī)、MEMS慣性傳感器的觀測模型， 將S15中得到的無人機(jī)的位置參數(shù)估計(jì)值作為系統(tǒng)的狀態(tài)空間模型的觀測量，與MEMS慣性 傳感器提供的由角速度、加速度、磁觀測量信息組成的慣性測量值通過濾波方法進(jìn)行融合， 進(jìn)一步修正慣性測量值誤差，以有效抑制慣性測量值誤差積累，獲得包括無人機(jī)的位置、速 度、姿態(tài)在內(nèi)的更為精確的最終運(yùn)動(dòng)狀態(tài)估計(jì)值。
[0040]進(jìn)一步地，步驟S2中基于RGB-D相機(jī)和MEMS慣性傳感器融合的三維環(huán)境實(shí)時(shí)建 模流程，如圖3所示，具體包括以下步驟：
[0041]S21:利用如步驟S16獲得的RGB-D運(yùn)動(dòng)參數(shù)估計(jì)值與MEMS測量值融合后的運(yùn)動(dòng) 狀態(tài)估計(jì)值，以及步驟S14獲得的兩組匹配的三維點(diǎn)云，求解所述兩組匹配的三維點(diǎn)云之 間的初始旋轉(zhuǎn)和平移關(guān)系；
[0042]S22:利用步驟S21中獲得的兩組三維點(diǎn)云及其初始旋轉(zhuǎn)和平移關(guān)系，定義目標(biāo)函 數(shù)為所述兩組三維點(diǎn)云中的每對三維點(diǎn)云之間旋轉(zhuǎn)和平移誤差之和，通過最小二乘優(yōu)化迭 代方法優(yōu)化所述目標(biāo)函數(shù)，得到更為精確的三維點(diǎn)云之間的旋轉(zhuǎn)和平移關(guān)系；
[0043]S23:定義與環(huán)境固連的全局坐標(biāo)系，根據(jù)步驟S22得到的不同幀三維點(diǎn)云之間的 相對旋轉(zhuǎn)、平移關(guān)系，以及各幀三維點(diǎn)云與初始幀三維點(diǎn)云之間的旋轉(zhuǎn)和平移關(guān)系，將連續(xù) 幀的三維點(diǎn)云組合至統(tǒng)一的全局坐標(biāo)系中，得到三維環(huán)境模型；
[0044]S24:采用通用的SLAM(同時(shí)定位與地圖構(gòu)建）方法進(jìn)行閉環(huán)監(jiān)測，當(dāng)監(jiān)測到閉環(huán) 時(shí)，觸發(fā)全局優(yōu)化過程，進(jìn)一步校正不同幀三維點(diǎn)云之間的旋轉(zhuǎn)和平移關(guān)系，對步驟S23已 經(jīng)建立的三維環(huán)境模型進(jìn)行優(yōu)化，得到更為精確的的三維環(huán)境模型。
[0045]進(jìn)一步地，所述步驟3中所述的實(shí)時(shí)可信路徑規(guī)劃與路徑跟蹤控制流程，如圖4所 示，具體包括以下步驟：
[0046]S31:對步驟S2獲得的三維環(huán)境模型進(jìn)行后處理，將三維環(huán)境模型轉(zhuǎn)換為體元形 式（柵格等）存儲，以便于利用三維環(huán)境模型進(jìn)行路徑規(guī)劃；
[0047]S32:設(shè)定無人機(jī)的起始點(diǎn)和目標(biāo)點(diǎn)，根據(jù)步驟S16建立的狀態(tài)空間模型設(shè)計(jì)控 制律，建立無人機(jī)的閉環(huán)系統(tǒng)數(shù)學(xué)模型（在自動(dòng)控制理論中，通過反饋可獲得被控系統(tǒng)的 實(shí)際輸出，控制律用于根據(jù)反饋計(jì)算控制量，從而改變被控系統(tǒng)的實(shí)際輸出，使其趨近期望 值。包含所述反饋、控制律和狀態(tài)空間模型的數(shù)學(xué)模型稱為閉環(huán)系統(tǒng)數(shù)學(xué)模型），將所述起 始點(diǎn)、目標(biāo)點(diǎn)和閉環(huán)系統(tǒng)數(shù)學(xué)模型作為無人機(jī)路徑規(guī)劃的輸入條件，對路徑進(jìn)行初始化；
[0048]S33:設(shè)定單步路徑規(guī)劃周期為At(該周期根據(jù)無人機(jī)系統(tǒng)的實(shí)際計(jì)算能力、飛 行速度等確定，一般△t為秒的量級），在△t周期內(nèi)，通過步驟S1獲得無人機(jī)包括位置、速 度、姿態(tài)在內(nèi)的運(yùn)動(dòng)狀態(tài)估計(jì)值，記為x(t)，并根據(jù)步驟S16中建立的系統(tǒng)狀態(tài)空間模型預(yù) 測At時(shí)刻后的無人機(jī)狀態(tài)x(t+At);
[0049]S34:在同一個(gè)規(guī)劃周期At的時(shí)間內(nèi)，在S31獲得的體元形式的三維環(huán)境模型中 連續(xù)進(jìn)行隨機(jī)采樣，獲得一系列采樣點(diǎn)，利用路徑的代價(jià)以及定位和運(yùn)動(dòng)狀態(tài)估計(jì)可信度 作為啟發(fā)式信息，根據(jù)該啟發(fā)式信息在上一規(guī)劃周期中生成的、以步驟S33預(yù)測得到的無 人機(jī)狀態(tài)x(t+△t)為起點(diǎn)的路徑集合中選擇與每個(gè)采樣點(diǎn)對應(yīng)的最近點(diǎn)，利用步驟32中 建立的無人機(jī)的閉環(huán)系統(tǒng)數(shù)學(xué)模型預(yù)測無人機(jī)的狀態(tài)，生成由一系列可行路徑組成的路徑 集合，并計(jì)算每條可行路徑的代價(jià)以及定位和運(yùn)動(dòng)狀態(tài)估計(jì)可信度，從而對路徑連續(xù)進(jìn)行 增量式擴(kuò)展；
[0050] 所述路徑代價(jià)代表無人機(jī)沿路徑飛行所對應(yīng)的代價(jià)，如該路徑的總距離、無人機(jī) 沿該路徑飛行消耗的時(shí)間、燃料或電能等；
[0051] 所述路徑對應(yīng)的定位和運(yùn)動(dòng)狀態(tài)估計(jì)可信度的具體含義如下：由步驟1可知，無 人機(jī)從某路徑的起始點(diǎn)飛行到該路徑終點(diǎn)的過程中，利用機(jī)載RGB-D相機(jī)可獲取環(huán)境的圖 像和深度數(shù)據(jù)，在此基礎(chǔ)上通過濾波方法可獲得無人機(jī)的包括位置、速度、姿態(tài)在內(nèi)的運(yùn)動(dòng) 狀態(tài)估計(jì)值，所述運(yùn)動(dòng)狀態(tài)估計(jì)值的協(xié)方差陣的大小用于衡量無人機(jī)運(yùn)動(dòng)狀態(tài)估計(jì)的可信 度，因此路徑終點(diǎn)對應(yīng)的無人機(jī)運(yùn)動(dòng)狀態(tài)估計(jì)值本質(zhì)上為無人機(jī)沿該路徑飛行過程中通過 濾波方法獲得的后驗(yàn)運(yùn)動(dòng)狀態(tài)估計(jì)值，無人機(jī)運(yùn)動(dòng)狀態(tài)估計(jì)值的協(xié)方差陣的大小即為該路 徑對應(yīng)的定位和運(yùn)動(dòng)狀態(tài)估計(jì)可信度。
[0052] S35 :以路徑的代價(jià)以及定位和運(yùn)動(dòng)狀態(tài)估計(jì)可信度為選擇指標(biāo)，從步驟S34中生 成的當(dāng)前可行路徑集合中選擇最優(yōu)路徑，作為無人機(jī)的執(zhí)行指令，引導(dǎo)無人機(jī)按照該最優(yōu) 路徑飛行；循環(huán)執(zhí)行步驟S33和S34,直至無人機(jī)飛行到目標(biāo)點(diǎn)。
[0053] 上述基于RGB-D相機(jī)和MEMS慣性傳感器的微型無人機(jī)運(yùn)動(dòng)狀態(tài)估計(jì)流程，基于 機(jī)載RGB-D相機(jī)環(huán)境的二維彩色圖像和三維深度數(shù)據(jù)，通過特征點(diǎn)檢測、特征點(diǎn)匹配、運(yùn)動(dòng) 狀態(tài)估計(jì)等計(jì)算過程，可獲得穩(wěn)定、可靠的微型無人機(jī)的姿態(tài)和位置參數(shù)估計(jì)值；利用機(jī)載 RGB-D相機(jī)的運(yùn)動(dòng)狀態(tài)估計(jì)值和MEMS慣性傳感器提供的慣性測量值，實(shí)現(xiàn)了基于多傳感器 融合的無人機(jī)定位和運(yùn)動(dòng)狀態(tài)估計(jì)，可有效修正低成本MEMS器件的誤差和漂移，提高無人 機(jī)定位以及運(yùn)動(dòng)狀態(tài)估計(jì)的精度，可在室內(nèi)無GPS及外部定位輔助的特殊情況下，完全依 靠機(jī)載傳感器自主為微型無人機(jī)提供高精度、可靠的無人機(jī)運(yùn)動(dòng)狀態(tài)估計(jì)參數(shù)；
[0054] 上述基于RGB-D相機(jī)和MEMS慣性傳感器融合的三維環(huán)境實(shí)時(shí)建模流程，一方面通 過RGB-D運(yùn)動(dòng)狀態(tài)估計(jì)和MEMS慣性測量值的融合獲得更高精度的無人機(jī)位置和姿態(tài)估計(jì) 參數(shù)，并用于確定兩組三維點(diǎn)云之間的相對關(guān)系，實(shí)現(xiàn)三維點(diǎn)云的精確配準(zhǔn)和融合，相比于 單純采用圖像或三維深度數(shù)據(jù)進(jìn)行三維環(huán)境建模的方法，本發(fā)明提出的三維環(huán)境實(shí)時(shí)建模 流程可獲得更高精度的三維環(huán)境模型；另一方面，采用閉環(huán)檢測方法觸發(fā)式地對三維環(huán)境 模型進(jìn)行優(yōu)化，以抑制誤差的積累，同時(shí)觸發(fā)式的優(yōu)化方式也減少了整體的運(yùn)算量，避免了 對三維環(huán)境建模實(shí)時(shí)性的影響。
[0055] 上述實(shí)時(shí)可信路徑規(guī)劃與路徑跟蹤控制流程的優(yōu)點(diǎn)如下：采用基于隨機(jī)采樣的路 徑規(guī)劃架構(gòu)，可以有效適應(yīng)無人機(jī)的復(fù)雜動(dòng)力學(xué)和運(yùn)動(dòng)學(xué)約束，在大規(guī)模環(huán)境空間下降低 計(jì)算復(fù)雜度，提高計(jì)算效率，同時(shí)生成的路徑可保證路徑代價(jià)的最優(yōu)性；在路徑規(guī)劃過程中 引入包含控制；率的無人機(jī)閉環(huán)系統(tǒng)數(shù)學(xué)模型進(jìn)行狀態(tài)預(yù)測，可保證生成的路徑更接近無 人機(jī)實(shí)際物理特性，減少路徑跟蹤控制誤差；在路徑執(zhí)行的同時(shí)連續(xù)擴(kuò)展路徑，保證路徑 規(guī)劃的實(shí)時(shí)性，可有效應(yīng)對不確定和動(dòng)態(tài)環(huán)境；將傳感器特性和定位和運(yùn)動(dòng)狀態(tài)估計(jì)可信 度因素結(jié)合到路徑規(guī)劃中，使生成的路徑在滿足路徑代價(jià)約束的同時(shí)保證無人機(jī)的定位性 能。
[0056] 本發(fā)明的方法可利用無人機(jī)的機(jī)載計(jì)算處理單元，通過常規(guī)的編程技術(shù)實(shí)現(xiàn)實(shí)時(shí) 執(zhí)行上述方法中的各項(xiàng)運(yùn)算和處理過程。上述方法中通過RGB-D相機(jī)，用于獲取環(huán)境的RBG 圖像信息和三維深度參數(shù)；MEMS慣性測量單元，測量并輸出無人機(jī)當(dāng)前的角速度、加速度 等慣性測量參數(shù)。
[0057] 實(shí)施例：
[0058]S1基于RGB-D和MEMS傳感器的微型無人機(jī)運(yùn)動(dòng)狀態(tài)估計(jì)，如圖2所示，具體包括 以下步驟：
[0059]S11:通過RGB-D相機(jī)獲取環(huán)境的圖像和深度數(shù)據(jù)，包括一系列二維色彩域（RGB) 像素點(diǎn)構(gòu)成的二維彩色圖像以及對應(yīng)的三維深度域數(shù)據(jù)。
[0060] 對于三維環(huán)境中的某點(diǎn)Pi，RGB-D相機(jī)所獲得的信息形式如下：Pi = (Xi, yi, Zi, n，gi,bj，其中Xi, yp Zi為該點(diǎn)的三維深度數(shù)據(jù)，代表改點(diǎn)相對于RGB-D相機(jī)中心 的三維位置，gi，h為該點(diǎn)對應(yīng)的色彩域（RGB)信息。因此，某時(shí)刻RGB-D相機(jī)視場中的 所有環(huán)境點(diǎn)的所述如Pi形式的信息構(gòu)成了RGB-D相機(jī)在該時(shí)刻產(chǎn)生的一幀二維彩色圖像 和三維深度數(shù)據(jù)。為進(jìn)一步實(shí)現(xiàn)特征點(diǎn)提取，將所述二維彩色圖像轉(zhuǎn)換為二維灰度圖像，轉(zhuǎn) 換公式如下：
[0061] Gj = A A2gt+ X3bj
[0062] 其中入i，入2,A3為轉(zhuǎn)換系數(shù)，Gi為像素點(diǎn)轉(zhuǎn)換后對應(yīng)的灰度值。
[0063]S12:基于SURF(快速魯棒圖像特征法，已公開的通用方法）方法從步驟S11產(chǎn)生 的二維灰度圖像中通過灰度梯度極值識別并提取一組特征點(diǎn)，計(jì)算每個(gè)特征點(diǎn)鄰域像素的 灰度梯度方向，基于灰度梯度方向構(gòu)建特征點(diǎn)的SURF特征描述向量，本實(shí)施例中采用128 維的SURF特征描述向量。為減少錯(cuò)誤特征點(diǎn)，利用步驟S11中RGB-D相機(jī)獲取的三維深度 數(shù)據(jù)對提取的特征點(diǎn)進(jìn)行篩選，將無對應(yīng)深度數(shù)據(jù)的特征點(diǎn)剔除。
[0064]S13:對比步驟S12提取的相鄰兩幀圖像中特征點(diǎn)的特征向量，進(jìn)行特征匹配。在 本實(shí)施例中，對其中一幀圖像中的特征點(diǎn)，以步驟S12中計(jì)算的兩特征點(diǎn)的SURF特征向量 的歐式距離來作為特征相似的測度，通過k-近鄰搜索方法（通用方法）在相鄰幀圖像中尋 找與某特征點(diǎn)最相近的k個(gè)對應(yīng)特征點(diǎn)（本實(shí)施例中采用k= 2)。令Pi為所述一幀圖像 中的某個(gè)特征點(diǎn)，dndi2分別為特征點(diǎn)Pi與其最優(yōu)和次優(yōu)匹配點(diǎn)qn，qi2的歐式距離，為提高 匹配的準(zhǔn)確性，設(shè)定e2為閾值，當(dāng)dndi2滿足約束dn<epdii/duS￡2時(shí)，確認(rèn)qn 為Pi對應(yīng)的匹配特征點(diǎn)（本實(shí)施例中ei= 0. 5,e2 = 〇. 8)。最后，通過RANSAC(隨機(jī)一 致性采樣，已公開的通用方法）方法再剔除匹配有誤的特征點(diǎn)。
[0065] S14:根據(jù)步驟S11中獲取的特征點(diǎn)的三維深度數(shù)據(jù)，將S13產(chǎn)生的匹配后的特征 點(diǎn)映射到三維空間，獲得具有匹配關(guān)系的相鄰兩組三維點(diǎn)云。如步驟S11中所述，RGB-D中 獲取的單個(gè)點(diǎn)的參數(shù)如下：Pi= (Xi,yi,Zi,n，gi,bD。針對步驟S13中獲得的二維圖像匹配 點(diǎn)，從步驟S11獲得的三維深度數(shù)據(jù)中提取出相鄰兩組二維圖像匹配點(diǎn)對應(yīng)的三維深度數(shù) 據(jù)（Xi，yi，Zi)，并將提取出的三維深度數(shù)據(jù)組成兩組三維點(diǎn)云Pi={Pi}，Qi= {qj，即可將 兩組具有匹配關(guān)系的二維圖像特征點(diǎn)映射到三維空間，獲得具有匹配關(guān)系的相鄰兩組三維 點(diǎn)云：Pi= {pj，Qi= {qj，i= 1. ? ?n，p"q!G□ 3
[0066] S15:根據(jù)S14步驟獲得的相鄰兩組具有匹配關(guān)系的三維點(diǎn)云之間的旋轉(zhuǎn)和平 移關(guān)系，求解無人機(jī)的姿態(tài)和位置參數(shù)估計(jì)值。具體過程如下：對于步驟S14獲得的兩組 具有匹配關(guān)系的三維點(diǎn)云Pi= {pj，Qi= {qj，i= 1. ? ?n,其中Pi= (xn,xi2,xi3)T,qi= (ymymyn)1為三維點(diǎn)云中具有匹配關(guān)系的某一對特征點(diǎn)，在特征點(diǎn)嚴(yán)格匹配且不存在傳 感器誤差的理想情況下，二者之間的關(guān)系可表示為：
[0067] Qi = RPi+t
[0068] 其中RG□ 3X3代表三維點(diǎn)云在三維空間的旋轉(zhuǎn)，t代表三維點(diǎn)云在三維空間的平 移；以上關(guān)系從物理意義上即對應(yīng)相機(jī)（及其無人機(jī)）在采集兩幀三維點(diǎn)云數(shù)據(jù)時(shí)刻之間 的運(yùn)動(dòng)。因此通過計(jì)算連續(xù)兩幀三維點(diǎn)云之間的旋轉(zhuǎn)和平移關(guān)系即可估計(jì)無人機(jī)的相對運(yùn) 動(dòng)。
[0069] 在本實(shí)施例中，為提高計(jì)算精度和對噪聲的魯棒性，通過基于RANSAC架構(gòu)的迭代 方法求解最小二乘意義上的最優(yōu)R，t，使得以下誤差函數(shù)取最小值：
[0070]

【權(quán)利要求】
1. 一種微型無人機(jī)室內(nèi)自主導(dǎo)航方法，其特征在于，該方法包括以下步驟： S1:基于RGB-D相機(jī)和MEMS慣性傳感器的微型無人機(jī)運(yùn)動(dòng)狀態(tài)估計(jì)：利用RGB-D相 機(jī)獲取環(huán)境的二維彩色圖像和三維深度數(shù)據(jù)，通過特征點(diǎn)檢測、特征點(diǎn)匹配、運(yùn)動(dòng)狀態(tài)估計(jì) 獲得微型無人機(jī)的姿態(tài)和位置參數(shù)估計(jì)值，利用MEMS傳感器獲取慣性測量值，將所述姿態(tài) 和位置參數(shù)估計(jì)值與慣性測量值通過濾波方法進(jìn)行信息融合，用以抑制慣性測量值誤差積 累，得到更為精確的無人機(jī)包括位置、速度、姿態(tài)在內(nèi)的運(yùn)動(dòng)狀態(tài)估計(jì)值； S2:基于RGB-D相機(jī)和MEMS慣性傳感器融合的三維環(huán)境實(shí)時(shí)建模：利用步驟S1中 RGB-D相機(jī)獲取的三維深度數(shù)據(jù)，以及融合后的運(yùn)動(dòng)狀態(tài)估計(jì)值進(jìn)行三維點(diǎn)云的精確配準(zhǔn) 與融合，構(gòu)建三維環(huán)境模型，并觸發(fā)式地對該模型進(jìn)行全局優(yōu)化，用以提高模型精度； S3:實(shí)時(shí)可信路徑規(guī)劃與路徑跟蹤控制：設(shè)定規(guī)劃周期，在每個(gè)規(guī)劃周期中，以步驟S2 得到的三維環(huán)境模型為基礎(chǔ)，同時(shí)建立RGB-D相機(jī)的測量誤差模型和定位可信度的評價(jià)函 數(shù)，從而生成無人機(jī)從當(dāng)前點(diǎn)到目標(biāo)點(diǎn)的路徑，在每個(gè)規(guī)劃周期結(jié)束后，選擇當(dāng)前最優(yōu)路 徑執(zhí)行；根據(jù)所述當(dāng)前最優(yōu)路徑，與步驟S1得到運(yùn)動(dòng)狀態(tài)估計(jì)值進(jìn)行比較，得到當(dāng)前的位 置偏差，從而根據(jù)當(dāng)前位置偏差生成無人機(jī)的位置控制指令，根據(jù)位置控制指令解算為無 人機(jī)的姿態(tài)控制指令，實(shí)施位置控制和姿態(tài)控制，引導(dǎo)無人機(jī)跟蹤預(yù)定路徑飛行。
2. 如權(quán)利要求1所述方法，其特征在于，所述步驟S1中的基于RGB-D相機(jī)和MEMS慣性 傳感器的微型無人機(jī)運(yùn)動(dòng)狀態(tài)估計(jì)，具體包括以下步驟： 511 :通過RGB-D相機(jī)獲取當(dāng)前環(huán)境的二維彩色圖像與三維深度數(shù)據(jù)，并將該二維彩色 圖像進(jìn)行預(yù)處理，轉(zhuǎn)換為以灰度值表示的二維灰度圖像； 512 :從步驟S11預(yù)處理后的每幀二維灰度圖像中提取出一組特征點(diǎn)，并計(jì)算每一特征 點(diǎn)的特征描述向量，并通過步驟S11中獲取的三維深度數(shù)據(jù)對所述特征點(diǎn)進(jìn)行篩選，剔除 與三維深度數(shù)據(jù)無對應(yīng)的特征點(diǎn)； 513 :對相鄰兩幀圖像中的特征點(diǎn)進(jìn)行特征匹配，通過對比特征點(diǎn)的對應(yīng)描述向量獲得 兩組匹配的特征點(diǎn)，并再剔除匹配有誤的特征點(diǎn)； 514 :根據(jù)特征點(diǎn)對應(yīng)的三維深度數(shù)據(jù)，將步驟S13匹配后的特征點(diǎn)映射到三維空間， 獲得具有匹配關(guān)系的相鄰兩幀圖像中的兩組三維點(diǎn)云； 515 :求解步驟S14中獲得的該兩組匹配的三維點(diǎn)云之間的旋轉(zhuǎn)和平移參數(shù)，利用所述 旋轉(zhuǎn)和平移參數(shù)反算出無人機(jī)的姿態(tài)和位置參數(shù)估計(jì)值； 516 :根據(jù)無人機(jī)的動(dòng)力學(xué)特性建立狀態(tài)空間模型以及RGB-D相機(jī)、MEMS慣性傳感器的 觀測模型，將S15中得到的無人機(jī)的位置參數(shù)估計(jì)值作為系統(tǒng)的狀態(tài)空間模型的觀測量， 與MEMS慣性傳感器提供的由角速度、加速度、磁觀測量信息組成的慣性測量值通過濾波方 法進(jìn)行融合，進(jìn)一步修正慣性測量值誤差，以有效抑制慣性測量值誤差積累，獲得包括無人 機(jī)的位置、速度、姿態(tài)在內(nèi)的更為精確的最終運(yùn)動(dòng)狀態(tài)估計(jì)值。
3. 如權(quán)利要求2所述方法，其特征在于，所述步驟S2中基于RGB-D相機(jī)和MEMS慣性傳 感器融合的三維環(huán)境實(shí)時(shí)建模，具體包括以下步驟： 521 :利用如步驟S16獲得的RGB-D運(yùn)動(dòng)參數(shù)估計(jì)值與MEMS測量值融合后的運(yùn)動(dòng)狀態(tài) 估計(jì)值，以及步驟S14獲得的相鄰兩幀圖像中的兩組匹配的三維點(diǎn)云，求解所述兩組匹配 的三維點(diǎn)云之間的初始旋轉(zhuǎn)和平移關(guān)系； 522 :利用步驟S21中獲得的兩組三維點(diǎn)云及其初始旋轉(zhuǎn)和平移關(guān)系，定義目標(biāo)函數(shù)為 所所述兩組三維點(diǎn)云中的每對三維點(diǎn)云之間旋轉(zhuǎn)和平移誤差之和，通過最小二乘優(yōu)化迭代 方法優(yōu)化所述目標(biāo)函數(shù)，得到更為精確的三維點(diǎn)云之間的旋轉(zhuǎn)和平移關(guān)系； S23 :定義與環(huán)境固連的全局坐標(biāo)系，根據(jù)步驟S22得到的不同幀三維點(diǎn)云之間的相對 旋轉(zhuǎn)、平移關(guān)系，以及各幀三維點(diǎn)云與初始幀三維點(diǎn)云之間的旋轉(zhuǎn)和平移關(guān)系，將連續(xù)幀的 三維點(diǎn)云組合至統(tǒng)一的全局坐標(biāo)系中，得到三維環(huán)境模型； S24:采用通用的SLAM方法進(jìn)行閉環(huán)監(jiān)測，當(dāng)監(jiān)測到閉環(huán)時(shí)，觸發(fā)全局優(yōu)化過程，進(jìn)一 步校正不同幀三維點(diǎn)云之間的旋轉(zhuǎn)和平移關(guān)系，對步驟S23已經(jīng)建立的三維環(huán)境模型進(jìn)行 優(yōu)化，得到更為精確的的三維環(huán)境模型。
4.如權(quán)利要求3所述方法，其特征在于，所述步驟3中的實(shí)時(shí)可信路徑規(guī)劃與路徑跟蹤 控制，具體包括以下步驟： 531 :對步驟S2獲得的三維環(huán)境模型進(jìn)行后處理，將三維環(huán)境模型轉(zhuǎn)換為體元形式存 儲，以便于利用三維環(huán)境模型進(jìn)行路徑規(guī)劃； 532 :設(shè)定無人機(jī)的起始點(diǎn)和目標(biāo)點(diǎn)，根據(jù)步驟S16建立的狀態(tài)空間模型設(shè)計(jì)控制律， 建立無人機(jī)的閉環(huán)系統(tǒng)數(shù)學(xué)模型，將所述起始點(diǎn)、目標(biāo)點(diǎn)和閉環(huán)系統(tǒng)數(shù)學(xué)模型作為無人機(jī) 路徑規(guī)劃的輸入條件，對路徑進(jìn)行初始化； S33:設(shè)定單步路徑規(guī)劃周期為At，在At周期內(nèi)，通過步驟S1獲得無人機(jī)包括位置、 速度、姿態(tài)在內(nèi)的運(yùn)動(dòng)狀態(tài)估計(jì)值，記為X (t)，并根據(jù)步驟S16中建立的系統(tǒng)狀態(tài)空間模型 預(yù)測Λ t時(shí)刻后的無人機(jī)狀態(tài)X (t+ Λ t); 534 :在同一個(gè)規(guī)劃周期At的時(shí)間內(nèi)，在S31獲得的體元形式的三維環(huán)境模型中連 續(xù)進(jìn)行隨機(jī)采樣，獲得一系列采樣點(diǎn)，利用路徑的代價(jià)以及定位和運(yùn)動(dòng)狀態(tài)估計(jì)可信度作 為啟發(fā)式信息，根據(jù)該啟發(fā)式信息在上一規(guī)劃周期中生成的、以步驟S33預(yù)測得到的無人 機(jī)狀態(tài)x(t+At)為起點(diǎn)的路徑集合中選擇與每個(gè)采樣點(diǎn)對應(yīng)的最近點(diǎn)，利用步驟32中建 立的無人機(jī)的閉環(huán)系統(tǒng)數(shù)學(xué)模型預(yù)測無人機(jī)的狀態(tài)，生成由一系列可行路徑組成的路徑集 合，并計(jì)算每條可行路徑的代價(jià)以及定位和運(yùn)動(dòng)狀態(tài)估計(jì)可信度，從而對路徑連續(xù)進(jìn)行增 量式擴(kuò)展； 535 :以路徑的代價(jià)以及定位和運(yùn)動(dòng)狀態(tài)估計(jì)可信度為選擇指標(biāo)，從步驟S34中生成的 當(dāng)前可行路徑集合中選擇最優(yōu)路徑，作為無人機(jī)的執(zhí)行指令，引導(dǎo)無人機(jī)按照該最優(yōu)路徑 飛行；循環(huán)執(zhí)行步驟S33和S34,直至無人機(jī)飛行到目標(biāo)點(diǎn)。
【文檔編號】G01C21/18GK104236548SQ201410466305
【公開日】2014年12月24日 申請日期:2014年9月12日 優(yōu)先權(quán)日:2014年9月12日
【發(fā)明者】李大川, 李清, 唐良文, 楊盛, 程農(nóng) 申請人:清華大學(xué)