一种自动泊车轨迹跟踪方法及系统、计算机可读存储介质与流程

1.本发明涉及自动泊车技术领域，具体涉及一种自动泊车轨迹跟踪方法及系统、计算机可读存储介质。


背景技术：

2.自动泊车是高级辅助驾驶的一项重要辅助功能，在车辆的自动泊车过程中，需要进行自动泊车轨迹跟踪，现有的一些自动泊车轨迹跟踪算法计算量大，对硬件要求较高；例如，根据车速确定预瞄距离，将大地坐标系下期望路径离散点序列转换为车辆坐标系下离散点，再根据预瞄距离和目标距离差异确定轨迹跟踪点，再根据车辆质心和轨迹跟踪点横向位移误差计算车辆前轮角度；在该方法中期望路径离散点计算本来就运算量大，数据多又占内存，再加上坐标系转换，再去求跟踪点，使得算法计算量较大，对硬件有较高的性能要求。


技术实现要素：

3.本发明的目的在于提出一种自动泊车轨迹跟踪方法及系统、计算机可读存储介质，以减少跟踪控制算法计算量，降低对算法对硬件的性能要求。
4.为实现上述目的，本发明第一方面提出一种自动泊车轨迹跟踪方法，包括：
5.获取当前车速和当前车辆挡位，并根据所述当前车速和当前车辆挡位确定当前预瞄距离；
6.获取泊车轨迹规划段、当前车辆位置，并根据所述泊车轨迹规划段、所述当前预瞄距离和所述当前车辆位置确定当前预瞄跟踪点位置；
7.获取当前车辆航向角，并根据所述当前车辆位置、所述当前车辆航向角、所述当前预瞄距离和所述当前预瞄跟踪点位置确定当前前轮转角；
8.根据所述当前前轮转角确定当前方向盘转角。
9.可选地，所述根据所述当前车速和当前车辆挡位确定当前预瞄距离，包括：
10.若当前车辆挡位为前进挡位，则当前预瞄距离l为：
11.l＝k
×
v+l
min
；
12.若当前车辆挡位为后退挡位，则当前预瞄距离l为：
13.l＝-k
×v–
l
min
；
14.并且，|l|小于等于预设l
max
；
15.其中，k为预设的系数，v为当前车速，l
min
为预设的最小视距，l
max
为预设的最大视距。
16.可选地，所述根据所述泊车轨迹规划段、所述当前预瞄距离和所述当前车辆位置确定当前预瞄跟踪点位置，包括：
17.判断所述泊车轨迹规划段的起始点和终点之间的横坐标变化量是否大于纵坐标变化量；
18.若是，则当前预瞄跟踪点的初始横坐标等于当前车辆位置横坐标加上所述当前预瞄距离，并根据所述初始横坐标与所述泊车轨迹规划段的位置关系确定当前预瞄跟踪点位置的横、纵坐标；
19.若否，则当前预瞄跟踪点的初始纵坐标等于当前车辆位置纵坐标加上所述当前预瞄距离，并根据所述初始纵坐标与所述泊车轨迹规划段的位置关系确定当前预瞄跟踪点位置的横、纵坐标。
20.可选地，其中，所述根据所述初始横坐标与所述泊车轨迹规划段的位置关系确定当前预瞄跟踪点位置的横、纵坐标，具体包括：若所述初始横坐标大于泊车轨迹规划段终点横坐标，则当前预瞄跟踪点位置的横、纵坐标确定为泊车轨迹规划段终点的横、纵坐标；若所述初始横坐标小于泊车轨迹规划段起始点横坐标，则当前预瞄跟踪点位置的横、纵坐标确定为泊车轨迹规划段起始点的横、纵坐标；
21.其中，所述根据所述初始纵坐标与所述泊车轨迹规划段的位置关系确定当前预瞄跟踪点位置的横、纵坐标，具体包括：若所述初始纵坐标大于泊车轨迹规划段终点纵坐标，则当前预瞄跟踪点位置的横、纵坐标确定为泊车轨迹规划段终点的横、纵坐标；若所述初始纵坐标小于泊车轨迹规划段起始点纵坐标，则当前预瞄跟踪点位置的横、纵坐标确定为泊车轨迹规划段起始点的横、纵坐标。
22.可选地，若所述泊车轨迹规划段为圆弧，则所述根据所述初始横坐标与所述泊车轨迹规划段的位置关系确定当前预瞄跟踪点位置的横、纵坐标，具体包括：若所述初始横坐标大于等于圆弧起始点横坐标，且小于等于圆弧终点横坐标，则确定所述初始横坐标作为当前预瞄跟踪点位置横坐标，并根据圆弧的圆心坐标和半径确定当前预瞄跟踪点位置纵坐标；
23.并且，所述根据所述初始纵坐标与所述泊车轨迹规划段的位置关系确定当前预瞄跟踪点位置的横、纵坐标，具体包括：若所述初始纵坐标大于等于圆弧起始点纵坐标，且小于等于圆弧终点纵坐标，则确定所述初始纵坐标作为当前预瞄跟踪点位置纵坐标，并根据圆弧的圆心坐标和半径确定当前预瞄跟踪点位置横坐标。
24.可选地，若所述泊车轨迹规划段为直线，则所述根据所述初始横坐标与所述泊车轨迹规划段的位置关系确定当前预瞄跟踪点位置的横、纵坐标，具体包括：若所述初始横坐标大于等于直线起始点横坐标，且小于等于直线终点横坐标，则确定所述初始横坐标作为当前预瞄跟踪点位置横坐标，并根据所述直线所对应的直线方程以及所述初始横坐标确定当前预瞄跟踪点位置纵坐标；
25.并且，所述根据所述初始纵坐标与所述泊车轨迹规划段的位置关系确定当前预瞄跟踪点位置的横、纵坐标，具体包括：若所述初始纵坐标大于等于直线起始点纵坐标，且小于等于直线终点纵坐标，则确定所述初始纵坐标作为当前预瞄跟踪点位置纵坐标，并根据所述直线所对应的直线方程以及所述初始纵坐标确定当前预瞄跟踪点位置横坐标。
26.可选地，所述根据所述当前车辆位置、所述当前车辆航向角、所述当前预瞄距离和所述当前预瞄跟踪点位置确定当前前轮转角，包括：
27.具体根据以下公式计算确定当前前轮转角；
[0028][0029][0030]
其中，δ为当前前轮转角，l为给定的车辆轴距，l为当前预瞄距离，ax为当前车辆位置的横坐标，ay为当前车辆位置的纵坐标，bx为当前预瞄跟踪点位置的横坐标，by为当前预瞄跟踪点位置的纵坐标，yaw为当前车辆航向角。
[0031]
可选地，所述根据所述当前前轮转角确定当前方向盘转角，包括：
[0032]
具体根据以下公式计算确定当前方向盘转角；
[0033]
θ＝kδ+b
[0034]
其中，θ为当前方向盘转角，k预设的线性拟合系数，δ为当前前轮转角，b为预设的线性拟合偏置常数。
[0035]
本发明第二方面提出一种自动泊车轨迹跟踪系统，用于实现第一方面所述自动泊车轨迹跟踪方法，所述系统包括：
[0036]
预瞄距离确定单元，用于获取当前车速和当前车辆挡位，并根据所述当前车速和当前车辆挡位确定当前预瞄距离；
[0037]
预瞄跟踪点确定单元，用于获取泊车轨迹规划段、当前车辆位置，并根据所述泊车轨迹规划段、所述当前预瞄距离和所述当前车辆位置确定当前预瞄跟踪点位置；
[0038]
前轮转角确定单元，用于根据所述当前车辆位置、所述当前车辆航向角、所述当前预瞄距离和所述当前预瞄跟踪点位置确定当前前轮转角；以及
[0039]
方向盘转角确定单元，用于根据所述当前前轮转角确定当前方向盘转角。
[0040]
本发明第三方面提出一种计算机可读存储介质，其上存储有计算机程序，所述计算机程序被处理器执行时实现第一方面所述自动泊车轨迹跟踪方法。
[0041]
综上，本发明的实施例提出一种自动泊车轨迹跟踪方法及系统、计算机可读存储介质，包括根据当前车速和当前车辆挡位确定当前预瞄距离；并根据泊车轨迹规划段、当前预瞄距离和当前车辆位置确定当前预瞄跟踪点位置；以及根据当前车辆位置、当前车辆航向角、当前预瞄距离和当前预瞄跟踪点位置确定当前前轮转角；最后，根据当前前轮转角确定当前方向盘转角。本发明实施例在进行自动泊车轨迹跟踪时，其预瞄跟踪点为动态根据车辆实时位置进行计算获得，不需要预瞄序列点，直接采用规划路径和车辆实时位置信息在大地坐标系，相对于传统将大地坐标系下期望路径离散点序列转换为车辆坐标系下离散点，再根据预瞄距离和目标距离差异确定轨迹跟踪点的方式，取消了坐标转换的繁琐运算，无需预瞄序列点，能够减少跟踪控制算法计算量，节省cpu内存空间，降低对算法对硬件的性能要求。
[0042]
本发明的其它特征和优点将在随后的说明书中阐述。
附图说明
[0043]
为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本
发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。
[0044]
图1为本发明一实施例中一种自动泊车轨迹跟踪方法流程图。
[0045]
图2为本发明一实施例中一种自动泊车轨迹跟踪系统结构示意图。
具体实施方式
[0046]
以下将参考附图详细说明本公开的各种示例性实施例、特征和方面。另外，为了更好的说明本发明，在下文的具体实施例中给出了众多的具体细节。本领域技术人员应当理解，没有某些具体细节，本发明同样可以实施。在一些实例中，对于本领域技术人员熟知的手段未作详细描述，以便于凸显本发明的主旨。
[0047]
参阅图1，本发明一实施例提出一种自动泊车轨迹跟踪方法，包括如下步骤s1～s4：
[0048]
步骤s1、获取当前车速和当前车辆挡位，并根据所述当前车速和当前车辆挡位确定当前预瞄距离；
[0049]
示例性地，所述根据所述当前车速和当前车辆挡位确定当前预瞄距离，包括：
[0050]
若当前车辆挡位为前进挡位，则当前预瞄距离l为：
[0051]
l＝k
×
v+l
min
；
[0052]
若当前车辆挡位为后退挡位，则当前预瞄距离l为：
[0053]
l＝-k
×v–
l
min
；
[0054]
并且，|l|小于等于预设l
max
；
[0055]
其中，k为预设的系数，v为当前车速，l
min
为预设的最小视距，l
max
为预设的最大视距；
[0056]
具体而言，本实施例获取的车速v是标量，因此本实施例中由车辆档位来决定预瞄距离方向，本实施例中对预瞄距离l的绝对值做限幅，防止预瞄距离l超出设定范围；
[0057]
步骤s2、获取泊车轨迹规划段、当前车辆位置，并根据所述泊车轨迹规划段、所述当前预瞄距离和所述当前车辆位置确定当前预瞄跟踪点位置；
[0058]
示例性地，设当前车辆位置在大地坐标系中为a点，当前预瞄跟踪点位置在大地坐标系中为b点，泊车轨迹规划段的起始点在大地坐标系中为q，终点在大地坐标系中为z；
[0059]
则，所述根据所述泊车轨迹规划段、所述当前预瞄距离和所述当前车辆位置确定当前预瞄跟踪点位置，包括：
[0060]
步骤s21、判断所述泊车轨迹规划段的起始点q和终点z之间的横坐标变化量是否大于纵坐标变化量；
[0061]
具体而言，所述横坐标变化量为起始点q和终点z的横坐标之差的绝对值，所述纵坐标变化量为起始点q和终点z的纵坐标之差的绝对值；
[0062]
步骤s22、若上述步骤s21的判断结果为是，则当前预瞄跟踪点b的初始横坐标等于当前车辆位置a横坐标ax加上所述当前预瞄距离l，并根据所述初始横坐标与所述泊车轨迹规划段的位置关系确定当前预瞄跟踪点b位置的横、纵坐标(bx，by)；
[0063]
其中，若所述初始横坐标大于泊车轨迹规划段终点z横坐标，即所述初始横坐标超出泊车轨迹规划段终点z横坐标，则当前预瞄跟踪点b位置的横、纵坐标(bx，by)确定为泊车
轨迹规划段终点z的横、纵坐标；
[0064]
其中，若所述初始横坐标小于泊车轨迹规划段起始点q横坐标，即所述初始横坐标未到泊车轨迹规划段终点q横坐标，则当前预瞄跟踪点b位置的横、纵坐标(bx，by)确定为泊车轨迹规划段起始点q的横、纵坐标；
[0065]
步骤s23、若上述步骤s21的判断结果为否，则当前预瞄跟踪点b的初始纵坐标等于当前车辆位置a纵坐标ay加上所述当前预瞄距离l，并根据所述初始纵坐标与所述泊车轨迹规划段的位置关系确定当前预瞄跟踪点b位置的横、纵坐标(bx，by)；
[0066]
其中，若所述初始纵坐标大于泊车轨迹规划段终点z纵坐标，即所述初始纵坐标超出泊车轨迹规划段终点z纵坐标，则当前预瞄跟踪点b位置的横、纵坐标(bx，by)确定为泊车轨迹规划段终点z的横、纵坐标；
[0067]
其中，若所述初始纵坐标小于泊车轨迹规划段起始点q纵坐标，即所述初始纵坐标未到泊车轨迹规划段终点q纵坐标，则当前预瞄跟踪点b位置的横、纵坐标(bx，by)确定为泊车轨迹规划段起始点q的横、纵坐标。
[0068]
更具体地，所述泊车轨迹规划段为圆弧或直线；
[0069]
其中，若所述泊车轨迹规划段为圆弧，则所述根据所述初始横坐标与所述泊车轨迹规划段的位置关系确定当前预瞄跟踪点b位置的横、纵坐标(bx，by)，具体还包括：
[0070]
若所述初始横坐标大于等于圆弧起始点q横坐标，且小于等于圆弧终点z横坐标，则确定所述初始横坐标作为当前预瞄跟踪点b位置横坐标bx，并根据圆弧的圆心坐标和半径确定当前预瞄跟踪点b位置纵坐标by；
[0071]
具体而言，根据圆弧的圆心坐标和半径可以确定圆弧所对应的公式方程，进一步地，将确定得到的当前预瞄跟踪点b位置横坐标bx代入该公式方程中，即可以计算确定当前预瞄跟踪点b位置纵坐标by；
[0072]
并且，所述根据所述初始纵坐标与所述泊车轨迹规划段的位置关系确定当前预瞄跟踪点b位置的横、纵坐标，具体还包括：
[0073]
若所述初始纵坐标大于等于圆弧起始点q纵坐标，且小于等于圆弧终点z纵坐标，则确定所述初始纵坐标作为当前预瞄跟踪点b位置纵坐标by，并根据圆弧的圆心坐标和半径确定当前预瞄跟踪点b位置横坐标bx；
[0074]
具体而言，根据圆弧的圆心坐标和半径可以确定圆弧所对应的公式方程，进一步地，将确定得到的当前预瞄跟踪点b位置纵坐标by代入该公式方程中，即可以计算确定当前预瞄跟踪点b位置横坐标bx。
[0075]
其中，若所述泊车轨迹规划段为直线，则所述根据所述初始横坐标与所述泊车轨迹规划段的位置关系确定当前预瞄跟踪点b位置的横、纵坐标(bx，by)，具体还包括：
[0076]
若所述初始横坐标大于等于直线起始点q横坐标，且小于等于直线终点z横坐标，即横坐标bx在所述泊车轨迹规划段圆弧qz上，则确定所述初始横坐标作为当前预瞄跟踪点b位置横坐标bx，并根据所述直线所对应的直线方程以及所述初始横坐标确定当前预瞄跟踪点b位置纵坐标；
[0077]
具体而言，将当前预瞄跟踪点b位置横坐标bx代入所述直线所对应的直线公式方程，即可计算得到当前预瞄跟踪点b位置纵坐标by；
[0078]
并且，所述根据所述初始纵坐标与所述泊车轨迹规划段的位置关系确定当前预瞄
跟踪点b位置的横、纵坐标(bx，by)，具体包括：
[0079]
若所述初始纵坐标大于等于直线起始点q纵坐标，且小于等于直线终点z纵坐标，即横坐标bx在所述泊车轨迹规划段直线qz上，则确定所述初始纵坐标作为当前预瞄跟踪点b位置纵坐标by，并根据所述直线所对应的直线方程以及所述初始纵坐标确定当前预瞄跟踪点b位置横坐标bx；
[0080]
具体而言，将当前预瞄跟踪点b位置横坐标bx代入所述直线所对应的直线公式方程，即可计算得到当前预瞄跟踪点b位置纵坐标bx；
[0081]
步骤s3、获取当前车辆航向角，并根据所述当前车辆位置、所述当前车辆航向角、所述当前预瞄距离和所述当前预瞄跟踪点位置确定当前前轮转角；
[0082]
示例性地，所述根据所述当前车辆位置、所述当前车辆航向角、所述当前预瞄距离和所述当前预瞄跟踪点位置确定当前前轮转角，包括：
[0083]
具体根据以下公式计算确定当前前轮转角；
[0084][0085][0086]
其中，δ为当前前轮转角，l为给定的车辆轴距，l为当前预瞄距离，ax为当前车辆位置的横坐标，ay为当前车辆位置的纵坐标，bx为当前预瞄跟踪点位置的横坐标，by为当前预瞄跟踪点位置的纵坐标，yaw为当前车辆航向角；
[0087]
步骤s4、根据所述当前前轮转角确定当前方向盘转角。
[0088]
示例性地，所述根据所述当前前轮转角确定当前方向盘转角，包括：
[0089]
具体根据以下公式计算确定当前方向盘转角；
[0090]
θ＝kδ+b
[0091]
其中，θ为当前方向盘转角，k预设的线性拟合系数，δ为当前前轮转角，b为预设的线性拟合偏置常数；
[0092]
具体而言，公式θ＝kδ+b是线性拟合车辆前轮转角和车辆方向盘转角关系，也可以用二次线性或非线性拟合；最后计算得到的当前方向盘转角θ可以进行限幅滤波处理得到最终控制量。
[0093]
应用本发明实施例方法，在进行自动泊车轨迹跟踪时，其预瞄跟踪点为动态根据车辆实时位置进行计算获得，不需要预瞄序列点，直接采用规划路径和车辆实时位置信息在大地坐标系，相对于传统将大地坐标系下期望路径离散点序列转换为车辆坐标系下离散点，再根据预瞄距离和目标距离差异确定轨迹跟踪点的方式，取消了坐标转换的繁琐运算，无需预瞄序列点，能够减少跟踪控制算法计算量，节省cpu内存空间，降低对算法对硬件的性能要求。
[0094]
参阅图2，本发明另一实施例提出一种自动泊车轨迹跟踪系统，用于实现上述实施例所述自动泊车轨迹跟踪方法，所述系统包括：
[0095]
预瞄距离确定单元1，用于获取当前车速和当前车辆挡位，并根据所述当前车速和当前车辆挡位确定当前预瞄距离；
[0096]
预瞄跟踪点确定单元2，用于获取泊车轨迹规划段、当前车辆位置，并根据所述泊车轨迹规划段、所述当前预瞄距离和所述当前车辆位置确定当前预瞄跟踪点位置；
[0097]
前轮转角确定单元3，用于根据所述当前车辆位置、所述当前车辆航向角、所述当前预瞄距离和所述当前预瞄跟踪点位置确定当前前轮转角；以及
[0098]
方向盘转角确定单元4，用于根据所述当前前轮转角确定当前方向盘转角。
[0099]
以上所描述的系统实施例仅仅是示意性的，其中所述作为分离部件说明的单元可以是或者也可以不是物理上分开的，作为单元显示的部件可以是或者也可以不是物理单元，即可以位于一个地方，或者也可以分布到多个网络单元上。可以根据实际的需要选择其中的部分或者全部模块来实现本实施例方案的目的。
[0100]
需说明的是，上述实施例所述系统与上述实施例所述方法对应，因此，上述实施例所述系统未详述部分可以参阅上述实施例所述方法的内容得到，此处不再赘述。
[0101]
并且，上述实施例所述自动泊车轨迹跟踪系统若以软件功能单元的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存储介质中。
[0102]
本发明另一实施例提出一种计算机可读存储介质，其上存储有计算机程序，所述计算机程序被处理器执行时实现上述实施例所述自动泊车轨迹跟踪方法。
[0103]
具体而言，所述计算机可读存储介质可以包括：能够携带所述计算机程序代码的任何实体或装置、记录介质、u盘、移动硬盘、磁碟、光盘、计算机存储器、只读存储器(r值om，r值ead-only memor值y)、随机存取存储器(r值am，r值andom access memor值y)、电载波信号、电信信号以及软件分发介质等。
[0104]
以上已经描述了本发明的各实施例，上述说明是示例性的，并非穷尽性的，并且也不限于所披露的各实施例。在不偏离所说明的各实施例的范围和精神的情况下，对于本技术领域的普通技术人员来说许多修改和变更都是显而易见的。本文中所用术语的选择，旨在最好地解释各实施例的原理、实际应用或对市场中的技术改进，或者使本技术领域的其它普通技术人员能理解本文披露的各实施例。