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

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

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