一种平行泊车轨迹规划方法、系统、设备及存储介质与流程

本发明属于自动泊车，具体涉及一种平行泊车轨迹规划方法、系统、设备及存储介质。

背景技术：

1、平行泊车在实际的泊车场景中非常普遍，使得自动平行泊车成为自动驾驶领域的重要组成部分。到目前为止学术界、工业界已经提出了众多的平行泊车解决方案，华为、理想、小鹏、特斯拉等拥有强劲自动驾驶业务的公司已经将自动平行泊车功能落地化，并进行了商业化运营。但技术需要市场的充分检验才能够趋于成熟稳定，自动泊车作为一项新技术也不例外，所以该技术的开发目前处于开放状态。

2、中国专利授权号为cn109866762b，名称为基于高精度地图的自主平行泊车方法的授权专利，包括：车辆获取来自服务器的基于高精度地图的泊车位信息，通过泊车位信息选择泊车位，车辆在泊车位附近获取基于高精度地图的全局坐标系以及位置关系属性，通过确定切换点，对切换点用圆弧或者五次样条曲线拟合，不仅能保证航向角的连续性，还能够确保曲率连续，通过最优路径进行自动泊车；能够确定平行泊车入库最优平行泊车轨迹。该专利申请虽然也能进行平行泊车轨迹规划，但所需获取参数多，计算复杂，难以实时调整。

技术实现思路

1、本发明的目的在于提供一种平行泊车轨迹规划方法、系统、设备及存储介质，在路径规划过程中，采用双梯形路径规划模型和二分法相结合的方式确定路径，计算过程中只需要调整2个参数，方便实时快速对轨迹进行规划和调整。

2、为实现上述目的，本发明采用的技术方案是：

3、第一方面，本发明提供一种平行泊车轨迹规划方法，包括以下步骤：

4、s1：建立以泊车终点为原点，以车位长轴方向为x轴，以车位短轴方向为y轴的车位坐标系；以方向盘回正状态的角度为0°；

5、s2：设定在车位坐标系下泊车的7个阶段轨迹：第1阶段为方向盘转角由0°均匀增加到背向泊车终点所在侧的某一规划转角所形成的回旋曲线；第2阶段维持方向盘转角不变并倒车所形成的圆弧曲线；第3阶段方向盘转角均匀减小到0°并倒车所形成的回旋曲线；第4阶段方向盘转角维持在0°并倒车所形成的直线；第5阶段方向盘转角由0°均匀增加到朝向泊车终点所在侧的某一规划转角并倒车所形成的回旋曲线；第6阶段维持方向盘转角并倒车所形成的圆弧曲线；第7阶段方向盘转角均匀减小到0°并倒车至终点所形成的回旋曲线；p0为泊车起点，p1为车辆第1阶段终点，p2为车辆第2阶段终点，p3为车辆第3阶段终点，p4为车辆第4阶段终点，p5为车辆第5阶段终点，p6为车辆第6阶段终点，p7为泊车终点，其中直线段p3p4与曲线段p2p3、p4p5相切；

6、s3：建立以p4为原点，以车位长轴方向为x轴，以车位短轴方向为y轴的车位坐标系；根据车辆转向机械结构限制得到最小转弯半径，根据泊车空间确定最大转弯半径；

7、s4：通过二分法遍历转弯半径并遍历p4纵坐标，满足自定义容差后，确定p4点的坐标，并依次连接点p0至p7得到平行泊车轨迹。

8、可选的，步骤s2中，分别计算p6处的横坐标p6，x和纵坐标p6，y、p5处的横坐标p5，x和纵坐标p5，y以及p4处横坐标p4，x、p1处的横坐标p1，x和纵坐标p1，y、p2处的横坐标p2，x和纵坐标p2，y、p3处的横坐标p3，x和纵坐标p3，y：

9、p6，x＝p7，x+lturn

10、

11、p5，x＝p4，x-lturn

12、

13、p1，x＝p0，x-lturn

14、

15、p3，x＝p2，x-lturn

16、

17、其中，θ为各阶段车辆在该点的航向角，lturn为方向盘匀速变化阶段的长度，为前轮转向角，pn，θ表示pn处的航向角，δ1为车辆方向盘朝向泊车终点所在侧的最大方向盘转角，δ2为车辆方向盘背向泊车终点所在侧的最大方向盘转角，r1为δ1对应转弯半径，r2为δ2对应转弯半径。

18、可选的，p3，θ＝p4，θ。

19、可选的，车辆在泊车起点位姿和泊车终点位姿相平行且车头朝向相同。

20、可选的，步骤s4中，令rlow＝r2，min、rupper＝r2，max，初始化r2＝(rlow+rupper)/2；当x3-x4<第一自定义容差时，若θp3，p4>p4，θ，则更新rlow＝r2并更新r2＝(rlow+rupper)/2；反之若θp3，p4<p4，θ，则更新rupper＝r2并更新r2＝(rlow+rupper)/2；当x3-x4>第二自定义容差时，若θp3，p4>p4，θ，则更新rupper＝r2并更新r2＝(rlow+rupper)/2；反之若θp3，p4<p4，θ，则更新rlow＝r2并更新r2＝(rlow+rupper)/2；若y3-y4<第一自定义容差，且|x3-x4|≤第二自定义容差时，则更新rlow＝r2并更新r2＝(rlow+rupper)/2；反之若y3-y4>第二自定义容差且|x3-x4|≤第二自定义容差时，则更新rupper＝r2并更新r2＝(rlow+rupper)/2；其中，xi为pi点对应横坐标，p3p4的斜率角度为θp3，p4，pi，θ为pi处的车辆自身航向角，r2为车辆方向盘背向泊车终点所在侧的最大方向盘转角对应转弯半径，r2，min为r2的最小转弯半径，r2，max为r2的最大转弯半径。

21、可选的，步骤s1前，确定泊车起点p0和泊车终点p7的位置。

22、可选的，步骤s1前，先计算可行驶区域，计算方法包括以下步骤：设定p4的纵坐标的序列区间，并将遍历每个离散值；结合左打死的最小转弯半径rmin和离散值p4，y得到p4的位姿；结合右打死的最小转弯半径rmin、p4位姿和起始航向角，得到p0的位姿；连接所有p0的位姿得到左边界lb；设定p5p6的转弯半径区间，并将遍历每个离散的转弯半径；设定p4，y的序列区间，若在当前的转弯半径和p4，y条件下所计算得到的p4的航向角小于起始航向角，则递增遍历p4，y的序列区间，直到p4的航向角不小于起始航向角时停止遍历，得到p4的位姿，并得到p0的位姿；连接所有p0的位姿得到下边界db。

23、第二方面，本发明提供一种平行泊车轨迹系统，包括：

24、第一坐标建立模块，用于建立以泊车终点为原点，以车位长轴方向为x轴，以车位短轴方向为y轴的车位坐标系；以方向盘回正状态的角度为0°；

25、路径规划模块，用于设定在车位坐标系下泊车的7个阶段轨迹：第1阶段为方向盘转角由0°均匀增加到背向泊车终点所在侧的某一规划转角所形成的回旋曲线；第2阶段维持方向盘转角不变并倒车所形成的圆弧曲线；第3阶段方向盘转角均匀减小到0°并倒车所形成的回旋曲线；第4阶段方向盘转角维持在0°并倒车所形成的直线；第5阶段方向盘转角由0°均匀增加到朝向泊车终点所在侧的某一规划转角并倒车所形成的回旋曲线；第6阶段维持方向盘转角并倒车所形成的圆弧曲线；第7阶段方向盘转角均匀减小到0°并倒车至终点所形成的回旋曲线；p0为泊车起点，p1为车辆第1阶段终点，p2为车辆第2阶段终点，p3为车辆第3阶段终点，p4为车辆第4阶段终点，p5为车辆第5阶段终点，p6为车辆第6阶段终点，p7为泊车终点，其中直线段p3p4与曲线段p2p3、p4p5相切；

26、坐标建立模块，用于建立以p4为原点，以车位长轴方向为x轴，以车位短轴方向为y轴的车位坐标系；根据车辆转向机械结构限制得到最小转弯半径，根据泊车空间确定最大转弯半径；

27、计算模块，用于通过二分法遍历转弯半径并遍历p4纵坐标，满足自定义容差后，确定p4点的坐标，并依次连接点p0至p7得到平行泊车轨迹。

28、第三方面，本发明提供一种电子设备，包括存储器、处理器以及存储在所述存储器中并可在所述处理器上运行的计算机程序，所述处理器执行所述计算机程序时实现所述平行泊车轨迹规划方法的步骤。

29、第四方面，本发明提供一种计算机可读存储介质，所述计算机可读存储介质存储有计算机程序，所述计算机程序被处理器执行时实现所述平行泊车轨迹规划方法的步骤。

30、与现有技术相比，本发明具有以下有益效果：

31、本发明的一种平行泊车轨迹规划方法，采用双梯形路径，算法基于实际的驾驶员经验，在路径规划时天然地考虑了方向盘转角的约束。在路径规划时仅需要考虑运动学约束，简化了约束，使规划的路径具有可落地性。本发明在双梯形路径的基础上，通过二分法遍历法确定最终的行车轨迹，整个过程中只需要调整2个参数，计算量较小，能够方便进行实时计算。在计算能力相同的条件下，采用本发明的轨迹规划方法能够实现更快速，更多次数的计算，在平行泊车时能够更高效地实时计算，避免路径规划出现误差而导致无法正确泊车。

32、进一步，本发明提供了可行驶区域的计算方法，能够确定平行泊车时满足条件的区域，使车辆能够在第一时间进行调整，避免车辆在无法进行平行泊车的区域陷入停滞。

技术特征：

1.一种平行泊车轨迹规划方法，其特征在于，包括以下步骤：

2.根据权利要求1所述的一种平行泊车轨迹规划方法，其特征在于，步骤s2中，分别计算p6处的横坐标p6，x和纵坐标p6，y、p5处的横坐标p5，x和纵坐标p5，y以及p4处横坐标p4，x、p1处的横坐标p1，x和纵坐标p1，y、p2处的横坐标p2，x和纵坐标p2，y、p3处的横坐标p3，x和纵坐标p3，y：

3.根据权利要求2所述的一种平行泊车轨迹规划方法，其特征在于，p3，θ＝p4，θ。

4.根据权利要求1所述的一种平行泊车轨迹规划方法，其特征在于，车辆在泊车起点位姿和泊车终点位姿相平行且车头朝向相同。

5.根据权利要求1所述的一种平行泊车轨迹规划方法，其特征在于，步骤s4中，令rlow＝r2，min、rupper＝r2，max，初始化r2＝(rlow+rupper)/2；当x3-x4<第一自定义容差时，若θp3，p4>p4，θ，则更新rlow＝r2并更新r2＝(rlow+rupper)/2；反之若θp3，p4<p4，θ，则更新rupper＝r2并更新r2＝(rlow+rupper)/2；当x3-x4>第二自定义容差时，若θp3，p4>p4，θ，则更新rupper＝r2并更新r2＝(rlow+rupper)/2；反之若θp3，p4<p4，θ，则更新rlow＝r2并更新r2＝(rlow+rupper)/2；若y3-y4<第一自定义容差，且|x3-x4|≤第二自定义容差时，则更新rlow＝r2并更新r2＝(rlow+rupper)/2；反之若y3-y4>第二自定义容差且|x3-x4|≤第二自定义容差时，则更新rupper＝r2并更新r2＝(rlow+rupper)/2；其中，xi为pi点对应横坐标，p3p4的斜率角度为θp3，p4，pi，θ为pi处的车辆自身航向角，r2为车辆方向盘背向泊车终点所在侧的最大方向盘转角对应转弯半径，r2，min为r2的最小转弯半径，r2，max为r2的最大转弯半径。

6.根据权利要求1所述的一种平行泊车轨迹规划方法，其特征在于，步骤s1前，确定泊车起点p0和泊车终点p7的位置。

7.根据权利要求1所述的一种平行泊车轨迹规划方法，其特征在于，步骤s1前，先计算可行驶区域，计算方法包括以下步骤：设定p4的纵坐标的序列区间，并将遍历每个离散值；结合左打死的最小转弯半径rmin和离散值p4，y得到p4的位姿；结合右打死的最小转弯半径rmin、p4位姿和起始航向角，得到p0的位姿；连接所有p0的位姿得到左边界lb；设定p5p6的转弯半径区间，并将遍历每个离散的转弯半径；设定p4，y的序列区间，若在当前的转弯半径和p4，y条件下所计算得到的p4的航向角小于起始航向角，则递增遍历p4，y的序列区间，直到p4的航向角不小于起始航向角时停止遍历，得到p4的位姿，并得到p0的位姿；连接所有p0的位姿得到下边界db。

8.一种平行泊车轨迹系统，其特征在于，包括：

9.一种电子设备，包括存储器、处理器以及存储在所述存储器中并可在所述处理器上运行的计算机程序，所述处理器执行所述计算机程序时实现权利要求1-7任一项所述平行泊车轨迹规划方法的步骤。

10.一种计算机可读存储介质，所述计算机可读存储介质存储有计算机程序，所述计算机程序被处理器执行时实现权利要求1-7任一项所述平行泊车轨迹规划方法的步骤。

技术总结
本发明属于自动泊车技术领域，本发明公开了一种平行泊车轨迹规划方法、系统、设备及存储介质，方法包括以下步骤：建立以泊车终点为原点，以车位长轴方向为X轴，以车位短轴方向为Y轴的车位坐标系；以方向盘回正状态的角度为0°；根据双梯形路径规划模型分别计算各点的横坐标和纵坐标；建立车位坐标系；根据车辆转向机械结构限制得到最小转弯半径，根据泊车空间确定最大转弯半径；通过二分法遍历转弯半径得到轨迹。本发明在路径规划时仅需要考虑运动学约束，简化了约束，使规划的路径具有可落地性，且能够方便进行实时计算。

技术研发人员：王耀,刘义,高文娟,郝海洋,史艳龙,董鹏
受保护的技术使用者：陕西法士特汽车传动集团有限责任公司
技术研发日：
技术公布日：2024/9/23