無人機室內(nèi)定位方法及系統(tǒng)的制作方法
【技術領域】
[0001 ] 本發(fā)明涉及室內(nèi)定位技術領域，尤其涉及一種無人機室內(nèi)定位方法及系統(tǒng)。
【背景技術】
[0002] 近十多年來，為解決定位"最后一公里"的問題，各科研機構以及創(chuàng)業(yè)型公司等在 室內(nèi)定位方面開展了大量研宄，旨在找到一套良好的室內(nèi)定位解決方案。
[0003] 目前，存在的定位系統(tǒng)主要有四種：
[0004] 第一種：基于GPS或北斗等的衛(wèi)星定位系統(tǒng)。但其主要應用的室外定位，不適用于 室內(nèi)定位，且其定位誤差較大。
[0005] 第二種：基于WIFI的定位。其便利性強，但由于信號在傳播中會收到折射、反射、 非視距因素、信號死角及2. 4GHz等高頻段干擾，定位誤差較大。
[0006] 第三種：基于藍牙的定位。其基于三角定位法，定位精準，但需要藍牙模塊及部署 藍牙基站，導致室內(nèi)定位成本較高。
[0007] 第四種：超寬帶室內(nèi)定位系統(tǒng)。其采用超寬帶脈沖信號，通過預設的傳感器對信號 標簽進行追蹤分析，多徑分辨能力強，精度高，但該類定位系統(tǒng)需要在室內(nèi)布置用于定位的 傳感器，成本高，定位范圍有限，可移植性差。
[0008] 綜上所述，現(xiàn)有的室內(nèi)定位技術在定位精度、成本、穩(wěn)定性、可移植等方面還無法 兼顧，造成室內(nèi)定位技術發(fā)展緩慢。
[0009] 近些年，隨著無人機技術的快速發(fā)展，無人機的應用領域也得到極大拓展。目前， 無人機定點定位飛行控制方面主要依賴傳統(tǒng)的GPS或第三方定位傳感器?；谇拔膶κ覂?nèi) 定位技術現(xiàn)狀的分析可知，無人機在室內(nèi)等實時定位飛行控制要求較高的地方仍然受到很 大技術局限性，其面臨的主要技術問題包括以下幾個方面：
[0010] 1、缺乏可用于準確判斷位置信息的可依賴特征。
[0011] 2、缺乏可用于準確估計位置信息和追蹤位置變化的方法。
[0012] 3、不能準確估計無人機空間六自由度定位信息。

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

[0013] 本發(fā)明所要解決的技術問題是，提供一種無人機室內(nèi)定位方法及系統(tǒng)，實現(xiàn)對無 人機進行室內(nèi)實時定位。本發(fā)明是這樣實現(xiàn)的：
[0014] -種無人機室內(nèi)定位方法，所述無人機上安裝有深度傳感器；所述定位方法包括 如下步驟：
[0015] 步驟A :利用所述深度傳感器按設定采集頻率實時采集室內(nèi)3D點云圖像，并檢測 其中的平面；
[0016] 步驟B :實時計算各幀3D點云圖像中的從各平面中預選出的預設數(shù)量的配準參考 平面的法向量和重心；各配準參考平面中的一個為定位基準平面；
[0017] 步驟C :實時根據(jù)當前幀與上一幀3D點云圖像中各配準參考平面的法向量和重心 計算所述深度傳感器從拍攝上一幀3D點云圖像時到拍攝當前幀3D點云圖像時的旋轉(zhuǎn)矩陣 和平移向量；
[0018] 步驟D :實時根據(jù)所述深度傳感器從拍攝上一幀3D點云圖像時到拍攝當前幀3D 點云圖像時的旋轉(zhuǎn)矩陣計算所述深度傳感器從拍攝上一幀3D點云圖像時到拍攝當前幀3D 點云圖像時的姿態(tài)角度變化量；
[0019] 步驟E :實時根據(jù)上一幀3D點云圖像中的所述定位基準平面的法向量和重心及所 述深度傳感器從拍攝上一幀3D點云圖像時到拍攝當前幀3D點云圖像時的姿態(tài)角度變化量 和平移向量計算在當前幀3D點云圖像中要將所述深度傳感器定位到所述定位基準平面的 重心所需要的姿態(tài)角度變化量和平移向量；
[0020] 步驟F :根據(jù)實時計算得出的在當前幀3D點云圖像中要將所述深度傳感器定位到 所述定位基準平面的重心所需要的姿態(tài)角度變化量和平移向量控制所述無人機向所述定 位基準平面的重心飛行；
[0021] 步驟G :當計算得出在當前幀3D點云圖像中要將所述深度傳感器定位到所述定位 基準平面的重心所需要的平移向量小于預設的第一閾值時，停止控制所述無人機向所述定 位基準平面的重心飛行，定位結束。
[0022] 進一步地，所述檢測其中的平面的步驟包括：
[0023] 將所述3D點云圖像劃分為若干點云單元；
[0024] 計算各點云單元的最小均方誤差，同時，剔除符合預設剔除條件的點云單元；
[0025] 建立基于剩余各點云單元的最小均方誤差的二叉堆排序，并選擇其中最小均方誤 差最小的點云單元作為膨脹起始節(jié)點；
[0026] 從所述膨脹起始節(jié)點開始進行膨脹生長以產(chǎn)生所述3D點云圖像中的各平面。
[0027] 進一步地，所述符合預設剔除條件的點云單元包括如下至少一種：
[0028] 最小均方誤差大于預設的第二閾值的點云單元；
[0029] 數(shù)據(jù)有丟失的點云單元；
[0030] 深度數(shù)據(jù)不連續(xù)的點云單元。
[0031] 進一步地，所述配準參考平面的法向量和重心的計算方法如下：
[0032] 提取各配準參考平面中的各點云單元進行主成分分析，并根據(jù)分析結果計算各配 準參考平面的法向量和重心。
[0033] 進一步地，所述步驟C包括：
[0034] 根據(jù)每一配準參考平面在當前幀與上一幀3D點云圖像中的法向量和重心分別計 算所述旋轉(zhuǎn)矩陣和平移向量；
[0035] 根據(jù)各配準參考平面的大小為根據(jù)每一配準參考平面在當前幀與上一幀3D點云 圖像中的法向量和重心計算出的所述旋轉(zhuǎn)矩陣和平移向量設定相應的權重；配準參考平面 越大，根據(jù)其在當前幀與上一幀3D點云圖像中的法向量和重心計算出的旋轉(zhuǎn)矩陣和平移 向量的權重越高；
[0036] 根據(jù)計算出的各旋轉(zhuǎn)矩陣和平移向量的權重對計算出的各旋轉(zhuǎn)矩陣和平移向量 進行加權求和，并將得到的最終的旋轉(zhuǎn)矩陣和平移向量作為所述深度傳感器從拍攝上一幀 3D點云圖像時到拍攝當前幀3D點云圖像時的旋轉(zhuǎn)矩陣和平移向量。
[0037] 一種無人機室內(nèi)定位系統(tǒng)，所述無人機上安裝有深度傳感器；所述定位系統(tǒng)包 括：
[0038] 平面檢測模塊，用于檢測利用所述深度傳感器按設定采集頻率實時采集的室內(nèi)3D 點云圖像中的平面；
[0039] 3D特征檢測模塊，用于實時計算各幀3D點云圖像中的從各平面中預選出的預設 數(shù)量的配準參考平面的法向量和重心；各配準參考平面中的一個為定位基準平面；
[0040] 幀間移動變量計算模塊，用于實時根據(jù)當前幀與上一幀3D點云圖像中各配準參 考平面的法向量和重心計算所述深度傳感器從拍攝上一幀3D點云圖像時到拍攝當前幀3D 點云圖像時的旋轉(zhuǎn)矩陣和平移向量；
[0041] 幀間姿態(tài)角度變化量計算模塊，用于實時根據(jù)所述深度傳感器從拍攝上一幀3D 點云圖像時到拍攝當前幀3D點云圖像時的旋轉(zhuǎn)矩陣計算所述深度傳感器從拍攝上一幀3D 點云圖像時到拍攝當前幀3D點云圖像時的姿態(tài)角度變化量；
[0042] 飛行控制參數(shù)計算模塊，用于實時根據(jù)上一幀3D點云圖像中的所述定位基準平 面的法向量和重心及所述深度傳感器從拍攝上一幀3D點云圖像時到拍攝當前幀3D點云圖 像時的姿態(tài)角度變化量和平移向量計算在當前幀3D點云圖像中要將所述深度傳感器定位 到所述定位基準平面的重心所需要的姿態(tài)角度變化量和平移向量；
[0043] 飛行控制模塊，用于根據(jù)實時計算得出的在當前幀3D點云圖像中要將所述深度 傳感器定位到所述定位基準平面的重心所需要的姿態(tài)角度變化量和平移向量控制所述無 人機向所述定位基準平面的重心飛行；
[0044] 所述飛行控制模塊還用于在當計算得出在當前幀3D點云圖像中要將所述深度傳 感器定位到所述定位基準平面的重心所需要的平移向量小于預設的第一閾值時，停止控制 所述無人機向所述定位基準平面的重心飛行，定位結束。
[0045] 進一步地，所述平面檢測模塊具體用于：
[0046] 將所述3D點云圖像劃分為若干點云單元；
[0047] 計算各點云單元的最小均方誤差，同時，剔除符合預設剔除條件的點云單元；
[0048] 建立基于剩余各點云單元的最小均方誤差的二叉堆排序，并選擇其中最小均方誤 差最小的點云單元作為膨脹起始節(jié)點；
[0049] 從所述膨脹起始節(jié)點開始進行膨脹生長以產(chǎn)生所述3D點云圖像中的各平面。
[0050] 進一步地，所述符合預設剔除條件的點云單元包括如下至少一種：
[0051] 最小均方誤差大于預設的第二閾值的點云單元；
[0052] 數(shù)據(jù)有丟失的點云單元；
[0053] 深度數(shù)據(jù)不連續(xù)的點云單元。
[0054] 進一步地，所述3D特征檢測模塊具體用于：
[0055] 提取各配準參考平面中的各點云單元進行主成分分析，并根據(jù)分析結果計算各配 準參考平面的法向量和重心。
[0056] 進一步地，所述幀間移動變量計算模塊具體用于：
[0057] 根據(jù)每一配準參考平面在當前幀與上一幀3D點云圖像中的法向量和重心分別計 算所述旋轉(zhuǎn)矩陣和平移向量；
[0058] 根據(jù)各配準參考平面的大小為根據(jù)每一配準參考平面在當前幀與上一幀3D點云 圖像中的法向量和重心計算出的所述旋轉(zhuǎn)矩陣和平移向量設定相應的權重；配準參考平面 越大，根據(jù)其在當前幀與上一幀3D點云圖像中的法向量和重心計算出的旋轉(zhuǎn)矩陣和平移 向量的權重越高；
[0059] 根據(jù)計算出的各旋轉(zhuǎn)矩陣和平移向量的權重對計算出的各旋轉(zhuǎn)矩陣和平移向量 進行加權求和，并將得到的最終的旋轉(zhuǎn)矩陣和平移向量作為所述深度傳感器從拍攝上一幀 3D點云圖像時到拍攝當前幀3D點云圖像時的旋轉(zhuǎn)矩陣和平移向量。
[0060] 與現(xiàn)有技術相比，本發(fā)明通過安裝在無人機上的深度傳感器實時采集室內(nèi)3D點 云圖像并檢測其中的平面，并利用平面的法向量和重心作為定位的依賴特征，相比依賴點 的三維特征進行定位穩(wěn)定性更高，可準確判斷出無人機的位置信息。同時，本發(fā)明通過幀間 各配準參考平面的法向量及重心實時計算幀間深度傳感器的位置變化信息，可實時準