水平自主泊车控制方法、装置、车辆及存储介质与流程

[0001]
本申请涉及车辆控制技术领域，具体涉及一种水平自主泊车控制方法、装置、车辆及存储介质。


背景技术：

[0002]
近年来，随着自动驾驶技术的不断发展和应用，自动驾驶的舒适性和合理性变得越来越重要。针对自动泊车过程，主要为车辆根据车位场景规划路径，车辆跟随规划路径完成自动泊车。
[0003]
然而，在实现本发明过程中，发明人发现现有技术中至少存在如下问题：传统水平泊车采用clc轨迹(圆弧-直线-圆弧)，可实现泊入，但其曲率不连续，泊车体验较差；在水平泊车轨迹设计中也会引入回旋线，从而规划出曲率连续的轨迹，这比较符合使用者的驾驶习惯，但是此种设计计算较为复杂、回旋线起始终止点计算也较为刻板。


技术实现要素：

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

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

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