目录
1. 基于采样
1.1. 随机路标PRM（Probabilistic Road Map, PRM）
预处理阶段：先在状态空间中随机采样N个点，然后将这N个点 以及起点和终点 尽量相互连接（如果穿过障碍则不连接）。使得这N+2个点构成一个连通图，
查询阶段：使用搜索算法（如A*）在这个离散连通图搜索一条可行路径。
struct ListNode
{
double x;
double y;
vector<int> childrenIndex;
ListNode() {}
ListNode(double px,double py) {x = px;y = py;}
ListNode(double px,double py, vector<int*> _children)
{x = px;y = py;childrenIndex = _children;}
}; //节点
vector<vector<double>> PRM(vector<double> start,vector<double> goal,scene scenario,int N,int checkPointsNum){
//start 和 goal 为起点终点坐标
// scenario为地图区域对象
//N为增加的采样点的个数,checkPointsNum为碰撞检测的点个数
//创建起点 和 终点 节点
ListNode startl = ListNode(start[0],start[1]);
ListNode goall = ListNode(goal[0],goal[1]);
vector<ListNode> vertices{ListNode(),startl,goall}; //节点从下标1开始吧
//随机采样
for(int i=1;i<=N;i++)
{
double sampleX = rand(0,scenario.width);
double sampleY = rand(0,scenario.height);
if(is_feasible(sampleX,sampleY) == 1)
vertices.push_back( ListNode(sampleX,sampleY) );
else
i--;
}
//图的构建
for(int i=1;i<=N+2;i++)
for(int j = i+1;j<=N+2;j++)
{
if( is_collision(vertices[i],vertices[j],checkPointsNum) == 1 ) //碰撞检测
continue;
vertices[i].children.push_back(j);
vertices[j].children.push_back(i);
}
//此处使用A*算法对图寻找可行路径
return Astar(vertices);
}
优点：随着采样点的增加可以找到一个解，即概率收敛为1，算法完备。
缺点：受采样点分布的影响，可能不是最优解。且无法成为连通图时，就需要重新预处理
1.2. 快速搜索随机树RRT（Rapid-exploration Random Tree, RRT ）
在状态空间内建立以起点为根的 搜索树，之后在在空间中不断采样。新的采样点如果不在障碍物内，且与其他节点的连线不发生避碰，则加入到树中。直到树的叶子延伸至目标点即可。
需要说明的是RRT代码可以使用点集和边集 分别记录节点和路径（如伪代码所写），也可以记录每个点的坐标、父节点、节点编号。
struct ListNode
{
double x;
double y;
int fatherIndex;
ListNode() {}
ListNode(double px,double py) {x = px;y = py;}
ListNode(double px,double py, int f)
{x = px;y = py;fatherIndex = f;}
};
vector<vector<double>> RRT(vector<double> start,vector<double> goal,scene scenario,double maxBranchDis,int N,int checkPointsNum,double epslion)
//maxBranchDis为树枝延伸的最大长度，N 为最大采样点数目，
//epslion表示 叶子节点距离 终点 有多近就表示到达目标点了
vector<vector<double>> res;
ListNode startl = ListNode(start[0],start[1],0);
ListNode goall = ListNode(goall[0],goall[1],0);
vector<ListNode> vertices{ListNode(),startl,goall};
int qnewIndex = 1;
for(int i=1;i<=N;i++)
{
if(distance(vertices[qnewIndex],vertices[2]) <= epslion)
{
vertices[2].fatherIndex = qnewIndex;
break;
}
ListNode qrand = ListNode(rand(0,scenario.width),rand(0,scenario.height));
int qnearIndex = findQnearIndex(qrand,vertices);
ListNode qnear = vertices[qnearIndex];
ListNode qnew = ListNode();
bool is_added = createQnew(qnew,qnearIndex,qnear,qrand,scenario,maxBranchDis);
//判断是否能够成功产生 qnew,产生失败则重新迭代
if(is_added)
qnewIndex = insert(qnew,vertices);
else
i--;
}
res = findPath(2,vertices);
return res；
}
其中寻找最近的树节点findQnear() 、insertQnew() **createQnew()**需要定义。
findQnearIndex()与insert()：KD树
对于findQnear()可以将qrand与vertices中的所有节点，一一计算距离找到最近的节点，这样的复杂度为O(n)。对于insertQnew()便可以直接vertices.push_back(qnew)
但可以采用 KD树（K-Dimension Tree） 存储vertices中的节点，通过二分搜索可以在O(logn)时间内完成搜索。
KD树其实是一种特殊的二叉搜索树，是一种多维度的搜索树。可以参考k-d tree算法原理及实现 可用于搜索距离某个节点最近的节点，即找到qnear。
- 构建
如下图所示KD树的构建。KD树是根据vertices中的所有点，划分为多个区域。
以图中ABCDEF点为例。先计算这几个点x轴和y轴哪个方差大，发现x轴方差大。所以按x轴排序为EDBAFC，选择中位数点A。
对A点垂直x轴砍一刀，数据分成EDB和CF两部分。对这两部分的y轴中位数点，垂直于y轴砍一刀，再垂直x轴砍…如此反复即可分成很多块。
因此KD树的构建也是如此，第一x中位数点为根，之后较小的那部分的y中位数点为左孩子，较大的那部分的y中位数点为右孩子。图中点的层序遍历为 ABCEDF空
-
搜索
搜索方法与二叉搜索树类似。以X点为例，其实就是寻找X所在区域的边界。遍历顺序应该为ABE，先令E为最近点。之后以X为圆心，XE长度为半径画圆。可以计算的X.y + || X - E || > B.y， 即圆会夸过B点的分割线，因此再对 B 和B 的 左孩子比较。 -
插入
由于RRT算法是不断增加点，如果每次插入新的点 都仅仅如同二叉搜索树那样只在叶子节点插入，就可能会使最终的KD树失去平衡（最坏情况是O(n)查询速度），因此需要适时平衡。
与平衡二叉搜索树一样，若 左右子树 的高度 差的绝对值 大于1，则进行平衡操作（旋转）
createQnew()
bool createQnew(ListNode &qnew,int qnearIndex,ListNode qnear,ListNode qrand,scene scenario,double maxBranchDis,int checkPointsNum )
{
double disQrand2Qnear = distance(qnear,qrand);
qnew = ListNode(qnear.x,qnear.y,qnearIndex);
double dx = maxBranchDis / disQrand2Qnear * (qrand.x - qnear.x) / (checkPointsNum +1);
double dy = maxBranchDis / disQrand2Qnear * (qrand.y - qnear.y)/ (checkPointsNum +1);
//每延伸一小步检测一次，直到延伸maxBranchDis
for(int i = 1;i<= checkPointsNum + 1;i++)
if(is_feasible(qnear.x + dx * i,qnear.y + dy * i,scenario) == 1)
{
qnew.x = qnear.x + dx * i;qnew.y = qnear.y + dy * i;
}
else
break;
return qnew == qnear;
}
is_feasible()
碰撞检测，用于判断状态空间中的某一点是否在障碍物内部。
该函数取决于 运动体的几何结构 以及场景障碍物特性。
- 实际上createQnew()已经给出碰撞检测方法，即在qnew和qnear的连接线上等分取点，每次判断点是否在障碍内部即可。再将所有的障碍物近似成规则图形即可。该方法比较简单，但是需要把握好checkPointsNum 和 maxBranchDis的选择
- 另一种方法重写createQnew()，将所有不规则的障碍都近似为矩形进行碰撞检测，例如2D圆形障碍则可以简化成其外接正方形。
下面给出所有障碍物为矩形时的createQnew()
bool createQnew_rectObstacle(ListNode &qnew,int qnearIndex,ListNode qnear,ListNode qrand,scene scenario,double maxBranchDis)
{
double disQrand2Qnear = distance(qnear,qrand);
double qnewx = maxBranchDis / disQrand2Qnear * (qrand.x - qnear.x);
double qnewy = maxBranchDis / disQrand2Qnear * (qrand.y - qnear.y);
qnew = ListNode(qnewx,qnewy,qnearIndex);
return getFeasibleQnew(qnew,qnear,scenario);;
}
is_feasible() 的思路是判断 qnew和qnear是否在矩形的同一侧（若是则安全），之后再判断qnew是否在矩形内部，然后再判断qnear和qnew不在矩形同侧时是否有如下情况发生
对于单个点而言则可以 判断该点是否与障碍物中心的距离 或者与二值化场景中的障碍物部分进行匹配判断。
findPath()
vector<vector<double>> findPath(int goalIndex,vector<ListNode> vertices)
vector<vector<double>> res;
stack<ListNode> s;
int index = goalIndex;
while(index != 0)
{
s.push(vertices[index]);
index = s.top().fatherIndex;
}
while(!s.empty())
{
res.push_back(vector<double>{s.top().x,s.top().y});
s.pop();
}
return res;
1.3. 基于采样法的不足
从求解效率 和 最优解 两个角度分析
- 求解效率：对于RRT算法来说，距离计算、采样点选择、障碍检测
- 最优解：PRM 和 RRT 都是随机采样法，很难得到最优解
距离计算
采样点选择 与RRT-Connect
1. 以一定概率，目标点设定 为 采样点
如果采样完全随机的话，可能会使树枝在整个状态空间内 随机延伸，造成RRT树覆盖整个空间 才找到可行解。这样会降低求解速度。
因此设定一定的概率p，在概率p下将采样点 直接设置为终点，提高效率
即
ListNode qrand = ListNode(rand(0,scenario.width),rand(0,scenario.height));
*****************************************改为**************************************************
if(rand(0,1) < prob)
ListNode qrand = goal;
else
ListNode qrand = ListNode(rand(0,scenario.width),rand(0,scenario.height),0);
2. RRT-Connect
思想是 每次得到新的采样点，均在起点、终点 分别向采样点延伸树枝，进而得到两个RRT树。
之后，叶子节点少的一方的新叶子 向另一方的新叶子延伸，判断是否能够连接。
如果可以 则说明两个RRT相连connect，可行路径达成。
则代码就改成
vector<vector<double>> RRT_Connect(vector<double> start,vector<double> goal,scene scenario,double maxBranchDis,int N,int checkPointsNum,double epslion，double prob)
//maxBranchDis 为可延伸的最大树枝长度
vector<vector<double>> res;
ListNode startl = ListNode(start[0],start[1],0);
ListNode goall = ListNode(goall[0],goall[1],-1);
vector<ListNode> vertices1{ListNode(),startl};
vector<ListNode> vertices2{ListNode(),goall};
int qnew1Index = 0;
int quew2Index = 0;
for(int i=1;i<=N;i++)
{
ListNode qrand1;
ListNode qrand2;
if(rand(0,1) < prob)
{qrand1 = vertices2[1];qrand2 = vertices1[1];}
else
{
qrand1= ListNode(rand(0,scenario.width),rand(0,scenario.height));
qrand2 = ListNode(rand(0,scenario.width),rand(0,scenario.height));
}
int qnear1Index = findQnearIndex(qrand,vertices1);
int qnear2Index = findQnearIndex(qrand,vertices2);
ListNode qnear1 = vertices1[qnear1Index];
ListNode qnear2 = vertices1[qnear2Index];
ListNode qnew1 = ListNode();
bool is_added1 = createQnew(qnew1,qnear1Index,qnear1,qrand,scenario,maxBranchDis);
ListNode qnew2 = ListNode();
bool is_add2 = createQnew(qnew2,qnear2Index,qnear2,qrand,scenario,maxBranchDis);
if(is_added1 )
qnew1Index = insertQnew( qnew1,vertices1 );
else
qnew1Index = qnear1Index;
if(is_added2 )
qnew2Index = insertQnew( qnew2,vertices2 );
else
qnew2Index = qnear2Index;
int qnew2NewIndex = 0;
ListNode qnew2New = ListNode();
bool is_added = createQnew(qnew2New,qnew2Index,qnew2,qnew1,scenario,maxBranchDis);
while(is_added)
{
qnew2NewIndex = insertQnew( qnew2New,vertices2 );
qnew2Index = qnew2NewIndex;
qnew2 = qnew2New;
is_added = createQnew(qnew2New,qnew2Index ,qnew2,qnew1,scenario,maxBranchDis);
}
if(distance(qnew1,qnew2) <= epslion)
break;
else if(vertices1.size() > vertices2.size())
{
swap(vertices1,vertices2);
swap(qnew1Index,qnew2Index);
} //两树保持平衡
}
if(vertices1[1].fatherIndex == -1)
{
swap(vertices1,vertices2);
swap(qnew1Index,qnew2Index);
} //保证树1根为起点，树2根为终点
//注意树1 的遍历顺序 和树2遍历顺序相反
res = findPath(qnew1Index,vertices1);
int index = qnew2Index;
while(index)
{res.push_back(vector<double>{vertices2[qnew2Index].x,vertices2[qnew2Index].y});index = vertices[qnew2Index].fatherIndex;}
return res;
优点：每次扩展都会有一个树向另一棵树连续扩展，扩展速度更快。而且两棵树相互靠近，每个树更容易逃离各自根节点，比起单树更容易找到可行路径。
最优性 与 RRT*
RRT和RRT_Connect的随机采样策略，都很难保证路径的最优策略。于是提出了一种RRT的渐进最优规划算法。
RRT的特点在于每次插入新节点时，判断新节点周围的叶节点 是否可以将新节点作为父节点 以降低这些节点的代价，进而做树的局部调整。
该操作非常类似于松弛 RELAX，即判断是否可以通过qnew，对其邻域内的的节点的沿着树的最短路径进行改进。
代价表示从起始状态 转移到该状态的的花费
** RRT* **
主程序代码与RRT很类似，变化主要体现在ListNode的构建、createQnew()，并删去了insert语句，ListNode添加了代价cost属性，createQnew()将考虑以qnew为圆心的邻居的松弛操作，并直接完成了insert
ListNode加入了代价cost属性
struct ListNode
{
double x;
double y;
double cost; //节点代价
int fatherIndex;
ListNode() {}
ListNode(double px,double py) {x = px;y = py;cost = 0;}
ListNode(double px,double py, double c,int f)
{x = px;y = py;cost = c;fatherIndex = f;}
};
createQnew将完成qnew的定位、碰撞检测，之后是选定父节点，插入qnew。最后直接relax周围节点
bool createQnew(ListNode &qnew,int qnearIndex,vector<ListNode>& vertices,ListNode qrand,scene scenario,double maxBranchDis,int checkPointsNum,double r )
{
qnear = vertices[qnearIndex];
double disQnew2Qnear = distance(qnew,qnear);
qnew = ListNode(qnear.x,qnear.y,qnear.cost ,qnearIndex);
double dx = maxBranchDis / disQnew2Qnear * (qrand.x - qnear.x) / (checkPointsNum +1);
double dy = maxBranchDis / disQnew2Qnear * (qrand.y - qnear.y)/ (checkPointsNum +1);
//每延伸一小步检测一次，直到延伸maxBranchDis
for(int i = 1;i<= checkPointsNum + 1;i++)
if(is_feasible(qnear.x + dx * i,qnear.y + dy * i,scenario) == 1)
{
qnew.x = qnear.x + dx * i;
qnew.y = qnear.y + dy * i;
qnew.cost = qnear.cost + sqrt(dx*dx + dy*dy) * i; //此处代价为欧氏距离
}
else
break;
//qnew存在则插入
if(qnew == qnear)
return 0;
vector<int> QnearInCircle = findQnearInCircle(qnew,vertices,r); //找到qnew半径为r的所有邻居的下标集合，与findQnearIndex()一样KD树的二分搜索+回溯
int QnearInCircleSize = QnearInCircle.size();
//判断是否有其他邻居点能给qnew更小的代价，注意如果选择cost就为欧式距离则这个foreach可以省去，否则不一定
foreach(int qnearIndex:QnearInCircle)
if( qnew.cost > vertices[qnearIndex].cost + distance(qnew,vertices[qnearIndex]) )
{
qnew.cost = vertices[qnearIndex].cost + distance(qnew,vertices[qnearIndex]);
qnew.fatherIndex = qnearIndex;
}
int qnewIndex = insert(qnew,vertices); //可以插入节点qnew了
rewire(qnewIndex,vertices,r);
return 1;
}
void rewire(int qnewIndex, vector<ListNode> & vertices,double r)
{
ListNode qnew = vertices[qnewIndex];
foreach(int qnearIndex:QnearInCircle)
{
if(vertices[qnearIndex].cost > qnew.cost + distance(qnew,vertices[qnearIndex]))
{
vertices[qnearIndex].cost = qnew.cost + distance(qnew,vertices[qnearIndex];
vertices[qnearIndex].fatherIndex = qnewIndex;
//由于qnear改变了代价，所以需要对KD树做调整保证AVL特性
delete(qnearIndex,vertices);
insert(qnearIndex,vertices);
}
}
}
Anytime RRT* with brand-and-bound
RRT*的确能够得到渐进最优的轨迹，轨迹的最优需要不断地采样构建RRT树。
因此为了提高RRT的实时性，一种实时版本的RRT * 被提出，进一步提高了规划路径的最优性。
Anytime RRT * 的基本思想
先为机器人规划出一条可行路径
X
X
X，再确定一个时间
t
c
o
m
t_{com}
tcom。在
X
X
X上找到
t
c
o
m
t_{com}
tcom时刻机器人预计到达的节点
n
n
n。从根到
n
n
n这一段叫做committed trajectory，这段committed trajectory是机器人确定要走的路径，在机器人在committed trajectory行走时，RRT*以
n
n
n为根起点使用不断规划，以此来不断优化
X
X
X/committed trajectory的结果。
分支界定 brand-out-bound algorithm
参考
- https://wenku.baidu.com/view/8de40fafbdeb19e8b8f67c1cfad6195f312be80a.html
- https://zhuanlan.zhihu.com/p/260707853
- https://zhuanlan.zhihu.com/p/51087819
- https://blog.csdn.net/Ms_yjk/article/details/95042764 Anytime Motion
- Planning using the RRT*. S. Karaman, M.R.Walter, A.Perez,etc. IEEE International Conference on Robotics and Automation, 2011.
扫一扫
5495
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
