本发明涉及无人驾驶车辆路径规划领域，尤其是涉及一种改进的无人车快速搜索随机树路径规划方法。
背景技术：
由于无人驾驶车辆在减少交通事故及人员伤亡、缓解降低交通拥堵、减少用户在驾驶上消耗的精力等方面的优势，获得了学术界和产业界的广泛关注。无人驾驶车辆的实施涉及各个领域，包括感知、规划、决策和控制技术。要实现无人车的自主导航，全局的路径规划具有十分重要的意义。路径规划能力是无人车的基础，只有具有良好的路径规划能力的无人车才有可能真正解放人类，自主地将人类带到其目的地。
路径规划是指寻找一条从给定起点到目标点的路径，使得无人车能够安全无碰撞地绕过环境中的障碍物达到目标点。其中，基于启发式的规划算法，包括遗传算法，蚁群算法等，在解决一般的规划问题时有其优越性，但由于它们需要对状态空间中的障碍物进行精确建模，计算复杂度高，不适用于复杂环境下的路径规划。基于搜索的规划算法，如Dijstra算法、A*等，在规划时虽然能满足最优性的要求，但并未考虑无人车非完整约束的限制，使得规划的路径不一定能够被无人车所跟随。
LaValle提出的快速搜索随机树(Rapidly Random-exploring Tree，RRT)算法，采用了随机采样的规划方法，避免了对自由状态空间和障碍物空间的精确建模，搜索速度快，而且可以考虑智能体存在的非完整约束，从而能够有效地解决复杂约束下的路径规划问题。并在机器人、机械臂和无人车等多个领域得到广泛应用。
但它也存在一些明显的缺陷：算法的随机性会导致较多冗余节点的存在，且生成的路径非平滑路径，无法被非完整约束的智能体直接跟随。针对这两个不足，国内外学者也对该算法进行了不断地改进，以适应实际的应用环境.
对于节点冗余问题，文献(Meng L，Qing S，Jun ZQ.UAV path re-planning based on improved bidirectional RRT algorithm in dynamic environment。The 3rd International Conference on Control，Automation and Robotics(ICCAR)，IEEE.p658-661.Apr.2017)采用单向剪枝来减少冗余的节点，简单快速，但仍然有很大的缩短空间；文献(Kuwata Y，Teo J，Fiore G，et al。Real-time motion planning with applications to autonomous urban driving.IEEE Transactions on Control Systems Technology，v 17，n 5，p 1105-1118，Sep.2009.)引入了一种Dubins路径，但是由于其通过直线和圆弧拼接来产生路径，因此得到的路径曲率不连续而无法被无人车所跟随。
技术实现要素：
本发明的目的就是为了克服上述现有技术存在的缺陷而提供一种改进的无人车快速搜索随机树路径规划方法。
本发明的目的可以通过以下技术方案来实现：
一种改进的无人车快速搜索随机树路径规划方法，具体包括以下步骤：
1)采用RRT算法搜索获取一条由起点到终点的初始路径；
2)采用采用单向剪枝法对初始路径剪枝得到初步剪枝路径；
3)对初步剪枝路径采用采用双向剪枝法进一步缩短路径的长度；
4)采用削峰法去掉路径上的尖锐夹角，满足最大曲率约束；
5)对削峰后的路径进行平滑处理，得到无人车的跟随路径。
所述的步骤2)具体包括以下步骤：
在RRT算法生成初始路径的基础上，从初始路径起点开始，进行碰撞检测，判断是否能从起点无碰撞地直接到达初始路径的第i个节点，i≥3，若是，则从起点到第i个节点之间的节点为多余节点，并删除，若否，则第i-1个节点为从起点能够无碰撞到达的最远节点，再以第i-1个节点作为起点，重复上述步骤直至到达终点完成单向剪枝。
所述的步骤3)具体包括以下步骤：
将起点单向剪枝生成的路径和终点单向剪枝生成路径相结合，根据两条路径的交叉点，依次选取每两个交叉点间对应的两段路径中较短的路径作为更优的路径，最终得到双向剪枝路径。
所述的步骤4)中，所述的最大曲率约束为：
无人车只能跟随平滑且曲率有界的路径，不能跟随非平滑路径，且路径的曲率不能大于无人车可跟随路径的最大曲率。
所述的步骤5)具体包括以下步骤：
采用均匀三次B样条曲线路径进行拟合得到平滑的B样条曲线，即无人车的跟随路径。
所述的尖锐夹角的范围为0-100度。
与现有技术相比，本发明具有以下优点：
一、本发明创新性地提出基于RRT的双向剪枝法，它能同时结合起点剪枝路径和终点剪枝路径的优势，选取两条路径在每一段中更优的部分，从而得到更短的路径。
二、本发明考虑了无人车的非完整约束，通过削峰法和B样条曲线拟合方法来满足该约束，从而获得连续且平滑的直接可跟随路径。
附图说明
图1为RRT算法节点扩展示意图。
图2为RRT生成路径示意图。
图3为单向剪枝过程示意图。
图4为单向剪枝路径示意图。
图5为终点剪枝路径示意图。
图6为双向剪枝路径示意图。
图7为无人车简化运动模型。
图8为路径削峰示意图。
图9为B样条曲线路径图。
图10为路径曲率变化图。
具体实施方式
下面结合附图和具体实施例对本发明进行详细说明。
实施例
本发明提供一种基于无人车非完整约束的双向剪枝优化RRT路径规划算法，具体包括如下步骤：
S1:采用RRT算法搜索得到一条路径：
如图1所示的RRT算法节点扩展示意图，该算法以起点为根节点，每次均匀随机的生成一个采样节点，并选择树上距它最近的点，从这个点向采样点的方向，按固定的步长扩展下一个节点，重复上述过程直到目标节点被添加到树中时，就找到了一条从起点到终点的路径。
在障碍物环境下，运用上述RRT规划算法生成的路径如图2所示。图2中，黑色条形区域为障碍物空间，白色区域为自由空间，左下角圆点为起点，右上角的方块为终点，浅色虚线为RRT算法尝试的搜索路径，深色折线为RRT算法搜索得到的无碰撞路径。
S2:采用单向剪枝法得到初步剪枝路径：
单向剪枝法是在RRT算法生成的初始路径基础上，如图3所示，从初始路径起点开始，进行碰撞检测，判断是否能从起点无碰撞地直接到达初始路径的第i个节点(i从3开始，因为第二个节点与起点直接相连，无需检测)，如果能够直接到达，则说明从起点到第i个节点之间的节点就是多余的；如果不能直接到达，如图3中的深色虚线所示，则说明从起点到第i个节点会障碍物相撞，则第i-1个节点为从起点能够无碰撞到达的最远节点，再以第i-1个节点作为起点，重复上述步骤直至到达终点。
最终得到的路径图如图4所示，经过单向剪枝之后的路径能较大程度减少初始路径中冗余的节点，但因为单向剪枝法为贪婪剪枝，即剪枝直到碰到障碍物才停止，因此，从图中可以看出得到的路径仍有可以缩短的空间。
S3:采用双向剪枝法进一步缩短路径长度：
对于单向剪枝法，现有的应用都是从起点开始向终点方向剪枝，同样，可以从终点开始向起点方向反向剪枝，得到图5，但会存在和单向剪枝法一样的局限性。因此，将起点剪枝生成路径和终点剪枝生成路径结合在一起，选取两条路径在每一段中更优的部分，作为最终的路径，得到双向剪枝路径，如图6所示。双向剪枝路径能同时结合起点剪枝路径和终点剪枝路径的优势，利用两者的相交探索到新的更优的节点，从而能够得到比起点剪枝路径和终点剪枝路径都要短的路径。
S4:建立非完整约束车辆模型
非完整约束是指含有系统广义坐标导数且不可积分的约束.其中，无人车是一个典型的非完整约束系统，其简化运动模型如图7所示.
在状态空间中无人车状态X＝(x,y,θ)，其中(x,y)是无人车在系统坐标系下的位置，θ是无人车纵轴与坐标系之间的夹角，代表无人车的方向；另外为前轮转角，L为前后轮主轴之间的距离，R为无人车的转弯半径，K则为对应的曲率。
在无人车的简化运动模型下，由于非完整约束的存在，车轮只能在平面内做纯滚动、无滑动的运动，即无横向速度，则必须满足微分约束:
dx·sinθ-dy·cosθ＝0 (1)
因为该约束的存在，在任何运动瞬间，车体的速度方向必然指向无人车的主轴，且不能发生突变，因此车辆无法跟随折线路径。
另外，因为无人车方向盘的物理限制，使得前轮转角有最大限制约束:
其中为无人车最大前轮转角.Rmin为无人车最小转弯半径，Kmax为无人车可跟随路径的最大曲率。
S5:采用削峰法去掉尖锐的夹角：
在非完整约束条件下，无人车只能跟随平滑且曲率有界的路径。不能跟随非平滑路径，且路径的曲率不能大于无人车可跟随路径的最大曲率。而RRT搜索得到的路径和双向剪枝后的路径，都没有考虑无人车的这两点约束，使得生成的路径无法被无人车直接跟随.常用的措施是通过后处理方法使得最终路径满足无人车的约束。
而在双向剪枝路径中，会存在一些较为尖锐的夹角，这些夹角会使得即使平滑后的路径也不能满足无人车的最大曲率约束。本发明通过削峰法来削去尖锐的夹角，如图8所示，削峰能使得新增的夹角更加平缓，进而满足最大曲率约束，同时，削峰后的路径还能减少路径的长度。
S6:采用均匀三次B样条曲线对削峰后的路径进行平滑处理：
对于路径的非平滑问题，常用B样条对路径进行拟合得到平滑的B样条曲线。B样条曲线具有连续性和局部性等优点，使得其在路径规划中的应用非常广泛。
k阶B样条曲线的表达形式为
其中，Pi为控制顶点，u为节点向量，u[u0,u1,u2…,um]，Ni,为k次B样条基函数，为由u决定的k次分段多项式，可以由Cox-deBoor递推公式得到:
采用均匀三次B样条对削峰后的路径进行平滑处理，得到图9，其曲率图为图10所示，可以看出处理后的路径曲率连续且有界，能满足无人车的最大曲率约束，无人车即可直接跟随该路径。