一种基于预瞄点的车辆路径跟踪侧向偏差计算方法

1.本发明涉及车辆路径跟踪控制技术领域，具体是一种基于预瞄点的车辆路径跟踪侧向偏差计算方法。


背景技术：

2.近些年来，随着智能交通、物联网和人工智能等科学技术的快速发展，越来越多的机构及企业对辅助驾驶和无人驾驶引起了重视。路径跟踪控制作为智能车辆研究的重要组成部分，对智能驾驶的安全性起着至关重要的作用。路径跟踪控制主要是通过对车辆的转向控制系统和驱动控制系统来保证车辆的质心与期望路径中心线的横向误差为0，使车辆对期望路径有一个比较强的跟踪能力。目前基于驾驶员预瞄特性的路径跟踪控制方法被广大研究人员使用，通过计算车辆经过预瞄时间tp后到达的实际位置与期望路径上的预瞄点的侧向偏差，然后再通过侧向偏差来实时计算出前轮转角从而达到路径跟踪控制。但是现有计算侧向偏差的方法存在计算复杂、结果不准确的问题。


技术实现要素：

3.本发明的目的在于克服现有技术的不足，而提供一种基于预瞄点的车辆路径跟踪侧向偏差计算方法，该方法解决了现有车辆路径跟踪控制侧向偏差计算过程复杂、计算结果不准确的问题。
4.实现本发明目的的技术方案是：
5.一种基于预瞄点的车辆路径跟踪侧向偏差计算方法，设xay为大地坐标系；xcy为车辆坐标系；xcy是以车辆质心为原点的车辆坐标系，车辆坐标系的x轴为车辆前进方向，y轴为驾驶员左侧与x轴垂直方向，车速为v，车速在x轴上的分量为u，车速在y轴上的分量为τ，ψ为车辆当前时刻的航向角，β为车辆的质心侧偏角，c(xc，yc)点为车辆质心坐标；p点为车辆在位置c时的预瞄点，即车辆经过预瞄时间tp后的目标位置；q点为车辆经过预瞄时间tp后到达的实际位置，弧cq为车辆tp时间内的轨迹，o点为汽车轨迹的圆心，θ为圆弧轨迹所对应的圆心角，为车辆的横摆角速度，m点是q点过车辆坐标系x轴作垂足的交点，d点是延长车速与qm的交点；假设汽车在未来一段时间内保持不变的横摆角速度，且由于车速τ远小于u，则认为汽车合速速的大小保持不变，在该假设条件下，汽车在未来一段时间内将作匀速圆周运动，且汽车合速度的方向始终与其轨迹相切；若已知p点和q点在车辆坐标系下的y坐标y
p
和yq，则有ey＝y
p-yq，ey为经过预瞄时间tp后的车辆侧向偏差，分别求解y
p
和yq，得到ey；具体步骤如下：
6.1)计算经过预瞄时间tp后圆弧轨迹所对应的圆心角∠θ，若车辆横摆角速度为则圆心角
7.2)计算∠qcm：∠qcd为车辆经过t
p
时间的圆弧轨迹对应的与车速v的夹角，由弦切角定理得∠dcm是车速v与车辆坐标系x轴的夹角，即车辆质心侧偏角β，有
∠dcm＝β，则有
8.3)计算q点在车辆坐标系下的侧向坐标yq，在三角形qcm中，已知，在三角形qcm中，已知cm＝tp
·
u，则
9.4)将参考轨迹在大地坐标系下进行离散化，离散后的参考轨迹上的点用{j0，j 1
，j 2
，
…
，j n
}的序列来表示，其中j0、j n
分别为参考轨迹的起点和终点，若是实时采集的参考轨迹，则n是无穷的，取n＝200，参考轨迹上的点在大地坐标系下的坐标记为(xi，yi)，其中0≤i≤n，且i和j的下标相对应，相邻两个点的距离记为l0，取l0＝1m；
10.5)在参考轨迹上找到预瞄点p；
11.6)将参考轨迹上的点在大地坐标下的坐标转化为在车辆坐标系下的坐标；
12.7)找到参考轨迹上距离当前车辆位置最近的点j
p
；
13.8)确定预瞄点p序号，p＝p m为参考轨迹离散后的预瞄点序号，其中p为车辆当前距离参考轨迹最近点的序号，m为经过预瞄时间t
p
后所增加的序号；
14.9)计算预瞄点p在车辆坐标系下的侧向坐标y
p
＝-(x
p m-xc)sinψ (y
p m-yc)cosψ，其中x
p m
为预瞄点p在大地坐标系下的横坐标，y
p m
为预瞄点p在大地坐标系下的纵坐标，则经过预瞄时间tp后的车辆侧向偏差ey＝y
p-yq；
15.经过上述步骤，计算出了跟踪参考轨迹的车辆侧向偏差。
16.本发明提供的一种基于预瞄点的车辆路径跟踪侧向偏差计算方法，该方法首先通过将参考轨迹上的离散点的坐标由大地坐标系下转换成车辆坐标系下，预瞄距离很方便的确定了预瞄点，在求出轨迹上离车辆当前位置最近的点，进一步求得预瞄点的序号，从而计算出车辆经过预瞄时间到达的预瞄点与车辆实际到达的点的侧向偏差，再通过侧向偏差得出适当的前轮转角。本发明所述的方法原理简单易理解，且计算过程简便快速，能够很好的实现车辆跟踪参考轨迹的目的。
附图说明
17.图1为本发明一种基于预瞄点的车辆路径跟踪侧向偏差计算方法的流程图；
18.图2为本发明实施例的大地坐标系xay下的汽车轨迹预测图；
19.图3为参考轨迹上预瞄点的确定原理图。
具体实施方式
20.下面结合附图和实施例对本发明内容做进一步阐述，但不是对本发明的限定。
21.实施例：
22.如图1所示，一种基于预瞄点的车辆路径跟踪侧向偏差计算方法，首先通过将参考轨迹上的离散点的坐标由大地坐标系下转换成车辆坐标系下，然后用预瞄距离很方便的确定了预瞄点，在求出轨迹上离车辆当前位置最近的点，进一步求得预瞄点的序号，从而计算出车辆经过预瞄时间到达的预瞄点与车辆实际到达的点的侧向偏差，再通过侧向偏差得出适当的前轮转角，能够很好的实现车辆跟踪参考轨迹的目的。
23.真实驾驶员都具有一定的汽车轨迹预测能力，其能够感知汽车运动状态并预测汽
车在一段时间后的大致位置。如图2所示，图2中包含大地坐标系xay和车辆坐标系xcy；xcy是以车辆质心为原点的车辆坐标系，车辆坐标系的x轴为车辆前进方向，y轴为驾驶员左侧与x轴垂直方向，车速为v，车速在x轴上的分量为u，车速在y轴上的分量为τ，ψ为车辆当前时刻的航向角，β为车辆的质心侧偏角，c(xc，yc)点为车辆质心坐标；p点为车辆在位置c时的预瞄点，即车辆经过预瞄时间tp后的目标位置；q点为车辆经过预瞄时间tp后到达的实际位置，弧cq为车辆tp时间内的轨迹，o点为汽车轨迹的圆心，θ为圆弧轨迹所对应的圆心角，为车辆的横摆角速度，m点是q点过车辆坐标系x轴作垂足的交点，d点是延长车速与qm的交点。
24.假设汽车在未来一段时间内保持不变的横摆角速度，且由于τ常远小于u可认为汽车合速度的大小保持不变。在该假设条件下，汽车在未来一段时间内将作匀速圆周运动，且汽车合速度的方向始终与其轨迹相切。则车辆侧向偏差计算方法，具体包括如下步骤：
25.1)计算经过预瞄时间tp后圆弧轨迹所对应的圆心角∠θ，若车辆横摆角速度为则圆心角
26.2)计算∠qcm：∠qcd为车辆经过t
p
时间的圆弧轨迹对应的与车速v的夹角，由弦切角定理得∠dcm是车速v与车辆坐标系x轴的夹角，即车辆质心侧偏角β，有∠dcm＝β，则有
27.3)计算q点在车辆坐标系下的侧向坐标yq，在三角形qcm中，已知，在三角形qcm中，已知cm＝tp
·
u，则
28.4)如图3所示，将参考轨迹在大地坐标系下进行离散化，离散后的参考轨迹上的点用{j0，j1，j2，
…
，jn}的序列来表示，其中j0、jn分别为参考轨迹的起点和终点，若是实时采集的参考轨迹，则n是无穷的，取n＝200，参考轨迹上的点在大地坐标系下的坐标记为(xi，yi)，其中0≤i≤n，且i和j的下标相对应，相邻两个点的距离记为l0，取l0＝1m；
29.5)在参考轨迹上找到预瞄点p，图2所示，以车辆质心c为圆心，以预瞄距离为半径画圆，这个圆通常与参考轨迹往往有两个交点，通过预瞄距离画的圆与参考轨迹的交点并不一定是我们设定好的目标采集上的点，也有可能在两个点之间，通常取车辆行驶方向上前进的那个点为预瞄点；
30.6)将参考轨迹上的点在大地坐标下的坐标转化为在车辆坐标系下的坐标，假设车辆当前坐标为(xc，yc),车辆相对于大地坐标系的航向角为ψ，则有参考轨迹上的点在车辆坐标系下的坐标为
31.7)找到参考轨迹上距离当前车辆位置最近的点j
p
(0≤p≤n)，车辆质心与参考轨迹上的离散点距离可表示为遍历每个离散点，可得到参考轨迹上的点距离当前车辆位置最小距离d
min
，则最近的点取为j
p
；
32.8)确定预瞄点p序号，由于弧段长约等于预瞄距离，则可得m的值为
由于m是整数且为车辆前方的点，则可取可确定预瞄点序号
33.9)计算预瞄点p在车辆坐标系下的侧向坐标y
p
＝-(x
p m-xc)sinψ (y
p m-yc)cosψ，其中x
p m
为预瞄点p在大地坐标系下的横坐标，y
p m
为预瞄点p在大地坐标系下的纵坐标，则经过预瞄时间tp后的车辆侧向偏差ey＝y
p-yq；
34.经过上述步骤，计算出了车辆跟踪参考轨迹的侧向偏差，在后续的车辆控制中，根据跟踪参考轨迹的车辆侧向偏差计算出前轮转角，从而达到路径跟踪控制，实现车辆跟踪参考轨迹的目的。
35.以上实施例，首先通过将参考轨迹上的离散点的坐标由大地坐标系下转换成车辆坐标系下，然后用预瞄距离很方便的确定了预瞄点，在求出轨迹上离车辆当前位置最近的点，进一步求得预瞄点的序号，从而计算出车辆经过预瞄时间到达的预瞄点与车辆实际到达的点的侧向偏差，再通过侧向偏差得出适当的前轮转角，能够很好的实现车辆跟踪参考轨迹的目的。