一種基于激光掃描傳感器的里程估計方法
【專利摘要】本發(fā)明公開了一種基于激光掃描傳感器的里程估計方法，機(jī)器人的激光掃描傳感器，可獲得一定范圍內(nèi)的障礙物角度和距離信息，在每個位置可以得到局部環(huán)境地圖，在相鄰位置通過匹配兩個局部地圖可以得到一個旋轉(zhuǎn)平移變換矩陣，根據(jù)變換矩陣對相鄰的局部地圖進(jìn)行數(shù)據(jù)融合，最終可以得到一個全局的環(huán)境地圖，然后機(jī)器人可利用激光掃描傳感器掃描到的數(shù)據(jù)與全局環(huán)境地圖匹配定位，從而得出一個準(zhǔn)確的里程估計。本發(fā)明僅僅利用激光數(shù)據(jù)估計里程計信息，利用外部環(huán)境信息的迭代匹配進(jìn)行估計里程，由于外部環(huán)境信息的相對穩(wěn)定性，減少累積誤差。
【專利說明】
一種基于激光掃描傳感器的里程估計方法
技術(shù)領(lǐng)域
[0001]本發(fā)明屬于機(jī)器人運(yùn)動技術(shù)領(lǐng)域，特別涉及一種基于激光掃描傳感器的里程估計方法。
【背景技術(shù)】
[0002]里程信息是機(jī)器人運(yùn)動狀態(tài)的最基本信息?，F(xiàn)有的機(jī)器人里程估計方法需要對機(jī)器人在一段局部運(yùn)行路徑中各點的位姿變化來確定，這個位姿變化可由編碼器和慣性導(dǎo)航傳感器來得到，傳統(tǒng)的編碼器估計里程方法僅僅利用內(nèi)部傳感器的信息累積得到里程信息，但該位姿變化誤差會不斷累積，誤差較大，最終得到的里程信息不準(zhǔn)確。

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

[0003]本發(fā)明的目的是為了解決上述問題，提供一種穩(wěn)定且準(zhǔn)確的基于激光掃描傳感器的里程估計方法。
[0004]為此，本發(fā)明的技術(shù)方案是:一種基于激光掃描傳感器的里程估計方法，在至少具有運(yùn)動控制系統(tǒng)、慣性導(dǎo)航系統(tǒng)、激光掃描傳感器的機(jī)器人上使用；其特征在于:包括以下步驟:
1)對雜亂無規(guī)律的掃描數(shù)據(jù)點進(jìn)行濾波處理，去除無用點；
2)將濾波后數(shù)據(jù)通過光線追蹤方法計算局部柵格地圖；局部柵格地圖是指機(jī)器人當(dāng)前位姿為坐標(biāo)原點，將該位置觀測到的數(shù)據(jù)構(gòu)建的柵格地圖，局部柵格地圖創(chuàng)建通過激光原點所在位置到激光光束結(jié)束位置通過光束追蹤計算得到，兩者之間的連線經(jīng)過的柵格點應(yīng)為非障礙物占據(jù)，激光光束結(jié)束點為障礙物占據(jù)狀態(tài)，在連線該點之后為未觀測狀態(tài)，即障礙物占據(jù)的概率等于非障礙物占據(jù)概率；
3)計算相鄰兩個位姿下的局部柵格地圖的sift特征點對及其對應(yīng)特征描述子;sift，即尺度不變特征變換，是用于圖像處理領(lǐng)域的一種描述;這種描述具有尺度不變性，可在圖像中檢測出關(guān)鍵點，是一種局部特征描述子；
4)根據(jù)特征描述子的相關(guān)性對特征點對進(jìn)行排序，濾除特征描述子相關(guān)性較小的特征點;在兩幅相鄰局部地圖中分別提取特征點及其描述子，遍歷該點集，尋找特征描述子距離滿足閾值條件的特征點對，并根據(jù)距離由小到大對其排序，不匹配的特征點舍棄;選取匹配的特征點對的前百分之三十的特征點對進(jìn)行ICP迭代計算變換矩陣；
5)將兩個相鄰位姿下的局部地圖進(jìn)行最近點迭代算法ICP，計算兩個局部地圖的變換矩陣；ICP迭代算法是利用與多條不平行直線的距離來確定一點的原理，通過計算直線相對機(jī)器人的距離變化信息得到機(jī)器人自身的相對位姿變化;利用兩條不平行直線特征即可定位，當(dāng)激光傳感器獲得多條不平行直線特征時，篩選線性相關(guān)系數(shù)較高的直線，兩兩直線進(jìn)行定位解算，最后將結(jié)果舍去最大值和最小值求平均值；
6)重復(fù)步驟1)_5)，記錄完成整個路徑的變換矩陣，最后進(jìn)行數(shù)據(jù)合并得到全局柵格地圖； 7)利用激光傳感器獲得環(huán)境特征，將環(huán)境特征與全局地圖匹配，從而計算出機(jī)器人的位姿變化，得出里程估計。
[0005]本發(fā)明僅僅利用激光數(shù)據(jù)估計里程計信息，不需要傳統(tǒng)的編碼器等傳感器，適用于無法安裝或不方便裝配該類傳感器的情況；利用外部環(huán)境信息的迭代匹配進(jìn)行估計里程，由于外部環(huán)境信息的相對穩(wěn)定性，減少累積誤差;本發(fā)明計算的里程信息可以和傳統(tǒng)的編碼器或IMU得到的里程信息通過擴(kuò)展卡爾曼濾波等方法進(jìn)行融合，從而得到更穩(wěn)定準(zhǔn)確的里程信息。
【附圖說明】
[0006]以下結(jié)合附圖和本發(fā)明的實施方式來作進(jìn)一步詳細(xì)說明圖1為本發(fā)明的流程框圖；
圖2為本發(fā)明的特征識別示意圖；
圖3為本發(fā)明的定位結(jié)算示意圖；
圖4為本發(fā)明的位姿校正示意圖。
【具體實施方式】
[0007]參見附圖。本實施例所述的里程估計方法，在至少具有運(yùn)動控制系統(tǒng)、慣性導(dǎo)航系統(tǒng)、激光掃描傳感器的機(jī)器人上使用;機(jī)器人系統(tǒng)裝有二維激光掃描傳感器，可獲得一定范圍內(nèi)的障礙物角度和距離信息，在每個位置可以得到局部環(huán)境地圖，在相鄰位置通過匹配兩個局部地圖可以得到一個旋轉(zhuǎn)平移變換矩陣，根據(jù)變換矩陣對相鄰的局部地圖進(jìn)行數(shù)據(jù)融合，最終我們可以得到一個全局的環(huán)境地圖，然后機(jī)器人可利用激光掃描傳感器掃描到的數(shù)據(jù)與全局環(huán)境地圖匹配定位，從而得出一個準(zhǔn)確的里程估計。
[0008]具體包括以下步驟:
1)對雜亂無規(guī)律的掃描數(shù)據(jù)點進(jìn)行濾波處理，去除無用點；
2)將濾波后數(shù)據(jù)通過光線追蹤方法計算局部柵格地圖；局部柵格地圖是指機(jī)器人當(dāng)前位姿為坐標(biāo)原點，將該位置觀測到的數(shù)據(jù)構(gòu)建的柵格地圖，由于該地圖是單次激光數(shù)據(jù)構(gòu)建的，因而地圖的估計誤差主要為激光傳感器的觀測誤差，不存在估計偏差。局部柵格地圖創(chuàng)建通過激光原點所在位置到激光光束結(jié)束位置(障礙物位置)通過光束追蹤計算得到，兩者之間的連線經(jīng)過的柵格點應(yīng)為非障礙物占據(jù)，激光光束結(jié)束點為障礙物占據(jù)狀態(tài)，在連線該點之后為未觀測狀態(tài)，即障礙物占據(jù)的概率等于非障礙物占據(jù)概率；
3)計算相鄰兩個位姿下的局部柵格地圖的sift特征點對及其對應(yīng)特征描述子;SIFT(Scale-1nvariant feature transform)是一種檢測局部特征的算法，該算法通過求一幅圖中的特征點(interest points ,or corner points)及其有關(guān)scale 和 orientat1n 的描述子，在尺度空間尋找極值點，提取位置，尺度，旋轉(zhuǎn)不變量，SIFT特征對旋轉(zhuǎn)、尺度縮放、亮度變化保持不變性，對視角變化、仿射變換、噪聲也保持一定程度的穩(wěn)定性；
4)根據(jù)特征描述子的相關(guān)性對特征點對進(jìn)行排序，濾除特征描述子相關(guān)性較小的特征點;在兩幅相鄰局部地圖中分別提取特征點及其描述子，遍歷該點集，尋找特征描述子距離滿足閾值條件的特征點對，并根據(jù)距離有小到大對其排序，不匹配的特征點舍棄;選取匹配的特征點對的前百分之三十的特征點對進(jìn)行ICP迭代計算變換矩陣； 5)將兩個相鄰位姿下的局部地圖進(jìn)行最近點迭代算法ICP，計算兩個局部地圖的變換矩陣；利用與多條不平行直線的距離來確定一點的原理，通過計算直線相對機(jī)器人的距離變化信息得到機(jī)器人自身的相對位姿變化;利用兩條不平行直線特征即可定位，當(dāng)激光傳感器獲得多條不平行直線特征時，篩選線性相關(guān)系數(shù)較高的直線，兩兩直線進(jìn)行定位解算，最后將結(jié)果舍去最大值和最小值求平均值(如圖2所示)；如圖3所示，已知兩條不平行直線
11、12和與兩條直線的距離(dl，d2)，可以確定點P;
6)重復(fù)步驟1)_5)，記錄完成整個路徑的變換矩陣，最后進(jìn)行數(shù)據(jù)合并得到全局柵格地圖；地圖表示采用幾何描述方式，在離線方式下獲得，首先設(shè)定初始位姿，然后控制機(jī)器人按預(yù)定路徑行進(jìn)，在行進(jìn)過程中，利用編碼器和慣性傳感器信息可以估計出機(jī)器人當(dāng)前位姿，與此同時激光傳感器不斷掃描環(huán)境信息，通過定位過程所述方法，從激光傳感器獲得數(shù)據(jù)中提取自然環(huán)境特征得到特征參數(shù)；由于在一定范圍內(nèi)，同一特征能夠一直被激光傳感器所掃描獲得，利用該特征的參數(shù)變化可計算出機(jī)器人運(yùn)動的相對位姿變化，利用該位姿變化來校正編碼器和慣性傳感器的位姿信息；由校正后的位姿信息，將特征參數(shù)變換到起始位姿確定的全局坐標(biāo)系下，然后將該特征添加到全局地圖中；機(jī)器人不斷前進(jìn)掃描到新的環(huán)境特征，重復(fù)上述過程完成全局地圖創(chuàng)建；
7)利用激光傳感器獲得環(huán)境特征，將環(huán)境特征與全局地圖匹配，從而計算出機(jī)器人的位姿變化，得出里程估計。如圖4所示，P1-P2-P3為機(jī)器人一段局部運(yùn)行路徑，在該局部范圍內(nèi)，激光傳感器均能獲得直線特征LI，L2，L3;從Pl至P3的位姿變化可由編碼器和慣性導(dǎo)航傳感器得到，但該位姿變化誤差會不斷累積，誤差較大;所以利用直線特征也可計算出Pl至P3的位姿變化，該位姿變化沒有累積誤差，因而誤差較小，通過比較位姿的變化，我們就得到了一個較為精準(zhǔn)的里程估計。
【主權(quán)項】
1.一種基于激光掃描傳感器的里程估計方法，在至少具有運(yùn)動控制系統(tǒng)、慣性導(dǎo)航系統(tǒng)、激光掃描傳感器的機(jī)器人上使用;其特征在于:包括以下步驟: .1)對雜亂無規(guī)律的掃描數(shù)據(jù)點進(jìn)行濾波處理，去除無用點； .2)將濾波后數(shù)據(jù)通過光線追蹤方法計算局部柵格地圖；局部柵格地圖是指機(jī)器人當(dāng)前位姿為坐標(biāo)原點，將該位置觀測到的數(shù)據(jù)構(gòu)建的柵格地圖，局部柵格地圖創(chuàng)建通過激光原點所在位置到激光光束結(jié)束位置通過光束追蹤計算得到，兩者之間的連線經(jīng)過的柵格點應(yīng)為非障礙物占據(jù)，激光光束結(jié)束點為障礙物占據(jù)狀態(tài)，在連線該點之后為未觀測狀態(tài)，即障礙物占據(jù)的概率等于非障礙物占據(jù)概率； .3 )計算相鄰兩個位姿下的局部柵格地圖的s i f t特征點對及其對應(yīng)特征描述子;s i f t，即尺度不變特征變換，是用于圖像處理領(lǐng)域的一種描述;這種描述具有尺度不變性，可在圖像中檢測出關(guān)鍵點，是一種局部特征描述子； .4)根據(jù)特征描述子的相關(guān)性對特征點對進(jìn)行排序，濾除特征描述子相關(guān)性較小的特征點;在兩幅相鄰局部地圖中分別提取特征點及其描述子，遍歷該點集，尋找特征描述子距離滿足閾值條件的特征點對，并根據(jù)距離由小到大對其排序，不匹配的特征點舍棄;選取匹配的特征點對的前百分之三十的特征點對進(jìn)行ICP迭代計算變換矩陣； .5)將兩個相鄰位姿下的局部地圖進(jìn)行最近點迭代算法ICP，計算兩個局部地圖的變換矩陣；ICP迭代算法是利用與多條不平行直線的距離來確定一點的原理，通過計算直線相對機(jī)器人的距離變化信息得到機(jī)器人自身的相對位姿變化;利用兩條不平行直線特征即可定位，當(dāng)激光傳感器獲得多條不平行直線特征時，篩選線性相關(guān)系數(shù)較高的直線，兩兩直線進(jìn)行定位解算，最后將結(jié)果舍去最大值和最小值求平均值； .6)重復(fù)步驟1)_5)，記錄完成整個路徑的變換矩陣，最后進(jìn)行數(shù)據(jù)合并得到全局柵格地圖； .7)利用激光傳感器獲得環(huán)境特征，將環(huán)境特征與全局地圖匹配，從而計算出機(jī)器人的位姿變化，得出里程估計。
【文檔編號】G01C22/00GK105973265SQ201610332725
【公開日】2016年9月28日
【申請日】2016年5月19日
【發(fā)明人】王海瑞, 陳如申, 黎勇躍
【申請人】杭州申昊科技股份有限公司