一種基于無線定位和激光地圖匹配的位姿解算方法
【技術領域】
[0001 ]本發明涉及室內定位、移動機器人和無人駕駛汽車自主定位導航技術領域，具體 涉及一種基于無線定位和激光地圖匹配的位姿解算方法。
【背景技術】
[0002] 在位置服務應用中，服務的內容與模式都是與服務對象的位置密切相關的，因此 對服務對象進行定位是位置服務應用首先要解決的問題。在室外環境下，目前可行的、常用 的方案是采用衛星定位。但是，在室內環境或衛星定位無法正常工作的地方，必須采用其它 方法來實現定位，這就是目前室內定位技術研究的內容，該研究的目標是解決定位導航的 最后一公里問題，實現全地域的精確定位。
[0003] 移動機器人要走向實用，必須擁有滿足地形要求的運動通行能力、準確高效的定 位導航能力和靈敏可靠的環境感知能力。精確的位置和姿態信息是保證移動機器人正確完 成導航和控制任務的關鍵點之一。常用的定位方法有：里程計、慣性導航、磁羅盤、主動燈 塔、衛星定位系統、視覺導航、路標導航、地圖模型匹配和仿生導航技術等。每種定位技術都 有各自的優點和局限性，一般在實際應用中都是綜合使用其中的幾種，通過有效的信息融 合技術，實現優缺點互補，以提高定位的精度、連續性和可靠性。
[0004] 隨著無線通信技術的發展和移動通信設備的廣泛應用，出現了利用無線通信技術 來實現定位的技術，即無線定位技術。常用的無線定位技術有:手機基站輔助定位(A-GPS)、 Wifi定位、藍牙iBeacon定位、UWB定位等。定位原理主要有四種：T0A(Time of Arrival data fusion，基于到達時間的數據融合）、A0A(Angle of Arrival data fusion，基于到達 角度的數據融合）、RSSI(接收信號強度衰減)和信號強度指紋。移動機器人應用對定位精度 要求較高，在上述無線定位技術中定位精度最高的是UWB定位技術，它理想的定位精度可以 達到0.1米，在我們所做的實際測試中，證明其定位精度可以達到0.3米，這個精度還不能滿 足移動機器人自主導航系統的要求。
[0005] 得益于低成本激光雷達設備的產業化，可以利用激光測距精度高的特點，把激光 掃描數據與無線定位結果進行有效融合，從而實現高精度的定位。把激光掃描數據與全局 地圖進行匹配，能夠精確計算出偏航角，對于在平面中移動的機器人而言，這就實現了對其 姿態的解算。

【發明內容】

[0006] 針對現有技術中存在的技術問題，利用激光測距精度高的特點，把激光掃描數據 與無線定位結果進行有效融合，從而實現高精度的定位。把激光掃描數據與全局地圖進行 匹配，能夠精確計算出偏航角，對于在平面中移動的機器人而言，這就實現了對其姿態的解 算，即本發明提供的這種基于無線定位和激光地圖匹配的位姿解算方法，該方法既避免了 無線定位精度低的缺點，又避免了激光掃描定位易受環境變化的干擾而發生定位位置跳變 的情況。
[0007] 為了實現上述目的，本發明采用如下技術方案。
[0008] 一種基于無線定位和激光地圖匹配的位姿解算方法，該方法包括如下步驟：
[0009] 步驟1，把至少三個無線定位錨點部署在環境中的不同位置，以確保所有無線定位 錨點不在空間形成一條直線；
[0010] 步驟2,使用安裝了激光掃描儀和無線定位標簽的定位設備，基于SLAM同步定位與 地圖構建算法（Simultaneous Localization and Mapping，SLAM)構建全局激光地圖，同時 記錄定位設備當前的位置和定位設備與各無線定位錨點之間的距離；
[0011] 步驟3,利用步驟2得到的定位設備位置數據與各無線定位錨點之間的距離數據， 計算無線定位錨點的位置；
[0012] 步驟4,利用步驟2得到的全局激光地圖，計算所有無障礙物像素點的模擬激光掃 描數據；
[0013] 步驟5,在對定位設備進行定位時，首先測量無線定位標簽與各無線定位錨點之間 的距離，再通過無線定位算法計算無線定位標簽的位置；
[0014] 步驟6,根據無線定位標簽在定位設備上的安裝位置和無線定位誤差大小，以步驟 5計算出的無線定位標簽的位置為中心，估計出定位設備的備選位置區域；
[0015] 步驟7,測量一幀激光掃描數據，在備選位置區域中的每一個無障礙物像素點處， 把測得的激光掃描數據與該像素點處的模擬激光掃描數據進行匹配，得到偏航角和匹配距 離；
[0016] 步驟8,選出激光掃描數據匹配距離最小的像素點，其對應的位置即為定位設備當 前位置，在該像素點通過激光掃描數據匹配得到的偏航角即為定位設備的偏航角。
[0017]優選的是，所述無線定位標簽的位置，以回歸法估計出的每個無線定位標簽的位 置為初始值，以計算距離與測量距離的均方誤差最小為準則，利用梯度下降法，精確計算每 個無線定位標簽的位置。
[0018] 在上述任一技術方案中優選的是，所述測量無線定位標簽與各無線定位錨點之間 的距離，在每一個采樣時刻，測量無線定位標簽到每個無線定位錨點的距離。
[0019] 在上述任一技術方案中優選的是，所述每一個采樣時刻的測量無線定位標簽到每 個無線定位錨點的距離，如果每個無線定位錨點的個數大于3個，則從中隨機選擇3個:如果 滿足平面三點定位要求，則利用三點定位法計算無線定位標簽的位置;否則再次隨機選擇， 直到計算出無線定位標簽的位置。
[0020] 在上述任一技術方案中優選的是，如步驟6所述的估計定位設備備選位置區域，以 無線定位標簽的位置為中心，以無線定位誤差與無線定位標簽偏離定位設備中心點的距離 的和為半徑，構造一個圓形的候選定位區域，在后續步驟中，激光匹配定位將在在候選定位 區域中進行。
[0021] 在上述任一技術方案中優選的是，如步驟4所述的計算模擬激光掃描數據，把全局 地圖中的候選定位點找出來，并通過仿真的方式把各定位點的模擬激光掃描數據算出來， 作為地圖匹配定位時的使用。
[0022] 在上述任一技術方案中優選的是，根據全局地圖統計候選定位點時，可以把地圖 中所有的無障礙物像素點作為候選定位點。
[0023] 在上述任一技術方案中優選的是，該方法通過計算各偏航角情況下激光掃描數據 與備選位置區域的各像素點的模擬激光數據的最小平均距離差，來確定定位設備的當前位 置像素和偏航角。
[0024]本發明的基于無線定位和激光地圖匹配的位姿解算方法，把無線定位方法與激光 掃描定位相結合，實現了一種不依賴于衛星定位系統的室內高精度定位方法，該方法同時 也實現了對定位設備偏航角的計算。該方法既避免了無線定位精度低的缺點，又避免了激 光掃描定位易受環境變化的干擾而發生定位位置跳變的情況。本發明實現的高精度位姿計 算方法，可廣泛應用于位置服務和移動機器人自主導航領域，是位置服務應用和移動機器 人的關鍵核心技術之一。
【附圖說明】
[0025]圖1為按照本發明的基于無線定位和激光地圖匹配的位姿解算方法的一優選實施 例的系統設備架構不意圖；
[0026] 圖2為按照本發明的基于無線定位和激光地圖匹配的位姿解算方法的一優選實施 例的方法流程框圖。
【具體實施方式】
[0027] 下面結合附圖和【具體實施方式】對本發明作詳細說明，以下描述僅作為示范和解 釋，并不對本發明作任何形式上的限制。
[0028] 基于無線定位和激光地圖匹配的位姿解算方法包括如下步驟：
[0029] 步驟1:把至少三個無線定位錨點部署在環境中的不同位置，確保所有無線定位錨 點不在空間形成一條直線；
[0030] 步驟2:使用安裝了激光掃描儀和無線定位標簽的定位設備，基于同步定位與地圖 構建算法（Simultaneous Locali