未知环境探索（三）RRT 与SRT
文章：Rapidly-Exploring Random Trees:A New Tool for Path Planning
作者的个人网站：http://msl.cs.uiuc.edu/rrt/ 可惜软件部分是基于一款收费软件，用不起来了。
文章介绍的算法思想很简单。但因为只下载到图片形式的文章，查单词太不方便了，再加上中间还有非完整性约束这些我不懂的东西，有的地方翻译起来比较吃力。
所以这里只介绍它的算法流程。
该文中有详细描述这种算法的优点和缺点。
1.初始化起始点。比如设置机器人所在的位置为初始点。
2.产生一系列随机点Xrand。（随机点是朝着目标前进的，否则到不了）
3.挑选随机点到目标点最近的一个为Xnear;
4.选择一个输入u，使Xnear 到Xrand距离最近（没太搞懂）
5.以Xnear为下一个起始点。
6.将新产生的节点压入堆栈，将Xnear到Xnew之间的边加入堆栈。
12.3日修正。以上的理解是错误的？
正确的应该参照路径规划（1）中。
1.初始化起始点
2.随机产生目标点（假设这个目标点是边界点）
3.遍历T，如果通过T能到达目标点，则画出初始到目标点的路径
4.否则 扩展T；
扩展树结构：
1.在树结构中找到离目标点最近的点Qnear
2.从这个最近的点，目标点，以及某种随机产生方式u，得到一个新的点Qnew.
3.判断Qnew是不是满足要求（朝目标点更近了一步，且分布在安全区域）
4.如果是，Qnew作为新的树的顶点，从Qnear到Qnew去。
返回树结构。
如下：
pos getNextPos(pos a)
{
double rnd=rand()%360*1.0;//0到360的整数，转化为浮点型。
double k=rand()%100;//rand ()是随机产生0到100 的随机数
double xDet=cos(rnd*3.1421592654/360.0)*minPath;
double yDet=sin(rnd*3.1421592654/360.0)*minPath;
double len=getDis(a,end1);//每个当前点到目标点的距离
pos temp;
temp.x=(end1.x-a.x)*minPath/len;//cos（当前点，目标点）
temp.y=(end1.y-a.y)*minPath/len;//sin（当前点，目标点）
a.x+=(xDet*(100-k)+temp.x*k)/100;
a.y+=(yDet*(100-k)+temp.y*k)/100;
return a;
}
pos是节点类型。
a.x+=(xDet*(100-k)+temp.x*k)/100;
a.y+=(yDet*(100-k)+temp.y*k)/100;
是产生一个任意方向的随机量以及朝着目标点去的随机量的加权值作为步进量。
void find_rrt()
{
rPath.clear();
pos temp=begin1;
int cnt=0;
while(getDis(temp,end1)>minPath)//当离任何一个障碍物过近的时候，该点不算作目标点，继续产生随机点。
{
pos aPos=getNextPos(temp);
bool flag=true;
int i=0;
for(i=0;i<data.size();i++)
{
if(getDis(aPos,data[i])<minDis)
{
flag=false;
cnt++;
break;
}
}
if(cnt>100000)break;//直到超过100000都无法远离目标
if(flag){rPath.push_back(temp);temp=aPos;}
}
rPath.push_back(temp);
if(cnt<=100000)rPath.push_back(end1);
}
以上就是运用了RRT思想朝着目标点奔跑的算法实现。
接下来， 我们来看看SRT，是如何运用这种方法进行环境探索的吧：
文献：The SRT Method: Randomized strategies for exploration
作者个人网站：http://www.dis.uniroma1.it/~labrob/research/SRT.html
随机点的选取范围在定义的一个安全区域（safe region).因此不同的传感器不同的感知方式，产生的安全范围不同。
这里主要讲述了两种：
1.适合有噪声、低分辨率的SRT-Ball
2.confident（这里怎么翻译实在不知）的SRT-Star。
以上两种方式是在以下工作假设下完成的：
1.工作区间是平面的；
2.机器人是圆盘形状的；
3.机器人总是知道自己所在的位置
4.机器人具有传感器，使得在每一个位置都能估算出周围环境中的free space。
SRT中的每个点都是不冲突且机器人能够达到过的点，以及每个点相应的传感器得到的free space。这样最后得到的路径一定是安全区域内产生的路径。
算法每次迭代，需传感器预估环境中的free space，产生一个以机器人所在位置为中心的星形的安全区域，那么包含当前位置的点Qcurr 以及此处的S被添加入T。
在当前点，由RANDOM_DIR产生一个随机的探索角，RAY则根据当前的安全区域在随机产生的探索角的方向计算出S的r（该点到边界的距离）；
如下图所示：
候选点Qcand点的选择是这样的：在随机选取角的方向前进r*随机选取角。这样就能保证得到的点一定在安全范围内，且通过安全区域内的线路能够达到。
随机选取角越接近1，搜索得越快，越小，探索越安全。
这样的Qcand找出之后，还要通过VALID算法来验证：
1.得到的获选点到当前位置的距离要比Dmin要大；
2.没有落在前向节点的安全区域内（那部分已经探索过了）
如果不满足，就一直寻找，直到满足上面条件的点产生，或者寻找的次数达到最大值，这时就退回到之前的节点从新选择。//这个地方会不会陷入无法跳出？？？
与RRT相比的优点：
1.自动的在空旷的区域步长较大，在杂乱的区域步长较小。并且不需要探测是否有碰撞，因为本生就是在安全距离中行走；
2.RRT是广度优先，SRT是深度优先；文中的回溯机制也完全来自与深度优先；
3.保留了RRT的一些优点，适用于高维，易于修改，适用于完全和非完全约束；
SRT-Ball
安全区域被定义为一个ball；其实是star的极限特殊情况；
安全区域的半径由到障碍物的最短距离和传感器的测距范围之间较小的一个定义；
安全区域是这些不同大小的ball的联合区域；
做了一组实验：
机器人从场景中间开始，探索到右下角出不去之后，回溯。最后机器人回溯到最初的位置停下来。
在这种环境下，本来出口离机器人很近，但它视而不见。直至保守的探查所有的区域之后才找到出口；
这种方式忽略了传感器本身提供的方向信息；
SRT-Star
安全区域用一系列的锥形集合表示；每个锥形的半径可以不同；最小的半径是到障碍物的半径，大的半径是传感器的测距范围；
因此用RAY来产生半径r的时候，必须对应每个椎体的随机角；
对比SRT-Ball。明显要少很多弯路；
用SRT-Star方法，用扫地机器人做的实验：
