LabVIEW - A*仿真
算是了解一下LabVIEW的基本的结构和类型。
虽然还是没看懂那个例程，刚开始接触的时候感觉一个框按进去又是一个框简直心理阴影……
一块是对例程（Astar on Occupancy Grid）的改写，这个例程是对已经栅格化的数据地图指定起始点和终止点，第二部分是基于这个的仿真。
l LabVIEW接口
在这一部分我们尝试对A*例程中的核心部分：FindPath With Astar重写。
LabVIEW中为了图形化编程的需要，数据类型比较复杂。而matlab脚本能接收的只有不多的类型（如图），所以这一块比较繁琐的部分是数据类型的转换。
1示例分析（Astar）
示例的第一部分为读取txt并传输对地图的引用。
估值函数有曼哈顿距离和欧拉距离两种选择。
初始地图中1代表可通行的路，100代表障碍。障碍的代价相对较大，但在这个例子中它并非不可穿越，只是在这种情况下我们可以相信如果存在可通行的路线，机器人会选择这条路线而不是障碍。而在实际地图仿真的情况下，如果地图过为繁琐，则可能出现无法正确寻路的情况。
对于搜索出路径上的值的处理方法是对于各个点减三。在实际显示的时候100对应黑，97对应红，1对应白，-2对应蓝。
2簇Cluster
LabView中的cluster数据类型能集合不同的基础数据类型。在这次的实验中最关键的传入参数map reference，是对地图参数的一个引用。而地图的参数为
3引用Reference
引用是以地址的形式传递，在这种情况下能够减少赋值的时间。
（以某次调试为例：
在读取引用的时候用到的结构为
4具体实现
一开始尝试用GetCell Reference来读取cost，形成一维数组，利用sizeX和sizeY在matlab中恢复原地图。然后实现的时候，发现例程的Get All Occupancy Grid Cell的subVI中，已经实现了这一功能。它的具体操作遍历所有的点，取出每个xy对应的cost。利用这样一个模块的传出参数cost array可以实现从LabVIEW到matlab地图信息的传递。
l Matlab代码
·传入参数：Start_node\end_node:起止点，格式为[x坐标，y坐标]
·Map_in: 二维数组，1 - 可通行，100 - 障碍。
·传出参数：Map_out:二维数组，-2&97 - 路径，1&100 – 同上。这个参数主要用于在Astar on Occupancy Grid例程上的调试。
·Dire(direction):一维数组，表示运动的方向。这个参数主要用于仿真。
具体方向为：
伪代码：
|
Loop: current node = has min ( Hamilton distance + cost from start) in active list //Hamilton distance可以替换为其他估值函数 If( search the destination || search the whole map ) return; Remove current node from active list For neighbor of current node if (cost form start is smaller than before) update cost from start& direction add it to the active list end end GotoLoop |
4、模块三代码思路
在这一部分的考虑还是借助Dead Reckoning.lvproj.理由如下：
1. 为了方便代码编写，预计每一步运动会分为旋转确定方向和直行两个阶段。而Dead Reckoning对于两轮运动提供转动速度(CCW Angular Velocity (rad/s))和前进速度(ForwardVelocity (m/s))的控制方式，可以比较方便地实现对小车的控制。
2. 实际仿真结果可能受到各种因素影响。在这种情况下我们可以借助积分求出的估计位置初步判断A*算法的正确性。
1参数设置：
State_temp: 0表示小车旋转阶段，小车方向正确后切入下状态1
1表示小车直行阶段，达到指定时间后转换到状态0
Index: 提供dire的索引(dire介绍见matlab传出参数）
timecount: 通过时间来控制状态的转换
state_now: 记录小车朝向(见matlab传出参数)
2具体实现：
根据state_temp判断目前处于哪一阶段。
·旋转阶段
某一阶段的time_count累计增长到一定值后判断为旋转45°，修改state_now，记录朝向的改变。在这里通过一个数组记录下一个方向。
比如，a[1] = 2代表1的下一个方向为2.
在具体实现的过程中，我们发现如果整个过程仅仅通过单个方向的旋转来调整小车的方向会产生比较大的累计误差。于是我们尝试通过增加一个int类型的变量ok来控制旋转的方向。在原先的单方向旋转的基础上，和a类似，增加反向旋转的数组b。
考虑到如果对每一个状态都进行正向还是逆向旋转的判断，代码过于冗长，我们在分析具体场景的时候，发现在实际情况下，经过膨胀形成的地图之上A*模拟出的路径都比较平滑。也就是说，连续的两个状态在通常情况下状态相同或者在具体方向上相差一个状态（认为1和2、6和9都只相差一个状态）。如下图为小车通过一个转角时候的情况。小车的状态为（从右侧进入）…4-1-2…。因此我们在这个基础上对我们的思路进行了调整。
在这种情况下，我们仅需要对相邻的情况进行判断就能达到较好的效果。如果state == b(state_now),则通过调整控制方向的参数ok的值使得其反向旋转。（后来跑D*的时候会多几次迭代，比如state == b((state_now))）.
那么在第二部分对state_now进行修改的时候，也需要考虑到旋转的方向。
如果state_now== dire[Index] ,也就是说达到这一步需要的方向的时候，进入直行阶段。
在这一步我们一开始没有对ok的值进行修改，在实际运行的时候发现在每次进入直行前都会有一段极短时间的旋转阶段(长度等于time step）。
修改之后，在每次运动到两个格子间的阶段会有一个短暂的静止状态。在这里我们考虑到如果要对这个间隙状态进行调整填补，由于这一部分的传入参数没有所有状态的数组Dire，是通过修改的index影响下一次传入的参数，所以没法在代码中直接获取下一个状态的情况，因此没有对这个状态进行优化。别的考虑是，现有的代码比较容易精简容易理解，此外在仿真中这个短时间的间隔没有很明显的影响。
·直行阶段
需要对直行的时间进行判断。斜对角线的运动比横纵方向的运动需要的时间更多（根号2倍）
这里很容易发现奇数对应的方向为斜对角线方向，偶数为横纵。
这里的问题是在这里时间是一个离散的概念。在这次的仿真中路线较短，如果在更加复杂的地图上可能需要通过调整速度而不是时间来实现对于运动距离的更加精确的控制。在这里，就算不考虑其他的误差，斜线的运动距离也不是横向运动距离标准的根号2倍。
l matlab代码
%%B&C
map = load('Grid1.txt');
[nrows , ncols] = size(map);
start_node = sub2ind(size(map), i_start_node(1)+1, i_start_node(2)+1);
dest_node = sub2ind(size(map), i_goal_node(1)+1, i_goal_node(2)+1);%索引方式不同
dest_y = i_goal_node(1);
dest_x = i_goal_node(2);
% Initialize distance array
distanceFromStart = Inf(nrows,ncols);
direction = zeros(nrows,ncols);
distanceFromStart(start_node) = map(start_node);
%====================
[X, Y] = meshgrid (1:ncols, 1:nrows);
H = abs(Y - dest_y) + abs(X - dest_x) ;
f = Inf(nrows,ncols);
f(start_node) = H(start_node);
%H 哈密顿距离
%f 价值
%distanceFromStart
%=======================
% For each grid cell this array holds the index of its parent
parent = zeros(nrows,ncols);
% Main Loop
while true
%====================
% Find the node with the minimum distance
[~, current] = min(f(:)+distanceFromStart(:));
% [min_dist, ~] = min(distanceFromStart(:));
min_dist = distanceFromStart(current);
%===================
if ((current == dest_node) || isinf(min_dist))
break;
end;
%============
f(current) = Inf;
%============
[i, j] = ind2sub(size(distanceFromStart), current);
%上下左右判断
neighbor = [i-1,j;...
i+1,j;...
i,j+1;...
i,j-1] ;
outRangetest = (neighbor(:,1)<1) + (neighbor(:,1)>nrows) +...
(neighbor(:,2)<1) + (neighbor(:,2)>ncols ) ;
locate = outRangetest>0;
%neighbor(locate,:)=[] ;
neighbor_temp =[];
for i =1:length(neighbor)
if(i~=locate)
neighbor_temp = [neighbor_temp;neighbor(i,:)];
end
end
neighbor = neighbor_temp;
neighborIndex = sub2ind(size(map),neighbor(:,1),neighbor(:,2)) ;
for i=1:length(neighborIndex)
if distanceFromStart(neighborIndex(i))> min_dist + map(neighborIndex(i)) + 1
distanceFromStart(neighborIndex(i)) = min_dist + map(neighborIndex(i)) + 1 ;
parent(neighborIndex(i)) = current;
[m1, n1] = ind2sub(size(map), neighborIndex(i));
[m2, n2] = ind2sub(size(map), current);
direction(neighborIndex(i)) = 3 * (m1-m2) + (n1-n2) + 5;
f(neighborIndex(i)) = H(neighborIndex(i));
end
end
%对角线判断
[i, j] = ind2sub(size(distanceFromStart), current);
neighbor = [i-1,j-1;...
i+1,j-1;...
i+1,j+1;...
i-1,j+1];
outRangetest = (neighbor(:,1)<1) + (neighbor(:,1)>nrows) +...
(neighbor(:,2)<1) + (neighbor(:,2)>ncols ) ;
locate = outRangetest>0;
%neighbor(locate,:)=[];
neighbor_temp =[];
for i =1:length(neighbor)
if(i~=locate)
neighbor_temp = [neighbor_temp;neighbor(i,:)];
end
end
neighbor = neighbor_temp;
neighborIndex = sub2ind(size(map),neighbor(:,1),neighbor(:,2)) ;
for i=1:length(neighborIndex)
if distanceFromStart(neighborIndex(i))> min_dist + map(neighborIndex(i))+ 1.4142
distanceFromStart(neighborIndex(i)) = min_dist + map(neighborIndex(i)) + 1.4142 ;
parent(neighborIndex(i)) = current;
[m1, n1] = ind2sub(size(map), neighborIndex(i));
[m2, n2] = ind2sub(size(map), current);
direction(neighborIndex(i)) = 3 * (m1-m2) + (n1-n2) + 5;
f(neighborIndex(i)) = H(neighborIndex(i));
end
end
end
%%
if (isinf(distanceFromStart(dest_node)))
route = [];
else
%提取路线坐标
route = [dest_node];
dire = [ direction(dest_node) ];
while (parent(route(1)) ~= 0)
route = [parent(route(1)), route];
dire = [direction(route(1)),dire];
end
% 动态显示出路线
for k =1:length(route)
map(route(k)) = map(route(k))-3;
end
end
浙公网安备 33010602011771号