特别感谢皎皎的大力帮助
项目要求
项目分析
要解决这个问题我们可以把这个项目划分为以下几个板块
解决步骤
数据接收:
我们在这次的项目中用的雷达是RPLIDAR A1的雷达
他的接收数据的格式为:
所以我们把线接好:如图:
然后我们就可以接收到数据了:
这是原始数据:
可以发现这是非常杂乱无章的,我们把这个数据处理和解算一下:
我们用以下代码来处理数据:
while(1)
{
if(mark)break;
if(jyj){//接收到数据:
if(flag==1)
{
RX_buffer[count]=m;
count++;
if(count==4){
if(!(RX_buffer[0]&0x01)){
flag=0;count=0;
}
angle2=(RX_buffer[1]<<1);
distance=(RX_buffer[3]<<6|RX_buffer[2]>>2);
if(distance!=0&&count!=0){//记录数据
angle=(RX_buffer[1]<<1);
dis=(RX_buffer[3]<<6|RX_buffer[2]>>2);
tot=tot+1;
JD[angle]=dis;
if(tot>650)mark=1;//扫描的点数
}
flag=0;
count=0;
}
}
if(flag==0)//点的检测标志
{
if(m==0x3E)//检测到点
{
flag=1;
}
}
jyj=0;
}
}
然后我们得到处理好的数据:
算法设计
有了数据我们就需要设计算法来判断直线和曲线了。
我设计的算法的大致的流程如下:
- 得到数据点集:存储一个数组,下标为角度,数值为方向
- 每三度计算一次平均值,去除噪点
- 在去除噪点的数据上,每三个数据计算一次斜率,若差值小则判断为直线,否则为曲线
- 对于每个判断的直线或曲线,在相应的方向加减权值
- 小车每次往权值最大的方向运动
- 反复执行上述代码
具体代码:
for(j=1;j<120;j++){//判断直线和弧线
if(res[j-1]==0||res[j]==0||res[j+1]==0)continue;
k1=(sin((j-1)*3*pi)*res[j-1]-sin(j*3*pi)*res[j])/(cos((j-1)*3*pi)*res[j-1]-cos(j*3*pi)*res[j]);
k2=(sin(j*3*pi)*res[j]-sin((j+1)*3*pi)*res[j+1])/(cos(j*3*pi)*res[j]-cos((j+1)*3*pi)*res[j+1]);
if(k1>=k2)cha=k1-k2;
else cha=k2-k1;
if(cha>0.38)continue;
if( cha<0.12 ){
theta=j*3;
d=res[j];
Add(j,0);
}
else{
theta=j*3;
d=res[j];
Add(j,1);
}
}
//判断哪个方向
forward[3]=-1000;
for(j=1;j<=4;j++){
if(forward[j]>Max)Max=forward[j];
}
//小车运行:
for(j=1;j<=4;j++){
if(forward[j]==Max){
if(j==1)USART_SendData(USART1,0x01);
if(j==2)USART_SendData(USART1,0x02);
if(j==3)USART_SendData(USART1,0x03);
if(j==4)USART_SendData(USART1,0x04);
}
forward[j]=0;
}
硬件连接
设置好串口,中断(具体详见代码)