垂直车位的出库轨迹规划方法、系统及存储介质

技术领域

本发明涉及纯电动汽车领域，特别涉及一种纯电动汽车续驶里程估算方法、系统、存储介质及设备。

背景技术

现有技术中，轨迹规划算法要求的输入参数为：起点坐标、起点航向角、终点坐标、终点航向角；然而，对于出库轨迹，终点的坐标一般都不确定，但是航向角确定，所以运用现有的轨迹规划算法计算不出轨迹；因此，针对上述情况，现有算法一般会假定终点坐标，然后输出出库轨迹，这就会导致轨迹不能自适应避障、且规划的轨迹也不是最优轨迹，实用性不高，用户体验差。

发明内容

本发明的提供一种垂直车位的出库轨迹规划方法、系统及存储介质，可实现自适应避障，规划出的轨迹逻辑简单、实用性高、安全舒适合理。

第一方面，提供一种垂直车位的出库轨迹规划方法，包括以下步骤：

获取车辆出库的直线轨迹与终点航向角；

以所述直线轨迹的直线终点为起点，并基于预设条件检测车辆与障碍物是否发生碰撞、及车辆与障碍物未发生碰撞时的航向角是否等于所述终点航向角来获取每一段曲线轨迹；

将所述直线轨迹与所述每一段曲线轨迹按顺序进行组合，输出车辆出库轨迹。

根据第一方面，在第一方面的第一种可能的实现方式中，所述“以所述直线轨迹的直线终点为起点，并基于预设条件检测车辆与障碍物是否发生碰撞、及车辆与障碍物未发生碰撞时的航向角是否等于所述终点航向角来获取每一段曲线轨迹”步骤，具体包括以下步骤：

以所述直线轨迹的直线终点为起点，以预设步长向右圆弧旋转前进移动，获取第一段曲线轨迹；当检测到车辆与障碍物发生碰撞时，则选取车辆碰撞障碍物之前的一个点为所述第一段曲线轨迹的第一终点；

以所述第一终点为起点，以预设步长向左圆弧旋转后退移动，获取第二段曲线轨迹；当检测到车辆与障碍物发生碰撞时，则选取车辆碰撞障碍物之前的一个点为所述第二段曲线轨迹的第二终点；

以所述第二终点为起点，按顺序重复进行以预设步长向右圆弧旋转前进移动及以预设步长向左圆弧旋转后退移动，直至检测到车辆与障碍物未发生碰撞；

当检测到车辆与障碍物未发生碰撞时的航向角等于所述终点航向角时，碰撞检测及航向角检测结束，并获取后续每一段曲线轨迹。

根据第一方面的第一种可能的实现方式，在第一方面的第二种可能的实现方式中，所述“以所述第二终点为起点，按顺序重复进行以预设步长向右圆弧旋转前进移动及以预设步长向左圆弧旋转后退移动，直至检测到车辆与障碍物未发生碰撞”步骤之后，具体包括以下步骤：

当检测到车辆与障碍物未发生碰撞时的航向角不等于所述终点航向角时，则对车辆与障碍物未发生碰撞时的以预设步长向右圆弧旋转前进移动或以预设步长向左圆弧旋转后退移动进行步长调整，并按调整后的以预设步长向右圆弧旋转前进移动或以预设步长向左圆弧旋转后退移动后再次进行车辆碰撞检测，直至检测到车辆与障碍物未发生碰撞时的航向角等于所述终点航向角。

根据第一方面的第二种可能的实现方式，在第一方面的第三种可能的实现方式中，所述“将所述直线轨迹与所述每一段曲线轨迹按顺序进行组合，输出车辆出库轨迹”步骤，具体包括以下步骤：

将所述直线轨迹、所述第一段曲线轨迹、所述第二段曲线轨迹及所述后续每一段曲线轨迹按顺序进行组合，输出车辆出库轨迹。

根据第一方面的第三种可能的实现方式，在第一方面的第四种可能的实现方式中，所述“获取车辆出库的直线轨迹”步骤，具体包括以下步骤：

车辆出库的直线轨迹的直线长度S计算公式如下：

S＝CD+margin

其中，

式中，(obs_x，obs_y)为障碍物端点坐标；R为车辆最小转弯半径；width为车辆宽度值；margin为车辆安全距离。

第二方面，提供了一种垂直车位的出库轨迹规划系统，包括：

直线轨迹获取模块，用于获取车辆出库的直线轨迹与终点航向角；

检测模块，与所述直线轨迹获取模块通信连接，用于以所述直线轨迹的直线终点为起点，并基于预设条件检测车辆与障碍物是否发生碰撞、及车辆与障碍物未发生碰撞时的航向角是否等于所述终点航向角来获取每一段曲线轨迹；

出库轨迹模块，与所述直线轨迹获取模块及所述检测模块通信连接，用于将所述直线轨迹与所述每一段曲线轨迹按顺序进行组合，输出车辆出库轨迹。

一些实施例中，所述检测模块，用于以所述直线轨迹的直线终点为起点，以预设步长向右圆弧旋转前进移动，获取第一段曲线轨迹；当检测到车辆与障碍物发生碰撞时，则选取车辆碰撞障碍物之前的一个点为所述第一段曲线轨迹的第一终点；以所述第一终点为起点，以预设步长向左圆弧旋转后退移动，获取第二段曲线轨迹；当检测到车辆与障碍物发生碰撞时，则选取车辆碰撞障碍物之前的一个点为所述第二段曲线轨迹的第二终点；以所述第二终点为起点，按顺序重复进行以预设步长向右圆弧旋转前进移动及以预设步长向左圆弧旋转后退移动，直至检测到车辆与障碍物未发生碰撞；当检测到车辆与障碍物未发生碰撞时的航向角等于所述终点航向角时，碰撞检测及航向角检测结束，并获取后续每一段曲线轨迹。

一些实施例中，所述检测模块，用于当检测到车辆与障碍物未发生碰撞时的航向角不等于所述终点航向角时，则对车辆与障碍物未发生碰撞时的以预设步长向右圆弧旋转前进移动或以预设步长向左圆弧旋转后退移动进行步长调整，并按调整后的以预设步长向右圆弧旋转前进移动或以预设步长向左圆弧旋转后退移动后再次进行车辆碰撞检测，直至检测到车辆与障碍物未发生碰撞时的航向角等于所述终点航向角。

一些实施例中，所述出库轨迹模块，用于将所述直线轨迹、所述第一段曲线轨迹、所述第二段曲线轨迹及所述后续每一段曲线轨迹按顺序进行组合，输出车辆出库轨迹。

第三方面，提供了一种存储介质，其上存储有计算机程序，所述计算机程序被处理器执行时实现如上述所述的纯电动汽车续驶里程估算方法。

与现有技术相比，本发明的优点如下：本发明可基于预设条件检测车辆与障碍物是否发生碰撞、及车辆与障碍物未发生碰撞时的航向角是否等于所述终点航向角来获取每一段曲线轨迹，可实现自适应避障，规划出的轨迹具备逻辑简单，计算量小，实用性高，安全舒适合理。

附图说明

图1是本发明一种垂直车位的出库轨迹规划方法的一实施例的流程示意图；

图2是本发明一种垂直车位的出库轨迹规划方法的又一实施例的流程示意图；

图3是本发明车辆出库轨迹的示意图；

图4是本发明计算直线轨迹的直线长度的示意图；

图5是本发明一种垂直车位的出库轨迹规划系统的结构示意图。

附图标记说明：

100、垂直车位的出库轨迹规划系统；110、直线轨迹获取模块；120、检测模块；130、出库轨迹模块。

具体实施方式

现在将详细参照本发明的具体实施例，在附图中例示了本发明的例子。尽管将结合具体实施例描述本发明，但将理解，不是想要将本发明限于所述的实施例。相反，想要覆盖由所附权利要求限定的在本发明的精神和范围内包括的变更、修改和等价物。应注意，这里描述的方法步骤都可以由任何功能块或功能布置来实现，且任何功能块或功能布置可被实现为物理实体或逻辑实体、或者两者的组合。

为了使本领域技术人员更好地理解本发明，下面结合附图和具体实施方式对本发明作进一步详细说明。

注意：接下来要介绍的示例仅是一个具体的例子，而不作为限制本发明的实施例必须为如下具体的步骤、数值、条件、数据、顺序等等。本领域技术人员可以通过阅读本说明书来运用本发明的构思来构造本说明书中未提到的更多实施例。

参见图1所示，本发明实施例提供一种垂直车位的出库轨迹规划方法，包括以下步骤：

S100，获取车辆出库的直线轨迹与终点航向角；

S200，以所述直线轨迹的直线终点为起点，并基于预设条件检测车辆与障碍物是否发生碰撞、及车辆与障碍物未发生碰撞时的航向角是否等于所述终点航向角来获取每一段曲线轨迹；

S300，将所述直线轨迹与所述每一段曲线轨迹按顺序进行组合，输出车辆出库轨迹。

具体地，本实施例中，现有技术中，轨迹规划算法要求的输入参数为：起点坐标、起点航向角、终点坐标、终点航向角；然而，对于出库轨迹，终点的坐标一般都不确定，但是航向角确定，所以运用现有的轨迹规划算法计算不出轨迹；因此，针对上述情况，现有算法一般会假定终点坐标，然后输出出库轨迹，这就会导致轨迹不能自适应避障、且规划的轨迹也不是最优轨迹，实用性不高，用户体验差。

因此为了解决上述问题，本发明可基于预设条件检测车辆与障碍物是否发生碰撞、及车辆与障碍物未发生碰撞时的航向角是否等于所述终点航向角来获取每一段曲线轨迹，可实现自适应避障，规划出的轨迹具备逻辑简单，计算量小，实用性高，安全舒适合理。

同时参见图2所示，优选地，在本申请另外的实施例中，所述“S200，以所述直线轨迹的直线终点为起点，并基于预设条件检测车辆与障碍物是否发生碰撞来获取每一段曲线轨迹”步骤，具体包括以下步骤：

S210，以所述直线轨迹的直线终点为起点，以预设步长向右圆弧旋转前进移动，获取第一段曲线轨迹；当检测到车辆与障碍物发生碰撞时，则选取车辆碰撞障碍物之前的一个点为所述第一段曲线轨迹的第一终点；

S220，以所述第一终点为起点，以预设步长向左圆弧旋转后退移动，获取第二段曲线轨迹；当检测到车辆与障碍物发生碰撞时，则选取车辆碰撞障碍物之前的一个点为所述第二段曲线轨迹的第二终点；

S230，以所述第二终点为起点，按顺序重复进行以预设步长向右圆弧旋转前进移动及以预设步长向左圆弧旋转后退移动，直至检测到车辆与障碍物未发生碰撞；

S240，当检测到车辆与障碍物未发生碰撞时的航向角等于所述终点航向角时，碰撞检测及航向角检测结束，并获取后续每一段曲线轨迹。

具体地，本实施例中，

1、以所述直线轨迹的直线终点为起点，以预设步长向右圆弧旋转前进移动，将车辆与障碍物进行碰撞检测，获取第一段曲线轨迹；

2、若未发生碰撞且此时车辆航向角等于终点航向角，则轨迹计算结束，输出第一段曲线轨迹与直线轨迹的组合为车辆出库轨迹；若移动过程中碰撞障碍物，则选取车辆碰撞障碍物之前的一个点为所述第一段曲线轨迹的第一终点。

3、以第一终点为起点，以预设步长向左圆弧旋转后退移动，将车身与障碍物进行碰撞检测，获取第二段曲线轨迹；

4、若未发生碰撞且此时车辆航向角等于终点航向角，则轨迹计算结束，输出第一段曲线轨迹、第二段曲线轨迹与直线轨迹的组合为车辆出库轨迹；若移动过程中碰撞障碍物，则则选取车辆碰撞障碍物之前的一个点为所述第二段曲线轨迹的第二终点。

重复流程1、2、3、4，直至轨迹计算结束。

同时参见图2所示，优选地，在本申请另外的实施例中，所述“S230，以所述第二终点为起点，按顺序重复进行以预设步长向右圆弧旋转前进移动及以预设步长向左圆弧旋转后退移动，直至检测到车辆与障碍物未发生碰撞”步骤之后，具体包括以下步骤：

S250，当检测到车辆与障碍物未发生碰撞时的航向角不等于所述终点航向角时，则对车辆与障碍物未发生碰撞时的以预设步长向右圆弧旋转前进移动或以预设步长向左圆弧旋转后退移动进行步长调整，并按调整后的以预设步长向右圆弧旋转前进移动或以预设步长向左圆弧旋转后退移动后再次进行车辆碰撞检测，直至检测到车辆与障碍物未发生碰撞时的航向角等于所述终点航向角。

优选地，在本申请另外的实施例中，所述“将所述直线轨迹与所述每一段曲线轨迹按顺序进行组合，输出车辆出库轨迹”步骤，具体包括以下步骤：

将所述直线轨迹、所述第一段曲线轨迹、所述第二段曲线轨迹及所述后续每一段曲线轨迹按顺序进行组合，输出车辆出库轨迹。

同时参见图3所示，设当前车已停在垂直车位中，当前场景下规划出的出库轨迹点连成的直线曲线轨迹如图3所示；

上面的方框为左侧障碍物矩形，下面的方框为右侧障碍物矩形，右边的方框为前方障碍物矩形；横轴和纵轴分别表示轨迹点的x和y坐标，中间较深的黑色线条为直线轨迹与圆弧曲线轨迹，因此规划出的轨迹安全舒适合理，且满足车辆动力学原理。

优选地，在本申请另外的实施例中，所述“获取车辆出库的直线轨迹”步骤，具体包括以下步骤：

车辆出库的直线轨迹的直线长度S计算公式如下：

S＝|CD|+margin；

其中，

式中，(obs_x，obs_y)为障碍物端点坐标；R为车辆最小转弯半径；width为车辆宽度值；margin为车辆安全距离

具体地，本实施例中，参见图4所示，A点为车从xy轴原点直行|CD|时，车后轴中心向右旋转、半径为最小转弯半径的转弯圆圆心；(obs_x，obs_y)为障碍物端点B坐标，设车身向右转弯时刚好碰撞障碍物B；R为车辆最小转弯半径；width为车辆宽度值；margin为车辆安全距离。

cir_y＝-R

|AB|＝R-width/2

|CD|＝obs_x-|BC|

联立以上公式可得：

则车辆出库的直线轨迹的直线长度S计算公式如下：

S＝|CD|+margin。

同时参见图5所示，本发明实施例还提供了一种垂直车位的出库轨迹规划系统100，包括：直线轨迹获取模块110、检测模块120、出库轨迹模块130；

直线轨迹获取模块110，用于获取车辆出库的直线轨迹与终点航向角；

检测模块120，与所述直线轨迹获取模块110通信连接，用于以所述直线轨迹的直线终点为起点，并基于预设条件检测车辆与障碍物是否发生碰撞、及车辆与障碍物未发生碰撞时的航向角是否等于所述终点航向角来获取每一段曲线轨迹；

出库轨迹模块130，与所述直线轨迹获取模块110及所述检测模块120通信连接，用于将所述直线轨迹与所述每一段曲线轨迹按顺序进行组合，输出车辆出库轨迹。

所述检测模块120，用于以所述直线轨迹的直线终点为起点，以预设步长向右圆弧旋转前进移动，获取第一段曲线轨迹；当检测到车辆与障碍物发生碰撞时，则选取车辆碰撞障碍物之前的一个点为所述第一段曲线轨迹的第一终点；以所述第一终点为起点，以预设步长向左圆弧旋转后退移动，获取第二段曲线轨迹；当检测到车辆与障碍物发生碰撞时，则选取车辆碰撞障碍物之前的一个点为所述第二段曲线轨迹的第二终点；以所述第二终点为起点，按顺序重复进行以预设步长向右圆弧旋转前进移动及以预设步长向左圆弧旋转后退移动，直至检测到车辆与障碍物未发生碰撞；当检测到车辆与障碍物未发生碰撞时的航向角等于所述终点航向角时，碰撞检测及航向角检测结束，并获取后续每一段曲线轨迹。

所述检测模块120，用于当检测到车辆与障碍物未发生碰撞时的航向角不等于所述终点航向角时，则对车辆与障碍物未发生碰撞时的以预设步长向右圆弧旋转前进移动或以预设步长向左圆弧旋转后退移动进行步长调整，并按调整后的以预设步长向右圆弧旋转前进移动或以预设步长向左圆弧旋转后退移动后再次进行车辆碰撞检测，直至检测到车辆与障碍物未发生碰撞时的航向角等于所述终点航向角。

所述出库轨迹模块130，用于将所述直线轨迹、所述第一段曲线轨迹、所述第二段曲线轨迹及所述后续每一段曲线轨迹按顺序进行组合，输出车辆出库轨迹。

本发明可基于预设条件检测车辆与障碍物是否发生碰撞、及车辆与障碍物未发生碰撞时的航向角是否等于所述终点航向角来获取每一段曲线轨迹，可实现自适应避障，规划出的轨迹具备逻辑简单，计算量小，实用性高，安全舒适合理。

具体的，本实施例与上述方法实施例一一对应，各个模块的功能在相应的方法实施例中已经进行详细说明，因此不再一一赘述。

基于同一发明构思，本申请实施例还提供了一种计算机可读存储介质，其上存储有计算机程序，计算机程序被处理器执行时实现上述方法的所有方法步骤或部分方法步骤。

本发明实现上述方法中的全部或部分流程，也可以通过计算机程序来指令相关的硬件来完成，计算机程序可存储于一计算机可读存储介质中，该计算机程序在被处理器执行时，可实现上述各个方法实施例的步骤。其中，计算机程序包括计算机程序代码，计算机程序代码可以为源代码形式、对象代码形式、可执行文件或某些中间形式等。计算机可读介质可以包括：能够携带计算机程序代码的任何实体或装置、记录介质、U盘、移动硬盘、磁碟、光盘、计算机存储器、只读存储器(ROM，Read-Only Memory)、随机存取存储器(RAM，RandomAccess Memory)、电载波信号、电信信号以及软件分发介质等。需要说明的是，计算机可读介质包含的内容可以根据司法管辖区内立法和专利实践的要求进行适当的增减，例如在某些司法管辖区，根据立法和专利实践，计算机可读介质不包括电载波信号和电信信号。

基于同一发明构思，本申请实施例还提供一种电子设备，包括存储器和处理器，存储器上储存有在处理器上运行的计算机程序，处理器执行计算机程序时实现上述方法中的所有方法步骤或部分方法步骤。

所称处理器可以是中央处理单元(Central Processing Unit，CPU)，还可以是其他通用处理器、数字信号处理器(Digital Signal Processor，DSP)、专用集成电路(Application Specific Integrated Circuit，ASIC)、现成可编程门阵列(Field-Programmable Gate Array，FPGA)或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件等。通用处理器可以是微处理器或者该处理器也可以是任何常规的处理器等，处理器是计算机装置的控制中心，利用各种接口和线路连接整个计算机装置的各个部分。

存储器可用于存储计算机程序和/或模块，处理器通过运行或执行存储在存储器内的计算机程序和/或模块，以及调用存储在存储器内的数据，实现计算机装置的各种功能。存储器可主要包括存储程序区和存储数据区，其中，存储程序区可存储操作系统、至少一个功能所需的应用程序(例如声音播放功能、图像播放功能等)；存储数据区可存储根据手机的使用所创建的数据(例如音频数据、视频数据等)。此外，存储器可以包括高速随机存取存储器，还可以包括非易失性存储器，例如硬盘、内存、插接式硬盘，智能存储卡(SmartMedia Card，SMC)，安全数字(Secure Digital，SD)卡，闪存卡(Flash Card)、至少一个磁盘存储器件、闪存器件、或其他易失性固态存储器件。

本领域内的技术人员应明白，本发明的实施例可提供为方法、系统、服务器或计算机程序产品。因此，本发明可采用完全硬件实施例、完全软件实施例、或结合软件和硬件方面的实施例的形式。而且，本发明可采用在一个或多个其中包含有计算机可用程序代码的计算机可用存储介质(包括但不限于磁盘存储器和光学存储器等)上实施的计算机程序产品的形式。

本发明是参照根据本发明实施例的方法、设备(系统)、服务器和计算机程序产品的流程图和/或方框图来描述的。应理解可由计算机程序指令实现流程图和/或方框图中的每一流程和/或方框、以及流程图和/或方框图中的流程和/或方框的结合。可提供这些计算机程序指令到通用计算机、专用计算机、嵌入式处理机或其他可编程数据处理设备的处理器以产生一个机器，使得通过计算机或其他可编程数据处理设备的处理器执行的指令产生用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的装置。

这些计算机程序指令也可存储在能引导计算机或其他可编程数据处理设备以特定方式工作的计算机可读存储器中，使得存储在该计算机可读存储器中的指令产生包括指令装置的制造品，该指令装置实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能。

这些计算机程序指令也可装载到计算机或其他可编程数据处理设备上，使得在计算机或其他可编程设备上执行一系列操作步骤以产生计算机实现的处理，从而在计算机或其他可编程设备上执行的指令提供用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的步骤。

显然，本领域的技术人员可以对本发明进行各种改动和变型而不脱离本发明的精神和范围。这样，倘若本发明的这些修改和变型属于本发明权利要求及其等同技术的范围之内，则本发明也意图包含这些改动和变型在内。