技術(shù)特征：

1.一種基于3D點(diǎn)云FPFH特征實(shí)時三維空間定位方法，其特征在于：所述定位方法包括如下步驟：

1)從深度攝像頭獲取3D點(diǎn)云數(shù)據(jù)；

2)點(diǎn)云關(guān)鍵幀選?。旱谝粠臅r候把第一幀當(dāng)成關(guān)鍵幀，剩下的關(guān)鍵幀選取方法是點(diǎn)云精確匹配后，匹配到的對應(yīng)點(diǎn)的個數(shù)，通過閾值進(jìn)行過濾；

3)點(diǎn)云預(yù)處理：首先對點(diǎn)云進(jìn)行分割，分割后能夠?qū)崟r精確地給出點(diǎn)云中所有可能的平面；接著采用網(wǎng)格降采樣方法對平面進(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)算法對兩片點(diǎn)云進(jìn)行基于FPFH特征的初始配準(zhǔn)，接著運(yùn)用ICP算法對初始配準(zhǔn)結(jié)果進(jìn)行二次配準(zhǔn)，實(shí)現(xiàn)點(diǎn)云的精確配準(zhǔn)；

6)坐標(biāo)轉(zhuǎn)換：獲得移動機(jī)器人三維空間坐標(biāo)的變化矩陣，將當(dāng)前點(diǎn)云的坐標(biāo)通過變換矩陣轉(zhuǎn)換到初始位置；

7)重復(fù)重復(fù)1)～6)，隨著機(jī)器人的移動計(jì)算得到相對于初始位置的機(jī)器人的坐標(biāo)。

2.如權(quán)利要求1所述的基于3D點(diǎn)云FPFH特征實(shí)時三維空間定位方法，其特征在于：所述步驟2)中，先將點(diǎn)云進(jìn)行精確匹配，再根據(jù)匹配的對應(yīng)點(diǎn)的個數(shù)來確定關(guān)鍵幀，當(dāng)對應(yīng)點(diǎn)個數(shù)滿足閾值條件則將當(dāng)前幀作為關(guān)鍵幀，隨著相機(jī)運(yùn)動，視場發(fā)生變化，當(dāng)前幀與關(guān)鍵幀重疊部分越來越少，因此當(dāng)前幀點(diǎn)云與關(guān)鍵幀點(diǎn)云之間能夠匹配上的對應(yīng)點(diǎn)的個數(shù)是呈減少的趨勢；但是對于計(jì)算變換矩陣而言，需要有足夠多的配準(zhǔn)點(diǎn)，確保計(jì)算結(jié)果的誤差在設(shè)定的范圍之內(nèi)；假設(shè)最新點(diǎn)云與關(guān)鍵幀配準(zhǔn)的對應(yīng)點(diǎn)個數(shù)為C，當(dāng)C>C_t時，通過對應(yīng)點(diǎn)計(jì)算得到的轉(zhuǎn)換矩陣誤差在可接受范圍之內(nèi)；當(dāng)C<C_t+C_r，將當(dāng)前幀作為關(guān)鍵幀其中C_r表示波動范圍，閾值C_t，C_r作為先驗(yàn)值給出。

3.如權(quán)利要求1或2所述的基于3D點(diǎn)云FPFH特征實(shí)時三維空間定位方法，其特征在于：所述步驟3)中，點(diǎn)云預(yù)處理過程如下：

首先對點(diǎn)云進(jìn)行分割處理，分割后能夠?qū)崟r精確的給出點(diǎn)云中所有可能的平面，設(shè)第c_i個關(guān)鍵幀點(diǎn)云中分割的區(qū)域集合為將當(dāng)前點(diǎn)云分割得到的區(qū)域集合R_j與進(jìn)行匹配，將未能匹配上的區(qū)域從R_j中剔除，區(qū)域匹配規(guī)則表示如下：

R_j＝{P_k}，k∈[1,N] (1)

R_ci＝{P_l}，l∈[1,M] (2)

||S_k-S_l||＜t_s (4)

其中P_k表示在R_j區(qū)域集合內(nèi)的其中一個點(diǎn)云子集，同理P_l也是區(qū)域集合內(nèi)點(diǎn)云子集；表示平面P_k對應(yīng)的平面法線，同理S_l為平面P_l的面積，為平面P_l對應(yīng)的平面法線，S_k表示平面P_k的平面大小，通過點(diǎn)的個數(shù)表示；同理和t_s通過先驗(yàn)給出；

計(jì)算各自區(qū)域內(nèi)的平面法線方向的歐式距離，將法線的歐式距離和面積差在給定閾值內(nèi)的平面作為匹配對；

在獲取對應(yīng)區(qū)域序列之后，各個區(qū)域內(nèi)對平面進(jìn)行降采樣和濾波。

4.如權(quán)利要求1或2所述的基于3D點(diǎn)云FPFH特征實(shí)時三維空間定位方法，其特征在于：所述步驟5)中，用采樣一致性進(jìn)行初始配準(zhǔn)，過程如下：

5.1.1)從點(diǎn)云A中選擇s個樣本點(diǎn)，同時確定他們的配對距離大于用戶設(shè)定的最小值d_min；

5.1.2)對s個樣本點(diǎn)，在點(diǎn)云B中分別找到滿足相似條件的點(diǎn)存入一個列表中，隨機(jī)選擇一些代表采樣點(diǎn)的對應(yīng)關(guān)系；

5.1.3)根據(jù)點(diǎn)云中關(guān)鍵點(diǎn)的對應(yīng)關(guān)系計(jì)算剛體變換矩陣，并通過計(jì)算度量來評價轉(zhuǎn)換矩陣的質(zhì)量，度量由Huber評價公式?jīng)Q定：

$L_h\left(e_i\right)=\left\{\begin{array}{cc}\frac{1}{2}{e}_i^2& \left|\right|e_i\left|\right|\&le;t_e\\ \frac{1}{2}{t}_e\left(2\right|\left|e_i\right||-t_e)& \left|\right|e_i\left|\right|\&gt;t_e\end{array}\right.---\left(5\right)$

重復(fù)這三個步驟直至達(dá)到最佳度量結(jié)果，最后使用一個Levenberg-Marquardt算法進(jìn)行非線性局部優(yōu)化；

運(yùn)用ICP算法對初始配準(zhǔn)結(jié)果進(jìn)行二次配準(zhǔn)，過程如下：每次迭代過程中，首先根據(jù)設(shè)定的準(zhǔn)則來對應(yīng)點(diǎn)集P與Q，其中對應(yīng)點(diǎn)對的個數(shù)為n，然后通過最小二乘法迭代計(jì)算最優(yōu)的坐標(biāo)變換，即旋轉(zhuǎn)矩陣R和平移矢量t，使得誤差函數(shù)最小：

$E(R,t)=\frac{1}{n}\underset{k=1}{\overset{n}{\&Sigma;}}\left|\right|q_k-(R_{p_k}+t)|{|}^2---\left(6\right)$

直至達(dá)到滿意的誤差要求；

ICP迭代過程為：5.2.1)對原始點(diǎn)云進(jìn)行采樣；5.2.2)確定初始對應(yīng)點(diǎn)集；

5.2.3)去除錯誤對應(yīng)點(diǎn)對；5.2.4)坐標(biāo)變化求解。

5.如權(quán)利要求1或2所述的基于3D點(diǎn)云FPFH特征實(shí)時三維空間定位方法，其特征在于：所述步驟6)中，通過優(yōu)化換算方法從而實(shí)現(xiàn)機(jī)器人的實(shí)時定位，假設(shè)在點(diǎn)云數(shù)據(jù)集合中{P_i}，存在k個關(guān)鍵幀點(diǎn)云由于點(diǎn)云配對是成對的，因此關(guān)鍵幀點(diǎn)云與當(dāng)前幀點(diǎn)云之間按時間順序存在k-1個旋轉(zhuǎn)變換矩陣T_l，關(guān)鍵幀處在連續(xù)場景中的某一個視點(diǎn)，通過步驟2)篩選得到。

6.如權(quán)利要求1或2所述的基于3D點(diǎn)云FPFH特征實(shí)時三維空間定位方法，其特征在于：所述步驟7)中，定位過程如下：

7.1)首選需要確定世界坐標(biāo)系，用傳感器的第一幀坐標(biāo)系作為世界坐標(biāo)系，將相機(jī)坐標(biāo)原點(diǎn)定義為世界坐標(biāo)系原點(diǎn)，同時將第一幀作為關(guān)鍵幀，記為在第二個關(guān)鍵幀確定之前中間所有點(diǎn)云通過配準(zhǔn)計(jì)算得到的旋轉(zhuǎn)平移矩陣換算到世界坐標(biāo)系下；

7.2)通過關(guān)鍵幀選取規(guī)則，產(chǎn)生第二幀關(guān)鍵幀，記為計(jì)算和的旋轉(zhuǎn)平移矩陣T₁，那么和之間的所有點(diǎn)云記為P_i，先計(jì)算與之間的旋轉(zhuǎn)平移矩陣，將P_i點(diǎn)云換算到坐標(biāo)系下，記為P_i'，再將P_i'通過旋轉(zhuǎn)平移矩陣T₁，換算到世界坐標(biāo)系下，該過程的依次迭代，表達(dá)式如下：

$P_0=P_i\&CenterDot;\underset{i=n}{\overset{1}{\&Pi;}}{T}_i---\left(7\right)$

P＝{p_i},i∈[1,n] (8)

Ti＝(R_i|t_i) (9)

其中P表示三維點(diǎn)p_i的集合，n表示點(diǎn)云數(shù)量，Ti表示4×4矩陣；

將當(dāng)前場景的坐標(biāo)全部轉(zhuǎn)換到世界坐標(biāo)系中，從而得到定位的效果。