机器人抓取与操作经典规划算法(深蓝)——2

1 经典规划算法

位姿估计：（1）相机系位姿（2）机器人系位姿

抓取位姿：（1）抓取位姿计算（2）抓取评估和优化

路径规划：（1）笛卡尔空间点（2）IK求解（3）关节空间插值

任务规划：（1）装配任务（2）派件任务（TSP问题，VRP问题）

路径规划：A点到B点

笛卡尔直线或者C-space直线

避障问题&环境改变

路径生成后速度规划

路径规划算法------全局规划避障
因为障碍物的存在，可能无法是一条直线，需要采样、连接和搜索

构型空间和笛卡尔空间

（1）在哪个空间做local planning

（2）一般约束会存在笛卡尔空间

（3）需要FK和IK的转换

轨迹生成和优化

在机械臂上面，基于采样的规划算法用的多一点，主要解决非凸的高维空间的搜索问题

（1）Single-query:RRT系列基于树结构的方法

（2）Multi-query:PRM系列基于图的方法

因为有障碍物的存在，碰撞检测需要用来确认路径的可行性

2 基于树结构的方法：RRT系列

随机采样、局部规划、碰撞检测

RRT算法，输入就是起始点和终点，还有一个上图所示的障碍物分布，输出就是起始点到终点无碰撞的一条路径

首先对树结构进行初始化，然后迭代N次循环，循环第一步是先随机采样一个随机点 x r a n d x_{rand} xrand，然后找到离随机点最近的点 x n e a r x_{near} xnear，在 x n e a r x_{near} xnear和 x r a n d x_{rand} xrand之间以StepSize为步长选择一个 x n e w x_{new} xnew，把 x n e w x_{new} xnew和 x n e a r x_{near} xnear连接成的边做碰撞检测，如果无碰撞，加入到树结构，如果 x n e w = x g o a l x_{new}=x_{goal} xnew=xgoal，成功搜索到路径

上面的 x n e w = x g o a l x_{new}=x_{goal} xnew=xgoal，在实际使用的时候一般说距离终点???m，即可Success

RRT拓展------RRT-Connect

(1) 简单但实用

(2) 不同于RRT，生成树是同时从起点和终点拓展

两个树同时开始拓展，以 q g o a l q_{goal} qgoal拓展的随机目标点和另一个树拓展有一个比较近的点 q n e a r q_{near} qnear，然后连接这两个点，在上面做无碰撞的拓展

RRT拓展------RRT*

（1）概率完备性

（2）渐近最优性（不能保证最优）

主要是在碰撞检测这块与RRT不一样，基于 x n e w x_{new} xnew有一个搜索半径，在这个半径里的节点，重新选择离 x n e w x_{new} xnew最近的父节点 x m i n x_{min} xmin，连接 x m i n x_{min} xmin和 x n e w x_{new} xnew边添加到树里，rewire让最优性得到了保证。