本發(fā)明涉及移動(dòng)機(jī)器人定位領(lǐng)域，對(duì)于由于光照不穩(wěn)定或者光線不足引起的特征提取不穩(wěn)定導(dǎo)致無法實(shí)現(xiàn)定位功能有較大的彌補(bǔ)，對(duì)于定位的結(jié)果也更加實(shí)時(shí)和精確。
背景技術(shù)：
：定位是確定機(jī)器人在其作業(yè)環(huán)境中所處的位置的過程，更加具體的說就是利用先驗(yàn)環(huán)境地圖的信息、當(dāng)前的機(jī)器人位姿估計(jì)以及傳感器的觀測(cè)值等輸入信息，經(jīng)過一定的處理和變換，產(chǎn)生更加準(zhǔn)確的對(duì)機(jī)器人的當(dāng)前位置的估計(jì)。利用傳感器感知的信息來獲得可靠的定位是自主移動(dòng)機(jī)器人最基本、最重要的一項(xiàng)功能，也是移動(dòng)機(jī)器人研究中備受關(guān)注、富有挑戰(zhàn)性的一個(gè)重要研究主題。Kosaka等人嘗試研發(fā)了一種基于環(huán)境幾何描述的室內(nèi)機(jī)器人導(dǎo)航系統(tǒng)，通過CAD建模。在正常運(yùn)行的情況下，通過相機(jī)輸入圖像，在生成期望場(chǎng)景的前提下，鎖定特征搜索區(qū)域，直接使用Hough變換提取直線，再與CAD模型場(chǎng)景進(jìn)行配準(zhǔn)。迭代至期望與觀察值最小誤差為止，從而得到當(dāng)前位置。室外環(huán)境導(dǎo)航主要依賴于公路的特定信息，如斑馬線，車道線等.。Navlab系統(tǒng)采用顏色和紋理分割來識(shí)別道路，然后在通過識(shí)別檢測(cè)障礙物。為了進(jìn)一步提高視覺識(shí)別系統(tǒng)的魯棒性和可靠性，將神經(jīng)網(wǎng)絡(luò)和自適應(yīng)控制規(guī)劃車輛運(yùn)動(dòng)的技術(shù)融入其中?；诔叨炔蛔兲匦缘腟IFT算法被成功融合到了移動(dòng)機(jī)器人的定位中。利用SIFT的尺度不變的特性，對(duì)兩幅相近的圖像特征配準(zhǔn)，利用視差模型進(jìn)行三維坐標(biāo)換算得到機(jī)器人的移動(dòng)位置。隨著立體相機(jī)的出現(xiàn)，更多的研究人員，直接將雙目立體相機(jī)或者RGB-D相機(jī)運(yùn)用到該研究中，使得獲得的定位結(jié)果更加實(shí)時(shí)和精確。然而這些基于紋理特征的移動(dòng)機(jī)器人定位方法,對(duì)光照極其敏感。光照的明暗直接影響算法的結(jié)果。技術(shù)實(shí)現(xiàn)要素：為了克服機(jī)器人在光照惡劣或者完全黑暗的條件下的顏色信息獲取不精確，導(dǎo)致配準(zhǔn)結(jié)果存在較大的誤差，使定位結(jié)果不準(zhǔn)確的不足，本發(fā)明提供一種不受光照條件影響、精確度較高、運(yùn)算的時(shí)空復(fù)雜度較低的基于3D點(diǎn)云FPFH特征實(shí)時(shí)三維空間定位方法，利用三維點(diǎn)云和三維局部特征對(duì)前后連續(xù)幀進(jìn)行配準(zhǔn)的方法，直接得到移動(dòng)機(jī)器人的三維位點(diǎn)，能實(shí)時(shí)獲取移動(dòng)機(jī)器人的三維空間定位信息，該方法可以用于但不限于基于視覺的移動(dòng)機(jī)器人三維空間定位。本發(fā)明解決其技術(shù)問題所采用的技術(shù)方案是：一種基于3D點(diǎn)云FPFH特征實(shí)時(shí)三維空間定位方法，包括如下步驟：1)從深度攝像頭獲取3D點(diǎn)云數(shù)據(jù)；2)點(diǎn)云關(guān)鍵幀選取。第一幀的時(shí)候把第一幀當(dāng)成關(guān)鍵幀，剩下的關(guān)鍵幀選取方法是點(diǎn)云精確匹配后，匹配到的對(duì)應(yīng)點(diǎn)的個(gè)數(shù)，通過閾值進(jìn)行過濾；3)點(diǎn)云預(yù)處理：首先對(duì)點(diǎn)云進(jìn)行分割，分割后能夠?qū)崟r(shí)精確地給出點(diǎn)云中所有可能的平面；接著采用網(wǎng)格降采樣方法對(duì)平面進(jìn)行降采樣和濾波；最后進(jìn)行區(qū)域過濾，剔除關(guān)鍵點(diǎn)較少的區(qū)域；4)特征描述：使用ISS算法得到點(diǎn)云的關(guān)鍵點(diǎn)，獲取關(guān)鍵點(diǎn)的FPFH特征；5)點(diǎn)云配準(zhǔn)：首先利用采樣一致性初始配準(zhǔn)算法對(duì)兩片點(diǎn)云進(jìn)行基于FPFH特征的初始配準(zhǔn)，接著運(yùn)用ICP算法對(duì)初始配準(zhǔn)結(jié)果進(jìn)行二次配準(zhǔn)，實(shí)現(xiàn)點(diǎn)云的精確配準(zhǔn)；6)坐標(biāo)轉(zhuǎn)換：獲得移動(dòng)機(jī)器人三維空間坐標(biāo)的變化矩陣，將當(dāng)前點(diǎn)云的坐標(biāo)通過變換矩陣轉(zhuǎn)換到初始位置。7)重復(fù)重復(fù)1)～6)，隨著機(jī)器人的移動(dòng)計(jì)算得到相對(duì)于初始位置的機(jī)器人的坐標(biāo)。本發(fā)明的有益效果主要表現(xiàn)在：基于FPFH特征的實(shí)時(shí)三維空間定位算法，采用基于光照無關(guān)的依賴于3D形狀特征描述子的FPFH特征，能夠在動(dòng)態(tài)場(chǎng)景中實(shí)時(shí)獲取移動(dòng)機(jī)器人的三維空間定位信息，同時(shí)關(guān)鍵幀的提取以及點(diǎn)云的預(yù)處理，大大降低了運(yùn)算的時(shí)空復(fù)雜度，使該算法能更好地應(yīng)用于實(shí)際環(huán)境中。具體實(shí)施方式下面對(duì)本發(fā)明做進(jìn)一步說明。一種基于3D點(diǎn)云FPFH特征實(shí)時(shí)三維空間定位方法，包括如下步驟：1)從深度攝像頭獲取3D點(diǎn)云數(shù)據(jù)；2)點(diǎn)云關(guān)鍵幀選取。第一幀的時(shí)候把第一幀當(dāng)成關(guān)鍵幀，剩下的關(guān)鍵幀選取方法是點(diǎn)云精確匹配后，匹配到的對(duì)應(yīng)點(diǎn)的個(gè)數(shù)，通過閾值進(jìn)行過濾；3)點(diǎn)云預(yù)處理：首先對(duì)點(diǎn)云進(jìn)行分割，分割后能夠?qū)崟r(shí)精確地給出點(diǎn)云中所有可能的平面；接著采用網(wǎng)格降采樣方法對(duì)平面進(jìn)行降采樣和濾波；最后進(jìn)行區(qū)域過濾，剔除關(guān)鍵點(diǎn)較少的區(qū)域；4)特征描述：使用ISS算法得到點(diǎn)云的關(guān)鍵點(diǎn)，獲取關(guān)鍵點(diǎn)的FPFH特征；5)點(diǎn)云配準(zhǔn)：首先利用采樣一致性初始配準(zhǔn)算法對(duì)兩片點(diǎn)云進(jìn)行基于FPFH特征的初始配準(zhǔn)，接著運(yùn)用ICP算法對(duì)初始配準(zhǔn)結(jié)果進(jìn)行二次配準(zhǔn)，進(jìn)一步優(yōu)化配準(zhǔn)結(jié)果，實(shí)現(xiàn)點(diǎn)云的精確配準(zhǔn)；6)坐標(biāo)轉(zhuǎn)換：獲得移動(dòng)機(jī)器人三維空間坐標(biāo)的變化矩陣，將當(dāng)前點(diǎn)云的坐標(biāo)通過變換矩陣轉(zhuǎn)換到初始位置。7)重復(fù)重復(fù)1)～6)，隨著機(jī)器人的移動(dòng)計(jì)算得到相對(duì)于初始位置的機(jī)器人的坐標(biāo)。所述步驟2)中，關(guān)鍵幀選取是定位算法中首先要解決的問題，我們的方法就是根據(jù)步驟5)中點(diǎn)云精確匹配后，匹配到的對(duì)應(yīng)點(diǎn)的個(gè)數(shù)，通過閾值進(jìn)行過濾。隨著視場(chǎng)的變化，當(dāng)前點(diǎn)云與關(guān)鍵幀之間能夠匹配上的對(duì)應(yīng)點(diǎn)的個(gè)數(shù)是呈減少的趨勢(shì)；但是對(duì)于計(jì)算變換矩陣而言，計(jì)算得到的結(jié)果誤差需要保證在一定的范圍之內(nèi)。因此在配準(zhǔn)過程中需要保證足夠多的配準(zhǔn)點(diǎn)。假設(shè)最新點(diǎn)云與關(guān)鍵幀配準(zhǔn)的對(duì)應(yīng)點(diǎn)個(gè)數(shù)為C，當(dāng)C>Ct時(shí)，通過對(duì)應(yīng)點(diǎn)計(jì)算得到的轉(zhuǎn)換矩陣誤差在可接受范圍之內(nèi)。當(dāng)C<Ct+Cr，將當(dāng)前幀作為關(guān)鍵幀其中Cr表示波動(dòng)范圍，Ct，Cr作為先驗(yàn)值給出。所述步驟3)的點(diǎn)云預(yù)處理過程如下：為了在后續(xù)的環(huán)節(jié)中，能夠高效執(zhí)行，需要對(duì)點(diǎn)云進(jìn)行預(yù)處理。首先對(duì)點(diǎn)云進(jìn)行分割處理。分割后能夠?qū)崟r(shí)精確的給出點(diǎn)云中所有可能的平面，設(shè)第ci個(gè)關(guān)鍵幀點(diǎn)云中分割的區(qū)域集合為將當(dāng)前點(diǎn)云分割得到的區(qū)域集合Rj與進(jìn)行匹配，將未能匹配上的區(qū)域從Rj中剔除。區(qū)域匹配規(guī)則表示如下：Rj＝{Pk}，k∈[1,N](1)Rci={Pl},l∈[1,M]---(2)]]>||Sk-Sl||＜ts(4)其中Pk表示在Rj區(qū)域集合內(nèi)的其中一個(gè)點(diǎn)云子集，同理Pl也是區(qū)域集合內(nèi)點(diǎn)云子集；表示平面Pk對(duì)應(yīng)的平面法線，同理Sl為平面Pl的面積。為平面Pl對(duì)應(yīng)的平面法線，Sk表示平面Pk的平面大小，通過點(diǎn)的個(gè)數(shù)表示；同理和ts通過先驗(yàn)給出。計(jì)算各自區(qū)域內(nèi)的平面法線方向的歐式距離，將法線的歐式距離和面積差在給定閾值內(nèi)的平面作為匹配對(duì)。這樣做可以提高配準(zhǔn)的速度和準(zhǔn)確性，因?yàn)樵邳c(diǎn)云配準(zhǔn)的過程中，只在限定的對(duì)應(yīng)區(qū)域內(nèi)進(jìn)行匹配點(diǎn)的掃描。盡管這樣做的后果是區(qū)域與區(qū)域之間邊界上的關(guān)鍵點(diǎn)會(huì)丟失，但通過實(shí)驗(yàn)發(fā)現(xiàn)，丟失的關(guān)鍵點(diǎn)對(duì)匹配的影響不大。在獲取對(duì)應(yīng)區(qū)域序列之后，各個(gè)區(qū)域內(nèi)對(duì)平面進(jìn)行降采樣和濾波。本算法采用網(wǎng)格降采樣，該方法的特點(diǎn)就是能夠真實(shí)的反映情況，保持失真程度降到最低，同時(shí)能夠剔除噪聲。區(qū)域過濾是進(jìn)一步剔除某些區(qū)域，這些區(qū)域可能存在關(guān)鍵點(diǎn)，但是關(guān)鍵點(diǎn)數(shù)量很少，忽略這些關(guān)鍵點(diǎn)不影響最終結(jié)算結(jié)果。這樣能盡可能減少不必要關(guān)鍵點(diǎn)和特征描述的計(jì)算。因此，我們的方法是將小于給定面積的區(qū)域直接舍棄。所述步驟5)的點(diǎn)云配準(zhǔn)，過程如下：配準(zhǔn)過程中，融合目前常用兩中配準(zhǔn)算法：用采樣一致性進(jìn)行初始配準(zhǔn)，再用迭代最近點(diǎn)算法進(jìn)行精確配準(zhǔn)。采樣一致性初始配準(zhǔn)算法(SampleConsensusInitialAlignment，SAC-IA)內(nèi)部分為兩部分：貪婪的初始對(duì)準(zhǔn)方法和采樣一致性方法。貪婪的初始對(duì)準(zhǔn)方法使用點(diǎn)云內(nèi)部旋轉(zhuǎn)不變性的特征，因此具有非常強(qiáng)的魯棒性。貪婪的初始對(duì)準(zhǔn)算法計(jì)算復(fù)雜度較高并且有可能只能得到局部最有解，因此采樣一致性方法，試圖保持相同的對(duì)應(yīng)的幾何關(guān)系而不必嘗試求解有限個(gè)對(duì)應(yīng)關(guān)系的所有組合。采樣一致性初始配準(zhǔn)算法(SampleconsensusInitialAlignment，SAC-IA)：5.1.1)從點(diǎn)云A中選擇s個(gè)樣本點(diǎn)，同時(shí)確定他們的配對(duì)距離大于用戶設(shè)定的最小值dmin；5.1.2)對(duì)s個(gè)樣本點(diǎn)，在點(diǎn)云B中分別找到滿足相似條件的點(diǎn)存入一個(gè)列表中，隨機(jī)選擇一些代表采樣點(diǎn)的對(duì)應(yīng)關(guān)系；5.1.3)根據(jù)點(diǎn)云中關(guān)鍵點(diǎn)的對(duì)應(yīng)關(guān)系計(jì)算剛體變換矩陣，并通過計(jì)算度量來評(píng)價(jià)轉(zhuǎn)換矩陣的質(zhì)量，度量由Huber評(píng)價(jià)公式?jīng)Q定：Lh(ei)=12ei2||ei||≤te12te(2||ei||-te)||ei||>te---(5)]]>重復(fù)這三個(gè)步驟直至達(dá)到最佳度量結(jié)果，最后使用一個(gè)Levenberg-Marquardt算法進(jìn)行非線性局部?jī)?yōu)化；由SAC-IA得到的剛體變換矩陣使得兩個(gè)點(diǎn)云數(shù)據(jù)大致重合，但是配準(zhǔn)精確度遠(yuǎn)遠(yuǎn)達(dá)不到實(shí)際工程應(yīng)用的要求，因此在初始配準(zhǔn)的基礎(chǔ)上再精確配準(zhǔn)。迭代最近點(diǎn)(IterativeClosestPoint，ICP)算法是常用的精確的配準(zhǔn)算法，它要求待配準(zhǔn)的點(diǎn)云數(shù)據(jù)與較近的初始位置相比較。每次迭代過程中，首先根據(jù)一定的準(zhǔn)則來對(duì)應(yīng)點(diǎn)集P與Q，其中對(duì)應(yīng)點(diǎn)對(duì)的個(gè)數(shù)為n。然后通過最小二乘法迭代計(jì)算最優(yōu)的坐標(biāo)變換，即旋轉(zhuǎn)矩陣R和平移矢量t，使得誤差函數(shù)最小。E(R,t)=1nΣk=1n||qk-(Rpk+t)||2---(6)]]>直至達(dá)到滿意的誤差要求。ICP迭代過程分為4個(gè)主要步驟：5.2.1)對(duì)原始點(diǎn)云進(jìn)行采樣；5.2.2)確定初始對(duì)應(yīng)點(diǎn)集；5.2.3)去除錯(cuò)誤對(duì)應(yīng)點(diǎn)對(duì)；5.2.4)坐標(biāo)變化求解。所述步驟6)中，定位過程完全依賴于點(diǎn)云的配準(zhǔn)過程，假設(shè)點(diǎn)云配準(zhǔn)的精確和實(shí)時(shí)性要求能夠完全滿足移動(dòng)機(jī)器人的動(dòng)態(tài)定位精確的。我們通過設(shè)立關(guān)鍵幀，對(duì)實(shí)時(shí)定位算法進(jìn)行優(yōu)化。對(duì)于靜態(tài)場(chǎng)景來說，機(jī)器人位置和當(dāng)前視點(diǎn)的場(chǎng)景是相對(duì)固定的。假設(shè)將初始位置作為機(jī)器人開始移動(dòng)的起點(diǎn)，相機(jī)坐標(biāo)系和點(diǎn)云坐標(biāo)系以及世界坐標(biāo)系是重疊的。隨著機(jī)器人的移動(dòng)，相機(jī)坐標(biāo)系也相應(yīng)移動(dòng)，此時(shí)的相機(jī)坐標(biāo)系和世界坐標(biāo)系是分離的。定位的目的就是將當(dāng)前的世界坐標(biāo)系中的信息換算到世界坐標(biāo)系中。可見我們所提到的定位是一個(gè)相對(duì)初始位置的定位。本實(shí)施例通過優(yōu)化換算方法從而實(shí)現(xiàn)機(jī)器人的實(shí)時(shí)定位。假設(shè)在點(diǎn)云數(shù)據(jù)集合中{Pi}，存在k個(gè)關(guān)鍵幀點(diǎn)云由于點(diǎn)云配對(duì)是成對(duì)的，因此關(guān)鍵幀點(diǎn)云與當(dāng)前幀點(diǎn)云之間按時(shí)間順序存在k-1個(gè)旋轉(zhuǎn)變換矩陣Tl。關(guān)鍵幀處在連續(xù)場(chǎng)景中的某一個(gè)視點(diǎn)，通過步驟2)篩選得到。該方法避免了直接計(jì)算兩兩3D點(diǎn)云之間旋轉(zhuǎn)平移矩陣。如果場(chǎng)景足夠大，這種大規(guī)模的膨脹過程，將消耗巨大的計(jì)算資源。通過設(shè)定關(guān)鍵幀，將旋轉(zhuǎn)平移矩陣的個(gè)數(shù)壓縮到最小，充分利用視場(chǎng)之間關(guān)聯(lián)部分，從而實(shí)現(xiàn)實(shí)時(shí)定位的目的。所述步驟7中，定位過程如下：7.1)首選需要確定世界坐標(biāo)系，我們將用傳感器的第一幀坐標(biāo)系作為世界坐標(biāo)系，將相機(jī)坐標(biāo)原點(diǎn)定義為世界坐標(biāo)系原點(diǎn)。同時(shí)將第一幀作為關(guān)鍵幀，記為在第二個(gè)關(guān)鍵幀確定之前中間所有點(diǎn)云通過配準(zhǔn)計(jì)算得到的旋轉(zhuǎn)平移矩陣換算到世界坐標(biāo)系下。7.2)通過關(guān)鍵幀選取規(guī)則，產(chǎn)生第二幀關(guān)鍵幀，記為計(jì)算和的旋轉(zhuǎn)平移矩陣T1，那么和之間的所有點(diǎn)云記為Pi，先計(jì)算與之間的旋轉(zhuǎn)平移矩陣，將Pi點(diǎn)云換算到坐標(biāo)系下，記為Pi'，再將Pi'通過旋轉(zhuǎn)平移矩陣T1，換算到世界坐標(biāo)系下。該過程的依次迭代，表達(dá)式如下：P0=Pi·Πi=n1Ti---(7)]]>P＝{pi},i∈[1,n](8)Ti＝(Ri|ti)(9)其中P表示三維點(diǎn)pi的集合，n表示點(diǎn)云數(shù)量，Ti表示4×4矩陣。將當(dāng)前場(chǎng)景的坐標(biāo)全部轉(zhuǎn)換到世界坐標(biāo)系中，從而得到定位的效果。當(dāng)前第1頁1 2 3