单线激光雷达SLAM（一）数据提取

雷达介绍与数据提取

1 雷达介绍

1.1 结构

本文使用的是镭神公司的单线 TOF 近距离机械式激光雷达M10.

M10 系列激光雷达采用 TOF(time of flight)方案，能够对周围 360°环境进行二维扫描探测。该系列激光雷达内部使用无线供电和光通讯，测量重频为 10KHz。 设计探 测精度达到±3cm，最大量程从 10 米。主要应用于室内服务机器人、AGV、清扫消杀机器人、无人机等精确定位和避障的应用场合。

1.2参数

M10 对外物理接口为 HY2.0-6P，实现系统供电和数据通信。

M10 的结构中心点为极点，定义顺时针为正， 出线端为零度角。

1.3 上位机使用

（1）连接好线，电脑—USB-TTL模块—Lidar主体，并打开电源开关，电脑需提前安装好"CP2101"驱动。

（2）双击  图标，打开上位机，点击【雷达连接】，出现点云如下图所示

其余功能自行研究，都很简单

保存CSV功能，后面会介绍

1.4 通讯协议

M10 系列雷达的点云数据输出和查询以及各种状态的配置都是通过串口来进行。点云 输出的协议为 90 个字节，包含了角度，转速和距离；

1 串口配置

波特率：460800 bps

校验位：NONE

数据位：8

停止位：1

数据格式：HEX

2 点云协议

1. A5 5A 为帧头

2. 帧头后面就是角度和转速参数，从第 6 个字节开始，就是 42 个距离参数

3. 数据字节说明 angle_CodedDisc: 角度参数，一共两个字节,高位在前，低位在后，为从 0 度（360 度） 开始每 15 度增加的角度信息。例如：0x8C 0xA0 即十进制 36000 表示角度为 360 度，也就 是 0 度； speed: 转速参数，一共两个字节,高位在前，低位在后，表示雷达从一个齿转到下一个 齿所需要的时间计数值，它和转速的计算公式为：转速=2500000/speed。例如：0x10 0x68 即十进制 4200 则转速为每分钟 595.239 转，也就是 10HZ； Distance: 距离参数，一共两个字节,高位在前，低位在后，表示 15 度差分 42 个点之 后对应角度的距离值，单位是毫米。例如：0x13 0x88 即十进制 5000 那么该角度对应的距 离值就是 5 米。

4. Lidar扫描1圈360°共24组数据，每组90个字节，共计1008个点。

2 数据提取

2.1 上位机提取数据

使用上位机保存CSV数据，可以选择特定的角度范围，最大0~360°

csv数据有以下几个内容需要注意：

Points_X：lidar自身坐标系X坐标

Points_Y：lidar自身坐标系Y坐标

Azimuth：当前点角度

Distance：当前点距离lidar距离

2.2 C##读取数据

实际上，CSV数据很难运用，因为我们一般不在excel里面处理数据，而且后期的激光雷达特征识别、特征提取、特征定位等也不可能在excel中进行，所以我们可以用编译器（VScode、Clion）等，先将数据读进数组里，再进行处理，这里提供一个C语言写的读取数据的程序，以供参考。

// 
// Created by Dakhin on 2022/7/28.
//
//***需先将CSV里x,y坐标，存进txt文件
//***读取所有x,y坐标，并存进数组x[],y[],并从端口打印出来。
​
#include "stdio.h"
#include "string.h"
#include "math.h"
​
#define MAXLine 1008 //数据有多少行，就填多少
​
int main()
{
    FILE *fp;
    int len, i;
    char buf[MAXLine];
    float x[MAXLine], y[MAXLine];//长度1008,2列
​
    int cha[MAXLine];
    int index = 0;
    double ave = 0;
    if ((fp = fopen("E:\\filter-1.txt", "r")) == NULL) //文件地址
    {
        perror("fail to read");
        return 0;
    }
    while (fgets(buf, MAXLine, fp) != NULL)
    {
        len = strlen(buf);
        buf[len - 1] = '\0';
        sscanf(buf, "%f  %f", &x[index], &y[index]);//格式必须严格对应txt文件（包括空格、字符）
        //从一个字符串中读进于指定格式相符的数据。利用它可以从字符串中取出整数、浮点数和字符串。
        index++;    //index成为总行数
    }
    fclose(fp);
    
    ///打印所有数据
    for (i = 0; i < index; i++)
        printf("%.2f  %.2f\n", x[i], y[i]);    //打印所有的数据
        
    return 0;
}

2.3 单片机读取数据

本文采用的是STM32F103ZET6型号芯片，使用串口2接收lidar点云数据，因为lidar数据是一组一组发过来的，每组90个字节，所以需要设置相应的标志位以示区分，核心代码如下：

// 
// Created by Dakhin on 2022/7/28.
// 
//***通过单片机串口2将lidar一圈数据（1008个点的角度和距离）存进数组Buffer[].
​
u8 flag = 0;                     //一圈的数据标志位
u8 zuhao = 0, number = 0, s = 0; //组号，计数器（0-89），s:帧头状态
u8 Buffer[2160] = {0};           //90*24
u8 DATA_NUM = 24;                //共24组
float Buffer_ang[1008] = {0};
float Buffer_dis[1008] = {0};
​
​
void USART2_IRQHandler(void)
{
    u8 res;
    if (USART_GetITStatus(USART2, USART_IT_RXNE) != RESET) //接收到数据
    {
        USART_ClearITPendingBit(USART2, USART_IT_RXNE);
        res = USART_ReceiveData(USART2);
        
        if (flag == 0) //一圈
        {
            if ((res == 0xA5) && (s == 0))
            {
                s = 1;
                Buffer[zuhao * 90] = res;
                number = number + 1;
            }
            else if ((res == 0x5A) && (s == 1))
            {
                s = 2;
                Buffer[zuhao * 90 + 1] = res;
                number = number + 1;
            }
            else if ((s == 2) && (number < 90))
            {
                Buffer[zuhao * 90 + number] = res;
                number = number + 1;
                if (zuhao != DATA_NUM - 1) //不是最后一次组
                {
                    if (number == 89)
                    {
                        number = 0;
                        s = 0;
                        zuhao = zuhao + 1;
                        //printf("zuhao=%d\r\n", zuhao);
                    }
                }
                if (zuhao == DATA_NUM - 1) //最后一组
                {
                    if (number == 89)
                    {
                        number = 0;
                        s = 0;
                        zuhao = 0;
                        flag = 1;
                    }
                }
            }
        }
        else
        {
            s = 0;
            number = 0;
            zuhao = 0;
        }
    }
}
​
void Data_get(void)
{
    int i, j;
    double Ang_pre;           //每组的初始角度
    double Ang_add = 0.35714; // 15度均分42个点
    printf("\r\nData_get\r\n");
    for (i = 0; i < DATA_NUM; i++) //每帧循环24组，每组15°，共360°
    {
        Ang_pre = (Buffer[i * 90 + 2] * 256 + Buffer[i * 90 + 3]) / 100; //该组角度，buf[2]高8位，buf[3]低八位
​
        for (j = 0; j < 42; j++)
        {
            Buffer_ang[i * 42 + j] = Ang_pre + j * Ang_add; //当前点的角度，24组*42个/组=1008个点
            if (Buffer_ang[i * 42 + j] > 360)
                Buffer_ang[i * 42 + j] = Buffer_ang[i * 42 + j] - 360;
​
            Buffer_dis[i * 42 + j] = (Buffer[i * 90 + 6 + 2 * j] * 256 + Buffer[i * 90 + 6 + 2 * j + 1]) / 10.0; //距离,单位cm
​
            if (Buffer_dis[i * 42 + j] == 0)
                Buffer_dis[i * 42 + j] = 9999; //无效点距离设置无限远
        }
    }
    flag = 0;
}

如此，每次调用 Data_get( )函数，便可以取一帧lidar数据，且已将角度和距离两个参数分别存进两个数组 Buffer_ang[ ] 和 Buffer_dis[ ]，方便后续的开发。

如需下载源码，链接：百度网盘 请输入提取码 提取码：c6kj

如果你觉得还不错，请点个赞鼓励一下( + _ + )