问题:小车快速找到迷宫终点并以最短路径回到起点
首先分析问题,将复杂的问题拆解成一个一个易解决的小问题,那么废话不多说,直接开始解析:
利用左手法则走到迷宫重点,利用LSRB算法计算最短路径并返回,我把这个项目分成了以下几
个部分
核心问题及解决方案
问题1:迷宫车如何走到终点 解决方案:左手法则
左手法则:当小车位于迷宫中遇到拐点时,优先走左边的路线,如果左边不存在线路时,其次走中间线路,当左边及中间都不存在线路时,最后才走右边线路。通过遵循这些规则,可以逐步探索迷宫,最终找到出口。
问题2:迷宫小车如何最短路径回到起点 解决方案:LSRB算法路径规划
LSRB算法路径规划:LSRB 算法的全称是"Left, Straight, Right, Backtrack",它通过模拟小车在迷宫中行驶的方式来寻找最短路径。小车首先尽可能向左转记录为’L',其次直行记录为‘S',最后右转记录为'R',如果这三个方向都无法前进,小车返回记录为’B‘。小车将经过的拐点数据全部记录下来,并通过算法将其计算出最短路径,具体算法为:
- LBR → B
- LBS → R
- RBL → B
- SBL → R
- SBS → B
- LBL → S
例如:
在此图中记录的数据应为[L L B R L]通过算法规则化为[L B L],再通过算法规则化为[S],其他规则同理可证
算法代码:
if(acho[i]==3)//按规则合并
{
if(acho[i-1]==1&&acho[i+1]==2)
{
acho[i-1]=3;
for(k=i+2;k<100;k++)
{
acho[k-2]=acho[k];
}
i=0;
goto c;
}
}
问题3:检测拐点问题
解决方案:线路与地面形成强烈反差,利用OPENMV灰度图,阈值不同将线路凸显出来,不同的线路在OPENMV中显示不同的图形,例如:左边有路即对应OPENMV视角中具有向左延申的线段,前方有路即对应OPENMV视角中具有向前延申的线段,右边有路即对应OPENMV视角中具有向右延申的线段。将OPENMV中左方,前方,右方延申线段框选适当的感兴趣区(roi),在这些感兴趣区出现对应线段的颜色说明此时在对应感兴趣区具有延申线段,即对应感兴趣区的延申线段对应哪一边有路。这样就可检测拐点的位置
注意1:OPENMV中左方,前方,右方延申线段框选的感兴趣区不应离中间太近,防止将不直的路线误测为拐点
注意2:OPENMV受光线影响,在T型路口易缺失左边检测(右边更快检测到线段,将路口误认为右转),根据左手法则,优先左边检测,可将左边感兴趣区框选到更快检测到左边线路的位置
问题4:转向不为直角
解决方案:转向闭环,向左向右转向时,当再次检测到循迹标志时停止转向
代码:
void left(void)
{
while(lenth<450)
{
motor_set(1000,1000);
}
lenth=0;
while(lenth<400)
{
motor_set(-1000,1000);
}
lenth=0;
while(rho!=1)
{
motor_set(-1000,1000);
}
}
问题5:循迹
解决方案:将所寻线段分为三个相邻感兴趣区,分别将三个感兴趣区检测到的线路坐标取出,将这些坐标按照不同权重计算出最终坐标,将当前最终坐标与目标坐标加入PID计算即可循迹。区别与线性回归,这样既可以识别,又可以循迹
代码:
#得到巡线模拟量函数:
def getLine(img):
global line_rho
global Element_data
global turn_stop
line_blobs=[]
centroid_sum = 0 #权重
for r in black_roi: #利用颜色识别分别寻找三个矩形区域内的线段
black_blobs = img.find_blobs([black], roi=r[0:4],y_stride=10,area_threshold=100, merge=True)
if black_blobs:
largest_blob = max(black_blobs, key=lambda b: b.pixels()) #查找像素最多的blob的索引
line_blobs.append(largest_blob) #加入 line_blobs 列表
centroid_sum += largest_blob.cx() * r[4] #计算centroid_sum,centroid_sum等于
#每个区域的最大颜色块的中心点的x坐标值乘
#本区域的权值
line_rho = int(centroid_sum / weight_sum) #中间值公式
turn_stop=0
if len(line_blobs)>2:
turn_stop = abs(line_blobs[0].cx() - line_blobs[2].cx())
else:
turn_stop = 100
return line_blobs
视频:
迷宫小车视频
资源:
- 制作不易,资源付费(作者早餐要吃几个馒头),需要资源私信我
- 请在我的资源中下载