基于能量函数的智能车辆路径曲率平滑方法研究

师强, 杨明

上海交通大学自动化系，上海 200240

师强,杨明. 基于能量函数的智能车辆路径曲率平滑方法研究[J]. 智能科学与技术学报, 2020, 2(2): 161-168.

SHI Q, YANG M. Research on path curvature smoothing method based on energy function for intelligent vehicles[J]. Chinese Journal of Intelligent Science and Technology, 2020, 2(2): 161-168.

1.引言 

智能车辆是指通过搭载先进传感器等装置，运用人工智能等新技术，具有自动驾驶功能，并逐步成为智能移动空间和应用终端的新一代车辆。智能车辆在预防交通事故、缓解交通拥堵、减少排放等社会功效方面具有重要意义，是当今国内外的研究热点。智能车辆的关键系统模块包括定位与建图、环境感知、规划与决策、车辆控制、人机交互等。路径跟踪指通过自动转向控制使车辆始终沿着期望路径行驶，它是车辆控制模块的重要组成部分。路径跟踪的性能包括乘坐舒适度和控制精度，是智能车辆品质的重要表现。路径曲率波动会引发路径跟踪控制的稳态前轮转角波动，这种波动会造成乘坐舒适度和控制精度降低。某些场景（如园区内部）下道路的建设具有任意性，其道路本身曲率平滑性较差，进而会影响路径跟踪性能。

针对路径曲率平滑优化问题，参考文献与贝塞尔曲线类似，但相比于贝塞尔曲线，优化了局部可调能力。多项式曲线、贝塞尔曲线和B样条曲线均可以构造出曲率连续的曲线，但构造方法割裂了横坐标和纵坐标对曲率平滑性的耦合影响，且无法说明最优性。回旋曲线的曲率平滑性良好，通常用于公路的缓和曲线，但对于边界位置方向曲率固定的路径来说，往往无法拟合出一条回旋曲线。回旋曲线的推广方法有两种，第一种是分段回旋曲线，连续曲率（continuous-curvature，CC）曲线满足给定最大曲率和最大曲率变化率约束，但无法进一步优化；参考文献提出了一种对称的分段回旋曲线，其平滑的曲线边界必须同向且曲率为零，应用的场景是直道变道场景，参考文献提出的分段回旋曲线方法无法对边界的曲率进行约束，适用于自动泊车这类极低速场景。第二种是多项式螺旋线，多项式螺旋线可以看作高次的回旋曲线，3 次及以上多项式螺旋线可以拟合边界位置方向曲率固定的曲线，参考介绍了一种数值迭代的求解方法，但求解过程复杂，有时无法收敛。

为进一步提升路径曲率平滑性，本文提出了一种基于能量函数的曲率平滑方法，该方法通过构造量化指标能量函数表征路径曲率平滑程度，基于离散化路径描述设计迭代规则，使能量函数下降；并通过增加道路边界约束来确保路径可通过性。对比实验证明，所提方法能够进一步提升路径跟踪的乘坐舒适度和控制精度。

2. 构造能量函数

能量函数能够从时间维度或空间维度反映事物的某种总体特征，以路径曲率变化率的平方和构造能量函数E，当路径是一条连续无限密集曲线时，能量函数为：

其中，l是路径上一点到起点的距离，c(l)是路径曲率，L是路径总长度，c′(l)是c(l)的导数。

E的特征为E≥0。E越小，路径曲率越平滑；当E为0时，路径为恒曲率圆弧，路径曲率平滑性达到最优。曲率优化是路径形状不断调整的过程，实际场景下一般路径很难用解析式表达出来，通常使用等距离散点列表示路径，路径曲率平滑优化的过程是离散点列不断调整位置的过程，等距离散化路径描述下的E为：

其中，k是路径离散化后的离散点序号，c_k是第k点的曲率，Δl是相邻离散间距，n是离散点总数。

把弧长增量近似为离散点的间距，方向增量近似为离散点指向下个点的方向的增量，曲率为：

其中，是第k-1点指向第k点的方向。

可以将特定等距离散化路径Δl认为是常量，将式（3）代入式（2），并且提取出Δl，最终构造能量函数如下：

3. 设计迭代规则

式（4）表示路径曲率平滑性的前提是路径离散等距化，迭代规则应在保证离散点等距的同时使E下降。设计迭代规则如图1所示，其中P_k-3至P_k+3是路径上相邻的7个离散点，第4个点P_k的移动会改变P_k附近路径的曲率分布，进而改变E，故存在P_k的最优位置使得当只有P_k移动到最优位置时E最小，为保证离散点的等距性，应在P_k-1P_k+1的中心法线上。

设P_k-1与P_k-1P_k+1间夹角为θ，可得P_k-1与P_k+1P_k-1间夹角为θ，P_k+1与P_k-1之间夹角为2θ。设Δθ₁、Δθ₂、Δθ₃、Δθ₄分别为P_k-1P_k-2与P_k-2P_k-3 、P_k+1P_k-1与P_k-1P_K-2、P_K+2P_K+1与P_K+1P_K-1、P_K+3P_K+2与P_K+2P_K+1间的夹角，Δθ₁、Δθ₂、Δθ₃、Δθ₄与θ无关， E可表示为式（5），其中省略号中的项不受θ的影响，不包含θ。

把E视作θ的函数E(θ)，当E′(θ)为 0 时， E(θ)取到最小值。此时θ满足式（6）：

其中，E′(θ)为E(θ)的导数。

由式（6）的θ可以确定的位置。

每次从路径上取相邻7个点均可优化第4个点的位置，进而使E下降，对路径上所有可优化的点进行一次优化为一次迭代，多次迭代即可实现路径曲率的平滑优化。对于离散化曲线来说，1 个点可以表示位置，2 个点可以表示方向，3 个点可以表示曲率，而非闭合曲线边界处均有3个点不会被优化（即不会被改变位置），这表示能量函数法会保存边界处的位置、方向、曲率，符合实际场景下的约束；对于闭合曲线来说，不存在边界概念，所有的点均为可优化点。

图2和图3分别表示两种典型路径下能量函数法的平滑曲率效果，迭代过程中的路径及其曲率变化用蓝线、橙线、黄线、紫线、绿线、青线表示，其中蓝线表示原始的路径及其曲率。图2（a）的原始路径是一条由直道、1/4 圆弧、直道依次相连的非闭合路径，从图2（b）中可以看出原始路径的曲率存在阶跃突变，经过平滑后，路径曲率阶跃突变消失，同时边界处的位置、方向、曲率得以保存。图3（a）是一个正方形闭合路径，4 个角方向不连续，表现为图3（b）中曲率分布存在4个尖峰脉冲，并且由于路径离散化作用，4个尖峰脉冲为有限幅值，随着迭代次数的增加，图3（b）中的尖峰不断平滑直至消失，路径从正方形（蓝线）平滑为圆形（青线）。

4.增加道路边界约束

车辆在实际场景中不应出现蹭路沿或压车道线的情况，路径的可通过性优于曲率的平滑性，对路径进行曲率平滑操作时应增加道路边界约束。每次对点进行平滑操作前先判定约束规则，约束规则分为道路左边界约束和道路右边界约束，这里以追踪左边界为例进行说明。如图4所示，对P_k进行曲率平滑操作时，求其最优位置，由P_K-2、P_K-1、、P_K+1、P_K+2组成点列P，并求取每个点的方向、曲率，由点的位置、方向、曲率可以唯一确定车的位置，在车体周围设置矩形包围盒ABCD，包围盒的设置应充分考虑轮胎摆动幅度、凸出传感器的安放位置，并且留有安全余量。P上每一个点都有唯一确定的包围盒ABCD与之对应。将道路左边界离散成一列点，并提取部分靠近P的点列edge，若edge中存在点edgei满足式（7）中的两个条件，表示道路左边界上至少有一点在包围盒内部，说明本次移动点会导致车辆通过道路边界外，舍弃本次移动点操作，继续对下一个点进行操作。

5.实验结果与分析

测试道路场景有两个，如图5、图6所示，场景一的起点在图5（a）右下角处，场景二的起点在图6（a）左上角处。道路中心线满足路径可通过性，同时在道路左右边界已知的情况下容易提取，以道路中心线为原始路径。图5（b）、图6（b）分别是两条道路中心线的曲率分布，可以看出其曲率平滑性均很差。两个道路场景均为普通园区常见的道路。

使用3次B样条曲线、3次多项式螺旋线和能量函数法平滑曲线对道路中心线进行平滑处理。3次B样条曲线根据不同离散点间距可分为多条，能量函数法分为无道路边界约束的平滑曲线和有道路边界约束的平滑曲线，平滑结果如图7~图10所示。实验场景下点离散间距越大，3次B样条曲线平滑效果越好，如图7、图8中的橙线、黄线、紫线和图9、图10 中的橙线、黄线。离散点间距越大，原始路径形成的控制点对平滑后曲线的曲率分布影响越小，但这种影响会持续存在。另外，3次B样条曲线平滑过程无法施加道路边界约束，随着离散点间距增大，路径可通过性可能丧失。3 次多项式螺旋线平滑效果良好，如图7~图10中的绿线，但同样无法施加道路边界约束，在本实验的两个道路场景下均不满足路径可通过性。能量函数法平滑效果良好，无道路边界约束的能量函数平滑曲线（青线）平滑效果与3次多项式螺旋线（绿线）相似，有道路无道路边界约束的能量函数平滑曲线（黑线）平滑效果虽比无道路边界约束的能量函数平滑曲线（青线）差，但由于迭代机制的存在，在满足路径可通过性的前提下有道路边界约束的能量函数平滑曲线可以达到较好的平滑效果。

测试车辆如图11 所示，定位传感器为RTK-GNSS，轴距为2.1 m，控制器输出频率为20 Hz。道路场景一中使用不做平滑处理的道路中心线、点离散间距4 m的3次B样条曲线、有道路边界约束的能量函数平滑曲线作为路径，道路场景二中使用不做平滑处理的道路中心线、点离散间距2 m的3次B样条曲线、有道路边界约束的能量函数平滑曲线作为路径，每个道路场景分别使用3.5 m/s和5 m/s的车速进行测试。测试结果如图12、图13所示。

图12 是傅里叶变换后的前轮转角幅频特性曲线，幅频特性曲线频率越高表示前轮转角波动越剧烈，幅值越大表示前轮转角波动幅度越大，轮转角波动越剧烈和波动幅度越大均会导致车辆乘坐舒适度降低，本文把车辆乘坐舒适度分为最低、较低、中等、较高、最高 5 类。按路径和车速把图12 分为4幅子图，每幅子图比较道路中心线（蓝线）、3次B样条曲线（橙色）、能量函数平滑曲线（黄色）的测试结果。在图12（a）中，频率小于0.1 Hz时3种方法幅频特性曲线表现相似，频率大于0.1 Hz时不做平滑路径（即道路中心线）的幅频特性曲线出现了两个尖峰，评其乘坐舒适度为最低；3 次B 样条曲线出现了一个尖峰，幅值比不做平滑路径的第一个尖峰小一半，评其乘坐舒适度为较高；能量函数平滑曲线没有出现尖峰，评其乘坐舒适度为最高。由图12（b）可以得出与图12（a）相同的结论。图12（c）中3种方法的幅频特性曲线均出现了一个尖峰，不做平滑路径的峰值和峰值频率最高，评其乘坐舒适度为最低；3次B样条曲线的峰值小于不做平滑路径的峰值，其峰值频率接近不做平滑路径的峰值频率，评其乘坐舒适度为较低；能量函数平滑路径的峰值和峰值频率为最低，评其乘坐舒适度为最高。由图12（d）可以得出与图12（c）相同的结论。

图13 是横向偏差的测试结果，横向偏差是车辆到路径的横向距离，是控制精度的重要参考，横向偏差越小表征控制精度越高。按道路场景和车速把图13 分为 4 幅子图，每幅子图比较道路中心线（蓝线）、3次B样条曲线（橙线）、能量函数平滑曲线（黄线）的测试结果，提取其中的横向偏差正向最大值、横向偏差负向最大值、横向偏差波动幅度并汇总，见表1。

表1汇总了图12和图13的部分数据和结论，其中加黑字体表示横向对比性能最优。可以看到，在所有测试场景车速下能量函数平滑方法的乘坐舒适度最高。横向偏差负向最大值、横向偏差正向最大值、横向偏差波动幅度均反映了控制精度，且越接近0表示控制精度越好，在道路场景一、车速5 m/s 下，能量函数平滑方法横向偏差正向最大值优于3次B样条曲线平滑方法；横向偏差负向最大值和横向偏差波动幅度弱于3次B样条曲线平滑方法，单看此场景车速无法说明能量函数平滑方法的控制精度优于3次B样条曲线平滑方法，原因在于道路场景一下的3次B样条曲线平滑方法的曲率分布十分接近能量函数平滑方法，如图8的黄线和黑线，这种接近具有偶然性，并且由于随机扰动的存在，导致能量函数平滑方法的横向偏差负向最大值比3次B样条曲线平滑方法大3 cm（绝对值比较），能量函数平滑方法的横向偏差波动幅度比3次B样条曲线大2 cm。在除场景一、车速5 m/s外的其他3 个场景、车速下，能量函数平滑方法的控制精度均优于3次B样条曲线平滑方法，在场景二中这种优越性更加明显，表现为车速为 3.5 m/s 时能量函数平滑方法横向偏差波动幅度比3次B样条曲线平滑方法提升约 2 倍，车速为 5m/s 时能量函数平滑方法横向偏差波动幅度比3次B样条曲线平滑方法提升约4倍。图12、图13和表1说明了能量函数平滑方法提升乘坐舒适度和控制精度的有效性。

6.结束语

本文从路径优化角度提升智能车辆路径跟踪性能，提出了一种基于能量函数的曲率平滑方法，实验结果表明相对于现有方法，所提方法能在确保路径可通过的前提下进一步提升乘坐舒适度和控制精度，提升智能车辆的品质，未来会进一步在多车多场地下验证该方法的通用有效性。

作者简介 About authors

师强（1995–），男，上海交通大学自动化系硕士生，主要研究方向为智能车辆的定位与控制 。

杨明（1975–），男，博士，上海交通大学自动化系教授，上海交通大学智能车实验室负责人，主要研究方向为智能车辆的定位、感知、导航、控制、自主驾驶、辅助驾驶和协作驾驶。