CN111610780B - 一种自动驾驶车路径跟踪控制方法及其装置 - Google Patents

Abstract

本发明涉及一种自动驾驶车路径跟踪控制方法及其装置，所述方法包括：获取自动驾驶车辆的期望路径和当前位姿信息；根据所述期望路径和当前位姿信息获取前轮转角纠正量；根据所述期望路径获取预瞄点处的侧偏角；根据车辆动力学模型和所述预瞄点处的侧偏角进行预瞄控制，获取前轮转角；根据所述前轮转角纠正量和所述前轮转角以及传动比计算方向盘转角。所述装置用于实现所述方法。实施本发明，能够精确地控制汽车在自动驾驶过程中跟随预期轨迹，提升了自动驾驶系统在存在干扰的工况下的鲁棒性，从而提升了自动驾驶汽车的安全性。

Description

一种自动驾驶车路径跟踪控制方法及其装置

技术领域

本发明涉及自动驾驶技术领域，具体涉及一种自动驾驶车路径跟踪控制方法及其装置。

背景技术

在无人驾驶车系统中，车辆自身的状态信息主要来自GPS定位导航模块，环境信息主要来自视觉系统以及雷达系统等，经过信息融合处理模块处理可以得到当前车辆的位姿等信息同时还可以得到前方一定距离处的道路信息、障碍物的信息。路径规划模块根据这些信息，会规划出一条从车辆当前位置到子目标点的参考路径。为了使车辆能最优的跟踪这条已经规划好的路径，驾驶员或者控制器必须同时处理当前的位姿误差和前方一定距离处的道路信息，以便调整车辆的方向和速度，使得车辆尽量跟踪参考路径。

无人驾驶车路径跟踪控制模拟熟练驾驶员开车时，眼睛不断对前方道路进行预瞄，根据前方期望路径的曲率以及当前车辆的车速等决定方向盘转角，使得无人驾驶车尽量逼近前方的道路。在最理想的情况下，车辆能完美地行驶在参考路径上，但在实际的行车环境中，车辆会受到侧向风、路面不平度等的干扰而偏离期望的参考路径，所以一个有经验的驾驶员还要纠正实际车辆行驶的车道和当前车道的偏差，并试图通过继续转动方向盘来减小这个偏差以求达到补偿的目的，根据以上分析建立无人驾驶车路径跟踪控制器。

发明内容

本发明所要解决的技术问题在于，提供一种自动驾驶车路径跟踪控制方法及其装置，以精确地控制汽车在自动驾驶过程中跟随预期轨迹，提升自动驾驶系统在存在干扰的工况下的鲁棒性和自动驾驶车的安全性。

为了解决上述技术问题，本发明实施例提供一种自动驾驶车路径跟踪控制方法，包括如下步骤：

获取自动驾驶车辆的期望路径和当前位姿信息；

根据所述期望路径和当前位姿信息获取前轮转角纠正量；

根据所述期望路径获取预瞄点处的侧偏角；

根据车辆动力学模型和所述预瞄点处的侧偏角进行预瞄控制以获取前轮转角；

根据所述前轮转角纠正量和所述前轮转角以及传动比计算方向盘转角。

优选地，所述车辆动力学模型为单轨线性二自由度车辆动力学模型。

优选地，所述车辆动力学模型如下公式所示：

其中，F_x1、F_y1分别为地面对前轮的纵向力和侧向力；F_x2、F_y2分别为地面对后轮的纵向力和侧向力；δ为前轮转角；β为单轨质心侧偏角；

为车辆横摆角

的二阶导数；I为车辆绕z轴的转动惯量；V为车辆速度；

为车辆速度V的一阶导数；m为车辆质量；l_f、l_r分别为单轨质心至前轮和后轮的距离；ρ为道路曲率半径；x为车辆行驶方向坐标，y为与车辆行驶方向垂直的坐标。

优选地，所述根据所述期望路径获取预瞄点处的侧偏角具体包括：

根据所述期望路径确定预瞄距离，所述预瞄距离等于单轨质心至前轮的距离加上单轨质心至预瞄点的距离；

根据所述预瞄距离确定预瞄点的轨迹曲率；

根据所述预瞄点的轨迹曲率获取预瞄点处的侧偏角。

优选地，所述根据车辆动力学模型和预瞄点处的侧偏角进行预瞄控制获取前轮转角具体包括：

基于所述预瞄点处的侧偏角和所述单轨质心侧偏角的关系，根据所述车辆动力学模型和所述预瞄点处的侧偏角获取车辆预瞄控制传递函数；

根据所述预瞄点的轨迹曲率和所述车辆预瞄控制传递函数获取前轮转角。

优选地，所述预瞄点处的侧偏角和所述单轨质心侧偏角的关系为：

其中，β_P为预瞄点处的侧偏角，β为单轨质心侧偏角。

优选地，所述车辆预瞄控制传递函数为：

其中：

G_P＝l(1+KV²)

l＝l_f+l_r

上式中，β_P为预瞄点处的侧偏角；l_p为车辆前轮与预瞄点P的距离；ρ_P为预瞄点P的曲率半径。

优选地，所述根据所述期望路径和当前位姿获取前轮转角纠正量具体包括：

根据所述期望路径和当前位姿确定预瞄点位姿与理想轨迹点位姿；

根据所述预瞄点位姿与理想轨迹点位姿获取侧向偏差和航向偏差；

根据所述侧向偏差和所述航向偏差获取前轮转角纠正量。

优选地，所述根据所述侧向偏差和所述航向偏差获取前轮转角纠正量具体采用PID控制方式，控制方式如下公式所示：

k_yd(Δy(i)-Δy(i-1))+

k_θd(Δθ(i)-Δθ(i-1))

其中，Δδ为前轮转角纠正量，Δy为侧向偏差，Δθ为航向偏差，k_yp、k_yi、k_yd分别为侧向偏差的比例、积分和微分系数；k_θp、k_θi、k_θd分别为航向偏差的比例、积分和微分系数。

本发明实施例还提供一种自动驾驶车路径跟踪控制装置，包括：

信息获取单元，用于获取自动驾驶车辆的期望路径和当前位姿信息；

预瞄控制器，用于根据所述期望路径获取预瞄点处的侧偏角，并根据预设车辆动力学模型和所述预瞄点处的侧偏角进行预瞄控制以获取前轮转角；

补偿控制器，用于根据所述期望路径和当前位姿信息获取前轮转角纠正量；

方向盘转角确定单元，用于根据所述前轮转角纠正量和所述前轮转角以及传动比计算方向盘转角。

实施本发明实施例具有以下有益效果：

本发明实施例提出一种自动驾驶车路径跟踪控制方法以及实现该方法的控制装置，其基于人类驾驶员驾驶行为，设计了基于预瞄补偿控制理论的预瞄补偿控制单元，所述预瞄补偿控制单元包括预瞄控制器和补偿控制器，通过所述预瞄控制器确定前轮转角，并通过所述补偿控制单元确定前轮转角纠正量，最后结合前轮转角纠正量和前轮转角以及传动比计算方向盘转角。本发明实施例预瞄补偿控制单元相对于现有技术中的预瞄控制器，进一步提升了路径跟踪精度，补偿控制器的引入，提升了车辆自动驾驶系统在存在干扰的工况下(例如侧向风、路面不平等)的鲁棒性，从而提升了自动驾驶汽车的安全性。

附图说明

为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。

图1为本发明实施例一所述一种自动驾驶车路径跟踪控制方法流程图。

图2为本发明实施例一所述方法中预瞄控制图解。

图3为本发明实施例一所述方法中补偿控制图解。

图4为本发明实施例一所述一种自动驾驶车路径跟踪控制方法具体流程图。

图5为本发明实施例二所述一种自动驾驶车路径跟踪控制系统结构图。

图6为本发明实施例二所述一种自动驾驶车路径跟踪控制系统控制原理图。

具体实施方式

以下各实施例的说明是参考附图，用以示例本发明可以用以实施的特定实施例。

如图1所示，本发明实施例一提供一种自动驾驶车路径跟踪控制方法，所述方法具体包括如下步骤：

S1获取自动驾驶车辆的期望路径和当前位姿信息；

S2根据所述期望路径和当前位姿信息获取前轮转角纠正量；

S3根据所述期望路径获取预瞄点处的侧偏角；

S4根据车辆动力学模型和所述预瞄点处的侧偏角进行预瞄控制以获取前轮转角；

S5根据所述前轮转角纠正量和所述前轮转角以及传动比计算方向盘转角。

具体而言，在自动驾驶软件系统中，包含感知、决策、规划和路径跟踪控制四个主要部分，本发明实施例主要针对路径跟踪控制进行改进，而所述期望路径包括描述路径形状的函数或连续路点信息等。

本实施例中整个控制策略由两部分组成，即预瞄控制和补偿控制。具体而言，假如道路是一条左转的曲线，驾驶员必须根据经验将转向盘左转一定的角度。当然不是车辆到达弯道起点才转动转向盘，而是在这个操纵之前就不断的观察、考虑和决策，驾驶员根据参考路径来对车辆进行预瞄控制。在最理想的情况下，车辆能完美地行驶在参考路径上，但是实际的情况并不是这样，如前所述，在有侧风等因素的影响下，车辆总会偏离期望的参考路径，所以驾驶员要来观察实际车辆行驶的车道和期望车道的偏差，并试图通过继续转动方向盘来减小这个偏差以求达到补偿的目的。

本发明实施例根据侧向风等实时干扰因素反馈来对方向盘转角进行校正，能够提升自动驾驶系统在存在干扰的工况下的鲁棒性，例如侧向风、路面不平等工况，从而提升了自动驾驶汽车的安全性。

本实施例中，所述车辆动力学模型为单轨线性二自由度车辆动力学模型。

具体而言，在现有技术中，基于预瞄的转向控制通常是基于四或六自由度车辆模型，并包括反馈部分和前馈部分；但是，采用四或六自由度车辆模型，算法对驾驶域控制器计算力要求会更高、运行周期会更长，不能满足自动驾驶路径跟踪控制器实时性的需求。因此，本实施例采用经典的单轨二自由度车辆模型，也即自行车模型，模型只有前后两个车轮，轮胎载荷即为前后轴荷，并且假设车辆纵向车速不变，因此，前后轴荷是固定的，在高附着系数的路面上，单轨车辆模型对横向加速度0.4g以内的转弯工况具有足够好的描述精度。

如图2所示为本发明实施例一所述方法的一个更为具体的流程图，下面结合图2详细介绍本发明实施例一所述方法。

本实施例中做如下模型假设：

忽略转向系统超调和滞后特性的影响，直接以前轮转角作为输入；采用线性轮胎侧偏模型；忽略空气动力学作用；忽略左右轮胎由于载荷变化而引起的轮胎特性变化；车辆处于水平路面上，忽略纵向和横向坡度角；忽略车身的侧倾和俯仰运动，不考虑各车轮载荷的瞬态变化。

经过上述简化后，建立的单轨车辆模型，在车辆坐标系即相对坐标(x，y)中，车辆行驶的模型为：

其中，F_x1、F_y1分别为地面对前轮的纵向力和侧向力；F_x2、F_y2分别为地面对后轮的纵向力和侧向力；δ为前轮转角；β为单轨质心侧偏角；

为车辆横摆角

的二阶导数；I为车辆绕z轴的转动惯量；V为车辆速度；

为车辆速度V的一阶导数；m为车辆质量；l_f、l_r分别为单轨质心至前轮和后轮的距离；ρ为道路曲率半径；x为车辆行驶方向坐标，y为与车辆行驶方向垂直的坐标。

本实施例中，当β、δ较小时(β、δ小于5°)有：

其中，K_f前轮侧偏刚度，K_r为后轮侧偏刚度。

因此，式(4)，式(5)，式(6)可简化为：

本实施例中，当车辆匀速行驶时，

由式(8)和式(9)可得：

设稳定性因数为

其单位为s²/m²；所述稳定性因数为表征汽车稳态的因数。

对式(10)和式(11)进行拉氏变换，并令相应初始条件为零得：

其中，

其中，

由于侧向加速度，

因此，

其中，

其中，在给定期望路径的情况下，驾驶员应如何转动转向盘，才能使车辆前面某点能准确地沿着期望路径运动。假设这个点和车辆固定相连，车辆能完美地跟随参考路径没有偏差，这种驾驶员就是理想的驾驶员。

本实施例中，所述S3具体包括如下子步骤：

S31根据所述期望路径确定预瞄距离，所述预瞄距离等于单轨质心至前轮的距离加上单轨质心至预瞄点的距离；

S32根据所述预瞄距离确定预瞄点的轨迹曲率；

S33根据所述预瞄点的轨迹曲率确定预瞄点处侧偏角。

具体而言，如图3所示，P为驾驶员预瞄点，其在车辆前轮的前方l_p处。由于驾驶员在车辆上的位置约为单轨质心SP处，所以总的预瞄距离为l_f+l_p。

其中，预瞄点P的轨迹曲线，其曲率为：

P点的侧偏角β_P可以由下式求出：

在β较小的情况下，有sinβ＝β，cosβ＝1，经线性化处理后得：

可得P点轨迹曲线的曲率1/ρ_P与单轨质心SP的轨迹曲线的曲率1/ρ_SP之间的关系：

以及单轨质心SP处的侧偏角速度

经积分得：

本实施例中，所述S4具体包括如下子步骤：

S41基于所述预瞄点处侧偏角和所述单轨质心侧偏角的关系，根据所述车辆动力学模型和所述预瞄点处侧偏角确定车辆预瞄控制传递函数；其中，

S42根据所述预瞄点的轨迹曲率和所述车辆预瞄控制传递函数确定前轮转角。

其中，所述车辆预瞄控制传递函数推导过程如下：

将式(30)代入式(12)，可得：

对上式进行拉氏变化，初值为零，得到车辆预瞄控制传递函数：

上式中：

l＝l_f+l_r

G_P＝l(1+KV²)

其中，β_P为预瞄点处侧偏角；l_p为车辆前轮与预瞄点P的距离；ρ_P为预瞄点P的曲率半径。

需说明的是，由于车辆在转向时有方向性，因此本实施例中曲率具有方向性。

综上推导，基于所述车辆预瞄控制传递函数，根据道路曲率通过预瞄控制传递函数即可以得到预瞄控制所需要的前轮转角输出。

本实施例中，所述S2包括如下子步骤：

S21根据所述期望路径和当前位姿确定预瞄点位姿与理想轨迹点位姿；

S22根据所述预瞄点位姿与理想轨迹点位姿获取侧向偏差和航向偏差；

S23根据所述侧向偏差和所述航向偏差获取前轮转角纠正量。

其中，所述S23根据确定所述侧向偏差和所述航向偏差确定前轮转角纠正量具体采用PID控制方式，图4为本发明实施例方法的补偿控制部分图解，下面结合图4对所述控制方式进行说明。

在本实施例中，由于存在侧向风等干扰的影响，无人驾驶车和实际路径之间可能存在偏差，因此除预瞄控制外，为了更好的跟踪期望的路径还需要对车辆进行补偿控制。车辆与当前路径的偏差可通过两个量表示，即侧向偏差和航向角偏差，其几何关系如如图4所示。

前视点位姿(X_p,Y_p,θ)与理想轨迹点位姿(X_d,Y_d,θ_d)的侧向偏差为：

航向角偏差为：

Δθ＝θ_d-θ (34)

在补偿控制中采用PID控制，输出量Δδ，即对预瞄控制的补偿量：

k_yd(Δy(i)-Δy(i-1))+

k_θd(Δθ(i)-Δθ(i-1)) (35)

其中，Δδ为前轮转角纠正量，Δy为侧向偏差，Δθ为航向偏差，k_yp、k_yi、k_yd分别为侧向偏差的比例、积分和微分系数；k_θp、k_θi、k_θd分别为航向偏差的比例、积分和微分系数。

如图5所示，本发明实施例二还提供一种自动驾驶车路径跟踪控制系统，其用于实现实施例一所述的自动驾驶车路径跟踪控制方法，包括：

信息获取单元1，用于获取自动驾驶车辆的期望路径和当前位姿信息；

预瞄控制器2，用于根据所述期望路径获取预瞄点处的侧偏角，并根据预设车辆动力学模型和所述预瞄点处的侧偏角进行预瞄控制以获取前轮转角；

补偿控制器3，用于根据所述期望路径和当前位姿信息获取前轮转角纠正量；

方向盘转角确定单元4，用于根据所述前轮转角纠正量和所述前轮转角以及传动比计算方向盘转角。

其中，本实施例所述控制系统的控制原理如图6所示。

本实施例二中，所述补偿控制器3的输入为自动驾驶的期望路径(描述路径形状的函数或连续路点信息等)及车辆位姿信息，所述预瞄控制器2的输入为自动驾驶的期望路径(描述路径形状的函数或连续路点信息等)，所述预瞄控制器经过计算得出方向盘需要的转向角，所述补偿控制器根据侧向误差进一步纠正转向角得到最终的方向盘转角。

对于实施例二公开的装置而言，由于其与实施例一公开的方法相对应，相关之处参见方法部分说明即可，此处不再赘述。

通过以上实施例的描述可知，本发明实施例提出一种自动驾驶车路径跟踪控制方法以及实现该方法的控制装置，其基于人类驾驶员驾驶行为，设计了基于预瞄补偿控制理论的预瞄补偿控制单元，所述预瞄补偿控制单元包括预瞄控制器和补偿控制器，通过所述所述预瞄控制器确定前轮转角，并通过所述补偿控制单元确定前轮转角纠正量，最后结合前轮转角纠正量和前轮转角以及传动比计算方向盘转角。本发明实施例预瞄补偿控制单元相对于现有技术中的预瞄控制器，进一步提升了路径跟踪精度，补偿控制器的引入，提升了车辆自动驾驶系统在存在干扰的工况下(例如侧向风、路面不平等)的鲁棒性，从而提升了自动驾驶汽车的安全性。

以上所揭露的仅为本发明较佳实施例而已，当然不能以此来限定本发明之权利范围，因此依本发明权利要求所作的等同变化，仍属本发明所涵盖的范围。

Claims (7)

1.一种自动驾驶车路径跟踪控制方法，其特征在于，包括如下步骤：

获取自动驾驶车辆的期望路径和当前位姿信息；

根据所述期望路径和当前位姿信息获取前轮转角纠正量；

根据所述期望路径获取预瞄点处的侧偏角；

根据车辆动力学模型和所述预瞄点处的侧偏角进行预瞄控制以获取前轮转角；

根据所述前轮转角纠正量和所述前轮转角以及传动比计算方向盘转角；

所述根据所述期望路径获取预瞄点处的侧偏角具体包括：

根据所述期望路径确定预瞄距离，所述预瞄距离等于单轨质心至前轮的距离加上单轨质心至预瞄点的距离；

根据所述预瞄距离确定预瞄点的轨迹曲率；

根据所述预瞄点的轨迹曲率获取预瞄点处的侧偏角；

所述车辆动力学模型为单轨线性二自由度车辆动力学模型，如下公式所示：

其中，F_x1、F_y1分别为地面对前轮的纵向力和侧向力；F_x2、F_y2分别为地面对后轮的纵向力和侧向力；δ为前轮转角；β为单轨质心侧偏角；

为车辆横摆角

的二阶导数；I为车辆绕z轴的转动惯量；V为车辆速度；

为车辆速度V的一阶导数；m为车辆质量；l_f、l_r分别为单轨质心至前轮和后轮的距离；ρ为道路曲率半径；x为车辆行驶方向坐标，y为与车辆行驶方向垂直的坐标。

2.如权利要求1所述的自动驾驶车路径跟踪控制方法，其特征在于，所述根据车辆动力学模型和预瞄点处的侧偏角进行预瞄控制获取前轮转角具体包括：

基于所述预瞄点处的侧偏角和所述单轨质心侧偏角的关系，根据所述车辆动力学模型和所述预瞄点处的侧偏角获取车辆预瞄控制传递函数；

根据所述预瞄点的轨迹曲率和所述车辆预瞄控制传递函数获取前轮转角。

3.如权利要求2所述的自动驾驶车路径跟踪控制方法，其特征在于，所述预瞄点处的侧偏角和所述单轨质心侧偏角的关系为：

其中，β_P为预瞄点处的侧偏角，β为单轨质心侧偏角。

4.如权利要求2所述的自动驾驶车路径跟踪控制方法，其特征在于，所述车辆预瞄控制传递函数为：

其中：

G_P＝l(1+KV²)

l＝l_f+l_r

上式中，s为复频率，β_P为预瞄点处的侧偏角；l_p为车辆前轮与预瞄点P的距离；ρ_P为预瞄点P的曲率半径；K_f为前轮侧偏刚度，K_r为后轮侧偏刚度。

5.如权利要求1所述的自动驾驶车路径跟踪控制方法，其特征在于，所述根据所述期望路径和当前位姿获取前轮转角纠正量具体包括：

根据所述期望路径和当前位姿确定预瞄点位姿与理想轨迹点位姿；

根据所述预瞄点位姿与理想轨迹点位姿获取侧向偏差和航向偏差；

根据所述侧向偏差和所述航向偏差获取前轮转角纠正量。

6.如权利要求5所述的自动驾驶车路径跟踪控制方法，其特征在于，所述根据所述侧向偏差和所述航向偏差获取前轮转角纠正量具体采用PID控制方式，控制方式如下公式所示：

其中，Δδ为前轮转角纠正量，Δy为侧向偏差，Δθ为航向偏差，k_yp、k_yi、k_yd分别为侧向偏差的比例、积分和微分系数；k_θp、k_θi、k_θd分别为航向偏差的比例、积分和微分系数。

7.一种自动驾驶车路径跟踪控制装置，其特征在于，用于实施如权利要求1所述的自动驾驶车路径跟踪控制方法，所述自动驾驶车路径跟踪控制装置包括：

信息获取单元，用于获取自动驾驶车辆的期望路径和当前位姿信息；

预瞄控制器，用于根据所述期望路径获取预瞄点处的侧偏角，并根据预设车辆动力学模型和所述预瞄点处的侧偏角进行预瞄控制以获取前轮转角；

补偿控制器，用于根据所述期望路径和当前位姿信息获取前轮转角纠正量；

方向盘转角确定单元，用于根据所述前轮转角纠正量和所述前轮转角以及传动比计算方向盘转角。

Images