路径规划神经网络 路径规划原理

一，路径规划是什么

在机器人导航中控制机器人从A点移动到B点时，形成的一条让机器人走的又快又好的路线，就叫路径规划。

路径规划有两个过程：

1，路径规划：使用A* RRT* 等算法在（栅格地图，四/八叉图 ，RRT地图等）搜索一条从A点到B点的路径，由离散点组成。

2，轨迹规划：将路径规划形成的离散点用光滑的线条连接起来（多项式 B样条）

二，轨迹是什么样的

轨迹一般用N次多项式（polynominal）表示 (B 样条 好像更好，具体形式后面详细说明)

 

 

 

 p0 p1 p2 .....pn是多项式的系数，把[p0 p1 p2.......pn]写成向量形式P=[p0 p1 p2.......pn]^T

多项式就可以写成

 p(t)=[t⁰,t,t²,t³......tⁿ]P

对于任意时刻可以求出机器人的速度V,加速度a,加加速度（jerk）,snap.

一段复杂的轨迹由多个多项式表示出来，形如

 

 k为轨迹的段数，i指的第几段

当然这是在一维空间对时间的函数，对于二维，三维，是多个多项式的合成

二，minimum snap

轨迹规划的目的是：求多项式的系数p

我们希望轨迹满足一定的约束条件，速度，加速度，加加速度，初始位置的速度，加速度，方向，末尾位置的速度，加速度，位置，方向，经过中间点的位置，相邻轨迹速度的连续（加速度可导，在安全区域内，轨迹的加速度小于最大加速度，速度小于最大速度等。通常满足这些约束条件的轨迹有无数条，于是我们需要构造一个目标函数，求最优解。

形如：

 

这是一个关于p的函数，可以求解出来最优轨迹的系数P（注意：这里的轨迹参数p是多端polynomial组成的大参数向量

），Minimum Snap顾名思义，Minimum Snap中的最小化目标函数是Snap（加速度的二阶导），当然你也可以最小化Acceleration（加速度）或者Jerk（加速度的导数），至于它们之间有什么区别，quora上有讨论。一般不会最小化速度。

三，例子

 给定起点和终点的位置，速度，加速度，中途经过K+1个路径点，给定时间T，规划一条光滑轨迹。

a.时间分配

　　两种方法1）平均分配 ,根据每段占总的路程的比例分配时间. 2)梯形分配，如图

 

 

 

 这里的轨迹分段和时间分配都是初始分配，在迭代算法中，如果 Obstruction check和 feasibility check不满足条件，会插点或增大某一段的时间。

b.构建优化函数

如图（图中的倒数第二行好像有点问题）

 

 其中

 

可以看出问题构造成了一个QP问题吗，接下来我们构建等式约束，（不等式约束主要用于软约束）

设定某一个位置的速度加速度为特定值，为一个等式约束

 

 

 

 例如：

 

 

 

     对中间点也构成等式约束

 

2.对相邻两段轨迹的速度连续，或加速度连续构成轨迹光滑约束

 

 

 

 例如，第i，i+1段 轨迹的光滑约束为

 

 合并所有等式约束为

 

 

 等式约束个数=3（起始PVA）+k-1（中间点的p）+3（终点pva）+3(k-1)（中间点PVA连续）=4k+2。

 

构建不等式约束

不等式约束与等式约束类似，也是设置某个点的P、V、A小于某一特定值，从而构建Aieqp=bieq

，不等式约束一般是在corridor中用的比较多，这里暂时先不使用不等式约束。

求解

利用QP求解器进行求解，在MATLAB中可以使用quadprog() 函数，C++的QP求解器如OOQP，也可以自己去网上找。