《基于slam算法的超视距小车》调研分析报告
本项目是博主在本科期间和同学一起制作的一个实训项目，关键代码以后会放出。
这是我们测试的视频
项目背景分析
机器人曾经是科幻电影中的形象，可目前已经渐渐走入我们的生活。机器人技术以包含机械、电子、自动控制、计算机、人工智能、传感器、通讯与网络等多个学科和领域为代表，是多种高新技术发展成果的综合集成，因此，它的发展与众多学科发展密切相关，代表了高科技发展的前沿。
随着电子技术的不断发展，人们发明了各式各样的具有感知，决策，行动和交互能力的机器人，从机器人的构想到今天机器人的相对普及，机器人的应用已经遍及机械、电子、冶金、交通、宇航、国防等多个领域。并且随着机器人的智能化水平不断提高，并且迅速的改变着人们的生活方式，随着它在人类生活领域中的应用不断扩大，将会给人们的生产生活带来了巨大的影响。
在国外机器人的发展有如下趋势。一方面机器人在制造业应用的范围越来越广阔，其标准化、模块化、网络化和智能化的程度越来越高，功能也越来越强，并向着技术和装备成套化的方向发展;另一方面，机器人向着非制造业应用以及,微小型方向发展。
智能小车可以理解为机器人的一种特例，它是一种能够通过编程手段，在智能小车的基础上，添加上了新的功能。
项目知识背景分析
本次项目我们的SLAM算法的超视距机器人，其基础机械框架是一个三轮的智能小车，集成使用了树莓派，STM32等物联网多种设备。
在物联网应用中有两项关键技术:
传感器技术:这也是计算机应用中的关键技术。大家都知道，到目前为止绝大部分计算机处理的都是数字信号。自从有计算机以来就需要传感器把模拟信号转换成数字信号计算机才能处理。嵌入式系统技术:是综合了计算机软硬件、传感器技术、集成电路技术、电子应用技术为一体的复杂技术。经过几十年的演变，以嵌入式系统为特征的智能终端产品随处可见;小到人们身边的MP3,大到航天航空的卫星系统。嵌入式系统正在改变着人们的生活，推动着工业生产以及国防工业的发展。如果把物联网用人体做一个简单比喻，传感器相当于人的眼睛、鼻子、皮肤等感官，网络就是神经系统用来传递信息，嵌入式系统则是人的大脑，在接收到信息后要进行分类处理。这个例子很形象的描述了传感器、嵌入式系统在物联网中的位置与作用。
树莓派是为学习计算机编程教育而设计，只有信用卡大小的微型电脑，其系统基于Linux。自问世以来，受众多计算机发烧友和创客的追捧，曾经一“派”难求。别看其外表“娇小”，内“心”却很强大，视频、音频等功能通通皆有，可谓是“麻雀虽小，五脏俱全”。
STM32系列基于专为要求高性能、低成本、低功耗的嵌入式应用专门设计的ARM Cortex®-M0，M0+，M3, M4和M7内核。Stm32 使用的是Cortex-M4构架， ARMCortex™-M4处理器是由ARM专门开发的最新嵌入式处理器，在M3的基础上强化了运算能力，新加了浮点、DSP、并行计算等，用以满足需要有效且易于使用的控制和信号处理功能混合的数字信号控制市场。其高效的信号处理功能与Cortex-M处理器系列的低功耗、低成本和易于使用的优点的组合，旨在满足专门面向电动机控制、汽车、电源管理、嵌入式音频和工业自动化市场的新兴类别的灵活解决方案。
关键词：
激光雷达
超视距
SLAM是同步定位与地图构建(Simultaneous Localization And Mapping)的缩写,SLAM主要用于解决移动机器人在未知环境中运行时定位导航与地图构建的问题。
SLAM通常包括如下几个部分，特征提取，数据关联，状态估计，状态更新以及特征更新等。对于其中每个部分，均存在多种方法。针对每个部分，我们将详细解释其中一种方法。在实际使用过程中，读者可以 使用其他的方法代替本文中说明的方法。这里，我们以室内环境中运行的移动机器人为例进行说明，读者可以将本文提出的方法应用于其他的环境以及机器人中。
项目过程分析
2.系统整体构架
整个系统以STM32F4为主控芯片，实现对小车简单运动的控制；软件部分在STM32集成开发环境Keil下编写各模块程序，包括PWM波输出模块、雷达模块等等，通过主控制程序将各模块融合一起。整个设计将硬件与软件相结合，实现对小车的控制，使小车能够做出前进、后退、左转、右转等动作，并通过计算机的指令，及对小车速度进行调节，在本次设计中将PWM 波占空比控制在50%以下，使小车不会因速度过高而导致转弯过程中其方向不易控制。本设计与实际应用相结合，利用高性能的STM32F407芯片，辅以雷达、树莓派、电机驱动模块、直流减速电机，通过高可靠性的软件设计，来实现小车的智能控制，具有很强的现实意义。
系统功能设计：
（1）电机驱动模块：
使用端口GPIO来连接电机，所以给GPIO编程就可以控制电机。使用系统时钟SysTick来周期性的给电机发送脉冲。用四个按钮来控制需要发送脉冲的个数，每个按钮被按下就设置给电机发送脉冲的个数，如果上一次给电机发送的脉冲没有发送完成，这次按钮发送的脉冲将不被响应。
+5V和GND为控制信号电源，如果控制信号为3.3V，那么+5V接3.3V；ENA、ENB分别为电机接口1和电机接口2的使能信号，可以外接PWM；IN1~IN4为两路电机正反转、制动(或称刹车)控制信号。控制逻辑如表 1.2和表 1.3所示。其中0为低电平、1为高平、×为任意电平，悬空时为高电平。
IN1 IN2 ENA OUT1、OUT2输出
× × 0 无输出，OUT1、OUT2悬空
0 0 1 刹车，VOUT1 = VOUT2 = VGND
1 0 1 正转，VOUT1 - VOUT2 = 电源电压
0 1 1 反转，VOUT2 - VOUT1 = 电源电压
1 1 1 刹车，VOUT1 = VOUT2 = 电源电压(未连接控制信号时的默认状态)
图2电机接口1控制信号逻辑
IN3 IN4 ENB OUT3、OUT4输出
× × 0 无输出，OUT3、OUT4悬空
0 0 1 刹车，VOUT3 = VOUT4 = VGND
1 0 1 正转，VOUT3 - VOUT4 = 电源电压
0 1 1 反转，VOUT4 - VOUT3 = 电源电压
1 1 1 刹车，VOUT3 = VOUT4 = 电源电压
(未连接控制信号时的默认状态)
使用STM32控制电机转动的时候，STM32的电源与驱动板控制信号电源应共地，但不要与电机电源PGND共地。当使用5V单片机时，驱动板+5V接电源+5V；当使用3.3V单片机时，驱动板+5V接电源+3.3V。单片机和驱动板控制信号可共用一电源或各自独立供电（但一定要共地）。ENA为与STM32的一个GPIO或PWM输出端口相连，当ENA为高电平时，驱动板使能，正反转或刹车有效，如果是PWM信号，那么可对电机进行调速；低电平时，驱动板禁能，电机接口无输出。IN1和IN2与单片机的两个GPIO相连（可支持STM32任意IO端口，无需上拉电阻），控制电机正反转及刹车。
（2）激光雷达模块
RPLIDAR A1 360 度激光扫描测距雷达是由SLAMTEC 公司开发的低成本二维激光雷达(LIDAR)解决方案。它可以实现在二维平面的6 米半径范围内进行360 度全方位的激光测距扫描，并产生所在空间的平面点云地图信息。这些云地图信息可用于地图测绘、机器人定位导航、物体/环境建模等实际应用中。
在将采样周期设为360 点采样/周的条件下，RPLIDAR 扫描频率达5.5hz，并且最高可达10hz 的扫描频率。
RPLIDAR 采用由SLAMTEC 研发的低成本的激光三角测距系统，在各种室内环境以及无日光直接照射的室外环境下均表现出色。
RPLIDAR A1 主要分为激光测距核心以及使得激光测距核心高速旋转的机械部分。在分别给各子系统供电后，测距核心将开始顺时针旋转扫描。用户可以通过RPLIDAR A1 的通讯接口（串口/USB 等）获取RPLIDAR 的扫描测距数据。
RPLIDAR A1 采用了激光三角测距技术，配合SLAMTEC 研发的高速的视觉采集处理机构，可进行每秒高达2000 次以上的测距动作。每次测距过程中， RPLIDAR A1 将发射经过调制的红外激光信号，该激光信号在照射到目标物体后产生的反光将被RPLIDAR A1 的视觉采集系统接受。经过嵌入在RPLIDAR A1 内部的DSP 处理器实时解算，被照射到的目标物体与RPLIDAR A1 的距离值以及当前的夹角信息将从通讯接口中输出。
Framegrabber的图形化调试界面如图6所示，这里，将雷达连接到智能机器人上，为了达成超视距的效果，我们需要通过无线网络将计算机与树莓派之间实现通信，然后树莓派通过串口与STM32接通，这样我们就能实现在计算机端对机器人的超视距控制了。
（3）SLAM算法模块
SLAM算法首先要考虑四个方面地图表示，信息感知问题， 数据关联问题，定位与构图问题。机器人在未知环境中从一个未知位置开始移动,在移动过程中根据位置估计和地图进行自身定位,同时在自身定位的基础上建造增量式地图，实现机器人的自主定位和导航。
（4）通讯模块
树莓派和电脑连接的是同一个wifi，在wpa_supplicant.conf里配置好网络名和密码，实现计算机与树莓派的通信，然后我们定好TCP协议，STM32写好写串口驱动，在树莓派上打开串口，按照TCP的协议通信就完成了树莓派与STM32之间的通信。
当然，我们也尝试着外网通讯，现在虚拟机里将网络连接设置成桥接模式，然后配置有线网络的IPV4的地址、子网掩码和网关，如图所示。
图7有线连接的配置
配置好以后，分别在阿里云和虚拟机下运行客户端和服务器程序，如图所示，最终实现信息互传，图8。但是最后的控制命令由外网实现还需要进一步的学习和探讨。
图8虚拟机与外网信息互传
最终结果
我们的最终结果如图9所示，附上几组图片。
Main.C程序如下，头文件已经省略
NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2);//设置系统中断优先级分组2
uart3_init(9600); //串口初始化波特率为9600
BEEP_Init(); //蜂鸣器初始化
delay_init(); //初始化延时函数
LED_Init(); //初始化LED
KEY_Init();
LCD_Init();
BRUSH_COLOR=RED; //设置画笔颜色为红色
TIM3_PWM_Init(4200,1); //84M/42=2Mhz的计数频率,重装载值40000，所以PWM频率为 2M/40000=50hz.
//go(3000,1,4200,3000,4200,4,0,1,1);
//delay_ms(1000);
/* go(3000,4200,1,3000,1,4200,0,1,1);
delay_ms(1000);
go(3000,1,4200,0,1,1,3000,4200,1);
delay_ms(1000);
go(3000,4200,1,0,1,1,3000,1,4200);
delay_ms(1000);
go(0,1,1,3000,1,4200,3000,4200,1);
delay_ms(1000);
go(0,1,1,3000,4200,1,3000,1,4200);
delay_ms(1000);
*/
//go(0,1,1,0,1,1,0,1,1);
while(1)
{
key_scan(0);
if(keyup_data==KEY0_DATA)
{
uart3SendChars("UART3 TEST",11);
}
}
pwm.c里的关键代码如下
void TIM3_PWM_Init(u32 auto_data,u32 fractional)
{
GPIO_InitTypeDef GPIO_InitStructureB4,GPIO_InitStructureB5,GPIO_InitStructureB0,GPIO_InitStructureB1;
GPIO_InitTypeDef GPIO_InitStructureD12,GPIO_InitStructureD13;
GPIO_InitTypeDef GPIO_InitStructureA0,GPIO_InitStructureA7,GPIO_InitStructureE6;
TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;
TIM_OCInitTypeDef TIM_OCInitStructure;
RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM3,ENABLE); //TIM3时钟使能
RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOB, ENABLE); //使能PORTB时钟
RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM4,ENABLE); //TIM4时钟使能
RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOD, ENABLE); //使能PORTD时钟
RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM5,ENABLE); //TIM5时钟使能
RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOA, ENABLE); //使能PORTA时钟
RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM14,ENABLE); //TIM5时钟使能
RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOA, ENABLE); //使能PORTA时钟
RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM9,ENABLE); //TIM5时钟使能
RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOE, ENABLE); //使能PORTA时钟
GPIO_PinAFConfig(GPIOB,GPIO_PinSource4,GPIO_AF_TIM3); //GPIOB4复用为定时器3
GPIO_PinAFConfig(GPIOB,GPIO_PinSource5,GPIO_AF_TIM3); //GPIOB5复用为定时器3
GPIO_PinAFConfig(GPIOB,GPIO_PinSource0,GPIO_AF_TIM3); //GPIOB0复用为定时器3
GPIO_PinAFConfig(GPIOB,GPIO_PinSource1,GPIO_AF_TIM3); //GPIOB1复用为定时器3
GPIO_PinAFConfig(GPIOD,GPIO_PinSource12,GPIO_AF_TIM4); //GPIOD12复用为定时器4
GPIO_PinAFConfig(GPIOD,GPIO_PinSource13,GPIO_AF_TIM4); //GPIOD13复用为定时器4
GPIO_PinAFConfig(GPIOA,GPIO_PinSource0,GPIO_AF_TIM5); //GPIOA0复用为定时器5
GPIO_PinAFConfig(GPIOA,GPIO_PinSource7,GPIO_AF_TIM14); //GPIOA0复用为定时器5
GPIO_PinAFConfig(GPIOE,GPIO_PinSource6,GPIO_AF_TIM9); //GPIOA0复用为定时器5
GPIO_InitStructureB4.GPIO_Pin = GPIO_Pin_4; //GPIOB
GPIO_InitStructureB4.GPIO_Mode = GPIO_Mode_AF; //复用功能
GPIO_InitStructureB4.GPIO_Speed = GPIO_Speed_100MHz; //速度100MHz
GPIO_InitStructureB4.GPIO_OType = GPIO_OType_PP; //推挽复用输出
GPIO_InitStructureB4.GPIO_PuPd = GPIO_PuPd_UP; //上拉
GPIO_Init(GPIOB,&GPIO_InitStructureB4); //初始化PB4
GPIO_InitStructureB5.GPIO_Pin = GPIO_Pin_5; //GPIOB5
GPIO_InitStructureB5.GPIO_Mode = GPIO_Mode_AF; //复用功能
GPIO_InitStructureB5.GPIO_Speed = GPIO_Speed_100MHz; //速度100MHz
GPIO_InitStructureB5.GPIO_OType = GPIO_OType_PP; //推挽复用输出
GPIO_InitStructureB5.GPIO_PuPd = GPIO_PuPd_UP; //上拉
GPIO_Init(GPIOB,&GPIO_InitStructureB5); //初始化PB5
TIM_TimeBaseStructure.TIM_Prescaler=fractional; //定时器分频
TIM_TimeBaseStructure.TIM_CounterMode=TIM_CounterMode_Up; //向上计数模式
TIM_TimeBaseStructure.TIM_Period=auto_data; //自动重装载值
TIM_TimeBaseStructure.TIM_ClockDivision=TIM_CKD_DIV1;
TIM_TimeBaseInit(TIM3,&TIM_TimeBaseStructure);//初始化定时器3
TIM_TimeBaseStructure.TIM_Prescaler=fractional; //定时器分频
TIM_TimeBaseStructure.TIM_CounterMode=TIM_CounterMode_Up; //向上计数模式
TIM_TimeBaseStructure.TIM_Period=auto_data; //自动重装载值
TIM_TimeBaseStructure.TIM_ClockDivision=TIM_CKD_DIV1;
TIM_TimeBaseInit(TIM4,&TIM_TimeBaseStructure);//初始化定时器3
TIM_TimeBaseStructure.TIM_Prescaler=fractional; //定时器分频
TIM_TimeBaseStructure.TIM_CounterMode=TIM_CounterMode_Up; //向上计数模式
TIM_TimeBaseStructure.TIM_Period=auto_data; //自动重装载值
TIM_TimeBaseStructure.TIM_ClockDivision=TIM_CKD_DIV1;
TIM_TimeBaseInit(TIM5,&TIM_TimeBaseStructure);//初始化定时器3
TIM_TimeBaseStructure.TIM_Prescaler=fractional; //定时器分频
TIM_TimeBaseStructure.TIM_CounterMode=TIM_CounterMode_Up; //向上计数模式
TIM_TimeBaseStructure.TIM_Period=auto_data; //自动重装载值
TIM_TimeBaseStructure.TIM_ClockDivision=TIM_CKD_DIV1;
TIM_TimeBaseInit(TIM14,&TIM_TimeBaseStructure);//初始化定时器3
TIM_TimeBaseStructure.TIM_Prescaler=83; //定时器分频
TIM_TimeBaseStructure.TIM_CounterMode=TIM_CounterMode_Up; //向上计数模式
TIM_TimeBaseStructure.TIM_Period=auto_data; //自动重装载值
TIM_TimeBaseStructure.TIM_ClockDivision=TIM_CKD_DIV1;
TIM_TimeBaseInit(TIM9,&TIM_TimeBaseStructure);//初始化定时器3
//初始化TIM3 Channel 1.2.3.4 PWM模式
TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1; //选择定时器模式:TIM脉冲宽度调制模式2
TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable; //比较输出使能
TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_Low; //输出极性:TIM输出比较极性低
TIM_OC1Init(TIM3, &TIM_OCInitStructure); //根据T指定的参数初始化外设TIM3 4OC1
TIM_OC1PreloadConfig(TIM3, TIM_OCPreload_Enable); //使能TIM3在CCR3上的预装载寄存器
TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable; //比较输出使能
TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_Low; //输出极性:TIM输出比较极性低
TIM_OC2Init(TIM3, &TIM_OCInitStructure); //根据T指定的参数初始化外设TIM3 4OC2
TIM_OC2PreloadConfig(TIM3, TIM_OCPreload_Enable); //使能TIM3在CCR3上的预装载寄存器
TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable; //比较输出使能
TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_Low; //输出极性:TIM输出比较极性低
TIM_OC3Init(TIM3, &TIM_OCInitStructure); //根据T指定的参数初始化外设TIM3 4OC3
TIM_OC3PreloadConfig(TIM3, TIM_OCPreload_Enable); //使能TIM3在CCR3上的预装载寄存器
TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable; //比较输出使能
TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_Low; //输出极性:TIM输出比较极性低
TIM_OC4Init(TIM3, &TIM_OCInitStructure); //根据T指定的参数初始化外设TIM3 4OC4
TIM_OC4PreloadConfig(TIM3, TIM_OCPreload_Enable); //使能TIM3在CCR4上的预装载寄存器
TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable; //比较输出使能
TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_Low; //输出极性:TIM输出比较极性低
TIM_OC1Init(TIM4, &TIM_OCInitStructure); //根据T指定的参数初始化外设TIM3 4OC4
TIM_OC1PreloadConfig(TIM4, TIM_OCPreload_Enable); //使能TIM3在CCR4上的预装载寄存器
TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable; //比较输出使能
TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_Low; //输出极性:TIM输出比较极性低
TIM_OC2Init(TIM4, &TIM_OCInitStructure); //根据T指定的参数初始化外设TIM3 4OC4
TIM_OC2PreloadConfig(TIM4, TIM_OCPreload_Enable); //使能TIM3在CCR4上的预装载寄存器
TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable; //比较输出使能
TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_Low; //输出极性:TIM输出比较极性低
TIM_OC1Init(TIM5, &TIM_OCInitStructure); //根据T指定的参数初始化外设TIM3 4OC4
TIM_OC1PreloadConfig(TIM5, TIM_OCPreload_Enable); //使能TIM3在CCR4上的预装载寄存器
TIM_ARRPreloadConfig(TIM3,ENABLE);//ARPE使能
TIM_ARRPreloadConfig(TIM4,ENABLE);//ARPE使能
TIM_ARRPreloadConfig(TIM5,ENABLE);//ARPE使能
TIM_ARRPreloadConfig(TIM14,ENABLE);//ARPE使能
TIM_ARRPreloadConfig(TIM9,ENABLE);//ARPE使能
TIM_Cmd(TIM3, ENABLE); //使能TIM3
TIM_Cmd(TIM4, ENABLE); //使能TIM3
TIM_Cmd(TIM5, ENABLE); //使能TIM3
TIM_Cmd(TIM14, ENABLE); //使能TIM3
TIM_Cmd(TIM9, ENABLE); //使能TIM3
}
void duoji(int a,int b,int c, int d,int e)
{
a=a*34+34200;
b=b*24+36000;
c=39200-c*20;
d=d*17+36000;
e=36800-e*11;
}
void forward(int a,int b,int c,int d)
{
a=a*400;
b=b*400;
c=c*400;
d=d*400;
}
void go(int D13,int D12,int B1,int B0,int A7,int E6,int A0,int B4,int B5)
{
TIM_SetCompare2(TIM4,D13); //PD13 ENA数值减小，转速增大
TIM_SetCompare1(TIM4,D12); //PD12 IN1
TIM_SetCompare4(TIM3,B1); //PB1 IN2
TIM_SetCompare3(TIM3,B0); //PB0 ENB
TIM_SetCompare1(TIM14,A7); //PA7 IN1
TIM_SetCompare2(TIM9,E6); //PE6 IN2
TIM_SetCompare1(TIM5,A0); //PA0 ENA 2
TIM_SetCompare1(TIM3,B4); //PB4 IN1 2
TIM_SetCompare2(TIM3,B5); //PB5 IN2 2