laser_scan_num的距離數(shù)組，表示激光掃描 儀按其掃描方向在laser_scan_num個(gè)角度上檢測到的障礙物距離，無效角度或無效距離的 距離數(shù)據(jù)為〇。
[0074] 全局地圖數(shù)據(jù)定義:地圖的長寬為map_sizexmap_size(像素），每個(gè)像素代表的物 理長度為pixel_si Ze(米），地圖數(shù)據(jù)用灰度圖表示，值為255的像素點(diǎn)表示該位置無障礙 物，值為〇的像素點(diǎn)表示是該位置是障礙物，其它值的像素點(diǎn)表示該位置為未探測環(huán)境。
[0075] 由于小車的位置和角度的不確定性，根據(jù)激光掃描數(shù)據(jù)繪出的局部地圖，與全局 地圖進(jìn)行匹配時(shí)，既包含平移變換，又包含旋轉(zhuǎn)變換，因此計(jì)算量較大。為了節(jié)省計(jì)算量，可 以把全局地圖中的候選定位點(diǎn)找出來，并通過仿真的方式把各定位點(diǎn)的模擬激光掃描數(shù)據(jù) 算出來，然后在地圖匹配定位的時(shí)候使用。
[0076] 根據(jù)全局地圖統(tǒng)計(jì)候選定位點(diǎn)時(shí)，一般把地圖中所有的無障礙物像素點(diǎn)作為候選 定位點(diǎn)。為了提高仿真數(shù)據(jù)的計(jì)算準(zhǔn)確度，并降低候選定位點(diǎn)數(shù)，可以預(yù)先對地圖做如下處 理:把障礙物邊界線做膨脹處理，把不期望作為候選定位點(diǎn)的像素改成不確定環(huán)境值，即改 成灰度像素即可。
[0077]在計(jì)算模擬激光掃描數(shù)據(jù)時(shí)，假設(shè)全局地圖是一個(gè)封閉的區(qū)域，同時(shí)不考慮激光 的作用范圍，那么模擬激光數(shù)據(jù)在laser_scan_num個(gè)角度上都有有效距離數(shù)據(jù)。注意，在模 擬激光掃描數(shù)據(jù)中的距離值是地圖坐標(biāo)系下的值，與世界坐標(biāo)系中的距離換算時(shí)，需要乘 以pixelsize。在這里就不用換算了，因?yàn)楹罄m(xù)的地圖匹配也是在地圖坐標(biāo)系中進(jìn)行。
[0078] 6.激光掃描數(shù)據(jù)與模擬激光數(shù)據(jù)的匹配：
[0079] 通過計(jì)算各偏航角情況下激光掃描數(shù)據(jù)與候選位置區(qū)域的各像素點(diǎn)的模擬激光 數(shù)據(jù)的最小平均距離差，來確定定位設(shè)備的當(dāng)前位置像素和偏航角。激光掃描數(shù)據(jù)的數(shù)學(xué) 描述：
[0080] 有效數(shù)據(jù)集：
[0081 ] I = {i ,0 < i<laser_scan_num&laser_data[i] >0}
[0082] 無效數(shù)據(jù)集：
[0083] J= {j ,0 < j<laser_scan_num&laser_data[ j] =0}
[0084] 假定偏航角為k時(shí)的激光掃描數(shù)據(jù)與候選定位點(diǎn)(x，y)的模擬激光數(shù)據(jù)匹配距離：
[0085] match _dist\y][x]f Α·] = V [laser _scdn[}\- candi_lmer _Sca/j[i-][χ][? Θ A'jj
[0086] 其中：
[0087]
[0088] 7 .在候選位置附近進(jìn)行地圖匹配定位算法：
[0089] 通過上述無線定位方法得到了定位設(shè)備的候選位置。
[0090] 在每個(gè)候選位置，計(jì)算當(dāng)前激光掃描數(shù)據(jù)與該位置的模擬激光掃描數(shù)據(jù)的最佳匹 配角度和匹配距離。
[0091] 選擇匹配距離最小的位置及其最佳匹配角度作為定位設(shè)備當(dāng)前的位姿計(jì)算結(jié)果。
[0092] 本發(fā)明提出的基于無線定位和激光地圖匹配的位姿解算方法，不僅可以實(shí)現(xiàn)高精 度的室內(nèi)定位，為位置服務(wù)應(yīng)用提供堅(jiān)實(shí)的技術(shù)支撐，同時(shí)可以精確估計(jì)偏航角，因此非常 適用于移動機(jī)器人自主導(dǎo)航領(lǐng)域。
[0093] 以上所述僅是對本發(fā)明的優(yōu)選實(shí)施方式進(jìn)行描述，并非是對本發(fā)明的范圍進(jìn)行限 定，在不脫離本發(fā)明設(shè)計(jì)精神的前提下，本領(lǐng)域普通工程技術(shù)人員對本發(fā)明的技術(shù)方案作 出的各種變形和改進(jìn)，均應(yīng)落入本發(fā)明的權(quán)利要求書確定的保護(hù)范圍內(nèi)。
【主權(quán)項(xiàng)】
1. 一種基于無線定位和激光地圖匹配的位姿解算方法，其特征在于：該方法包括如下 步驟： 步驟1，把至少三個(gè)無線定位錨點(diǎn)部署在環(huán)境中的不同位置，以確保所有無線定位錨點(diǎn) 不在空間形成一條直線； 步驟2,使用安裝了激光掃描儀和無線定位標(biāo)簽的定位設(shè)備，基于SLAM同步定位與地圖 構(gòu)建算法構(gòu)建全局激光地圖，同時(shí)記錄定位設(shè)備當(dāng)前的位置和定位設(shè)備與各無線定位錨點(diǎn) 之間的距離； 步驟3,利用步驟2得到的定位設(shè)備位置數(shù)據(jù)與各無線定位錨點(diǎn)之間的距離數(shù)據(jù)，計(jì)算 無線定位錨點(diǎn)的位置； 步驟4,利用步驟2得到的全局激光地圖，計(jì)算所有無障礙物像素點(diǎn)的模擬激光掃描數(shù) 據(jù)； 步驟5,在對定位設(shè)備進(jìn)行定位時(shí)，首先測量無線定位標(biāo)簽與各無線定位錨點(diǎn)之間的距 離，再通過無線定位算法計(jì)算無線定位標(biāo)簽的位置； 步驟6,根據(jù)無線定位標(biāo)簽在定位設(shè)備上的安裝位置和無線定位誤差大小，以步驟5計(jì) 算出的無線定位標(biāo)簽的位置為中心，估計(jì)出定位設(shè)備的備選位置區(qū)域； 步驟7,測量一幀激光掃描數(shù)據(jù)，在備選位置區(qū)域中的每一個(gè)無障礙物像素點(diǎn)處，把測 得的激光掃描數(shù)據(jù)與該像素點(diǎn)處的模擬激光掃描數(shù)據(jù)進(jìn)行匹配，得到偏航角和匹配距離； 步驟8,選出激光掃描數(shù)據(jù)匹配距離最小的像素點(diǎn)，其對應(yīng)的位置即為定位設(shè)備當(dāng)前位 置，在該像素點(diǎn)通過激光掃描數(shù)據(jù)匹配得到的偏航角即為定位設(shè)備的偏航角。2. 如權(quán)利要求1所述的基于無線定位和激光地圖匹配的位姿解算方法，其特征在于:所 述無線定位標(biāo)簽的位置，以回歸法估計(jì)出的每個(gè)無線定位標(biāo)簽的位置為初始值，以計(jì)算距 離與測量距離的均方誤差最小為準(zhǔn)則，利用梯度下降法，精確計(jì)算每個(gè)無線定位標(biāo)簽的位 置。3. 如權(quán)利要求1所述的基于無線定位和激光地圖匹配的位姿解算方法，其特征在于:所 述測量無線定位標(biāo)簽與各無線定位錨點(diǎn)之間的距離，在每一個(gè)采樣時(shí)刻，測量無線定位標(biāo) 簽到每個(gè)無線定位錨點(diǎn)的距離。4. 如權(quán)利要求3所述的基于無線定位和激光地圖匹配的位姿解算方法，其特征在于:所 述每一個(gè)采樣時(shí)刻的測量無線定位標(biāo)簽到每個(gè)無線定位錨點(diǎn)的距離，如果每個(gè)無線定位錨 點(diǎn)的個(gè)數(shù)大于3個(gè)，則從中隨機(jī)選擇3個(gè):如果滿足平面三點(diǎn)定位要求，則利用三點(diǎn)定位法計(jì) 算無線定位標(biāo)簽的位置;否則再次隨機(jī)選擇，直到計(jì)算出無線定位標(biāo)簽的位置。5. 如權(quán)利要求1所述的基于無線定位和激光地圖匹配的位姿解算方法，其特征在于:如 步驟6所述的估計(jì)定位設(shè)備備選位置區(qū)域，以無線定位標(biāo)簽的位置為中心，以無線定位誤差 與無線定位標(biāo)簽偏離定位設(shè)備中心點(diǎn)的距離的和為半徑，構(gòu)造一個(gè)圓形的候選定位區(qū)域， 在后續(xù)步驟中，激光匹配定位將在在候選定位區(qū)域中進(jìn)行。6. 如權(quán)利要求1所述的基于無線定位和激光地圖匹配的位姿解算方法，其特征在于:如 步驟4所述的計(jì)算模擬激光掃描數(shù)據(jù)，把全局地圖中的候選定位點(diǎn)找出來，并通過仿真的方 式把各定位點(diǎn)的模擬激光掃描數(shù)據(jù)算出來，作為地圖匹配定位時(shí)的使用。7. 如權(quán)利要求6所述的基于無線定位和激光地圖匹配的位姿解算方法，其特征在于:根 據(jù)全局地圖統(tǒng)計(jì)候選定位點(diǎn)時(shí)，可以把地圖中所有的無障礙物像素點(diǎn)作為候選定位點(diǎn)。8.如權(quán)利要求1所述的基于無線定位和激光地圖匹配的位姿解算方法，其特征在于:該 方法通過計(jì)算各偏航角情況下激光掃描數(shù)據(jù)與備選位置區(qū)域的各像素點(diǎn)的模擬激光數(shù)據(jù) 的最小平均距離差，來確定定位設(shè)備的當(dāng)前位置像素和偏航角。
【專利摘要】本發(fā)明公開了一種基于無線定位和激光地圖匹配的位姿解算方法，該方法把無線定位方法與激光掃描定位相結(jié)合，實(shí)現(xiàn)了一種不依賴于衛(wèi)星定位系統(tǒng)的室內(nèi)高精度定位方法，該方法同時(shí)也實(shí)現(xiàn)了對定位設(shè)備偏航角的計(jì)算。該方法既避免了無線定位精度低的缺點(diǎn)，又避免了激光掃描定位易受環(huán)境變化的干擾而發(fā)生定位位置跳變的情況。本發(fā)明實(shí)現(xiàn)的高精度位姿計(jì)算方法，可廣泛應(yīng)用于位置服務(wù)和移動機(jī)器人自主導(dǎo)航領(lǐng)域，是位置服務(wù)應(yīng)用和移動機(jī)器人的關(guān)鍵核心技術(shù)之一。
【IPC分類】G01C21/30
【公開號】CN105547305
【申請?zhí)枴緾N201510883727
【發(fā)明人】謝傳泉
【申請人】北京布科思科技有限公司
【公開日】2016年5月4日
【申請日】2015年12月4日