一种面向无人驾驶的拟人化参考轨迹规划方法与流程

本发明属于无人驾驶汽车导航领域，尤其涉及一种面向无人驾驶的拟人化参考轨迹规划方法。

背景技术：

无人车导航模块综合了感知的结果，并考虑车辆动力学特性，输出一条可以行驶的轨迹。在城市结构化道路的完全自动驾驶汽车导航中，导航任务一般分为三层：任务规划层、行为决策层和轨迹规划层。轨迹规划层一般分两步进行：不考虑移动障碍物的参考轨迹生成和考虑移动障碍物的局部轨迹规划。对于轨迹规划问题，在路口这样复杂的场景中，参考轨迹尤其重要，它弥补了上层任务规划的不完整性，同时利用地理信息降低感知难度，而且增加了最终行车轨迹的全局最优性。

目前，路口转向参考轨迹的生成方法尚不完善，一般用直线连接、圆弧连接或在地图上手动画出虚拟车道线的方式生成，对汽车动力学特性、人类驾驶员驾驶习惯以及乘坐舒适性等因素的考虑尚不深入。另外，手动画线方式本身效率不高，不适于无人车大范围的行驶导航。因此，现在亟需一种适用于解决多种路口内部多种转向情形下的参考轨迹自动规划方法，同时考虑汽车动力学特性、人类驾驶员驾驶习惯和乘坐舒适性等因素，提高轨迹规划的合理性。

技术实现要素：

针对上述问题，本发明的目的是提供一种面向无人驾驶的拟人化参考轨迹规划方法，适用于多种路口内部、多种转向情形下的参考轨迹自动规划，同时考虑汽车动力学特性、人类驾驶员驾驶习惯和乘坐舒适性等因素，提高轨迹规划的合理性，为进一步的无人车控制提供目标轨迹。

为实现上述目的，本发明采取以下技术方案：一种面向无人驾驶的拟人化参考轨迹规划方法，包括以下步骤：

1)建立车辆转向模型，根据车辆基本参数，生成车辆转向时的动力学约束；

2)根据车辆上层决策获取道路的地图信息，根据地图信息得到参考轨迹的起终点约束信息；

3)根据车辆转向时的动力学约束和参考轨迹的起终点约束信息，生成若干条基本满足车辆转向的几何学和动力学约束且符合人的驾驶习惯和转向规律的转弯轨迹曲线，形成基本可行轨迹簇；

4)以横向加速度对时间的积分为指标，从基本可行轨迹簇中选择最为舒适的一条作为最终生成的最佳路口内参考行驶轨迹。

所述步骤1)中的车辆转向模型为：

式中，π为转弯轨迹曲线；s为转弯轨迹曲线的任意弧长；l为转弯轨迹曲线π的长度。

所述步骤1)中的车辆基本参数包括车辆轴距l、外侧转向轮最大转角β和转弯轨迹曲线曲率变化率最大值σmax；

车辆转向时的动力学约束为：

κ(s)≤κmax，s∈[0,l]

式中，κ(s)为转弯轨迹曲线任意弧长s处的曲率；κmax为转弯轨迹曲线曲率的最大值；为转弯轨迹曲线曲率的导数。

所述步骤2)中的车辆上层决策是指给出从“入车道”进入路口，后经“出车道”驶出路口的决策；道路的地图信息包括入车道结束中心点pi的位置和方向信息pi＝(xi，yi，θi)，以及出车道起始中心点pe的位置和方向信息pe＝(xe，ye，θe)；其中，xi、yi为入车道结束中心点的经纬度坐标，θi为入车道结束中心点处的理想行车方向；xe、ye为出车道起始中心点的经纬度坐标，θe为出车道起始中心点处的理想行车方向；得到的参考轨迹的起终点约束信息为：π(0)＝pi且π(l)＝pe。

所述步骤3)中基本可行轨迹簇的生成，具体包括以下步骤：

①根据车辆转向时的动力学约束和参考轨迹的起终点约束信息，确定转向轨迹曲线参数l、l1、l2、κmax的求解空间为：

l1＝λ1l,l2＝λ2l且0≤λ1≤λ2≤1

0≤κmax≤σmax·max(l1,l-l2)

式中，l1和l2分别为转弯轨迹曲线中曲率增加段和曲率减小段的长度；λ1和λ2分别为转弯轨迹曲线中曲率增加段和曲率减小段占转弯轨迹曲线总长度的比例；

②建立转向轨迹曲线终点处的车辆车身方向角以及横坐标和纵坐标的计算公式；

③根据转向轨迹曲线终点处的车辆车身方向角以及横坐标和纵坐标的计算公式，计算求解空间中的点(li,l1i，l2i，κmaxi)的终点处的车辆车身方向角及横纵坐标θei，xei，yei；如果θei，xei，yei在出车道中心点信息θe，xe，ye的可行领域内，则将该曲线留在曲线簇中，最终得到一定数量曲线的基本可行轨迹簇。

所述步骤②中的转向轨迹曲线终点处的车辆车身方向角计算公式为：

转向轨迹曲线终点的横坐标和纵坐标的计算公式分别为：

式中，θ(s)为转弯轨迹曲线任意弧长s处的方向角。

所述步骤③中的可行领域是指转向轨迹曲线的终点位置加上半个车身的宽度不能超出车道实线，且终点处的车身方向角不能超过限定值。

所述步骤4)中横向加速度对时间的积分为：

式中，k为基本可行轨迹簇中转向轨迹曲线的曲率；v为车速；s为转弯轨迹曲线的任意弧长；l为转弯轨迹曲线的长度；最为舒适即横向加速度对时间的积分a值最小。

本发明由于采取以上技术方案，其具有以下优点：1、本发明的一种面向无人驾驶的拟人化参考轨迹规划方法，生成的参考轨迹与上下游路段连接处曲率连续，从而保证轨迹追踪的可行性，降低控制的难度。2、本发明的一种面向无人驾驶的拟人化参考轨迹规划方法，考虑不同车辆的转向特性，保证轨迹适用于不同车型车辆的追踪控制。3、本发明的一种面向无人驾驶的拟人化参考轨迹规划方法，考虑车辆动力学特性，尤其考虑乘坐感受，提高了轨迹追踪时的舒适性。4、本发明的一种面向无人驾驶的拟人化参考轨迹规划方法，考虑人的驾驶特性，模拟人的驾驶规律，生成的轨迹符合人的驾驶习惯。

附图说明

图1是本发明方法的流程示意图；

图2是四轮汽车转向时车辆姿态及转弯轨迹示意图；

图3是交叉路口地图道路信息示意图；

图4是入车道结束中心点信息示意图；

图5是理想的行驶轨迹示意图；

图6是转向曲率模型示意图；

图7是转向过程示意图；

图8是基本可行轨迹簇示意图。

具体实施方式

下面结合附图和实施例对本发明进行详细的描述。

如图1所示，本发明提供的一种面向无人驾驶的拟人化参考轨迹规划方法，适用于无人车在路口场景下的路口内部参考行驶轨迹的规划，具体包括以下步骤：

1)车辆转向建模：建立车辆转向模型，根据车辆基本参数，生成车辆转向时的动力学约束。

如图2所示，转弯轨迹曲线π为车辆后轴中点p扫过的轨迹，在点p处，转弯轨迹曲线π的切线与车身方向平行，瞬时半径ρ即为此刻的车辆转弯半径。采用曲率-弧长参数来表示转弯轨迹曲线π，路口内参考行驶轨迹的规划问题可归结为计算转弯轨迹曲线其中，s为转弯轨迹曲线的任意弧长，l为转弯轨迹曲线π的长度。

转弯轨迹曲线π应满足车辆转向时的动力学约束。其中，车辆转弯过程中，由于转向机构的限制，车辆转弯半径不能小于最小转弯半径rmin，即转弯轨迹曲线的曲率不应超过最大值κmax，因此有κ(s)≤κmax，s∈[0,l]，其中，κ(s)为转弯轨迹曲线任意弧长s处的曲率。

设车辆后轴中点p的坐标为(x，y)，车身方向与x轴夹角为θ；获取车辆轴距l和外侧转向轮最大转角β，可以生成转向约束中的最大曲率为：

考虑到实际转向过程中转向角不能发生突变，这要求转弯轨迹曲线曲率的导数存在且连续。事实上，转向轮转动的速度是受限的，转弯轨迹曲线曲率变化率存在最大值σmax(由经验得到)，因此有s∈[0,l]，其中，为转弯轨迹曲线曲率的导数。

2)地图信息获取：根据车辆上层决策获取道路的地图信息，根据地图信息得到参考轨迹的起终点约束信息。

如图3所示，当车辆上层决策给出从“入车道”进入路口，后经“出车道”驶出路口时，从地图中获取的道路信息应包括入车道结束中心点pi的位置和方向信息pi＝(xi，yi，θi)，以及出车道起始中心点pe的位置和方向信息pe＝(xe，ye，θe)；其中，xi、yi为入车道结束中心点的经纬度坐标，θi为入车道结束中心点处的理想行车方向；xe、ye为出车道起始中心点的经纬度坐标，θe为出车道起始中心点处的理想行车方向。如图4所示，入车道结束中心点处的理想行车方向θi定义为垂直于车道结束线指向交叉口内部的向量与正北方向所呈逆时针夹角，出车道起始中心点处的理想行车方向θe定义与入车道结束中心点处的理想行车方向θi定义类似。

如图5所示，理想的行驶轨迹假定汽车沿入车道结束中心点pi沿方向θi进入交叉口，经历转向过程后，沿出车道起始中心点pe沿方向θe驶出交叉口，因此有起终点限制条件为：π(0)＝pi且π(l)＝pe。

3)基本可行轨迹簇生成：根据车辆转向时的动力学约束和起终点限制条件，生成若干条基本满足车辆转向的几何学和动力学约束且符合人的驾驶习惯和转向规律的转弯轨迹曲线，形成基本可行轨迹簇。

首先，以入车道结束中心点为坐标原点，入车道中心点的入方向为y轴，将出车道起始中心点的经纬度坐标转化为笛卡尔坐标，相关方法已很成熟，这里不赘述。

然后，生成考虑人的驾驶习惯和转向规律的连续曲率转弯轨迹曲线。人的驾驶习惯可以描述为：在车辆进入交叉口时，开始转动方向盘；方向盘到达一定角度后，保持方向盘转角一段时间，如交叉口较小，该段时间会缩短或不存在；在车辆即将驶出交叉口时，回正方向盘至方向盘转角为0。如图6所示，为了模拟这一转向过程，同时不增加转弯轨迹曲线的复杂程度，本发明的曲率模型将转向过程分为三段：第一段中，转弯轨迹曲线曲率从0开始随弧长增加均匀增加，对应车辆进入交叉口的逐渐转动方向盘过程；第二段中曲率到达最大值，并保持一段弧长，对应车辆经过交叉口的保持方向盘转角过程；第三段中曲率均匀减小到0，对应转向过程最后阶段方向盘逐渐回正的过程。各段对应的转向轨迹如图7所示。

给定转弯过程中的参数，即唯一确定转弯轨迹曲线。适当放开入车道结束中心点的约束，即终点限制条件π(l)＝pe转化为π(l)在pe的一定矩形范围中，这样即会得到一个参考轨迹曲线的基本可行轨迹簇。

基于上述原理，基本可行轨迹簇的生成，具体包括以下步骤：

①根据车辆转向时的动力学约束和起终点限制条件，确定转向轨迹曲线参数l、l1、l2、κmax的求解空间：

l1＝λ1l,l2＝λ2l且0≤λ1≤λ2≤1

0≤kmax≤σmax·max(l1,-l2)

式中，l1和l2分别为转弯轨迹曲线中曲率增加段和曲率减小段的长度；λ1和λ2分别为转弯轨迹曲线中曲率增加段和曲率减小段占转弯轨迹曲线总长度的比例。

②建立转向轨迹曲线终点处的车辆车身方向角以及横坐标和纵坐标的计算公式：

由曲率的定义可知，转向轨迹曲线任意弧长s处的切线方向为：

因此在转向轨迹曲线的终点l处的车辆车身方向角为：

则转向轨迹曲线终点的横坐标和纵坐标分别为：

式中，θ(s)为转弯轨迹曲线任意弧长s处的方向角。

③根据转向轨迹曲线终点处的车辆车身方向角以及横坐标和纵坐标的计算公式，计算求解空间中的点(li,l1i，l2i，κmaxi)在终点处的车身方向角及横纵坐标θei，xei，yei；如果θei，xei，yei在出车道中心点信息θe，xe，ye的可行领域内，则将该曲线留在曲线簇中；最终得到一定数量曲线的基本可行轨迹簇，如图8所示。

其中，可行领域是指转向轨迹曲线的终点位置加上半个车身的宽度不能超出车道实线，且终点处的车身方向角不能超过限定值；限定值根据经验一般取±30°。

4)最优轨迹选取：以横向加速度对时间的积分为指标，从基本可行轨迹簇中选择最为舒适的一条作为本发明最终生成的参考行驶轨迹。

转向轨迹曲线上任意一点的横向加速度为：

at＝kv²

则横向加速度对时间的积分为：

式中，k为基本可行轨迹簇中转向轨迹曲线的曲率；v为车速，一般取平均车速，比如30km/h。

遍历基本可行轨迹簇中所有的转向轨迹曲线，选择a值最小的转向轨迹曲线，即得到最终的最佳路口内参考行驶轨迹。

上述各实施例仅用于说明本发明，其中各部件的结构、设置位置及其连接方式等都是可以有所变化的，凡是在本发明技术方案的基础上进行的等同变换和改进，均不应排除在本发明的保护范围之外。