智能小车中的迷宫算法

智能小车中的迷宫算法

2008-10-27 15:20

智能小车中的迷宫算法

看了周立功上面的电脑鼠走迷宫的视频感觉非常有趣，一直都做个小车玩，可没材料，只能看着视频上的小车路行轨迹整出来了这个算法，我不知道真正的算法是怎么实现的，这只是我自己想的一个算法，而且没有完整的小车程序，有空买了小车的再整理总程序。

/pro_ydkz/MicroMouse615.asp这是视频地址。

先看看那大体的迷宫图，随便画的，不是很准确，红色的是小车的运行轨迹，蓝色小圈表示要保存的节点，右下角是起始点：

首先是数据结构，对于整个程序来说，首先要做的是把整个图存下来，有过数据结构基础的这个应该不难，图一般是以结点的方式存储，也就是图中的蓝色小圈，结点的存储格式也是很重要的，我前后尝试了好几种才确定下来。节点有两中逻辑相连方式，一个是图形连接，对应* lt_north,*lt_west,*lt_south,*lt_east，一个是线性连接，对应*frontpoint和*nextpoint，线性连接是为了容易判断当前小车所到结点是否已经记录，也为了后面迷宫算法的树形实现。如下：Struct mappoint

{

Float point_x,point_y; //我是以坐标形式存储，这事相对坐标

Bool ltb_north,ltb_west,ltb_south,ltb_east; //这是记录每个结前后左右

是否有相通结

Mappoint * lt_north,*lt_west,*lt_south,*lt_east; //这是前后左右相通节点的地址

Mappoint *frontpoint,*nextpoint; //这是前一结点和后一结点的

地址

}

看着很简单，但他确实很简单，但你也得尝试几次才会知道怎样组织最合适。

下面是算法了，首先是程序的流程，可怜的我到现在还没用过超声波传感器和伺服电机，所以只能假设了，伺服电机貌似得用时钟中断控制，超声波假设也是轮询寄存器。那大体流程图是这样的：

先说一下的是小车的行驶流程，小车是以右手法则判断的，即先为右，次为前，再次为左，每当传感器变化时先计算出此结点的相对坐标，并判断这个结点是否存在，不存在就建立新结点，并根据当前值设置其方向量和其他相邻结点的指针并设置线性表的当前顺序结点。因为小车是以右为先，当前面有两个方向时他会转向优先方向，这时可以把没走的bool型设置成true，相应的指针设置成空，这样表示此方向可以走但没有行驶过。如果结点存在，先将当前结点与此结点连接，并以此结点为起始结点向上查询最近的有方向为通但未行驶过的结点，并找出这两个结点最短路径并回走，继续探索未探索的路径。这个寻找两结点的算法是这个程序中的中心算法。

首先是时钟控制伺服电机的转动并记录脉冲以计算小车的坐标，设定一个方向量，可以首先在主程序中设置一个枚举类型，这样好理解，也可以直接一个整型量，只要小心点用就好，可以设个int lt_dir;用1、2、3、4表示东、西、南、北，当小车转弯时判断当前值并改变。

接下来是图的建立，先建立一个图的跟结点，mappoint lt_map;并根据传感器信息设置四个方向的值，其坐标值设置为0，0。主程序中要有个当前结点实例，以保存小车最近走过的结点lt_this。当判断到有个新的结点时就创建一个新的结点，不用实例一个实例名，直接用当前结点相对方向的指针指向就行了，并将当前的线性结构的下一指针指向当前结点，例如假设现在小车向北行驶（以上北下南左西右东为准），行驶到一个北向导通，右向也导通的结点，由于右向比前向的优先级大，所以就要往右拐，并保存当前结点，如下：

*Lt_this.north=new mappoint(); //这是上一结点北方向结点的指针，即指向此结点

*Lt_this.nextpoint=lt_this.north; //这是上结点的线性存储指针，保存当前结点指针

*Lt_this.north.frontpoint=lt_this; //将当前结点的线性表的前一结点指向前一结点

*lt_this=lt_this.north; //将当前结点切换成此结点

*lt_this.south=lt_this.front; //相对于此结点上一结点位于他的南向*lt_this.ltb_north=true; //北向和西向和南向都导通，

*lt_this.ltb_west=true;

*lt_this.ltb_south=true;

当行驶到以记录过的点时就要靠线性结构从当前结点往上查询看哪一个点

有导通方向但小车没行驶过的路径，就以此两点计算出最短路径，计算出以此路径回走继续探索迷宫，记录完整个迷宫就回起点准备第二次进军最短迷宫路径了。

最短路径算法：

这是这个的中心算法，其实有很多种算法都可以实现，我是结合树形结构实现的。就是另外建一张图，当前结点为树根，依照右手法则将每个结点线性的存储到表中，新加结点都要查询一次是否和当前树中的两个结点有连接，若是，就以此点延两棵树枝逆向计算出两个路径的长度，将此结点保存到长度短的树枝中，如果一样长就以右手先保存。当探索到目标结点时就加入此树枝并结束此树枝的探索，如果此树枝连接不到目标结点就结束了可以删除也可以不做处理直接终结。最后结束所有结点的探索时就延此树根逆向超找目标结点，超找到就以此树枝逆向求长度，找到最短路径的结点，以此结点前进。

算法就不写了，等器件买来整个程序写完再发上来。

最后发一下牢骚，TMD真是无奈，到了这个破烂三本学校啥子东西都没有，实验室是刚建的什么器件都没有，想做个什么东西都得自己买，要是等学校买还不知道等到哪辈子。以前还觉得大学都差不多，随便上个都一样，现在才知道差距，有实力学校的什么实验室都有，什么器件都有，想做哪个方面就给你器材。哎！无奈了，刚给凌阳小车下了订单，等着到货就把这个做好，还是自己掏的腰包，可怜啊！！！