分享一个自己写的超声测距扫描雷达扫描算法模拟代码
应用场景：带舵机控制的超声测距雷达。舵机不停旋转，获得各个位置点距离，代码里体现为数组dist[],值为探测的距离，下标为当前舵机位置。
该算法获得足够宽度足够深度的一个提供小车前进的方向.代码是PC机上的模拟。
#define total_loc 10
unsigned char dist2=
{
1,2,3,4,5,6,7,8,9,10
};
unsigned char dist5=
{
8,7,7,7,3,4,7,8,9,10
};
unsigned char dist1=
{
10,9,8,7,6,5,4,3,2,1
};
unsigned char dist9=
{
1,1,1,1,1,1,1,1,1,1
};
unsigned char dist=
{
1,6,2,6,1,7,6,1,1,9
};
unsigned char dist3=
{
10,10,19,20,100,100,120,13,15,18
};
#define low_bar 4
#define high_bar 5
#include <stdio.h>
#define MINIMAL_SIZE 1
int calc_dir()
{
int start_loc = -1;
int end_loc = -1;
int breadth = 0;
int dir=-1;
int i;
for(i=0;i<total_loc; i++){
if(start_loc == -1 ){
if(dist > high_bar){
start_loc = i;
printf("above bar %d\n",dist);
}
}else{
if(dist < low_bar){
end_loc = i;
printf("start loc is %d\n",start_loc);
printf("end loc is %d\n", end_loc);
if(end_loc - start_loc > breadth){ dir = (end_loc+start_loc)/2; breadth = end_loc-start_loc;}
start_loc = -1;
}
if(i == total_loc-1){
end_loc = total_loc-1;
printf("start loc is %d\n",start_loc);
printf("end loc is %d\n", end_loc);
if(end_loc - start_loc > breadth) { dir = (end_loc+start_loc)/2; breadth = end_loc-start_loc;}
start_loc = -1;
}
}
}
if(breadth <= MINIMAL_SIZE) dir = -1;
printf("final dir is %d\n",dir);
printf("final breadth is %d\n",breadth);
return dir;
}
int main()
{
calc_dir();
return 0;
}
测试效果如何？ MARK期待效果.... 能够用，小车要开的慢。 主要还是超声扫描测距速度不行啊。慢速车还好，快速车就力不从心了。 6K6K 发表于 2013-2-25 21:57 static/image/common/back.gif
主要还是超声扫描测距速度不行啊。慢速车还好，快速车就力不从心了。
确实如此，换了几个办法，基本上还是这样。舵机转速高，噪音大，耗电也厉害 本帖最后由 ronic 于 2013-2-27 07:53 编辑
总结一下三种策略：
策略1. 雷达不停旋转，计算最佳方向，小车不停顿的前进。缺点是在靠近障碍物的时候，如果雷达刚好不对着障碍物，则会碰撞；另外是在碰到死角的时候，无法出来。一个办法是简单降低车速，提高雷达舵机转速。但是舵机转速过高，功耗大，噪音也大；车速也不能太低。
策略2 在雷达测到障碍物时，雷达即停止转动，如果雷达指向位置在车子左边，则驱动车子右边转动；如果雷达指向位置在车子右边，则左转。一直到雷达测到安全距离之后才驱动车子前进。
优点是能够很好的开出死角位置，缺点是如果障碍物刚好在左边，而雷达位置右边的时候扫描到障碍物，则车子左转，直接碰上障碍物。
综合前面，目前使用策略3， 一旦雷达测到障碍物，则立即停车，然后做一个全方向扫描，获得最佳方向，驱动小车向最佳方向转向，而雷达停在刚测到障碍物时候的位置，直到雷达检测到安全直行距离之后直行。目前测试相对结果较好。 第一种算法效果，
http://www.amobbs.com/forum.php?mod=viewthread&tid=5519016&highlight=%E9%81%BF%E9%9A%9C
这个要标记 应该把每次不同角度扫描的结果存储下来，然后根据车前进的路线作个运算。这样就相当于同时有多个超声传感器在工作了。
这样应该可以提高点车速 本帖最后由 ronic 于 2013-2-27 13:24 编辑
inkfish321 发表于 2013-2-27 10:08 static/image/common/back.gif
应该把每次不同角度扫描的结果存储下来，然后根据车前进的路线作个运算。这样就相当于同时有多个超声传感器 ...
嗯，有道理，不过需要更加复杂的计算了。。需要保持之前的“地形”信息。回头我试试看这个算法，应该有较大优化余地 这个可以有 ronic 发表于 2013-2-27 07:47 static/image/common/back.gif
总结一下三种策略：
策略1. 雷达不停旋转，计算最佳方向，小车不停顿的前进。缺点是在靠近障碍物的时候，如 ...
我以前也做过超声波避障的，对此深有体会。
1、测距范围：市售的普通超声波模块测距范围一般4m左右，因此使用超声波模块前瞻只有4m，因此只适合小载体小范围“静态”环境当中使用;
2、检测周期：众所周知，声速传播相对较慢的，单个方向检测时长从几ms到几十ms不等，若多个方向则耗时更长，实时性不能保证，这就限制了载体的运动速度；
我之前的方案是利用空间换取时间，利用多个模块从不同角度检测，当然速度还是很慢。
http://v.youku.com/v_show/id_XNTAyMjc4ODM2.html
瓶颈虽然在此，但还是希望楼主能继续挖掘优化哈 北小斗 发表于 2013-2-27 11:39 static/image/common/back.gif
我以前也做过超声波避障的，对此深有体会。
1、测距范围：市售的普通超声波模块测距范围一般4m左右，因此 ...
呵呵，你的不错了哦，还是比较快，跟我的比较。。。 另外，由于是四轮驱动；而且四轮不时会有其中的轮子打滑，导致转向无法精确控制。。也增加了一些麻烦 ronic 发表于 2013-2-27 13:12 static/image/common/back.gif
呵呵，你的不错了哦，还是比较快，跟我的比较。。。
对于我当时1m/s以上的速度设计要求，还是差远了。另外我当时花的时间不长，做的也比较粗糙，后来工作了也一直没时间改进。
看到你的帖子又勾起了我的兴趣，不知道不知道你现在在工作还是在上学呢？
PS：这是我的帖子http://www.amobbs.com/thread-5516448-1-1.html ronic 发表于 2013-2-27 10:27 static/image/common/back.gif
嗯，有道理，不过需要更加复杂的计算了。。需要保持之前的“地形”信息。回头我试试看这个算法，应该有较 ...
不需要保存地形信息吧。只需要知道车转动的角度和行使的距离，然后根据这个角度对每个虚拟的超声传感器的值加减运算。而传感器扫描到某一角度时则更新相应角度上的虚拟传感器值。 inkfish321 发表于 2013-2-27 14:21 static/image/common/back.gif
不需要保存地形信息吧。只需要知道车转动的角度和行使的距离，然后根据这个角度对每个虚拟的超声传感器的 ...
不错的想法。。可以试试
页:
[1]