垂直泊车轨迹生成方法、装置、车辆及存储介质与流程

[0001]
本申请涉及车辆控制技术领域，具体涉及一种垂直泊车轨迹生成方法、装置、车辆及存储介质。


背景技术：

[0002]
近年来，随着自动驾驶技术的不断发展和应用，自动驾驶的舒适性和合理性变得越来越重要。针对自动泊车过程，主要为车辆根据车位场景规划路径，车辆跟随规划路径完成自动泊车。
[0003]
然而，在实现本发明过程中，发明人发现现有技术中至少存在如下问题：曲率连续的泊车轨迹可以带来良好的泊车体验，泊车轨迹设计引入回旋线可以很好的满足曲率连续的要求。但也带来了计算复杂，回旋线起始终止点计算刻板等问题。


技术实现要素：

[0004]
鉴于以上问题，本申请提出一种垂直泊车轨迹生成方法、装置、车辆及存储介质，可以使得自动泊车更加符合驾驶习惯。
[0005]
本申请的第一方面提供一种垂直泊车轨迹生成方法，所述方法包括：
[0006]
初始阶段、调整阶段、入库阶段和揉库阶段；
[0007]
在所述初始阶段，获取目标库位中的目标位置、障碍物的位置坐标(ob
x
,ob
y
)和所述车辆的初始位姿信息，所述初始位姿信息包括所述车辆的初始位置坐标(carp
x
,carp
y
)和航向角信息；
[0008]
在所述调整阶段，基于所述车辆的初始位置坐标和所述障碍物的位置坐标确定第一基准曲线，并计算所述入库阶段的最小泊入圆，其中所述最小泊入圆的圆心坐标为(ox，oy)；若所述初始位置坐标的横坐标小于所述圆心坐标的横坐标，基于所述初始位置和所述第一基准曲线，生成第一目标轨迹，所述第一目标轨迹包括第一目标点；
[0009]
在所述入库阶段，若所述车辆的车尾没有位于所述目标库位中，以所述最小泊入圆作为所述入库阶段的第二基准曲线，并基于所述第一目标点和所述第二基准曲线生成第二目标轨迹，所述第二目标轨迹包括第二目标点；及
[0010]
在所述揉库阶段，根据所述目标库位所在位置确定第三基准曲线，并基于所述第二目标点和所述第三基准曲线生成第三目标轨迹。
[0011]
本申请的第二方面提供一种垂直泊车轨迹生成装置，所述装置包括：
[0012]
划分模块，用于将车辆泊入目标库位的整个阶段划分为初始阶段、调整阶段、入库阶段和揉库阶段；
[0013]
获取模块，用于在所述初始阶段，获取目标库位中的目标位置、障碍物的位置坐标(ob
x
,ob
y
)和所述车辆的初始位姿信息，所述初始位姿信息包括所述车辆的初始位置坐标(carp
x
,carp
y
)和航向角信息；
[0014]
生成模块，用于在所述调整阶段，基于所述车辆的初始位置坐标和所述障碍物的
位置坐标确定第一基准曲线，并计算所述入库阶段的最小泊入圆，其中所述最小泊入圆的圆心坐标为(ox，oy)；若所述初始位置坐标的横坐标小于所述圆心坐标的横坐标，基于所述初始位置和所述第一基准曲线，生成第一目标轨迹，所述第一目标轨迹包括第一目标点；
[0015]
所述生成模块，还用于在所述入库阶段，若所述车辆的车尾没有位于所述目标库位中，以所述最小泊入圆作为所述入库阶段的第二基准曲线，并基于所述第一目标点和所述第二基准曲线生成第二目标轨迹，所述第二目标轨迹包括第二目标点；及
[0016]
所述生成模块，还用于在所述揉库阶段，根据所述目标库位所在位置确定第三基准曲线，并基于所述第二目标点和所述第三基准曲线生成第三目标轨迹。
[0017]
本申请第三方面提供一种车辆，所述车辆包括：
[0018]
存储器、处理器和通讯总线，所述存储器通过所述通讯总线与所述处理器通信连接；以及所述存储器中存储有多个程序模块，所述多个程序模块由所述处理器加载并执行如上所述的垂直泊车轨迹生成方法。
[0019]
本申请第四方面提供一种计算机可读存储介质，其上存储有计算机程序，所述计算机程序被处理器执行时实现如前所述的垂直泊车轨迹生成方法。
[0020]
本申请提供的垂直泊车轨迹生成方法、装置、车辆及介质，本申请提供的垂直泊车轨迹生成方法，将车辆泊入目标库位的整个阶段划分为初始阶段、调整阶段、入库阶段和揉库阶段，针对不同的阶段生成不同的目标轨迹，控制所述车辆按照生成的目标轨迹泊入目标库位中。本申请可以生成车辆可执行的曲率连续的轨迹点，计算量简单，且生成的轨迹满足驾驶员的驾驶习惯。
附图说明
[0021]
图1是本申请一实施例所提供的垂直泊车轨迹生成方法的流程示意图。
[0022]
图2是本申请一实施例所提供的包括障碍物的应用场景示意图。
[0023]
图3是本申请一实施例所提供的无障碍物的应用场景示意图
[0024]
图4是本申请一实施例所提供的生成第二目标轨迹的示意图。
[0025]
图5是本申请一实施例所提供的垂直泊车轨迹生成装置的功能模块图。
[0026]
图6为本申请一实施例所提供的车辆的架构示意图。
具体实施方式
[0027]
为了能够更清楚地理解本申请的所述目的、特征和优点，下面结合附图和具体实施例对本申请进行详细描述。需要说明的是，在不冲突的情况下，本申请的实施例及实施例中的特征可以相互组合。在下面的描述中阐述了很多具体细节以便于充分理解本申请，所描述的实施例仅是本申请一部分实施例，而不是全部的实施例。
[0028]
除非另有定义，本文所使用的所有的技术和科学术语与属于本申请的技术领域的技术人员通常理解的含义相同。本文中在本申请的说明书中所使用的术语只是为了描述具体的实施例的目的，不是旨在于限制本申请。
[0029]
请参阅图1，图1为本申请一个实施例提供的垂直泊车轨迹生成方法的流程示意图。根据不同的需求，所述流程图中步骤的顺序可以改变，某些步骤可以省略。为了便于说明，仅示出了与本申请实施例相关的部分。
[0030]
本申请实施例的垂直泊车轨迹生成方法应用在车辆中。所述对于需要进垂直泊车轨迹生成的车辆，可以直接在所述车辆上集成本申请的方法所提供的垂直泊车轨迹生成功能，或者安装用于实现本申请的垂直泊车轨迹生成方法的客户端。再如，本申请所提供的垂直泊车轨迹生成方法还可以以软件开发工具包(software development kit，sdk)的形式运行在所述车辆上，以sdk的形式提供垂直泊车轨迹生成功能的接口，处理器或其他设备通过提供的接口即可实现垂直泊车轨迹生成功能。所述垂直泊车轨迹生成方法包括以下步骤。
[0031]
步骤s1、将车辆泊入目标库位的整个阶段划分为初始阶段、调整阶段、入库阶段和揉库阶段。
[0032]
在本实施方式中，先将泊车路径规划过程分为四个阶段，针对不同阶段根据车辆模型及当前场景计算出车辆当前阶段对应的路径的基准曲线，再采用前视窗口跟踪算法，生成曲率连续的目标轨迹，并根据所述目标轨迹生成泊入轨迹，控制所述车辆按照所述泊入轨迹泊入目标库位中。
[0033]
所述四个阶段包括第一阶段、第二阶段、第三阶段和第四阶段。所述第一阶段为初始阶段，即所述车辆刚进入泊车规划的阶段；所述第二阶段为调整阶段(如图2中的p点到a点)，即所述车辆在目标库位外，但是所述车辆在当前位置无法通过倒车使得车尾进入所述目标库位；所述第三阶段为入库阶段(如图2中的a点到b点)，即所述车辆在目标库位外，但是车辆在当前位置可通过倒车使得车尾进入库位；所述第四阶段为揉库阶段(如图2中的b点到c点，和c点至d点)，即所述车辆的车尾进入目标库位，可通过前进、倒车规划轨迹使得车辆泊入所述目标库位中。
[0034]
步骤s2、在所述初始阶段，获取目标库位的位置坐标、障碍物的位置坐标和待入库车辆的初始位姿信息，所述初始位姿信息包括所述车辆的初始位置坐标和航向角信息。
[0035]
在本实施方式中，在世界坐标系下，当所述车辆在所述初始位置时，车辆接收到泊车入位的泊车指令，车辆的摄像头、超声波雷达等传感器对车辆周围环境进行感知，获得车位状况，并根据车位状况确定障碍物的位置坐标和目标库位d的位置坐标。
[0036]
在其他实施方式中，可以通过车辆二自由度模型对连续时间内车辆的位置和姿态进行计算，或者基于gps和惯性导航组合对车辆的位置和姿态进行计算。需要说明的是，所述车辆二自由度模型和所述基于gps和惯性导航组合为现有技术，在此不再赘述。
[0037]
在一实施方式中，可以通过基于双目视觉的障碍物检测方法获取所述障碍物的位置坐标。
[0038]
步骤s3、在所述调整阶段，基于所述车辆的初始位置坐标和所述障碍物的位置坐标确定第一基准曲线，并计算所述入库阶段的最小泊入圆，其中所述最小泊入圆的圆心坐标为(ox，oy)；若所述初始位置坐标的横坐标小于所述圆心坐标的横坐标，基于所述初始位置和所述第一基准曲线，生成第一目标轨迹，所述第一目标轨迹包括第一目标点。
[0039]
在本实施方式中，不管车辆当前处于初始阶段还是处于调整阶段，都需要根据初始位置的横坐标和所述最小泊入圆的圆心的横坐标大小，来确定所述车辆是可以直接倒车入库，进入入库阶段；还是需要再次经过调整阶段调整车身后，再进入入库阶段。
[0040]
具体地，若述初始位置坐标的横坐标大于或等于所述圆心坐标的横坐标，则确定不需要再调整车身，可以控制所述车辆进入揉库阶段；若初始位置坐标的横坐标小于所述
圆心坐标的横坐标，则确定需要进入调整阶段调整所述车辆。
[0041]
如图2所示，在所述调整阶段，所述车辆并不需要进入目标库位，而是将车辆从点p行驶至点a处，以使车辆可以倒车入库。因此，在所述调整阶段的第一基准曲线由所述车辆的初始位置坐标和所述障碍物的位置坐标来确定。具体地，通过如下公式确定所述调整阶段的第一基准曲线：
[0042]
其中，carp
x
为所述车辆的初始位置的横坐标，ob
x
为所述障碍物的位置的横坐标，l为车身宽度，d1为所述车辆车身与所述目标库位之间的距离。在本实施方式中，d1等于1米。
[0043]
再基于所述第一基准曲线和障碍物的位置坐标计算所述入库阶段的最小泊入圆，其中所述最小泊入圆的圆心坐标为(o
x
，o
y
)。
[0044]
在本实施方式中，可以根据所述目标库位右侧是否存在障碍物来确定所述最小泊入圆的圆心坐标。若所述目标库位右侧存在障碍物，所述最小泊入圆的半径为车辆最小转弯半径r，且满足车辆若沿圆弧行驶可以避开障碍物(obx，oby)(如图2所示)，此处安全距离为d2＝0.2m，通过以下公式计算所述圆心坐标：
[0045][0046]
(ob
x-o
x
)2+(ob
y-o
y
)2＝(r-d2-l/2)2[0047][0048]
若所述目标库位右侧不存在障碍物，如图3所示，通过以下公式计算计算所述圆心坐标：
[0049][0050]
o
y
＝targetp
y-r
[0051]
其中，targetp
y
为目标位置点的坐标。斜车位场景要求圆心o到目标位置点对应的基准线(targetp线)的距离为r，同样可以求得圆心o坐标。
[0052]
若确定所述车辆进入调整阶段，基于所述初始位置和所述第一基准曲线，通过前视窗口跟踪算法生成第一目标轨迹，所述第一目标轨迹包括第一目标点。
[0053]
在本实施方式中，根据所述第一基准曲线，通过前视窗口跟踪算法生成第一目标轨迹(如图2所示的step1生成轨迹)，所述第一目标轨迹的终点为所述第一目标点，如图2中的a。
[0054]
具体地，将所述车辆模型简化为自行车模型，以所述车辆的后轴为切点，以车辆纵向车身为切线，通过控制前轮偏转角，模拟车辆以速度为v沿着第一圆弧轨迹行驶，获得曲率连续路径点，单步模拟车辆的行驶距离为dl，单步前轮最大调整偏转角为δα
max
。采用前视窗口跟踪算法生成第一目标轨迹的方法包括：
[0055]
设定所述车辆的初始位姿为q(qx，qy，qyaw)，其中，q
x
为所述车辆在初始位置时对应世界坐标系中的x轴方向的坐标，q
y
为所述车辆在初始位置时对应世界坐标系中的y轴方向的坐标，qyaw为所述车辆在初始位置时的航向角。
[0056]
基于所述车辆初始位置坐标，将所述车辆的后轴中心映射到所述第一基准曲线
上，得到映射点p1(p1x，p1y)；
[0057]
以所述映射点p1为起点根据车辆行驶方向，在所述第一基准曲线上移动至第一目标点g1，并确定所述车辆在所述第一目标点的位姿信息g1(g1x，g1y，g1yaw)。例如，在所述第一基准曲线上移动距离(如1米)到达所述第一目标点g1。其中，g1x为所述车辆在所述第一目标点对应世界坐标系中的x轴方向的坐标，g1y为所述车辆在所述第一目标点对应世界坐标系中的y轴方向的坐标，g1yaw为所述车辆在所述第一目标点的航向角。
[0058]
根据所述初始位置q和所述第一目标点g1点生成一个与车辆纵相切的第一目标圆，所述第一目标圆的圆心为点o1，半径为r1。在本实施方式中，所述第一目标圆经过所述初始位置q和所述第一目标点g1，且在所述初始位置q与车辆纵相切。所述第一目标圆与车辆纵相切是指所述第一目标圆与以所述车辆后轴中心为起点，方向垂直于所述后轴的直线相切。在确定所述初始位置q和第一目标点g1后，根据几何原理可以得到所述第一目标圆的半径r1。
[0059]
控制所述车辆以所述初始位置为起点，在所述第一目标圆上移动预设距离dl，到达第i预设目标点qi，其中，i为正整数；
[0060]
计算所述车辆在所述第i预设目标点qi的前轮偏转角和位姿信息；
[0061]
判断所述第i预设目标点qi的位姿信息与所述第一目标点g1的位姿信息之间的误差是否小于或等于预设误差；
[0062]
当所述第i预设目标点qi的位姿与所述第一目标点g1的位姿之间的误差小于或等于所述预设误差时，确定所述车辆到达第一目标点g1；
[0063]
输出所述第一目标轨迹，所述第一目标轨迹为所述车辆在所述第一目标圆上，从所述初始位置行驶到所述第i目标点(即第一目标点g1)的轨迹。
[0064]
当所述第i预设目标点qi的位姿信息与所述第一目标点g1的位姿信息之间的误差大于所述预设误差时，控制所述车辆根据所述第i预设目标点qi的前轮偏转角继续移动预设距离dl；获取所述车辆继续移动预设距离dl后的位姿信息，直到获取的位姿信息与所述第一目标点的位姿之间的误差小于或等于所述预设误差时，确定所述车辆到达第一目标点，输出所述第一目标轨迹。
[0065]
在本实施方式中，所述计算所述车辆在所述第i预设目标点qi的前轮偏转角和位姿信息包括：
[0066]
根据所述第一目标圆的半径r1计算所述车辆的目标前轮偏转角α1
exp
，其中α1
exp
＝tan-1
(l/r1)，l为所述车辆的轴距，即所述车辆的前轮与所述车辆的后轮之间的距离。
[0067]
基于所述目标前轮偏转角α1
exp
计算所述车辆在所述第i预设目标点qi的前轮偏转角的角度增量δα
i
，δα
i
＝α1
exp-α
i
，其中，α
i
为所述车辆在所述第i预设目标点qi的实际前轮偏转角。当i＝1时，α1为所述车辆在初始位置的前轮偏转角。当i大于1时，通过车辆当前位置(如第i预设目标点qi的坐标)与第一基准曲线上的点得到的圆，可以确定所述圆的半径ri(第i次)，由阿克曼转向原理计算的所述车辆的前轮偏转角度α
i
＝tan-1
(l/ri)。
[0068]
需要说明的是，车辆在初始位置的前轮偏转角为已知角度，并且所述δα
i
∈[-δα
max
，δα
max
]，δα
max
单步前轮最大调整偏转角。
[0069]
基于所述角度增量更新所述车辆在所述第i预设目标点qi的前轮偏转角为α
i
′
＝α
i-1
+δα
i
。
[0070]
计算第一圆心角β
i
，所述第一圆心角为所述车辆在所述第一目标圆上行驶至所述第i预设目标点时的弧长对应的圆心角，其中，β
i
＝i
×
dl/r
′
i
，r
′
i
＝l/tanα
i
′
；
[0071]
根据所述待入库车辆初始位姿信息和所述第一圆心角β
i
，通过以下公式计算所述车辆在所述第i预设目标点的位姿信息：
[0072]
qi
yaw
＝q(i-1)
yaw
+β
i
[0073]
qi
x
＝q(i-1)
x
+r
′
i
*(sin(qi
yaw
)-sin(q(i-1)
yaw
))
[0074]
qi
y
＝q(i-1)
y
+r
′
i
*(cos(q(i-1)
yaw
)-cos(qi
yaw
))
[0075]
其中，qi
x
为所述车辆在第i预设目标点对应世界坐标系中的x轴方向的坐标，qi
y
为所述车辆在第i预设目标点对应世界坐标系中的y轴方向的坐标，qi
yaw
为所述车辆在第i预设目标点的航向角，r
′
i
＝l/tanα
i
′
。
[0076]
例如，在初始位置时，所述车辆的位姿信息为q(qx，qy，qyaw)，在第1预设目标点时，所述车辆的位姿为：
[0077]
q1
yaw
＝q
yaw
+β1[0078]
q1
x
＝q
x
+r
′1*(sin(q1
yaw
)-sin(q
yaw
))
[0079]
q1
y
＝q
y
+r
′1*(cos(q
yaw
)-cos(q1
yaw
))
[0080]
在第2预设目标点时，所述车辆的位姿为：
[0081]
q2
yaw
＝q1
yaw
+β2[0082]
q2
x
＝q1
x
+r
′2*(sin(q2
yaw
)-sin(q1
yaw
))
[0083]
q2
y
＝q1
y
+r
′2*(cos(q1
yaw
)-cos(q2
yaw
))
[0084]
依此类推，得到车辆在所述第i预设目标点的位姿信息：
[0085]
qi
yaw
＝q(i-1)
yaw
+β
i
[0086]
qi
x
＝q(i-1)
x
+r
′
i
*(sin(qi
yaw
)-sin(q(i-1)
yaw
))
[0087]
qi
y
＝q(i-1)
y
+r
′
i
*(cos(q(i-1)
yaw
)-cos(qi
yaw
))
[0088]
步骤s4、在所述入库阶段，若所述车辆的车尾没有位于所述目标库位中，以所述最小泊入圆作为所述入库阶段的第二基准曲线，并基于所述第一目标点和所述第二基准曲线生成第二目标轨迹，所述第二目标轨迹包括第二目标点。
[0089]
在生成所述第一目标轨迹后，控制所述车辆按照所述第一目标轨迹行驶至所述第一目标点，之后所述车辆进入入库阶段。在本实施方式中，通过车辆的车尾是否在所述目标库位中，来确认所述车辆是进入入库阶段，还是直接进入揉库阶段。若所述车辆的车尾没有位于所述目标库位中，以所述最小泊入圆作为所述入库阶段的第二基准曲线；当所述车辆的车尾位于所述目标库位中时，控制所述车辆进入揉库阶段，根据所述目标库位所在位置确定所述揉库阶段的第三基准曲线。
[0090]
在本实施方式中，所述入库阶段以所述最小泊入圆作为第二基准曲线，再通过前视窗口跟踪算法生成第二目标轨迹，再控制所述车辆按照所述第二目标轨迹行驶后，以进入揉库阶段。基于所述第一目标点和所述第二基准曲线，通过前视窗口跟踪算法生成第二目标轨迹，所述第二目标轨迹包括第二目标点。
[0091]
在本实施方式中，根据所述第二基准曲线，通过前视窗口跟踪算法生成第二目标轨迹(如图2所示的step2生成轨迹)，所述第二目标轨迹的终点为所述第二目标点。
[0092]
具体地，需要说明的是，生成第二目标轨迹与生成第一目标轨迹一样，需将车辆简
化为自行车模型，在此不再赘述。如图4所示，采用前视窗口跟踪算法生成第二目标轨迹的方法包括：
[0093]
更新所述车辆的初始位置为所述第一目标点；
[0094]
基于所述第一目标点，将所述车辆的后轴中心映射到所述第二基准曲线上，得到第二映射点p2(p2x，p2y)；
[0095]
以所述第二映射点p2为起点根据车辆行驶方向，在所述第二基准曲线上移动至第二目标点g2，并确定所述车辆在所述第二目标点的位姿信息g2(g2x，g2y，g2yaw)。例如，在所述第二基准曲线上移动距离(如1米)到达所述第二目标点g2。其中，g2x为所述车辆在所述第二目标点对应世界坐标系中的x轴方向的坐标，g2y为所述车辆在所述第二目标点对应世界坐标系中的y轴方向的坐标，g2yaw为所述车辆在所述第二目标点的航向角。
[0096]
根据所述第一目标点g1和第二目标点g2生成一个与车辆纵相切的第二目标圆，所述第二目标圆的圆心为点o2，半径为r2；在确定所述第一目标点g1和第二目标点g2后，根据几何原理可以得到所述第二目标圆的半径r2。
[0097]
控制所述车辆以第一目标点为起点，在所述第二目标圆上移动预设距离dl，到达第j预设目标点qj，其中，j大于或等于i，且j为正整数；
[0098]
计算所述车辆在所述第j预设目标点qj的前轮偏转角和位姿信息；
[0099]
判断所述第j预设目标点qj的位姿信息与所述第二目标点g2的位姿信息之间的误差是否小于或等于所述预设误差；
[0100]
当所述第j预设目标点qj的位姿信息与所述第二目标点g2的位姿信息之间的误差小于或等于所述预设误差时，确定所述车辆到达第二目标点g2；
[0101]
输出所述第二目标轨迹，所述第二目标轨迹为所述车辆在所述第二目标圆上，从所述第一目标点行驶到所述第j目标点(即第二目标点g2)的轨迹；
[0102]
当所述第j预设目标点qj的位姿信息与所述第二目标点g2的位姿信息之间的误差大于所述预设误差时，控制所述车辆根据所述第j预设目标点qj的前轮偏转角继续移动预设距离dl；获取所述车辆继续移动预设距离dl后的位姿信息，直到获取的位姿信息与所述第二目标点g2的位姿信息之间的误差小于或等于所述预设误差时，确定所述车辆到达第二目标点g2，输出所述第二目标轨迹。
[0103]
在本实施方式中，所述计算所述车辆在所述第j预设目标点的位姿信息包括：
[0104]
根据所述第二目标圆的半径r2计算所述车辆的目标前轮偏转角α2
exp
，其中α2
exp
＝tan-1
(l/r2)，l为所述车辆的轴距，即所述车辆的前轮与所述车辆的后轮之间的距离。
[0105]
基于所述目标前轮偏转角α2
exp
计算所述车辆在所述第j预设目标点qj的前轮偏转角的角度增量δα
j
，δα
j
＝α2
exp-α
j
。其中，α
j
为所述车辆在所述第j预设目标点qj的实际前轮偏转角。当j＝i时，α
j
为所述车辆在所述第一目标点的前轮偏转角。当j大于i时，通过车辆当前位置(如第j预设目标点qj的坐标)与所述第二基准曲线上的点得到的圆，可以确定得到的圆的半径rj(第j次)，由阿克曼转向原理计算的所述车辆的前轮偏转角度α
j
＝tan-1
(l/rj)，l为所述车辆的轴距。
[0106]
基于所述角度增量更新所述车辆在所述第j预设目标点qj的前轮偏转角为α
j
′
＝α
j-1
+δα
j
。
[0107]
计算第二圆心角β
j
，所述第二圆心角为所述车辆在所述第二目标圆上行驶至所述
第j预设目标点时的弧长对应的圆心角，其中，β
j
＝j
×
dl/r
′
j
，r
′
j
＝l/tanα
j
′
；
[0108]
根据所述车辆在第j预设目标点的位姿信息和所述第二圆心角β
j
，通过以下公式计算所述车辆在所述第j预设目标点的位姿信息：
[0109]
qj
yaw
＝q(j-1)
yaw
+β
j
[0110]
qj
x
＝q(j-1)
x
+r
′
j
*(sin(qj
yaw
)-sin(q(j-1)
yaw
))
[0111]
qj
y
＝q(j-1)
y
+r
′
j
*(cos(q(j-1)
yaw
)-cos(qj
yaw
))
[0112]
其中，qj
x
为所述车辆在第j预设目标点对应世界坐标系中的x轴方向的坐标，qj
y
为所述车辆在第j预设目标点对应世界坐标系中的y轴方向的坐标，qj
yaw
为所述车辆在第j预设目标点的航向角，q(j-1)
x
为所述车辆在第j-1预设目标点对应世界坐标系中的x轴方向的坐标，q(j-1)
y
为所述车辆在第j-1预设目标点对应世界坐标系中的y轴方向的坐标，q(j-1)
yaw
为所述车辆在第j-1预设目标点的航向角。需要说明的是，当j＝i时，以所述车辆在所述第i预设目标点的位姿信息(qi
x
，qi
y
，qi
yaw
)作为计算所述车辆在第j预设目标点的位姿信息的初始位姿信息。
[0113]
步骤s5、在所述揉库阶段，根据所述目标库位所在位置确定第三基准曲线，并基于所述第二目标点和所述第三基准曲线生成第三目标轨迹。
[0114]
在本实施方式中，如图3所示，在世界坐标系中，所述目标库位为一矩形，确定所述矩形的四个顶点为第一顶点a1、第二顶点a2、第三顶点a3和第四顶点a4，分别获取所述四个顶点的坐标。以过所述第一顶点和第二顶点构成的边的中点，且垂直于所述第一顶点和第二顶点构成的边的线为所述第三基准曲线，也可以以过所述第三顶点和第四顶点构成的边的中点，且垂直于所述第三顶点和第四顶点构成的边的线为所述第三基准曲线。
[0115]
在本实施方式中，根据所述第三基准曲线，通过前视窗口跟踪算法生成第三目标轨迹(如图2所示的step3生成轨迹)，所述第三目标轨迹的终点为所述第三目标点。
[0116]
需要说明的是，生成第三目标轨迹与生成第一目标轨迹一样，需将车辆简化为自行车模型，在此不再赘述。具体地，采用前视窗口跟踪算法生成第三目标轨迹的方法包括：
[0117]
更新所述第一目标点为所述第二目标点；
[0118]
基于所述第二目标点，将所述待入库车辆的后轴中心映射到所述第三基准曲线上，得到第三映射点p3(p3x，p3y)；
[0119]
以所述第三映射点p3为起点根据车辆行驶方向，在所述第三基准曲线上移动至目标库位d，确定所述车辆在所述目标库位的位姿信息d(dx，dy，dyaw)；其中，dx为所述车辆在所述目标库位对应世界坐标系中的x轴方向的坐标，dy为所述车辆在所述目标库位对应世界坐标系中的y轴方向的坐标，dyaw为所述车辆在所述目标库位的航向角。
[0120]
根据所述第二目标点g2和目标库位d生成一个与车辆纵相切的第三目标圆，所述第三目标圆的圆心为点o3，半径为r3；在确定所述第二目标点g2和目标库位d后，根据几何原理可以得到所述第三目标圆的半径r3。
[0121]
控制所述车辆以第二目标点为起点，在所述第三目标圆上移动预设距离dl，到达第k预设目标点qk，其中，k大于或等于j，且k为正整数；
[0122]
计算所述车辆在所述第k预设目标点qk的前轮偏转角和位姿信息；
[0123]
判断所述第k预设目标点qk的位姿信息与所述目标库位的位姿信息之间的误差是否小于或等于所述预设误差；
[0124]
当所述第k预设目标点qk的位姿信息与所述目标库位的位姿信息之间的误差小于或等于所述预设误差时，确定所述车辆到达所述目标库位；
[0125]
输出所述第三目标轨迹，所述第三目标轨迹为所述车辆在所述第三目标圆上，从所述第二目标点行驶到所述目标库位的轨迹；
[0126]
当所述第k预设目标点qk的位姿信息与所述目标库位的位姿信息之间的误差大于所述预设误差时，控制所述车辆根据所述第k预设目标点qk的前轮偏转角继续移动预设距离dl；获取所述车辆继续移动预设距离d1后的位姿信息，直到获取的位姿信息与所述目标库位的位姿信息之间的误差小于或等于所述预设误差时，确定所述车辆到达目标库位，输出所述第三目标轨迹。
[0127]
在本实施方式中，所述计算所述车辆在所述第k预设目标点qk的前轮偏转角和位姿信息包括：
[0128]
根据所述第三目标圆的半径r3计算所述车辆的目标前轮偏转角α3
exp
，其中α3
exp
＝tan-1
(l/r3)，l为所述车辆的轴距，即所述车辆的前轮与所述车辆的后轮之间的距离。
[0129]
基于所述目标前轮偏转角α3
exp
计算所述车辆在所述第k预设目标点qk的前轮偏转角的角度增量δα
k
，δα
k
＝α3
exp-α
k
。其中，α
k
为所述车辆在所述第k预设目标点qk的实际前轮偏转角。当k＝j时，α
k
为所述车辆在所述第二目标点的前轮偏转角。当k大于j时，通过车辆当前位置(如第k预设目标点qk的坐标)与所述切线轨迹上的点得到的圆，可以确定得到的圆的半径rk(第k次)，由阿克曼转向原理计算的所述车辆的前轮偏转角度α
k
＝tan-1
(l/rk)，l为所述车辆的轴距。
[0130]
基于所述角度增量更新所述车辆在所述第k预设目标点qk的前轮偏转角为α
k
′
＝α
k-1
+δα
k
。
[0131]
计算第三圆心角β
k
，所述第三圆心角为所述车辆在所述第三目标圆上行驶至所述第k预设目标点时的弧长对应的圆心角，其中，β
k
＝k
×
dl/r
k
′
；
[0132]
根据所述车辆在第j预设目标点的位姿信息和的所述第三圆心角β
k
，通过以下公式计算所述车辆在所述第k预设目标点的位姿信息：
[0133]
qk
yaw
＝q(k-1)
yaw
+β
k
[0134]
qk
x
＝q(k-1)
x
+r
′
k*(sin(qk
yaw
)-sin(q(k-1)
yaw
))
[0135]
qk
y
＝q(k-1)
y
+r
′
k
*(cos(q(k-1)
yaw
)-cos(qk
yaw
))
[0136]
其中，qk
x
为所述车辆在第k预设目标点对应世界坐标系中的x轴方向的坐标，qk
y
为所述车辆在第k预设目标点对应世界坐标系中的y轴方向的坐标，qk
yaw
为所述车辆在第k预设目标点的航向角，r
′
k
＝l/tanα
k
′
，q(k-1)
x
为所述车辆在第k-1预设目标点对应世界坐标系中的x轴方向的坐标，q(k-1)
y
为所述车辆在第k-1预设目标点对应世界坐标系中的y轴方向的坐标，q(k-1)
变aw
为所述车辆在第k-1预设目标点的航向角。需要说明的是，当k＝j时，以所述车辆在所述第j预设目标点的位姿信息(qj
x
，qj
y
，qj
yaw
)作为计算所述车辆在第k预设目标点的位姿信息的初始位姿信息。
[0137]
在一实施方式中，所述垂直泊车轨迹生成方法还包括：
[0138]
基于所述第一目标轨迹、第二目标轨迹和第三目标轨迹生成泊入轨迹。
[0139]
在本实施方式中，控制所述车辆从初始位置按照所述第一目标轨迹行驶至所述第一目标点，再控制所述车辆保持第一目标点的前轮偏转角不变，从所述第一目标点按照所
述第二目标轨迹行驶至所述第二目标点，继续控制所述车辆保持第二目标点的前轮偏转角不变，从所述第二目标点按照所述第三目标轨迹行驶至目标库位中。从而实现在第一目标轨迹与第二目标轨迹的衔接处(即第一目标点)、第二目标轨迹与第三目标轨迹的衔接处(即第二目标点)，通过固定所述衔接处的前轮偏转角，满足了曲率的连续性。使得车辆的方向盘转角能够连续变化，比较符合真实驾驶场景。
[0140]
若述初始位置坐标的横坐标大于或等于所述圆心坐标的横坐标，则确定不需要再调整车身，可以控制所述车辆进入揉库阶段，在所述车辆进入揉库阶段后，以所述最小泊入圆作为所述揉库阶段的第四基准曲线，基于所述初始位置和所述第四基准曲线，通过前视窗口跟踪算法生成第四目标轨迹。
[0141]
在本实施方式中，当所述车辆的初始位置坐标的横坐标大于或等于所述圆心坐标的横坐标时，可以直接控制所述车辆进入揉库阶段，并且以所述最小泊入圆作为所述揉库阶段的第四基准曲线，生成第四目标轨迹，并控制车辆根据所述第四目标轨迹直接倒车入库。
[0142]
在本实施方式中，所述生成第四目标轨迹的方法与所述生成第二目标轨迹的方法相似。第二目标轨迹是基于所述第一目标点和所述第二基准曲线，通过前视窗口跟踪算法生成的，所述第四目标轨迹是基于所述初始位置和所述第四基准曲线，通过前视窗口跟踪算法生成的。而所述第二基准曲线和所述第四基准曲线都是所述最小泊入圆。因此，在这里不再赘述生成第四目标轨迹的方法。
[0143]
在本实施方式中，当确定可以直接倒车入库时，可以直接以所述第四目标轨迹作为泊入轨迹，根据所述泊入轨迹控制所述车辆泊入所述目标库位。
[0144]
图1详细介绍了本申请的垂直泊车轨迹生成方法，通过所述方法，能够提高垂直泊车轨迹生成速度。下面结合图5和图6，对实现所述垂直泊车轨迹生成装置的功能模块以及硬件装置架构进行介绍。应所述了解，所述实施例仅为说明之用，在专利申请范围上并不受此结构的限制。
[0145]
图5为本申请一实施方式提供的垂直泊车轨迹生成装置的功能模块图。
[0146]
在一些实施方式中，所述垂直泊车轨迹生成装置100可以包括多个由程序代码段所组成的功能模块。所述垂直泊车轨迹生成装置100中的各个程序段的程序代码可以存储于车辆10的存储器中，并由车辆10中的至少一个处理器所执行，以实现水平自主泊车的功能。
[0147]
参考图5，本实施方式中，垂直泊车轨迹生成装置100根据其所执行的功能，可以被划分为多个功能模块，所述各个功能模块用于执行图1对应实施方式中的各个步骤，以实现水平自主泊车的功能。本实施方式中，所述垂直泊车轨迹生成装置100的功能模块包括：划分模块101、获取模块102及生成模块103。
[0148]
所述划分模块101用于将车辆泊入目标库位的整个阶段划分为初始阶段、调整阶段、入库阶段和揉库阶段；
[0149]
所述获取模块102用于在所述初始阶段，获取所述目标库位中的目标位置、障碍物的位置坐标(ob
x
，ob
y
)和所述车辆的初始位姿信息，所述初始位姿信息包括所述车辆的初始位置坐标(carp
x
，carp
y
)和航向角信息；
[0150]
所述生成模块103用于在所述调整阶段，基于所述车辆的初始位置坐标和所述障
碍物的位置坐标确定的第一基准曲线，并计算所述入库阶段的最小泊入圆，其中所述最小泊入圆的圆心坐标为(ox，oy)；若所述初始位置坐标的横坐标小于所述圆心坐标的横坐标，基于所述初始位置和所述第一基准曲线，生成第一目标轨迹，所述第一目标轨迹包括第一目标点；
[0151]
所述生成模块103还用于在所述入库阶段，若所述车辆的车尾没有位于所述目标库位中，以所述最小泊入圆作为所述入库阶段的第二基准曲线，并基于所述第一目标点和所述第二基准曲线生成第二目标轨迹，所述第二目标轨迹包括第二目标点；及
[0152]
所述生成模块103还用于在所述揉库阶段，根据所述目标库位所在位置确定第三基准曲线，并基于所述第二目标点和所述第三基准曲线生成第三目标轨迹。
[0153]
图6为本申请一实施例所提供的车辆的架构示意图。所述车辆10包括存储器11、处理器12和通讯总线13，所述存储器11通过所述通讯总线13与所述处理器12通信连接。
[0154]
所述车辆10还包括存储在所述存储器11中并可在所述处理器12上运行的计算机程序14，例如垂直泊车轨迹生成的程序。
[0155]
所述处理器12执行所述计算机程序14时实现所述方法实施例中垂直泊车轨迹生成方法的步骤。或者，所述处理器12执行所述计算机程序14实现所述系统实施例中各模块/单元的功能。
[0156]
示例性的，所述计算机程序14可以被分割成一个或多个模块/单元，所述一个或者多个模块/单元被存储在所述存储器11中，并由所述处理器12执行，以完成本申请。所述一个或多个模块/单元可以是能够完成特定功能的一系列计算机程序指令段，所述指令段用于描述所述计算机程序14在所述电子装置1中的执行过程。例如，所述计算机程序14可以被分割成图5中的模块101-103。
[0157]
本领域技术人员可以理解，所述示意图6仅仅是车辆10的示例，并不构成对车辆10的限定，车辆10可以包括比图示更多或更少的部件，或者组合某些部件，或者不同的部件，例如所述车辆10还可以包括输入设备等。
[0158]
所称处理器12可以是中央处理单元(central processing unit，cpu)，还可以包括其他通用处理器、数字信号处理器(digital signal processor，dsp)、专用集成电路(application specific integrated circuit，asic)、现成可编程门阵列(field-programmable gate array，fpga)或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件等。通用处理器可以是微处理器或者所述处理器也可以是任何常规的处理器等，所述处理器12是所述车辆10的控制中心，利用各种接口和线路连接整个车辆10的各个部分。
[0159]
所述存储器11可用于存储所述计算机程序14和/或模块/单元，所述处理器12通过运行或执行存储在所述存储器11内的计算机程序和/或模块/单元，以及调用存储在存储器11内的数据，实现所述车辆10的各种功能。存储器11可以包括外部存储介质，也可以包括内存。此外，存储器11可以包括高速随机存取存储器，还可以包括非易失性存储器，例如硬盘、内存、插接式硬盘，智能存储卡(smart media card，smc)，安全数字(secure digital，sd)卡，闪存卡(flash card)、至少一个磁盘存储器件、闪存器件、或其他易失性固态存储器件。
[0160]
所述车辆10集成的模块/单元如果以软件功能单元的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存储介质中。基于这样的理解，本申请实现所
述实施例方法中的全部或部分流程，也可以通过计算机程序来指令相关的硬件来完成，所述的计算机程序可存储于一计算机可读存储介质中，所述计算机程序在被处理器执行时，可实现所述各个方法实施例的步骤。需要说明的是，所述计算机可读介质包含的内容可以根据司法管辖区内立法和专利实践的要求进行适当的增减，例如在某些司法管辖区，根据立法和专利实践，计算机可读介质不包括电载波信号和电信信号。
[0161]
最后应说明的是，以上实施例仅用以说明本申请的技术方案而非限制，尽管参照较佳实施例对本申请进行了详细说明，本领域的普通技术人员应当理解，可以对本申请的技术方案进行修改或等同替换，而不脱离本申请技术方案的精神和范围。

起点商标作为专业知识产权交易平台，可以帮助大家解决很多问题，如果大家想要了解更多知产交易信息请点击 【在线咨询】或添加微信 【19522093243】与客服一对一沟通，为大家解决相关问题。

此文章来源于网络,如有侵权,请联系删除