本发明属于自主泊车路径规划技术领域，特别涉及一种基于混合a*算法的两段式自主泊车路径规划方法。
背景技术：
随着汽车技术的发展与人们生活水平的提高，汽车出行已经成为人们生活必须，与之伴随的是公路与停车场显得越发拥挤。同时人们对汽车出行质量的要求也越来越高，泊车的实现一直以来都是司机群体头疼的一件事。近年来对自主泊车系统的研究逐渐开展，如何规划从进入停车场到实现具体停车的路径是一件值得着重考虑的事情。自主泊车路径规划本身包括从进入停车场或小区到泊车点附近的正向行驶过程与从泊车点附近实现泊车的倒车入库过程。并且由于两段路径对汽车的要求不同，应该对两段路径采用不同的节点扩展方式。但是现有的研究一般只研究了其中一段，并没有将两段路径放在同一个问题上考虑。所以两段式自主泊车路径规划的研究就显得有必要了。
技术实现要素：
本发明的目的是克服现有技术的缺陷，提供了一种基于混合a*算法的两段式自主泊车路径规划方法，根据车辆所需要的最小泊车距离将泊车路径划分为两部分，并且结合两部分路径的特点对两部分路径分别采用不同的启发函数进行路径规划，能够使路径搜索过程以最小的迭代次数得到最优路径，从而提高路径规划的效率，实现简单高效的停车。
本发明提供的技术方案为：
一种基于混合a*算法的两段式自主泊车路径规划方法，包括：
将泊车路径分为第一段和第二段；所述第一段为从车辆进入停车场到车辆行驶到最小泊车距离点的路径，所述第二段为车辆从所述最小泊车距离点行驶到泊车终止点的路径；
其中，当车辆与所述泊车终止点之间的距离为最小泊车距离时，判断所述车辆到达所述最小泊车距离点；
通过混合a*算法，采用第一启发函数对所述第一段进行路径规划；
其中，所述第一启发式函数为：
cost(ni，ni+1)＝dis(ni，ni+1)*(1+a*backcost+b*turncost)；
通过混合a*算法，采用第二启发函数对所述第二段进行路径规划；
其中，所述第二启发函数为：
cost(ni,ni+1)＝dis(ni,ni+1)*{(1+a*backcost+b*turncost+cost[theta(ni),theta(ni+1)]}；
式中，cost(ni,ni+1)表示从当前节点ni扩展到下一节点ni+1的所需的代价；dis(ni,ni+1)表示当前节点ni扩展到下一节点ni+1的距离大小，backcost表示从当前节点ni扩展到下一节点ni+1的倒车成本与转向成本；turncost表示从当前节点ni扩展到下一节点ni+1的转向成本theta(ni)表示车辆在当前节点ni的航向角；theta(ni+1)表示车辆在当下一节点ni+1的航向角；cost[theta(ni),theta(ni+1)]表示表示从当前节点航向角theta(ni)扩展到下一节点航向角theta(ni+1)所需的代价；a、b分别表示权重系数。
优选的是，垂直泊车时，所述最小泊车距离的计算方法为：
s＝sinθ*rmin+l；
其中，rmin表示车辆不与停车位发生碰撞的最小转弯半径；θ表示车辆以不与停车位发生碰撞的最小转弯半径rmin驶出停车位时的前轮转角；l表示车身长度。
优选的是，平行泊车时，所述最小泊车距离的计算方法为：
s＝sin(θ1+θ2)*rmin+l+l2；
其中，rmin表示车辆不与停车位发生碰撞的最小转弯半径；θ1表示车辆离开停车位时与原方向偏离的转角，θ2表示车辆从停车位驶出恢复车身姿态的转角；l表示车身长度，l2表示车辆的前轮中心与车身中心的距离。
优选的是，在进行泊车路径规划之前还包括：将环境建模地图进行栅格化离散。
优选的是，所述的基于混合a*算法的两段式自主泊车路径规划方法，还包括：在进行路径规划时，在每次进行节点扩展时，判断在下一节点车辆是否与环境障碍物发生碰撞，如果判断车辆与环境障碍物发生碰撞，则舍弃本次扩展，重新进行节点扩展规划；
其中，如果前位姿为：[x0,y0,theta0]，下一节点的位姿为：[x,y,theta]；则
x＝x0+d*cos(theta0)
y＝y0+d*sin(theta0)；
theta＝theta0+d/l*tan(delta)
式中，g表示环境建模地图中的栅格的分辨率，d表示每次节点扩展的位姿变化的分辨率，delta表示每次节点扩展时的转向角度变化值。
优选的是，在所述第一启发函数中，backcost的取值范围为5～10，turncost的取值范围为5～10。
优选的是，在所述第二启发函数中，backcost＝1，turncost＝1。
本发明的有益效果是：
本发明提供的基于混合a*算法的两段式自主泊车路径规划方法，根据车辆所需要的最小泊车距离将泊车路径划分为两部分，并且结合两部分路径的特点对两部分路径分别采用不同的启发函数进行路径规划，能够使路径搜索过程以最小的迭代次数得到最优路径，从而提高路径规划的效率，实现简单高效的停车。
附图说明
图1为本发明所述的基于混合a*算法的两段式自主泊车路径规划方法的流程图。
图2a-2b为本发明所述的混合a*算法reeds-shepp节点扩展示意图。
图3为本发明所述的垂直泊车时最小泊车距离的计算过程示意图。
图4为本发明所述的垂直泊车时平行泊车距离的计算过程示意图。
图5为本发明所述的启发函数一的设计依据示意图。
图6为本发明所述的启发函数二的设计依据示意图。
具体实施方式
下面结合附图对本发明做进一步的详细说明，以令本领域技术人员参照说明书文字能够据以实施。
如图1所示，本发明提供了一种基于混合a*算法的两段式自主泊车路径规划方法，包括如下步骤：
一、环境建模地图离散化
(1)混合a*算法主要适用于在环境地图已知的情况进行全局路径规划。由于混合a*算法的节点扩展方式是基于栅格地图进行，所以首先需要将已知的环境地图进行离散化。在进行栅格离散化时，主要需要考虑栅格的分辨率g、每次节点扩展的位姿变化的分辨率d以及在车辆动力学要求最大的转弯弧度delta。
如图2所示，每次节点扩展时，若当前位姿为[x,y,theta]，下一节点的位姿计算公式如下：
x＝x+d*cos(theta)；
y＝y+d*sin(theta)；
theta＝theta+d/l*tan(delta)；
式中，g表示环境建模地图中的栅格的分辨率，d表示每次节点扩展的位姿变化的分辨率。
在进行混合a*搜索之前，需要先基于混合a*算法搜索的最短路径作为混合a*算法的启发值，两个栅格节点扩展的最大次数n＝g/d。
二、泊车路径分段设计
1、最小泊车距离的计算
根据车辆本身尺寸、停车位信息、最小转弯半径以及泊车方式计算所需的最小泊车距离，主要考虑垂直泊车与平行泊车。
本发明给出最小泊车距离的定义：车辆以不与停车位发生碰撞最小转弯半径rmin从停车位行驶出来到车身平行于车的前进方向的距离。对于最小泊车距离s的求解，主要是基于停车位信息、最小转弯半径r与车身长度l求出最小泊车距离s。首先可以根据车位信息，最小转弯半径r求解出不与停车位发生碰撞的最小转弯半径rmin。其中，最小转弯半径r由车的出厂单位提供。同时求解最小泊车距离s，必须已知在车辆以不与停车位发生碰撞的最小转弯半径rmin驶出停车位时的前轮转角θ与车身长度l，从而求出最小泊车距离s所包含的转弯行驶距离与直线行驶距离。为了更加清晰地表达最小泊车距离的求解，本发明提供了垂直泊车与水平泊车的最小泊车距离的具体求解方法。
(1)垂直泊车：如图3所示，最小泊车距离应该满足，在车身不与停车位发生碰撞的情况下，以最大方向盘转角从停车位驶出并恢复正常向前行驶姿态所需要的距离。并同时满足最小转弯半径与无碰撞约束。以停车位中心与车头为参考点，车辆以不与停车位发生碰撞的最小转弯半径rmin驶出停车位时的前轮转角的大小为θ，此时通过车身尺寸信息可以得到最小泊车距离s。s为后轮中心到车头的长度l1与rmin*sinθ的和。但是为了考虑路径规划算法的误差与确保具体泊车的实现，需给定最小泊车距离s一个相对于理论值偏大的值。基于上述考虑，并且简化计算，选择s＝sinθ*rmin+l作为最小泊车距离的计算公式。最小泊车距离的理论计算值如图3所示。
(2)平行泊车：如图4所示，平行泊车时最小泊车距离应该满足：在车身不与停车位发生碰撞的情况下，以最大方向盘转角从停车位驶出并恢复正常向前行驶姿态所需要的距离；以及同时满足最小转弯半径与无碰撞约束。对于平行泊车，在停车位与驶出停车位后车的方向相同，那么此时驶出的车轮转角包括：车辆离开停车位时与原方向偏离的转角θ1，以及恢复车身姿态的转角为θ2。以车身前后对称平面为参考面，车辆以不与停车位发生碰撞的最小转弯半径rmin驶出停车位时的累计前轮转角的大小为θ1+θ2，此时通过车身尺寸信息可以得到最小泊车距离s。s为后轮中心到车头的长度l1、前轮中心与车身中心的距离l2与rmin*sin(θ1+θ2)的和。同样为了考虑路径规划算法的误差与确保具体泊车的实现，需给定最小泊车距离s一个相对于理论值偏大的值。基于上述考虑，并且简化计算，可以选择s＝sin(θ1+θ2)*rmin+l+l2。最小泊车距离的理论计算值如图4所示。
2、两段启发函数的设计
根据车辆所需要的最小泊车距离将泊车路径划分为两部分，即从进入停车场或小区到最小泊车距离点(与泊车终止点之间的距离为最小泊车距离时车辆质心所处的位置)的正向行驶过程，以及从最小泊车距离点实现泊车的倒车入库过程。
根据两段路径的车辆的行驶特征，基于混合a*算法设计不同的启发式函数。在正向行驶过程，车辆总是朝着目标点行驶，则主要考虑其避障，并且应该尽量有较少的转向与倒车行为。所以启发式函数应该在转向代价与倒车代价给一个较大的权重。而在倒车行驶过程，对车辆位姿要求很高，所以启发函数在最小转弯半径给一个较大的权重，并且对于转向与倒车代价给与一个较小的权重，从而在路径搜索过程以最小的迭代次数得到最优路径。
在求解出最小泊车距离后，应该基于最小泊车距离s对泊车路径进行分段，并根据两段路径行驶的特征对两段路径规划应该设计不同的启发式函数。本发明中混合a*算法进行路径搜索时，扩展节点路径主要是通过reeds-shepp曲线进行，所以设计混合a*算法时主要是考虑节点扩展过程的倒车与转向行为。图2a-2b表示reeds-shepp曲线扩展节点的方式，主要涉及前后各3个节点扩展方向。
(1)从进入停车场到最小泊车距离点：该段路径主要是低速下的正向行驶过程，由于此时车辆距目标点距离较远，因此认为车辆总是沿着目标点行驶，所以不用过度在意车辆的位姿约束。同时在低速正向行驶时，进行转向与倒车行为将会大大增加到达目标点的时间或驾驶员的精力，因此规划的路径不应该有较多的转弯与倒车行为，所以在启发函数的约束中应该尽量对转向代价turncost与倒车代价backcost给与较大的值，从而减少搜索的迭代次数和转向行为与倒车行为，并保证路径的相对较优。若使用cost函数来表示从当前节点ni扩展到下一节点ni+1时预估的代价，cost函数形式如下：
cost(ni，ni+1)＝dis(ni，ni+1)*(1+a*backcost+b*turncost)；
上述公式中，backcost与turncost分别是在当前节点ni扩展到下一节点ni+1倒车成本与转向成本，本发明给backcost与turncost的取值范围均为5～10；从而用来减少倒车与转向行为。a与b分别表示各项参数的权重，可以分别取为倒车路径与转向路径长度占整个路径长度的比率；对于相邻两个节点之间，可以直接取1。这主要是取决转弯路径与倒车路径占节点扩展路径的长度，对于相邻两个节点，一般主要是只有一段路径，所以直接取1。
图6用于简单的说明如何通过减少转向与倒车行为来规划该段路径，从而为该段路径的启发函数的设计提供依据。如图6所示，车辆以图示的位姿进入停车场，想要到达目标点g，如基于启发函数一进行路径规划，首选的扩展路径应该为路径2。因为刚开始对路径节点进行扩展时不需要转弯，并且整段路径只需要一次转向，而路径1需要两次转向。
(2)从最小泊车距离点到实现泊车：该段路径主要是通过转向与倒车从而以正确的车辆位姿实现泊车，并且在泊车过程不能与停车位碰撞。所以转向代价turncost与倒车代价backcost应该给与较小的值，甚至不额外增加转向代价与倒车代价。并且应该增加动力学的航向角与最小转弯半径的约束，从而保证车辆能够并且以较优的路径实现泊车。使用cost[theta(ni),theta(ni+1)]来表示从当前节点航向角theta(ni)扩展到下一节点航向角theta(ni+1)所需的代价，从而来实现对位姿的约，以保证最终的到达的终点与目标终点位姿的误差在阈值范围内。则此时若使用cost函数来表示从当前节点ni扩展到下一节点ni+1时预估的代价，启发函数二的cost函数形式如下：
cost(ni,ni+1)＝dis(ni,ni+1)*{(1+a*backcost+b*turncost+cost[theta(ni),theta(ni+1)]}；
并且判断是否可以在最小转弯半径下无碰撞实现该节点的扩展，如果不行，则放弃该节点的扩展。上述公式中，backcost与turncost分别是在当前节点ni扩展到下一节点ni+1倒车成本与转向成本，cost[theta(ni),theta(ni+1)]表示两个位姿的航向角变化的所需的代价。本发明给backcost与turncost分别赋值为1，表示不对倒车与转向行为进行额外限制；令cost[theta(ni),theta(ni+1)]取值为两个位姿弧度变化值的5倍到10倍。在此段路径节点扩展至与终点的位姿误差在一定范围内后结束，即通过混合a*算法实现整个路径搜索过程。a与b分别表示各项参数的权重，可以取为倒车路径与转向路径长度占整个路径长度的比率。对于相邻两个节点之间，可以直接取1。
图6为该段路径一个具体的规划结果，用于展示具体实现泊车时车辆的主要运动形式。由图6可知，此段主要由一次转弯行为与一次倒车行为组成。进一步说明了，分段的意义与有效性，从而为第二段路径规划的启发函数设计提供了依据。
三、路径的生成
混合a*算法的节点扩展方式主要是基于混合a*算法与rs曲线，其中图二左图即代表rs曲线六个节点扩展方向，即向前直行、左转、右转，向后直行、左转、右转，并且每次转向角度变化都是delta。在第一段路径节点扩展主要是基于启发函数一，从当前栅格进行节点扩展时，使用混合a*算法进行搜索，首先判断节点扩展次数n1是否小于n＝g/d，并且每扩展五个节点进行一次rs连接判断，即通过遍历48种rs曲线判断是否可以实现当前位姿到下一个栅格的平滑连接。当进行rs连接后，对于rs曲线进行离散化，即通过一个个离散的位姿实现rs曲线的扩展，每次节点扩展时，若当前位姿为[x,y,theta]，下一节点的位姿计算公式如下：
x＝x+d*cos(theta)；
y＝y+d*sin(theta)；
theta＝theta+d/l*tan(delta)；
在节点扩展同时必须进行路径的碰撞检测，即通过检测车辆模型是否与环境障碍物发生碰撞，若发生碰撞则舍弃该rs曲线，重新搜索，最后得到一条从进入停车场到最小泊车距离点的路径。在此特别说明，判断车辆是否到达泊车终止点，是通过判断车辆位姿[x,y,theta]与终点的位姿误差是否在阈值范围内，即当车辆的位姿为[x±δx,y±δy,theta±δtheta]时判断车辆到达泊车终止点。根据经验，可设定δx≤0.05米，δy≤0.5米，δtheta≤0.05弧度。同样，在最小泊车点到实现泊车的具体过程的路径规划主要是基于启发函数二进行，节点扩展方式与碰撞检测与第一段路径规划相似。
本发明主要是基于混合a*算法对泊车路径进行分段设计。首先解决了，泊车过程全局路径规划问题，相比于现有的研究只考虑在泊车位附近考虑路径规划更具有现实意义，同时也能节约泊车等待时间。其次，在路径规划的过程，基于混合a*算法的特点将泊车路径进行分段，从而在两段路径都可以尽量减少节点的扩展数量，从而有利于节省路径计算的时间。
尽管本发明的实施方案已公开如上，但其并不仅仅限于说明书和实施方式中所列运用，它完全可以被适用于各种适合本发明的领域，对于熟悉本领域的人员而言，可容易地实现另外的修改，因此在不背离权利要求及等同范围所限定的一般概念下，本发明并不限于特定的细节和这里示出与描述的图例。
1.一种基于混合a*算法的两段式自主泊车路径规划方法，其特征在于，包括：
将泊车路径分为第一段和第二段；所述第一段为从车辆进入停车场到车辆行驶到最小泊车距离点的路径，所述第二段为车辆从所述最小泊车距离点行驶到泊车终止点的路径；
其中，当车辆与所述泊车终止点之间的距离为最小泊车距离时，判断所述车辆到达所述最小泊车距离点；
通过混合a*算法，采用第一启发函数对所述第一段进行路径规划；
其中，所述第一启发式函数为：
cost(ni，ni+1)＝dis(ni，ni+1)*(1+a*backcost+b*turncost)；
通过混合a*算法，采用第二启发函数对所述第二段进行路径规划；
其中，所述第二启发函数为：
cost(ni,ni+1)＝dis(ni,ni+1)*{(1+a*backcost+b*turncost+cost[theta(ni),theta(ni+1)]}；
式中，cost(ni,ni+1)表示从当前节点ni扩展到下一节点ni+1的所需的代价；dis(ni,ni+1)表示当前节点ni扩展到下一节点ni+1的距离大小，backcost表示从当前节点ni扩展到下一节点ni+1的倒车成本与转向成本；turncost表示从当前节点ni扩展到下一节点ni+1的转向成本theta(ni)表示车辆在当前节点ni的航向角；theta(ni+1)表示车辆在当下一节点ni+1的航向角；cost[theta(ni),theta(ni+1)]表示表示从当前节点航向角theta(ni)扩展到下一节点航向角theta(ni+1)所需的代价；a、b分别表示权重系数。
2.根据权利要求1所述的基于混合a*算法的两段式自主泊车路径规划方法，其特征在于，垂直泊车时，所述最小泊车距离的计算方法为：
s＝sinθ*rmin+l；
其中，rmin表示车辆不与停车位发生碰撞的最小转弯半径；θ表示车辆以不与停车位发生碰撞的最小转弯半径rmin驶出停车位时的前轮转角；l表示车身长度。
3.根据权利要求2所述的基于混合a*算法的两段式自主泊车路径规划方法，其特征在于，平行泊车时，所述最小泊车距离的计算方法为：
s＝sin(θ1+θ2)*rmin+l+l2；
其中，rmin表示车辆不与停车位发生碰撞的最小转弯半径；θ1表示车辆离开停车位时与原方向偏离的转角，θ2表示车辆从停车位驶出恢复车身姿态的转角；l表示车身长度，l2表示车辆的前轮中心与车身中心的距离。
4.根据权利要求2或3所述的基于混合a*算法的两段式自主泊车路径规划方法，其特征在于，在进行泊车路径规划之前还包括：将环境建模地图进行栅格化离散。
5.根据权利要求4所述的基于混合a*算法的两段式自主泊车路径规划方法，其特征在于，还包括：在进行路径规划时，在每次进行节点扩展时，判断在下一节点车辆是否与环境障碍物发生碰撞，如果判断车辆与环境障碍物发生碰撞，则舍弃本次扩展，重新进行节点扩展规划；
其中，如果前位姿为：[x0,y0,theta0]，下一节点的位姿为：[x,y,theta]；则
式中，g表示环境建模地图中的栅格的分辨率，d表示每次节点扩展的位姿变化的分辨率，delta表示每次节点扩展时的转向角度变化值。
6.根据权利要求5所述的基于混合a*算法的两段式自主泊车路径规划方法，其特征在于，在所述第一启发函数中，backcost的取值范围为5～10，turncost的取值范围为5～10。
7.根据权利要求6所述的基于混合a*算法的两段式自主泊车路径规划方法，其特征在于，在所述第二启发函数中，backcost＝1，turncost＝1。
技术总结