本發(fā)明涉及點云匹配定位領(lǐng)域，具體涉及一種基于特征法向量的點云匹配定位方法。

背景技術(shù)：

1、智能機器人，例如建筑機器人、物流機器人以及清潔機器人等，需要在室內(nèi)進行工作的機器人，確定智能機器人其自身定位以及與其他智能機器人的相對位置，即進行精確定位，是十分重要的。然而，由于信號的嚴(yán)重衰減和多徑效應(yīng)，通用的室外定位設(shè)施，難以在室內(nèi)環(huán)境有效地工作，例如gps，gps可以定位出智能機器人當(dāng)前在哪一個建筑物，但是gps不能得到更精確的室內(nèi)位置，這需要更精密的地圖信息和更高的定位精度，同時復(fù)雜多變的室內(nèi)場景，對各個傳感器的性能也提出了更高的要求。

2、現(xiàn)有技術(shù)中，在室內(nèi)工作移動的智能機器人通常使用以下幾種方案進行定位：1、超聲波定位技術(shù)，通過發(fā)送和接收超聲波進行定位，但是存在鏡面反射、多徑效應(yīng)、受環(huán)境溫度影響、信號衰減明顯以及有限的波束角等缺點，不能充分獲取環(huán)境信息：2、視覺導(dǎo)航定位技術(shù)，通過采集并處理環(huán)境圖像，從而進行定位，但是對光照要求高，訓(xùn)練模型麻煩；3、基于輔助節(jié)點的定位技術(shù)，通過輔助節(jié)點的位置確定自身位置，但是輔助節(jié)點需要人工預(yù)先在現(xiàn)場設(shè)置并存儲，實用性較差；4、slam技術(shù)。其中，slam技術(shù)由于其具有較強的靈活性且不需要提前在場景中設(shè)置輔助設(shè)備的特點，同時其精度可以達到毫米級，被廣泛使用。slam技術(shù)分為vslam和lidar?slam，分別通過視覺點云和激光點云匹配算法，來進行地圖構(gòu)建和定位。但是，無論vslam還是lidar?slam技術(shù)，都是通過點云特征匹配的方式，典型的算法有icp(iterative?closest?point)與ndt(normal?distributions?transform)，其中ndt算法計算速度快，精度較icp低，常用于粗配準(zhǔn)；而icp算法由于需要尋找鄰近匹配點，耗時較高，精度較好，常用于精配準(zhǔn)，但是在某些場景下，例如室內(nèi)出現(xiàn)薄墻，且墻體兩面的距離較短，采用icp算法在通過距離尋找鄰近點時，由于點云的位置誤差，導(dǎo)致匹配墻面點云發(fā)生錯誤，影響定位精度。

技術(shù)實現(xiàn)思路

1、本發(fā)明針對上述存在的問題，提供一種基于特征法向量的點云匹配定位方法，它采用slam技術(shù)的icp算法，解決了該算法部分墻面存在的點云位置誤差的問題，提高了智能機器人的定位精度。

2、本發(fā)明的技術(shù)方案為：一種基于特征法向量的點云匹配定位方法，包括以下步驟：

3、1)通過slam技術(shù)獲得室內(nèi)墻面點云地圖；

4、2)啟動機器人的激光雷達，采集一幀二維點云數(shù)據(jù)，所述二維點云數(shù)據(jù)包括各個點云的橫坐標(biāo)x，縱坐標(biāo)y；

5、3)對采集的二維點云數(shù)據(jù)進行聚類，計算二維點云數(shù)據(jù)中各個點云之間的距離，并根據(jù)二維點云數(shù)據(jù)中各個點云之間的距離將二維點云數(shù)據(jù)分成多個類；

6、4)分別對每一類二維點云數(shù)據(jù)進行直線擬合，生成各個對應(yīng)的墻面矢量數(shù)據(jù)；

7、5)根據(jù)各類二維點云數(shù)據(jù)的墻面矢量數(shù)據(jù)，分別生成兩條垂直于各個墻面的法向量；

8、6)分別計算兩條垂直于墻面的法向量與激光雷達射向該墻面的射線的夾角，將夾角大于90°的法向量作為該墻面的法向量，并生成一幀三維點云數(shù)據(jù)，所述三維點云數(shù)據(jù)包括各個點云的橫坐標(biāo)x，縱坐標(biāo)y，以及法向量n；

9、7)根據(jù)icp點云匹配算法，將三維點云數(shù)據(jù)與室內(nèi)墻面點云地圖進行匹配，計算墻面點云的最近點，并計算該時刻機器人的位置和姿態(tài)信息，實現(xiàn)定位。

10、優(yōu)選地，所述步驟3)中，將各個點云之間距離小于5cm的點云分成一個類。

11、優(yōu)選地，所述步驟5)中，生成的兩條垂直于墻面的法向量的方向相反。

12、優(yōu)選地，步驟7)中，計算墻面點云的最近點的公式為：

13、

14、式中，distance為當(dāng)前幀所有點云到墻面最近點云的距離總和，x、y為室內(nèi)墻面點云地圖中的墻面點云位置坐標(biāo)，xref、yref為當(dāng)前幀點云的位置坐標(biāo)，ω為歸一化系數(shù)，n為特征法向量角度，nref為當(dāng)前幀點云的法向量。

15、優(yōu)選地，步驟7)中，計算該時刻機器人的位置和姿態(tài)信息的公式為：

16、

17、式中，ps表示當(dāng)前幀點云，pt表示匹配到的最近墻面點云，r、t代表機器人位置和姿態(tài)的旋轉(zhuǎn)和平移矩陣。

18、本發(fā)明的優(yōu)點在于：通過將室內(nèi)墻面進行分離，單獨處理，從而提取墻面矢量信息，并在每面墻打上法向量信息標(biāo)簽，在墻面點云中加入多維信息，使得點云在尋找鄰近點時，不單單依靠歐式距離，而是增加了特征法向量角度參數(shù)，將二維點云擴展為三維點云，并通過計算歸一化空間距離，區(qū)分了距離相近但不歸屬于同一平面的墻面點云，解決了現(xiàn)有技術(shù)中icp算法存在的相隔較近的薄墻面存在的點云位置匹配誤差的問題，增加了點云匹配的魯棒性和精確性，使定位算法可以穩(wěn)定輸出毫米級定位結(jié)果，提高了智能機器人的定位精度。而且本方法抗干擾性高、靈活性高、精度高且無需預(yù)先鋪設(shè)輔助設(shè)備。

技術(shù)特征：

1.一種基于特征法向量的點云匹配定位方法，其特征在于，包括以下步驟：

2.根據(jù)權(quán)利要求1所述的一種基于特征法向量的點云匹配定位方法，其特征在于：所述步驟3)中，將各個點云之間距離小于5cm的點云分成一個類。

3.根據(jù)權(quán)利要求1所述的一種基于特征法向量的點云匹配定位方法，其特征在于：所述步驟5)中，生成的兩條垂直于墻面的法向量的方向相反。

4.根據(jù)權(quán)利要求1所述的一種基于特征法向量的點云匹配定位方法，其特征在于：步驟7)中，計算墻面點云的最近點的公式為：

5.根據(jù)權(quán)利要求1所述的一種基于特征法向量的點云匹配定位方法，其特征在于：步驟7)中，計算該時刻機器人的位置和姿態(tài)信息的公式為：

技術(shù)總結(jié)
一種基于特征法向量的點云匹配定位方法，包括以下步驟：1)獲得室內(nèi)墻面點云地圖；2)啟動激光雷達，采集二維點云數(shù)據(jù)；3)對采集的二維點云數(shù)據(jù)進行聚類，計算二維點云數(shù)據(jù)中各個點云之間的距離，并根據(jù)二維點云數(shù)據(jù)中各個點云之間的距離將二維點云數(shù)據(jù)分成多個類；4)分別對每一類二維點云數(shù)據(jù)進行直線擬合，生成各個對應(yīng)的墻面矢量數(shù)據(jù)；5)分別生成兩條垂直于各個墻面的法向量；6)分別計算兩條法向量與激光雷達射向該墻面的射線的夾角，將夾角大于90°的法向量作為該墻面的法向量，并生成三維點云數(shù)據(jù)；7)將三維點云數(shù)據(jù)與室內(nèi)墻面點云地圖進行匹配，計算墻面點云的最近點，計算該時刻機器人的位置和姿態(tài)信息，實現(xiàn)定位。

技術(shù)研發(fā)人員：張曉威,楊洋,陳杰,李俊辰,燕明哲,謝川人,劉海,黃青龍,張文雄
受保護的技術(shù)使用者：重慶長安工業(yè)（集團）有限責(zé)任公司
技術(shù)研發(fā)日：
技術(shù)公布日：2025/1/6