一种交叉口进口道自动驾驶车辆极限换道轨迹的确定方法
1.本发明属于自动驾驶车辆轨迹规划领域，具体涉及一种交叉口进口道自动驾驶车辆极限换道轨迹的确定方法。
背景技术：
2.轨迹规划是自动驾驶技术中的重点与难点，换道是基本驾驶行为之一。在交叉口进口道的车辆换道具有强制性，而且换道频率高，需要考虑的因素比常规路段更为复杂。换道轨迹规划不合理会影响通行效率或者导致换道失败，甚至会出现车辆压线换道等违反交通规则的情况，同时也将威胁行车安全。为自动驾驶车辆提供极限换道轨迹的预警后，就能够避免发生上述情况。因此，确定交叉口进口道自动驾驶车辆的极限换道轨迹对于提高驾驶安全性、换道成功率和通行效率具有重要意义。
3.现有对轨迹规划的研究比较广泛，但大多数集中在避障上，并且较少涉及交叉口进口道场景。发明专利202111661902.6根据道路环境、障碍物信息以及当前车速，通过求解关于舒适性、避障效率、侧滑的多目标优化问题，确定最优的避障换道回正点和基于多项式曲线的避障换道轨迹；发明专利202111406002.7首先根据道路形式、车辆的位姿和速度信息生成候选轨迹，然后构建并计算候选轨迹的成本函数，选择成本最小的候选轨迹作为最优轨迹。然而，以往未见自动驾驶车辆在交叉口进口道极限换道轨迹的研究。交叉口进口道的换道行为与常规路段不同，为了保证行车安全、不违反交通规则和成功换至目标车道以满足转向需求，自动驾驶车辆需要极限换道轨迹的预警以指导其进行换道操作，因此，在轨迹规划领域，交叉口进口道自动驾驶车辆极限换道轨迹的确定方法是非常有必要的。
4.基于上述背景，亟需提出一种交叉口进口道自动驾驶车辆极限换道轨迹的确定方法，在考虑车辆自身条件约束和道路条件约束的情况下，基于五次多项式确定极限换道轨迹，以提高驾驶安全性、换道成功率和通行效率。经查，暂未有交叉口进口道自动驾驶车辆极限换道轨迹的确定方法的相关报道。
技术实现要素：
5.1.一种交叉口进口道自动驾驶车辆极限换道轨迹的确定方法，包括如下步骤：
6.步骤一、生成基于五次多项式的换道轨迹集：
7.步骤1.1建立frenet坐标系：
8.定义换道前自动驾驶车辆所在车道为原车道，以下将自动驾驶车辆简称为车辆，以原车道的中心线为参考线，以车辆自身为原点，分别以参考线的切向量s和参考线的法向量d所在直线为坐标轴建立frenet坐标系，定义参考线的切向量s方向为纵向，参考线的法向量d方向为横向；
9.步骤1.2建立并求解基于五次多项式的换道轨迹模型：
10.在frenet坐标系下分别建立纵向位移和横向位移关于换道过程中某一时刻t的五次多项式函数s(t)和d(t)：
[0011][0012]
其中，pk、qk是时刻t的系数，k＝0，1，2，3，4，5；
[0013]
求解纵向位移关于时刻t的函数s(t)，对s(t)进行两次求导分别得到纵向速度和纵向加速度关于时刻t的函数和
[0014][0015]
将换道开始时刻t＝0和此时车辆的纵向位置s(0)、纵向速度和纵向加速度分别带入上式求得p0＝s(0)、和
[0016]
将五次多项式函数s(t)写成如下矩阵形式：
[0017][0018]
以交叉口进口道导向车道线远离交叉口方向的端点位置为参考确定换道结束时刻t＝t车辆的纵向位置s(t)，定义交叉口进口道限速为v
limit
，车辆最大加速度为a
max
，将交叉口进口道限速的纵向分量v
limit,s
和车辆最大加速度的纵向分量a
max,s
分别作为换道结束时刻车辆的纵向速度和纵向加速度，再将换道结束时刻车辆的纵向位置s(t)、纵向速度v
limit,s
和纵向加速度a
max,s
代入上面的矩阵中求得p3、p4和p5；
[0019]
求解横向位移关于时刻t的函数d(t)，对d(t)进行两次求导分别得到横向速度和横向加速度关于时刻t的函数和
[0020][0021]
将换道开始时刻t＝0和此时车辆的横向位置d(0)、横向速度和横向加速度分别带入上式求得q0＝d(0)、和
[0022]
将五次多项式函数d(t)写成如下矩阵形式：
[0023][0024]
以交叉口进口道导向车道线远离交叉口方向的端点位置为参考确定换道结束时刻车辆的横向位置d(t)，将交叉口进口道限速的横向分量v
limit,d
和车辆最大加速度的横向分量a
max,d
分别作为换道结束时刻车辆的横向速度和横向加速度，再将换道结束时刻车辆的横向位置d(t)、横向速度v
limit,d
和横向加速度a
max,d
代入上面的矩阵中求得q3、q4和q5；
[0025]
最后，以0.2秒为时间步长将换道结束时刻t取为[3秒，10秒]，生成换道轨迹集；
[0026]
步骤二、考虑车辆动力学约束对换道轨迹集进行优化：
[0027]
加入车辆动力学约束，优化步骤一所生成的换道轨迹集：
[0028]
速度约束：换道过程中车辆的速度v(t)不大于交叉口进口道限速，即0≤v(t)≤v
limit
；加速度约束：换道过程中车辆的加速度a(t)不大于车辆最大加速度a
max
，不小于车辆最大减速度a-max
，即a-max
≤a(t)≤a
max
；横向加速度约束：换道过程中车辆的横向加速度应保证车辆不发生侧滑和侧倾，即其中b为车辆轮距，单位：cm，hg为质心高度，单位：cm，hr为侧倾中心高度，单位：cm，g为重力加速度，为车辆侧倾率，单位：rad/g，为路面附着系数；
[0029]
步骤三、遍历换道起点位置，确定极限换道起点及其对应的极限换道轨迹：
[0030]
将交叉口进口道可跨越同向车道线分界线的区域称为可换道区域，以0.1秒为时间步长，在可换道区域内，从步骤一中所代入的换道开始时刻的车辆位置(s(0)，d(0))向交叉口方向遍历换道起点位置，直到步骤二所得到的换道轨迹集中只剩下一条轨迹时停止遍历，遍历停止时的换道起点即为极限换道起点，其对应的轨迹为极限换道轨迹；
[0031]
跨越两条车道以上的换道操作需要分多次进行计算，每次仅可换道至相邻车道，每次确定极限换道起点及极限换道轨迹时均重复步骤一至三。
[0032]
与现有技术相比，本发明的有益效果是：
[0033]
本发明所述的一种交叉口进口道自动驾驶车辆极限换道轨迹的确定方法，考虑车辆自身条件约束和道路条件约束，为自动驾驶车辆提供了极限换道轨迹的预警，避免出现车辆压线换道等违反交通规则的情况，同时也提高了驾驶安全性、换道成功率和通行效率，为自动驾驶车辆合理规划换道轨迹提供了技术支撑。
附图说明
[0034]
图1是本发明的总体流程图；
[0035]
图2是本发明的frenet坐标系示意图；
[0036]
图3是本发明步骤一至三仿真计算结果图；
[0037]
图4是本发明一次换道的极限换道轨迹示例图；
[0038]
图5是本发明多次换道的极限换道轨迹示例图。
具体实施方式
[0039]
本发明专利所述的一种交叉口进口道自动驾驶车辆极限换道轨迹的确定方法，适用于确定车辆在进入交叉口前的换道过程中周围无其他车辆干扰的情况下保证车辆行驶安全和不违反交通规则的极限换道轨迹。该极限换道轨迹不会与导向车道线相交，满足车辆动力学约束，车辆驶过极限换道起点后便不可进行换道操作，并且，刚好到达极限换道起点的车辆必须按照规划的极限换道轨迹行驶。
[0040]
参阅图1，本发明所述的一种交叉口进口道自动驾驶车辆极限换道轨迹的确定方法有以下几个步骤：
[0041]
步骤一、在frenet坐标系下，如图2所示，建立并求解基于五次多项式的换道轨迹模型，生成换道轨迹集；
[0042]
步骤二、加入车辆动力学约束，优化步骤一中所生成的换道轨迹集；
[0043]
步骤三、由步骤一中所代入的换道开始时刻的车辆位置向交叉口方向遍历换道起点，直到步骤二的换道轨迹集中只剩下一条轨迹时，即确定极限换道起点及其对应的极限换道轨迹。
[0044]
步骤一至三的仿真计算结果如图3所示，本发明确定的一次换道的极限换道轨迹如图4所示，极限换道终点即为以交叉口进口道导向车道线远离交叉口方向的端点位置为参考确定的换道结束时刻车辆的位置(s(t)，d(t))。
[0045]
《中华人民共和国道路交通安全法》规定车辆不得一次连续变更两条以上机动车道。因此，跨越两条车道以上的换道操作需要分多次进行计算，每次仅可换道至相邻车道，每次确定极限换道起点及极限换道轨迹均重复步骤一至三，如图5所示。
[0046]
实施例
[0047]
本发明所述的一种交叉口进口道自动驾驶车辆极限换道轨迹的确定方法的实施例，给出实施过程和检验结果，但本发明的保护范围不限于下述的实施例。
[0048]
通过仿真实验进行实例论述。选取石家庄市裕华路与友谊大街交叉口西进口道。该进口道每车道宽为3.25米，导向车道线长为70米，共7条车道，分别为左转专用道两条，直行专用道四条，右转专用道一条。根据本发明所述的一种交叉口进口道自动驾驶车辆极限换道轨迹的确定方法对自动驾驶车辆在各车道间的极限换道轨迹进行计算，主要的换道操作如下表，仿真计算结果如图3所示。
[0049]
表1各车道间的主要换道操作
[0050][0051]
对同时段该交叉口进口道的航拍视频数据进行观察统计，发现在禁止换道区域，即导向车道线区域，压线换道的车辆有6辆。采用本发明所述的一种交叉口进口道自动驾驶车辆极限换道轨迹的确定方法后，仿真实验中未出现压线换道的车辆，且仿真实验中临近导向车道线进行换道操作的车辆，比实际场景中的平均行驶时间减少了9.3％。由此可见，采用该方法为自动驾驶车辆提供极限换道轨迹的预警，确保了车辆行驶安全，提高了换道成功率和通行效率。
技术特征：
1.一种交叉口进口道自动驾驶车辆极限换道轨迹的确定方法，包括如下步骤：步骤一、生成基于五次多项式的换道轨迹集：步骤1.1建立frenet坐标系：定义换道前自动驾驶车辆所在车道为原车道，以下将自动驾驶车辆简称为车辆，以原车道的中心线为参考线，以车辆自身为原点，分别以参考线的切向量s和参考线的法向量d所在直线为坐标轴建立frenet坐标系，定义参考线的切向量s方向为纵向，参考线的法向量d方向为横向；步骤1.2建立并求解基于五次多项式的换道轨迹模型：在frenet坐标系下分别建立纵向位移和横向位移关于换道过程中某一时刻t的五次多项式函数s(t)和d(t)：其中，p
k
、q
k
是时刻t的系数，k＝0，1，2，3，4，5；求解纵向位移关于时刻t的函数s(t)，对s(t)进行两次求导分别得到纵向速度和纵向加速度关于时刻t的函数和和将换道开始时刻t＝0和此时车辆的纵向位置s(0)、纵向速度和纵向加速度分别带入上式求得p0＝s(0)、和将五次多项式函数s(t)写成如下矩阵形式：以交叉口进口道导向车道线远离交叉口方向的端点位置为参考确定换道结束时刻t＝t车辆的纵向位置s(t)，定义交叉口进口道限速为v
limit
，车辆最大加速度为a
max
，将交叉口进口道限速的纵向分量v
limit,s
和车辆最大加速度的纵向分量a
max,s
分别作为换道结束时刻车辆的纵向速度和纵向加速度，再将换道结束时刻车辆的纵向位置s(t)、纵向速度v
limit,s
和纵向加速度a
max,s
代入上面的矩阵中求得p3、p4和p5；求解横向位移关于时刻t的函数d(t)，对d(t)进行两次求导分别得到横向速度和横向加速度关于时刻t的函数和
将换道开始时刻t＝0和此时车辆的横向位置d(0)、横向速度和横向加速度分别带入上式求得q0＝d(0)、和将五次多项式函数d(t)写成如下矩阵形式：以交叉口进口道导向车道线远离交叉口方向的端点位置为参考确定换道结束时刻车辆的横向位置d(t)，将交叉口进口道限速的横向分量v
limit,d
和车辆最大加速度的横向分量a
max,d
分别作为换道结束时刻车辆的横向速度和横向加速度，再将换道结束时刻车辆的横向位置d(t)、横向速度v
limit,d
和横向加速度a
max,d
代入上面的矩阵中求得q3、q4和q5；最后，以0.2秒为时间步长将换道结束时刻t取为[3秒，10秒]，生成换道轨迹集；步骤二、考虑车辆动力学约束对换道轨迹集进行优化：加入车辆动力学约束，优化步骤一所生成的换道轨迹集：速度约束：换道过程中车辆的速度v(t)不大于交叉口进口道限速，即0≤v(t)≤v
limit
；加速度约束：换道过程中车辆的加速度a(t)不大于车辆最大加速度a
max
，不小于车辆最大减速度a-max
，即a-max
≤a(t)≤a
max
；横向加速度约束：换道过程中车辆的横向加速度应保证车辆不发生侧滑和侧倾，即其中b为车辆轮距，单位：cm，h
g
为质心高度，单位：cm，h
r
为侧倾中心高度，单位：cm，g为重力加速度，为车辆侧倾率，单位：rad/g，为路面附着系数；步骤三、遍历换道起点位置，确定极限换道起点及其对应的极限换道轨迹：将交叉口进口道可跨越同向车道线分界线的区域称为可换道区域，以0.1秒为时间步长，在可换道区域内，从步骤一中所代入的换道开始时刻的车辆位置(s(0),d(0))向交叉口方向遍历换道起点位置，直到步骤二所得到的换道轨迹集中只剩下一条轨迹时停止遍历，遍历停止时的换道起点即为极限换道起点，其对应的轨迹为极限换道轨迹；跨越两条车道以上的换道操作需要分多次进行计算，每次仅可换道至相邻车道，每次确定极限换道起点及极限换道轨迹时均重复步骤一至三。
技术总结
本发明专利公开了一种交叉口进口道自动驾驶车辆极限换道轨迹的确定方法。具体方法主要包括三个步骤：一、生成基于五次多项式的换道轨迹集；二、考虑车辆动力学约束对换道轨迹集进行优化；三、遍历换道起点位置，确定极限换道起点及其对应的极限换道轨迹。本发明专利考虑车辆自身条件约束和道路条件约束，提出交叉口进口道自动驾驶车辆极限换道轨迹的确定方法，为自动驾驶车辆提供了极限换道轨迹的预警，从而提高驾驶安全性、换道成功率和通行效率。率。率。
