Sequential Planning

本文主要是介绍Sequential Planning，希望对大家解决编程问题提供一定的参考价值，需要的开发者们随着小编来一起学习吧！

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) );elsei--;
}	//图的构建
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);elsei--;}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);
//每延伸一小步检测一次，直到延伸maxBranchDisfor(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;}elsebreak;return qnew == qnear;
}

is_feasible()

碰撞检测，用于判断状态空间中的某一点是否在障碍物内部。
该函数取决于 运动体的几何结构 以及场景障碍物特性。

1. 实际上createQnew()已经给出碰撞检测方法，即在qnew和qnear的连接线上等分取点，每次判断点是否在障碍内部即可。再将所有的障碍物近似成规则图形即可。该方法比较简单，但是需要把握好checkPointsNum 和 maxBranchDis的选择
2. 另一种方法重写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;
elseListNode 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 );elseqnew1Index = qnear1Index;if(is_added2 )qnew2Index = insertQnew( qnew2,vertices2 );elseqnew2Index = 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);//每延伸一小步检测一次，直到延伸maxBranchDisfor(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;	//此处代价为欧氏距离}elsebreak;//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$，再确定一个时间$t_{com}$。在$X$上找到$t_{com}$时刻机器人预计到达的节点$n$。从根到$n$这一段叫做committed trajectory，这段committed trajectory是机器人确定要走的路径，在机器人在committed trajectory行走时，RRT*以$n$为根起点使用不断规划，以此来不断优化$X$/committed trajectory的结果。

分支界定 brand-out-bound algorithm

参考

1. https://wenku.baidu.com/view/8de40fafbdeb19e8b8f67c1cfad6195f312be80a.html
2. https://zhuanlan.zhihu.com/p/260707853
3. https://zhuanlan.zhihu.com/p/51087819
4. https://blog.csdn.net/Ms_yjk/article/details/95042764 Anytime Motion
5. Planning using the RRT*. S. Karaman, M.R.Walter, A.Perez,etc. IEEE International Conference on Robotics and Automation, 2011.

这篇关于Sequential Planning的文章就介绍到这儿，希望我们推荐的文章对编程师们有所帮助！

---