基于地图已知的机器人自主导航
摘要
基于地图环境已知下的机器人自主导航，通过Matlab2018实现RRT与双向RRT算法实现全局路径规划，并将规划的点列写入txt文件。之后通过VS2013读取点列数据并以一种比较粗糙的控制方法实现对机器人移动路径控制，实现从起始点运动到目标点的任务。
关键词： RRT；双向扩展平衡的连结型双树RRT；全局路径规划；机器人自主导航
一、实验方法-基于地图已知全局路径规划
传统的全局路径规划算法有人工势场法、模糊规则法、遗传算法、神经网络、模拟退火算法、蚁群优化算法等。但这些方法都需要在一个确定的空间内对障碍物进行建模，计算复杂度与机器人自由度呈指数关系，不适合解决多自由度机器人在复杂环境中的规划。基于快速扩展随机树（RRT / rapidly exploring random tree）的路径规划算法，通过对状态空间中的采样点进行碰撞检测，避免了对空间的建模，能够有效地解决高维空间和复杂约束的路径规划问题。该方法的特点是能够快速有效地搜索高维空间，通过状态空间的随机采样点，把搜索导向空白区域，从而寻找到一条从起始点到目标点的规划路径，适合解决多自由度机器人在复杂环境下和动态环境中的路径规划。
本实验在理解了RRT算法为基础，使用双向扩展平衡的连结型双树RRT算法（双向RRT），即RRT_Connect算法来实现在地图已知情况下的全局路径规划问题，采用Matlab与VS2013以txt文件通信相结合方式。
1.1RRT算法
该算法与原始RRT相比，在目标点区域建立第二棵树进行扩展。每一次迭代中，开始步骤与原始的RRT算法一样，都是采样随机点然后进行扩展。然后扩展完第一棵树的新节点
𝑞
𝑛
𝑒
𝑤
𝑞𝑛𝑒𝑤
qnew后，以这个新的目标点作为第二棵树扩展的方向。同时第二棵树扩展的方式略有不同（15~22行），首先它会扩展第一步得到
𝑞
′
𝑛
𝑒
𝑤
𝑞′𝑛𝑒𝑤
q′new，如果没有碰撞，继续往相同的方向扩展第二步，直到扩展失败或者
𝑞
′
𝑛
𝑒
𝑤
=
𝑞
𝑛
𝑒
𝑤
𝑞′𝑛𝑒𝑤=𝑞𝑛𝑒𝑤
q′new=qnew表示与第一棵树相连了，即connect了，整个算法结束。当然每次迭代中必须考虑两棵树的平衡性，即两棵树的节点数的多少（也可以考虑两棵树总共花费的路径长度），交换次序选择“小”的那棵树进行扩展。这种双向的RRT技术具有良好的搜索特性，比原始RRT算法的搜索速度、搜索效率有了显著提高，被广泛应用。首先，Connect算法较之前的算法在扩展的步长上更长，使得树的生长更快；其次，两棵树不断朝向对方交替扩展，而不是采用随机扩展的方式，特别当起始位姿和目标位姿处于约束区域时，两棵树可以通过朝向对方快速扩展而逃离各自的约束区域。这种带有启发性的扩展使得树的扩展更加贪婪和明确，使得双向RRT算法较之单向RRT算法更加有效。
1.3 Matlab与VS2013相结合
如图二程序设计图所示，Matlab与VS2013数据通信采用了txt方式，具体操纵说明如下：
1.3.1在VS2013中对txt进行操作
（1）将初始点与目标点数据写入txt以供matlab读取
void outdata()
{
INT16 Code[4] = { Initial_rPos.coor_x, Initial_rPos.coor_y, Cur_dPos.coor_x, Cur_dPos.coor_y };//需要存入的数据
ofstream output;
output.open("D:\\Users\\Desktop\\Machine\\Plan_client\\Point.txt");//存入的文件路径
if (!output.is_open())
{
cout << "the file open fail" << endl;
exit(1);
}
for (int i = 0; i < 4; i++)
{
output << Code[i] << " " << endl;
}
output.close();
}
（2）在matlab生成坐标点序列后，VS要读取相应txt文件，以获取matlab的点列
void indata()
{
ifstream input;
input.open("E:\\Matlab2018a\\Path_Plan\\Plan_path.txt");
if (!input.is_open())
{
cout << "the file open fail" << endl;
exit(1);
}
for (int i = 0; i < 100; i++)
{
for (int j = 0; j < 2; j++)
{
input >> Trajm[i][j];
if (Trajm[i][j] != 0)
steps++;
}
}
input.close();
}
1.3.2在Matlab2018a中对txt进行操作
（1）读取VS2013写入的txt文件获取初始点与目标点
Point=load('D:\\Users\\Desktop\\Machine\\Plan_client\\Point.txt')
（2）将全局规划的点列写入txt文件以供VS读取
fid = fopen('Plan_path.txt','wt');
mat = int16(New_path);
for i = 1:size(mat, 1)
fprintf(fid, '%d', mat(i,1));fprintf(fid, '\n');
fprintf(fid, '%d', mat(i,2));fprintf(fid, '\n');
end
二、实验过程
2.1从单向RRT出发——发现问题
在matlab2018a编写RRT程序，程序见附录1，实现800*1200像素的图片上寻找目标点到初始点可行路径，在这里先不考虑与VS2013的数据通信，选取初始点为（50，50），目标点为（450，450），matlab运行结果如图三所示。经过反复尝试，设定步长为30，节点阈值为20时，既可以满足全局路径规划的速度性，又可以避免在临近目标点出现多次路径寻找。
但是在实验中发现，RRT全局路径规划会出现如图四所示的情况。对于RRT本身来说这是没有问题的，但是考虑到VS程序中我们假定机器人是一个半径为15的圆，这样此图RRT的轨迹便使机器人与障碍物相撞。为了避免这种情况发生，采取了使障碍物膨胀的方法。
此外为了使目标点更好向外扩张，并加快程序的运行速度。我使用时RRT引入了比较常用的概率法：在随机树每次的生长过程中，根据随机概率来决定qrand是目标点还是随机点。在Sample函数中设定参数Prob，每次得到一个0到1.0的随机值p，当0<p<Prob的时候，随机树朝目标点生长行；当Prob<p<1.0时，随机树朝一个随机方向生长，我设置Prob设置为0.5，经过图三与图四对比实验发现：该方法确实减少了无用路径扩展，加快了程序的运行速度。
经过文献查找，上述采用RRT为单向RRT算法，每次搜索都只有从初始状态点生长的快速扩展随机树来搜索整个状态空间，如果从初始状态点和目标状态点同时生长两棵快速扩展随机树来搜索状态空间，效率会更高，于是接下来在单向RRT基础上，进行了双向RRT实验。
2.2障碍物膨胀
障碍物膨胀的原理很简单：以某白像素点为中心，上移20，下移20，左移20，右移20，形成的40*40的区域内进行判断，缩小计算量。之后如果遍历该区域，如果有黑像素点，且该黑像素点到白像素点欧式距离小于等于20，则该白像素点变为黑像素点。Matlab程序如下，程序包含了对图边缘的处理，其中MapT为标志位，目的是使已经变为黑像素的点不影响之后的判断。
MapT=logical(ones(800,1200)-1);
for i=1:800
for j=1:1200
if((map(i,j)==true)&&(MapT(i,j)==false))
if(i-20>0),m1=i-20;else,m1=1;end
if(i+20<=800),m2=i+20;else,m2=800;end
if(j-20>0),n1=j-20;else,n1=1;end
if(j+20<=1200),n2=j+20;else,n2=1200;end
num=0;
for m=m1:m2
for n=n1:n2
if(((m-i)^2+(n-j)^2<=20^2)&&(MapT(m,n)==false))…
&&(num==0)&&(map(m,n)==false)
map(i,j)=false;MapT(i,j)=true;num=1;end
end
end
end
end
end
其实现效果如图六所示，仔细观察下图我们发现障碍物膨胀边缘会不平整，但是因为膨胀20的缘故，机器人在运动时候不会受其影响而出现碰撞的情况，这也就是选取障碍物膨胀20的原因。
2.3 双向RRT实现
在单向RRT算法的基础上实现双向RRT算法比较简单，参考网上资料与自己理解，编写双向RRT的RRT_Connect程序，程序详见附录2。运行程序如图七所示。
a
2.4 路径化直
双向RRT返回回来的数据是一系列的坐标点，这些坐标点是随机扩展出来的。这些点连接接起来的路径时曲折的，为了使机器人少走弯路，将坐标点去掉一部分，并将路径化直。该想法实现效果图如图八所示，（b）图将（a）图的RRT生成点轨迹化直。
New_path(1,:)=path(1,:);
step=size(path,1);
j=1;
sign=0;
a=New_path(j,:);
for i=2:step
b=path(i,:);
if(a(1)<b(1)),m1=a(1);m2=b(1);else,m1=b(1);m2=a(1);end
if(a(2)<b(2)),n1=a(2);n2=b(2);else,n1=b(2);n2=a(2);end
for m=m1:m2
for n=n1:n2
if(((map(m,n)==false)&&(map(m,n+1)==false))&&(det([a-b;[m,n]-b])*det([a-b;[m,n+1]-b])<=0))...
||(((map(m,n)==false)&&(map(m+1,n)==false))&&(det([a-b;[m,n]-b])*det([a-b;[m+1,n]-b])<=0))...
||(((map(m,n)==false)&&(map(m+1,n+1)==false))&&(det([a-b;[m,n]-b])*det([a-b;[m+1,n+1]-b])<=0))
sign=1; break;end
end
if(sign==1),break;end
end
if(sign==1),j=j+1;New_path(j,:)=path(i-1,:);a=New_path(j,:);end
sign=0;
end
j=j+1;New_path(j,:)=path(i,:);
2.5 机器人控制
双向RRT返回路径点序列， VS2013只要根据这些点序列控制机器人移动即可。程序控制思想如下：角速度控制与线速度控制分开，机器人先转角度，在移动。粗糙一点，直接计算角度差与距离差，通过固定帧数平摊角度差与距离差赋值给角速度与线速度，这样便可以实现对机器人控制。程序见附录3。
三、实验结果
3.1双向RRT实现
双向RRT路径点规划结果如图九所示，这里去掉膨胀后显示的路径规划，实际上也就是机器人移动路径。
通过实验发现，Matalb2018a实现双向RRT全局路径规划返回了精确的点序列，方便了在VS2013端实现对机器人运动的控制。
3.2机器人控制实现
通过设置断点方式并利用txt文本文件实现VS2013与Matlab2018a通信的操作，便可实现对机器人控制，通过上述图九返回的全局规划路径，并利用之前对机器人控制方法可以实现对机器人控制，控制结果如图十所示。下面的图(a)参数设置为10帧，及每一次机器人转10次旋转到下一次要运动方向，机器人位移10次便到达下一个位置点，同理图(b)与图©设置为50帧，而图(d)参数设置为100帧。
通过实验结果，VS2013获取双向RRT的路径规划点序列后，通过粗糙的控制方法是可以实现机器人运动控制，当然也存在一些不足，这里放在实验总结中去详细叙述。
四、实验总结
本次作业使用Matlab2018a与VS2013双软件完成。Matlab2018a实现双向RRT程序的“外包“，当然这样外包是在通过txt文本文件与VS断点实现数据传输通信，它通过读入机器人设定的起始点与目标点坐标规划出一条可行路径，并将此路径点序列写入txt文本文件。VS2013在继续运行后读取RRT路径规划点序列并通过粗糙的控制算法实现对机器人运动控制。当然这样方法存在一些不足之处。
4.1不足之处
4.1.1双软件——操作繁琐
如果要实现上述方法，首先在VS2013中读取RRT点列前设置断点，然后运行VS程序，选择好起始点与目标点后，程序将两点信息写入txt文件中后便进入断点暂停。这时运行Matlab中的RRT_Connect程序，程序便会读取你设置的起始点与目标点信息，经过双向RRT后获取优化的路径点序列并将点序列写入另一个txt文件中。完成后你便可以继续运行VS程序，最终你会看到机器人在你控制下从起始点运动至目标点。这样的操作步骤是比较繁琐的，但是幸好传递过来的数据是非常精确的，不会出现数据传输错误问题。
4.1.2 粗糙控制方法存在弊端——决定运行速度不能过快
最终程序选用50帧参数，因为50帧意味着50次完成依次角度旋转或位置移动，每一次旋转角度，位移距离都是存在误差的，并且这样的误差会累积，形成累计误差。如果累计误差过大会深深影响机器热的运动精度，最终导致机器人偏离预想航向，如下图十一所示，当帧数设置为5，其累计误差较大，最终出现碰撞。
4.1.3 起始点与目标点不能设置靠近障碍物20范围内
因为当初为了避免RRT算法出现通过狭窄路径情况并避免机器人半径15带来的影响，选择在进行RRT算法之前将障碍物膨胀20。如果你选定目标点与起始点靠近障碍物20内，RRT算法将自动出现错误，因为读入数据将首先进行与地图比较，程序如下，看是否与地图内障碍物冲突。
if ~feasiblePoint(source,map), error('source lies on an obstacle or outside map'); end
if ~feasiblePoint(goal,map), error('goal lies on an obstacle or outside map'); end
…………………………………………………………………………….
%% feasiblePoint函数 检查无碰撞点和内部地图
function feasible=feasiblePoint(point,map)
feasible=true;
if ~(point(1)>=1 && point(1)<=size(map,1) && point(2)>=1 && point(2)<=size(map,2) && map(point(1),point(2))==1)
feasible=false;
end
end
致谢
感谢石老师在机器人自主导航与环境建模这门课上的精彩讲解。通过对门课的学习与实践，使我对视觉感知、多传感器信息融合、运动规划与自主定位有了初步的了解。在此了解的基础上，我深入学习了运动规划的内容，并利用其中的RRT算法实现了本次个人作业的考核要求。此外还要感谢石老师认真对我们作业遇到问题进行答疑，解答我们的困惑，使我和同学们能顺利完成此次个人作业。
二二年十二月 于南京
链接: 视频演示：link.
程序代码：link.
881
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
