详情

原文传递一种基于非线性规划的曲线道路自动车换道轨迹规划方法

专利名称：	一种基于非线性规划的曲线道路自动车换道轨迹规划方法
摘要：	本发明涉及智能车的轨迹规划领域，具体涉及一种基于非线性规划的曲线道路自动车换道轨迹规划方法。首先利用高精度地图、传感器和GPS设备采集曲线道路信息并实时拟合道路方程；其次将换道总时间等分，构造含有未知参数的换道轨迹多项式曲线方程；然后建立车辆模型，通过计算换道车辆与周边车辆的实时距离来检测是否可能发生碰撞；最后基于换道行为起始点和终点的已知状态以及驾驶规范，设置合理的目标函数和约束条件，将轨迹规划问题转化为非线性优化求解问题，采用序列二次规划算法(SQP)求出换道车辆在不同时间点的位置、速度和加速度，以此规划出高效且安全的换道轨迹。
专利类型：	发明专利
国家地区组织代码：	北京;11
申请人：	北京交通大学
发明人：	卫翀;王莹;马路;闫学东;邵春福;李殊荣
专利状态：	有效
申请日期：	2019-07-11T00:00:00+0800
发布日期：	2019-11-19T00:00:00+0800
申请号：	CN201910624865.8
公开号：	CN110466516A
代理机构：	北京集智东方知识产权代理有限公司
代理人：	陈亚斌;关兆辉
分类号：	B60W30/095(2012.01);B;B60;B60W;B60W30
申请人地址：	100044 北京市海淀区西直门外上园村3号
主权项：	1.一种基于非线性规划的曲线道路自动车换道轨迹规划方法，其特征在于，包括如下步骤： (1)采集道路信息：利用高精度地图、传感器和GPS设备采集曲线道路信息，并基于道路位置点回归拟合出表示道路线型的多项式方程； (2)表征换道轨迹曲线方程：将换道总时间等分为I个样本点，采用依赖于时间的六次多项式表征横向和纵向的换道轨迹，在换道行为起始点和终点部分交通状态已知的基础上，计算换道轨迹曲线多项式方程的未知参数； (3)防碰撞检测：建立合适的车辆动力学模型，通过计算换道车辆与周边车辆的实时距离来检测是否可能发生碰撞； (4)将轨迹规划问题转化为求解未知参数的非线性优化求解问题：考虑换道总时长、换道车辆速度和加速度对换道效率产生的影响，建立目标函数；考虑车辆跟驰行为、防碰撞检测以及驾驶规范，建立一系列约束条件； (5)对非线性优化求解问题采用序列二次规划算法，根据优化计算结果，选取出安全、高效且舒适的最优换道轨迹。 2.根据权利要求1所述一种基于非线性规划的曲线道路自动车换道轨迹规划方法，其特征在于，步骤(1)中，采集道路信息是通过高精度地图、传感器和GPS设备得到曲线道路上一系列参考样本点的x，y轴坐标及相邻两个参考点之间的长度s，将数据离线存储；在轨迹规划时直接依据车辆换道初始位姿，回归拟合出表示道路线型的多项式方程，具体形式如下所示： y(x)＝A0+A1x+A2x2+A3x3+A4x4+A5x5+A6x6 x(s)＝B0+B1s+B2s2+B3s3+B4s4+B5s5+B6s6 s(x)＝C0+C1x+C2x2+C3x3+C4x4+C5x5+C6x6 其中，y(x)表示车道曲线的横纵坐标关系；x(s)表示以s为自变量的车道曲线长度与横坐标关系；s(x)表示以x为自变量的车道横坐标与车道曲线长度的关系，A0，...，A6、B0，...，B6、C0，...，C6表示换道轨迹曲线多项式方程的参数，为常数。 3.根据权利要求1所述一种基于非线性规划的曲线道路自动车换道轨迹规划方法，其特征在于，步骤(2)表征换道轨迹曲线方程的过程中，将换道总时间等分为I个样本点，具体方法为：平分换道总时长T，并用i＝0，1，...，I对样本时间点进行标号，初始时间点t0＝0，换道完成时间点tI＝T；ti表示第i个时间点，ti的具体表达式如下： 4.根据权利要求3所述一种基于非线性规划的曲线道路自动车换道轨迹规划方法，其特征在于，步骤(2)中，换道轨迹曲线方程是基于时间变量的横向和纵向位姿方程，采用两个六次多项式来表示空间轨迹，以此来保证换道车辆在换道期间速度、加速度和加加速度连续且光滑的，具体形式如下： x0，i(ti)＝β0+β1ti+β2ti2+β3ti3+β4ti4+β5ti5+β6ti6 y0，i(ti)＝α0+α1ti+α2ti2+α3ti3+α4ti4+α5ti5+α6ti6 其中，ti表示第i个时间点，x0，i(ti)、y0，i(ti)分别表示换道车辆在不同样本时间点的纵向和横向坐标位置，α0，...，α6和β0，...，β6表示换道轨迹曲线方程的待求解参数。 5.根据权利要求4所述一种基于非线性规划的曲线道路自动车换道轨迹规划方法，其特征在于，已知的换道行为起始点和终点交通状态为： y0，i(t0)＝y0，0， x0，i(t0)＝x0，0， t0表示换道初始时刻；分别表示横向轨迹方程在从初始时刻的一阶导数和二阶导数；分别表示纵向轨迹方程在从初始时刻的一阶导数和二阶导数； x0，0，y0，0分别表示换道车辆初始时刻纵向和横向的位置；分别表示换道车辆初始时刻纵向和横向的速度；分别表示换道车辆初始时刻纵向和横向的加速度；根据已知的换道行为起始点和终点交通状态，代入横向和纵向位姿方程x0，i(ti)、y0，i(ti)，计算多项式换道轨迹曲线方程中α0，α1，α2和β0，β1，β2的数值。 6.根据权利要求1所述一种基于非线性规划的曲线道路自动车换道轨迹规划方法，其特征在于，步骤(3)的防碰撞检测具体过程为：根据车辆动力学理论，用相互交叉的5个圆对机动车进行简单的建模，其中每个圆的直径为m，在换道过程中通过实时计算换道车辆0与周边车辆的距离来检测是否可能发生碰撞，包括： (1)换道车辆0与本车道前车1的避免碰撞条件： k表示相互交叉的5个圆的编号；i为样本时间点；分别表示在不同时间样本点上包裹换道车辆0的五个圆圆心的横纵坐标；分别表示在不同时间样本点上包裹本车道前车1的第一个圆圆心的横纵坐标，为避免碰撞，要求在换道过程中两辆车的欧式距离大于圆的直径m； (2)换道车辆0与目标车道前车3及目标车道后车2的避免碰撞条件： r2≤(x0，i-x2，i)2+(y0，i-y2，i)2 i＝1，...，I r2≤(x0，i-x3，i)2+(y0，i-y3，i)2 i＝1，...，I x0，i，y0，i分别表示换道车辆0中心点的横纵坐标；x2，i，y2，i分别表示换道目标车道后车2中心点的横纵坐标；x3，i，y3，i分别表示目标车道前车3中心点的横纵坐标，为避免碰撞，要求在换道过程中换道车辆0与目标车道前车3及目标车道后车2的欧式距离大于车辆的对角线r。 7.根据权利要求1所述一种基于非线性规划的曲线道路自动车换道轨迹规划方法，其特征在于，步骤(4)，将轨迹规划问题转化为求解未知参数的非线性优化求解问题，具体为： (1)构造换道目标函数Jmin：其中，分别表示换道车辆0在不同时间点纵向和横向的加速度，分别表示换道车辆0在不同时间点纵向和横向的加加速度；T表示换道总时长，j2表示目标车道后车2的加加速度；ρ0，...，ρ5是目标函数中的权重参数； (2)考虑换道车辆0的跟驰行为约束： g0，I＝c(v0，I，v3，I，Δs) gmin≤g2，I≤c(v2，I，v0，I，Δd) g0，I，g2，I分别表示换道车辆0和目标车道后车2在换道结束状态时的瞬时加速度，v0，I，v2，I，v3，I分别表示车辆0、车辆2和车辆3在时刻T的瞬时速度，Δs，Δd分别表示车辆0与车辆3、车辆2与车辆0在时刻T的距离；为避免车辆突然从换道行为转变为跟驰行为时加速度变化太大，造成乘客不适性，考虑换道行为结束时，换道车辆0的瞬时加速度等于跟驰模型安全加速度；目标车道后车2的瞬时加速度大于最小加速度，小于等于跟驰模型安全加速度； (3)考虑车辆驾驶规范准则约束：分别表示换道车辆0在第i个时间点纵向和横向的速度；分别表示换道车辆0在第i个时间点纵向和横向的加速度；分别表示换道车辆0在第i个时间点纵向和横向的加加速度；分别表示换道车辆0在换道过程中的纵向和横向速度的上限；分别表示换道车辆0在换道过程中的纵向加速度的上下限；分别表示换道车辆0在换道过程中的横向加速度的上下限；分别表示换道车辆0在换道过程中的纵向加加速度的上下限；分别表示换道车辆0在换道过程中的横向加加速度的上下限；Tmax表示换道车辆0在换道过程中换道总时长的上限。 8.根据权利要求7所述一种基于非线性规划的曲线道路自动车换道轨迹规划方法，其特征在于，步骤(2)考虑换道车辆0的跟驰行为约束过程中： g0，I，v0，I，v3，I，Δs，v2，I，Δd按如下公式计算得到：计算换道车辆0在T时刻的加速度：计算换道车辆0在T时刻的速度：计算目标车道前车3在T时刻的速度：计算车辆3与车辆0在T时刻的距离： Δs＝s3，I-s(x0，I) 计算目标车道后车2在T时刻的速度：计算车辆2与车辆0在T时刻的距离： Δd＝s(x0，I)-s2，I 其中，是轨迹规划过程中需要优化求解的变量，s3，0，v3，0，a3，0，j3，0分别表示初始时刻车辆3的道路位置、速度、加速度和加加速度，s(x0，I)表示T时刻车辆0的道路位置，s2，0，v2，0，a2，0分别表示初始时刻车辆2的道路位置、速度和加速度，j2表示车辆2的在换道过程中的加加速度。 9.根据权利要求8所述一种基于非线性规划的曲线道路自动车换道轨迹规划方法，其特征在于，步骤(2)考虑换道车辆0的跟驰行为约束过程中：跟驰模型安全加速度计算公式按如下： c(v0，I，v3，I，Δs)＝κ{V1+V2tanh[C1(Δs-sc)-C2]-v0，I}+λ(v3，I-v0，I) c(v2，I，v0，I，Δd)＝κ{V1+V2tanh[C1(Δd-sc)-C2]-v2，I}+λ(v0，I-v2，I) 其中，κ，λ，sc，C1，C2，V1，V2为跟驰模型中预先设定的参数，为常量。 10.根据权利要求1所述一种基于非线性规划的曲线道路自动车换道轨迹规划方法，其特征在于，步骤(5)中采用序列二次规划算法对建立的非线性规划模型进行优化求解，在满足约束条件基础下快速得出换道车辆在不同时间样本点上的位置、速度和加速度，由此规划出相应的最优换道轨迹。
所属类别：	发明专利