CN112793562B - 一种自动泊车路径的规划和跟踪控制方法、规划装置、存储介质和计算机设备 - Google Patents

Abstract

本发明公开了自动泊车路径的规划和跟踪控制方法、规划装置、存储介质和计算机设备，所述规划方法采用动态的泊车轨迹规划方法，实时根据定位信息和障碍物信息更新泊车轨迹，在满足边界约束和车辆动力学约束的前提下最大限度的适应各种停车环境。而且，所述规划方法采用预瞄跟踪算法，闭环转向控制和速度控制，分段跟踪轨迹点，实时消除运动累积误差，保证轨迹跟踪的精度。泊车完成后的距离偏差不大于20cm，车身中心与车位中心线的角度不超过2°，完全满足泊车***的性能要求。

Description

一种自动泊车路径的规划和跟踪控制方法、规划装置、存储介 质和计算机设备

技术领域

本发明涉及汽车辅助驾驶技术领域，尤其涉及一种自动泊车路径的规划和跟踪控制方法、规划装置、存储介质和计算机设备。

背景技术

随着汽车工业的飞速发展，汽车辅助驾驶的功能日趋完善；自动泊车功能也是汽车辅助驾驶功能之一。目前的自动泊车***的组成主要由环境感知模块、控制决策模块和执行模块组成，泊车过程可以分为车位探测、轨迹规划和轨迹跟踪3个部分。

目前，实现泊车轨迹规划主要依据车辆运动模型和阿克曼转角原理将泊车的过程分解成直线、圆弧、曲线，通过分段拼接形成泊车轨迹，从而计算出相应的期望车速、期望方向盘转角和档位。但是，现有的泊车路径规划主要为静态规划，其分段轨迹的精度受边界约束和车辆运动学模型以及标定参数的影响大。而且，静态规划不能确保车辆在泊车过程中遇到障碍物或环境条件发生改变时动态调整泊车轨迹，泊车的失败率较高。

发明内容

本发明所解决的技术问题是提供一种自动泊车路径的规划和跟踪控制方法、规划装置、存储介质和计算机设备，其可以根据车辆的实时位置、车位信息以及车辆动力学约束和泊车边界约束等实时推算泊车轨迹，泊车精度高。

为解决上述技术问题，本发明所采用的技术方案内容具体如下：

一种自动泊车路径的规划和跟踪控制方法，包括如下步骤：

获取车位信息和车辆信息，所述车位信息包括车位的四个顶点的坐标；所述车辆信息包括车辆的初始位置坐标、车辆的泊入方式、车辆的车身长度、车辆的车身宽度、车辆的前悬长和车辆的后悬长；

对获取的数据进行处理，计算出边界约束，通过坐标转换，计算目标车位与当前车辆的实际位置，确定目标停车点；

根据车位信息、车辆信息和障碍物信息得到泊车轨迹；

利用跟踪算法对车辆进行控制，完成对规划轨迹的跟踪。

作为上述方案的优选，根据车位信息、车辆信息和障碍物信息得到泊车轨迹包括如下步骤：

根据车辆信息和车位信息确定车辆的目标停车点；

在P坐标系中，以目标停车点为终点、以车辆初始位置为起点，利用Dubins路径规划方法确定目标停车点和车辆初始位置之间的轨迹曲线组合；

根据车辆信息得到车辆轮廓参数；

根据车位信息得到车位边界；

根据车辆轮廓参数和车位边界对轨迹曲线组合中的轨迹曲线依次进行边界约束验证和动力学约束进行验证，并将其中符合边界约束和动力学约束的轨迹曲线确认为泊车曲线；

利用贝塞尔公式对泊车曲线进行平滑处理；

针对经过平滑处理的泊车曲线，利用车辆运动模型分解出车辆的运动信息，即得到泊车轨迹。

作为上述方案的优选，根据车辆信息和车位信息确定车辆的目标停车点包括如下步骤：

根据车辆的车身长度、车辆的车身宽度、车辆的前悬长和车辆的后悬长，确定车辆的后轴中心；

根据车位的四个顶点的坐标，确定车辆停泊至车位时车辆的后轴中心在车位的投影点，该投影点即为目标停车点。

作为上述方案的优选，所述车辆轮廓参数为车辆的四个顶点相对于车辆的后轴中心的坐标值。

作为上述方案的优选，利用跟踪算法对车辆进行控制，实现泊车轨迹跟踪包括如下步骤：

对车辆进行横向控制，得到车辆的方向盘转角；

对车辆进行纵向控制，得到车辆的控制模式、纵向路程和纵向速度控制模块；

根据方向盘转角、车辆的控制模式、纵向路程和纵向速度控制模块对泊车轨迹进行修正，得到泊车路径。

作为上述方案的优选，对车辆进行横向控制，得到车辆的方向盘转角包括如下步骤：

根据车辆的车身参数确定纯跟踪算法预瞄距离公式的二次项与一次项系数分别为1/6和1/5；

确定跟踪算法的预瞄距离，获取预瞄点，且预瞄距离ld的计算公式为

计算预瞄点在车辆坐标系下的坐标，预瞄点在车辆坐标系下的纵坐标即为预瞄偏差；

以预瞄偏差作为控制量，使用PID控制算法闭环控制方向盘转角。

本发明还提供了自动泊车路径的规划装置，包括获取模块和路径规划模块，所述获取模块获取车位信息和车辆信息，所述车位信息包括车位的四个顶点的坐标；所述车辆信息包括车辆的初始位置坐标、车辆的泊入方式、车辆的车身长度、车辆的车身宽度、车辆的前悬长和车辆的后悬长；所述路径规划模块根据车位信息、车辆信息和障碍物信息得到泊车轨迹。

作为上述方案的优选，所述获取模块包括第一获取单元、第一确定单元、第二确定单元、第三确定单元、第四确定单元和平滑处理单元，所述第一获取单元根据车辆信息和车位信息确定车辆的目标停车点；在P坐标系中，所述第一确定单元以目标停车点为终点、以车辆初始位置为起点，利用Dubins路径规划方法确定目标停车点和车辆初始位置之间的轨迹曲线组合；所述第二确定单元根据车辆信息得到车辆轮廓参数；所述第三确定单元根据车位信息得到车位边界；所述第四确定单元根据车辆轮廓参数和车位边界对轨迹曲线组合中的轨迹曲线依次进行边界约束验证和动力学约束进行验证，并将其中符合边界约束和动力学约束的轨迹曲线确认为泊车曲线；所述平滑处理单元利用贝塞尔公式对泊车曲线进行平滑处理。

本发明还提供了一种计算机可读存储介质，其上存储有计算机程序，所述计算机程序被处理器执行时实现所述的规划方法的步骤。

本发明还提供了一种计算机设备，包括存储器以及与所述存储器相连接的处理器，所述存储器存储计算机程序，所述计算机程序被所述处理器执行时实现所述的规划方法的步骤。

与现有技术相比，本发明的有益效果在于：

本发明提供的自动泊车路径的规划和跟踪控制方法，其采用动态的泊车轨迹规划方法，实时根据定位信息和障碍物信息更新泊车轨迹，在满足边界约束和车辆动力学约束的前提下最大限度的适应各种停车环境。而且，所述规划方法采用预瞄跟踪算法，闭环转向控制和速度控制，分段跟踪轨迹点，实时消除运动累积误差，保证轨迹跟踪的精度。泊车完成后的距离偏差不大于20cm，车身中心与车位中心线的角度不超过2°，完全满足泊车***的性能要求。

上述说明仅是本发明技术方案的概述，为了能够更清楚了解本发明的技术手段，而可依照说明书的内容予以实施，并且为了让本发明的上述和其他目的、特征和优点能够更明显易懂，以下特举较佳实施例，并配合附图，详细说明如下。

附图说明

图1为较优选实施例的自动泊车路径的规划和跟踪控制方法的应用环境图；

图2为较优选实施例的自动泊车路径的规划和跟踪控制方法的流程示意图；

图3为较优选实施例的自动泊车路径的规划装置的结构框图；

图4为较优选实施例的计算机设备的结构框图；

图5为车位信息参数图；

其中，各附图的附图标记为：

1、终端；2、服务器；3、获取模块；4、路径规划模块；5、第一获取单元；6、第一确定单元；7、第二确定单元；8、第三确定单元；9、第四确定单元；10、平滑处理单元。

具体实施方式

为更进一步阐述本发明为达成预定发明目的所采取的技术手段及功效，以下结合附图及较佳实施例，对依据本发明的具体实施方式、结构、特征及其功效，详细说明如下：

实施例一

如图1所示是本发明的自动泊车路径的规划和跟踪控制方法的应用环境图，所述自动泊车路径的规划和跟踪控制方法应用于自动泊车路径的规划***，该自动泊车路径的规划***包括终端1和服务器2，所述终端1和所述服务器2通过网络连接，所述终端1具体可以是台式终端或移动终端，移动终端具体可以手机、平板电脑、笔记本电脑、便携式可穿戴设备等中的至少一种，所述服务器2可以用独立的服务器或者是多个服务器组成的服务器集群来实现。

如图2所示，在一个实施例中，本发明提供了自动泊车路径的规划和跟踪控制方法，以该方法应用于图1中的服务器2为例进行说明，包括：

获取车位信息和车辆信息，所述车位信息包括车位的四个顶点的坐标；所述车辆信息包括车辆的初始位置坐标、车辆的泊入方式、车辆的车身长度、车辆的车身宽度、车辆的前悬长和车辆的后悬长；

对获取的数据进行处理，计算出边界约束，通过坐标转换，计算目标车位与当前车辆的实际位置，确定目标停车点；

根据车位信息、车辆信息和障碍物信息得到泊车轨迹；

利用跟踪算法对车辆进行控制，完成对规划轨迹的跟踪。

具体地，如图5所示的车位为例，A、B、C、D为该车位的四个顶点，且A、B、C、D四个点在大地坐标系中的坐标分别为(x_A,y_A)、(x_B，y_B)、(x_C，y_C)和(x_D，y_D)，则车位的宽度Wid的计算公式为：Wid＝x_B-x_A＝x_C-x_D；车位的深度Dep的计算公式为Dep＝y_D-y_A＝y_C-y_B。

作为上述方案的优选，根据车位信息、车辆信息和障碍物信息得到泊车轨迹包括如下步骤：

根据车辆信息和车位信息确定车辆的目标停车点；在大地坐标系下，目标停车点坐标x_o＝Wid/2＝(x_B-x_A)/2，y_o＝Dep/2-l_r-l_s＝(y_D-y_A)/2-l_r-l_s；其中，l_r为车辆后悬长，l_s为后方预留安全距离。

在P坐标系中，以目标停车点为终点、以车辆初始位置为起点，利用Dubins路径规划方法确定目标停车点和车辆初始位置之间的轨迹曲线组合，具体地，使用CSC类型(即曲线+直线+曲线)组合，假设起点为S＝(x_s,y_s,α1)，终点g＝(x_g,y_g,α2)，最小转弯半径为r_min，则三段路径的计算公式分别为：

则路径L_CSC＝p1+p2+p3。

根据车辆信息得到车辆轮廓参数；

根据车位信息得到车位边界；

根据车辆轮廓参数和车位边界对轨迹曲线组合中的轨迹曲线依次进行边界约束验证和动力学约束进行验证，并将其中符合边界约束和动力学约束的轨迹曲线确认为泊车曲线；

利用贝塞尔公式对泊车曲线进行平滑处理；

针对经过平滑处理的泊车曲线，利用车辆运动模型分解出车辆的运动信息，即得到泊车轨迹。

具体地，任意时刻上述路径曲线上的点向量为(x_r(t),y_r(t),α)，其中，x_r(t)为轨迹横坐标，y_r(t)为轨迹纵坐标，α为轨迹切向角。假设车辆后轴中心点跟踪该路径曲线，则任意时刻车辆后轴中心点的坐标也即曲线上的点坐标，根据车辆运动学方程，在低速下不考虑车辆侧滑，则有：

其中，L为车辆前后轴的轴距，V为车速(匀速)，β为前轮转向角。

作为上述方案的优选，根据车辆信息和车位信息确定车辆的目标停车点包括如下步骤：

根据车辆的车身长度、车辆的车身宽度、车辆的前悬长和车辆的后悬长，确定车辆的后轴中心，具体地，以左前轮外侧中心为原点，则后轴点坐标x_r＝L_C/2，y_r＝L_l-L_r-L_f，其中，L_C为车辆宽度，L_l为车长，L_r、L_f分别为车的后悬和前悬长；

根据车位的四个顶点的坐标，确定车辆停泊至车位时车辆的后轴中心在车位的投影点，该投影点即为目标停车点。

作为上述方案的优选，所述车辆轮廓参数为车辆的四个顶点相对于车辆的后轴中心的坐标值。

作为上述方案的优选，利用跟踪算法对车辆进行控制，实现泊车轨迹跟踪包括如下步骤：

对车辆进行横向控制，得到车辆的方向盘转角；

对车辆进行纵向控制，得到车辆的控制模式、纵向路程和纵向速度控制模块；

根据方向盘转角、车辆的控制模式、纵向路程和纵向速度控制模块对泊车轨迹进行修正，得到泊车路径。

作为上述方案的优选，对车辆进行横向控制，得到车辆的方向盘转角包括如下步骤：

根据车辆的车身参数确定纯跟踪算法预瞄距离公式的二次项系数A与一次项系数B分别为1/6和1/5，具体计算时：

其中，a_max为最大减速度，本案例中取3m/s^2；B为反应时间，本发明中B的取值为0.2s。

确定跟踪算法的预瞄距离，获取预瞄点，且预瞄距离ld的计算公式为

具体地，Cons通常为车辆的最小转弯半径r_min，本发明采用工程化方法标定出Cons，通过对不同车位形式，不同泊入泊出方式，不同阶段下的车辆运动轨迹实车测试标定而得；

计算预瞄点在车辆坐标系下的坐标，预瞄点在车辆坐标系下的纵坐标即为预瞄偏差，具体为通过旋转和平移变化，将预瞄点在大地坐标系下的坐标转换至车辆坐标系下，且坐标转换公式为：

其中：x’、y’为转换后车辆坐标系下预瞄点坐标，x_lp、y_lp为大地坐标系下预瞄点坐标，θ为坐标旋转角度，x_car、y_car为车辆在大地坐标系下坐标；y′就是预瞄偏差

以预瞄偏差作为控制量，使用PID控制算法闭环控制方向盘转角，具体地，根据预瞄偏差，通过PID计算待停泊车辆的车轮转角，且车轮转角的计算公式为：

其中：δ_wheel是车轮转角；err是预瞄偏差；kp是比例控制系数；kd是微分控制系数；

根据车轮转角查表即可求得请求方向盘转角。

应该理解的是，虽然图2的流程图中的各个步骤按照箭头的指示依次显示，但是这些步骤并不是必然按照箭头指示的顺序依次执行。除非本文中有明确的说明，这些步骤的执行并没有严格的顺序限制，这些步骤可以以其它的顺序执行。而且，图2中的至少一部分步骤可以包括多个子步骤或者多个阶段，这些子步骤或者阶段并不必然是在同一时刻执行完成，而是可以在不同的时刻执行，这些子步骤或者阶段的执行顺序也不必然是依次进行，而是可以与其它步骤或者其它步骤的子步骤或者阶段的至少一部分轮流或者交替地执行。

实施例二

如图3所示，本实施例提供了自动泊车路径的规划装置，包括获取模块3和路径规划模块4，所述获取模块3获取车位信息和车辆信息，所述车位信息包括车位的四个顶点的坐标，所述车辆信息包括车辆的初始位置坐标、车辆的泊入方式、车辆的车身长度、车辆的车身宽度、车辆的前悬长和车辆的后悬长；所述路径规划模块4根据车位信息、车辆信息和障碍物信息得到泊车轨迹。

作为进一步优选的方案，所述获取模块3包括第一获取单元5、第一确定单元6、第二确定单元7、第三确定单元8、第四确定单元9和平滑处理单元10，所述第一获取单元5根据车辆信息和车位信息确定车辆的目标停车点；在P坐标系中，所述第一确定单元6以目标停车点为终点、以车辆初始位置为起点，利用Dubins路径规划方法确定目标停车点和车辆初始位置之间的轨迹曲线组合；所述第二确定单元7根据车辆信息得到车辆轮廓参数；所述第三确定单元8根据车位信息得到车位边界；所述第四确定单元9根据车辆轮廓参数和车位边界对轨迹曲线组合中的轨迹曲线依次进行边界约束验证和动力学约束进行验证，并将其中符合边界约束和动力学约束的轨迹曲线确认为泊车曲线；所述平滑处理单元10利用贝塞尔公式对泊车曲线进行平滑处理。

需要说明的是，所述自动泊车路径的规划装置中的各个模块可全部或部分通过软件、硬件及其组合来实现。上述各模块可以硬件形式内嵌于或独立于计算机设备中的处理器中，也可以以软件形式存储于计算机设备中的存储器中，以便于处理器调用执行以上各个模块对应的操作。

实施例三

本实施例提供了一种计算机设备，所述计算机设备可以为服务器，如图4所示，所述计算机设备包括通过***总线连接的处理器、存储器、网络接口和数据库。其中，该计算机设备的处理器用于提供计算和控制能力。该计算机设备的存储器包括非易失性存储介质、内存储器。该非易失性存储介质存储有操作***、计算机程序和数据库。该内存储器为非易失性存储介质中的操作***和计算机程序的运行提供环境。该计算机设备的数据库用于存储操作行为数据、商品信息数据等等。该计算机设备的网络接口用于与外部的终端通过网络连接通信。该计算机程序被处理器执行时实现所述寻找项目的方法的步骤。

本领域技术人员可以理解，图4中示出的结构，仅仅是与本申请方案相关的部分结构的框图，并不构成对本申请方案所应用于其上的计算机设备的限定，具体的计算机设备可以包括比图中所示更多或更少的部件，或者组合某些部件，或者具有不同的部件布置。

在其他实施例中，提供了一种计算机设备，包括存储器和处理器，该存储器存储有计算机程序，该处理器执行计算机程序时实现以下步骤：

获取车位信息和车辆信息，所述车位信息包括车位的四个顶点的坐标；所述车辆信息包括车辆的初始位置坐标、车辆的泊入方式、车辆的车身长度、车辆的车身宽度、车辆的前悬长和车辆的后悬长；根据车位信息、车辆信息和障碍物信息得到泊车轨迹；利用跟踪算法对车辆进行控制，实现泊车轨迹跟踪。

在其他一些实施例中，处理器执行计算机程序时实现根据车位信息、车辆信息和障碍物信息得到泊车轨迹的步骤，具体包括以下步骤：根据车辆信息和车位信息确定车辆的目标停车点；在P坐标系中，以目标停车点为终点、以车辆初始位置为起点，利用Dubins路径规划方法确定目标停车点和车辆初始位置之间的轨迹曲线组合；根据车辆信息得到车辆轮廓参数；根据车位信息得到车位边界；根据车辆轮廓参数和车位边界对轨迹曲线组合中的轨迹曲线依次进行边界约束验证和动力学约束进行验证，并将其中符合边界约束和动力学约束的轨迹曲线确认为泊车曲线；利用贝塞尔公式对泊车曲线进行平滑处理；针对经过平滑处理的泊车曲线，利用车辆运动模型分解出车辆的运动信息，即得到泊车轨迹。

在其他一些实施例中，处理器执行计算机程序时实现根据车辆信息和车位信息确定车辆的目标停车点的步骤，具体包括以下步骤：根据车辆的车身长度、车辆的车身宽度、车辆的前悬长和车辆的后悬长，确定车辆的后轴中心；根据车位的四个顶点的坐标，确定车辆停泊至车位时车辆的后轴中心在车位的投影点，该投影点即为目标停车点。

在其他一些实施例中，处理器执行计算机程序时实现利用跟踪算法对车辆进行控制，实现泊车轨迹跟踪的步骤，具体包括以下步骤：对车辆进行横向控制，得到车辆的方向盘转角；对车辆进行纵向控制，得到车辆的控制模式、纵向路程和纵向速度控制模块；根据方向盘转角、车辆的控制模式、纵向路程和纵向速度控制模块对泊车轨迹进行修正，得到泊车路径。

在其他一些实施例中，处理器执行计算机程序时实现对车辆进行横向控制，得到车辆的方向盘转角的步骤，具体包括以下步骤：根据车辆的车身参数确定纯跟踪算法预瞄距离公式的二次项与一次项系数分别为1/6和1/5；确定跟踪算法的预瞄距离，获取预瞄点，且预瞄距离ld的计算公式为

采用工程化方法标定出Cons，通过对不同车位形式，不同泊入泊出方式，不同阶段下的车辆运动轨迹实车测试标定而得；计算预瞄点在车辆坐标系下的坐标，预瞄点在车辆坐标系下的纵坐标即为预瞄偏差；以预瞄偏差作为控制量，使用PID控制算法闭环控制方向盘转角。

实施例四

本实施例提供了一种计算机可读存储介质，其上存储有计算机程序，所述计算机程序被处理器执行时实现如下步骤：获取车位信息和车辆信息，所述车位信息包括车位的四个顶点的坐标；所述车辆信息包括车辆的初始位置坐标、车辆的泊入方式、车辆的车身长度、车辆的车身宽度、车辆的前悬长和车辆的后悬长；根据车位信息、车辆信息和障碍物信息得到泊车轨迹；利用跟踪算法对车辆进行控制，实现泊车轨迹跟踪。

在其他一些实施例中，计算机程序被处理器执行实现根据车位信息、车辆信息和障碍物信息得到泊车轨迹的步骤，具体包括以下步骤：根据车辆信息和车位信息确定车辆的目标停车点；在P坐标系中，以目标停车点为终点、以车辆初始位置为起点，利用Dubins路径规划方法确定目标停车点和车辆初始位置之间的轨迹曲线组合；根据车辆信息得到车辆轮廓参数；根据车位信息得到车位边界；根据车辆轮廓参数和车位边界对轨迹曲线组合中的轨迹曲线依次进行边界约束验证和动力学约束进行验证，并将其中符合边界约束和动力学约束的轨迹曲线确认为泊车曲线；利用贝塞尔公式对泊车曲线进行平滑处理；针对经过平滑处理的泊车曲线，利用车辆运动模型分解出车辆的运动信息，即得到泊车轨迹。

在其他一些实施例中，计算机程序被处理器执行实现根据车辆信息和车位信息确定车辆的目标停车点的步骤，具体包括以下步骤：根据车辆的车身长度、车辆的车身宽度、车辆的前悬长和车辆的后悬长，确定车辆的后轴中心；根据车位的四个顶点的坐标，确定车辆停泊至车位时车辆的后轴中心在车位的投影点，该投影点即为目标停车点。

在其他一些实施例中，计算机程序被处理器执行实现利用跟踪算法对车辆进行控制，实现泊车轨迹跟踪的步骤，具体包括以下步骤：对车辆进行横向控制，得到车辆的方向盘转角；对车辆进行纵向控制，得到车辆的控制模式、纵向路程和纵向速度控制模块；根据方向盘转角、车辆的控制模式、纵向路程和纵向速度控制模块对泊车轨迹进行修正，得到泊车路径。

在其他一些实施例中，计算机程序被处理器执行实现对车辆进行横向控制，得到车辆的方向盘转角的步骤，具体包括以下步骤：根据车辆的车身参数确定纯跟踪算法预瞄距离公式的二次项与一次项系数分别为1/6和1/5；确定跟踪算法的预瞄距离，获取预瞄点，且预瞄距离ld的计算公式为

采用工程化方法标定出Cons，通过对不同车位形式，不同泊入泊出方式，不同阶段下的车辆运动轨迹实车测试标定而得；计算预瞄点在车辆坐标系下的坐标，预瞄点在车辆坐标系下的纵坐标即为预瞄偏差；以预瞄偏差作为控制量，使用PID控制算法闭环控制方向盘转角。

本领域普通技术人员可以理解实现上述实施例方法中的全部或部分流程，是可以通过计算机程序来指令相关的硬件来完成，所述的程序可存储于一非易失性计算机可读取存储介质中，该程序在执行时，可包括如上述各方法的实施例的流程。其中，本申请所提供的各实施例中所使用的对存储器、数据库或其它介质的任何引用，均可包括非易失性存储器和/或易失性存储器，其中：(1)非易失性存储器可包括只读存储器(ROM)、可编程ROM(PROM)、电可编程ROM(EPROM)、电可擦除可编程ROM(EEPROM)或闪存；(2)易失性存储器可包括随机存取存储器(RAM)或者外部高速缓冲存储器。作为说明而非局限，RAM以多种形式可得，诸如静态RAM(SRAM)、动态RAM(DRAM)、同步DRAM(SDRAM)、双数据率SDRAM(DDRSDRAM)、增强型SDRAM(ESDRAM)、同步链路(Synchlink)DRAM(SLDRAM)、存储器总线(Rambus)直接RAM(RDRAM)、直接存储器总线动态RAM(DRDRAM)、以及存储器总线动态RAM(RDRAM)等。

上述实施方式仅为本发明的优选实施方式，不能以此来限定本发明保护的范围，本领域的技术人员在本发明的基础上所做的任何非实质性的变化及替换均属于本发明所要求保护的范围。

Claims (3)

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

获取车位信息和车辆信息，所述车位信息包括车位的四个顶点的坐标；所述车辆信息包括车辆的初始位置坐标、车辆的泊入方式、车辆的车身长度、车辆的车身宽度、车辆的前悬长和车辆的后悬长；

对获取的数据进行处理，计算出边界约束，通过坐标转换，计算目标车位与当前车辆的实际位置，确定目标停车点；

根据车位信息、车辆信息和障碍物信息得到泊车轨迹；

利用跟踪算法对车辆进行控制，完成对规划轨迹的跟踪；

车位信息和车辆信息获取包括如下步骤：

通过云服务器端获取车位信息，所述信息包括车位的四个顶点的坐标和泊车方式指令；读取本地存储介质中保存的本车的车辆信息：

A、B、C、D为该车位的四个顶点，且A、B、C、D四个点在大地坐标系中的坐标分别为(x_A,y_A)、(x_B,y_B)、(x_C,y_C)和(x_D,y_D)，则车位的宽度Wid的计算公式为：Wid＝x_B-x_A＝x_C-x_D；车位的深度Dep的计算公式为：Dep＝y_D-y_A＝y_C-y_B；

根据车位信息、车辆信息和障碍物信息得到泊车轨迹，包括如下步骤：

根据车辆信息和车位信息确定车辆的目标停车点；在大地坐标系下，目标停车点坐标x_o＝Wid/2＝(x_B-x_A)/2，y_o＝Dep/2-l_r-l_s＝(y_D-y_A)/2-l_r-l_s；其中，l_r为车辆后悬长，l_s为后方预留安全距离；

在P坐标系中，以目标停车点为终点、以车辆初始位置为起点，利用DuBins路径规划方法确定目标停车点和车辆初始位置之间的轨迹曲线组合，具体地，使用CSC类型即曲线+直线+曲线组合，假设起点为S＝(x_s,y_s,α1)，终点g＝(x_g,y_g,α2)，最小转弯半径为r_min，则三段路径的计算公式分别为：

则路径L_CSC＝p1+p2+p3；

根据车辆信息得到车辆轮廓参数；

根据车位信息得到车位边界；

根据车辆轮廓参数和车位边界对轨迹曲线组合中的轨迹曲线依次进行边界约束验证和动力学约束进行验证，并将其中符合边界约束和动力学约束的轨迹曲线确认为泊车曲线；

利用贝塞尔公式对泊车曲线进行平滑处理；

针对经过平滑处理的泊车曲线，利用车辆运动模型分解出车辆的运动信息，即得到泊车轨迹；

任意时刻上述泊车曲线上的点向量为(x_r(t),y_r(t),α)，其中，x_r(t)为轨迹横坐标，y_r(t)为轨迹纵坐标，α为轨迹切向角；

假设车辆后轴中心点跟踪该泊车曲线，则任意时刻车辆后轴中心点的坐标也即曲线上的点坐标，根据车辆运动学方程，在低速下不考虑车辆侧滑，则有：

其中，L为车辆前后轴的轴距，V为车速，β为前轮转向角；

根据车辆信息和车位信息确定车辆的目标停车点包括如下步骤：

根据车辆的车身长度、车辆的车身宽度、车辆的前悬长和车辆的后悬长，确定车辆的后轴中心，具体地，以左前轮外侧中心为原点，则后轴点坐标x_r＝L_C/2，y_r＝L_l-L_r-L_f，其中，L_C为车辆宽度，L_l为车长，L_r、L_f分别为车的后悬和前悬长；

根据车位的四个顶点的坐标，确定车辆停泊至车位时车辆的后轴中心在车位的投影点，该投影点即为目标停车点；所述车辆轮廓参数为车辆的四个顶点相对于车辆的后轴中心的坐标值；

利用跟踪算法对车辆进行控制，实现泊车轨迹跟踪包括如下步骤：

对车辆进行横向控制，得到车辆的方向盘转角；

对车辆进行纵向控制，得到车辆的控制模式、纵向路程和纵向速度控制模块；

根据方向盘转角、车辆的控制模式、纵向路程和纵向速度控制模块对泊车轨迹进行修正，得到泊车路径；

对车辆进行横向控制，得到车辆的方向盘转角包括如下步骤：

根据车辆的车身参数确定纯跟踪算法预瞄距离公式的二次项系数A与一次项系数B分别为1/6和1/5，具体计算时：

其中，a_max为最大减速度，本案例中取3m/s^2；B为反应时间；

确定跟踪算法的预瞄距离，获取预瞄点，且预瞄距离ld的计算公式为

Cons通常为车辆的最小转弯半径r_min，采用工程化方法标定出Cons，通过对不同车位形式，不同泊入泊出方式，不同阶段下的车辆运动轨迹实车测试标定而得；

计算预瞄点在车辆坐标系下的坐标，预瞄点在车辆坐标系下的纵坐标即为预瞄偏差，具体为通过旋转和平移变化，将预瞄点在大地坐标系下的坐标转换至车辆坐标系下，且坐标转换公式为：

其中：x’、y’为转换后车辆坐标系下预瞄点坐标，x_lp、y_lp为大地坐标系下预瞄点坐标，θ为坐标旋转角度，x_car、y_car为车辆在大地坐标系下坐标；y′就是预瞄偏差；

以预瞄偏差作为控制量，使用PID控制算法闭环控制方向盘转角，具体地，根据预瞄偏差，通过PID计算待停泊车辆的车轮转角，且车轮转角的计算公式为：

其中：δ_wheel是车轮转角；err是预瞄偏差；k_p是比例控制系数；k_d是微分控制系数；

根据车轮转角查表即可求得请求方向盘转角。

2.一种计算机可读存储介质，其上存储有计算机程序，所述计算机程序被处理器执行时实现权利要求1所述的自动泊车路径的规划和跟踪控制方法。

3.一种计算机设备，其特征在于，包括所述权利要求2所述的计算机可读存储介质以及与所述可读存储介质相连接的处理器，所述可读存储介质存储计算机程序，所述计算机程序被所述处理器执行时实现权利要求1所述的自动泊车路径的规划和跟踪控制方法。

Images