搜索
发新帖本帖赏金 200.00元(功能说明)我要提问
返回列表

[STM32] 手持建图仪--基于STM32F103+思岚A1激光雷达的扫描仪

[复制链接]
21012|18
手机看帖
扫描二维码
随时随地手机跟帖
呐咯密密|  楼主 | 2021-10-11 17:00 | 显示全部楼层 |阅读模式
本帖最后由 呐咯密密 于 2021-10-12 13:52 编辑

[url=home.php?mod=space&uid=760190]@21小跑堂 #申请原创#[/url]
前言
一个朋友在做服务机器人项目,用到思岚的激光雷达,于是便把淘汰的A1M8雷达送我一个,本着拿到啥就玩啥的态度,必须整一波。其实激光雷达还是搭配ROS才能发挥最大的作用,奈何资源有限,实力不足,只能依靠STM32开发板做一个及其简陋的地图扫描。
思岚A1M8激光雷达
829916163ebb8ecb64.png
这款激光雷达属于低成本的360度激光扫描测距雷达,外置电机,使用皮带带动雷达转台转动,实现360度的测距扫描,电机的转速由MCU发送PWM控制。
外部系统通过 TTL 电平的 UART 串口信号与 RPLIDAR 测距核心进行通讯。通过本文档定义的通讯协议,外部系统可以实时获取 RPLIDAR 的扫描数据、设备信息、设备健康状态。并且通过相关命令调整 RPLIDAR 的工作模式。
按照不同的请求类型, RPLIDAR 具有三种不同的请求/应答模式:
标准的单次请求-单次应答模式
522306163ecf2c319f.png
单次请求-多次应答模式
232376163ed093ca67.png
单次请求/无应答模式
876736163ed1cb7cc2.png
对于停止扫描、重启测距核心这类请求命令, RPLIDAR 采用单次请求,但不做应答的通讯模式。此时外部系统需要在发送请求后等待一定的时间,待RPLIDAR 完成了上一次请求操作后方可继续执行下一次请求。否则第二次的请求将可能被 RPLIDAR 丢弃。
在此次应用中,主要采用后两种请求/应答模式,使用单次请求-多次应答模式采集测距数据,使用单次请求/无应答模式停止采样,进行数据的处理。
在单次请求-多次应答模式采集测距数据时,MCU发送采集指令,雷达会先回复一条起使应答报文,之后便会循环回复数据应答报文。
937206163eea356f60.png
请求报文及起始应答数据格式如下:
698616163eee46ac1f.png
在回复起始应答之后,雷达会循环回复测距数据。长度为5bytes.
136256163ef3d9993e.png 496216163ef47ef9fa.png
例如测距数据为 3E D5 16 77 06。
第一个字节:3E,二进制为:0011 1110。代表信号质量为0x0f。信号质量不为零代表数据有效,起始标志位为0,代表不是新的一圈,该标志位只有在新的一圈的第一帧数据才会置一,该圈内的其余数据改为依旧是0。
第二个字节:D5,角度数据低七位。
第三个字节:16,角度数据高八位,加上第二个字节的低七位等于166A,再右移一位得B35。实际角度=835/64=44°,该角度表示与雷达零度的顺时针偏移角度,如下图。
第四个字节:77,距离数据低八位。
第五个字节:06,距离角度高八位。则此时距离为0x0677/4 = 413mm。
496826163f1b6c1f03.png
激光雷达测试:
接线:
雷达             MCU
GND----------->GND
RX------------->TX
TX------------->RX
V5.0----------->5V
GND----------->GND
MOTOCTL---->PWM
VMOTO------->5V
首先测试使用串口助手进行数据采集,这里将MOTOCTL接到5V电源,直接以最高速度进行采样。串口助手发送A5 20,可以看到数据滚动。
501936163f38668742.png
其中开头的七位数据对应起始应答,后面每5个字节一组,对应测距数据。雷达无损坏,开始连接开发板调试。
MCU代码:
既然是USART通信,我们先初始化USART,使用串口接收中断接收数据。
void USART_Config(void)
{
        GPIO_InitTypeDef GPIO_InitStructure;
        USART_InitTypeDef USART_InitStructure;
        NVIC_InitTypeDef NVIC_InitStructure;
        // 打开串口GPIO的时钟
        DEBUG_USART_GPIO_APBxClkCmd(DEBUG_USART_GPIO_CLK, ENABLE);
        
        // 打开串口外设的时钟
        DEBUG_USART_APBxClkCmd(DEBUG_USART_CLK, ENABLE);

        // 将USART Tx的GPIO配置为推挽复用模式
        GPIO_InitStructure.GPIO_Pin = DEBUG_USART_TX_GPIO_PIN;
        GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP;
        GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
        GPIO_Init(DEBUG_USART_TX_GPIO_PORT, &GPIO_InitStructure);

  // 将USART Rx的GPIO配置为浮空输入模式
        GPIO_InitStructure.GPIO_Pin = DEBUG_USART_RX_GPIO_PIN;
        GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING;
        GPIO_Init(DEBUG_USART_RX_GPIO_PORT, &GPIO_InitStructure);
        
        //Usart1 NVIC 配置
        NVIC_InitStructure.NVIC_IRQChannel = USART1_IRQn;
        NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority=2 ;//抢占优先级3
        NVIC_InitStructure.NVIC_IRQChannelSubPriority = 3;                //子优先级3
        NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;                        //IRQ通道使能
        NVIC_Init(&NVIC_InitStructure);        //根据指定的参数初始化VIC寄存器
        
        // 配置串口的工作参数
        // 配置波特率
        USART_InitStructure.USART_BaudRate = DEBUG_USART_BAUDRATE;
        // 配置 针数据字长
        USART_InitStructure.USART_WordLength = USART_WordLength_8b;
        // 配置停止位
        USART_InitStructure.USART_StopBits = USART_StopBits_1;
        // 配置校验位
        USART_InitStructure.USART_Parity = USART_Parity_No ;
        // 配置硬件流控制
        USART_InitStructure.USART_HardwareFlowControl = USART_HardwareFlowControl_None;
        // 配置工作模式,收发一起
        USART_InitStructure.USART_Mode = USART_Mode_Rx | USART_Mode_Tx;
        // 完成串口的初始化配置
        USART_Init(DEBUG_USARTx, &USART_InitStructure);

        USART_ITConfig(USART1, USART_IT_RXNE, ENABLE);//开启接收中断
        USART_ClearFlag(USART1,USART_FLAG_TC|USART_FLAG_RXNE);
//    USART_DMACmd(USART1, USART_DMAReq_Rx, ENABLE);  // 开启串口DMA接收
        // 使能串口
        USART_Cmd(DEBUG_USARTx, ENABLE);            
}
然后编写中断服务函数
void USART1_IRQHandler(void)                        //串口1中断服务程序
{

        if(USART_GetITStatus(DEBUG_USARTx,USART_IT_RXNE)!=RESET)
        {
                rxbuff[Res] = USART_ReceiveData(DEBUG_USARTx);
                Res++;
                if(Res==1807)
                {
                        USART_ITConfig(USART1, USART_IT_RXNE, DISABLE);//开启接收中断
                        USART_SendData(USART1,0xA5);
                        while (USART_GetFlagStatus(DEBUG_USARTx, USART_FLAG_TXE) == RESET);        
                        USART_SendData(USART1,0x25);
                        while (USART_GetFlagStatus(DEBUG_USARTx, USART_FLAG_TXE) == RESET);
                        Data_Processing();
                        Res=0;
                        ClearFlag=1;
                }
//                MYDMA_Enable(DMA1_Channel5);//开始一次DMA传输!
        }
}
在串口中断服务函数中,需要采集1807个数据(360个测距点*5字节+起始7个字节)。我采用全速采样,即MOTOCTL直接接5V,这里采集360个数据点其实不止一圈的数据,但是因为每个360度都有无效数据,多采集点可以使后期画图更完整。在提取数据使用EXCEL分析以后,全速转一圈大概采样258个点左右,这个数据无法固定,每一圈采样数均不一样。
在采集数据完成后我们需要关闭采样,因为STM32F103的数据处理能力并不理想,这里需要一定的时间,于是通过串口发送指令A5 25让雷达停止采样,同时调用函数Data_Processing();进行数据处理以及在屏幕上画点。这里要注意,雷达在停止采样前会将最后一帧数据发送完整,我们在发送停止指令的期间,雷达可能已经在准备下一帧数据,在发送完停止指令之后,可能会存在这一帧数据的最后一位未触发中断,但是串口的数据寄存器中已经保存了这位数据,且已经改变了标志位,所以在下一次启动采样时会导致收到的第一个数据是上一次未接收完的数据。这个在进行处理。
在此之前我们还需要一个触发采样的按键。按下按键后触发采样,为了保持持续采样,在串口接收中断关闭采样并处理完数据后,可在主循环中再次开启。
void KEY1_IRQHandler(void)
{
        u8 RX;
  //确保是否产生了EXTI Line中断
        if(EXTI_GetITStatus(KEY1_INT_EXTI_LINE) != RESET)
        {
        
                USART_SendData(USART1,0xA5);
                while (USART_GetFlagStatus(DEBUG_USARTx, USART_FLAG_TXE) == RESET);        
                USART_SendData(USART1,0x20);
                while (USART_GetFlagStatus(DEBUG_USARTx, USART_FLAG_TXE) == RESET);                        
                USART_ITConfig(USART1, USART_IT_RXNE, ENABLE);//开启空闲中断
                Res=0;
    //清除中断标志位
                EXTI_ClearITPendingBit(KEY1_INT_EXTI_LINE);     
        }  
}
733256163f98a4a3e7.png
数据处理如下:
void Data_Processing(void)
{
        u16 i,j=7;
        u8 quality;

        for(i=0;i<360;i++)
        {
                quality = rxbuff[j]>>2;
                if(quality!=0)
                {
                        data_rage1 = rxbuff[j+2]<<8;
                        data_rage2 = rxbuff[j+1];
                        angle[i] = (data_rage1 | data_rage2)>>1;
                        angle[i] = angle[i];
                        data_rage1 = rxbuff[j+4]<<8;
                        data_rage2 = rxbuff[j+3];
                        distance[i] = (data_rage1|data_rage2);        
//                Usart_SendHalfWord(USART2,angle[i]);        
//                Usart_SendHalfWord(USART2,distance[i]);        
                }
               
                j = j+5;
        }

        if(i==360)
        {
                LCD_Draw();
                i=0;
//               
        }
}

从串口缓存数组中取出角度值和距离值,保存在数组angle[]和distance[]中。当360个数据点处理完,调用画图函数进行屏幕绘制。
void LCD_Draw(void)
{
        u16 i;
         ILI9341_Clear(0,0,LCD_X_LENGTH,LCD_Y_LENGTH);        /* 清屏,显示全黑 */
        LCD_SetTextColor(RED);
        for(i=0;i<360;i++)
        {
                x=return_x(angle[i], distance[i]/scale);
                y=return_y(angle[i], distance[i]/scale);
//                ILI9341_DrawLine(120,160,x,y);
                ILI9341_SetPointPixel(x,y);
                /*为了点更清楚,在点周围画辅助点*/
                ILI9341_SetPointPixel(x+1,y+1);
                ILI9341_SetPointPixel(x-1,y-1);
                ILI9341_SetPointPixel(x-1,y+1);
                ILI9341_SetPointPixel(x+1,y-1);
                ILI9341_SetPointPixel(x+2,y+2);
                ILI9341_SetPointPixel(x-2,y-2);
                ILI9341_SetPointPixel(x-2,y+2);
                ILI9341_SetPointPixel(x+2,y-2);               
        }        
}
画点直接调用野火的库,其中参数scale为地图放大倍数,因为屏幕大小有限,为了适应不同大小的地图,使用该参数进行地图放大。
return_x,return_y函数是将测距点转换为屏幕坐标。原函数如下:
//x坐标转换函数
//ang:0~359度数,    d:距离
//返回:x坐标0~239
float return_x(u16 ang, signed int d)
{
        float x;
        double ang_deg,dd;
        ang_deg = ang/64;
        dd = d/4;
        if(dd!=0)
        {
                if(ang_deg <= 90)
                {
                        
                        x = dd*sin(ang_deg)+120;//角度转换成弧度
                }
                else if((ang_deg > 90) && (ang_deg <= 180))
                {
                        
                        x = 120+dd*sin(ang_deg);
                }
                else if((ang_deg > 180) && (ang_deg <= 270))
                {

                        x = 120-dd*sin(ang_deg);
                }
                else if((ang_deg > 270) && (ang_deg <= 359))
                {

                        x = 120-dd*sin(ang_deg);
                }        
        
        }

        if(x > 239)
                x = 239;
        if(x < 0)
                x = 0;
        return x;
}
//y坐标转换函数
//ang:0~359度数,    d:距离
//返回:y坐标0~319
float return_y(u16 ang, signed int d)
{
        float y,dd;
        double ang_deg;
        ang_deg = ang/64;
        dd = d/4;
        if(dd!=0)
        {
                if(ang_deg <= 90)
                {

                        y = 160-dd*cos(ang_deg);//角度转换成弧度
                }
                else if((ang_deg > 90) && (ang_deg <= 180))
                {
               
                        y = dd*cos(ang_deg)+160;
                }
                else if((ang_deg > 180) && (ang_deg <= 270))
                {

                        y = dd*cos(ang_deg)+160;
                }
                else if((ang_deg > 270) && (ang_deg <= 359))
                {
               
                        y = 160-dd*cos(ang_deg);
                }        
        }

        if(y > 319)
                y = 319;
        if(y < 0)
                y = 0;
        return y;
}

此时在屏幕上便可绘制出雷达采样点
30546163fc435df18.png
这里是动态监测的,但是动态图在后面补,后续也会优化绘图和数据处理,这里先给出大致的效果。时间有限,目前先这样,后面会完善此贴。

游客,如果您要查看本帖隐藏内容请回复

使用特权

评论回复

打赏榜单

21小跑堂 打赏了 200.00 元 2021-10-13
理由:恭喜通过原创奖文章审核!请多多加油哦!

相关帖子

呐咯密密|  楼主 | 2021-10-12 13:45 | 显示全部楼层
更新贴,从正文可以看出该屏幕的显示的扫描地图是圆形,但是我的房间却不是圆的。这个地图明显是有问题。但是无论无如何调整算法,显示到屏幕上的测距点总是不正确。分析得出大概问题是出在屏幕上,因为屏幕分辨率有限,测的的尺寸为了能在屏幕上显示,不得已将尺寸缩小几十倍,导致数据严重失真。于是我将测距数据导出研究。此次用已知大小的物料箱将雷达倒扣在里面。物料箱的尺寸大约为36cm*45cm。手头没有卷尺,用一个小尺子量的,所以只是大概值。
8465361651e67469cd.png
雷达位于箱子中间,那么到最短到箱壁两边的距离大概是18和22.5厘米。
测试开始:

3368061651c92d4b4c.png
使用串口二将原始角度和距离值打印到串口助手:
959861651cc637c37.png
使用world文档将数据整理:
7498361651cfa98356.png
然后复制数据到excle,进行数据处理,将角度和距离分别提取;
981961651dc93e6ea.png

根据真实角度值选取一整圈距离数据(mm),插入雷达图:
2201561651f6c36a34.png
此图因为有无效点,取出零点以及错误点后得到如下图。
1016961651fbd76ac1.png
可以看到此时的雷达图很接近我们的箱子真实形状,距离大小也符合箱子尺寸。此时才可以算作成功,虽然屏幕任然无法完整显示扫描地图,但是数据的处理并无问题,单片机速度跟不上,屏幕分辨率也不够,难受啊。。

使用特权

评论回复
呐咯密密|  楼主 | 2021-10-12 13:49 | 显示全部楼层
源代码已上传在主帖,有兴趣的可下载调试。开发板是野火的指南者。

使用特权

评论回复
jstiger| | 2021-10-12 15:58 | 显示全部楼层
楼主牛,不错。

使用特权

评论回复
怀揣少年梦| | 2021-10-14 08:27 | 显示全部楼层
楼主,太强了

使用特权

评论回复
653949915| | 2021-10-14 10:09 | 显示全部楼层
6666666666666666

使用特权

评论回复
yjwpm| | 2021-10-14 13:02 | 显示全部楼层
66666666666666666666666666666

使用特权

评论回复
waass| | 2021-10-14 18:18 | 显示全部楼层
楼主,牛呀

使用特权

评论回复
lovelessing| | 2021-10-15 12:26 | 显示全部楼层
厉害了,有疑问为什么雷达的反馈数据是速度而不是绝对角度或者位置,速度的积分难免会有误差,但是位置不会

使用特权

评论回复
呐咯密密|  楼主 | 2021-10-15 12:35 | 显示全部楼层
反馈的就是角度和距离

使用特权

评论回复
呐咯密密|  楼主 | 2021-10-15 12:36 | 显示全部楼层
lovelessing 发表于 2021-10-15 12:26
厉害了,有疑问为什么雷达的反馈数据是速度而不是绝对角度或者位置,速度的积分难免会有误差,但是位置不会 ...

反馈的就是角度和距离

使用特权

评论回复
laocuo1142| | 2021-10-15 13:10 | 显示全部楼层
这个不错啊,精度如何?

使用特权

评论回复
呐咯密密|  楼主 | 2021-10-15 13:38 | 显示全部楼层
laocuo1142 发表于 2021-10-15 13:10
这个不错啊,精度如何?

这个是最低端的,精度一般般,今天好像又出新款了

使用特权

评论回复
潜力变实力| | 2021-10-15 14:14 | 显示全部楼层
点赞

使用特权

评论回复
qjp1988113| | 2021-10-15 15:25 | 显示全部楼层
不错不错有意思~~我记得B站有个做雷达的也很不错~

使用特权

评论回复
zhangtao3b608| | 2021-10-15 16:38 | 显示全部楼层
这样太强了

使用特权

评论回复
Gavin3389| | 2021-10-15 17:37 | 显示全部楼层
这个老高大尚了~~

使用特权

评论回复
laocuo1142| | 2021-10-16 10:58 | 显示全部楼层
呐咯密密 发表于 2021-10-15 13:38
这个是最低端的,精度一般般,今天好像又出新款了

挺不错的

使用特权

评论回复
songchenping| | 2021-10-19 16:29 | 显示全部楼层
大牛啊。

使用特权

评论回复
返回列表 发新帖 本帖赏金 200.00元(功能说明)我要提问
您需要登录后才可以回帖 登录 | 注册

本版积分规则

我要发帖 我要提问 投诉建议 申请版主

本版热帖

本版活跃用户

优质原创写原创,赢大奖

编辑推荐

  • 1 呐咯密密 得到打赏 ¥310.00
  • 2 qbwww 得到打赏 ¥265.00
  • 3 yanzhengxin1 得到打赏 ¥200.00
  • 4 full_stack 得到打赏 ¥100.00
  • 5 laocuo1142 得到打赏 ¥55.00
  • 6 zhangjiantao 得到打赏 ¥50.00
  • 7 小叶三千 得到打赏 ¥50.00
  • 8 cjseng 得到打赏 ¥50.00
  • 9 改名了 得到打赏 ¥30.00
  • 10 HXM1593 得到打赏 ¥15.00
在线客服 快速回复 返回顶部 返回列表