本發(fā)明涉及機器人控制領域，具體涉及一種全自動高精度室內(nèi)快速定位方法。

背景技術：

隨著現(xiàn)代經(jīng)濟的快速發(fā)展,人們的生活水平不斷提高，越來越多的個人/家庭用服務機器人走進千家萬戶，讓人們的家居生活更加舒適。據(jù)統(tǒng)計，2012年全球個人/家用服務機器人銷量約為300萬臺，較2011年增長20％，銷售額為12.21億美元；2014年全球個人/家用服務機器人銷量約為470萬臺，較2013年增長17.50％，銷售額約為22.5億美元。其中，家務機器人(主要包括吸塵機器人、除草機器人、泳池清理機器人、窗戶清洗機器人等)的銷量約為270萬臺，銷售額約為7.99億美元。娛樂休閑機器人(主要包括玩具機器人、教育訓練機器人、娛樂用途機器人等)銷量約為120萬臺；銷售額約為9.11億美元。根據(jù)IFR的數(shù)據(jù)預測，2015-2018年，未來清潔機器人會首先打入家庭市場，掃地機器人的全球銷量將達到2500萬臺，拖地機器人的銷量會達到49.6萬臺。未來三年內(nèi)，個人/家庭用服務機器人的全球銷量將高達2590萬臺，市場規(guī)模高達到122億美元，超過2014年市場規(guī)模的5倍。

個人/家庭用服務機器人的工作主要是在室內(nèi)環(huán)境下進行的，而室內(nèi)環(huán)境與戶外的空曠環(huán)境有著很大的不同，在室內(nèi)機器人無法正常接收衛(wèi)星定位信號和基站定位信號，使得定位的精度大大降低。同時，室內(nèi)的環(huán)境相對擁擠，家具等物品的擺放和變動更增加了室內(nèi)環(huán)境的復雜度和動態(tài)性。而在復雜的室內(nèi)環(huán)境下工作的機器人，對定位的精度和速度也有著更高的要求，相對成熟的室外定位技術并不能很好的滿足這些需求。為了讓個人/家庭用服務機器人更好地在室內(nèi)環(huán)境下正常工作，需要一種實用的室內(nèi)定位解決方案，要求其在精度、覆蓋范圍、可靠性、成本、功耗、可擴展性和響應時間這幾個方面都能夠滿足要求。

目前，普遍而公知的室內(nèi)定位方法主要有慣性導航定位方法、室內(nèi)無線定位方法、雙目視覺定位方法以及激光雷達定位方法等，現(xiàn)在存在的室內(nèi)定位與控制專利公開方案，在精度、覆蓋范圍、可靠性、成本、功耗、可擴展性和響應時間方面都存在一定的不足。

申請公布號CN104567931A采用的是IMU(Inertial Measurement Unit，慣性測量單元，包含三軸加速度計和三軸陀螺儀)與三軸磁力計相結合，構成AHRS(Attitude and Heading Reference System,航姿參考系統(tǒng))，通過匹配已知地圖，實現(xiàn)室內(nèi)定位功能。該方法提出了一種航向漂移誤差的消除方法，可有效解決由于靜止時陀螺儀不工作導致的航向漂移誤差，以此提高室內(nèi)慣性導航系統(tǒng)的精度。但是在目標長時間運動時，陀螺儀和加速度計估計誤差的積累仍然會對定位精度造成很大的影響。同時，該定位方法對地圖有一定的依賴性，當室內(nèi)環(huán)境發(fā)生變化時不能及時更新地圖，大大降低了實用性和靈活性。

申請公布號CN106125045A采用基于Wi-Fi(Wireless-Fidelity，無線保真)的自適應混合室內(nèi)定位方法，通過將接收點對應AP(Wireless Access Point，無線訪問接入點)的RSSI(Received Signal Strength Indication，接收的信號強度指示)值轉換為其到相應AP的距離，采用三邊定位算法計算測試點的三邊定位坐標，采用位置指紋定位算法計算測試點的指紋定位坐標，再根據(jù)樣本數(shù)據(jù)計算二者的權重系數(shù)進行融合，實現(xiàn)在減少室內(nèi)中心區(qū)域采樣數(shù)量的同時保持較高的定位精度。該方法中，信號的傳播會受到室內(nèi)結構布局、建筑及家具材料差異的影響，而信號的穿墻、折射、反射、繞射等傳播會直接產(chǎn)生NLOS(Non Line of Sight，非視距)誤差，同時信號強度也存在著不穩(wěn)定性，給出的定位精度低，大概能得到10ˉ20m的精度，有些情況可能更低。再者，加入指紋定位算法后，雖然可以使定位精度提高，達到3ˉ5m的精度，但由于系統(tǒng)依賴射頻信號強度的指紋數(shù)據(jù)庫,引入的指紋坐標數(shù)據(jù)庫需要進行大量的信號強度檢測，對于大規(guī)模的使用，數(shù)據(jù)庫大，產(chǎn)生和維護成本相對較高，布局成本也相應提高，同時也在一定程度上造成可移植性差。

申請公布號CN106204595A采用的方法是利用雙目攝像機覆蓋需要監(jiān)視的場面范圍，并將這些攝像機進行預標定，根據(jù)雙目視覺原理監(jiān)控恢復出每個雙目攝像機的視頻幀圖像對應的立體稠密點云，再將這些點云統(tǒng)一到三維世界坐標下，融合成全景監(jiān)視場景，最后將所有點云重建為表面三維模型，從而實現(xiàn)空間定位。該定位方法對攝像機的標定要求較高，不允許任意移動或者改變觀測方向及畫面變焦縮放，故操作性較低。同組的兩個攝像機在進行雙目定位時，要求兩幅圖像為同一時間采集的，故同步要求比較高。圖像在采集、預處理、匹配等階段對計算能力要求較高，同時雙目視覺算法復雜，進一步增加了計算難度，嚴重影響了定位速度。而攝像機為被動接收圖像信息，受光線影響比較大。

申請公布號CN106226780A采用的是二維激光雷達與二軸云臺相結合，對整個空間進行掃描定位的方法，將二軸云臺靠墻放置在中間位置，激光雷達置于云臺頂部，通過給云臺設置俯仰角0°～60°，激光掃描雷達在四維空間不斷掃描，對采集的數(shù)據(jù)進行處理，建立室內(nèi)環(huán)境模型，通過比對多組環(huán)境模型，得到多旋翼無人機相對于云臺和激光掃描雷達的空間三維位置信息。此定位方法適用于較為空曠，障礙物不多的室內(nèi)環(huán)境，且目前激光掃描雷達掃描頻率低，一般只有10Hz左右，掃描角度有限，價格高昂，無法滿足家用機器人的室內(nèi)定位要求。

綜上所述，雖然慣性導航定位方法、室內(nèi)無線定位方法、雙目視覺定位方法以及激光雷達定位方法可以在一定程度上實現(xiàn)室內(nèi)定位功能，但慣性導航定位方法存在定位精度低、可靠性低、靈活性低等問題，室內(nèi)無線定位方法存在定位精度低、可靠性低、響應時間長等問題，雙目視覺定位方法存在可擴展性低、響應時間長、抗干擾能力弱等問題，激光雷達定位方法存在覆蓋范圍小、響應時間長、成本高等問題。

技術實現(xiàn)要素：

本發(fā)明所要解決的主要技術問題是：為了克服現(xiàn)有室內(nèi)定位技術存在的不足，提供一種全自動高精度室內(nèi)快速定位方法，具備高精度、高可靠性、高可擴展性、高覆蓋范圍、低響應時間、低成本等特點。

為了解決上述的技術問題，本發(fā)明提供了一種全自動高精度室內(nèi)快速定位方法，包括一定位系統(tǒng)，所述定位系統(tǒng)包括：硬件機械部分和軟件系統(tǒng)；

所述硬件機械部分包括彩色圖像采集裝置、激光散斑投射裝置、激光散斑圖像采集裝置、云臺控制裝置、站點控制裝置、通信裝置、中央控制裝置組成；

軟件系統(tǒng)包括激光散斑投射系統(tǒng)、彩色圖像與激光散斑圖像采集系統(tǒng)、圖像處理系統(tǒng)、圖像匹配系統(tǒng)、模式識別系統(tǒng)、云臺控制系統(tǒng)、站點控制系統(tǒng)、通信系統(tǒng)、中央控制系統(tǒng)、路徑規(guī)劃與控制系統(tǒng)；

其中，硬件機械部分中：

彩色圖像采集裝置：采用高性能ARM處理器控制圖像傳感器對視野內(nèi)的彩色圖像信息進行采集，得到的信號經(jīng)過FPGA進行預處理；同時配置光敏元件和無級補光燈，在光線較弱時進行可控強度的補光；

激光散斑投射裝置：采用高性能ARM處理器控制紅外投影模塊投射特定的激光散斑圖像；

激光散斑圖像采集裝置：采用高性能ARM處理器控制紅外圖像傳感器對視野內(nèi)的激光散斑圖像信息進行采集，得到的信號經(jīng)過FPGA進行預處理，并與紅外投影模塊投射的激光散斑圖像進行比對，通過匹配標定的激光散斑圖像與深度信息的關系，得到深度圖信息；

云臺控制裝置：包括水平控制有感無刷電機、俯仰控制有感無刷電機、水平支撐軸承座、三軸加速度計與三軸陀螺儀、電機驅動模塊、控制單元；控制單元與集成光學成像系統(tǒng)站點的控制中心進行通信，根據(jù)控制中心的需要控制云臺水平轉動與俯仰轉動至特定角度，并反饋實際的轉動角度給控制中心；

通信裝置：集成光學成像系統(tǒng)站點的站點控制裝置與中央控制裝置通過Wi-Fi無線組網(wǎng)進行通信，中央控制裝置隨時添加和刪除一個或多個集成光學成像系統(tǒng)站點，實現(xiàn)室內(nèi)定位系統(tǒng)的高擴展性；中央控制裝置提供給個人/家庭用服務機器人Wi-Fi組網(wǎng)通信和藍牙通信兩種通信接口，個人/家庭用服務機器人根據(jù)實際需求進行選擇；

中央控制裝置：包括運行Linux操作系統(tǒng)的微型計算機、Wi-Fi路由模塊、藍牙模塊、人機交互模塊；微型計算機通過Wi-Fi組網(wǎng)通信技術與各站點進行通信，計算融合各站點定位信息，實現(xiàn)室內(nèi)定位系統(tǒng)的高覆蓋范圍。微型計算機與個人/家庭用服務機器人進行通信，根據(jù)個人/家庭用服務機器人的需求進行路徑規(guī)劃與控制；人機交互模塊提供用戶對室內(nèi)定位系統(tǒng)進行設置的友好接口；

其中，軟件系統(tǒng)部分中：

激光散斑投射系統(tǒng)：激光散斑圖像生成函數(shù)接口；

彩色圖像與激光散斑圖像采集系統(tǒng)：彩色圖像獲取接口、激光散斑圖像獲取接口；

圖像處理系統(tǒng)：FFT函數(shù)(Fast Fourier Transform，快速傅里葉變換)；

圖像匹配系統(tǒng)：SURF函數(shù)(Speeded Up Robust Features,加速穩(wěn)健特征)；

模式識別系統(tǒng)：BP神經(jīng)網(wǎng)絡；

云臺控制系統(tǒng)：卡爾曼濾波函數(shù)、四元數(shù)姿態(tài)解算函數(shù)；

中央控制系統(tǒng)：貝葉斯最佳融合函數(shù)；

路徑規(guī)劃與控制系統(tǒng)：Dijkstra函數(shù)、PI控制函數(shù)、PD控制函數(shù)；

采用上述定位系統(tǒng)進行定位方法，包括以下步驟：

步驟S1：集成光學成像系統(tǒng)站點獲取視野范圍圖像信息及記錄當前云臺偏轉角；

步驟S2：站點計算中心處理獲取的彩色圖像并進行SURF(Speeded Up Robust Features,加速穩(wěn)健特征)特征匹配識別，獲得室內(nèi)物體特征信息及二維位置信息；

步驟S3：站點計算中心解析獲取的激光散斑圖像，通過與事先標定得到的激光散斑圖像進行匹配，得到室內(nèi)物體的深度圖信息；

步驟S4：站點計算中心結合步驟S1-S3獲得的云臺偏轉角、室內(nèi)物體特征信息、二維位置信息以及深度圖信息，并結合該站點在空間中已知的位置信息，計算得到室內(nèi)物體在三維空間中的位置信息；

步驟S5：中央控制系統(tǒng)通過無線組網(wǎng)通信獲取多個站點信息，通過進行信息融合、修正與拼接，得到整個空間三維位置信息；

步驟S6：中央控制系統(tǒng)與目標機器人之間通過選定的無線通信方式進行通信，中央控制系統(tǒng)根據(jù)目標機器人工作需求，發(fā)送位置信息與控制命令給目標機器人；

所述步驟S1包括如下步驟：

步驟S11：云臺根據(jù)需要轉至一個特定角度G(α，β)；

步驟S12：彩色攝像機采集當前視野范圍內(nèi)的彩色圖像信息；

步驟S13：紅外投影機投射具有一定編碼信息的激光散斑圖像；

步驟S14：紅外攝像機采集視野范圍內(nèi)紅外投影機投射的圖像照射在室內(nèi)物體上后形成的激光散斑圖像。

所述步驟S2包括如下步驟：

步驟S21：使用FFT對圖像進行預處理；

步驟S22：使用SURF匹配算法進行特征匹配，具體包括如下步驟：通過計算Hessian矩陣的行列式和矩陣的跡來檢測特征點，通過計算歐式距離度量兩個特征點描述子的相似度，計算得到待配準圖上的特征點到參考圖像上所有特征點的歐氏距離集合；

步驟S23：通過對歐氏距離集合進行比較，計算得到最小歐氏距離和次最小歐氏距離，當最小歐氏距離和次最小歐氏距離的比值小于設定的閾值是，認為特征點與對應最小歐氏距離的特征點是匹配的，否則沒有點與該特征點相匹配；

步驟S24：由匹配得到的特征點結合匹配庫信息，設定特征標識符，并計算和標記二維空間信息。

所述步驟S4包括如下步驟：

步驟S41：事先通過使用標定算法針對彩色攝像機找到最優(yōu)的攝像機內(nèi)部參數(shù)和畸變向量，得到彩色攝像機矩陣，記為A；

步驟S42：事先通過使用標定算法針對紅外攝像機找到最優(yōu)的攝像機內(nèi)部參數(shù)和畸變向量，得到紅外攝像機矩陣，記為B；

步驟S43：彩色攝像機矩陣A和紅外攝像機矩陣B分別結合云臺角度G(α，β)，得到新的攝像機矩陣C和D；

步驟S44：根據(jù)三維情形下，標記的位置與其在二維空間的投影關系P＝A*[R|T]*M，其中：M表示三維空間的一個點；[R|T]表示一個[3|4]矩陣，該矩陣為一個歐氏空間變換；A表示攝像機矩陣或內(nèi)部參數(shù)矩陣；P表示M在屏幕上的投影；因此有M＝[R|T]-1*A-1*P，結合步驟S43得到的矩陣C和D，融合二者及站點在空間中的已知位置信息，計算得到特征室內(nèi)物體在三維空間中的位置信息；

所述步驟S5包括如下步驟：

步驟S51：中央控制系統(tǒng)通過無線組網(wǎng)通信獲取多個站點在步驟S4中得到的室內(nèi)特征物體在三維空間中的位置信息；

步驟S52：對多個站點獲取的室內(nèi)特征物體在三維空間中的位置信息進行互補濾波并使用貝葉斯最佳融合算法，消除因單個站點覆蓋范圍帶來的局限，得到特征物體在整個三維空間中的位置信息；

步驟S53：根據(jù)步驟S52得到的特征物體在整個三維空間中的位置信息；

所述步驟S6包括如下步驟：

步驟S61：中央控制系統(tǒng)與目標機器人之間通過選定的無線通信方式進行通信，獲取目標機器人的工作需求；

步驟S62：中央控制系統(tǒng)根據(jù)目標機器人當前位置與目標位置，結合空間中物體的分布狀態(tài)，進行路徑規(guī)劃；

步驟S63：中央控制系統(tǒng)通過無線通信將位置信息與路徑信息發(fā)送給目標機器人，控制目標機器人的運動。

相較于現(xiàn)有技術，本發(fā)明的技術方案具備以下有益效果：

智能軟件、硬件、機械三者高度集成化工作，在單個集成光學成像系統(tǒng)站點中，高度集成了彩色圖像采集處理及智能補光、激光散斑圖像投射與采集處理、二軸自動穩(wěn)定云臺控制與姿態(tài)反饋，集成光學成像系統(tǒng)站點能夠獨立完成局部區(qū)域的彩色圖像、激光散斑圖像的采集，彩色圖像采集與智能補光構成反饋環(huán)、激光散斑圖像投射與采集處理構成反饋環(huán)、二軸自動穩(wěn)定云臺控制與姿態(tài)反饋構成反饋環(huán)。同時，通過彩色圖像進行室內(nèi)物體的特征匹配識別，通過處理激光散斑圖像得到深度圖像，通過二軸自動穩(wěn)定云臺的姿態(tài)反饋得到站點當前的視角信息。不同于專利公開號CN105955258A公開的“基于Kinect傳感器信息融合的機器人全局柵格地圖構建方法”，在構建地圖時需要由機器人搭載Kinect傳感器在需要構建地圖的空間中運動，根據(jù)采集的環(huán)境信息建立局部柵格地圖，將地圖中的柵格分為占用、空閑和未知三種狀態(tài)，并通過讓機器人不斷探索未知狀態(tài)的柵格來逐步完善地圖，需要耗費大量的時間，已探知區(qū)域發(fā)生變化時也不能及時發(fā)現(xiàn)；類似地，專利公開號CN105904468A公開的“一種具有自主地圖構建和無線充電的多功能巡邏機器人”，也是采用了由機器人搭載Kinect傳感器采集環(huán)境圖像信息并由導航模塊進行自主地圖的構建，也存在著時效性的問題；本方案中集成光學成像系統(tǒng)通過結合三個反饋環(huán)，最大限度地擴大了單個集成光學成像系統(tǒng)站點的室內(nèi)定位覆蓋范圍，同時通過使用高性能的ARM+FPGA硬件電路，大大提高了圖像處理的能力，縮短了處理時間，提高了整個室內(nèi)定位系統(tǒng)的實時性。

不同于專利公開號CN106204595A公開的“一種基于雙目攝像機的機場場面三維全景監(jiān)視方法”，要求雙目攝像機是固定方式的，不能夠任意的運動，改變觀測方向及畫面變焦縮放，在使用兩套雙目攝像機時如果它們之間沒有重疊則需要每一套攝像機進行預先的配準到統(tǒng)一的機場世界坐標系下，站點的加入與退出都需要預先配準；本方案中多站點集群協(xié)同工作，充分利用成熟且廉價的Wi-Fi無線組網(wǎng)通信技術的優(yōu)勢，在保證中央控制系統(tǒng)與各集成光學成像系統(tǒng)站點通信帶寬的前提下，通過使用TCP/IP協(xié)議，使得集成光學成像系統(tǒng)站點加入與退出室內(nèi)定位系統(tǒng)時非常的便捷。當有新的集成光學成像系統(tǒng)站點加入時，中央控制系統(tǒng)能夠自動為新加入集成光學成像系統(tǒng)站點進行編號，同時通過使用貝葉斯最佳融合算法，對單目標進行多角度融合，并通過各集成光學成像系統(tǒng)站點的反饋信息，自動確定新加入集成光學成像系統(tǒng)站點的布局位置，并自動更新整個室內(nèi)定位系統(tǒng)的站點布局數(shù)據(jù)庫；當有集成光學成像系統(tǒng)站點因故障等因素退出時，中央控制系統(tǒng)能夠識別到通信失敗，同時標記故障的集成光學成像系統(tǒng)站點，并將其從整個室內(nèi)定位系統(tǒng)中去除，并自動更新整個室內(nèi)定位系統(tǒng)的站點布局數(shù)據(jù)庫，待故障集成光學成像系統(tǒng)站點恢復正常工作時，再將其按上述新加入集成光學成像系統(tǒng)站點加入系統(tǒng)時的方式進行處理。本方案的多站點集群協(xié)同工作模式，能夠最大程度地提高室內(nèi)定位系統(tǒng)的定位精度，同時也使系統(tǒng)具有更高的可靠性和更強的自適應能力。

不同于專利公開號CN106227212A公開的“基于柵格地圖和動態(tài)校準的精度可控室內(nèi)導航系統(tǒng)及方法”，導航系統(tǒng)終端與被導物體合為一體，導航時需要結合事先鋪好的柵格化地圖進行路徑規(guī)劃，路徑規(guī)劃原則為走直線、拐直角彎的原則，在沿規(guī)劃的路徑運動時，如遇到障礙物才標記位置并重新規(guī)劃路徑；本發(fā)明方案中，中央控制系統(tǒng)在對個人/家庭用服務機器人的通信中提供了Wi-Fi無線通信和藍牙無線通信兩種常用的通信方式，中央控制系統(tǒng)提供的人機交互方式，可以讓室內(nèi)定位系統(tǒng)在添加新的個人/家庭用服務機器人特征信息的同時，自動查詢個人/家庭用服務機器人生產(chǎn)廠家提供的相關數(shù)據(jù)，自動選擇通信方式并自動設置通信數(shù)據(jù)包，同時在對個人/家庭用服務機器人進行路徑規(guī)劃時，結合該個人/家庭用服務機器人的相關運動特性參數(shù)，為其進行更為合理的實時路徑規(guī)劃與控制。中央控制系統(tǒng)與個人/家庭用服務機器人相互獨立，使得個人/家庭用服務機器人可以精簡結構、降低功耗，而中央控制系統(tǒng)也可以同時為對臺個人/家庭用服務機器人通過路徑規(guī)劃與控制服務，大大提高了室內(nèi)定位系統(tǒng)的利用率和適用性。

對比現(xiàn)有的其他專利公開方案，在1.機械裝置方面：其他方案只有0維或者1維的運動掃描方式，掃描范圍窄，室內(nèi)定位覆蓋率低；而本方案采用二軸自動穩(wěn)定云臺，使集成光學成像系統(tǒng)站點能夠靈活穩(wěn)定的進行大角度(超過180°)的掃描，極大地提高提高了室內(nèi)定位的覆蓋率，最大程度的限制死角區(qū)域。2.傳感器方面：其他方案只使用單個慣性導航傳感器；只使用單個激光掃描雷達；只使用一對雙目視覺攝像機；這些都使其室內(nèi)定位系統(tǒng)在進行定位時容易因為單個傳感器的錯誤數(shù)據(jù)而導致定位錯誤，同時單個激光雷達和一對雙目視覺攝像機在對整個空間的室內(nèi)定位方面表現(xiàn)乏力，實時性不高；本系統(tǒng)通過采用紅外投影儀投射激光散斑、紅外攝像機采集激光散斑圖像并計算獲取深度圖像，結合二軸自動穩(wěn)定云臺，以及多站點集群協(xié)同工作的方式，使室內(nèi)定位系統(tǒng)在對整個空間進行室內(nèi)定位的覆蓋率和實時性得到很大提高，精度上每個站點的誤差小于±5cm，多個站點的結合可以使誤差在±1cm。3.路徑規(guī)劃方面：其他方案只進行了室內(nèi)定位，并沒有根據(jù)個人/家庭用服務機器人的工作需求進行路徑規(guī)劃，使得個人/家庭用服務機器人所走的路徑并不是最優(yōu)的；本系統(tǒng)使用根據(jù)個人/家庭用服務機器人的工作需求，采用根據(jù)室內(nèi)物體大小和布局密集程度進行加權的Dijkstra算法進行路徑規(guī)劃。

自動室內(nèi)定位系統(tǒng)控制方法創(chuàng)新在于：

不同于專利公開號CN103258188A公開的“基于跨平臺計算機視覺庫的移動目標物體檢測跟蹤方法”，PC機進行數(shù)據(jù)分析處理包括了OpenCV視覺算法庫、圖像平滑濾波處理、圖像閾值二值化處理、圖像背景實時更新、檢測運動目標物體輪廓、分割前景物和背景、跟蹤運動目標物體一系列需要大量復雜運算的處理算法；本發(fā)明方案使用FFT算法與SURF算法相結合的更為快捷有效的方式，能夠很好地降低噪聲信號的干擾，提高處理效率和系統(tǒng)的實時響應性。

不同于專利公開號CN105758397A公開的“一種飛行器攝像定位方法”，使用單組兩個攝像機進行圖像采集，為防止飛行器兩側攝像機采集圖像時刻的不同，兩臺攝像機需要接收同步信號后同時開始自動曝光，同時要求按特定結構布局為飛行器安裝激光目標源，操作復雜，適用性不高；本發(fā)明方案中，使用多站點集群協(xié)同工作方式，使用貝葉斯最佳融合算法對同目標進行多傳感器多角度融合，有效減少物體遮擋引起的誤判，不需要對目標進行特別改造，同時通過多組數(shù)據(jù)的融合，過濾誤差數(shù)據(jù)，使室內(nèi)定位的精確度更高。

不同于專利公開號CN106127117A公開的“基于雙目視覺快速高魯棒性識別、定位的自動跟隨行李箱”，下位機的控制算法采用基于速度和航向的綜合模糊控制，根據(jù)高精度識別定位后的行李箱相對主人的水平距離和兩者前后的偏移距離與給定跟隨保持相對距離作對比，用來分別對速度和航向進行模糊控制，該模糊控制需要建立一定的模型，并依靠經(jīng)驗確定模糊隸屬度；本發(fā)明方案中，使用基于參數(shù)自校定的PI控制的速度控制算法和基于參數(shù)自校定的PD控制的轉向控制算法，不需要對個人/家庭用服務機器人建立模型，只需要根據(jù)中央控制系統(tǒng)結合個人/家庭用服務機器人的工作需求規(guī)劃的路徑，就可以對個人/家庭用服務機器人沿指定路徑運動時的運動速度和轉向進行更為有效地控制，使個人/家庭用服務機器人能夠更為穩(wěn)定、可靠、安全地運動。

附圖說明

圖1是本發(fā)明的集成光學成像系統(tǒng)站點組成圖；

圖2是本發(fā)明的站點布局圖；

圖中：

1.彩色攝像機； 2.紅外投影儀； 3.紅外攝像機

4.俯仰云臺伺服電機； 5.水平云臺伺服電機； 6.軸承座；

7.集成光學成像系統(tǒng)站點； 8.掃地機器人； 9.人形機器人；

10.內(nèi)置式中央控制系統(tǒng)； 11.家具；

圖3是本發(fā)明的系統(tǒng)結構圖；

圖4是本發(fā)明的定位與控制過程。

具體實施方式

為能進一步了解本發(fā)明的發(fā)明內(nèi)容、特點及功效，茲例舉以下實施例，并配合附圖詳細說明如下：

一種全自動高精度室內(nèi)快速定位方法，包括一定位系統(tǒng)，所述定位系統(tǒng)包括：硬件機械部分和軟件系統(tǒng)；

所述硬件機械部分包括彩色圖像采集裝置、激光散斑投射裝置、激光散斑圖像采集裝置、云臺控制裝置、站點控制裝置、通信裝置、中央控制裝置組成；

軟件系統(tǒng)包括激光散斑投射系統(tǒng)、彩色圖像與激光散斑圖像采集系統(tǒng)、圖像處理系統(tǒng)、圖像匹配系統(tǒng)、模式識別系統(tǒng)、云臺控制系統(tǒng)、站點控制系統(tǒng)、通信系統(tǒng)、中央控制系統(tǒng)、路徑規(guī)劃與控制系統(tǒng)；

其中，硬件機械部分中：

彩色圖像采集裝置：采用高性能ARM處理器控制圖像傳感器對視野內(nèi)的彩色圖像信息進行采集，得到的信號經(jīng)過FPGA進行預處理；同時配置光敏元件和無級補光燈，在光線較弱時進行可控強度的補光；

激光散斑投射裝置：采用高性能ARM處理器控制紅外投影模塊投射特定的激光散斑圖像；

激光散斑圖像采集裝置：采用高性能ARM處理器控制紅外圖像傳感器對視野內(nèi)的激光散斑圖像信息進行采集，得到的信號經(jīng)過FPGA進行預處理，并與紅外投影模塊投射的激光散斑圖像進行比對，通過匹配標定的激光散斑圖像與深度信息的關系，得到深度圖信息；

云臺控制裝置：包括水平控制有感無刷電機、俯仰控制有感無刷電機、水平支撐軸承座、三軸加速度計與三軸陀螺儀、電機驅動模塊、控制單元；控制單元與集成光學成像系統(tǒng)站點的控制中心進行通信，根據(jù)控制中心的需要控制云臺水平轉動與俯仰轉動至特定角度，并反饋實際的轉動角度給控制中心；

通信裝置：集成光學成像系統(tǒng)站點的站點控制裝置與中央控制裝置通過Wi-Fi無線組網(wǎng)進行通信，中央控制裝置隨時添加和刪除一個或多個集成光學成像系統(tǒng)站點，實現(xiàn)室內(nèi)定位系統(tǒng)的高擴展性；中央控制裝置提供給個人/家庭用服務機器人Wi-Fi組網(wǎng)通信和藍牙通信兩種通信接口，個人/家庭用服務機器人根據(jù)實際需求進行選擇；

中央控制裝置：包括運行Linux操作系統(tǒng)的微型計算機、Wi-Fi路由模塊、藍牙模塊、人機交互模塊；微型計算機通過Wi-Fi組網(wǎng)通信技術與各站點進行通信，計算融合各站點定位信息，實現(xiàn)室內(nèi)定位系統(tǒng)的高覆蓋范圍。微型計算機與個人/家庭用服務機器人進行通信，根據(jù)個人/家庭用服務機器人的需求進行路徑規(guī)劃與控制；人機交互模塊提供用戶對室內(nèi)定位系統(tǒng)進行設置的友好接口；

其中，軟件系統(tǒng)部分中：

激光散斑投射系統(tǒng)：激光散斑圖像生成函數(shù)接口；

彩色圖像與激光散斑圖像采集系統(tǒng)：彩色圖像獲取接口、激光散斑圖像獲取接口；

圖像處理系統(tǒng)：FFT函數(shù)；

圖像匹配系統(tǒng)：SURF函數(shù)；

模式識別系統(tǒng)：BP神經(jīng)網(wǎng)絡；

云臺控制系統(tǒng)：卡爾曼濾波函數(shù)、四元數(shù)姿態(tài)解算函數(shù)；

中央控制系統(tǒng)：貝葉斯最佳融合函數(shù)；

路徑規(guī)劃與控制系統(tǒng)：Dijkstra函數(shù)、PI控制函數(shù)、PD控制函數(shù)；

采用上述定位系統(tǒng)進行定位方法，包括以下步驟：

步驟S1：集成光學成像系統(tǒng)站點獲取視野范圍圖像信息及記錄當前云臺偏轉角；

步驟S2：站點計算中心處理獲取的彩色圖像并進行SURF特征匹配識別，獲得室內(nèi)物體特征信息及二維位置信息；

步驟S3：站點計算中心解析獲取的激光散斑圖像，通過與事先標定得到的激光散斑圖像進行匹配，得到室內(nèi)物體的深度圖信息；

步驟S4：站點計算中心結合步驟S1-S3獲得的云臺偏轉角、室內(nèi)物體特征信息、二維位置信息以及深度圖信息，并結合該站點在空間中已知的位置信息，計算得到室內(nèi)物體在三維空間中的位置信息；

步驟S5：中央控制系統(tǒng)通過無線組網(wǎng)通信獲取多個站點信息，通過進行信息融合、修正與拼接，得到整個空間三維位置信息；

步驟S6：中央控制系統(tǒng)與目標機器人之間通過選定的無線通信方式進行通信，中央控制系統(tǒng)根據(jù)目標機器人工作需求，發(fā)送位置信息與控制命令給目標機器人；

所述步驟S1包括如下步驟：

步驟S11：云臺根據(jù)需要轉至一個特定角度G(α，β)；

步驟S12：彩色攝像機采集當前視野范圍內(nèi)的彩色圖像信息；

步驟S13：紅外投影機投射具有一定編碼信息的激光散斑圖像；

步驟S14：紅外攝像機采集視野范圍內(nèi)紅外投影機投射的圖像照射在室內(nèi)物體上后形成的激光散斑圖像。

所述步驟S2包括如下步驟：

步驟S21：使用FFT對圖像進行預處理；

步驟S22：使用SURF匹配算法進行特征匹配，具體包括如下步驟：通過計算Hessian矩陣的行列式和矩陣的跡來檢測特征點，通過計算歐式距離度量兩個特征點描述子的相似度，計算得到待配準圖上的特征點到參考圖像上所有特征點的歐氏距離集合；

步驟S23：通過對歐氏距離集合進行比較，計算得到最小歐氏距離和次最小歐氏距離，當最小歐氏距離和次最小歐氏距離的比值小于設定的閾值是，認為特征點與對應最小歐氏距離的特征點是匹配的，否則沒有點與該特征點相匹配；

步驟S24：由匹配得到的特征點結合匹配庫信息，設定特征標識符，并計算和標記二維空間信息。

所述步驟S4包括如下步驟：

步驟S41：事先通過使用標定算法針對彩色攝像機找到最優(yōu)的攝像機內(nèi)部參數(shù)和畸變向量，得到彩色攝像機矩陣，記為A；

步驟S42：事先通過使用標定算法針對紅外攝像機找到最優(yōu)的攝像機內(nèi)部參數(shù)和畸變向量，得到紅外攝像機矩陣，記為B；

步驟S43：彩色攝像機矩陣A和紅外攝像機矩陣B分別結合云臺角度G(α，β)，得到新的攝像機矩陣C和D；

步驟S44：根據(jù)三維情形下，標記的位置與其在二維空間的投影關系P＝A*[R|T]*M，其中：M表示三維空間的一個點；[R|T]表示一個[3|4]矩陣，該矩陣為一個歐氏空間變換；A表示攝像機矩陣或內(nèi)部參數(shù)矩陣；P表示M在屏幕上的投影；因此有M＝[R|T]-1*A-1*P，結合步驟S43得到的矩陣C和D，融合二者及站點在空間中的已知位置信息，計算得到特征室內(nèi)物體在三維空間中的位置信息；

所述步驟S5包括如下步驟：

步驟S51：中央控制系統(tǒng)通過無線組網(wǎng)通信獲取多個站點在步驟S4中得到的室內(nèi)特征物體在三維空間中的位置信息；

步驟S52：對多個站點獲取的室內(nèi)特征物體在三維空間中的位置信息進行互補濾波并使用貝葉斯最佳融合算法，消除因單個站點覆蓋范圍帶來的局限，得到特征物體在整個三維空間中的位置信息；

步驟S53：根據(jù)步驟S52得到的特征物體在整個三維空間中的位置信息；

所述步驟S6包括如下步驟：

步驟S61：中央控制系統(tǒng)與目標機器人之間通過選定的無線通信方式進行通信，獲取目標機器人的工作需求；

步驟S62：中央控制系統(tǒng)根據(jù)目標機器人當前位置與目標位置，結合空間中物體的分布狀態(tài)，進行路徑規(guī)劃；

步驟S63：中央控制系統(tǒng)通過無線通信將位置信息與路徑信息發(fā)送給目標機器人，控制目標機器人的運動。

實施例1

參見圖1-圖4，自動室內(nèi)定位系統(tǒng)，包括：7.集成光學成像系統(tǒng)站點裝置；10.內(nèi)置式中央控制系統(tǒng)裝置。所述內(nèi)置式中央控制系統(tǒng)裝置10與多個集成光學成像系統(tǒng)站點裝置7相連。

所述集成光學成像系統(tǒng)站點裝置7包括：1.彩色攝像機；2.紅外投影儀；3紅外攝像機；4俯仰云臺伺服電機；5.水平云臺伺服電機；6.軸承座；以及集成光學成像系統(tǒng)站點裝置7內(nèi)的電源模塊、通信模塊、處理模塊，所述電源模塊與通信模塊、處理模塊相連。

所述集成光學成像系統(tǒng)站點裝置具體工作步驟如下：

對單個集成光學成像系統(tǒng)站點進行自動標定，其中包括：對彩色攝像機的參數(shù)標定、對紅外攝像機采集的激光散斑與紅外投影儀投射的激光散斑對應于深度信息的標定、云臺轉角與彩色攝像機融合參數(shù)的標定、云臺轉角與紅外攝像機融合參數(shù)的標定、集成光學成像系統(tǒng)站點覆蓋范圍的標定。

根據(jù)室內(nèi)空間大小、單個集成光學成像系統(tǒng)站點的覆蓋范圍以及實際需求，使用多個集成光學成像系統(tǒng)站點進行布局，覆蓋整個空間范圍，圖2為其中一種布局方式。

各集成光學成像系統(tǒng)站點通過Wi-Fi無線組網(wǎng)通信，接收中央控制系統(tǒng)發(fā)送的進行異步圖像采集命令，開始進行圖像采集。

各集成光學成像系統(tǒng)站點通過實時檢測環(huán)境光照強度，實時修正補光燈的光照強度，提高系統(tǒng)適應性。

各集成光學成像系統(tǒng)站點對自身采集的彩色圖像信息進行預處理以及SURF特征匹配識別和模式識別、對自身采集的激光散斑圖像信息進行深度圖信息變換、對自身采集的云臺轉角信息進行融合。

各集成光學成像系統(tǒng)站點將自身計算得到的室內(nèi)物體在三維空間中的位置信息通過無線通信傳輸給中央控制系統(tǒng)。

所述中央控制系統(tǒng)裝置具體工作步驟如下：

中央控制系統(tǒng)通過人機交互進行相關設置并獲取目標機器人特征信息。

中央控制系統(tǒng)通過Wi-Fi無線組網(wǎng)與各集成光學成像系統(tǒng)站點進行通信，在有新的集成光學成像系統(tǒng)站點加入組網(wǎng)時，為新加入組網(wǎng)的站點分配標識號。

中央控制系統(tǒng)將目標機器人特征信息下發(fā)給各集成光學成像系統(tǒng)站點，更新其特征數(shù)據(jù)庫。

中央控制系統(tǒng)控制各集成光學成像系統(tǒng)站點進行異步圖像采集，消除各集成光學成像系統(tǒng)站點間的相互干擾。

中央控制系統(tǒng)通過無線通信獲取各集成光學成像系統(tǒng)站點傳輸過來的各自計算得到的室內(nèi)物體在三維空間中的位置信息。

中央控制系統(tǒng)通過進行信息融合、修正與拼接，得到整個空間三維位置信息。

中央控制系統(tǒng)與目標機器人之間通過選定的無線通信方式進行通信，中央控制系統(tǒng)根據(jù)目標機器人工作需求和當前位置信息，為目標機器人進行路徑規(guī)劃。

中央控制系統(tǒng)通過無線通信將位置信息與路徑信息發(fā)送給目標機器人，通過PI速度控制算法和PD轉向控制算法控制目標機器人的運動。

通過中央控制系統(tǒng)與一個或多個集成光學成像系統(tǒng)站點的協(xié)同工作，完成本系統(tǒng)對室內(nèi)物體的高精度快速定位功能，以及對目標機器人的控制功能。

以上所述，僅為本發(fā)明較佳的具體實施方式，但本發(fā)明的設計構思并不局限于此，任何熟悉本技術領域的技術人員在本發(fā)明揭露的技術范圍內(nèi)，利用此構思對本發(fā)明進行非實質(zhì)性的改動，均屬于侵犯本發(fā)明保護范圍的行為。