代码收藏家 “Path Planning with STM32 and LiDAR Technology”
在上一篇文章中，我们可以用激光雷达得到精确的雷达数据了，根据这些数据，我们可以用程
序画个图验证一下。
。
可以发现，我们得到的点是非常准确的，噪点也很少。那么我们可以据此导航了。
关于算法目标，就是避开矩形障碍物，找到圆弧所在的角度。
在算法编写过程中，我也深刻的感受到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的能力。路漫漫其修远兮，诸君共勉。
