CN108710294B - 一种无人车路径跟踪前馈控制方法 - Google Patents

Abstract

本发明提供了一种无人车路径跟踪前馈控制方法。该方法在传统车辆路径跟踪系统模型中加入道路曲率作为干扰输入项，给出了加入曲率干扰之后的系统模型状态空间表达式。针对该模型，以横向位置偏差趋于零作为目标函数设计了一种前馈控制方法。本发明算法简单，不仅适用于仿真技术领域，还能够在实车嵌入式控制器上实现，应用前景好。

Description

一种无人车路径跟踪前馈控制方法

技术领域

本发明涉无人车自动控制领域，尤其涉及一种无人车路径跟踪前馈控制方法。

背景技术

无人驾驶汽车路径跟踪控制是指无人车横向控制，即通过自动转向控制，使车辆始终自主沿期望路径行驶，同时保证车辆的行驶安全性和乘坐舒适性。针对路径跟踪的精确控制是无人驾驶汽车自主运行稳定的前提。前馈控制是指通过预测未来将要产生的扰动，根据扰动大小提前采取控制措施，以补偿扰动对被控变量的影响，使被控变量不会因扰动作用而产生偏差。

目前，国内外许多研究者针对路径无人车路径跟踪控制，通常将横向位置偏差与航向偏差作为输入，使用反馈控制方法实现无人驾驶汽车完成对路径的跟踪：美国卡内基梅隆大学机器人研究所提出了一种纯跟踪控制方法，其实质是一种将自身位置与预瞄处期望位置的横向偏差转化为转向控制量的反馈控制，这种方法仅考虑横向位置偏差的反馈，控制系统受道路曲率干扰的影响较大。Salvuccia等提出的“预瞄-补偿跟踪”控制方法通过预瞄近点与远点来控制转向，即通过注视近点保持车辆行驶在道路中心，通过注视远点补偿道路曲率变化。但该方法仅通过预瞄单个远点来补偿道路曲率变化，参考路径信息不足，不能对前方道路曲率进行准确预估，且没有考虑神经反应迟滞以及人体动作机制，造成一定的预测误差。Taewan Kim提出的基于高斯稀疏过程在线学习的改进性模型预测控制(MPC)是一种特殊的最优控制方法，控制效果较好，但由于其算法复杂程度较高，使得该方法只能适用于仿真技术领域，难以在实车嵌入式控制器上实现。

发明内容

为解决上述问题，本发明提供了一种无人车路径跟踪前馈控制方法，包括如下步骤：

步骤一:按照如下公式将所有期望路径点转化到车身坐标系下:

其中N为整数，k＝0,T,2T.....,T为控制周期；(X_0i,Y_0i),i＝1,2,...,N是期望离散路径点在地球坐标系下的坐标；N表示全局期望路径点个数；δ_r是当前时刻实际前轮转角；(x_c,y_c)是车辆在地球坐标系下的坐标；θ₀是当前时刻车辆航向角度；y_εi(k)是期望路径点的横向位置偏差； x_εi(k)是期望路径点的纵向位置偏差；θ_εi(k)是期望路径点航向偏差；Φ_i(k)是地球坐标系下期望路径点指向下一点的方向与正北方向夹角，即有：

N为整数。

步骤二：按照如下公式确定前方道路是直道还是弯道：

其中start表示从当前时刻距离车辆最近的期望路径点的坐标；NUM 表示从当前时刻距离车辆最近的期望路径点向前搜索的路径点个数, 同时也是需要拟合的路径点个数；Δ_1i是第i个路径点的一阶差分；Δ₂是NUM-1个路径点一阶差分平均值；NUM为需要拟合的路径点个数。

Δ₂超过预定阈值时则将前方道路判断为弯道，否则为直道。

步骤三：计算道路曲率，包括如下步骤。

步骤3.1：采用二阶最小二乘法拟合法得到道路连续函数，计算公式为：

m₁＝(sumx)/NUM,m₂＝(sumx²)/NUM

m₃＝(sumx³-m₂sumx)/(sumx²-m₁sumx)

m₄＝sumx²y-m₂sumy-m₃(sumxy-m₁sumy)

根据最小二乘拟合原理，有：

a₂＝m₄/((sumx⁴-m₂sumx²)-m₃(sumx³-m₁sumx²))

a₁＝(sumxy-m₁sumy-a₂(sumx³-m₁sumx²))/(sumx²-m₁sumx)

a₀＝(sumy-a₂sumx²-a₁sumx))/NUM

y＝a₀+a₁x+a₂x²

步骤3.2：则按照下式得到前方道路曲率：

其中T为控制周期；

步骤四：计算得到期望前轮转角，计算公式为；

其中δ_d是期望前轮转角，K_L是道路曲率，t表示当前时刻；

其中，u_x为车速m为整车质量；k₁、k₂分别为前、后车轮的侧偏刚度；a、b分别为前、后轴到质心的距离，I_z是转动惯量。

进一步的，所述NUM取值大于10。

进一步的，Δ₂超过0.01时，则将前方道路判断为弯道，否则为直道。

本发明的有益效果为：

本发明针对无人驾驶汽车，在道路曲率复杂路况下，提供了一种无人车路径跟踪前馈控制方法，消除复杂曲率带来的干扰。本发明在传统车辆路径跟踪系统模型中加入道路曲率作为干扰输入项，给出了加入曲率干扰之后的系统模型状态空间表达式。针对该模型，以横向位置偏差趋于零作为目标函数设计了一种前馈控制方法。实车试验时，根据基于最小二乘的道路曲率拟合方法，结合前馈控制方法实时计算无人车期望前轮转角。实车试验结果表明所提前馈控制方法对曲率复杂路况具有一定适应性。本发明算法简单，不仅适用于仿真技术领域，还能够在实车嵌入式控制器上实现，应用前景好。

附图说明

图1是无人车前馈控制原理示意图

图2是传递函数形式表达的无人车前馈控制原理示意图

图3是基于传感器及期望路径点的曲率计算方法示意图

图4是实车试验中车辆实际运行轨迹与期望轨迹对比示意图

具体实施方式

本发明的总体设计思路是：在传统车辆路径跟踪系统模型中加入道路曲率作为干扰输入项，给出加入曲率干扰之后的系统模型状态空间表达式，针对该新模型以横向位置偏差为零作为目标函数设计前馈控制方法。实车试验时，根据基于最小二乘拟合的道路曲率预测方法，结合前馈控制方法实时得出无人车的期望前轮转角。

在介绍本发明的实施步骤前，先介绍本发明的技术原理。该技术原理包括如下步骤：

步骤一：在车辆二自由度动力学模型基础上，结合传统无人车路径跟踪系统模型，加入道路曲率作为模型的干扰输入项，在车辆运动过程中，通过分析车辆的运动状态，得到新的路径跟踪系统模型状态空间表达式：

式中，

u＝[δ_r,K_L]^T；

其中，m为整车质量；k₁、k₂分别为前、后车轮的侧偏刚度；a、 b分别为前、后轴到质心的距离；y_e为车辆横向位移；u_x为车速；δ_r为实际前轮转角；w_r为横摆角速度；Y是车辆当前横向位置偏差；

是车辆当前车辆航向偏差；I_z是转动惯量；L是车辆当前纵向位置偏差，即预瞄距离；K_L为期望路径点对应的道路曲率。

步骤二：分析步骤一给出的新无人车路径跟踪模型，给出无人车前馈控制原理图如附图1所示。图1中δ_fd、δ_bd分别表示前馈、反馈控制器输出的期望前轮转角，两者之和为作用于转向执行机构的期望前轮转角δ_d，即δ_fd+δ_bd＝δ_d，在本发明中δ_bd＝0，有δ_fd＝δ_d。

步骤三：将新的模型状态空间表达式解耦，得到模型传递矩阵：

其中，

步骤四：结合步骤二和步骤三得到无人车前馈控制的传递函数形式表达原理图如附图2所示。其中G₂(s)是转向执行机构开环传递函数， G₃(s)、G₄(s)分别是新路径跟踪模型经过解耦之后的传递函数，G₃(s)以道路曲率为输入，横向位置偏差为输出，G₄(s)以实际前轮转角为输入，横向位置偏差为输出。G₁(s)是需要设计的前馈控制器传递函数。

根据模型传递矩阵式(2)得到：

将转向执行机构看作一般位移控制系统，在整体系统稳态误差趋于零时，有：

步骤五：以横向位置偏差趋于零作为目标函数，得到目标函数为：

将模型传递矩阵式(3)、(4)代入目标函数式(7)，横向位置偏差趋于零时，整体系统稳态误差趋于零，则解出前馈控制器的传递函数表达式：

步骤六：将传递函数表达式作拉式反变换，得到道路曲率与期望前轮转角的实时控制关系：

其中δ_d是期望前轮转角，K_L是道路曲率，t表示当前时刻。

下面再对本发明的实施步骤进行说明。

步骤一：根据附图3，按照如下公式将所有期望路径点转化到车身坐标系下：

其中N为整数；k表示离散控制时间，k＝0，T，2T.....；T为控制周期。其中(X_0i,Y_0i),i＝1,2,...,N是期望离散路径点在地球坐标系下的坐标；N表示全局期望路径点个数，由路径规划子系统基于栅格地图给出；δ_r是当前时刻实际前轮转角；(x_c,y_c)是车辆在地球坐标系下的坐标，由GPS传感器实时得到；θ₀是当前时刻车辆航向角度，由IMU传感器实时测得，以正北方向为零度，顺时针旋转时航向角度增大；y_εi是期望路径点的横向位置偏差；x_εi是期望路径点的纵向位置偏差；θ_εi是期望路径点航向偏差；Φ_i是地球坐标系下期望路径点指向下一点的方向与正北方向夹角，即有：

N为整数。

步骤二：按照如下公式确定前方道路是直道还是弯道：

使用式(12)时，首先计算在当前时刻所有全局路径点与车辆的距离，找出距离最近的点，坐标记为(x_εstart,y_εstart)，然后向前搜索NUM个路径点，记录其坐标。其中start表示从当前时刻距离车辆最近的期望路径点的坐标；NUM表示从当前时刻距离车辆最近的期望路径点(距离最近的点下标记为start)向前搜索的路径点个数，同时也是需要拟合的路径点个数。Δ_1i是第i个路径点一阶差分；Δ₂是NUM-1个路径点一阶差分平均值；NUM取定值。NUM表示当前时刻向前搜索的路径点个数，同时也是需要拟合的路径点个数。

Δ₂超过阈值CURTHRE时则将前方道路判断为弯道，否则为直道，实验中阈值取定值为0.01。

步骤三：采用二阶最小二乘法拟合法得到道路连续函数，计算道路曲率。

记：

m₁＝(sumx)/NUM,m₂＝(sumx²)/NUM(17)

m₃＝(sumx³-m₂sumx)/(sumx²-m₁sumx)(18)

m₄＝sumx²y-m₂sumy-m₃(sumxy-m₁sumy)(19)

根据最小二乘拟合原理，有：

则按照下式得到前方道路曲率：

其中T为控制周期。

实车试验时，车速u_x取10km/h，控制周期T取50ms，一个控制周期内选择需要拟合的离散路径点个数NUM为10。得到车辆实际运行轨迹与期望轨迹对比图如附图4，横向误差分布表如表1所示。

表1：横向误差分布表

横向误差区间	0-1m	1-2m	2-3m	>3m	合计
						路径点个数	156	518	443	66	1183
比率	13.2％	43.8％	37.4％	5.6％	100％

步骤四：计算得到期望前轮转角，计算公式为：

其中δ_d是期望前轮转角，K_L是道路曲率，t表示当前时刻；

其中，u_x为车速m为整车质量；k₁、k₂分别为前、后车轮的侧偏刚度；a、b分别为前、后轴到质心的距离，I_z是转动惯量。

如图4所示，从本发明的实车试验结果可得跟踪复杂曲率的路径时，横向位置偏差小于3m的概率为94.4％，横向位置偏差小于2m的概率为57.0％，体现了该发明对复杂道路曲率具有一定的适应性。

Claims (3)

1.一种无人车路径跟踪前馈控制方法，其特征在于，包括如下步骤：

步骤一:按照如下公式将所有期望路径点转化到车身坐标系下:

其中N为整数，k为离散控制时间，k＝0,T,2T.....,T为控制周期；(X_0i,Y_0i),i＝1,2,...,N是期望离散路径点在地球坐标系下的坐标；N表示全局期望路径点个数；δ_r是当前时刻实际前轮转角；(x_c,y_c)是车辆在地球坐标系下的坐标；θ₀是当前时刻车辆航向角度；y_εi(k)是期望路径点的横向位置偏差；x_εi(k)是期望路径点的纵向位置偏差；θ_εi(k)是期望路径点航向偏差；Φ_i(k)是地球坐标系下期望路径点指向下一点的方向与正北方向夹角，即有：

N为整数；

步骤二：按照如下公式确定前方道路是直道还是弯道：

其中start表示从当前时刻距离车辆最近的期望路径点的坐标；NUM表示从当前时刻距离车辆最近的期望路径点向前搜索的路径点个数,同时也是需要拟合的路径点个数；Δ_1i是第i个路径点的一阶差分；Δ₂是NUM-1个路径点一阶差分平均值；NUM为需要拟合的路径点个数；

Δ₂超过预定阈值时则将前方道路判断为弯道，否则为直道；

步骤三：计算道路曲率，包括如下步骤；

步骤3.1：采用二阶最小二乘法拟合法得到道路连续函数，计算公式为：

m₁＝(sumx)/NUM,m₂＝(sumx²)/NUM

m₃＝(sumx³-m₂sumx)/(sumx²-m₁sumx)

m₄＝sumx²y-m₂sumy-m₃(sumxy-m₁sumy)

根据最小二乘拟合原理，有：

a₂＝m₄/((sumx⁴-m₂sumx²)-m₃(sumx³-m₁sumx²))

a₁＝(sumxy-m₁sumy-a₂(sumx³-m₁sumx²))/(sumx²-m₁sumx)

a₀＝(sumy-a₂sumx²-a₁sumx))/NUM

y＝a₀+a₁x+a₂x²

其中，y是拟合之后的函数值；

步骤3.2：则按照下式得到前方道路曲率：

其中T为控制周期，x_εstart(K)是第K时刻距离车辆最近的参考坐标点和车辆坐标的纵向误差；

步骤四：计算得到期望前轮转角，计算公式为；

其中，δ_d是期望前轮转角，K_L是道路曲率，t表示当前时刻；a₁₁，a₁₂，a₂₁，a₂₂，b₁₁，b₁₂均是计算参数，且：

u_x为车速m为整车质量；k₁、k₂分别为前、后车轮的侧偏刚度；a、b分别为前、后轴到质心的距离，I_z是转动惯量。

2.如权利要求1所述的一种无人车路径跟踪前馈控制方法，其特征在于，所述NUM取值大于10。

3.如权利要求1所述的一种无人车路径跟踪前馈控制方法，其特征在于，步骤二中，Δ₂超过0.01时，则将前方道路判断为弯道，否则为直道。

Images