基于STM32和激光雷达的路径规划_stm32小车路径规划

在上一篇文章中，我们可以用激光雷达得到精确的雷达数据了，根据这些数据，我们可以用程

序画个图验证一下。

。

可以发现，我们得到的点是非常准确的，噪点也很少。那么我们可以据此导航了。

关于算法目标，就是避开矩形障碍物，找到圆弧所在的角度。

在算法编写过程中，我也深刻的感受到32算力相当有限。接收雷达数据由串口中断接收，我一旦在主函数里面放入运算量比较大的任务，比如说解算，那么我收到的数据就很不准。或者说直接报错说内存不够了。

我的想法是，32算力不够，那我就只设置一个360个数据的数组，对应360个角度，存放各个角度的数据。

又得知测试所用的圆筒，半径135mm左右，那么，就可以根据这个半径，在雷达扫描到的点中，检索符合这个条件的弧形，具体代码如下可见

#include "cmath"
 
int map[360]={0};
int x[360]={0};
int y[360]={0};
int center_x[360]={0};
int center_y[360]={0};
int score[360]={0};
int re(void)//得到角度
{
int i=0,j=0;
	for(i=0;i<360;i++)//360个圆心
	{
		for(j=0;j<360;j++)//遍历360个点
		{
			if(x[j]!=0&&y[j]!=0)
			{
			if((pow(x[j]-center_x[i],2)+pow(y[j]-center_y[i],2))<17900&&(pow(x[j]-center_x[i],2)+pow(y[j]-center_y[i],2))>15900)
			{score[i]++;}
		  }
		}
	}
	j=0;
	for(i=0;i<360;i++)
	{
		if(score[j]<score[i])
		{j=i;}
	}
	return j;
}
 
 
int quality(void)
{
int i=0,c=0;
	for(i=0;i<360;i++)
	{
		if(map[i]!=0)
		{c++;}
	}
	return c;
}
 
void change(void)
{
	int i=0;float ceta=0.0;
	for(i=0;i<360;i++)
	{
		ceta=i*0.01745;
			x[i]=map[i]*cos(ceta);
			y[i]=map[i]*sin(ceta);
	}
}
void get_center(void)
{
int i=0;float ceta=0.0;
	for(i=0;i<360;i++)
	{
	ceta=i*0.01745;
	center_x[i]=(map[i]+132)*cos(ceta);
	center_y[i]=(map[i]+132)*sin(ceta);
	}
}
void zero(void)
{
int i=0;
	for(i=0;i<360;i++)
	{
	map[i]=0;x[i]=0;y[i]=0;center_x[i]=0;center_y[i]=0;score[i]=0;
	}
}
 
 

 我这个解算大大减少了运算量，通过比较各个角度的的分值，确定最佳的角度。

由于我使用了两块STM32，所以我决定通过串口通讯连接上位机与下位机，由下位机将角度信息传给上位机，上位机比较角度信息与算法得到的目标角度，得到小车的运动状态，然后上位机将状态用串口传给下位机，实现小车的运行。

为了实现更好的效果，我设定在接收十万个点的数据后，进行一次解算，小车同时运动3秒左右，一点一点走，就可以比较好的逼近目标。

以下为上位机主函数

while(1) 			
	{
		//LCD_ShowNum(50,50,Yaw,40,16);	
		if(caokong==1)
		{
			while(1)
			{
				//delay_ms(50);
				delay(50);
			if(Yaw>direction+15)
			{
			UART1_TX(0x62);
			}
			else if(Yaw<direction-15)
			{
			UART1_TX(0x61);
			}
			else if(Yaw<direction+15&&Yaw>direction-15)
			{
			UART1_TX(0x63);
			}
			j++;
			if(j>60)
			{		
				TIM_SetCompare2(TIM3,70);
				caokong=0;
				j=0;
				for(r=0;r<10;r++)
				{
				UART1_TX(0x64);
				delay_ms(10);
				}
				zero();
				break;
			}
			}	
		}
		if(jyj)
		{
			hhh++;
		//printf("%c",m);
		if(flag==1)
		{
			RX_buffer[count]=m;
			count++;
			if(count==4)//开始解算数据
			{
				if(!(RX_buffer[0]&0x01))
				{
				flag=0;count=0;
				}
				//angle2=(RX_buffer[1]<<7|RX_buffer[0]>>1);
				angle2=(RX_buffer[1]<<7|RX_buffer[0]>>1);
				distance=(RX_buffer[3]<<6|RX_buffer[2]>>2);
				
				if(distance!=0&&count!=0)
				{
				//printf("distance=%d\n",RX_buffer[3]<<6|RX_buffer[2]>>2);		
				//printf("\r\n");
				//printf("angle=%d\n",angle2);
				//printf("\r\n");
				if(map[angle2/64]!=0)
				{
				map[angle2/64]=(distance+map[angle2/64])/2;
				}
				else
				{
				map[angle2/64]=distance;
				}
				}
				flag=0;//点的检测标志归位
				count=0;//计数归0
				
			}
		} 
			if(flag==0)//点的检测标志
			{				
				if(m==0x3E)//检测到点
				{
					flag=1;
				}
			}
			jyj=0;
			
		}
		//if(quality()>350)
		if(hhh>100000)
		{
			change();
			get_center();
			direction=re(); 
			//UART1_TX(0xf1);
			//UART1_TX(direction>>8);//先发高位，再发低位
			//UART1_TX(direction);
			LCD_ShowNum(50,50,direction,40,16);
			caokong=1;
			hhh=0;
			TIM_SetCompare2(TIM3,98);
		}		
	}		

以下为下位机的主要函数，在中断中运行

每次新的解算开始时，mpu6050初始化一次，实现坐标的变换

if(Yaw<0)
		 {
		 yy=-Yaw;
		 usart2_send(0xff);
		 usart2_send(yy>>8);//先发高位，再发低位
		 usart2_send(yy);
		 }
		 else
		 {
		 yy=Yaw;
		 usart2_send(0xfe);
		 usart2_send(yy>>8);//先发高位，再发低位
		 usart2_send(yy);
		 }
		 if(m==0x61)
		 {
		 Set_Pwm(1500,2000,0);
		 }
		 else if(m==0x62)
		 {
		 Set_Pwm(2000,1500,0);
		 }
		 else if(m==0x63)
		 {
		 Set_Pwm(1500,1500,0); 
			mm=1;
		 }
		 else{Set_Pwm(0,0,0);mm=0;}
			if(m==0x64)
			{
    MPU6050_initialize();           //=====MPU6050初始化	
  	DMP_Init();                     //=====初始化DMP   
			}
		 if(mm==1)
		 {
		 Set_Pwm(1500,1500,0); 
		 }
 }

在测试后，也是发现识别圆弧效果比较好，但是还有一个避障的问题。

于是，通过更改每次运动旋转、直走的角度，虽然不能每次都通过测试，但也实现了一定的成功率。

总结来看，本次项目，嵌入式部分，是一个“字节跳动”的感觉，激光雷达超级快的发送字节，让精准接收数据成了很困难的挑战。32的任务分配，任务调度，外设的配置，如何更好的配合算法。在调试过程中，也涉及到很多字节、数据类型方面的知识，比如说串口发送、解算浮点数据。意识到，在计算机的世界中，二进制，十六进制，字节，这种基本的语言是如何实现操作寄存器，机器与机器之间互相沟通的。

算法部分，我认为最大的困难就是，用尽可能少的运算，实现想要的功能。小组中也有同学写过更高识别能力的算法，但是都因为内存占用过多，而无法部署到32上，这就很考验算法能力。

在这次项目中，也认识到自己对32还不够熟悉，代码很不美观，任务调度也非常简陋。没有很好利用32的能力。路漫漫其修远兮，诸君共勉。