激光雷達獲取的信息是和周圍物體之間的距離信息，在移動機器人尤其是自主移動機器人領域具有非常廣泛的應用，那我們就從移動機器人的自主導航開始聊吧。

移動機器人導航是指移動機器人依靠傳感器在特定環境中，按時間最優、路徑最短或能耗最低等準則實現從起始位置到目標位置的無碰撞運動。

傳統的移動機器人導航問題包含三大要素：地圖創建、定位和運動控制，通過三大要素，解決三個基本問題：我在哪里？我要去哪里？如何去？

機器人定位的目的是回答“我在什么地方？”這個基本的問題。

激光雷達采集數據之后通過一定的運算可以準確的計算出當前的絕對位置或者相對位置，那么就可以知道機器人在什么位置，當然有時地圖的創建和定位是同步進行的，例如SLAM算法（Simultaneous?localization and mapping），此時可以看成是一個探索問題。 ??

說了這么多，無非是想說激光雷達是多么的有用，多么的強大，而完成這些功能首先還是應該獲取激光雷達數據的特征，這就回到了特征提取這一主題上。

特征提取主要分為兩個步驟：區域分割和特征提取。區域分割階段主要完成特征模式的分類及識別確定，即確定特征屬于哪類模式，如直線，圓弧等，并確定屬于該特征模式的區域及區域內的激光數據點集。特征提取階段主要完成各類特征模式參數的確定以及特征點的提取。

一、區域分割

對于每一幀距離數據，首先把激光掃描點分割成不同的區塊。如果連續兩個掃描點的距離小于一個閾值，這兩個掃描點屬于同一個區塊。如果連續兩個掃描點的距離大于一個閾值，數據幀就從這個地方分割開。最后把一幀距離數據分割成幾個區塊。分割的區塊表示為 Ri(i=1,…,Q，其中 Q 是分割的區塊數)，每一個區塊包含 Ni個點。由于掃描點的分布并不是均勻的，通常情況下，離傳感器近的掃描點密度大一些，而遠離傳感器的掃描點密度小一些。所以進行距離數據分割時，應用自適應變閾值分割方法。例如當某個掃描點離傳感器中心的距離為 D 時，分割閾值選擇為 d，當掃描點離傳感器中心的距離為3D 時，閾值選擇為 3d。除此之外，也可以選用其它的線性或非線性函數來定義自適應分割閾值。總之，在不同的掃描點選用不同的分割閾值，以求距離數據的分割區塊能夠更好地與實際環境特征模型一致。如果激光的有效測距距離為 10 米，并且角度分辨率為 0.25度，則相鄰掃描點之間的距離最小為：2×10m×sin(0.125°)=0.0436m。根據該值可以設定合適的分隔閾值。

UTM-30LX 有效距離60m 角度分辨率0.25度，，但是一般對于室內應用一般不會超過10m

a、計算相鄰兩個點之間的距離Dj
b、判斷Dj和閾值delta的關系

如果Dj,大于閩值delta,則認為點(x,y)是兩個區域的分割點，閾值的選擇一般按照動態閾值的方式

c、（可選）判斷每個區域內的數據點的個數,如果某個區域包含數據點的個數小于等于三個,那么該區域被視為噪聲區域,舍棄這些噪聲點

//激光雷達區域分割效果，不同的區域用不同的顏色分割（同一種顏色并不代表在同一區域，只是顏色有限，幾種顏色在循環使用）

二、特征提取

激光雷達掃描的數據中，幾個重要的特征：撕裂點（breakPoint）、角點（Corner）、直線、圓弧等。

區域分割實際上就已經找到了數據中的撕裂點。折線也可以當成是一個特征，是直線加角點構成的特征。

直線作為一個很關鍵的特征在很多的論文中都是提取的關鍵，鑒于折線的是普遍存在的，那么角點的檢測同樣是一個難以回避的問題。那么我們就先提取角點，將所有的折線都打斷成直線和角點。

1、角點檢測

假設有一條折線，只有單個角點，那么我們可以采用多變形擬合方式確定角點的位置。首先將區域內的點擬合成一條直線，然后找出離直線最遠的點，這個距離如果大于某個閾值，則可以認為是折線，而該點就是折線的分割點，否者就是一條直線。

當某區域含有多個角點時，就需要采用迭代或者遞歸的方式，不斷的尋找角點-->拆分成兩段，循環進行，直到每個區域都不存在角點。

//多邊形擬合的方式確定是否存在角點，以及角點的位置

// 進行多邊形擬合： Points : 輪廓上的點 n -- 輪廓點數目 Eps -- 擬合精度 // 返回值： 若該輪廓段需要分段，則返回分段點在該輪廓點列中的索引，否則，返回 0 表示不需要分段 // 這里是整個算法計算復雜性最大的一個地方 // 為了提高程序運行效率，對點到直線的距離計算進行改進： // 多邊形擬合中的直線是由點列中的點決定的 // 為了計算點到直線的距離， // 采用坐標系旋轉，將直線旋轉到x軸方向，這樣點到直線的距離即為各個點 // 在坐標旋轉后的y值的絕對值 // 同時，坐標旋轉矩陣在該次運算中為定值，只需一次計算，不需要多次的開方或三角計算 int OpenRadar::PolyContourFit( int* X, int* Y, int n , float Eps ) // 根據輪廓點，用多邊形擬合該輪廓點 {double dis = sqrt((double)(((X[0] - X[n - 1])*(X[0] - X[n - 1])) + ((Y[0] - Y[n - 1])* (Y[0] - Y[n - 1]))));double cosTheta = (X[n- 1] - X[0]) / dis;double sinTheta = - ( Y[n- 1] - Y[0] )/dis;double MaxDis = 0;int i ;int MaxDisInd = -1;double dbDis;for(i = 1 ; i < n - 1 ; i++){// 進行坐標旋轉，求旋轉后的點到x軸的距離dbDis = abs( (Y[i] - Y[0]) * cosTheta + (X[i] - X[0])* sinTheta);if( dbDis > MaxDis){MaxDis = dbDis;MaxDisInd = i;}}if(MaxDis > Eps){return MaxDisInd;// cout << "Line 1 : " << endl;// cout << "Start :" << Points[0].x << " " << Points[0].y << " --- " << Points[MaxDisInd].x << " " << Points[MaxDisInd].y << endl;// cout << "角度： "<<180 * atan2(Points[0].y - Points[MaxDisInd].y , Points[0].x - Points[MaxDisInd].x ) / 3.1415926;// cout << "Line 2 :" << endl;// cout << "Start :" << Points[MaxDisInd].x << " " << Points[MaxDisInd].y << " --- " << Points[n - 1].x << " " << Points[n - 1].y << endl;// cout << "角度： "<< 180 * atan2(Points[n - 1].y - Points[MaxDisInd].y , Points[n - 1].x - Points[MaxDisInd].x ) / 3.1415926;}// else{// cout << "Line 1 : " << endl;// cout << "Start :" << Points[0].x << " " << Points[0].y << " --- " << Points[n - 1].x << " " << Points[n - 1].y << endl;// cout << "角度： "<<180 * atan2(Points[n - 1].y - Points[0].y , Points[n - 1].x - Points[0].x ) / 3.1415926;// }return 0; }
以上只能檢測具有單個角點的折線，任意個角點的折線采用了遞歸的方式，想提速的可以自己轉化為迭代的方式實現。

//將折線拆多段 int OpenRadar::BreakPolyLine(vector<int>& BreakedRadarRho,vector<double>& BreakedRadarTheta,vector<int>& SepRadarRho , vector<double>&SepRadarTheta) {int rho = 0;double theta = 0.0;int X[1200] = {0};int Y[1200] = {0};int rhoCopy[1200] = {0};double thetaCopy[1200] = {0};int pointCnt = 0;int lineCnt = 0;int N = 0;SepRadarRho.clear();SepRadarTheta.clear();Corners.clear();//進行多次迭代，將所有的折線都拆分成直線段vector<int>CornerIndex;int CornerCnt = 0;int tempIndex = 0;for (int i = 0; i < static_cast<int>(BreakedRadarRho.size());i++){rho = BreakedRadarRho.at(i);theta = BreakedRadarTheta.at(i);if (rho < 0){if (pointCnt > 200)//數目比較少的點直接拋棄{CornerIndex.clear();CornerCnt = FindCorners(CornerIndex,X,Y,0,pointCnt,200);if (CornerIndex.size() == 0){for (int k = 0 ; k < pointCnt;k++){SepRadarRho.push_back(rhoCopy[k]);SepRadarTheta.push_back(thetaCopy[k]);}SepRadarRho.push_back(-1);SepRadarTheta.push_back(1000.0);lineCnt++;}else{tempIndex = 0;for (int k = 0 ; k < pointCnt;k++){SepRadarRho.push_back(rhoCopy[k]);SepRadarTheta.push_back(thetaCopy[k]);if (k == CornerIndex.at(tempIndex)){SepRadarRho.push_back(-1);SepRadarTheta.push_back(1000.0);lineCnt++;if (tempIndex < static_cast<int>(CornerIndex.size()) -1){tempIndex++;} }}SepRadarRho.push_back(-1);SepRadarTheta.push_back(1000.0);lineCnt++;}}pointCnt = 0;continue;}X[pointCnt] = static_cast<int>(rho*cos(theta));Y[pointCnt] = static_cast<int>(rho*sin(theta));rhoCopy[pointCnt] = rho;thetaCopy[pointCnt] = theta;pointCnt++;}//cout<<"lineCnt: "<<lineCnt<<endl;return lineCnt; }
2、直線擬合

如果某區域不存在角點，并且點數據比較大，那么一般都是直線，（不是絕對，直線的判定方法后面的博客再寫）

直線擬合的原理比較簡單，實際就是一個最小二乘法，或者為了提高擬合的精度可以采用加權的最下二乘法，這里采用的是加權最小二乘。

//原始數據圖 藍點是雷達的位置

//擬合直線和角點圖，粗點是角點，粗實線是擬合出的直線

由于只對點數比價多的直線進行了擬合，上部分的直線并為畫出，實際上點數比較少的直線誤差也會大，并不是關鍵的特征。

源碼下載：http://download.csdn.net/detail/u012700322/9748910

原文地址： 點擊打開鏈接

總結

以上是生活随笔為你收集整理的激光数据的直线拟合的全部內容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。