单线激光雷达SLAM（二）特征识别——断点、角点

单线激光雷达学习（二）

特征识别——断点、角点

• 斜率差算法——使用相邻点之间斜率的变化关系来提取特征点，再通过特征点对点云进行分割来提取线段，本算法不需要迭代，工作量较小，而且对阈值不敏感，准确率也较高。既克服了多数序惯类算法都存在无法对相交直线进行分割的缺点，也克服了递归算法计算量过大和因阈值选取不当过分割和欠分割的缺点。

1 斜率差法原理

以激光雷达为圆心，利用相邻点的斜率差值来区分断点、角点和散点。

图1 扫描点示意图

如图1所示，从$O$点向直线L以等角度Δθ的间隔画直线，交点依次为$Q_1$，$Q_2$,$Q_3$,$Q_4$…,$O$点到交点的长度依次为$d_1,d_2,d_3,d_4$…。然后，由$Q_1$点向$O{Q}_2$做垂线相交于$Q_1^{’}$点，则${\omega }_1$为直线L与$Q_1{Q}_1^{’}$的夹角，同理可得${\omega }_2$和${\omega }_3$。当$\varDelta \theta $很小时，$\sin \theta \approx \theta $，故有
$$\tan {\omega }_1=\frac{Q_{i+1}Q{’}_i}{Q_{i}Q{’}_i}\approx \frac{d_{i+1}-d_i}{d_i\Delta \theta }i=1,2,3$$
令第i点的斜率$k_i$为
$$k_i=\frac{d_{i+1}-di}{di\Delta \theta }$$
因为由几何关系得: $\omega _3=\omega _2+\varDelta \theta =\omega _1+2\varDelta \theta $
$$\tan {\omega }_2-\tan {\omega }_1=\tan \theta \left(1+\tan {\omega }_2+\tan {\omega }_1\right)$$
且因为$\tan \theta \approx 0$，故

$$k_3-k_2\approx k_2-k_1=\tan {\omega }_2-\tan {\omega }_1\approx 0$$

如图2、图3所示，当相邻点的斜率$k_i$值差距很小时，就认为它们位于同一条直线上，否则斜率突变点为断点、角点或散点。

• 图2 断点处$\Delta k$分布示意图

断点，$\Delta k_{\left(i-1\right)}$和$\Delta k_{\left(i\right)}$处峰值非常明显，且这两处峰值的符号相反，由此可以判断该点为断点。

• 图3 角点处$\Delta k$分布示意图

角点，$\Delta k_{\left(i\right)}$处有较明显的峰值，且呈现为独点单峰，由此可以判断出该点为角点。

2 代码实现

2.1 角度化弧度

在上一节，我们已经实现了将雷达雷达扫描的数据提取出来，并存储在Buffer_ang[]数组和Buffer_dis[]数组中，因为C语言三角函数（sin、cos、tan）等使用的都是弧度制，而不是角度制，所以，第一步我们需要将角度Buffer_ang[]化成弧度制。

void Ang_change(void) //角度化弧度
{
    int i;
    printf("Data_change\r\n");
    for (i = 0; i < DATASIZE; i++)
    {
        Buffer_ang[i] = Buffer_ang[i] + CAR_ANGLE;
        if (Buffer_ang[i] > 360)
        {
            Buffer_ang[i] = Buffer_ang[i] - 360;
        }
        Buffer_ang[i] = Buffer_ang[i] * 0.0174533; //角度化弧度公式
    }
}

同理，可以提前将所有极坐标系化为笛卡尔坐标系，如此，计算斜率与斜率差时就可以直接用坐标计算

#define MAXLine 1008
float x[MAXLine], y[MAXLine];

void Data_change(void)
{
    int i;
    for (i = 0; i < MAXLine; i++)
    {
        x[i] = Buffer_dis[i] * cos(Buffer_ang[i]);
        y[i] = Buffer_dis[i] * sin(Buffer_ang[i]);
    }
}

2.2 斜率差滤波

根据斜率差算法的原理，编写其代码实现，将不符合斜率差的点删除，仅保留同一条直线特征上的点。

	for (i = 0; i < MAXLine; i++)
    {
        k[i] = (y[i] - y[i + 1]) / (x[i] - x[i + 1]);
    }
    for (i = 0; i < MAXLine; i++)
    {
        dk[i] = k[i] - k[i + 1];
    }
    for (i = 0; i < MAXLine; i++)
    {
        jd1[i] = fabsf(dk[i + 1] - dk[i]);
        jd2[i] = fabsf(dk[i + 2] - dk[i + 1]);
    }
    for (i = 0; i < MAXLine; i++)
    {
        if (fabsf(dk[i]) > 0.5 && fabsf(dk[i + 1]) > 0.5 && fabsf(dk[i + 2]) > 0.5 && jd1[i] > 1 && jd2[i] > 1)
        {
            det_flag[i] = 1;
        }
    }

    for (i = 0; i < MAXLine; i++)
    {
        printf("%6.2f  %6.2f  %6.2f  %6.2f   %d\r\n", x[i], y[i], k[i], dk[i], det_flag[i]);
    }

3 效果

如果觉得还不错，记得点个赞👍ye