预瞄路径跟踪计算方法及跟踪计算模块与流程

本发明涉及汽车领域，特别是涉及一种用于自动驾驶车辆路径跟踪预瞄模型的预瞄路径跟踪计算方法。本发明还涉及一种用于自动驾驶车辆的预瞄路径跟踪计算模块。

背景技术：

作为未来汽车工业的发展方向，自动驾驶近年来的发展十分迅速。路径跟踪作为自动驾驶底层控制的核心之一，其性能的好坏决定了车辆的安全性和稳定性。在路径跟踪研究领域，较常用的方法有：pid、驾驶员预瞄模型、模型预测控制(modelpredictivecontrol,mpc)等。其中,pid算法结构简单，但参数较难调节，固定参数无法应对复杂多变的行驶状态.模型预测控制能够应对不同工况，但对车辆模型要求较高，计算量较大。

驾驶员预瞄模型是由吉大的郭孔辉院士提出的，驾驶员预瞄模型的工作是基于转向角输入来实现。预瞄路径指的是按车辆当前车头方向及航向角方向的前方路径。驾驶员预瞄模型结构简单，计算量较小，能够应对较多工况，因此在路径跟踪领域被广泛采用。现有驾驶员预瞄模型在预瞄路径跟踪计算时采用理想工况作为计算基础，未考虑客观存在系统延时造成预瞄路径跟踪计算存在误差，为自动驾驶车辆带来了安全隐患。

技术实现要素：

在发明内容部分中引入了一系列简化形式的概念,该简化形式的概念均为本领域现有技术简化，这将在具体实施方式部分中进一步详细说明。本发明的发明内容部分并不意味着要试图限定出所要求保护的技术方案的关键特征和必要技术特征，更不意味着试图确定所要求保护的技术方案的保护范围。

本发明要解决的技术问题是提供一种用于自动驾驶车辆路径跟踪预瞄模型，能降低(甚至克服)系统延时造成预瞄路径跟踪计算误差的预瞄路径跟踪计算方法。

本发明要解决的另一技术问题是提供一种用于自动驾驶车辆的路径跟踪预瞄模块，能降低(甚至克服)系统延时造成预瞄路径跟踪计算误差的预瞄路径跟踪计算模块。

为解决上述技术问题，本发明提供用于自动驾驶车辆路径跟踪预瞄模型的预瞄路径跟踪计算方法，包括以下步骤：

s1，获取自动驾驶车辆系统延时δt；

可选择的，系统延时δt包括由控制程序发出方向盘转角信息，测试底层执行机构响应到相应转角时所需时间，系统延时δt能通过计算准确获得；

由控制中心发出期望方向盘转角信息，发出时间记为t1。从can上获取实际方向盘转角信息，记录响应到期望方向盘转角时时间t2。t2-t1为测得系统延时δt。

相应的，系统延时δt也可通过系统工况估算设置，估算设置的系统延时δt虽然没有计算获得的准确，但由于考虑到了系统延时δt对于预瞄路径的影响，相对现有技术来说仍具有更高的准确性。

s2，获取全局路径，在全局路径中基于匀加速模型外推计算系统延时δt后车辆位置；

在现有自动驾驶系统中，通常由预存地图提供全局路径信息，路径规划根据车辆周围状态进行局部路径规划，然后进行路径跟踪。但为了便于对本发明进行单独实现和性能验证，本发明可以直接利用全球定位系统(例如gps)录制全局路径信息，然后利用本发明提供的预瞄路径跟踪计算方法对全局路径进行跟踪；应该清楚的是，相应的本发明提供的预瞄路径跟踪计算方法仍可以利用预存地图提供全局路径信息。

当前时刻t计算出的期望方向盘转角在t+δt时刻才会被车辆响应。如果以当前时刻t车辆位置和状态为基础外推预测t+δt时车辆位置，然后以该位置为基础根据预瞄模型计算期望方向盘转角，则t+δt时刻期望的方向盘转角可在t时刻发出并在t+δt时刻被响应到。此时实际响应和期望值之间不存在延时,通过该方法可解决系统延迟引起的跟踪误差。

在车辆坐标系中，一般认为车头方向为x轴方向，车辆左侧为y方向；设定全局坐标系中，正东方向为x轴方向，正北方向为y轴方向；车辆航向角为车辆质心方向与全局坐标系x轴的夹角，其位置关系如图2所示。

车辆系统延时δt一般小于1s，因此可使用匀加速模型外推预测δt后车辆位置。若已知车辆t时刻纵向速度vx(t)，纵向加速度ax(t)，横向速度vy(t)，和横向加速ay(t)，则δt后车辆在横纵方向位移为：

d_x(δt)＝vx(t)×δt+1/2×ax(t)×δt²；

d_y(δt)＝vy(t)×δt+1/2×ay(t)×δt²；

δt后在车辆坐标下坐标为(d_x(δt),d_y(δt))。

得到δt后车辆在车辆坐标系下坐标后，通过t时刻车辆在全局坐标系下的坐标和航向角，可坐标变换得到t+δt时车辆在全局坐标系下的坐标。车辆当前全局坐标(xgps(t),ygps(t))和航向角θ(t)可从gps/imu获得，imu是测量物体三轴姿态角及加速度的装置。一般imu包括三轴陀螺仪及三轴加速度计,某些九轴imu还包括三轴磁力计，则δt后车辆的全局坐标系下坐标为：

xgps(t+δt)＝xgps(t)+d_x(δt)×cos(θ(t))+d_y(δt)×sin(θ(t))；

ygps(t+δt)＝ygps(t)+d_y(δt)×cos(θ(t))-d_x(δt)×sin(θ(t))；

t是当前时刻，xgps(t)是车辆当前全局横坐标,ygps(t)是车辆当前全局纵坐标，xgps(t+δt)是车辆δt时间后全局坐标系下横坐标，ygps(t+δt)是车辆δt时间后全局坐标系下纵坐标，d_x(δt)是车辆δt时间后车辆坐标系下横坐标，d_y(δt)是车辆δt时间后车辆坐标系下纵坐标，θ(t)是车辆当前全局航向角。

s3,计算预瞄误差；

在t时刻，以外推预测得到t+δt时刻位置为基础，沿着车辆行驶的方向向前预瞄一段距离，预瞄点计算方式如下式；

xpreview(t)＝xgps(t+δt)+d×cos(θ(t))；

ypreview(t)＝ygps(t+δt)+d×sin(θ(t))；

d＝d0+vx(t)*tpreview；

则，

xpreview(t)是预瞄点横坐标，ypreview(t)是预瞄点纵坐标，(x_pathi,y_pathi)为路径坐标点，i∈[0,n],n为路径坐标点数量，oss是预瞄误差，d0是最短预瞄距离，为常数，d是预瞄距离，tpreview为预瞄时间，vx(t)为车辆t时刻纵向速度。

s4,根据预瞄误差与轮胎转角关系计算得到轮胎转角，根据轮胎转角计算得到方向盘转角，将所述方向盘转角发送给底盘can执行方向盘转角控制。

计算预瞄误差与轮胎转角关系如下；

计算轮胎转角转换为方向盘转角关系如下；

δ＝i×δss；

a为车辆质心到前轴的距离，b为车辆质心到后轴的距离，m为车辆质量，cf、cr分别为前后轮的等效侧偏刚度，vx为车辆纵向速度，δss为轮胎转角，i为方向盘转角到轮胎转角的传动比，δ为方向盘转角。

本发明提供一种用于自动驾驶车辆的预瞄路径跟踪计算模块，包括：

延时计算单元，其适用于获取自动驾驶车辆系统延时δt；

由控制中心发出期望方向盘转角信息，发出时间记为t1。从can上获取实际方向盘转角信息，记录响应到期望方向盘转角时时间t2。t2-t1为测得系统延时δt。

全局路径获取单元，其适用于获取全局路径；

在现有自动驾驶系统中，通常由预存地图提供全局路径信息，路径规划模块根据车辆周围状态进行局部路径规划，然后传递给路径跟踪模块进行跟踪。但为了便于对本发明进行单独实现和性能验证，本发明可以直接利用全球定位系统(例如gps)录制全局路径信息，然后利用本发明提供的预瞄路径跟踪计算模块对全局路径进行跟踪；应该清楚的是，相应的本发明提供的车辆预瞄路径跟踪系统方法仍可以利用预存地图提供全局路径信息。

预测位置计算单元，其适用于在全局路径中基于匀加速模型外推计算系统延时δt后车辆位置；

当前时刻t计算出的期望方向盘转角在t+δt时刻才会被车辆响应。如果以当前时刻t车辆位置和状态为基础外推预测t+δt时车辆位置，然后以该位置为基础根据预瞄模型计算期望方向盘转角，则t+δt时刻期望的方向盘转角可在t时刻发出并在t+δt时刻被响应到。此时实际响应和期望值之间不存在延时,通过该方法可解决系统延迟引起的跟踪误差。

车辆系统延时δt一般小于1s，因此可使用匀加速模型外推预测δt后车辆位置。若已知车辆t时刻纵向速度vx(t)，纵向加速度ax(t)，横向速度vy(t)，和横向加速ay(t)，则δt后车辆在横纵方向位移为：

d_x(δt)＝vx(t)×δt+1/2×ax(t)×δt²；

d_y(δt)＝vy(t)×δt+1/2×ay(t)×δt²；

得到δt时间内车辆在横纵方向的位移后，通过t时刻车辆在全局坐标系下的坐标和航向角，可外推预测得到t+δt时车辆在全局坐标系下的坐标。其中，车辆当前全局坐标(xgps(t),ygps(t))和航向角θ(t)可从gps/imu获得，imu是测量物体三轴姿态角及加速度的装置。一般imu包括三轴陀螺仪及三轴加速度计,某些九轴imu还包括三轴磁力计，则δt后车辆的全局坐标系下坐标为：

xgps(t+δt)＝xgps(t)+d_x(δt)×cos(θ(t))+d_y(δt)×sin(θ(t))；

ygps(t+δt)＝ygps(t)+d_y(δt)×cos(θ(t))-d_x(δt)×sin(θ(t))；

t是当前时刻，xgps(t)是车辆当前全局横坐标,ygps(t)是车辆当前全局纵坐标，xgps(t+δt)是车辆δt时间后全局坐标系下横坐标，ygps(t+δt)是车辆δt时间后全局坐标系下纵坐标，d_x(δt)是车辆δt时间后车辆坐标系下横坐标，d_y(δt)是车辆δt时间后车辆坐标系下纵坐标，θ(t)是车辆当前全局航向角。

预瞄误差计算单元，其适用于计算预瞄误差；

在t时刻，以外推预测得到t+δt时刻位置为基础，沿着车辆行驶的方向向前预瞄一段距离，预瞄点计算方式下式；

xpreview(t)＝xgps(t+δt)+d×cos(θ(t))；

ypreview(t)＝ygps(t+δt)+d×sin(θ(t))；

d＝d0+vx(t)×tpreview；

则，

xpreview(t)是预瞄点横坐标，ypreview(t)是预瞄点纵坐标，(x_pathi,y_pathi)为路径坐标点，i∈[0,n],n为路径坐标点数量，oss是预瞄误差，d0是最短预瞄距离，为常数，d是预瞄距离，tpreview为预瞄时间，vx(t)为车辆t时刻纵向速度。

方向盘转角计算单元，其适用于根据预瞄误差与轮胎转角关系计算得到轮胎转角，根据轮胎转角计算得到方向盘转角，将所述方向盘转角发送给底盘can执行方向盘转角控制。

计算预瞄误差与轮胎转角关系如下；

计算轮胎转角转换为方向盘转角关系如下；

δ＝i×δss；

a为车辆质心到前轴的距离，b为车辆质心到后轴的距离，m为车辆质量，cf、cr分别为前后轮的等效侧偏刚度，vx为车辆纵向速度，δss为轮胎转角，i为方向盘转角到轮胎转角的传动比，δ为方向盘转角。

本发明至少具有以下技术效果：

1、准确获取客观存在的系统延时，将系统延时引入预瞄路径计算，能降低预瞄路径计算误差。

2、利用匀加速模型进行车辆位置外推预测，能够通过较小计算量得到相对精确的车辆外推预测位置。

3、考虑到在当前技术情况下系统延时通常小于1s，随着技术的进步系统延时会进一步减少。本发明以此为理论基础，通过匀速模型外推预测车辆位置，大幅减小了因系统延时导致的跟踪误差。测试速度20km/h以下时误差由1m减小为0.2m，为车辆安全可靠行驶提供了基础。

如前所述，随着技术的进步系统延时进一步减少，通过本发明计算的预瞄路径误差会进一步降低，甚至达到误差可忽略的级别进而完全克服系统延时带来预瞄路径误差造成的安全隐患。

附图说明

本发明附图旨在示出根据本发明的特定示例性实施例中所使用的方法、结构和/或材料的一般特性，对说明书中的描述进行补充。然而，本发明附图是未按比例绘制的示意图，因而可能未能够准确反映任何所给出的实施例的精确结构或性能特点，本发明附图不应当被解释为限定或限制由根据本发明的示例性实施例所涵盖的数值或属性的范围。下面结合附图与具体实施方式对本发明作进一步详细的说明：

图1是车辆、预瞄准路径、预瞄距离和预瞄准误差位置关系示意图。

图2是车辆、全局坐标系和车辆坐标系位置关系示意图。

图3是本发明预瞄路径跟踪计算方法流程示意图。

图4是本发明预瞄误差计算车辆位置示意图。

具体实施方式

以下通过特定的具体实施例说明本发明的实施方式，本领域技术人员可由本说明书所公开的内容充分地了解本发明的其他优点与技术效果。本发明还可以通过不同的具体实施方式加以实施或应用，本说明书中的各项细节也可以基于不同观点加以应用，在没有背离发明总的设计思路下进行各种修饰或改变。需说明的是，在不冲突的情况下，以下实施例及实施例中的特征可以相互组合。本发明下述示例性实施例可以多种不同的形式来实施，并且不应当被解释为只限于这里所阐述的具体实施例。应当理解的是，提供这些实施例是为了使得本发明的公开彻底且完整，并且将这些示例性具体实施例的技术方案充分传达给本领域技术人员。

还应当清楚的是，本发明预瞄路径跟踪计算方法的各步骤编号s1-s4仅是用于更好的区分各步骤的编号，并不是代表各步骤的执行先后顺序。各步骤之间存在可以倒序执行或并行执行(例如，s1和s2可以倒序也可以并行)的可能性，也应视为本发明的所要求保护的方法。

如图3所示，本发明提供用于自动驾驶车辆路径跟踪预瞄模型的预瞄路径跟踪计算方法第一实施例，包括以下步骤：

s1，获取自动驾驶车辆系统延时δt；

可选择的，系统延时δt包括由控制程序发出方向盘转角信息，测试底层执行机构响应到相应转角时所需时间，系统延时δt能通过计算准确获得；

由控制中心发出期望方向盘转角信息，发出时间记为t1。从can上获取实际方向盘转角信息，记录响应到期望方向盘转角时时间t2。t2-t1为测得系统延时δt。

s2，通过gps获取全局路径，在全局路径中基于匀加速模型外推计算系统延时δt后车辆位置；

当前时刻t计算出的期望方向盘转角在t+δt时刻才会被车辆响应。如果以当前时刻t车辆位置和状态为基础外推预测t+δt时车辆位置，然后以该位置为基础根据预瞄模型计算期望方向盘转角，则t+δt时刻期望的方向盘转角可在t时刻发出并在t+δt时刻被响应到。此时实际响应和期望值之间不存在延时,通过该方法可解决系统延迟引起的跟踪误差。

车辆系统延时δt一般小于1s，因此可使用匀加速模型外推预测δt后车辆位置。若已知车辆t时刻纵向速度vx(t)，纵向加速度ax(t)，横向速度vy(t)，和横向加速ay(t)，则δt后车辆在横纵方向位移为：

d_x(δt)＝vx(t)×δt+1/2×ax(t)×δt²；

d_y(δt)＝vy(t)×δt+1/2×ay(t)×δt²；

得到δt时间内车辆在横纵方向的位移后，通过t时刻车辆在全局坐标系下的坐标和航向角，可外推预测得到t+δt时车辆在全局坐标系下的坐标。其中，车辆当前全局坐标(xgps(t),ygps(t))和航向角θ(t)可从gps/imu获得，，则δt后车辆的全局坐标系下坐标为：

xgps(t+δt)＝xgps(t)+d_x(δt)×cos(θ(t))+d_y(δt)×sin(θ(t))；

ygps(t+δt)＝ygps(t)+d_y(δt)×cos(θ(t))-d_x(δt)×sin(θ(t))；

t是当前时刻，xgps(t)是车辆当前全局横坐标,ygps(t)是车辆当前全局纵坐标，xgps(t+δt)是车辆δt时间后全局坐标系下横坐标，ygps(t+δt)是车辆δt时间后全局坐标系下纵坐标，d_x(δt)是车辆δt时间后车辆坐标系下横坐标，d_y(δt)是车辆δt时间后车辆坐标系下纵坐标，θ(t)是车辆当前全局航向角。

s3,如图4所示计算预瞄误差；

在t时刻，以外推预测得到t+δt时刻位置为基础，沿着车辆行驶的方向向前预瞄一段距离，预瞄点计算方式下式；

xpreview(t)＝xgps(t+δt)+d×cos(θ(t))；

ypreview(t)＝ygps(t+δt)+d×sin(θ(t))；

d＝d0+vx(t)*tpreview；

则

xpreview(t)是预瞄点横坐标，ypreview(t)是预瞄点纵坐标，(x_pathi,y_pathi)为路径坐标点，i∈[0,n],n为路径坐标点数量，oss是预瞄误差，d0是最短预瞄距离，为常数，d是预瞄距离，tpreview为预瞄时间，vx(t)为车辆t时刻纵向速度。

s4,根据预瞄误差与轮胎转角关系计算得到轮胎转角，根据轮胎转角计算得到方向盘转角，将所述方向盘转角发送给底盘can执行方向盘转角控制；

其中，计算预瞄误差与轮胎转角关系如下；

其中，计算轮胎转角转换为方向盘转角关系如下；

δ＝i×δss；

a为车辆质心到前轴的距离，b为车辆质心到后轴的距离，m为车辆质量，cf、cr分别为前后轮的等效侧偏刚度，vx为车辆纵向速度，δss为轮胎转角，i为方向盘转角到轮胎转角的传动比，δ为方向盘转角。

本发明提供一种用于自动驾驶车辆的预瞄路径跟踪计算模块第一实施例，包括：

延时计算单元，其适用于获取自动驾驶车辆系统延时δt；

由控制中心发出期望方向盘转角信息，发出时间记为t1。从can上获取实际方向盘转角信息，记录响应到期望方向盘转角时时间t2。t2-t1为测得系统延时δt。

全局路径获取单元，其适用于通过全球定位系统或预存地图获取全局路径；

预测位置计算单元，其适用于在全局路径中基于匀加速模型外推计算系统延时δt后车辆位置；

当前时刻t计算出的期望方向盘转角在t+δt时刻才会被车辆响应。如果以当前时刻t车辆位置和状态为基础外推预测t+δt时车辆位置，然后以该位置为基础根据预瞄模型计算期望方向盘转角，则t+δt时刻期望的方向盘转角可在t时刻发出并在t+δt时刻被响应到。此时实际响应和期望值之间不存在延时,通过该方法可解决系统延迟引起的跟踪误差。

车辆系统延时δt一般小于1s，因此可使用匀加速模型外推预测δt后车辆位置。若已知车辆t时刻纵向速度vx(t)，纵向加速度ax(t)，横向速度vy(t)，和横向加速ay(t)，则δt后车辆在横纵方向位移为：

d_x(δt)＝vx(t)×δt+1/2×ax(t)×δt²；

d_y(δt)＝vy(t)×δt+1/2×ay(t)×δt²；

得到δt时间内车辆在横纵方向的位移后，通过t时刻车辆在全局坐标系下的坐标和航向角，可外推预测得到t+δt时车辆在全局坐标系下的坐标。其中，车辆当前全局坐标(xgps(t),ygps(t))和航向角θ(t)可从gps/imu获得，imu是测量物体三轴姿态角及加速度的装置。一般imu包括三轴陀螺仪及三轴加速度计,某些九轴imu还包括三轴磁力计，则δt后车辆的全局坐标系下坐标为：

xgps(t+δt)＝xgps(t)+d_x(δt)×cos(θ(t))+d_y(δt)×sin(θ(t))；

ygps(t+δt)＝ygps(t)+d_y(δt)×cos(θ(t))-d_x(δt)×sin(θ(t))；

t是当前时刻，xgps(t)是车辆当前全局横坐标,ygps(t)是车辆当前全局纵坐标，xgps(t+δt)是车辆δt时间后全局坐标系下横坐标，ygps(t+δt)是车辆δt时间后全局坐标系下纵坐标，d_x(δt)是车辆δt时间后车辆坐标系下横坐标，d_y(δt)是车辆δt时间后车辆坐标系下纵坐标，θ(t)是车辆当前全局航向角。

预瞄误差计算单元，其适用于计算预瞄误差；

在t时刻，以外推预测得到t+δt时刻位置为基础，沿着车辆行驶的方向向前预瞄一段距离，预瞄点计算方式下式；

xpreview(t)＝xgps(t+δt)+d×cos(θ(t))；

ypreview(t)＝ygps(t+δt)+d×sin(θ(t))；

d＝d0+vx(t)×tpreview；

则，

xpreview(t)是预瞄点横坐标，ypreview(t)是预瞄点纵坐标，(x_pathi,y_pathi)为路径坐标点，i∈[0,n],n为路径坐标点数量，oss是预瞄误差，d0是最短预瞄距离，为常数，d是预瞄距离，tpreview为预瞄时间，vx(t)为车辆t时刻纵向速度。

方向盘转角计算单元其适用于根据预瞄误差与轮胎转角关系计算得到轮胎转角，根据轮胎转角计算得到方向盘转角，将所述方向盘转角发送给底盘can执行方向盘转角控制；

其中，计算预瞄误差与轮胎转角关系如下；

其中，计算轮胎转角转换为方向盘转角关系如下；

δ＝i×δss；

a为车辆质心到前轴的距离，b为车辆质心到后轴的距离，m为车辆质量，cf、cr分别为前后轮的等效侧偏刚度，vx为车辆纵向速度，δss为轮胎转角，i为方向盘转角到轮胎转角的传动比，δ为方向盘转角。

将计算得到的方向盘转角发送给底盘can进行响应执行，则一个预瞄路径跟踪控制周期完成。

除非另有定义，否则这里所使用的全部术语(包括技术术语和科学术语)都具有与本发明所属领域的普通技术人员通常理解的意思相同的意思。还将理解的是，除非这里明确定义，否则诸如在通用字典中定义的术语这类术语应当被解释为具有与它们在相关领域语境中的意思相一致的意思，而不以理想的或过于正式的含义加以解释。

以上通过具体实施方式和实施例对本发明进行了详细的说明，但这些并非构成对本发明的限制。在不脱离本发明原理的情况下，本领域的技术人员还可做出许多变形和改进，这些也应视为本发明的保护范围。