1、第 44 卷第 2 期2 0 2 3 年 2 月兵工学报ACTA AMAMENTAIIVol 44 No 2Feb2023DOI:10 12382/bgxb 2021 0756基于微分平坦的分层轨迹规划算法周孝添1，任宏斌1，苏波2，齐志权2，汪洋2(1 北京理工大学 机械与车辆学院，北京 100081;2 中国北方车辆研究所，北京 100072)摘要:为充分考虑横纵向耦合和汽车运动学特性对轨迹规划的影响，提出一种分层优化的轨迹规划算法框架。利用贝塞尔曲线的凸包性设计安全走廊约束，以轨迹平滑性为目标函数得到一个基于贝塞尔曲线节点的下层规划器。在上层规划器中，基于下层规划器求解得到的横纵向贝塞尔
2、曲线和车辆运动学模型的微分平坦输出进行三维耦合，构建满足车辆乘坐舒适性、高效性和安全性的目标函数，利用粒子群优化算法对贝塞尔轨迹初始参数进行二次优化得到综合性能最优的行驶轨迹。仿真结果表明:新算法在保证安全性的同时，具有良好的乘坐舒适性和可跟踪性;由于二次规划与粒子群优化算法的求解效率高，此框架实时性强，具有概率完备性。关键词:轨迹规划;微分平坦;贝塞尔曲线;二次规划;粒子群优化算法中图分类号:TJ810.2文献标志码:A文章编号:1000-1093(2023)02-0394-12Hierarchical Trajectory Planning Algorithm based on Diffe
3、rential FlatnessZHOU Xiaotian1，EN Hongbin1，SU Bo2，QI Zhiquan2，WANG Yang2(1 School of Mechanical Engineering，Beijing Institute of Technology，Beijing 100081，China;2 China North Vehicle esearch Institute，Beijing 100072，China)Abstract:To fully consider the influence of transverse and longitudinal coupli
4、ng and vehicle kinematicson trajectory planning，a hierarchical optimization-based trajectory planning algorithm framework isproposed The safe corridor constraint is designed with the convex hull of a Bezier curve Taking thetrajectory smoothness as the objective function，we obtain a lower planner bas
5、ed on Bezier curve nodesIn the upper planner，based on the transverse and longitudinal Bezier curves solved by the lower plannerand the differentially flat output of the vehicle kinematics model，the objective function meeting thevehicle ride comfort，efficiency and safety requirements is constructed，a
6、nd quadratic optimization isapplied to the initial parameters of the Bezier trajectory by particle swarm optimization algorithm to obtainthe driving trajectory with the best comprehensive performance The simulation results show that:thealgorithm has good ride comfort and traceability while ensuring
7、safety;due to the high efficiency ofquadratic programming and particle swarm optimization，this framework has strong real-time andprobabilistic completenessKeywords:trajectory planning;differential flatness;bezier curve;quadratic programming;particleswarm optimization algorithm收稿日期:2021-11-09基金项目:国家自
8、然科学基金项目(52002025)第 2 期基于微分平坦的分层轨迹规划算法0引言在人工智能的热潮之下，智能汽车行业的发展也受到社会的广泛关注，因为它将在降低交通事故发生率、减少交通拥堵、改善大气环境、降低能源消耗等方面做出重要贡献1。无人驾驶汽车是智能汽车发展的高级阶段，避障轨迹规划作为无人驾驶技术最核心的技术之一，对无人驾驶车辆的研究具有重大意义。在结构化道路下，其基本任务是考虑车辆与前方障碍物的几何关系，规划出一条避免发生碰撞的换道轨迹，同时应该具有较高的乘坐舒适性，从而提高出行效率1 2。针对结构化道路下的变道避障轨迹规划，国内外学者进行了各种研 究3 9。Zhang 等4和聂枝根等5
9、利用三角函数去模拟车辆变道轨迹，此算法虽然简单高效，但忽略了运动学约束，且存在变道终点已知的局限。Kanaris 等6 提出一种基于正反梯形横向加速度的方法，通过对横向加速度优化求解、积分得到变道轨迹，此方法虽然有效结合了车辆横向运动学，但不够灵活，无法高效地调节整个变道过程。牛国臣等7 设计了一种双5 次多项式换道避障轨迹规划算法，虽然此算法通过优化换道时间来改善乘坐舒性，但忽略了横纵向耦合对行驶安全性乘坐舒适性的影响。Li 等8 设计了一种基于多阶贝塞尔曲线的变道避障路径规划方法，此方法虽然能有效地调节变道效率和乘坐舒适性之间的平衡关系，但只适用于恒速变道。Shim 等9 设计了横纵向上关
10、于时间的 6 阶多项式模型，通过求解非凸目标函数得到了期望轨迹，虽然此方法考虑了乘坐舒适性和运动学约束，但因为是一个非凸优化过程，所以求解效率较低，实用性差。近年来，微分平坦定理在无人机和无人车运动规划中得到的广泛关注，其主要优点是能将非线性系统的微分约束映射为平滑集合约束，有效减少了优化空间的维数10 12。Cong 等10 利用车辆系统的平坦输出建立运动学和稳定性约束来弥补基本质点模型规划算法的不足。Wang 等11 利用车辆动力学模型的微分平坦特性线性化车辆非线性动力学模型，大大提高了预测控制精度。Guo 等12 基于微分平坦定理提出了一种具有轮胎稳定性控制的最大功率控制器。综上所述，目
11、前运动规划问题普遍都是把三维的轨迹规划(横向纵向时间)解耦为两个二维规划问题，但 McNaughton 等13 认为解耦会损失车辆运动的最优性。由于在横纵向轨迹设计时并没有考虑其耦合性，为提升规划算法的最优性且能满足更多的运动学约束，本文提出一种分层段轨迹规划算法，在底层构建以贝塞尔曲线(Bzier curve)节点为自变量且满足避障、速度、加速度约束等凸约束的二次规划问题，保证算法的概率完备性和行车安全性。利用车辆运动学的微分平坦特性，构建以乘坐舒适性、运动学约束、能耗、行车效率等指标的目标函数，利用粒子群优化(PSO)算法优化终止车速和行驶时间，得到一条满足车辆多目标约束的行驶轨迹。1基于
12、贝塞尔曲线的局部路径规划1.1贝塞尔曲线贝塞尔曲线是应用于二维图形应用程序的数学曲线14。曲线定义起始点、终止点(也称锚点)、控制点。通过调整控制点，贝塞尔曲线的形状会发生变化。故基于贝塞尔曲线进行避障规划是通过寻找贝塞尔曲线控制点的最优位置来实现避障。已知贝塞尔曲线方程为Bj(t)=c0jb0n(t)+c1jb1n(t)+cnjbnn(t)=ni=0cijbin(t)(1)bin(t)=n()i ti(1 t)n i(2)式中:Bj(t)为贝塞尔曲线;cij为第 j 段曲线的第 i 个节点坐标;n 为节点个数;t 为时间变量。从上述推导可知，贝塞尔曲线本质是关于变量 t的多项式，由于形状由每
13、个节点的坐标决定，具有如下性质:1)它总是从第 1 个控制点开始，到最后一个控制点结束，不经过中间其他节点，但是曲线始终在所有节点连接构成的凸多边形内。2)曲线每个节点的导数由曲线节点的线性组合求得，即曲线的导数同样也可以被约束在凸多边形内。3)曲线自变量 t 的区间为 0，1。1.2避障轨迹规划根据贝塞尔曲线形状可知，当给定控制点范围约束时，可以保证整条曲线都位于此安全范围内，利用此性质大大减少了约束的数量，提升了求解效率如图 1 所示，图中点划线为两车道的中心线，黑色虚线为车道分割线，由于静态障碍物和动态障碍物的存在，贝塞尔曲线节点的可行域是一个非凸域。为了使算法的求解效率更高，在非凸域上
14、构造两个凸593兵工学报第 44 卷的安全走廊，以两段 7 阶贝塞尔曲线构造变道曲线，变道起点和变道终点都位于车道中心线上，则把原始的非凸优化问题转化为在两个安全走廊的凸优化问题。图 1避障轨迹规划Fig 1Obstacle avoidance trajectory planning由于贝塞尔曲线时间的取值在 0，1内，故需要对曲线进行归一化，则实际变道曲线函数15 为f(t)=s1ni=0ci1b(ints)1，t 0，1s2ni=0ci2b(ints)2，t 0，1(3)式中:s1、s2为每一段贝塞尔曲线对应的实际时间;x，y 表示横向和纵向两个方位。由于贝塞尔曲线的安全走廊存在一定范围内
15、的重叠，使得优化问题具有更高的自由度，对每一段贝塞尔曲线时间分配进行隐性调节。把三维空间中的轨迹规划问题分解为两个二维空间中的贝塞尔曲线优化问题，建立以曲线 3 次导数的平方和为目标函数:J=T(0d(4s1cib(its)1dt)42d+T(0d(4s2cib(its)2dt)42d=10s5(1d(4cibi()d)42d+10s5(2d(4cibi()d)42d(4)式中:T 为贝塞尔曲线对应的时间;为仿真时间，0，1，节点编号 i 0，1，2，3，4，5，6，7;j 1，2 表示两段贝塞尔曲线。设节点 i 的第 f 阶导数为af，ij=n!(n f)!(af 1，i+1f af 1，i
16、j)，f1(5)式中:a0，ij=cij。则起始点和终止点的边界约束为af，11 s(1 f)1=df，11，af，n2 s(1 f)2=df，n2(6)式中:f 0，1，2 表示位置、速度和加速度约束;df，11、df，n2分别表示第 1 段曲线的起始条件和第 2 段曲线的终止条件。由于轨迹是由两段曲线拼接而成，需要设置连续性约束:af，n1 s1 f1=af，1，2 s1 f2，a0，i1=ci1，a0，i2=ci2(7)由式(7)可知通过保证第 1 段曲线末尾节点与第 2 段曲线起始节点的初始位置、速度加速度相等来保证连续性约束。为保证行车安全行，避免与障碍物发生碰撞，需要添加边界约束:jczj+j，z=1，2，n(8)式中:j、+j边界值的大小由安全走廊决定，其保证曲线节点一定是在安全走廊内。实际车辆行驶过程中受到运动学约束，需要对横纵方向上的速度、加速度进行约束:vn (cij ci 1j)v+(9)an (n 1)(cij2ci 1j+ci 2j)/sja+(10)式中:v、v+、a、a+分别为车辆车速、加速度的上下边界，由无人驾驶车辆特性决定。由于目标函数是一个二次型，且
