基于雷达探测技术和stm32平台的自动避障智能车

对于毕业设计，我最初的构想就是，首先要有一个能够无线控制移动的小车，然后在小车上面搭载一个激光雷达，激光雷达工作返回的数据都通过无线传输到计算机上面，然后计算机可视化显示雷达数据并且能够同时定位和生成地图。之后我们可以给定目标点，自动规划合理的行驶路径，小车自动行驶到目标点。

整体结构

总体结构包括：电机模块、L289N驱动模块、STM32F407单片机模块、激光雷达模块、ESP8266WIFI模块和PC上位机模块。

• 电机模块：该模块用来实现小车的各种移动功能。在本系统在电机模块采用了4个12V的直流减速电机。
• L289N驱动模块：该模块用来接收单片机发出的PWM(Pulse width modulation脉冲宽度调试)信号，并据此来控制电机的旋转速度和旋转方向。
• STM32F407单片机模块：该模块是小车下位机的主要控制芯片，用来连接各个模块并发出相应的控制指令。可以控制电机转速进行移动、接收雷达数据、通过WIFI模块连接PC上位机等。
• 激光雷达模块：该模块相当于此系统中的眼睛，用于观测周围环境识别障碍。本课题中采用的是SLAMTEC公司的RPLIDAR A1型号的激光雷达。
• ESP8266WIFI模块：该模块用来建立单片机与PC上位机之间的TCP连接，并实现二者之间的高速数据传输。
• PC上位机模块：该模块首先可以接收到激光雷达通过无线传输过来的数据，对数据解析后进行可视化显示。然后根据这些雷达数据可以同时生成地图和定位小车在地图中的位置。并且可以选择目的地，规划好路径后可以自动发送控制信号，控制小车绕过障碍移动到目的地。
  

激光雷达

本系统中我们采用的是中采用的是SLAMTEC公司的RPLIDAR A1型号的激光雷达，如下所示。

对RPLIDAR的命令一般有如图所示格式：

每个请求报文均以0xA5为固定开始，RPLIDAR也将这个标志作为一个新命令的开始。常用的RPLIDAR指令如表所示：

本系统中我们使用如下命令

当RPLIDAR没有其他工作时，单片机发送的该命令后，RPLIDAR将开始进入扫描测距模式。每个获得测距数据都会以应答报文的形式返回给单片机。如果RPLIDAR之间就在扫描测距模式运作，那么它会首先停止该工作，并重新开始新一轮的扫描测距采样工作。当RPLIDAR在停机状态时，所有的外部请求命令都会被忽略。如果RPLIDAR接收到该请求命令后立即返回了起始应答报文，这表明RPLIDAR接收了该请求并且进入了扫描测距模式，之后RPLIDAR将会不间断的发送采集到的数据给单片机，直到单片机发出新的命令或者RPLIDAR出现故障。
RPLIDAR会将扫描测距获得的采样点数据通过下图所示的结构作为应答报文发送至单片机。

其中各个字段的定义及含义如下：

• S是扫描的起始标志位，S=1时代表激光雷达开始了新一圈的扫描。
• 是S的取反， =！S，可以作为一种确定开始新一圈扫描数据的检验。
• C是检验为，它的值永远为1，可用于检测该报文是否发生错误和进行数据校验。
• Quality是采样点的质量，它与激光雷达接收到的信号的质量相关。
• angle_q6是扫描点相对于激光雷达朝向的夹角，实际的角度为angle_q6/64.0 Deg。
• distance_q2是扫描点和激光雷达之间的距离，无效点的距离为0，实际距离的值为distance_q2/4.0 mm。

在C#中，我们可以使用如下代码对返回的数据进行处理，转化成可以的数据。

//byte b[5] 激光雷达返回的一个扫描点的信息
angle = (double)(((b[2] << 7) | (b[1] >> 1)) / 64.0);
distance = (double)(((b[4] << 8) | b[3])/4.0);
quality = b[0] >> 2;
start = Convert.ToBoolean(b[0] & 0x01);
check = ((b[1] & 0x01) == 0x01 && (b[0] & 0x01) != ((b[0] >> 1) & 0x01));

关于雷达的其他各种详细信息请到思岚的网站上下载http://www.slamtec.com/cn/Support#rplidar-a-series

ESP8266

{% note warning %}
在于电脑连接时，如果收到不消息，很有可能是自己电脑防火墙的问题。
{% endnote %}

ESP8266支持多种工作模式。
STA模式：ESP8266模块可以连接到路由器或者任意其他WIFI可以连接上的网络设备上。
AP模式：ESP8266模块作为WIFI热点，其他设备可以连接到此模式下的ESP8266模块，可以实现电脑或者手机于设备直接通信，完成局域网控制。
STA+AP模式：上述两种模式的共存。

我们使用STA模式，笔记本电脑做热点，ESP8266连接到笔记本电脑上并开启透传。

//设置工作模式 1：station模式   2：AP模式  3：兼容 AP+station模式
Esp8266_Send_Cmd("AT+CWMODE=1");

//让Wifi模块重启的命令
Esp8266_Send_Cmd("AT+RST");
Esp8266_Send_Data("+++");
delay_ms(1000);					//等待500ms太少 要1000ms才可以退出

//让模块连接上自己的路由
Esp8266_Send_Cmd("AT+CWJAP=\"wifi\",\"00000000\"");
delay_ms(200);   

//=0：单路连接模式     =1：多路连接模式
Esp8266_Send_Cmd("AT+CIPMUX=0");
delay_ms(200);   

//是否开启透传模式  0：表示关闭 1：表示开启透传
Esp8266_Send_Cmd("AT+CIPMODE=1");
delay_ms(200);   
//建立TCP连接  这四项分别代表了 要连接的ID号0~4   连接类型  远程服务器IP地址   远程服务器端口号
Esp8266_Send_Cmd("AT+CIPSTART=\"TCP\",\"192.168.137.1\",8888");
delay_ms(200);  
//透传模式下 开始发送数据的指令 这个指令之后就可以直接发数据了
Esp8266_Send_Cmd("AT+CIPSEND");
delay_ms(200);   

连接建立

对于上位机来说，首先要开启WIFI热点，使下位机有连接的前提，然后我们需要初始化TCP服务器，建立一个新线程并且时刻监听相应的IP地址和端口，若此时有下位机请求建立连接，与对应的下位机建立TCP连接，此时二者之间就可以开始收发数据了。

一般来说上位机接收到的数据基本都是激光雷达通过单片机返回的激光雷达数据，首先我们需要对数据进行验证，然后再进行处理。

上位机发送数据时，为了保证命令的可靠和安全性，每次发送的命令最开始都会上0xF0，0x0F两个字节的标志位，用来区分是上位机的命令还是其他的数据。上位机发送命令最常用的就是控制下位机移动的命令，在本课题中，我们用单个字符作为移动控制命令，即q、w、e、a、s、d、z、x、c,分别控制下位机进行左转、前进、右转、左移、后退、右移、原地左转、停止、原地右转。

雷达数据解析

完成雷达数据的解析之后，便可以将雷达数据可视化的显示到上位机的界面上了。因解析的扫描点数据是以极坐标形式存在的，我们首先需要将它们转化成笛卡尔坐标的形式。

$x = d \times sin \left( \theta \right)$
$y = d \times cos \left( \theta \right)$

转化后的坐标是以雷达的坐标点为原点存在的，x表示的是扫描点和雷达之间在x轴上的距离，x为正值表示此点在雷达的右侧，负值为左侧，y同理。然后只需要将这些点通过为位图的方式显示出来即可，具体效果如下：

红色三角形是智能车的位置，红色三角形所指向的方向就是激光雷达的正向，也就是激光雷达夹角为0的方向。周围的黑色点集就是激光雷达扫描一圈后返回的点集的角度和距离信息的可视化显示。上位机每获取激光雷达返回一圈数据就刷新一次位图，这样上位机就能实时的显示雷达所采集回来的数据了。

SLAM

首先我们要对整个系统初始化，主要包括对系统的全局地图、当前位姿、智能车移动路径（位姿变化过程）、栅格地图更新要求、栅格地图边界尺寸、栅格地图单元方块大小、包括角度和距离的搜索分辨率以及最小的位姿更新要求等。设置当前坐标为初始坐标，也就是原点，当前方向为雷达的正方向。
当雷达获取到一组数据时，首先将其从极坐标转化成笛卡尔坐标，然后判断当前获得的数据是否是第一组获得的数据，如果是则更新全局地图，否则进行下一步，这个更新全局地图就是将这一组数据作为最初的全局地图的数据。

然后判断整个系统是否需要更新栅格地图，判断依据是智能车移动的距离是否满足栅格地图更新的最小要求。若需要更新栅格地图，先将这些数据从局部坐标由当前位姿转化成全局坐标：

$wx = xcos(p\theta) + ysin(p\theta) + px$
$wy = xsin(p\theta) + ycos(p\theta) + py$

其中px、py、pθ是智能车在全局地图中的坐标和方向，x、y是当前数据在局部地图中的点的坐标，wx、wy是转化成后的点在全局地图中的坐标。然后由这些坐标生成一个全局地图内的矩形区域，这个区域再加上栅格地图的边界大小生成一个更大的矩形区域，然后将处于这个区域内的全局地图提取出来，再根据预先定义的栅格单元大小生成栅格占用地图，记为A。栅格占用地图就是：若在一个栅格单元内存在提取出来的地图的任意一个点，则此栅格单元就被占用，相当于将原地图的分辨率降低，可以极大提高运算效率。然后将此栅格占用地图通过距离变换转化成栅格占用距离矩阵C，其中每个被占用的栅格单元的值为0，未被占用的栅格单元的值为此栅格单元到最近的被占用的栅格单元的距离。

下一步就是要进行位姿预测，进行位姿预测时，我们将当前位姿与上一位姿的差值作为下一位姿和当前位姿的差值，这样可以直接得出预测位姿，使用这种方法进行预测的位姿最大的好处就是直观和简单。

之后需要进行的是根据栅格占用距离矩阵进行预测位姿的优化工作，具体方法是使用搜索分辨率，以预测位姿为起点，对各个方向及各个方向上的不同距离进行遍历，生成一个新的预测位姿。然后将当前的扫描数据点集通过这个新的预测位姿生成一个新的栅格占用矩阵B，计算栅格占用矩阵B和栅格占用地图A之间的距离：

$d=\sum\limits_^m C[Y[i],X[i]]$

其中d是二者之间的距离，X为当前扫描点集生成的占用矩阵B中被占用的单元横坐标集合，同理Y是纵坐标集合。在遍历完成后，选择一个距离最小的预测位姿作为最佳预测位姿，若在遍历过程中没有找到最佳预测位姿，可以选择减小搜索分辨率。

在完成预测位姿的优化之后就需要更新全局地图和位姿，在更新之前要先判断预测的位姿是否正确。方法是计算预测位姿于当前位姿的差值，若差值中的距离或者角度超过一定范围，这表明此次预测的位姿是无效的，不应使用。否则进行位姿和全局地图的更新。位姿的更新是把预测位姿变成当前位姿然后加入到位姿变化也就是移动轨迹中。对于全局地图的更新，先将当前扫描点集依据更新的当前位姿转化成全局坐标的点集，判断这些点集在栅格地图中占用的闪个单元是否在全局地图中提取的栅格占用地图A中也被占用，若有则忽略此点，否则将这些转化成全局坐标的点加入到全局地图中，完成全局地图的更新。

最后将生成的地图和智能车移动路径在上位机的界面上显示，例如在卧室内进行智能车移动和地图生成的结果如图所示：

其中红色的点形成的曲线是智能车的移动轨迹，每个点都是移动过程的中的一个姿态。在最外层黑色的部分是未探索的区域，内部白色的区域是已探索完成，没有障碍可以移动的区域。黑色部分和白色部分的交界处是墙壁或者其他的障碍物，是无法移动越过去的。

参考https://github.com/meyiao/LaserSLAM

路径规划

A算法又称AStar算法，是一种在平面区域内，找到两个节点之间绕过障碍的低成本路径的方法。它被广泛的用于各种游戏的人物的寻路。严格上来说A算法找到路径并不是最短路径，它的主要目标是尽可能快速度的找到通过目标节点的路径。

A*算法是启发式的搜索，也就是说从起点开始，它对自己周围的位置进行评估，选择一个到达终点的最好的位置，然后在从这个最好的位置开始搜索，直到找到移动到终点。

首先初始化开启列表和关闭列表，并将开始节点加入到开启列表中。先检测开启列表是否可用，若不可用则表明找不到通往目的地的路径，算法结束。

若可用则查找开启列表中是否含有终点，如果没有在开启列表中找到终点，遍历开启列表中的所有节点，找到其中代价最小的节点，将其移动到关闭列表中并设置为当前节点。然后对于当前节点周围的8个节点，先将不在地图范围内的、障碍不可通过的和已经在关闭列表中的点排除。计算剩余节点的代价。

$F=G+H$

其中F代表当前节点从此节点经过到达终点的距离，G为从当前节点移动到此节点的花费，H为从此节点移动到终点的距离。若此节点不在开启列表中，将其直接加入开启列表并将此节点的父节点设置为当前节点，否则将此次计算的代价与原有的代价比较，若大于原有代价，则不进行任何操作，否则更新此节点的代价和它的父节点。然后返回继续检测开启列表是否可用。

若在开启列表中找到了终点，这表明已经找到了起始节点到终点的路径，接下来需要找到此路径，我们从终点开始，依次查找节点的父节点，最终会找到一条通往开始节点的路径，这条路径就是我们找到的路径。最后将此路径逆序返回，算法结束。

在本课题中应用此算法时，我们不能直接套用，需要对相关的输入数据做一定的修改。主要有两个原因，一是原算法是理想中的模型，生成的路径是一连串的点的集合，在绕过障碍的路径规划中，规划好的路径会很有可能紧贴着障碍，而由算法指导进行移动的实际平台是有一定大小的，这显然不可取；另一个是当智能车按照规划的路径移动时，很难完全按照原始路径移动，移动过程中势必会有偏差，若路径靠近障碍物，很容易造成设备损坏。为解决这些问题，我们需要对地图障碍进行放大，具体操作是找到障碍与可以自由移动区域分界线，将这一分界线向自由移动区域移动一定范围，将移动后的分界线与原分界线的区域填充为障碍等不可到达的区域，这样在实际应用是，即使规划的路径紧贴障碍，在实际的地图中移动时也会留有一定的空间。

首先红色的点形成的轨迹是用于生成地图时智能车的运动轨迹，轨迹的最后一个点是智能车在地图中的当前位置，我们将它作为进行路径规划的起点。我们分别选择了两个不同的点作为终点，首先对于右边的终点，两者之间没有任何障碍，故生成了从起点到终点的一条直线路径，显示的蓝色的直线。而对于左边的终点，二者之间是由障碍阻挡的，不能直接去往终点，需要在进行路径规划时绕过障碍，就像图中左边的折线一样，这条路径绕过障碍，而且与障碍之间还有一定的间隔，用来保证智能车在按照这个路径前进的时候不会撞到障碍。

循迹

所以对于自动避障行驶功能的实现，我们只需要使智能车按照规划好的路径移动即可。

首先我们要得到规划好的路径图，之后遍历路径中的所有点，找到一个与智能车当前位置距离最近的点，要求每次找到的距离最近的点都应该在上次找到的距离最近的点与终点的那一段路径之中，这就保证了智能车不会往反向移动。然后计算当前找到的距离最近的点与终点之间的距离，判断是否达到目的地，若没有，根据这个点在路径中选择一个局部目标点，并朝这个局部目标点前进，对于局部目标点的选择，我们可以直接使用这个距离最近的点往终点方向前进一段距离的点。对于朝目标点移动，我们可以根据当前智能车的朝向与局部目标点相对于智能车当前位置的方向，选择相应的移动模式，从而达到朝向目标点移动的目的。

移动之后我们需要重新获取智能车的当前位置，这一点我们可以由之前的SLAM算法传递给我们，获得位置后，我们需要再次在路径中找到距离当前位置最近点，然后判断是否到达了终点，持续重复这个过程直到到达目的地，至此算法结束，完成自动避障行驶。

红色的点形成的线条是小车的移动轨迹，蓝色的点形成的线条是规划的路径。在实验开始前我们需要首先生成地图，生成地图的过程就是我们令小车随机移动的过程，是在图中就是那些不在蓝色线条周围的红点形成的不规则线条。我们选择了起点和终点之间没有障碍的两个点，蓝色线条左上方为起点，右下方位终点，蓝色线条就是从起点到终点之间规划的路径。我们可以看到在蓝色线条附近有一条由红点形成的贴近蓝色线条的不规则曲线，这些红点就是智能车从起点到终点按照规划的路径行驶时到达过的位置。可以看到起点和终点之间是有障碍的，规划的路径绕开了起点和终点之间的障碍，智能车的行驶轨迹也随着路径自动绕开了障碍。

总结

本课题的下位机硬件部分，根据我们需要实现的功能，首先要选择需要的模块和器件。其中主要包括激光雷达、WIFI模块、主控芯片STM32F407单片机和麦克纳姆轮车模。想要使用这些模块并将它们组合成具有一定功能的小车，需要清楚他们各个模块的原理，还要清楚如何使用程序使它们协同配合，一起完成功能。完成各个模块的选择后，需要对这些模块进行组装和电路上的连接。对于电路的连接，尽量选择插线法，相对于直接焊接，插线具有良好的可拓展性和容错性。在连接完成进行硬件调试时，遇到了各种各样的奇怪问题。其中最难解决并且印象最深的问题就是激光雷达莫名不工作的问题，经过各方面检查和查阅各种资料后，最后了解到由于激光雷达和单片机共用一个电源电路，激光雷达导致单片机的供电电压电流不稳定，又返回来向激光雷达发送了一些奇怪的命令，导致激光雷达进入了保护模式从而停止工作。
本课题的上位机部分，主要需要实现的功能有，首先要完成与下位机的无线连接，能够进行数据传输和指令交互，然后需要对激光雷达返回的数据进行解析和显示，然后根据这些数据完成对下位机周围地图的构建，同时需要完成对下位机在地图中进行定位，最后就是完成从下位机当前位置完成到自由选择的目的终点的路径规划，进行路径规划时尤其要注意对障碍的躲避，之后便由上位机自动控制下位机按照指定的路径前进。在实现这些功能的过程中，不可避免的遇到了很多的问题，经过测试、调试都一一解决了，最后成功实现的所需要的功能，达到了预期的效果，成功的完成了毕业设计的工作。

由于毕业设计时间和经费有限，因此整个系统还有许多可以优化和改进的地方，比如可以对SLAM算法的精确度和速度做进一步优化，可以选择精度、稳定性和速度更好的激光雷达，选择性能更好的WIFI模块，从而提升整个系统精确度和稳定性。
识。

对于演示视频，之后如能找到好用的视频解决方案再上传。

更新（因为是直接用的github直链，想要加速请挂vpn）

上位机演示

{% dplayer "url=https://slyli.cn/video/2020-07/上位机演示视频.mp4" %}

下位机演示

{% dplayer "url=https://slyli.cn/video/2020-07/下位机演示视频.mp4" %}