激光传感器采集数据的障碍物特征提取

问题描述

激光测距仪能采集到的数据是一个180度的扇形范围内每隔0.5度的距离数据，每次采集有361个数据。怎么根据这些数据把空间中的障碍物建立出来，并把这些障碍物中的直线特征提取出来。

解决方案

1.在matlab中读取361个数据

由于可能出现像下图红框中这样的情况

所以需要计算每两个点之间的连续情况

对于任意相邻的需要每个点，如果它满足这个公式，那么这两个点属于一个线段。

2.获取每个点向前和向后满足直线要求的最远的点

这个在数学上的描述是

这是向后寻找的，向前寻找的同理。

3.然后计算每个点向前和向后引出的两条直线的夹角

f和b分别代表一个点往前和往后的向量，然后可以根据公式计算这两个向量的夹角。

4.寻找直线

寻找满足一定长度，并且夹角在-180度的连续点，这些点组成的图形一定是直线。

5.进行数据拟合

得到这些点的回归方程的斜率和截距。

在matlab中有polyfit这个函数可供调用。

说明

从第二步到第五步可能会执行多遍，这取决于在第一步中把数据分成几段。第五步可能会执行多遍，这取决于第四步中找到的直线有多少段。

代码实现：

代码写的比较粗糙，一些与寻找特征相关的参数还没有细调。但是注释清晰，而且有上面的解决方案中算法的流程，应该是学习特征提取的一个初步的入门程序。

%% 直线特征提取
% 作者：qianrenzhan
% QQ:1006325356
% 邮箱：[email protected]
% 时间：2017年11月28号

%% 载入数据并画图
clear
clc
data=load('juli_data9.txt');   %4
b=0:0.5:180;
c=(b'*pi)/180;
x=data.*cos(c);
y=data.*sin(c);
figure
plot(x,y);
grid on;

%% 找出间断点
%计算是否连续
different = zeros(1,360);
for i = 1:360
    different(i) = abs(data(i+1)-data(i))>(data(i)*sin(0.5*pi/180)/sin(9.5*pi/180)+0.015);
end
%由different把各段分离出来
range = [1 360];
head = zeros(1,20);
tail = zeros(1,20);
index = 1;
for i = range(1):range(2)
    if different(i)>0.5
        if tail(1) == 0
            head(index)=range(1);
        else
            head(index)=tail(index-1);
        end
        tail(index)= i;
        index = index+1;
    end
end
head(index)=tail(index-1);
tail(index)= range(2);
%去掉比较短的段
resulthead = zeros(1,20);
resulttail = zeros(1,20);
resultindex = 1;
for i = 1:index
    if (tail(i)-head(i))>3
        resulthead(resultindex) = head(i);
        resulttail(resultindex) = tail(i);
        resultindex = resultindex+1;
    end
end
resultindex = resultindex -1;  %连续的一共有几段

%% 在找到的连续的段间寻找特征，包括直线，圆弧，拐角等
%在每一段里面分别找直线
Kf = zeros(1,361);
Kb = zeros(1,361);
f = zeros(2,361);  %每个点往前的向量
b = zeros(2,361);  %每个点往后的向量
angle = zeros(1,361);  %每个点处的夹角
for findline = 1:resultindex
    %找直线
    %对于任何一个点往回找，直到不满足要求，记录这个点；往后找，直到不满足要求，记录这个点
    disdis = 0;
    yuzhi = 1;
    for i = resulthead(findline):resulttail(findline)
        while(1)
            Kb(i) = Kb(i) + 1;
            if i+Kb(i)>361
                break;
            end
            disdis = 0;
            for j = i:i+Kb(i)-1
                disdis = disdis + dis(x(j),y(j),x(j+1),y(j+1));
            end
            if dis(x(i),y(j),x(i+Kb(i)),y(i+Kb(i)))< disdis -yuzhi
                break;
            end
        end
    end

    for i = resulthead(findline):resulttail(findline)
        while(1)
            Kf(i) = Kf(i) + 1;
            if i-Kf(i)<1
                break;
            end
            disdis = 0;
            for j = i:-1:i-Kf(i)+1
                disdis = disdis + dis(x(j),y(j),x(j-1),y(j-1));
            end
            if dis(x(i),y(j),x(i-Kf(i)),y(i-Kf(i)))< disdis -yuzhi
                break;
            end
        end
    end

    % 计算每个点的夹角
    for i = resulthead(findline):resulttail(findline)
        if i-Kf(i) ==0
            continue;
        end
        if i+Kb(i) > 361
            continue;
        end
        f(1,i)= x(i - Kf(i))-x(i);
        f(2,i)= y(i - Kf(i))-y(i);
        b(1,i)= x(i + Kb(i))-x(i);
        b(2,i)= y(i + Kb(i))-y(i);
        fenzi = f(1,i) * b(1,i) + f(2,i) * b(2,i);
        fenmu = sqrt(f(1,i)^2+f(2,i)^2) * sqrt(b(1,i)^2+b(2,i)^2);
        angle(i) = fenzi/fenmu;
    end
    % figure
    % plot(angle)     %每个点的夹角

    %通过每个点的夹角，寻找直线，直线是值为-1，并且有一定长度
    %这里假定寻找长度超过20的直线，一直小于-0.95
    result2head = zeros(1,10);
    result2tail = zeros(1,10);
    result2index = 1;
    currentlocation = 0;
    linelength = 3;        %满足要求的线的最短长度

    for i = resulthead(findline):resulttail(findline)
        if currentlocation >i
            continue;
        end
        dotcount = 0;
        for j = i:361
            if angle(j) < -0.85     % 夹角
                dotcount = dotcount +1;
            else
                break;
            end
        end
        if dotcount > linelength
            %记录
            resulthead(result2index) = i;
            resulttail(result2index) = j;
            result2index = result2index + 1;
            currentlocation = j;
        end
    end

    %拟合找到的result2index条直线
    for plotline = 1:result2index-1
        p = polyfit(x(resulthead(plotline):resulttail(plotline)),y(resulthead(plotline):resulttail(plotline)),1);

        %2个对角点
        tempminx = min(x(resulthead(plotline)),x(resulttail(plotline)));
        tempmaxx = max(x(resulthead(plotline)),x(resulttail(plotline)));
        tempminy = min(y(resulthead(plotline)),y(resulttail(plotline)));
        tempmaxy = max(y(resulthead(plotline)),y(resulttail(plotline)));
        %实际的对角点x坐标
        xxx1 = (tempmaxy - p(2))/p(1);
        xxx2 = (tempminy - p(2))/p(1);

        %重新框定的x范围
        tempminxxx = max(tempminx,min(xxx1,xxx2));
        tempmaxxxx = min(tempmaxx,max(xxx1,xxx2));

        %判断是否选择重新框定的范围
        %？？？？不好确定

        xx = tempminxxx:tempmaxxxx;
        xx = tempminx:tempmaxx;
        yy = xx*p(1)+p(2);
        hold on
        plot(xx,yy,'LineWidth',3)
        hold on
    end
end

用到的一些自定义函数

拟合函数

数学原理如下：

matlab实现的函数：

function [r,angle] = myfit(data,low,high)
temp1 = 0;
temp2 = 0;
temp3 = 0;
temp4 = 0;
for i = low:high
    for j=low:high
        temp1 =  temp1 + data(i)*data(j)*cos((i-1)/2*pi/180)*sin((j-1)/2*pi/180)/(high-low)*2;
        temp2 =  temp2 + data(i)*data(j)*cos((i-1)/2*pi/180 + (j-1)/2*pi/180)/(high-low)*2;
    end
end
for i=low:high
    temp3 =  temp3 + data(i)*data(i)*sin(2*(i-1)/2*pi/180);
end

angle = atan((temp3-temp1)/(temp3-temp2))/2;
for i = low:high
    temp4 = temp4+data(i)*cos((i-1)/2*pi/180 - angle)/(high-low);
end
r = temp4;

计算欧氏距离函数

function tempdis = dis(x,y,x1,y1)
tempdis = sqrt((y1-y)^2+(x1-x)^2);

结果展示

原始数据绘图结果

直线特征提取后效果

关于本程序用到的原始数据和源码文件，可以在csdn上下载，搜索本文标题即可。