一种商用铰接车辆自动驾驶规划轨迹完美演绎方法

技术领域

本发明属于商用车自动驾驶仿真技术领域，具体涉及一种商用铰接车辆自动驾驶规划轨迹完美演绎方法。

背景技术

自动驾驶规划轨迹需要在各类复杂交通场景下满足安全性、舒适性、竞技型、智能性和物理可行性等方面的要求。根据自动及杀死规划轨迹，可以确定在完美控制实现的情况下，可以确定在完美控制实现的情况下，与车身固连的参考轨迹点在什么时间以什么姿态出现在什么位置。虽然实际控制实现的轨迹与规划轨迹必然存在一定的偏差，但规划轨迹本身的优劣程度决定了自动驾驶控制执行效果的上限。在算法开发阶段，需要结合各类复杂交通场景，通过仿真平台对自动驾驶规划轨迹进行完美演绎，并对演绎结果进行可视化和数据后处理分析，进而对规划轨迹各项性能进行综合评价，指导规划算法的跌代更新。因此，自动驾驶规划轨迹的完美演绎方法构成了自动驾驶算法开发与测试的重要技术支撑。现有的自动驾驶规划轨迹完美演绎步骤如图1所示。在每个演绎循环周期内，具体步骤如下：

步骤1、自动驾驶规划轨迹被表达为规划参考点的离散时空轨迹点，包括三组基本曲线：规划参考点x坐标关于行驶距离s的曲线x(s)；规划轨迹参考点y坐标关于行驶距离s的曲线y(s)；行驶距离s关于时间t的曲线s(t)；

步骤2、对连续时空轨迹曲线按照等时间间隔进行离散化采样，每个离散点包括基本采样信息和补充采样信息，其中基本采样信息包括采样时刻t

步骤3、根据系统模拟的虚拟时钟信号，在离散参考规划轨迹点中通过二分法找到对应的参考区间t

步骤4、根据相邻参考点插值得到的当前时刻轨迹点参考信息，分别模拟计算当前车辆的零阶位姿信息、一阶和二阶运动信息，其中零阶位姿信息包括车辆位置坐标x、y和朝向h，一阶运动信息包括车辆的纵向速度v

步骤5、将车辆各阶运动学量的演绎结果输入到交通场景演绎程序中，交通场景演绎程序结合动态场景文件和静态地图信息，各个交通参与者产生与主车运动轨迹之间的交互行为，整个过程数据最终被输入到可视化与测评模块，供自动驾驶算法开发和测试人员进行分析评估。

最后，当演绎终止时间条件成立，循环退出，商用铰接车辆自动规划轨迹完美演绎过程结束。

现有自动驾驶规划轨迹完美演绎方法存在如下缺点：

(1)现有自动驾驶规划轨迹完美演绎方法中，主车适用于二轴乘用车，对于商用铰接车辆，现有方法由于无法估算铰接角度，无法对牵引车车体和挂车车体的运动学轨迹进行区别演绎。在转向行驶工况下，忽略铰接角的轨迹演绎结果是实际无法实现的，必然导致对规划轨迹效果的误判。

(2)现有方法对规划轨迹的表达方式要求严苛，必须是表达完整的时空规划轨迹，具有较大的局限性。现有方法无法对实际商用车自动驾驶中采用的“空间轨线+实时期望加速度”的规划轨迹表达方式进行完美演绎。

发明内容

本发明的目的在于，针对现有技术所存在的问题，提出一种商用铰接车辆自动驾驶规划轨迹完美演绎方法。

为了达到以上目的，本发明提供一种商用铰接车辆自动驾驶规划轨迹完美演绎方法，包括以下步骤：

步骤1、商用铰接车辆的自动驾驶规划轨迹通过空间轨线拟合系数a

步骤2、根据上一步牵引车实际朝向角h

步骤3、对空间轨线进行采样，0≤p≤1范围内对p进行N

步骤4、查找上一步车身实际规划参考点坐标x

步骤5、根据上一步车身规划参考点的实际行驶速度v

v

其中，T

步骤6、根据步骤4得到的上一时刻最近点路程距离s

步骤7、根据步骤6更新得到的s

步骤8、根据步骤7计算得到的车辆更新点参数p

步骤9、根据前述步骤5更新的车身规划参考点速度v

步骤10、根据当前规划参考点更新曲率c

步骤11、根据前述步骤9演绎的牵引车体各阶运动信息和步骤10演绎的铰接角各阶运动信息，对挂车车体质心位置相关的零阶位姿、一阶和二阶运动学量进行完美演绎；

步骤12、将步骤9和步骤11分别演绎得到的牵引车和挂车的各阶运动学量的输入到交通场景演绎程序中，并将整个演绎过程的数据输入到可视化与测评模块，供自动驾驶算法开发和测试人员进行分析评估。

本发明进一步的采用如下技术方案：

所述步骤1中，规划轨迹的具体表达方式如下：在每个控制执行周期，规划模块发出车身规划参考点的期望空间轨线(x(p),y(p))和当前时刻的期望加速度信息；车身规划参考点一般选取为牵引车驱动轴的中心点，期望空间轨线的坐标(x,y)描述在车辆自身的坐标系方向上，参数p在[0,1]之间连续取值，期望空间轨线的数学方程如下：

其中，a

所述步骤2中，转换方程如下：

转换后得到的全局坐标系下的空间轨线参数方程为：

所述步骤3中，采样得到的距离离散序列s

所述步骤4中，首先通过求解下面的数学优化问题求得最近点对应的参数p

然后通过步骤3构建的行驶距离s

s

其中，interp1D代表一维线性插值函数。

所述步骤7中，通过下式更新距离s

p

所述步骤8中，略去离散步索引号k和下标p，更新后参考路径点p

坐标值(x,y)关于参数p的0至4阶导数依次为：

切向方向计算：

h＝arctan2(y′,x′)

曲率计算：

曲率关于路程的一阶导计算：

曲率关于路程的二阶导计算：

所述步骤9中，演绎算法如下：

牵引车体零阶位姿包括牵引车质心位置坐标(x

其中，L

牵引车体一阶运动学量包括牵引车质心位置速度在自车坐标系方向下的分量(v

牵引车体二阶运动学量包括牵引车质心位置加速度在自车坐标系方向下的分量(a

其中，

所述步骤10中，估算过程如下：

假设整车圆周运动圆心位于前后车体质心处的垂线相交点处附近，依此为原则估算铰接角θ如下：

θ＝-(arctan(L

其中，铰接角θ为挂车相对牵引车绕铰接角转动的角度，L

铰接角的一阶变化率计算如下：

铰接角的二阶变化率计算如下：

所述步骤11中，演绎过程如下：

挂车车体零阶位姿包括挂车质心位置坐标(x

挂车车体一阶运动学量包括挂车质心位置速度在挂车坐标系方向下的分量(v

挂车车体二阶运动学量包括挂车车质心位置加速度在挂车坐标系方向下的分量(a

其中，

与现有技术相比，本发明的优点如下：

(1)本发明采用“空间轨线+实时期望加速度”方式进行完美演绎，本发明提出的规划轨迹演绎方法可以同时实现对牵引车体和挂车车体的零阶位姿和一、二阶运动信息完美演绎。在转向行驶工况下，本发明可以合理展现前后车体的位姿差异，方便实现对规划轨迹结果的准确可信评估。

(2)本发明对规划轨迹的演绎不依赖于表达完整的时空规划轨迹为输入，可以对实际商用车自动驾驶中通过“空间轨线+实时期望加速度”方式表达的“欠完整”规划轨迹进行完美演绎，实用性强。

(3)本发明提出的铰接角估算方法基于假设“整车圆周运动中心位于前后车体质心处的垂线相交点附近”，该方法不依赖于挂车车轴的数量及空间布置方式，对牵引车体的驱动和转向形式也不作限定，对于一般商用铰接车辆具有较强的普适性。

附图说明

下面结合附图对本发明作进一步的说明。

图1为现有自动驾驶规划轨迹完美演绎方法的流程图。

图2为本发明中商用铰接车辆的示意图。

图3为本发明中商用铰接车辆自动驾驶规划轨迹完美演绎的流程图。

图4为本发明中“借道避让”规划空间轨道线的示意图。

图5为本发明中实时期望加速度指令曲线的示意图。

图6为本发明中铰接角的演绎结果示意图。

图7为本发明中铰接角一阶变化率的演绎结果示意图。

图8为本发明中铰接角二阶变化率的演绎结果示意图。

图9为本发明中前后车体质心轨迹的演绎结果示意图。

图10为本发明中前后车体朝向角的演绎结果示意图。

图11为本发明中前后车体质心纵向速度的演绎结果示意图。

图12为本发明中前后车体质心横向速度的演绎结果示意图。

图13为本发明中前后车体横摆角速度的演绎结果示意图。

图14为本发明中前后车体质心纵向加速度的演绎结果示意图。

图15为本发明中前后车体质心横向加速度的演绎结果示意图。

图16为本发明中前后车体横摆角加速度的演绎结果示意图。

图17为本发明中前后车体形式轮廓印迹的示意图。

具体实施方式

本发明商用铰接车辆的构型如图2所示，铰接车辆的车体由牵引车和挂车两部分组成，二者通过平面铰接机构连接，平面铰接机构包括五根轴。具体为：牵引车具有前、后两个车轴，约定编号依次为第一轴和第二轴，第一轴为转向轴，第二轴为驱动轴；挂车车体所含的车轴均为从动非转向轴，本发明针对具有任意车轴数量的挂车车体均适用，且不限定挂车各车轴之间的排布方式，作为一个举例，图2中展示的挂车车体具有3个车轴，约定编号依次为第三轴、第四轴和第五轴。

本发明所述“空间轨线+实时期望加速度”方式定义的规划轨迹的具体表达方式如下：在每个控制执行周期，规划模块通过4次贝赛尔拟合曲线的方式发出车身规划参考点的期望空间轨线(x(p),y(p))和当前时刻的期望加速度信息a

其中，a

本发明提出的规划轨迹完美演绎方法适用于前述商用铰接车辆和“空间轨线+实时期望加速度”方式定义的规划轨迹。演绎的结果应该覆盖牵引车体和挂车车体的零阶到二阶的姿态和运动信息,为自动驾驶场景演绎、结果可视化和功性能评测提供输入。本发明提出的自动驾驶规划轨迹具体演绎步骤如图3所示，给出了本发明对规划轨迹进行完美演绎的过程。

在每个演绎循环周期内，具体步骤如下：

步骤1、“空间轨线+实时期望加速度”方式表达的规划轨迹，可通过10个空间轨线拟合系数a

步骤2、根据上一步牵引车实际朝向角h

其中，k为离散时间步序号；

转换后得到的全局坐标系下的空间轨线参数方程为：

步骤3、对空间轨线进行采样，0≤p≤1范围内对参数p进行N

其中，i为离散采样点序号，x

采样得到距离离散序列s

步骤3所述空间轨线采样方法建立了空间轨线各点位置参数与轨线距离之间的对应关系，结合二分法搜索和线性插值，方便二者之间的快速索引，构成步骤4和步骤7的重要前提。

步骤4、查找上一步车身实际规划参考点坐标x

然后通过步骤3构建的行驶距离s

s

其中，interp1D代表一维线性插值函数。

步骤5、根据上一步车身规划参考点的实际行驶速度v

v

其中，T

步骤6、根据步骤4得到的上一时刻最近点路程距离s

步骤5和步骤6基于实时期望加速度的二次积分对行驶车速和轨线距离进行演绎，实现了对空间域描述的轨线和时域描述的车辆运动之间的关联，这是实现对“空间轨线+实时期望加速度”方式描述的规划轨迹进行完美演绎的关键步骤。

步骤7、根据步骤6更新得到的s

p

步骤8、根据步骤7计算得到的车辆更新点参数p

坐标值(x,y)关于参数p的0至4阶导数依次为：

其中，x′为x关于参数p的一阶导数，y′为y关于参数p的一阶导数，x″为x关于参数p的二阶导数，y″为y关于参数p的二阶导数，x″′为x关于参数p的三阶导数，y″′为y关于参数p的三阶导数，x

切向方向计算：

h＝arctan2(y′,x′)

其中，h为切向方向角；

曲率计算：

其中，s′为距离坐标s关于参数p的一阶导数，h′为切向方向角h关于参数p的一阶导数，c为道路曲率；

曲率关于路程的一阶导计算：

其中，s″″为距离坐标s关于参数p的二阶导数，h″为切向方向角h关于参数p的二阶导数，c′为道路曲率c关于参数p的一阶导数，dc/ds为道路曲率c关于距离s的一阶导数；

曲率关于路程的二阶导计算：

其中，m为中间变量，是为了简化公式而引入的，s″′为距离坐标s关于参数p的三阶导数，h″′为切向方向角h关于参数p的三阶导数，c″为道路曲率c关于参数p的二阶导数，(dc/ds)′为dc/ds关于参数p的一阶导数，d

步骤8推导了从4次贝塞尔曲线求解规划参考点坐标、朝向、曲率及其一、二阶变化率的数学方程，构成步骤9至11的数据基础。

步骤9、根据前述步骤5更新的车身规划参考点速度v

牵引车体零阶位姿包括牵引车质心位置坐标(x

其中，h

牵引车体一阶运动学量包括牵引车质心位置速度在自车坐标系方向下的分量(v

其中，ω

牵引车体二阶运动学量包括牵引车质心位置加速度在自车坐标系方向下的分量(a

其中，α

步骤9基于步骤5更新的车身规划参考点运动信息和步骤8计算得到的空间轨线参考点出的位姿几何信息，实现对牵引车体质心位置处的零阶位姿、一阶和二阶运动学量的计算，是本发明演绎结果的主要呈现之一。

步骤10、根据当前规划参考点更新曲率c

假设整车圆周运动圆心位于前后车体质心处的垂线相交点处附近，依此为原则估算铰接角θ如下：

θ＝-(arctan(L

其中，铰接角θ定义为挂车相对牵引车绕铰接角转动的角度，逆时针转动为正，L

铰接角的一阶变化率

铰接角的二阶变化率

其中，

步骤10基于整车圆周运动圆心位于前后车体质心处的垂线相交点附近的假设，提出了一种对商用铰接车辆普遍适用的铰接角及其一、二阶变化率估算方法，是本发明实现挂车车体运动演绎的重要前提。

步骤11、根据前述步骤9演绎的牵引车体各阶运动信息和步骤10演绎的铰接角各阶运动信息，对挂车车体质心位置相关的零阶位姿、一阶和二阶运动学量进行完美演绎，演绎过程如下：

挂车车体零阶位姿包括挂车质心位置坐标(x

其中，h

挂车车体一阶运动学量包括挂车质心位置速度在挂车坐标系方向下的分量(v

其中，ω

挂车车体二阶运动学量包括挂车车质心位置加速度在挂车坐标系方向下的分量(a

其中，

步骤11根据步骤9演绎的牵引车体各阶运动信息和步骤10演绎的铰接角各阶运动信息，实现对挂车车体质心位置处的零阶位姿、一阶和二阶运动学量的计算，是本发明演绎结果的主要呈现之一。

步骤12、将步骤9和步骤11分别演绎得到的牵引车和挂车的各阶运动学量的输入到交通场景演绎程序中，交通场景演绎程序结合动态场景文件和静态地图信息，各个交通参与者产生与铰接主车之间的交互行为，整个过程数据最终被输入到可视化与测评模块，供自动驾驶算法开发和测试人员进行分析评估。

最后，当演绎终止时间条件成立，循环退出，商用铰接车辆自动驾驶规划轨迹完美演绎过程结束。

实施例一

为了展示本发明提出的商用铰接车辆规划轨迹完美演绎方法的效果，本实施例以图2所示某实际商用铰接车为原型进行演示，演绎装置中用到的车辆几何参数值见表1。

表1铰接车辆几何参数示例

为方便演示，选取一组代表车辆“借道避让”工况的空间轨线作为前述演绎装置的输入，初始车速设定为10km/h,同时通过一个虚拟速度规划器实时调节期望加速度，保障铰接车辆以符合物理约束和安全需要的车速完成快速“借道避让”行驶。“借道避让”空间轨线对应的拟合系数见表2，对应的空间轨线见图4，整个演绎过程虚拟速度规划器输出的期望加速度指令曲线见图5。

表2“借道避让”空间轨线拟合系数

上述设定明确了原型车辆的几何参数和一条通过“空间轨线+实时期望加速度”方式定义的商用铰接车辆自动驾驶规划轨迹，通过本发明提出的规划轨迹完美演绎装置的演绎，可以得到牵引车、挂车和铰接角的各阶运动过程信息。图6、图7、图8分别展示了铰接角及其一阶、二阶变化率的演绎结果。图9至图17充分展示了牵引车和挂车的各阶运动信息的演绎结果。其中，图9对比展示了牵引车质心、挂车质心和规划参考点对应的平面运动轨迹；图10对比展示了两车体的朝向角变化；图11对比展示了两车体质心纵向速度变化；图12对比展示了两车体质心横向速度变化；图13对比展示了两车体横摆角速度变化；图14对比展示了两车体质心纵向加速度变化；图15对比展示了两车体质心横向加速度变化；图16对比展示了两车体横摆角加速度变化；图17展示了牵引车体和挂车车体的矩形轮廓在平面上驶过的印迹。

综合图6-图17可以看出，本发明提出的商用铰接车辆规划轨迹完美演绎方法可以实现对牵引车和挂车车体运动轨迹的完美演绎，演绎结果能够满足规划轨迹的“借道避让”预期，且符合前后双车体之间的铰接约束关系。本发明提出的规划轨迹完美演绎方法可以为商用铰接车辆自动驾驶规划轨迹的可视化和功/性能评测提供有力依据。

除上述实施例外，本发明还可以有其他实施方式。凡采用等同替换或等效变换形成的技术方案，均落在本发明要求的保护范围。