[發(fā)明專利]一種微型無人機室內自主導航方法

有效

• 申請?zhí)枺?/td>: 201410466305.1
• 申請日: 2014-09-12
• 公開（公告）號: CN104236548B
• 公開（公告）日: 2017-04-05
• 發(fā)明（設計）人: 李大川;李清;唐良文;楊盛;程農
• 申請（專利權）人: 清華大學
• 主分類號: G01C21/18
• 分類號: G01C21/18
• 代理公司: 北京清亦華知識產權代理事務所(普通合伙)11201
• 代理人: 廖元秋

1.一種微型無人機室內自主導航方法,其特征在于，該方法包括以下步驟：

S1:基于RGB-D相機和MEMS慣性傳感器的微型無人機運動狀態(tài)估計：利用RGB-D相機獲取環(huán)境的二維彩色圖像和三維深度數(shù)據(jù)，通過特征點檢測、特征點匹配、運動狀態(tài)估計獲得微型無人機的姿態(tài)和位置參數(shù)估計值，利用MEMS傳感器獲取慣性測量值，將所述姿態(tài)和位置參數(shù)估計值與慣性測量值通過濾波方法進行信息融合，用以抑制慣性測量值誤差積累，得到更為精確的無人機包括位置、速度、姿態(tài)在內的運動狀態(tài)估計值；

S2:基于RGB-D相機和MEMS慣性傳感器融合的三維環(huán)境實時建模：利用步驟S1中RGB-D相機獲取的三維深度數(shù)據(jù)，以及融合后的運動狀態(tài)估計值進行三維點云的精確配準與融合，構建三維環(huán)境模型，并觸發(fā)式地對該模型進行全局優(yōu)化，用以提高模型精度；

S3:實時可信路徑規(guī)劃與路徑跟蹤控制：設定規(guī)劃周期,在每個規(guī)劃周期中，以步驟S2得到的三維環(huán)境模型為基礎，同時建立RGB-D相機的測量誤差模型和定位可信度的評價函數(shù),從而生成無人機從當前點到目標點的路徑，在每個規(guī)劃周期結束后，選擇當前最優(yōu)路徑執(zhí)行；根據(jù)所述當前最優(yōu)路徑，與步驟S1得到運動狀態(tài)估計值進行比較，得到當前的位置偏差,從而根據(jù)當前位置偏差生成無人機的位置控制指令，根據(jù)位置控制指令解算為無人機的姿態(tài)控制指令，實施位置控制和姿態(tài)控制，引導無人機跟蹤預定路徑飛行。

2.如權利要求1所述方法，其特征在于，所述步驟S1中的基于RGB-D相機和MEMS慣性傳感器的微型無人機運動狀態(tài)估計，具體包括以下步驟：

S11：通過RGB-D相機獲取當前環(huán)境的二維彩色圖像與三維深度數(shù)據(jù)，并將該二維彩色圖像進行預處理，轉換為以灰度值表示的二維灰度圖像；

S12：從步驟S11預處理后的每幀二維灰度圖像中提取出一組特征點，并計算每一特征點的特征描述向量，并通過步驟S11中獲取的三維深度數(shù)據(jù)對所述特征點進行篩選，剔除與三維深度數(shù)據(jù)無對應的特征點；

S13：對相鄰兩幀圖像中的特征點進行特征匹配，通過對比特征點的對應描述向量獲得兩組匹配的特征點,并再剔除匹配有誤的特征點；

S14：根據(jù)特征點對應的三維深度數(shù)據(jù)，將步驟S13匹配后的特征點映射到三維空間，獲得具有匹配關系的相鄰兩幀圖像中的兩組三維點云；

S15：求解步驟S14中獲得的該兩組匹配的三維點云之間的旋轉和平移參數(shù)，利用所述旋轉和平移參數(shù)反算出無人機的姿態(tài)和位置參數(shù)估計值；

S16：根據(jù)無人機的動力學特性建立狀態(tài)空間模型以及RGB-D相機、MEMS慣性傳感器的觀測模型，將S15中得到的無人機的位置參數(shù)估計值作為系統(tǒng)的狀態(tài)空間模型的觀測量，與MEMS慣性傳感器提供的由角速度、加速度、磁觀測量信息組成的慣性測量值通過濾波方法進行融合，進一步修正慣性測量值誤差，以有效抑制慣性測量值誤差積累，獲得包括無人機的位置、速度、姿態(tài)在內的更為精確的最終運動狀態(tài)估計值。

3.如權利要求2所述方法，其特征在于，所述步驟S2中基于RGB-D相機和MEMS慣性傳感器融合的三維環(huán)境實時建模，具體包括以下步驟：

S21：利用如步驟S16獲得的RGB-D運動參數(shù)估計值與MEMS測量值融合后的運動狀態(tài)估計值，以及步驟S14獲得的相鄰兩幀圖像中的兩組匹配的三維點云，求解所述兩組匹配的三維點云之間的初始旋轉和平移關系；

S22：利用步驟S21中獲得的兩組三維點云及其初始旋轉和平移關系，定義目標函數(shù)為所所述兩組三維點云中的每對三維點云之間旋轉和平移誤差之和，通過最小二乘優(yōu)化迭代方法優(yōu)化所述目標函數(shù)，得到更為精確的三維點云之間的旋轉和平移關系；

S23：定義與環(huán)境固連的全局坐標系，根據(jù)步驟S22得到的不同幀三維點云之間的相對旋轉、平移關系，以及各幀三維點云與初始幀三維點云之間的旋轉和平移關系，將連續(xù)幀的三維點云組合至統(tǒng)一的全局坐標系中，得到三維環(huán)境模型；

S24：采用通用的SLAM方法進行閉環(huán)監(jiān)測，當監(jiān)測到閉環(huán)時，觸發(fā)全局優(yōu)化過程，進一步校正不同幀三維點云之間的旋轉和平移關系，對步驟S23已經建立的三維環(huán)境模型進行優(yōu)化，得到更為精確的的三維環(huán)境模型。

4.如權利要求3所述方法，其特征在于，所述步驟3中的實時可信路徑規(guī)劃與路徑跟蹤控制，具體包括以下步驟：

S31：對步驟S2獲得的三維環(huán)境模型進行后處理，將三維環(huán)境模型轉換為體元形式存儲，以便于利用三維環(huán)境模型進行路徑規(guī)劃；

S32：設定無人機的起始點和目標點，根據(jù)步驟S16建立的狀態(tài)空間模型設計控制律，建立無人機的閉環(huán)系統(tǒng)數(shù)學模型，將所述起始點、目標點和閉環(huán)系統(tǒng)數(shù)學模型作為無人機路徑規(guī)劃的輸入條件，對路徑進行初始化；

S33：設定單步路徑規(guī)劃周期為Δt，在Δt周期內，通過步驟S1獲得無人機包括位置、速度、姿態(tài)在內的運動狀態(tài)估計值，記為x(t)，并根據(jù)步驟S16中建立的系統(tǒng)狀態(tài)空間模型預測Δt時刻后的無人機狀態(tài)x(t+Δt)；

S34：在同一個規(guī)劃周期Δt的時間內，在S31獲得的體元形式的三維環(huán)境模型中連續(xù)進行隨機采樣，獲得一系列采樣點，利用路徑的代價以及定位和運動狀態(tài)估計可信度作為啟發(fā)式信息，根據(jù)該啟發(fā)式信息在上一規(guī)劃周期中生成的、以步驟S33預測得到的無人機狀態(tài)x(t+Δt)為起點的路徑集合中選擇與每個采樣點對應的最近點，利用步驟32中建立的無人機的閉環(huán)系統(tǒng)數(shù)學模型預測無人機的狀態(tài)，生成由一系列可行路徑組成的路徑集合，并計算每條可行路徑的代價以及定位和運動狀態(tài)估計可信度，從而對路徑連續(xù)進行增量式擴展；

S35：以路徑的代價以及定位和運動狀態(tài)估計可信度為選擇指標，從步驟S34中生成的當前可行路徑集合中選擇最優(yōu)路徑，作為無人機的執(zhí)行指令，引導無人機按照該最優(yōu)路徑飛行；循環(huán)執(zhí)行步驟S33和S34，直至無人機飛行到目標點。