一种基于控制采样的智能车辆路径规划系统及方法与流程
本发明属于计算机和自动化技术，特别是智能车辆路径规划技术领域，具体涉及一种基于控制采样的智能车辆路径规划系统及方法。
背景技术：
路径规划技术是智能车辆关键技术问题之一。智能车辆在动态的不确定环境中，根据环境感知系统和车辆状态测量系统提供的信息，和所要到达的目的地，实时地规划出车辆当前应该行驶的路径，使车辆能安全快速地到达终点。
中国专利申请：用于无人驾驶汽车局部路径规划的装置及方法(申请号：201110007154.x)采用人工势场法综合环障碍物和目标点等因素的影响产生最终路径。该方法简单易行，实时性有保证，但是产生的路径可能违背车辆运动学约束，运动控制系统执行起来会有较大误差。中国专利申请：一种随机路线图的快速路径规划方法及增强方法(申请号：201010519248.0)改进的prm(probabilisticroadmapsmethod，概率路径图法)方法解决了一般prm方法完全等概率均匀分布自由节点导致困难区域自由节点数目分布不够的问题，具有较好的完备性，减少了遗漏某些通过困难区域的可行路径的概率，但与传统prm方法一样未能解决基于随机获得的自由节点产生的路径不能保证符合车辆运动学约束的问题。文献：”杜明博等，复杂环境下基于rrt的智能车辆运动规划算法，机器人,2015(04):443-450”在距离度量函数中加入了对路段间夹角的度量，一定程度上改善路径的平顺性，但此限制排除了大量符合车辆运动学的路径形态，导致在较复杂的环境下路径规划成功率较低。文献：”hundelshausenf.v.，etal.drivingwithtentacles:integralstructuresforsensingandmotion，journaloffieldrobotics，2009，25(9):640-673”通过对预先设定好的平滑路径簇中路径进行评价来选取一条最优路径执行，但没有考虑到车辆前轮转角对路径选择的限制而导致选取的最优路径与车辆实际运动轨迹有较大差距。文献：”leecheng-lung,etal.anewtrajectory-basedpathplanningapproachfordifferentialdrivevehicles，2013ieeeinternationalsymposiumonroboticandsensorsenvironments”依据差分运动方程生成其在不同控制输入和运动状态下的轨迹集，依据探测到的实时障碍位置和实时车速对这些轨迹进行筛选匹配出合适的轨迹执行。该类探索型机器人的应用场景中环境完全是未知的，因此机器人的运动过程并没有清晰的目标，因此是一种反应式运动规划方法，与智能车辆的路径规划应用场景不同。
在现有的路径规划方法中，基于采样的路径规划方法避免了对路径与环境精确建模，具有实现简单，实时性高等特点，因此在智能车辆路径规划领域内被广泛使用。目前流行的prm等基于采样的路径规划方法直接采样于车辆所在的构型空间，产生的路径具有较强的不确定性，路径过于抖动、曲折，不可避免地会违背车辆运动学约束，使得路径的可执行性差。
技术实现要素：
本发明的目的是解决无人驾驶智能车辆的路径规划问题。当存在大量无规则分布障碍物时，现有智能车辆路径规划方法存在建模复杂，计算复杂度高，收敛速度慢，规划出的路径可执行性差等问题，为此本发明提出了一种无须对环境、轨迹等进行复杂的建模、有较快的收敛速度、规划出的路径符合车辆运动学约束，可执行性佳的基于控制采样的智能车辆路径规划系统及方法。本发明的技术方案如下：
一种基于控制采样的智能车辆路径规划系统，包括环境地图模块、车辆状态计算模块和路径搜索模块。其中，所述环境地图模块，用于将车载传感器提供的环境信息以栅格地图形式存储下来，为路径规划提供任务场景信息，供车辆状态计算模块和路径搜索模块访问；还用于环境地图更新，实时接收车辆参数测量装置提供的车辆状态信息和车载传感器提供的环境信息来更新栅格地图。车辆状态计算模块，用于通过碰撞检查算法，判断某一车辆状态是否与环境中障碍物发生碰撞；依据车辆状态扩展模型描述的车辆状态扩展方程和控制空间采样策略，对任意车辆状态扩展，得一簇新的状态；并基于当前状态、环境障碍物信息和本次任务的目标状态，实现对车辆状态的状态评价。路径搜索模块，用于采用初步路径搜索算法，得到以离散状态序列形式的初步路径；并通过路径插值，对初步路径的状态序列，利用满足切向约束的2次b样条插值优化处理，得到连续可执行的最终路径。
进一步的，所述车辆状态计算模块：在实现碰撞检查功能时，将车辆抽象地看做一个矩形，称为车辆区域，车辆区域将车辆整体包括在内，一次碰撞检查过程分为如下两步：
(1)确定扫描栅格范围，为减小一次碰撞检查过程中扫描的栅格范围，取车辆区域的四个顶点，勾勒出一个扫描范围；
(2)判断障碍栅格是否在车辆区域内，对于扫描范围内的栅格，首先判断其是否为障碍物栅格，若不是障碍物栅格，则继续扫描下一个栅格；若为障碍物栅格，则判断障碍栅格是否在车辆区域内，若障碍物栅格在车辆区域内，则判定为碰撞。
进一步的，所述车辆状态计算模块：依据车辆状态扩展方程，在控制输入下，预测一个步长后的车辆状态。
进一步的，所述路径搜索模块：用于采用初步路径搜索算法，得到以离散状态序列形式的初步路径。路径搜索算法具体分为路径选择，节点扩展，节点更新，初步路径生成这四个步骤：
(1)路径选择：从根状态节点起，依据节点选择公式从子状态节点中选择一个状态节点，重复直至到达当前状态树的叶节点；
(2)节点扩展：采样控制输入，并代入状态扩展方程，得到一批新的节点，对这些新节点对应的车辆空间位姿作碰撞检查，保留无碰撞节点为新的叶节点，计算新叶节点的评价值；
(3)节点更新：当状态扩展产生的新状态中没有到达目标状态的节点时，将该次状态扩展的信息沿所选择的路径反向传播，形式为更新路径途径节点的评价值和节点访问次数；
(4)初步路径生成，当状态扩展产生的某新状态到达目标状态时，由根状态节点到该新状态节点的状态序列即为初步路径。
进一步的，所述路径选择，采用uct(upperconfidenceboundappliedtotreesearch，应用于树搜索的上限置信区间方法)选择路径。
进一步的，所述路径搜索模块，用于路径插值，对初步路径的状态序列，利用满足切向约束的2次b样条插值优化处理，得到连续可执行的最终路径。具体包括：对初步路径的状态序列，利用满足切向约束的2次b样条插值优化处理，得到最终的连续曲率且符合车辆非完整约束的易于跟踪的最终路径。
一种基于所述系统的基于控制采样的智能车辆路径规划方法，其包括以下步骤：
(1)更新当前环境地图：从车载传感器读入实时环境障碍信息，将它们以栅格地图形式存储于环境地图模块；每个栅格有占据和非占据两种状态。占据状态表示该栅格所处位置存在障碍物；
(2)设定车辆当前状态：从车辆参数测量装置得到的实时车辆位姿和速度信息，这些参数组成了本次路径规划任务的车辆起始状态；
(3)设定车辆目标状态：设定车辆在任务结束时的状态，在驾驶辅助系统中，目标状态由驾驶员设定；在无人驾驶系统中，目标状态由上层决策系统给出；
(4)初步路径搜索：针对任务起始状态和目标状态运行初步路径搜索算法，路径搜索算法具体分为四步：
(4.1)路径选择：从根状态节点起，依据节点选择公式从子状态节点中选择一个状态节点，重复直至到达当前状态树的叶节点。
(4.2)节点扩展：采样控制输入，并代入状态扩展方程，得到一批新的节点。对这些新节点对应的车辆空间位姿作碰撞检查，保留无碰撞节点为新的叶节点。计算新叶节点的评价值；
(4.3)节点更新：当状态扩展产生的新状态中没有到达目标状态的节点时，将该次状态扩展的信息沿所选择的路径反向传播，形式为更新路径途径节点的评价值和节点访问次数；
(4.4)初步路径生成：重复(4.1)-(4.3)，当状态扩展产生的某新状态到达目标状态时，由根状态节点到该新状态节点的状态序列即为初步路径；
(5)路径插值：对初步路径的状态序列，利用满足切向约束的2次b样条插值优化处理，得到最终的连续曲率且符合车辆非完整约束的易于跟踪的最终路径。
本发明的优点及有益效果如下：
本专利通过建立车辆状态扩展方程，得到控制输入与未来状态的对应关系。依据车辆实际转向能力和控制机构实时状态，实时构建控制输入采样区间。通过在采样区间采样控制输入来不断扩展车辆状态，进而实现增量式路径搜索。这种方式使得路径树上相邻状态点间的过渡满足车辆运动学约束。因此，通过插值得到的最终路径平滑、稳定，曲率半径满足车辆最小转向半径的约束，可大大减少车辆路径跟踪过程的误差。
同时，通过车辆状态扩展方程，本发明将增量搜索过程建模为蒙特卡洛搜索树。将路径搜索问题转化成博弈问题，通过uct方法实现最优路径搜索。uct搜索作为一种并行搜索算法，能突破其他路径规划算法遇到的性能瓶颈，在得到最优路径的同时实时性更好。
附图说明
图1是本发明提供优选实施例一种基于控制采样的智能车辆路径规划系统总体框架；
图2本发明基于控制采样的智能车辆路径规划方法流程；
图3本发明车辆状态及车辆区域定义；
图4本发明碰撞检查算法原理；
图5本发明初步路径搜索过程流程图；
图6本发明执行一次路径规划的实例表示；
具体实施方式
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、详细地描述。所描述的实施例仅仅是本发明的一部分实施例。
本发明解决上述技术问题的技术方案是：
首先依据车辆参数、运动学方程和推导车辆状态扩展方程以及控制输入(前轮转角)的采样范围。然后，从起始状态(当前状态)开始，利用采样得到的控制输入不断扩展状态树，直到状态树叶节点到达目标点。此时得到由状态点序列组成的初始路径。最后，对初始路径进行处理，以使其便于被跟踪执行。
以下结合附图和具体实例对本发明所述系统的具体工作方式进行描述：
如图1所示为本发明一种基于控制采样的智能车辆路径规划系统总体框架,系统包括环境地图模块、车辆状态计算模块和路径搜索模块。
其中，所述环境地图模块，用于将车载传感器提供的环境信息以栅格地图形式存储下来，为路径规划提供任务场景信息，供车辆状态计算模块和路径搜索模块访问；还用于环境地图更新，实时接收车辆参数测量装置提供的车辆状态信息和车载传感器提供的环境信息来更新栅格地图；
车辆状态计算模块，用于通过碰撞检查算法，判断某一车辆状态是否与环境中障碍物发生碰撞；依据车辆状态扩展模型描述的车辆状态扩展方程和控制空间采样策略，对任意车辆状态扩展，得一簇新的状态；并基于当前状态、环境障碍物信息和本次任务的目标状态，实现对车辆状态的状态评价；
路径搜索模块，用于采用初步路径搜索算法，得到以离散状态序列形式的初步路径；并通过路径插值，对初步路径的状态序列，利用满足切向约束的2次b样条插值优化处理，得到连续可执行的最终路径。
如图2所示为本发明一种基于控制采样的智能车辆路径规划方法流程，该方法包括以下步骤：
(1)更新当前环境地图：从车载传感器读入实时环境障碍信息，将它们以栅格地图形式存储于环境地图模块；每个栅格有占据和非占据两种状态。占据状态表示该栅格所处位置存在障碍物；
(2)设定车辆当前状态：从车辆参数测量装置得到的实时车辆位姿和速度信息，这些参数组成了本次路径规划任务的车辆起始状态；
(3)设定车辆目标状态：设定车辆在任务结束时的状态，驾驶辅助系统中目标状态由驾驶员设定；在无人驾驶系统中目标状态由上层决策系统给出；
(4)初步路径搜索：针对任务起始状态和目标状态运行初步路径搜索算法，路径搜索算法具体分为四步：
(4.1)路径选择：从根状态节点起，依据节点选择公式从子状态节点中选择一个状态节点，重复直至到达当前状态树的叶节点。
(4.2)节点扩展：采样控制输入，并代入状态扩展方程，得到一批新的节点。对这些新节点对应的车辆空间位姿作碰撞检查，保留无碰撞节点为新的叶节点。计算新叶节点的评价值；
(4.3)节点更新：当状态扩展产生的新状态中没有到达目标状态的节点时，将该次状态扩展的信息沿所选择的路径反向传播，形式为更新路径途径节点的评价值和节点访问次数；
(4.4)初步路径生成：重复(4.1)-(4.3)，当状态扩展产生的某新状态到达目标状态时，由根状态节点到该新状态节点的状态序列即为初步路径。
(5)路径插值：初步路径是由空间中的车辆状态点序列组成的，属分段线性路径。为使得路径的表示更直观明确，同时提高控制器在路径跟踪时候的跟踪品质，减小跟踪误差，需要对初步路径进行一些优化处理，得到曲率连续且满足非完整约束的最终路径。
由于车辆状态点的状态包括车辆坐标位置和姿态角，相连的两状态点由状态采样扩展而来，其姿态角分别代表该段路径的起点的切线方向和终点的切线方向，因此采用二次b样条曲线插值方法连接各个车辆状态点，得到曲率连续且满足非完整约束的最终路径。
假设组成初步路径的n个状态点的序列为q1,q2,…,qn,提取各状态中的坐标信息得到对应的插值点序列p1,p2,…,pn。提取各状态中的姿态角信息，得到各个插值点处曲线单位切矢v1,v2,…,vn。令
c(ti+1)＝pi,i＝1,2,…,n
采用基于切向约束构造复合二次b样条插值方法，可求出
其中，bi,3(t)是规范二次b样条的基函数。
如图3所示为本发明车辆状态及车辆区域定义，用于车辆状态计算，包括碰撞检查与车辆状态扩展。
将车辆抽象地看做一个矩形范围，称为车辆区域。其中心与车辆中心重合，将车辆整体包括在矩形内。车辆区域的长为length，宽为width。lengthmin和widthmin对应于车长与车宽。参数length,width可依据环境因素和车速设置，以满足不同条件下所规划路径的安全性和稳定性需求。例如，在高速公路等较为开阔平直且车速高的环境中，可取较长的length和width提高路径搜索效率；而在障碍物情况复杂且车速较低的十字路口或车库等环境中，可取较小的length和width以求规划出更精细复杂的路径。
如图4所示为本发明碰撞检查算法原理。碰撞检查依据环境模型中定义的车辆区域和碰撞检查算法，判断某一车辆状态是否与环境中障碍物发生碰撞。若有障碍物栅格出现在车辆区域内，则判定为碰撞。一次碰撞检查过程分为如下两步：
(1)确定扫描栅格范围：为减小一次碰撞检查过程中扫描的栅格范围，取车辆区域的四个顶点abcd，利用他们勾勒出一个较小的扫描范围，扫描范围为点a和点d确定的矩形范围abcd内的所有栅格，即中心坐标在集合
{(x,y)|xa≤x≤xd,ya≤y≤yd}
中的所有栅格。其中有
(2)判断障碍栅格是否在车辆区域内：对于扫描范围内的栅格，首先判断其是否为障碍物栅格，若不是障碍物栅格，则继续扫描下一个栅格；若为障碍物栅格，则判断障碍栅格是否在车辆区域内。以图中障碍物栅格o为例，该栅格坐标为(xo,yo)。若以下不等式组
成立，则代表栅格在车辆区域内，判定该车辆区域与障碍物发生碰撞。
如图5所示为本发明初步路径搜索流程图。初步路径搜索过程是路径树的扩展过程，也是车辆状态扩展过程，依据车辆状态扩展模型描述的车辆状态扩展方程和控制空间采样策略，对任意车辆状态扩展，得一簇新的状态，并对车辆状态进行评价，作为路径搜索的依据。
车辆状态扩展：已知车辆参数ωmax为车辆前轮最大转向角速度，车辆前后轴距离为l，δmax代表前轮最大转向角的绝对值，车辆当前线速度为u。定义车辆状态为
其中
在确定控制输入采样策略时，考虑到车辆转向机构存在前轮最大转向角速度ωmax限制和前轮最大转向角δmax的限制，得到当前前轮转角为δnow时，控制输入的采样区间为
δi＝[min(-δmax,δnow-ωmaxt),max(δmax,δnow+ωmaxt)]
δi为扩展到下一状态的前轮转角输入，为该状态扩展方程唯一输入。其中t为状态扩展的时间步长。假设此时车速为u,当前状态下车辆的前轮转角为δnow，则可得状态扩展的时间步长t＝δs/u。在该区间内均匀采样得到离散的控制输入以扩展当前状态。
车辆状态评价：基于当前状态、环境障碍物信息和本次任务的目标状态对车辆状态的状态评价，状态节点评价值vq的定义如下：
vq＝r(wdistvdist+wheurvheur)
其中vdist表示从起始节点到节点q所经过的路程，wdist为权值。有：
vdist(q)＝vdist(p)+δs
vheur表示状态节q的启发值，为从q节点到qgoal的二维欧式距离,wheur为权值。有：
r为目标点威胁惩罚因子，其计算公式如下
其中n为当前节点q的所有兄弟结点数目，ncollison为当前节点q的所有兄弟节点中，与障碍物发生了碰撞的节点的数目。
计算本次节点扩展过程所产生的新节点的评价值均值
基于上述车辆状态扩展及评价方法，初步路径搜索的步骤包括路径选择，节点扩展，节点更新三个子过程，三个子过程循环执行，直到状态树扩展到目标点，得到以离散状态序列形式的初步路径。
(1)路径选择：
路径选择过程中，从根节点出发，逐层地选择节点组成一条新的搜索路径。因此，路径选择过程的关键在于节点选择方法。采用uct方法选择路径，标准uct方法的节点选择公式为：
其中，k为选择的状态节点，q代表为候选状态节点，q为候选节点集合，vq为节点q的评价值，c为常数，通常依据经验选择为0.8。np为节点q的父节点p的访问次数，nq为节点q的访问次数。
(2)节点扩展：
当路径搜索到当前状态树的某一叶节点时，需要进行节点扩展。在该叶节点的状态基础上，通过对当前控制输入空间的均匀采样，得n个控制输入δi(i＝1,2…n)，以此扩展出n个新状态节点qi(i＝1,2…n)。若新状态节点中存在到达目标点的节点，代表已经搜索到从起点到目标点的初步路径，则退出整个路径搜索循环。否则，对每个新节点作碰撞检查，从状态树上删除其中有碰撞的状态节点ncollison个，保留无碰撞节点n-ncollison个。计算节点的评价值vi(i＝1,2…n)，其中碰撞节点的评价值记为0。
(3)节点更新：
节点更新过程是一个信息的反向传播过程，利用节点扩展过程对环境的探索结果，对状态树相关节点的信息进行更新，保证后续的路径搜索过程能利用到之前搜索过程的信息。
状态节点q的信息包括节点评价值vq和节点访问次数nq两部分。对于搜索路径上经过的所有节点，更新其评价值为v′q:
更新其节点访问次数为：
n′q＝nq+1
图6所示，即为本专利所描述的方法进行路径规划的一次实例。其中空心点和黑色实心点均表示当前车辆状态树上的状态。黑色实心点序列即为初步路径，连接初步路径的黑色粗曲线即为得到的最终路径，其符合车辆运动学约束，易于被执行机构控制车辆跟踪。
上文给出的实例描述是为了便于该领域内专家学者和技术人员理解和应用本发明。该领域内的专家学者与技术人员显然可以容易地基于本专利的实例描述，加以简单修改后将本专利说明的一般原理应用到相关实例中，而不必经过创造性的劳动。因此，本领域技术人员依据本发明所述一般原理，对本发明做出的修改和改进都在本专利保护范围之内。
以上这些实施例应理解为仅用于说明本发明而不用于限制本发明的保护范围。在阅读了本发明的记载的内容之后，技术人员可以对本发明作各种改动或修改，这些等效变化和修饰同样落入本发明权利要求所限定的范围。
- 还没有人留言评论。精彩留言会获得点赞！