CN113561968A - 一种车辆水平泊出轨迹规划方法及装置、车辆、存储介质 - Google Patents

一种车辆水平泊出轨迹规划方法及装置、车辆、存储介质 Download PDF

Info

Publication number
CN113561968A
CN113561968A CN202110995153.4A CN202110995153A CN113561968A CN 113561968 A CN113561968 A CN 113561968A CN 202110995153 A CN202110995153 A CN 202110995153A CN 113561968 A CN113561968 A CN 113561968A
Authority
CN
China
Prior art keywords
track
vehicle
backward
target
trajectory
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Granted
Application number
CN202110995153.4A
Other languages
English (en)
Other versions
CN113561968B (zh
Inventor
李飞
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Shanghai Ofilm Intelligent Vehicle Co ltd
Original Assignee
Shanghai Ofilm Intelligent Vehicle Co ltd
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Shanghai Ofilm Intelligent Vehicle Co ltd filed Critical Shanghai Ofilm Intelligent Vehicle Co ltd
Priority to CN202110995153.4A priority Critical patent/CN113561968B/zh
Publication of CN113561968A publication Critical patent/CN113561968A/zh
Application granted granted Critical
Publication of CN113561968B publication Critical patent/CN113561968B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B60—VEHICLES IN GENERAL
    • B60W—CONJOINT CONTROL OF VEHICLE SUB-UNITS OF DIFFERENT TYPE OR DIFFERENT FUNCTION; CONTROL SYSTEMS SPECIALLY ADAPTED FOR HYBRID VEHICLES; ROAD VEHICLE DRIVE CONTROL SYSTEMS FOR PURPOSES NOT RELATED TO THE CONTROL OF A PARTICULAR SUB-UNIT
    • B60W30/00—Purposes of road vehicle drive control systems not related to the control of a particular sub-unit, e.g. of systems using conjoint control of vehicle sub-units
    • B60W30/06—Automatic manoeuvring for parking
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B60—VEHICLES IN GENERAL
    • B60W—CONJOINT CONTROL OF VEHICLE SUB-UNITS OF DIFFERENT TYPE OR DIFFERENT FUNCTION; CONTROL SYSTEMS SPECIALLY ADAPTED FOR HYBRID VEHICLES; ROAD VEHICLE DRIVE CONTROL SYSTEMS FOR PURPOSES NOT RELATED TO THE CONTROL OF A PARTICULAR SUB-UNIT
    • B60W2510/00—Input parameters relating to a particular sub-units
    • B60W2510/18—Braking system
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B60—VEHICLES IN GENERAL
    • B60W—CONJOINT CONTROL OF VEHICLE SUB-UNITS OF DIFFERENT TYPE OR DIFFERENT FUNCTION; CONTROL SYSTEMS SPECIALLY ADAPTED FOR HYBRID VEHICLES; ROAD VEHICLE DRIVE CONTROL SYSTEMS FOR PURPOSES NOT RELATED TO THE CONTROL OF A PARTICULAR SUB-UNIT
    • B60W2520/00—Input parameters relating to overall vehicle dynamics

Landscapes

  • Engineering & Computer Science (AREA)
  • Automation & Control Theory (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Transportation (AREA)
  • Mechanical Engineering (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Control Of Driving Devices And Active Controlling Of Vehicle (AREA)

Abstract

本发明实施例公开一种车辆水平泊出轨迹规划方法及装置、车辆、存储介质，该方法包括：根据车辆信息在根据车辆的初始位置建立的初始世界坐标系上设置目标停车线；将根据车辆环视信息提取得到的可通行空间栅格地图的数据转换到初始世界坐标系上获得目标世界坐标系；根据车辆的当前位置和车辆的最小转弯半径在坐标系上模拟车辆前进规划获得前进轨迹和模拟车辆后退规划获得后退轨迹；判断前进轨迹的终点是否满足完全泊出条件；满足时将前进轨迹选择为车辆行驶的目标轨迹；未满足时判断后退轨迹的长度是否小于前进轨迹的长度；小于则选择前进轨迹为目标轨迹；不小于则选择后退轨迹为目标轨迹；提供合理的泊出轨迹，提高车辆水平泊出的可靠性和安全性。

Description

一种车辆水平泊出轨迹规划方法及装置、车辆、存储介质
技术领域
本发明涉及汽车自动辅助驾驶技术领域，尤其涉及一种车辆水平泊出轨迹规划方法及装置、车辆、存储介质。
背景技术
随着智能驾驶技术的不断发展和应用，自动泊车技术在汽车领域也逐渐被普及开来，自动泊车有自动泊入和自动泊出。目前，在水平车位泊出中，通过将几何场景中的障碍物等信息进行角点简化，如只通过两个角点来表示某障碍物，并没有指示出障碍区域，只能知道该角点附近有障碍物，但是并不知道障碍物所在区域的大小，导致泊出轨迹失败或者规划出无用的泊出轨迹，使得车辆在水平泊出过程中存在一定的安全隐患。
发明内容
本发明实施例公开了一种车辆水平泊出轨迹规划方法及装置、车辆、存储介质，用于提供合理的泊出轨迹，提高车辆水平泊出的可靠性和安全性。
本发明实施例第一方面公开了一种车辆水平泊出轨迹规划方法，可包括：
根据车辆信息在初始世界坐标系上设置目标停车线，所述初始世界坐标系根据所述车辆的初始位置建立；
将可通行空间栅格地图的数据转换到所述初始世界坐标系上，获得目标世界坐标系，所述可通行空间栅格地图根据车辆环视信息提取得到；
根据所述车辆的当前位置和所述车辆的最小转弯半径，在所述目标世界坐标系模拟车辆前进规划获得前进轨迹和模拟车辆后退规划获得后退轨迹；
判断所述前进轨迹的终点是否满足完全泊出条件；
如果满足所述完全泊出条件，将所述前进轨迹选择为所述车辆行驶的目标轨迹；
如果未满足所述完全泊出条件，判断所述后退轨迹的长度是否小于所述前进轨迹的长度；
如果小于所述前进轨迹的长度，选择所述前进轨迹为所述目标轨迹；
如果不小于所述前进轨迹的长度，选择所述后退轨迹为所述目标轨迹。
作为一种可选的实施方式，在本发明实施例第一方面中，在选择出所述目标轨迹之后，所述方法还包括：
控制所述车辆按照所述目标轨迹行驶。
作为一种可选的实施方式，在本发明实施例第一方面中，所述方法还包括：
在控制所述车辆按照所述目标轨迹行驶时，判断所述车辆的当前位置是否满足所述完全泊出条件；
如果未满足，检测所述目标轨迹是否行驶完毕或者所述车辆是否被刹停；
如果所述目标轨迹行驶完毕或者所述车辆被刹停，执行所述将可通行空间栅格地图的数据转换到所述初始世界坐标系上，获得目标世界坐标系，所述可通行空间栅格地图根据车辆环视信息提取得到的步骤；
如果所述目标轨迹没有行驶完毕且所述车辆没有被刹停，执行所述控制所述车辆按照所述目标轨迹行驶。
作为一种可选的实施方式，在本发明实施例第一方面中，所述根据车辆信息在初始世界坐标系上设置目标停车线，包括：
获取停车阈值，所述停车阈值等于所述车辆的宽度与预设值的和；
在所述初始世界坐标系上设置所述目标停车线，所述目标停车线的方程式为liney:y＝d，d＝carwidth+c，liney:y为目标停车线，d为所述停车阈值，carwidth为所述车辆的宽度，c为所述预设值。
作为一种可选的实施方式，在本发明实施例第一方面中，所述判断所述前进轨迹的终点是否满足完全泊出条件，包括：
获取所述前进轨迹的终点的位姿参数，所述终点的位姿参数包括在所述目标世界坐标系中的x值、y值和航向角对应的弧度；
计算圆周率π与2的第一比值；
计算所述航向角对应的弧度与所述第一比值的差值的绝对值；
判断所述y值是否大于等于所述停车阈值且所述绝对值是否小于弧度阈值，所述弧度阈值等于10与180的第二比值再乘以圆周率π得到的乘积；
当所述y值大于等于所述停车阈值且所述绝对值小于所述弧度阈值时，确定所述当前轨迹的终点满足所述完全泊出条件。
作为一种可选的实施方式，在本发明实施例第一方面中，所述根据所述车辆的当前位置和所述车辆的最小转弯半径，在所述目标世界坐标系模拟车辆前进规划获得前进轨迹和模拟车辆后退规划获得后退轨迹，包括：
根据所述车辆的当前位置和所述车辆的最小转弯半径，在所述目标世界坐标系上设置第一参考圆、第二参考圆和第三参考圆，其中，所述第一参考圆与所述目标停车线相切于第一相切点，所述第一参考圆与所述第三参考圆相切于第二相切点，所述第二参考圆与所述第三参考圆相切于第三相切点，所述第三相切点为所述车辆的当前位置；
获取前进参考曲线，根据所述前进参考曲线，采用轨迹生成算法生成前进轨迹，所述前进参考曲线由所述第三参考圆上从所述第三相切点至所述第二相切点所对应的弧线与所述第一参考圆上从所述第二相切点至所述第一相切点所对应的弧线组成；
在所述车辆的当前位置对应的航向角小于第一角度阈值时，获取第一后退参考曲线，根据所述第一后退参考曲线，采用所述轨迹生成算法生成后退轨迹，所述第一后退参考曲线为y＝0；
在所述车辆的航向角不小于所述第一角度阈值时，获取第二后退参考曲线，根据所述第二后退参考曲线，采用所述轨迹生成算法生成所述后退轨迹，所述第二后退参考曲线为所述第二参考圆。
作为一种可选的实施方式，在本发明实施例第一方面中，在所述后退轨迹是根据所述第一后退参考曲线生成时，所述后退轨迹的长度不大于长度阈值；
在所述后退轨迹是根据所述第二后退参考曲线生成时，所述后退轨迹在所述车辆的当前位置对应的航向角大于第二角度阈值时停止生成。
作为一种可选的实施方式，在本发明实施例第一方面中，所述第三参考圆的圆心坐标计算公式如下：
O3.x＝carp.cx-R*sin(carp.cyaw)
O3.y＝carp.cy+R*cos(carp.cyaw)
所述第二参考圆的圆心坐标计算公式如下：
O2.x＝carp.cx+R*sin(carp.cyaw)
O3.y＝carp.cy-R*cos(carp.cyaw)
所述第一参考圆的圆心坐标计算公式如下：
O1.y＝d-R；
Figure BDA0003233788920000041
其中，carp.cx为所述车辆的当前位置对应的在所述目标世界坐标系上的X轴上的值，carp.cy为所述车辆的当前位置对应的在所述目标世界坐标系上的Y轴上的值，carp.cyaw为所述车辆的当前位置对应的航向角对应的弧度。
作为一种可选的实施方式，在本发明实施例第一方面中，所述获取前进参考曲线，根据所述前进参考曲线，采用轨迹生成算法生成前进轨迹，包括：
获取前进参考曲线，根据所述前进参考曲线，采用轨迹生成算法生成前进生成轨迹；
对所述前进生成轨迹进行碰撞检测；
在未检测到碰撞时，将所述前进生成轨迹作为前进轨迹；
所述根据所述第一后退参考曲线，采用所述轨迹生成算法生成后退轨迹，包括：
根据所述第一后退参考曲线，采用所述轨迹生成算法生成第一后退生成轨迹；
对所述第一后退生成轨迹进行碰撞检测；
在未检测到碰撞时，将所述第一后退生成轨迹作为后退轨迹；
所述根据所述第二后退参考曲线，采用所述轨迹生成算法生成所述后退轨迹，包括：
根据所述第二后退参考曲线，采用所述轨迹生成算法生成第二后退生成轨迹；
对所述第二后退生成轨迹进行碰撞检测；
在未检测到碰撞时，将所述第二后退生成轨迹作为后退轨迹。
本发明实施例第二方面公开了一种车辆水平泊出轨迹规划装置，可包括：
设置模块，用于根据车辆信息在初始世界坐标系上设置目标停车线，所述初始世界坐标系根据所述车辆的初始位置建立；
转换模块，用于将可通行空间栅格地图的数据转换到所述初始世界坐标系上，获得目标世界坐标系，所述可通行空间栅格地图根据车辆环视信息提取得到；
模拟模块，用于根据所述车辆的当前位置和所述车辆的最小转弯半径，在所述目标世界坐标系模拟车辆前进规划获得前进轨迹和模拟车辆后退规划获得后退轨迹；
判断模块，用于判断所述前进轨迹的终点是否满足完全泊出条件；
选择模块，用于如果所述判断模块判定所述前进轨迹的终点满足所述完全泊出条件，将所述前进轨迹选择为所述车辆行驶的目标轨迹；
所述判断模块，还用于如果判定所述前进轨迹的终点未满足所述完全泊出条件，判断所述后退轨迹的长度是否小于所述前进轨迹的长度；
所述选择模块，还用于如果所述判断模块判定所述后退轨迹的长度小于所述前进轨迹的长度，选择所述前进轨迹为所述目标轨迹；以及，如果所述判断模块判定所述后退轨迹的长度不小于所述前进轨迹的长度，选择所述后退轨迹为所述目标轨迹。
本发明实施例第三方面公开了一种车辆，可包括：
存储有可执行程序代码的存储器；
与所述存储器耦合的处理器；
所述处理器调用所述存储器中存储的所述可执行程序代码，执行本发明实施例第一方面公开的一种车辆水平泊出轨迹规划方法。
本发明实施例第四方面公开一种计算机可读存储介质，其上存储有计算机程序，该程序被处理器执行时实现本发明实施例第一方面的任意一种方法的步骤。
本发明实施例第五方面公开一种计算机程序产品，当所述计算机程序产品在计算机上运行时，使得所述计算机执行第一方面的任意一种方法的部分或全部步骤。
本发明实施例第六方面公开一种应用发布平台，所述应用发布平台用于发布计算机程序产品，其中，当所述计算机程序产品在计算机上运行时，使得所述计算机执行第一方面的任意一种方法的部分或全部步骤。
与现有技术相比，本发明实施例具有以下有益效果：
在本发明实施例中，首先，根据车辆信息在初始世界坐标系上设置目标停车线，初始世界坐标系是根据车辆的初始位置建立的，然后再将可通行空间栅格地图的数据转换到初始世界坐标系上，获得目标世界坐标系，该可通行空间栅格地图根据车辆环视信息提取得到，进一步，根据该车辆的当前位置和车辆的最小转弯半径，在目标世界坐标系中模拟车辆前进规划获得前进轨迹和模拟车辆后退规划获得后退轨迹，判断该前进轨迹的终点是否满足完全泊出条件，如果满足完全泊出条件，将前进轨迹选择为车辆行驶的目标轨迹，如果未满足完全泊出条件，再进一步判断后退轨迹的长度是否小于前进轨迹的长度，如果小于前进轨迹的长度，选择前进轨迹为目标轨迹，如果不小于前进轨迹的长度，选择后退轨迹为目标轨迹；可见，通过实施本发明实施例，能够结合可通行空间栅格地图为车辆模拟车辆前进规划和车辆后退规划，然后再结合前进轨迹和后退轨迹的情况，以及车辆水平完全泊出条件，以从前进轨迹或后退轨迹中选择出合理的目标轨迹，适用于水平各种车位泊出场景，使得车辆可安全的水平完全泊出，可靠性较高。
附图说明
为了更清楚地说明本发明实施例中的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。
图1为本发明实施例一公开的车辆水平泊出轨迹规划方法的流程示意图；
图2为本发明实施例二公开的车辆水平泊出轨迹规划方法的流程示意图；
图3为本发明实施例一公开的车辆水平泊出轨迹规划装置的结构示意图；
图4为本发明实施例二公开的车辆水平泊出轨迹规划装置的结构示意图；
图5为本发明实施例公开的车辆的结构示意图；
图6为本发明实施例公开的目标停车线的示意图；
图7为本发明实施例公开的车辆最小转弯半径的示意图；
图8为本发明实施例一公开的前进/后退参考曲线及前进/后退轨迹的示意图；
图9为本发明实施例二公开的前进/后退参考曲线及前进/后退轨迹的示意图；
图10为本发明实施例公开的轨迹生成算法的原理示意图。
具体实施方式
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
需要说明的是，本发明的说明书和权利要求书中的术语“第一”、“第二”、“第三”及“第四”等是用于区别不同的对象，而不是用于描述特定顺序。本发明实施例的术语“包括”和“具有”以及他们的任何变形，意图在于覆盖不排他的包含，例如，包含了一系列步骤或单元的过程、方法、系统、产品或设备不必限于清楚地列出的那些步骤或单元，而是可包括没有清楚地列出的或对于这些过程、方法、产品或设备固有的其它步骤或单元。
本发明实施例公开了一种车辆水平泊出轨迹规划方法及装置、车辆、存储介质，用于提供合理的泊出轨迹，提高车辆水平泊出的可靠性和安全性。
下面将通过具体实施例，对本发明技术方案进行详细说明。
请参阅图1，图1为本发明实施例一公开的车辆水平泊出轨迹规划方法的流程示意图；如图1所示，该车辆水平泊出轨迹规划方法可包括：
101、根据车辆信息在初始世界坐标系上设置目标停车线，该初始世界坐标系根据车辆的初始位置建立。
其中，本发明实施例用于车辆的水平泊出，因此可以理解，车辆的初始位置为车辆泊在水平车位上的位置，初始世界坐标系WXOY以车辆的初始位置为原点建立，其中，Y轴方向垂直于车身向外，X轴方向与车身方向平行。
作为一种可选的实施方式，步骤101具体包括以下实施步骤：
获取停车阈值，停车阈值等于车辆的宽度与预设值的和；
在初始世界坐标系上设置目标停车线，该目标停车线的方程式为liney:y＝d，d＝carwidth+c，liney:y为目标停车线，d为停车阈值，carwidth为车辆的宽度，c为预设值。
其中，获取车辆信息，该车辆信息包括车辆的宽度、长度等，根据车辆信息中的车辆的宽度计算出停车阈值，停车阈值等于车辆的宽度与预设值的和，而目标停车线的方程式即为liney:y＝d，d＝carwidth+c，liney:y表示目标停车线，d为停车阈值，carwidth为车辆的宽度，c为预设值。
可选的，预设值可以设定，根据实际情况设定，示例性的，预设值可以设定为1.0m，即liney:y＝carwidth+1.0m，并设置车辆的初始行驶方向为前进。
请参阅图6，图6为本发明实施例公开的目标停车线的示意图；图6中的长方形表示水平停车位，所示的三个停车位均表示泊有车辆，针对中间停车位的车辆的水平泊出，以车辆的初始位置为原点建立初始世界坐标系WXOY，其中，Y轴方向垂直于车身向外，而X轴与车身方向平行，方向沿车头方向，图中的虚线表示目标停车线，而虚线长方形为车辆完全水平泊出后的理想位置。
102、将可通行空间栅格地图的数据转换到初始世界坐标系上，获得目标世界坐标系，该可通行空间栅格地图根据车辆环视信息提取得到。
其中，本发明实施例提供的视觉可通行空间(freespace)栅格地图是由视觉算法模块根据车辆环视信息提取得到，具体的提取方法为现有技术，在此不再赘述，该视觉freespace栅格地图以车辆后轴中心为地图中心，基于车辆坐标系建立，示例性的，freespace栅格地图的大小可以为200x400，尺寸5cm，数值为0或1，其中，0表示可通行，1表示被占有，不可通行。
在本发明实施例中，将freespace栅格地图的数据转换到初始时间坐标系中，从而获得目标世界坐标系，需要说明的是，目标世界坐标系中已经设置有目标停车线。同时，还需要说明的是，车辆环视信息与车辆的当前位置有关，会因为车辆行驶导致当前位置不同而不同，准确指示车辆的周边环境。
103、根据车辆的当前位置和车辆的最小转弯半径，在目标世界坐标系模拟车辆前进规划获得前进轨迹和模拟车辆后退规划获得后退轨迹。
请参阅图7，图7为本发明实施例公开的车辆最小转弯半径的示意图。结合图7，本发明实施例提供的车辆最小转弯半径，为车辆后轴中心最小转弯半径，是指车辆以最大前轮转向角度θmax时的转弯半径，由阿克曼转向原理可得：
Figure BDA0003233788920000091
其中，L为车辆轴距，carwidth为车辆的宽度。
另外，车辆以最大前轮转向角度θmax时，车辆头部最小转弯半径为Rf，由阿克曼转向原理可得：
Figure BDA0003233788920000092
其中，L1为后轴中心P到车辆前保险的距离。
104、判断前进轨迹的终点是否满足完全泊出条件；其中，如果满足完全泊出条件，转向步骤105；如果未满足完全泊出条件，转向步骤106。
作为一种可选的实施方式，上述判断前进轨迹的终点是否满足完全泊出条件，包括：
获取前进轨迹的终点的位姿参数，该终点的位姿参数包括在目标世界坐标系中的x值、y值和航向角对应的弧度；
计算圆周率π与2的第一比值；
计算航向角对应的弧度与上述第一比值的差值的绝对值；
判断y值是否大于等于停车阈值且绝对值是否小于弧度阈值，该弧度阈值等于10与180的第二比值再乘以圆周率π得到的乘积；
当y值大于等于停车阈值且绝对值小于弧度阈值时，确定当前轨迹的终点满足完全泊出条件；
当y值小于停车阈值或者绝对值小于弧度阈值时，确定当前轨迹的终点未满足完全泊出条件。
其中，在上述计算中采用的是航向角的弧度来计算，弧度阈值可以根据(10/180)*π设置，10即是10°，180即是180°，通过计算(10/180)*π转换成弧度。
具体的，该完全泊出条件可以是：
carp.cy>＝d且|carp.cyaw-π/2|<(10/180)*π；
其中，车辆位置的位姿参数为carp(cx，cy，cyaw)，其中，carp.cy为车辆位置中在目标世界坐标系Y轴上的坐标值，也就是位姿参数中的cx，即carp.cy的表示方式仅表示位置点carp的cy，cx为车辆位置中在目标世界坐标系X轴上的坐标值，也就是位姿参数中的cy，carp.cyaw为车辆的航向角对应的弧度，也就是位姿参数中的cyaw，d即上述介绍的停车阈值，π为圆周率。
因此，在步骤104中，前进轨迹的终点的位姿参数表示为Pathfendp(cx，cy，cyaw)，其中，将Pathfendp.cx作为上述x值，将Pathfendp.cx作为上述y值，Pathfendp.cyaw为前进轨迹的终点航向角对应的弧度，进而判断前进轨迹的终点是否满足完全泊出条件，即：
Pathfendp.cy>＝d且|Pathfendp.cyaw-π/2|<(10/180)*π；
Pathfendp.cy为前进轨迹的终点在目标世界坐标系Y轴上的坐标值y值，carp.cyaw为前进轨迹的终点对应的车辆航向角对应的弧度。
105、将前进轨迹选择为车辆行驶的目标轨迹。
在步骤105中，前进轨迹的终点满足完全泊出条件，因此，在选择前进轨迹作为目标轨迹，并控制车辆按照该目标轨迹行驶完之后，将完全水平泊出停车位，完全泊出，此时将结束本流程。
106、判断后退轨迹的长度是否小于前进轨迹的长度；如果小于前进轨迹的长度，转向步骤107；如果不小于前进轨迹的长度，转向步骤108。
在步骤106中，前进轨迹的终点未满足完全泊出条件，那么进一步判断是选择前进轨迹作为目标轨迹，还是选择后退轨迹作为目标轨迹，具体的，是判断后退轨迹的长度是否小于前进轨迹的长度。
107、选择前进轨迹为目标轨迹。
在步骤107中，如果后退轨迹的长度小于前进轨迹的长度，那么优选前进轨迹作为目标轨迹，车辆行驶方向为前进。
108、选择后退轨迹为目标轨迹。
在后退轨迹的长度不小于前进轨迹的长度时，选择后退轨迹作为目标轨迹，车辆行驶方向为后退。
通过实施上述实施例，根据车辆信息在初始世界坐标系上设置目标停车线，初始世界坐标系是根据车辆的初始位置建立的，然后再将可通行空间栅格地图的数据转换到初始世界坐标系上，获得目标世界坐标系，该可通行空间栅格地图根据车辆环视信息提取得到，进一步，根据该车辆的当前位置和车辆的最小转弯半径，在目标世界坐标系中模拟车辆前进规划获得前进轨迹和模拟车辆后退规划获得后退轨迹，判断该前进轨迹的终点是否满足完全泊出条件，如果满足完全泊出条件，将前进轨迹选择为车辆行驶的目标轨迹，如果未满足完全泊出条件，再进一步判断后退轨迹的长度是否小于前进轨迹的长度，如果小于前进轨迹的长度，选择前进轨迹为目标轨迹，如果不小于前进轨迹的长度，选择后退轨迹为目标轨迹；可见，通过实施本发明实施例，能够结合可通行空间栅格地图为车辆模拟车辆前进规划和车辆后退规划，然后再结合前进轨迹和后退轨迹的情况，以及车辆水平完全泊出条件，以从前进轨迹或后退轨迹中选择出合理的目标轨迹，适用于水平各种车位泊出场景，使得车辆可安全的水平完全泊出，可靠性较高。
请参阅图2，图2为本发明实施例二公开的车辆水平泊出轨迹规划方法的流程示意图；如图2所示，该车辆水平泊出轨迹规划方法可包括：
201、根据车辆信息在初始世界坐标系上设置目标停车线，该初始世界坐标系根据车辆的初始位置建立。
202、将可通行空间栅格地图的数据转换到初始世界坐标系上，获得目标世界坐标系，该可通行空间栅格地图根据车辆环视信息提取得到。
203、根据车辆的当前位置和车辆的最小转弯半径，在目标世界坐标系模拟车辆前进规划获得前进轨迹和模拟车辆后退规划获得后退轨迹。
作为一种可选的实施方式，上述根据车辆的当前位置和车辆的最小转弯半径，在目标世界坐标系模拟车辆前进规划获得前进轨迹和模拟车辆后退规划获得后退轨迹可以包括：
根据车辆的当前位置和车辆的最小转弯半径，在目标世界坐标系上设置第一参考圆、第二参考圆和第三参考圆，其中，该第一参考圆与目标停车线相切于第一相切点，该第一参考圆与第三参考圆相切于第二相切点，该第二参考圆与第三参考圆相切于第三相切点，该第三相切点为车辆的当前位置；
获取前进参考曲线，根据前进参考曲线，采用轨迹生成算法生成前进轨迹，该前进参考曲线由第三参考圆上从第三相切点至第二相切点所对应的弧线与第一参考圆上从第二相切点至所述第一相切点所对应的弧线组成；
在车辆的当前位置对应的航向角小于第一角度阈值时，获取第一后退参考曲线，根据第一后退参考曲线，采用轨迹生成算法生成后退轨迹，该第一后退参考曲线为y＝0；
在车辆的航向角不小于第一角度阈值时，获取第二后退参考曲线，根据第二后退参考曲线，采用轨迹生成算法生成后退轨迹，该第二后退参考曲线为所述第二参考圆。
其中，上述车辆的最小转弯半径即为上述介绍的最小转弯半径R，根据车辆的当前位置的位姿参数carp(cx，cy，cyaw)和车辆的最小转弯半径R在世界坐标系上设置三个圆，分别为第一参考圆O1，第二参考圆O2，第三参考圆O3，其中，第三参考圆O3的圆心坐标计算公式如下：
O3.x＝carp.cx-R*sin(carp.cyaw)
O3.y＝carp.cy+R*cos(carp.cyaw)
第二参考圆O2的圆心坐标计算公式如下：
O2.x＝carp.cx+R*sin(carp.cyaw)
O3.y＝carp.cy-R*cos(carp.cyaw)
第一参考圆O1的圆心坐标计算公式如下：
O1.y＝d-R；
Figure BDA0003233788920000131
其中，carp.cx为车辆的当前位置对应的在目标世界坐标系上的X轴上的值，即坐姿参数中的cx，carp.cy为车辆的当前位置对应的在目标世界坐标系上的Y轴上的值，即坐姿参数中的cy，carp.cyaw为车辆的当前位置对应的航向角对应的弧度，即坐姿参数中的cyaw。
请进一步参阅图8和图9，图8为本发明实施例一公开的前进/后退参考曲线及前进/后退轨迹的示意图，图9为本发明实施例二公开的前进/后退参考曲线及前进/后退轨迹的示意图；在图8和图9中，第一参考圆O1的圆心表示为(O1.x，O1.y)，第一参考圆O1的半径为车辆的最小转弯半径R，且该第一参考圆O1与目标停车线liney:y＝d相切于第一相切点p1。第二参考圆O2的圆心表示为(O2.x，O2.y)，第二参考圆O2的半径也为车辆的最小转弯半径R，且第二参考圆O2与第三参考圆O3相切于第三相切点，即车辆的当前位置carp(cx，cy，cyaw)。第三参考圆O3的圆心表示为(O3.x，O3.y)，第三参考圆O3的半径也为车辆的最小转弯半径R，第一参考圆O1与第三参考圆O3相切于第二相切点P13。
在采用轨迹生成算法生成前进轨迹时，由第三参考圆O3上从第三相切点carp(cx，cy，cyaw)至第二相切点P13所对应的弧线与第一参考圆O1上从第二相切点P13至第一相切点p1所对应的弧线得到前进参考曲线，根据该前进参考曲线生成前进轨迹。其中，第三参考圆O3上从第三相切点carp(cx，cy，cyaw)至第二相切点P13所对应的弧线，即图8和图9所示的弧线carpp13，为第三参考圆O3边上的一段弧线；第二相切点P13至第一相切点p1所对应的弧线，即图8和图9中所示的弧线p13p1，为第一参考圆O1边上的一段弧线，而生成后的前进轨迹如图8和图9中加粗的线条所示，起点从carp(cx，cy，cyaw)开始，并不完全包括弧线carpp13和弧线p13p1，且在p1处还向外延伸一定长度。
进一步的，上述获取前进参考曲线，根据前进参考曲线，采用轨迹生成算法生成前进轨迹，包括：
获取前进参考曲线，根据前进参考曲线，采用轨迹生成算法生成前进生成轨迹；
对前进生成轨迹进行碰撞检测；
在未检测到碰撞时，将前进生成轨迹作为前进轨迹。
在上述实施方式中，根据轨迹生成算法先生成前进生成轨迹，然后需要进一步对该前进生成轨迹进行碰撞检测，如果检测到发生碰撞，那么将退出检测，并记录结束点，将截取发生碰撞前的前进生成轨迹作为前进轨迹，或者重新规划，而没有检测到碰撞时，该前进生成轨迹作为前进轨迹。
进一步具体的，对前进生成轨迹进行碰撞检测具体包括：
从第三相切点(车辆的当前位置carp(cx，cy，cyaw))开始逐个轨迹点检测该前进生成轨迹上是否存在障碍物，当检测到障碍物时，将检测到该障碍物的前一轨迹点记录为结束点，当未检测到障碍物时，直接将该前进生成轨迹作为前进轨迹。
通过上述实施方式，能够增加碰撞检测，以提高前进轨迹的可靠性，从而提高车辆水平泊出的安全性。
另外，还将进行后退规划，而后退轨迹所获得的后退轨迹分两种情况获得：
第一种情况下，在车辆的当前位置对应的航向角小于第一角度阈值时(或者将第一角度阈值转换成对应的弧度，航向角对应的弧度小于该弧度时)，根据第一后退参考曲线，采用轨迹生成算法生成后退轨迹，其中，第一角度阈值可以预先设定，示例性的，可以将第一角度阈值设置为5°，在航向角＜5°，根据第一后退参考曲线y＝0，即是参考目标世界坐标系的X轴，采用轨迹生成算法向后生成后退轨迹，并且该后退轨迹的长度不大于长度阈值。
可选的，上述长度阈值可以预先设定，示例性的，该长度阈值可以为2m。
进一步的，上述根据第一后退参考曲线，采用轨迹生成算法生成后退轨迹，包括：
根据第一后退参考曲线，采用轨迹生成算法生成第一后退生成轨迹；
对第一后退生成轨迹进行碰撞检测；
在未检测到碰撞时，将第一后退生成轨迹作为后退轨迹。
在上述实施方式中，根据轨迹生成算法先生成第一后退轨迹，然后需要进一步对该第一后退轨迹进行碰撞检测，如果检测到发生碰撞，那么将退出检测，并记录结束点，将截取发生碰撞前的轨迹作为后退轨迹，或者重新规划，而没有检测到碰撞时，该第一后退生成轨迹作为后退轨迹；通过该实施方式，能够提高后退轨迹的可靠性，从而提高车辆水平泊出的安全性。
第二种情况下，在车辆的当前位置对应的航向角不小于第一角度阈值时，在车辆的航向角不小于第一角度阈值时，根据第二后退参考曲线，采用轨迹生成算法生成后退轨迹，该后退轨迹在车辆的当前位置对应的航向角大于第二角度阈值时停止生成。
进一步的，上述根据第二后退参考曲线，采用轨迹生成算法生成后退轨迹包括：
根据第二后退参考曲线，采用轨迹生成算法生成第二后退生成轨迹；对第二后退生成轨迹进行碰撞检测；在未检测到碰撞时，将第二后退生成轨迹作为后退轨迹。
其中，根据第二参考圆O2，采用轨迹生成算法生成第二后退生成轨迹然后需要进一步对该第二后退轨迹进行碰撞检测，如果检测到发生碰撞，那么将退出检测，并记录结束点，将截取发生碰撞前的轨迹作为后退轨迹，或者重新规划，而没有检测到碰撞时，该第二后退生成轨迹作为后退轨迹；通过该实施方式，能够提高后退轨迹的可靠性，从而提高车辆水平泊出的安全性。
另外，还需要说明的是，在图8和图9中，Pathref1表示上述前进参考曲线，Pathforward表示前进生成轨迹(如图8所示的黑色加粗曲线)，Pathref2表示上述第二后退参考曲线(第二参考圆O2)，pathback表示第二后退生成轨迹(如图8所示的灰色加粗曲线)。
作为一种可选的实施方式，请参阅图10，图10为本发明实施例公开的轨迹生成算法的原理示意图；结合图10，轨迹生成算法采用前视窗口纯跟踪法，通过将车辆简化为自行车模型，以车辆后轴中心为切点(图中Q点)，以车辆纵向车身为切线，通过控制前轮转角，模拟车辆以速度为v沿着基准曲线行驶，获得曲率连续路径点，单步模拟行驶距离dl，单步前轮最大调整偏角Δαmax，具体流程如下：
A1、获取Q(Qx，Qy，Qyaw)。
A2、将Q点映射到基准曲线上，获得映射点P(Px，Py，Pyaw)；
以P点为起点根据车辆行驶方向，在基准曲线上移动L＝1m距离，获取参考点G(Gx，Gy)，此时G点为车辆即将行驶的目标点。
A3、设置O，使得圆过Q和G两点，且在Q点与车辆纵相切，圆心为O，半径R，根据几何计算车辆最先转弯半径R，并根据阿克曼转向原理tanα＝L/R可以得出车辆前轮目标偏转角度αexp，根据车辆速度和当前转角αact，偏差Δα＝αexp-αact，其中αact∈[-Δαmax，Δαmax]，真实车辆转角：αact＝αact+Δα。
A4、车辆根据当前前轮转角αact并行驶dl距离达到Q1(Q1x，Q1y，Q1yaw)，由圆O可以计算β＝L/R，其中L为P和G两点圆弧长度，则可计算Q1点位姿：
Q1yaw＝Qyaw+β
Q1X＝QX+R*(sin(Q1yaw)-sin(Qyaw))
Q1y＝Qy+R*(cos(Qyaw)-cos(Q1yaw))。
A5、根据车辆位置Q1(Q1x，Q1y，Q1yaw)判断是否规划完成(可以判断航向角大小情况或者位置偏差大小情况等)，是则退出循环，若未规划完成，则跳至A2继续执行。
根据上述轨迹生成算法，最后得到的以Q点为起点的轨迹为生成轨迹，如图10中的虚线所示。
基于上述轨迹生成算法，将前进参考曲线代入基准曲线即可生成前进生成轨迹，将第一后退参考曲线代入基准曲线即可生成第一后退生成轨迹，将第二后退参考曲线代入基准曲线即可生成第二后退生成轨迹，在此不再一一代入介绍。
204、判断前进轨迹的终点是否满足完全泊出条件；其中，如果满足完全泊出条件，转向步骤205；如果未满足完全泊出条件，转向步骤206。
205、将前进轨迹选择为车辆行驶的目标轨迹。
206、判断后退轨迹的长度是否小于前进轨迹的长度；如果小于前进轨迹的长度，转向步骤207；如果不小于前进轨迹的长度，转向步骤208。
207、选择前进轨迹为目标轨迹；
208、选择后退轨迹为目标轨迹。
209、控制车辆按照目标轨迹行驶。
210、判断车辆的当前位置是否满足完全泊出条件；其中，如果满足，结束本流程；如果未满足，转向步骤211。
211、检测目标轨迹是否行驶完毕或者车辆是否被刹停；如果目标轨迹行驶完毕或者车辆被刹停，转向步骤202；如果目标轨迹没有行驶完毕且车辆没有被刹停，转向步骤209。
可以理解，在目标轨迹行驶完毕，但是并未能完全泊出车位时，将继续进行前进/后退轨迹规划，直至完全泊出车位为止，或者在行驶完目标轨迹后车辆被刹停或者在未行驶完目标轨迹而车辆被刹停时，也将重新进行前进/后退轨迹规划，直至完全泊出车位为止；在目标轨迹没有行驶完毕且车辆没有被刹停时，继续控制车辆按照目标轨迹行驶。
通过实施上述实施例，能够结合可通行空间栅格地图为车辆模拟车辆前进规划和车辆后退规划，然后再结合前进轨迹和后退轨迹的情况，以及车辆水平完全泊出条件，以从前进轨迹或后退轨迹中选择出合理的目标轨迹，控制车辆按照目标轨迹行驶，在控制目标轨迹行驶过程中，如果当前位置已经满足完全泊出条件，那么将结束本流程，如果当前位置没有满足完全泊出条件，但是目标轨迹行驶完毕或者车辆已经刹停，将重新开始模拟规划轨迹，在目标轨迹没有行驶完毕且车辆也未刹停时，控制车辆继续行驶，本发明实施例适用于水平各种车位泊出场景，使得车辆可安全的水平完全泊出，可靠性较高。
请参阅图3，图3为本发明实施例一公开的车辆水平泊出轨迹规划装置的结构示意图；如图3所示，该车辆水平泊出轨迹规划装置可包括：
设置模块310，用于根据车辆信息在初始世界坐标系上设置目标停车线，初始世界坐标系根据车辆的初始位置建立；
转换模块320，用于将可通行空间栅格地图的数据转换到初始世界坐标系上，获得目标世界坐标系，可通行空间栅格地图根据车辆环视信息提取得到；
模拟模块330，用于根据车辆的当前位置和车辆的最小转弯半径，在目标世界坐标系模拟车辆前进规划获得前进轨迹和模拟车辆后退规划获得后退轨迹；
判断模块340，用于判断前进轨迹的终点是否满足完全泊出条件；
选择模块350，用于如果判断模块判定前进轨迹的终点满足完全泊出条件，将前进轨迹选择为车辆行驶的目标轨迹；
判断模块340，还用于如果判定前进轨迹的终点未满足完全泊出条件，判断后退轨迹的长度是否小于前进轨迹的长度；
选择模块350，还用于如果判断模块340判定后退轨迹的长度小于前进轨迹的长度，选择前进轨迹为目标轨迹；以及，如果判断模块340判定后退轨迹的长度不小于前进轨迹的长度，选择后退轨迹为目标轨迹。
通过实施上述装置，能够结合可通行空间栅格地图为车辆模拟车辆前进规划和车辆后退规划，然后再结合前进轨迹和后退轨迹的情况，以及车辆水平完全泊出条件，以从前进轨迹或后退轨迹中选择出合理的目标轨迹，适用于水平各种车位泊出场景，使得车辆可安全的水平完全泊出，可靠性较高。
请参阅图4，图4为本发明实施例二公开的车辆水平泊出轨迹规划装置的结构示意图；图4所示的车辆水平泊出轨迹规划装置是在图3所示的车辆水平泊出轨迹规划装置的基础上进行优化得到的，图4所示的车辆水平泊出轨迹规划装置还包括：
行驶控制模块410，用于在该选择模块350选择出目标轨迹之后，控制车辆按照目标轨迹行驶。
通过上述实施方式，能够结合可通行空间栅格地图，进行车辆前进/后退规划，通过模拟车辆前进后退获得目标轨迹，以提高车辆自动化完全水平泊出车位的可靠性，而且目标轨迹经过无碰撞检测，能够提高泊出的安全性，使得车辆能安全泊出。
进一步的，该行驶控制模块410，还用于控制车辆按照目标轨迹行驶时，判断车辆的当前位置是否满足完全泊出条件；如果未满足，检测目标轨迹是否行驶完毕或者车辆是否被刹停；如果目标轨迹行驶完毕或者车辆被刹停，触发转换模块320执行将可通行空间栅格地图的数据转换到初始世界坐标系上，获得目标世界坐标系的步骤；如果目标轨迹没有行驶完毕且车辆没有被刹停，触发行驶控制模块执行控制车辆按照目标轨迹行驶。
作为一种可选的实施方式，上述设置模块310用于根据车辆信息在初始世界坐标系上设置目标停车线的方式具体为：
获取停车阈值，停车阈值等于车辆的宽度与预设值的和；以及，在初始世界坐标系上设置目标停车线，目标停车线的方程式为liney:y＝d，d＝carwidth+c，liney:y为目标停车线，d为停车阈值，carwidth为车辆的宽度，c为预设值。
作为一种可选的实施方式，上述判断模块340用于判断前进轨迹的终点是否满足完全泊出条件的方式具体为：
获取前进轨迹的终点的位姿参数，终点的位姿参数包括在目标世界坐标系中的x值、y值和航向角对应的弧度；
计算圆周率π与2的第一比值；
计算航向角对应的弧度与上述第一比值的差值的绝对值；
判断y值是否大于等于停车阈值且绝对值是否小于弧度阈值，该弧度阈值等于10与180的第二比值再乘以圆周率π得到的乘积；
当y值大于等于停车阈值且差值小于弧度阈值时，确定当前轨迹的终点满足完全泊出条件；
当y值小于停车阈值或者绝对值小于弧度阈值时，确定当前轨迹的终点未满足完全泊出条件。
作为一种可选的实施方式，上述模拟模块330用于根据车辆的当前位置和车辆的最小转弯半径，在目标世界坐标系模拟车辆前进规划获得前进轨迹和模拟车辆后退规划获得后退轨迹的方式具体为：
根据车辆的当前位置和车辆的最小转弯半径，在目标世界坐标系上设置第一参考圆、第二参考圆和第三参考圆，其中，第一参考圆与目标停车线相切于第一相切点，第一参考圆与第三参考圆相切于第二相切点，第二参考圆与第三参考圆相切于第三相切点，第三相切点为车辆的当前位置；
获取前进参考曲线，根据前进参考曲线，采用轨迹生成算法生成前进轨迹，前进参考曲线由第三参考圆上从第三相切点至第二相切点所对应的弧线与第一参考圆上从第二相切点至第一相切点所对应的弧线组成；
在车辆的当前位置对应的航向角小于第一角度阈值时，获取第一后退参考曲线，根据第一后退参考曲线，采用轨迹生成算法生成后退轨迹，第一后退参考曲线为y＝0；
在车辆的航向角不小于第一角度阈值时，获取第二后退参考曲线，根据第二后退参考曲线，采用轨迹生成算法生成后退轨迹，第二后退参考曲线为第二参考圆。
进一步，上述模拟模块330用于获取前进参考曲线，根据前进参考曲线，采用轨迹生成算法生成前进轨迹的方式为：
获取前进参考曲线，根据前进参考曲线，采用轨迹生成算法生成前进生成轨迹；
对前进生成轨迹进行碰撞检测；
在未检测到碰撞时，将前进生成轨迹作为前进轨迹；
上述模拟模块330用于根据第一后退参考曲线，采用轨迹生成算法生成后退轨迹的方式具体为：
根据第一后退参考曲线，采用轨迹生成算法生成第一后退生成轨迹；
对第一后退生成轨迹进行碰撞检测；
在未检测到碰撞时，将第一后退生成轨迹作为后退轨迹；
上述模拟模块330用于根据第二后退参考曲线，采用轨迹生成算法生成后退轨迹的方式具体为：
根据第二后退参考曲线，采用轨迹生成算法生成第二后退生成轨迹；
对第二后退生成轨迹进行碰撞检测；
在未检测到碰撞时，将第二后退生成轨迹作为后退轨迹。
可选的，在后退轨迹是根据第一后退参考曲线生成时，后退轨迹的长度不大于长度阈值；
在后退轨迹是根据第二后退参考曲线生成时，后退轨迹在车辆的当前位置对应的航向角大于第二角度阈值时停止生成。
可选的，第三参考圆的圆心坐标计算公式如下：
O3.x＝carp.cx-R*sin(carp.cyaw)
O3.y＝carp.cy+R*cos(carp.cyaw)
第二参考圆的圆心坐标计算公式如下：
O2.x＝carp.cx+R*sin(carp.cyaw)
O3.y＝carp.cy-R*cos(carp.cyaw)
第一参考圆的圆心坐标计算公式如下：
O1.y＝d-R；
Figure BDA0003233788920000211
其中，carp.cx为车辆的当前位置对应的在目标世界坐标系上的X轴上的值，即坐姿参数中的cx，carp.cy为车辆的当前位置对应的在目标世界坐标系上的Y轴上的值，即坐姿参数中的cy，carp.cyaw为车辆的当前位置对应的航向角对应的弧度，即坐姿参数中的cyaw。
请参阅图5，图5为本发明实施例公开的车辆的结构示意图；图5所示的车辆可包括：
存储有可执行程序代码的存储器501；
与存储器501耦合的处理器502；
其中，处理器502调用存储器501中存储的可执行程序代码，执行图1至图2任意一种车辆水平泊出轨迹规划方法的部分步骤。
本发明实施例还公开一种计算机可读存储介质，其存储计算机程序，其中，所述计算机程序使得计算机执行图1至图2公开的一种车辆水平泊出轨迹规划方法。
本发明实施例还公开一种计算机程序产品，当所述计算机程序产品在计算机上运行时，使得所述计算机执行图1至图2公开的任意一种方法的部分或全部步骤。
本发明实施例还公开一种应用发布平台，所述应用发布平台用于发布计算机程序产品，其中，当所述计算机程序产品在计算机上运行时，使得所述计算机执行图1至图2公开的任意一种方法的部分或全部步骤。
本领域普通技术人员可以理解上述实施例的各种方法中的全部或部分步骤是可以通过程序来指令相关的硬件来完成，该程序可以存储于一计算机可读存储介质中，存储介质包括只读存储器(Read-Only Memory，ROM)、随机存储器(Random Access Memory，RAM)、可编程只读存储器(Programmable Read-only Memory，PROM)、可擦除可编程只读存储器(Erasable Programmable Read Only Memory，EPROM)、一次可编程只读存储器(One-time Programmable Read-Only Memory，OTPROM)、电子抹除式可复写只读存储器(Electrically-Erasable Programmable Read-Only Memory，EEPROM)、只读光盘(CompactDisc Read-Only Memory，CD-ROM)或其他光盘存储器、磁盘存储器、磁带存储器、或者能够用于携带或存储数据的计算机可读的任何其他介质。
以上对本发明实施例公开的一种车辆水平泊出轨迹规划方法及装置、车辆、存储介质进行了详细介绍，本文中应用了具体个例对本发明的原理及实施方式进行了阐述，以上实施例的说明只是用于帮助理解本发明的方法及其核心思想；同时，对于本领域的一般技术人员，依据本发明的思想，在具体实施方式及应用范围上均会有改变之处，综上所述，本说明书内容不应理解为对本发明的限制。

Claims (12)

1.一种车辆水平泊出轨迹规划方法，其特征在于，包括：
根据车辆信息在初始世界坐标系上设置目标停车线，所述初始世界坐标系根据所述车辆的初始位置建立；
将可通行空间栅格地图的数据转换到所述初始世界坐标系上，获得目标世界坐标系，所述可通行空间栅格地图根据车辆环视信息提取得到；
根据所述车辆的当前位置和所述车辆的最小转弯半径，在所述目标世界坐标系模拟车辆前进规划获得前进轨迹和模拟车辆后退规划获得后退轨迹；
判断所述前进轨迹的终点是否满足完全泊出条件；
如果满足所述完全泊出条件，将所述前进轨迹选择为所述车辆行驶的目标轨迹；
如果未满足所述完全泊出条件，判断所述后退轨迹的长度是否小于所述前进轨迹的长度；
如果小于所述前进轨迹的长度，选择所述前进轨迹为所述目标轨迹；
如果不小于所述前进轨迹的长度，选择所述后退轨迹为所述目标轨迹。
2.根据权利要求1所述的方法，其特征在于，在选择出所述目标轨迹之后，所述方法还包括：
控制所述车辆按照所述目标轨迹行驶。
3.根据权利要求2所述的方法，其特征在于，所述方法还包括：
在控制所述车辆按照所述目标轨迹行驶时，判断所述车辆的当前位置是否满足所述完全泊出条件；
如果未满足，检测所述目标轨迹是否行驶完毕或者所述车辆是否被刹停；
如果所述目标轨迹行驶完毕或者所述车辆被刹停，执行所述将可通行空间栅格地图的数据转换到所述初始世界坐标系上，获得目标世界坐标系的步骤；
如果所述目标轨迹没有行驶完毕且所述车辆没有被刹停，执行所述控制所述车辆按照所述目标轨迹行驶。
4.根据权利要求1至3任一项所述的方法，其特征在于，所述根据车辆信息在初始世界坐标系上设置目标停车线，包括：
获取停车阈值，所述停车阈值等于所述车辆的宽度与预设值的和；
在所述初始世界坐标系上设置所述目标停车线，所述目标停车线的方程式为liney:y＝d，d＝carwidth+c，liney:y为目标停车线，d为所述停车阈值，carwidth为所述车辆的宽度，c为所述预设值。
5.根据权利要求4所述的方法，其特征在于，所述判断所述前进轨迹的终点是否满足完全泊出条件，包括：
获取所述前进轨迹的终点的位姿参数，所述终点的位姿参数包括在所述目标世界坐标系中的x值、y值和航向角对应的弧度；
计算圆周率π与2的第一比值；
计算所述航向角对应的弧度与所述第一比值的差值的绝对值；
判断所述y值是否大于等于所述停车阈值且所述绝对值是否小于弧度阈值，所述弧度阈值等于10与180的第二比值再乘以圆周率π得到的乘积；
当所述y值大于等于所述停车阈值且所述绝对值小于所述弧度阈值时，确定所述当前轨迹的终点满足所述完全泊出条件。
6.根据权利要求1至3任一项所述的方法，其特征在于，所述根据所述车辆的当前位置和所述车辆的最小转弯半径，在所述目标世界坐标系模拟车辆前进规划获得前进轨迹和模拟车辆后退规划获得后退轨迹，包括：
根据所述车辆的当前位置和所述车辆的最小转弯半径，在所述目标世界坐标系上设置第一参考圆、第二参考圆和第三参考圆，其中，所述第一参考圆与所述目标停车线相切于第一相切点，所述第一参考圆与所述第三参考圆相切于第二相切点，所述第二参考圆与所述第三参考圆相切于第三相切点，所述第三相切点为所述车辆的当前位置；
获取前进参考曲线，根据所述前进参考曲线，采用轨迹生成算法生成前进轨迹，所述前进参考曲线由所述第三参考圆上从所述第三相切点至所述第二相切点所对应的弧线与所述第一参考圆上从所述第二相切点至所述第一相切点所对应的弧线组成；
在所述车辆的当前位置对应的航向角小于第一角度阈值时，获取第一后退参考曲线，根据所述第一后退参考曲线，采用所述轨迹生成算法生成后退轨迹，所述第一后退参考曲线为y＝0；
在所述车辆的航向角不小于所述第一角度阈值时，获取第二后退参考曲线，根据所述第二后退参考曲线，采用所述轨迹生成算法生成所述后退轨迹，所述第二后退参考曲线为所述第二参考圆。
7.根据权利要求6所述的方法，其特征在于，在所述后退轨迹是根据所述第一后退参考曲线生成时，所述后退轨迹的长度不大于长度阈值；
在所述后退轨迹是根据所述第二后退参考曲线生成时，所述后退轨迹在所述车辆的当前位置对应的航向角大于第二角度阈值时停止生成。
8.根据权利要求7所述的方法，其特征在于，所述第三参考圆的圆心坐标计算公式如下：
O3.x＝carp.cx-R*sin(carp.cyaw)
O3.y＝carp.cy+R*cos(carp.cyaw)
所述第二参考圆的圆心坐标计算公式如下：
O2.x＝carp.cx+R*sin(carp.cyaw)
O3.y＝carp.cy-R*cos(carp.cyaw)
所述第一参考圆的圆心坐标计算公式如下：
O1.y＝d-R；
Figure FDA0003233788910000031
其中，carp.cx为所述车辆的当前位置对应的在所述目标世界坐标系上的X轴上的值，carp.cy为所述车辆的当前位置对应的在所述目标世界坐标系上的Y轴上的值，carp.cyaw为所述车辆的当前位置对应的航向角对应的弧度。
9.根据权利要求8所述的方法，其特征在于，所述获取前进参考曲线，根据所述前进参考曲线，采用轨迹生成算法生成前进轨迹，包括：
获取前进参考曲线，根据所述前进参考曲线，采用轨迹生成算法生成前进生成轨迹；
对所述前进生成轨迹进行碰撞检测；
在未检测到碰撞时，将所述前进生成轨迹作为前进轨迹；
所述根据所述第一后退参考曲线，采用所述轨迹生成算法生成后退轨迹，包括：
根据所述第一后退参考曲线，采用所述轨迹生成算法生成第一后退生成轨迹；
对所述第一后退生成轨迹进行碰撞检测；
在未检测到碰撞时，将所述第一后退生成轨迹作为后退轨迹；
所述根据所述第二后退参考曲线，采用所述轨迹生成算法生成所述后退轨迹，包括：
根据所述第二后退参考曲线，采用所述轨迹生成算法生成第二后退生成轨迹；
对所述第二后退生成轨迹进行碰撞检测；
在未检测到碰撞时，将所述第二后退生成轨迹作为后退轨迹。
10.一种车辆水平泊出轨迹规划装置，其特征在于，包括：
设置模块，用于根据车辆信息在初始世界坐标系上设置目标停车线，所述初始世界坐标系根据所述车辆的初始位置建立；
转换模块，用于将可通行空间栅格地图的数据转换到所述初始世界坐标系上，获得目标世界坐标系，所述可通行空间栅格地图根据车辆环视信息提取得到；
模拟模块，用于根据所述车辆的当前位置和所述车辆的最小转弯半径，在所述目标世界坐标系模拟车辆前进规划获得前进轨迹和模拟车辆后退规划获得后退轨迹；
判断模块，用于判断所述前进轨迹的终点是否满足完全泊出条件；
选择模块，用于如果所述判断模块判定所述前进轨迹的终点满足所述完全泊出条件，将所述前进轨迹选择为所述车辆行驶的目标轨迹；
所述判断模块，还用于如果判定所述前进轨迹的终点未满足所述完全泊出条件，判断所述后退轨迹的长度是否小于所述前进轨迹的长度；
所述选择模块，还用于如果所述判断模块判定所述后退轨迹的长度小于所述前进轨迹的长度，选择所述前进轨迹为所述目标轨迹；以及，如果所述判断模块判定所述后退轨迹的长度不小于所述前进轨迹的长度，选择所述后退轨迹为所述目标轨迹。
11.一种车辆，其特征在于，包括：
存储有可执行程序代码的存储器；
与所述存储器耦合的处理器；
所述处理器调用所述存储器中存储的所述可执行程序代码，执行权利要求1至9任一项所述的一种车辆水平泊出轨迹规划方法。
12.一种计算机可读存储介质，其上存储有计算机程序，其特征在于，该程序被处理器执行时实现权利要求1至9任一项所述方法的步骤。
CN202110995153.4A 2021-08-27 2021-08-27 一种车辆水平泊出轨迹规划方法及装置、车辆、存储介质 Active CN113561968B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202110995153.4A CN113561968B (zh) 2021-08-27 2021-08-27 一种车辆水平泊出轨迹规划方法及装置、车辆、存储介质

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110995153.4A CN113561968B (zh) 2021-08-27 2021-08-27 一种车辆水平泊出轨迹规划方法及装置、车辆、存储介质

Publications (2)

Publication Number Publication Date
CN113561968A true CN113561968A (zh) 2021-10-29
CN113561968B CN113561968B (zh) 2022-12-30

Family

ID=78173018

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110995153.4A Active CN113561968B (zh) 2021-08-27 2021-08-27 一种车辆水平泊出轨迹规划方法及装置、车辆、存储介质

Country Status (1)

Country Link
CN (1) CN113561968B (zh)

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114506314A (zh) * 2021-12-31 2022-05-17 爱驰汽车（上海）有限公司 车辆平行泊出方法、装置、设备及存储介质
CN117885756A (zh) * 2024-03-14 2024-04-16 同致电子科技（厦门）有限公司 一种他车泊出轨迹的确定方法、装置、设备及存储介质

Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2017122723A1 (ja) * 2016-01-12 2017-07-20 三菱重工業株式会社 駐車支援システム、駐車支援方法及びプログラム
CN108569281A (zh) * 2018-02-27 2018-09-25 蔚来汽车有限公司 用于车辆从狭窄侧方位车位自动泊出的系统及方法、电动车辆
CN110440823A (zh) * 2019-08-27 2019-11-12 广州小鹏汽车科技有限公司 一种路径规划方法及路径规划系统
CN110562244A (zh) * 2019-08-07 2019-12-13 武汉乐庭软件技术有限公司 一种基于目标停车位出库的自动泊车轨迹规划方法
CN111746517A (zh) * 2020-05-25 2020-10-09 中国第一汽车股份有限公司 一种车辆泊出控制方法及装置
CN112277930A (zh) * 2020-10-20 2021-01-29 上海欧菲智能车联科技有限公司 水平自主泊车控制方法、装置、车辆及存储介质

Patent Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2017122723A1 (ja) * 2016-01-12 2017-07-20 三菱重工業株式会社 駐車支援システム、駐車支援方法及びプログラム
CN108569281A (zh) * 2018-02-27 2018-09-25 蔚来汽车有限公司 用于车辆从狭窄侧方位车位自动泊出的系统及方法、电动车辆
CN110562244A (zh) * 2019-08-07 2019-12-13 武汉乐庭软件技术有限公司 一种基于目标停车位出库的自动泊车轨迹规划方法
CN110440823A (zh) * 2019-08-27 2019-11-12 广州小鹏汽车科技有限公司 一种路径规划方法及路径规划系统
CN111746517A (zh) * 2020-05-25 2020-10-09 中国第一汽车股份有限公司 一种车辆泊出控制方法及装置
CN112277930A (zh) * 2020-10-20 2021-01-29 上海欧菲智能车联科技有限公司 水平自主泊车控制方法、装置、车辆及存储介质

Cited By (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114506314A (zh) * 2021-12-31 2022-05-17 爱驰汽车（上海）有限公司 车辆平行泊出方法、装置、设备及存储介质
WO2023123935A1 (zh) * 2021-12-31 2023-07-06 爱驰汽车（上海）有限公司 车辆平行驶出方法、装置、设备及存储介质
CN117885756A (zh) * 2024-03-14 2024-04-16 同致电子科技（厦门）有限公司 一种他车泊出轨迹的确定方法、装置、设备及存储介质

Also Published As

Publication number Publication date
CN113561968B (zh) 2022-12-30

Similar Documents

Publication Publication Date Title
US10814876B2 (en) Vehicle lane-changing control method, vehicle lane-changing control device and related equipment
CN110361013B (zh) 一种用于车辆模型的路径规划系统及方法
US9789816B2 (en) Method and system for determining an ego-motion of a vehicle
CN113561968B (zh) 一种车辆水平泊出轨迹规划方法及装置、车辆、存储介质
CN110834630A (zh) 一种车辆的驾驶控制方法、装置、车辆及存储介质
CN113267199B (zh) 行驶轨迹规划方法及装置
EP3290281B1 (en) Automatic parking method and system of vehicle
CN111309013A (zh) 一种碰撞距离确定方法及系统、车辆、存储介质
CN105835870B (zh) 车辆行驶控制方法、装置及计算机设备
CN110239536B (zh) 一种车道保持控制方法、装置及终端
JP2007253745A (ja) 回避操作算出装置、回避制御装置、各装置を備える車両、回避操作算出方法および回避制御方法
US11390272B2 (en) Parking assistance device
US10977946B2 (en) Vehicle lane change assist improvements
CN114735002B (zh) 一种车辆的转向控制方法、装置、车辆及存储介质
CN114030463B (zh) 一种自动泊车系统的路径规划方法及装置
CN113504782B (zh) 障碍物防撞方法、装置、系统和移动工具
US20240054793A1 (en) Information processing device, information processing method, and program
CN114132305B (zh) 用于垂直与斜列泊车路径规划方法、系统、存储介质及车辆
CN113895429B (zh) 一种自动泊车方法、系统、终端及存储介质
CN114185337A (zh) 一种车辆、车辆预碰撞检测方法及装置
US20230242107A1 (en) Vehicle control device and vehicle control system
CN112477855B (zh) 一种车辆转向辅助方法、装置及设备
CN112882476A (zh) 用于控制agv车体转向的控制方法及控制装置
CN112550280A (zh) 一种车辆泊出的方法和装置
CN116476840A (zh) 变道行驶方法、装置、设备及存储介质

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant