一种基于动态轨迹规划的自动驾驶连续避障方法

专利名称：一种基于动态轨迹规划的自动驾驶连续避障方法

专利类型：发明专利

专利申请号：CN202210588550.4

专利申请（专利权）人：电子科技大学
权利人地址：四川省成都市高新区(西区)西源大道2006号

专利发明（设计）人：李曙光,魏文博,赵洋,杜菁禹,罗钟林

专利摘要：本发明公开了一种基于动态轨迹规划的自动驾驶连续避障方法，属于自动驾驶领域。在典型多障碍物场景中，存在障碍物场景复杂导致智能车辆避障能力不足的问题。因此，本文提出基于改进A*算法的避障规划方法，通过利用曲线坐标系，充分考虑障碍物位置对连续避障轨迹产生的影响，实现对于多障碍物的连续安全避让。相比传统A*算法，本文的规划方法充分考虑障碍物信息，提高了连续避障轨迹的安全性。此外，本文通过结合动态轨迹规划方法与改进A*算法，实现了对于感知盲区障碍物的有效避让。在仿真实验场景中，本文的规划方法实现了对于多个障碍物的良好避让，证明了本文方法的合理性。

主权利要求：
1.一种基于动态轨迹规划的自动驾驶连续避障方法，该方法包括：步骤1：建立避障曲线坐标系，规定s方向为道路中心线方向，d方向为垂直于道路中心线的方向；
步骤2：根据障碍物的位置及障碍物之间的距离计算自身车辆前方如果通过自身车辆是会发生接触的碰撞区域；
步骤3：在自身车辆前方区域中，去除步骤2得到的碰撞区域和障碍物所占据区域，得到可行使区域，对可行驶区域内以纵向间隔Δs，横向间隔Δd对可行驶区域进行离散化处理，得到可行驶轨迹点；在可行驶轨迹点中采用下式规划自身车辆行使路径；
fi＝k1(gi+hi)+k2(di+ci)+k3bi式中，fi是节点i的综合估计代价，gi是节点i距离起点的代价，hi是接待点i距离终点的预计代价；di为轨迹点在d方向上距离全局参考轨迹的偏移；bi为轨迹点与最近障碍物之间的距离代价，离障碍物越近，轨迹点代价应该越大；ci为累计纵向偏移代价，计算过程为上一轨迹点的累计偏移加上当前轨迹点距上一轨迹点的纵向偏移；k1、k2、k3为启发函数的调节因子，且k1+k2+k3＝1；gi、hi、ci与bi的定义分别为：gi＝gi‑1+Δcost
ci＝ci‑1+Δd
式中，δ表示比例系数，Δcost表示点i‑1到点i之间距离代价的变化情况，(send，dend)表示目标点的位置坐标，(sobstacle，dobstacle)表示障碍物的位置坐标；
以综合估计代价最小的点为轨迹父节点，然后依次向前搜索最小的轨迹子节点，得到一条规划轨迹；
步骤4：轨迹拟合；
步骤4.1：在步骤1建立避障曲线坐标系下，建立5阶贝塞尔曲线；
其中，fs(τ)和fd(τ)分别表示s方向与d方向表现了n阶贝塞尔曲线，参数τ的取值范围是[0,1]；
步骤4.2：设定n阶贝塞尔曲线的控制点为P0，P1，P2，……，Pn，其中，P0为步骤3得到规划轨迹的起始点(s0,d0)，Pn为步骤3得到规划轨迹的终止点(sn,dn)；根据起始点和终止点计算其余点的位置步骤4.3：根据步骤2得到的所有控制点的位置，拟合出n阶贝塞尔曲线；
步骤5：根据设定最大速度、最大加速度、最大曲率剔除不合适的轨迹。
2.如权利要求1所述的一种基于动态轨迹规划的自动驾驶连续避障方法，其特征在于，所述步骤2的方法为：(1)当两障碍物存在纵向距离但不能保证自身车辆安全通过时，两障碍物之间的碰撞区域计算方法为：l′1＝max(|s1‑s2+(l2‑l1)/2|,|s1‑s2‑(l1+l2)/2|,|s1‑s2+(l2+l1)/2|,|s1‑s2+(l1‑l2)/2|)w′1＝min(|d1‑d2+(w2‑w1)/2|,|d1‑d2‑(w1+w2)/2|,|d1‑d2+(w2+w1)/2|,|d1‑d2+(w1‑w2)/2|)s′1＝min(s1‑l1/2,s2‑l2/2)+l′1/2d′1＝min(d1‑w1/2,d2‑w2/2)+w′1/2+w2其中，l′1和w′1表示碰撞区域长度和宽度，s′1和d′1表示碰撞区域中心坐标，(s1,d1)表示障碍物1的中心坐标，(s2,d2)表示障碍物2的中心坐标，l1和w1表示障碍物1的长度和宽度，l2和w2表示障碍物2的长度和宽度；
(2)当两障碍物之间只存在横向间距且不能满足自身车辆通行要求时，两障碍物之间的碰撞区域计算方法为：l′2＝max(l1,l3)
w′2＝min(|d1‑d3+(w3‑w1)/2|,|d1‑d3‑(w1+w3)/2|,|d1‑d3+(w3+w1)/2|,|d1‑d3+(w1‑w3)/2|)s′2＝min(s1‑l1/2,s3‑l3/2)+l′2/2d′2＝min(d1‑w1/2,d3‑w3/2)+w′2/2+w1。
3.如权利要求1所述的一种基于动态轨迹规划的自动驾驶连续避障方法，其特征在于，步骤4.2中n等于5，计算方法为：步骤4.2：设定5阶贝塞尔曲线的6个控制点为P0，P1，P2，P3，P4，P5，其中，P0为步骤3得到规划轨迹的起始点(s0,d0)，P5为步骤3得到规划轨迹的终止点(s5,d5)；再采用下式计算其余点的位置；
步骤4.3：根据步骤4.2得到的6个点的位置，计算拟合轨迹；
5 4 3 2 2 3
fs(τ)＝s0(1‑τ) +(s5‑s0)(1‑τ) τ+4(s5‑s0)(1‑τ) τ+6(s5‑s0)(1‑τ) τ+4(s5‑s0)(1‑τ)
4 5
τ +s5τ
5 4 3 2 2 3 4 5
fd(τ)＝d0(1‑τ) +5d0(1‑τ) τ+10d0(1‑τ) τ +10d5(1‑τ) τ +5d5(1‑τ)τ +d5τ 。
4.如权利要求3所述的一种基于动态轨迹规划的自动驾驶连续避障方法，其特征在于，步骤5的具体方法为：步骤5.1：设定参数τ到时间t的映射关系为τ＝t/T，其中，T为车辆通过规划轨迹所要经过的总时间；建立贝塞尔曲线表达式与时间t的关系：其中，Psi和Pdi分别表示贝塞尔曲线在s方向与d方向的控制点坐标；
步骤5.2：采用如下公式计算速度v(t)；
计算加速度：
as(t)和ad(t)表示s方向上和d方向上的加速度；
计算曲率K(t)；
将如果步骤4拟合的轨迹中存在大于设定的最大速度、最大加速度、最大曲率的点，则剔除该拟合轨迹。
5.如权利要求4所述的一种基于动态轨迹规划的自动驾驶连续避障方法，其特征在于，所述步骤5中再计算轨迹点在障碍物长度方向和宽度方向上的偏移值Δl和Δw：(si，di)表示轨迹点，α表示障碍物当前与s轴的夹角，将(si,di)与(so,do)连线，该连线与s轴的夹角为β，发生碰撞的条件设为:式中，rsafe为自动驾驶车辆的安全距离，lnew和wnew表示通过障碍物的长度和宽度；
根据该碰撞条件在剩下的轨迹中进一步剔除不合适的轨迹。 说明书 : 一种基于动态轨迹规划的自动驾驶连续避障方法技术领域[0001] 本发明属于自动驾驶领域，特别是基于A*算法的自动驾驶避障方法。背景技术[0002] 在自动驾驶技术的典型应用场景中，对于障碍物的避障规划能力是自动驾驶技术的重要组成部分。自动驾驶的规划方法主要包括基于图搜索的规划方法与基于轨迹生成的规划方法，基于图搜索的方法由于能够较为充分的考虑全局地图信息，因此在全局轨迹规划方面具有重要的应用，主要包括深度优先搜索、广度优先搜索、Dijkstra算法、A*算法等。在可行路径存在的条件下，基于图搜索的方法可找到可行解。基于轨迹生成的规划方法则可以应对局部交通场景，在智能车辆避障方面可以有良好的效果。[0003] 在基于图搜索方法的轨迹规划方法中，主要通过传统的广度优先、A*等方法实现对于可行驶轨迹的规划。在典型障碍物场景中，研究人员针对不同的障碍物场景对该类方法进行调整升级，从而适应不同的障碍物对自动驾驶规划带来的挑战。基于图搜索的方法由于可以考虑整体的路径与障碍物信息，可以在较为复杂的障碍物场景中实现对于全局最优轨迹的搜索，能够应对不同的障碍物对于车辆的行驶带来的挑战。[0004] 在基于轨迹生成的规划方法中，基于多项式生成轨迹的方法在实车场景中具有广泛的应用。基于多项式曲线生成规划轨迹的方法的特点在于可以根据车辆的行驶情况对轨迹进行解耦规划，分别进行车辆轨迹规划与车辆速度规划，根据车辆在障碍物场景下的实际需求规划车辆在当前交通场景中的速度与轨迹信息。基于多项式曲线的规划方法具有数学逻辑严谨、轨迹较为连续、实车应用性强等特点，在面向障碍物的场景中对于车辆的避障能力的提升具有积极的作用。由于该方法并不依赖神经网络模型，规划结果的可解释性较好，因此在实车平台的应用也十分充分，能够保障车辆在较低期望速度下的避障能力，实现自动驾驶规划水平的提升。[0005] 以上方法在避障轨迹规划过程中虽然能够在一定程度上考虑障碍物信息与车辆避障效率，但是却无法满足车辆在较为复杂的障碍物场景中的避障安全性。基于图搜索的方法依赖全局路径信息与障碍物位置，而基于轨迹生成的方法容易陷入局部最优解从而导致连续避障方法的失败。在实车场景中，如果仅依赖智能车辆的自车感知系统，可能会造成车辆对于感知盲区障碍物的无法及时处理从而使得车辆连续避障失败的情况出现。目前虽然有研究人员针对多障碍物场景进行规划方法研究，但是未能考虑基于实车感系统的局限性，无法对感知盲区的障碍物进行有效处理。针对以上问题，本发明提出一种结合动态轨迹规划与改进A*算法的避障轨迹规划方法，该方法主要面向较为复杂的多障碍物场景，在基于图搜索方法的基础上，结合基于贝塞尔曲线生成轨迹的动态规划方法，充分考虑障碍物信息，提高连续避障轨迹的安全性，实现对于多障碍物场景下安全避障轨迹的规划目的。发明内容[0006] 在应对较为复杂的障碍物场景时，传统的规划方法存在场景适应性差，避障效率不高等问题。本发明改进设计了一种结合动态轨迹规划与A*算法的自动驾驶连续避障方法。[0007] 本发明技术方案为一种基于动态轨迹规划的自动驾驶连续避障方法，该方法包括：[0008] 步骤1：建立避障曲线坐标系，规定s方向为道路中心线方向，d方向为垂直于道路中心线的方向；[0009] 步骤2：根据障碍物的位置及障碍物之间的距离计算自身车辆前方如果通过自身车辆是会发生接触的碰撞区域；[0010] 步骤3：在自身车辆前方区域中，去除步骤2得到的碰撞区域和障碍物所占据区域，得到可行使区域，对可行驶区域内以纵向间隔Δs，横向间隔Δd对可行驶区域进行离散化处理，得到可行驶轨迹点；在可行驶轨迹点中采用下式规划自身车辆行使路径；[0011] fi＝k1(gi+hi)+k2(di+ci)+k3bi[0012] 式中，fi是节点i的综合估计代价，gi是节点i距离起点的代价，hi是接待点i距离终点的预计代价；di为轨迹点在d方向上距离全局参考轨迹的偏移；bi为轨迹点与最近障碍物之间的距离代价，离障碍物越近，轨迹点代价应该越大；ci为累计纵向偏移代价，计算过程为上一轨迹点的累计偏移加上当前轨迹点距上一轨迹点的纵向偏移；k1、k2、k3为启发函数的调节因子，且k1+k2+k3＝1；gi、hi、ci与bi的定义分别为：[0013] gi＝gi‑1+Δcost[0014][0015] ci＝ci‑1+Δd[0016][0017] 式中，δ表示比例系数，Δcost表示点i‑1到点i之间距离代价的变化情况，(send，dend)表示目标点的位置坐标，(sobstacle，dobstacle)表示障碍物的位置坐标；[0018] 以综合估计代价最小的点为轨迹父节点，然后依次向前搜索最小的轨迹子节点，得到一条规划轨迹；[0019] 步骤4：轨迹拟合；[0020] 步骤4.1：在步骤1建立避障曲线坐标系下，建立5阶贝塞尔曲线；[0021][0022][0023] 其中，fs(τ)和fd(τ)分别表示s方向与d方向表现了n阶贝塞尔曲线，参数τ的取值范围是[0,1]；[0024] 步骤4.2：设定n阶贝塞尔曲线的控制点为P0，P1，P2，……，Pn，其中，P0为步骤3得到规划轨迹的起始点(s0,d0)，Pn为步骤3得到规划轨迹的终止点(sn,dn)；根据起始点和终止点计算其余点的位置[0025] 步骤4.3：根据步骤2得到的所有控制点的位置，拟合出n阶贝塞尔曲线；[0026] 步骤5：根据设定最大速度、最大加速度、最大曲率剔除不合适的轨迹。[0027] 进一步的，所述步骤2的方法为：[0028] (1)当两障碍物存在纵向距离但不能保证自身车辆安全通过时，两障碍物之间的碰撞区域计算方法为：[0029] l′1＝max(|s1‑s2+(l2‑l1)/2|,|s1‑s2‑(l1+l2)/2|,|s1‑s2+(l2+l1)/2|,|s1‑s2+(l1‑l2)/2|)[0030] w′1＝min(|d1‑d2+(w2‑w1)/2|,|d1‑d2‑(w1+w2)/2|,|d1‑d2+(w2+w1)/2|,|d1‑d2+(w1‑w2)/2|)[0031] s′1＝min(s1‑l1/2,s2‑l2/2)+l′1/2[0032] d′1＝min(d1‑w1/2,d2‑w2/2)+w′1/2+w2[0033] 其中，l′1和w′1表示碰撞区域长度和宽度，s′1和d′1表示碰撞区域中心坐标，(s1,d1)表示障碍物1的中心坐标，(s2,d2)表示障碍物2的中心坐标，l1和w1表示障碍物1的长度和宽度，l2和w2表示障碍物2的长度和宽度；[0034] (2)当两障碍物之间只存在横向间距且不能满足自身车辆通行要求时，两障碍物之间的碰撞区域计算方法为：[0035] l′2＝max(l1,l3)[0036] w′2＝min(|d1‑d3+(w3‑w1)/2|,|d1‑d3‑(w1+w3)/2|,|d1‑d3+(w3+w1)/2|,|d1‑d3+(w1‑w3)/2|)[0037] s′2＝min(s1‑l1/2,s3‑l3/2)+l′2/2[0038] d′2＝min(d1‑w1/2,d3‑w3/2)+w′2/2+w1。[0039] 进一步的，步骤4.2中n等于5，计算方法为：[0040] 步骤4.2：设定5阶贝塞尔曲线的6个控制点为P0，P1，P2，P3，P4，P5，其中，P0为步骤3得到规划轨迹的起始点(s0,d0)，P5为步骤3得到规划轨迹的终止点(s5,d5)；再采用下式计算其余点的位置；[0041][0042][0043][0044][0045] 步骤4.3：根据步骤4.2得到的6个点的位置，计算拟合轨迹；[0046] fs(τ)＝s0(1‑τ)5+(s5‑s0)(1‑τ)4τ+4(s5‑s0)(1‑τ)3τ2+6(s5‑s0)(1‑τ)2τ3+4(s5‑s0)4 5(1‑τ)τ +s5τ[0047] fd(τ)＝d0(1‑τ)5+5d0(1‑τ)4τ+10d0(1‑τ)3τ2+10d5(1‑τ)2τ3+5d5(1‑τ)τ4+d5τ5。[0048] 进一步的，步骤5的具体方法为：[0049] 步骤5.1：设定参数τ到时间t的映射关系为τ＝t/T，其中，T为车辆通过规划轨迹所要经过的总时间；建立贝塞尔曲线表达式与时间t的关系：[0050][0051][0052] 其中，Psi和Pdi分别表示贝塞尔曲线在s方向与d方向的控制点坐标；[0053] 步骤5.2：采用如下公式计算速度v(t)；[0054][0055] 计算加速度：[0056][0057][0058] as(t)和ad(t)表示s方向上和d方向上的加速度；[0059] 计算曲率K(t)；[0060][0061] 将如果步骤4拟合的轨迹中存在大于设定的最大速度、最大加速度、最大曲率的点，则剔除该拟合轨迹。[0062] 进一步的，所述步骤5中再计算轨迹点在障碍物长度方向和宽度方向上的偏移值Δl和Δw：[0063][0064][0065] (si，di)表示轨迹点，α表示障碍物当前与s轴的夹角，将(si,di)与(so,do)连线，该连线与s轴的夹角为β，发生碰撞的条件设为:[0066][0067] 式中，rsafe为自动驾驶车辆的安全距离，lnew和wnew表示通过障碍物的长度和宽度；[0068] 根据该碰撞条件在剩下的轨迹中进一步剔除不合适的轨迹。[0069] 本发明通过设计评估函数来评价规划轨迹的安全性，按照轨迹函数的计算结果对轨迹进行排序，可以为自动驾驶车辆提供一条最佳的避障轨迹。本发明的评估函数在设计过程中考虑了规划轨迹对于驾驶效率、安全性、人体舒适度等方面的影响，从每条轨迹中提取出n个点，用离散的形式来表现评估函数Ctotal，实现对于轨迹评价的目标。在保证安全性的前提下，本发明希望能兼顾效率与乘坐舒适性的轨迹来实现避障的目的。在评估函数的设计中，本发明需要规划轨迹在d方向上的偏移更少，这样可以降低自动驾驶车辆的横向位移时间，于是在函数设计中考虑规划轨迹相对于参考轨迹在d方向上的偏移dcenter；对于规划轨迹，本发明希望在曲率均小于最大可行驶曲率Kmax的情况下整体的曲率可以尽可能小，这样可以降低车辆对于轨迹追踪的难度，提高安全性；对于车内人员的乘坐舒适度而言，本发明可以通过考虑车辆的横向加速度ad来实现对于乘车舒适性的评价，有助于车辆驾驶过程中稳定性的提升；此外，从行车效率的角度出发，本发明更倾向于长度更短的避障路径，可以有效提升车辆的行驶效率，这也是本发明将lp与lorg纳入考虑因素的主要原因。[0070] 根据本发明所描述的方法，通过基于改进A*算法可搜索出一条连续避障轨迹，但是该轨迹的生成基于车辆在避障轨迹规划之前对于障碍物的感知定位结果，若在初始感知时存在障碍物被遮挡的情况，车辆规划出的轨迹可能会经过被遮挡的障碍物或者距离被遮挡的障碍物过近，这会导致危险情况的发生。[0071] 通过结合动态轨迹规划的方法，能够更加灵活高效的处理含有多个障碍物的交通场景，提高车辆的避障成功率，增强车辆的安全性保障，实现对于较为复杂的障碍物场景的处理，对于上述的交通场景，经过动态轨迹规划之后，车辆实际的避障轨迹如图7所示。[0072] 在图7中，虚线为动态轨迹规划方法生成的避障轨迹，结合动态轨迹规划的连续避障轨迹生成方法实现了将改进A*算法与动态轨迹规划结合的目的，能够灵活且高效的实现车辆在存在多个障碍物的交通场景中的安全通行，解决了自动驾驶车辆在多个障碍物场景中难以规划出安全可行驶路径问题，提高了自动驾驶的智能性，为自动驾驶技术的进一步落地应用奠定了基础。附图说明[0073] 图1为碰撞区域展示示意图。[0074] 图2为可行驶区域离散化处理示意图。[0075] 图3为连续避障轨迹点选择对比示意图。[0076] 图4为连续避障轨迹拟合过程展示示意图。[0077] 图5为碰撞检测区域设置示意图。[0078] 图6为轨迹碰撞检测示意图。[0079] 图7为含有盲区障碍物的避障轨迹规划过程示意图。[0080] 图8为避障轨迹规划流程示意图。[0081] 图9为盲区障碍物对车辆连续避障的影响示意图。具体实施方式[0082] 基于改进A*算法的连续避障轨迹规划方法[0083] 基于贝塞尔曲线的动态轨迹规划方法在面对单个障碍物的场景中可以实现良好的避障功能，但在面向多个障碍物的场景中，可能会存在由于选择局部最优避障轨迹，导致车辆无法搜索出全局最优轨迹的情况。因此，本发明使用基于改进A*算法的连续避障轨迹规划方法来应对多障碍物场景对轨迹规划带来的挑战。为了简化计算，本发明的规划方法均在曲线坐标系下完成。在曲线坐标系中，规定s方向为道路中心线方向，d方向为垂直于道路中心线的方向。[0084] 碰撞区域设置[0085] 在多障碍物场景中，需要对不同障碍物设置安全区域，避免避障轨迹离障碍物太近导致的碰撞情况的发生，同时，由于不同障碍物之间互相影响，可能导致车辆在缺乏碰撞区域检测的情况下规划出无法真正保证安全通行的轨迹。本发明通过对障碍物建立矩形安全区域来实现对于障碍物安全裕度的建立。通过设置smin参数与dmin参数来表示能够保证自动驾驶车辆通行的最小纵向与横向距离，当障碍物之间的纵向距离小于smin或横向距离小于dmin时，则判定该区域为不可通行区域。本章在不可行区域基础上，设置碰撞区域，降低后续路径搜索的复杂度，提高规划轨迹的效率。本发明在不可行区域的基础上，基于障碍物的位置(s,d)与安全区域长度、宽度(l,w)信息，适当扩大不可行区域的覆盖面积从而实现碰撞区域的设置，这可以减少区域中的搜索面积，降低不必要的计算任务，实现提高搜索效率的目标。如图1所示。[0086] 图1中，障碍物在曲线坐标系中的位置(s,d)为障碍物中心点的位置，以碰撞区域1为例，该区域的长宽度具体信息的计算过程为：[0087] l′1＝max(|s1‑s2+(l2‑l1)/2|,|s1‑s2‑(l1+l2)/2|,|s1‑s2+(l2+l1)/2|,|s1‑s2+(l1‑l2)/2|)[0088] w′1＝min(|d1‑d2+(w2‑w1)/2|,|d1‑d2‑(w1+w2)/2|,|d1‑d2+(w2+w1)/2|,|d1‑d2+(w1‑w2)/2|)[0089] 根据碰撞区域的长宽信息，结合障碍物的位置信息，本发明可以得出碰撞区域中心点的位置：[0090] s′1＝min(s1‑l1/2,s2‑l2/2)+l′1/2[0091] d′1＝min(d1‑w1/2,d2‑w2/2)+w′1/2+w2[0092] 经过上述公式，本发明可以获得碰撞区域1的具体信息(s′1,d′1,l′1,w′1)，这种方法主要适用于两障碍物存在一定的纵向距离但不能保证车辆安全通过的场景，而对于图中碰撞区域2的位置确定，可以通过以下方法：[0093] l′2＝max(l1,l3)[0094] w′2＝min(|d1‑d3+(w3‑w1)/2|,|d1‑d3‑(w1+w3)/2|,|d1‑d3+(w3+w1)/2|,|d1‑d3+(w1‑w3)/2|)[0095] s′2＝min(s1‑l1/2,s3‑l3/2)+l′2/2[0096] d′2＝min(d1‑w1/2,d3‑w3/2)+w′2/2+w1[0097] 通过这种方法，本发明可以得到图中碰撞区域2的具体信息(s′2,d′2,l′2,w′2)，这在障碍物之间存在横向间距但是不能满足车辆通行要求的场景中可以减少通行区域的搜索，对于图1中碰撞区域3的处理方法类似。通过对于碰撞区域的设置，可以保证路径规划的稳定性，避免规划出不能满足自动驾驶车辆通行要求的轨迹。[0098] 基于改进A*的连续避障轨迹搜索[0099] 传统的A*算法作为启发式路径搜索算法，可以实现在均匀离散化的栅格图中搜索出一条最短行驶路径，相比于贪心算法和Dijkstra算法，A*算法具备更快的搜索速度和更优的搜索结果。但是，在多障碍物场景中，本发明希望能够在保证车辆行驶安全的情况下搜索出连续避障轨迹，而传统的A*算法由于没有考虑轨迹的纵向(d方向)偏移距离与障碍物信息，无法满足本发明对于连续避障轨迹需求。因此，通过使用改进A*算法来得到连续避障路径点，并通过五阶贝塞尔曲线来拟合出平滑且连续的最优避障轨迹。[0100] 通过上节的碰撞区域的设置，本发明可以实现对于多障碍物场景中可行驶区域的划分，通过对可行驶区域内以纵向间隔Δs，横向间隔Δd对可行驶区域进行离散化处理，得到可行驶轨迹点(Δs＝Δd＝0.5m)，如图2所示，图中离散点为可行驶轨迹点。[0101] 在避障连续避障过程中，本发明期望车辆的连续避障轨迹能够充分考虑轨迹点的纵向偏移距离与累计偏移程度，也能够利用轨迹点与障碍物的距离来避免选择离障碍物过近的点来产生危险的避障行为。传统的A*算法以距起始点的代价与距终止点的代价为评估指标，启发函数的具体表达形式为：[0102] fi＝gi+hi[0103] 式中，fi是节点i的综合估计代价，gi是节点i距离起点的代价，hi是接待点i距离终点的预计代价。[0104] A*算法更倾向于寻找到能够到达终点的最短路径，但是在连续避障过程中，最短路径并不一定为最优避障路径，本发明希望将安全性作为第一考虑目标，然后考虑车内人员的乘坐舒适性与行车效率。因此，本发明可以将A*算法进行改进，启发函数定义为：[0105] fi＝k1(gi+hi)+k2(di+ci)+k3bi[0106] 式中，gi、hi、ci与bi的定义分别为：[0107] gi＝gi‑1+Δcost[0108][0109] ci＝ci‑1+Δd(33)[0110][0111] 式中，gi与hi的含义与传统A*算法中相同，di为轨迹点在d方向上距离全局参考轨迹的偏移，本发明均用欧式距离表示；bi为轨迹点与最近障碍物之间的距离代价，离障碍物越近，轨迹点代价应该越大，δ表示比例系数；ci为累计纵向偏移代价，计算过程为上一轨迹点的累计偏移加上当前轨迹点距上一轨迹点的纵向偏移，k1、k2、k3为启发函数的调节因子，且k1+k2+k3＝1。[0112] 在本发明提出的基于改进A*算法的连续避障路径搜索中，将f值最小的可行轨迹点设为父节点，对周围离散轨迹点进行搜索，以代价最小的点作为子节点，如此循环搜索，直到搜索至目标点，完成一条可行连续避障轨迹的搜索任务。本发明将轨迹点的纵向偏移程度与轨迹点和障碍物的距离融入启发函数的计算过程，通过比例因子的调节，本发明可以在路径长度、纵向偏移与轨迹安全性方面做出侧重选择，规划出符合本发明要求的最优轨迹。通过图3，可以看到基于本发明的方法与传统A*算法在轨迹搜索方面的区别。[0113] 从图3来看，五角星点为传统A*算法选择的轨迹点，虽然满足了最短路径的要求，但是没有考虑车辆在行驶过程中对于安全性的要求。图中空心点为改进A*算法选取的轨迹点，虽然轨迹长度并不是最短，但是由于轨迹对于障碍物的避让效果较好，能够提高避障轨迹的安全性。但是，直接连接轨迹点并不能生成连续平滑的避障轨迹，因此需要通过轨迹拟合的方法来实现。[0114] 基于贝塞尔曲线的轨迹拟合[0115] 本发明基于贝塞尔曲线拟合的方式来生成避障轨迹。五阶贝塞尔曲线在保持轨迹的曲率连续性方面具有优势，因此，本发明采用五阶贝塞尔曲线来实现轨迹拟合。[0116] 在曲线坐标系中，可将五阶贝塞尔曲线的表达式定义与τ相关的函数：[0117] f(τ)＝(1‑τ)5P0+5(1‑τ)4τP1+10(1‑τ)3τ2P2+10(1‑τ)2τ3P3+5(1‑τ)τ4P4+τ5P5,τ∈[0,1][0118] 由于基于改进A*算法生成的连续避障轨迹为一条连续折线，在折点处会存在曲率突变的情况，贝塞尔曲线的作用就是对这些折线进行平滑处理，形成能够满足实车通行条件的平滑轨迹。在拟合过程中，由于连续避障轨迹中的直线部分并不需要额外拟合，因此可以通过设定长度阈值ls，筛选出满足车辆通行条件的直线，将剩余折线部分利用贝塞尔曲线进行拟合，其中最重要的是控制点的选取。[0119] 为了保证拟合曲线的曲率连续性，可以选取可通行直线的端点为拟合曲线的起始点，控制点的选取如图4中空心点所示，通过控制点进行轨迹生成，可得出图4中虚线所示的轨迹，这样不仅可以实现满足实车通行条件的避障轨迹，也可提升轨迹拟合效率。[0120] 根据曲率连续性原理，为了保证避障轨迹能够满足车辆行驶的要求，需要确保轨迹连接处曲率的连续性，这要求轨迹连接处的位置连续、斜率连续、曲率相等且主法线方向一致。曲率连续的几何意义为曲线段Q首端的二阶导矢应处在曲线段P末端的二阶导矢和一阶导矢所张成的平面内。根据上文的拟合过程描述，贝塞尔曲线的起点与终点处的斜率均为零，与直线的曲率一致，因此通过本发明提出的基于贝塞尔曲线的连续避障轨迹的拟合2方法，可以满足避障曲线的G连续性要求。[0121] 基于贝塞尔曲线的动态轨迹规划方法[0122] 基于五阶贝塞尔曲线的轨迹规划[0123] 传统的基于多项式曲线的路径规划方法存在规划过程复杂，避障效果不理想等问题。本发明在曲线坐标系中基于贝塞尔曲线，结合全局路径、车辆自身定位信息与障碍物信息，进行避障轨迹动态规划，实现对于车辆避障过程中的稳定性与安全性的要求。[0124] 本发明采用五阶贝塞尔曲线来生成避障轨迹。对于在起始点与终止点的曲率都为零的五阶贝塞尔曲线而言，需要分别将前三个控制点P0，P1，P2与后三个控制点P3，P4，P5放在两条直线上，因此，设自动驾驶车辆开始规划时在曲线坐标系中的坐标为P0＝(s0,d0)，规划终止点的坐标为P5＝(s5,d5)，根据上述理论，可以得出剩余四个控制点的位置：[0125][0126][0127] 将六个控制点的坐标信息带入五阶贝塞尔曲线参数方程，得：[0128] fs(τ)＝s0(1‑τ)5+(s5‑s0)(1‑τ)4τ+4(s5‑s0)(1‑τ)3τ2+6(s5‑s0)(1‑τ)2τ3+4(s5‑s0)4 5(1‑τ)τ +s5τ[0129] fd(τ)＝d0(1‑τ)5+5d0(1‑τ)4τ+10d0(1‑τ)3τ2+10d5(1‑τ)2τ3+5d5(1‑τ)τ4+d5τ5[0130] 在利用五阶贝塞尔曲线进行避障轨迹规划时，只要确定轨迹规划的目标点的位置，就可以得出五阶贝塞尔曲线拟合所需的六个控制点的坐标。但是，如果仅通过一个目标点来实现最终避障轨迹的规划，无法保证该轨迹是当前车辆状态下的最优避障轨迹。因此，本发明的方法是先确定避障轨迹终点在s方向上的位置se，再采用间隔固定位置取样的方法，选择避障轨迹终点在d方向上的位置de。通过分别设置在s方向和d方向上的适当的纵向间隔距离Δs与横向间隔距离Δd，可以实现避障轨迹对于可行驶区域的全覆盖，在这些生成的避障轨迹中，本发明通过设计评估函数来选取一条满足当前状态的最优避障轨迹。[0131] 避障轨迹筛选[0132] 在对规划出的避障轨迹进行筛选时，本发明需要检测轨迹的最大速度、最大加速度、最大曲率是否满足条件以及是否会与障碍物发生碰撞等因素，来实现对于避障轨迹的筛选，但在上文提到的在曲线坐标系下的贝塞尔曲线表达式中，f(τ)为参数τ的函数，为了能够更好的求出规划轨迹的速度与加速度信息，本发明建立参数τ到时间t的映射关系，可表达为：[0133] τ＝t/T[0134] 式中，T为车辆通过规划轨迹所要经过的总时间。[0135] 通过该表达式，本发明可以建立贝塞尔曲线表达式与时间t的关系，具体可表达为：[0136][0137][0138] 对于轨迹的速度约束，本发明可以预先设定车辆的最大行驶轨迹vmax，若某一轨迹点的最大速度值大于vmax，则将该轨迹舍弃，通过公式可表达为：[0139][0140] 速度公式也可表达为：[0141][0142] 对于车辆的加速度约束而言，由于加速度的值与变化率将对车内人员的乘坐舒适性产生重要影响，因此本发明需要分别对d方向上和s方向上的加速度分别进行与 与进行对比，实现对于轨迹的加速度约束，通过公式可表达为：[0143][0144][0145] 加速度公式也可表达为：[0146][0147][0148] 通过这种方式，本发明可以实现对于横向与纵向加速度的分别约束，实现对于规划轨迹在加速度方面的筛选任务。[0149] 对于规划轨迹的曲率约束，本发明可以采用与上述内容相同的思想，设置能够确保自动驾驶车辆安全行驶的最大轨迹曲率Kmax，通过与规划轨迹中每一个轨迹点的曲率进行对比，筛除不符合曲率约束的避障轨迹。公式表达为:[0150][0151] 在碰撞检测时，通过感知系统获得的障碍物的具体信息，本发明可以生成对于障碍物的安全检测范围，如果规划轨迹与安全检测区域相交，表明该轨迹与障碍物可能会发生碰撞，从而舍弃该轨迹。[0152] 设规划轨迹中每一点的位置为(si,di)，障碍物的位置为(so,do)，通过感知系统对于障碍物的识别定位，本发明可以获得障碍物当前与s轴的夹角α，通过(si,di)与(so,do)的连线，本发明可以获得该连线与s轴的夹角β，如图6所示。[0153] 通过对于轨迹点与障碍物位置等信息的分析，可以获得轨迹点在障碍物两个方向上的偏移值：[0154][0155][0156] 对于发生碰撞的条件，可以设为:[0157][0158] 式中，rsafe为自动驾驶车辆的安全距离。[0159] 本发明将检测区域设置为矩形，既保证了一定的安全冗余度，又能对于可行驶区域进行充分利用，提高了轨迹规划的成功率。在进行碰撞检测时，若规划轨迹通过了障碍物的安全检测区域，则判定该轨迹为不安全轨迹，从而实现对于该条轨迹的筛除。[0160] 面向感知盲区障碍物场景的连续避障轨迹规划方法[0161] 根据上文所描述的方法，通过基于改进A*算法可搜索出一条连续避障轨迹，但是该轨迹的生成基于车辆在避障轨迹规划之前对于障碍物的感知定位结果，若在初始感知时存在障碍物被遮挡的情况，车辆规划出的轨迹可能会经过被遮挡的障碍物或者距离被遮挡的障碍物过近，这会导致危险情况的发生。[0162] 通过结合动态轨迹规划的方法，能够更加灵活高效的处理含有多个障碍物的交通场景，提高车辆的避障成功率，增强车辆的安全性保障，实现对于较为复杂的障碍物场景的处理，对于上述的交通场景，经过动态轨迹规划之后，车辆实际的避障轨迹如图7所示。[0163] 在图7中，虚线为动态轨迹规划方法生成的避障轨迹，结合动态轨迹规划的连续避障轨迹生成方法实现了将改进A*算法与动态轨迹规划结合的目的，能够灵活且高效的实现车辆在存在多个障碍物的交通场景中的安全通行，解决了自动驾驶车辆在多个障碍物场景中难以规划出安全可行驶路径问题，提高了自动驾驶的智能性，为自动驾驶技术的进一步落地应用奠定了基础。[0164] 本发明的进一步的具体实施过程如图8所示。在图9所示的含有感知盲区的典型障碍物场景中，由于车辆在初始位置经过自车感知结果时只能发现障碍物1、障碍物2和障碍物3，障碍物4被障碍物1所遮挡，因此在碰撞区域划分时并没有考虑障碍物4对于车辆自动驾驶的影响，如果车辆按照上节方法规划出的连续避障轨迹行驶，会让自动驾驶车辆距离障碍物4过近，如图9中危险区域所示。[0165] 1.利用自车感知系统获多个障碍物的信息；[0166] 2.根据多个障碍物的位置信息搜索连续避障轨迹，具体过程为：[0167] 2.1基于障碍物位置信息进行碰撞区域设置[0168] 根据障碍物1、障碍物2、障碍物3的具体位置，设置碰撞区域位置，图()碰撞区域的位置信息可表达为：[0169] l′1＝max(|s1‑s2+(l2‑l1)/2|,|s1‑s2‑(l1+l2)/2|,|s1‑s2+(l2+l1)/2|,|s1‑s2+(l1‑l2)/2|)[0170] w′1＝min(|d1‑d2+(w2‑w1)/2|,|d1‑d2‑(w1+w2)/2|,|d1‑d2+(w2+w1)/2|,|d1‑d2+(w1‑w2)/2|)[0171] 根据碰撞区域的长宽信息，结合障碍物的位置信息，本发明可以得出碰撞区域中心点的位置：[0172] s′1＝min(s1‑l1/2,s2‑l2/2)+l′1/2[0173] d′1＝min(d1‑w1/2,d2‑w2/2)+w′1/2+w2[0174] 式中，(s1,d1,l1,w1)为障碍物1的位置信息，而(s2,d2,l2,w2)为障碍物2的位置信息。[0175] 2.2根据障碍物信息与碰撞区域设置，本发明可以实现对于多障碍物场景中可行驶区域的划分，通过对可行驶区域内以纵向间隔Δs，横向间隔Δd对可行驶区域进行离散化处理，得到可行驶轨迹点(Δs＝Δd＝0.5m)，图2中的点为可行驶轨迹点[0176] 2.3根据改进A*算法，进行连续避障轨迹规划，启发函数方程为：[0177] fi＝k1(gi+hi)+k2(di+ci)+k3bi[0178] 式中，gi、hi、ci与bi的定义分别为：[0179] gi＝gi‑1+Δcost(31)[0180][0181] ci＝ci‑1+Δd(33)[0182][0183] 式中，gi与hi的含义与传统A*算法中相同，di为轨迹点在d方向上距离全局参考轨迹的偏移，本发明均用欧式距离表示；bi为轨迹点与最近障碍物之间的距离代价，离障碍物越近，轨迹点代价应该越大，δ表示比例系数；ci为累计纵向偏移代价，计算过程为上一轨迹点的累计偏移加上当前轨迹点距上一轨迹点的纵向偏移，k1、k2、k3为启发函数的调节因子，且k1+k2+k3＝1。[0184] 3.基于动态轨迹规划方法，实现对于感知盲区障碍物的避障轨迹规划[0185] 3.1随着车辆沿基于改进A*算法规划出的连续避障轨迹向前行驶，可以发现处于感知盲区障碍物的具体位置，然后结合动态轨迹规划的方法，基于五阶贝塞尔曲线规划避障轨迹。设自动驾驶车辆开始规划时在曲线坐标系中的坐标为P0＝(s0,d0)，规划终止点的坐标为P5＝(s5,d5)，根据上述理论，可以得出剩余四个控制点的位置：[0186][0187][0188] 将六个控制点的坐标信息带入五阶贝塞尔曲线参数方程，可得：[0189] fs(τ)＝s0(1‑τ)5+(s5‑s0)(1‑τ)4τ+4(s5‑s0)(1‑τ)3τ2+6(s5‑s0)(1‑τ)2τ3+4(s5‑s0)4 5(1‑τ)τ +s5τ[0190] fd(τ)＝d0(1‑τ)5+5d0(1‑τ)4τ+10d0(1‑τ)3τ2+10d5(1‑τ)2τ3+5d5(1‑τ)τ4+d5τ5[0191] 本发明的方法是先确定避障轨迹终点在s方向上的位置se，再采用间隔固定位置取样的方法，选择避障轨迹终点在d方向上的位置de。通过分别设置在s方向和d方向上的适当的纵向间隔距离Δs与横向间隔距离Δd，可以实现避障轨迹对于可行驶区域的全覆盖，在这些生成的避障轨迹中，本发明通过设计评估函数来选取一条满足当前状态的最优避障轨迹。[0192] 3.2通过对于动态规划轨迹的速度、加速度与曲率的限制分析以及是否与障碍物发生碰撞等因素，实现对于避障轨迹的筛选。[0193] 3.2.1通过建立参数τ到时间t的映射关系,来对轨迹的速度、加速度与曲率进行分析。映射关系可以表达为：[0194] τ＝t/T[0195] 式中，T为车辆通过规划轨迹所要经过的总时间。[0196] 通过设置设定车辆的最大行驶速度vmax，d方向上和s方向上的加速度限制 与以及自动驾驶车辆安全行驶的最大轨迹曲率Kmax，可以实现对于动态规划轨迹的筛选，主要筛选过程为：[0197][0198][0199][0200][0201] 筛除不满足限制条件的轨迹，将剩余的轨迹进行碰撞分析。[0202] 3.2.2进行轨迹的碰撞检测，设规划轨迹中每一点的位置为(si,di)，障碍物的位置为(so,do)，通过感知系统对于障碍物的识别定位，本文可以获得障碍物当前与s轴的夹角α，通过(si,di)与(so,do)的连线，本文可以获得该连线与s轴的夹角β，如图6所示。通过对于轨迹点与障碍物位置等信息的分析，可以获得轨迹点在障碍物两个方向上的偏移值：[0203][0204][0205] 对于发生碰撞的条件，设为：[0206][0207] 式中，rsafe为自动驾驶车辆的安全距离。[0208] 通过这种方式，可以将与障碍物发生危险的轨迹筛除。[0209] 3.3通过评估函数的设计，实现对于最优避障轨迹的筛选，评估函数的设计为：[0210][0211] 式中，dcenter(i)表示在第i个点规划轨迹与全局参考轨迹在d方向上的偏移，Ki表示规划轨迹在第i个点的曲率，ad(i)表示第i个点在d方向上的加速度，lp(i)表示第i段规划轨迹的曲线长度，lorg(i)表示第i段参考轨迹的长度，ωd、ωk、ωa、ωl分别表示四部分各自的代价系数。[0212] 将每条轨迹按照评价函数的值进行排序，选择损失值最小的轨迹最为最优避障轨迹。[0213] 4.在改进A*算法连续避障轨迹的基础上，结合动态规划轨迹，实现在多障碍物场景下对于感知盲区障碍物的避让。[0214] 在应对较为复杂的障碍物场景时，传统的规划方法存在场景适应性差，避障效率不高等问题。在单障碍物的场景中，本文提出的基于五阶贝塞尔曲线的动态轨迹决策规划方法相比于传统的基于多项式曲线生成轨迹的规划方法，具有横向加速度更小、规划效率更高、避障效果更稳定的优势；在面向多个障碍物的场景中，通过基于智能车辆在开始避障前对于障碍物的感知定位结果，提出基于改进A*算法的连续避障轨迹规划方法，充分考虑规划轨迹与障碍物的距离信息。相比于传统A*方法，本文提出的规划方法能提高连续避障过程中的安全性。在含有视野盲区障碍物的场景中，通过结合改进A*算法与动态轨迹规划方法来实现对于盲区障碍物的安全避让，这在当前以自车感知系统为主要感知方法的自动驾驶技术落地过程中具有重要意义。

专利地区：四川

专利申请日期：2022-05-26

专利公开日期：2024-07-26

专利公告号：CN115042809B