基于STM32開發(fā)板和思嵐A1M8激光雷達(dá)制作地圖掃描儀

前言

一個朋友在做服務(wù)機(jī)器人項目，用到思嵐的激光雷達(dá)，于是便把淘汰的A1M8雷達(dá)送我一個，本著拿到啥就玩啥的態(tài)度，必須整一波。其實激光雷達(dá)還是搭配ROS才能發(fā)揮最大的作用，奈何資源有限，實力不足，只能依靠STM32開發(fā)板做一個及其簡陋的地圖掃描。

思嵐A1M8激光雷達(dá)

這款激光雷達(dá)屬于低成本的360度激光掃描測距雷達(dá)，外置電機(jī)，使用皮帶帶動雷達(dá)轉(zhuǎn)臺轉(zhuǎn)動，實現(xiàn)360度的測距掃描，電機(jī)的轉(zhuǎn)速由MCU發(fā)送PWM控制。

外部系統(tǒng)通過 TTL 電平的 UART 串口信號與 RPLIDAR 測距核心進(jìn)行通訊。通過本文檔定義的通訊協(xié)議，外部系統(tǒng)可以實時獲取 RPLIDAR 的掃描數(shù)據(jù)、設(shè)備信息、設(shè)備健康狀態(tài)。并且通過相關(guān)命令調(diào)整 RPLIDAR 的工作模式。

按照不同的請求類型， RPLIDAR 具有三種不同的請求/應(yīng)答模式：

標(biāo)準(zhǔn)的單次請求-單次應(yīng)答模式

單次請求-多次應(yīng)答模式

單次請求/無應(yīng)答模式

對于停止掃描、重啟測距核心這類請求命令， RPLIDAR 采用單次請求，但不做應(yīng)答的通訊模式。此時外部系統(tǒng)需要在發(fā)送請求后等待一定的時間，待RPLIDAR 完成了上一次請求操作后方可繼續(xù)執(zhí)行下一次請求。否則第二次的請求將可能被 RPLIDAR 丟棄。

在此次應(yīng)用中，主要采用后兩種請求/應(yīng)答模式，使用單次請求-多次應(yīng)答模式采集測距數(shù)據(jù)，使用單次請求/無應(yīng)答模式停止采樣，進(jìn)行數(shù)據(jù)的處理。

在單次請求-多次應(yīng)答模式采集測距數(shù)據(jù)時，MCU發(fā)送采集指令，雷達(dá)會先回復(fù)一條起使應(yīng)答報文，之后便會循環(huán)回復(fù)數(shù)據(jù)應(yīng)答報文。

請求報文及起始應(yīng)答數(shù)據(jù)格式如下：

在回復(fù)起始應(yīng)答之后，雷達(dá)會循環(huán)回復(fù)測距數(shù)據(jù)。長度為5bytes。

例如測距數(shù)據(jù)為 3E D5 16 77 06。

第一個字節(jié)：3E，二進(jìn)制為：0011 1110。代表信號質(zhì)量為0x0f。信號質(zhì)量不為零代表數(shù)據(jù)有效，起始標(biāo)志位為0，代表不是新的一圈，該標(biāo)志位只有在新的一圈的第一幀數(shù)據(jù)才會置一，該圈內(nèi)的其余數(shù)據(jù)改為依舊是0。

第二個字節(jié)：D5，角度數(shù)據(jù)低七位。

第三個字節(jié)：16，角度數(shù)據(jù)高八位，加上第二個字節(jié)的低七位等于166A，再右移一位得B35。實際角度=835/64=44°，該角度表示與雷達(dá)零度的順時針偏移角度，如下圖。

第四個字節(jié)：77，距離數(shù)據(jù)低八位。

第五個字節(jié)：06，距離角度高八位。則此時距離為0x0677/4 = 413mm。

激光雷達(dá)測試：

接線：

雷達(dá) MCU

GND----------->GND

RX------------->TX

TX------------->RX

V5.0----------->5V

GND----------->GND

MOTOCTL---->PWM

VMOTO------->5V

首先測試使用串口助手進(jìn)行數(shù)據(jù)采集，這里將MOTOCTL接到5V電源，直接以最高速度進(jìn)行采樣。串口助手發(fā)送A5 20,可以看到數(shù)據(jù)滾動。

其中開頭的七位數(shù)據(jù)對應(yīng)起始應(yīng)答，后面每5個字節(jié)一組，對應(yīng)測距數(shù)據(jù)。雷達(dá)無損壞，開始連接開發(fā)板調(diào)試。

MCU代碼：

既然是USART通信，我們先初始化USART，使用串口接收中斷接收數(shù)據(jù)。

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);        // 打開串口外設(shè)的時鐘        DEBUG_USART_APBxClkCmd(DEBUG_USART_CLK, ENABLE);        // 將USART Tx的GPIO配置為推挽復(fù)用模式        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 ;//搶占優(yōu)先級3        NVIC_InitStructure.NVIC_IRQChannelSubPriority = 3;                //子優(yōu)先級3        NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;                        //IRQ通道使能        NVIC_Init(&NVIC_InitStructure);        //根據(jù)指定的參數(shù)初始化VIC寄存器        // 配置串口的工作參數(shù)        // 配置波特率        USART_InitStructure.USART_BaudRate = DEBUG_USART_BAUDRATE;        // 配置 針數(shù)據(jù)字長        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;        // 配置工作模式，收發(fā)一起        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);            }

然后編寫中斷服務(wù)函數(shù)：

void USART1_IRQHandler(void)                        //串口1中斷服務(wù)程序{        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傳輸！        }}

在串口中斷服務(wù)函數(shù)中，需要采集1807個數(shù)據(jù)（360個測距點*5字節(jié)+起始7個字節(jié)）。我采用全速采樣，即MOTOCTL直接接5V，這里采集360個數(shù)據(jù)點其實不止一圈的數(shù)據(jù)，但是因為每個360度都有無效數(shù)據(jù)，多采集點可以使后期畫圖更完整。在提取數(shù)據(jù)使用EXCEL分析以后，全速轉(zhuǎn)一圈大概采樣258個點左右，這個數(shù)據(jù)無法固定，每一圈采樣數(shù)均不一樣。

在采集數(shù)據(jù)完成后我們需要關(guān)閉采樣，因為STM32F103的數(shù)據(jù)處理能力并不理想，這里需要一定的時間，于是通過串口發(fā)送指令A(yù)5 25讓雷達(dá)停止采樣，同時調(diào)用函數(shù)Data_Processing();進(jìn)行數(shù)據(jù)處理以及在屏幕上畫點。這里要注意，雷達(dá)在停止采樣前會將最后一幀數(shù)據(jù)發(fā)送完整，我們在發(fā)送停止指令的期間，雷達(dá)可能已經(jīng)在準(zhǔn)備下一幀數(shù)據(jù)，在發(fā)送完停止指令之后，可能會存在這一幀數(shù)據(jù)的最后一位未觸發(fā)中斷，但是串口的數(shù)據(jù)寄存器中已經(jīng)保存了這位數(shù)據(jù)，且已經(jīng)改變了標(biāo)志位，所以在下一次啟動采樣時會導(dǎo)致收到的第一個數(shù)據(jù)是上一次未接收完的數(shù)據(jù)。這個在進(jìn)行處理。

在此之前我們還需要一個觸發(fā)采樣的按鍵。按下按鍵后觸發(fā)采樣，為了保持持續(xù)采樣，在串口接收中斷關(guān)閉采樣并處理完數(shù)據(jù)后，可在主循環(huán)中再次開啟。

void KEY1_IRQHandler(void){        u8 RX;  //確保是否產(chǎn)生了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;    //清除中斷標(biāo)志位                EXTI_ClearITPendingBit(KEY1_INT_EXTI_LINE);             }  }

數(shù)據(jù)處理如下：

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;//                        }}

從串口緩存數(shù)組中取出角度值和距離值，保存在數(shù)組angle[]和distance[]中。當(dāng)360個數(shù)據(jù)點處理完，調(diào)用畫圖函數(shù)進(jìn)行屏幕繪制。

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);                        }        }

畫點直接調(diào)用野火的庫，其中參數(shù)scale為地圖放大倍數(shù)，因為屏幕大小有限，為了適應(yīng)不同大小的地圖，使用該參數(shù)進(jìn)行地圖放大。

return_x，return_y函數(shù)是將測距點轉(zhuǎn)換為屏幕坐標(biāo)。原函數(shù)如下：

//x坐標(biāo)轉(zhuǎn)換函數(shù)//ang:0~359度數(shù),    d:距離//返回：x坐標(biāo)0~239float 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;//角度轉(zhuǎn)換成弧度                }                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坐標(biāo)轉(zhuǎn)換函數(shù)//ang:0~359度數(shù),    d:距離//返回：y坐標(biāo)0~319float 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);//角度轉(zhuǎn)換成弧度                }                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;}

此時在屏幕上便可繪制出雷達(dá)采樣點

這里是動態(tài)監(jiān)測的，但是動態(tài)圖在后面補(bǔ)，后續(xù)也會優(yōu)化繪圖和數(shù)據(jù)處理，這里先給出大致的效果。時間有限，目前先這樣，后面會完善此貼。

從正文可以看出該屏幕的顯示的掃描地圖是圓形，但是我的房間卻不是圓的。這個地圖明顯是有問題。但是無論無如何調(diào)整算法，顯示到屏幕上的測距點總是不正確。分析得出大概問題是出在屏幕上，因為屏幕分辨率有限，測的的尺寸為了能在屏幕上顯示，不得已將尺寸縮小幾十倍，導(dǎo)致數(shù)據(jù)嚴(yán)重失真。于是我將測距數(shù)據(jù)導(dǎo)出研究。此次用已知大小的物料箱將雷達(dá)倒扣在里面。物料箱的尺寸大約為36cm*45cm。手頭沒有卷尺，用一個小尺子量的，所以只是大概值。

雷達(dá)位于箱子中間，那么到最短到箱壁兩邊的距離大概是18和22.5厘米。

測試開始：

使用串口二將原始角度和距離值打印到串口助手：

使用world文檔將數(shù)據(jù)整理：

然后復(fù)制數(shù)據(jù)到excle，進(jìn)行數(shù)據(jù)處理，將角度和距離分別提取;

根據(jù)真實角度值選取一整圈距離數(shù)據(jù)（mm），插入雷達(dá)圖：

此圖因為有無效點，取出零點以及錯誤點后得到如下圖。

可以看到此時的雷達(dá)圖很接近我們的箱子真實形狀，距離大小也符合箱子尺寸。此時才可以算作成功，雖然屏幕任然無法完整顯示掃描地圖，但是數(shù)據(jù)的處理并無問題，單片機(jī)速度跟不上，屏幕分辨率也不夠，難受啊。

原文標(biāo)題：帥小伙自制手持建圖儀！基于STM32F103+思嵐A1激光雷達(dá)